From 33377b8ae33bf9c5979996d5b4eec04794f10a2a Mon Sep 17 00:00:00 2001 From: Reece Humphreys Date: Mon, 22 Sep 2025 10:41:12 -0600 Subject: [PATCH 1/6] Run pre-commit over all files in the repo --- .github/pull_request_template.md | 4 +- LICENSE | 1 - docs/source/Install/buildExtModules.rst | 2 - docs/source/Learn.rst | 1 - .../Learn/bskPrinciples/bskPrinciples-0.rst | 1 - .../Learn/bskPrinciples/bskPrinciples-1.rst | 2 - .../Learn/bskPrinciples/bskPrinciples-2a.rst | 2 - .../Learn/bskPrinciples/bskPrinciples-2b.rst | 2 - .../Learn/bskPrinciples/bskPrinciples-7.rst | 2 - .../Learn/bskPrinciples/bskPrinciples-8.rst | 2 - .../Learn/bskPrinciples/bskPrinciples-9.rst | 2 +- docs/source/Learn/makingModules.rst | 2 - .../advancedTopics/creatingDynObject.rst | 1 - docs/source/Learn/makingModules/cModules.rst | 2 - .../makingModules/cModules/cModules-1.rst | 1 - .../makingModules/cModules/cModules-3.rst | 1 - .../source/Learn/makingModules/cppModules.rst | 2 - .../makingModules/cppModules/cppModules-2.rst | 2 +- .../Learn/makingModules/makingModules-1.rst | 2 +- .../Learn/makingModules/makingModules-3.rst | 2 - .../Learn/makingModules/makingModules-4.rst | 2 +- docs/source/Learn/makingModules/pyModules.rst | 4 +- docs/source/Support.rst | 5 - docs/source/Support/Developer/Debugging.rst | 12 +- .../Support/Developer/MigratingToPython3.rst | 2 +- .../Developer/UnderstandingBasilisk.rst | 1 - docs/source/Support/User/migratingToBsk2.rst | 2 +- ...n_container_layout_BasicOrbitFormation.svg | 2 +- .../_Support/test_scenarioBasicOrbit.nb | 653 +- .../_Support/test_scenarioIntegrator.nb | 1633 +- .../test_scenario_AttEclipseUpdated.svg | 2 +- .../source/_images/static/qs-bsk-2b-order.svg | 1242 +- .../_images/static/scenario_AttEclipse.svg | 2 +- .../static/scenario_basicOrbit_BSKSim.svg | 2 +- .../_images/static/simpleDataConcept.svg | 2 +- .../test_scenarioAttitudeFeedback2T_TH.svg | 2 +- .../test_scenarioAttitudeFeedbackRWPower.svg | 2 +- .../static/test_scenarioAttitudeGG.svg | 2 +- .../static/test_scenarioAttitudeGuidance.svg | 2 +- .../static/test_scenarioAttitudeSteering.svg | 2 +- .../static/test_scenarioBasicOrbit.svg | 2 +- .../_images/static/test_scenarioCSS.svg | 2 +- .../_images/static/test_scenarioDataToViz.svg | 2 +- .../static/test_scenarioFormationBasic.svg | 2 +- .../test_scenarioFormationMeanOEFeedback.svg | 2 +- .../static/test_scenarioFormationReconfig.svg | 2 +- .../static/test_scenarioHingedRigidBody.svg | 2 +- .../static/test_scenarioIntegrators.svg | 2 +- .../static/test_scenarioOrbitManeuver.svg | 2 +- .../static/test_scenarioOrbitMultiBody.svg | 2 +- .../test_scenarioSmallBodyLandmarks.svg | 2 +- .../static/test_scenarioSmallBodyNavUKF.svg | 2 +- .../test_scenario_AttEclipseUpdated.svg | 2 +- .../test_scenario_AttGuidHyperbolic.svg | 2 +- .../static/test_scenario_AttGuidance.svg | 2 +- .../static/test_scenario_AttSteering.svg | 2 +- .../test_scenario_BasicOrbitFormation.svg | 2 +- .../static/test_scenario_InertialPoint.svg | 2 +- .../static/test_scenario_MagneticField.svg | 2 +- ...t_scenario_MagneticFieldCenteredDipole.svg | 2 +- .../static/test_scenario_MagneticFieldWMM.svg | 2 +- ...est_scenario_RelativePointingFormation.svg | 2 +- .../static/test_scenario_basicOrbit_v1.1.svg | 2 +- docs/source/resources/mac_fix_path.pth | 2 +- examples/BskSim/BSK_masters.py | 2 +- .../BskSim/models/BSK_FormationDynamics.py | 1 - .../modelsMultiSat/BSK_MultiSatDynamics.py | 2 +- .../plottingMultiSat/BSK_MultiSatPlotting.py | 1 - .../scenariosOpNav/OpNavMC/MonteCarlo.py | 2 - examples/Support/run_MC_IC/run0.json | 2 +- examples/Support/run_MC_IC/run1.json | 2 +- examples/Support/run_MC_IC/run2.json | 2 +- .../Itokawa/ItokawaHayabusa.mtl | 1 - .../dataForExamples/Loral-1300Com-main.mtl | 1 - examples/dataForExamples/triangularPanel.obj | 2 +- examples/scenarioAerocapture.py | 22 +- examples/scenarioDeployingPanel.py | 18 +- examples/scenarioDragSensitivity.py | 8 +- examples/scenarioFuelSlosh.py | 8 +- examples/scenarioIntegratorsComparison.py | 22 +- examples/scenarioSatelliteConstellation.py | 14 +- examples/scenarioTAMcomparison.py | 5 +- externalTools/_doc.rst | 2 +- src/architecture/alg_contain/alg_contain.h | 4 +- .../_UnitTest/test_CMsgTimeWritten.py | 1 - .../msgPayloadDefC/AlbedoMsgPayload.h | 2 +- .../msgPayloadDefC/CSSRawDataMsgPayload.h | 2 +- .../msgPayloadDefC/CSSUnitConfigMsgPayload.h | 2 +- .../CmdForceInertialMsgPayload.h | 2 +- .../msgPayloadDefC/DvBurnCmdMsgPayload.h | 2 +- .../msgPayloadDefC/InertialFilterMsgPayload.h | 2 +- .../msgPayloadDefC/MTBArrayConfigMsgPayload.h | 1 - .../msgPayloadDefC/OpNavFilterMsgPayload.h | 2 +- .../msgPayloadDefC/OpNavLimbMsgPayload.h | 1 - .../msgPayloadDefC/RWArrayConfigMsgPayload.h | 2 +- .../msgPayloadDefC/RateCmdMsgPayload.h | 2 +- .../ReconfigBurnArrayInfoMsgPayload.h | 2 +- .../ReconfigBurnInfoMsgPayload.h | 2 +- .../msgPayloadDefC/SunlineFilterMsgPayload.h | 2 +- .../THRArrayCmdForceMsgPayload.h | 2 +- .../msgPayloadDefC/TemperatureMsgPayload.h | 1 - .../msgPayloadDefC/VehicleConfigMsgPayload.h | 2 +- .../system_model/_UnitTest/test_PySysModel.py | 6 +- src/architecture/utilities/BSpline.h | 8 +- .../utilities/_Documentation/AVS.sty | 6 +- .../_Documentation/Basilisk-IMU-20170712.tex | 4 +- .../_Documentation/BasiliskCorruptions.tex | 4 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/discretize/AVS.sty | 6 +- .../Basilisk-discretize-20180108.tex | 4 +- .../discretize/BasiliskReportMemo.cls | 1 - .../discretize/secModelDescription.tex | 2 +- .../discretize/secModelFunctions.tex | 8 +- .../_Documentation/discretize/secTest.tex | 10 +- .../discretize/secUserGuide.tex | 6 +- .../_Documentation/gaussMarkov/AVS.sty | 6 +- .../Basilisk-gaussMarkov-20180108.tex | 4 +- .../gaussMarkov/BasiliskReportMemo.cls | 1 - .../gaussMarkov/secModelDescription.tex | 2 +- .../gaussMarkov/secModelFunctions.tex | 8 +- .../_Documentation/gaussMarkov/secTest.tex | 10 +- .../gaussMarkov/secUserGuide.tex | 6 +- .../utilities/_Documentation/saturate/AVS.sty | 6 +- .../saturate/Basilisk-saturate-20180108.tex | 4 +- .../saturate/BasiliskReportMemo.cls | 1 - .../saturate/secModelFunctions.tex | 2 +- .../_Documentation/saturate/secTest.tex | 10 +- .../_Documentation/saturate/secUserGuide.tex | 6 +- .../_Documentation/secModelDescription.tex | 2 +- .../_Documentation/secModelFunctions.tex | 8 +- .../utilities/_Documentation/secTest.tex | 10 +- .../utilities/_Documentation/secUserGuide.tex | 6 +- src/architecture/utilities/avsEigenMRP.h | 10 +- src/architecture/utilities/avsEigenSupport.h | 2 +- src/architecture/utilities/bskSemaphore.h | 4 +- src/architecture/utilities/linearAlgebra.h | 1 - .../moduleIdGenerator/moduleIdGenerator.h | 2 +- .../utilities/rigidBodyKinematics.h | 2 +- src/architecture/utilities/saturate.h | 6 +- src/architecture/utilities/signalCondition.h | 4 +- src/architecture/utilities/simDefinitions.h | 2 +- .../utilities/tests/test_avsEigenMRP.cpp | 1 - src/architecture/utilities/ukfUtilities.c | 4 +- .../utilitiesSelfCheck/_Documentation/AVS.sty | 6 +- .../Basilisk-avsLibrary20170812.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/discretize/AVS.sty | 6 +- .../Basilisk-discretize-20180108.tex | 4 +- .../discretize/BasiliskReportMemo.cls | 1 - .../discretize/secModelFunctions.tex | 2 +- .../_Documentation/discretize/secTest.tex | 8 +- .../discretize/secUserGuide.tex | 2 +- .../_Documentation/gaussMarkov/AVS.sty | 6 +- .../Basilisk-gaussMarkov-20180108.tex | 4 +- .../gaussMarkov/BasiliskReportMemo.cls | 1 - .../_Documentation/gaussMarkov/secTest.tex | 6 +- .../gaussMarkov/secUserGuide.tex | 2 +- .../_Documentation/saturate/AVS.sty | 6 +- .../saturate/Basilisk-saturate-20180108.tex | 4 +- .../saturate/BasiliskReportMemo.cls | 1 - .../saturate/secModelFunctions.tex | 2 +- .../_Documentation/saturate/secTest.tex | 6 +- .../_Documentation/saturate/secUserGuide.tex | 2 +- .../_Documentation/secModelDescription.tex | 106 +- .../_Documentation/secModelFunctions.tex | 37 +- .../_Documentation/secTest.tex | 51 +- .../_Documentation/secUserGuide.tex | 39 +- .../_UnitTest/test_BSpline.py | 26 +- src/cmake/conan.cmake | 5 +- ...im-LowPassFilterControlTorque-20160108.tex | 26 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_UnitTest/Support/LPF-Validation.nb | 270 +- .../lowPassFilterTorqueCommand.h | 4 +- .../lowPassFilterTorqueCommand.rst | 2 - .../Support/MRP-Feedback-Validation.nb | 531 +- src/fswAlgorithms/attControl/mrpPD/mrpPD.h | 4 +- src/fswAlgorithms/attControl/mrpPD/mrpPD.rst | 1 - .../_Documentation/Support/fOptions.nb | 650 +- .../Support/MRP-Steering-Validation.nb | 683 +- .../attControl/mrpSteering/mrpSteering.h | 6 +- .../_UnitTest/test_mtbFeedforward.py | 65 +- .../mtbFeedforward/mtbFeedforward.h | 6 +- .../_UnitTest/test_mtbMomentumManagement.py | 32 +- .../mtbMomentumManagement.h | 8 +- .../mtbMomentumManagement.rst | 1 - .../test_mtbMomentumManagementSimple.py | 30 +- .../mtbMomentumManagementSimple.h | 6 +- .../AVS-Sim-PRV_Steering-2016-0108.tex | 96 +- .../prvSteering/_Documentation/AVS.sty | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/Support/fOptions.nb | 650 +- .../Support/PRV-Steering-Validation.nb | 626 +- .../attControl/prvSteering/prvSteering.h | 6 +- .../AVS-Sim-nonlinRateServo-2019-0327.tex | 52 +- .../_Documentation/AVS.sty | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../Support/MRP-Steering-Validation.nb | 683 +- .../rateServoFullNonlinear.h | 4 +- .../rateServoFullNonlinear.rst | 1 - .../_Documentation/AVS.sty | 8 +- ...asilisk-thrMomentumManagement-20160817.tex | 34 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_UnitTest/Support/moduleValidation.nb | 69 +- .../thrMomentumManagement.h | 4 +- .../thrMomentumManagement.rst | 1 - .../CSSEst/_Documentation/AVS.sty | 6 +- .../Basilisk-cssWlsEst-20180429.tex | 168 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../CSSEst/_Documentation/secUserGuide.tex | 8 +- .../_UnitTest/test_cssWlsEstUnitTest.py | 2 +- .../attDetermination/CSSEst/cssWlsEst.h | 4 +- .../InertialUKF/_Documentation/AVS.sty | 6 +- .../Basilisk-inertialUKF-20190402.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 26 +- .../_Documentation/secModuleDescription.tex | 12 +- .../_Documentation/secModuleFunctions.tex | 13 +- .../InertialUKF/_Documentation/secTest.tex | 26 +- .../_Documentation/secUserGuide.tex | 7 +- .../InertialUKF/_UnitTest/test_inertialKF.py | 2 +- .../InertialUKF/inertialUKF.h | 8 +- .../InertialUKF/inertialUKF.rst | 1 - .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/heading_SuKF.tex | 40 +- .../headingSuKF/_Documentation/references.bib | 8 +- .../headingSuKF/headingSuKF.h | 16 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../okeefeEKF/_Documentation/Okeefe_EKF.tex | 38 +- .../okeefeEKF/_Documentation/references.bib | 8 +- .../attDetermination/okeefeEKF/okeefeEKF.h | 12 +- .../attDetermination/okeefeEKF/okeefeEKF.rst | 1 - .../_Documentation/BasiliskReportMemo.cls | 1 - .../sunlineEKF/_Documentation/Sunline_EKF.tex | 38 +- .../sunlineEKF/_Documentation/references.bib | 8 +- .../attDetermination/sunlineEKF/sunlineEKF.h | 18 +- .../sunlineEphem/_Documentation/AVS.sty | 6 +- .../Basilisk-SunlineEphem-20181204.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 14 +- .../_Documentation/secModuleDescription.tex | 7 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../sunlineEphem/_Documentation/secTest.tex | 30 +- .../sunlineEphem/sunlineEphem.h | 6 +- .../sunlineEphem/sunlineEphem.rst | 1 - .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/Sunline_SEKF.tex | 52 +- .../sunlineSEKF/_Documentation/references.bib | 8 +- .../sunlineSEKF/sunlineSEKF.h | 20 +- .../sunlineSEKF/sunlineSEKF.rst | 1 - .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/Sunline_SuKF.tex | 40 +- .../sunlineSuKF/_Documentation/references.bib | 8 +- .../sunlineSuKF/sunlineSuKF.h | 10 +- .../sunlineSuKF/sunlineSuKF.rst | 1 - .../_Documentation/BasiliskReportMemo.cls | 1 - .../sunlineUKF/_Documentation/Sunline_UKF.tex | 28 +- .../sunlineUKF/_Documentation/references.bib | 8 +- .../attDetermination/sunlineUKF/sunlineUKF.h | 8 +- .../_UnitTest/test_attRefCorrection.py | 14 +- .../attRefCorrection/attRefCorrection.rst | 7 +- .../attTrackingError/_Documentation/AVS.sty | 6 +- .../Basilisk-attTrackingError-20190220.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleDescription.tex | 18 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../_Documentation/secTest.tex | 47 +- .../_Documentation/secUserGuide.tex | 2 +- .../attTrackingError/attTrackingError.rst | 2 - .../_Documentation/AVS.sty | 6 +- .../_Documentation/AVSSimReportMemo.cls | 1 - ...asilisk-celestialTwoBodyPoint-20190311.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModelDescription.tex | 39 +- .../_Documentation/secModelFunctions.tex | 4 +- .../_Documentation/secTest.tex | 49 +- .../_Documentation/secUserGuide.tex | 5 +- .../celestialTwoBodyPoint.h | 8 +- .../celestialTwoBodyPoint.rst | 1 - .../test_ConstrainedAttitudeManeuver.py | 30 +- .../constrainedAttitudeManeuver.h | 2 +- .../constrainedAttitudeManeuver.rst | 10 +- .../Support/results_eulerRotation.py | 4 - .../attGuidance/eulerRotation/eulerRotation.h | 8 +- .../hillPoint/_Documentation/AVS.sty | 6 +- .../_Documentation/AVSSimReportMemo.cls | 1 - .../hillPoint/_Documentation/hillPoint.tex | 50 +- .../attGuidance/hillPoint/hillPoint.h | 4 +- .../inertial3D/_Documentation/AVS.sty | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModuleDescription.tex | 10 +- .../_Documentation/secModuleFunctions.tex | 4 +- .../inertial3D/_Documentation/secTest.tex | 19 +- .../attGuidance/inertial3D/inertial3D.rst | 1 - .../Support/results_inertial3DSpin.py | 2 +- .../inertial3DSpin/inertial3DSpin.h | 4 +- .../inertial3DSpin/inertial3DSpin.rst | 2 - .../_UnitTest/test_locationPointing.py | 14 +- .../locationPointing/locationPointing.rst | 18 +- .../mrpRotation/_Documentation/AVS.sty | 6 +- .../Basilisk-MRPROTATION-20180522.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModuleDescription.tex | 10 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../mrpRotation/_Documentation/secTest.tex | 26 +- .../_Documentation/secUserGuide.tex | 6 +- .../_UnitTest/Support/truth_mrpRotation.py | 3 - .../attGuidance/mrpRotation/mrpRotation.h | 8 +- .../attGuidance/mrpRotation/mrpRotation.rst | 1 - .../opNavPoint/_Documentation/AVS.sty | 6 +- .../Basilisk-opNavPoint-20190820.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModelDescription.tex | 22 +- .../_Documentation/secModelFunctions.tex | 10 +- .../opNavPoint/_Documentation/secTest.tex | 35 +- .../_Documentation/secUserGuide.tex | 17 +- .../attGuidance/opNavPoint/opNavPoint.h | 2 +- .../attGuidance/opNavPoint/opNavPoint.rst | 1 - .../attGuidance/rasterManager/rasterManager.h | 4 +- .../rasterManager/rasterManager.rst | 1 - .../simpleDeadband/simpleDeadband.h | 4 +- .../simpleDeadband/simpleDeadband.rst | 1 - .../sunSafePoint/_Documentation/AVS.sty | 6 +- .../Basilisk-sunSafePoint-20180427.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModelDescription.tex | 28 +- .../_Documentation/secModelFunctions.tex | 10 +- .../sunSafePoint/_Documentation/secTest.tex | 41 +- .../_Documentation/secUserGuide.tex | 17 +- .../attGuidance/sunSafePoint/sunSafePoint.h | 2 +- .../attGuidance/sunSafePoint/sunSafePoint.rst | 1 - .../velocityPoint/_Documentation/AVS.sty | 6 +- .../_Documentation/AVSSimReportMemo.cls | 1 - .../_Documentation/velocityPoint.tex | 50 +- .../Support/results_velocityPoint.py | 2 +- .../velocityPoint/velocityPoint.rst | 1 - .../waypointReference/waypointReference.h | 10 +- .../waypointReference/waypointReference.rst | 24 +- .../dvAttGuidance/_Documentation/AVS.sty | 6 +- .../Basilisk-dvGuidance-2019-03-28.tex | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleDescription.tex | 22 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../dvAttGuidance/_Documentation/secTest.tex | 20 +- .../dvExecuteGuidance/dvExecuteGuidance.h | 2 +- .../_GeneralModuleFiles/thrustGroupData.h | 2 +- .../errorConversion/dvAttEffect.h | 4 +- .../errorConversion/sunSafeACS.h | 2 +- .../errorConversion/sunSafeACS.rst | 2 +- .../_UnitTest/test_hingedRigidBodyPIDMotor.py | 4 +- .../hingedRigidBodyPIDMotor.rst | 8 +- .../rwMotorTorque/_Documentation/AVS.sty | 8 +- .../Basilisk-rwMotorTorque-20190320.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModelDescription.tex | 21 +- .../_Documentation/secModelFunctions.tex | 6 +- .../rwMotorTorque/_Documentation/secTest.tex | 90 +- .../_Documentation/secUserGuide.tex | 4 +- .../_UnitTest/Support/rwMotorTorque.nb | 97 +- .../rwMotorTorque/rwMotorTorque.h | 4 +- .../rwMotorTorque/rwMotorTorque.rst | 2 - .../Basilisk-rwMotorVoltage-20170113.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModelDescription.tex | 10 +- .../_Documentation/secModelFunctions.tex | 2 +- .../rwMotorVoltage/_Documentation/secTest.tex | 10 +- .../_Documentation/secUserGuide.tex | 2 +- .../_UnitTest/Support/unitTestSupport.nb | 305 +- .../rwMotorVoltage/rwMotorVoltage.h | 4 +- .../rwNullSpace/_Documentation/AVS.sty | 6 +- .../Basilisk-rwNullSpace-20190209.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModuleDescription.tex | 14 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../rwNullSpace/_Documentation/secTest.tex | 23 +- .../rwNullSpace/rwNullSpace.h | 4 +- .../rwNullSpace/rwNullSpace.rst | 5 - .../thrFiringRemainder/_Documentation/AVS.sty | 6 +- ...Basilisk-thrFiringRemainder-2019-03-28.tex | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModuleDescription.tex | 22 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../_Documentation/secTest.tex | 23 +- .../thrFiringRemainder/thrFiringRemainder.h | 6 +- .../thrFiringSchmitt/_Documentation/AVS.sty | 6 +- .../Basilisk-thrFiringSchmitt-2019-03-29.tex | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModuleDescription.tex | 22 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../_Documentation/secTest.tex | 22 +- .../thrFiringSchmitt/thrFiringSchmitt.h | 6 +- .../thrForceMapping/_Documentation/AVS.sty | 8 +- .../Basilisk-ThrusterForces-20160627.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/Support/thrusterMapping.nb | 51355 ++++++++-------- .../_Documentation/secModuleDescription.tex | 50 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../_Documentation/secTest.tex | 71 +- .../_Documentation/secUserGuide.tex | 14 +- .../thrForceMapping/thrForceMapping.h | 2 +- .../thrForceMapping/thrForceMapping.rst | 2 - .../Basilisk-thrMomentumDumping-20160820.tex | 34 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../thrMomentumDumping/thrMomentumDumping.h | 6 +- .../thrMomentumDumping/thrMomentumDumping.rst | 1 - .../thrustRWDesat/thrustRWDesat.h | 4 +- .../torque2Dipole/torque2Dipole.h | 4 +- .../Support/etSphericalControl_testMatlab.m | 52 +- .../etSphericalControl/etSphericalControl.rst | 2 +- .../formationBarycenter/formationBarycenter.h | 4 +- .../formationBarycenter.rst | 23 +- .../_UnitTest/test_hillStateConverter.py | 4 +- .../hillStateConverter/hillStateConverter.rst | 8 +- .../_UnitTest/test_hillToAttRef.py | 10 +- .../hillToAttRef/hillToAttRef.h | 3 +- .../hillToAttRef/hillToAttRef.rst | 10 +- .../spacecraftPointing/_Documentation/AVS.sty | 6 +- .../Basilisk-spacecraftPointing-20190116.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/example/AVS.sty | 6 +- .../example/BasiliskReportMemo.cls | 1 - .../_Documentation/example/bibliography.bib | 24 +- .../modularizedDocumentationExample.tex | 4 +- .../example/secModelDescription.tex | 4 +- .../example/secModelFunctions.tex | 2 +- .../_Documentation/example/secTest.tex | 6 +- .../_Documentation/example/secUserGuide.tex | 2 +- .../_Documentation/secModuleDescription.tex | 14 +- .../_Documentation/secModuleFunctions.tex | 2 - .../_Documentation/secTest.tex | 10 +- .../_Documentation/secUserGuide.tex | 1 - .../spacecraftPointing/spacecraftPointing.h | 4 +- .../spacecraftPointing/spacecraftPointing.rst | 1 - .../spacecraftReconfig/spacecraftReconfig.rst | 2 +- .../fswUtilities/fswDefinitions.h | 2 +- .../centerRadiusCNN/centerRadiusCNN.h | 9 +- .../centerRadiusCNN/centerRadiusCNN.rst | 3 +- .../horizonOpNav/_Documentation/AVS.sty | 6 +- .../Basilisk-horizonOpNav-20190918.tex | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 26 +- .../_Documentation/secModuleDescription.tex | 8 +- .../_Documentation/secModuleFunctions.tex | 4 +- .../horizonOpNav/_Documentation/secTest.tex | 26 +- .../_Documentation/secUserGuide.tex | 2 +- .../horizonOpNav/horizonOpNav.h | 6 +- .../horizonOpNav/horizonOpNav.rst | 4 - .../houghCircles/_Documentation/AVS.sty | 6 +- .../Basilisk-houghCircles-20190213.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleDescription.tex | 2 +- .../_Documentation/secModuleFunctions.tex | 8 +- .../houghCircles/_Documentation/secTest.tex | 13 +- .../_Documentation/secUserGuide.tex | 2 +- .../houghCircles/houghCircles.h | 5 +- .../houghCircles/houghCircles.rst | 4 - .../limbFinding/_Documentation/AVS.sty | 6 +- .../Basilisk-limbFinding-20190916.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleDescription.tex | 2 +- .../_Documentation/secModuleFunctions.tex | 6 +- .../limbFinding/_Documentation/secTest.tex | 21 +- .../_Documentation/secUserGuide.tex | 2 +- .../imageProcessing/limbFinding/limbFinding.h | 7 +- .../limbFinding/limbFinding.rst | 3 - .../pixelLineConverter/_Documentation/AVS.sty | 6 +- .../Basilisk-pixelLineConverter-20190524.tex | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleDescription.tex | 14 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../_Documentation/secTest.tex | 18 +- .../_Documentation/secUserGuide.tex | 2 +- .../pixelLineConverter/pixelLineConverter.h | 4 +- .../pixelLineConverter/pixelLineConverter.rst | 1 - .../pixelLineBiasUKF/_Documentation/AVS.sty | 6 +- .../Basilisk-pixelLineBiasUKF-20190620.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 26 +- .../_Documentation/secModuleDescription.tex | 19 +- .../_Documentation/secModuleFunctions.tex | 9 +- .../_Documentation/secTest.tex | 18 +- .../_Documentation/secUserGuide.tex | 10 +- .../_UnitTest/pixelLineBias_test_utilities.py | 2 +- .../pixelLineBiasUKF/pixelLineBiasUKF.h | 30 +- .../pixelLineBiasUKF/pixelLineBiasUKF.rst | 1 - .../relativeODuKF/_Documentation/AVS.sty | 6 +- .../Basilisk-relativeOD-20190620.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 26 +- .../_Documentation/secModuleDescription.tex | 12 +- .../_Documentation/secModuleFunctions.tex | 9 +- .../relativeODuKF/_Documentation/secTest.tex | 20 +- .../_Documentation/secUserGuide.tex | 9 +- .../_UnitTest/relativeODuKF_test_utilities.py | 2 +- .../relativeODuKF/relativeODuKF.h | 26 +- .../relativeODuKF/relativeODuKF.rst | 2 - .../smallBodyWaypointFeedback.rst | 8 +- src/fswAlgorithms/rwConfigData/rwConfigData.h | 4 +- .../CSSSensorData/_Documentation/AVS.sty | 6 +- .../Basilisk-CSSSensorDataModule-20190207.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 26 +- .../_Documentation/secModuleDescription.tex | 17 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../CSSSensorData/_Documentation/secTest.tex | 24 +- .../_Documentation/secUserGuide.tex | 2 +- .../sensorInterfaces/CSSSensorData/cssComm.h | 4 +- .../CSSSensorData/cssComm.rst | 1 - .../sensorInterfaces/IMUSensorData/imuComm.h | 4 +- .../IMUSensorData/imuComm.rst | 1 - .../sensorInterfaces/STSensorData/stComm.h | 4 +- .../sensorInterfaces/STSensorData/stComm.rst | 1 - .../sensorInterfaces/TAMSensorData/tamComm.h | 4 +- .../rateMsgConverter/rateMsgConverter.h | 4 +- .../rateMsgConverter/rateMsgConverter.rst | 5 +- .../test_scanningInstrumentController.py | 60 +- .../scanningInstrumentController.rst | 36 +- .../test_simpleInstrumentController.py | 4 +- .../simpleInstrumentController.rst | 14 +- .../Images/moduleIOSmallBodyNavigation.svg | 2 +- .../smallBodyNavEKF/smallBodyNavEKF.rst | 10 +- .../Images/moduleIOSmallBodyNavigationUKF.svg | 2 +- .../smallBodyNavUKF/smallBodyNavUKF.h | 4 +- .../smallBodyNavUKF/smallBodyNavUKF.rst | 30 +- .../thrustCMEstimation/thrustCMEstimation.h | 8 +- .../thrustCMEstimation/thrustCMEstimation.rst | 3 +- .../_GeneralModuleFiles/ephemerisUtilities.c | 6 +- .../chebyPosEphem/chebyPosEphem.h | 6 +- .../dvAccumulation/_Documentation/AVS.sty | 6 +- .../Basilisk-dvAccumulation-2019-03-28.tex | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleDescription.tex | 6 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../dvAccumulation/_Documentation/secTest.tex | 20 +- .../dvAccumulation/dvAccumulation.h | 6 +- .../dvAccumulation/dvAccumulation.rst | 1 - .../ephemDifference/_Documentation/AVS.sty | 6 +- .../Basilisk-ephemDifference-2019-03-27.tex | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleDescription.tex | 1 - .../_Documentation/secModuleFunctions.tex | 4 +- .../_Documentation/secTest.tex | 26 +- .../_Documentation/secUserGuide.tex | 9 +- .../ephemDifference/ephemDifference.h | 6 +- .../ephemDifference/ephemDifference.rst | 1 - .../ephemNavConverter/ephemNavConverter.h | 4 +- .../ephemNavConverter/ephemNavConverter.rst | 2 - .../navAggregate/_Documentation/AVS.sty | 6 +- .../Basilisk-navAggregate-2019-02-21.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleDescription.tex | 6 +- .../_Documentation/secModuleFunctions.tex | 4 +- .../navAggregate/_Documentation/secTest.tex | 107 +- .../_Documentation/secUserGuide.tex | 8 +- .../navAggregate/navAggregate.h | 4 +- .../navAggregate/navAggregate.rst | 1 - .../oeStateEphem/_Documentation/AVS.sty | 6 +- .../Basilisk-oeStateEphem-20190426.tex | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleDescription.tex | 8 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../oeStateEphem/_Documentation/secTest.tex | 23 +- .../_Documentation/secUserGuide.tex | 6 +- .../oeStateEphem/oeStateEphem.h | 4 +- .../oeStateEphem/oeStateEphem.rst | 1 - src/moduleTemplates/_doc.rst | 2 - .../cModuleTemplate/_Documentation/AVS.sty | 6 +- .../_Documentation/Basilisk-MODULENAME.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/Images/fig1.svg | 2 +- .../_UnitTest/test_cModuleTemplate.py | 2 +- .../cppModuleTemplate/cppModuleTemplate.rst | 2 +- src/reportconf.py | 4 +- .../deviceInterface/encoder/encoder.rst | 2 +- .../test_hingedBodyLinearProfiler.py | 22 +- .../hingedBodyLinearProfiler.rst | 1 - .../motorVoltageInterface.h | 4 +- .../tempMeasurement/tempMeasurement.rst | 8 +- .../boreAngCalc/_Documentation/AVS.sty | 6 +- .../Basilisk-BOREANGLECALC-20170722.tex | 42 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModelDescription.tex | 8 +- .../_Documentation/secModelFunctions.tex | 2 +- .../boreAngCalc/_Documentation/secTest.tex | 30 +- .../DynOutput/boreAngCalc/boreAngCalc.h | 4 +- .../orbElemConvert/_Documentation/AVS.sty | 6 +- .../Basilisk-ORBELEMCONVERT-20170703.tex | 46 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModelDescription.tex | 12 +- .../_Documentation/secModelFunctions.tex | 4 +- .../orbElemConvert/_Documentation/secTest.tex | 30 +- .../_Documentation/secUserGuide.tex | 4 +- .../Basilisk-extPulsedTorque-20170324.tex | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/Support/diagram.nb | 65 +- .../_UnitTest/test_extPulsedTorque.py | 1 - .../dynamics/FuelTank/_Documentation/AVS.sty | 6 +- .../Basilisk-FUELTANK-20171203.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../PlanarFlexibleDynamicsDerivation.nb | 4431 +- .../FuelTank/_Documentation/bibliography.bib | 4 +- .../_Documentation/secModelDescription.tex | 69 +- .../_Documentation/secModelFunctions.tex | 6 +- .../FuelTank/_Documentation/secTest.tex | 5 +- src/simulation/dynamics/FuelTank/fuelTank.rst | 2 +- .../GravityGradientEffector.h | 2 +- .../GravityGradientEffector.rst | 1 - .../HingedRigidBodies/_Documentation/AVS.sty | 6 +- ...-HINGEDRIGIDBODYSTATEEFFECTOR-20170703.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../PlanarFlexibleDynamicsDerivation.nb | 4431 +- .../_Documentation/secModelDescription.tex | 22 +- .../_Documentation/secModelFunctions.tex | 2 +- .../_Documentation/secTest.tex | 46 +- .../hingedRigidBodyStateEffector.rst | 4 - .../Integrators/_Documentation/AVS.sty | 6 +- .../Basilisk-Integrators20170724.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModelDescription.tex | 8 +- .../_Documentation/secModelFunctions.tex | 2 +- .../Integrators/_Documentation/secTest.tex | 4 +- .../_Documentation/secUserGuide.tex | 2 +- .../Integrators/svIntegratorEuler.rst | 9 - .../dynamics/Integrators/svIntegratorRK2.rst | 10 - .../Integrators/svIntegratorRKF45.rst | 6 - .../Integrators/svIntegratorRKF78.rst | 9 - .../_Documentation/AVS.sty | 6 +- ...silisk-LINEARSPRINGMASSDAMPER-20180102.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 2 +- .../_Documentation/secModelDescription.tex | 46 +- .../_Documentation/secModelFunctions.tex | 2 +- .../_Documentation/secTest.tex | 24 +- .../linearSpringMassDamper.rst | 9 - .../dynamics/MtbEffector/MtbEffector.h | 8 +- .../dynamics/MtbEffector/MtbEffector.rst | 7 +- .../MtbEffector/_UnitTest/test_MtbEffector.py | 60 +- .../NHingedRigidBodies/_Documentation/AVS.sty | 6 +- ...NHINGEDRIGIDBODYSTATEEFFECTOR-20180103.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 2 +- .../_Documentation/secModelDescription.tex | 54 +- .../_Documentation/secModelFunctions.tex | 2 +- .../_Documentation/secTest.tex | 6 +- .../nHingedRigidBodyStateEffector.rst | 10 - .../RadiationPressure/_Documentation/AVS.sty | 6 +- .../Basilisk-RadiationPressure-20170712.tex | 4 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 2 +- .../_Documentation/secModelDescription.tex | 6 +- .../_Documentation/secModelFunctions.tex | 4 +- .../_Documentation/secTest.tex | 12 +- .../_Documentation/secUserGuide.tex | 4 +- .../_UnitTest/cannonballLookup.xml | 2 +- .../_UnitTest/cube_lookup.xml | 2 +- .../RadiationPressure/cube_lookup.xml | 2 +- .../RadiationPressure/radiationPressure.h | 2 +- .../RadiationPressure/radiationPressure.rst | 15 - .../_Documentation/AVS.sty | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../thrusterStateEffector.rst | 6 +- .../dynamics/VSCMGs/_Documentation/AVS.sty | 6 +- .../Basilisk-VSCMGSTATEEFFECTOR-20180718.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../VSCMGs/_Documentation/bibliography.bib | 10 +- .../_Documentation/secModelDescription.tex | 65 +- .../_Documentation/secModelFunctions.tex | 8 +- .../VSCMGs/_Documentation/secTest.tex | 34 +- .../VSCMGs/_Documentation/secUserGuide.tex | 2 +- .../_GeneralModuleFiles/dynParamManager.rst | 2 +- .../_GeneralModuleFiles/gravityEffector.h | 12 +- .../_GeneralModuleFiles/gravityEffector.rst | 2 +- .../_GeneralModuleFiles/hubEffector.cpp | 4 +- .../_GeneralModuleFiles/hubEffector.rst | 19 - .../_GeneralModuleFiles/svIntegratorRK4.rst | 10 - .../dragEffector/dragDynamicEffector.h | 2 +- .../dragEffector/dragDynamicEffector.rst | 1 - .../_Documentation/AVS.sty | 6 +- ...LHINGEDRIGIDBODYSTATEEFFECTOR-20180102.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 2 +- .../_Documentation/secModelDescription.tex | 334 +- .../_Documentation/secModelFunctions.tex | 2 +- .../_Documentation/secTest.tex | 6 +- .../dualHingedRigidBodyStateEffector.rst | 2 - .../_Documentation/BasiliskReportMemo.cls | 1 - .../extForceTorque/extForceTorque.rst | 1 - .../facetDragEffector/_Documentation/AVS.sty | 6 +- .../Basilisk-facet_drag-20190515.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleFunctions.tex | 4 +- .../_Documentation/secTest.tex | 19 +- .../_Documentation/secUserGuide.tex | 1 - .../facetDragDynamicEffector.h | 2 +- .../facetDragDynamicEffector.rst | 1 - .../gravityEffector/_Documentation/AVS.sty | 6 +- .../Basilisk-GravityEffector-20170712.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 38 +- .../_Documentation/secModelDescription.tex | 6 +- .../_Documentation/secModelFunctions.tex | 8 +- .../_Documentation/secTest.tex | 18 +- .../_Documentation/secUserGuide.tex | 4 +- .../_UnitTest/EROS856Vert1708Fac.txt | 2 +- .../gravityEffector/_UnitTest/GGM03S.txt | 2 +- .../Validation_code/computePolyAcc.c | 153 +- .../polyhedralTestdataGenerator.m | 22 +- .../dynamics/gravityEffector/gravCoeffOps.py | 10 +- .../_UnitTest/test_hingedRigidBodyMotor.py | 18 +- .../hingedRigidBodyMotor.h | 2 +- .../hingedRigidBodyMotor.rst | 6 +- .../linearTranslationOneDOFStateEffector.h | 2 +- .../linearTranslationOneDOFStateEffector.rst | 1 - .../_UnitTest/Support/msmCheck.nb | 843 +- .../_UnitTest/test_msmForceTorque.py | 14 +- .../dynamics/msmForceTorque/msmForceTorque.h | 2 +- .../msmForceTorque/msmForceTorque.rst | 6 +- .../reactionWheels/_Documentation/AVS.sty | 6 +- ...sk-REACTIONWHEELSTATEEFFECTOR-20170816.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../Basilisk-RWJitterBackSub-20161221.tex | 22 +- .../BasiliskReportMemo.cls | 1 - .../_Documentation/Support/RW_BOE.nb | 45 +- .../_Documentation/bibliography.bib | 4 +- .../_Documentation/secModelDescription.tex | 36 +- .../_Documentation/secModelFunctions.tex | 10 +- .../reactionWheels/_Documentation/secTest.tex | 28 +- .../_Documentation/secUserGuide.tex | 4 +- ...est_reactionWheelStateEffector_RWUpdate.py | 4 +- .../_Documentation/Spacecraft/AVS.sty | 6 +- .../Basilisk-SPACECRAFT-20170808.tex | 2 +- .../Spacecraft/BasiliskReportMemo.cls | 1 - .../PlanarFlexibleDynamicsDerivation.nb | 4431 +- .../Spacecraft/secModelDescription.tex | 12 +- .../Spacecraft/secModelFunctions.tex | 10 +- .../_Documentation/Spacecraft/secTest.tex | 24 +- .../dynamics/spacecraft/spacecraft.rst | 20 - .../_Documentation/SpacecraftSystem/AVS.sty | 6 +- .../Basilisk-SPACECRAFTSYSTEM-20180712.tex | 2 +- .../SpacecraftSystem/BasiliskReportMemo.cls | 1 - .../SpacecraftSystem/secModelDescription.tex | 44 +- .../SpacecraftSystem/secModelFunctions.tex | 2 +- .../SpacecraftSystem/secTest.tex | 4 +- .../spacecraftSystem/spacecraftSystem.rst | 5 +- .../sphericalPendulum/_Documentation/AVS.sty | 6 +- .../Basilisk-SPHERICALPENDULUM-20180518.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/references.bib | 2 +- .../_Documentation/secModelDescription.tex | 70 +- .../_Documentation/secModelFunctions.tex | 4 +- .../_Documentation/secTest.tex | 8 +- .../spinningBodyNDOFStateEffector.cpp | 2 +- .../test_spinningBodyOneDOFStateEffector.py | 2 +- .../spinningBodyOneDOFStateEffector.rst | 1 - .../test_spinningBodyTwoDOFStateEffector.py | 2 +- .../_Documentation/AVS.sty | 6 +- .../Basilisk-atmosphere-20190221.tex | 4 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleDescription.tex | 4 +- .../_Documentation/secModuleFunctions.tex | 4 +- .../_Documentation/secTest.tex | 51 +- .../_Documentation/secUserGuide.tex | 1 - .../MsisAtmosphere/_Documentation/AVS.sty | 6 +- .../Basilisk-atmosphere-20190221.tex | 4 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleFunctions.tex | 6 +- .../MsisAtmosphere/_Documentation/secTest.tex | 28 +- .../_Documentation/secUserGuide.tex | 4 +- .../MsisAtmosphere/_UnitTest/truthOutputs.txt | 2 +- .../MsisAtmosphere/msisAtmosphere.rst | 2 - .../environment/MsisAtmosphere/nrlmsise-00.c | 26 +- .../environment/MsisAtmosphere/nrlmsise-00.h | 26 +- .../MsisAtmosphere/nrlmsise-00_data.c | 15 +- .../TabularAtmosphere/tabularAtmosphere.h | 4 +- .../TabularAtmosphere/tabularAtmosphere.rst | 3 +- .../_GeneralModuleFiles/atmosphereBase.rst | 2 - src/simulation/environment/albedo/albedo.h | 2 +- .../_UnitTest/test_dentonFluxModel.py | 8 +- .../dentonFluxModel/dentonFluxModel.h | 8 +- .../dentonFluxModel/dentonFluxModel.rst | 6 +- .../_Documentation/Images/conical_shadow.svg | 2 +- .../_Documentation/Images/diskModel.svg | 2 +- src/simulation/environment/eclipse/eclipse.h | 4 +- .../ephemerisConverter/_Documentation/AVS.sty | 6 +- .../Basilisk-EPHEMERIS_CONVERTER20170712.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModelDescription.tex | 2 - .../_Documentation/secModelFunctions.tex | 2 +- .../_Documentation/secTest.tex | 3 +- .../_Documentation/secUserGuide.tex | 2 +- .../ephemerisConverter/ephemerisConverter.h | 2 +- .../groundLocation/groundLocation.h | 2 +- .../groundMapping/groundMapping.rst | 6 +- .../_Documentation/AVS.sty | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModuleDescription.tex | 12 +- .../_Documentation/secModuleFunctions.tex | 6 +- .../_Documentation/secTest.tex | 49 +- .../_Documentation/secUserGuide.tex | 9 +- .../_UnitTest/Support/magFieldModels.nb | 803 +- .../magneticFieldCenteredDipole.rst | 1 - .../magneticFieldWMM/GeomagnetismHeader.h | 18 +- .../magneticFieldWMM/GeomagnetismLibrary.c | 117 +- .../magneticFieldWMM/README-includingWMM.txt | 6 +- .../magneticFieldWMM/_Documentation/AVS.sty | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModuleDescription.tex | 8 +- .../_Documentation/secModuleFunctions.tex | 6 +- .../_Documentation/secTest.tex | 15 +- .../_Documentation/secUserGuide.tex | 8 +- .../magneticFieldWMM/magneticFieldWMM.rst | 1 - .../Support/planetOrientation.nb | 121 +- .../planetEphemeris/_Documentation/AVS.sty | 6 +- .../Basilisk-planetEphemeris-20190422.tex | 6 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModuleDescription.tex | 6 +- .../_Documentation/secModuleFunctions.tex | 2 +- .../_Documentation/secTest.tex | 55 +- .../planetEphemeris/planetEphemeris.rst | 1 - .../environment/solarFlux/solarFlux.h | 2 +- .../environment/solarFlux/solarFlux.rst | 1 - .../_UnitTest/Support/spacecraftLocation.nb | 431 +- .../spiceInterface/_Documentation/AVS.sty | 6 +- .../Basilisk-SPICE_INTERFACE20170712.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModelDescription.tex | 3 +- .../_Documentation/secModelFunctions.tex | 6 +- .../spiceInterface/_Documentation/secTest.tex | 27 +- .../pinholeCamera/pinholeCamera.rst | 2 +- .../navigation/planetHeading/planetHeading.h | 2 +- .../planetHeading/planetHeading.rst | 1 - .../navigation/planetNav/planetNav.rst | 8 +- .../simpleNav/_Documentation/AVS.sty | 6 +- .../Basilisk-SIMPLE_NAV20170712.tex | 4 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../simpleNav/_Documentation/bibliography.bib | 24 +- .../_Documentation/secModelDescription.tex | 54 +- .../_Documentation/secModelFunctions.tex | 12 +- .../simpleNav/_Documentation/secTest.tex | 66 +- .../simpleNav/_Documentation/secUserGuide.tex | 2 +- .../dataStorageUnitBase.rst | 2 +- src/simulation/onboardDataHandling/_doc.rst | 2 +- .../_UnitTest/test_mappingInstrument.py | 14 +- .../mappingInstrument/mappingInstrument.rst | 6 +- .../storageUnit/simpleStorageUnit.h | 2 - .../_Documentation/Images/PowerRW.svg | 2 +- .../power/_GeneralModuleFiles/powerNodeBase.h | 2 +- .../_GeneralModuleFiles/powerStorageBase.rst | 2 +- src/simulation/power/_doc.rst | 2 +- src/simulation/sensors/camera/camera.h | 4 +- src/simulation/sensors/camera/camera.rst | 2 +- .../coarseSunSensor/_Documentation/AVS.sty | 6 +- .../Basilisk-CoarseSunSensor-20170803.tex | 4 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/Support/notes.nb | 65 +- .../_Documentation/bibliography.bib | 24 +- .../_Documentation/secModelDescription.tex | 19 +- .../_Documentation/secModelFunctions.tex | 2 +- .../_Documentation/secTest.tex | 8 +- .../_Documentation/secUserGuide.tex | 29 +- .../_UnitTest/test_CSSConfig.py | 1 - .../test_hingedRigidBodyMotorSensor.py | 32 +- .../hingedRigidBodyMotorSensor.h | 4 +- .../hingedRigidBodyMotorSensor.rst | 6 +- .../sensors/imuSensor/_Documentation/AVS.sty | 6 +- .../_Documentation/Basilisk-IMU-20170712.tex | 4 +- .../_Documentation/BasiliskCorruptions.tex | 2 +- .../_Documentation/BasiliskReportMemo.cls | 1 - .../_Documentation/secModelDescription.tex | 2 +- .../_Documentation/secModelFunctions.tex | 8 +- .../imuSensor/_Documentation/secTest.tex | 10 +- .../imuSensor/_Documentation/secUserGuide.tex | 6 +- .../sensors/imuSensor/imuSensor.rst | 1 - .../sensors/simpleMassProps/simpleMassProps.h | 2 +- .../simpleVoltEstimator.rst | 2 +- .../starTracker/_Documentation/AVS.sty | 6 +- .../Basilisk-star_tracker-20161101.tex | 4 +- .../_Documentation/BasiliskReportMemo.cls | 1 - src/simulation/simSynch/simSynch.h | 6 +- src/simulation/simSynch/simSynch.rst | 1 - .../thermal/motorThermal/motorThermal.rst | 8 +- .../thermal/sensorThermal/sensorThermal.rst | 2 +- .../_UnitTest/test_dataFileToViz.py | 3 +- .../vizard/dataFileToViz/dataFileToViz.h | 6 +- .../vizard/dataFileToViz/dataFileToViz.rst | 1 - src/tests/test_bskPrinciples.py | 2 +- src/tests/test_deprecated.py | 4 +- src/tests/test_orbitalMotion.py | 2 +- src/tests/test_rbkRoutine.py | 2 +- src/tests/test_scenarioAlbedo.py | 1 - src/tests/test_scenarioAttGuideHyperbolic.py | 2 - src/tests/test_scenarioAttLocPoint.py | 3 - ...est_scenarioAttitudeConstrainedManeuver.py | 1 - ...est_scenarioAttitudeConstraintViolation.py | 3 +- src/tests/test_scenarioAttitudeFeedback.py | 1 - .../test_scenarioAttitudeFeedback2T_TH.py | 1 - ...t_scenarioAttitudeFeedback2T_stateEffTH.py | 1 - .../test_scenarioAttitudeFeedbackNoEarth.py | 1 - src/tests/test_scenarioAttitudeFeedbackRW.py | 1 - .../test_scenarioAttitudeFeedbackRWPower.py | 1 - src/tests/test_scenarioAttitudeGG.py | 1 - src/tests/test_scenarioAttitudeGuidance.py | 3 - src/tests/test_scenarioAttitudePointing.py | 1 - src/tests/test_scenarioAttitudePointingPy.py | 1 - src/tests/test_scenarioAttitudePrescribed.py | 3 - src/tests/test_scenarioAttitudeSteering.py | 1 - src/tests/test_scenarioBskLog.py | 1 - src/tests/test_scenarioCSS.py | 1 - src/tests/test_scenarioCSSFilters.py | 2 - src/tests/test_scenarioCentralBody.py | 1 - src/tests/test_scenarioCustomGravBody.py | 2 - src/tests/test_scenarioDataDemo.py | 2 - src/tests/test_scenarioDataToViz.py | 2 - src/tests/test_scenarioDebrisReorbitET.py | 2 +- src/tests/test_scenarioDragRendezvous.py | 1 - src/tests/test_scenarioDragSensitivity.py | 1 - src/tests/test_scenarioFormationBasic.py | 1 - src/tests/test_scenarioFormationReconfig.py | 2 +- src/tests/test_scenarioGroundMapping.py | 2 - src/tests/test_scenarioHingedRigidBody.py | 1 - src/tests/test_scenarioHohmann.py | 3 - src/tests/test_scenarioLagrangePointOrbit.py | 2 +- ...est_scenarioMagneticFieldCenteredDipole.py | 1 - src/tests/test_scenarioMomentumDumping.py | 1 - src/tests/test_scenarioOrbitMultiBody.py | 1 - src/tests/test_scenarioPowerDemo.py | 2 - src/tests/test_scenarioRotatingPanel.py | 1 - .../test_scenarioSatelliteConstellation.py | 1 - src/tests/test_scenarioSmallBodyLandmarks.py | 2 +- src/tests/test_scenarioSpacecraftLocation.py | 2 +- .../test_scenarioStationKeepingMultiSat.py | 2 +- src/tests/test_scenarioTAM.py | 1 - src/tests/test_scenarioTAMcomparison.py | 2 +- src/tests/test_scenarioVizPoint.py | 1 - src/utilities/SpherePlot.py | 1 - src/utilities/macros.py | 1 - src/utilities/simSetPlanetEnvironment.py | 2 - src/utilities/simulationProgessBar.py | 1 - .../AtmosphereData/NRLMSISE00Nominal.txt | 18 +- .../support/EarthGRAM_bsk_namelist.txt | 34 +- .../support/JupiterGRAM_bsk_namelist.txt | 70 +- .../support/MarsGRAM_bsk_namelist.txt | 40 +- .../support/NeptuneGRAM_bsk_namelist.txt | 113 +- supportData/AtmosphereData/support/README.txt | 54 +- .../support/TitanGRAM_bsk_namelist.txt | 115 +- .../support/UranusGRAM_bsk_namelist.txt | 89 +- .../support/VenusGRAM_bsk_namelist.txt | 107 +- supportData/EphemerisData/de-403-masses.tpc | 8 +- supportData/EphemerisData/naif0011.tls | 69 +- supportData/EphemerisData/naif0012.tls | 72 +- supportData/EphemerisData/pck00010.tpc | 2541 +- supportData/LocalGravData/GGM03S-J2-only.txt | 2 +- supportData/LocalGravData/GGM03S.txt | 2 +- supportData/LocalGravData/GGM2BData.txt | 6638 +- supportData/LocalGravData/VESTA20H.txt | 460 +- supportData/LocalGravData/eros007790.tab | 2 +- 974 files changed, 46580 insertions(+), 47389 deletions(-) diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index 8a8011c690..25776a96c8 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -7,11 +7,11 @@ commits organized? --> ## Verification - ## Documentation - ## Future work diff --git a/LICENSE b/LICENSE index 5653a30d00..3fe71b7873 100644 --- a/LICENSE +++ b/LICENSE @@ -14,4 +14,3 @@ 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. - diff --git a/docs/source/Install/buildExtModules.rst b/docs/source/Install/buildExtModules.rst index de49ae009f..48dd67ddd4 100644 --- a/docs/source/Install/buildExtModules.rst +++ b/docs/source/Install/buildExtModules.rst @@ -70,5 +70,3 @@ Frequently Asked Questions - The custom Basilisk modules are built into a ``Basilisk.ExternalModules`` package. For example, to load a module called ``customCppModule``, use:: from Basilisk.ExternalModules import customCppModule - - diff --git a/docs/source/Learn.rst b/docs/source/Learn.rst index 05f317eda6..a41c8f9d74 100644 --- a/docs/source/Learn.rst +++ b/docs/source/Learn.rst @@ -26,4 +26,3 @@ Finally, there is the source-code itself. That is always the ultimate guide to Learn/bskPrinciples examples/index Learn/makingModules - diff --git a/docs/source/Learn/bskPrinciples/bskPrinciples-0.rst b/docs/source/Learn/bskPrinciples/bskPrinciples-0.rst index 8083f98c5e..4b00ddc62c 100644 --- a/docs/source/Learn/bskPrinciples/bskPrinciples-0.rst +++ b/docs/source/Learn/bskPrinciples/bskPrinciples-0.rst @@ -43,4 +43,3 @@ Module Behavior Testing ----------------------- How do we know that the BSK modules are doing what they are supposed to? In each BSK module folder there is a ``_UnitTest`` folder which contains python scripts to test the functionality of the given module. This assures that each module is working as planned. The ``src/examples`` folder contains many integrated simulation scripts where the modules are assembled into sample spacecraft simulations. These scripts are also tested to ensure the integrated simulation is working. Thus, unit tests ensure a module is working individually, and integrated tests ensure that the module connections and interfaces are working properly. As a result of this integrated testing, developing and enhancing Basilisk can be done readily while being confident that other code has not been broken. - diff --git a/docs/source/Learn/bskPrinciples/bskPrinciples-1.rst b/docs/source/Learn/bskPrinciples/bskPrinciples-1.rst index 0c762169e8..91c347359e 100644 --- a/docs/source/Learn/bskPrinciples/bskPrinciples-1.rst +++ b/docs/source/Learn/bskPrinciples/bskPrinciples-1.rst @@ -67,5 +67,3 @@ Next, the simulation length is set through ``ConfigureStopTime(stopTime)`` where scSim.ExecuteSimulation() scSim.ConfigureStopTime(macros.sec2nano(10.0)) scSim.ExecuteSimulation() - - diff --git a/docs/source/Learn/bskPrinciples/bskPrinciples-2a.rst b/docs/source/Learn/bskPrinciples/bskPrinciples-2a.rst index 0768275ead..1d4ed25fa0 100644 --- a/docs/source/Learn/bskPrinciples/bskPrinciples-2a.rst +++ b/docs/source/Learn/bskPrinciples/bskPrinciples-2a.rst @@ -44,5 +44,3 @@ If you execute this python code you should see the following terminal output: 0.0 BSK_INFORMATION: C Module ID 1 ran Update at 0.000000s 1.0 - - diff --git a/docs/source/Learn/bskPrinciples/bskPrinciples-2b.rst b/docs/source/Learn/bskPrinciples/bskPrinciples-2b.rst index 39189602bf..4171fbc9e9 100644 --- a/docs/source/Learn/bskPrinciples/bskPrinciples-2b.rst +++ b/docs/source/Learn/bskPrinciples/bskPrinciples-2b.rst @@ -63,5 +63,3 @@ or saved off for future use. For example, adding this command to this sample sc .. image:: ../../_images/static/qs-bsk-2b-order.svg :align: center - - diff --git a/docs/source/Learn/bskPrinciples/bskPrinciples-7.rst b/docs/source/Learn/bskPrinciples/bskPrinciples-7.rst index 0df651f174..d1d2b1ecec 100644 --- a/docs/source/Learn/bskPrinciples/bskPrinciples-7.rst +++ b/docs/source/Learn/bskPrinciples/bskPrinciples-7.rst @@ -93,5 +93,3 @@ After executing the script you should see the following terminal output: [2.0, 0.0, 0.0] cppMsg: [2.0, 0.0, 0.0] - - diff --git a/docs/source/Learn/bskPrinciples/bskPrinciples-8.rst b/docs/source/Learn/bskPrinciples/bskPrinciples-8.rst index de08f8371d..fea8c7fe76 100644 --- a/docs/source/Learn/bskPrinciples/bskPrinciples-8.rst +++ b/docs/source/Learn/bskPrinciples/bskPrinciples-8.rst @@ -45,5 +45,3 @@ To disable a single task, this is done with the :ref:`SimulationBaseClass` metho BSK executed a single simulation step BSK_INFORMATION: C Module ID 1 ran Update at 4.000000s BSK executed a single simulation step - - diff --git a/docs/source/Learn/bskPrinciples/bskPrinciples-9.rst b/docs/source/Learn/bskPrinciples/bskPrinciples-9.rst index 97241a5863..e38840d723 100644 --- a/docs/source/Learn/bskPrinciples/bskPrinciples-9.rst +++ b/docs/source/Learn/bskPrinciples/bskPrinciples-9.rst @@ -32,4 +32,4 @@ at 1Hz as this method is called at the task update period. The integration type is determined by the integrator assigned to the primary ``DynamicObject`` to which the other ``DynamicObject`` integrations is synchronized. By default this is the ``RK4`` integrator. It doesn't matter what integrator is assigned to the secondary ``DynamicObject`` instances, -the integrator of the primary object is used. \ No newline at end of file +the integrator of the primary object is used. diff --git a/docs/source/Learn/makingModules.rst b/docs/source/Learn/makingModules.rst index b85e716a59..afc8c90265 100644 --- a/docs/source/Learn/makingModules.rst +++ b/docs/source/Learn/makingModules.rst @@ -32,5 +32,3 @@ The following pages cover the primary tasks required to make a Basilisk module. makingModules/makingDraftModule makingModules/makingModules-5 makingModules/advancedTopics - - diff --git a/docs/source/Learn/makingModules/advancedTopics/creatingDynObject.rst b/docs/source/Learn/makingModules/advancedTopics/creatingDynObject.rst index a6389f4f49..4f8ee5ba46 100644 --- a/docs/source/Learn/makingModules/advancedTopics/creatingDynObject.rst +++ b/docs/source/Learn/makingModules/advancedTopics/creatingDynObject.rst @@ -35,4 +35,3 @@ scenarios as with the spacecraft example. See the discussion in :ref:`bskPrinci Basilisk modules that inherit from the ``DynamicObject`` class can be linked. If linked, then the associated module ordinate differential equations (ODEs) are integrated simultaneously. - diff --git a/docs/source/Learn/makingModules/cModules.rst b/docs/source/Learn/makingModules/cModules.rst index 557f41ca6c..9a467cff38 100644 --- a/docs/source/Learn/makingModules/cModules.rst +++ b/docs/source/Learn/makingModules/cModules.rst @@ -13,5 +13,3 @@ This section covers how to write a C Basilisk Module. A sample C Basilisk modul cModules/cModules-3 cModules/cModules-4 cModules/cModules-5 - - diff --git a/docs/source/Learn/makingModules/cModules/cModules-1.rst b/docs/source/Learn/makingModules/cModules/cModules-1.rst index 744a07102b..167f02062a 100644 --- a/docs/source/Learn/makingModules/cModules/cModules-1.rst +++ b/docs/source/Learn/makingModules/cModules/cModules-1.rst @@ -83,4 +83,3 @@ For example, assume the module needs an array of input messages of type ``SomeMs The module needs to implement separate logic to determine how many messages have been set. For example, the reset function could loop over this array and up to what slot the associate message object has been linked. As the C wrapped message object can act as either input or output messages, the above example can readily be converted to an outpout message example by renaming the array variable ``moreOutMsgs``. - diff --git a/docs/source/Learn/makingModules/cModules/cModules-3.rst b/docs/source/Learn/makingModules/cModules/cModules-3.rst index 823700d529..e6537ca0d0 100644 --- a/docs/source/Learn/makingModules/cModules/cModules-3.rst +++ b/docs/source/Learn/makingModules/cModules/cModules-3.rst @@ -101,4 +101,3 @@ Assume the module has an array called ``moreInMsgs`` which contain input message .. code:: cpp inMsgBuffer = SomeMsg_C_read(&configData->moreInMsgs[3]); - diff --git a/docs/source/Learn/makingModules/cppModules.rst b/docs/source/Learn/makingModules/cppModules.rst index 98137c56d7..eca78b7435 100644 --- a/docs/source/Learn/makingModules/cppModules.rst +++ b/docs/source/Learn/makingModules/cppModules.rst @@ -16,5 +16,3 @@ This section covers how to write a C++ Basilisk Module. A sample C++ Basilisk m cppModules/cppModules-3 cppModules/cppModules-4 cppModules/cppModules-5 - - diff --git a/docs/source/Learn/makingModules/cppModules/cppModules-2.rst b/docs/source/Learn/makingModules/cppModules/cppModules-2.rst index acf33e41cf..f4787eebc2 100644 --- a/docs/source/Learn/makingModules/cppModules/cppModules-2.rst +++ b/docs/source/Learn/makingModules/cppModules/cppModules-2.rst @@ -86,4 +86,4 @@ The output message object has the following support methods: Returns an input message (i.e. read functor) that is able to read this output message. ``addAuthor()`` - Returns the message write functor, granting write access to this message object. \ No newline at end of file + Returns the message write functor, granting write access to this message object. diff --git a/docs/source/Learn/makingModules/makingModules-1.rst b/docs/source/Learn/makingModules/makingModules-1.rst index 51828ca4bb..73bbba7fe8 100644 --- a/docs/source/Learn/makingModules/makingModules-1.rst +++ b/docs/source/Learn/makingModules/makingModules-1.rst @@ -46,4 +46,4 @@ Further, this module is written to not only provide an eclipse output message fo .. image:: ../../_images/static/qs-mm-1-2.svg :align: center -To see how a C module handles a variable number of messages, see :ref:`navAggregate`. \ No newline at end of file +To see how a C module handles a variable number of messages, see :ref:`navAggregate`. diff --git a/docs/source/Learn/makingModules/makingModules-3.rst b/docs/source/Learn/makingModules/makingModules-3.rst index 91b3d4ee4f..2436a5c2ea 100644 --- a/docs/source/Learn/makingModules/makingModules-3.rst +++ b/docs/source/Learn/makingModules/makingModules-3.rst @@ -10,5 +10,3 @@ Each module should have a plain text RST file in the module folder, along with t Testing the Documentation ------------------------- To test the RST documentation, see :ref:`createHtmlDocumentation` on how to run ``sphinx`` to create the HTML documentation in ``basilisk/docs/build/html``. Note that building the full documentation can take a while. At the bottom of the ``basilisk.docs/source/conf.py`` file there are lines that determine what modules are included. Instead of including the entire source folder, comment out that line and uncomment another line that points just to the new module folder. This significantly speeds up the documentation build process. Naturally, don't commit this change to ``conf.py``, but only use it testing the formatting. Note that if only the single module documentation is built links to other modules etc. won't work. Be sure to build the entire BSK documentation to give this a final test and read through. - - diff --git a/docs/source/Learn/makingModules/makingModules-4.rst b/docs/source/Learn/makingModules/makingModules-4.rst index 0f813d899a..210a756c1c 100644 --- a/docs/source/Learn/makingModules/makingModules-4.rst +++ b/docs/source/Learn/makingModules/makingModules-4.rst @@ -5,4 +5,4 @@ Module Unit Test File Inside the module folder there should be a sub-folder called ``_UnitTest``. It needs to contain one or more python unit test scripts that check the functionality of the Basilisk module. Each unit test file name must start with ``test_xxxx.py`` such that ``pytest`` recognizes this python file as a test file. -See C module template :ref:`UnitTestcModuleTemplate` to find two sample unit test scripts for :ref:`cModuleTemplate`. Ideally only the module being tested should be loaded. Any input messages can be created from Python and connected to the module. \ No newline at end of file +See C module template :ref:`UnitTestcModuleTemplate` to find two sample unit test scripts for :ref:`cModuleTemplate`. Ideally only the module being tested should be loaded. Any input messages can be created from Python and connected to the module. diff --git a/docs/source/Learn/makingModules/pyModules.rst b/docs/source/Learn/makingModules/pyModules.rst index 1e4221bf18..d0c47ecc72 100644 --- a/docs/source/Learn/makingModules/pyModules.rst +++ b/docs/source/Learn/makingModules/pyModules.rst @@ -9,10 +9,10 @@ Making Python Modules Python modules are a good alternative to C and C++ modules for quick prototyping. They are defined entirely in a Python script, which means that there is no need -for a header (``.h``), definition (``.cpp``), or SWIG interface file (``.i``). However, they +for a header (``.h``), definition (``.cpp``), or SWIG interface file (``.i``). However, they are much slower than C or C++ modules, which will significantly slow down your simulation. -Python modules are implemented by subclassing ``SysModel`` from ``Basilisk.architecture.sysModel``. +Python modules are implemented by subclassing ``SysModel`` from ``Basilisk.architecture.sysModel``. Then, one can implement the ``__init__``, ``Reset``, and ``UpdateState`` methods in the same way that one would implement these methods in C++. Remember to always call ``__init__`` of diff --git a/docs/source/Support.rst b/docs/source/Support.rst index 22699d601a..6bd6853208 100644 --- a/docs/source/Support.rst +++ b/docs/source/Support.rst @@ -12,8 +12,3 @@ as how to code up C or C++ Basilisk modules. Support/bskKnownIssues Support/User Support/Developer - - - - - diff --git a/docs/source/Support/Developer/Debugging.rst b/docs/source/Support/Developer/Debugging.rst index ec3b84cb28..d462185d37 100644 --- a/docs/source/Support/Developer/Debugging.rst +++ b/docs/source/Support/Developer/Debugging.rst @@ -7,7 +7,7 @@ Accessing the Debugger for C/C++ Basilisk Modules Motivation ---------- -Debuggers are very powerful tools for any developer. While some problems can be diagnosed using temporary print statements throughout the code, this technique can be unnecessarily time consuming and cluttering -- requiring recompilation, iteration, and all-the-while dramatically increasing the likelihood of accidentally modifying code. Using debuggers allows the developer to carefully analyze control flow and access the program's memory in real time, allowing developers to solve problems more efficiently and in a more controlled manner. +Debuggers are very powerful tools for any developer. While some problems can be diagnosed using temporary print statements throughout the code, this technique can be unnecessarily time consuming and cluttering -- requiring recompilation, iteration, and all-the-while dramatically increasing the likelihood of accidentally modifying code. Using debuggers allows the developer to carefully analyze control flow and access the program's memory in real time, allowing developers to solve problems more efficiently and in a more controlled manner. When developing for Basilisk, it is particularly useful to know how to access a debugger at the C/C++ module level. One might assume that the Python debugger would be sufficient to access the call stack of a C/C++ module, because the C/C++ modules are wrapped into Python using SWIG. **This is incorrect.** Instead, developers need to invoke a C/C++ debugger like GDB either through the command line or a C/C++ compatible IDE to gain access to the module's stack. This support page provides the developer with the steps necessary to accomplish this. @@ -18,19 +18,19 @@ Before debugging, one must first configure Basilisk with the appropriate build t python3 conanfile.py --buildType Debug -This ensures that the correct conan dependencies will be pulled, and that the configured project will be set to ``Debug`` rather than ``Release`` by default. +This ensures that the correct conan dependencies will be pulled, and that the configured project will be set to ``Debug`` rather than ``Release`` by default. .. Note:: - When projects are configured to build in ``Release``, the compiler automatically applies optimizations to the machine code during the build. These optimizations effectively rewrite small algorithms, and such rewrites will not necessarily be reflected in the human-readable code. This is why using debuggers to step through programs that are compiled for Release/Profiling is unstable and may yield unintuitive control flow or unreadable memory. Configuring projects for ``Debug`` ensures that these optimizations are not applied, such that developers can be sure that there is a direct mapping between the human-readable code and the machine code being executed. + When projects are configured to build in ``Release``, the compiler automatically applies optimizations to the machine code during the build. These optimizations effectively rewrite small algorithms, and such rewrites will not necessarily be reflected in the human-readable code. This is why using debuggers to step through programs that are compiled for Release/Profiling is unstable and may yield unintuitive control flow or unreadable memory. Configuring projects for ``Debug`` ensures that these optimizations are not applied, such that developers can be sure that there is a direct mapping between the human-readable code and the machine code being executed. By default, the Basilisk project will also be built using the previous command. However, if the developer prefers to build the project manually, they should add ``--buildProject False`` to the previous command and remember to explicitly build for ``Debug``. More information on building the software framework can be read in :ref:`configureBuild`. How to catch breakpoints in a C/C++ module ------------------------------------------ -The first step to catching a breakpoint set in a C/C++ module is to add a breakpoint to the Python script that calls said module. The location of this breakpoint in the Python script is not +The first step to catching a breakpoint set in a C/C++ module is to add a breakpoint to the Python script that calls said module. The location of this breakpoint in the Python script is not important, as long as it is caught by the Python debugger before the C/C++ code in question is executed. After setting the Python breakpoint, run the script using the Python debugger (or in a debugging mode if using an IDE) and wait until the Python breakpoint is reached. -After that, the developer must attach the C/C++ debugger to the newly spawned Python process. This can be done within the developer's C/C++ IDE of choice or using GDB directly. This page focuses on the former case. Within the developer's C/C++ IDE, one must search for the option ``Attach to Process`` that is usually under the ``Debug`` tab on the IDE. A list of active processes will appear, and the developer can search for ``Python`` to find the current active Python processes. Multiple Python processes may be active, but only one is currently stopped at the breakpoint in the Python script. Identifying the correct process identifier (PID) can be difficult. Some IDEs report the PID in the first line of text output to the terminal. If this is not true for the developer's IDE, it is recommended to select the Python PID with the highest value. This generally corresponds to most recently spawned process which is often the process containing the relevant breakpoint. +After that, the developer must attach the C/C++ debugger to the newly spawned Python process. This can be done within the developer's C/C++ IDE of choice or using GDB directly. This page focuses on the former case. Within the developer's C/C++ IDE, one must search for the option ``Attach to Process`` that is usually under the ``Debug`` tab on the IDE. A list of active processes will appear, and the developer can search for ``Python`` to find the current active Python processes. Multiple Python processes may be active, but only one is currently stopped at the breakpoint in the Python script. Identifying the correct process identifier (PID) can be difficult. Some IDEs report the PID in the first line of text output to the terminal. If this is not true for the developer's IDE, it is recommended to select the Python PID with the highest value. This generally corresponds to most recently spawned process which is often the process containing the relevant breakpoint. -Once the C/C++ debugger is attached to the correct Python process, the developer can allow the Python debugger to ``continue``. The C/C++ debugger will then stop at the set breakpoint in the C/C++ Basilisk module and the developer can then begin using the C/C++ debugger in its entirety. \ No newline at end of file +Once the C/C++ debugger is attached to the correct Python process, the developer can allow the Python debugger to ``continue``. The C/C++ debugger will then stop at the set breakpoint in the C/C++ Basilisk module and the developer can then begin using the C/C++ debugger in its entirety. diff --git a/docs/source/Support/Developer/MigratingToPython3.rst b/docs/source/Support/Developer/MigratingToPython3.rst index f9fba2c354..b56114b5ba 100644 --- a/docs/source/Support/Developer/MigratingToPython3.rst +++ b/docs/source/Support/Developer/MigratingToPython3.rst @@ -27,7 +27,7 @@ use either of the following options which work in both version of Python:: a = 3//2 - a = 3./2 + a = 3./2 Without modification the user will see an error in Python 3 complaining about an unsupported type conversion:: diff --git a/docs/source/Support/Developer/UnderstandingBasilisk.rst b/docs/source/Support/Developer/UnderstandingBasilisk.rst index a641c41ffd..e2dd0c0e18 100644 --- a/docs/source/Support/Developer/UnderstandingBasilisk.rst +++ b/docs/source/Support/Developer/UnderstandingBasilisk.rst @@ -11,4 +11,3 @@ the Basilisk simulation architecture. All these resources are always - :download:`Understanding the Basilisk Architecture ` - diff --git a/docs/source/Support/User/migratingToBsk2.rst b/docs/source/Support/User/migratingToBsk2.rst index 27047e41b0..a62f0dc4a5 100644 --- a/docs/source/Support/User/migratingToBsk2.rst +++ b/docs/source/Support/User/migratingToBsk2.rst @@ -59,7 +59,7 @@ Module and Message Naming Changes Some early Basilisk modules and message names never complied with the naming guidelines in :ref:`codingGuidelines`. The following list outlines any module or message naming changes that occurred in this upgrade process. That is, message naming is listed if it is outside the standard adjusted (see :ref:`codingGuidelines`) where -``descriptionOutMsgName`` becomes ``descriptionOutMsg``. +``descriptionOutMsgName`` becomes ``descriptionOutMsg``. This list makes it simple to see what naming will need to be changed. Further, some modules have now a new name. For example, the module ``spacecraftPlus`` is now called simply ``spacecraft``. These module name changes are also listed in this table below. diff --git a/docs/source/_images/static/Simulation_container_layout_BasicOrbitFormation.svg b/docs/source/_images/static/Simulation_container_layout_BasicOrbitFormation.svg index 0a05abd48c..9bf8e01799 100644 --- a/docs/source/_images/static/Simulation_container_layout_BasicOrbitFormation.svg +++ b/docs/source/_images/static/Simulation_container_layout_BasicOrbitFormation.svg @@ -28,7 +28,7 @@ - Produced by OmniGraffle 7.9.4 + Produced by OmniGraffle 7.9.4 2019-01-08 22:01:00 +0000 diff --git a/docs/source/_images/static/_Support/test_scenarioBasicOrbit.nb b/docs/source/_images/static/_Support/test_scenarioBasicOrbit.nb index c8bcdd4d6a..74af4e6fcf 100644 --- a/docs/source/_images/static/_Support/test_scenarioBasicOrbit.nb +++ b/docs/source/_images/static/_Support/test_scenarioBasicOrbit.nb @@ -29,7 +29,7 @@ Cell[BoxData[ CellChangeTimes->{{3.690912522719018*^9, 3.690912536677989*^9}}], Cell[BoxData["398600.4415`"], "Output", - CellChangeTimes->{{3.690912537694816*^9, 3.690912541709106*^9}, + CellChangeTimes->{{3.690912537694816*^9, 3.690912541709106*^9}, 3.692706732780223*^9}] }, Open ]], @@ -50,145 +50,145 @@ Cell["Exact Solution LEO", "Section", Cell[BoxData[{ RowBox[{ - RowBox[{"sma", " ", "=", " ", "7000.00000`"}], ";"}], "\[IndentingNewLine]", + RowBox[{"sma", " ", "=", " ", "7000.00000`"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"ecc", " ", "=", " ", "0.000100000`"}], - ";"}], "\[IndentingNewLine]", + RowBox[{"ecc", " ", "=", " ", "0.000100000`"}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"inc", " ", "=", " ", - RowBox[{"33.300000`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"inc", " ", "=", " ", + RowBox[{"33.300000`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[CapitalOmega]", " ", "=", " ", - RowBox[{"48.2`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[CapitalOmega]", " ", "=", " ", + RowBox[{"48.2`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Omega]", " ", "=", " ", - RowBox[{"347.8`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[Omega]", " ", "=", " ", + RowBox[{"347.8`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"f0", " ", "=", " ", + RowBox[{"f0", " ", "=", " ", RowBox[{"85.3`", " ", "Degree"}]}], ";"}]}], "Input", - CellChangeTimes->{{3.690656456092209*^9, 3.6906565042546587`*^9}, + CellChangeTimes->{{3.690656456092209*^9, 3.6906565042546587`*^9}, 3.690656534585354*^9, {3.690657603205*^9, 3.690657637810052*^9}}], Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ - RowBox[{"{", - RowBox[{"r0", ",", "v0"}], "}"}], "=", - RowBox[{"elem2rv", "[", + RowBox[{"{", + RowBox[{"r0", ",", "v0"}], "}"}], "=", + RowBox[{"elem2rv", "[", RowBox[{ - "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", ",", + "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", ",", "\[Omega]", ",", "f0"}], "]"}]}]], "Input", CellChangeTimes->{{3.690656073578787*^9, 3.6906561159828*^9}, { - 3.6906563399393578`*^9, 3.6906563468782997`*^9}, {3.6906565090493793`*^9, + 3.6906563399393578`*^9, 3.6906563468782997`*^9}, {3.6906565090493793`*^9, 3.690656536914942*^9}, {3.690657859458934*^9, 3.690657861202614*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "2816.8016010234915`"}], ",", "5248.174846916147`", ",", - "3677.157264677297`"}], "}"}], ",", - RowBox[{"{", + RowBox[{"-", "2816.8016010234915`"}], ",", "5248.174846916147`", ",", + "3677.157264677297`"}], "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", "6.179638215820373`"}], ",", - RowBox[{"-", "4.1598620701988285`"}], ",", "1.2047711796182983`"}], + RowBox[{"-", "6.179638215820373`"}], ",", + RowBox[{"-", "4.1598620701988285`"}], ",", "1.2047711796182983`"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{{3.690656082426065*^9, 3.6906561164937077`*^9}, + CellChangeTimes->{{3.690656082426065*^9, 3.6906561164937077`*^9}, 3.6906563475089693`*^9, {3.6906565159496603`*^9, 3.690656539328861*^9}, { - 3.690657618700452*^9, 3.690657639605751*^9}, 3.690657861755019*^9, - 3.690799670913288*^9, 3.6908146123428097`*^9, 3.690912541774909*^9, + 3.690657618700452*^9, 3.690657639605751*^9}, 3.690657861755019*^9, + 3.690799670913288*^9, 3.6908146123428097`*^9, 3.690912541774909*^9, 3.692706750961833*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"n", " ", "=", " ", - RowBox[{"Sqrt", "[", - RowBox[{"\[Mu]", "/", + RowBox[{"n", " ", "=", " ", + RowBox[{"Sqrt", "[", + RowBox[{"\[Mu]", "/", RowBox[{"sma", "^", "3"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.690656519316794*^9, 3.6906565259676027`*^9}}], Cell[BoxData["0.0010780076124668337`"], "Output", - CellChangeTimes->{3.6906565263127747`*^9, 3.690657621044787*^9, - 3.6907996709408913`*^9, 3.690814612956794*^9, 3.690912541812231*^9, + CellChangeTimes->{3.6906565263127747`*^9, 3.690657621044787*^9, + 3.6907996709408913`*^9, 3.690814612956794*^9, 3.690912541812231*^9, 3.692706750988435*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"P", " ", "=", " ", - RowBox[{"2", " ", + RowBox[{"P", " ", "=", " ", + RowBox[{"2", " ", RowBox[{"\[Pi]", "/", "n"}]}]}]], "Input", CellChangeTimes->{{3.690656631821364*^9, 3.6906566347895193`*^9}}], Cell[BoxData["5828.516639879384`"], "Output", - CellChangeTimes->{3.690656635194646*^9, 3.690657621722246*^9, - 3.690799670994615*^9, 3.6908146135075207`*^9, 3.6909125418584747`*^9, + CellChangeTimes->{3.690656635194646*^9, 3.690657621722246*^9, + 3.690799670994615*^9, 3.6908146135075207`*^9, 3.6909125418584747`*^9, 3.692706751022718*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"T", " ", "=", " ", + RowBox[{"T", " ", "=", " ", RowBox[{"P", " ", "0.75"}]}]], "Input", CellChangeTimes->{{3.690656641373308*^9, 3.690656645262651*^9}}], Cell[BoxData["4371.387479909537`"], "Output", - CellChangeTimes->{3.690656645877283*^9, 3.690657622249775*^9, - 3.690799671037856*^9, 3.690814614125465*^9, 3.690912541891884*^9, + CellChangeTimes->{3.690656645877283*^9, 3.690657622249775*^9, + 3.690799671037856*^9, 3.690814614125465*^9, 3.690912541891884*^9, 3.692706751056263*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"M0", " ", "=", " ", - RowBox[{"E2M", "[", + RowBox[{"M0", " ", "=", " ", + RowBox[{"E2M", "[", RowBox[{ - RowBox[{"f2E", "[", + RowBox[{"f2E", "[", RowBox[{"f0", ",", "ecc"}], "]"}], ",", "ecc"}], "]"}]}]], "Input", CellChangeTimes->{{3.690656542247233*^9, 3.690656581869342*^9}}], Cell[BoxData["1.4885665256990677`"], "Output", - CellChangeTimes->{{3.69065656828483*^9, 3.6906565823889637`*^9}, - 3.690657622773707*^9, 3.6907996710773687`*^9, 3.690814614765023*^9, + CellChangeTimes->{{3.69065656828483*^9, 3.6906565823889637`*^9}, + 3.690657622773707*^9, 3.6907996710773687`*^9, 3.690814614765023*^9, 3.690912541940442*^9, 3.692706751089246*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"fList", " ", "=", " ", - RowBox[{"Table", "[", "\[IndentingNewLine]", + RowBox[{"fList", " ", "=", " ", + RowBox[{"Table", "[", "\[IndentingNewLine]", RowBox[{ - RowBox[{"E2f", "[", + RowBox[{"E2f", "[", RowBox[{ - RowBox[{"M2E", "[", + RowBox[{"M2E", "[", RowBox[{ - RowBox[{"M0", " ", "+", " ", - RowBox[{"n", "*", "i", " ", "1100"}]}], ",", "ecc"}], "]"}], ",", - "ecc"}], "]"}], "\[IndentingNewLine]", ",", - RowBox[{"{", + RowBox[{"M0", " ", "+", " ", + RowBox[{"n", "*", "i", " ", "1100"}]}], ",", "ecc"}], "]"}], ",", + "ecc"}], "]"}], "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "4"}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.690656590261936*^9, 3.690656600791767*^9}, { - 3.690656650886994*^9, 3.690656717545055*^9}, {3.690657439249593*^9, - 3.690657439528192*^9}, 3.690657550847658*^9, {3.690814794411097*^9, + 3.690656650886994*^9, 3.690656717545055*^9}, {3.690657439249593*^9, + 3.690657439528192*^9}, 3.690657550847658*^9, {3.690814794411097*^9, 3.690814794457123*^9}, 3.6908155343813*^9}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - "1.4887658519511633`", ",", "2.6744649701549554`", ",", - "3.8600516206265216`", ",", "5.045802665347993`", ",", + "1.4887658519511633`", ",", "2.6744649701549554`", ",", + "3.8600516206265216`", ",", "5.045802665347993`", ",", "6.231789746735361`"}], "}"}]], "Output", - CellChangeTimes->{3.690656718717534*^9, 3.690657440625499*^9, - 3.6906575515552588`*^9, 3.690657623298479*^9, 3.6907996711281147`*^9, - 3.690814615776546*^9, 3.690814795347313*^9, 3.690912541988936*^9, + CellChangeTimes->{3.690656718717534*^9, 3.690657440625499*^9, + 3.6906575515552588`*^9, 3.690657623298479*^9, 3.6907996711281147`*^9, + 3.690814615776546*^9, 3.690814795347313*^9, 3.690912541988936*^9, 3.692706751122754*^9}] }, Open ]], @@ -196,27 +196,27 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"ans", " ", "=", " ", - RowBox[{"Table", "[", "\[IndentingNewLine]", + RowBox[{"ans", " ", "=", " ", + RowBox[{"Table", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"elem2rv", "[", + RowBox[{"elem2rv", "[", RowBox[{ - "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", - ",", "\[Omega]", ",", - RowBox[{"fList", "[", - RowBox[{"[", "i", "]"}], "]"}]}], "]"}], "[", - RowBox[{"[", "1", "]"}], "]"}], "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"i", ",", - RowBox[{"Length", "[", "fList", "]"}]}], "}"}]}], "]"}]}], - ";"}], "\[IndentingNewLine]", - RowBox[{"CForm", "[", + "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", + ",", "\[Omega]", ",", + RowBox[{"fList", "[", + RowBox[{"[", "i", "]"}], "]"}]}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}], "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"i", ",", + RowBox[{"Length", "[", "fList", "]"}]}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{"CForm", "[", RowBox[{"ans", " ", "*", "1000"}], "]"}]}], "Input", CellChangeTimes->{{3.690656741674877*^9, 3.690656826017735*^9}, { - 3.69065757064723*^9, 3.690657585014105*^9}, {3.690657630642823*^9, + 3.69065757064723*^9, 3.690657585014105*^9}, {3.690657630642823*^9, 3.6906576309910793`*^9}, {3.690657684072776*^9, 3.690657687321257*^9}, { - 3.690657730420833*^9, 3.690657734503971*^9}, {3.690657778133027*^9, + 3.690657730420833*^9, 3.690657734503971*^9}, {3.690657778133027*^9, 3.6906577887291527`*^9}, {3.69081481160341*^9, 3.6908148340345707`*^9}}], Cell["\<\ @@ -226,11 +226,11 @@ List(List(-2.8168016010234966e6,5.248174846916143e6,3.6771572646772987e6), List(4.890526131271289e6,-3.2440700705588777e6,-3.815174368497354e6), List(5.642601923783563e6,4.017875774686192e6,-1.00395777577729e6))\ \>", "Output", - CellChangeTimes->{{3.690656797978344*^9, 3.6906568264437532`*^9}, + CellChangeTimes->{{3.690656797978344*^9, 3.6906568264437532`*^9}, 3.690657443730393*^9, {3.690657554291491*^9, 3.690657585743434*^9}, { - 3.690657624135494*^9, 3.6906576314761066`*^9}, 3.69065768777134*^9, - 3.690657734922233*^9, {3.690657783707725*^9, 3.6906577891309757`*^9}, - 3.690799671174663*^9, 3.690814617139358*^9, {3.6908147975882463`*^9, + 3.690657624135494*^9, 3.6906576314761066`*^9}, 3.69065768777134*^9, + 3.690657734922233*^9, {3.690657783707725*^9, 3.6906577891309757`*^9}, + 3.690799671174663*^9, 3.690814617139358*^9, {3.6908147975882463`*^9, 3.690814834482308*^9}, 3.690912542040049*^9, 3.692706751154957*^9}] }, Open ]] }, Open ]], @@ -239,36 +239,36 @@ Cell[CellGroupData[{ Cell["Exact Solution GTO", "Section", CellChangeTimes->{{3.690657839694964*^9, 3.690657841895734*^9}, { - 3.690814677128963*^9, 3.690814678443512*^9}, {3.6908150283123283`*^9, + 3.690814677128963*^9, 3.690814678443512*^9}, {3.6908150283123283`*^9, 3.6908150313598537`*^9}, {3.69081565114151*^9, 3.6908156515241013`*^9}}], Cell[BoxData[{ RowBox[{ - RowBox[{"rLEO", " ", "=", " ", "7000.00000`"}], - ";"}], "\[IndentingNewLine]", + RowBox[{"rLEO", " ", "=", " ", "7000.00000`"}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"sma", " ", "=", + RowBox[{"sma", " ", "=", RowBox[{ - RowBox[{"(", " ", - RowBox[{"rLEO", " ", "+", " ", "42000.0`"}], ")"}], "/", "2"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"(", " ", + RowBox[{"rLEO", " ", "+", " ", "42000.0`"}], ")"}], "/", "2"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"ecc", " ", "=", " ", - RowBox[{"1.0", " ", "-", " ", - RowBox[{"rLEO", "/", "sma"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"ecc", " ", "=", " ", + RowBox[{"1.0", " ", "-", " ", + RowBox[{"rLEO", "/", "sma"}]}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"inc", " ", "=", " ", - RowBox[{"0.00000`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"inc", " ", "=", " ", + RowBox[{"0.00000`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[CapitalOmega]", " ", "=", " ", - RowBox[{"48.2`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[CapitalOmega]", " ", "=", " ", + RowBox[{"48.2`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Omega]", " ", "=", " ", - RowBox[{"347.8`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[Omega]", " ", "=", " ", + RowBox[{"347.8`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"f0", " ", "=", " ", + RowBox[{"f0", " ", "=", " ", RowBox[{"85.3`", " ", "Degree"}]}], ";"}]}], "Input", - CellChangeTimes->{{3.690656456092209*^9, 3.6906565042546587`*^9}, + CellChangeTimes->{{3.690656456092209*^9, 3.6906565042546587`*^9}, 3.690656534585354*^9, {3.690657603205*^9, 3.690657637810052*^9}, { 3.6908150389764442`*^9, 3.6908151001220293`*^9}}], @@ -276,123 +276,123 @@ Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ - RowBox[{"{", - RowBox[{"r0", ",", "v0"}], "}"}], "=", - RowBox[{"elem2rv", "[", + RowBox[{"{", + RowBox[{"r0", ",", "v0"}], "}"}], "=", + RowBox[{"elem2rv", "[", RowBox[{ - "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", ",", + "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", ",", "\[Omega]", ",", "f0"}], "]"}]}]], "Input", CellChangeTimes->{{3.690656073578787*^9, 3.6906561159828*^9}, { - 3.6906563399393578`*^9, 3.6906563468782997`*^9}, {3.6906565090493793`*^9, + 3.6906563399393578`*^9, 3.6906563468782997`*^9}, {3.6906565090493793`*^9, 3.690656536914942*^9}, {3.690657859458934*^9, 3.690657861202614*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "5889.529848066479`"}], ",", "9686.57489000767`", ",", - "0.`"}], "}"}], ",", - RowBox[{"{", + RowBox[{"-", "5889.529848066479`"}], ",", "9686.57489000767`", ",", + "0.`"}], "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", "7.344323553471102`"}], ",", "0.3362949834858043`", ",", + RowBox[{"-", "7.344323553471102`"}], ",", "0.3362949834858043`", ",", "0.`"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{{3.690656082426065*^9, 3.6906561164937077`*^9}, + CellChangeTimes->{{3.690656082426065*^9, 3.6906561164937077`*^9}, 3.6906563475089693`*^9, {3.6906565159496603`*^9, 3.690656539328861*^9}, { - 3.690657618700452*^9, 3.690657639605751*^9}, 3.690657861755019*^9, - 3.690799670913288*^9, 3.6908146123428097`*^9, 3.690815105765272*^9, + 3.690657618700452*^9, 3.690657639605751*^9}, 3.690657861755019*^9, + 3.690799670913288*^9, 3.6908146123428097`*^9, 3.690815105765272*^9, 3.690815372474331*^9, 3.690912542107685*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"n", " ", "=", " ", - RowBox[{"Sqrt", "[", - RowBox[{"\[Mu]", "/", + RowBox[{"n", " ", "=", " ", + RowBox[{"Sqrt", "[", + RowBox[{"\[Mu]", "/", RowBox[{"sma", "^", "3"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.690656519316794*^9, 3.6906565259676027`*^9}}], Cell[BoxData["0.00016463408759939056`"], "Output", - CellChangeTimes->{3.6906565263127747`*^9, 3.690657621044787*^9, - 3.6907996709408913`*^9, 3.690814612956794*^9, 3.690815106436891*^9, + CellChangeTimes->{3.6906565263127747`*^9, 3.690657621044787*^9, + 3.6907996709408913`*^9, 3.690814612956794*^9, 3.690815106436891*^9, 3.6908153725108767`*^9, 3.690912542139209*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"P", " ", "=", " ", - RowBox[{"2", " ", + RowBox[{"P", " ", "=", " ", + RowBox[{"2", " ", RowBox[{"\[Pi]", "/", "n"}]}]}]], "Input", CellChangeTimes->{{3.690656631821364*^9, 3.6906566347895193`*^9}}], Cell[BoxData["38164.54659419417`"], "Output", - CellChangeTimes->{3.690656635194646*^9, 3.690657621722246*^9, - 3.690799670994615*^9, 3.6908146135075207`*^9, 3.690815107105021*^9, + CellChangeTimes->{3.690656635194646*^9, 3.690657621722246*^9, + 3.690799670994615*^9, 3.6908146135075207`*^9, 3.690815107105021*^9, 3.690815372544787*^9, 3.690912542188995*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"T", " ", "=", " ", + RowBox[{"T", " ", "=", " ", RowBox[{"P", " ", "0.75"}]}]], "Input", CellChangeTimes->{{3.690656641373308*^9, 3.690656645262651*^9}}], Cell[BoxData["28623.40994564563`"], "Output", - CellChangeTimes->{3.690656645877283*^9, 3.690657622249775*^9, - 3.690799671037856*^9, 3.690814614125465*^9, 3.69081510782698*^9, + CellChangeTimes->{3.690656645877283*^9, 3.690657622249775*^9, + 3.690799671037856*^9, 3.690814614125465*^9, 3.69081510782698*^9, 3.690815372577879*^9, 3.6909125422399263`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"M0", " ", "=", " ", - RowBox[{"E2M", "[", + RowBox[{"M0", " ", "=", " ", + RowBox[{"E2M", "[", RowBox[{ - RowBox[{"f2E", "[", + RowBox[{"f2E", "[", RowBox[{"f0", ",", "ecc"}], "]"}], ",", "ecc"}], "]"}]}]], "Input", CellChangeTimes->{{3.690656542247233*^9, 3.690656581869342*^9}}], Cell[BoxData["0.24873420666836954`"], "Output", - CellChangeTimes->{{3.69065656828483*^9, 3.6906565823889637`*^9}, - 3.690657622773707*^9, 3.6907996710773687`*^9, 3.690814614765023*^9, + CellChangeTimes->{{3.69065656828483*^9, 3.6906565823889637`*^9}, + 3.690657622773707*^9, 3.6907996710773687`*^9, 3.690814614765023*^9, 3.69081510922223*^9, 3.690815372611536*^9, 3.690912542286748*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"fList", " ", "=", " ", - RowBox[{"Table", "[", "\[IndentingNewLine]", + RowBox[{"fList", " ", "=", " ", + RowBox[{"Table", "[", "\[IndentingNewLine]", RowBox[{ - RowBox[{"E2f", "[", + RowBox[{"E2f", "[", RowBox[{ - RowBox[{"M2E", "[", + RowBox[{"M2E", "[", RowBox[{ - RowBox[{"M0", " ", "+", " ", - RowBox[{"n", "*", "i", " ", "6960"}]}], ",", "ecc"}], "]"}], ",", - "ecc"}], "]"}], "\[IndentingNewLine]", ",", - RowBox[{"{", + RowBox[{"M0", " ", "+", " ", + RowBox[{"n", "*", "i", " ", "6960"}]}], ",", "ecc"}], "]"}], ",", + "ecc"}], "]"}], "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "4"}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.690656590261936*^9, 3.690656600791767*^9}, { - 3.690656650886994*^9, 3.690656717545055*^9}, {3.690657439249593*^9, - 3.690657439528192*^9}, 3.690657550847658*^9, {3.690814794411097*^9, + 3.690656650886994*^9, 3.690656717545055*^9}, {3.690657439249593*^9, + 3.690657439528192*^9}, 3.690657550847658*^9, {3.690814794411097*^9, 3.690814794457123*^9}, {3.690815172394753*^9, 3.69081517559921*^9}, { 3.690815510440259*^9, 3.690815513532783*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ "1.488765851951163`", ",", "2.6468929098073315`", ",", "2.995917836308651`", ",", "3.273170990547786`", ",", "3.6140378582600867`"}], "}"}]], "Output",\ - CellChangeTimes->{3.690656718717534*^9, 3.690657440625499*^9, - 3.6906575515552588`*^9, 3.690657623298479*^9, 3.6907996711281147`*^9, - 3.690814615776546*^9, 3.690814795347313*^9, 3.6908151108192244`*^9, - 3.690815176508151*^9, 3.690815372649382*^9, 3.69081551431544*^9, + CellChangeTimes->{3.690656718717534*^9, 3.690657440625499*^9, + 3.6906575515552588`*^9, 3.690657623298479*^9, 3.6907996711281147`*^9, + 3.690814615776546*^9, 3.690814795347313*^9, 3.6908151108192244`*^9, + 3.690815176508151*^9, 3.690815372649382*^9, 3.69081551431544*^9, 3.6909125423394413`*^9}] }, Open ]], @@ -400,27 +400,27 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"ans", " ", "=", " ", - RowBox[{"Table", "[", "\[IndentingNewLine]", + RowBox[{"ans", " ", "=", " ", + RowBox[{"Table", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"elem2rv", "[", + RowBox[{"elem2rv", "[", RowBox[{ - "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", - ",", "\[Omega]", ",", - RowBox[{"fList", "[", - RowBox[{"[", "i", "]"}], "]"}]}], "]"}], "[", - RowBox[{"[", "1", "]"}], "]"}], "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"i", ",", - RowBox[{"Length", "[", "fList", "]"}]}], "}"}]}], "]"}]}], - ";"}], "\[IndentingNewLine]", - RowBox[{"CForm", "[", + "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", + ",", "\[Omega]", ",", + RowBox[{"fList", "[", + RowBox[{"[", "i", "]"}], "]"}]}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}], "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"i", ",", + RowBox[{"Length", "[", "fList", "]"}]}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{"CForm", "[", RowBox[{"ans", " ", "*", "1000"}], "]"}]}], "Input", CellChangeTimes->{{3.690656741674877*^9, 3.690656826017735*^9}, { - 3.69065757064723*^9, 3.690657585014105*^9}, {3.690657630642823*^9, + 3.69065757064723*^9, 3.690657585014105*^9}, {3.690657630642823*^9, 3.6906576309910793`*^9}, {3.690657684072776*^9, 3.690657687321257*^9}, { - 3.690657730420833*^9, 3.690657734503971*^9}, {3.690657778133027*^9, + 3.690657730420833*^9, 3.690657734503971*^9}, {3.690657778133027*^9, 3.6906577887291527`*^9}, {3.69081481160341*^9, 3.6908148340345707`*^9}}], Cell["\<\ @@ -430,12 +430,12 @@ List(List(-5.889529848066479e6,9.686574890007671e6,0.), List(-2.9802077401931673e7,-2.831957848900475e7,0.), List(-1.4932981196798025e7,-2.939523308237971e7,0.))\ \>", "Output", - CellChangeTimes->{{3.690656797978344*^9, 3.6906568264437532`*^9}, + CellChangeTimes->{{3.690656797978344*^9, 3.6906568264437532`*^9}, 3.690657443730393*^9, {3.690657554291491*^9, 3.690657585743434*^9}, { - 3.690657624135494*^9, 3.6906576314761066`*^9}, 3.69065768777134*^9, - 3.690657734922233*^9, {3.690657783707725*^9, 3.6906577891309757`*^9}, - 3.690799671174663*^9, 3.690814617139358*^9, {3.6908147975882463`*^9, - 3.690814834482308*^9}, 3.6908151129477882`*^9, 3.690815178108506*^9, + 3.690657624135494*^9, 3.6906576314761066`*^9}, 3.69065768777134*^9, + 3.690657734922233*^9, {3.690657783707725*^9, 3.6906577891309757`*^9}, + 3.690799671174663*^9, 3.690814617139358*^9, {3.6908147975882463`*^9, + 3.690814834482308*^9}, 3.6908151129477882`*^9, 3.690815178108506*^9, 3.690815372681299*^9, 3.6908155164030323`*^9, 3.690912542389656*^9}] }, Open ]] }, Open ]], @@ -444,154 +444,154 @@ Cell[CellGroupData[{ Cell["Exact Solution GEO", "Section", CellChangeTimes->{{3.690657839694964*^9, 3.690657841895734*^9}, { - 3.690814677128963*^9, 3.690814678443512*^9}, {3.6908150283123283`*^9, + 3.690814677128963*^9, 3.690814678443512*^9}, {3.6908150283123283`*^9, 3.6908150313598537`*^9}, {3.6908156492541428`*^9, 3.690815649826756*^9}}], Cell[BoxData[{ RowBox[{ - RowBox[{"sma", " ", "=", "42000.0`"}], ";"}], "\[IndentingNewLine]", + RowBox[{"sma", " ", "=", "42000.0`"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"ecc", " ", "=", " ", "0.00001"}], ";"}], "\[IndentingNewLine]", + RowBox[{"ecc", " ", "=", " ", "0.00001"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"inc", " ", "=", " ", - RowBox[{"0.00000`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"inc", " ", "=", " ", + RowBox[{"0.00000`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[CapitalOmega]", " ", "=", " ", - RowBox[{"48.2`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[CapitalOmega]", " ", "=", " ", + RowBox[{"48.2`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Omega]", " ", "=", " ", - RowBox[{"347.8`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[Omega]", " ", "=", " ", + RowBox[{"347.8`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"f0", " ", "=", " ", + RowBox[{"f0", " ", "=", " ", RowBox[{"85.3`", " ", "Degree"}]}], ";"}]}], "Input", - CellChangeTimes->{{3.690656456092209*^9, 3.6906565042546587`*^9}, + CellChangeTimes->{{3.690656456092209*^9, 3.6906565042546587`*^9}, 3.690656534585354*^9, {3.690657603205*^9, 3.690657637810052*^9}, { - 3.6908150389764442`*^9, 3.6908151001220293`*^9}, {3.690815669593705*^9, + 3.6908150389764442`*^9, 3.6908151001220293`*^9}, {3.690815669593705*^9, 3.690815687687243*^9}}], Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ - RowBox[{"{", - RowBox[{"r0", ",", "v0"}], "}"}], "=", - RowBox[{"elem2rv", "[", + RowBox[{"{", + RowBox[{"r0", ",", "v0"}], "}"}], "=", + RowBox[{"elem2rv", "[", RowBox[{ - "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", ",", + "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", ",", "\[Omega]", ",", "f0"}], "]"}]}]], "Input", CellChangeTimes->{{3.690656073578787*^9, 3.6906561159828*^9}, { - 3.6906563399393578`*^9, 3.6906563468782997`*^9}, {3.6906565090493793`*^9, + 3.6906563399393578`*^9, 3.6906563468782997`*^9}, {3.6906565090493793`*^9, 3.690656536914942*^9}, {3.690657859458934*^9, 3.690657861202614*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "21819.784817951124`"}], ",", "35887.24145651876`", ",", - "0.`"}], "}"}], ",", - RowBox[{"{", + RowBox[{"-", "21819.784817951124`"}], ",", "35887.24145651876`", ",", + "0.`"}], "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", "2.6323181135444305`"}], ",", - RowBox[{"-", "1.6004385668033225`"}], ",", "0.`"}], "}"}]}], + RowBox[{"-", "2.6323181135444305`"}], ",", + RowBox[{"-", "1.6004385668033225`"}], ",", "0.`"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{{3.690656082426065*^9, 3.6906561164937077`*^9}, + CellChangeTimes->{{3.690656082426065*^9, 3.6906561164937077`*^9}, 3.6906563475089693`*^9, {3.6906565159496603`*^9, 3.690656539328861*^9}, { - 3.690657618700452*^9, 3.690657639605751*^9}, 3.690657861755019*^9, - 3.690799670913288*^9, 3.6908146123428097`*^9, 3.690815105765272*^9, + 3.690657618700452*^9, 3.690657639605751*^9}, 3.690657861755019*^9, + 3.690799670913288*^9, 3.6908146123428097`*^9, 3.690815105765272*^9, 3.690815372474331*^9, 3.690815695281649*^9, 3.690912542445437*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"n", " ", "=", " ", - RowBox[{"Sqrt", "[", - RowBox[{"\[Mu]", "/", + RowBox[{"n", " ", "=", " ", + RowBox[{"Sqrt", "[", + RowBox[{"\[Mu]", "/", RowBox[{"sma", "^", "3"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.690656519316794*^9, 3.6906565259676027`*^9}}], Cell[BoxData["0.00007334912748276923`"], "Output", - CellChangeTimes->{3.6906565263127747`*^9, 3.690657621044787*^9, - 3.6907996709408913`*^9, 3.690814612956794*^9, 3.690815106436891*^9, + CellChangeTimes->{3.6906565263127747`*^9, 3.690657621044787*^9, + 3.6907996709408913`*^9, 3.690814612956794*^9, 3.690815106436891*^9, 3.6908153725108767`*^9, 3.690815696255642*^9, 3.690912542491734*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"P", " ", "=", " ", - RowBox[{"2", " ", + RowBox[{"P", " ", "=", " ", + RowBox[{"2", " ", RowBox[{"\[Pi]", "/", "n"}]}]}]], "Input", CellChangeTimes->{{3.690656631821364*^9, 3.6906566347895193`*^9}}], Cell[BoxData["85661.35035015375`"], "Output", - CellChangeTimes->{3.690656635194646*^9, 3.690657621722246*^9, - 3.690799670994615*^9, 3.6908146135075207`*^9, 3.690815107105021*^9, + CellChangeTimes->{3.690656635194646*^9, 3.690657621722246*^9, + 3.690799670994615*^9, 3.6908146135075207`*^9, 3.690815107105021*^9, 3.690815372544787*^9, 3.6908156970322733`*^9, 3.690912542535741*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"T", " ", "=", " ", + RowBox[{"T", " ", "=", " ", RowBox[{"P", " ", "0.75"}]}]], "Input", CellChangeTimes->{{3.690656641373308*^9, 3.690656645262651*^9}}], Cell[BoxData["64246.01276261531`"], "Output", - CellChangeTimes->{3.690656645877283*^9, 3.690657622249775*^9, - 3.690799671037856*^9, 3.690814614125465*^9, 3.69081510782698*^9, + CellChangeTimes->{3.690656645877283*^9, 3.690657622249775*^9, + 3.690799671037856*^9, 3.690814614125465*^9, 3.69081510782698*^9, 3.690815372577879*^9, 3.690815697820571*^9, 3.690912542586594*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"M0", " ", "=", " ", - RowBox[{"E2M", "[", + RowBox[{"M0", " ", "=", " ", + RowBox[{"E2M", "[", RowBox[{ - RowBox[{"f2E", "[", + RowBox[{"f2E", "[", RowBox[{"f0", ",", "ecc"}], "]"}], ",", "ecc"}], "]"}]}]], "Input", CellChangeTimes->{{3.690656542247233*^9, 3.690656581869342*^9}}], Cell[BoxData["1.4887459192156767`"], "Output", - CellChangeTimes->{{3.69065656828483*^9, 3.6906565823889637`*^9}, - 3.690657622773707*^9, 3.6907996710773687`*^9, 3.690814614765023*^9, - 3.69081510922223*^9, 3.690815372611536*^9, 3.6908156989092712`*^9, + CellChangeTimes->{{3.69065656828483*^9, 3.6906565823889637`*^9}, + 3.690657622773707*^9, 3.6907996710773687`*^9, 3.690814614765023*^9, + 3.69081510922223*^9, 3.690815372611536*^9, 3.6908156989092712`*^9, 3.690912542635941*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"fList", " ", "=", " ", - RowBox[{"Table", "[", "\[IndentingNewLine]", + RowBox[{"fList", " ", "=", " ", + RowBox[{"Table", "[", "\[IndentingNewLine]", RowBox[{ - RowBox[{"E2f", "[", + RowBox[{"E2f", "[", RowBox[{ - RowBox[{"M2E", "[", + RowBox[{"M2E", "[", RowBox[{ - RowBox[{"M0", " ", "+", " ", - RowBox[{"n", "*", "i", " ", "15600"}]}], ",", "ecc"}], "]"}], ",", - "ecc"}], "]"}], "\[IndentingNewLine]", ",", - RowBox[{"{", + RowBox[{"M0", " ", "+", " ", + RowBox[{"n", "*", "i", " ", "15600"}]}], ",", "ecc"}], "]"}], ",", + "ecc"}], "]"}], "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "4"}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.690656590261936*^9, 3.690656600791767*^9}, { - 3.690656650886994*^9, 3.690656717545055*^9}, {3.690657439249593*^9, - 3.690657439528192*^9}, 3.690657550847658*^9, {3.690814794411097*^9, + 3.690656650886994*^9, 3.690656717545055*^9}, {3.690657439249593*^9, + 3.690657439528192*^9}, 3.690657550847658*^9, {3.690814794411097*^9, 3.690814794457123*^9}, {3.690815172394753*^9, 3.69081517559921*^9}, { - 3.690815510440259*^9, 3.690815513532783*^9}, {3.690815827514475*^9, + 3.690815510440259*^9, 3.690815513532783*^9}, {3.690815827514475*^9, 3.690815829503528*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ "1.4887658519511635`", ",", "2.6330020469451165`", ",", "3.77722682284748`", ",", "4.921465520979693`", ",", "6.0657271592054`"}], "}"}]], "Output", - CellChangeTimes->{3.690656718717534*^9, 3.690657440625499*^9, - 3.6906575515552588`*^9, 3.690657623298479*^9, 3.6907996711281147`*^9, - 3.690814615776546*^9, 3.690814795347313*^9, 3.6908151108192244`*^9, - 3.690815176508151*^9, 3.690815372649382*^9, 3.69081551431544*^9, + CellChangeTimes->{3.690656718717534*^9, 3.690657440625499*^9, + 3.6906575515552588`*^9, 3.690657623298479*^9, 3.6907996711281147`*^9, + 3.690814615776546*^9, 3.690814795347313*^9, 3.6908151108192244`*^9, + 3.690815176508151*^9, 3.690815372649382*^9, 3.69081551431544*^9, 3.690815699914011*^9, 3.690816022970752*^9, 3.6909125426876707`*^9}] }, Open ]], @@ -599,27 +599,27 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"ans", " ", "=", " ", - RowBox[{"Table", "[", "\[IndentingNewLine]", + RowBox[{"ans", " ", "=", " ", + RowBox[{"Table", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"elem2rv", "[", + RowBox[{"elem2rv", "[", RowBox[{ - "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", - ",", "\[Omega]", ",", - RowBox[{"fList", "[", - RowBox[{"[", "i", "]"}], "]"}]}], "]"}], "[", - RowBox[{"[", "1", "]"}], "]"}], "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"i", ",", - RowBox[{"Length", "[", "fList", "]"}]}], "}"}]}], "]"}]}], - ";"}], "\[IndentingNewLine]", - RowBox[{"CForm", "[", + "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", + ",", "\[Omega]", ",", + RowBox[{"fList", "[", + RowBox[{"[", "i", "]"}], "]"}]}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}], "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"i", ",", + RowBox[{"Length", "[", "fList", "]"}]}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{"CForm", "[", RowBox[{"ans", " ", "*", "1000"}], "]"}]}], "Input", CellChangeTimes->{{3.690656741674877*^9, 3.690656826017735*^9}, { - 3.69065757064723*^9, 3.690657585014105*^9}, {3.690657630642823*^9, + 3.69065757064723*^9, 3.690657585014105*^9}, {3.690657630642823*^9, 3.6906576309910793`*^9}, {3.690657684072776*^9, 3.690657687321257*^9}, { - 3.690657730420833*^9, 3.690657734503971*^9}, {3.690657778133027*^9, + 3.690657730420833*^9, 3.690657734503971*^9}, {3.690657778133027*^9, 3.6906577887291527`*^9}, {3.69081481160341*^9, 3.6908148340345707`*^9}}], Cell["\<\ @@ -629,13 +629,13 @@ List(List(-2.1819784817951165e7,3.588724145651873e7,0.), List(3.1201815137542922e7,-2.8114754297243357e7,0.), List(3.850428014786283e7,1.677456292503084e7,0.))\ \>", "Output", - CellChangeTimes->{{3.690656797978344*^9, 3.6906568264437532`*^9}, + CellChangeTimes->{{3.690656797978344*^9, 3.6906568264437532`*^9}, 3.690657443730393*^9, {3.690657554291491*^9, 3.690657585743434*^9}, { - 3.690657624135494*^9, 3.6906576314761066`*^9}, 3.69065768777134*^9, - 3.690657734922233*^9, {3.690657783707725*^9, 3.6906577891309757`*^9}, - 3.690799671174663*^9, 3.690814617139358*^9, {3.6908147975882463`*^9, - 3.690814834482308*^9}, 3.6908151129477882`*^9, 3.690815178108506*^9, - 3.690815372681299*^9, 3.6908155164030323`*^9, 3.690815701048983*^9, + 3.690657624135494*^9, 3.6906576314761066`*^9}, 3.69065768777134*^9, + 3.690657734922233*^9, {3.690657783707725*^9, 3.6906577891309757`*^9}, + 3.690799671174663*^9, 3.690814617139358*^9, {3.6908147975882463`*^9, + 3.690814834482308*^9}, 3.6908151129477882`*^9, 3.690815178108506*^9, + 3.690815372681299*^9, 3.6908155164030323`*^9, 3.690815701048983*^9, 3.690815807869759*^9, 3.6909125427565517`*^9}] }, Open ]] }, Open ]], @@ -644,7 +644,7 @@ Cell[CellGroupData[{ Cell["Exact Solution LMO", "Section", CellChangeTimes->{{3.690657839694964*^9, 3.690657841895734*^9}, { - 3.690814677128963*^9, 3.690814678443512*^9}, {3.690816037732939*^9, + 3.690814677128963*^9, 3.690814678443512*^9}, {3.690816037732939*^9, 3.690816038193124*^9}}], Cell[CellGroupData[{ @@ -678,151 +678,151 @@ Cell[BoxData["42828.31`"], "Output", Cell[BoxData[{ RowBox[{ - RowBox[{"sma", " ", "=", " ", "7000.00000`"}], ";"}], "\[IndentingNewLine]", + RowBox[{"sma", " ", "=", " ", "7000.00000`"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"ecc", " ", "=", " ", "0.000100000`"}], - ";"}], "\[IndentingNewLine]", + RowBox[{"ecc", " ", "=", " ", "0.000100000`"}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"inc", " ", "=", " ", - RowBox[{"33.300000`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"inc", " ", "=", " ", + RowBox[{"33.300000`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[CapitalOmega]", " ", "=", " ", - RowBox[{"48.2`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[CapitalOmega]", " ", "=", " ", + RowBox[{"48.2`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Omega]", " ", "=", " ", - RowBox[{"347.8`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[Omega]", " ", "=", " ", + RowBox[{"347.8`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"f0", " ", "=", " ", + RowBox[{"f0", " ", "=", " ", RowBox[{"85.3`", " ", "Degree"}]}], ";"}]}], "Input", - CellChangeTimes->{{3.690656456092209*^9, 3.6906565042546587`*^9}, + CellChangeTimes->{{3.690656456092209*^9, 3.6906565042546587`*^9}, 3.690656534585354*^9, {3.690657603205*^9, 3.690657637810052*^9}}], Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ - RowBox[{"{", - RowBox[{"r0", ",", "v0"}], "}"}], "=", - RowBox[{"elem2rv", "[", + RowBox[{"{", + RowBox[{"r0", ",", "v0"}], "}"}], "=", + RowBox[{"elem2rv", "[", RowBox[{ - "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", ",", + "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", ",", "\[Omega]", ",", "f0"}], "]"}]}]], "Input", CellChangeTimes->{{3.690656073578787*^9, 3.6906561159828*^9}, { - 3.6906563399393578`*^9, 3.6906563468782997`*^9}, {3.6906565090493793`*^9, + 3.6906563399393578`*^9, 3.6906563468782997`*^9}, {3.6906565090493793`*^9, 3.690656536914942*^9}, {3.690657859458934*^9, 3.690657861202614*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "2816.8016010234915`"}], ",", "5248.174846916147`", ",", - "3677.157264677297`"}], "}"}], ",", - RowBox[{"{", + RowBox[{"-", "2816.8016010234915`"}], ",", "5248.174846916147`", ",", + "3677.157264677297`"}], "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", "2.0256277189438467`"}], ",", - RowBox[{"-", "1.3635639534375639`"}], ",", "0.3949127458905012`"}], + RowBox[{"-", "2.0256277189438467`"}], ",", + RowBox[{"-", "1.3635639534375639`"}], ",", "0.3949127458905012`"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{{3.690656082426065*^9, 3.6906561164937077`*^9}, + CellChangeTimes->{{3.690656082426065*^9, 3.6906561164937077`*^9}, 3.6906563475089693`*^9, {3.6906565159496603`*^9, 3.690656539328861*^9}, { - 3.690657618700452*^9, 3.690657639605751*^9}, 3.690657861755019*^9, - 3.690799670913288*^9, 3.6908146123428097`*^9, {3.6908160503086348`*^9, + 3.690657618700452*^9, 3.690657639605751*^9}, 3.690657861755019*^9, + 3.690799670913288*^9, 3.6908146123428097`*^9, {3.6908160503086348`*^9, 3.690816077479035*^9}, 3.690912542856641*^9, 3.6909128475873947`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"n", " ", "=", " ", - RowBox[{"Sqrt", "[", - RowBox[{"\[Mu]", "/", + RowBox[{"n", " ", "=", " ", + RowBox[{"Sqrt", "[", + RowBox[{"\[Mu]", "/", RowBox[{"sma", "^", "3"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.690656519316794*^9, 3.6906565259676027`*^9}}], Cell[BoxData["0.0003533608319423934`"], "Output", CellChangeTimes->{ - 3.6906565263127747`*^9, 3.690657621044787*^9, 3.6907996709408913`*^9, - 3.690814612956794*^9, {3.6908160503448563`*^9, 3.690816077520937*^9}, + 3.6906565263127747`*^9, 3.690657621044787*^9, 3.6907996709408913`*^9, + 3.690814612956794*^9, {3.6908160503448563`*^9, 3.690816077520937*^9}, 3.690912542887168*^9, 3.6909128476303377`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"P", " ", "=", " ", - RowBox[{"2", " ", + RowBox[{"P", " ", "=", " ", + RowBox[{"2", " ", RowBox[{"\[Pi]", "/", "n"}]}]}]], "Input", CellChangeTimes->{{3.690656631821364*^9, 3.6906566347895193`*^9}}], Cell[BoxData["17781.2160805754`"], "Output", CellChangeTimes->{ - 3.690656635194646*^9, 3.690657621722246*^9, 3.690799670994615*^9, - 3.6908146135075207`*^9, {3.690816050379136*^9, 3.690816077561973*^9}, + 3.690656635194646*^9, 3.690657621722246*^9, 3.690799670994615*^9, + 3.6908146135075207`*^9, {3.690816050379136*^9, 3.690816077561973*^9}, 3.690912542936118*^9, 3.690912847680664*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"T", " ", "=", " ", + RowBox[{"T", " ", "=", " ", RowBox[{"P", " ", "0.75"}]}]], "Input", CellChangeTimes->{{3.690656641373308*^9, 3.690656645262651*^9}}], Cell[BoxData["13335.91206043155`"], "Output", CellChangeTimes->{ - 3.690656645877283*^9, 3.690657622249775*^9, 3.690799671037856*^9, - 3.690814614125465*^9, {3.6908160504122133`*^9, 3.6908160776022882`*^9}, + 3.690656645877283*^9, 3.690657622249775*^9, 3.690799671037856*^9, + 3.690814614125465*^9, {3.6908160504122133`*^9, 3.6908160776022882`*^9}, 3.690912542988265*^9, 3.690912847715248*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"M0", " ", "=", " ", - RowBox[{"E2M", "[", + RowBox[{"M0", " ", "=", " ", + RowBox[{"E2M", "[", RowBox[{ - RowBox[{"f2E", "[", + RowBox[{"f2E", "[", RowBox[{"f0", ",", "ecc"}], "]"}], ",", "ecc"}], "]"}]}]], "Input", CellChangeTimes->{{3.690656542247233*^9, 3.690656581869342*^9}}], Cell[BoxData["1.4885665256990677`"], "Output", - CellChangeTimes->{{3.69065656828483*^9, 3.6906565823889637`*^9}, + CellChangeTimes->{{3.69065656828483*^9, 3.6906565823889637`*^9}, 3.690657622773707*^9, 3.6907996710773687`*^9, 3.690814614765023*^9, { - 3.690816050445689*^9, 3.6908160776375313`*^9}, 3.690912543037883*^9, + 3.690816050445689*^9, 3.6908160776375313`*^9}, 3.690912543037883*^9, 3.690912847763558*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"fList", " ", "=", " ", - RowBox[{"Table", "[", "\[IndentingNewLine]", + RowBox[{"fList", " ", "=", " ", + RowBox[{"Table", "[", "\[IndentingNewLine]", RowBox[{ - RowBox[{"E2f", "[", + RowBox[{"E2f", "[", RowBox[{ - RowBox[{"M2E", "[", + RowBox[{"M2E", "[", RowBox[{ - RowBox[{"M0", " ", "+", " ", - RowBox[{"n", "*", "i", " ", "3360"}]}], ",", "ecc"}], "]"}], ",", - "ecc"}], "]"}], "\[IndentingNewLine]", ",", - RowBox[{"{", + RowBox[{"M0", " ", "+", " ", + RowBox[{"n", "*", "i", " ", "3360"}]}], ",", "ecc"}], "]"}], ",", + "ecc"}], "]"}], "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "4"}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.690656590261936*^9, 3.690656600791767*^9}, { - 3.690656650886994*^9, 3.690656717545055*^9}, {3.690657439249593*^9, - 3.690657439528192*^9}, 3.690657550847658*^9, {3.690814794411097*^9, - 3.690814794457123*^9}, 3.6908155343813*^9, {3.6908160737351313`*^9, + 3.690656650886994*^9, 3.690656717545055*^9}, {3.690657439249593*^9, + 3.690657439528192*^9}, 3.690657550847658*^9, {3.690814794411097*^9, + 3.690814794457123*^9}, 3.6908155343813*^9, {3.6908160737351313`*^9, 3.690816074924892*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - "1.4887658519511633`", ",", "2.675948726696593`", ",", - "3.8630192176130445`", ",", "5.050255023536483`", ",", + "1.4887658519511633`", ",", "2.675948726696593`", ",", + "3.8630192176130445`", ",", "5.050255023536483`", ",", "6.23772701915905`"}], "}"}]], "Output", CellChangeTimes->{ - 3.690656718717534*^9, 3.690657440625499*^9, 3.6906575515552588`*^9, - 3.690657623298479*^9, 3.6907996711281147`*^9, 3.690814615776546*^9, - 3.690814795347313*^9, {3.6908160504958963`*^9, 3.690816077676166*^9}, + 3.690656718717534*^9, 3.690657440625499*^9, 3.6906575515552588`*^9, + 3.690657623298479*^9, 3.6907996711281147`*^9, 3.690814615776546*^9, + 3.690814795347313*^9, {3.6908160504958963`*^9, 3.690816077676166*^9}, 3.690912543086906*^9, 3.690912847814042*^9}] }, Open ]], @@ -830,27 +830,27 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"ans", " ", "=", " ", - RowBox[{"Table", "[", "\[IndentingNewLine]", + RowBox[{"ans", " ", "=", " ", + RowBox[{"Table", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"elem2rv", "[", + RowBox[{"elem2rv", "[", RowBox[{ - "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", - ",", "\[Omega]", ",", - RowBox[{"fList", "[", - RowBox[{"[", "i", "]"}], "]"}]}], "]"}], "[", - RowBox[{"[", "1", "]"}], "]"}], "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"i", ",", - RowBox[{"Length", "[", "fList", "]"}]}], "}"}]}], "]"}]}], - ";"}], "\[IndentingNewLine]", - RowBox[{"CForm", "[", + "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", + ",", "\[Omega]", ",", + RowBox[{"fList", "[", + RowBox[{"[", "i", "]"}], "]"}]}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}], "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"i", ",", + RowBox[{"Length", "[", "fList", "]"}]}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{"CForm", "[", RowBox[{"ans", " ", "*", "1000"}], "]"}]}], "Input", CellChangeTimes->{{3.690656741674877*^9, 3.690656826017735*^9}, { - 3.69065757064723*^9, 3.690657585014105*^9}, {3.690657630642823*^9, + 3.69065757064723*^9, 3.690657585014105*^9}, {3.690657630642823*^9, 3.6906576309910793`*^9}, {3.690657684072776*^9, 3.690657687321257*^9}, { - 3.690657730420833*^9, 3.690657734503971*^9}, {3.690657778133027*^9, + 3.690657730420833*^9, 3.690657734503971*^9}, {3.690657778133027*^9, 3.6906577887291527`*^9}, {3.69081481160341*^9, 3.6908148340345707`*^9}}], Cell["\<\ @@ -860,12 +860,12 @@ List(List(-2.8168016010234966e6,5.248174846916143e6,3.6771572646772987e6), List(4.90876381054031e6,-3.2188851633259663e6,-3.8130784005532693e6), List(5.624745335101494e6,4.0482452444431377e6,-981916.985774726))\ \>", "Output", - CellChangeTimes->{{3.690656797978344*^9, 3.6906568264437532`*^9}, + CellChangeTimes->{{3.690656797978344*^9, 3.6906568264437532`*^9}, 3.690657443730393*^9, {3.690657554291491*^9, 3.690657585743434*^9}, { - 3.690657624135494*^9, 3.6906576314761066`*^9}, 3.69065768777134*^9, - 3.690657734922233*^9, {3.690657783707725*^9, 3.6906577891309757`*^9}, - 3.690799671174663*^9, 3.690814617139358*^9, {3.6908147975882463`*^9, - 3.690814834482308*^9}, {3.6908160505272427`*^9, 3.6908160777229557`*^9}, + 3.690657624135494*^9, 3.6906576314761066`*^9}, 3.69065768777134*^9, + 3.690657734922233*^9, {3.690657783707725*^9, 3.6906577891309757`*^9}, + 3.690799671174663*^9, 3.690814617139358*^9, {3.6908147975882463`*^9, + 3.690814834482308*^9}, {3.6908160505272427`*^9, 3.6908160777229557`*^9}, 3.6909125431361017`*^9, 3.69091284784732*^9}] }, Open ]] }, Open ]] @@ -1039,4 +1039,3 @@ Cell[30902, 855, 876, 13, 104, "Output"] } ] *) - diff --git a/docs/source/_images/static/_Support/test_scenarioIntegrator.nb b/docs/source/_images/static/_Support/test_scenarioIntegrator.nb index 246c08e7e6..d56560f224 100644 --- a/docs/source/_images/static/_Support/test_scenarioIntegrator.nb +++ b/docs/source/_images/static/_Support/test_scenarioIntegrator.nb @@ -20,7 +20,7 @@ WindowFrame->Normal*) Notebook[{ Cell[BoxData[ RowBox[{"<<", "orbitalMotion`"}]], "Input", - CellChangeTimes->{{3.690656012562312*^9, + CellChangeTimes->{{3.690656012562312*^9, 3.690656017904612*^9}},ExpressionUUID->"13e6cdb8-6476-4162-842b-\ 7d71ef3d015e"], @@ -28,13 +28,13 @@ Cell[CellGroupData[{ Cell[BoxData[ RowBox[{"\[Mu]", " ", "=", " ", "398600.4415"}]], "Input", - CellChangeTimes->{{3.6909116204375887`*^9, + CellChangeTimes->{{3.6909116204375887`*^9, 3.69091163413379*^9}},ExpressionUUID->"d5019e5b-29af-452a-b517-\ e00be3d52d04"], Cell[BoxData["398600.4415`"], "Output", - CellChangeTimes->{3.690911636224205*^9, 3.6909121441270943`*^9, - 3.692720990624939*^9, 3.692722834964164*^9, + CellChangeTimes->{3.690911636224205*^9, 3.6909121441270943`*^9, + 3.692720990624939*^9, 3.692722834964164*^9, 3.709917384641786*^9},ExpressionUUID->"d6b792f3-dd0a-4c50-8be2-\ 28d76bb6d0c3"] }, Open ]], @@ -43,12 +43,12 @@ Cell[CellGroupData[{ Cell[BoxData[ RowBox[{"reqEarth", " ", "=", " ", "6378.1363"}]], "Input", - CellChangeTimes->{{3.692722789881296*^9, + CellChangeTimes->{{3.692722789881296*^9, 3.692722798754932*^9}},ExpressionUUID->"4603f634-bef2-4a52-88bb-\ 7b9f5fcbd568"], Cell[BoxData["6378.1363`"], "Output", - CellChangeTimes->{3.692722799620097*^9, + CellChangeTimes->{3.692722799620097*^9, 3.709917385259251*^9},ExpressionUUID->"53a9bb4b-438c-472f-8b4a-\ 94116282ccdd"] }, Open ]], @@ -60,24 +60,24 @@ Cell["Exact Solution", "Section", Cell[BoxData[{ RowBox[{ - RowBox[{"sma", " ", "=", " ", "7000.00000`"}], ";"}], "\[IndentingNewLine]", + RowBox[{"sma", " ", "=", " ", "7000.00000`"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"ecc", " ", "=", " ", "0.000100000`"}], - ";"}], "\[IndentingNewLine]", + RowBox[{"ecc", " ", "=", " ", "0.000100000`"}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"inc", " ", "=", " ", - RowBox[{"33.300000`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"inc", " ", "=", " ", + RowBox[{"33.300000`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[CapitalOmega]", " ", "=", " ", - RowBox[{"48.2`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[CapitalOmega]", " ", "=", " ", + RowBox[{"48.2`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Omega]", " ", "=", " ", - RowBox[{"347.8`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[Omega]", " ", "=", " ", + RowBox[{"347.8`", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"f0", " ", "=", " ", + RowBox[{"f0", " ", "=", " ", RowBox[{"85.3`", " ", "Degree"}]}], ";"}]}], "Input", - CellChangeTimes->{{3.690656456092209*^9, 3.6906565042546587`*^9}, - 3.690656534585354*^9, {3.690657603205*^9, + CellChangeTimes->{{3.690656456092209*^9, 3.6906565042546587`*^9}, + 3.690656534585354*^9, {3.690657603205*^9, 3.690657637810052*^9}},ExpressionUUID->"5ab50ff1-7c9b-443b-b305-\ 5292d9e64aa4"], @@ -85,35 +85,35 @@ Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ - RowBox[{"{", - RowBox[{"r0", ",", "v0"}], "}"}], "=", - RowBox[{"elem2rv", "[", + RowBox[{"{", + RowBox[{"r0", ",", "v0"}], "}"}], "=", + RowBox[{"elem2rv", "[", RowBox[{ - "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", ",", + "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", ",", "\[Omega]", ",", "f0"}], "]"}]}]], "Input", CellChangeTimes->{{3.690656073578787*^9, 3.6906561159828*^9}, { - 3.6906563399393578`*^9, 3.6906563468782997`*^9}, {3.6906565090493793`*^9, - 3.690656536914942*^9}, {3.690657859458934*^9, + 3.6906563399393578`*^9, 3.6906563468782997`*^9}, {3.6906565090493793`*^9, + 3.690656536914942*^9}, {3.690657859458934*^9, 3.690657861202614*^9}},ExpressionUUID->"d288114c-9763-4d0b-976a-\ 424069c64ca1"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "2816.8016010234915`"}], ",", "5248.174846916147`", ",", - "3677.157264677297`"}], "}"}], ",", - RowBox[{"{", + RowBox[{"-", "2816.8016010234915`"}], ",", "5248.174846916147`", ",", + "3677.157264677297`"}], "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", "6.179638215820373`"}], ",", - RowBox[{"-", "4.1598620701988285`"}], ",", "1.2047711796182983`"}], + RowBox[{"-", "6.179638215820373`"}], ",", + RowBox[{"-", "4.1598620701988285`"}], ",", "1.2047711796182983`"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{{3.690656082426065*^9, 3.6906561164937077`*^9}, + CellChangeTimes->{{3.690656082426065*^9, 3.6906561164937077`*^9}, 3.6906563475089693`*^9, {3.6906565159496603`*^9, 3.690656539328861*^9}, { - 3.690657618700452*^9, 3.690657639605751*^9}, 3.690657861755019*^9, - 3.690799670913288*^9, 3.690910760499894*^9, 3.690911640591095*^9, - 3.690912144198064*^9, 3.692720993508987*^9, 3.692727712245901*^9, + 3.690657618700452*^9, 3.690657639605751*^9}, 3.690657861755019*^9, + 3.690799670913288*^9, 3.690910760499894*^9, 3.690911640591095*^9, + 3.690912144198064*^9, 3.692720993508987*^9, 3.692727712245901*^9, 3.709917388367366*^9},ExpressionUUID->"1e5c68fd-f6df-4c0e-953b-\ 4d609bd0a2dc"] }, Open ]], @@ -121,18 +121,18 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"n", " ", "=", " ", - RowBox[{"Sqrt", "[", - RowBox[{"\[Mu]", "/", + RowBox[{"n", " ", "=", " ", + RowBox[{"Sqrt", "[", + RowBox[{"\[Mu]", "/", RowBox[{"sma", "^", "3"}]}], "]"}]}]], "Input", - CellChangeTimes->{{3.690656519316794*^9, + CellChangeTimes->{{3.690656519316794*^9, 3.6906565259676027`*^9}},ExpressionUUID->"b7a2bde1-a25c-4019-bd7a-\ 1299cdf6e53d"], Cell[BoxData["0.0010780076124668337`"], "Output", - CellChangeTimes->{3.6906565263127747`*^9, 3.690657621044787*^9, - 3.6907996709408913`*^9, 3.6909107605340433`*^9, 3.690911640637835*^9, - 3.69091214423491*^9, 3.692720993547284*^9, 3.692727712280298*^9, + CellChangeTimes->{3.6906565263127747`*^9, 3.690657621044787*^9, + 3.6907996709408913`*^9, 3.6909107605340433`*^9, 3.690911640637835*^9, + 3.69091214423491*^9, 3.692720993547284*^9, 3.692727712280298*^9, 3.709917389228149*^9},ExpressionUUID->"0f73403d-ad41-48b6-9e9c-\ dd0baec39ad0"] }, Open ]], @@ -140,17 +140,17 @@ dd0baec39ad0"] Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"P", " ", "=", " ", - RowBox[{"2", " ", + RowBox[{"P", " ", "=", " ", + RowBox[{"2", " ", RowBox[{"\[Pi]", "/", "n"}]}]}]], "Input", - CellChangeTimes->{{3.690656631821364*^9, + CellChangeTimes->{{3.690656631821364*^9, 3.6906566347895193`*^9}},ExpressionUUID->"53a3906b-2ed7-4774-930f-\ e45206ee4216"], Cell[BoxData["5828.516639879384`"], "Output", - CellChangeTimes->{3.690656635194646*^9, 3.690657621722246*^9, - 3.690799670994615*^9, 3.69091076056813*^9, 3.690911640686471*^9, - 3.690912144279821*^9, 3.69272099361306*^9, 3.692727712323764*^9, + CellChangeTimes->{3.690656635194646*^9, 3.690657621722246*^9, + 3.690799670994615*^9, 3.69091076056813*^9, 3.690911640686471*^9, + 3.690912144279821*^9, 3.69272099361306*^9, 3.692727712323764*^9, 3.709917389839498*^9},ExpressionUUID->"6e07af87-71e2-448b-b0dc-\ 3055ba4f1db2"] }, Open ]], @@ -158,16 +158,16 @@ Cell[BoxData["5828.516639879384`"], "Output", Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"T", " ", "=", " ", + RowBox[{"T", " ", "=", " ", RowBox[{"P", " ", "0.75"}]}]], "Input", - CellChangeTimes->{{3.690656641373308*^9, + CellChangeTimes->{{3.690656641373308*^9, 3.690656645262651*^9}},ExpressionUUID->"f128edc0-0e27-4ff1-9f41-\ 63ca50d174bd"], Cell[BoxData["4371.387479909537`"], "Output", - CellChangeTimes->{3.690656645877283*^9, 3.690657622249775*^9, - 3.690799671037856*^9, 3.690910760604686*^9, 3.6909116407471113`*^9, - 3.690912144314827*^9, 3.692720993664197*^9, 3.692727712363413*^9, + CellChangeTimes->{3.690656645877283*^9, 3.690657622249775*^9, + 3.690799671037856*^9, 3.690910760604686*^9, 3.6909116407471113`*^9, + 3.690912144314827*^9, 3.692720993664197*^9, 3.692727712363413*^9, 3.709917390524444*^9},ExpressionUUID->"7e031bba-d9fa-4828-b2d4-\ 095db1d8bc99"] }, Open ]], @@ -175,20 +175,20 @@ Cell[BoxData["4371.387479909537`"], "Output", Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"M0", " ", "=", " ", - RowBox[{"E2M", "[", + RowBox[{"M0", " ", "=", " ", + RowBox[{"E2M", "[", RowBox[{ - RowBox[{"f2E", "[", + RowBox[{"f2E", "[", RowBox[{"f0", ",", "ecc"}], "]"}], ",", "ecc"}], "]"}]}]], "Input", - CellChangeTimes->{{3.690656542247233*^9, + CellChangeTimes->{{3.690656542247233*^9, 3.690656581869342*^9}},ExpressionUUID->"6760164b-3873-4198-a7cd-\ d4031177d77a"], Cell[BoxData["1.4885665256990677`"], "Output", - CellChangeTimes->{{3.69065656828483*^9, 3.6906565823889637`*^9}, - 3.690657622773707*^9, 3.6907996710773687`*^9, 3.6909107606519823`*^9, - 3.690911640787664*^9, 3.6909121443612213`*^9, 3.6927209937173157`*^9, - 3.69272771240757*^9, + CellChangeTimes->{{3.69065656828483*^9, 3.6906565823889637`*^9}, + 3.690657622773707*^9, 3.6907996710773687`*^9, 3.6909107606519823`*^9, + 3.690911640787664*^9, 3.6909121443612213`*^9, 3.6927209937173157`*^9, + 3.69272771240757*^9, 3.709917391154442*^9},ExpressionUUID->"b1331471-2743-4c83-9c05-\ e239835ece69"] }, Open ]], @@ -196,34 +196,34 @@ e239835ece69"] Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"fList", " ", "=", " ", - RowBox[{"Table", "[", "\[IndentingNewLine]", + RowBox[{"fList", " ", "=", " ", + RowBox[{"Table", "[", "\[IndentingNewLine]", RowBox[{ - RowBox[{"E2f", "[", + RowBox[{"E2f", "[", RowBox[{ - RowBox[{"M2E", "[", + RowBox[{"M2E", "[", RowBox[{ - RowBox[{"M0", " ", "+", " ", - RowBox[{"n", "*", "i", " ", "1000"}]}], ",", "ecc"}], "]"}], ",", - "ecc"}], "]"}], "\[IndentingNewLine]", ",", - RowBox[{"{", + RowBox[{"M0", " ", "+", " ", + RowBox[{"n", "*", "i", " ", "1000"}]}], ",", "ecc"}], "]"}], ",", + "ecc"}], "]"}], "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "4"}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.690656590261936*^9, 3.690656600791767*^9}, { - 3.690656650886994*^9, 3.690656717545055*^9}, {3.690657439249593*^9, - 3.690657439528192*^9}, 3.690657550847658*^9, + 3.690656650886994*^9, 3.690656717545055*^9}, {3.690657439249593*^9, + 3.690657439528192*^9}, 3.690657550847658*^9, 3.6908155479289913`*^9},ExpressionUUID->"7602398f-b003-4d7a-97cb-\ 58fc4859ef64"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - "1.4887658519511633`", ",", "2.5666828968228153`", ",", - "3.6444853518758733`", ",", "4.7223893732506`", ",", + "1.4887658519511633`", ",", "2.5666828968228153`", ",", + "3.6444853518758733`", ",", "4.7223893732506`", ",", "5.800504150595946`"}], "}"}]], "Output", - CellChangeTimes->{3.690656718717534*^9, 3.690657440625499*^9, - 3.6906575515552588`*^9, 3.690657623298479*^9, 3.6907996711281147`*^9, - 3.6909107607013702`*^9, 3.690911640825323*^9, 3.690912144410684*^9, - 3.692720993763721*^9, 3.692727712458733*^9, + CellChangeTimes->{3.690656718717534*^9, 3.690657440625499*^9, + 3.6906575515552588`*^9, 3.690657623298479*^9, 3.6907996711281147`*^9, + 3.6909107607013702`*^9, 3.690911640825323*^9, 3.690912144410684*^9, + 3.692720993763721*^9, 3.692727712458733*^9, 3.7099173919589167`*^9},ExpressionUUID->"3e65895c-06dd-4910-8fa1-\ 07f5827c4224"] }, Open ]], @@ -232,100 +232,100 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"ans", " ", "=", " ", - RowBox[{"Table", "[", "\[IndentingNewLine]", + RowBox[{"ans", " ", "=", " ", + RowBox[{"Table", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"elem2rv", "[", + RowBox[{"elem2rv", "[", RowBox[{ - "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", - ",", "\[Omega]", ",", - RowBox[{"fList", "[", - RowBox[{"[", "i", "]"}], "]"}]}], "]"}], "[", - RowBox[{"[", "1", "]"}], "]"}], "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"i", ",", - RowBox[{"Length", "[", "fList", "]"}]}], "}"}]}], "]"}]}], - ";"}], "\[IndentingNewLine]", - RowBox[{"MatrixForm", "[", - RowBox[{"NumberForm", "[", + "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", + ",", "\[Omega]", ",", + RowBox[{"fList", "[", + RowBox[{"[", "i", "]"}], "]"}]}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}], "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"i", ",", + RowBox[{"Length", "[", "fList", "]"}]}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{"MatrixForm", "[", + RowBox[{"NumberForm", "[", RowBox[{"ans", ",", "12"}], " ", "]"}], "]"}]}], "Input", CellChangeTimes->{{3.690656741674877*^9, 3.690656826017735*^9}, { - 3.69065757064723*^9, 3.690657585014105*^9}, {3.690657630642823*^9, + 3.69065757064723*^9, 3.690657585014105*^9}, {3.690657630642823*^9, 3.6906576309910793`*^9}, {3.690657684072776*^9, 3.690657687321257*^9}, { - 3.690657730420833*^9, 3.690657734503971*^9}, {3.690657778133027*^9, + 3.690657730420833*^9, 3.690657734503971*^9}, {3.690657778133027*^9, 3.6906577887291527`*^9}},ExpressionUUID->"70b3c191-89f7-4f9e-80e1-\ 0893e8f1f55c"], Cell[BoxData[ TagBox[ TagBox[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ InterpretationBox["\<\"-2816.80160102\"\>", -2816.8016010234965`, - AutoDelete->True], ",", + AutoDelete->True], ",", InterpretationBox["\<\"5248.17484692\"\>", 5248.174846916143, - AutoDelete->True], ",", + AutoDelete->True], ",", InterpretationBox["\<\"3677.15726468\"\>", 3677.157264677299, - AutoDelete->True]}], "}"}], ",", - RowBox[{"{", + AutoDelete->True]}], "}"}], ",", + RowBox[{"{", RowBox[{ InterpretationBox["\<\"-6383.21936016\"\>", -6383.219360156923, - AutoDelete->True], ",", + AutoDelete->True], ",", InterpretationBox["\<\"-916.780719895\"\>", -916.7807198948866, - AutoDelete->True], ",", + AutoDelete->True], ",", InterpretationBox["\<\"2724.38035754\"\>", 2724.380357539382, - AutoDelete->True]}], "}"}], ",", - RowBox[{"{", + AutoDelete->True]}], "}"}], ",", + RowBox[{"{", RowBox[{ InterpretationBox["\<\"-3224.20723229\"\>", -3224.2072322891595`, - AutoDelete->True], ",", + AutoDelete->True], ",", InterpretationBox["\<\"-6115.99975448\"\>", -6115.999754479678, - AutoDelete->True], ",", + AutoDelete->True], ",", InterpretationBox["\<\"-1098.91835781\"\>", -1098.9183578099735`, - AutoDelete->True]}], "}"}], ",", - RowBox[{"{", + AutoDelete->True]}], "}"}], ",", + RowBox[{"{", RowBox[{ InterpretationBox["\<\"3331.68993768\"\>", 3331.68993768218, - AutoDelete->True], ",", + AutoDelete->True], ",", InterpretationBox["\<\"-4871.32657135\"\>", -4871.326571349695, - AutoDelete->True], ",", + AutoDelete->True], ",", InterpretationBox["\<\"-3764.29550014\"\>", -3764.2955001357345`, - AutoDelete->True]}], "}"}], ",", - RowBox[{"{", + AutoDelete->True]}], "}"}], ",", + RowBox[{"{", RowBox[{ InterpretationBox["\<\"6376.23848873\"\>", 6376.238488731508, - AutoDelete->True], ",", + AutoDelete->True], ",", InterpretationBox["\<\"1506.67298169\"\>", 1506.6729816914842`, - AutoDelete->True], ",", + AutoDelete->True], ",", InterpretationBox["\<\"-2462.68939349\"\>", -2462.689393485881, AutoDelete->True]}], "}"}]}], "}"}], NumberForm[#, 12]& ], - Function[BoxForm`e$, + Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", - CellChangeTimes->{{3.690656797978344*^9, 3.6906568264437532`*^9}, + CellChangeTimes->{{3.690656797978344*^9, 3.6906568264437532`*^9}, 3.690657443730393*^9, {3.690657554291491*^9, 3.690657585743434*^9}, { - 3.690657624135494*^9, 3.6906576314761066`*^9}, 3.69065768777134*^9, - 3.690657734922233*^9, {3.690657783707725*^9, 3.6906577891309757`*^9}, - 3.690799671174663*^9, 3.6909107607493887`*^9, 3.690911640872286*^9, - 3.6909121444609423`*^9, 3.692720993814171*^9, 3.692727712508593*^9, + 3.690657624135494*^9, 3.6906576314761066`*^9}, 3.69065768777134*^9, + 3.690657734922233*^9, {3.690657783707725*^9, 3.6906577891309757`*^9}, + 3.690799671174663*^9, 3.6909107607493887`*^9, 3.690911640872286*^9, + 3.6909121444609423`*^9, 3.692720993814171*^9, 3.692727712508593*^9, 3.709917393340817*^9},ExpressionUUID->"0db7ab78-2b6b-47ac-802e-\ da153e04c037"] }, Open ]] @@ -335,7 +335,7 @@ Cell[CellGroupData[{ Cell["Integration Solution (Keplerian, Earth)", "Section", CellChangeTimes->{{3.69065784668974*^9, 3.690657848815713*^9}, { - 3.690659220781633*^9, 3.6906592221884403`*^9}, {3.6927069512317343`*^9, + 3.690659220781633*^9, 3.6906592221884403`*^9}, {3.6927069512317343`*^9, 3.6927069534818497`*^9}, {3.6927218987189407`*^9, 3.692721899495422*^9}}], Cell[CellGroupData[{ @@ -345,99 +345,99 @@ Cell["Setup", "Subsection", Cell[BoxData[ RowBox[{ - RowBox[{"F", "[", "X_", "]"}], ":=", - RowBox[{"Block", "[", + RowBox[{"F", "[", "X_", "]"}], ":=", + RowBox[{"Block", "[", RowBox[{ - RowBox[{"{", - RowBox[{"ans", ",", "r"}], "}"}], ",", "\[IndentingNewLine]", - "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"ans", ",", "r"}], "}"}], ",", "\[IndentingNewLine]", + "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", " ", "=", " ", "X"}], ";", "\[IndentingNewLine]", + RowBox[{"ans", " ", "=", " ", "X"}], ";", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "1", "]"}], "]"}], " ", "=", " ", - RowBox[{"X", "[", - RowBox[{"[", "4", "]"}], "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"ans", "[", + RowBox[{"[", "1", "]"}], "]"}], " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", "4", "]"}], "]"}]}], ";", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "2", "]"}], "]"}], " ", "=", " ", - RowBox[{"X", "[", - RowBox[{"[", "5", "]"}], "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"ans", "[", + RowBox[{"[", "2", "]"}], "]"}], " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", "5", "]"}], "]"}]}], ";", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "3", "]"}], "]"}], " ", "=", " ", - RowBox[{"X", "[", - RowBox[{"[", "6", "]"}], "]"}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"r", " ", "=", " ", - RowBox[{"Sqrt", "[", + RowBox[{"ans", "[", + RowBox[{"[", "3", "]"}], "]"}], " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", "6", "]"}], "]"}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"r", " ", "=", " ", + RowBox[{"Sqrt", "[", RowBox[{ RowBox[{ - RowBox[{"X", "[", - RowBox[{"[", "1", "]"}], "]"}], "^", "2"}], " ", "+", " ", + RowBox[{"X", "[", + RowBox[{"[", "1", "]"}], "]"}], "^", "2"}], " ", "+", " ", RowBox[{ - RowBox[{"X", "[", - RowBox[{"[", "2", "]"}], "]"}], "^", "2"}], " ", "+", " ", + RowBox[{"X", "[", + RowBox[{"[", "2", "]"}], "]"}], "^", "2"}], " ", "+", " ", RowBox[{ - RowBox[{"X", "[", - RowBox[{"[", "3", "]"}], "]"}], "^", "2"}]}], "]"}]}], ";", - "\[IndentingNewLine]", "\[IndentingNewLine]", + RowBox[{"X", "[", + RowBox[{"[", "3", "]"}], "]"}], "^", "2"}]}], "]"}]}], ";", + "\[IndentingNewLine]", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "4", "]"}], "]"}], " ", "=", " ", + RowBox[{"ans", "[", + RowBox[{"[", "4", "]"}], "]"}], " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"-", " ", "\[Mu]"}], "/", - RowBox[{"r", "^", "3"}]}], " ", - RowBox[{"X", "[", - RowBox[{"[", "1", "]"}], "]"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"-", " ", "\[Mu]"}], "/", + RowBox[{"r", "^", "3"}]}], " ", + RowBox[{"X", "[", + RowBox[{"[", "1", "]"}], "]"}]}]}], ";", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "5", "]"}], "]"}], " ", "=", " ", + RowBox[{"ans", "[", + RowBox[{"[", "5", "]"}], "]"}], " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"-", " ", "\[Mu]"}], "/", - RowBox[{"r", "^", "3"}]}], " ", - RowBox[{"X", "[", - RowBox[{"[", "2", "]"}], "]"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"-", " ", "\[Mu]"}], "/", + RowBox[{"r", "^", "3"}]}], " ", + RowBox[{"X", "[", + RowBox[{"[", "2", "]"}], "]"}]}]}], ";", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "6", "]"}], "]"}], " ", "=", " ", + RowBox[{"ans", "[", + RowBox[{"[", "6", "]"}], "]"}], " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"-", " ", "\[Mu]"}], "/", - RowBox[{"r", "^", "3"}]}], " ", - RowBox[{"X", "[", - RowBox[{"[", "3", "]"}], "]"}]}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"Return", "[", "ans", "]"}], ";"}]}], "\[IndentingNewLine]", + RowBox[{"-", " ", "\[Mu]"}], "/", + RowBox[{"r", "^", "3"}]}], " ", + RowBox[{"X", "[", + RowBox[{"[", "3", "]"}], "]"}]}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"Return", "[", "ans", "]"}], ";"}]}], "\[IndentingNewLine]", "]"}]}]], "Input", CellChangeTimes->{{3.6906578694027767`*^9, 3.690658027163175*^9}, { - 3.69270693512519*^9, + 3.69270693512519*^9, 3.692706939904386*^9}},ExpressionUUID->"51eeb06b-62e5-4523-a7ed-\ 2e52f8160814"], Cell[BoxData[{ RowBox[{ - RowBox[{"h", " ", "=", " ", "120"}], ";"}], "\[IndentingNewLine]", + RowBox[{"h", " ", "=", " ", "120"}], ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"Tmax", " ", "=", " ", "4400"}], ";"}]}], "Input", CellChangeTimes->{ 3.690659238399549*^9, {3.690808414154882*^9, 3.6908084142425222`*^9}, { - 3.690810617229479*^9, + 3.690810617229479*^9, 3.690810621873021*^9}},ExpressionUUID->"ab87c54d-5248-46b3-9025-\ 305e4d5ec55b"], Cell[BoxData[ RowBox[{ - RowBox[{"timeList", " ", "=", " ", - RowBox[{"{", + RowBox[{"timeList", " ", "=", " ", + RowBox[{"{", RowBox[{ - "0", ",", "1080", ",", "2160", ",", " ", "3240", ",", " ", "4320"}], + "0", ",", "1080", ",", "2160", ",", " ", "3240", ",", " ", "4320"}], "}"}]}], ";"}]], "Input", CellChangeTimes->{{3.690810482620377*^9, 3.690810508911993*^9}, { - 3.690810586078121*^9, 3.690810588100753*^9}, {3.690910944395762*^9, - 3.690910949592393*^9}, {3.690910983254991*^9, + 3.690810586078121*^9, 3.690810588100753*^9}, {3.690910944395762*^9, + 3.690910949592393*^9}, {3.690910983254991*^9, 3.69091098714532*^9}},ExpressionUUID->"dfdd609c-2bb5-4a57-8352-\ 6b8e82230d03"] }, Open ]], @@ -451,86 +451,86 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"X", " ", "=", " ", - RowBox[{"Flatten", "[", - RowBox[{"Append", "[", - RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"Flatten", "[", + RowBox[{"Append", "[", + RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"c", " ", "=", " ", "1"}], ";"}], "\[IndentingNewLine]", + RowBox[{"c", " ", "=", " ", "1"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"rk4Pos", "=", - RowBox[{"{", "}"}]}], ";"}], "\[IndentingNewLine]", - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"rk4Pos", "=", + RowBox[{"{", "}"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"If", "[", + RowBox[{"If", "[", RowBox[{ - RowBox[{"i", " ", "\[Equal]", " ", - RowBox[{"timeList", "[", - RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", + RowBox[{"i", " ", "\[Equal]", " ", + RowBox[{"timeList", "[", + RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", RowBox[{ - RowBox[{"Print", "[", - RowBox[{"i", ",", "\"\< \>\"", ",", - RowBox[{"CForm", "[", + RowBox[{"Print", "[", + RowBox[{"i", ",", "\"\< \>\"", ",", + RowBox[{"CForm", "[", RowBox[{ - RowBox[{"X", "[", - RowBox[{"[", + RowBox[{"X", "[", + RowBox[{"[", RowBox[{"1", ";;", "3"}], "]"}], "]"}], "1000"}], "]"}]}], "]"}], - ";", "\[IndentingNewLine]", - RowBox[{"c", " ", "=", " ", - RowBox[{"c", "+", "1"}]}], ";", "\[IndentingNewLine]", - RowBox[{"AppendTo", "[", - RowBox[{"rk4Pos", ",", - RowBox[{"X", "[", - RowBox[{"[", - RowBox[{"1", ";;", "3"}], "]"}], "]"}]}], "]"}], ";"}]}], - "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"k1", " ", "=", " ", - RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"X2", " ", "=", " ", - RowBox[{"X", " ", "+", " ", + ";", "\[IndentingNewLine]", + RowBox[{"c", " ", "=", " ", + RowBox[{"c", "+", "1"}]}], ";", "\[IndentingNewLine]", + RowBox[{"AppendTo", "[", + RowBox[{"rk4Pos", ",", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"1", ";;", "3"}], "]"}], "]"}]}], "]"}], ";"}]}], + "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"k1", " ", "=", " ", + RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"X2", " ", "=", " ", + RowBox[{"X", " ", "+", " ", RowBox[{ - RowBox[{"h", "/", "2"}], " ", "k1"}]}]}], ";", "\[IndentingNewLine]", - RowBox[{"k2", " ", "=", " ", - RowBox[{"F", "[", "X2", "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"X3", " ", "=", " ", - RowBox[{"X", " ", "+", " ", + RowBox[{"h", "/", "2"}], " ", "k1"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"k2", " ", "=", " ", + RowBox[{"F", "[", "X2", "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"X3", " ", "=", " ", + RowBox[{"X", " ", "+", " ", RowBox[{ - RowBox[{"h", "/", "2"}], " ", "k2"}]}]}], ";", "\[IndentingNewLine]", - RowBox[{"k3", " ", "=", " ", - RowBox[{"F", "[", "X3", "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"X4", " ", "=", " ", - RowBox[{"X", " ", "+", " ", - RowBox[{"h", " ", "k3"}]}]}], ";", "\[IndentingNewLine]", - RowBox[{"k4", " ", "=", " ", - RowBox[{"F", "[", "X4", "]"}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"X", " ", "=", " ", - RowBox[{"X", " ", "+", " ", + RowBox[{"h", "/", "2"}], " ", "k2"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"k3", " ", "=", " ", + RowBox[{"F", "[", "X3", "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"X4", " ", "=", " ", + RowBox[{"X", " ", "+", " ", + RowBox[{"h", " ", "k3"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"k4", " ", "=", " ", + RowBox[{"F", "[", "X4", "]"}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"X", " ", "+", " ", RowBox[{ - RowBox[{"h", "/", "6"}], " ", - RowBox[{"(", - RowBox[{"k1", " ", "+", " ", - RowBox[{"2", " ", "k2"}], " ", "+", " ", - RowBox[{"2", " ", "k3"}], " ", "+", " ", "k4"}], ")"}]}]}]}], ";"}], - "\[IndentingNewLine]", "\[IndentingNewLine]", ",", - RowBox[{"{", + RowBox[{"h", "/", "6"}], " ", + RowBox[{"(", + RowBox[{"k1", " ", "+", " ", + RowBox[{"2", " ", "k2"}], " ", "+", " ", + RowBox[{"2", " ", "k3"}], " ", "+", " ", "k4"}], ")"}]}]}]}], ";"}], + "\[IndentingNewLine]", "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "Tmax", ",", "h"}], "}"}]}], "]"}]}], "Input",\ CellChangeTimes->{{3.690658089837873*^9, 3.690658435879587*^9}, { - 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, + 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, 3.690658693323777*^9}, {3.690658737820347*^9, 3.6906587448498087`*^9}, { - 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, + 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, 3.690658820023254*^9}, {3.690658870471861*^9, 3.690658907468793*^9}, { - 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, + 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, 3.690659162340767*^9}, {3.6906592323424683`*^9, 3.690659242248395*^9}, { - 3.690810542777762*^9, 3.690810577166408*^9}, {3.690810613449697*^9, + 3.690810542777762*^9, 3.690810577166408*^9}, {3.690810613449697*^9, 3.6908106259732733`*^9}, {3.690811046321291*^9, 3.6908110550383167`*^9}, { - 3.690811096531872*^9, 3.6908111095022383`*^9}, {3.690811144254981*^9, + 3.690811096531872*^9, 3.6908111095022383`*^9}, {3.690811144254981*^9, 3.690811165268138*^9}, {3.690910989398423*^9, 3.690910994110746*^9}, { - 3.709926858239127*^9, 3.7099268889142942`*^9}, {3.70992701264898*^9, + 3.709926858239127*^9, 3.7099268889142942`*^9}, {3.70992701264898*^9, 3.709927017356965*^9}},ExpressionUUID->"349967f1-ebbc-4594-a4ae-\ b9e857614bd6"], @@ -538,104 +538,104 @@ Cell[CellGroupData[{ Cell[BoxData[ InterpretationBox[ - RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-2.8168016010234915e6,5.248174846916147e6,3.\ 677157264677297e6)\"\>", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[0, " ", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + SequenceForm[0, " ", + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}]], Editable->False]], "Print", CellChangeTimes->{{3.690811097989266*^9, 3.690811109848225*^9}, { - 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, - 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, - 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, - 3.7099268230383253`*^9, 3.7099268898884687`*^9, + 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, + 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, + 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, + 3.7099268230383253`*^9, 3.7099268898884687`*^9, 3.7099270178242483`*^9},ExpressionUUID->"a524e1b7-bd43-4f6d-896f-\ 5296ef3cb3cd"], Cell[BoxData[ InterpretationBox[ - RowBox[{"1080", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"1080", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-6.379381726549218e6,-1.4688565370540658e6,2.\ 4807857675497606e6)\"\>", - CForm[{-6.379381726549218*^6, -1.4688565370540658`*^6, + CForm[{-6.379381726549218*^6, -1.4688565370540658`*^6, 2.4807857675497606`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[1080, " ", - CForm[{-6.379381726549218*^6, -1.4688565370540658`*^6, + SequenceForm[1080, " ", + CForm[{-6.379381726549218*^6, -1.4688565370540658`*^6, 2.4807857675497606`*^6}]], Editable->False]], "Print", CellChangeTimes->{{3.690811097989266*^9, 3.690811109848225*^9}, { - 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, - 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, - 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, - 3.7099268230383253`*^9, 3.7099268898884687`*^9, + 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, + 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, + 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, + 3.7099268230383253`*^9, 3.7099268898884687`*^9, 3.7099270178347588`*^9},ExpressionUUID->"78d6d748-e935-416b-8065-\ 0e6272cf2aea"], Cell[BoxData[ InterpretationBox[ - RowBox[{"2160", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"2160", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-2.230094305694789e6,-6.410420020364709e6,-1.\ 7146277675541767e6)\"\>", CForm[{-2.230094305694789*^6, -6.410420020364709*^6, \ -1.7146277675541767`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[2160, " ", + SequenceForm[2160, " ", CForm[{-2.230094305694789*^6, -6.410420020364709*^6, \ -1.7146277675541767`*^6}]], Editable->False]], "Print", CellChangeTimes->{{3.690811097989266*^9, 3.690811109848225*^9}, { - 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, - 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, - 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, - 3.7099268230383253`*^9, 3.7099268898884687`*^9, + 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, + 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, + 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, + 3.7099268230383253`*^9, 3.7099268898884687`*^9, 3.709927017843713*^9},ExpressionUUID->"30dfebb1-f16a-4c8a-ab66-\ 298a1dfea589"], Cell[BoxData[ InterpretationBox[ - RowBox[{"3240", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"3240", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(4.614900659014343e6,-3.60224207689023e6,-3.\ 837022825958977e6)\"\>", CForm[{4.614900659014343*^6, -3.60224207689023*^6, -3.837022825958977*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[3240, " ", + SequenceForm[3240, " ", CForm[{4.614900659014343*^6, -3.60224207689023*^6, -3.837022825958977*^6}]], Editable->False]], "Print", CellChangeTimes->{{3.690811097989266*^9, 3.690811109848225*^9}, { - 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, - 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, - 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, - 3.7099268230383253`*^9, 3.7099268898884687`*^9, + 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, + 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, + 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, + 3.7099268230383253`*^9, 3.7099268898884687`*^9, 3.7099270178522997`*^9},ExpressionUUID->"935dc59b-f5b7-4aca-9a6f-\ 83e740978f26"], Cell[BoxData[ InterpretationBox[ - RowBox[{"4320", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"4320", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(5.879095186201691e6,3.561495655367985e6,-1.\ 3195821703218794e6)\"\>", - CForm[{5.879095186201691*^6, + CForm[{5.879095186201691*^6, 3.561495655367985*^6, -1.3195821703218794`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[4320, " ", - CForm[{5.879095186201691*^6, + SequenceForm[4320, " ", + CForm[{5.879095186201691*^6, 3.561495655367985*^6, -1.3195821703218794`*^6}]], Editable->False]], "Print", CellChangeTimes->{{3.690811097989266*^9, 3.690811109848225*^9}, { - 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, - 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, - 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, - 3.7099268230383253`*^9, 3.7099268898884687`*^9, + 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, + 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, + 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, + 3.7099268230383253`*^9, 3.7099268898884687`*^9, 3.709927017860919*^9},ExpressionUUID->"33c5a29e-5537-481b-b568-\ 3f24812d315e"] }, Open ]] @@ -645,12 +645,12 @@ Cell[CellGroupData[{ Cell[BoxData[ RowBox[{"Dimensions", "[", "rk4Pos", "]"}]], "Input", - CellChangeTimes->{{3.7099268927938957`*^9, + CellChangeTimes->{{3.7099268927938957`*^9, 3.7099268957711887`*^9}},ExpressionUUID->"cee7b493-7787-4ec1-87b0-\ bea3a6cde87b"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{"37", ",", "3"}], "}"}]], "Output", CellChangeTimes->{ 3.709926896145767*^9},ExpressionUUID->"f2136b7a-ddd1-40a7-9c24-\ @@ -668,70 +668,70 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"X", " ", "=", " ", - RowBox[{"Flatten", "[", - RowBox[{"Append", "[", - RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"Flatten", "[", + RowBox[{"Append", "[", + RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"c", "=", "1"}], ";"}], "\[IndentingNewLine]", + RowBox[{"c", "=", "1"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"rk2Pos", "=", - RowBox[{"{", "}"}]}], ";"}], "\[IndentingNewLine]", - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"rk2Pos", "=", + RowBox[{"{", "}"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"If", "[", + RowBox[{"If", "[", RowBox[{ - RowBox[{"i", " ", "\[Equal]", " ", - RowBox[{"timeList", "[", - RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", + RowBox[{"i", " ", "\[Equal]", " ", + RowBox[{"timeList", "[", + RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", RowBox[{ - RowBox[{"Print", "[", - RowBox[{"i", ",", "\"\< \>\"", ",", - RowBox[{"CForm", "[", + RowBox[{"Print", "[", + RowBox[{"i", ",", "\"\< \>\"", ",", + RowBox[{"CForm", "[", RowBox[{ - RowBox[{"X", "[", - RowBox[{"[", + RowBox[{"X", "[", + RowBox[{"[", RowBox[{"1", ";;", "3"}], "]"}], "]"}], "1000"}], "]"}]}], "]"}], - ";", "\[IndentingNewLine]", - RowBox[{"c", " ", "=", " ", - RowBox[{"c", "+", "1"}]}], ";", "\[IndentingNewLine]", - RowBox[{"AppendTo", "[", - RowBox[{"rk2Pos", ",", - RowBox[{"X", "[", - RowBox[{"[", - RowBox[{"1", ";;", "3"}], "]"}], "]"}]}], "]"}], ";"}]}], - "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"k1", " ", "=", " ", - RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"X2", " ", "=", " ", - RowBox[{"X", " ", "+", " ", - RowBox[{"h", " ", "k1"}]}]}], ";", "\[IndentingNewLine]", - RowBox[{"k2", " ", "=", " ", - RowBox[{"F", "[", "X2", "]"}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"X", " ", "=", " ", - RowBox[{"X", " ", "+", " ", + ";", "\[IndentingNewLine]", + RowBox[{"c", " ", "=", " ", + RowBox[{"c", "+", "1"}]}], ";", "\[IndentingNewLine]", + RowBox[{"AppendTo", "[", + RowBox[{"rk2Pos", ",", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"1", ";;", "3"}], "]"}], "]"}]}], "]"}], ";"}]}], + "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"k1", " ", "=", " ", + RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"X2", " ", "=", " ", + RowBox[{"X", " ", "+", " ", + RowBox[{"h", " ", "k1"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"k2", " ", "=", " ", + RowBox[{"F", "[", "X2", "]"}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"X", " ", "+", " ", RowBox[{ - RowBox[{"h", "/", "2"}], " ", - RowBox[{"(", - RowBox[{"k1", " ", "+", " ", "k2"}], ")"}]}]}]}], ";"}], - "\[IndentingNewLine]", "\[IndentingNewLine]", ",", - RowBox[{"{", + RowBox[{"h", "/", "2"}], " ", + RowBox[{"(", + RowBox[{"k1", " ", "+", " ", "k2"}], ")"}]}]}]}], ";"}], + "\[IndentingNewLine]", "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "Tmax", ",", "h"}], "}"}]}], "]"}]}], "Input",\ CellChangeTimes->{{3.690658089837873*^9, 3.690658435879587*^9}, { - 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, + 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, 3.690658693323777*^9}, {3.690658737820347*^9, 3.6906587448498087`*^9}, { - 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, + 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, 3.690658820023254*^9}, {3.690658870471861*^9, 3.690658907468793*^9}, { - 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, + 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, 3.690659162340767*^9}, {3.6906592323424683`*^9, 3.690659242248395*^9}, { - 3.690799638205999*^9, 3.690799657725514*^9}, {3.6908112751281033`*^9, + 3.690799638205999*^9, 3.690799657725514*^9}, {3.6908112751281033`*^9, 3.690811278031749*^9}, {3.6908113547561417`*^9, 3.690811357138974*^9}, { - 3.709927078609377*^9, + 3.709927078609377*^9, 3.709927109350202*^9}},ExpressionUUID->"66669507-c898-4826-8814-\ ebcfbecb7a45"], @@ -739,89 +739,89 @@ Cell[CellGroupData[{ Cell[BoxData[ InterpretationBox[ - RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-2.8168016010234915e6,5.248174846916147e6,3.\ 677157264677297e6)\"\>", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[0, " ", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + SequenceForm[0, " ", + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, - 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121446931887`*^9, + CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, + 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121446931887`*^9, 3.709927122623267*^9},ExpressionUUID->"83213bab-0471-4db4-aebd-\ b38f1fc63850"], Cell[BoxData[ InterpretationBox[ - RowBox[{"1080", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"1080", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-6.425636528569288e6,-1.466693214251768e6,2.\ 50438327358707e6)\"\>", CForm[{-6.425636528569288*^6, -1.466693214251768*^6, 2.50438327358707*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[1080, " ", + SequenceForm[1080, " ", CForm[{-6.425636528569288*^6, -1.466693214251768*^6, 2.50438327358707*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, - 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121446931887`*^9, + CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, + 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121446931887`*^9, 3.709927122634205*^9},ExpressionUUID->"d3efb001-5462-4496-8542-\ 472d660847ec"], Cell[BoxData[ InterpretationBox[ - RowBox[{"2160", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"2160", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-2.466642497083674e6,-6.509473992136429e6,-1.\ 6421621818735446e6)\"\>", CForm[{-2.466642497083674*^6, -6.509473992136429*^6, \ -1.6421621818735446`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[2160, " ", + SequenceForm[2160, " ", CForm[{-2.466642497083674*^6, -6.509473992136429*^6, \ -1.6421621818735446`*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, - 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121446931887`*^9, + CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, + 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121446931887`*^9, 3.7099271226435747`*^9},ExpressionUUID->"44eda348-3575-4e91-bb85-\ c1b2711aeb7d"], Cell[BoxData[ InterpretationBox[ - RowBox[{"3240", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"3240", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(4.342561337924192e6,-4.1593822658140697e6,-3.\ 947594705237753e6)\"\>", CForm[{ 4.342561337924192*^6, -4.1593822658140697`*^6, -3.947594705237753*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[3240, " ", + SequenceForm[3240, " ", CForm[{ 4.342561337924192*^6, -4.1593822658140697`*^6, -3.947594705237753*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, - 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121446931887`*^9, + CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, + 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121446931887`*^9, 3.7099271226525784`*^9},ExpressionUUID->"cc5db3ac-5bfc-420f-b5aa-\ 871e1cd663cc"], Cell[BoxData[ InterpretationBox[ - RowBox[{"4320", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"4320", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(6.279757158711852e6,2.8527385905952943e6,-1.\ 8260959147806289e6)\"\>", - CForm[{6.279757158711852*^6, + CForm[{6.279757158711852*^6, 2.8527385905952943`*^6, -1.8260959147806289`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[4320, " ", - CForm[{6.279757158711852*^6, + SequenceForm[4320, " ", + CForm[{6.279757158711852*^6, 2.8527385905952943`*^6, -1.8260959147806289`*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, - 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121446931887`*^9, + CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, + 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121446931887`*^9, 3.709927122661703*^9},ExpressionUUID->"e27a0029-d261-400a-b202-\ 11c31d37587b"] }, Open ]] @@ -838,62 +838,62 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"X", " ", "=", " ", - RowBox[{"Flatten", "[", - RowBox[{"Append", "[", - RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"Flatten", "[", + RowBox[{"Append", "[", + RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"c", "=", "1"}], ";"}], "\[IndentingNewLine]", + RowBox[{"c", "=", "1"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"rk1Pos", " ", "=", - RowBox[{"{", "}"}]}], ";"}], "\[IndentingNewLine]", - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"rk1Pos", " ", "=", + RowBox[{"{", "}"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"If", "[", + RowBox[{"If", "[", RowBox[{ - RowBox[{"i", " ", "\[Equal]", " ", - RowBox[{"timeList", "[", - RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", + RowBox[{"i", " ", "\[Equal]", " ", + RowBox[{"timeList", "[", + RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", RowBox[{ - RowBox[{"Print", "[", - RowBox[{"i", ",", "\"\< \>\"", ",", - RowBox[{"CForm", "[", + RowBox[{"Print", "[", + RowBox[{"i", ",", "\"\< \>\"", ",", + RowBox[{"CForm", "[", RowBox[{ - RowBox[{"X", "[", - RowBox[{"[", + RowBox[{"X", "[", + RowBox[{"[", RowBox[{"1", ";;", "3"}], "]"}], "]"}], "1000"}], "]"}]}], "]"}], - ";", "\[IndentingNewLine]", - RowBox[{"c", " ", "=", " ", - RowBox[{"c", "+", "1"}]}], ";", "\[IndentingNewLine]", - RowBox[{"AppendTo", "[", - RowBox[{"rk1Pos", ",", - RowBox[{"X", "[", - RowBox[{"[", - RowBox[{"1", ";;", "3"}], "]"}], "]"}]}], "]"}], ";"}]}], - "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"k1", " ", "=", " ", - RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"X", " ", "=", " ", - RowBox[{"X", " ", "+", " ", - RowBox[{"h", " ", "k1"}]}]}], ";"}], "\[IndentingNewLine]", - "\[IndentingNewLine]", "\[IndentingNewLine]", ",", - RowBox[{"{", + ";", "\[IndentingNewLine]", + RowBox[{"c", " ", "=", " ", + RowBox[{"c", "+", "1"}]}], ";", "\[IndentingNewLine]", + RowBox[{"AppendTo", "[", + RowBox[{"rk1Pos", ",", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"1", ";;", "3"}], "]"}], "]"}]}], "]"}], ";"}]}], + "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"k1", " ", "=", " ", + RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"X", " ", "+", " ", + RowBox[{"h", " ", "k1"}]}]}], ";"}], "\[IndentingNewLine]", + "\[IndentingNewLine]", "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "Tmax", ",", "h"}], "}"}]}], "]"}]}], "Input",\ CellChangeTimes->{{3.690658089837873*^9, 3.690658435879587*^9}, { - 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, + 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, 3.690658693323777*^9}, {3.690658737820347*^9, 3.6906587448498087`*^9}, { - 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, + 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, 3.690658820023254*^9}, {3.690658870471861*^9, 3.690658907468793*^9}, { - 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, + 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, 3.690659162340767*^9}, {3.690659280982617*^9, 3.690659292512804*^9}, { - 3.6908085454540243`*^9, 3.690808545791629*^9}, {3.690811281113037*^9, + 3.6908085454540243`*^9, 3.690808545791629*^9}, {3.690811281113037*^9, 3.6908112845344257`*^9}, {3.690811367512064*^9, 3.690811368872841*^9}, { - 3.709927085123609*^9, + 3.709927085123609*^9, 3.709927120172014*^9}},ExpressionUUID->"b6217755-c781-4355-b431-\ 1b0181c02e83"], @@ -901,89 +901,89 @@ Cell[CellGroupData[{ Cell[BoxData[ InterpretationBox[ - RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-2.8168016010234915e6,5.248174846916147e6,3.\ 677157264677297e6)\"\>", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[0, " ", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + SequenceForm[0, " ", + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, - 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144781008*^9, + CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, + 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144781008*^9, 3.709927124733724*^9},ExpressionUUID->"07a984d7-ad50-4e06-845c-\ a0d99672d931"], Cell[BoxData[ InterpretationBox[ - RowBox[{"1080", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"1080", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-7.061548530211288e6,-1.4488790844105487e6,2.\ 823580168201031e6)\"\>", - CForm[{-7.061548530211288*^6, -1.4488790844105487`*^6, + CForm[{-7.061548530211288*^6, -1.4488790844105487`*^6, 2.823580168201031*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[1080, " ", - CForm[{-7.061548530211288*^6, -1.4488790844105487`*^6, + SequenceForm[1080, " ", + CForm[{-7.061548530211288*^6, -1.4488790844105487`*^6, 2.823580168201031*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, - 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144781008*^9, + CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, + 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144781008*^9, 3.709927124743511*^9},ExpressionUUID->"35e6804e-1589-4117-8312-\ 9aecfb403cf2"], Cell[BoxData[ InterpretationBox[ - RowBox[{"2160", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"2160", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-4.831279689590867e6,-8.015202650472983e6,-1.\ 1434851461593418e6)\"\>", CForm[{-4.831279689590867*^6, -8.015202650472983*^6, \ -1.1434851461593418`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[2160, " ", + SequenceForm[2160, " ", CForm[{-4.831279689590867*^6, -8.015202650472983*^6, \ -1.1434851461593418`*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, - 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144781008*^9, + CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, + 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144781008*^9, 3.709927124755391*^9},ExpressionUUID->"5c010472-56a2-4ba2-b84a-\ 48477969d005"], Cell[BoxData[ InterpretationBox[ - RowBox[{"3240", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"3240", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(719606.5825106134,-1.0537603309084207e7,-4.\ 966060248346598e6)\"\>", CForm[{719606.5825106134, -1.0537603309084207`*^7, -4.966060248346598*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[3240, " ", + SequenceForm[3240, " ", CForm[{719606.5825106134, -1.0537603309084207`*^7, -4.966060248346598*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, - 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144781008*^9, + CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, + 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144781008*^9, 3.709927124765057*^9},ExpressionUUID->"8bb84f01-8867-4321-86ec-\ bf9027e19a43"], Cell[BoxData[ InterpretationBox[ - RowBox[{"4320", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"4320", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(6.431097055190775e6,-9.795566286964862e6,-7.\ 438012269629238e6)\"\>", CForm[{ 6.431097055190775*^6, -9.795566286964862*^6, -7.438012269629238*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[4320, " ", + SequenceForm[4320, " ", CForm[{ 6.431097055190775*^6, -9.795566286964862*^6, -7.438012269629238*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, - 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144781008*^9, + CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, + 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144781008*^9, 3.709927124773917*^9},ExpressionUUID->"12647a88-d793-4151-b68b-\ 24b8b36ffc91"] }, Open ]] @@ -993,40 +993,40 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell["Plot Results", "Subsection", - CellChangeTimes->{{3.709927060973318*^9, + CellChangeTimes->{{3.709927060973318*^9, 3.709927063006878*^9}},ExpressionUUID->"c54fe71c-881f-4a0b-9678-\ f6871fb327bb"], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"trueOrbit", "=", - RowBox[{"ParametricPlot3D", "[", + RowBox[{"trueOrbit", "=", + RowBox[{"ParametricPlot3D", "[", RowBox[{ RowBox[{ - RowBox[{"elem2rv", "[", + RowBox[{"elem2rv", "[", RowBox[{ - "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", - ",", "\[Omega]", ",", "fAngle"}], "]"}], "[", - RowBox[{"[", "1", "]"}], "]"}], ",", - RowBox[{"{", - RowBox[{"fAngle", ",", "f0", ",", - RowBox[{"f0", "+", - RowBox[{"2", "\[Pi]", "*", ".75"}]}]}], "}"}], ",", - "\[IndentingNewLine]", - RowBox[{"PlotStyle", "\[Rule]", - RowBox[{"{", - RowBox[{"Thick", ",", "Black", ",", "Dashed"}], "}"}]}]}], + "\[Mu]", ",", "sma", ",", "ecc", ",", "inc", ",", "\[CapitalOmega]", + ",", "\[Omega]", ",", "fAngle"}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}], ",", + RowBox[{"{", + RowBox[{"fAngle", ",", "f0", ",", + RowBox[{"f0", "+", + RowBox[{"2", "\[Pi]", "*", ".75"}]}]}], "}"}], ",", + "\[IndentingNewLine]", + RowBox[{"PlotStyle", "\[Rule]", + RowBox[{"{", + RowBox[{"Thick", ",", "Black", ",", "Dashed"}], "}"}]}]}], "]"}]}]], "Input", CellChangeTimes->{{3.709926639851205*^9, 3.709926759177766*^9}, { - 3.709926964085369*^9, + 3.709926964085369*^9, 3.709926965388302*^9}},ExpressionUUID->"985a79e8-8eb4-473a-ad5f-\ 3cc0a2fd0e80"], Cell[BoxData[ - Graphics3DBox[{{}, {}, + Graphics3DBox[{{}, {}, TagBox[ - {GrayLevel[0], Thickness[Large], Dashing[{Small, Small}], + {GrayLevel[0], Thickness[Large], Dashing[{Small, Small}], Line3DBox[CompressedData[" 1:eJwV13c8lf8XAHASUV80jIpQZkZDVIo6KSIV0jCSjBSaiLJ3NtdM5nXvNS7P XdY1ylGhhKIiRUtGG6Wk4ff8/nper/fr8zqvz3M+55zn86x2uXDo1DwBAYF8 @@ -1243,15 +1243,15 @@ i/yp0fdSLHgzW3xeN5GC+Qe2H0p1rYXZaFmBUVUWeFO3+Hv6UvB/Vgw9yg== DisplayFunction->Identity, FaceGridsStyle->Automatic, Method->{}, - PlotRange->{{-6387.398939957307, 6386.302456558513}, {-6514.892315527384, + PlotRange->{{-6387.398939957307, 6386.302456558513}, {-6514.892315527384, 5248.174475812641}, {-3843.078487065164, 3843.2409194944166`}}, PlotRangePadding->{ - Scaled[0.02], - Scaled[0.02], + Scaled[0.02], + Scaled[0.02], Scaled[0.02]}, Ticks->{Automatic, Automatic, Automatic}]], "Output", - CellChangeTimes->{{3.709926706476358*^9, 3.70992671821058*^9}, - 3.709926759691657*^9, + CellChangeTimes->{{3.709926706476358*^9, 3.70992671821058*^9}, + 3.709926759691657*^9, 3.7099269659151373`*^9},ExpressionUUID->"d43eb36d-c32e-4617-85a3-\ 002398200094"] }, Open ]], @@ -1259,72 +1259,72 @@ i/yp0fdSLHgzW3xeN5GC+Qe2H0p1rYXZaFmBUVUWeFO3+Hv6UvB/Vgw9yg== Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"p1", "=", - RowBox[{"Show", "[", "\[IndentingNewLine]", + RowBox[{"p1", "=", + RowBox[{"Show", "[", "\[IndentingNewLine]", RowBox[{ - RowBox[{"Graphics3D", "[", "\[IndentingNewLine]", - RowBox[{"{", - RowBox[{"Black", ",", " ", - RowBox[{"PointSize", "[", "Large", "]"}], ",", - RowBox[{"Point", "[", "ans", "]"}]}], "}"}], "\[IndentingNewLine]", - "]"}], ",", "\[IndentingNewLine]", - RowBox[{"Graphics3D", "[", - RowBox[{"{", - RowBox[{"Green", ",", - RowBox[{"Sphere", "[", + RowBox[{"Graphics3D", "[", "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"Black", ",", " ", + RowBox[{"PointSize", "[", "Large", "]"}], ",", + RowBox[{"Point", "[", "ans", "]"}]}], "}"}], "\[IndentingNewLine]", + "]"}], ",", "\[IndentingNewLine]", + RowBox[{"Graphics3D", "[", + RowBox[{"{", + RowBox[{"Green", ",", + RowBox[{"Sphere", "[", RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", "6378"}], "]"}]}], - "}"}], "]"}], ",", "\[IndentingNewLine]", "trueOrbit", ",", - "\[IndentingNewLine]", - RowBox[{"Graphics3D", "[", "\[IndentingNewLine]", - RowBox[{"{", - RowBox[{"Blue", ",", " ", - RowBox[{"PointSize", "[", "Large", "]"}], ",", - RowBox[{"Point", "[", "rk4Pos", "]"}]}], "}"}], "\[IndentingNewLine]", - "]"}], "\[IndentingNewLine]", ",", - RowBox[{"Graphics3D", "[", "\[IndentingNewLine]", - RowBox[{"{", - RowBox[{"Green", ",", " ", - RowBox[{"PointSize", "[", "Large", "]"}], ",", - RowBox[{"Point", "[", "rk2Pos", "]"}]}], "}"}], "\[IndentingNewLine]", - "]"}], "\[IndentingNewLine]", ",", - RowBox[{"Graphics3D", "[", "\[IndentingNewLine]", - RowBox[{"{", - RowBox[{"Red", ",", " ", - RowBox[{"PointSize", "[", "Large", "]"}], ",", - RowBox[{"Point", "[", "rk1Pos", "]"}]}], "}"}], "\[IndentingNewLine]", - "]"}], ",", "\[IndentingNewLine]", - RowBox[{"Axes", "\[Rule]", "True"}], ",", "\[IndentingNewLine]", - RowBox[{"AxesLabel", "\[Rule]", - RowBox[{"{", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", "6378"}], "]"}]}], + "}"}], "]"}], ",", "\[IndentingNewLine]", "trueOrbit", ",", + "\[IndentingNewLine]", + RowBox[{"Graphics3D", "[", "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"Blue", ",", " ", + RowBox[{"PointSize", "[", "Large", "]"}], ",", + RowBox[{"Point", "[", "rk4Pos", "]"}]}], "}"}], "\[IndentingNewLine]", + "]"}], "\[IndentingNewLine]", ",", + RowBox[{"Graphics3D", "[", "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"Green", ",", " ", + RowBox[{"PointSize", "[", "Large", "]"}], ",", + RowBox[{"Point", "[", "rk2Pos", "]"}]}], "}"}], "\[IndentingNewLine]", + "]"}], "\[IndentingNewLine]", ",", + RowBox[{"Graphics3D", "[", "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"Red", ",", " ", + RowBox[{"PointSize", "[", "Large", "]"}], ",", + RowBox[{"Point", "[", "rk1Pos", "]"}]}], "}"}], "\[IndentingNewLine]", + "]"}], ",", "\[IndentingNewLine]", + RowBox[{"Axes", "\[Rule]", "True"}], ",", "\[IndentingNewLine]", + RowBox[{"AxesLabel", "\[Rule]", + RowBox[{"{", RowBox[{ - "\"\\"", ",", " ", "\"\\"", ",", " ", - "\"\\""}], "}"}]}]}], "\[IndentingNewLine]", + "\"\\"", ",", " ", "\"\\"", ",", " ", + "\"\\""}], "}"}]}]}], "\[IndentingNewLine]", "]"}]}]], "Input", CellChangeTimes->{{3.709917397148383*^9, 3.70991741217104*^9}, { - 3.709917474329096*^9, 3.709917520912739*^9}, {3.709917564352306*^9, + 3.709917474329096*^9, 3.709917520912739*^9}, {3.709917564352306*^9, 3.7099175777489643`*^9}, {3.70991771089671*^9, 3.70991776477134*^9}, { - 3.709917801517527*^9, 3.709917833307193*^9}, {3.709917943710285*^9, + 3.709917801517527*^9, 3.709917833307193*^9}, {3.709917943710285*^9, 3.7099179454711246`*^9}, {3.7099180559580507`*^9, 3.709918066983081*^9}, { - 3.709926723903593*^9, 3.7099267322051086`*^9}, {3.7099267777294292`*^9, + 3.709926723903593*^9, 3.7099267322051086`*^9}, {3.7099267777294292`*^9, 3.70992677817037*^9}, {3.709926928220496*^9, 3.709926953443923*^9}, { - 3.709927205615677*^9, + 3.709927205615677*^9, 3.7099272533032093`*^9}},ExpressionUUID->"00273d30-3a21-4fcc-bd78-\ 9b895cb6981c"], Cell[BoxData[ Graphics3DBox[{ - {GrayLevel[0], PointSize[Large], - Point3DBox[{{-2816.8016010234965`, 5248.174846916143, - 3677.157264677299}, {-6383.219360156923, -916.7807198948866, + {GrayLevel[0], PointSize[Large], + Point3DBox[{{-2816.8016010234965`, 5248.174846916143, + 3677.157264677299}, {-6383.219360156923, -916.7807198948866, 2724.380357539382}, {-3224.2072322891595`, -6115.999754479678, \ -1098.9183578099735`}, { 3331.68993768218, -4871.326571349695, -3764.2955001357345`}, { - 6376.238488731508, 1506.6729816914842`, -2462.689393485881}}]}, - {RGBColor[0, 1, 0], SphereBox[{0, 0, 0}, 6378]}, {{}, {}, + 6376.238488731508, 1506.6729816914842`, -2462.689393485881}}]}, + {RGBColor[0, 1, 0], SphereBox[{0, 0, 0}, 6378]}, {{}, {}, TagBox[ - {GrayLevel[0], Thickness[Large], Dashing[{Small, Small}], + {GrayLevel[0], Thickness[Large], Dashing[{Small, Small}], Line3DBox[CompressedData[" 1:eJwV13c8lf8XAHASUV80jIpQZkZDVIo6KSIV0jCSjBSaiLJ3NtdM5nXvNS7P XdY1ylGhhKIiRUtGG6Wk4ff8/nper/fr8zqvz3M+55zn86x2uXDo1DwBAYF8 @@ -1536,40 +1536,40 @@ hBNGb6fI/87ISzb1jRRc4xgpzDhbC0+9CujjQiywf7fE7SeNgqcaQ7Llz9SC i/yp0fdSLHgzW3xeN5GC+Qe2H0p1rYXZaFmBUVUWeFO3+Hv6UvB/Vgw9yg== "]]}, - Annotation[#, "Charting`Private`Tag$21303#1"]& ]}, - {RGBColor[0, 0, 1], PointSize[Large], - Point3DBox[{{-2816.8016010234915`, 5248.174846916147, - 3677.157264677297}, {-6379.381726549218, -1468.856537054066, + Annotation[#, "Charting`Private`Tag$21303#1"]& ]}, + {RGBColor[0, 0, 1], PointSize[Large], + Point3DBox[{{-2816.8016010234915`, 5248.174846916147, + 3677.157264677297}, {-6379.381726549218, -1468.856537054066, 2480.7857675497607`}, {-2230.094305694789, -6410.420020364709, \ -1714.6277675541767`}, { 4614.900659014344, -3602.2420768902302`, -3837.022825958977}, { - 5879.095186201691, 3561.495655367985, -1319.5821703218794`}}]}, - {RGBColor[0, 1, 0], PointSize[Large], - Point3DBox[{{-2816.8016010234915`, 5248.174846916147, - 3677.157264677297}, {-6425.636528569287, -1466.693214251768, + 5879.095186201691, 3561.495655367985, -1319.5821703218794`}}]}, + {RGBColor[0, 1, 0], PointSize[Large], + Point3DBox[{{-2816.8016010234915`, 5248.174846916147, + 3677.157264677297}, {-6425.636528569287, -1466.693214251768, 2504.38327358707}, {-2466.6424970836742`, -6509.473992136429, \ -1642.1621818735446`}, { 4342.561337924191, -4159.38226581407, -3947.5947052377533`}, { - 6279.757158711852, 2852.738590595294, -1826.0959147806288`}}]}, - {RGBColor[1, 0, 0], PointSize[Large], - Point3DBox[{{-2816.8016010234915`, 5248.174846916147, - 3677.157264677297}, {-7061.5485302112875`, -1448.8790844105488`, + 6279.757158711852, 2852.738590595294, -1826.0959147806288`}}]}, + {RGBColor[1, 0, 0], PointSize[Large], + Point3DBox[{{-2816.8016010234915`, 5248.174846916147, + 3677.157264677297}, {-7061.5485302112875`, -1448.8790844105488`, 2823.580168201031}, {-4831.279689590867, -8015.202650472983, \ -1143.4851461593419`}, { 719.6065825106134, -10537.603309084207`, -4966.060248346598}, { 6431.097055190775, -9795.566286964862, -7438.012269629238}}]}}, Axes->True, AxesLabel->{ - FormBox["\"X-Axis [km]\"", TraditionalForm], - FormBox["\"Y-Axis [km]\"", TraditionalForm], + FormBox["\"X-Axis [km]\"", TraditionalForm], + FormBox["\"Y-Axis [km]\"", TraditionalForm], FormBox["\"Z-Axis [km]\"", TraditionalForm]}, ImageSize->{381.4583885939831, 372.19696138982}, ImageSizeRaw->Automatic, ViewPoint->{1.2216030924311425`, -1.7421732622904207`, 2.6310678837162342`}, - - ViewVertical->{-0.16911758943449798`, 0.2173822493764948, + + ViewVertical->{-0.16911758943449798`, 0.2173822493764948, 1.1836017732479132`}]], "Output", - CellChangeTimes->{3.7099272189016953`*^9, + CellChangeTimes->{3.7099272189016953`*^9, 3.709927254656282*^9},ExpressionUUID->"4f863c9d-2b37-4bb7-a36a-\ 0a2ee395145b"] }, Open ]] @@ -1580,22 +1580,22 @@ Cell[CellGroupData[{ Cell["Integration Solution (J2, LEO, Earth)", "Section", CellChangeTimes->{{3.69065784668974*^9, 3.690657848815713*^9}, { - 3.690659220781633*^9, 3.6906592221884403`*^9}, {3.6927069512317343`*^9, + 3.690659220781633*^9, 3.6906592221884403`*^9}, {3.6927069512317343`*^9, 3.6927069583191357`*^9}, {3.692721891329471*^9, 3.692721893600409*^9}}], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Ak", " ", "=", " ", + RowBox[{"Ak", " ", "=", " ", RowBox[{ - RowBox[{"-", "4.841692638330"}], " ", - RowBox[{"10", "^", + RowBox[{"-", "4.841692638330"}], " ", + RowBox[{"10", "^", RowBox[{"-", "4"}]}]}]}]], "Input", CellChangeTimes->{{3.6927259708527803`*^9, 3.692725983987135*^9}}], Cell[BoxData[ RowBox[{"-", "0.000484169263833`"}]], "Output", - CellChangeTimes->{{3.69272597897396*^9, 3.692725984766035*^9}, + CellChangeTimes->{{3.69272597897396*^9, 3.692725984766035*^9}, 3.692727889243473*^9}] }, Open ]], @@ -1603,39 +1603,39 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"l", "=", "2"}], ";"}], "\[IndentingNewLine]", + RowBox[{"l", "=", "2"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"m", " ", "=", " ", "0"}], ";"}], "\[IndentingNewLine]", - RowBox[{"Nlm", "=", - RowBox[{"Sqrt", "[", + RowBox[{"m", " ", "=", " ", "0"}], ";"}], "\[IndentingNewLine]", + RowBox[{"Nlm", "=", + RowBox[{"Sqrt", "[", RowBox[{ RowBox[{ - RowBox[{"(", - RowBox[{"l", "-", "m"}], ")"}], "!"}], " ", - RowBox[{"(", - RowBox[{"2.", "-", "1"}], ")"}], + RowBox[{"(", + RowBox[{"l", "-", "m"}], ")"}], "!"}], " ", + RowBox[{"(", + RowBox[{"2.", "-", "1"}], ")"}], RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"2", "l"}], "+", "1"}], ")"}], "/", - RowBox[{"(", + RowBox[{"2", "l"}], "+", "1"}], ")"}], "/", + RowBox[{"(", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{"l", "+", "m"}], ")"}], "!"}], ")"}]}]}], "]"}]}]}], "Input", CellChangeTimes->{{3.692727586017049*^9, 3.6927276707993193`*^9}}], Cell[BoxData["2.23606797749979`"], "Output", - CellChangeTimes->{{3.69272764010435*^9, 3.692727671127564*^9}, + CellChangeTimes->{{3.69272764010435*^9, 3.692727671127564*^9}, 3.692727890041768*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"J2", " ", "=", " ", + RowBox[{"J2", " ", "=", " ", RowBox[{ RowBox[{"-", "Ak"}], "*", "Nlm"}]}]], "Input", - CellChangeTimes->{{3.6927276927594624`*^9, 3.692727693919928*^9}, + CellChangeTimes->{{3.6927276927594624`*^9, 3.692727693919928*^9}, 3.692727886963264*^9}], Cell[BoxData["0.0010826353865466185`"], "Output", @@ -1649,143 +1649,143 @@ Cell["Setup", "Subsection", Cell[BoxData[ RowBox[{ - RowBox[{"F", "[", "X_", "]"}], ":=", - RowBox[{"Block", "[", + RowBox[{"F", "[", "X_", "]"}], ":=", + RowBox[{"Block", "[", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{"ans", ",", "r", ",", "k", ",", "x", ",", "y", ",", "z"}], "}"}], - ",", "\[IndentingNewLine]", "\[IndentingNewLine]", + ",", "\[IndentingNewLine]", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", " ", "=", " ", "X"}], ";", "\[IndentingNewLine]", + RowBox[{"ans", " ", "=", " ", "X"}], ";", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "1", "]"}], "]"}], " ", "=", " ", - RowBox[{"X", "[", - RowBox[{"[", "4", "]"}], "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"ans", "[", + RowBox[{"[", "1", "]"}], "]"}], " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", "4", "]"}], "]"}]}], ";", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "2", "]"}], "]"}], " ", "=", " ", - RowBox[{"X", "[", - RowBox[{"[", "5", "]"}], "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"ans", "[", + RowBox[{"[", "2", "]"}], "]"}], " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", "5", "]"}], "]"}]}], ";", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "3", "]"}], "]"}], " ", "=", " ", - RowBox[{"X", "[", - RowBox[{"[", "6", "]"}], "]"}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"x", " ", "=", " ", - RowBox[{"X", "[", - RowBox[{"[", "1", "]"}], "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"y", " ", "=", " ", - RowBox[{"X", "[", - RowBox[{"[", "2", "]"}], "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"z", " ", "=", " ", - RowBox[{"X", "[", - RowBox[{"[", "3", "]"}], "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"r", " ", "=", " ", - RowBox[{"Sqrt", "[", + RowBox[{"ans", "[", + RowBox[{"[", "3", "]"}], "]"}], " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", "6", "]"}], "]"}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"x", " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", "1", "]"}], "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"y", " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", "2", "]"}], "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"z", " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"r", " ", "=", " ", + RowBox[{"Sqrt", "[", RowBox[{ - RowBox[{"x", "^", "2"}], "+", - RowBox[{"y", "^", "2"}], "+", - RowBox[{"z", "^", "2"}]}], "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"k", " ", "=", " ", + RowBox[{"x", "^", "2"}], "+", + RowBox[{"y", "^", "2"}], "+", + RowBox[{"z", "^", "2"}]}], "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"k", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"-", "3"}], "/", "2"}], " ", "J2", " ", - RowBox[{"(", - RowBox[{"\[Mu]Earth", "/", - RowBox[{"r", "^", "2"}]}], ")"}], " ", + RowBox[{"-", "3"}], "/", "2"}], " ", "J2", " ", + RowBox[{"(", + RowBox[{"\[Mu]Earth", "/", + RowBox[{"r", "^", "2"}]}], ")"}], " ", RowBox[{ - RowBox[{"(", - RowBox[{"reqEarth", "/", "r"}], ")"}], "^", "2"}]}]}], ";", - "\[IndentingNewLine]", "\[IndentingNewLine]", + RowBox[{"(", + RowBox[{"reqEarth", "/", "r"}], ")"}], "^", "2"}]}]}], ";", + "\[IndentingNewLine]", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "4", "]"}], "]"}], " ", "=", " ", + RowBox[{"ans", "[", + RowBox[{"[", "4", "]"}], "]"}], " ", "=", " ", RowBox[{ RowBox[{ RowBox[{ - RowBox[{"-", " ", "\[Mu]"}], "/", - RowBox[{"r", "^", "3"}]}], " ", "x"}], " ", "+", " ", - RowBox[{"k", - RowBox[{"(", - RowBox[{"1", "-", - RowBox[{"5", + RowBox[{"-", " ", "\[Mu]"}], "/", + RowBox[{"r", "^", "3"}]}], " ", "x"}], " ", "+", " ", + RowBox[{"k", + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"5", RowBox[{ - RowBox[{"(", - RowBox[{"z", "/", "r"}], ")"}], "^", "2"}]}]}], ")"}], - RowBox[{"x", "/", "r"}]}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"(", + RowBox[{"z", "/", "r"}], ")"}], "^", "2"}]}]}], ")"}], + RowBox[{"x", "/", "r"}]}]}]}], ";", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "5", "]"}], "]"}], " ", "=", " ", + RowBox[{"ans", "[", + RowBox[{"[", "5", "]"}], "]"}], " ", "=", " ", RowBox[{ RowBox[{ RowBox[{ - RowBox[{"-", " ", "\[Mu]"}], "/", - RowBox[{"r", "^", "3"}]}], " ", "y"}], " ", "+", " ", - RowBox[{"k", - RowBox[{"(", - RowBox[{"1", "-", - RowBox[{"5", + RowBox[{"-", " ", "\[Mu]"}], "/", + RowBox[{"r", "^", "3"}]}], " ", "y"}], " ", "+", " ", + RowBox[{"k", + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"5", RowBox[{ - RowBox[{"(", - RowBox[{"z", "/", "r"}], ")"}], "^", "2"}]}]}], ")"}], - RowBox[{"y", "/", "r"}]}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"(", + RowBox[{"z", "/", "r"}], ")"}], "^", "2"}]}]}], ")"}], + RowBox[{"y", "/", "r"}]}]}]}], ";", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ans", "[", - RowBox[{"[", "6", "]"}], "]"}], " ", "=", " ", + RowBox[{"ans", "[", + RowBox[{"[", "6", "]"}], "]"}], " ", "=", " ", RowBox[{ RowBox[{ RowBox[{ - RowBox[{"-", " ", "\[Mu]"}], "/", - RowBox[{"r", "^", "3"}]}], " ", "z"}], " ", "+", " ", - RowBox[{"k", - RowBox[{"(", - RowBox[{"3", "-", - RowBox[{"5", + RowBox[{"-", " ", "\[Mu]"}], "/", + RowBox[{"r", "^", "3"}]}], " ", "z"}], " ", "+", " ", + RowBox[{"k", + RowBox[{"(", + RowBox[{"3", "-", + RowBox[{"5", RowBox[{ - RowBox[{"(", - RowBox[{"z", "/", "r"}], ")"}], "^", "2"}]}]}], ")"}], " ", - RowBox[{"z", "/", "r"}]}]}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"Return", "[", "ans", "]"}], ";"}]}], "\[IndentingNewLine]", + RowBox[{"(", + RowBox[{"z", "/", "r"}], ")"}], "^", "2"}]}]}], ")"}], " ", + RowBox[{"z", "/", "r"}]}]}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"Return", "[", "ans", "]"}], ";"}]}], "\[IndentingNewLine]", "]"}]}]], "Input", CellChangeTimes->{{3.6906578694027767`*^9, 3.690658027163175*^9}, { - 3.69270693512519*^9, 3.692706939904386*^9}, {3.6927071346114397`*^9, + 3.69270693512519*^9, 3.692706939904386*^9}, {3.6927071346114397`*^9, 3.692707189455841*^9}, {3.692707228798436*^9, 3.692707259116948*^9}, { - 3.692720933728251*^9, 3.692720982146381*^9}, {3.692722250999217*^9, + 3.692720933728251*^9, 3.692720982146381*^9}, {3.692722250999217*^9, 3.692722280326672*^9}}], Cell[BoxData[{ RowBox[{ - RowBox[{"h", " ", "=", " ", "10"}], ";"}], "\[IndentingNewLine]", + RowBox[{"h", " ", "=", " ", "10"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"n", " ", "=", " ", - RowBox[{"Sqrt", "[", - RowBox[{"\[Mu]", "/", - RowBox[{"sma", "^", "3"}]}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"n", " ", "=", " ", + RowBox[{"Sqrt", "[", + RowBox[{"\[Mu]", "/", + RowBox[{"sma", "^", "3"}]}], "]"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"P", " ", "=", " ", - RowBox[{"2", " ", - RowBox[{"\[Pi]", "/", "n"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"P", " ", "=", " ", + RowBox[{"2", " ", + RowBox[{"\[Pi]", "/", "n"}]}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"Tmax", " ", "=", " ", + RowBox[{"Tmax", " ", "=", " ", RowBox[{"3", " ", "P"}]}], ";"}]}], "Input", CellChangeTimes->{ 3.690659238399549*^9, {3.690808414154882*^9, 3.6908084142425222`*^9}, { 3.690810617229479*^9, 3.690810621873021*^9}, 3.692721315478848*^9, { - 3.692721585078209*^9, 3.6927216262741327`*^9}, {3.692721770604747*^9, + 3.692721585078209*^9, 3.6927216262741327`*^9}, {3.692721770604747*^9, 3.692721781695468*^9}}], Cell[BoxData[ RowBox[{ - RowBox[{"timeList", " ", "=", " ", - RowBox[{"{", + RowBox[{"timeList", " ", "=", " ", + RowBox[{"{", RowBox[{ - "0", ",", "4350", ",", "8700", ",", " ", "13050", ",", " ", "17400"}], + "0", ",", "4350", ",", "8700", ",", " ", "13050", ",", " ", "17400"}], "}"}]}], ";"}]], "Input", CellChangeTimes->{{3.690810482620377*^9, 3.690810508911993*^9}, { - 3.690810586078121*^9, 3.690810588100753*^9}, {3.690910944395762*^9, + 3.690810586078121*^9, 3.690810588100753*^9}, {3.690910944395762*^9, 3.690910949592393*^9}, {3.690910983254991*^9, 3.69091098714532*^9}, { 3.692721904559225*^9, 3.692721924120841*^9}}], @@ -1809,82 +1809,82 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"X", " ", "=", " ", - RowBox[{"Flatten", "[", - RowBox[{"Append", "[", - RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"Flatten", "[", + RowBox[{"Append", "[", + RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"c", " ", "=", " ", "1"}], ";"}], "\[IndentingNewLine]", - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"c", " ", "=", " ", "1"}], ";"}], "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"If", "[", + RowBox[{"If", "[", RowBox[{ - RowBox[{"i", " ", "\[Equal]", " ", - RowBox[{"timeList", "[", - RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", + RowBox[{"i", " ", "\[Equal]", " ", + RowBox[{"timeList", "[", + RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", RowBox[{ - RowBox[{"Print", "[", - RowBox[{"i", ",", "\"\< \>\"", ",", - RowBox[{"CForm", "[", + RowBox[{"Print", "[", + RowBox[{"i", ",", "\"\< \>\"", ",", + RowBox[{"CForm", "[", RowBox[{ - RowBox[{"X", "[", - RowBox[{"[", + RowBox[{"X", "[", + RowBox[{"[", RowBox[{"1", ";;", "3"}], "]"}], "]"}], "1000"}], "]"}]}], "]"}], - ";", "\[IndentingNewLine]", - RowBox[{"c", " ", "=", " ", - RowBox[{"c", "+", "1"}]}], ";", "\[IndentingNewLine]", - RowBox[{"If", "[", + ";", "\[IndentingNewLine]", + RowBox[{"c", " ", "=", " ", + RowBox[{"c", "+", "1"}]}], ";", "\[IndentingNewLine]", + RowBox[{"If", "[", RowBox[{ - RowBox[{"c", ">", - RowBox[{"Length", "[", "timeList", "]"}]}], ",", " ", - RowBox[{"c", " ", "=", " ", - RowBox[{"Length", "[", "timeList", "]"}]}]}], "]"}], ";"}]}], - "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"k1", " ", "=", " ", - RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"X2", " ", "=", " ", - RowBox[{"X", " ", "+", " ", + RowBox[{"c", ">", + RowBox[{"Length", "[", "timeList", "]"}]}], ",", " ", + RowBox[{"c", " ", "=", " ", + RowBox[{"Length", "[", "timeList", "]"}]}]}], "]"}], ";"}]}], + "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"k1", " ", "=", " ", + RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"X2", " ", "=", " ", + RowBox[{"X", " ", "+", " ", RowBox[{ - RowBox[{"h", "/", "2"}], " ", "k1"}]}]}], ";", "\[IndentingNewLine]", - RowBox[{"k2", " ", "=", " ", - RowBox[{"F", "[", "X2", "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"X3", " ", "=", " ", - RowBox[{"X", " ", "+", " ", + RowBox[{"h", "/", "2"}], " ", "k1"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"k2", " ", "=", " ", + RowBox[{"F", "[", "X2", "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"X3", " ", "=", " ", + RowBox[{"X", " ", "+", " ", RowBox[{ - RowBox[{"h", "/", "2"}], " ", "k2"}]}]}], ";", "\[IndentingNewLine]", - RowBox[{"k3", " ", "=", " ", - RowBox[{"F", "[", "X3", "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"X4", " ", "=", " ", - RowBox[{"X", " ", "+", " ", - RowBox[{"h", " ", "k3"}]}]}], ";", "\[IndentingNewLine]", - RowBox[{"k4", " ", "=", " ", - RowBox[{"F", "[", "X4", "]"}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"X", " ", "=", " ", - RowBox[{"X", " ", "+", " ", + RowBox[{"h", "/", "2"}], " ", "k2"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"k3", " ", "=", " ", + RowBox[{"F", "[", "X3", "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"X4", " ", "=", " ", + RowBox[{"X", " ", "+", " ", + RowBox[{"h", " ", "k3"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"k4", " ", "=", " ", + RowBox[{"F", "[", "X4", "]"}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"X", " ", "+", " ", RowBox[{ - RowBox[{"h", "/", "6"}], " ", - RowBox[{"(", - RowBox[{"k1", " ", "+", " ", - RowBox[{"2", " ", "k2"}], " ", "+", " ", - RowBox[{"2", " ", "k3"}], " ", "+", " ", "k4"}], ")"}]}]}]}], ";"}], - "\[IndentingNewLine]", "\[IndentingNewLine]", ",", - RowBox[{"{", + RowBox[{"h", "/", "6"}], " ", + RowBox[{"(", + RowBox[{"k1", " ", "+", " ", + RowBox[{"2", " ", "k2"}], " ", "+", " ", + RowBox[{"2", " ", "k3"}], " ", "+", " ", "k4"}], ")"}]}]}]}], ";"}], + "\[IndentingNewLine]", "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "Tmax", ",", "h"}], "}"}]}], "]"}]}], "Input",\ CellChangeTimes->{{3.690658089837873*^9, 3.690658435879587*^9}, { - 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, + 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, 3.690658693323777*^9}, {3.690658737820347*^9, 3.6906587448498087`*^9}, { - 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, + 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, 3.690658820023254*^9}, {3.690658870471861*^9, 3.690658907468793*^9}, { - 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, + 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, 3.690659162340767*^9}, {3.6906592323424683`*^9, 3.690659242248395*^9}, { - 3.690810542777762*^9, 3.690810577166408*^9}, {3.690810613449697*^9, + 3.690810542777762*^9, 3.690810577166408*^9}, {3.690810613449697*^9, 3.6908106259732733`*^9}, {3.690811046321291*^9, 3.6908110550383167`*^9}, { - 3.690811096531872*^9, 3.6908111095022383`*^9}, {3.690811144254981*^9, + 3.690811096531872*^9, 3.6908111095022383`*^9}, {3.690811144254981*^9, 3.690811165268138*^9}, {3.690910989398423*^9, 3.690910994110746*^9}, { 3.692722155529421*^9, 3.692722175564185*^9}}], @@ -1892,102 +1892,102 @@ Cell[CellGroupData[{ Cell[BoxData[ InterpretationBox[ - RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-2.8168016010234915e6,5.248174846916147e6,3.\ 677157264677297e6)\"\>", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[0, " ", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + SequenceForm[0, " ", + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}]], Editable->False]], "Print", CellChangeTimes->{{3.690811097989266*^9, 3.690811109848225*^9}, { - 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, - 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, - 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, - 3.692721010287491*^9, 3.692721930861013*^9, 3.692722176998756*^9, + 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, + 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, + 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, + 3.692721010287491*^9, 3.692721930861013*^9, 3.692722176998756*^9, 3.6927222558235188`*^9, 3.692722351325509*^9, 3.692727722124918*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"4350", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"4350", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(5.787240887314784e6,3.7547029876434486e6,-1.\ 1653623184693705e6)\"\>", - CForm[{5.787240887314784*^6, + CForm[{5.787240887314784*^6, 3.7547029876434486`*^6, -1.1653623184693705`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[4350, " ", - CForm[{5.787240887314784*^6, + SequenceForm[4350, " ", + CForm[{5.787240887314784*^6, 3.7547029876434486`*^6, -1.1653623184693705`*^6}]], Editable->False]], "Print", CellChangeTimes->{{3.690811097989266*^9, 3.690811109848225*^9}, { - 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, - 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, - 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, - 3.692721010287491*^9, 3.692721930861013*^9, 3.692722176998756*^9, + 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, + 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, + 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, + 3.692721010287491*^9, 3.692721930861013*^9, 3.692722176998756*^9, 3.6927222558235188`*^9, 3.692722351325509*^9, 3.692727722173842*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"8700", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"8700", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(2.5908823579481775e6,-5.38042751586389e6,-3.\ 6401355110844015e6)\"\>", CForm[{ 2.5908823579481775`*^6, -5.38042751586389*^6, -3.6401355110844015`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[8700, " ", + SequenceForm[8700, " ", CForm[{ 2.5908823579481775`*^6, -5.38042751586389*^6, -3.6401355110844015`*^6}]], Editable->False]], "Print", CellChangeTimes->{{3.690811097989266*^9, 3.690811109848225*^9}, { - 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, - 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, - 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, - 3.692721010287491*^9, 3.692721930861013*^9, 3.692722176998756*^9, + 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, + 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, + 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, + 3.692721010287491*^9, 3.692721930861013*^9, 3.692722176998756*^9, 3.6927222558235188`*^9, 3.692722351325509*^9, 3.692727722237863*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"13050", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"13050", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-5.905673984221732e6,-3.5332208726054016e6,1.\ 2748483822117285e6)\"\>", - CForm[{-5.905673984221732*^6, -3.5332208726054016`*^6, + CForm[{-5.905673984221732*^6, -3.5332208726054016`*^6, 1.2748483822117285`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[13050, " ", - CForm[{-5.905673984221732*^6, -3.5332208726054016`*^6, + SequenceForm[13050, " ", + CForm[{-5.905673984221732*^6, -3.5332208726054016`*^6, 1.2748483822117285`*^6}]], Editable->False]], "Print", CellChangeTimes->{{3.690811097989266*^9, 3.690811109848225*^9}, { - 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, - 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, - 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, - 3.692721010287491*^9, 3.692721930861013*^9, 3.692722176998756*^9, + 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, + 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, + 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, + 3.692721010287491*^9, 3.692721930861013*^9, 3.692722176998756*^9, 3.6927222558235188`*^9, 3.692722351325509*^9, 3.692727722304596*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"17400", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"17400", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-2.3741237403798397e6,5.508156976353034e6,3.\ 6085612280591857e6)\"\>", - CForm[{-2.3741237403798397`*^6, 5.508156976353034*^6, + CForm[{-2.3741237403798397`*^6, 5.508156976353034*^6, 3.6085612280591857`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[17400, " ", - CForm[{-2.3741237403798397`*^6, 5.508156976353034*^6, + SequenceForm[17400, " ", + CForm[{-2.3741237403798397`*^6, 5.508156976353034*^6, 3.6085612280591857`*^6}]], Editable->False]], "Print", CellChangeTimes->{{3.690811097989266*^9, 3.690811109848225*^9}, { - 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, - 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, - 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, - 3.692721010287491*^9, 3.692721930861013*^9, 3.692722176998756*^9, + 3.6908111462625933`*^9, 3.6908111656733313`*^9}, 3.690811195869598*^9, + 3.690811343050273*^9, 3.69091078355617*^9, 3.690910952719233*^9, + 3.690910994601252*^9, 3.690911654327626*^9, 3.690912144548354*^9, + 3.692721010287491*^9, 3.692721930861013*^9, 3.692722176998756*^9, 3.6927222558235188`*^9, 3.692722351325509*^9, 3.6927277223684587`*^9}] }, Open ]] }, Open ]] @@ -2003,139 +2003,139 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"X", " ", "=", " ", - RowBox[{"Flatten", "[", - RowBox[{"Append", "[", - RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"Flatten", "[", + RowBox[{"Append", "[", + RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"c", "=", "1"}], ";"}], "\[IndentingNewLine]", - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"c", "=", "1"}], ";"}], "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"If", "[", + RowBox[{"If", "[", RowBox[{ - RowBox[{"i", " ", "\[Equal]", " ", - RowBox[{"timeList", "[", - RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", + RowBox[{"i", " ", "\[Equal]", " ", + RowBox[{"timeList", "[", + RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", RowBox[{ - RowBox[{"Print", "[", - RowBox[{"i", ",", "\"\< \>\"", ",", - RowBox[{"CForm", "[", + RowBox[{"Print", "[", + RowBox[{"i", ",", "\"\< \>\"", ",", + RowBox[{"CForm", "[", RowBox[{ - RowBox[{"X", "[", - RowBox[{"[", + RowBox[{"X", "[", + RowBox[{"[", RowBox[{"1", ";;", "3"}], "]"}], "]"}], "1000"}], "]"}]}], "]"}], - ";", "\[IndentingNewLine]", - RowBox[{"c", " ", "=", " ", + ";", "\[IndentingNewLine]", + RowBox[{"c", " ", "=", " ", RowBox[{"c", "+", "1"}]}], ";"}]}], "\[IndentingNewLine]", "]"}], ";", - "\[IndentingNewLine]", "\[IndentingNewLine]", - RowBox[{"k1", " ", "=", " ", - RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"X2", " ", "=", " ", - RowBox[{"X", " ", "+", " ", - RowBox[{"h", " ", "k1"}]}]}], ";", "\[IndentingNewLine]", - RowBox[{"k2", " ", "=", " ", - RowBox[{"F", "[", "X2", "]"}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"X", " ", "=", " ", - RowBox[{"X", " ", "+", " ", + "\[IndentingNewLine]", "\[IndentingNewLine]", + RowBox[{"k1", " ", "=", " ", + RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"X2", " ", "=", " ", + RowBox[{"X", " ", "+", " ", + RowBox[{"h", " ", "k1"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"k2", " ", "=", " ", + RowBox[{"F", "[", "X2", "]"}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"X", " ", "+", " ", RowBox[{ - RowBox[{"h", "/", "2"}], " ", - RowBox[{"(", - RowBox[{"k1", " ", "+", " ", "k2"}], ")"}]}]}]}], ";"}], - "\[IndentingNewLine]", "\[IndentingNewLine]", ",", - RowBox[{"{", + RowBox[{"h", "/", "2"}], " ", + RowBox[{"(", + RowBox[{"k1", " ", "+", " ", "k2"}], ")"}]}]}]}], ";"}], + "\[IndentingNewLine]", "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "Tmax", ",", "h"}], "}"}]}], "]"}]}], "Input",\ CellChangeTimes->{{3.690658089837873*^9, 3.690658435879587*^9}, { - 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, + 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, 3.690658693323777*^9}, {3.690658737820347*^9, 3.6906587448498087`*^9}, { - 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, + 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, 3.690658820023254*^9}, {3.690658870471861*^9, 3.690658907468793*^9}, { - 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, + 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, 3.690659162340767*^9}, {3.6906592323424683`*^9, 3.690659242248395*^9}, { - 3.690799638205999*^9, 3.690799657725514*^9}, {3.6908112751281033`*^9, + 3.690799638205999*^9, 3.690799657725514*^9}, {3.6908112751281033`*^9, 3.690811278031749*^9}, {3.6908113547561417`*^9, 3.690811357138974*^9}}], Cell[CellGroupData[{ Cell[BoxData[ InterpretationBox[ - RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-2.8168016010234915e6,5.248174846916147e6,3.\ 677157264677297e6)\"\>", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[0, " ", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + SequenceForm[0, " ", + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, + CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121446931887`*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"1080", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"1080", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-6.425636528569288e6,-1.466693214251768e6,2.\ 50438327358707e6)\"\>", CForm[{-6.425636528569288*^6, -1.466693214251768*^6, 2.50438327358707*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[1080, " ", + SequenceForm[1080, " ", CForm[{-6.425636528569288*^6, -1.466693214251768*^6, 2.50438327358707*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, + CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, 3.690911654447742*^9, 3.690911919156315*^9, 3.690912144699355*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"2160", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"2160", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-2.466642497083674e6,-6.509473992136429e6,-1.\ 6421621818735446e6)\"\>", CForm[{-2.466642497083674*^6, -6.509473992136429*^6, \ -1.6421621818735446`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[2160, " ", + SequenceForm[2160, " ", CForm[{-2.466642497083674*^6, -6.509473992136429*^6, \ -1.6421621818735446`*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, + CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, 3.690911654447742*^9, 3.690911919156315*^9, 3.690912144708363*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"3240", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"3240", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(4.342561337924192e6,-4.1593822658140697e6,-3.\ 947594705237753e6)\"\>", CForm[{ 4.342561337924192*^6, -4.1593822658140697`*^6, -3.947594705237753*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[3240, " ", + SequenceForm[3240, " ", CForm[{ 4.342561337924192*^6, -4.1593822658140697`*^6, -3.947594705237753*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, + CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, 3.690911654447742*^9, 3.690911919156315*^9, 3.690912144718368*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"4320", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"4320", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(6.279757158711852e6,2.8527385905952943e6,-1.\ 8260959147806289e6)\"\>", - CForm[{6.279757158711852*^6, + CForm[{6.279757158711852*^6, 2.8527385905952943`*^6, -1.8260959147806289`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[4320, " ", - CForm[{6.279757158711852*^6, + SequenceForm[4320, " ", + CForm[{6.279757158711852*^6, 2.8527385905952943`*^6, -1.8260959147806289`*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, + CellChangeTimes->{3.6908114254546022`*^9, 3.690910783628553*^9, 3.690911654447742*^9, 3.690911919156315*^9, 3.6909121447318068`*^9}] }, Open ]] }, Open ]] @@ -2151,131 +2151,131 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"X", " ", "=", " ", - RowBox[{"Flatten", "[", - RowBox[{"Append", "[", - RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"Flatten", "[", + RowBox[{"Append", "[", + RowBox[{"r0", ",", "v0"}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"c", "=", "1"}], ";"}], "\[IndentingNewLine]", - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"c", "=", "1"}], ";"}], "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"If", "[", + RowBox[{"If", "[", RowBox[{ - RowBox[{"i", " ", "\[Equal]", " ", - RowBox[{"timeList", "[", - RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", + RowBox[{"i", " ", "\[Equal]", " ", + RowBox[{"timeList", "[", + RowBox[{"[", "c", "]"}], "]"}]}], ",", "\[IndentingNewLine]", RowBox[{ - RowBox[{"Print", "[", - RowBox[{"i", ",", "\"\< \>\"", ",", - RowBox[{"CForm", "[", + RowBox[{"Print", "[", + RowBox[{"i", ",", "\"\< \>\"", ",", + RowBox[{"CForm", "[", RowBox[{ - RowBox[{"X", "[", - RowBox[{"[", + RowBox[{"X", "[", + RowBox[{"[", RowBox[{"1", ";;", "3"}], "]"}], "]"}], "1000"}], "]"}]}], "]"}], - ";", "\[IndentingNewLine]", - RowBox[{"c", " ", "=", " ", + ";", "\[IndentingNewLine]", + RowBox[{"c", " ", "=", " ", RowBox[{"c", "+", "1"}]}], ";"}]}], "\[IndentingNewLine]", "]"}], ";", - "\[IndentingNewLine]", "\[IndentingNewLine]", - RowBox[{"k1", " ", "=", " ", - RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"X", " ", "=", " ", - RowBox[{"X", " ", "+", " ", - RowBox[{"h", " ", "k1"}]}]}], ";"}], "\[IndentingNewLine]", - "\[IndentingNewLine]", "\[IndentingNewLine]", ",", - RowBox[{"{", + "\[IndentingNewLine]", "\[IndentingNewLine]", + RowBox[{"k1", " ", "=", " ", + RowBox[{"F", "[", "X", "]"}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"X", " ", "=", " ", + RowBox[{"X", " ", "+", " ", + RowBox[{"h", " ", "k1"}]}]}], ";"}], "\[IndentingNewLine]", + "\[IndentingNewLine]", "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "Tmax", ",", "h"}], "}"}]}], "]"}]}], "Input",\ CellChangeTimes->{{3.690658089837873*^9, 3.690658435879587*^9}, { - 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, + 3.690658482700925*^9, 3.690658485659665*^9}, {3.6906586906502733`*^9, 3.690658693323777*^9}, {3.690658737820347*^9, 3.6906587448498087`*^9}, { - 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, + 3.690658780828869*^9, 3.6906587820645103`*^9}, {3.6906588188054533`*^9, 3.690658820023254*^9}, {3.690658870471861*^9, 3.690658907468793*^9}, { - 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, + 3.690658966600305*^9, 3.690658998200219*^9}, {3.690659136714168*^9, 3.690659162340767*^9}, {3.690659280982617*^9, 3.690659292512804*^9}, { - 3.6908085454540243`*^9, 3.690808545791629*^9}, {3.690811281113037*^9, + 3.6908085454540243`*^9, 3.690808545791629*^9}, {3.690811281113037*^9, 3.6908112845344257`*^9}, {3.690811367512064*^9, 3.690811368872841*^9}}], Cell[CellGroupData[{ Cell[BoxData[ InterpretationBox[ - RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"0", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-2.8168016010234915e6,5.248174846916147e6,3.\ 677157264677297e6)\"\>", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[0, " ", - CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, + SequenceForm[0, " ", + CForm[{-2.8168016010234915`*^6, 5.248174846916147*^6, 3.677157264677297*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, + CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144781008*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"1080", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"1080", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-7.061548530211288e6,-1.4488790844105487e6,2.\ 823580168201031e6)\"\>", - CForm[{-7.061548530211288*^6, -1.4488790844105487`*^6, + CForm[{-7.061548530211288*^6, -1.4488790844105487`*^6, 2.823580168201031*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[1080, " ", - CForm[{-7.061548530211288*^6, -1.4488790844105487`*^6, + SequenceForm[1080, " ", + CForm[{-7.061548530211288*^6, -1.4488790844105487`*^6, 2.823580168201031*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, + CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, 3.690911654531302*^9, 3.690911965064349*^9, 3.6909121447857637`*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"2160", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"2160", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(-4.831279689590867e6,-8.015202650472983e6,-1.\ 1434851461593418e6)\"\>", CForm[{-4.831279689590867*^6, -8.015202650472983*^6, \ -1.1434851461593418`*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[2160, " ", + SequenceForm[2160, " ", CForm[{-4.831279689590867*^6, -8.015202650472983*^6, \ -1.1434851461593418`*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, + CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, 3.690911654531302*^9, 3.690911965064349*^9, 3.690912144793936*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"3240", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"3240", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(719606.5825106134,-1.0537603309084207e7,-4.\ 966060248346598e6)\"\>", CForm[{719606.5825106134, -1.0537603309084207`*^7, -4.966060248346598*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[3240, " ", + SequenceForm[3240, " ", CForm[{719606.5825106134, -1.0537603309084207`*^7, -4.966060248346598*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, + CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, 3.690911654531302*^9, 3.690911965064349*^9, 3.6909121447961273`*^9}], Cell[BoxData[ InterpretationBox[ - RowBox[{"4320", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", + RowBox[{"4320", "\[InvisibleSpace]", "\<\" \"\>", "\[InvisibleSpace]", InterpretationBox["\<\"List(6.431097055190775e6,-9.795566286964862e6,-7.\ 438012269629238e6)\"\>", CForm[{ 6.431097055190775*^6, -9.795566286964862*^6, -7.438012269629238*^6}], AutoDelete->True, Editable->True]}], - SequenceForm[4320, " ", + SequenceForm[4320, " ", CForm[{ 6.431097055190775*^6, -9.795566286964862*^6, -7.438012269629238*^6}]], Editable->False]], "Print", - CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, + CellChangeTimes->{3.690811369271599*^9, 3.690910783706669*^9, 3.690911654531302*^9, 3.690911965064349*^9, 3.6909121447983294`*^9}] }, Open ]] }, Open ]] @@ -2558,4 +2558,3 @@ Cell[95271, 2264, 619, 14, 24, "Print", "ExpressionUUID" -> \ } ] *) - diff --git a/docs/source/_images/static/_Support/test_scenario_AttEclipseUpdated.svg b/docs/source/_images/static/_Support/test_scenario_AttEclipseUpdated.svg index 78e2839833..b3d12e24f4 100644 --- a/docs/source/_images/static/_Support/test_scenario_AttEclipseUpdated.svg +++ b/docs/source/_images/static/_Support/test_scenario_AttEclipseUpdated.svg @@ -28,7 +28,7 @@ - Produced by OmniGraffle 7.8 + Produced by OmniGraffle 7.8 2018-07-30 20:44:46 +0000 diff --git a/docs/source/_images/static/qs-bsk-2b-order.svg b/docs/source/_images/static/qs-bsk-2b-order.svg index d5fe964c40..973cf8d701 100644 --- a/docs/source/_images/static/qs-bsk-2b-order.svg +++ b/docs/source/_images/static/qs-bsk-2b-order.svg @@ -21,235 +21,235 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -257,239 +257,239 @@ z - - - - - - - - - - - - @@ -514,110 +514,110 @@ z - - - - - - @@ -646,82 +646,82 @@ z - - - - - @@ -744,28 +744,28 @@ z - @@ -907,83 +907,83 @@ z - - - - @@ -1013,29 +1013,29 @@ z - @@ -1151,23 +1151,23 @@ z - diff --git a/docs/source/_images/static/scenario_AttEclipse.svg b/docs/source/_images/static/scenario_AttEclipse.svg index db19f14feb..44b39cf1a3 100644 --- a/docs/source/_images/static/scenario_AttEclipse.svg +++ b/docs/source/_images/static/scenario_AttEclipse.svg @@ -23,7 +23,7 @@ - Produced by OmniGraffle 7.8 + Produced by OmniGraffle 7.8 2018-07-20 21:24:07 +0000 diff --git a/docs/source/_images/static/scenario_basicOrbit_BSKSim.svg b/docs/source/_images/static/scenario_basicOrbit_BSKSim.svg index 57325bd6d1..34cf8d793f 100644 --- a/docs/source/_images/static/scenario_basicOrbit_BSKSim.svg +++ b/docs/source/_images/static/scenario_basicOrbit_BSKSim.svg @@ -23,7 +23,7 @@ - Produced by OmniGraffle 7.8 + Produced by OmniGraffle 7.8 2018-07-30 16:23:29 +0000 diff --git a/docs/source/_images/static/simpleDataConcept.svg b/docs/source/_images/static/simpleDataConcept.svg index e687595579..2594d20467 100644 --- a/docs/source/_images/static/simpleDataConcept.svg +++ b/docs/source/_images/static/simpleDataConcept.svg @@ -18,7 +18,7 @@ - Produced by OmniGraffle 7.11.3 + Produced by OmniGraffle 7.11.3 2019-12-03 23:09:58 +0000 diff --git a/docs/source/_images/static/test_scenarioAttitudeFeedback2T_TH.svg b/docs/source/_images/static/test_scenarioAttitudeFeedback2T_TH.svg index 6976f3990e..10e988ed65 100644 --- a/docs/source/_images/static/test_scenarioAttitudeFeedback2T_TH.svg +++ b/docs/source/_images/static/test_scenarioAttitudeFeedback2T_TH.svg @@ -18,7 +18,7 @@ - Produced by OmniGraffle 7.10.2 + Produced by OmniGraffle 7.10.2 2019-06-27 22:26:48 +0000 diff --git a/docs/source/_images/static/test_scenarioAttitudeFeedbackRWPower.svg b/docs/source/_images/static/test_scenarioAttitudeFeedbackRWPower.svg index f5abdd24fd..16ceff0ff9 100644 --- a/docs/source/_images/static/test_scenarioAttitudeFeedbackRWPower.svg +++ b/docs/source/_images/static/test_scenarioAttitudeFeedbackRWPower.svg @@ -38,7 +38,7 @@ - Produced by OmniGraffle 7.12.1 + Produced by OmniGraffle 7.12.1 2020-01-26 17:34:12 +0000 diff --git a/docs/source/_images/static/test_scenarioAttitudeGG.svg b/docs/source/_images/static/test_scenarioAttitudeGG.svg index e4d0c2358e..fb0a0dc9f0 100644 --- a/docs/source/_images/static/test_scenarioAttitudeGG.svg +++ b/docs/source/_images/static/test_scenarioAttitudeGG.svg @@ -13,7 +13,7 @@ - Produced by OmniGraffle 7.12.1 + Produced by OmniGraffle 7.12.1 2020-01-19 17:11:41 +0000 diff --git a/docs/source/_images/static/test_scenarioAttitudeGuidance.svg b/docs/source/_images/static/test_scenarioAttitudeGuidance.svg index 7fd1b8723b..a9c55d310e 100644 --- a/docs/source/_images/static/test_scenarioAttitudeGuidance.svg +++ b/docs/source/_images/static/test_scenarioAttitudeGuidance.svg @@ -1,7 +1,7 @@ - Produced by OmniGraffle 7.3 + Produced by OmniGraffle 7.3 2017-03-31 22:09:07 +0000 diff --git a/docs/source/_images/static/test_scenarioAttitudeSteering.svg b/docs/source/_images/static/test_scenarioAttitudeSteering.svg index baed0d97d4..f84e59c05c 100644 --- a/docs/source/_images/static/test_scenarioAttitudeSteering.svg +++ b/docs/source/_images/static/test_scenarioAttitudeSteering.svg @@ -1,7 +1,7 @@ - Produced by OmniGraffle 7.4.2 + Produced by OmniGraffle 7.4.2 2017-08-15 21:30:06 +0000 diff --git a/docs/source/_images/static/test_scenarioBasicOrbit.svg b/docs/source/_images/static/test_scenarioBasicOrbit.svg index 2cdb01ec34..00e5a0a6a1 100644 --- a/docs/source/_images/static/test_scenarioBasicOrbit.svg +++ b/docs/source/_images/static/test_scenarioBasicOrbit.svg @@ -1,7 +1,7 @@ - Produced by OmniGraffle 7.3 + Produced by OmniGraffle 7.3 2017-03-31 22:14:05 +0000 diff --git a/docs/source/_images/static/test_scenarioCSS.svg b/docs/source/_images/static/test_scenarioCSS.svg index f8e44bce25..daf07946d2 100644 --- a/docs/source/_images/static/test_scenarioCSS.svg +++ b/docs/source/_images/static/test_scenarioCSS.svg @@ -1,7 +1,7 @@ - Produced by OmniGraffle 7.4 + Produced by OmniGraffle 7.4 2017-07-25 21:16:29 +0000 diff --git a/docs/source/_images/static/test_scenarioDataToViz.svg b/docs/source/_images/static/test_scenarioDataToViz.svg index 9f3d61d110..27339045e5 100644 --- a/docs/source/_images/static/test_scenarioDataToViz.svg +++ b/docs/source/_images/static/test_scenarioDataToViz.svg @@ -13,7 +13,7 @@ - Produced by OmniGraffle 7.15 + Produced by OmniGraffle 7.15 2020-05-13 17:29:03 +0000 diff --git a/docs/source/_images/static/test_scenarioFormationBasic.svg b/docs/source/_images/static/test_scenarioFormationBasic.svg index a0bbb2a9d7..47f47ca302 100644 --- a/docs/source/_images/static/test_scenarioFormationBasic.svg +++ b/docs/source/_images/static/test_scenarioFormationBasic.svg @@ -23,7 +23,7 @@ - Produced by OmniGraffle 7.12 + Produced by OmniGraffle 7.12 2019-12-30 00:56:27 +0000 diff --git a/docs/source/_images/static/test_scenarioFormationMeanOEFeedback.svg b/docs/source/_images/static/test_scenarioFormationMeanOEFeedback.svg index 022e5fff1b..66d394c1ee 100644 --- a/docs/source/_images/static/test_scenarioFormationMeanOEFeedback.svg +++ b/docs/source/_images/static/test_scenarioFormationMeanOEFeedback.svg @@ -13,7 +13,7 @@ - Produced by OmniGraffle 7.14.1 + Produced by OmniGraffle 7.14.1 2020-03-27 07:46:46 +0000 diff --git a/docs/source/_images/static/test_scenarioFormationReconfig.svg b/docs/source/_images/static/test_scenarioFormationReconfig.svg index f2c237df9f..60af60acdc 100644 --- a/docs/source/_images/static/test_scenarioFormationReconfig.svg +++ b/docs/source/_images/static/test_scenarioFormationReconfig.svg @@ -18,7 +18,7 @@ - Produced by OmniGraffle 7.15 + Produced by OmniGraffle 7.15 2020-04-05 10:32:15 +0000 diff --git a/docs/source/_images/static/test_scenarioHingedRigidBody.svg b/docs/source/_images/static/test_scenarioHingedRigidBody.svg index e52ee8d019..3d4698063a 100644 --- a/docs/source/_images/static/test_scenarioHingedRigidBody.svg +++ b/docs/source/_images/static/test_scenarioHingedRigidBody.svg @@ -1,7 +1,7 @@ - Produced by OmniGraffle 7.4 + Produced by OmniGraffle 7.4 2017-08-25 17:23:15 +0000 diff --git a/docs/source/_images/static/test_scenarioIntegrators.svg b/docs/source/_images/static/test_scenarioIntegrators.svg index 139ba8f3c4..70796c408e 100644 --- a/docs/source/_images/static/test_scenarioIntegrators.svg +++ b/docs/source/_images/static/test_scenarioIntegrators.svg @@ -1,7 +1,7 @@ - Produced by OmniGraffle 7.3 + Produced by OmniGraffle 7.3 2017-03-31 22:15:25 +0000 diff --git a/docs/source/_images/static/test_scenarioOrbitManeuver.svg b/docs/source/_images/static/test_scenarioOrbitManeuver.svg index 4dd48afc4c..b10920a59a 100644 --- a/docs/source/_images/static/test_scenarioOrbitManeuver.svg +++ b/docs/source/_images/static/test_scenarioOrbitManeuver.svg @@ -1,7 +1,7 @@ - Produced by OmniGraffle 7.3 + Produced by OmniGraffle 7.3 2017-03-31 22:15:42 +0000 diff --git a/docs/source/_images/static/test_scenarioOrbitMultiBody.svg b/docs/source/_images/static/test_scenarioOrbitMultiBody.svg index 6579585f1e..9f75c9884e 100644 --- a/docs/source/_images/static/test_scenarioOrbitMultiBody.svg +++ b/docs/source/_images/static/test_scenarioOrbitMultiBody.svg @@ -1,7 +1,7 @@ - Produced by OmniGraffle 7.3 + Produced by OmniGraffle 7.3 2017-03-31 22:15:56 +0000 diff --git a/docs/source/_images/static/test_scenarioSmallBodyLandmarks.svg b/docs/source/_images/static/test_scenarioSmallBodyLandmarks.svg index 0f0c9e60ab..f9b6abcebb 100644 --- a/docs/source/_images/static/test_scenarioSmallBodyLandmarks.svg +++ b/docs/source/_images/static/test_scenarioSmallBodyLandmarks.svg @@ -34,7 +34,7 @@ inkscape:window-maximized="0" inkscape:current-layer="svg159" /> Produced by OmniGraffle 7.3 + id="metadata2"> Produced by OmniGraffle 7.3 2017-03-31 22:09:07 +0000 Produced by OmniGraffle 7.3 + id="metadata2"> Produced by OmniGraffle 7.3 2017-03-31 22:16:21 +0000 - Produced by OmniGraffle 7.8 + Produced by OmniGraffle 7.8 2018-07-30 21:05:00 +0000 diff --git a/docs/source/_images/static/test_scenario_AttGuidHyperbolic.svg b/docs/source/_images/static/test_scenario_AttGuidHyperbolic.svg index ff7e86f8f2..25fccb8901 100644 --- a/docs/source/_images/static/test_scenario_AttGuidHyperbolic.svg +++ b/docs/source/_images/static/test_scenario_AttGuidHyperbolic.svg @@ -28,7 +28,7 @@ - Produced by OmniGraffle 7.8 + Produced by OmniGraffle 7.8 2018-07-31 21:49:33 +0000 diff --git a/docs/source/_images/static/test_scenario_AttGuidance.svg b/docs/source/_images/static/test_scenario_AttGuidance.svg index ee2c2e450b..2ee1556105 100644 --- a/docs/source/_images/static/test_scenario_AttGuidance.svg +++ b/docs/source/_images/static/test_scenario_AttGuidance.svg @@ -23,7 +23,7 @@ - Produced by OmniGraffle 7.8 + Produced by OmniGraffle 7.8 2018-07-31 21:39:44 +0000 diff --git a/docs/source/_images/static/test_scenario_AttSteering.svg b/docs/source/_images/static/test_scenario_AttSteering.svg index efafffc176..1a2508b3bd 100644 --- a/docs/source/_images/static/test_scenario_AttSteering.svg +++ b/docs/source/_images/static/test_scenario_AttSteering.svg @@ -23,7 +23,7 @@ - Produced by OmniGraffle 7.8 + Produced by OmniGraffle 7.8 2018-07-31 21:21:14 +0000 diff --git a/docs/source/_images/static/test_scenario_BasicOrbitFormation.svg b/docs/source/_images/static/test_scenario_BasicOrbitFormation.svg index 27e53386c1..7cddf2a86b 100644 --- a/docs/source/_images/static/test_scenario_BasicOrbitFormation.svg +++ b/docs/source/_images/static/test_scenario_BasicOrbitFormation.svg @@ -23,7 +23,7 @@ - Produced by OmniGraffle 7.9.4 + Produced by OmniGraffle 7.9.4 2019-01-04 18:05:46 +0000 diff --git a/docs/source/_images/static/test_scenario_InertialPoint.svg b/docs/source/_images/static/test_scenario_InertialPoint.svg index 935ad52b08..9b7014e903 100644 --- a/docs/source/_images/static/test_scenario_InertialPoint.svg +++ b/docs/source/_images/static/test_scenario_InertialPoint.svg @@ -28,7 +28,7 @@ - Produced by OmniGraffle 7.8 + Produced by OmniGraffle 7.8 2018-07-30 19:55:40 +0000 diff --git a/docs/source/_images/static/test_scenario_MagneticField.svg b/docs/source/_images/static/test_scenario_MagneticField.svg index c71ee44e83..5037571204 100644 --- a/docs/source/_images/static/test_scenario_MagneticField.svg +++ b/docs/source/_images/static/test_scenario_MagneticField.svg @@ -13,7 +13,7 @@ - Produced by OmniGraffle 7.9.4 + Produced by OmniGraffle 7.9.4 2019-03-18 15:23:07 +0000 diff --git a/docs/source/_images/static/test_scenario_MagneticFieldCenteredDipole.svg b/docs/source/_images/static/test_scenario_MagneticFieldCenteredDipole.svg index cef0a97ac3..b9c24a2e3a 100644 --- a/docs/source/_images/static/test_scenario_MagneticFieldCenteredDipole.svg +++ b/docs/source/_images/static/test_scenario_MagneticFieldCenteredDipole.svg @@ -13,7 +13,7 @@ - Produced by OmniGraffle 7.12 + Produced by OmniGraffle 7.12 2019-11-10 21:49:40 +0000 diff --git a/docs/source/_images/static/test_scenario_MagneticFieldWMM.svg b/docs/source/_images/static/test_scenario_MagneticFieldWMM.svg index 40c6576855..4d3769fb12 100644 --- a/docs/source/_images/static/test_scenario_MagneticFieldWMM.svg +++ b/docs/source/_images/static/test_scenario_MagneticFieldWMM.svg @@ -13,7 +13,7 @@ - Produced by OmniGraffle 7.12 + Produced by OmniGraffle 7.12 2019-11-10 21:48:03 +0000 diff --git a/docs/source/_images/static/test_scenario_RelativePointingFormation.svg b/docs/source/_images/static/test_scenario_RelativePointingFormation.svg index b79766cab2..4ab59afc8a 100644 --- a/docs/source/_images/static/test_scenario_RelativePointingFormation.svg +++ b/docs/source/_images/static/test_scenario_RelativePointingFormation.svg @@ -23,7 +23,7 @@ - Produced by OmniGraffle 7.9.4 + Produced by OmniGraffle 7.9.4 2019-01-08 21:59:39 +0000 diff --git a/docs/source/_images/static/test_scenario_basicOrbit_v1.1.svg b/docs/source/_images/static/test_scenario_basicOrbit_v1.1.svg index 3ddae717b1..6ef14f7c42 100644 --- a/docs/source/_images/static/test_scenario_basicOrbit_v1.1.svg +++ b/docs/source/_images/static/test_scenario_basicOrbit_v1.1.svg @@ -28,7 +28,7 @@ - Produced by OmniGraffle 7.8 + Produced by OmniGraffle 7.8 2018-07-31 20:20:53 +0000 diff --git a/docs/source/resources/mac_fix_path.pth b/docs/source/resources/mac_fix_path.pth index 6c4b2be55e..80a6b82f90 100644 --- a/docs/source/resources/mac_fix_path.pth +++ b/docs/source/resources/mac_fix_path.pth @@ -1 +1 @@ -import sys; std_paths=[p for p in sys.path if p.startswith('/System/') and not '/Extras/' in p]; sys.path=[p for p in sys.path if not p.startswith('/System/')]+std_paths \ No newline at end of file +import sys; std_paths=[p for p in sys.path if p.startswith('/System/') and not '/Extras/' in p]; sys.path=[p for p in sys.path if not p.startswith('/System/')]+std_paths diff --git a/examples/BskSim/BSK_masters.py b/examples/BskSim/BSK_masters.py index f1f4e0e02d..530c6c72e7 100644 --- a/examples/BskSim/BSK_masters.py +++ b/examples/BskSim/BSK_masters.py @@ -46,7 +46,7 @@ def __init__(self, fswRate=0.1, dynRate=0.1): self.FSWProcessName = None self.dynProc = None self.fswProc = None - + self.oneTimeRWFaultFlag = 0 self.oneTimeFaultTime = -1 self.repeatRWFaultFlag = 0 diff --git a/examples/BskSim/models/BSK_FormationDynamics.py b/examples/BskSim/models/BSK_FormationDynamics.py index 449de3f504..5c2e4bc65e 100644 --- a/examples/BskSim/models/BSK_FormationDynamics.py +++ b/examples/BskSim/models/BSK_FormationDynamics.py @@ -140,4 +140,3 @@ def InitAllDynObjects(self): self.SetSimpleNavObject() self.SetReactionWheelDynEffector() self.SetExternalForceTorqueObject() - diff --git a/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatDynamics.py b/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatDynamics.py index c3ed7ffe4b..61462d85d4 100644 --- a/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatDynamics.py +++ b/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatDynamics.py @@ -183,7 +183,7 @@ def SetFuelTank(self): self.fuelTankStateEffector.r_TB_B = [[0.0], [0.0], [0.0]] self.tankModel.radiusTankInit = 1 self.tankModel.lengthTank = 1 - + # Add the tank and connect the thrusters self.scObject.addStateEffector(self.fuelTankStateEffector) self.fuelTankStateEffector.addThrusterSet(self.thrusterDynamicEffector) diff --git a/examples/MultiSatBskSim/plottingMultiSat/BSK_MultiSatPlotting.py b/examples/MultiSatBskSim/plottingMultiSat/BSK_MultiSatPlotting.py index 9971b9509f..fda13654f9 100644 --- a/examples/MultiSatBskSim/plottingMultiSat/BSK_MultiSatPlotting.py +++ b/examples/MultiSatBskSim/plottingMultiSat/BSK_MultiSatPlotting.py @@ -296,4 +296,3 @@ def plot_orbital_element_differences(timeData, oed, id=None): plt.legend() plt.xlabel("time [orbit]") plt.ylabel("Orbital Element Difference") - diff --git a/examples/OpNavScenarios/scenariosOpNav/OpNavMC/MonteCarlo.py b/examples/OpNavScenarios/scenariosOpNav/OpNavMC/MonteCarlo.py index f421d9ac32..8f355d1d36 100644 --- a/examples/OpNavScenarios/scenariosOpNav/OpNavMC/MonteCarlo.py +++ b/examples/OpNavScenarios/scenariosOpNav/OpNavMC/MonteCarlo.py @@ -190,5 +190,3 @@ def run(show_plots): if __name__ == "__main__": run(False) - - diff --git a/examples/Support/run_MC_IC/run0.json b/examples/Support/run_MC_IC/run0.json index 8f3a0061ba..b357111d94 100644 --- a/examples/Support/run_MC_IC/run0.json +++ b/examples/Support/run_MC_IC/run0.json @@ -1 +1 @@ -{"rwVoltageIO.voltage2TorqueGain[0]": "0.019647514", "RW2.Omega": "202.733977", "rwVoltageIO.voltage2TorqueGain[1]": "0.019647514", "RW1.Omega": "99.97586904", "hubref.IHubPntBc_B": "[[895.6643618520395, 9.136839836234794, -30.06671637364518], [9.136839836234794, 801.9055497738021, -5.806319750691131], [-30.06671637364518, -5.806319750691131, 603.9219126741585]]", "rwVoltageIO.voltage2TorqueGain[2]": "0.019647514", "RW1.gsHat_B": "[1.006106488, 0.001579056, -1.15e-05]", "TaskList[0].TaskModels[0].hub.omega_BN_BInit": "[0.0026129154760136737, -0.007033953764569684, 0.0008161536573443914]", "RW3.Omega": "302.609442", "RW2.gsHat_B": "[-0.001526595, 1.000311915, -0.002070768]", "TaskList[0].TaskModels[0].hub.r_BcB_B": "[0.000455952, 3.59e-05, 1.014309138]", "TaskList[0].TaskModels[0].hub.mHub": "777.2725585", "TaskList[0].TaskModels[0].hub.sigma_BNInit": "[3.237948691, 1.170132402, 0.34954584]", "RW3.gsHat_B": "[0.003492717, 0.001448398, 0.99683411]"} \ No newline at end of file +{"rwVoltageIO.voltage2TorqueGain[0]": "0.019647514", "RW2.Omega": "202.733977", "rwVoltageIO.voltage2TorqueGain[1]": "0.019647514", "RW1.Omega": "99.97586904", "hubref.IHubPntBc_B": "[[895.6643618520395, 9.136839836234794, -30.06671637364518], [9.136839836234794, 801.9055497738021, -5.806319750691131], [-30.06671637364518, -5.806319750691131, 603.9219126741585]]", "rwVoltageIO.voltage2TorqueGain[2]": "0.019647514", "RW1.gsHat_B": "[1.006106488, 0.001579056, -1.15e-05]", "TaskList[0].TaskModels[0].hub.omega_BN_BInit": "[0.0026129154760136737, -0.007033953764569684, 0.0008161536573443914]", "RW3.Omega": "302.609442", "RW2.gsHat_B": "[-0.001526595, 1.000311915, -0.002070768]", "TaskList[0].TaskModels[0].hub.r_BcB_B": "[0.000455952, 3.59e-05, 1.014309138]", "TaskList[0].TaskModels[0].hub.mHub": "777.2725585", "TaskList[0].TaskModels[0].hub.sigma_BNInit": "[3.237948691, 1.170132402, 0.34954584]", "RW3.gsHat_B": "[0.003492717, 0.001448398, 0.99683411]"} diff --git a/examples/Support/run_MC_IC/run1.json b/examples/Support/run_MC_IC/run1.json index 5585d1dc44..d861f23bc1 100644 --- a/examples/Support/run_MC_IC/run1.json +++ b/examples/Support/run_MC_IC/run1.json @@ -1 +1 @@ -{"rwVoltageIO.voltage2TorqueGain[0]": "0.019300004", "RW2.Omega": "201.2462049", "rwVoltageIO.voltage2TorqueGain[1]": "0.019300004", "RW1.Omega": "102.3314478", "hubref.IHubPntBc_B": "[[898.8193432597527, -4.783464976987985, -20.402903995299603], [-4.783464976987986, 798.757216869081, -13.809593155236549], [-20.402903995299603, -13.809593155236556, 603.8153089711661]]", "rwVoltageIO.voltage2TorqueGain[2]": "0.019300004", "RW1.gsHat_B": "[0.989879864, -0.001311646, 0.001291574]", "TaskList[0].TaskModels[0].hub.omega_BN_BInit": "[0.0025676858967667716, -0.006572170170300603, -0.0013538343602130189]", "RW3.Omega": "302.440986", "RW2.gsHat_B": "[-0.000190763, 1.001491934, -0.002122633]", "TaskList[0].TaskModels[0].hub.r_BcB_B": "[-0.000628679, 0.000944588, 1.026397207]", "TaskList[0].TaskModels[0].hub.mHub": "756.3948916", "TaskList[0].TaskModels[0].hub.sigma_BNInit": "[5.671744175, 4.418833228, 0.795579284]", "RW3.gsHat_B": "[-0.001794227, -0.003005013, 0.992727748]"} \ No newline at end of file +{"rwVoltageIO.voltage2TorqueGain[0]": "0.019300004", "RW2.Omega": "201.2462049", "rwVoltageIO.voltage2TorqueGain[1]": "0.019300004", "RW1.Omega": "102.3314478", "hubref.IHubPntBc_B": "[[898.8193432597527, -4.783464976987985, -20.402903995299603], [-4.783464976987986, 798.757216869081, -13.809593155236549], [-20.402903995299603, -13.809593155236556, 603.8153089711661]]", "rwVoltageIO.voltage2TorqueGain[2]": "0.019300004", "RW1.gsHat_B": "[0.989879864, -0.001311646, 0.001291574]", "TaskList[0].TaskModels[0].hub.omega_BN_BInit": "[0.0025676858967667716, -0.006572170170300603, -0.0013538343602130189]", "RW3.Omega": "302.440986", "RW2.gsHat_B": "[-0.000190763, 1.001491934, -0.002122633]", "TaskList[0].TaskModels[0].hub.r_BcB_B": "[-0.000628679, 0.000944588, 1.026397207]", "TaskList[0].TaskModels[0].hub.mHub": "756.3948916", "TaskList[0].TaskModels[0].hub.sigma_BNInit": "[5.671744175, 4.418833228, 0.795579284]", "RW3.gsHat_B": "[-0.001794227, -0.003005013, 0.992727748]"} diff --git a/examples/Support/run_MC_IC/run2.json b/examples/Support/run_MC_IC/run2.json index 8839b26fda..aa0a9d4812 100644 --- a/examples/Support/run_MC_IC/run2.json +++ b/examples/Support/run_MC_IC/run2.json @@ -1 +1 @@ -{"rwVoltageIO.voltage2TorqueGain[0]": "0.019106836", "RW2.Omega": "195.215673", "rwVoltageIO.voltage2TorqueGain[1]": "0.019106836", "RW1.Omega": "101.6096256", "hubref.IHubPntBc_B": "[[898.2754233148783, 11.86596331886969, 2.0058470883349564], [11.86596331886969, 798.8719954297682, -23.707620426772493], [2.0058470883349564, -23.70762042677248, 601.6273496553539]]", "rwVoltageIO.voltage2TorqueGain[2]": "0.019106836", "RW1.gsHat_B": "[0.99640817, 0.001482462, 0.001508889]", "TaskList[0].TaskModels[0].hub.omega_BN_BInit": "[0.002259940035730463, 0.0017313524109982634, -0.004000683993620538]", "RW3.Omega": "305.1903812", "RW2.gsHat_B": "[-0.001054728, 1.001123453, -0.001590313]", "TaskList[0].TaskModels[0].hub.r_BcB_B": "[0.000256741, 0.000499502, 1.010909615]", "TaskList[0].TaskModels[0].hub.mHub": "759.7191595", "TaskList[0].TaskModels[0].hub.sigma_BNInit": "[2.618612279, 1.859859205, 0.123022446]", "RW3.gsHat_B": "[0.001869383, -0.001276409, 1.001156124]"} \ No newline at end of file +{"rwVoltageIO.voltage2TorqueGain[0]": "0.019106836", "RW2.Omega": "195.215673", "rwVoltageIO.voltage2TorqueGain[1]": "0.019106836", "RW1.Omega": "101.6096256", "hubref.IHubPntBc_B": "[[898.2754233148783, 11.86596331886969, 2.0058470883349564], [11.86596331886969, 798.8719954297682, -23.707620426772493], [2.0058470883349564, -23.70762042677248, 601.6273496553539]]", "rwVoltageIO.voltage2TorqueGain[2]": "0.019106836", "RW1.gsHat_B": "[0.99640817, 0.001482462, 0.001508889]", "TaskList[0].TaskModels[0].hub.omega_BN_BInit": "[0.002259940035730463, 0.0017313524109982634, -0.004000683993620538]", "RW3.Omega": "305.1903812", "RW2.gsHat_B": "[-0.001054728, 1.001123453, -0.001590313]", "TaskList[0].TaskModels[0].hub.r_BcB_B": "[0.000256741, 0.000499502, 1.010909615]", "TaskList[0].TaskModels[0].hub.mHub": "759.7191595", "TaskList[0].TaskModels[0].hub.sigma_BNInit": "[2.618612279, 1.859859205, 0.123022446]", "RW3.gsHat_B": "[0.001869383, -0.001276409, 1.001156124]"} diff --git a/examples/dataForExamples/Itokawa/ItokawaHayabusa.mtl b/examples/dataForExamples/Itokawa/ItokawaHayabusa.mtl index 9f1c0b73d9..206fbdbabd 100644 --- a/examples/dataForExamples/Itokawa/ItokawaHayabusa.mtl +++ b/examples/dataForExamples/Itokawa/ItokawaHayabusa.mtl @@ -7,4 +7,3 @@ newmtl surface Ns 0 illum 4 - diff --git a/examples/dataForExamples/Loral-1300Com-main.mtl b/examples/dataForExamples/Loral-1300Com-main.mtl index e3ea69e3f0..ec8f8e4a26 100644 --- a/examples/dataForExamples/Loral-1300Com-main.mtl +++ b/examples/dataForExamples/Loral-1300Com-main.mtl @@ -773,4 +773,3 @@ Tr 0 d 1 Ni 1 Tf 1 1 1 - diff --git a/examples/dataForExamples/triangularPanel.obj b/examples/dataForExamples/triangularPanel.obj index 8908dc2d36..d4139ba71e 100644 --- a/examples/dataForExamples/triangularPanel.obj +++ b/examples/dataForExamples/triangularPanel.obj @@ -1,6 +1,6 @@ # Apple ModelIO OBJ File: triangularPanel mtllib triangularPanel.mtl -g +g v 1.9021 0 -0.05 v -1.9021 -1.23605 -0.05 v -1.9021 1.23605 -0.05 diff --git a/examples/scenarioAerocapture.py b/examples/scenarioAerocapture.py index 15a4aa6849..2c27bac9fc 100644 --- a/examples/scenarioAerocapture.py +++ b/examples/scenarioAerocapture.py @@ -115,14 +115,14 @@ def sph2rv(xxsph): NOTE: this function assumes inertial and planet-fixed frames are aligned at this time """ - + r = xxsph[0] lon = xxsph[1] lat = xxsph[2] u = xxsph[3] gam = xxsph[4] hda = xxsph[5] - + NI = np.eye(3) IE = np.array([[np.cos(lat) * np.cos(lon), -np.sin(lon), -np.sin(lat) * np.cos(lon)], [np.cos(lat) * np.sin(lon), np.cos(lon), -np.sin(lat) * np.sin(lon)], @@ -130,13 +130,13 @@ def sph2rv(xxsph): ES = np.array([[np.cos(gam), 0, np.sin(gam)], [-np.sin(gam) * np.sin(hda), np.cos(hda), np.cos(gam) * np.sin(hda)], [-np.sin(gam) * np.cos(hda), -np.sin(hda), np.cos(gam) * np.cos(hda)]]) - + e1_E = np.array([1,0,0]) rvec_N = (r * NI @ IE) @ e1_E - + s3_S = np.array([0,0,1]) uvec_N = u * ( NI @ IE @ ES) @ s3_S - + return rvec_N, uvec_N @@ -171,7 +171,7 @@ def run(show_plots, planetCase): tabAtmo = tabularAtmosphere.TabularAtmosphere() # update with current values tabAtmo.ModelTag = "tabularAtmosphere" # update python name of test module atmoTaskName = "atmosphere" - + # define constants & load data if planetCase == 'Earth': r_eq = 6378136.6 @@ -181,10 +181,10 @@ def run(show_plots, planetCase): r_eq = 3397.2 * 1000 dataFileName = bskPath + '/supportData/AtmosphereData/MarsGRAMNominal.txt' altList, rhoList, tempList = readAtmTable(dataFileName, 'MarsGRAM') - + # assign constants & ref. data to module tabAtmo.planetRadius = r_eq - tabAtmo.altList = tabularAtmosphere.DoubleVector(altList) + tabAtmo.altList = tabularAtmosphere.DoubleVector(altList) tabAtmo.rhoList = tabularAtmosphere.DoubleVector(rhoList) tabAtmo.tempList = tabularAtmosphere.DoubleVector(tempList) @@ -217,7 +217,7 @@ def run(show_plots, planetCase): scObject.ModelTag = "spacecraftBody" scObject.hub.mHub = m_sc tabAtmo.addSpacecraftToModel(scObject.scStateOutMsg) - + simpleNavObj = simpleNav.SimpleNav() scSim.AddModelToTask(simTaskName, simpleNavObj) simpleNavObj.scStateInMsg.subscribeTo(scObject.scStateOutMsg) @@ -253,7 +253,7 @@ def run(show_plots, planetCase): hda = np.pi/2 xxsph = [r,lon,lat,u,gam,hda] rN, vN = sph2rv(xxsph) - + scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m - v_CN_N @@ -378,5 +378,3 @@ def run(show_plots, planetCase): # close the plots being saved off to avoid over-writing old and new figures if __name__ == '__main__': run(True, 'Mars') # planet arrival case, can be Earth or Mars - - diff --git a/examples/scenarioDeployingPanel.py b/examples/scenarioDeployingPanel.py index c5d56bb8fd..8e985ec216 100644 --- a/examples/scenarioDeployingPanel.py +++ b/examples/scenarioDeployingPanel.py @@ -30,7 +30,7 @@ python3 scenarioDeployingPanel.py -The simulation includes two deploying panels that start undeployed. The first panel deploys fully, +The simulation includes two deploying panels that start undeployed. The first panel deploys fully, but the second panel deploys off-nominally (to 80%), leading to a reduced power output. @@ -163,9 +163,9 @@ def run(show_plots): panel1.ModelTag = "panel1" panel1.mass = 100.0 panel1.IPntS_S = [[100.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] - panel1.d = 1.5 + panel1.d = 1.5 panel1.k = 200. - panel1.c = 20. + panel1.c = 20. panel1.r_HB_B = [[-.5], [0.0], [-1.0]] panel1.dcm_HB = [[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 1.0]] panel1.thetaInit = -np.pi @@ -175,9 +175,9 @@ def run(show_plots): panel2.ModelTag = "panel2" panel2.mass = 100.0 panel2.IPntS_S = [[100.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] - panel2.d = 1.5 + panel2.d = 1.5 panel2.k = 200. - panel2.c = 20. + panel2.c = 20. panel2.r_HB_B = [[.5], [0.0], [-1.0]] panel2.dcm_HB = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] panel2.thetaInit = -np.pi @@ -228,7 +228,7 @@ def run(show_plots): solarPanel1 = simpleSolarPanel.SimpleSolarPanel() solarPanel1.ModelTag = "pwr1" - solarPanel1.nHat_B = [0, 0, 1] + solarPanel1.nHat_B = [0, 0, 1] solarPanel1.panelArea = 2.0 # m^2 solarPanel1.panelEfficiency = 0.9 # 90% efficiency in power generation solarPanel1.stateInMsg.subscribeTo(panel1.hingedRigidBodyConfigLogOutMsg) @@ -236,7 +236,7 @@ def run(show_plots): solarPanel2 = simpleSolarPanel.SimpleSolarPanel() solarPanel2.ModelTag = "pwr2" - solarPanel2.nHat_B = [0, 0, 1] + solarPanel2.nHat_B = [0, 0, 1] solarPanel2.panelArea = 2.0 # m^2 solarPanel2.panelEfficiency = 0.9 # 90% efficiency in power generation solarPanel2.stateInMsg.subscribeTo(panel2.hingedRigidBodyConfigLogOutMsg) @@ -316,9 +316,9 @@ def run(show_plots): np.set_printoptions(precision=16) - figureList = plotOrbits(dataLog.times(), dataSigmaBN, dataOmegaBN_B, + figureList = plotOrbits(dataLog.times(), dataSigmaBN, dataOmegaBN_B, panel1thetaLog, panel1thetaDotLog, - panel2thetaLog, panel2thetaDotLog, + panel2thetaLog, panel2thetaDotLog, pwrLog1, pwrLog2) if show_plots: diff --git a/examples/scenarioDragSensitivity.py b/examples/scenarioDragSensitivity.py index e1b0c06648..8201190d95 100644 --- a/examples/scenarioDragSensitivity.py +++ b/examples/scenarioDragSensitivity.py @@ -22,7 +22,7 @@ This script executes a parametric analysis of the control law examined in :ref:`scenarioDragRendezvous`, considering the performance of that control -law across both increasing initial displacements and variations in atmospheric density. +law across both increasing initial displacements and variations in atmospheric density. This script is found in the folder ``src/examples`` and executed by using:: @@ -100,13 +100,13 @@ def drag_sensitivity_analysis(ctrlType, nuOffsetNum, densityNum, rerunSims=False else: with open(ctrlType+"_sweep_results.pickle", "rb") as fp: sim_results = pickle.load(fp) - + pool.close() def results_to_ranges_and_plot(results_list): """ - Converts a results dict from scenarioDragRendezvous to a set of initial and final distance and speed errors, + Converts a results dict from scenarioDragRendezvous to a set of initial and final distance and speed errors, and returns a plot of all of the Hill-frame trajectories. """ fig = plt.figure() @@ -188,4 +188,4 @@ def run(show_plots, nuOffsetNum, densityNum, rerunSims=False, savePickle=False): if __name__=='__main__': nuOffsetNum = 15 densityNum = 20 - run(True, nuOffsetNum, densityNum, rerunSims=True, savePickle=False) \ No newline at end of file + run(True, nuOffsetNum, densityNum, rerunSims=True, savePickle=False) diff --git a/examples/scenarioFuelSlosh.py b/examples/scenarioFuelSlosh.py index 19ab8de7a9..8d2d5a8869 100755 --- a/examples/scenarioFuelSlosh.py +++ b/examples/scenarioFuelSlosh.py @@ -321,7 +321,7 @@ def run(show_plots, damping_parameter, timeStep): scSim.AddModelToTask(simTaskName, dataLog) scLog = scObject.logger( - ["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"], + ["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"], simulationTimeStep) scSim.AddModelToTask(simTaskName, scLog) @@ -330,13 +330,13 @@ def run(show_plots, damping_parameter, timeStep): def get_rho(CurrentSimNanos, i): stateName = f'linearSpringMassDamperRho{i}' return scObject.dynManager.getStateObject(stateName).getState()[0][0] - + damperRhoLog = pythonVariableLogger.PythonVariableLogger({ "damperRho1": lambda CurrentSimNanos: get_rho(CurrentSimNanos, 1), "damperRho2": lambda CurrentSimNanos: get_rho(CurrentSimNanos, 2), "damperRho3": lambda CurrentSimNanos: get_rho(CurrentSimNanos, 3), }, simulationTimeStep) - + scSim.AddModelToTask(simTaskName, damperRhoLog) # @@ -436,7 +436,7 @@ def get_rho(CurrentSimNanos, i): plt.figure(6, figsize=(5, 4)) for rhojOut in rhojOuts: plt.plot(time, rhojOut) - + plt.legend(['Particle 1', 'Particle 2', 'Particle 3'], loc='lower right') plt.xlabel('Time (s)') plt.ylabel('Displacement (m)') diff --git a/examples/scenarioIntegratorsComparison.py b/examples/scenarioIntegratorsComparison.py index 747d7102fd..0c6020973b 100644 --- a/examples/scenarioIntegratorsComparison.py +++ b/examples/scenarioIntegratorsComparison.py @@ -26,12 +26,12 @@ python3 scenarioIntegratorsComparison.py -For information on how to setup different integrators, see :ref:`scenarioIntegrators` and :ref:`scenarioVariableTimeStepIntegrators`. +For information on how to setup different integrators, see :ref:`scenarioIntegrators` and :ref:`scenarioVariableTimeStepIntegrators`. Currently, Basilisk only supports explicit Runge-Kutta integrators, both of the regular and adaptive variations. Non-adaptive Runge-Kutta integrators can be controlled solely by the step size: larger step -sizes means that faster computation, but less accurate results. +sizes means that faster computation, but less accurate results. In contrast, adaptive Runge-Kutta methods are affected both by the step size and absolute and relative tolerance. These integrators will try @@ -39,10 +39,10 @@ smaller time step is used internally for greater accuracy. When using an adaptive integrator, the Basilisk dynamics task time step -can be increased without risk of increasing the integration error. However, +can be increased without risk of increasing the integration error. However, this also means that other modules in the task are updated less often, which might be undesirable. Additionally, spacecraft state messages will -also be updated less frequently. +also be updated less frequently. Finally, each integrator is associated with an order. Greater order integrators are more accurate, but more computationally @@ -54,11 +54,11 @@ These are the Euler method (order 1), Heun's method (order 2), the Runge-Kutta 4 (order 4), the Runge-Kutta-Fehlberg 4(5) (adaptive with order 5), and the Runge-Kutta-Fehlberg 7(8) (adaptive with order 8). -The adaptive integrators are used with two different absolute +The adaptive integrators are used with two different absolute tolerances: 0.1 and 10. Each integrator is used to propagate a two-body orbit around the -Earth. The final position of each propagation is compared to +Earth. The final position of each propagation is compared to the analytical solution, and a final position error is obtained. Moreover, the time that it takes to propagate each orbit is recorded. @@ -93,12 +93,12 @@ for the larger time steps in which time adaption takes place. Here, a tighter tolerance would translate into higher computational costs. However, this is hard to see in the plot given the inherent -noisiness of performance measuring. +noisiness of performance measuring. -Fixed-timestep integrators are helpful when you want your simulation runtime -to be consistent as you vary simulation parameters. Since there is no adaptation, -runtime will be similar even if the parameters change the stiffness of the system's -dynamic equations. Of course, this comes at the cost of accuracy, but can it be +Fixed-timestep integrators are helpful when you want your simulation runtime +to be consistent as you vary simulation parameters. Since there is no adaptation, +runtime will be similar even if the parameters change the stiffness of the system's +dynamic equations. Of course, this comes at the cost of accuracy, but can it be very useful for hardware-in-the-loop simulations. One should note that adaptive RK methods are inherently slower than diff --git a/examples/scenarioSatelliteConstellation.py b/examples/scenarioSatelliteConstellation.py index b51a90d290..14e81f6250 100644 --- a/examples/scenarioSatelliteConstellation.py +++ b/examples/scenarioSatelliteConstellation.py @@ -21,7 +21,7 @@ Overview -------- -This scenario demonstrates how to set up a Walker-Delta constellation of satellites. Note that this scenario +This scenario demonstrates how to set up a Walker-Delta constellation of satellites. Note that this scenario uses the stand-alone Basilisk architecture rather than using the ''examples/FormationBskSim`` or ``examples/MultiSatBskSim`` architectures for simultaneously simulating multiple spacecraft. @@ -29,8 +29,8 @@ python3 scenarioSatelliteConstellation.py -When designing a satellite constellation, symmetry provides repeatable performance and makes design and operations simpler. One -such symmetric constellation design methodology is the Walker constellation which consists of circular orbits in evenly spaced +When designing a satellite constellation, symmetry provides repeatable performance and makes design and operations simpler. One +such symmetric constellation design methodology is the Walker constellation which consists of circular orbits in evenly spaced planes at a chosen inclination. Walker constellation layouts are fully specified by four parameters written as "i:T/P/F" where: * i = inclination angle @@ -38,7 +38,7 @@ * P = numer of orbit planes evenly spaced in node angle * F = relative spacing between satellites in adjacent orbit planes -In order to simulate the constellation a semi-major axis 'a' is also specified as an input. The total number of satellites T must +In order to simulate the constellation a semi-major axis 'a' is also specified as an input. The total number of satellites T must be specified as an integer multiple of the number of planes P. The relative spacing F must be an integer between 0 and (P-1). Illustration of Simulation Results @@ -53,7 +53,7 @@ .. image:: /_images/Scenarios/scenarioSatelliteConstellation.svg :align: center -Each line color corresponds to a different orbital plane, in this case 3 planes. Green circles mark the start of each of the 24 +Each line color corresponds to a different orbital plane, in this case 3 planes. Green circles mark the start of each of the 24 individual satellite's ground tracks and a red circle marks their end. Simulation Visualization In Vizard @@ -75,7 +75,7 @@ # # Basilisk Scenario Script and Integrated Test # -# Purpose: Demonstrates how to use the stand alone Basilisk architecture to automate +# Purpose: Demonstrates how to use the stand alone Basilisk architecture to automate # the setup of a Walker constellation. # Author: Andrew Morell # Creation Date: Dec. 13, 2023 @@ -156,7 +156,7 @@ def run(show_plots, a, i, T, P, F): n = np.sqrt(mu / a / a / a) Pr = 2. * np.pi / n simulationTime = macros.sec2nano(1.5 * Pr) - + # create a logging task object of the spacecraft output message at the desired down sampling ratio numDataPoints = 500 samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) diff --git a/examples/scenarioTAMcomparison.py b/examples/scenarioTAMcomparison.py index dcd71d6b78..4d2b12f925 100644 --- a/examples/scenarioTAMcomparison.py +++ b/examples/scenarioTAMcomparison.py @@ -140,7 +140,7 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): """ The scenarios can be run with the following setups parameters: - + Args: show_plots (bool): Determines if the script should display plots orbitCase (str): Specify the type of orbit to be simulated {'elliptical','circular'} @@ -148,7 +148,7 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): useBias2 (bool): Flag to use a sensor bias on TAM 2 useBounds1 (bool): Flag to use TAM 1 sensor bounds useBounds2 (bool): Flag to use TAM 2 sensor bounds - + """ # Create simulation variable names @@ -466,4 +466,3 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): True, #Use sensor bounds 1 (True,False) False #Use sensor bounds 2 (True,False) ) - diff --git a/externalTools/_doc.rst b/externalTools/_doc.rst index c703e009dd..06229cee8a 100644 --- a/externalTools/_doc.rst +++ b/externalTools/_doc.rst @@ -1,3 +1,3 @@ This folder contains Basilisk related tools and is not part of the Basilisk source code. -These tools can be used to enhance the use of Basilisk. test \ No newline at end of file +These tools can be used to enhance the use of Basilisk. test diff --git a/src/architecture/alg_contain/alg_contain.h b/src/architecture/alg_contain/alg_contain.h index 85e193930e..4d972f76bf 100755 --- a/src/architecture/alg_contain/alg_contain.h +++ b/src/architecture/alg_contain/alg_contain.h @@ -36,7 +36,7 @@ class AlgContain: public SysModel { AlgContain(void *DataIn, void(*UpPtr) (void*, uint64_t, uint64_t), void (*SelfPtr)(void*, uint64_t)=NULL, void(*ResetPtr)(void*, uint64_t, uint64_t) = NULL); //!< constructor - + void UseData(void *IncomingData) {DataPtr = IncomingData;} //!< method void UseUpdate(void (*LocPtr)(void*, uint64_t, uint64_t)) {AlgUpdate = LocPtr;} //!< method void UseSelfInit(void (*LocPtr)(void*, uint64_t)) {AlgSelfInit = LocPtr;} //!< method @@ -47,7 +47,7 @@ class AlgContain: public SysModel { uint64_t getSelfInitAddress() {return reinterpret_cast(*AlgSelfInit);} //!< method uint64_t getResetAddress() {return reinterpret_cast(*AlgReset);} //!< method uint64_t getUpdateAddress() {return reinterpret_cast(*AlgUpdate);} //!< method - + public: void *DataPtr; //!< class variable AlgPtr AlgSelfInit; //!< class variable diff --git a/src/architecture/messaging/_UnitTest/test_CMsgTimeWritten.py b/src/architecture/messaging/_UnitTest/test_CMsgTimeWritten.py index 725a545cab..eb65df90cd 100644 --- a/src/architecture/messaging/_UnitTest/test_CMsgTimeWritten.py +++ b/src/architecture/messaging/_UnitTest/test_CMsgTimeWritten.py @@ -72,4 +72,3 @@ def test_CMsgTimeWritten(): if __name__ == "__main__": CMsgTimeWritten() - diff --git a/src/architecture/msgPayloadDefC/AlbedoMsgPayload.h b/src/architecture/msgPayloadDefC/AlbedoMsgPayload.h index 5cfe9858d8..c6306d4ab1 100644 --- a/src/architecture/msgPayloadDefC/AlbedoMsgPayload.h +++ b/src/architecture/msgPayloadDefC/AlbedoMsgPayload.h @@ -23,7 +23,7 @@ /*! albedo message definition */ typedef struct { // Maximum albedo acting on the instrument - // considering the instrument's position + // considering the instrument's position double albedoAtInstrumentMax; //!< [-] Max albedo flux ratio at instrument double AfluxAtInstrumentMax; //!< [W/m^2] Max albedo flux at instrument // Albedo acting on the instrument diff --git a/src/architecture/msgPayloadDefC/CSSRawDataMsgPayload.h b/src/architecture/msgPayloadDefC/CSSRawDataMsgPayload.h index 1754c66956..59a158d85b 100755 --- a/src/architecture/msgPayloadDefC/CSSRawDataMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CSSRawDataMsgPayload.h @@ -23,7 +23,7 @@ //!@brief CSS raw data output message definition. typedef struct { - double OutputData; //!< CSS measurement output + double OutputData; //!< CSS measurement output }CSSRawDataMsgPayload; diff --git a/src/architecture/msgPayloadDefC/CSSUnitConfigMsgPayload.h b/src/architecture/msgPayloadDefC/CSSUnitConfigMsgPayload.h index 0548709072..a3b0b4331f 100644 --- a/src/architecture/msgPayloadDefC/CSSUnitConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CSSUnitConfigMsgPayload.h @@ -27,7 +27,7 @@ each sun sensor*/ typedef struct { double nHat_B[3]; //!< [-] CSS unit normal expressed in structure - double CBias; //!< [W] Individual calibration coefficient bias for CSS. If all CSS have the same gain, then this is set to 1.0. If one CSS has a 10% stronger response for the same input, then the value would be 1.10 + double CBias; //!< [W] Individual calibration coefficient bias for CSS. If all CSS have the same gain, then this is set to 1.0. If one CSS has a 10% stronger response for the same input, then the value would be 1.10 }CSSUnitConfigMsgPayload; diff --git a/src/architecture/msgPayloadDefC/CmdForceInertialMsgPayload.h b/src/architecture/msgPayloadDefC/CmdForceInertialMsgPayload.h index 923a132b05..b7421c6d21 100755 --- a/src/architecture/msgPayloadDefC/CmdForceInertialMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CmdForceInertialMsgPayload.h @@ -22,7 +22,7 @@ /*! @brief Message used to define the vehicle control force vector in Inertial frame components*/ typedef struct { - double forceRequestInertial[3]; //!< [N] control force request + double forceRequestInertial[3]; //!< [N] control force request }CmdForceInertialMsgPayload; diff --git a/src/architecture/msgPayloadDefC/DvBurnCmdMsgPayload.h b/src/architecture/msgPayloadDefC/DvBurnCmdMsgPayload.h index 9f24bb4807..5053c6a6a0 100755 --- a/src/architecture/msgPayloadDefC/DvBurnCmdMsgPayload.h +++ b/src/architecture/msgPayloadDefC/DvBurnCmdMsgPayload.h @@ -25,7 +25,7 @@ /*! @brief Input burn command structure used to configure the burn*/ typedef struct { - double dvInrtlCmd[3]; //!< [m/s] The commanded DV we need in inertial + double dvInrtlCmd[3]; //!< [m/s] The commanded DV we need in inertial double dvRotVecUnit[3]; //!< [-] The commanded vector we need to rotate about double dvRotVecMag; //!< [r/s] The commanded rotation rate for the vector uint64_t burnStartTime; //!< [ns] The commanded time to start the burn diff --git a/src/architecture/msgPayloadDefC/InertialFilterMsgPayload.h b/src/architecture/msgPayloadDefC/InertialFilterMsgPayload.h index 3811835e13..725a375a19 100755 --- a/src/architecture/msgPayloadDefC/InertialFilterMsgPayload.h +++ b/src/architecture/msgPayloadDefC/InertialFilterMsgPayload.h @@ -28,7 +28,7 @@ /*! @brief structure for filter-states output for the unscented kalman filter implementation of the inertial state estimator*/ typedef struct { - double timeTag; //!< [s] Current time of validity for output + double timeTag; //!< [s] Current time of validity for output double covar[AKF_N_STATES*AKF_N_STATES]; //!< [-] Current covariance of the filter double state[AKF_N_STATES]; //!< [-] Current estimated state of the filter int numObs; //!< [-] Valid observation count for this frame diff --git a/src/architecture/msgPayloadDefC/MTBArrayConfigMsgPayload.h b/src/architecture/msgPayloadDefC/MTBArrayConfigMsgPayload.h index 02f494e757..58ae362e76 100644 --- a/src/architecture/msgPayloadDefC/MTBArrayConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/MTBArrayConfigMsgPayload.h @@ -31,4 +31,3 @@ typedef struct{ #endif - diff --git a/src/architecture/msgPayloadDefC/OpNavFilterMsgPayload.h b/src/architecture/msgPayloadDefC/OpNavFilterMsgPayload.h index 6bc51342ed..c2b91ed165 100755 --- a/src/architecture/msgPayloadDefC/OpNavFilterMsgPayload.h +++ b/src/architecture/msgPayloadDefC/OpNavFilterMsgPayload.h @@ -27,7 +27,7 @@ /*! @brief structure for filter-states output for the unscented kalman filter implementation of the sunline state estimator*/ typedef struct { - double timeTag; //!< [s] Current time of validity for output + double timeTag; //!< [s] Current time of validity for output double covar[ODUKF_N_STATES*ODUKF_N_STATES]; //!< [-] Current covariance of the filter double state[ODUKF_N_STATES]; //!< [-] Current estimated state of the filter double stateError[ODUKF_N_STATES]; //!< [-] Current deviation of the state from the reference state diff --git a/src/architecture/msgPayloadDefC/OpNavLimbMsgPayload.h b/src/architecture/msgPayloadDefC/OpNavLimbMsgPayload.h index 9290cc0167..2ecfacc4e5 100755 --- a/src/architecture/msgPayloadDefC/OpNavLimbMsgPayload.h +++ b/src/architecture/msgPayloadDefC/OpNavLimbMsgPayload.h @@ -35,4 +35,3 @@ typedef struct { #endif - diff --git a/src/architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h b/src/architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h index c5e0c440db..7a5bcc2a8b 100755 --- a/src/architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h @@ -25,7 +25,7 @@ /*! @brief RW array configuration FSW msg */ typedef struct{ - double GsMatrix_B[3*MAX_EFF_CNT]; //!< [-] The RW spin axis matrix in body frame components + double GsMatrix_B[3*MAX_EFF_CNT]; //!< [-] The RW spin axis matrix in body frame components double JsList[MAX_EFF_CNT]; //!< [kgm2] The spin axis inertia for RWs int numRW; //!< [-] The number of reaction wheels available on vehicle double uMax[MAX_EFF_CNT]; //!< [Nm] The maximum RW motor torque diff --git a/src/architecture/msgPayloadDefC/RateCmdMsgPayload.h b/src/architecture/msgPayloadDefC/RateCmdMsgPayload.h index 5b12176a40..f409482b41 100755 --- a/src/architecture/msgPayloadDefC/RateCmdMsgPayload.h +++ b/src/architecture/msgPayloadDefC/RateCmdMsgPayload.h @@ -24,7 +24,7 @@ /*! @brief Structure used to define the output definition for attitude guidance*/ typedef struct { - double omega_BastR_B[3]; //!< [r/s] Desired body rate relative to R + double omega_BastR_B[3]; //!< [r/s] Desired body rate relative to R double omegap_BastR_B[3]; //!< [r/s^2] Body-frame derivative of omega_BastR_B }RateCmdMsgPayload; diff --git a/src/architecture/msgPayloadDefC/ReconfigBurnArrayInfoMsgPayload.h b/src/architecture/msgPayloadDefC/ReconfigBurnArrayInfoMsgPayload.h index ce44a4ba5b..38c382b375 100644 --- a/src/architecture/msgPayloadDefC/ReconfigBurnArrayInfoMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ReconfigBurnArrayInfoMsgPayload.h @@ -30,4 +30,4 @@ typedef struct { }ReconfigBurnArrayInfoMsgPayload; -#endif \ No newline at end of file +#endif diff --git a/src/architecture/msgPayloadDefC/ReconfigBurnInfoMsgPayload.h b/src/architecture/msgPayloadDefC/ReconfigBurnInfoMsgPayload.h index 1368435c8a..3d98327e8f 100644 --- a/src/architecture/msgPayloadDefC/ReconfigBurnInfoMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ReconfigBurnInfoMsgPayload.h @@ -29,4 +29,4 @@ typedef struct { }ReconfigBurnInfoMsgPayload; -#endif \ No newline at end of file +#endif diff --git a/src/architecture/msgPayloadDefC/SunlineFilterMsgPayload.h b/src/architecture/msgPayloadDefC/SunlineFilterMsgPayload.h index 160ca2228c..ad5baf251b 100755 --- a/src/architecture/msgPayloadDefC/SunlineFilterMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SunlineFilterMsgPayload.h @@ -30,7 +30,7 @@ /*! @brief structure for filter-states output for the unscented kalman filter implementation of the sunline state estimator*/ typedef struct { - double timeTag; //!< [s] Current time of validity for output + double timeTag; //!< [s] Current time of validity for output double covar[SKF_N_STATES*SKF_N_STATES]; //!< [-] Current covariance of the filter double state[SKF_N_STATES]; //!< [-] Current estimated state of the filter double stateError[SKF_N_STATES]; //!< [-] Current deviation of the state from the reference state diff --git a/src/architecture/msgPayloadDefC/THRArrayCmdForceMsgPayload.h b/src/architecture/msgPayloadDefC/THRArrayCmdForceMsgPayload.h index 626962de93..c892d2af0e 100755 --- a/src/architecture/msgPayloadDefC/THRArrayCmdForceMsgPayload.h +++ b/src/architecture/msgPayloadDefC/THRArrayCmdForceMsgPayload.h @@ -26,7 +26,7 @@ /*! @brief Message used to define a vector of thruster force commands */ typedef struct { - double thrForce[MAX_EFF_CNT]; //!< [N] array of thruster force values + double thrForce[MAX_EFF_CNT]; //!< [N] array of thruster force values }THRArrayCmdForceMsgPayload; diff --git a/src/architecture/msgPayloadDefC/TemperatureMsgPayload.h b/src/architecture/msgPayloadDefC/TemperatureMsgPayload.h index 6a19ec8981..2d993bb1ed 100644 --- a/src/architecture/msgPayloadDefC/TemperatureMsgPayload.h +++ b/src/architecture/msgPayloadDefC/TemperatureMsgPayload.h @@ -29,4 +29,3 @@ typedef struct { #endif //TEMPERATURE_H - diff --git a/src/architecture/msgPayloadDefC/VehicleConfigMsgPayload.h b/src/architecture/msgPayloadDefC/VehicleConfigMsgPayload.h index 6f1f8106af..3f81d4bae6 100755 --- a/src/architecture/msgPayloadDefC/VehicleConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/VehicleConfigMsgPayload.h @@ -25,7 +25,7 @@ /*! @brief Structure used to define a common structure for top level vehicle information*/ typedef struct { - double ISCPntB_B[9]; //!< [kg m^2] Spacecraft Inertia + double ISCPntB_B[9]; //!< [kg m^2] Spacecraft Inertia double CoM_B[3]; //!< [m] Center of mass of spacecraft in body double massSC; //!< [kg] Spacecraft mass uint32_t CurrentADCSState; //!< [-] Current ADCS state for subsystem diff --git a/src/architecture/system_model/_UnitTest/test_PySysModel.py b/src/architecture/system_model/_UnitTest/test_PySysModel.py index d4c7362893..d1e338d679 100644 --- a/src/architecture/system_model/_UnitTest/test_PySysModel.py +++ b/src/architecture/system_model/_UnitTest/test_PySysModel.py @@ -92,7 +92,7 @@ def test_ErrorPySysModel(): mod = ErroringPythonModule() - simulated_syserr_reset = io.StringIO("") + simulated_syserr_reset = io.StringIO("") with contextlib.redirect_stderr(simulated_syserr_reset): try: @@ -116,7 +116,7 @@ def test_ErrorPySysModel(): pass error_update = simulated_syserr_update.getvalue() - + if len(error_update) == 0: testMessage.append("Reset did not print its exception") elif not error_update.rstrip().endswith("ValueError: Error in UpdateState"): @@ -152,4 +152,4 @@ def UpdateState(self, CurrentSimNanos): if __name__ == "__main__": test_PySysModel() - test_ErrorPySysModel() \ No newline at end of file + test_ErrorPySysModel() diff --git a/src/architecture/utilities/BSpline.h b/src/architecture/utilities/BSpline.h index 6b86e6e394..2d43e957f2 100644 --- a/src/architecture/utilities/BSpline.h +++ b/src/architecture/utilities/BSpline.h @@ -39,7 +39,7 @@ class InputDataSet { void setT(Eigen::VectorXd T); void setW(Eigen::VectorXd W); void setAvgXDot(double AvgXDot); - + double AvgXDot; //!< desired average velocity norm Eigen::VectorXd T; //!< time tags: specifies at what time each waypoint is hit Eigen::VectorXd W; //!< weight vector for the LS approximation @@ -59,7 +59,7 @@ class InputDataSet { bool XDDot_N_flag; //!< indicates that second derivative at final point has been specified }; -//! @brief The OutputDataSet class is used as a data structure to contain the interpolated function and its first- and +//! @brief The OutputDataSet class is used as a data structure to contain the interpolated function and its first- and //! second-order derivatives, together with the time-tag vector T. class OutputDataSet { public: @@ -67,7 +67,7 @@ class OutputDataSet { ~OutputDataSet(); void getData(double t, double x[3], double xDot[3], double xDDot[3]); double getStates(double t, int derivative, int index); - + Eigen::VectorXd T; //!< time tags for each point of the interpolated trajectory Eigen::VectorXd X1; //!< coordinate #1 of the interpolated trajectory Eigen::VectorXd X2; //!< coordinate #2 of the interpolated trajectory @@ -90,4 +90,4 @@ void interpolate(InputDataSet Input, int Num, int P, OutputDataSet *Output); void approximate(InputDataSet Input, int Num, int Q, int P, OutputDataSet *Output); -void basisFunction(double t, Eigen::VectorXd U, int I, int P, double *NN, double *NN1, double *NN2); \ No newline at end of file +void basisFunction(double t, Eigen::VectorXd U, int I, int P, double *NN, double *NN1, double *NN2); diff --git a/src/architecture/utilities/_Documentation/AVS.sty b/src/architecture/utilities/_Documentation/AVS.sty index 73e5dd7956..c02abd9dfe 100644 --- a/src/architecture/utilities/_Documentation/AVS.sty +++ b/src/architecture/utilities/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/architecture/utilities/_Documentation/Basilisk-IMU-20170712.tex b/src/architecture/utilities/_Documentation/Basilisk-IMU-20170712.tex index c43def8e61..e1c3978806 100644 --- a/src/architecture/utilities/_Documentation/Basilisk-IMU-20170712.tex +++ b/src/architecture/utilities/_Documentation/Basilisk-IMU-20170712.tex @@ -82,7 +82,7 @@ \hline 1.2 & Rewrote for new IMU implementation and test & S. Carnahan & 20170914 \\ \hline - + \end{longtable} } @@ -92,7 +92,7 @@ \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/architecture/utilities/_Documentation/BasiliskCorruptions.tex b/src/architecture/utilities/_Documentation/BasiliskCorruptions.tex index 5f984b4f80..da1ac9b178 100644 --- a/src/architecture/utilities/_Documentation/BasiliskCorruptions.tex +++ b/src/architecture/utilities/_Documentation/BasiliskCorruptions.tex @@ -122,7 +122,7 @@ \section{Desired Corruptions} \begin{enumerate} \item Max \item Min - \end{enumerate} + \end{enumerate} \item Discretization \begin{enumerate} \item Max/min only = 2 bit @@ -286,7 +286,7 @@ \subsubsection{General for IMU} \item attach .noise, .discrete, and .sat to the corruption groups. \end{enumerate} -I am not sure of the "best practice" for building these types of things. Please provide whatever critiques you see fit. +I am not sure of the "best practice" for building these types of things. Please provide whatever critiques you see fit. diff --git a/src/architecture/utilities/_Documentation/BasiliskReportMemo.cls b/src/architecture/utilities/_Documentation/BasiliskReportMemo.cls index 1e869ba0c3..6256f116db 100644 --- a/src/architecture/utilities/_Documentation/BasiliskReportMemo.cls +++ b/src/architecture/utilities/_Documentation/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/architecture/utilities/_Documentation/discretize/AVS.sty b/src/architecture/utilities/_Documentation/discretize/AVS.sty index 73e5dd7956..c02abd9dfe 100644 --- a/src/architecture/utilities/_Documentation/discretize/AVS.sty +++ b/src/architecture/utilities/_Documentation/discretize/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/architecture/utilities/_Documentation/discretize/Basilisk-discretize-20180108.tex b/src/architecture/utilities/_Documentation/discretize/Basilisk-discretize-20180108.tex index c43def8e61..e1c3978806 100644 --- a/src/architecture/utilities/_Documentation/discretize/Basilisk-discretize-20180108.tex +++ b/src/architecture/utilities/_Documentation/discretize/Basilisk-discretize-20180108.tex @@ -82,7 +82,7 @@ \hline 1.2 & Rewrote for new IMU implementation and test & S. Carnahan & 20170914 \\ \hline - + \end{longtable} } @@ -92,7 +92,7 @@ \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/architecture/utilities/_Documentation/discretize/BasiliskReportMemo.cls b/src/architecture/utilities/_Documentation/discretize/BasiliskReportMemo.cls index 1e869ba0c3..6256f116db 100644 --- a/src/architecture/utilities/_Documentation/discretize/BasiliskReportMemo.cls +++ b/src/architecture/utilities/_Documentation/discretize/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/architecture/utilities/_Documentation/discretize/secModelDescription.tex b/src/architecture/utilities/_Documentation/discretize/secModelDescription.tex index 6591ca8d0d..1e5da46121 100644 --- a/src/architecture/utilities/_Documentation/discretize/secModelDescription.tex +++ b/src/architecture/utilities/_Documentation/discretize/secModelDescription.tex @@ -18,7 +18,7 @@ \subsubsection{Angular Rates} Where $\cal{P}$ is the sensor platform frame, $\cal{B}$ is the vehicle body frame, and $\cal{N}$ is the inertial frame. [PB] is the direction cosine matrix from $\cal{B}$ to $\cal{P}$. This allows for an arbitrary angular offset between $\cal{B}$ and $\cal{P}$ and allows for that offset to be time-varying. $\leftexp{B}{\bm{\omega}_{B/N}}$ is provided by the spacecraft module output message from the most recent dynamics integration. \subsubsection{Angular Displacement} -The IMU also outputs the angular displacement accumulated between IMU calls. In order to avoid complexities having to do with the relative timestep between the dynamics process and the IMU calls, this is not calculated in the same way as an IMU works physically. In this way, also, the dynamics do not have to be run at a fast enough rate for a physical IMU angular accumulation to be simulated. +The IMU also outputs the angular displacement accumulated between IMU calls. In order to avoid complexities having to do with the relative timestep between the dynamics process and the IMU calls, this is not calculated in the same way as an IMU works physically. In this way, also, the dynamics do not have to be run at a fast enough rate for a physical IMU angular accumulation to be simulated. The modified Rodriguez parameter (MRP) is recorded for the last time (1) the IMU was called. Once the new MRP is received, both are converted to DCMs and the step-PRV is computed as follows The current MRP is always provided by the spacecraft message from the most recent dynamics integration. \begin{equation} [PN]_2 = [PB][BN]_2 diff --git a/src/architecture/utilities/_Documentation/discretize/secModelFunctions.tex b/src/architecture/utilities/_Documentation/discretize/secModelFunctions.tex index 67ae18f2ba..6c1be36f41 100644 --- a/src/architecture/utilities/_Documentation/discretize/secModelFunctions.tex +++ b/src/architecture/utilities/_Documentation/discretize/secModelFunctions.tex @@ -18,8 +18,8 @@ \section{Model Assumptions and Limitations} \begin{itemize} \item \textbf{Error Inputs}: Because the error models rely on user inputs, these inputs are the most likely source of error in IMU output. Instrument bias would have to be measured experimentally or an educated guess would have to be made. The Guass-Markov noise model has well-known assumptions and is generally accepted to be a good model for this application. \item \textbf{Error Integration}: Errors for integrated values (DV and PRV) are calculated as acceleration and angular velocity errors multiplied by the IMU time step. If the IMU timestep matches the dynamics process rate, this is possibly a good assumption. However, if the IMU is run slower than the dynamics process, then the velocity errors may not be related to the instantaneous acceleration errors at the sampling time. - + \item \textbf{Integral Saturation}: Because the DV and PRV output values are calculated only at the IMU time step and not actually by integrating rates multiple times between calls, their saturation values are taken as the time integral of the rate saturation values. This misses some possibilities with varying accelerations between IMU time steps. Furthermore, the PRV is taken to be the integral of the angular rate over the time step. This should be a good approximation if the attitude of the spacecraft doesn't change "too much" over a relevant time step. - - \item \textbf{IMU Rate Limitation}: As with a real IMU, this model will only be run at a finite speed. It is limited in that it cannot correctly capture dynamics that are happening much faster than the IMU is called. -\end{itemize} \ No newline at end of file + + \item \textbf{IMU Rate Limitation}: As with a real IMU, this model will only be run at a finite speed. It is limited in that it cannot correctly capture dynamics that are happening much faster than the IMU is called. +\end{itemize} diff --git a/src/architecture/utilities/_Documentation/discretize/secTest.tex b/src/architecture/utilities/_Documentation/discretize/secTest.tex index 34a5fb2140..3349cab7f8 100644 --- a/src/architecture/utilities/_Documentation/discretize/secTest.tex +++ b/src/architecture/utilities/_Documentation/discretize/secTest.tex @@ -73,18 +73,18 @@ \subsection{Test Descriptions} \subitem \textbf{Success Criteria}: The outputs match to acceptable tolerance and are visually confirmed to be capped. \item \underline{Discretization} The IMU is run with all clean inputs, i.e. nonzero accelerations and angular accelerations of the spacecraft and this is compared to the truth values generated in python. Outputs are discretized. \subitem \textbf{Success Criteria}: The outputs match to acceptable tolerance and are visually confirmed to be discretized. Note. Two points in time always fail this test. This has to do with the python generated and c++ generated values being ever-so-slightly off and not discretizing at the same point. They match at the next timesteps and have been ignored for the test. -\end{enumerate} +\end{enumerate} As an additional check, $[PB]$ is calculated separately for the truth values and $yaw$, $pitch$, and $roll$ are fed to the IMU which calculates this value independently. In this way, the multiple set-up options for the IMU are validated. \section{Test Parameters} -This section summarizes the specific error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. +This section summarizes the specific error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. \begin{table}[H] \caption{Error tolerance for each test. Note that tolerances are relative $\frac{truth-output}{truth}$} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c | c | c | c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c | c | c | c | c | c | c } % Column formatting, \hline \rot{\textbf{Test}} & \rot{\textbf{Tolerance}} &\rot{\textbf{GyroLSB}}& \rot{\textbf{AccelLSB}}& \rot{\textbf{RotMax}}&\rot{\textbf{TransMax}}&\rot{\textbf{RotNoise}}&\rot{\textbf{TransNoise}}&\rot{\textbf{RotBias}}&\rot{\textbf{TransBias}} \\ \hline Clean & \input{AutoTex/cleanaccuracy} & \input{AutoTex/cleangyroLSB}& \input{AutoTex/cleanaccelLSB}& \input{AutoTex/cleanrotMax}& \input{AutoTex/cleantransMax}& \input{AutoTex/cleanrotNoise}& \input{AutoTex/cleantransNoise}& \input{AutoTex/cleanrotBias}& \input{AutoTex/cleanTransBias} \\ \hline @@ -105,7 +105,7 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline \textbf{Test} &\textbf{Pass/Fail} \\ \hline Clean & \input{AutoTex/cleanpassFail} \\ \hline @@ -157,4 +157,4 @@ \section{Test Results} \input{AutoTex/AccelNoise} \input{AutoTex/DRnoise} -\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. \ No newline at end of file +\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. diff --git a/src/architecture/utilities/_Documentation/discretize/secUserGuide.tex b/src/architecture/utilities/_Documentation/discretize/secUserGuide.tex index 6ca35e7a5d..051e441914 100644 --- a/src/architecture/utilities/_Documentation/discretize/secUserGuide.tex +++ b/src/architecture/utilities/_Documentation/discretize/secUserGuide.tex @@ -1,5 +1,5 @@ \section{User Guide} -This section contains conceptual overviews of the code and clear examples for the prospective user. +This section contains conceptual overviews of the code and clear examples for the prospective user. \subsection{Code Diagram} The diagram in Fig. \ref{img:codeFlow} demonstrates the basic logic of the IMU module. There is additional code that deals with auxiliary functions. An example of IMU use is given in test\_imu\_sensor.py in the imu\_sensor \_UnitTest folder. Application of each IMU function follows a simple, linear progression until realistic IMU outputs are achieved and sent out via the messaging system. @@ -16,7 +16,7 @@ \subsection{Variable Definitions} \caption{Definition and Explanation of Variables Used.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ | m{3cm}| m{3cm} | m{3cm} | m{6cm} |} % Column formatting, + \begin{tabular}{ | m{3cm}| m{3cm} | m{3cm} | m{6cm} |} % Column formatting, \hline \textbf{Variable} & \textbf{LaTeX Equivalent} & \textbf{Variable Type} & \textbf{Notes} \\ \hline InputStateMsg&N/A & string & Default setting: "inertial\_state\_output". This is the message from which the IMU receives spacecraft inertial data.\\ \hline @@ -36,4 +36,4 @@ \subsection{Variable Definitions} gyroLSB & (LSB) & double & Default: 0.0. This is the discretization value (least significant bit) for acceleration. Zero indicates no discretization. \\ \hline \end{tabular} \label{tabular:vars} -\end{table} \ No newline at end of file +\end{table} diff --git a/src/architecture/utilities/_Documentation/gaussMarkov/AVS.sty b/src/architecture/utilities/_Documentation/gaussMarkov/AVS.sty index 73e5dd7956..c02abd9dfe 100644 --- a/src/architecture/utilities/_Documentation/gaussMarkov/AVS.sty +++ b/src/architecture/utilities/_Documentation/gaussMarkov/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/architecture/utilities/_Documentation/gaussMarkov/Basilisk-gaussMarkov-20180108.tex b/src/architecture/utilities/_Documentation/gaussMarkov/Basilisk-gaussMarkov-20180108.tex index c43def8e61..e1c3978806 100644 --- a/src/architecture/utilities/_Documentation/gaussMarkov/Basilisk-gaussMarkov-20180108.tex +++ b/src/architecture/utilities/_Documentation/gaussMarkov/Basilisk-gaussMarkov-20180108.tex @@ -82,7 +82,7 @@ \hline 1.2 & Rewrote for new IMU implementation and test & S. Carnahan & 20170914 \\ \hline - + \end{longtable} } @@ -92,7 +92,7 @@ \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/architecture/utilities/_Documentation/gaussMarkov/BasiliskReportMemo.cls b/src/architecture/utilities/_Documentation/gaussMarkov/BasiliskReportMemo.cls index 1e869ba0c3..6256f116db 100644 --- a/src/architecture/utilities/_Documentation/gaussMarkov/BasiliskReportMemo.cls +++ b/src/architecture/utilities/_Documentation/gaussMarkov/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/architecture/utilities/_Documentation/gaussMarkov/secModelDescription.tex b/src/architecture/utilities/_Documentation/gaussMarkov/secModelDescription.tex index 8283b768d9..cb163c8da7 100644 --- a/src/architecture/utilities/_Documentation/gaussMarkov/secModelDescription.tex +++ b/src/architecture/utilities/_Documentation/gaussMarkov/secModelDescription.tex @@ -18,7 +18,7 @@ \subsubsection{Angular Rates} Where $\cal{P}$ is the sensor platform frame, $\cal{B}$ is the vehicle body frame, and $\cal{N}$ is the inertial frame. [PB] is the direction cosine matrix from $\cal{B}$ to $\cal{P}$. This allows for an arbitrary angular offset between $\cal{B}$ and $\cal{P}$ and allows for that offset to be time-varying. $\leftexp{B}{\bm{\omega}_{B/N}}$ is provided by the spacecraft output message from the most recent dynamics integration. \subsubsection{Angular Displacement} -The IMU also outputs the angular displacement accumulated between IMU calls. In order to avoid complexities having to do with the relative timestep between the dynamics process and the IMU calls, this is not calculated in the same way as an IMU works physically. In this way, also, the dynamics do not have to be run at a fast enough rate for a physical IMU angular accumulation to be simulated. +The IMU also outputs the angular displacement accumulated between IMU calls. In order to avoid complexities having to do with the relative timestep between the dynamics process and the IMU calls, this is not calculated in the same way as an IMU works physically. In this way, also, the dynamics do not have to be run at a fast enough rate for a physical IMU angular accumulation to be simulated. The modified Rodriguez parameter (MRP) is recorded for the last time (1) the IMU was called. Once the new MRP is received, both are converted to DCMs and the step-PRV is computed as follows The current MRP is always provided by the spacecraft message from the most recent dynamics integration. \begin{equation} [PN]_2 = [PB][BN]_2 diff --git a/src/architecture/utilities/_Documentation/gaussMarkov/secModelFunctions.tex b/src/architecture/utilities/_Documentation/gaussMarkov/secModelFunctions.tex index 67ae18f2ba..6c1be36f41 100644 --- a/src/architecture/utilities/_Documentation/gaussMarkov/secModelFunctions.tex +++ b/src/architecture/utilities/_Documentation/gaussMarkov/secModelFunctions.tex @@ -18,8 +18,8 @@ \section{Model Assumptions and Limitations} \begin{itemize} \item \textbf{Error Inputs}: Because the error models rely on user inputs, these inputs are the most likely source of error in IMU output. Instrument bias would have to be measured experimentally or an educated guess would have to be made. The Guass-Markov noise model has well-known assumptions and is generally accepted to be a good model for this application. \item \textbf{Error Integration}: Errors for integrated values (DV and PRV) are calculated as acceleration and angular velocity errors multiplied by the IMU time step. If the IMU timestep matches the dynamics process rate, this is possibly a good assumption. However, if the IMU is run slower than the dynamics process, then the velocity errors may not be related to the instantaneous acceleration errors at the sampling time. - + \item \textbf{Integral Saturation}: Because the DV and PRV output values are calculated only at the IMU time step and not actually by integrating rates multiple times between calls, their saturation values are taken as the time integral of the rate saturation values. This misses some possibilities with varying accelerations between IMU time steps. Furthermore, the PRV is taken to be the integral of the angular rate over the time step. This should be a good approximation if the attitude of the spacecraft doesn't change "too much" over a relevant time step. - - \item \textbf{IMU Rate Limitation}: As with a real IMU, this model will only be run at a finite speed. It is limited in that it cannot correctly capture dynamics that are happening much faster than the IMU is called. -\end{itemize} \ No newline at end of file + + \item \textbf{IMU Rate Limitation}: As with a real IMU, this model will only be run at a finite speed. It is limited in that it cannot correctly capture dynamics that are happening much faster than the IMU is called. +\end{itemize} diff --git a/src/architecture/utilities/_Documentation/gaussMarkov/secTest.tex b/src/architecture/utilities/_Documentation/gaussMarkov/secTest.tex index 34a5fb2140..3349cab7f8 100644 --- a/src/architecture/utilities/_Documentation/gaussMarkov/secTest.tex +++ b/src/architecture/utilities/_Documentation/gaussMarkov/secTest.tex @@ -73,18 +73,18 @@ \subsection{Test Descriptions} \subitem \textbf{Success Criteria}: The outputs match to acceptable tolerance and are visually confirmed to be capped. \item \underline{Discretization} The IMU is run with all clean inputs, i.e. nonzero accelerations and angular accelerations of the spacecraft and this is compared to the truth values generated in python. Outputs are discretized. \subitem \textbf{Success Criteria}: The outputs match to acceptable tolerance and are visually confirmed to be discretized. Note. Two points in time always fail this test. This has to do with the python generated and c++ generated values being ever-so-slightly off and not discretizing at the same point. They match at the next timesteps and have been ignored for the test. -\end{enumerate} +\end{enumerate} As an additional check, $[PB]$ is calculated separately for the truth values and $yaw$, $pitch$, and $roll$ are fed to the IMU which calculates this value independently. In this way, the multiple set-up options for the IMU are validated. \section{Test Parameters} -This section summarizes the specific error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. +This section summarizes the specific error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. \begin{table}[H] \caption{Error tolerance for each test. Note that tolerances are relative $\frac{truth-output}{truth}$} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c | c | c | c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c | c | c | c | c | c | c } % Column formatting, \hline \rot{\textbf{Test}} & \rot{\textbf{Tolerance}} &\rot{\textbf{GyroLSB}}& \rot{\textbf{AccelLSB}}& \rot{\textbf{RotMax}}&\rot{\textbf{TransMax}}&\rot{\textbf{RotNoise}}&\rot{\textbf{TransNoise}}&\rot{\textbf{RotBias}}&\rot{\textbf{TransBias}} \\ \hline Clean & \input{AutoTex/cleanaccuracy} & \input{AutoTex/cleangyroLSB}& \input{AutoTex/cleanaccelLSB}& \input{AutoTex/cleanrotMax}& \input{AutoTex/cleantransMax}& \input{AutoTex/cleanrotNoise}& \input{AutoTex/cleantransNoise}& \input{AutoTex/cleanrotBias}& \input{AutoTex/cleanTransBias} \\ \hline @@ -105,7 +105,7 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline \textbf{Test} &\textbf{Pass/Fail} \\ \hline Clean & \input{AutoTex/cleanpassFail} \\ \hline @@ -157,4 +157,4 @@ \section{Test Results} \input{AutoTex/AccelNoise} \input{AutoTex/DRnoise} -\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. \ No newline at end of file +\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. diff --git a/src/architecture/utilities/_Documentation/gaussMarkov/secUserGuide.tex b/src/architecture/utilities/_Documentation/gaussMarkov/secUserGuide.tex index 6ca35e7a5d..051e441914 100644 --- a/src/architecture/utilities/_Documentation/gaussMarkov/secUserGuide.tex +++ b/src/architecture/utilities/_Documentation/gaussMarkov/secUserGuide.tex @@ -1,5 +1,5 @@ \section{User Guide} -This section contains conceptual overviews of the code and clear examples for the prospective user. +This section contains conceptual overviews of the code and clear examples for the prospective user. \subsection{Code Diagram} The diagram in Fig. \ref{img:codeFlow} demonstrates the basic logic of the IMU module. There is additional code that deals with auxiliary functions. An example of IMU use is given in test\_imu\_sensor.py in the imu\_sensor \_UnitTest folder. Application of each IMU function follows a simple, linear progression until realistic IMU outputs are achieved and sent out via the messaging system. @@ -16,7 +16,7 @@ \subsection{Variable Definitions} \caption{Definition and Explanation of Variables Used.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ | m{3cm}| m{3cm} | m{3cm} | m{6cm} |} % Column formatting, + \begin{tabular}{ | m{3cm}| m{3cm} | m{3cm} | m{6cm} |} % Column formatting, \hline \textbf{Variable} & \textbf{LaTeX Equivalent} & \textbf{Variable Type} & \textbf{Notes} \\ \hline InputStateMsg&N/A & string & Default setting: "inertial\_state\_output". This is the message from which the IMU receives spacecraft inertial data.\\ \hline @@ -36,4 +36,4 @@ \subsection{Variable Definitions} gyroLSB & (LSB) & double & Default: 0.0. This is the discretization value (least significant bit) for acceleration. Zero indicates no discretization. \\ \hline \end{tabular} \label{tabular:vars} -\end{table} \ No newline at end of file +\end{table} diff --git a/src/architecture/utilities/_Documentation/saturate/AVS.sty b/src/architecture/utilities/_Documentation/saturate/AVS.sty index 73e5dd7956..c02abd9dfe 100644 --- a/src/architecture/utilities/_Documentation/saturate/AVS.sty +++ b/src/architecture/utilities/_Documentation/saturate/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/architecture/utilities/_Documentation/saturate/Basilisk-saturate-20180108.tex b/src/architecture/utilities/_Documentation/saturate/Basilisk-saturate-20180108.tex index dd26b777f7..ecf9b1abc1 100644 --- a/src/architecture/utilities/_Documentation/saturate/Basilisk-saturate-20180108.tex +++ b/src/architecture/utilities/_Documentation/saturate/Basilisk-saturate-20180108.tex @@ -78,7 +78,7 @@ \hline 1.0 & First draft & S. Carnahan & 20180108 \\ \hline - + \end{longtable} } @@ -88,7 +88,7 @@ \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/architecture/utilities/_Documentation/saturate/BasiliskReportMemo.cls b/src/architecture/utilities/_Documentation/saturate/BasiliskReportMemo.cls index 1e869ba0c3..6256f116db 100644 --- a/src/architecture/utilities/_Documentation/saturate/BasiliskReportMemo.cls +++ b/src/architecture/utilities/_Documentation/saturate/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/architecture/utilities/_Documentation/saturate/secModelFunctions.tex b/src/architecture/utilities/_Documentation/saturate/secModelFunctions.tex index a9a77b14dc..46889fb1ea 100644 --- a/src/architecture/utilities/_Documentation/saturate/secModelFunctions.tex +++ b/src/architecture/utilities/_Documentation/saturate/secModelFunctions.tex @@ -7,4 +7,4 @@ \section{Model Functions} \section{Model Assumptions and Limitations} -This code makes no real assumptions. It is up to the user not to give max values that are less than min values and any other inputs that could cause trouble. \ No newline at end of file +This code makes no real assumptions. It is up to the user not to give max values that are less than min values and any other inputs that could cause trouble. diff --git a/src/architecture/utilities/_Documentation/saturate/secTest.tex b/src/architecture/utilities/_Documentation/saturate/secTest.tex index 34a5fb2140..3349cab7f8 100644 --- a/src/architecture/utilities/_Documentation/saturate/secTest.tex +++ b/src/architecture/utilities/_Documentation/saturate/secTest.tex @@ -73,18 +73,18 @@ \subsection{Test Descriptions} \subitem \textbf{Success Criteria}: The outputs match to acceptable tolerance and are visually confirmed to be capped. \item \underline{Discretization} The IMU is run with all clean inputs, i.e. nonzero accelerations and angular accelerations of the spacecraft and this is compared to the truth values generated in python. Outputs are discretized. \subitem \textbf{Success Criteria}: The outputs match to acceptable tolerance and are visually confirmed to be discretized. Note. Two points in time always fail this test. This has to do with the python generated and c++ generated values being ever-so-slightly off and not discretizing at the same point. They match at the next timesteps and have been ignored for the test. -\end{enumerate} +\end{enumerate} As an additional check, $[PB]$ is calculated separately for the truth values and $yaw$, $pitch$, and $roll$ are fed to the IMU which calculates this value independently. In this way, the multiple set-up options for the IMU are validated. \section{Test Parameters} -This section summarizes the specific error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. +This section summarizes the specific error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. \begin{table}[H] \caption{Error tolerance for each test. Note that tolerances are relative $\frac{truth-output}{truth}$} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c | c | c | c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c | c | c | c | c | c | c } % Column formatting, \hline \rot{\textbf{Test}} & \rot{\textbf{Tolerance}} &\rot{\textbf{GyroLSB}}& \rot{\textbf{AccelLSB}}& \rot{\textbf{RotMax}}&\rot{\textbf{TransMax}}&\rot{\textbf{RotNoise}}&\rot{\textbf{TransNoise}}&\rot{\textbf{RotBias}}&\rot{\textbf{TransBias}} \\ \hline Clean & \input{AutoTex/cleanaccuracy} & \input{AutoTex/cleangyroLSB}& \input{AutoTex/cleanaccelLSB}& \input{AutoTex/cleanrotMax}& \input{AutoTex/cleantransMax}& \input{AutoTex/cleanrotNoise}& \input{AutoTex/cleantransNoise}& \input{AutoTex/cleanrotBias}& \input{AutoTex/cleanTransBias} \\ \hline @@ -105,7 +105,7 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline \textbf{Test} &\textbf{Pass/Fail} \\ \hline Clean & \input{AutoTex/cleanpassFail} \\ \hline @@ -157,4 +157,4 @@ \section{Test Results} \input{AutoTex/AccelNoise} \input{AutoTex/DRnoise} -\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. \ No newline at end of file +\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. diff --git a/src/architecture/utilities/_Documentation/saturate/secUserGuide.tex b/src/architecture/utilities/_Documentation/saturate/secUserGuide.tex index 6ca35e7a5d..051e441914 100644 --- a/src/architecture/utilities/_Documentation/saturate/secUserGuide.tex +++ b/src/architecture/utilities/_Documentation/saturate/secUserGuide.tex @@ -1,5 +1,5 @@ \section{User Guide} -This section contains conceptual overviews of the code and clear examples for the prospective user. +This section contains conceptual overviews of the code and clear examples for the prospective user. \subsection{Code Diagram} The diagram in Fig. \ref{img:codeFlow} demonstrates the basic logic of the IMU module. There is additional code that deals with auxiliary functions. An example of IMU use is given in test\_imu\_sensor.py in the imu\_sensor \_UnitTest folder. Application of each IMU function follows a simple, linear progression until realistic IMU outputs are achieved and sent out via the messaging system. @@ -16,7 +16,7 @@ \subsection{Variable Definitions} \caption{Definition and Explanation of Variables Used.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ | m{3cm}| m{3cm} | m{3cm} | m{6cm} |} % Column formatting, + \begin{tabular}{ | m{3cm}| m{3cm} | m{3cm} | m{6cm} |} % Column formatting, \hline \textbf{Variable} & \textbf{LaTeX Equivalent} & \textbf{Variable Type} & \textbf{Notes} \\ \hline InputStateMsg&N/A & string & Default setting: "inertial\_state\_output". This is the message from which the IMU receives spacecraft inertial data.\\ \hline @@ -36,4 +36,4 @@ \subsection{Variable Definitions} gyroLSB & (LSB) & double & Default: 0.0. This is the discretization value (least significant bit) for acceleration. Zero indicates no discretization. \\ \hline \end{tabular} \label{tabular:vars} -\end{table} \ No newline at end of file +\end{table} diff --git a/src/architecture/utilities/_Documentation/secModelDescription.tex b/src/architecture/utilities/_Documentation/secModelDescription.tex index 8283b768d9..cb163c8da7 100644 --- a/src/architecture/utilities/_Documentation/secModelDescription.tex +++ b/src/architecture/utilities/_Documentation/secModelDescription.tex @@ -18,7 +18,7 @@ \subsubsection{Angular Rates} Where $\cal{P}$ is the sensor platform frame, $\cal{B}$ is the vehicle body frame, and $\cal{N}$ is the inertial frame. [PB] is the direction cosine matrix from $\cal{B}$ to $\cal{P}$. This allows for an arbitrary angular offset between $\cal{B}$ and $\cal{P}$ and allows for that offset to be time-varying. $\leftexp{B}{\bm{\omega}_{B/N}}$ is provided by the spacecraft output message from the most recent dynamics integration. \subsubsection{Angular Displacement} -The IMU also outputs the angular displacement accumulated between IMU calls. In order to avoid complexities having to do with the relative timestep between the dynamics process and the IMU calls, this is not calculated in the same way as an IMU works physically. In this way, also, the dynamics do not have to be run at a fast enough rate for a physical IMU angular accumulation to be simulated. +The IMU also outputs the angular displacement accumulated between IMU calls. In order to avoid complexities having to do with the relative timestep between the dynamics process and the IMU calls, this is not calculated in the same way as an IMU works physically. In this way, also, the dynamics do not have to be run at a fast enough rate for a physical IMU angular accumulation to be simulated. The modified Rodriguez parameter (MRP) is recorded for the last time (1) the IMU was called. Once the new MRP is received, both are converted to DCMs and the step-PRV is computed as follows The current MRP is always provided by the spacecraft message from the most recent dynamics integration. \begin{equation} [PN]_2 = [PB][BN]_2 diff --git a/src/architecture/utilities/_Documentation/secModelFunctions.tex b/src/architecture/utilities/_Documentation/secModelFunctions.tex index 67ae18f2ba..6c1be36f41 100644 --- a/src/architecture/utilities/_Documentation/secModelFunctions.tex +++ b/src/architecture/utilities/_Documentation/secModelFunctions.tex @@ -18,8 +18,8 @@ \section{Model Assumptions and Limitations} \begin{itemize} \item \textbf{Error Inputs}: Because the error models rely on user inputs, these inputs are the most likely source of error in IMU output. Instrument bias would have to be measured experimentally or an educated guess would have to be made. The Guass-Markov noise model has well-known assumptions and is generally accepted to be a good model for this application. \item \textbf{Error Integration}: Errors for integrated values (DV and PRV) are calculated as acceleration and angular velocity errors multiplied by the IMU time step. If the IMU timestep matches the dynamics process rate, this is possibly a good assumption. However, if the IMU is run slower than the dynamics process, then the velocity errors may not be related to the instantaneous acceleration errors at the sampling time. - + \item \textbf{Integral Saturation}: Because the DV and PRV output values are calculated only at the IMU time step and not actually by integrating rates multiple times between calls, their saturation values are taken as the time integral of the rate saturation values. This misses some possibilities with varying accelerations between IMU time steps. Furthermore, the PRV is taken to be the integral of the angular rate over the time step. This should be a good approximation if the attitude of the spacecraft doesn't change "too much" over a relevant time step. - - \item \textbf{IMU Rate Limitation}: As with a real IMU, this model will only be run at a finite speed. It is limited in that it cannot correctly capture dynamics that are happening much faster than the IMU is called. -\end{itemize} \ No newline at end of file + + \item \textbf{IMU Rate Limitation}: As with a real IMU, this model will only be run at a finite speed. It is limited in that it cannot correctly capture dynamics that are happening much faster than the IMU is called. +\end{itemize} diff --git a/src/architecture/utilities/_Documentation/secTest.tex b/src/architecture/utilities/_Documentation/secTest.tex index 34a5fb2140..3349cab7f8 100644 --- a/src/architecture/utilities/_Documentation/secTest.tex +++ b/src/architecture/utilities/_Documentation/secTest.tex @@ -73,18 +73,18 @@ \subsection{Test Descriptions} \subitem \textbf{Success Criteria}: The outputs match to acceptable tolerance and are visually confirmed to be capped. \item \underline{Discretization} The IMU is run with all clean inputs, i.e. nonzero accelerations and angular accelerations of the spacecraft and this is compared to the truth values generated in python. Outputs are discretized. \subitem \textbf{Success Criteria}: The outputs match to acceptable tolerance and are visually confirmed to be discretized. Note. Two points in time always fail this test. This has to do with the python generated and c++ generated values being ever-so-slightly off and not discretizing at the same point. They match at the next timesteps and have been ignored for the test. -\end{enumerate} +\end{enumerate} As an additional check, $[PB]$ is calculated separately for the truth values and $yaw$, $pitch$, and $roll$ are fed to the IMU which calculates this value independently. In this way, the multiple set-up options for the IMU are validated. \section{Test Parameters} -This section summarizes the specific error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. +This section summarizes the specific error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. \begin{table}[H] \caption{Error tolerance for each test. Note that tolerances are relative $\frac{truth-output}{truth}$} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c | c | c | c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c | c | c | c | c | c | c } % Column formatting, \hline \rot{\textbf{Test}} & \rot{\textbf{Tolerance}} &\rot{\textbf{GyroLSB}}& \rot{\textbf{AccelLSB}}& \rot{\textbf{RotMax}}&\rot{\textbf{TransMax}}&\rot{\textbf{RotNoise}}&\rot{\textbf{TransNoise}}&\rot{\textbf{RotBias}}&\rot{\textbf{TransBias}} \\ \hline Clean & \input{AutoTex/cleanaccuracy} & \input{AutoTex/cleangyroLSB}& \input{AutoTex/cleanaccelLSB}& \input{AutoTex/cleanrotMax}& \input{AutoTex/cleantransMax}& \input{AutoTex/cleanrotNoise}& \input{AutoTex/cleantransNoise}& \input{AutoTex/cleanrotBias}& \input{AutoTex/cleanTransBias} \\ \hline @@ -105,7 +105,7 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline \textbf{Test} &\textbf{Pass/Fail} \\ \hline Clean & \input{AutoTex/cleanpassFail} \\ \hline @@ -157,4 +157,4 @@ \section{Test Results} \input{AutoTex/AccelNoise} \input{AutoTex/DRnoise} -\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. \ No newline at end of file +\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. diff --git a/src/architecture/utilities/_Documentation/secUserGuide.tex b/src/architecture/utilities/_Documentation/secUserGuide.tex index 6ca35e7a5d..051e441914 100644 --- a/src/architecture/utilities/_Documentation/secUserGuide.tex +++ b/src/architecture/utilities/_Documentation/secUserGuide.tex @@ -1,5 +1,5 @@ \section{User Guide} -This section contains conceptual overviews of the code and clear examples for the prospective user. +This section contains conceptual overviews of the code and clear examples for the prospective user. \subsection{Code Diagram} The diagram in Fig. \ref{img:codeFlow} demonstrates the basic logic of the IMU module. There is additional code that deals with auxiliary functions. An example of IMU use is given in test\_imu\_sensor.py in the imu\_sensor \_UnitTest folder. Application of each IMU function follows a simple, linear progression until realistic IMU outputs are achieved and sent out via the messaging system. @@ -16,7 +16,7 @@ \subsection{Variable Definitions} \caption{Definition and Explanation of Variables Used.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ | m{3cm}| m{3cm} | m{3cm} | m{6cm} |} % Column formatting, + \begin{tabular}{ | m{3cm}| m{3cm} | m{3cm} | m{6cm} |} % Column formatting, \hline \textbf{Variable} & \textbf{LaTeX Equivalent} & \textbf{Variable Type} & \textbf{Notes} \\ \hline InputStateMsg&N/A & string & Default setting: "inertial\_state\_output". This is the message from which the IMU receives spacecraft inertial data.\\ \hline @@ -36,4 +36,4 @@ \subsection{Variable Definitions} gyroLSB & (LSB) & double & Default: 0.0. This is the discretization value (least significant bit) for acceleration. Zero indicates no discretization. \\ \hline \end{tabular} \label{tabular:vars} -\end{table} \ No newline at end of file +\end{table} diff --git a/src/architecture/utilities/avsEigenMRP.h b/src/architecture/utilities/avsEigenMRP.h index 32aa0f1280..34563e008f 100644 --- a/src/architecture/utilities/avsEigenMRP.h +++ b/src/architecture/utilities/avsEigenMRP.h @@ -33,7 +33,7 @@ namespace Eigen { template class MRP; - + /*************************************************************************** * Definition of MRPBase * The implementation is at the end of the file @@ -609,7 +609,7 @@ namespace Eigen { res.coeffRef(2,1) = s2s3 + Scalar(4)*this->x()*ms2; res.coeffRef(2,2) = Scalar(4)*(-s1Sq - s2Sq + s3Sq)+ms2Sq; res = res / ps2 / ps2; - + return res; } @@ -818,7 +818,7 @@ namespace Eigen { sig.z() = q.z()/num; } }; - + // set from a vector of coefficients assumed to be a MRP template /** @@ -833,9 +833,9 @@ namespace Eigen { q.coeffs() = vec; } }; - + } // end namespace internal - + } // end namespace Eigen #endif // EIGEN_MRP_H diff --git a/src/architecture/utilities/avsEigenSupport.h b/src/architecture/utilities/avsEigenSupport.h index 8cdde9d480..4bdafabbad 100644 --- a/src/architecture/utilities/avsEigenSupport.h +++ b/src/architecture/utilities/avsEigenSupport.h @@ -44,7 +44,7 @@ Eigen::MRPd cArray2EigenMRPd(double* inArray); Eigen::Matrix3d cArray2EigenMatrix3d(double *inArray); //!@brief Specfici conversion between a C 2D array and an Eigen 3x3 matrix Eigen::Matrix3d c2DArray2EigenMatrix3d(double in2DArray[3][3]); -//!@brief returns the first axis DCM with the input angle +//!@brief returns the first axis DCM with the input angle Eigen::Matrix3d eigenM1(double angle); //!@brief returns the second axis DCM with the input angle Eigen::Matrix3d eigenM2(double angle); diff --git a/src/architecture/utilities/bskSemaphore.h b/src/architecture/utilities/bskSemaphore.h index 14d27377e7..8efacea39b 100644 --- a/src/architecture/utilities/bskSemaphore.h +++ b/src/architecture/utilities/bskSemaphore.h @@ -38,7 +38,7 @@ class BSKSemaphore : count(count_in) { } - + /*! release the lock */ inline void release() { @@ -49,7 +49,7 @@ class BSKSemaphore } cv.notify_one(); } - + /*! aquire the lock */ inline void acquire() { diff --git a/src/architecture/utilities/linearAlgebra.h b/src/architecture/utilities/linearAlgebra.h index aa16b45ad8..aee7eb6ebc 100644 --- a/src/architecture/utilities/linearAlgebra.h +++ b/src/architecture/utilities/linearAlgebra.h @@ -261,4 +261,3 @@ extern "C" { #endif #endif - diff --git a/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h b/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h index 90624cea68..df54cc7f35 100644 --- a/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h +++ b/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h @@ -41,7 +41,7 @@ class ModuleIdGenerator ~ModuleIdGenerator(); ModuleIdGenerator(ModuleIdGenerator const &) {}; ModuleIdGenerator& operator =(ModuleIdGenerator const &){return(*this);}; - + }; #endif /* _ModuleIdGenerator_HH_ */ diff --git a/src/architecture/utilities/rigidBodyKinematics.h b/src/architecture/utilities/rigidBodyKinematics.h index 8d76755756..fa69031714 100644 --- a/src/architecture/utilities/rigidBodyKinematics.h +++ b/src/architecture/utilities/rigidBodyKinematics.h @@ -258,7 +258,7 @@ extern "C" { void subPRV(double *q10, double *q20, double *q); void Mi(double angle, int axis, double C[3][3]); void tilde(double *v, double mat[3][3]); - + #ifdef __cplusplus } #endif diff --git a/src/architecture/utilities/saturate.h b/src/architecture/utilities/saturate.h index 4550aced14..02a05d94a1 100644 --- a/src/architecture/utilities/saturate.h +++ b/src/architecture/utilities/saturate.h @@ -28,17 +28,17 @@ */ class Saturate { - + public: Saturate(); - Saturate(int64_t size); //!< class constructor + Saturate(int64_t size); //!< class constructor ~Saturate(); void setBounds(Eigen::MatrixXd bounds); Eigen::VectorXd saturate(Eigen::VectorXd unsaturatedStates); /*!@brief Saturates the given unsaturated states @param unsaturated States, a vector of the unsaturated states @return saturatedStates*/ - + private: int64_t numStates; //!< -- Number of states to generate noise for Eigen::MatrixXd stateBounds; //!< -- one row for each state. lower bounds in left column, upper in right column diff --git a/src/architecture/utilities/signalCondition.h b/src/architecture/utilities/signalCondition.h index bfd348c6be..0d6b3211b1 100644 --- a/src/architecture/utilities/signalCondition.h +++ b/src/architecture/utilities/signalCondition.h @@ -32,9 +32,9 @@ typedef struct { #ifdef __cplusplus extern "C" { #endif - + void lowPassFilterSignal(double newMeas, LowPassFilterData *lpData); - + #ifdef __cplusplus } #endif diff --git a/src/architecture/utilities/simDefinitions.h b/src/architecture/utilities/simDefinitions.h index 7ecaef083e..35785827c9 100644 --- a/src/architecture/utilities/simDefinitions.h +++ b/src/architecture/utilities/simDefinitions.h @@ -26,4 +26,4 @@ #define SIGNAL_NOMINAL 0 #define SIGNAL_OFF 1 -#define SIGNAL_STUCK 2 \ No newline at end of file +#define SIGNAL_STUCK 2 diff --git a/src/architecture/utilities/tests/test_avsEigenMRP.cpp b/src/architecture/utilities/tests/test_avsEigenMRP.cpp index 9ffa69ec67..0b10419b56 100644 --- a/src/architecture/utilities/tests/test_avsEigenMRP.cpp +++ b/src/architecture/utilities/tests/test_avsEigenMRP.cpp @@ -28,4 +28,3 @@ TEST(eigenMRP, testIdentity) { EXPECT_TRUE(sigma.norm() < 1e-10); } - diff --git a/src/architecture/utilities/ukfUtilities.c b/src/architecture/utilities/ukfUtilities.c index b5c19d1b67..7119c78a45 100644 --- a/src/architecture/utilities/ukfUtilities.c +++ b/src/architecture/utilities/ukfUtilities.c @@ -353,7 +353,7 @@ int32_t ukfCholDownDate(double *rMat, double *xVec, double beta, int32_t nStates int i, j; double wVec[UKF_MAX_DIM]; double rEl2, bParam, gamma; - + vCopy(xVec, (size_t) nStates, wVec); mSetZero(rOut, (size_t) nStates, (size_t) nStates); @@ -375,7 +375,7 @@ int32_t ukfCholDownDate(double *rMat, double *xVec, double beta, int32_t nStates } bParam += beta * wVec[i]*wVec[i]/rEl2; } - + return 0; } diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/AVS.sty b/src/architecture/utilitiesSelfCheck/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/AVS.sty +++ b/src/architecture/utilitiesSelfCheck/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/Basilisk-avsLibrary20170812.tex b/src/architecture/utilitiesSelfCheck/_Documentation/Basilisk-avsLibrary20170812.tex index 5a29e39e6e..64b841ed9f 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/Basilisk-avsLibrary20170812.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/Basilisk-avsLibrary20170812.tex @@ -49,7 +49,7 @@ \newcommand{\subject}{AVS Library of Astrodynamics and C-Based Linear Algebra Support Sub-Routines} \newcommand{\status}{Initial Document} \newcommand{\preparer}{H. Schaub} -\newcommand{\summary}{The AVS Library has released a series of C, Matlab and Mathematica support routines to perform common astrodynamics functions. This document outlines how these support files are automatically validated within the Basilisk environment. There are C-based functions that run each routine with sample input, and compare to hand computed answers if the function response is correct. This checking is performed automatically every time {\tt pytest} is run on the root Basilisk folder. } +\newcommand{\summary}{The AVS Library has released a series of C, Matlab and Mathematica support routines to perform common astrodynamics functions. This document outlines how these support files are automatically validated within the Basilisk environment. There are C-based functions that run each routine with sample input, and compare to hand computed answers if the function response is correct. This checking is performed automatically every time {\tt pytest} is run on the root Basilisk folder. } \begin{document} diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/BasiliskReportMemo.cls b/src/architecture/utilitiesSelfCheck/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/BasiliskReportMemo.cls +++ b/src/architecture/utilitiesSelfCheck/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/AVS.sty b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/AVS.sty index 73e5dd7956..c02abd9dfe 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/AVS.sty +++ b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/Basilisk-discretize-20180108.tex b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/Basilisk-discretize-20180108.tex index 95e4d8871c..67361ac5fb 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/Basilisk-discretize-20180108.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/Basilisk-discretize-20180108.tex @@ -78,7 +78,7 @@ \hline 1.0 & First draft & S Carnahan & 20180116\\ \hline - + \end{longtable} } @@ -88,7 +88,7 @@ \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/BasiliskReportMemo.cls b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/BasiliskReportMemo.cls index 1e869ba0c3..6256f116db 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/BasiliskReportMemo.cls +++ b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secModelFunctions.tex b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secModelFunctions.tex index 8f152b1675..e89e4dcd7a 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secModelFunctions.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secModelFunctions.tex @@ -12,4 +12,4 @@ \section{Model Functions} \section{Model Assumptions and Limitations} \begin{itemize} \item \textbf{LSB Only}: Right now, discretization can only be set by giving a least significant bit. In the future, capability should be added to allow for discretization by max/min and number of bits. -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secTest.tex b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secTest.tex index e81e3db82f..d56664f7a0 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secTest.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secTest.tex @@ -9,8 +9,8 @@ \subsection{Test Descriptions} \item \underline{From Zero}: Checks discretization away from zero. \item \underline{Near} Checks discretization to the closest LSB multiple. \item \underline{Carry} Checks error carrying. - -\end{enumerate} + +\end{enumerate} \section{Test Results} All test results below @@ -19,7 +19,7 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline \textbf{Test} &\textbf{Pass/Fail} \\ \hline All Tests & \input{_Documentation/AutoTex/discretizePass} \\ \hline @@ -29,4 +29,4 @@ \section{Test Results} -\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. \ No newline at end of file +\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secUserGuide.tex b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secUserGuide.tex index 5e7f24b4ec..6c99f943db 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secUserGuide.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/discretize/secUserGuide.tex @@ -1,2 +1,2 @@ \section{User Guide} -For the best examples of the using the discretize utility, please see the IMU unit test and .cpp files. \ No newline at end of file +For the best examples of the using the discretize utility, please see the IMU unit test and .cpp files. diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/AVS.sty b/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/AVS.sty index 73e5dd7956..c02abd9dfe 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/AVS.sty +++ b/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/Basilisk-gaussMarkov-20180108.tex b/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/Basilisk-gaussMarkov-20180108.tex index 655b95a1da..2077d9df68 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/Basilisk-gaussMarkov-20180108.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/Basilisk-gaussMarkov-20180108.tex @@ -78,7 +78,7 @@ \hline 1.0 & First draft & S Carnahan & 20180116\\ \hline - + \end{longtable} } @@ -88,7 +88,7 @@ \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/BasiliskReportMemo.cls b/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/BasiliskReportMemo.cls index 1e869ba0c3..6256f116db 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/BasiliskReportMemo.cls +++ b/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/secTest.tex b/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/secTest.tex index abc66db732..0bac607519 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/secTest.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/secTest.tex @@ -8,7 +8,7 @@ \subsection{Test Descriptions} \item \underline{Standard Deviation} The module is called 1 hundred thousand times and the standard deviation is found to be within 1 percent of the input. \subitem \textbf{Mean}: The utility is run one hundred thousand times and the mean is found to be within 0.5\% of the standard deviation from zero for each state. \item \underline{Bounds} The utility is run one million times and the max/min outputs are tested against a known value which is close to, but not exactly the walk bounds. -\end{enumerate} +\end{enumerate} \section{Test Results} All test results below @@ -17,7 +17,7 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline \textbf{Test} &\textbf{Pass/Fail} \\ \hline All Tests & \input{_Documentation/AutoTex/markovPass} \\ \hline @@ -27,4 +27,4 @@ \section{Test Results} -\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. \ No newline at end of file +\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/secUserGuide.tex b/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/secUserGuide.tex index 1321788b65..9a5b80ba83 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/secUserGuide.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/gaussMarkov/secUserGuide.tex @@ -1,2 +1,2 @@ \section{User Guide} -For the best examples of the using the Gauss Markov utility, please see the IMU unit test and .cpp files. For other examples, see the simple navigation unit and coarse sun sensor. \ No newline at end of file +For the best examples of the using the Gauss Markov utility, please see the IMU unit test and .cpp files. For other examples, see the simple navigation unit and coarse sun sensor. diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/AVS.sty b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/AVS.sty index 73e5dd7956..c02abd9dfe 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/AVS.sty +++ b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/Basilisk-saturate-20180108.tex b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/Basilisk-saturate-20180108.tex index 6f8bfead5e..6fefc6d342 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/Basilisk-saturate-20180108.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/Basilisk-saturate-20180108.tex @@ -78,7 +78,7 @@ \hline 1.0 & First draft & S Carnahan & 20180116\\ \hline - + \end{longtable} } @@ -88,7 +88,7 @@ \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/BasiliskReportMemo.cls b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/BasiliskReportMemo.cls index 1e869ba0c3..6256f116db 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/BasiliskReportMemo.cls +++ b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secModelFunctions.tex b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secModelFunctions.tex index 6e467f7fd1..ddcdc7d5b5 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secModelFunctions.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secModelFunctions.tex @@ -9,4 +9,4 @@ \section{Model Functions} \section{Model Assumptions and Limitations} \begin{itemize} \item \textbf{Reasonable Bounds}: The utility has no checking for inverted bounds, NaNs, or any other way the user may improperly use it. -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secTest.tex b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secTest.tex index 6aa50347ca..8d8c6e10a5 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secTest.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secTest.tex @@ -6,7 +6,7 @@ \subsection{Test Descriptions} \begin{enumerate} \item \underline{Saturate} A 3-vector is tested such that one element is railed low, one railed high, and one left alone. -\end{enumerate} +\end{enumerate} \section{Test Results} All test results below @@ -15,7 +15,7 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline \textbf{Test} &\textbf{Pass/Fail} \\ \hline All Tests & \input{_Documentation/AutoTex/saturatePass} \\ \hline @@ -25,4 +25,4 @@ \section{Test Results} -\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. \ No newline at end of file +\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secUserGuide.tex b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secUserGuide.tex index 374e6ff3da..a65ae5086a 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secUserGuide.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/saturate/secUserGuide.tex @@ -1,2 +1,2 @@ \section{User Guide} -For the best examples of the using the Saturate utility, please see the IMU unit test and .cpp files. For other examples, see the coarse sun sensor. \ No newline at end of file +For the best examples of the using the Saturate utility, please see the IMU unit test and .cpp files. For other examples, see the coarse sun sensor. diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/secModelDescription.tex b/src/architecture/utilitiesSelfCheck/_Documentation/secModelDescription.tex index 02fe05c999..353a394701 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/secModelDescription.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/secModelDescription.tex @@ -19,11 +19,11 @@ \subsection{{\tt rigidBodyKinematics} Library} \subsection{{\tt orbitalMotion} Library} -Common celestial mechanics tools are implemented in the {\tt orbitalMotion} library, including transforming between anomaly angles, converting between inertial Cartesian position and velocity vector components and classical orbit elements, as well as evaluating simple space environment parameters. The following developments use the notation and formulation of Chapter 9 in Reference~\citenum{schaub}. The variable naming is reflected within the software code. +Common celestial mechanics tools are implemented in the {\tt orbitalMotion} library, including transforming between anomaly angles, converting between inertial Cartesian position and velocity vector components and classical orbit elements, as well as evaluating simple space environment parameters. The following developments use the notation and formulation of Chapter 9 in Reference~\citenum{schaub}. The variable naming is reflected within the software code. \subsubsection{Classical Orbital Elements} -The classcial orbit elements are given by +The classcial orbit elements are given by $$ (a, e, i, \Omega, \omega, f) $$ @@ -34,22 +34,22 @@ \subsubsection{Classical Orbital Elements} \begin{figure}[t] \centering \subfigure[Orbit Frame Orientation Illustration] - {\label{fig:orbit313} - \includegraphics[width=0.45\textwidth]{Figures/orbit313}} - \\ + {\label{fig:orbit313} + \includegraphics[width=0.45\textwidth]{Figures/orbit313}} + \\ \subfigure[Classical orbital parameter Illustration for an Elliptic Orbit] {\label{fig:orbitElliptic} - \includegraphics[width=0.45\textwidth]{Figures/orbitEllipse}} + \includegraphics[width=0.45\textwidth]{Figures/orbitEllipse}} \subfigure[Classical orbital parameter Illustration for a Hyperbolic Orbit] - {\label{fig:orbitHyperbolic} - \includegraphics[width=0.45\textwidth]{Figures/orbitHyperbola}} + {\label{fig:orbitHyperbolic} + \includegraphics[width=0.45\textwidth]{Figures/orbitHyperbola}} \caption{Orbit Elements, Axes and Frames Illustrations.\cite{schaub}} \label{fig:orbitParameters} \end{figure} Finally, the true anomaly angle is given by $f$, while the eccentric anomaly angle is expressed through $E$ as illustrated in Figure~\ref{fig:orbitElliptic} for an elliptic orbit scenario. The true and eccentric anomaly are related through\cite{schaub} \begin{equation} - \tan\left( \frac{f}{2} \right) = \sqrt{ \frac{1+e}{1-e} } \tan\left( \frac{E}{2} \right) + \tan\left( \frac{f}{2} \right) = \sqrt{ \frac{1+e}{1-e} } \tan\left( \frac{E}{2} \right) \end{equation} The mean anomaly angle $M$ relates to the eccentric anomaly angle $E$ through\cite{schaub} \begin{equation} @@ -58,7 +58,7 @@ \subsubsection{Classical Orbital Elements} The classical orbit elements for a hyperbolic scenario are shown in Figure~\ref{fig:orbitHyperbolic}. Note that the convention is used where the SMA is a negative value for a hyperbolic orbit, and thus $a < 0$ in this case. The true anomaly angle $f$ definition is universal for all orbit types, while the hyperbolic anomaly $H$ relates to $f$ through \begin{equation} - \tanh\left( \frac{f}{2} \right) = \sqrt{ \frac{e+1}{e-1}} \tanh\left( \frac{H}{2} \right) + \tanh\left( \frac{f}{2} \right) = \sqrt{ \frac{e+1}{e-1}} \tanh\left( \frac{H}{2} \right) \end{equation} The hyperbolic mean anomaly is defined in terms of the hyperbolic anomaly through \begin{equation} @@ -66,13 +66,13 @@ \subsubsection{Classical Orbital Elements} \end{equation} -While mapping from eccentric or hyperbolic anomalies to mean anomalies is analytical, the inverse is not. The sub-routines use Newton's method to numerically solve from mean anomalies to the corresponding eccentric or hyperbolic anomalies. This is called solving Kepler's equation. The iterations continue until a change tolerance of $10^{-13}$ radians is achieved, or a maximum of 200 iterations are performed. Note that solving this Kepler's equation for most cases converges very quickly within 3-5 iterations. +While mapping from eccentric or hyperbolic anomalies to mean anomalies is analytical, the inverse is not. The sub-routines use Newton's method to numerically solve from mean anomalies to the corresponding eccentric or hyperbolic anomalies. This is called solving Kepler's equation. The iterations continue until a change tolerance of $10^{-13}$ radians is achieved, or a maximum of 200 iterations are performed. Note that solving this Kepler's equation for most cases converges very quickly within 3-5 iterations. \subsubsection{Orbit Element to Cartesian Coordinate Conversions} -Let $\leftexp{N}{\bm r}_{C/N}$ and $\leftexp{N}{\bm v}_{C/N}$ be the inertial spacecraft center of mass $C$ position and velocity vector matrix representations, expressed with respect to inertial frame \frameDefinition{N} components. Functions are provided to map from classical orbit elements to these Cartesian coordinates through {\tt elem2rv()}, as well as the inverse mapping from Cartesian to classical orbit elements through {\tt rv2elem()}. +Let $\leftexp{N}{\bm r}_{C/N}$ and $\leftexp{N}{\bm v}_{C/N}$ be the inertial spacecraft center of mass $C$ position and velocity vector matrix representations, expressed with respect to inertial frame \frameDefinition{N} components. Functions are provided to map from classical orbit elements to these Cartesian coordinates through {\tt elem2rv()}, as well as the inverse mapping from Cartesian to classical orbit elements through {\tt rv2elem()}. \paragraph{{\tt elem2rv()} Function} The general conversion of classical orbit elements to the inertial Cartesian components is outlined in detail in section 9.6.2 of Reference~\citenum{schaub}. Given the true anomaly angle $f$, the orbit radius is for any orbit type given by @@ -83,7 +83,7 @@ \subsubsection{Orbit Element to Cartesian Coordinate Conversions} \begin{equation} p = a(1-e^{2}) \end{equation} -If the satellite is on a parabola, then the algorithm assumes $a=0$ (computers don't like infinity), and +If the satellite is on a parabola, then the algorithm assumes $a=0$ (computers don't like infinity), and \begin{equation} p = 2*r_{p} \end{equation} @@ -97,11 +97,11 @@ \subsubsection{Orbit Element to Cartesian Coordinate Conversions} \begin{equation} \bm r_{C/N} =r {\begin{matrix} ~ \\ ~ \\ ~ \end{matrix}}^{\cal N}\!\!\!\! \begin{pmatrix} -\cos\Omega\cos\theta -- \sin\Omega\sin\theta\cos i \\ -\sin\Omega\cos\theta + \cos\Omega\sin\theta\cos +\cos\Omega\cos\theta +- \sin\Omega\sin\theta\cos i \\ +\sin\Omega\cos\theta + \cos\Omega\sin\theta\cos i \\ - \sin\theta\sin i + \sin\theta\sin i \end{pmatrix} \label{eq:tb:r2} \end{equation} @@ -110,15 +110,15 @@ \subsubsection{Orbit Element to Cartesian Coordinate Conversions} \dot{\bm r}_{C/N} = -\frac{\mu}{h} {\begin{matrix} ~ \\ ~ \\ ~ \end{matrix}}^{\cal N}\!\!\!\! \begin{pmatrix} -\cos\Omega (\sin\theta + e\sin\omega ) + \sin\Omega +\cos\Omega (\sin\theta + e\sin\omega ) + \sin\Omega (\cos\theta+e\cos\omega )\cos i \\ -\sin\Omega (\sin\theta + e\sin\omega ) - \cos\Omega ( \cos\theta +\sin\Omega (\sin\theta + e\sin\omega ) - \cos\Omega ( \cos\theta + e\cos\omega )\cos i \\ -(\cos\theta + e\cos\omega )\sin i \end{pmatrix} \label{eq:tb:dr2} \end{equation} -where $\mu$ is the gravitational constant and $h$ is the massless orbital angular momentum $\bm h = \bm r \times \dot{\bm r}$. +where $\mu$ is the gravitational constant and $h$ is the massless orbital angular momentum $\bm h = \bm r \times \dot{\bm r}$. The {\tt elem2rv()} function checks for a special rectilinear motion case. Here the eccentricity is $e = 1$ while the SMA is non-zero with $|a|>0$. Under this condition the spacecraft is moving purely along a radial direction relative to the planet, and the true anomaly angle no longer can be used to determine the spacecrafts location. In this scenario the Eccentric or Hyperbolic anomaly angle $E$ can still be used to determine spacecraft position, and the {\tt elem2rv()} function assumes here that the anomaly angle provided is either $E$ or $H$ and not $f$. For an ellipse the orbit radius is here computed using\cite{schaub} @@ -132,11 +132,11 @@ \subsubsection{Orbit Element to Cartesian Coordinate Conversions} The radial unit direction vector along which all rectilinear motion is occuring is \begin{equation} \leftexp{N}{\hat{\bm\imath}}_{r}=\leftexp{N\!\!\!}{\begin{pmatrix} -\cos\Omega\cos\omega -- \sin\Omega\sin\omega\cos i \\ -\sin\Omega\cos\omega + \cos\Omega\sin\omega\cos +\cos\Omega\cos\omega +- \sin\Omega\sin\omega\cos i \\ +\sin\Omega\cos\omega + \cos\Omega\sin\omega\cos i \\ - \sin\theta\sin i + \sin\theta\sin i \end{pmatrix}} \end{equation} The inertial position vector is then given by @@ -166,12 +166,12 @@ \subsubsection{Orbit Element to Cartesian Coordinate Conversions} \begin{equation} p = \frac{\bm h \cdot \bm h}{\mu} \end{equation} -The line of nodes vector $\bm n$ is +The line of nodes vector $\bm n$ is \begin{equation} \bm n = \hat{\bm n}_{3} \times \bm h \end{equation} -Let $\hat{\bm n}$ be the normalized line of node axis. -If $|\bm n|$ is near zero, then the orbit is equatorial and the line of nodes is ill-defined. In this case we set $\hat{\bm n} = \hat{\bm n}_{1}$ such that $\Omega = 0$. +Let $\hat{\bm n}$ be the normalized line of node axis. +If $|\bm n|$ is near zero, then the orbit is equatorial and the line of nodes is ill-defined. In this case we set $\hat{\bm n} = \hat{\bm n}_{1}$ such that $\Omega = 0$. The eccentricity vector $\bm e$ is computed using\cite{schaub} \begin{equation} @@ -182,7 +182,7 @@ \subsubsection{Orbit Element to Cartesian Coordinate Conversions} e = |\bm e| \end{equation} Let $\hat{\bm\imath}_{e}$ be the normalized eccentricity vector. -If $e$ is near zero, then the orbit is near circular and the periapses direction is ill defined. In this case we set $\hat{\bm\imath}_{e} = \hat{\bm n}$ which results in $\omega = 0$. +If $e$ is near zero, then the orbit is near circular and the periapses direction is ill defined. In this case we set $\hat{\bm\imath}_{e} = \hat{\bm n}$ which results in $\omega = 0$. Within this function the orbit radius is set through \begin{equation} @@ -205,27 +205,27 @@ \subsubsection{Orbit Element to Cartesian Coordinate Conversions} \begin{equation} r_{a} = \frac{p}{1 - e} \end{equation} -If $\alpha$ is zero, then the motion is parabolic and the SMA is not defined (i.e. is infinite). In this case the code sets +If $\alpha$ is zero, then the motion is parabolic and the SMA is not defined (i.e. is infinite). In this case the code sets $$ a = 0.0 $$ while $r_{a} = 0.0$ is returned as a value that is not defined in this case. -Next the orbit frame orientation angles $\Omega$, $i$ and $\omega$ must be determined. As classical elements are used, care must be given for particular singular orientations where some angles are ill-defined. First, assume a non-circular non-equatorial orbit scenario. In this case $\Omega$ is the angle between $\hat{\bm n}_{1}$ and $\bm n$ where care must be taken that the right quadrant is used. +Next the orbit frame orientation angles $\Omega$, $i$ and $\omega$ must be determined. As classical elements are used, care must be given for particular singular orientations where some angles are ill-defined. First, assume a non-circular non-equatorial orbit scenario. In this case $\Omega$ is the angle between $\hat{\bm n}_{1}$ and $\bm n$ where care must be taken that the right quadrant is used. \begin{equation} \tan \Omega = \frac{(\hat{\bm n}_{1} \times \hat{\bm n}) \cdot \hat{\bm n}_{3}} {\hat{\bm n}_{1} \cdot \hat{\bm n}} \end{equation} -The argument of periapses is the angle between $\hat{\bm n}$ and $\bm e$, where again quadrants must be checked. +The argument of periapses is the angle between $\hat{\bm n}$ and $\bm e$, where again quadrants must be checked. \begin{equation} \tan \omega = \frac{(\hat{\bm \imath}_{n} \times \hat{\bm \imath}_{e}) \cdot \hat{\bm \imath}_{h}} {\hat{\bm \imath}_{n} \cdot \hat{\bm \imath}_{e}} \end{equation} -Finally, the inclination angle $i$ is the angle between $\bm h$ and $\hat{\bm n}_{3}$, but here no quadrants must be checked as this angle is defined as $0 \le i \le \pi$. +Finally, the inclination angle $i$ is the angle between $\bm h$ and $\hat{\bm n}_{3}$, but here no quadrants must be checked as this angle is defined as $0 \le i \le \pi$. \begin{equation} \cos i = \hat{\bm \imath}_{h} \cdot \hat{\bm n}_{3} \end{equation} @@ -240,7 +240,7 @@ \subsubsection{Orbit Element to Cartesian Coordinate Conversions} The 3-1-3 Euler angles representing the orbit frame orientation have singular configurations. The following list discusses how each case is handled: \begin{itemize} - \item {\bfseries Equatorial non-circular orbit:} Here the ascending node $\Omega$ is ill-defined, and is set to 0.0 radians. + \item {\bfseries Equatorial non-circular orbit:} Here the ascending node $\Omega$ is ill-defined, and is set to 0.0 radians. \item {\bfseries Inclined circular orbit:} Here the argument of periapses $\omega$ is ill-defined, and is set to 0.0 radians. \item {\bfseries Equatorial circular orbit:} Here both $\Omega$ and $\omega$ are ill-defined are are each set to 0.0 radians. \end{itemize} @@ -277,7 +277,7 @@ \subsubsection{Space Environment Parameters} \paragraph{Gravitational Zonal Harmonics} This function computes Earth's zonal harmonics from $J_{2}$ through $J_{6}$. For other planets only their $J_{2}$ value is used, while the higher order harmonics are set to zero. The gravitational zonal harmonic accelerations are then given by\cite{schaub} \begin{align} - \bm a_{J_{2}} &= -\frac{3}{2} J_{2} + \bm a_{J_{2}} &= -\frac{3}{2} J_{2} \left(\frac{\mu}{r^{2}}\right) \left( \frac{r_{eq}}{r}\right)^{2} \leftexp{N\!\!\!}{ @@ -288,63 +288,63 @@ \subsubsection{Space Environment Parameters} \end{pmatrix}} \label{eq:gm:aJ2} \\ - \bm a_{J_{3}} &= -\frac{1}{2} J_{3} + \bm a_{J_{3}} &= -\frac{1}{2} J_{3} \left(\frac{\mu}{r^{2}}\right) \left( \frac{r_{eq}}{r}\right)^{3} \leftexp{N\!\!\!}{ \begin{pmatrix} - 5\left(7 \left(\frac{z}{r}\right)^{3} - 3 \left(\frac{z}{r}\right) + 5\left(7 \left(\frac{z}{r}\right)^{3} - 3 \left(\frac{z}{r}\right) \right) \frac{x}{r} \\ - 5\left(7 \left(\frac{z}{r}\right)^{3} - 3 \left(\frac{z}{r}\right) + 5\left(7 \left(\frac{z}{r}\right)^{3} - 3 \left(\frac{z}{r}\right) \right) \frac{y}{r} \\ - 3\left(10\left(\frac{z}{r}\right)^{2} - \frac{35}{3} - \left(\frac{z}{r}\right)^{4} - 1 \right) + 3\left(10\left(\frac{z}{r}\right)^{2} - \frac{35}{3} + \left(\frac{z}{r}\right)^{4} - 1 \right) \end{pmatrix}} \label{eq:gm:aJ3} \\ - \bm a_{J_{4}} &= -\frac{5}{8} J_{4} + \bm a_{J_{4}} &= -\frac{5}{8} J_{4} \left(\frac{\mu}{r^{2}}\right) \left( \frac{r_{eq}}{r}\right)^{4} \leftexp{N\!\!\!}{ \begin{pmatrix} - \left(3-42 \left(\frac{z}{r}\right)^{2} +63 \left(\frac{z}{r}\right)^{4} + \left(3-42 \left(\frac{z}{r}\right)^{2} +63 \left(\frac{z}{r}\right)^{4} \right) \frac{x}{r} \\ - \left(3-42 \left(\frac{z}{r}\right)^{2} +63 \left(\frac{z}{r}\right)^{4} + \left(3-42 \left(\frac{z}{r}\right)^{2} +63 \left(\frac{z}{r}\right)^{4} \right) \frac{y}{r} \\ - -\left(15-70\left(\frac{z}{r}\right)^{2} + 63 + -\left(15-70\left(\frac{z}{r}\right)^{2} + 63 \left(\frac{z}{r}\right)^{4} \right) \frac{z}{r} \end{pmatrix}} \label{eq:gm:aJ4} \\ - \bm a_{J_{5}} &= -\frac{J_{5}}{8} + \bm a_{J_{5}} &= -\frac{J_{5}}{8} \left(\frac{\mu}{r^{2}}\right) \left( \frac{r_{eq}}{r}\right)^{5} \leftexp{N\!\!\!}{ \begin{pmatrix} - 3\left(35\left(\frac{z}{r}\right) - 210 + 3\left(35\left(\frac{z}{r}\right) - 210 \left(\frac{z}{r}\right)^{3} +231 \left(\frac{z}{r}\right)^{5} \right) \frac{x}{r} \\ - 3\left(35\left(\frac{z}{r}\right) - 210 + 3\left(35\left(\frac{z}{r}\right) - 210 \left(\frac{z}{r}\right)^{3} +231 \left(\frac{z}{r}\right)^{5} \right) \frac{y}{r} \\ - \left(15- 315\left(\frac{z}{r}\right)^{2} \!\!+ 945 - \left( \frac{z}{r}\right)^{4}\!\! -693 \left(\frac{z}{r}\right)^{6} - \right) + \left(15- 315\left(\frac{z}{r}\right)^{2} \!\!+ 945 + \left( \frac{z}{r}\right)^{4}\!\! -693 \left(\frac{z}{r}\right)^{6} + \right) \end{pmatrix}} \label{eq:gm:aJ5} \\ - \bm a_{J_{6}} &= \frac{J_{6}}{16} + \bm a_{J_{6}} &= \frac{J_{6}}{16} \left(\frac{\mu}{r^{2}}\right)\! \left( \frac{r_{eq}}{r}\right)^{6}\! \leftexp{N\!\!\!}{ \begin{pmatrix} \left(35 - 945 \left(\frac{z}{r}\right)^{2} \!\! + 3465 \left(\frac{z}{r}\right)^{4} \!\! - - 3003 \left(\frac{z}{r}\right)^{6} + - 3003 \left(\frac{z}{r}\right)^{6} \right) \frac{x}{r} \\ \left(35 - 945 \left(\frac{z}{r}\right)^{2} \!\! + 3465 \left(\frac{z}{r}\right)^{4} \!\! - - 3003 \left(\frac{z}{r}\right)^{6} + - 3003 \left(\frac{z}{r}\right)^{6} \right) \frac{y}{r} \\ \left( 3003 \left(\frac{z}{r}\right)^{6} \!\! -4851 \left(\frac{z}{r}\right)^{4}\!\! @@ -368,4 +368,4 @@ \subsubsection{Space Environment Parameters} %9 orbital anomalies %8 orbital elements %9 environment -%234 rigid body kinematics \ No newline at end of file +%234 rigid body kinematics diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/secModelFunctions.tex b/src/architecture/utilitiesSelfCheck/_Documentation/secModelFunctions.tex index 49b0d88733..3e967464fe 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/secModelFunctions.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/secModelFunctions.tex @@ -8,8 +8,8 @@ \section{Library Functions} \item \textbf{C-Code implementation}: All function calls only use C-code. \item \textbf{Linear Algebra}: Basic matrix math function that are common in Matlab and Python environment quickly become tedious when programming in C. To avoid this, a custom written C library is developed to perform specialized 2, 3, 4, and 6 dimensional matrix math, but some linear algebra math on matrices of general size. \item \textbf{Rigid Body Kinematics}: Provides a comprehensive C-based software library to convert between a broad range of rigid body attitude coordinates, as well as add and subtract orientations. This library can also compute the differential kinematic equations of select coordinate types. The textbook Reference~\citenum{schaub} contains a complete listing of all the attitude library function in Appendix E. - \item \textbf{Orbital Motion Library}: Provides a comprehensive C-based software library to convert between orbit anomaly angles, as well as orbit elements and Cartesian coordinates. This library also contains some simple space environment modeling functions. - + \item \textbf{Orbital Motion Library}: Provides a comprehensive C-based software library to convert between orbit anomaly angles, as well as orbit elements and Cartesian coordinates. This library also contains some simple space environment modeling functions. + \end{itemize} @@ -18,19 +18,19 @@ \section{Library Assumptions and Limitations} \subsection{Assumptions} \subsubsection{Linear Algebra Library} -The vector dimension can be declared either explicitly through +The vector dimension can be declared either explicitly through \begin{verbatim} double vec[3] \end{verbatim} -or implicitly through -\begin{verbatim} +or implicitly through +\begin{verbatim} double *vec -\end{verbatim} -and allocating the required memory dynamically. However, with the specialized matrix dimensions the matrices are assumed to be defined through commands like +\end{verbatim} +and allocating the required memory dynamically. However, with the specialized matrix dimensions the matrices are assumed to be defined through commands like \begin{verbatim} double m33[3][3] \end{verbatim} - for a $3\times 3$ matrix. + for a $3\times 3$ matrix. \subsubsection{Rigid Body Kinematics} These attitude kinematic relationships are general enough that no assumptions are made on the mathematics. However, when the $3\times 3$ DCM are defined, they must be of type @@ -40,33 +40,20 @@ \subsubsection{Rigid Body Kinematics} \subsubsection{Orbital Motion} -This orbital motion library accepts the inertial position and velocity vectors as pointers to a 3-dimensional array. As with the linear algebra library, these can be declared explicitly or implicitly. +This orbital motion library accepts the inertial position and velocity vectors as pointers to a 3-dimensional array. As with the linear algebra library, these can be declared explicitly or implicitly. \subsection{Limitations} \subsubsection{Linear Algebra Library} -There are no limitations on the provided linear algebra routines. They are written in a general manner. +There are no limitations on the provided linear algebra routines. They are written in a general manner. \subsubsection{Rigid Body Kinematics} -There are no limitations on the provided linear algebra routines. They are written in a general manner. +There are no limitations on the provided linear algebra routines. They are written in a general manner. \subsubsection{Orbital Motion} The orbital motion support library has the following limitations. \begin{itemize} \item While the {\tt elem2rv()} support rectilinear cases, the inverse mapping in {\tt rv2elem()} does not. \item The gravitational zonal harmonics for $J_{3}$-$J_{6}$ are only implemented for Earth, and not other celestial objects. - \item The atmospheric density and drag, as well as the Debye length models are only implemented for Earth orbiting scenarios. + \item The atmospheric density and drag, as well as the Debye length models are only implemented for Earth orbiting scenarios. \end{itemize} - - - - - - - - - - - - - diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/secTest.tex b/src/architecture/utilitiesSelfCheck/_Documentation/secTest.tex index 002946441d..6d66d178a6 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/secTest.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/secTest.tex @@ -13,11 +13,11 @@ \section{Test Description and Success Criteria} \caption{Unit Test Absolute Difference Tolerances} \label{tbl:tolerance} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c} % Column formatting, - \hline - \hline + \begin{tabular}{c | c} % Column formatting, + \hline + \hline Test & Absolute Tolerance \\ - \hline + \hline Linear Algebra & $10^{-10}$ \\ Rigid Body Kinematics & $10^{-10}$ \\ Orbital Motion: Anomalies & $10^{-10}$ \\ @@ -35,27 +35,27 @@ \section{Test Description and Success Criteria} \section{Test Parameters} \subsection{Linear Algebra} -In these unit tests each function is provided non-trivial inputs, such as {\tt v1 = [1,2,3]} to evaluate the associate matrix math. For specific information on the values used, look at the source code for the function {\tt testLinearAlgebra()} in the file {\tt avsLibrarySelfCheck.c}. +In these unit tests each function is provided non-trivial inputs, such as {\tt v1 = [1,2,3]} to evaluate the associate matrix math. For specific information on the values used, look at the source code for the function {\tt testLinearAlgebra()} in the file {\tt avsLibrarySelfCheck.c}. \subsection{Rigid Body Kinematics} -The attitude kinematics sub-routines are provided with non-trivial inputs to test the functions. For specific information on the values used, look at the source code for the function {\tt testRigidBodyKinematics()} in the file {\tt avsLibrarySelfCheck.c}. +The attitude kinematics sub-routines are provided with non-trivial inputs to test the functions. For specific information on the values used, look at the source code for the function {\tt testRigidBodyKinematics()} in the file {\tt avsLibrarySelfCheck.c}. \subsection{Orbital Motion} -The orbital anomaly sub-routines are provided with non-trivial inputs to test the functions. For specific information on the values used, look at the source code for the function {\tt testOrbitalAnomalies()} in the file {\tt avsLibrarySelfCheck.c}. +The orbital anomaly sub-routines are provided with non-trivial inputs to test the functions. For specific information on the values used, look at the source code for the function {\tt testOrbitalAnomalies()} in the file {\tt avsLibrarySelfCheck.c}. -Regarding the orbit element to Cartesian coordinate conversion routines, an Earth orbit is simulated that is either non-equatorial and non-circular (case 1), non-circular equatorial (case 2), circular and inclined (case 3) and circular equatorial (case 4). The resulting orbit elements used are shown in Table~\ref{tbl:orbitValues}. +Regarding the orbit element to Cartesian coordinate conversion routines, an Earth orbit is simulated that is either non-equatorial and non-circular (case 1), non-circular equatorial (case 2), circular and inclined (case 3) and circular equatorial (case 4). The resulting orbit elements used are shown in Table~\ref{tbl:orbitValues}. \begin{table}[htbp] \caption{Orbit Elements Used in Orbit Coordinate Conversion Routine Checking} \label{tbl:orbitValues} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c || c | c | c | c | c | c} % Column formatting, - \hline - \hline + \begin{tabular}{c || c | c | c | c | c | c} % Column formatting, + \hline + \hline Case & SMA [km]& $e$ & $i$[\dg] & $\Omega$[\dg\ & $\omega$[\dg\ & $f$[\dg] \\ - \hline + \hline 1 & 7500 & 0.5 & 40 & 133 & 113 & 123 \\ 2 & 7500 & 0.5 & 0 & 133 & 113 & 123 \\ 3 & 7500 & 0.0 & 40 & 133 & 113 & 123 \\ @@ -70,38 +70,37 @@ \subsection{Orbital Motion} \section{Test Results} -An automated suite of tests are run to perform unit tests on all the AVS support library functions. The results are shown in Table~\ref{tbl:avsLibraryResults}. +An automated suite of tests are run to perform unit tests on all the AVS support library functions. The results are shown in Table~\ref{tbl:avsLibraryResults}. \begin{table}[h] \caption{Integration test results.} \label{tbl:avsLibraryResults} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | l } % Column formatting, + \begin{tabular}{c | c | l } % Column formatting, \hline\hline - \textbf{Test} & \textbf{Pass/Fail} & \textbf{BSK Error Notes} + \textbf{Test} & \textbf{Pass/Fail} & \textbf{BSK Error Notes} \\ \hline - {\tt rigidBodyKinematics} & + {\tt rigidBodyKinematics} & \input{AutoTex/test_avsLibrarySelfCheckTestMsg-testRigidBodyKinematics} & \input{AutoTex/test_avsLibrarySelfCheckMsg-testRigidBodyKinematics} - \\ - {\tt orbitalElements} & + \\ + {\tt orbitalElements} & \input{AutoTex/test_avsLibrarySelfCheckTestMsg-testOrbitalElements} & \input{AutoTex/test_avsLibrarySelfCheckMsg-testOrbitalElements} - \\ - {\tt orbitalAnomalies} & + \\ + {\tt orbitalAnomalies} & \input{AutoTex/test_avsLibrarySelfCheckTestMsg-testOrbitalAnomalies} & \input{AutoTex/test_avsLibrarySelfCheckMsg-testOrbitalAnomalies} - \\ - {\tt linearAlgebra} & + \\ + {\tt linearAlgebra} & \input{AutoTex/test_avsLibrarySelfCheckTestMsg-testLinearAlgebra} & \input{AutoTex/test_avsLibrarySelfCheckMsg-testLinearAlgebra} - \\ - {\tt environment} & + \\ + {\tt environment} & \input{AutoTex/test_avsLibrarySelfCheckTestMsg-testEnvironment} & \input{AutoTex/test_avsLibrarySelfCheckMsg-testEnvironment} - \\ + \\ \hline\hline \end{tabular} \end{table} - diff --git a/src/architecture/utilitiesSelfCheck/_Documentation/secUserGuide.tex b/src/architecture/utilitiesSelfCheck/_Documentation/secUserGuide.tex index 73cefe623e..4ec17a5d72 100644 --- a/src/architecture/utilitiesSelfCheck/_Documentation/secUserGuide.tex +++ b/src/architecture/utilitiesSelfCheck/_Documentation/secUserGuide.tex @@ -3,12 +3,12 @@ \section{User Guide} \subsection{{\tt linearAlgebra} Library} -The linear algebra library provides numerous C-based functions to perform basic matrix math operations. For a complete list of functions supported, consult the {\tt linearAlgebra.h} file. +The linear algebra library provides numerous C-based functions to perform basic matrix math operations. For a complete list of functions supported, consult the {\tt linearAlgebra.h} file. \subsubsection{Vector Operations} The vector related functions all begin with a letter '{\tt v}' and are broken down in the following categories depending on if the linear algebra operation is performed on a matrix of a general size, or of a specific size of 2, 3, 4 or 6. \begin{itemize} - \item if the function is {\tt vXXXX()} the code works on a $n$x1 matrix of arbitrary length $n$. + \item if the function is {\tt vXXXX()} the code works on a $n$x1 matrix of arbitrary length $n$. \item if the function is {\tt v2XXXX()} the code works on a matrix of length 2 defined as {\tt double vec[2]}. \item if the function is {\tt v3XXXX()} the code works on a matrix of length 2 defined as {\tt double vec[3]}. \item if the function is {\tt v4XXXX()} the code works on a matrix of length 2 defined as {\tt double vec[4]}. @@ -26,7 +26,7 @@ \subsubsection{Vector Operations} \item {\tt OuterProduct}: Returns the outer product $\bm v_{1} \bm v_{2}^{T}$ \item {\tt Normalize}: Returns a normalized vector $\bm v_{1}/ v_{1}$ \item {\tt MaxAbs}: Returns the largest element of a vector - \item {\tt IsEqual}: Checks if two vector represnetations are identical + \item {\tt IsEqual}: Checks if two vector represnetations are identical \item {\tt IsZero}: Checks if a vector is full of zero elements \item {\tt Print}: Prints the vector representation to a file \item {\tt Sort}: Sorts the vector elements by size @@ -42,12 +42,12 @@ \subsubsection{Matrix Operations} The matrix related functions are all labeled with a letter '{\tt M}' and are broken down into the following matrix dimension related categories: \begin{itemize} \item {\tt mXXXXy()}: the code works on a $n\times m$ matrix of arbitrary dimension $n$ and $m$. - \item {\tt m22XXXX()}: the code works on a $2\times 2$ matrix defined through {\tt mat[2][2]}. - \item {\tt m33XXXX()}: the code works on a $3\times 3$ matrix defined through {\tt mat[3]3]}. - \item {\tt m44XXXX()}: the code works on a $4\times 4$ matrix defined through {\tt mat[4][4]}. + \item {\tt m22XXXX()}: the code works on a $2\times 2$ matrix defined through {\tt mat[2][2]}. + \item {\tt m33XXXX()}: the code works on a $3\times 3$ matrix defined through {\tt mat[3]3]}. + \item {\tt m44XXXX()}: the code works on a $4\times 4$ matrix defined through {\tt mat[4][4]}. \end{itemize} -The following list provides an overview of the supported matrix functions. Note that not al dimensions have all functions provided, but the 2, 3, and 4 dimensional matrix support is pretty complete. +The following list provides an overview of the supported matrix functions. Note that not al dimensions have all functions provided, but the 2, 3, and 4 dimensional matrix support is pretty complete. \begin{itemize} \item {\tt SetIdentity}: Returns an identity matrix \item {\tt SetZero}: Returns a zero matrix @@ -56,9 +56,9 @@ \subsubsection{Matrix Operations} \item {\tt m33MultM33}: Performs the matrix to matrix multiplication $[M_{1}][M_{2}]$ \item {\tt m33MultM33t}: Performs the matrix to matrix multiplication $[M_{1}][M_{2}]^{T}$ \item {\tt m33tMultM33}: Performs the matrix to matrix multiplication $[M_{1}]^{T}[M_{2}]$ - \item {\tt m33MultV3}: Computes $[M_{1}]\bm v_{1}$ - \item {\tt m33tMultV3}: Computes $[M_{1}]^{T}\bm v_{1}$ - \item {\tt v3tMultM33}: Computes $\bm v_{1}^{T}[M_{1}]$ + \item {\tt m33MultV3}: Computes $[M_{1}]\bm v_{1}$ + \item {\tt m33tMultV3}: Computes $[M_{1}]^{T}\bm v_{1}$ + \item {\tt v3tMultM33}: Computes $\bm v_{1}^{T}[M_{1}]$ \item {\tt v3tMultM33t}: Computes $\bm v_{1}^{T}[M_{1}]^{T}$ \item {\tt Tilde}: Returns the skew-symmetric matrix $[\tilde{\bm v}_{1}]$ \item {\tt Transpose}: Returns $[M_{1}]^{T}$ @@ -119,24 +119,24 @@ \subsubsection{Orbit Element Conversion} \begin{verbatim} elem2rv(double mu, classicElements *elements, double *rVec, double *vVec) \end{verbatim} -is used where {\tt mu} is the gravitational constant of the 2-body problem, the classical elements are defined through +is used where {\tt mu} is the gravitational constant of the 2-body problem, the classical elements are defined through $$ (a, e, i, \Omega, \omega, f) $$ -where the anomaly angle is typically given by $f$, unless the orbit is a rectilinear motion in which case the anomaly angle input is $E$. The function returns the inertial position and velocity vectors in the arrays {\tt rVec} and {\tt vVec}. +where the anomaly angle is typically given by $f$, unless the orbit is a rectilinear motion in which case the anomaly angle input is $E$. The function returns the inertial position and velocity vectors in the arrays {\tt rVec} and {\tt vVec}. To convert from inertial Cartesian coordinates to classical orbit elements, the function \begin{verbatim} rv2elem(double mu, double *rVec, double *vVec, classicElements *elements) \end{verbatim} -Beyond the classical elements listed above, this routine also stores the radius of perapses $r_{p}$ and apoapses $r_{a}$, as well as $\alpha = \dfrac{1}{a}$. +Beyond the classical elements listed above, this routine also stores the radius of perapses $r_{p}$ and apoapses $r_{a}$, as well as $\alpha = \dfrac{1}{a}$. \subsubsection{Space Environment Functions} \paragraph{Atmospheric Density} The Earth's atmospheric density $\rho$ is computed using\\ \indent {\tt double atmosphericDensity(double alt)} \\ -The density is returned as a scalar value. +The density is returned as a scalar value. \paragraph{Mean Debye Length} The mean Debye Length for the near-Earth environment is approximated, very crudely, through a polynomial fit. The function returns the scalar $\lambda_{d}$ and is called through \\ @@ -146,21 +146,16 @@ \subsubsection{Space Environment Functions} To compute an estimate of the Earth's atmospheric drag acceleration, use the function: \\ \indent {\tt void atmosphericDrag(double Cd, double A, double m, double *rvec, double *vvec, double *advec)} -The inputs are the ballistic drag coefficient {\tt Cd}, the velocity-projected cross-sectional area {\tt A}, as well as the spacecraft mass {\tt m}. Given the inertial position vectors {\tt rvec} and {\tt vvec}, the function returns the drag acceleration {\tt advec}. +The inputs are the ballistic drag coefficient {\tt Cd}, the velocity-projected cross-sectional area {\tt A}, as well as the spacecraft mass {\tt m}. Given the inertial position vectors {\tt rvec} and {\tt vvec}, the function returns the drag acceleration {\tt advec}. \paragraph{Gravitational Zonal Harmonics} This function returns the inertial acceleration due to a planets zonal Harmonic. The function call is:\\ \indent {\tt void jPerturb(double *rvec, int num, double *ajtot, ...)} -If not option argument is provided, then the zonal harmonics of Earth are simulated. About other celestial objects only the $J_{2}$ harmonic is implemented. Here the object is specified through the {\tt CelestialObject\_t} enumeration. For example, to get the $J_{2}$ zonal harmonic about Venus the argument {\tt CELESTIAL\_VENUS} is provided. +If not option argument is provided, then the zonal harmonics of Earth are simulated. About other celestial objects only the $J_{2}$ harmonic is implemented. Here the object is specified through the {\tt CelestialObject\_t} enumeration. For example, to get the $J_{2}$ zonal harmonic about Venus the argument {\tt CELESTIAL\_VENUS} is provided. \paragraph{Solar Radiation Pressure Acceleration} To compute the inertial disturbance acceleration due to the solar radiation pressure use the function\\ \indent {\tt void solarRad(double A, double m, double *sunvec, double *arvec)} -Here {\tt A} is the projected surface area, {\tt m} is the spacecraft mass, {\tt sunvec} is the sun position vector to the Sun in units of AU. - - - - - +Here {\tt A} is the projected surface area, {\tt m} is the spacecraft mass, {\tt sunvec} is the sun position vector to the Sun in units of AU. diff --git a/src/architecture/utilitiesSelfCheck/_UnitTest/test_BSpline.py b/src/architecture/utilitiesSelfCheck/_UnitTest/test_BSpline.py index 7b6c73080b..469b6f6f13 100644 --- a/src/architecture/utilitiesSelfCheck/_UnitTest/test_BSpline.py +++ b/src/architecture/utilitiesSelfCheck/_UnitTest/test_BSpline.py @@ -44,7 +44,7 @@ def test_BSpline(show_plots, P, XDot_flag, XDDot_flag, accuracy): r""" **Validation Test Description** - This unit test script tests the capability of the BSpline function to correctly interpolate + This unit test script tests the capability of the BSpline function to correctly interpolate a series of points in 3 dimensions. The coordinates of these 7 points are stored in 3 numpy arrays: @@ -54,8 +54,8 @@ def test_BSpline(show_plots, P, XDot_flag, XDDot_flag, accuracy): X3 = np.array([3, 2, 1, 2, 3, 4, 5]). - The input arrays are initialized through ``Input = BSpline.InputDataSet(X1, X2, X3)``. - The time tags at which each waypoint is to be hit are provided through ``Input.setT([0, 2, 3, 5, 7, 8, 10])``. + The input arrays are initialized through ``Input = BSpline.InputDataSet(X1, X2, X3)``. + The time tags at which each waypoint is to be hit are provided through ``Input.setT([0, 2, 3, 5, 7, 8, 10])``. Alternatively, it is possible to specify the average velocity norm through ``Input.setAvgXDot()``. The endpoint derivatives are specified through the methods: @@ -69,8 +69,8 @@ def test_BSpline(show_plots, P, XDot_flag, XDDot_flag, accuracy): The interpolation happens calling the method ``BSpline.interpolate(Input, N, P, Output)`` where: - N is the desired number of equally spaced data points in the interpolated function; - - - P is the polynomial order of the B-Spline function. The order should be at least 3 when first-order derivatives are specified, + + - P is the polynomial order of the B-Spline function. The order should be at least 3 when first-order derivatives are specified, and 5 when second-order derivatives are specified. The maximum oder is P = n + k - 1, with n being the number of waypoints and k being the number of endpoint derivatives that are being specified. @@ -89,11 +89,11 @@ def test_BSpline(show_plots, P, XDot_flag, XDDot_flag, accuracy): **Description of Variables Being Tested** - This unit test checks the correctness of the interpolated function: + This unit test checks the correctness of the interpolated function: - a check is performed on whether or not each waypoint is hit at the specified time; - when the derivatives are specified, it checks whether the starting point derivative actually matches the input derivative. """ - + # each test method requires a single assert method to be called [testResults, testMessage] = BSplineTestFunction(P, XDot_flag, XDDot_flag, accuracy) assert testResults < 1, testMessage @@ -133,14 +133,14 @@ def BSplineTestFunction(P, XDot_flag, XDDot_flag, accuracy): testFailCount += 1 testMessages.append("FAILED: BSpline." + " Function of order {} failed coordinate #3 check at time t = {}".format(P,Input.T[j][0])) if XDot_flag: - if not ((abs(Output.XD1[0][0]-Input.XDot_0[0][0]) < accuracy) and - (abs(Output.XD2[0][0]-Input.XDot_0[1][0]) < accuracy) and + if not ((abs(Output.XD1[0][0]-Input.XDot_0[0][0]) < accuracy) and + (abs(Output.XD2[0][0]-Input.XDot_0[1][0]) < accuracy) and (abs(Output.XD3[0][0]-Input.XDot_0[2][0]) < accuracy)): testFailCount += 1 testMessages.append("FAILED: BSpline." + " Function of order {} failed first derivative at starting point".format(P)) if XDDot_flag: - if not ((abs(Output.XDD1[0][0]-Input.XDDot_0[0][0]) < accuracy) and - (abs(Output.XDD2[0][0]-Input.XDDot_0[1][0]) < accuracy) and + if not ((abs(Output.XDD1[0][0]-Input.XDDot_0[0][0]) < accuracy) and + (abs(Output.XDD2[0][0]-Input.XDDot_0[1][0]) < accuracy) and (abs(Output.XDD3[0][0]-Input.XDDot_0[2][0]) < accuracy)): testFailCount += 1 testMessages.append("FAILED: BSpline." + " Function of order {} failed second derivative at starting point".format(P)) @@ -155,7 +155,7 @@ def BSplineTestFunction(P, XDot_flag, XDDot_flag, accuracy): # if __name__ == "__main__": BSplineTestFunction( - 5, # polynomial order + 5, # polynomial order True, # XDot_flag False, # XDDot_flag - 1e-6) + 1e-6) diff --git a/src/cmake/conan.cmake b/src/cmake/conan.cmake index b30a547828..ed2a10dd04 100644 --- a/src/cmake/conan.cmake +++ b/src/cmake/conan.cmake @@ -192,8 +192,8 @@ function(conan_cmake_settings result) message(FATAL_ERROR "Conan: Unknown MSVC architecture [${MSVC_${LANGUAGE}_ARCHITECTURE_ID}]") endif() endif() - - + + conan_cmake_detect_vs_runtime(_vs_runtime) message(STATUS "Detected VS runtime: ${_vs_runtime}") set(_SETTINGS ${_SETTINGS} -s compiler.runtime=${_vs_runtime}) @@ -490,4 +490,3 @@ macro(conan_add_remote) execute_process(COMMAND ${CONAN_CMD} remote add ${CONAN_NAME} ${CONAN_URL} ${CONAN_INDEX_ARG} -f) endmacro() - diff --git a/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_Documentation/AVS-Sim-LowPassFilterControlTorque-20160108.tex b/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_Documentation/AVS-Sim-LowPassFilterControlTorque-20160108.tex index 7960815c43..997be347a7 100755 --- a/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_Documentation/AVS-Sim-LowPassFilterControlTorque-20160108.tex +++ b/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_Documentation/AVS-Sim-LowPassFilterControlTorque-20160108.tex @@ -44,32 +44,32 @@ ~\\ \hrule ~\\ \section{Introduction} -A low-pass filter module provides the ability to apply a frequency based filter to the ADCS control torque vector $\bm L_{r}$. The module has the ability to be individually reset, separate from any reset on the ADCS control modules. The cut-off frequency is given by $\omega_{c}$, while the filter time step is given by $h$. +A low-pass filter module provides the ability to apply a frequency based filter to the ADCS control torque vector $\bm L_{r}$. The module has the ability to be individually reset, separate from any reset on the ADCS control modules. The cut-off frequency is given by $\omega_{c}$, while the filter time step is given by $h$. \section{Initialization} Prior to using the module, the filter time step and 1st order filter frequency cut-off value must be set. \begin{gather*} {\tt Config->h} \\ - {\tt Config->wc} + {\tt Config->wc} \end{gather*} \section{Algorithm} -Since the shown mappings between the Laplace domain and the $Z$-domain -are approximate, some frequency warping will occur. If a continuous -filter design has a critical frequency $\omega_{c}$, then the digital -implementation might have a slightly different critical frequency. -Franklin in Reference~\citenum{franklin1} compares the continuous and -digital filter performances by studying the half-power point. This -leads to the following relationship between the continuous time -critical filter frequency $\omega_{c}$ and the digital filter +Since the shown mappings between the Laplace domain and the $Z$-domain +are approximate, some frequency warping will occur. If a continuous +filter design has a critical frequency $\omega_{c}$, then the digital +implementation might have a slightly different critical frequency. +Franklin in Reference~\citenum{franklin1} compares the continuous and +digital filter performances by studying the half-power point. This +leads to the following relationship between the continuous time +critical filter frequency $\omega_{c}$ and the digital filter frequency $\hat\omega$: \begin{equation} \label{eq:wa} \tan \left(\frac{ w_{c} h}{2}\right) = \frac{\hat\omega h}{2} \end{equation} -where $h=1/f$ is the digital sample time. Note that $\hat\omega \approx -\omega_{c}$ if the sample frequency is much higher than the critical -filter frequency. +where $h=1/f$ is the digital sample time. Note that $\hat\omega \approx +\omega_{c}$ if the sample frequency is much higher than the critical +filter frequency. The first-order digital filter formula is given by: \begin{equation} diff --git a/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_Documentation/BasiliskReportMemo.cls b/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_Documentation/BasiliskReportMemo.cls +++ b/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_UnitTest/Support/LPF-Validation.nb b/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_UnitTest/Support/LPF-Validation.nb index bf05b17393..a6e41ed402 100644 --- a/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_UnitTest/Support/LPF-Validation.nb +++ b/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_UnitTest/Support/LPF-Validation.nb @@ -44,63 +44,63 @@ Cell["Setup filter parameters", "Subsection", Cell[BoxData[{ RowBox[{ - RowBox[{"h", " ", "=", " ", "0.5"}], ";"}], "\[IndentingNewLine]", + RowBox[{"h", " ", "=", " ", "0.5"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Omega]c", " ", "=", " ", - RowBox[{"0.1", " ", "*", "2", " ", "\[Pi]"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"\[Omega]c", " ", "=", " ", + RowBox[{"0.1", " ", "*", "2", " ", "\[Pi]"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"Num", " ", "=", " ", "500"}], ";"}]}], "Input", - CellChangeTimes->{{3.658680922010996*^9, 3.658680946091288*^9}, + CellChangeTimes->{{3.658680922010996*^9, 3.658680946091288*^9}, 3.658681203177627*^9, 3.658681236819653*^9, 3.6586815153683167`*^9, { - 3.6586847614768057`*^9, 3.658684773480979*^9}, {3.6586848323739967`*^9, + 3.6586847614768057`*^9, 3.658684773480979*^9}, {3.6586848323739967`*^9, 3.658684846593954*^9}, 3.658703429437562*^9}], Cell[BoxData[ RowBox[{ - RowBox[{"h\[Omega]", " ", "=", " ", + RowBox[{"h\[Omega]", " ", "=", " ", RowBox[{ - RowBox[{"2", "/", "h"}], " ", - RowBox[{"Tan", "[", - RowBox[{"\[Omega]c", " ", - RowBox[{"h", "/", "2"}]}], "]"}], " ", "*", " ", "h"}]}], + RowBox[{"2", "/", "h"}], " ", + RowBox[{"Tan", "[", + RowBox[{"\[Omega]c", " ", + RowBox[{"h", "/", "2"}]}], "]"}], " ", "*", " ", "h"}]}], ";"}]], "Input", CellChangeTimes->{{3.658680992080245*^9, 3.658681017258251*^9}}], Cell[BoxData[ RowBox[{ - RowBox[{"input", " ", "=", - RowBox[{"Table", "[", + RowBox[{"input", " ", "=", + RowBox[{"Table", "[", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"i", " ", "h"}], ",", - RowBox[{"Cos", "[", - RowBox[{"i", "*", - RowBox[{"h", "/", "10"}]}], "]"}]}], "}"}], ",", " ", - RowBox[{"{", + RowBox[{"i", " ", "h"}], ",", + RowBox[{"Cos", "[", + RowBox[{"i", "*", + RowBox[{"h", "/", "10"}]}], "]"}]}], "}"}], ",", " ", + RowBox[{"{", RowBox[{"i", ",", "0", ",", "Num"}], "}"}]}], "]"}]}], ";"}]], "Input", - CellChangeTimes->{{3.658681076949716*^9, 3.658681136240497*^9}, + CellChangeTimes->{{3.658681076949716*^9, 3.658681136240497*^9}, 3.658681215995064*^9, {3.658681278735218*^9, 3.658681284392406*^9}, { - 3.658681520535328*^9, 3.658681528643573*^9}, {3.658684782119936*^9, + 3.658681520535328*^9, 3.658681528643573*^9}, {3.658684782119936*^9, 3.658684791648251*^9}, 3.658703411641711*^9}], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"ListPlot", "[", - RowBox[{"input", ",", " ", - RowBox[{"Joined", "\[Rule]", "True"}], ",", - RowBox[{"AxesLabel", "\[Rule]", - RowBox[{"{", - RowBox[{"\"\ - Produced by OmniGraffle 7.11.3 + Produced by OmniGraffle 7.11.3 2023-04-13 20:29:59 +0000 diff --git a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.rst b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.rst index 828e2de214..5974a4060b 100644 --- a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.rst +++ b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.rst @@ -10,9 +10,9 @@ not every source of uncertainty in the problem is an estimated parameter. Future Message Connection Descriptions ------------------------------- -The following table lists all the module input and output messages. -The module msg connection is set by the user from python. -The msg type contains a link to the message structure definition, while the description +The following table lists all the module input and output messages. +The module msg connection is set by the user from python. +The msg type contains a link to the message structure definition, while the description provides information on what this message is used for. .. _ModuleIO_smallBodyNavEKF: @@ -23,7 +23,7 @@ provides information on what this message is used for. Note that this C++ FSW module provides both C- and C++-wrapped output messages. The regular C++ wrapped output messages end with the usual ``...OutMsg``. The C wrapped output messages have the same payload type, but end -with ``...OutMsgC``. +with ``...OutMsgC``. .. list-table:: Module I/O Messages :widths: 25 25 50 @@ -225,4 +225,4 @@ The user then must set the following module variables: - ``P_k`` to initialize :math:`P_0` -The user must connect to each input message described in Table 1. \ No newline at end of file +The user must connect to each input message described in Table 1. diff --git a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/_Documentation/Images/moduleIOSmallBodyNavigationUKF.svg b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/_Documentation/Images/moduleIOSmallBodyNavigationUKF.svg index 658c8d6b01..4858dfcd3e 100644 --- a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/_Documentation/Images/moduleIOSmallBodyNavigationUKF.svg +++ b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/_Documentation/Images/moduleIOSmallBodyNavigationUKF.svg @@ -97,7 +97,7 @@ Produced by OmniGraffle 7.11.3 + id="metadata361"> Produced by OmniGraffle 7.11.3 2021-07-20 16:23:27 +0000 \>\>\>\>\> w^{[0]}_{c}=w^{[0]}_{m}+1-\alpha^2+\beta,\>\>\>\>\>\> w^{[i]}_{m}=w^{[i]}_{c}=1/(2N+\kappa) \>\> i\neq 0 - + Algorithm ^^^^^^^^^^ This module employs an unscented Kalman filter (UKF) `Wan and Van Der Merwe `__ to estimate the -relevant states. The UKF relies on the unscented transform (UT) to compute the non-linear transformation of a Gaussian distribution. Let -consider a random variable :math:`\mathbf{x}` of dimension :math:`N` modelled as a Gaussian distribution with mean :math:`\hat{\mathbf{x}}` +relevant states. The UKF relies on the unscented transform (UT) to compute the non-linear transformation of a Gaussian distribution. Let +consider a random variable :math:`\mathbf{x}` of dimension :math:`N` modelled as a Gaussian distribution with mean :math:`\hat{\mathbf{x}}` and covariance :math:`P`. The UT computes numerically the resulting mean and covariance of :math:`\mathbf{y}=\mathbf{f}(\mathbf{x})` by creating :math:`2N+1` samples named sigma points as @@ -96,7 +96,7 @@ creating :math:`2N+1` samples named sigma points as \pmb{\chi}^{[i]} = \hat{\mathbf{x}} \pm \left(\sqrt{(N+\kappa) P}\right)_{|i|},\>\> i = -N...N -where :math:`|i|` denotes the columns of the matrix. Then, transform each sigma point as :math:`\pmb{\xi}^{[i]}=\mathbf{f}(\pmb{\chi}^{[i]})`. Finally, compute the mean and covariance of +where :math:`|i|` denotes the columns of the matrix. Then, transform each sigma point as :math:`\pmb{\xi}^{[i]}=\mathbf{f}(\pmb{\chi}^{[i]})`. Finally, compute the mean and covariance of :math:`\mathbf{y}=\mathbf{f}(\mathbf{x})` as .. math:: @@ -109,7 +109,7 @@ where :math:`|i|` denotes the columns of the matrix. Then, transform each sigma R = \sum^{N}_{i=-N}w^{[i]}_{c}(\pmb{\xi}^{[i]} - \hat{\mathbf{y}})(\pmb{\xi}^{[i]} - \hat{\mathbf{y}})^T -In the small body scenario under consideration, there are two transformation functions. The process propagation, assumed as simple +In the small body scenario under consideration, there are two transformation functions. The process propagation, assumed as simple forward Euler integration, as .. math:: @@ -139,7 +139,7 @@ Note that :math:`{}^A\Omega_{A/N}` is the cross-product matrix associated to the Under the previous considerations, the UKF estimation is as follows: -1) Compute the a-priori state estimation :math:`\hat{\mathbf{x}}^{-}_{k+1}` and :math:`P^{-}_{k+1}` +1) Compute the a-priori state estimation :math:`\hat{\mathbf{x}}^{-}_{k+1}` and :math:`P^{-}_{k+1}` through the UT to the propagation function. Add the process noise uncertainty :math:`P_{proc}` .. math:: @@ -178,7 +178,7 @@ the UT to the state to measurements transformation function. Add the measurement :label: eq:kalman_update_covar P_{k+1} = P^{-}_{k+1} - KR^{-}_{k+1}K^T - + These steps are based on `Wan and Van Der Merwe `__ (see algorithm 3.1). The weights selection can be consulted there but it is the one described in the initialization step. The filter hyper-parameters are :math:`\{\alpha, \beta, \kappa\}`. @@ -213,4 +213,4 @@ The user could opt to set the following module variables (initialized by default - ``beta``, filter hyper-parameter (0 by default) - ``kappa``, filter hyper-parameter (:math:`10^{-3}` by default) -The user must connect to each input message described in Table 1. \ No newline at end of file +The user must connect to each input message described in Table 1. diff --git a/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.h b/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.h index d466b4ded7..bbf5c0bbe4 100644 --- a/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.h +++ b/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.h @@ -1,12 +1,12 @@ /* ISC License - + Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, 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 @@ -14,7 +14,7 @@ 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. - + */ diff --git a/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.rst b/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.rst index 6cdec9e589..a0f253fbf7 100644 --- a/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.rst +++ b/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.rst @@ -83,7 +83,7 @@ The required module configuration is:: cmEstimation.P0 = [0.0025, 0.0025, 0.0025] cmEstimation.R0 = [1e-9, 1e-9, 1e-9] unitTestSim.AddModelToTask(unitTaskName, cmEstimation) - + The module is configurable with the following parameters: .. list-table:: Module Parameters @@ -105,4 +105,3 @@ The module is configurable with the following parameters: * - ``R0`` - [0, 0, 0] - diagonal elements of the measurement noise covariance - diff --git a/src/fswAlgorithms/transDetermination/_GeneralModuleFiles/ephemerisUtilities.c b/src/fswAlgorithms/transDetermination/_GeneralModuleFiles/ephemerisUtilities.c index 2a746f3d9b..d06d05b7d3 100644 --- a/src/fswAlgorithms/transDetermination/_GeneralModuleFiles/ephemerisUtilities.c +++ b/src/fswAlgorithms/transDetermination/_GeneralModuleFiles/ephemerisUtilities.c @@ -28,11 +28,11 @@ double calculateChebyValue(double *chebyCoeff, uint32_t nCoeff, double valueMult; double estValue; uint32_t i; - + chebyPrev = 1.0; chebyNow = evalValue; valueMult = 2.0*evalValue; - + estValue = chebyCoeff[0]*chebyPrev; if(nCoeff <= 1) { @@ -46,6 +46,6 @@ double calculateChebyValue(double *chebyCoeff, uint32_t nCoeff, chebyPrev = chebyLocalPrev; estValue += chebyCoeff[i]*chebyNow; } - + return(estValue); } diff --git a/src/fswAlgorithms/transDetermination/chebyPosEphem/chebyPosEphem.h b/src/fswAlgorithms/transDetermination/chebyPosEphem/chebyPosEphem.h index d680a43427..6ae237278d 100755 --- a/src/fswAlgorithms/transDetermination/chebyPosEphem/chebyPosEphem.h +++ b/src/fswAlgorithms/transDetermination/chebyPosEphem/chebyPosEphem.h @@ -41,7 +41,7 @@ typedef struct { double velChebyCoeff[3*MAX_CHEB_COEFF]; /*!< [-] Set of coefficients for the velocity estimate*/ }ChebyEphemRecord; -/*! @brief Top level structure for the Chebyshev position ephemeris +/*! @brief Top level structure for the Chebyshev position ephemeris fit system. e */ typedef struct { @@ -59,13 +59,13 @@ typedef struct { #ifdef __cplusplus extern "C" { #endif - + void SelfInit_chebyPosEphem(ChebyPosEphemData *configData, int64_t moduleID); void Update_chebyPosEphem(ChebyPosEphemData *configData, uint64_t callTime, int64_t moduleID); void Reset_chebyPosEphem(ChebyPosEphemData *configData, uint64_t callTime, int64_t moduleID); - + #ifdef __cplusplus } #endif diff --git a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/AVS.sty b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/AVS.sty +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/Basilisk-dvAccumulation-2019-03-28.tex b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/Basilisk-dvAccumulation-2019-03-28.tex index 47d90b4120..b17163d70d 100755 --- a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/Basilisk-dvAccumulation-2019-03-28.tex +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/Basilisk-dvAccumulation-2019-03-28.tex @@ -17,8 +17,8 @@ % sec_user_guide.tex % % NOTE: if the TeX document is reading in auto-generated TeX snippets from the AutoTeX folder, then -% pytest must first be run for the unit test of this module. This process creates the required unit test results -%. that are read into this document. +% pytest must first be run for the unit test of this module. This process creates the required unit test results +%. that are read into this document. % %-Some rules about referencing within the document: %1. If writing the user guide, assume the module description is present @@ -92,7 +92,7 @@ - + \input{secModuleDescription.tex} %This section includes mathematical models, code description, etc. \input{secModuleFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/BasiliskReportMemo.cls b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100755 --- a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/BasiliskReportMemo.cls +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/bibliography.bib b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/bibliography.bib +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secModuleDescription.tex b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secModuleDescription.tex index f7cfb4d761..ceddc27ec9 100644 --- a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secModuleDescription.tex +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secModuleDescription.tex @@ -9,7 +9,7 @@ \subsection{General Formulation} \subsection{Reset() Functionality} -The reset function has a few critical behaviors. +The reset function has a few critical behaviors. \begin{itemize} \item The vector containing the net $\Delta \bm v$ is zero on reset. \item The prior time tag $t_{n-1}$ is zero to zero on default. @@ -19,10 +19,10 @@ \subsection{Reset() Functionality} \subsection{Update() Functionality} -The update function reads in the latest accelerometer data input message and must process all the new measurements since the last update function call. +The update function reads in the latest accelerometer data input message and must process all the new measurements since the last update function call. \begin{itemize} \item The accelerometer input message is read in and sorted by the time tags. \item If the initialization flag is not true, then the integration is occurring for the first time. To avoid large $\Delta t$ evaluations because of an old prior time $t_{n-1}$, the input data is looped over from the end of the array (i.e. from the newest to oldest) to find the first data time tag $t_{i}$ which is newer then the prior data time tag $t_{n-1}$. Once found we set $t_{n-1} = t_{i}$, set the initialization flag to true and break the loop. As a result the first new data set is not included in the $\Delta\bm v$ evaluation. \item The next step is to loop over all data sets and see if $t_{i}>t_{n-1}$. If yes, the associate data set has not been processed and it is integrated using $$\leftexp{B}{\Delta\bm v} += \leftexp{B}{\bm a_{i}} \Delta t$$ where $\Delta t = t_{i} - t_{n-1}$. The prior time is set to the $t_{i}$ current data time and the loop is repeated. \item The final step before writing the output message is to zero all output message data and then set the {\tt timeTag} to the latest accelerometer measurement time tag, and copy over the $\Delta\bm v$ vector. -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secModuleFunctions.tex b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secModuleFunctions.tex index eec542eae2..24924c4b64 100644 --- a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secModuleFunctions.tex +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secModuleFunctions.tex @@ -8,4 +8,4 @@ \section{Module Functions} \end{itemize} \section{Module Assumptions and Limitations} -The module assumes all accelerometer vector measurements have their components taken with respect to the body frame $\cal B$. \ No newline at end of file +The module assumes all accelerometer vector measurements have their components taken with respect to the body frame $\cal B$. diff --git a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secTest.tex b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secTest.tex index d0689378db..eed46f94a1 100644 --- a/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secTest.tex +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/_Documentation/secTest.tex @@ -1,26 +1,26 @@ % !TEX root = ./Basilisk-dvAccumulation-2019-03-28.tex \section{Test Description and Success Criteria} -The unit test creates an input message with time tagged accelerometer measurements. +The unit test creates an input message with time tagged accelerometer measurements. \section{Test Parameters} -Test and simulation parameters and inputs go here. Basically, describe your test in the section above, but put any specific numbers or inputs to the tests in this section. The test simulation period is 2 seconds with a 0.5 second time step. +Test and simulation parameters and inputs go here. Basically, describe your test in the section above, but put any specific numbers or inputs to the tests in this section. The test simulation period is 2 seconds with a 0.5 second time step. The unit test verifies that the module output navigation message vectors match expected values. \begin{table}[htbp] \caption{Error tolerance for each test.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ \hline - {\tt vehAccumDV} & \input{AutoTeX/toleranceValue} \\ - {\tt timeTag} & \input{AutoTeX/toleranceValue} \\ + {\tt vehAccumDV} & \input{AutoTeX/toleranceValue} \\ + {\tt timeTag} & \input{AutoTeX/toleranceValue} \\ \hline\hline \end{tabular} \end{table} @@ -34,13 +34,11 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline\hline - \textbf{Check} &\textbf{Pass/Fail} \\ + \textbf{Check} &\textbf{Pass/Fail} \\ \hline - 1 & \input{AutoTeX/passFail} \\ + 1 & \input{AutoTeX/passFail} \\ \hline\hline \end{tabular} \end{table} - - diff --git a/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.h b/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.h index d1175a840a..225d0f24a2 100755 --- a/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.h +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.h @@ -31,7 +31,7 @@ typedef struct { NavTransMsg_C dvAcumOutMsg; //!< accumulated DV output message AccDataMsg_C accPktInMsg; //!< [-] input accelerometer message - + uint32_t msgCount; //!< [-] The total number of messages read from inputs uint32_t dvInitialized; //!< [-] Flag indicating whether DV has been started completely uint64_t previousTime; //!< [ns] The clock time associated with the previous run of algorithm @@ -43,7 +43,7 @@ typedef struct { #ifdef __cplusplus extern "C" { #endif - + void SelfInit_dvAccumulation(DVAccumulationData *configData, int64_t moduleID); void Update_dvAccumulation(DVAccumulationData *configData, uint64_t callTime, int64_t moduleID); @@ -52,7 +52,7 @@ extern "C" { void dvAccumulation_swap(AccPktDataMsgPayload *p, AccPktDataMsgPayload *q); int dvAccumulation_partition(AccPktDataMsgPayload *A, int start, int end); void dvAccumulation_QuickSort(AccPktDataMsgPayload *A, int start, int end); - + #ifdef __cplusplus } #endif diff --git a/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.rst b/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.rst index 6a63397332..982a59eb5f 100644 --- a/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.rst +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.rst @@ -31,4 +31,3 @@ provides information on what this message is used for. * - accPktInMsg - :ref:`AccDataMsgPayload` - input accelerometer message - diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/AVS.sty b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/AVS.sty index bb896422fb..e1dbb759e9 100644 --- a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/AVS.sty +++ b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -95,5 +95,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/Basilisk-ephemDifference-2019-03-27.tex b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/Basilisk-ephemDifference-2019-03-27.tex index 81e72d0cd0..12c2bae327 100755 --- a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/Basilisk-ephemDifference-2019-03-27.tex +++ b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/Basilisk-ephemDifference-2019-03-27.tex @@ -17,8 +17,8 @@ % sec_user_guide.tex % % NOTE: if the TeX document is reading in auto-generated TeX snippets from the AutoTeX folder, then -% pytest must first be run for the unit test of this module. This process creates the required unit test results -%. that are read into this document. +% pytest must first be run for the unit test of this module. This process creates the required unit test results +%. that are read into this document. % %-Some rules about referencing within the document: %1. If writing the user guide, assume the module description is present @@ -92,7 +92,7 @@ - + \input{secModuleDescription.tex} %This section includes mathematical models, code description, etc. \input{secModuleFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/BasiliskReportMemo.cls b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100755 --- a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/BasiliskReportMemo.cls +++ b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/bibliography.bib b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/bibliography.bib +++ b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secModuleDescription.tex b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secModuleDescription.tex index 495e123479..6f5f5a4801 100644 --- a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secModuleDescription.tex +++ b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secModuleDescription.tex @@ -15,4 +15,3 @@ \section{Model Description} The time tag of the output message is copied from the corresponding input message, not the base ephemeris message. The number of input messages to consider is determined by searching the {\tt ephInMsg} and {\tt ephOutMsg} names and finding the first zero string where either name was not set. - diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secModuleFunctions.tex b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secModuleFunctions.tex index e7ad27d8d7..2285025383 100644 --- a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secModuleFunctions.tex +++ b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secModuleFunctions.tex @@ -8,6 +8,6 @@ \section{Module Functions} \end{itemize} \section{Module Assumptions and Limitations} -The module assumes all vectors are provided with respect to a common coordinate frame. +The module assumes all vectors are provided with respect to a common coordinate frame. -Only the first $n$ non-empty string names are used to subscribe to the ephemeris input messages. The user must setup the equivalent output messages. \ No newline at end of file +Only the first $n$ non-empty string names are used to subscribe to the ephemeris input messages. The user must setup the equivalent output messages. diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secTest.tex b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secTest.tex index 804a9902da..c5f2f86911 100644 --- a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secTest.tex +++ b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secTest.tex @@ -1,13 +1,13 @@ % !TEX root = ./Basilisk-ephemDifference-2019-03-27.tex \section{Test Description and Success Criteria} -The unit test creates input ephemeris messages for Mars, Jupiter and Saturn relative to the sun. The base ephemeris message is created for Earth relative to the sun. The test evaluates the Mars, Jupiter and Saturn position and velocity vectors relative to the Earth. +The unit test creates input ephemeris messages for Mars, Jupiter and Saturn relative to the sun. The base ephemeris message is created for Earth relative to the sun. The test evaluates the Mars, Jupiter and Saturn position and velocity vectors relative to the Earth. The module requires matching pairs of input and output ephemeris messages. The body counting logic only adds adds messages if both input and output message is specified. This is tested by including two more input messages names where only the output message defined, but not the input message. This should terminate the message counting with a value of 3. The test also checks that the output message has the time tag of the input message, not the base ephemeris message. -The simulation is run for a single time step to ensure the math is performed correctly. +The simulation is run for a single time step to ensure the math is performed correctly. @@ -18,12 +18,12 @@ \section{Test Parameters} \caption{Error tolerance for each test.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ \hline - {\tt r\_BdyZero\_N} & \input{AutoTeX/toleranceValuePos}m \\ - {\tt v\_BdyZero\_N} & \input{AutoTeX/toleranceValueVel}m/s \\ + {\tt r\_BdyZero\_N} & \input{AutoTeX/toleranceValuePos}m \\ + {\tt v\_BdyZero\_N} & \input{AutoTeX/toleranceValueVel}m/s \\ \hline\hline \end{tabular} \end{table} @@ -37,18 +37,14 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline\hline - \textbf{Check} &\textbf{Pass/Fail} \\ + \textbf{Check} &\textbf{Pass/Fail} \\ \hline - Mars & \input{AutoTeX/passFail3} \\ - Jupiter & \input{AutoTeX/passFail3} \\ - Saturn & \input{AutoTeX/passFail3} \\ + Mars & \input{AutoTeX/passFail3} \\ + Jupiter & \input{AutoTeX/passFail3} \\ + Saturn & \input{AutoTeX/passFail3} \\ No Bodies & \input{AutoTeX/passFail0} \\ \hline\hline \end{tabular} \end{table} - - - - diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secUserGuide.tex b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secUserGuide.tex index d6d00b7f06..6b9386b8dc 100644 --- a/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secUserGuide.tex +++ b/src/fswAlgorithms/transDetermination/ephemDifference/_Documentation/secUserGuide.tex @@ -2,22 +2,21 @@ \section{User Guide} -A fixed length array of type {\tt EphemChangeConfig} is setup to contain the input and output message names. The array size is hard coded to {\tt MAX\_NUM\_CHANGE\_BODIES} which is currently set to 10. +A fixed length array of type {\tt EphemChangeConfig} is setup to contain the input and output message names. The array size is hard coded to {\tt MAX\_NUM\_CHANGE\_BODIES} which is currently set to 10. To use this module, the user first creates an instance of the {\tt EphemChangeConfig} container to store unique input and output message names. \begin{verbatim} changeBodyMsg = ephemDifference.EphemChangeConfig() - changeBodyMsg.ephInMsg + changeBodyMsg.ephInMsg changeBodyMsg.ephOutMsg \end{verbatim} -Next, the container is added to a list of these ephemeris information containers using +Next, the container is added to a list of these ephemeris information containers using \begin{verbatim} changeBodyList.append(changeBodyMsg) \end{verbatim} -Note that the output message must be setup in consecutive order. Leaving a blank output message will cause any following message names to be ignored. +Note that the output message must be setup in consecutive order. Leaving a blank output message will cause any following message names to be ignored. Finally, the list of these containers is stored in the {\tt ephemDifference} module using \begin{verbatim} ephemDiffConfig.changeBodies = changeBodyList \end{verbatim} - diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.h b/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.h index a937627b83..88b37a233c 100644 --- a/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.h +++ b/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.h @@ -37,7 +37,7 @@ typedef struct{ typedef struct { EphemerisMsg_C ephBaseInMsg; //!< base ephemeris input message name EphemChangeConfig changeBodies[MAX_NUM_CHANGE_BODIES]; //!< [-] The list of bodies to change out - + uint32_t ephBdyCount; //!< [-] The number of ephemeris bodies we are changing BSKLogger *bskLogger; //!< BSK Logging @@ -46,13 +46,13 @@ typedef struct { #ifdef __cplusplus extern "C" { #endif - + void SelfInit_ephemDifference(EphemDifferenceData *configData, int64_t moduleID); void Update_ephemDifference(EphemDifferenceData *configData, uint64_t callTime, int64_t moduleID); void Reset_ephemDifference(EphemDifferenceData *configData, uint64_t callTime, int64_t moduleID); - + #ifdef __cplusplus } #endif diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.rst b/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.rst index 849eaa87c8..a32f281806 100644 --- a/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.rst +++ b/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.rst @@ -35,4 +35,3 @@ provides information on what this message is used for. * - ephOutMsg - :ref:`EphemerisMsgPayload` - converted ephemeris output message, stored in ``changeBodies[i]`` - diff --git a/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.h b/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.h index 09b526f179..1c42ce95ce 100644 --- a/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.h +++ b/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.h @@ -37,13 +37,13 @@ typedef struct { #ifdef __cplusplus extern "C" { #endif - + void SelfInit_ephemNavConverter(EphemNavConverterData *configData, int64_t moduleID); void Update_ephemNavConverter(EphemNavConverterData *configData, uint64_t callTime, int64_t moduleID); void Reset_ephemNavConverter(EphemNavConverterData *configData, uint64_t callTime, int64_t moduleID); - + #ifdef __cplusplus } #endif diff --git a/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.rst b/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.rst index be5a46e0fa..5229d8d5bd 100644 --- a/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.rst +++ b/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.rst @@ -28,5 +28,3 @@ provides information on what this message is used for. * - stateOutMsg - :ref:`NavTransMsgPayload` - navigation output message - - diff --git a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/AVS.sty b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/AVS.sty +++ b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/Basilisk-navAggregate-2019-02-21.tex b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/Basilisk-navAggregate-2019-02-21.tex index ba8ab1ef94..07827b2507 100755 --- a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/Basilisk-navAggregate-2019-02-21.tex +++ b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/Basilisk-navAggregate-2019-02-21.tex @@ -88,7 +88,7 @@ - + \input{secModuleDescription.tex} %This section includes mathematical models, code description, etc. \input{secModuleFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/BasiliskReportMemo.cls b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100755 --- a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/BasiliskReportMemo.cls +++ b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/bibliography.bib b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/bibliography.bib +++ b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secModuleDescription.tex b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secModuleDescription.tex index e2706e5274..d733a038f1 100644 --- a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secModuleDescription.tex +++ b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secModuleDescription.tex @@ -1,8 +1,8 @@ % !TEX root = ./Basilisk-navAggregate-2019-02-21.tex \section{Module Description} -The purpose of this simple aggregate module is to read in a series of navigation messages, and combine their values into a single output message. The module is able to blend both attitude and translation navigation messages. +The purpose of this simple aggregate module is to read in a series of navigation messages, and combine their values into a single output message. The module is able to blend both attitude and translation navigation messages. -The number of input messages is defined through either {\tt attMsgCount} or {\tt transMsgCount}. If either of these values is zero, then the corresponding output navigation message is populated with zero components. +The number of input messages is defined through either {\tt attMsgCount} or {\tt transMsgCount}. If either of these values is zero, then the corresponding output navigation message is populated with zero components. -To select which input message value to use, the module index value must be set for that particular parameter. All these variables end with {\tt Idx}. Their default values are 0, indicating that by default the values of the first navigation message are used. By changing the {\tt Idx} value the user selects which message content to use for that variable. This can be set individually for each navigation message variable. If the {\tt Idx} value is larger than the number of input messages, then the corresponding variable is set to zero values. This allows the output message to zero out particular variables. In all cases the {\tt Idx} index must be less than input navigation message counter {\tt attMsgCount} or {\tt transMsgCount} respectively. +To select which input message value to use, the module index value must be set for that particular parameter. All these variables end with {\tt Idx}. Their default values are 0, indicating that by default the values of the first navigation message are used. By changing the {\tt Idx} value the user selects which message content to use for that variable. This can be set individually for each navigation message variable. If the {\tt Idx} value is larger than the number of input messages, then the corresponding variable is set to zero values. This allows the output message to zero out particular variables. In all cases the {\tt Idx} index must be less than input navigation message counter {\tt attMsgCount} or {\tt transMsgCount} respectively. diff --git a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secModuleFunctions.tex b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secModuleFunctions.tex index 69f3f721da..76c62b53bb 100644 --- a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secModuleFunctions.tex +++ b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secModuleFunctions.tex @@ -4,8 +4,8 @@ \section{Module Functions} \begin{itemize} \item \textbf{Read in Navigation messages}: This module reads in a list of attitude and navigation messages. The remaining message slots are zeroed. - \item \textbf{Assemble blended output navigation messages}: The input navigation message content can be selectively blended to create a single attitude and translation navigation message. + \item \textbf{Assemble blended output navigation messages}: The input navigation message content can be selectively blended to create a single attitude and translation navigation message. \end{itemize} \section{Module Assumptions and Limitations} -This module doesn't have any assumptions. \ No newline at end of file +This module doesn't have any assumptions. diff --git a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secTest.tex b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secTest.tex index 0b8bc2c049..62242c06e8 100644 --- a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secTest.tex +++ b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secTest.tex @@ -1,7 +1,7 @@ % !TEX root = ./Basilisk-navAggregate-2019-02-21.tex \section{Test Description and Success Criteria} -The unit test sets up a range of module input conditions ranging from using no message, 1 message, 2 messages, using index for an empty message, as well as setting out of range index and message count variables. If a single message count is specified, then no index value is set. Here the default value of 0 is used to read in the first and only message, If 2 messages are available, and the index is set to 2 (i.e. the 3rd message location), then the module read in a zero'd message value. If the message count is larger than the allowable array size of 10, i.e. set the count to 11, then the count is reduced back to 10. If the index value is larger than 9 (largest slot location within a 10 dimensional array), then the index is reduced to 9. All permutations between setting these conditions between the attitude and translation navigation messages are tested. +The unit test sets up a range of module input conditions ranging from using no message, 1 message, 2 messages, using index for an empty message, as well as setting out of range index and message count variables. If a single message count is specified, then no index value is set. Here the default value of 0 is used to read in the first and only message, If 2 messages are available, and the index is set to 2 (i.e. the 3rd message location), then the module read in a zero'd message value. If the message count is larger than the allowable array size of 10, i.e. set the count to 11, then the count is reduced back to 10. If the index value is larger than 9 (largest slot location within a 10 dimensional array), then the index is reduced to 9. All permutations between setting these conditions between the attitude and translation navigation messages are tested. @@ -14,14 +14,14 @@ \section{Test Parameters} \caption{Attitude Navigation Message \#1 and Error tolerance for each test.} \label{tab:attNav1} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c } % Column formatting, + \begin{tabular}{ c | c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Value} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Value} & \textbf{Tolerated Error} \\ \hline - {\tt timeTag} & \input{AutoTeX/navAtt1Msg.timeTag} & \input{AutoTeX/toleranceValue} \\ - {\tt sigma\_BN} & \input{AutoTeX/navAtt1Msg.sigma_BN} & \input{AutoTeX/toleranceValue} \\ - {\tt omega\_BN\_B} & \input{AutoTeX/navAtt1Msg.omega_BN_B} & \input{AutoTeX/toleranceValue} \\ - {\tt vehSunPntBdy} & \input{AutoTeX/navAtt1Msg.vehSunPntBdy} & \input{AutoTeX/toleranceValue} \\ + {\tt timeTag} & \input{AutoTeX/navAtt1Msg.timeTag} & \input{AutoTeX/toleranceValue} \\ + {\tt sigma\_BN} & \input{AutoTeX/navAtt1Msg.sigma_BN} & \input{AutoTeX/toleranceValue} \\ + {\tt omega\_BN\_B} & \input{AutoTeX/navAtt1Msg.omega_BN_B} & \input{AutoTeX/toleranceValue} \\ + {\tt vehSunPntBdy} & \input{AutoTeX/navAtt1Msg.vehSunPntBdy} & \input{AutoTeX/toleranceValue} \\ \hline\hline \end{tabular} \end{table} @@ -30,14 +30,14 @@ \section{Test Parameters} \caption{Attitude Navigation Message \#2 and Error tolerance for each test.} \label{tab:attNav2} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c } % Column formatting, + \begin{tabular}{ c | c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Value} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Value} & \textbf{Tolerated Error} \\ \hline - {\tt timeTag} & \input{AutoTeX/navAtt2Msg.timeTag} & \input{AutoTeX/toleranceValue} \\ - {\tt sigma\_BN} & \input{AutoTeX/navAtt2Msg.sigma_BN} & \input{AutoTeX/toleranceValue} \\ - {\tt omega\_BN\_B} & \input{AutoTeX/navAtt2Msg.omega_BN_B} & \input{AutoTeX/toleranceValue} \\ - {\tt vehSunPntBdy} & \input{AutoTeX/navAtt2Msg.vehSunPntBdy} & \input{AutoTeX/toleranceValue} \\ + {\tt timeTag} & \input{AutoTeX/navAtt2Msg.timeTag} & \input{AutoTeX/toleranceValue} \\ + {\tt sigma\_BN} & \input{AutoTeX/navAtt2Msg.sigma_BN} & \input{AutoTeX/toleranceValue} \\ + {\tt omega\_BN\_B} & \input{AutoTeX/navAtt2Msg.omega_BN_B} & \input{AutoTeX/toleranceValue} \\ + {\tt vehSunPntBdy} & \input{AutoTeX/navAtt2Msg.vehSunPntBdy} & \input{AutoTeX/toleranceValue} \\ \hline\hline \end{tabular} \end{table} @@ -49,14 +49,14 @@ \section{Test Parameters} \caption{Translational Navigation Message \#1 and Error tolerance for each test.} \label{tab:transNav1} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c } % Column formatting, + \begin{tabular}{ c | c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Value} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Value} & \textbf{Tolerated Error} \\ \hline - {\tt timeTag} & \input{AutoTeX/navTrans1Msg.timeTag} & \input{AutoTeX/toleranceValue} \\ - {\tt r\_BN\_N} & \input{AutoTeX/navTrans1Msg.r_BN_N} & \input{AutoTeX/toleranceValue} \\ - {\tt v\_BN\_N} & \input{AutoTeX/navTrans1Msg.v_BN_N} & \input{AutoTeX/toleranceValue} \\ - {\tt vehAccumDV} & \input{AutoTeX/navTrans1Msg.vehAccumDV} & \input{AutoTeX/toleranceValue} \\ + {\tt timeTag} & \input{AutoTeX/navTrans1Msg.timeTag} & \input{AutoTeX/toleranceValue} \\ + {\tt r\_BN\_N} & \input{AutoTeX/navTrans1Msg.r_BN_N} & \input{AutoTeX/toleranceValue} \\ + {\tt v\_BN\_N} & \input{AutoTeX/navTrans1Msg.v_BN_N} & \input{AutoTeX/toleranceValue} \\ + {\tt vehAccumDV} & \input{AutoTeX/navTrans1Msg.vehAccumDV} & \input{AutoTeX/toleranceValue} \\ \hline\hline \end{tabular} \end{table} @@ -65,14 +65,14 @@ \section{Test Parameters} \caption{Translational Navigation Message \#2 and Error tolerance for each test.} \label{tab:transNav2} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c } % Column formatting, + \begin{tabular}{ c | c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Value} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Value} & \textbf{Tolerated Error} \\ \hline - {\tt timeTag} & \input{AutoTeX/navTrans2Msg.timeTag} & \input{AutoTeX/toleranceValue} \\ - {\tt r\_BN\_N} & \input{AutoTeX/navTrans2Msg.r_BN_N} & \input{AutoTeX/toleranceValue} \\ - {\tt v\_BN\_N} & \input{AutoTeX/navTrans2Msg.v_BN_N} & \input{AutoTeX/toleranceValue} \\ - {\tt vehAccumDV} & \input{AutoTeX/navTrans2Msg.vehAccumDV} & \input{AutoTeX/toleranceValue} \\ + {\tt timeTag} & \input{AutoTeX/navTrans2Msg.timeTag} & \input{AutoTeX/toleranceValue} \\ + {\tt r\_BN\_N} & \input{AutoTeX/navTrans2Msg.r_BN_N} & \input{AutoTeX/toleranceValue} \\ + {\tt v\_BN\_N} & \input{AutoTeX/navTrans2Msg.v_BN_N} & \input{AutoTeX/toleranceValue} \\ + {\tt vehAccumDV} & \input{AutoTeX/navTrans2Msg.vehAccumDV} & \input{AutoTeX/toleranceValue} \\ \hline\hline \end{tabular} \end{table} @@ -87,37 +87,36 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | c | c | c } % Column formatting, + \begin{tabular}{c | c | c | c | c } % Column formatting, \hline\hline - & Attitude & & Translation & \\ - {\tt attMsgCount} & Variable {\tt Idx} & {\tt transMsgCount} & Variable {\tt Idx} &\textbf{Pass/Fail} \\ + & Attitude & & Translation & \\ + {\tt attMsgCount} & Variable {\tt Idx} & {\tt transMsgCount} & Variable {\tt Idx} &\textbf{Pass/Fail} \\ \hline - 0 & N/A & 0 & N/A & \input{AutoTeX/passFail00} \\ - 1 & N/A & 1 & N/A & \input{AutoTeX/passFail11} \\ - 0 & N/A & 1 & N/A & \input{AutoTeX/passFail01} \\ - 1 & N/A & 0 & N/A & \input{AutoTeX/passFail10} \\ - 2 & 1 & 2 & 1 & \input{AutoTeX/passFail22} \\ - 1 & N/A & 2 & 1 & \input{AutoTeX/passFail12} \\ - 0 & N/A & 2 & 1 & \input{AutoTeX/passFail02} \\ - 2 & 1 & 1 & N/A & \input{AutoTeX/passFail21} \\ - 2 & 1 & 0 & N/A & \input{AutoTeX/passFail20} \\ - 2 & 2 & 2 & 2 & \input{AutoTeX/passFail33} \\ - 2 & 2 & 2 & 1 & \input{AutoTeX/passFail32} \\ - 2 & 2 & 1 & N/A & \input{AutoTeX/passFail31} \\ - 2 & 2 & 0 & N/A & \input{AutoTeX/passFail30} \\ - 2 & 1 & 2 & 2 & \input{AutoTeX/passFail23} \\ - 1 & N/A & 2 & 2 & \input{AutoTeX/passFail13} \\ - 0 & N/A & 2 & 2 & \input{AutoTeX/passFail03} \\ - 11 & 11 & 11 & 11 & \input{AutoTeX/passFail1111} \\ - 2 & 2 & 11 & 11 & \input{AutoTeX/passFail311} \\ - 2 & 1 & 11 & 11 & \input{AutoTeX/passFail211} \\ - 1 & N/A & 11 & 11 & \input{AutoTeX/passFail111} \\ - 0 & N/A & 11 & 11 & \input{AutoTeX/passFail011} \\ - 11 & 11 & 2 & 2 & \input{AutoTeX/passFail113} \\ - 11 & 11 & 2 & 1 & \input{AutoTeX/passFail112} \\ - 11 & 11 & 1 & N/A & \input{AutoTeX/passFail111} \\ - 11 & 11 & 0 & N/A & \input{AutoTeX/passFail110} \\ + 0 & N/A & 0 & N/A & \input{AutoTeX/passFail00} \\ + 1 & N/A & 1 & N/A & \input{AutoTeX/passFail11} \\ + 0 & N/A & 1 & N/A & \input{AutoTeX/passFail01} \\ + 1 & N/A & 0 & N/A & \input{AutoTeX/passFail10} \\ + 2 & 1 & 2 & 1 & \input{AutoTeX/passFail22} \\ + 1 & N/A & 2 & 1 & \input{AutoTeX/passFail12} \\ + 0 & N/A & 2 & 1 & \input{AutoTeX/passFail02} \\ + 2 & 1 & 1 & N/A & \input{AutoTeX/passFail21} \\ + 2 & 1 & 0 & N/A & \input{AutoTeX/passFail20} \\ + 2 & 2 & 2 & 2 & \input{AutoTeX/passFail33} \\ + 2 & 2 & 2 & 1 & \input{AutoTeX/passFail32} \\ + 2 & 2 & 1 & N/A & \input{AutoTeX/passFail31} \\ + 2 & 2 & 0 & N/A & \input{AutoTeX/passFail30} \\ + 2 & 1 & 2 & 2 & \input{AutoTeX/passFail23} \\ + 1 & N/A & 2 & 2 & \input{AutoTeX/passFail13} \\ + 0 & N/A & 2 & 2 & \input{AutoTeX/passFail03} \\ + 11 & 11 & 11 & 11 & \input{AutoTeX/passFail1111} \\ + 2 & 2 & 11 & 11 & \input{AutoTeX/passFail311} \\ + 2 & 1 & 11 & 11 & \input{AutoTeX/passFail211} \\ + 1 & N/A & 11 & 11 & \input{AutoTeX/passFail111} \\ + 0 & N/A & 11 & 11 & \input{AutoTeX/passFail011} \\ + 11 & 11 & 2 & 2 & \input{AutoTeX/passFail113} \\ + 11 & 11 & 2 & 1 & \input{AutoTeX/passFail112} \\ + 11 & 11 & 1 & N/A & \input{AutoTeX/passFail111} \\ + 11 & 11 & 0 & N/A & \input{AutoTeX/passFail110} \\ \hline\hline \end{tabular} \end{table} - diff --git a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secUserGuide.tex b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secUserGuide.tex index da0a1d5113..17cce82988 100644 --- a/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secUserGuide.tex +++ b/src/fswAlgorithms/transDetermination/navAggregate/_Documentation/secUserGuide.tex @@ -3,13 +3,13 @@ \section{User Guide} \subsection{Required Module Parameters} \begin{itemize} - \item {\tt navAttOutMsg} --- Name of the blended attitude output navigation message - \item {\tt navTransOutMsg} --- Name of the blended translation output navigation message + \item {\tt navAttOutMsg} --- Name of the blended attitude output navigation message + \item {\tt navTransOutMsg} --- Name of the blended translation output navigation message \end{itemize} \subsection{Optional Module Parameters} -The following parameters are used to specify 1 or more navigation input messages. The array of messages must be of size 10 or less. The message count should be set to the number of input messages. If this is 0, then no input messages are read in and a zero output navigation message is produced. In the message count is larger than 10, then the variable is restricted to 10. +The following parameters are used to specify 1 or more navigation input messages. The array of messages must be of size 10 or less. The message count should be set to the number of input messages. If this is 0, then no input messages are read in and a zero output navigation message is produced. In the message count is larger than 10, then the variable is restricted to 10. \begin{itemize} \item {\tt attMsgs} --- Array of {\tt AggregateAttInput} navigation message structures which have the input message name specified in {\tt navAttInMsg} \item {\tt transMsgs} --- Array of {\tt AggregateTransInput} navigation message structures which have the input message name specified in {\tt navTransInMsg} @@ -18,7 +18,7 @@ \subsection{Optional Module Parameters} \end{itemize} -The following are the index values that can be set. These parameters indicate which input message should be used to create a particular content of the output message. The default value is always zero, i.e. the first message. If the index is larger than the message count variable, then a zero'd message is read. This is convenient to force a component of the output message to be zero. If the index value is larger than 9 (last slot in a 10 dimensional array), then the value is restricted to 9. +The following are the index values that can be set. These parameters indicate which input message should be used to create a particular content of the output message. The default value is always zero, i.e. the first message. If the index is larger than the message count variable, then a zero'd message is read. This is convenient to force a component of the output message to be zero. If the index value is larger than 9 (last slot in a 10 dimensional array), then the value is restricted to 9. \begin{itemize} \item {\tt attTimeIdx} --- the message index to use for this value. The first message has an index of zero. Default value is zero. \item {\tt attIdx} --- the message index to use for this value. The first message has an index of zero. Default value is zero. diff --git a/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.h b/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.h index f90b3e801b..80a2171ab0 100755 --- a/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.h +++ b/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.h @@ -46,7 +46,7 @@ typedef struct { AggregateTransInput transMsgs[MAX_AGG_NAV_MSG]; /*!< [-] The incoming nav message buffer */ NavAttMsg_C navAttOutMsg; /*!< blended attitude navigation output message */ NavTransMsg_C navTransOutMsg; /*!< blended translation navigation output message */ - + uint32_t attTimeIdx; /*!< [-] The index of the message to use for attitude message time */ uint32_t transTimeIdx; /*!< [-] The index of the message to use for translation message time */ uint32_t attIdx; /*!< [-] The index of the message to use for inertial MRP*/ @@ -64,7 +64,7 @@ typedef struct { #ifdef __cplusplus extern "C" { #endif - + void SelfInit_aggregateNav(NavAggregateData *configData, int64_t moduleID); void Update_aggregateNav(NavAggregateData *configData, uint64_t callTime, int64_t moduleID); void Reset_aggregateNav(NavAggregateData *configData, uint64_t callTime, int64_t moduleID); diff --git a/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.rst b/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.rst index 23981d2642..d16ccbde04 100644 --- a/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.rst +++ b/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.rst @@ -36,4 +36,3 @@ provides information on what this message is used for. * - navTransInMsg - :ref:`NavTransMsgPayload` - translation navigation input message stored inside the ``AggregateTransInput`` structure - diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/AVS.sty b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/AVS.sty +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/Basilisk-oeStateEphem-20190426.tex b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/Basilisk-oeStateEphem-20190426.tex index 1c9a5adf01..4a0148e5d6 100755 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/Basilisk-oeStateEphem-20190426.tex +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/Basilisk-oeStateEphem-20190426.tex @@ -17,8 +17,8 @@ % sec_user_guide.tex % % NOTE: if the TeX document is reading in auto-generated TeX snippets from the AutoTeX folder, then -% pytest must first be run for the unit test of this module. This process creates the required unit test results -%. that are read into this document. +% pytest must first be run for the unit test of this module. This process creates the required unit test results +%. that are read into this document. % %-Some rules about referencing within the document: %1. If writing the user guide, assume the module description is present @@ -93,7 +93,7 @@ - + \input{secModuleDescription.tex} %This section includes mathematical models, code description, etc. diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/BasiliskReportMemo.cls b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100755 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/BasiliskReportMemo.cls +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/bibliography.bib b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/bibliography.bib +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secModuleDescription.tex b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secModuleDescription.tex index 0d1a6371af..d68dd94426 100644 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secModuleDescription.tex +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secModuleDescription.tex @@ -5,15 +5,15 @@ \section{Model Description} The purpose of this module is to model a trajectory in space using a fitted set of Chebyshev coefficients. As illustrated in Figure~\ref{fig:moduleImg}, there is a single input message which provides the ephemeris time associated with the vehicle on the trajectory, as well as vehicle time code information. This is used to compute a corrected mission time used to compute the location on a given trajectory. -The output of the module is an ephemeris interface message which contains the inertial position vector $\leftexp{N}{\bm r}$ and inertial velocity $\leftexp{N}{\dot{\bm r}}$. To model the trajectory a Chebyshev polynomial approximation is used of the orbit elements. The elements modeled include the radius at periapses $r_{p}$, the eccentricity $e$, the inclination angle $i$, the ascending node angle $\Omega$, the argument of periapses angle $\omega$ and an anomaly angle. The elements were chosen as they are defined finite quantities for ellipses, parabolas and hyperbolas. The anomaly can can be the true anomaly directly (default), or the mean elliptical or mean hyperbolic angle depending on the value of the eccentricity $e$. The input type is controlled within the Chebyshev record structure through the variable {\tt anomalyFlag}. In all cases the fitted anomaly angle is mapped to a current true anomaly angle $f$ that is used to determine the current position and velocity vectors. +The output of the module is an ephemeris interface message which contains the inertial position vector $\leftexp{N}{\bm r}$ and inertial velocity $\leftexp{N}{\dot{\bm r}}$. To model the trajectory a Chebyshev polynomial approximation is used of the orbit elements. The elements modeled include the radius at periapses $r_{p}$, the eccentricity $e$, the inclination angle $i$, the ascending node angle $\Omega$, the argument of periapses angle $\omega$ and an anomaly angle. The elements were chosen as they are defined finite quantities for ellipses, parabolas and hyperbolas. The anomaly can can be the true anomaly directly (default), or the mean elliptical or mean hyperbolic angle depending on the value of the eccentricity $e$. The input type is controlled within the Chebyshev record structure through the variable {\tt anomalyFlag}. In all cases the fitted anomaly angle is mapped to a current true anomaly angle $f$ that is used to determine the current position and velocity vectors. -The current time is used to first determine which Chebyshev coefficient record to use. Each polynomial fit is good for a discrete period of time determined through the interval mid-point $t_{m,i}$ and radius $t_{r,i}$. The algorithm determines which record interval mid-point $t_{m,i}$ is closest to the current simulation time $t_{i}$. +The current time is used to first determine which Chebyshev coefficient record to use. Each polynomial fit is good for a discrete period of time determined through the interval mid-point $t_{m,i}$ and radius $t_{r,i}$. The algorithm determines which record interval mid-point $t_{m,i}$ is closest to the current simulation time $t_{i}$. -The next step computes a scaled time $t^{\ast}$ where the time relative to $t_{m,i}$ is normalized that that it yields a value between [-1,1] where +1 would be the upper bounder at $t_{i} = t_{m,i} + t_{r,i}$. If there is a cap in the time intervals the module is set to fail high or low relative to the nearest time interval. +The next step computes a scaled time $t^{\ast}$ where the time relative to $t_{m,i}$ is normalized that that it yields a value between [-1,1] where +1 would be the upper bounder at $t_{i} = t_{m,i} + t_{r,i}$. If there is a cap in the time intervals the module is set to fail high or low relative to the nearest time interval. After selecting the proper set of Chebyshev coefficients for the current time interval the corresponding orbit elements are retrieved. The {\tt elem2rv()} function of the {\tt orbitalMotion.c/h} support library is used to map these into the desired inertial position and velocity vectors. This requires mapping the radius of perapses into a semi-major axis $a$ measure using \begin{equation} a = \frac{r_{p}}{1-e} \end{equation} -If $e \approx 1$ then {\tt elem2rv()} assumes $a = 0$. +If $e \approx 1$ then {\tt elem2rv()} assumes $a = 0$. diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secModuleFunctions.tex b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secModuleFunctions.tex index ae08f22e87..7d31abf1f4 100644 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secModuleFunctions.tex +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secModuleFunctions.tex @@ -8,4 +8,4 @@ \section{Module Functions} \end{itemize} \section{Module Assumptions and Limitations} -The model assumes that all orbit elements are modeled using the same order Chebyshev polynomial. \ No newline at end of file +The model assumes that all orbit elements are modeled using the same order Chebyshev polynomial. diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secTest.tex b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secTest.tex index 5b7a66200e..a0534f9dd9 100644 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secTest.tex +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secTest.tex @@ -14,18 +14,18 @@ \subsection{Check 2: TDRSS Data Fit } \section{Test Parameters} -The Spice TDRSS trajectory data is loaded as the truth trajectory. A 14th order Chebyshev model is then created and uploaded to the BSK module. +The Spice TDRSS trajectory data is loaded as the truth trajectory. A 14th order Chebyshev model is then created and uploaded to the BSK module. The unit tests verify that the module output guidance message vectors match expected values. \begin{table}[htbp] \caption{Error tolerance for each test.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ \hline - {\tt r\_BdyZero\_N} & \input{AutoTeX/tolerancePosValue} m \\ - {\tt v\_BdyZero\_N} & \input{AutoTeX/toleranceVelValue} m/s \\ + {\tt r\_BdyZero\_N} & \input{AutoTeX/tolerancePosValue} m \\ + {\tt v\_BdyZero\_N} & \input{AutoTeX/toleranceVelValue} m/s \\ \hline\hline \end{tabular} \end{table} @@ -39,17 +39,12 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline\hline - \textbf{Check} &\textbf{Pass/Fail} \\ + \textbf{Check} &\textbf{Pass/Fail} \\ \hline - 1 & \input{AutoTeX/passFailFalse} \\ - 2 & \input{AutoTeX/passFailTrue} \\ + 1 & \input{AutoTeX/passFailFalse} \\ + 2 & \input{AutoTeX/passFailTrue} \\ \hline\hline \end{tabular} \end{table} - - - - - diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secUserGuide.tex b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secUserGuide.tex index 6ce743b899..8f875475c9 100644 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secUserGuide.tex +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/_Documentation/secUserGuide.tex @@ -5,6 +5,6 @@ \section{User Guide} \begin{itemize} \item {\tt muCentral}: set the central body gravity constant \item {\tt ephArray}: Set the array of orbit element Chebyshef coefficients - \item {\tt stateFitOutMsg}: module output message - \item {\tt clockCorrInMsg}: module input message -\end{itemize} \ No newline at end of file + \item {\tt stateFitOutMsg}: module output message + \item {\tt clockCorrInMsg}: module input message +\end{itemize} diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.h b/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.h index 2481c526f5..6997cc03b8 100755 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.h +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.h @@ -65,13 +65,13 @@ typedef struct { #ifdef __cplusplus extern "C" { #endif - + void SelfInit_oeStateEphem(OEStateEphemData *configData, int64_t moduleID); void Update_oeStateEphem(OEStateEphemData *configData, uint64_t callTime, int64_t moduleID); void Reset_oeStateEphem(OEStateEphemData *configData, uint64_t callTime, int64_t moduleID); - + #ifdef __cplusplus } #endif diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.rst b/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.rst index f8e98041b4..077c0f1d85 100644 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.rst +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.rst @@ -31,4 +31,3 @@ provides information on what this message is used for. * - clockCorrInMsg - :ref:`TDBVehicleClockCorrelationMsgPayload` - clock correlation input message - diff --git a/src/moduleTemplates/_doc.rst b/src/moduleTemplates/_doc.rst index 5c432473f8..eb399f98f2 100644 --- a/src/moduleTemplates/_doc.rst +++ b/src/moduleTemplates/_doc.rst @@ -74,5 +74,3 @@ already checks if the input messages are connected (assuming they are all requir provides sample code the creates buffer variables for the input and output messages, reads in the input messages, and write to the output messages. The unit test already loads up the module, creates blank input message copies that are subscribed to the module, creates recorder modules for each output message, etc. - - diff --git a/src/moduleTemplates/cModuleTemplate/_Documentation/AVS.sty b/src/moduleTemplates/cModuleTemplate/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/moduleTemplates/cModuleTemplate/_Documentation/AVS.sty +++ b/src/moduleTemplates/cModuleTemplate/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/moduleTemplates/cModuleTemplate/_Documentation/Basilisk-MODULENAME.tex b/src/moduleTemplates/cModuleTemplate/_Documentation/Basilisk-MODULENAME.tex index 7789cf43cb..4794f24c44 100755 --- a/src/moduleTemplates/cModuleTemplate/_Documentation/Basilisk-MODULENAME.tex +++ b/src/moduleTemplates/cModuleTemplate/_Documentation/Basilisk-MODULENAME.tex @@ -63,7 +63,7 @@ \section{Alternate Citation Options} \begin{verbatim} [link name](http://example.link) \end{verbatim} - We are trying to keep the Basilisk repository as small as possible, thus using a PDF technical note should be used sparingly. + We are trying to keep the Basilisk repository as small as possible, thus using a PDF technical note should be used sparingly. diff --git a/src/moduleTemplates/cModuleTemplate/_Documentation/BasiliskReportMemo.cls b/src/moduleTemplates/cModuleTemplate/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/moduleTemplates/cModuleTemplate/_Documentation/BasiliskReportMemo.cls +++ b/src/moduleTemplates/cModuleTemplate/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/moduleTemplates/cModuleTemplate/_Documentation/Images/fig1.svg b/src/moduleTemplates/cModuleTemplate/_Documentation/Images/fig1.svg index 65346ad76b..4b8152f4f6 100644 --- a/src/moduleTemplates/cModuleTemplate/_Documentation/Images/fig1.svg +++ b/src/moduleTemplates/cModuleTemplate/_Documentation/Images/fig1.svg @@ -13,7 +13,7 @@ - Produced by OmniGraffle 7.11.4 + Produced by OmniGraffle 7.11.4 2019-11-03 15:44:24 +0000 diff --git a/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplate.py b/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplate.py index 1d97a53623..d47649d06f 100644 --- a/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplate.py +++ b/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplate.py @@ -59,7 +59,7 @@ def test_module(show_plots): # update "module" in this function name to refl - ``dataVector[3]`` **General Documentation Comments** - + If the script generates figures, these figures will be automatically pulled from ``matplotlib`` and included below. Make sure that the figures have appropriate axes labels and a figure title if needed. The figures content should be understood by just looking at the figure. diff --git a/src/moduleTemplates/cppModuleTemplate/cppModuleTemplate.rst b/src/moduleTemplates/cppModuleTemplate/cppModuleTemplate.rst index c857d56ebf..024e9c45b4 100644 --- a/src/moduleTemplates/cppModuleTemplate/cppModuleTemplate.rst +++ b/src/moduleTemplates/cppModuleTemplate/cppModuleTemplate.rst @@ -3,7 +3,7 @@ Executive Summary This is a very basic dummy C++ Basilisk module that can be used as a template to create other C++ modules. It mimics the functionality of :ref:`cModuleTemplate`. See that module for a more complete discussion -of how to write the RST module documentation file. +of how to write the RST module documentation file. Message Connection Descriptions diff --git a/src/reportconf.py b/src/reportconf.py index 48591686f3..769053bd15 100644 --- a/src/reportconf.py +++ b/src/reportconf.py @@ -18,7 +18,7 @@ """ This reportconf.py file is executed by src/conftest.py if and only if -the user has pytest-html installed. It is used to customize the contents of +the user has pytest-html installed. It is used to customize the contents of the pytest-html report and control where it is written. """ @@ -106,5 +106,3 @@ def pytest_html_results_table_header(cells): def pytest_html_results_table_row(report, cells): # remove the "links column from the report cells.pop() - - diff --git a/src/simulation/deviceInterface/encoder/encoder.rst b/src/simulation/deviceInterface/encoder/encoder.rst index 842a40c5ee..8bc0237e48 100644 --- a/src/simulation/deviceInterface/encoder/encoder.rst +++ b/src/simulation/deviceInterface/encoder/encoder.rst @@ -35,7 +35,7 @@ for the first iteration the time step is set at 0, the wheel speeds will not be Discretization ~~~~~~~~~~~~~~ -Under normal operating conditions, the discretizer works by setting the wheel speed as a multiple of the resolution of the sensor. This resolution is calculated through the number of +Under normal operating conditions, the discretizer works by setting the wheel speed as a multiple of the resolution of the sensor. This resolution is calculated through the number of clicks per rotation that the sensor can handle. Let :math:`N` be the number of clicks measured during a time step: .. math:: diff --git a/src/simulation/deviceInterface/hingedBodyLinearProfiler/_UnitTest/test_hingedBodyLinearProfiler.py b/src/simulation/deviceInterface/hingedBodyLinearProfiler/_UnitTest/test_hingedBodyLinearProfiler.py index 533957cd15..cd748f119e 100644 --- a/src/simulation/deviceInterface/hingedBodyLinearProfiler/_UnitTest/test_hingedBodyLinearProfiler.py +++ b/src/simulation/deviceInterface/hingedBodyLinearProfiler/_UnitTest/test_hingedBodyLinearProfiler.py @@ -1,12 +1,12 @@ -# +# # ISC License -# +# # Copyright (c) 2022, 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 @@ -14,8 +14,8 @@ # 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. -# -# +# +# from math import pi @@ -48,9 +48,9 @@ def test_hingedBodyLinearProfiler(show_plots, startTime, endTime, startTheta, en endTheta (double): ending angle of deployment in radians **Description of Variables Being Tested** - + For a deployment from 0 to 1 degree, starting at 1 second and ending at 2 seconds into the simulation, checks that the angle and angle rates are as expected before, during, and after deployment. - + Before deployment, theta should be 0 and ``thetaDot`` 0. During deployment, ``thetaDot`` should be 1 degree per second, with theta varying linearly. After deployment, theta should be 1 degree and ``thetaDot`` 0. """ @@ -64,7 +64,7 @@ def hingedBodyLinearProfilerTestFunction(show_plots, startTime, endTime, startTh testMessages = [] unitTaskName = "unitTask" unitProcessName = "TestProcess" - + # accuracy to which double values compared accuracy = 1e-12 @@ -93,7 +93,7 @@ def hingedBodyLinearProfilerTestFunction(show_plots, startTime, endTime, startTh # pull module data and make sure it is correct trueTheta = np.array([0, 0, 0, pi/360, pi/180, pi/180, pi/180]); trueThetaDot = np.array([0, 0, pi/180, pi/180, pi/180, 0, 0]) - + testFailCount, testMessages = unitTestSupport.compareVector(trueTheta, dataLog.theta, accuracy, "theta", testFailCount, testMessages) testFailCount, testMessages = unitTestSupport.compareVector(trueThetaDot, dataLog.thetaDot, accuracy, "thetaDot", testFailCount, testMessages) @@ -107,5 +107,3 @@ def hingedBodyLinearProfilerTestFunction(show_plots, startTime, endTime, startTh if __name__ == "__main__": test_hingedBodyLinearProfiler(False, macros.sec2nano(1), macros.sec2nano(2), 0, pi/180) - - diff --git a/src/simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.rst b/src/simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.rst index 21db3b4056..8a45c8b811 100644 --- a/src/simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.rst +++ b/src/simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.rst @@ -53,4 +53,3 @@ A sample setup is done using: testModule.endTime = macros.sec2nano(330) # [ns] continue deploying for 300 seconds testModule.startTheta = 0 # [rad] starting angle in radians testModule.endTheta = 10*pi/180 # [rad] ending angle is 10 degrees in the positive direction as defined by hinged rigid body frame - diff --git a/src/simulation/deviceInterface/motorVoltageInterface/motorVoltageInterface.h b/src/simulation/deviceInterface/motorVoltageInterface/motorVoltageInterface.h index 84974f0113..7af8d8f847 100644 --- a/src/simulation/deviceInterface/motorVoltageInterface/motorVoltageInterface.h +++ b/src/simulation/deviceInterface/motorVoltageInterface/motorVoltageInterface.h @@ -36,7 +36,7 @@ class MotorVoltageInterface: public SysModel { public: MotorVoltageInterface(); ~MotorVoltageInterface(); - + void computeMotorTorque(); void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); @@ -45,7 +45,7 @@ class MotorVoltageInterface: public SysModel { void setGains(Eigen::VectorXd gains); //!< -- Takes in an array of gains to set for rws and sets them, leaving blanks up to MAX_EFF_COUNT void setScaleFactors(Eigen::VectorXd scaleFactors); //!< -- Takes in an array of scale factors to set for rws and sets them, leaving blanks up to MAX_EFF_COUNT void setBiases(Eigen::VectorXd biases); //!< -- Takes in an array of biases to set for rws and sets them, leaving blanks up to MAX_EFF_COUNT - + public: ReadFunctor motorVoltageInMsg; //!< -- Message that contains motor voltage input states Message motorTorqueOutMsg;//!< -- Output Message for motor torques diff --git a/src/simulation/deviceInterface/tempMeasurement/tempMeasurement.rst b/src/simulation/deviceInterface/tempMeasurement/tempMeasurement.rst index 39adaa8621..dd2627fc5a 100644 --- a/src/simulation/deviceInterface/tempMeasurement/tempMeasurement.rst +++ b/src/simulation/deviceInterface/tempMeasurement/tempMeasurement.rst @@ -4,8 +4,8 @@ Models the addition of noise, bias, and faults to temperature measurements. Message Connection Descriptions ------------------------------- -The following table lists all module input and output messages. The module msg connection -is set by the user from python. The msg type contains a link to the message structure definition, +The following table lists all module input and output messages. The module msg connection +is set by the user from python. The msg type contains a link to the message structure definition, while the description provides information on what this message is used for. .. list-table:: Module I/O Messages @@ -30,7 +30,7 @@ bias, and three faults: - ``TEMP_FAULT_STUCK_VALUE`` is faulty behavior where the measurement sticks to a specific value - ``TEMP_FAULT_STUCK_CURRENT`` fixes the measurement to the value -- ``TEMP_FAULT_SPIKING`` is faulty behavior where the measurement spikes +- ``TEMP_FAULT_SPIKING`` is faulty behavior where the measurement spikes to a specified multiplier times the actual value, with a given probability - ``TEMP_FAULT_NOMINAL`` has no faulty behavior but may still have noise and bias @@ -90,7 +90,7 @@ A sample setup is done using: tempMeasurementModel.senBias = 1.0 # [C] bias amount tempMeasurementModel.senNoiseStd = 5.0 # [C] noise standard devation - tempMeasurementModel.walkBounds = 2.0 # + tempMeasurementModel.walkBounds = 2.0 # tempMeasurementModel.stuckValue = 10.0 # [C] if the sensor gets stuck, stuck at 10 degrees C tempMeasurementModel.spikeProbability = 0.3 # [-] 30% chance of spiking at each time step tempMeasurementModel.spikeAmount = 10.0 # [-] 10x the actual sensed value if the spike happens diff --git a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/AVS.sty b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100755 --- a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/AVS.sty +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/Basilisk-BOREANGLECALC-20170722.tex b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/Basilisk-BOREANGLECALC-20170722.tex index 89b4e60ffe..7d58eb71d2 100755 --- a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/Basilisk-BOREANGLECALC-20170722.tex +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/Basilisk-BOREANGLECALC-20170722.tex @@ -51,9 +51,9 @@ \newcommand{\summary}{This unit test calculates the miss angle and the azimuth angle of a given boresight and celestial body. The unit test goes through all of the calculations made in the module in order to confirm the correctness of the functions used, as well as separate calculations in the inertial reference frame for comparison.} \begin{document} - + \makeCover - + % % enter the revision documentation here % to add more lines, copy the table entry and the \hline, and paste after the current entry. @@ -73,35 +73,35 @@ \hline 2.0 & Add Inertial Heading Option & J. Vaz Carneiro & 20230112\\ \hline - + \end{longtable} } - - - + + + \newpage \setcounter{page}{1} \pagestyle{fancy} - + \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - - - - - - - - - - - + + + + + + + + + + + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. - + \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations - + \input{secTest.tex} % This includes test description, test parameters, and test results - + \input{secUserGuide.tex} % Contains a discussion of how to setup and configure the BSK module \end{document} diff --git a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/bibliography.bib b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/bibliography.bib index dcac27fa44..9212109fb3 100755 --- a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/bibliography.bib @@ -1,25 +1,25 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -27,7 +27,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secModelDescription.tex b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secModelDescription.tex index f5a04564df..1f319fa6c9 100755 --- a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secModelDescription.tex @@ -6,13 +6,13 @@ \section{Model Description} \centerline{ \includegraphics[height = 3in]{Figures/missAngleImg.JPG} } - \caption{All of the vectors and the miss angle are labeled here. $\bm O$ is the vector that represents the boresight of the instrument that the test is interested in. $\bm{L}$ is the direction that the spacecraft should be pointing its boresight in. $\bm M$ is the miss angle of the boresight to its desired direction. All of the $\bm r$ vectors are various position vectors designated by their subscripts.} + \caption{All of the vectors and the miss angle are labeled here. $\bm O$ is the vector that represents the boresight of the instrument that the test is interested in. $\bm{L}$ is the direction that the spacecraft should be pointing its boresight in. $\bm M$ is the miss angle of the boresight to its desired direction. All of the $\bm r$ vectors are various position vectors designated by their subscripts.} \label{fig:Fig1} \end{figure} The model begins by creating a direction cosine martix to represent the pointing frame from the inertial frame. Then the boresight vector is projected into this frame, and the miss angle and the azimuth angle are calculated using standard trigonometry. These calculations are performed in the following way: -First, the unit relative position vector of the spacecraft is calculated relative to the celestial body that the spacecraft is supposed to be pointing at. Using Equation \ref{eq:rePos}. Where $\hat{\bm r}_{B/S}$ is the unit relative position of the spacecraft to the planet, $\bm r_{N/S}$ is the position of the celestial body in the inertial frame, and $\bm r_{N/B}$ is the position of the spacecraft in the inertial frame. +First, the unit relative position vector of the spacecraft is calculated relative to the celestial body that the spacecraft is supposed to be pointing at. Using Equation \ref{eq:rePos}. Where $\hat{\bm r}_{B/S}$ is the unit relative position of the spacecraft to the planet, $\bm r_{N/S}$ is the position of the celestial body in the inertial frame, and $\bm r_{N/B}$ is the position of the spacecraft in the inertial frame. \begin{equation} \label{eq:rePos} @@ -60,7 +60,7 @@ \section{Model Description} \caption{The azimuth angle, A is shown here with respect to the pointing coordinate frame and the vectors used to construct the azimuth angle.} \label{fig:Fig2} \end{figure} - + The azimuth angle is calculated by passing the product of the last two components of the borsight vector. This is shown in Equation \ref{eq:azAng} and in Figure \ref{fig:Fig2}. \begin{equation} @@ -68,4 +68,4 @@ \section{Model Description} A = \arctan\bigg(\frac{\bm O_{\hat k}}{\bm O_{\hat j}}\bigg) \end{equation} -For the case where the inertial heading is used, the azimuth angle is undefined and set to zero. \ No newline at end of file +For the case where the inertial heading is used, the azimuth angle is undefined and set to zero. diff --git a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secModelFunctions.tex index 380a574a9a..ea68ee8518 100755 --- a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secModelFunctions.tex @@ -8,4 +8,4 @@ \section{Model Assumptions and Limitations} When the miss angle goes to 0$^{\circ}$ the azimuth angle is ill defined. The model will default to setting the azimuth angle to 0$^{\circ}$. This also occurs when an inertial heading is used instead of the celestial body. -The module defaults to using the celestial body to compute the miss and azimuth angles. This means that when a celestial body message is provided along with an inertial heading, it will output the results using the celestial body data. \ No newline at end of file +The module defaults to using the celestial body to compute the miss and azimuth angles. This means that when a celestial body message is provided along with an inertial heading, it will output the results using the celestial body data. diff --git a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secTest.tex b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secTest.tex index 9023d9e846..81a8b02987 100755 --- a/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secTest.tex +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/_Documentation/secTest.tex @@ -1,26 +1,26 @@ \section{Test Description and Success Criteria} In order to test the model, the boresight vector on the spacecraft, and the orientation of the spacecraft were varied to be in each quadrant of the body frame. There were a total of 16 different tests performed in this manner. Eight different boresight vector placements (holding the spacecraft orientation constant), and eight different spacecraft orientations (holding the boresight vector constant). There was a 17th test performed in order to check the azimuth angle when the miss angle is zero. For every combination of parameters, the pointing reference frame was calculated and compared to the simulated pointing frame, and using that, the miss angle and azimuth angle were calculated. -The boresight vector was calculated in the same manner that it was calculated in the model using numpy libraries to do so. It was then compared to the model's boresight vector. The test was deemed a success if the boresight vector was withing 1E-10 of the model's boresight vector using the unitTestSupport script. It should be noted that the boresight vector that the user passes to the model doesn't need to be a unit vector because the model will normalize the vector. +The boresight vector was calculated in the same manner that it was calculated in the model using numpy libraries to do so. It was then compared to the model's boresight vector. The test was deemed a success if the boresight vector was withing 1E-10 of the model's boresight vector using the unitTestSupport script. It should be noted that the boresight vector that the user passes to the model doesn't need to be a unit vector because the model will normalize the vector. -The miss angle was calculated in two separate ways. The first method mirrored the module's method. Just as in the module, a direction cosine martix was created to represent the pointing frame from the inertial frame. Then the boresight vector was projected into this frame, and the miss angle was calculated using standard trigonometry. The key difference in the first method of validation is that the validation used the python numpy library primarily rather than the RigidBodyKinematics.py file. +The miss angle was calculated in two separate ways. The first method mirrored the module's method. Just as in the module, a direction cosine martix was created to represent the pointing frame from the inertial frame. Then the boresight vector was projected into this frame, and the miss angle was calculated using standard trigonometry. The key difference in the first method of validation is that the validation used the python numpy library primarily rather than the RigidBodyKinematics.py file. The second method used the existing inertial reference frame in order to calculate the miss angle of the boresight. In this method, the baseline vector was projected into the inertial frame. Then just as the first, the miss angle was calculated using standard trigonometry. -The second method relied on the direction cosine matrices created in the first method. That being said, the first operation in the second method was to calculate the position vector of the spacecraft in the inertial reference frame. Then the baseline vector was projected into the inertial frame using the direction cosine matrix from the inertial frame to the pointing frame. Then the position vector in the inertial frame was dotted with the baseline projection. Just as in the first method, the product of the two vectors was passed through the arccosine function in order to calculate the miss angle of the boresight. +The second method relied on the direction cosine matrices created in the first method. That being said, the first operation in the second method was to calculate the position vector of the spacecraft in the inertial reference frame. Then the baseline vector was projected into the inertial frame using the direction cosine matrix from the inertial frame to the pointing frame. Then the position vector in the inertial frame was dotted with the baseline projection. Just as in the first method, the product of the two vectors was passed through the arccosine function in order to calculate the miss angle of the boresight. Once the miss angle calculations were complete, the calculated miss angles were compared to the miss angle pulled from the mission simulation. If the calculated miss angles were withing 1E-10 of the simulation miss angle, the test passed. -Then the azimuth angle was calculated using the same method as in the model. Again, it should be noted that the standard python numpy library was used in the validation calculation. The test passed when the calculated azimuth angle was within 1E-10 of the simulation azimuth angle. +Then the azimuth angle was calculated using the same method as in the model. Again, it should be noted that the standard python numpy library was used in the validation calculation. The test passed when the calculated azimuth angle was within 1E-10 of the simulation azimuth angle. \begin{table}[htbp] \caption{Tolerance Table (Absolute)} \label{tab:label} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, - \hline + \begin{tabular}{ c | c } % Column formatting, + \hline Parameter & Tolerance\\ - \hline + \hline Boresight Vector & 1E-10 \\ Miss Angle & 1E-10 \\ Azimuth Angle & 1E-10 \\ @@ -38,10 +38,10 @@ \section{Test Parameters} \caption{Table of test parameters} \label{tab:label} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c | c | c } % Column formatting, - \hline + \begin{tabular}{ c | c | c | c | c | c } % Column formatting, + \hline Test & Boresight Vector & $\sigma_{B/N}$ & Test & Boresight Vector & $\sigma_{B/N}$ \\ - \hline + \hline 1 & $\frac{\sqrt[]{3}}{3}$, $\frac{\sqrt[]{3}}{3}$, $\frac{\sqrt[]{3}}{3}$ & 0.0, 0.0, 0.0 & 9 & 0.0, 0.0, 1.0 & -0.079, 0.191, 0.191 \\ 2 & -$\frac{\sqrt[]{3}}{3}$, $\frac{\sqrt[]{3}}{3}$, $\frac{\sqrt[]{3}}{3}$ & 0.0, 0.0, 0.0 & 10 & 0.0, 0.0, 1.0 & -0.261, 0.108, 0.631 \\ 3 & $\frac{\sqrt[]{3}}{3}$, -$\frac{\sqrt[]{3}}{3}$, $\frac{\sqrt[]{3}}{3}$ & 0.0, 0.0, 0.0 & 11 & 0.0, 0.0, 1.0 & 0.261, 0.108, -0.631 \\ @@ -54,7 +54,7 @@ \section{Test Parameters} \hline \end{tabular} \end{table} -It should be noted that the boresight vector passed to the model does not need to be a unit vector because the model will normalize the vector. +It should be noted that the boresight vector passed to the model does not need to be a unit vector because the model will normalize the vector. \section{Test Results} @@ -62,10 +62,10 @@ \section{Test Results} \caption{Pass or Fail Table} \label{tab:label} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, - \hline + \begin{tabular}{ c | c | c | c } % Column formatting, + \hline Test & Pass/Fail & Test & Pass/Fail\\ - \hline + \hline 1 & \input{AutoTex/Result0} & 9 & \input{AutoTex/Result8} \\ 2 & \input{AutoTex/Result1} & 10 & \input{AutoTex/Result9} \\ 3 & \input{AutoTex/Result2} & 11 & \input{AutoTex/Result10} \\ @@ -77,4 +77,4 @@ \section{Test Results} & & 17 & \input{AutoTex/Result16} \\ \hline \end{tabular} -\end{table} \ No newline at end of file +\end{table} diff --git a/src/simulation/dynamics/DynOutput/boreAngCalc/boreAngCalc.h b/src/simulation/dynamics/DynOutput/boreAngCalc/boreAngCalc.h index d3e13f29ed..4325558adb 100755 --- a/src/simulation/dynamics/DynOutput/boreAngCalc/boreAngCalc.h +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/boreAngCalc.h @@ -39,7 +39,7 @@ class BoreAngCalc: public SysModel { public: BoreAngCalc(); ~BoreAngCalc(); - + void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); void computeCelestialAxisPoint(); @@ -47,7 +47,7 @@ class BoreAngCalc: public SysModel { void computeInertialOutputData(); void WriteOutputMessages(uint64_t CurrentClock); void ReadInputs(); - + ReadFunctor scStateInMsg; //!< (-) spacecraft state input message ReadFunctor celBodyInMsg; //!< (-) celestial body state msg at which we pointing at Message angOutMsg; //!< (-) bore sight output message diff --git a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/AVS.sty b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/AVS.sty +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/Basilisk-ORBELEMCONVERT-20170703.tex b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/Basilisk-ORBELEMCONVERT-20170703.tex index f4aadf20ec..54290b17d0 100755 --- a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/Basilisk-ORBELEMCONVERT-20170703.tex +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/Basilisk-ORBELEMCONVERT-20170703.tex @@ -54,9 +54,9 @@ %\newcommand{\summary}{The orbital element conversion module is responsible for converting between Keplerian orbital elements and Cartesian vectors. A unit test has been written and performed, testing basic input/output, element to Cartesian conversion, and Cartesian to element conversion.} \begin{document} - + \makeCover - + % % enter the revision documentation here % to add more lines, copy the table entry and the \hline, and paste after the current entry. @@ -70,35 +70,35 @@ \hline 1.0 & First version - Mathematical formulation and implementation & G. Chapel & 07/27/17\\ \hline - + \end{longtable} } - - - + + + \newpage \setcounter{page}{1} \pagestyle{fancy} - + \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - - - - - - - - - - - + + + + + + + + + + + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. - + \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations - + \input{secTest.tex} % This includes test description, test parameters, and test results - + \input{secUserGuide.tex} % Contains a discussion of how to setup and configure the BSK module - -\end{document} \ No newline at end of file + +\end{document} diff --git a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/bibliography.bib b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secModelDescription.tex b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secModelDescription.tex index 89b6246abb..12fc56dea7 100644 --- a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secModelDescription.tex @@ -13,7 +13,7 @@ \subsubsection{Elements to Cartesian} \captionsetup{justification=centering} \includegraphics[width=0.3\textwidth]{Figures/orb_elem.png} \caption{Illustration of Keplerian Orbital Elements and Plane}\label{fig:OrbElem} -\end{figure} +\end{figure} The mathematical methods for conversion vary depending on parameter values, specifically semimajor axis and eccentricity inputs. Outlined below are three cases for varying $a$ and $e$. Although the first case is supported when converting from orbital elements to Cartesian, it is not accurate for the reverse process and is considered a limitation of this module. The mathematical model is still briefly explained below but the first case is not tested. These cases all result in three-component position and velocity vectors. \paragraph{Case 1 (\boldmath$e=1.0$, $a>0$):} This case specifies a rectilinear, elliptical orbit. The first steps in obtaining position and velocity vectors is to find the orbit radius and the magnitude of rectilinear velocity. The orbit radius $r$ depends on the semimajor axis, eccentricity, and true anomaly. The velocity, as expected, depends on the orbit radius, semimajor axis, and gravitational constant. Both of these values can be found using the following equations: @@ -136,7 +136,7 @@ \subsubsection{Cartesian to Elements} \paragraph{Case 1 (Non-circular, inclined orbit):} A non-circular orbit is specified with $e>0$ and an inclined orbit has an inclination $i>0$. Since it is for inclined orbits, this case depends on the line of nodes. Each remaining orbital element is calculated with cosine expressions, so depending on the sign of the line of nodes components, the angles may need to be adjusted by a $2\pi$ radians. Subcases are shown for these adjustments. \begin{equation} \label{eq:29} -\Omega = +\Omega = \begin{cases} \cos^{-1}(\frac{\bm{\hat{n}}_1}{n}) & \bm{\hat{n}}_2 > 0\\ 2\pi - \cos^{-1}(\frac{\bm{\hat{n}}_1}{n}) & \bm{\hat{n}}_2 < 0 @@ -144,13 +144,13 @@ \subsubsection{Cartesian to Elements} \end{equation} Calculating the ascending node requires the eccentricity vector found previously in Eq. \ref{eq:18}. \begin{equation} \label{eq:30} -\omega = +\omega = \begin{cases} \cos^{-1}(\frac{\bm{n} \cdot \bm{e}}{n e}) & \bm{\hat{n}}_3 > 0\\ 2\pi - \cos^{-1}(\frac{\bm{n} \cdot \bm{e}}{n e}) & \bm{\hat{n}}_3 < 0 \end{cases} \end{equation} -Finding the true anomaly also requires the eccentricity vector as well as the position vector, its magnitude, and the velocity vector. +Finding the true anomaly also requires the eccentricity vector as well as the position vector, its magnitude, and the velocity vector. \begin{equation} \label{eq:31} f = \begin{cases} @@ -165,7 +165,7 @@ \subsubsection{Cartesian to Elements} \end{equation} It also uses the true longitude of periapsis, defining \begin{equation} -\omega = +\omega = \begin{cases} \cos^{-1}(\frac{\bm{\hat{e}}_1}{n e}) & \bm{\hat{e}}_2 > 0 \\ 2\pi - \cos^{-1}(\frac{\bm{\hat{e}}_1}{n e}) & \bm{\hat{e}}_2 < 0 @@ -201,7 +201,7 @@ \subsubsection{Cartesian to Elements} \end{equation} Lastly, to ensure the appropriate value is used for the true anomaly, it should be checked whether the orbit is parabolic or hyperbolic. For both of these orbit types, a true anomaly greater than $\pi$ and less then $2\pi$ would mean the body is outside of the orbit. Thus, a limit must be applied to the true anomaly for parabolic and hyperbolic orbits, as shown below. The sign of $f$ remains the same after changing the magnitude. \begin{equation} -f = +f = \begin{cases} \pm 2\pi & \quad |e|\geq 1.0 \quad \textrm{and} \quad |\pm f|>\pi \end{cases} diff --git a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secModelFunctions.tex index 0a7695f421..d7caf07570 100644 --- a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secModelFunctions.tex @@ -53,5 +53,5 @@ \subsection{Limitations} \begin{itemize} \item Must be greater than or equal to zero \item Must be radians - \end{itemize} -\end{itemize} \ No newline at end of file + \end{itemize} +\end{itemize} diff --git a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secTest.tex b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secTest.tex index 3a6fe32874..0f89068b56 100644 --- a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secTest.tex +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secTest.tex @@ -1,5 +1,5 @@ \section{Test Description and Success Criteria} -The unit test, test\_orb\_elem\_convert.py, validates the internal aspects of the Basilisk orb\_elem\_convert module by comparing module output to expected output. The unit test validates the conversions performed between Keplerian orbital elements and Cartesian vectors. This test begins by converting elements to Cartesian and is immediately followed by converting the results back to orbital elements, providing the opportunity to compare the initial input with the final output. This is repeated for a variety of parameters. +The unit test, test\_orb\_elem\_convert.py, validates the internal aspects of the Basilisk orb\_elem\_convert module by comparing module output to expected output. The unit test validates the conversions performed between Keplerian orbital elements and Cartesian vectors. This test begins by converting elements to Cartesian and is immediately followed by converting the results back to orbital elements, providing the opportunity to compare the initial input with the final output. This is repeated for a variety of parameters. Differences between simulated and calculated output are measured, and those that approximately equal zero (typically $\delta < 10^{-9}$) qualify as a successful test. However, since the semimajor axis and the Cartesian vectors are greater than the other parameters by a factor of at least $10^7$, a higher discrepency is more tolerable. These tolerances are more clearly defined with the results. @@ -8,7 +8,7 @@ \subsection{Element to Cartesian} \subsubsection{Calculation vs. Module} The first check compares calculated Cartesian vectors to the simulated results and verifies that the discrepancy is less than $10^{-9}$. -\subsection{Cartesian to Element} +\subsection{Cartesian to Element} This test verifies, via two checks, that the conversion from Cartesian vectors to orbital elements is accurate. This test requires position and velocity vectors, which are obtained through initially converting from orbital elements to Cartesian, as described above. It also needs a gravitational constant input. The \textit{Elements2Cart} is set to \textit{False} and \textit{inputsGood} must be set to \textit{True}. Like in the previous conversion test, \textit{useEphemFormat} is alternated between \textit{True} and \textit{False} to ensure that the Cartesian vectors can use both planet and spacecraft state data identically and accurately. The calculations for this conversion are also performed seperately in the test to compare with the simulated results. \subsubsection{Calculation vs. Module} Similar to the first conversion test, this check compares the calculated orbital elements to the elements obtained from the module. @@ -17,7 +17,7 @@ \subsubsection{Input vs. Module} \section{Test Parameters} -The previous description only briefly mentions the input parameters required to perform the tests on this module, so this section details the parameters further. In addition to the parameters being converted, the unit test requires inputs that identify which conversion is performed, what type of state data are being used, and whether or not to proceed with the given input. +The previous description only briefly mentions the input parameters required to perform the tests on this module, so this section details the parameters further. In addition to the parameters being converted, the unit test requires inputs that identify which conversion is performed, what type of state data are being used, and whether or not to proceed with the given input. \begin{enumerate} \item \underline{Euler Angles}:\\ @@ -33,7 +33,7 @@ \section{Test Parameters} \end{equation} Inputs alternate between zero and these given values depending on the orbit type, which is specified in the results section. All angles must be input as radians, so the conversion from degrees is performed as shown in Eq. \ref{eq:42} - \ref{eq:44}. \item \underline{True Anomaly}:\\ - The true anomaly input stays the same for each parameter set because the different orbits do not require a unique value, as they may for inclination, ascending node, and argument of periapsis. The true anomaly value, however, is pulled from the same module as those parameters, giving + The true anomaly input stays the same for each parameter set because the different orbits do not require a unique value, as they may for inclination, ascending node, and argument of periapsis. The true anomaly value, however, is pulled from the same module as those parameters, giving \begin{equation} \label{eq:41} f = 85.3^\circ = 1.489 \text{rad} \end{equation} @@ -64,7 +64,7 @@ \section{Test Parameters} \end{table} \item \underline{Valid Input Identifier}\\ The last necessary parameter is another boolean flag, this time indicating whether the inputs are valid. This value is read from a message and distinguishes if it is written succesfully. If succesful, this allows the rest of the module to be run, initiating the conversion process. The test uses \textit{inputsGood} to execute this check where \textit{True} lets the module proceed and \textit{False} does not, so this value should always be \textit{True} for the module to execute properly. -\end{enumerate} +\end{enumerate} The orbital element parameters are varied throughout the test to validate a range of orbits. Since the test performs both conversions consecutively and the Cartesian results depend on the Keplerian inputs, the position and velocity vectors used for the second conversion also vary. These parameters are discussed with the results in the next section. \begin{table}[H] \caption{Error Tolerance-Note: Absolute Tolerance is abs(truth-value)} @@ -95,8 +95,8 @@ \section{Test Results} \hline \textbf{Parameter Sets} & \textbf{Results} & \textbf{Notes} \\ \hline 1 & \input{AutoTex/IncEllip_a_1PassedText1} &\input{AutoTex/IncEllip_a_1PassFailMsg1}\\ - 2 & \input{AutoTex/EquEllip_a_1PassedText1} &\input{AutoTex/EquEllip_a_1PassFailMsg1}\\ - 3 & \input{AutoTex/IncCirc_1PassedText1} &\input{AutoTex/IncCirc_1PassFailMsg1}\\ + 2 & \input{AutoTex/EquEllip_a_1PassedText1} &\input{AutoTex/EquEllip_a_1PassFailMsg1}\\ + 3 & \input{AutoTex/IncCirc_1PassedText1} &\input{AutoTex/IncCirc_1PassFailMsg1}\\ 4 & \input{AutoTex/EquCirc_1PassedText1} &\input{AutoTex/EquCirc_1PassFailMsg1}\\ 5 & \input{AutoTex/IncPara_1PassedText1} &\input{AutoTex/IncPara_1PassFailMsg1}\\ 6 & \input{AutoTex/EquPara_1PassedText1} &\input{AutoTex/EquPara_1PassFailMsg1}\\ @@ -109,7 +109,7 @@ \section{Test Results} \begin{enumerate} \item \underline{Inclined Elliptic Orbit ($00$)}\\ To test elliptic orbtits, a range of semimajor axis inputs from $10-10^7$km is used. The eccentricity is also varied with inputs from $0.01-0.75$. As $a$ changes, $e=0.5$ and as $e$ changes, $a=10^7$km. All parameter sets passed for this case since the difference when comparing simulated and calculated results was always zero, as shown by Fig. \ref{fig:2} and \ref{fig:3}. -\begin{figure}[H] +\begin{figure}[H] \centering \subfigure[$e=0.01$ and $a=10^7$km]{\includegraphics[width=0.5\textwidth]{AutoTex/IncEllip_e_1.pdf}} \subfigure[$e=0.75$ and $a=10^7$km]{\includegraphics[width=0.5\textwidth]{AutoTex/IncEllip_e_2.pdf}} @@ -207,12 +207,12 @@ \section{Test Results} \caption{Cartesian to Element Test Results} \label{tab:Elem to Cart results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c|c|c} % Column formatting, + \begin{tabular}{c|c|c} % Column formatting, \hline \textbf{Parameter Sets} & \textbf{Results} & \textbf{Notes} \\ \hline 1 & \input{AutoTex/IncEllip_a_1PassedText2} &\input{AutoTex/IncEllip_a_1PassFailMsg2}\\ - 2 & \input{AutoTex/EquEllip_a_1PassedText2} &\input{AutoTex/EquEllip_a_1PassFailMsg2}\\ - 3 & \input{AutoTex/IncCirc_1PassedText2} &\input{AutoTex/IncCirc_1PassFailMsg2}\\ + 2 & \input{AutoTex/EquEllip_a_1PassedText2} &\input{AutoTex/EquEllip_a_1PassFailMsg2}\\ + 3 & \input{AutoTex/IncCirc_1PassedText2} &\input{AutoTex/IncCirc_1PassFailMsg2}\\ 4 & \input{AutoTex/EquCirc_1PassedText2} &\input{AutoTex/EquCirc_1PassFailMsg2}\\ 5 & \input{AutoTex/IncPara_1PassedText2} &\input{AutoTex/IncPara_1PassFailMsg2}\\ 6 & \input{AutoTex/EquPara_1PassedText2} &\input{AutoTex/EquPara_1PassFailMsg2}\\ @@ -222,9 +222,9 @@ \section{Test Results} \end{tabular} \end{table} Orbital elements are tested by making two comparisons. The first compares the simulated output with the calculated output, like in the previous conversion. The second compares elements initially used as inputs for the first conversion with those produced as the output of the second. As observed when testing the previous conversion, the comparison between calculated and simulated elements results in complete agreement. Also, the two cases presented by the astronomical body identifier, once again, produce identical results. Thus, the main focus of the Cartesian to element test results is the input/output comparison. - - Displayed below are explanations of the results after converting Cartesian to elements. Graphical representations of the parameter comparisons for each orbit type are not provided for this test, since the output is already specified above as the input when converting from elements to Cartesian. Figures from above can also be referenced for the Cartesian inputs. - + + Displayed below are explanations of the results after converting Cartesian to elements. Graphical representations of the parameter comparisons for each orbit type are not provided for this test, since the output is already specified above as the input when converting from elements to Cartesian. Figures from above can also be referenced for the Cartesian inputs. + \begin{enumerate} \item \underline{Inclined Elliptic Orbit ($00$)}\\ All variations passed the tests performed, however, the discrepancy between semimajor axis initial input and simulated result increases as eccentricity approaches 1.0. This is because it approaches the parabolic case, which requires a different conversion method and makes the elliptic calculations less accurate. The discrepancy with the tested parameters never increases past $10^{-7}$km, though, and since the semimajor axis is typically on the scale of at least $10^7$km, this is acceptable. Fig. \ref{fig:aDiff} shows the change in semimajor axis discrepancy as eccentricity reaches 0.999. %and Figures \ref{fig:14} and \ref{fig:15} illustrate the test results. @@ -326,4 +326,4 @@ \section{Test Results} % \end{figure} \end{enumerate} \end{itemize} -\pagebreak \ No newline at end of file +\pagebreak diff --git a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secUserGuide.tex b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secUserGuide.tex index d62b9dab3e..c5757cc8e3 100644 --- a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secUserGuide.tex +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/secUserGuide.tex @@ -5,7 +5,7 @@ \section{User Guide} \item {\tt spiceStateInMsg}: spice state input message \item {\tt elemInMsg}: classical orbit elements input message \end{itemize} -Note that only one can be connected to. +Note that only one can be connected to. The three output messages are: \begin{itemize} @@ -28,4 +28,4 @@ \section{User Guide} Vallado, D. A., and McClain, W. D., \textit{Fundamentals of Astrodynamics and Applications, 4th ed}. Hawthorne, CA: Published by Microcosm Press, 2013. \bibitem{bib:2} Schaub, H., and Junkins, J. L., \textit{Analytical Mechanics of Space Systems, 3rd ed.}. Reston, VA: American Institute of Aeronautics and Astronautics. -\end{thebibliography} \ No newline at end of file +\end{thebibliography} diff --git a/src/simulation/dynamics/ExtPulsedTorque/_Documentation/Basilisk-extPulsedTorque-20170324.tex b/src/simulation/dynamics/ExtPulsedTorque/_Documentation/Basilisk-extPulsedTorque-20170324.tex index 7e9678dbe7..4ed3d4c881 100755 --- a/src/simulation/dynamics/ExtPulsedTorque/_Documentation/Basilisk-extPulsedTorque-20170324.tex +++ b/src/simulation/dynamics/ExtPulsedTorque/_Documentation/Basilisk-extPulsedTorque-20170324.tex @@ -49,7 +49,7 @@ \label{fig:extPulse1} \end{figure} \section{Introduction} -This module allows a special pulsed external disturbance torque $\bm \tau$ to be applied onto a rigid body. The torque is taken about the body-fixed point $B$, and the vector components are given in the body frame $\mathcal{B}$ as illustrated in Figure~\ref{fig:extPulse1}. +This module allows a special pulsed external disturbance torque $\bm \tau$ to be applied onto a rigid body. The torque is taken about the body-fixed point $B$, and the vector components are given in the body frame $\mathcal{B}$ as illustrated in Figure~\ref{fig:extPulse1}. @@ -61,7 +61,7 @@ \section{Introduction} \label{fig:extPulse2} \end{figure} \section{Specifying the Pulsed Disturbance Torque} -The module creates a cyclic disturbance torque which is applied to the rigid body. The torque vector $\bm\tau$ is applied for equal time periods as $+\bm\tau$ and $-\bm\tau$. This is followed by a specified off period before repeating. This pattern is illustrated in Figure~\ref{fig:extPulse2}. +The module creates a cyclic disturbance torque which is applied to the rigid body. The torque vector $\bm\tau$ is applied for equal time periods as $+\bm\tau$ and $-\bm\tau$. This is followed by a specified off period before repeating. This pattern is illustrated in Figure~\ref{fig:extPulse2}. Note that the pulse and off periods are specified through integer counts of the simulation integration time. @@ -69,7 +69,7 @@ \section{Specifying the Pulsed Disturbance Torque} \section{Module Parameters} -The external disturbance torque vector and pulsing parameters are set directly from python. +The external disturbance torque vector and pulsing parameters are set directly from python. \subsection{{\tt pulsedTorqueExternalPntB\_B} Parameter} This vector sets the external torque, about point $B$, in $\mathcal{B}$ body-frame vector components. diff --git a/src/simulation/dynamics/ExtPulsedTorque/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/ExtPulsedTorque/_Documentation/BasiliskReportMemo.cls index 7096e85b10..2c8157c432 100755 --- a/src/simulation/dynamics/ExtPulsedTorque/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/ExtPulsedTorque/_Documentation/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/ExtPulsedTorque/_Documentation/Support/diagram.nb b/src/simulation/dynamics/ExtPulsedTorque/_Documentation/Support/diagram.nb index e1cc51bc2a..afcf3a12ea 100644 --- a/src/simulation/dynamics/ExtPulsedTorque/_Documentation/Support/diagram.nb +++ b/src/simulation/dynamics/ExtPulsedTorque/_Documentation/Support/diagram.nb @@ -21,27 +21,27 @@ Notebook[{ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"list", "=", - RowBox[{"{", - RowBox[{"0", ",", "1", ",", "1", ",", - RowBox[{"-", "1"}], ",", + RowBox[{"list", "=", + RowBox[{"{", + RowBox[{"0", ",", "1", ",", "1", ",", + RowBox[{"-", "1"}], ",", RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0", ",", "0", ",", "0", ",", - "1", ",", "1", ",", - RowBox[{"-", "1"}], ",", - RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], + "1", ",", "1", ",", + RowBox[{"-", "1"}], ",", + RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}]}]], "Input", - CellChangeTimes->{{3.69954817229069*^9, + CellChangeTimes->{{3.69954817229069*^9, 3.6995481987000027`*^9}},ExpressionUUID->"f608c001-c930-4f7b-b278-\ 52a92500c895"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"0", ",", "1", ",", "1", ",", - RowBox[{"-", "1"}], ",", - RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0", ",", "0", ",", "0", ",", - "1", ",", "1", ",", - RowBox[{"-", "1"}], ",", - RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], + RowBox[{"{", + RowBox[{"0", ",", "1", ",", "1", ",", + RowBox[{"-", "1"}], ",", + RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0", ",", "0", ",", "0", ",", + "1", ",", "1", ",", + RowBox[{"-", "1"}], ",", + RowBox[{"-", "1"}], ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}]], "Output", CellChangeTimes->{ 3.699548200365662*^9},ExpressionUUID->"cea2c040-ca18-43e8-b033-\ @@ -51,28 +51,28 @@ bc62f1a070d5"] Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"ListPlot", "[", - RowBox[{"list", ",", - RowBox[{"GridLines", "\[Rule]", - RowBox[{"{", + RowBox[{"ListPlot", "[", + RowBox[{"list", ",", + RowBox[{"GridLines", "\[Rule]", + RowBox[{"{", RowBox[{ - RowBox[{"Table", "[", - RowBox[{"i", ",", - RowBox[{"{", - RowBox[{"i", ",", "19"}], "}"}]}], "]"}], ",", "None"}], "}"}]}]}], + RowBox[{"Table", "[", + RowBox[{"i", ",", + RowBox[{"{", + RowBox[{"i", ",", "19"}], "}"}]}], "]"}], ",", "None"}], "}"}]}]}], "]"}]], "Input", - CellChangeTimes->{{3.699548200865472*^9, + CellChangeTimes->{{3.699548200865472*^9, 3.699548259633494*^9}},ExpressionUUID->"5d715267-e78e-4579-93b1-\ 026fa74c07a3"], Cell[BoxData[ GraphicsBox[{{}, {{{}, { {RGBColor[0.368417, 0.506779, 0.709798], PointSize[ - 0.012833333333333334`], AbsoluteThickness[1.6], + 0.012833333333333334`], AbsoluteThickness[1.6], PointBox[CompressedData[" 1:eJxTTMoPSmViYGAQBmIQDQEf7BlQgQOqOAcaXwDG3w+hRdD4Eg6oxsmg8RXQ +EpofBU0vhqa/RpofC00+3XQ+Hpo5hmg8Q3R+EZofGM4HwD5lRVG - + "]]}}, {}}}, {}, {}, {{}, {}}, {{}, {}}}, AspectRatio->NCache[GoldenRatio^(-1), 0.6180339887498948], Axes->{True, True}, @@ -82,30 +82,30 @@ Cell[BoxData[ Frame->{{False, False}, {False, False}}, FrameLabel->{{None, None}, {None, None}}, FrameTicks->{{Automatic, Automatic}, {Automatic, Automatic}}, - GridLines->{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, + GridLines->{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19}, None}, GridLinesStyle->Directive[ GrayLevel[0.5, 0.4]], ImagePadding->All, Method->{"CoordinatesToolOptions" -> {"DisplayFunction" -> ({ (Identity[#]& )[ - Part[#, 1]], + Part[#, 1]], (Identity[#]& )[ Part[#, 2]]}& ), "CopiedValueFunction" -> ({ (Identity[#]& )[ - Part[#, 1]], + Part[#, 1]], (Identity[#]& )[ Part[#, 2]]}& )}}, PlotRange->{{0., 19.}, {-1., 1.}}, PlotRangeClipping->True, PlotRangePadding->{{ - Scaled[0.02], + Scaled[0.02], Scaled[0.02]}, { - Scaled[0.05], + Scaled[0.05], Scaled[0.05]}}, Ticks->{Automatic, Automatic}]], "Output", CellChangeTimes->{{3.699548208907251*^9, 3.699548220355818*^9}, { - 3.6995482523337917`*^9, + 3.6995482523337917`*^9, 3.699548260039016*^9}},ExpressionUUID->"31e6dcc1-995e-4543-94ae-\ e44620d193bf"] }, Open ]] @@ -144,4 +144,3 @@ Cell[1968, 67, 1529, 42, 263, "Output", "ExpressionUUID" -> \ *) (* End of internal cache information *) - diff --git a/src/simulation/dynamics/ExtPulsedTorque/_UnitTest/test_extPulsedTorque.py b/src/simulation/dynamics/ExtPulsedTorque/_UnitTest/test_extPulsedTorque.py index eff6cbe7be..59912ba3ac 100755 --- a/src/simulation/dynamics/ExtPulsedTorque/_UnitTest/test_extPulsedTorque.py +++ b/src/simulation/dynamics/ExtPulsedTorque/_UnitTest/test_extPulsedTorque.py @@ -167,4 +167,3 @@ def run(show_plots, offCount): test_module(False, # show_plots 3 # offCount ) - diff --git a/src/simulation/dynamics/FuelTank/_Documentation/AVS.sty b/src/simulation/dynamics/FuelTank/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/FuelTank/_Documentation/AVS.sty +++ b/src/simulation/dynamics/FuelTank/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/FuelTank/_Documentation/Basilisk-FUELTANK-20171203.tex b/src/simulation/dynamics/FuelTank/_Documentation/Basilisk-FUELTANK-20171203.tex index 815f5bef31..03728e5433 100755 --- a/src/simulation/dynamics/FuelTank/_Documentation/Basilisk-FUELTANK-20171203.tex +++ b/src/simulation/dynamics/FuelTank/_Documentation/Basilisk-FUELTANK-20171203.tex @@ -94,7 +94,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/FuelTank/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/FuelTank/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/FuelTank/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/FuelTank/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/FuelTank/_Documentation/Support/PlanarFlexibleDynamicsDerivation.nb b/src/simulation/dynamics/FuelTank/_Documentation/Support/PlanarFlexibleDynamicsDerivation.nb index 37b87f117c..20aba4294a 100644 --- a/src/simulation/dynamics/FuelTank/_Documentation/Support/PlanarFlexibleDynamicsDerivation.nb +++ b/src/simulation/dynamics/FuelTank/_Documentation/Support/PlanarFlexibleDynamicsDerivation.nb @@ -22,669 +22,669 @@ Notebook[{ Cell[CellGroupData[{ Cell["Planar Flexible Dynamics with two solar panels", "Section", CellChangeTimes->{{3.6476211926645107`*^9, 3.6476212207444983`*^9}, { - 3.647621411462695*^9, 3.647621435484638*^9}, {3.6476218320778303`*^9, + 3.647621411462695*^9, 3.647621435484638*^9}, {3.6476218320778303`*^9, 3.6476218344203043`*^9}, {3.649439747948255*^9, 3.649439759068905*^9}}], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Rhub", " ", "=", " ", - RowBox[{"{", + RowBox[{"Rhub", " ", "=", " ", + RowBox[{"{", RowBox[{ - RowBox[{"xHub", "[", "t", "]"}], ",", " ", + RowBox[{"xHub", "[", "t", "]"}], ",", " ", RowBox[{"yHub", "[", "t", "]"}]}], "}"}]}]], "Input", CellChangeTimes->{{3.6476077582392817`*^9, 3.64760777925314*^9}, { - 3.647615079246727*^9, 3.647615079629671*^9}, {3.647621147852172*^9, + 3.647615079246727*^9, 3.647615079629671*^9}, {3.647621147852172*^9, 3.647621148259598*^9}, {3.649439738791636*^9, 3.649439776241705*^9}, { - 3.7096453803292313`*^9, 3.709645383457893*^9}, {3.70964542147725*^9, + 3.7096453803292313`*^9, 3.709645383457893*^9}, {3.70964542147725*^9, 3.7096454318767157`*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"xHub", "[", "t", "]"}], ",", + RowBox[{"xHub", "[", "t", "]"}], ",", RowBox[{"yHub", "[", "t", "]"}]}], "}"}]], "Output", - CellChangeTimes->{3.7096454421264477`*^9, 3.7098930254953527`*^9, - 3.70990443182268*^9, 3.709912186034545*^9, 3.709991565698237*^9, + CellChangeTimes->{3.7096454421264477`*^9, 3.7098930254953527`*^9, + 3.70990443182268*^9, 3.709912186034545*^9, 3.709991565698237*^9, 3.71006682731984*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"VHub", " ", "=", " ", - RowBox[{"D", "[", + RowBox[{"VHub", " ", "=", " ", + RowBox[{"D", "[", RowBox[{"Rhub", ",", "t"}], "]"}]}]], "Input", CellChangeTimes->{{3.647608152658317*^9, 3.6476081672650843`*^9}, { - 3.6494398162615347`*^9, 3.649439828455247*^9}, {3.7096466475466137`*^9, + 3.6494398162615347`*^9, 3.649439828455247*^9}, {3.7096466475466137`*^9, 3.7096466482166*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], ",", + MultilineFunction->None], "[", "t", "]"}], ",", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "}"}]], "Output", CellChangeTimes->{ - 3.647608169805852*^9, {3.647614323833295*^9, 3.64761434783421*^9}, - 3.647615089563908*^9, 3.647621228054256*^9, {3.649439822196238*^9, - 3.649439831862186*^9}, 3.650026329921008*^9, 3.650026435544567*^9, - 3.650113943872491*^9, 3.6501140768873177`*^9, 3.650114124028068*^9, - 3.650121181032531*^9, 3.6501254656908484`*^9, 3.650126350692739*^9, - 3.650126432976297*^9, 3.650126661304862*^9, 3.6501271224543867`*^9, - 3.709645396421109*^9, 3.7096454641959667`*^9, 3.7096466511650953`*^9, - 3.7098930294802837`*^9, 3.709904431894704*^9, 3.709912186140971*^9, + 3.647608169805852*^9, {3.647614323833295*^9, 3.64761434783421*^9}, + 3.647615089563908*^9, 3.647621228054256*^9, {3.649439822196238*^9, + 3.649439831862186*^9}, 3.650026329921008*^9, 3.650026435544567*^9, + 3.650113943872491*^9, 3.6501140768873177`*^9, 3.650114124028068*^9, + 3.650121181032531*^9, 3.6501254656908484`*^9, 3.650126350692739*^9, + 3.650126432976297*^9, 3.650126661304862*^9, 3.6501271224543867`*^9, + 3.709645396421109*^9, 3.7096454641959667`*^9, 3.7096466511650953`*^9, + 3.7098930294802837`*^9, 3.709904431894704*^9, 3.709912186140971*^9, 3.709991565807317*^9, 3.710066827425913*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP1Hub", " ", "=", " ", - RowBox[{"{", + RowBox[{"RSP1Hub", " ", "=", " ", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge1", "*", - RowBox[{"Cos", "[", + RowBox[{"Rhinge1", "*", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], - " ", "+", " ", - RowBox[{"d1", "*", " ", - RowBox[{"Cos", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], + " ", "+", " ", + RowBox[{"d1", "*", " ", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", - - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", " ", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", + + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", " ", RowBox[{ - RowBox[{"Rhinge1", "*", - RowBox[{"Sin", "[", + RowBox[{"Rhinge1", "*", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], - " ", "+", " ", - RowBox[{"d1", "*", " ", - RowBox[{"Sin", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], + " ", "+", " ", + RowBox[{"d1", "*", " ", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", - + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]}]], "Input", CellChangeTimes->{{3.649439849206862*^9, 3.649439917339305*^9}, { - 3.649440015026457*^9, 3.649440104067552*^9}, {3.709645886898323*^9, + 3.649440015026457*^9, 3.649440104067552*^9}, {3.709645886898323*^9, 3.709645916521826*^9}, {3.7096459487470293`*^9, 3.709646004845886*^9}, { - 3.709646048343261*^9, 3.709646092722971*^9}, {3.709646222601664*^9, + 3.709646048343261*^9, 3.709646092722971*^9}, {3.709646222601664*^9, 3.709646380970314*^9}, {3.709893035295415*^9, 3.709893153573711*^9}, { 3.709904347427107*^9, 3.7099043599351263`*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]], "Output", - CellChangeTimes->{3.649440107978813*^9, 3.650026329938651*^9, - 3.650026435571876*^9, 3.650113943890592*^9, 3.650114076912006*^9, - 3.6501141240599203`*^9, 3.650121181063718*^9, 3.6501254657141533`*^9, - 3.65012635072224*^9, 3.650126432995634*^9, 3.650126661323832*^9, - 3.650127122474761*^9, 3.709646389923583*^9, 3.7098931733877287`*^9, - 3.7099043619306583`*^9, 3.709904431944851*^9, 3.709912186205373*^9, + CellChangeTimes->{3.649440107978813*^9, 3.650026329938651*^9, + 3.650026435571876*^9, 3.650113943890592*^9, 3.650114076912006*^9, + 3.6501141240599203`*^9, 3.650121181063718*^9, 3.6501254657141533`*^9, + 3.65012635072224*^9, 3.650126432995634*^9, 3.650126661323832*^9, + 3.650127122474761*^9, 3.709646389923583*^9, 3.7098931733877287`*^9, + 3.7099043619306583`*^9, 3.709904431944851*^9, 3.709912186205373*^9, 3.7099915659045143`*^9, 3.710066827499762*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP1", " ", "=", " ", + RowBox[{"RSP1", " ", "=", " ", RowBox[{"Rhub", " ", "+", " ", "RSP1Hub"}]}]], "Input", CellChangeTimes->{{3.709646395997994*^9, 3.709646412127426*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"xHub", "[", "t", "]"}]}], ",", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"xHub", "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", RowBox[{"yHub", "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.709646415182599*^9, 3.709893180581501*^9, - 3.709904365017598*^9, 3.709904432007655*^9, 3.7099121862900763`*^9, + CellChangeTimes->{3.709646415182599*^9, 3.709893180581501*^9, + 3.709904365017598*^9, 3.709904432007655*^9, 3.7099121862900763`*^9, 3.70999156597033*^9, 3.710066827566654*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP2Hub", " ", "=", " ", - RowBox[{"{", + RowBox[{"RSP2Hub", " ", "=", " ", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge2", "*", - RowBox[{"Cos", "[", + RowBox[{"Rhinge2", "*", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], - " ", "+", " ", - RowBox[{"d2", "*", " ", - RowBox[{"Cos", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], + " ", "+", " ", + RowBox[{"d2", "*", " ", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", - - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", " ", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", + + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", " ", RowBox[{ - RowBox[{"Rhinge2", "*", - RowBox[{"Sin", "[", + RowBox[{"Rhinge2", "*", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], - " ", "+", " ", - RowBox[{"d2", "*", " ", - RowBox[{"Sin", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], + " ", "+", " ", + RowBox[{"d2", "*", " ", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", - + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]}]], "Input", CellChangeTimes->{{3.709646463873159*^9, 3.709646543110506*^9}, { - 3.70989319908799*^9, 3.709893260993504*^9}, {3.709904369663701*^9, + 3.70989319908799*^9, 3.709893260993504*^9}, {3.709904369663701*^9, 3.709904375505327*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]], "Output", - CellChangeTimes->{{3.709646522302372*^9, 3.709646546358492*^9}, - 3.709893305041185*^9, 3.709904376632481*^9, 3.709904432100214*^9, + CellChangeTimes->{{3.709646522302372*^9, 3.709646546358492*^9}, + 3.709893305041185*^9, 3.709904376632481*^9, 3.709904432100214*^9, 3.709912186356444*^9, 3.709991566037938*^9, 3.710066827630702*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP2", " ", "=", " ", + RowBox[{"RSP2", " ", "=", " ", RowBox[{"Rhub", " ", "+", " ", "RSP2Hub"}]}]], "Input", CellChangeTimes->{{3.709646526347693*^9, 3.709646553485578*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"xHub", "[", "t", "]"}]}], ",", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"xHub", "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", RowBox[{"yHub", "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.7096465578787622`*^9, 3.709893308250244*^9, - 3.709904379426956*^9, 3.70990443217269*^9, 3.709912186422678*^9, + CellChangeTimes->{3.7096465578787622`*^9, 3.709893308250244*^9, + 3.709904379426956*^9, 3.70990443217269*^9, 3.709912186422678*^9, 3.709991566104603*^9, 3.7100668277002287`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"VSP1", " ", "=", " ", - RowBox[{"D", "[", + RowBox[{"VSP1", " ", "=", " ", + RowBox[{"D", "[", RowBox[{"RSP1", ",", "t"}], "]"}]}]], "Input", CellChangeTimes->{{3.649440115855461*^9, 3.649440124808634*^9}, { 3.709646572311063*^9, 3.70964658152712*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ",", + MultilineFunction->None], "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.649440131399378*^9, 3.650026329966713*^9, - 3.650026435604875*^9, 3.650113943920773*^9, 3.650114076942313*^9, - 3.6501141240781507`*^9, 3.650121181083191*^9, 3.6501254657517776`*^9, - 3.650126350757868*^9, 3.6501264330338707`*^9, 3.6501266613621817`*^9, - 3.650127122516333*^9, 3.709646585406389*^9, 3.709893320897305*^9, - 3.70990438813186*^9, 3.70990443223833*^9, 3.709912186471583*^9, + CellChangeTimes->{3.649440131399378*^9, 3.650026329966713*^9, + 3.650026435604875*^9, 3.650113943920773*^9, 3.650114076942313*^9, + 3.6501141240781507`*^9, 3.650121181083191*^9, 3.6501254657517776`*^9, + 3.650126350757868*^9, 3.6501264330338707`*^9, 3.6501266613621817`*^9, + 3.650127122516333*^9, 3.709646585406389*^9, 3.709893320897305*^9, + 3.70990438813186*^9, 3.70990443223833*^9, 3.709912186471583*^9, 3.7099915661712847`*^9, 3.710066827767516*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"VSP2", " ", "=", " ", - RowBox[{"D", "[", + RowBox[{"VSP2", " ", "=", " ", + RowBox[{"D", "[", RowBox[{"RSP2", ",", "t"}], "]"}]}]], "Input", CellChangeTimes->{{3.649440262161566*^9, 3.64944026549428*^9}, { 3.709646616832868*^9, 3.709646621688724*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ",", + MultilineFunction->None], "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.649440268747218*^9, 3.650026330022114*^9, - 3.650026435667726*^9, 3.650113943975575*^9, 3.650114076989862*^9, - 3.6501141241372833`*^9, 3.650121181143683*^9, 3.650125465840637*^9, - 3.650126350821439*^9, 3.650126433110935*^9, 3.650126661418356*^9, - 3.650127122571417*^9, 3.709646629412587*^9, 3.709893323420001*^9, - 3.7099043906789207`*^9, 3.7099044323057747`*^9, 3.70991218652265*^9, + CellChangeTimes->{3.649440268747218*^9, 3.650026330022114*^9, + 3.650026435667726*^9, 3.650113943975575*^9, 3.650114076989862*^9, + 3.6501141241372833`*^9, 3.650121181143683*^9, 3.650125465840637*^9, + 3.650126350821439*^9, 3.650126433110935*^9, 3.650126661418356*^9, + 3.650127122571417*^9, 3.709646629412587*^9, 3.709893323420001*^9, + 3.7099043906789207`*^9, 3.7099044323057747`*^9, 3.70991218652265*^9, 3.7099915662376337`*^9, 3.710066827832479*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"T", " ", "=", " ", + RowBox[{"T", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"1", "/", "2"}], "*", "mHub", "*", - RowBox[{"Dot", "[", - RowBox[{"VHub", ",", "VHub"}], "]"}]}], "+", + RowBox[{"1", "/", "2"}], "*", "mHub", "*", + RowBox[{"Dot", "[", + RowBox[{"VHub", ",", "VHub"}], "]"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "IHub", "*", + RowBox[{"1", "/", "2"}], "*", "IHub", "*", RowBox[{ RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}], "^", "2"}]}], "+", + RowBox[{"theta", "'"}], "[", "t", "]"}], "^", "2"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "mSP1", "*", - RowBox[{"Dot", "[", - RowBox[{"VSP1", ",", "VSP1"}], "]"}]}], "+", + RowBox[{"1", "/", "2"}], "*", "mSP1", "*", + RowBox[{"Dot", "[", + RowBox[{"VSP1", ",", "VSP1"}], "]"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "ISP1", "*", + RowBox[{"1", "/", "2"}], "*", "ISP1", "*", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}], "+", + RowBox[{"theta", "'"}], "[", "t", "]"}], "+", RowBox[{ - RowBox[{"theta1", "'"}], "[", "t", "]"}]}], ")"}], "^", "2"}]}], "+", - + RowBox[{"theta1", "'"}], "[", "t", "]"}]}], ")"}], "^", "2"}]}], "+", + RowBox[{ - RowBox[{"1", "/", "2"}], "*", "mSP2", "*", - RowBox[{"Dot", "[", - RowBox[{"VSP2", ",", "VSP2"}], "]"}]}], "+", + RowBox[{"1", "/", "2"}], "*", "mSP2", "*", + RowBox[{"Dot", "[", + RowBox[{"VSP2", ",", "VSP2"}], "]"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "ISP2", "*", + RowBox[{"1", "/", "2"}], "*", "ISP2", "*", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}], "+", + RowBox[{"theta", "'"}], "[", "t", "]"}], "+", RowBox[{ - RowBox[{"theta2", "'"}], "[", "t", "]"}]}], ")"}], "^", + RowBox[{"theta2", "'"}], "[", "t", "]"}]}], ")"}], "^", "2"}]}]}]}]], "Input", CellChangeTimes->{{3.6476081740482817`*^9, 3.647608248074851*^9}, { - 3.6476082871491823`*^9, 3.647608323868623*^9}, {3.6476084126718693`*^9, + 3.6476082871491823`*^9, 3.647608323868623*^9}, {3.6476084126718693`*^9, 3.647608426646431*^9}, {3.647614434493647*^9, 3.647614437644298*^9}, { - 3.649440314162457*^9, 3.649440454966736*^9}, {3.650113501057988*^9, + 3.649440314162457*^9, 3.649440454966736*^9}, {3.650113501057988*^9, 3.650113561077476*^9}, {3.6501140530388117`*^9, 3.650114070671464*^9}, { - 3.650125417777068*^9, 3.650125455538785*^9}, {3.650126331537952*^9, + 3.650125417777068*^9, 3.650125455538785*^9}, {3.650126331537952*^9, 3.650126341438471*^9}, {3.650126402719391*^9, 3.650126428999959*^9}, { - 3.650126617109906*^9, 3.6501266521408863`*^9}, {3.709646659345621*^9, + 3.650126617109906*^9, 3.6501266521408863`*^9}, {3.709646659345621*^9, 3.709646783134551*^9}, {3.7098933386684723`*^9, 3.709893360494198*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - FractionBox["1", "2"], " ", "IHub", " ", + FractionBox["1", "2"], " ", "IHub", " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP1", " ", + FractionBox["1", "2"], " ", "ISP1", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP2", " ", + FractionBox["1", "2"], " ", "ISP2", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mHub", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mHub", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}], "2"], "+", SuperscriptBox[ RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}], - "+", + "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}]}]], "Output", - CellChangeTimes->{3.650126433152005*^9, 3.650126661451419*^9, - 3.6501271226026382`*^9, 3.709647565406519*^9, 3.709893450106464*^9, - 3.709904399733959*^9, 3.709904432371422*^9, 3.709912186587597*^9, + CellChangeTimes->{3.650126433152005*^9, 3.650126661451419*^9, + 3.6501271226026382`*^9, 3.709647565406519*^9, 3.709893450106464*^9, + 3.709904399733959*^9, 3.709904432371422*^9, 3.709912186587597*^9, 3.709991566302853*^9, 3.7100668278993692`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"V", " ", "=", " ", + RowBox[{"V", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"1", "/", "2"}], "k1", + RowBox[{"1", "/", "2"}], "k1", RowBox[{ - RowBox[{"(", - RowBox[{"theta1", "[", "t", "]"}], ")"}], "^", "2"}]}], "+", + RowBox[{"(", + RowBox[{"theta1", "[", "t", "]"}], ")"}], "^", "2"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "k2", + RowBox[{"1", "/", "2"}], "k2", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{"theta2", "[", "t", "]"}], ")"}], "^", "2"}]}]}]}]], "Input", CellChangeTimes->{{3.647608457408671*^9, 3.647608497714343*^9}, { - 3.6494404974964943`*^9, 3.6494405178638973`*^9}, {3.709893455995377*^9, + 3.6494404974964943`*^9, 3.6494405178638973`*^9}, {3.709893455995377*^9, 3.709893465684599*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - FractionBox["1", "2"], " ", "k1", " ", + FractionBox["1", "2"], " ", "k1", " ", SuperscriptBox[ - RowBox[{"theta1", "[", "t", "]"}], "2"]}], "+", + RowBox[{"theta1", "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "k2", " ", + FractionBox["1", "2"], " ", "k2", " ", SuperscriptBox[ RowBox[{"theta2", "[", "t", "]"}], "2"]}]}]], "Output", CellChangeTimes->{ - 3.647608499924172*^9, 3.64761448705923*^9, 3.647615089616064*^9, - 3.647621228105358*^9, 3.649440521266033*^9, 3.650026330069805*^9, - 3.650026435698052*^9, 3.650113944040901*^9, 3.6501140770340567`*^9, - 3.6501141241886263`*^9, 3.650121181193581*^9, 3.65012546600323*^9, - 3.650126350893875*^9, 3.650126433184325*^9, 3.65012666148414*^9, + 3.647608499924172*^9, 3.64761448705923*^9, 3.647615089616064*^9, + 3.647621228105358*^9, 3.649440521266033*^9, 3.650026330069805*^9, + 3.650026435698052*^9, 3.650113944040901*^9, 3.6501140770340567`*^9, + 3.6501141241886263`*^9, 3.650121181193581*^9, 3.65012546600323*^9, + 3.650126350893875*^9, 3.650126433184325*^9, 3.65012666148414*^9, 3.650127122632825*^9, 3.7096475779352207`*^9, 3.709893468268532*^9, { - 3.709904403476265*^9, 3.709904432440607*^9}, 3.709912186653967*^9, + 3.709904403476265*^9, 3.709904432440607*^9}, 3.709912186653967*^9, 3.70999156637119*^9, 3.710066827966717*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Lag", " ", "=", " ", + RowBox[{"Lag", " ", "=", " ", RowBox[{"T", "-", "V"}]}]], "Input", CellChangeTimes->{{3.647608505756122*^9, 3.6476085105548487`*^9}, { 3.647614570390028*^9, 3.647614570501202*^9}}], @@ -692,626 +692,626 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", - FractionBox["1", "2"]}], " ", "k1", " ", + RowBox[{"-", + FractionBox["1", "2"]}], " ", "k1", " ", SuperscriptBox[ - RowBox[{"theta1", "[", "t", "]"}], "2"]}], "-", + RowBox[{"theta1", "[", "t", "]"}], "2"]}], "-", RowBox[{ - FractionBox["1", "2"], " ", "k2", " ", + FractionBox["1", "2"], " ", "k2", " ", SuperscriptBox[ - RowBox[{"theta2", "[", "t", "]"}], "2"]}], "+", + RowBox[{"theta2", "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "IHub", " ", + FractionBox["1", "2"], " ", "IHub", " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP1", " ", + FractionBox["1", "2"], " ", "ISP1", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP2", " ", + FractionBox["1", "2"], " ", "ISP2", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mHub", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mHub", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}], "2"], "+", SuperscriptBox[ RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}], - "+", + "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}]}]], "Output", CellChangeTimes->{ - 3.647608512913962*^9, 3.647614573588203*^9, 3.647615089649335*^9, - 3.647621228134879*^9, 3.649440526896332*^9, 3.6500263301004267`*^9, - 3.650026435722816*^9, 3.650113791252983*^9, 3.6501139440765142`*^9, - 3.650114077051147*^9, 3.650114124208144*^9, 3.650121181214005*^9, - 3.650125466055393*^9, 3.650126350927471*^9, 3.650126433224345*^9, - 3.650126661504125*^9, 3.650127122654311*^9, 3.709647615366233*^9, - 3.709893471758278*^9, {3.709904407003621*^9, 3.7099044325072527`*^9}, + 3.647608512913962*^9, 3.647614573588203*^9, 3.647615089649335*^9, + 3.647621228134879*^9, 3.649440526896332*^9, 3.6500263301004267`*^9, + 3.650026435722816*^9, 3.650113791252983*^9, 3.6501139440765142`*^9, + 3.650114077051147*^9, 3.650114124208144*^9, 3.650121181214005*^9, + 3.650125466055393*^9, 3.650126350927471*^9, 3.650126433224345*^9, + 3.650126661504125*^9, 3.650127122654311*^9, 3.709647615366233*^9, + 3.709893471758278*^9, {3.709904407003621*^9, 3.7099044325072527`*^9}, 3.709912186722497*^9, 3.7099915664381742`*^9, 3.710066828032853*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq1", " ", "=", " ", + RowBox[{"Eq1", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{ - RowBox[{"xHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"xHub", "[", "t", "]"}]}], "]"}], "-", "Tx"}], " ", "\[Equal]", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{ + RowBox[{"xHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"xHub", "[", "t", "]"}]}], "]"}], "-", "Tx"}], " ", "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.647614579441598*^9, 3.647614600924193*^9}, { - 3.647614866603059*^9, 3.647614887329845*^9}, {3.649440543327643*^9, + 3.647614866603059*^9, 3.647614887329845*^9}, {3.649440543327643*^9, 3.649440562004551*^9}, {3.649440749072403*^9, 3.6494407508768377`*^9}, { - 3.649440951552544*^9, 3.649440961562614*^9}, {3.709647639508596*^9, + 3.649440951552544*^9, 3.649440961562614*^9}, {3.709647639508596*^9, 3.709647646811626*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", "Tx"}], "+", - RowBox[{"mHub", " ", + RowBox[{"-", "Tx"}], "+", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{{3.647614875914831*^9, 3.647614889063602*^9}, - 3.647615089667774*^9, 3.647621228172324*^9, 3.6494405665702753`*^9, - 3.6494407535950603`*^9, 3.649440963907598*^9, 3.650026330119986*^9, - 3.650026435739256*^9, 3.6501137981351843`*^9, 3.6501139441069593`*^9, - 3.650114077086082*^9, 3.650114124242312*^9, 3.650121181242817*^9, - 3.650125466102409*^9, 3.650126350975716*^9, 3.650126433267352*^9, - 3.65012666153375*^9, 3.650127122695442*^9, 3.7096476604132757`*^9, - 3.70989348146432*^9, {3.709904409115304*^9, 3.709904432575056*^9}, + CellChangeTimes->{{3.647614875914831*^9, 3.647614889063602*^9}, + 3.647615089667774*^9, 3.647621228172324*^9, 3.6494405665702753`*^9, + 3.6494407535950603`*^9, 3.649440963907598*^9, 3.650026330119986*^9, + 3.650026435739256*^9, 3.6501137981351843`*^9, 3.6501139441069593`*^9, + 3.650114077086082*^9, 3.650114124242312*^9, 3.650121181242817*^9, + 3.650125466102409*^9, 3.650126350975716*^9, 3.650126433267352*^9, + 3.65012666153375*^9, 3.650127122695442*^9, 3.7096476604132757`*^9, + 3.70989348146432*^9, {3.709904409115304*^9, 3.709904432575056*^9}, 3.7099121867900543`*^9, 3.7099915665063963`*^9, 3.7100668281002493`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq1", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq1", ",", - RowBox[{"{", + RowBox[{"Eq1", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq1", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.647614906346649*^9, 3.647614911027494*^9}, { - 3.649440758351534*^9, 3.649440832552063*^9}, {3.709647671852079*^9, + 3.649440758351534*^9, 3.649440832552063*^9}, {3.709647671852079*^9, 3.7096476989589853`*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"mHub", " ", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "Tx"}]], "Output", CellChangeTimes->{ - 3.647614926214444*^9, 3.647615092999082*^9, 3.6476212284034767`*^9, - 3.649440684330357*^9, 3.649440807189921*^9, 3.6494408416192007`*^9, - 3.649440967306904*^9, 3.6500263368342657`*^9, 3.650026436202402*^9, - 3.6501138061520643`*^9, 3.65011394412993*^9, 3.650114077120083*^9, - 3.650114124276799*^9, 3.6501211812611856`*^9, 3.650125466152321*^9, - 3.6501263510091753`*^9, 3.650126433305233*^9, 3.650126661553743*^9, - 3.650127122727659*^9, 3.709647677516232*^9, 3.7096477099497433`*^9, - 3.709893503042837*^9, {3.7099044268732767`*^9, 3.709904443064567*^9}, + 3.647614926214444*^9, 3.647615092999082*^9, 3.6476212284034767`*^9, + 3.649440684330357*^9, 3.649440807189921*^9, 3.6494408416192007`*^9, + 3.649440967306904*^9, 3.6500263368342657`*^9, 3.650026436202402*^9, + 3.6501138061520643`*^9, 3.65011394412993*^9, 3.650114077120083*^9, + 3.650114124276799*^9, 3.6501211812611856`*^9, 3.650125466152321*^9, + 3.6501263510091753`*^9, 3.650126433305233*^9, 3.650126661553743*^9, + 3.650127122727659*^9, 3.709647677516232*^9, 3.7096477099497433`*^9, + 3.709893503042837*^9, {3.7099044268732767`*^9, 3.709904443064567*^9}, 3.709912195685472*^9, 3.709991574358623*^9, 3.7100668369914494`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq1", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq1", ",", RowBox[{ RowBox[{"xHub", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.6494409813785467`*^9, 3.649440988496232*^9}, { - 3.649441103290543*^9, 3.649441120586372*^9}, {3.70964771655809*^9, + 3.649441103290543*^9, 3.649441120586372*^9}, {3.70964771655809*^9, 3.709647717438199*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", - RowBox[{ - FractionBox["1", - RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], - RowBox[{"(", - RowBox[{"Tx", "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + RowBox[{ + FractionBox["1", + RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], + RowBox[{"(", + RowBox[{"Tx", "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", - CellChangeTimes->{3.650113944169817*^9, 3.650114077139044*^9, - 3.650114124304409*^9, 3.650121181294386*^9, 3.650125466202359*^9, - 3.650126351042288*^9, 3.650126433338122*^9, 3.65012666158496*^9, - 3.650127122765802*^9, 3.709647720541893*^9, 3.70989354325331*^9, - 3.709904443140407*^9, 3.709912195760069*^9, 3.709991574470261*^9, + CellChangeTimes->{3.650113944169817*^9, 3.650114077139044*^9, + 3.650114124304409*^9, 3.650121181294386*^9, 3.650125466202359*^9, + 3.650126351042288*^9, 3.650126433338122*^9, 3.65012666158496*^9, + 3.650127122765802*^9, 3.709647720541893*^9, 3.70989354325331*^9, + 3.709904443140407*^9, 3.709912195760069*^9, 3.709991574470261*^9, 3.710066837095171*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq2", " ", "=", " ", + RowBox[{"Eq2", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{ - RowBox[{"yHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"yHub", "[", "t", "]"}]}], "]"}], "-", "Ty"}], " ", "\[Equal]", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{ + RowBox[{"yHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"yHub", "[", "t", "]"}]}], "]"}], "-", "Ty"}], " ", "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.64944132623348*^9, 3.6494413532739983`*^9}, { 3.7096477376723948`*^9, 3.709647742814459*^9}}], @@ -1319,132 +1319,132 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", "Ty"}], "+", - RowBox[{"mHub", " ", + RowBox[{"-", "Ty"}], "+", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{{3.649441330224839*^9, 3.649441354551085*^9}, - 3.6500263369901237`*^9, 3.650026436270349*^9, 3.650113944223764*^9, - 3.65011407719658*^9, 3.650114124360874*^9, 3.6501211813409653`*^9, - 3.650125466295785*^9, 3.6501263511254063`*^9, 3.6501264334209547`*^9, - 3.6501266616626263`*^9, 3.650127122799155*^9, 3.7096477442006702`*^9, - 3.709893548368627*^9, 3.709904443200683*^9, 3.70991219582679*^9, + CellChangeTimes->{{3.649441330224839*^9, 3.649441354551085*^9}, + 3.6500263369901237`*^9, 3.650026436270349*^9, 3.650113944223764*^9, + 3.65011407719658*^9, 3.650114124360874*^9, 3.6501211813409653`*^9, + 3.650125466295785*^9, 3.6501263511254063`*^9, 3.6501264334209547`*^9, + 3.6501266616626263`*^9, 3.650127122799155*^9, 3.7096477442006702`*^9, + 3.709893548368627*^9, 3.709904443200683*^9, 3.70991219582679*^9, 3.7099915745328817`*^9, 3.7100668371544437`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq2", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq2", ",", - RowBox[{"{", + RowBox[{"Eq2", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq2", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.6494413919573603`*^9, 3.649441395319479*^9}, { 3.709647749463461*^9, 3.7096477703687468`*^9}}], @@ -1452,1008 +1452,1008 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"mHub", " ", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "Ty"}]], "Output", - CellChangeTimes->{{3.649441384353636*^9, 3.649441404905581*^9}, - 3.650026343707588*^9, 3.65002643654138*^9, 3.650113944258304*^9, - 3.650114077232009*^9, 3.6501141243879232`*^9, 3.650121181377021*^9, - 3.6501254663447037`*^9, 3.650126351158494*^9, 3.6501264334502573`*^9, - 3.6501266616822643`*^9, 3.650127122819181*^9, 3.709647784487842*^9, - 3.709893574056983*^9, 3.7099044610384007`*^9, 3.709912208124202*^9, + CellChangeTimes->{{3.649441384353636*^9, 3.649441404905581*^9}, + 3.650026343707588*^9, 3.65002643654138*^9, 3.650113944258304*^9, + 3.650114077232009*^9, 3.6501141243879232`*^9, 3.650121181377021*^9, + 3.6501254663447037`*^9, 3.650126351158494*^9, 3.6501264334502573`*^9, + 3.6501266616822643`*^9, 3.650127122819181*^9, 3.709647784487842*^9, + 3.709893574056983*^9, 3.7099044610384007`*^9, 3.709912208124202*^9, 3.7099915860370693`*^9, 3.710066849681682*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq2", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq2", ",", RowBox[{ RowBox[{"yHub", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.649441467695437*^9, 3.649441470984462*^9}, { 3.709647777488635*^9, 3.709647783103689*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", - RowBox[{ - FractionBox["1", - RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], - RowBox[{"(", - RowBox[{"Ty", "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + RowBox[{ + FractionBox["1", + RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], + RowBox[{"(", + RowBox[{"Ty", "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", - CellChangeTimes->{3.650113944303441*^9, 3.650114077264604*^9, - 3.650114124408894*^9, 3.65012118141026*^9, 3.650125466395176*^9, - 3.650126351191698*^9, 3.650126433471068*^9, 3.650126661717382*^9, - 3.650127122848145*^9, 3.7096477862767563`*^9, 3.709893604933693*^9, - 3.709904461095764*^9, 3.709912208203537*^9, 3.70999158617177*^9, + CellChangeTimes->{3.650113944303441*^9, 3.650114077264604*^9, + 3.650114124408894*^9, 3.65012118141026*^9, 3.650125466395176*^9, + 3.650126351191698*^9, 3.650126433471068*^9, 3.650126661717382*^9, + 3.650127122848145*^9, 3.7096477862767563`*^9, 3.709893604933693*^9, + 3.709904461095764*^9, 3.709912208203537*^9, 3.70999158617177*^9, 3.71006684977703*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq3", " ", "=", " ", + RowBox[{"Eq3", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", "-", " ", "Torque"}], + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{ + RowBox[{"theta", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", "-", " ", "Torque"}], " ", "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.649441490019846*^9, 3.649441526048011*^9}, { - 3.6494416329455757`*^9, 3.649441639953199*^9}, {3.709647999226737*^9, + 3.6494416329455757`*^9, 3.649441639953199*^9}, {3.709647999226737*^9, 3.709648001657591*^9}, {3.7098936614514217`*^9, 3.709893666760483*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", "Torque"}], "-", + RowBox[{"-", "Torque"}], "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "-", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", - RowBox[{"IHub", " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{"IHub", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"ISP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"ISP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"ISP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"ISP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{3.6494416430996*^9, 3.650026343837957*^9, - 3.6500264366053953`*^9, 3.6501139443755608`*^9, 3.6501140773356533`*^9, - 3.650114124465275*^9, 3.650121181475136*^9, 3.650125466495*^9, - 3.6501263512510633`*^9, 3.6501264335329323`*^9, 3.6501266617780237`*^9, - 3.650127122873707*^9, 3.7096478122779713`*^9, 3.709648036046701*^9, - 3.7098936714787397`*^9, 3.7099044611434526`*^9, 3.7099122082757*^9, + CellChangeTimes->{3.6494416430996*^9, 3.650026343837957*^9, + 3.6500264366053953`*^9, 3.6501139443755608`*^9, 3.6501140773356533`*^9, + 3.650114124465275*^9, 3.650121181475136*^9, 3.650125466495*^9, + 3.6501263512510633`*^9, 3.6501264335329323`*^9, 3.6501266617780237`*^9, + 3.650127122873707*^9, 3.7096478122779713`*^9, 3.709648036046701*^9, + 3.7098936714787397`*^9, 3.7099044611434526`*^9, 3.7099122082757*^9, 3.709991586226976*^9, 3.710066849868989*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq3", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq3", ",", - RowBox[{"{", + RowBox[{"Eq3", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq3", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.649441701396606*^9, 3.649441704434289*^9}, { 3.709647818203969*^9, 3.709647839090629*^9}}], @@ -2461,687 +2461,687 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"IHub", "+", "ISP1", "+", "ISP2"}], ")"}], " ", + RowBox[{"(", + RowBox[{"IHub", "+", "ISP1", "+", "ISP2"}], ")"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - SuperscriptBox["Rhinge1", "2"], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + SuperscriptBox["Rhinge1", "2"], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP2", " ", - SuperscriptBox["Rhinge2", "2"], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP2", " ", + SuperscriptBox["Rhinge2", "2"], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"ISP1", " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"ISP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"ISP2", " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"ISP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", - RowBox[{"Torque", "+", + MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"Torque", "+", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}]}]], "Output", - CellChangeTimes->{3.649441711039947*^9, 3.650026348595045*^9, - 3.650026437159946*^9, 3.6501139490906878`*^9, 3.65011408062339*^9, - 3.650114124719529*^9, 3.650121181507895*^9, 3.650125466545499*^9, - 3.650126351285775*^9, 3.650126434062338*^9, 3.650126661823897*^9, - 3.650127122910791*^9, 3.709647853944254*^9, 3.70964806140898*^9, - 3.709893706843128*^9, 3.709904495809168*^9, 3.70991222827071*^9, + CellChangeTimes->{3.649441711039947*^9, 3.650026348595045*^9, + 3.650026437159946*^9, 3.6501139490906878`*^9, 3.65011408062339*^9, + 3.650114124719529*^9, 3.650121181507895*^9, 3.650125466545499*^9, + 3.650126351285775*^9, 3.650126434062338*^9, 3.650126661823897*^9, + 3.650127122910791*^9, 3.709647853944254*^9, 3.70964806140898*^9, + 3.709893706843128*^9, 3.709904495809168*^9, 3.70991222827071*^9, 3.7099916041330023`*^9, 3.710066868405623*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq3", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq3", ",", RowBox[{ RowBox[{"theta", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.649441744480874*^9, 3.649441755989689*^9}, { 3.7098939348877172`*^9, 3.709893936213263*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{ - RowBox[{"(", - RowBox[{"Torque", "-", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{"Torque", "-", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"ISP1", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"ISP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"ISP2", " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"ISP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}], "/", - RowBox[{"(", - RowBox[{"IHub", "+", "ISP1", "+", "ISP2", "+", + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}], "/", + RowBox[{"(", + RowBox[{"IHub", "+", "ISP1", "+", "ISP2", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", + SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", - RowBox[{"mSP1", " ", - SuperscriptBox["Rhinge1", "2"]}], "+", - RowBox[{"mSP2", " ", - SuperscriptBox["Rhinge2", "2"]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}]}]}], "}"}], + SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", + RowBox[{"mSP1", " ", + SuperscriptBox["Rhinge1", "2"]}], "+", + RowBox[{"mSP2", " ", + SuperscriptBox["Rhinge2", "2"]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", CellChangeTimes->{ - 3.650113949170033*^9, 3.650114080665533*^9, 3.650114124738514*^9, - 3.650121181527328*^9, 3.6501254665943823`*^9, 3.6501263513202057`*^9, - 3.65012643412017*^9, 3.650126661855892*^9, 3.650127122950385*^9, - 3.709647869432714*^9, 3.70964807076822*^9, {3.70989392998943*^9, - 3.709893938934258*^9}, 3.709904495937688*^9, 3.7099122283751574`*^9, + 3.650113949170033*^9, 3.650114080665533*^9, 3.650114124738514*^9, + 3.650121181527328*^9, 3.6501254665943823`*^9, 3.6501263513202057`*^9, + 3.65012643412017*^9, 3.650126661855892*^9, 3.650127122950385*^9, + 3.709647869432714*^9, 3.70964807076822*^9, {3.70989392998943*^9, + 3.709893938934258*^9}, 3.709904495937688*^9, 3.7099122283751574`*^9, 3.7099916042609997`*^9, 3.710066868501666*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq4", " ", "=", " ", + RowBox[{"Eq4", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", + RowBox[{"D", "[", + RowBox[{"Lag", ",", RowBox[{ RowBox[{"theta1", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", "+", - RowBox[{"c1", "*", + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", "+", + RowBox[{"c1", "*", RowBox[{ - RowBox[{"theta1", "'"}], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"theta1", "'"}], "[", "t", "]"}]}]}], "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.649441857154335*^9, 3.6494418688045387`*^9}, { - 3.649442193911736*^9, 3.649442218793474*^9}, {3.650026298715227*^9, + 3.649442193911736*^9, 3.649442218793474*^9}, {3.650026298715227*^9, 3.650026299976062*^9}, {3.7096481588067293`*^9, 3.709648159022726*^9}, { 3.709893944199317*^9, 3.7098939571022673`*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k1", " ", - RowBox[{"theta1", "[", "t", "]"}]}], "+", - RowBox[{"c1", " ", + RowBox[{"k1", " ", + RowBox[{"theta1", "[", "t", "]"}]}], "+", + RowBox[{"c1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", - RowBox[{"ISP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{"ISP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{3.6494418700740137`*^9, 3.6494422226433496`*^9, - 3.6500263034503527`*^9, 3.650026348745247*^9, 3.650026437210599*^9, - 3.650113949253563*^9, 3.6501140807222767`*^9, 3.650114124804435*^9, - 3.6501211815963173`*^9, 3.650125466805504*^9, 3.650126351387097*^9, - 3.650126434183649*^9, 3.650126661922491*^9, 3.650127122999509*^9, - 3.709648163644412*^9, 3.709893959183962*^9, 3.709904495992156*^9, + CellChangeTimes->{3.6494418700740137`*^9, 3.6494422226433496`*^9, + 3.6500263034503527`*^9, 3.650026348745247*^9, 3.650026437210599*^9, + 3.650113949253563*^9, 3.6501140807222767`*^9, 3.650114124804435*^9, + 3.6501211815963173`*^9, 3.650125466805504*^9, 3.650126351387097*^9, + 3.650126434183649*^9, 3.650126661922491*^9, 3.650127122999509*^9, + 3.709648163644412*^9, 3.709893959183962*^9, 3.709904495992156*^9, 3.709912228430462*^9, 3.709991604325783*^9, 3.710066868562544*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq4", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq4", ",", - RowBox[{"{", + RowBox[{"Eq4", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq4", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.649441900454019*^9, 3.6494419029884357`*^9}, { 3.7096481750058613`*^9, 3.709648196975309*^9}}], @@ -3149,489 +3149,489 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k1", " ", - RowBox[{"theta1", "[", "t", "]"}]}], "+", - RowBox[{"c1", " ", + RowBox[{"k1", " ", + RowBox[{"theta1", "[", "t", "]"}]}], "+", + RowBox[{"c1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP1", "+", + RowBox[{"(", + RowBox[{"ISP1", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP1", "+", + RowBox[{"(", + RowBox[{"ISP1", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}]}], ")"}], " ", + SuperscriptBox["d1", "2"], " ", "mSP1"}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}]], "Output", - CellChangeTimes->{3.649441907035841*^9, 3.649442235781934*^9, - 3.650026351853695*^9, 3.650026437327095*^9, 3.6501139515691*^9, - 3.650114081436472*^9, 3.650114125027214*^9, 3.650121181630754*^9, - 3.6501254668447866`*^9, 3.650126351421269*^9, 3.6501264342223673`*^9, - 3.650126662116922*^9, 3.650127123045363*^9, 3.709648349946191*^9, - 3.7098939733623323`*^9, 3.709904501105431*^9, 3.70991223188218*^9, + CellChangeTimes->{3.649441907035841*^9, 3.649442235781934*^9, + 3.650026351853695*^9, 3.650026437327095*^9, 3.6501139515691*^9, + 3.650114081436472*^9, 3.650114125027214*^9, 3.650121181630754*^9, + 3.6501254668447866`*^9, 3.650126351421269*^9, 3.6501264342223673`*^9, + 3.650126662116922*^9, 3.650127123045363*^9, 3.709648349946191*^9, + 3.7098939733623323`*^9, 3.709904501105431*^9, 3.70991223188218*^9, 3.709991607844681*^9, 3.710066872120063*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq4", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq4", ",", RowBox[{ RowBox[{"theta1", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.649441926418256*^9, 3.64944194178972*^9}, { 3.7098939782495117`*^9, 3.7098939802637053`*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{ - FractionBox["1", - RowBox[{"ISP1", "+", + FractionBox["1", + RowBox[{"ISP1", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}]}]], - RowBox[{"(", + SuperscriptBox["d1", "2"], " ", "mSP1"}]}]], + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "k1"}], " ", - RowBox[{"theta1", "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "k1"}], " ", + RowBox[{"theta1", "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"c1", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"c1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"ISP1", " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"ISP1", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", - CellChangeTimes->{3.6501139516163483`*^9, 3.6501140814531193`*^9, - 3.6501141250632668`*^9, 3.650121181649961*^9, 3.650125466894619*^9, - 3.650126351454706*^9, 3.6501264342548857`*^9, 3.650126662157115*^9, - 3.650127123079134*^9, 3.709648353498505*^9, 3.709893982607676*^9, - 3.709904501156394*^9, 3.7099122319746513`*^9, 3.709991607949237*^9, + CellChangeTimes->{3.6501139516163483`*^9, 3.6501140814531193`*^9, + 3.6501141250632668`*^9, 3.650121181649961*^9, 3.650125466894619*^9, + 3.650126351454706*^9, 3.6501264342548857`*^9, 3.650126662157115*^9, + 3.650127123079134*^9, 3.709648353498505*^9, 3.709893982607676*^9, + 3.709904501156394*^9, 3.7099122319746513`*^9, 3.709991607949237*^9, 3.7100668721947823`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq5", " ", "=", " ", + RowBox[{"Eq5", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", + RowBox[{"D", "[", + RowBox[{"Lag", ",", RowBox[{ RowBox[{"theta2", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", "+", - RowBox[{"c2", "*", + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", "+", + RowBox[{"c2", "*", RowBox[{ - RowBox[{"theta2", "'"}], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"theta2", "'"}], "[", "t", "]"}]}]}], "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.649441974232287*^9, 3.649441974602599*^9}, { - 3.649442024415121*^9, 3.6494420297328167`*^9}, {3.6494422687274637`*^9, + 3.649442024415121*^9, 3.6494420297328167`*^9}, {3.6494422687274637`*^9, 3.649442279760858*^9}, {3.650026425804619*^9, 3.650026427138609*^9}, { - 3.7096483649009*^9, 3.709648365220821*^9}, {3.709893986529037*^9, + 3.7096483649009*^9, 3.709648365220821*^9}, {3.709893986529037*^9, 3.7098939986630163`*^9}, {3.709894594889039*^9, 3.7098945981436768`*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k2", " ", - RowBox[{"theta2", "[", "t", "]"}]}], "+", - RowBox[{"c2", " ", + RowBox[{"k2", " ", + RowBox[{"theta2", "[", "t", "]"}]}], "+", + RowBox[{"c2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", - RowBox[{"ISP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{"ISP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", CellChangeTimes->{ 3.649442031278104*^9, 3.649442281353739*^9, 3.650026351903562*^9, { - 3.650026428718976*^9, 3.650026437375152*^9}, 3.650113951691698*^9, - 3.650114081506241*^9, 3.650114125109868*^9, 3.6501211816981277`*^9, - 3.650125466995118*^9, 3.650126351524879*^9, 3.6501264343228188`*^9, - 3.650126662222809*^9, 3.650127123116081*^9, 3.709648368112361*^9, - 3.7098946055579844`*^9, 3.7099045012015133`*^9, 3.709912232028257*^9, + 3.650026428718976*^9, 3.650026437375152*^9}, 3.650113951691698*^9, + 3.650114081506241*^9, 3.650114125109868*^9, 3.6501211816981277`*^9, + 3.650125466995118*^9, 3.650126351524879*^9, 3.6501264343228188`*^9, + 3.650126662222809*^9, 3.650127123116081*^9, 3.709648368112361*^9, + 3.7098946055579844`*^9, 3.7099045012015133`*^9, 3.709912232028257*^9, 3.709991608051722*^9, 3.710066872251883*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq5", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq5", ",", - RowBox[{"{", + RowBox[{"Eq5", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq5", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.649442050675033*^9, 3.6494420532928762`*^9}, { 3.709648373091796*^9, 3.7096483936980247`*^9}}], @@ -3639,146 +3639,146 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k2", " ", - RowBox[{"theta2", "[", "t", "]"}]}], "+", - RowBox[{"c2", " ", + RowBox[{"k2", " ", + RowBox[{"theta2", "[", "t", "]"}]}], "+", + RowBox[{"c2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP2", "+", + RowBox[{"(", + RowBox[{"ISP2", "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP2", "+", + RowBox[{"(", + RowBox[{"ISP2", "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}]}], ")"}], " ", + SuperscriptBox["d2", "2"], " ", "mSP2"}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}]], "Output", - CellChangeTimes->{3.649442057869686*^9, 3.649442290890633*^9, - 3.650026354551691*^9, 3.650026439581772*^9, 3.650113954518001*^9, - 3.650114081534142*^9, 3.650114125129826*^9, 3.650121181727723*^9, - 3.650125467045177*^9, 3.6501263515581093`*^9, 3.650126435015355*^9, - 3.650126662243575*^9, 3.650127123150807*^9, 3.7096483991918497`*^9, - 3.709894614262093*^9, 3.709904506228737*^9, 3.7099122352551403`*^9, + CellChangeTimes->{3.649442057869686*^9, 3.649442290890633*^9, + 3.650026354551691*^9, 3.650026439581772*^9, 3.650113954518001*^9, + 3.650114081534142*^9, 3.650114125129826*^9, 3.650121181727723*^9, + 3.650125467045177*^9, 3.6501263515581093`*^9, 3.650126435015355*^9, + 3.650126662243575*^9, 3.650127123150807*^9, 3.7096483991918497`*^9, + 3.709894614262093*^9, 3.709904506228737*^9, 3.7099122352551403`*^9, 3.709991611016552*^9, 3.7100668766011467`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq5", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq5", ",", RowBox[{ RowBox[{"theta2", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.6494421764678802`*^9, 3.649442183039885*^9}, { 3.709894635218473*^9, 3.7098946388520184`*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{ - FractionBox["1", - RowBox[{"ISP2", "+", + FractionBox["1", + RowBox[{"ISP2", "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}]}]], - RowBox[{"(", + SuperscriptBox["d2", "2"], " ", "mSP2"}]}]], + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "k2"}], " ", - RowBox[{"theta2", "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "k2"}], " ", + RowBox[{"theta2", "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"c2", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"c2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"ISP2", " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"ISP2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", CellChangeTimes->{ - 3.650113954545787*^9, 3.650114081552614*^9, 3.65011412516045*^9, - 3.650121181746997*^9, 3.6501254670958147`*^9, 3.6501263515786343`*^9, - 3.650126435034376*^9, 3.650126662274653*^9, 3.6501271231820707`*^9, - 3.709648402930709*^9, {3.709894632465825*^9, 3.709894639761241*^9}, - 3.709904506291833*^9, 3.709912235339995*^9, 3.7099916111335163`*^9, + 3.650113954545787*^9, 3.650114081552614*^9, 3.65011412516045*^9, + 3.650121181746997*^9, 3.6501254670958147`*^9, 3.6501263515786343`*^9, + 3.650126435034376*^9, 3.650126662274653*^9, 3.6501271231820707`*^9, + 3.709648402930709*^9, {3.709894632465825*^9, 3.709894639761241*^9}, + 3.709904506291833*^9, 3.709912235339995*^9, 3.7099916111335163`*^9, 3.7100668766653957`*^9}] }, Open ]] }, Open ]] @@ -3911,4 +3911,3 @@ Cell[139379, 3713, 2762, 68, 197, "Output"] } ] *) - diff --git a/src/simulation/dynamics/FuelTank/_Documentation/bibliography.bib b/src/simulation/dynamics/FuelTank/_Documentation/bibliography.bib index 5148f610ab..a7ab1aefa8 100755 --- a/src/simulation/dynamics/FuelTank/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/FuelTank/_Documentation/bibliography.bib @@ -16,7 +16,7 @@ @book{schaub Publisher = {{AIAA} Education Series}, Title = {Analytical Mechanics of Space Systems}, Year = {2014}} - + @conference{Panicucci:2017fu, Address = {Breckenridge, {CO}}, Author = {Paolo Panicucci and Cody Allard and Hanspeter Schaub}, @@ -24,4 +24,4 @@ @conference{Panicucci:2017fu Month = {Feb. 2--8}, Note = {{P}aper AAS~17-011}, Title = {Spacecraft Dynamics Employing a General Multi-tank and Multi-thruster Mass Depletion Formulation}, - Year = {2017}} \ No newline at end of file + Year = {2017}} diff --git a/src/simulation/dynamics/FuelTank/_Documentation/secModelDescription.tex b/src/simulation/dynamics/FuelTank/_Documentation/secModelDescription.tex index dbf0f088b9..3bb843443c 100644 --- a/src/simulation/dynamics/FuelTank/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/FuelTank/_Documentation/secModelDescription.tex @@ -11,7 +11,7 @@ \subsection{Problem Statement} \end{figure} The following derivation is a shortened version from Reference\cite{Panicucci:2017fu}. To help define the problem, Figure~\ref{fig:Spacecraft} is displayed. This problem involves a spacecraft consisting of a hub which is a rigid body and has a center of mass location labeled as point $B_c$. The hub has $M$ number of tanks and $N$ number of thrusters attached to it. The figure only shows one tank and one thruster but the analytical development is general. The $i_{\text{th}}$ tank has a center of mass location labeled as $F_{c_i}$ and the $j_{\text{th}}$ thruster is located at $B_j$. The body fixed reference frame $\mathcal{B}$: $\{\bm{\hat{b}}_1\,, \bm{\hat{b}}_2\,,\bm{\hat{b}}_3 \}$ with origin $B$ can be oriented in any direction and point $B$ can be located anywhere fixed to the hub. This means that point $B$ and the center of mass location of the spacecraft, $C$, are not necessarily coincident. As a result, the vector $\bm c$ defines the vector pointing from the body frame origin to the center of mass fo the spacecraft. The inertial reference frame $\mathcal{N}$: $\{\bm{\hat{n}}_1\,, \bm{\hat{n}}_2\,,\bm{\hat{n}}_3 \}$ is centered at $N$ and is fixed in inertial space. - + Throughout this paper, vector calculus is used and the notation to define certain quantities needs to be introduced. A position vector, $ \bm{r}_{C/N}$, is the vector pointing from $N$ to $C$. $\bm{\omega}_{\mathcal{B}/\mathcal{N}}$ is the angular velocity of the $\mathcal{B}$ frame with respect to the $\mathcal{N}$ frame. $\dot{\bm{r}}$ denotes an inertial time derivate of vector $\bm{r}$ and $\bm{r}'$ defines a time derivate of $\bm{r}$ with respect to the body frame. Using these definitions, the following describes the Reynolds transport theorem used in this formulation. @@ -60,7 +60,7 @@ \subsubsection{Translational Equation of Motion} \end{equation} where $\dot{\bm{r}}_{M/N}$ is the velocity of the particle at the $M$ point expressed with respect to the inertial reference frame and $\bm{F}_{\text{ext}}$ is the sum of the external forces experienced by the body. -As the total mass of the system is constant, the differentiation operator is brought inside the integration: +As the total mass of the system is constant, the differentiation operator is brought inside the integration: \begin{equation}\label{eq:eq4} \frac{^{\mathcal{N}}\text{d}}{\text{d}\,t}\int_{\text{Body}}\dot{\bm{r}}_{M/N}\, \text{d}m= \int_{\text{Body}}\ddot{\bm{r}}_{M/N}\,\text{d}m \end{equation} @@ -79,7 +79,7 @@ \subsubsection{Translational Equation of Motion} \bm{r}'_{M/B} + \dot{\bm{\omega}}_{\cal{B}/\cal{N}}\times \bm{r}_{M/B} + \bm{\omega}_{\cal{B}/\cal{N}}\times\left(\bm{\omega}_{\cal{B}/\cal{N}} \times \bm{r}_{B/M} \right) \end{equation} -A Lagrangian formulation of the linear momentum equation is deduced by using Equations \eqref{eq:eq3}, \eqref{eq:eq4}, \eqref{eq:RcRbacc} and \eqref{eq:rMB_dot}: +A Lagrangian formulation of the linear momentum equation is deduced by using Equations \eqref{eq:eq3}, \eqref{eq:eq4}, \eqref{eq:RcRbacc} and \eqref{eq:rMB_dot}: \begin{multline}\label{eq:eq11} \int_{\text{Body}}\left(\ddot{\bm{r}}_{B/N} + \dot{\bm{\omega}}_{\mathcal{B}/\mathcal{N}}\times\bm{r}_{M/B} + \bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\left(\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\bm{r}_{M/B}\right)\right)\text{d}m +\\+ @@ -88,7 +88,7 @@ \subsubsection{Translational Equation of Motion} The system mass is constant, therefore the derivative operator can be applied after the integration. This yields: -\begin{equation} +\begin{equation} \int_{\text{Body}}\bm{r}'_{M/B}\text{d}m = \frac{^{\mathcal{B}}\text{d}}{\text{d}t}\int_{\text{Body}}\bm{r}_{M/B}\text{d}m \end{equation} \begin{equation} @@ -108,21 +108,21 @@ \subsubsection{Translational Equation of Motion} \begin{multline} \int_{\text{Body}}\left(\ddot{\bm{r}}_{B/N} + \dot{\bm{\omega}}_{\mathcal{B}/\mathcal{N}}\times\bm{r}_{M/B} + \bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\left(\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\bm{r}_{M/B}\right)\right)\text{d}m +\\+ - 2\,\bm{\omega}_{\mathcal{B}/\mathcal{N}}\times\left(\,\frac{^{\mathcal{B}} \text{d}}{\text{d}t}\int_{\mathcal{V}_{\text{sc}}} \rho\,\bm{r}_{M/B}\,\text{d}\mathcal{V} + \int_{\mathcal{A}_{\text{sc}}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}}\,\bm{r}_{M/B}\text{d}A\right) \;+ \frac{^{\mathcal{B}} \text{d}^2}{\text{d}t^2}\int_{\mathcal{V}_{\text{sc}}} \rho\,\bm{r}_{M/B} \,\text{d}\mathcal{V} +\\+ \frac{^{\mathcal{B}} \text{d}}{\text{d}t} \int_{\mathcal{A}_\text{sc}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}} \,\bm{r}_{M/B}\,\text{d}A + \int_{\mathcal{A}_\text{sc}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}} \,\bm{r}'_{M/B}\,\text{d}A + 2\,\bm{\omega}_{\mathcal{B}/\mathcal{N}}\times\left(\,\frac{^{\mathcal{B}} \text{d}}{\text{d}t}\int_{\mathcal{V}_{\text{sc}}} \rho\,\bm{r}_{M/B}\,\text{d}\mathcal{V} + \int_{\mathcal{A}_{\text{sc}}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}}\,\bm{r}_{M/B}\text{d}A\right) \;+ \frac{^{\mathcal{B}} \text{d}^2}{\text{d}t^2}\int_{\mathcal{V}_{\text{sc}}} \rho\,\bm{r}_{M/B} \,\text{d}\mathcal{V} +\\+ \frac{^{\mathcal{B}} \text{d}}{\text{d}t} \int_{\mathcal{A}_\text{sc}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}} \,\bm{r}_{M/B}\,\text{d}A + \int_{\mathcal{A}_\text{sc}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}} \,\bm{r}'_{M/B}\,\text{d}A = \bm{F}_{\text{ext}} -\end{multline} +\end{multline} As explained in previous work, if all of the mass is contained in the control volume at the initial time, then a particular relation results because no mass is outside the control volume at $t=0$ and the dynamic quantities will be transported out during the integration. This relationship is quantified in the following equation: \begin{multline} - \bm{F}_{\text{ext}} - \int_{\text{Body}}\left(\ddot{\bm{r}}_{B/N} + \dot{\bm{\omega}}_{\mathcal{B}/\mathcal{N}}\times\bm{r}_{M/B} + \bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\left(\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\bm{r}_{M/B}\right)\right)\text{d}m + \bm{F}_{\text{ext}} - \int_{\text{Body}}\left(\ddot{\bm{r}}_{B/N} + \dot{\bm{\omega}}_{\mathcal{B}/\mathcal{N}}\times\bm{r}_{M/B} + \bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\left(\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\bm{r}_{M/B}\right)\right)\text{d}m = \int_{\mathcal{V}_{\text{sc}}} \text{d}\bm{F}_{\text{vol}}+\\ +\int_{\mathcal{A}_{\text{sc}}} \text{d}\bm{F}_{\text{surf}} - \int_{\mathcal{V}_{\text{sc}}}\rho\,\left(\ddot{\bm{r}}_{B/N} + \dot{\bm{\omega}}_{\mathcal{B}/\mathcal{N}}\times\bm{r}_{M/B} + \bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\left(\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\bm{r}_{M/B}\right)\right)\text{d}\mathcal{V} -\end{multline} +\end{multline} \noindent where the forces are divided into volumetric forces and the forces applied on the spacecraft surface. Rearranging this result, replacing the definition of $\bm{F}_{\text{ext}}$, and isolating the forces to the right hand side of the equation yields: \begin{multline}\label{eq:ComplTranslEOM} \int_{\mathcal{V}_{\text{sc}}}\rho\,\left(\ddot{\bm{r}}_{B/N} + \dot{\bm{\omega}}_{\mathcal{B}/\mathcal{N}}\times\bm{r}_{M/B} + \bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\left(\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\bm{r}_{M/B}\right)\right)\text{d}\mathcal{V} +\\+ - 2\,\bm{\omega}_{\mathcal{B}/\mathcal{N}}\times\left(\,\frac{^{\mathcal{B}} \text{d}}{\text{d}t}\int_{\mathcal{V}_{\text{sc}}} \rho\,\bm{r}_{M/B}\,\text{d}\mathcal{V}\; + \int_{\mathcal{A}_{\text{sc}}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}}\,\bm{r}_{M/B}\text{d}A\right) \;+ \frac{^{\mathcal{B}} \text{d}^2}{\text{d}t^2}\int_{\mathcal{V}_{\text{sc}}} \rho\,\bm{r}_{M/B} \,\text{d}\mathcal{V} +\\+ \frac{^{\mathcal{B}} \text{d}}{\text{d}t} \int_{\mathcal{A}_\text{sc}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}} \,\bm{r}_{M/B}\,\text{d}A + \int_{\mathcal{A}_\text{sc}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}} \,\bm{r}'_{M/B}\,\text{d}A + 2\,\bm{\omega}_{\mathcal{B}/\mathcal{N}}\times\left(\,\frac{^{\mathcal{B}} \text{d}}{\text{d}t}\int_{\mathcal{V}_{\text{sc}}} \rho\,\bm{r}_{M/B}\,\text{d}\mathcal{V}\; + \int_{\mathcal{A}_{\text{sc}}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}}\,\bm{r}_{M/B}\text{d}A\right) \;+ \frac{^{\mathcal{B}} \text{d}^2}{\text{d}t^2}\int_{\mathcal{V}_{\text{sc}}} \rho\,\bm{r}_{M/B} \,\text{d}\mathcal{V} +\\+ \frac{^{\mathcal{B}} \text{d}}{\text{d}t} \int_{\mathcal{A}_\text{sc}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}} \,\bm{r}_{M/B}\,\text{d}A + \int_{\mathcal{A}_\text{sc}} \rho\,\bm{r}'_{M/B}\cdot\bm{\hat{n}} \,\bm{r}'_{M/B}\,\text{d}A = \int_{\mathcal{V}_{\text{sc}}} \text{d}\bm{F}_{\text{vol}} +\int_{\mathcal{A}_{\text{sc}}} \text{d}\bm{F}_{\text{surf}} \end{multline} @@ -150,9 +150,9 @@ \subsubsection{Translational Equation of Motion} \begin{multline}\label{eq:eq19} \int_{\mathcal{V}_{\text{sc}}}\rho\,\left(\ddot{\bm{r}}_{B/N} + \dot{\bm{\omega}}_{\mathcal{B}/\mathcal{N}}\times\bm{r}_{M/B} + \bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\left(\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\bm{r}_{M/B}\right)\right)\text{d}\mathcal{V} = \\= - m_{\text{sc}} \,\bm{\ddot{r}}_{B/N} + m_{\text{sc}}\,\bm{\dot{\omega}}_{\mathcal{B}/\mathcal{N}} \times\bm{c} + m_{\text{sc}}\,\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\left(\bm{\omega}_{\mathcal{B}/\mathcal{N}}\times\bm{c}\right) + m_{\text{sc}} \,\bm{\ddot{r}}_{B/N} + m_{\text{sc}}\,\bm{\dot{\omega}}_{\mathcal{B}/\mathcal{N}} \times\bm{c} + m_{\text{sc}}\,\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times\left(\bm{\omega}_{\mathcal{B}/\mathcal{N}}\times\bm{c}\right) \end{multline} -\noindent +\noindent where $m_{\text{sc}}=m_{\text{hub}}+ \sum_{i=1}^{M}m_{\text{fuel}_i}$ is the instantaneous mass of the spacecraft. The second and fourth integrals are computed and yield: \begin{equation}\label{eq:eq20} @@ -205,9 +205,9 @@ \subsubsection{Translational Equation of Motion} \begin{multline}\label{eq:eq29} \ddot{\bm{r}}_{B/N} + \left[\tilde{\bm{c}}\right]^T\dot{\bm{\omega}}_{\cal{B}/\cal{N}} = \frac{\bm{F}_{\text{thr}}}{m_{\text{sc}}} - 2\,\frac{\dot{m}_{\text{fuel}}}{m_{\text{sc}}}\,\left(c' + \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right]\times\bm{c}\right) - \bm{c}''+2\,\left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right]^T \bm{c}'\\- \ddot{m}_{\text{fuel}}\,\bm{c} + \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right]^T \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right] \bm{c} +\frac{2}{m_{\text{sc}}}\sum_{j=1}^{N}\dot{m}_{\text{noz}_j} \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right] \bm{r}_{N_j/B} - \\+ \frac{ 1}{m_{\text{sc}}} \sum_{j=1}^{N}\ddot{m}_{\text{noz}_j}\bm{r}_{Fc_j/B} +\frac{\bm{F}_{\text{ext, vol}}}{m_{\text{sc}}} + \frac{\bm{F}_{\text{ext, surf}}}{m_{\text{sc}}} + \\+ \frac{ 1}{m_{\text{sc}}} \sum_{j=1}^{N}\ddot{m}_{\text{noz}_j}\bm{r}_{Fc_j/B} +\frac{\bm{F}_{\text{ext, vol}}}{m_{\text{sc}}} + \frac{\bm{F}_{\text{ext, surf}}}{m_{\text{sc}}} \end{multline} -Multiplying my +Multiplying my \begin{multline}\label{eq:eq30} m_{\text{sc}} \ddot{\bm{r}}_{B/N} - m_{\text{sc}} \left[\tilde{\bm{c}}\right]\dot{\bm{\omega}}_{\cal{B}/\cal{N}} = \bm{F}_{\text{thr}} - 2\,\dot{m}_{\text{fuel}} \left(c' + \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right]\times\bm{c}\right) - m_{\text{sc}} \bm{c}''-2 m_{\text{sc}} \,\left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right] @@ -237,7 +237,7 @@ \subsubsection{Rotational Equation of Motion} Knowing that $\ddot{\bm{r}}_{M/N}\,\text{\text{d}}m=\text{d}\bm{F}$, the torque caused by the forces acting on the body is easily defined: \begin{equation} - \int_{\text{Body}}\bm{r}_{M/N} \times \text{d}\bm{F}-\int_{\text{Body}} \rho\,\bm{r}_{B/N} \times \ddot{\bm{r}}_{M/N}\,\text{d}\mathcal{V} + \int_{\text{Body}}\bm{r}_{M/N} \times \text{d}\bm{F}-\int_{\text{Body}} \rho\,\bm{r}_{B/N} \times \ddot{\bm{r}}_{M/N}\,\text{d}\mathcal{V} = \int_{\text{Body}} \left(\bm{r}_{M/N}-\bm{r}_{B/N}\right) \times \text{d}\bm{F} = \bm{L}_{B} \end{equation} \noindent @@ -247,7 +247,7 @@ \subsubsection{Rotational Equation of Motion} \begin{multline} \int_{\text{Body}} \rho\,\bm{r}_{M/B} \times \ddot{\bm{r}}_{M/B}\,\text{d}\mathcal{V} = \frac{^\mathcal{N}\text{d}}{\text{d}t} \int_{\mathcal{V}_{\text{sc}}} \rho\,\bm{r}_{M/B} \times \dot{\bm{r}}_{M/B}\,\text{d}\mathcal{V} + \int_{\mathcal{A}_{\text{sc}}} \rho\,\bm{r}'_{M/B} \cdot\bm{\hat{n}} \left(\bm{r}_{M/B} \times \dot{\bm{r}}_{M/B}\right)\text{d}A -\end{multline} +\end{multline} Moreover, similar to the translational equation, if all the mass of the system is assumed to be contained inside the control volume at the initial time, the following relationship results: @@ -268,8 +268,8 @@ \subsubsection{Rotational Equation of Motion} where $\left[I_{\text{hub},\,B_c}\right]$ is the hub's inertia about its center of mass, $B_c$, and $\left[I_{\text{fuel}_i,\,Fc_i}\right]$ is the $i-$th tank's inertia about its center of mass, $Fc_i$. Furthermore, an analytical expression of mass depletion in the rotational motion is deduced: \begin{multline}\label{eq:HscB_dot} \dot{\bm{H}}_{\text{sc, }B} = \left[I_{\text{hub},\,Bc}\right]\dot{\bm{\omega}}_{\cal{B}/\cal{N}} + \bm{\omega}_{\cal{B}/\cal{N}} \times \left(\left[I_{\text{hub},\,Bc}\right]\bm{\omega}_{\cal{B}/\cal{N}}\right)+ \bm{r}_{Bc/B} \times m_\text{hub}\,\ddot{\bm{r}}_{Bc/B} +\\ - + \sum_{i=1}^{M}\left(\left[I_{\text{fuel}_i,\,Fc_i}\right] \dot{\bm{\omega}}_{\cal{B}/\cal{N}} + \bm{\omega}_{\cal{B}/\cal{N}} \times \left(\left[I_{\text{fuel}_i,\,Fc_i}\right]\, \bm{\omega}_{\cal{B}/\cal{N}}\right) - + \bm{r}_{Fc_i/B} \times m_{\text{fuel}_i}\, \ddot{\bm{r}}_{Fc_i/B}+\right.\\ + + \sum_{i=1}^{M}\left(\left[I_{\text{fuel}_i,\,Fc_i}\right] \dot{\bm{\omega}}_{\cal{B}/\cal{N}} + \bm{\omega}_{\cal{B}/\cal{N}} \times \left(\left[I_{\text{fuel}_i,\,Fc_i}\right]\, \bm{\omega}_{\cal{B}/\cal{N}}\right) + + \bm{r}_{Fc_i/B} \times m_{\text{fuel}_i}\, \ddot{\bm{r}}_{Fc_i/B}+\right.\\ \left.+ \bm{r}_{Fc_i/B} \times \dot{m}_{\text{fuel}_i} \,\dot{\bm{r}}_{Fc_i/B} + \left[I_{\text{fuel}_i,\, Fc_i}\right]' \, \bm{\omega}_{\cal{B}/\cal{N}}\right) \end{multline} @@ -278,21 +278,21 @@ \subsubsection{Rotational Equation of Motion} Additionally, the inertial time derivate of the vectors $\bm{r}_{Bc/B}$ and $\bm{r}_{Fc_i/B}$ is computed using the transport theorem between the two reference frames given in Equations \eqref{eq:rMB_ddot} and \eqref{eq:rMB_dot} and, considering that the point $B_c$ is fixed in the $\mathcal{B}$ frame, Equation \eqref{eq:HscB_dot} is rewritten: \begin{multline}\label{eq:HscB_complic} - \dot{\bm{H}}_{\text{sc, }B} = \left[I_{\text{hub},\,Bc}\right]\dot{\bm{\omega}}_{\cal{B}/\cal{N}} + \bm{\omega}_{\cal{B}/\cal{N}} \times \left(\left[I_{\text{hub},\,Bc}\right]\bm{\omega}_{\cal{B}/\cal{N}}\right) + \dot{\bm{H}}_{\text{sc, }B} = \left[I_{\text{hub},\,Bc}\right]\dot{\bm{\omega}}_{\cal{B}/\cal{N}} + \bm{\omega}_{\cal{B}/\cal{N}} \times \left(\left[I_{\text{hub},\,Bc}\right]\bm{\omega}_{\cal{B}/\cal{N}}\right) + \bm{r}_{Bc/B} \times m_\text{hub}\,\left(\dot{\bm{\omega}}_{\cal{B}/\cal{N}} \times \bm{r}_{Bc/B} +\right.\\ - +\left. \bm{\omega}_{\cal{B}/\cal{N}} \times \left(\bm{\omega}_{\cal{B}/\cal{N}} \times \bm{r}_{Bc/B}\right) \right) - +\sum_{i=1}^{M} \bigl(\left[I_{\text{fuel}_i,\,Fc_i}\right] \dot{\bm{\omega}}_{\cal{B}/\cal{N}} + \bm{\omega}_{\cal{B}/\cal{N}} \times \left(\left[I_{\text{fuel}_i,\,Fc_i}\right] \bm{\omega}_{\cal{B}/\cal{N}}\right) +\\ + +\left. \bm{\omega}_{\cal{B}/\cal{N}} \times \left(\bm{\omega}_{\cal{B}/\cal{N}} \times \bm{r}_{Bc/B}\right) \right) + +\sum_{i=1}^{M} \bigl(\left[I_{\text{fuel}_i,\,Fc_i}\right] \dot{\bm{\omega}}_{\cal{B}/\cal{N}} + \bm{\omega}_{\cal{B}/\cal{N}} \times \left(\left[I_{\text{fuel}_i,\,Fc_i}\right] \bm{\omega}_{\cal{B}/\cal{N}}\right) +\\ + \bm{r}_{Fc_i/B} \times m_{\text{fuel}_i}\, \bigl( \bm{r}''_{Fc_i/B} + 2\,\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times \bm{r}'_{Fc_i/B} + \dot{\bm{\omega}}_{\cal{B}/\cal{N}} \times \bm{r}_{Fc_i/B} + \bm{\omega}_{\cal{B}/\cal{N}} \times - \left(\bm{\omega}_{\cal{B}/\cal{N}} \times \bm{r}_{Fc_i/B}\right) \bigr)+\\ + \left(\bm{\omega}_{\cal{B}/\cal{N}} \times \bm{r}_{Fc_i/B}\right) \bigr)+\\ + - \bm{r}_{Fc_i/B} \times \dot{m}_{\text{fuel}_i} \,\left(\bm{r}'_{Fc_i/B}+\bm{\omega}_{\cal{B}/\cal{N}} \times \bm{r}_{Fc_i/B}\right) + \bm{r}_{Fc_i/B} \times \dot{m}_{\text{fuel}_i} \,\left(\bm{r}'_{Fc_i/B}+\bm{\omega}_{\cal{B}/\cal{N}} \times \bm{r}_{Fc_i/B}\right) + \left[I_{\text{fuel,}_i,\,Fc_i}\right]' \, \bm{\omega}_{\cal{B}/\cal{N}}\bigr) \end{multline} In order to simplify compact Equation \eqref{eq:HscB_complic} the following inertia matrices are defined using the skew symmetric matrix to replace the cross product: \begin{equation}\label{eq:eq44} - \left[I_{\text{hub},\,B}\right]= \left[I_{\text{hub},\,Bc}\right] + m_\text{hub}\, \left[\tilde{\bm{r}}_{Bc/B}\right]\left[\tilde{\bm{r}}_{Bc/B}\right]^T + \left[I_{\text{hub},\,B}\right]= \left[I_{\text{hub},\,Bc}\right] + m_\text{hub}\, \left[\tilde{\bm{r}}_{Bc/B}\right]\left[\tilde{\bm{r}}_{Bc/B}\right]^T \end{equation} \begin{equation}\label{eq:eq45} \left[I_{\text{fuel}_i,\,B}\right]= \left[I_{\text{fuel}_i,\,Fc_i}\right] + m_{\text{fuel}_i}\, \left[\tilde{\bm{r}}_{Fc_i/B}\right]\left[\tilde{\bm{r}}_{Fc_i/B}\right]^T @@ -304,7 +304,7 @@ \subsubsection{Rotational Equation of Motion} \mathbf {a} \times (\mathbf {b} \times \mathbf {c} )+\mathbf {b} \times (\mathbf {c} \times \mathbf {a} )+\mathbf {c} \times (\mathbf {a} \times \mathbf {b} )=\mathbf {0}$, the body relative time derivative of the fuel inertia in the $\mathcal{B}$ reference frame is introduced: \begin{multline}\label{eq:eq47} - \bm{r}_{Fc_i/B} \times \left(2\,\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times \bm{r}'_{Fc_i/B}\right) =-\bm{r}_{Fc_i/B} \times \left( \bm{r}'_{Fc_i/B} \times \bm{\omega}_{\mathcal{B}/\mathcal{N}}\right) + \bm{r}_{Fc_i/B} \times \left(2\,\bm{\omega}_{\mathcal{B}/\mathcal{N}} \times \bm{r}'_{Fc_i/B}\right) =-\bm{r}_{Fc_i/B} \times \left( \bm{r}'_{Fc_i/B} \times \bm{\omega}_{\mathcal{B}/\mathcal{N}}\right) +\\-\bm{r}'_{Fc_i/B} \times \left( \bm{r}_{Fc_i/B} \times \bm{\omega}_{\mathcal{B}/\mathcal{N}}\right) + \bm{\omega}_{\mathcal{B}/\mathcal{N}} \times \left(\bm{r}_{Fc_i/B}\times \bm{r}'_{Fc_i/B}\right) \end{multline} \begin{multline}\label{eq:eq48} @@ -328,7 +328,7 @@ \subsubsection{Rotational Equation of Motion} +\right. \\ \left.+ \dot{m}_{\text{fuel}_i} \left[\tilde{\bm{r}}_{Fc_i/B}\right]\bm{r}'_{Fc_i/B}+ {m}_{\text{fuel}_i}\,\left[\tilde{\bm{\omega}}_{\mathcal{B}/\mathcal{N}}\right] \left[\tilde{\bm{r}}_{Fc_i/B}\right] \bm{r}'_{Fc_i/B}\right) +\sum_{j=1}^{N}\int_{\dot{m}_{\text{noz}_j}}\left[\tilde{\bm{r}}_{M/B}\right] ^T \bm{v}_{\text{exh}_j}\,\text{d}\dot{m}+\\ - +\sum_{j=1}^{N}\int_{\dot{m}_{\text{noz}_j}}\left[\tilde{\bm{r}}_{M/B}\right] \left[\tilde{\bm{r}}_{M/B}\right] \bm{\omega} _{\mathcal{B}/\mathcal{N}} \,\text{d}\dot{m} +\left[\tilde{\bm{c}}\right] m_{\text{sc}}\,\ddot{\bm{r}}_{B/N}=\bm{L}_{B,\,\text{vol}}+\bm{L}_{B,\,\text{surf}} + +\sum_{j=1}^{N}\int_{\dot{m}_{\text{noz}_j}}\left[\tilde{\bm{r}}_{M/B}\right] \left[\tilde{\bm{r}}_{M/B}\right] \bm{\omega} _{\mathcal{B}/\mathcal{N}} \,\text{d}\dot{m} +\left[\tilde{\bm{c}}\right] m_{\text{sc}}\,\ddot{\bm{r}}_{B/N}=\bm{L}_{B,\,\text{vol}}+\bm{L}_{B,\,\text{surf}} \end{multline} The torque of each thruster nozzle is computed by the exhausting flow pressure distribution and by the lever arm distance from point $B$ and the application point of the force: @@ -338,13 +338,13 @@ \subsubsection{Rotational Equation of Motion} Furthermore, a term taking into account the angular momentum variation caused by mass depletion is defined: \begin{equation}\label{eq:eq53} - \left[K\right]= \sum_{i=1}^{M}\left[I_{\text{fuel}_i,\,B}\right] ' +\sum_{j=1}^{N} \int_{\dot{m}_{\text{noz}_j}}\left[\tilde{\bm{r}}_{M/B}\right] \left[\tilde{\bm{r}}_{M/B}\right]\,\text{d}\dot{m} + \left[K\right]= \sum_{i=1}^{M}\left[I_{\text{fuel}_i,\,B}\right] ' +\sum_{j=1}^{N} \int_{\dot{m}_{\text{noz}_j}}\left[\tilde{\bm{r}}_{M/B}\right] \left[\tilde{\bm{r}}_{M/B}\right]\,\text{d}\dot{m} \end{equation} The second integral in Equation \eqref{eq:eq53} is computed evaluating the momentum exchanged due to the fuel exiting the nozzle area (assuming a circular nozzle), coincident the interface surface between the spacecraft and the exhausted fuel: \begin{multline} - \int_{\dot{m}_{\text{noz}_j}}\left[\tilde{\bm{r}}_{M/B}\right] \left[\tilde{\bm{r}}_{M/B}\right]\,\text{d}\dot{m} + \int_{\dot{m}_{\text{noz}_j}}\left[\tilde{\bm{r}}_{M/B}\right] \left[\tilde{\bm{r}}_{M/B}\right]\,\text{d}\dot{m} =\int_{\dot{m}_{\text{noz}_j}}\left(\left[\tilde{\bm{r}}_{N_j/B}\right] + \left[\tilde{\bm{r}}_{M/N_j}\right]\right)\left(\left[\tilde{\bm{r}}_{N_j/B}\right] + \left[\tilde{\bm{r}}_{M/N_j}\right]\right)\,\text{d}\dot{m} =\\ =-\dot{m}_{\text{noz}_j}\left(\left[\tilde{\bm{r}}_{N_j/B}\right]\left[\tilde{\bm{r}}_{N_j/B}\right]^T + \frac{A_{\text{noz}_j}}{4\,\pi}\left[BM_j\right]\left[\begin{matrix} 2&0&0\\ @@ -356,10 +356,10 @@ \subsubsection{Rotational Equation of Motion} where $A_{\text{noz}_j}$ is the exiting area of the j-th nozzle and $\left[BM_j\right]$ is the direction cosine matrix (DCM) from from the j-th nozzle frame $\mathcal{M}_j$, defined to have its origin at the $N_j$ point and its first axis in the exhausting velocity direction $\bm{v}_{\text{exh}_j}$, to the $\mathcal{B}$ frame. Finally the rotational EOM is written as: \begin{multline}\label{eq:eq55} - \left[I_{\text{sc, }B}\right]\,\dot{\bm{\omega}}_{\cal{B}/\cal{N}} + m_{\text{sc}} \left[\tilde{\bm{c}}\right]\ddot{\bm{r}}_{B/N}= \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right]^T\left[I_{\text{sc, }B}\right]\,\bm{\omega}_{\cal{B}/\cal{N}} - \left[K\right]\,\bm{\omega} _{\mathcal{B}/\mathcal{N}}+\\+ \sum_{i=1}^{M}\left(m_{\text{fuel}_i}\,\left[\tilde{\bm{r}}_{Fc_i/B}\right]^T \bm{r}''_{Fc_i/B} + + \left[I_{\text{sc, }B}\right]\,\dot{\bm{\omega}}_{\cal{B}/\cal{N}} + m_{\text{sc}} \left[\tilde{\bm{c}}\right]\ddot{\bm{r}}_{B/N}= \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right]^T\left[I_{\text{sc, }B}\right]\,\bm{\omega}_{\cal{B}/\cal{N}} - \left[K\right]\,\bm{\omega} _{\mathcal{B}/\mathcal{N}}+\\+ \sum_{i=1}^{M}\left(m_{\text{fuel}_i}\,\left[\tilde{\bm{r}}_{Fc_i/B}\right]^T \bm{r}''_{Fc_i/B} + {m}_{\text{fuel}_i}\, \left[\tilde{\bm{\omega}}_{\mathcal{B}/\mathcal{N}}\right]^T \left[\tilde{\bm{r}}_{Fc_i/B}\right] \bm{r}'_{Fc_i/B}+\right.\\+\left.\dot{m}_{\text{fuel}_i} \left[\tilde{\bm{r}}_{Fc_i/B}\right]^T\bm{r}'_{Fc_i/B}\right) - + \bm{L}_{B,\,\text{vol}}+\bm{L}_{B,\,\text{surf}} + \sum_{j=1}^{N}\bm{L}_{B_{\text{thr}_j}} + + \bm{L}_{B,\,\text{vol}}+\bm{L}_{B,\,\text{surf}} + \sum_{j=1}^{N}\bm{L}_{B_{\text{thr}_j}} \end{multline} This concludes the derivation of the EOMs needed to describe the translational and rotational motion of a spacecraft with depleting mass due to thrusters. The following section describes the models used for the fuel tanks. @@ -369,7 +369,7 @@ \subsection{Tank models} \begin{itemize} \item The constant tank's volume model where a spherical reservoir maintains a fixed geometry, i.e. a constant radius, and a fixed barycenter. - \item The constant fuel's density model where a spherical tank keeps its geometrical shape but gradually change its volume, so its radius, to maintain constant the density of the fuel and it has a fixed center of mass. + \item The constant fuel's density model where a spherical tank keeps its geometrical shape but gradually change its volume, so its radius, to maintain constant the density of the fuel and it has a fixed center of mass. \item The emptying tank model where the fuel leaks out from an outlet in the spherical reservoir and the quantity of fuel decrease perpendicularly to the output direction modifying the barycenter position and the body's inertia accordingly to the mass distribution inside the tank. \item The uniform burn cylinder model where a cylindrical tank does not change its geometrical shape and volume but the gas gradually decrease its density. As a consequence, the fuel barycenter remains fixed and the inertia varies accordingly to the mass variation. \item The centrifugal burn cylinder model where a cylindrical tank is considered and the fuel burns radially from the center until the walls without breaking the tank's symmetry. The inertia tensor derivative is computed from these hypothesis and the barycenter remains in its initial position because the symmetry is conserved. @@ -390,7 +390,7 @@ \subsubsection{The constant tank's volume model} \end{equation} \begin{equation} \left[I_{\text{fuel, }Tc}\right] = \frac{2}{5}\,m_{\text{fuel}} \,R_{\text{tank}}^2\, \left[I_{3\times3}\right] -\end{equation} +\end{equation} \begin{equation} \left[I_{\text{fuel, }Tc}\right]' = \frac{2}{5}\,\dot{m}_{\text{fuel}} \,R_{\text{tank}}^2\, \left[I_{3\times3}\right] \end{equation} @@ -603,13 +603,10 @@ \subsection{Update-Only Equations} Eqs.\eqref{eq:eq30} and \eqref{eq:eq55} are the translational and rotational EOMs for the fully coupled dynamics of mass depletion. However, it is common to use an "update-only" method for mass depletion which does not consider mass depletion as a dynamical effect. In contrast to the fully coupled model it simply updates the mass properties of the spacecraft. The equations for the "update-only" can be simplified to: \begin{equation}\label{eq:eq31} -m_{\text{sc}} \ddot{\bm{r}}_{B/N} - m_{\text{sc}} \left[\tilde{\bm{c}}\right]\dot{\bm{\omega}}_{\cal{B}/\cal{N}} = \bm{F}_{\text{thr}} - m_{\text{sc}} \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right] \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right] \bm{c} +m_{\text{sc}} \ddot{\bm{r}}_{B/N} - m_{\text{sc}} \left[\tilde{\bm{c}}\right]\dot{\bm{\omega}}_{\cal{B}/\cal{N}} = \bm{F}_{\text{thr}} - m_{\text{sc}} \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right] \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right] \bm{c} \end{equation} \begin{equation}\label{eq:eq55} m_{\text{sc}} \left[\tilde{\bm{c}}\right]\ddot{\bm{r}}_{B/N} + \left[I_{\text{sc, }B}\right]\,\dot{\bm{\omega}}_{\cal{B}/\cal{N}} = - \left[\tilde{\bm{\omega}}_{\cal{B}/\cal{N}}\right]\left[I_{\text{sc, }B}\right]\,\bm{\omega}_{\cal{B}/\cal{N}} -+ \bm{L}_{B} + \sum_{j=1}^{N}\bm{L}_{B_{\text{thr}_j}} ++ \bm{L}_{B} + \sum_{j=1}^{N}\bm{L}_{B_{\text{thr}_j}} \end{equation} - - - diff --git a/src/simulation/dynamics/FuelTank/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/FuelTank/_Documentation/secModelFunctions.tex index c2b93ad307..12142d457b 100644 --- a/src/simulation/dynamics/FuelTank/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/FuelTank/_Documentation/secModelFunctions.tex @@ -4,7 +4,7 @@ \section{Model Functions} \begin{itemize} \item Compute tank properties depending on the tank model being used - \item Provides its contributions to the mass properties of the spacecraft + \item Provides its contributions to the mass properties of the spacecraft \item Provides its contributions to the back-substitution matrices \item Computes its derivative for its mass flow rate using the vector of attached thrusters \item Provides its contributions to energy and momentum of the spacecraft @@ -14,10 +14,10 @@ \section{Model Assumptions and Limitations} Below is a summary of the assumptions/limitations: \begin{itemize} - \item There can be no relative rotational motion between the fuel in the tank and rigid body hub + \item There can be no relative rotational motion between the fuel in the tank and rigid body hub \item The thrusters can't be vectored but rather need to be fixed with respect to the rigid body hub \item There is no relative rotational motion between the mass leaving the spacecraft through the thrusters and the rigid body hub \item The movement of the mass between tanks and thrusters is not considered as a dynamical effect (i.e. the motion of the mass through piping in the spacecraft) \item Fuel slosh is not being considered by this module alone, however fuel slosh can be attached to the spacecraft by using the \textit{fuelSloshParticle} effector \item The mass and inertia of the fuel tank is not included in the rigid body mass when setting up a simulation, but rather the fuel tank mass properties is added to the spacecraft dynamically during the simulation -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/simulation/dynamics/FuelTank/_Documentation/secTest.tex b/src/simulation/dynamics/FuelTank/_Documentation/secTest.tex index f80019c69a..09146cd7d6 100644 --- a/src/simulation/dynamics/FuelTank/_Documentation/secTest.tex +++ b/src/simulation/dynamics/FuelTank/_Documentation/secTest.tex @@ -18,14 +18,13 @@ \section{Test Parameters} \caption{Error Tolerance - Note: Relative Tolerance is $\textnormal{abs}(\frac{\textnormal{truth} - \textnormal{value}}{\textnormal{truth}}$)} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c |} % Column formatting, + \begin{tabular}{| c | c |} % Column formatting, \hline Test & Relative Tolerance \\ \hline Update-Only Position/Attitude Tests & 1e-7 \\ \hline Tank Model Tests & 1e-10 \\ - \hline + \hline \end{tabular} \end{table} - diff --git a/src/simulation/dynamics/FuelTank/fuelTank.rst b/src/simulation/dynamics/FuelTank/fuelTank.rst index 0e74c35d09..4a1ba2b9f3 100644 --- a/src/simulation/dynamics/FuelTank/fuelTank.rst +++ b/src/simulation/dynamics/FuelTank/fuelTank.rst @@ -48,4 +48,4 @@ A thruster effector can be attached to a tank effector to simulate fuel mass dep .. code-block:: python - fuelTankEffector.addThrusterSet(thrustersEffector) \ No newline at end of file + fuelTankEffector.addThrusterSet(thrustersEffector) diff --git a/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.h b/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.h index 23496097f9..9e4332aaf5 100644 --- a/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.h +++ b/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.h @@ -49,7 +49,7 @@ class GravityGradientEffector: public SysModel, public DynamicEffector { public: - Message gravityGradientOutMsg; //!< output message containing the gravity gradient + Message gravityGradientOutMsg; //!< output message containing the gravity gradient StateData *hubSigma; //!< Hub/Inertial attitude represented by MRP StateData *r_BN_N; //!< Hub/Inertial position vector in inertial frame components Eigen::MatrixXd *ISCPntB_B; //!< [kg m^2] current spacecraft inertia about point B, B-frame components diff --git a/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.rst b/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.rst index 5ae4f31032..7ab3155031 100644 --- a/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.rst +++ b/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.rst @@ -97,4 +97,3 @@ Module Output Message Name ^^^^^^^^^^^^^^^^^^^^^^^^^^ The effector write an output message with the current gravity gradient torque information at each ``update`` cycle. The output message is ``gravityGradientOutMsg``. - diff --git a/src/simulation/dynamics/HingedRigidBodies/_Documentation/AVS.sty b/src/simulation/dynamics/HingedRigidBodies/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/HingedRigidBodies/_Documentation/AVS.sty +++ b/src/simulation/dynamics/HingedRigidBodies/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/HingedRigidBodies/_Documentation/Basilisk-HINGEDRIGIDBODYSTATEEFFECTOR-20170703.tex b/src/simulation/dynamics/HingedRigidBodies/_Documentation/Basilisk-HINGEDRIGIDBODYSTATEEFFECTOR-20170703.tex index d09dfb4980..a8094ecc1a 100755 --- a/src/simulation/dynamics/HingedRigidBodies/_Documentation/Basilisk-HINGEDRIGIDBODYSTATEEFFECTOR-20170703.tex +++ b/src/simulation/dynamics/HingedRigidBodies/_Documentation/Basilisk-HINGEDRIGIDBODYSTATEEFFECTOR-20170703.tex @@ -96,7 +96,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/HingedRigidBodies/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/HingedRigidBodies/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/HingedRigidBodies/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/HingedRigidBodies/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/HingedRigidBodies/_Documentation/Support/PlanarFlexibleDynamicsDerivation.nb b/src/simulation/dynamics/HingedRigidBodies/_Documentation/Support/PlanarFlexibleDynamicsDerivation.nb index 37b87f117c..20aba4294a 100644 --- a/src/simulation/dynamics/HingedRigidBodies/_Documentation/Support/PlanarFlexibleDynamicsDerivation.nb +++ b/src/simulation/dynamics/HingedRigidBodies/_Documentation/Support/PlanarFlexibleDynamicsDerivation.nb @@ -22,669 +22,669 @@ Notebook[{ Cell[CellGroupData[{ Cell["Planar Flexible Dynamics with two solar panels", "Section", CellChangeTimes->{{3.6476211926645107`*^9, 3.6476212207444983`*^9}, { - 3.647621411462695*^9, 3.647621435484638*^9}, {3.6476218320778303`*^9, + 3.647621411462695*^9, 3.647621435484638*^9}, {3.6476218320778303`*^9, 3.6476218344203043`*^9}, {3.649439747948255*^9, 3.649439759068905*^9}}], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Rhub", " ", "=", " ", - RowBox[{"{", + RowBox[{"Rhub", " ", "=", " ", + RowBox[{"{", RowBox[{ - RowBox[{"xHub", "[", "t", "]"}], ",", " ", + RowBox[{"xHub", "[", "t", "]"}], ",", " ", RowBox[{"yHub", "[", "t", "]"}]}], "}"}]}]], "Input", CellChangeTimes->{{3.6476077582392817`*^9, 3.64760777925314*^9}, { - 3.647615079246727*^9, 3.647615079629671*^9}, {3.647621147852172*^9, + 3.647615079246727*^9, 3.647615079629671*^9}, {3.647621147852172*^9, 3.647621148259598*^9}, {3.649439738791636*^9, 3.649439776241705*^9}, { - 3.7096453803292313`*^9, 3.709645383457893*^9}, {3.70964542147725*^9, + 3.7096453803292313`*^9, 3.709645383457893*^9}, {3.70964542147725*^9, 3.7096454318767157`*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"xHub", "[", "t", "]"}], ",", + RowBox[{"xHub", "[", "t", "]"}], ",", RowBox[{"yHub", "[", "t", "]"}]}], "}"}]], "Output", - CellChangeTimes->{3.7096454421264477`*^9, 3.7098930254953527`*^9, - 3.70990443182268*^9, 3.709912186034545*^9, 3.709991565698237*^9, + CellChangeTimes->{3.7096454421264477`*^9, 3.7098930254953527`*^9, + 3.70990443182268*^9, 3.709912186034545*^9, 3.709991565698237*^9, 3.71006682731984*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"VHub", " ", "=", " ", - RowBox[{"D", "[", + RowBox[{"VHub", " ", "=", " ", + RowBox[{"D", "[", RowBox[{"Rhub", ",", "t"}], "]"}]}]], "Input", CellChangeTimes->{{3.647608152658317*^9, 3.6476081672650843`*^9}, { - 3.6494398162615347`*^9, 3.649439828455247*^9}, {3.7096466475466137`*^9, + 3.6494398162615347`*^9, 3.649439828455247*^9}, {3.7096466475466137`*^9, 3.7096466482166*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], ",", + MultilineFunction->None], "[", "t", "]"}], ",", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "}"}]], "Output", CellChangeTimes->{ - 3.647608169805852*^9, {3.647614323833295*^9, 3.64761434783421*^9}, - 3.647615089563908*^9, 3.647621228054256*^9, {3.649439822196238*^9, - 3.649439831862186*^9}, 3.650026329921008*^9, 3.650026435544567*^9, - 3.650113943872491*^9, 3.6501140768873177`*^9, 3.650114124028068*^9, - 3.650121181032531*^9, 3.6501254656908484`*^9, 3.650126350692739*^9, - 3.650126432976297*^9, 3.650126661304862*^9, 3.6501271224543867`*^9, - 3.709645396421109*^9, 3.7096454641959667`*^9, 3.7096466511650953`*^9, - 3.7098930294802837`*^9, 3.709904431894704*^9, 3.709912186140971*^9, + 3.647608169805852*^9, {3.647614323833295*^9, 3.64761434783421*^9}, + 3.647615089563908*^9, 3.647621228054256*^9, {3.649439822196238*^9, + 3.649439831862186*^9}, 3.650026329921008*^9, 3.650026435544567*^9, + 3.650113943872491*^9, 3.6501140768873177`*^9, 3.650114124028068*^9, + 3.650121181032531*^9, 3.6501254656908484`*^9, 3.650126350692739*^9, + 3.650126432976297*^9, 3.650126661304862*^9, 3.6501271224543867`*^9, + 3.709645396421109*^9, 3.7096454641959667`*^9, 3.7096466511650953`*^9, + 3.7098930294802837`*^9, 3.709904431894704*^9, 3.709912186140971*^9, 3.709991565807317*^9, 3.710066827425913*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP1Hub", " ", "=", " ", - RowBox[{"{", + RowBox[{"RSP1Hub", " ", "=", " ", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge1", "*", - RowBox[{"Cos", "[", + RowBox[{"Rhinge1", "*", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], - " ", "+", " ", - RowBox[{"d1", "*", " ", - RowBox[{"Cos", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], + " ", "+", " ", + RowBox[{"d1", "*", " ", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", - - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", " ", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", + + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", " ", RowBox[{ - RowBox[{"Rhinge1", "*", - RowBox[{"Sin", "[", + RowBox[{"Rhinge1", "*", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], - " ", "+", " ", - RowBox[{"d1", "*", " ", - RowBox[{"Sin", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], + " ", "+", " ", + RowBox[{"d1", "*", " ", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", - + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]}]], "Input", CellChangeTimes->{{3.649439849206862*^9, 3.649439917339305*^9}, { - 3.649440015026457*^9, 3.649440104067552*^9}, {3.709645886898323*^9, + 3.649440015026457*^9, 3.649440104067552*^9}, {3.709645886898323*^9, 3.709645916521826*^9}, {3.7096459487470293`*^9, 3.709646004845886*^9}, { - 3.709646048343261*^9, 3.709646092722971*^9}, {3.709646222601664*^9, + 3.709646048343261*^9, 3.709646092722971*^9}, {3.709646222601664*^9, 3.709646380970314*^9}, {3.709893035295415*^9, 3.709893153573711*^9}, { 3.709904347427107*^9, 3.7099043599351263`*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]], "Output", - CellChangeTimes->{3.649440107978813*^9, 3.650026329938651*^9, - 3.650026435571876*^9, 3.650113943890592*^9, 3.650114076912006*^9, - 3.6501141240599203`*^9, 3.650121181063718*^9, 3.6501254657141533`*^9, - 3.65012635072224*^9, 3.650126432995634*^9, 3.650126661323832*^9, - 3.650127122474761*^9, 3.709646389923583*^9, 3.7098931733877287`*^9, - 3.7099043619306583`*^9, 3.709904431944851*^9, 3.709912186205373*^9, + CellChangeTimes->{3.649440107978813*^9, 3.650026329938651*^9, + 3.650026435571876*^9, 3.650113943890592*^9, 3.650114076912006*^9, + 3.6501141240599203`*^9, 3.650121181063718*^9, 3.6501254657141533`*^9, + 3.65012635072224*^9, 3.650126432995634*^9, 3.650126661323832*^9, + 3.650127122474761*^9, 3.709646389923583*^9, 3.7098931733877287`*^9, + 3.7099043619306583`*^9, 3.709904431944851*^9, 3.709912186205373*^9, 3.7099915659045143`*^9, 3.710066827499762*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP1", " ", "=", " ", + RowBox[{"RSP1", " ", "=", " ", RowBox[{"Rhub", " ", "+", " ", "RSP1Hub"}]}]], "Input", CellChangeTimes->{{3.709646395997994*^9, 3.709646412127426*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"xHub", "[", "t", "]"}]}], ",", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"xHub", "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", RowBox[{"yHub", "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.709646415182599*^9, 3.709893180581501*^9, - 3.709904365017598*^9, 3.709904432007655*^9, 3.7099121862900763`*^9, + CellChangeTimes->{3.709646415182599*^9, 3.709893180581501*^9, + 3.709904365017598*^9, 3.709904432007655*^9, 3.7099121862900763`*^9, 3.70999156597033*^9, 3.710066827566654*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP2Hub", " ", "=", " ", - RowBox[{"{", + RowBox[{"RSP2Hub", " ", "=", " ", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge2", "*", - RowBox[{"Cos", "[", + RowBox[{"Rhinge2", "*", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], - " ", "+", " ", - RowBox[{"d2", "*", " ", - RowBox[{"Cos", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], + " ", "+", " ", + RowBox[{"d2", "*", " ", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", - - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", " ", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", + + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", " ", RowBox[{ - RowBox[{"Rhinge2", "*", - RowBox[{"Sin", "[", + RowBox[{"Rhinge2", "*", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], - " ", "+", " ", - RowBox[{"d2", "*", " ", - RowBox[{"Sin", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], + " ", "+", " ", + RowBox[{"d2", "*", " ", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", - + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]}]], "Input", CellChangeTimes->{{3.709646463873159*^9, 3.709646543110506*^9}, { - 3.70989319908799*^9, 3.709893260993504*^9}, {3.709904369663701*^9, + 3.70989319908799*^9, 3.709893260993504*^9}, {3.709904369663701*^9, 3.709904375505327*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]], "Output", - CellChangeTimes->{{3.709646522302372*^9, 3.709646546358492*^9}, - 3.709893305041185*^9, 3.709904376632481*^9, 3.709904432100214*^9, + CellChangeTimes->{{3.709646522302372*^9, 3.709646546358492*^9}, + 3.709893305041185*^9, 3.709904376632481*^9, 3.709904432100214*^9, 3.709912186356444*^9, 3.709991566037938*^9, 3.710066827630702*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP2", " ", "=", " ", + RowBox[{"RSP2", " ", "=", " ", RowBox[{"Rhub", " ", "+", " ", "RSP2Hub"}]}]], "Input", CellChangeTimes->{{3.709646526347693*^9, 3.709646553485578*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"xHub", "[", "t", "]"}]}], ",", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"xHub", "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", RowBox[{"yHub", "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.7096465578787622`*^9, 3.709893308250244*^9, - 3.709904379426956*^9, 3.70990443217269*^9, 3.709912186422678*^9, + CellChangeTimes->{3.7096465578787622`*^9, 3.709893308250244*^9, + 3.709904379426956*^9, 3.70990443217269*^9, 3.709912186422678*^9, 3.709991566104603*^9, 3.7100668277002287`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"VSP1", " ", "=", " ", - RowBox[{"D", "[", + RowBox[{"VSP1", " ", "=", " ", + RowBox[{"D", "[", RowBox[{"RSP1", ",", "t"}], "]"}]}]], "Input", CellChangeTimes->{{3.649440115855461*^9, 3.649440124808634*^9}, { 3.709646572311063*^9, 3.70964658152712*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ",", + MultilineFunction->None], "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.649440131399378*^9, 3.650026329966713*^9, - 3.650026435604875*^9, 3.650113943920773*^9, 3.650114076942313*^9, - 3.6501141240781507`*^9, 3.650121181083191*^9, 3.6501254657517776`*^9, - 3.650126350757868*^9, 3.6501264330338707`*^9, 3.6501266613621817`*^9, - 3.650127122516333*^9, 3.709646585406389*^9, 3.709893320897305*^9, - 3.70990438813186*^9, 3.70990443223833*^9, 3.709912186471583*^9, + CellChangeTimes->{3.649440131399378*^9, 3.650026329966713*^9, + 3.650026435604875*^9, 3.650113943920773*^9, 3.650114076942313*^9, + 3.6501141240781507`*^9, 3.650121181083191*^9, 3.6501254657517776`*^9, + 3.650126350757868*^9, 3.6501264330338707`*^9, 3.6501266613621817`*^9, + 3.650127122516333*^9, 3.709646585406389*^9, 3.709893320897305*^9, + 3.70990438813186*^9, 3.70990443223833*^9, 3.709912186471583*^9, 3.7099915661712847`*^9, 3.710066827767516*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"VSP2", " ", "=", " ", - RowBox[{"D", "[", + RowBox[{"VSP2", " ", "=", " ", + RowBox[{"D", "[", RowBox[{"RSP2", ",", "t"}], "]"}]}]], "Input", CellChangeTimes->{{3.649440262161566*^9, 3.64944026549428*^9}, { 3.709646616832868*^9, 3.709646621688724*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ",", + MultilineFunction->None], "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.649440268747218*^9, 3.650026330022114*^9, - 3.650026435667726*^9, 3.650113943975575*^9, 3.650114076989862*^9, - 3.6501141241372833`*^9, 3.650121181143683*^9, 3.650125465840637*^9, - 3.650126350821439*^9, 3.650126433110935*^9, 3.650126661418356*^9, - 3.650127122571417*^9, 3.709646629412587*^9, 3.709893323420001*^9, - 3.7099043906789207`*^9, 3.7099044323057747`*^9, 3.70991218652265*^9, + CellChangeTimes->{3.649440268747218*^9, 3.650026330022114*^9, + 3.650026435667726*^9, 3.650113943975575*^9, 3.650114076989862*^9, + 3.6501141241372833`*^9, 3.650121181143683*^9, 3.650125465840637*^9, + 3.650126350821439*^9, 3.650126433110935*^9, 3.650126661418356*^9, + 3.650127122571417*^9, 3.709646629412587*^9, 3.709893323420001*^9, + 3.7099043906789207`*^9, 3.7099044323057747`*^9, 3.70991218652265*^9, 3.7099915662376337`*^9, 3.710066827832479*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"T", " ", "=", " ", + RowBox[{"T", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"1", "/", "2"}], "*", "mHub", "*", - RowBox[{"Dot", "[", - RowBox[{"VHub", ",", "VHub"}], "]"}]}], "+", + RowBox[{"1", "/", "2"}], "*", "mHub", "*", + RowBox[{"Dot", "[", + RowBox[{"VHub", ",", "VHub"}], "]"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "IHub", "*", + RowBox[{"1", "/", "2"}], "*", "IHub", "*", RowBox[{ RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}], "^", "2"}]}], "+", + RowBox[{"theta", "'"}], "[", "t", "]"}], "^", "2"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "mSP1", "*", - RowBox[{"Dot", "[", - RowBox[{"VSP1", ",", "VSP1"}], "]"}]}], "+", + RowBox[{"1", "/", "2"}], "*", "mSP1", "*", + RowBox[{"Dot", "[", + RowBox[{"VSP1", ",", "VSP1"}], "]"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "ISP1", "*", + RowBox[{"1", "/", "2"}], "*", "ISP1", "*", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}], "+", + RowBox[{"theta", "'"}], "[", "t", "]"}], "+", RowBox[{ - RowBox[{"theta1", "'"}], "[", "t", "]"}]}], ")"}], "^", "2"}]}], "+", - + RowBox[{"theta1", "'"}], "[", "t", "]"}]}], ")"}], "^", "2"}]}], "+", + RowBox[{ - RowBox[{"1", "/", "2"}], "*", "mSP2", "*", - RowBox[{"Dot", "[", - RowBox[{"VSP2", ",", "VSP2"}], "]"}]}], "+", + RowBox[{"1", "/", "2"}], "*", "mSP2", "*", + RowBox[{"Dot", "[", + RowBox[{"VSP2", ",", "VSP2"}], "]"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "ISP2", "*", + RowBox[{"1", "/", "2"}], "*", "ISP2", "*", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}], "+", + RowBox[{"theta", "'"}], "[", "t", "]"}], "+", RowBox[{ - RowBox[{"theta2", "'"}], "[", "t", "]"}]}], ")"}], "^", + RowBox[{"theta2", "'"}], "[", "t", "]"}]}], ")"}], "^", "2"}]}]}]}]], "Input", CellChangeTimes->{{3.6476081740482817`*^9, 3.647608248074851*^9}, { - 3.6476082871491823`*^9, 3.647608323868623*^9}, {3.6476084126718693`*^9, + 3.6476082871491823`*^9, 3.647608323868623*^9}, {3.6476084126718693`*^9, 3.647608426646431*^9}, {3.647614434493647*^9, 3.647614437644298*^9}, { - 3.649440314162457*^9, 3.649440454966736*^9}, {3.650113501057988*^9, + 3.649440314162457*^9, 3.649440454966736*^9}, {3.650113501057988*^9, 3.650113561077476*^9}, {3.6501140530388117`*^9, 3.650114070671464*^9}, { - 3.650125417777068*^9, 3.650125455538785*^9}, {3.650126331537952*^9, + 3.650125417777068*^9, 3.650125455538785*^9}, {3.650126331537952*^9, 3.650126341438471*^9}, {3.650126402719391*^9, 3.650126428999959*^9}, { - 3.650126617109906*^9, 3.6501266521408863`*^9}, {3.709646659345621*^9, + 3.650126617109906*^9, 3.6501266521408863`*^9}, {3.709646659345621*^9, 3.709646783134551*^9}, {3.7098933386684723`*^9, 3.709893360494198*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - FractionBox["1", "2"], " ", "IHub", " ", + FractionBox["1", "2"], " ", "IHub", " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP1", " ", + FractionBox["1", "2"], " ", "ISP1", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP2", " ", + FractionBox["1", "2"], " ", "ISP2", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mHub", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mHub", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}], "2"], "+", SuperscriptBox[ RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}], - "+", + "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}]}]], "Output", - CellChangeTimes->{3.650126433152005*^9, 3.650126661451419*^9, - 3.6501271226026382`*^9, 3.709647565406519*^9, 3.709893450106464*^9, - 3.709904399733959*^9, 3.709904432371422*^9, 3.709912186587597*^9, + CellChangeTimes->{3.650126433152005*^9, 3.650126661451419*^9, + 3.6501271226026382`*^9, 3.709647565406519*^9, 3.709893450106464*^9, + 3.709904399733959*^9, 3.709904432371422*^9, 3.709912186587597*^9, 3.709991566302853*^9, 3.7100668278993692`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"V", " ", "=", " ", + RowBox[{"V", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"1", "/", "2"}], "k1", + RowBox[{"1", "/", "2"}], "k1", RowBox[{ - RowBox[{"(", - RowBox[{"theta1", "[", "t", "]"}], ")"}], "^", "2"}]}], "+", + RowBox[{"(", + RowBox[{"theta1", "[", "t", "]"}], ")"}], "^", "2"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "k2", + RowBox[{"1", "/", "2"}], "k2", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{"theta2", "[", "t", "]"}], ")"}], "^", "2"}]}]}]}]], "Input", CellChangeTimes->{{3.647608457408671*^9, 3.647608497714343*^9}, { - 3.6494404974964943`*^9, 3.6494405178638973`*^9}, {3.709893455995377*^9, + 3.6494404974964943`*^9, 3.6494405178638973`*^9}, {3.709893455995377*^9, 3.709893465684599*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - FractionBox["1", "2"], " ", "k1", " ", + FractionBox["1", "2"], " ", "k1", " ", SuperscriptBox[ - RowBox[{"theta1", "[", "t", "]"}], "2"]}], "+", + RowBox[{"theta1", "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "k2", " ", + FractionBox["1", "2"], " ", "k2", " ", SuperscriptBox[ RowBox[{"theta2", "[", "t", "]"}], "2"]}]}]], "Output", CellChangeTimes->{ - 3.647608499924172*^9, 3.64761448705923*^9, 3.647615089616064*^9, - 3.647621228105358*^9, 3.649440521266033*^9, 3.650026330069805*^9, - 3.650026435698052*^9, 3.650113944040901*^9, 3.6501140770340567`*^9, - 3.6501141241886263`*^9, 3.650121181193581*^9, 3.65012546600323*^9, - 3.650126350893875*^9, 3.650126433184325*^9, 3.65012666148414*^9, + 3.647608499924172*^9, 3.64761448705923*^9, 3.647615089616064*^9, + 3.647621228105358*^9, 3.649440521266033*^9, 3.650026330069805*^9, + 3.650026435698052*^9, 3.650113944040901*^9, 3.6501140770340567`*^9, + 3.6501141241886263`*^9, 3.650121181193581*^9, 3.65012546600323*^9, + 3.650126350893875*^9, 3.650126433184325*^9, 3.65012666148414*^9, 3.650127122632825*^9, 3.7096475779352207`*^9, 3.709893468268532*^9, { - 3.709904403476265*^9, 3.709904432440607*^9}, 3.709912186653967*^9, + 3.709904403476265*^9, 3.709904432440607*^9}, 3.709912186653967*^9, 3.70999156637119*^9, 3.710066827966717*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Lag", " ", "=", " ", + RowBox[{"Lag", " ", "=", " ", RowBox[{"T", "-", "V"}]}]], "Input", CellChangeTimes->{{3.647608505756122*^9, 3.6476085105548487`*^9}, { 3.647614570390028*^9, 3.647614570501202*^9}}], @@ -692,626 +692,626 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", - FractionBox["1", "2"]}], " ", "k1", " ", + RowBox[{"-", + FractionBox["1", "2"]}], " ", "k1", " ", SuperscriptBox[ - RowBox[{"theta1", "[", "t", "]"}], "2"]}], "-", + RowBox[{"theta1", "[", "t", "]"}], "2"]}], "-", RowBox[{ - FractionBox["1", "2"], " ", "k2", " ", + FractionBox["1", "2"], " ", "k2", " ", SuperscriptBox[ - RowBox[{"theta2", "[", "t", "]"}], "2"]}], "+", + RowBox[{"theta2", "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "IHub", " ", + FractionBox["1", "2"], " ", "IHub", " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP1", " ", + FractionBox["1", "2"], " ", "ISP1", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP2", " ", + FractionBox["1", "2"], " ", "ISP2", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mHub", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mHub", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}], "2"], "+", SuperscriptBox[ RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}], - "+", + "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}]}]], "Output", CellChangeTimes->{ - 3.647608512913962*^9, 3.647614573588203*^9, 3.647615089649335*^9, - 3.647621228134879*^9, 3.649440526896332*^9, 3.6500263301004267`*^9, - 3.650026435722816*^9, 3.650113791252983*^9, 3.6501139440765142`*^9, - 3.650114077051147*^9, 3.650114124208144*^9, 3.650121181214005*^9, - 3.650125466055393*^9, 3.650126350927471*^9, 3.650126433224345*^9, - 3.650126661504125*^9, 3.650127122654311*^9, 3.709647615366233*^9, - 3.709893471758278*^9, {3.709904407003621*^9, 3.7099044325072527`*^9}, + 3.647608512913962*^9, 3.647614573588203*^9, 3.647615089649335*^9, + 3.647621228134879*^9, 3.649440526896332*^9, 3.6500263301004267`*^9, + 3.650026435722816*^9, 3.650113791252983*^9, 3.6501139440765142`*^9, + 3.650114077051147*^9, 3.650114124208144*^9, 3.650121181214005*^9, + 3.650125466055393*^9, 3.650126350927471*^9, 3.650126433224345*^9, + 3.650126661504125*^9, 3.650127122654311*^9, 3.709647615366233*^9, + 3.709893471758278*^9, {3.709904407003621*^9, 3.7099044325072527`*^9}, 3.709912186722497*^9, 3.7099915664381742`*^9, 3.710066828032853*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq1", " ", "=", " ", + RowBox[{"Eq1", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{ - RowBox[{"xHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"xHub", "[", "t", "]"}]}], "]"}], "-", "Tx"}], " ", "\[Equal]", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{ + RowBox[{"xHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"xHub", "[", "t", "]"}]}], "]"}], "-", "Tx"}], " ", "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.647614579441598*^9, 3.647614600924193*^9}, { - 3.647614866603059*^9, 3.647614887329845*^9}, {3.649440543327643*^9, + 3.647614866603059*^9, 3.647614887329845*^9}, {3.649440543327643*^9, 3.649440562004551*^9}, {3.649440749072403*^9, 3.6494407508768377`*^9}, { - 3.649440951552544*^9, 3.649440961562614*^9}, {3.709647639508596*^9, + 3.649440951552544*^9, 3.649440961562614*^9}, {3.709647639508596*^9, 3.709647646811626*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", "Tx"}], "+", - RowBox[{"mHub", " ", + RowBox[{"-", "Tx"}], "+", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{{3.647614875914831*^9, 3.647614889063602*^9}, - 3.647615089667774*^9, 3.647621228172324*^9, 3.6494405665702753`*^9, - 3.6494407535950603`*^9, 3.649440963907598*^9, 3.650026330119986*^9, - 3.650026435739256*^9, 3.6501137981351843`*^9, 3.6501139441069593`*^9, - 3.650114077086082*^9, 3.650114124242312*^9, 3.650121181242817*^9, - 3.650125466102409*^9, 3.650126350975716*^9, 3.650126433267352*^9, - 3.65012666153375*^9, 3.650127122695442*^9, 3.7096476604132757`*^9, - 3.70989348146432*^9, {3.709904409115304*^9, 3.709904432575056*^9}, + CellChangeTimes->{{3.647614875914831*^9, 3.647614889063602*^9}, + 3.647615089667774*^9, 3.647621228172324*^9, 3.6494405665702753`*^9, + 3.6494407535950603`*^9, 3.649440963907598*^9, 3.650026330119986*^9, + 3.650026435739256*^9, 3.6501137981351843`*^9, 3.6501139441069593`*^9, + 3.650114077086082*^9, 3.650114124242312*^9, 3.650121181242817*^9, + 3.650125466102409*^9, 3.650126350975716*^9, 3.650126433267352*^9, + 3.65012666153375*^9, 3.650127122695442*^9, 3.7096476604132757`*^9, + 3.70989348146432*^9, {3.709904409115304*^9, 3.709904432575056*^9}, 3.7099121867900543`*^9, 3.7099915665063963`*^9, 3.7100668281002493`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq1", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq1", ",", - RowBox[{"{", + RowBox[{"Eq1", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq1", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.647614906346649*^9, 3.647614911027494*^9}, { - 3.649440758351534*^9, 3.649440832552063*^9}, {3.709647671852079*^9, + 3.649440758351534*^9, 3.649440832552063*^9}, {3.709647671852079*^9, 3.7096476989589853`*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"mHub", " ", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "Tx"}]], "Output", CellChangeTimes->{ - 3.647614926214444*^9, 3.647615092999082*^9, 3.6476212284034767`*^9, - 3.649440684330357*^9, 3.649440807189921*^9, 3.6494408416192007`*^9, - 3.649440967306904*^9, 3.6500263368342657`*^9, 3.650026436202402*^9, - 3.6501138061520643`*^9, 3.65011394412993*^9, 3.650114077120083*^9, - 3.650114124276799*^9, 3.6501211812611856`*^9, 3.650125466152321*^9, - 3.6501263510091753`*^9, 3.650126433305233*^9, 3.650126661553743*^9, - 3.650127122727659*^9, 3.709647677516232*^9, 3.7096477099497433`*^9, - 3.709893503042837*^9, {3.7099044268732767`*^9, 3.709904443064567*^9}, + 3.647614926214444*^9, 3.647615092999082*^9, 3.6476212284034767`*^9, + 3.649440684330357*^9, 3.649440807189921*^9, 3.6494408416192007`*^9, + 3.649440967306904*^9, 3.6500263368342657`*^9, 3.650026436202402*^9, + 3.6501138061520643`*^9, 3.65011394412993*^9, 3.650114077120083*^9, + 3.650114124276799*^9, 3.6501211812611856`*^9, 3.650125466152321*^9, + 3.6501263510091753`*^9, 3.650126433305233*^9, 3.650126661553743*^9, + 3.650127122727659*^9, 3.709647677516232*^9, 3.7096477099497433`*^9, + 3.709893503042837*^9, {3.7099044268732767`*^9, 3.709904443064567*^9}, 3.709912195685472*^9, 3.709991574358623*^9, 3.7100668369914494`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq1", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq1", ",", RowBox[{ RowBox[{"xHub", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.6494409813785467`*^9, 3.649440988496232*^9}, { - 3.649441103290543*^9, 3.649441120586372*^9}, {3.70964771655809*^9, + 3.649441103290543*^9, 3.649441120586372*^9}, {3.70964771655809*^9, 3.709647717438199*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", - RowBox[{ - FractionBox["1", - RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], - RowBox[{"(", - RowBox[{"Tx", "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + RowBox[{ + FractionBox["1", + RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], + RowBox[{"(", + RowBox[{"Tx", "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", - CellChangeTimes->{3.650113944169817*^9, 3.650114077139044*^9, - 3.650114124304409*^9, 3.650121181294386*^9, 3.650125466202359*^9, - 3.650126351042288*^9, 3.650126433338122*^9, 3.65012666158496*^9, - 3.650127122765802*^9, 3.709647720541893*^9, 3.70989354325331*^9, - 3.709904443140407*^9, 3.709912195760069*^9, 3.709991574470261*^9, + CellChangeTimes->{3.650113944169817*^9, 3.650114077139044*^9, + 3.650114124304409*^9, 3.650121181294386*^9, 3.650125466202359*^9, + 3.650126351042288*^9, 3.650126433338122*^9, 3.65012666158496*^9, + 3.650127122765802*^9, 3.709647720541893*^9, 3.70989354325331*^9, + 3.709904443140407*^9, 3.709912195760069*^9, 3.709991574470261*^9, 3.710066837095171*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq2", " ", "=", " ", + RowBox[{"Eq2", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{ - RowBox[{"yHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"yHub", "[", "t", "]"}]}], "]"}], "-", "Ty"}], " ", "\[Equal]", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{ + RowBox[{"yHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"yHub", "[", "t", "]"}]}], "]"}], "-", "Ty"}], " ", "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.64944132623348*^9, 3.6494413532739983`*^9}, { 3.7096477376723948`*^9, 3.709647742814459*^9}}], @@ -1319,132 +1319,132 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", "Ty"}], "+", - RowBox[{"mHub", " ", + RowBox[{"-", "Ty"}], "+", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{{3.649441330224839*^9, 3.649441354551085*^9}, - 3.6500263369901237`*^9, 3.650026436270349*^9, 3.650113944223764*^9, - 3.65011407719658*^9, 3.650114124360874*^9, 3.6501211813409653`*^9, - 3.650125466295785*^9, 3.6501263511254063`*^9, 3.6501264334209547`*^9, - 3.6501266616626263`*^9, 3.650127122799155*^9, 3.7096477442006702`*^9, - 3.709893548368627*^9, 3.709904443200683*^9, 3.70991219582679*^9, + CellChangeTimes->{{3.649441330224839*^9, 3.649441354551085*^9}, + 3.6500263369901237`*^9, 3.650026436270349*^9, 3.650113944223764*^9, + 3.65011407719658*^9, 3.650114124360874*^9, 3.6501211813409653`*^9, + 3.650125466295785*^9, 3.6501263511254063`*^9, 3.6501264334209547`*^9, + 3.6501266616626263`*^9, 3.650127122799155*^9, 3.7096477442006702`*^9, + 3.709893548368627*^9, 3.709904443200683*^9, 3.70991219582679*^9, 3.7099915745328817`*^9, 3.7100668371544437`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq2", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq2", ",", - RowBox[{"{", + RowBox[{"Eq2", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq2", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.6494413919573603`*^9, 3.649441395319479*^9}, { 3.709647749463461*^9, 3.7096477703687468`*^9}}], @@ -1452,1008 +1452,1008 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"mHub", " ", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "Ty"}]], "Output", - CellChangeTimes->{{3.649441384353636*^9, 3.649441404905581*^9}, - 3.650026343707588*^9, 3.65002643654138*^9, 3.650113944258304*^9, - 3.650114077232009*^9, 3.6501141243879232`*^9, 3.650121181377021*^9, - 3.6501254663447037`*^9, 3.650126351158494*^9, 3.6501264334502573`*^9, - 3.6501266616822643`*^9, 3.650127122819181*^9, 3.709647784487842*^9, - 3.709893574056983*^9, 3.7099044610384007`*^9, 3.709912208124202*^9, + CellChangeTimes->{{3.649441384353636*^9, 3.649441404905581*^9}, + 3.650026343707588*^9, 3.65002643654138*^9, 3.650113944258304*^9, + 3.650114077232009*^9, 3.6501141243879232`*^9, 3.650121181377021*^9, + 3.6501254663447037`*^9, 3.650126351158494*^9, 3.6501264334502573`*^9, + 3.6501266616822643`*^9, 3.650127122819181*^9, 3.709647784487842*^9, + 3.709893574056983*^9, 3.7099044610384007`*^9, 3.709912208124202*^9, 3.7099915860370693`*^9, 3.710066849681682*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq2", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq2", ",", RowBox[{ RowBox[{"yHub", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.649441467695437*^9, 3.649441470984462*^9}, { 3.709647777488635*^9, 3.709647783103689*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", - RowBox[{ - FractionBox["1", - RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], - RowBox[{"(", - RowBox[{"Ty", "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + RowBox[{ + FractionBox["1", + RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], + RowBox[{"(", + RowBox[{"Ty", "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", - CellChangeTimes->{3.650113944303441*^9, 3.650114077264604*^9, - 3.650114124408894*^9, 3.65012118141026*^9, 3.650125466395176*^9, - 3.650126351191698*^9, 3.650126433471068*^9, 3.650126661717382*^9, - 3.650127122848145*^9, 3.7096477862767563`*^9, 3.709893604933693*^9, - 3.709904461095764*^9, 3.709912208203537*^9, 3.70999158617177*^9, + CellChangeTimes->{3.650113944303441*^9, 3.650114077264604*^9, + 3.650114124408894*^9, 3.65012118141026*^9, 3.650125466395176*^9, + 3.650126351191698*^9, 3.650126433471068*^9, 3.650126661717382*^9, + 3.650127122848145*^9, 3.7096477862767563`*^9, 3.709893604933693*^9, + 3.709904461095764*^9, 3.709912208203537*^9, 3.70999158617177*^9, 3.71006684977703*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq3", " ", "=", " ", + RowBox[{"Eq3", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", "-", " ", "Torque"}], + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{ + RowBox[{"theta", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", "-", " ", "Torque"}], " ", "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.649441490019846*^9, 3.649441526048011*^9}, { - 3.6494416329455757`*^9, 3.649441639953199*^9}, {3.709647999226737*^9, + 3.6494416329455757`*^9, 3.649441639953199*^9}, {3.709647999226737*^9, 3.709648001657591*^9}, {3.7098936614514217`*^9, 3.709893666760483*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", "Torque"}], "-", + RowBox[{"-", "Torque"}], "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "-", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", - RowBox[{"IHub", " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{"IHub", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"ISP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"ISP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"ISP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"ISP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{3.6494416430996*^9, 3.650026343837957*^9, - 3.6500264366053953`*^9, 3.6501139443755608`*^9, 3.6501140773356533`*^9, - 3.650114124465275*^9, 3.650121181475136*^9, 3.650125466495*^9, - 3.6501263512510633`*^9, 3.6501264335329323`*^9, 3.6501266617780237`*^9, - 3.650127122873707*^9, 3.7096478122779713`*^9, 3.709648036046701*^9, - 3.7098936714787397`*^9, 3.7099044611434526`*^9, 3.7099122082757*^9, + CellChangeTimes->{3.6494416430996*^9, 3.650026343837957*^9, + 3.6500264366053953`*^9, 3.6501139443755608`*^9, 3.6501140773356533`*^9, + 3.650114124465275*^9, 3.650121181475136*^9, 3.650125466495*^9, + 3.6501263512510633`*^9, 3.6501264335329323`*^9, 3.6501266617780237`*^9, + 3.650127122873707*^9, 3.7096478122779713`*^9, 3.709648036046701*^9, + 3.7098936714787397`*^9, 3.7099044611434526`*^9, 3.7099122082757*^9, 3.709991586226976*^9, 3.710066849868989*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq3", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq3", ",", - RowBox[{"{", + RowBox[{"Eq3", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq3", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.649441701396606*^9, 3.649441704434289*^9}, { 3.709647818203969*^9, 3.709647839090629*^9}}], @@ -2461,687 +2461,687 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"IHub", "+", "ISP1", "+", "ISP2"}], ")"}], " ", + RowBox[{"(", + RowBox[{"IHub", "+", "ISP1", "+", "ISP2"}], ")"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - SuperscriptBox["Rhinge1", "2"], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + SuperscriptBox["Rhinge1", "2"], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP2", " ", - SuperscriptBox["Rhinge2", "2"], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP2", " ", + SuperscriptBox["Rhinge2", "2"], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"ISP1", " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"ISP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"ISP2", " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"ISP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", - RowBox[{"Torque", "+", + MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"Torque", "+", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}]}]], "Output", - CellChangeTimes->{3.649441711039947*^9, 3.650026348595045*^9, - 3.650026437159946*^9, 3.6501139490906878`*^9, 3.65011408062339*^9, - 3.650114124719529*^9, 3.650121181507895*^9, 3.650125466545499*^9, - 3.650126351285775*^9, 3.650126434062338*^9, 3.650126661823897*^9, - 3.650127122910791*^9, 3.709647853944254*^9, 3.70964806140898*^9, - 3.709893706843128*^9, 3.709904495809168*^9, 3.70991222827071*^9, + CellChangeTimes->{3.649441711039947*^9, 3.650026348595045*^9, + 3.650026437159946*^9, 3.6501139490906878`*^9, 3.65011408062339*^9, + 3.650114124719529*^9, 3.650121181507895*^9, 3.650125466545499*^9, + 3.650126351285775*^9, 3.650126434062338*^9, 3.650126661823897*^9, + 3.650127122910791*^9, 3.709647853944254*^9, 3.70964806140898*^9, + 3.709893706843128*^9, 3.709904495809168*^9, 3.70991222827071*^9, 3.7099916041330023`*^9, 3.710066868405623*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq3", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq3", ",", RowBox[{ RowBox[{"theta", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.649441744480874*^9, 3.649441755989689*^9}, { 3.7098939348877172`*^9, 3.709893936213263*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{ - RowBox[{"(", - RowBox[{"Torque", "-", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{"Torque", "-", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"ISP1", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"ISP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"ISP2", " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"ISP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}], "/", - RowBox[{"(", - RowBox[{"IHub", "+", "ISP1", "+", "ISP2", "+", + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}], "/", + RowBox[{"(", + RowBox[{"IHub", "+", "ISP1", "+", "ISP2", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", + SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", - RowBox[{"mSP1", " ", - SuperscriptBox["Rhinge1", "2"]}], "+", - RowBox[{"mSP2", " ", - SuperscriptBox["Rhinge2", "2"]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}]}]}], "}"}], + SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", + RowBox[{"mSP1", " ", + SuperscriptBox["Rhinge1", "2"]}], "+", + RowBox[{"mSP2", " ", + SuperscriptBox["Rhinge2", "2"]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", CellChangeTimes->{ - 3.650113949170033*^9, 3.650114080665533*^9, 3.650114124738514*^9, - 3.650121181527328*^9, 3.6501254665943823`*^9, 3.6501263513202057`*^9, - 3.65012643412017*^9, 3.650126661855892*^9, 3.650127122950385*^9, - 3.709647869432714*^9, 3.70964807076822*^9, {3.70989392998943*^9, - 3.709893938934258*^9}, 3.709904495937688*^9, 3.7099122283751574`*^9, + 3.650113949170033*^9, 3.650114080665533*^9, 3.650114124738514*^9, + 3.650121181527328*^9, 3.6501254665943823`*^9, 3.6501263513202057`*^9, + 3.65012643412017*^9, 3.650126661855892*^9, 3.650127122950385*^9, + 3.709647869432714*^9, 3.70964807076822*^9, {3.70989392998943*^9, + 3.709893938934258*^9}, 3.709904495937688*^9, 3.7099122283751574`*^9, 3.7099916042609997`*^9, 3.710066868501666*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq4", " ", "=", " ", + RowBox[{"Eq4", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", + RowBox[{"D", "[", + RowBox[{"Lag", ",", RowBox[{ RowBox[{"theta1", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", "+", - RowBox[{"c1", "*", + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", "+", + RowBox[{"c1", "*", RowBox[{ - RowBox[{"theta1", "'"}], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"theta1", "'"}], "[", "t", "]"}]}]}], "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.649441857154335*^9, 3.6494418688045387`*^9}, { - 3.649442193911736*^9, 3.649442218793474*^9}, {3.650026298715227*^9, + 3.649442193911736*^9, 3.649442218793474*^9}, {3.650026298715227*^9, 3.650026299976062*^9}, {3.7096481588067293`*^9, 3.709648159022726*^9}, { 3.709893944199317*^9, 3.7098939571022673`*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k1", " ", - RowBox[{"theta1", "[", "t", "]"}]}], "+", - RowBox[{"c1", " ", + RowBox[{"k1", " ", + RowBox[{"theta1", "[", "t", "]"}]}], "+", + RowBox[{"c1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", - RowBox[{"ISP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{"ISP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{3.6494418700740137`*^9, 3.6494422226433496`*^9, - 3.6500263034503527`*^9, 3.650026348745247*^9, 3.650026437210599*^9, - 3.650113949253563*^9, 3.6501140807222767`*^9, 3.650114124804435*^9, - 3.6501211815963173`*^9, 3.650125466805504*^9, 3.650126351387097*^9, - 3.650126434183649*^9, 3.650126661922491*^9, 3.650127122999509*^9, - 3.709648163644412*^9, 3.709893959183962*^9, 3.709904495992156*^9, + CellChangeTimes->{3.6494418700740137`*^9, 3.6494422226433496`*^9, + 3.6500263034503527`*^9, 3.650026348745247*^9, 3.650026437210599*^9, + 3.650113949253563*^9, 3.6501140807222767`*^9, 3.650114124804435*^9, + 3.6501211815963173`*^9, 3.650125466805504*^9, 3.650126351387097*^9, + 3.650126434183649*^9, 3.650126661922491*^9, 3.650127122999509*^9, + 3.709648163644412*^9, 3.709893959183962*^9, 3.709904495992156*^9, 3.709912228430462*^9, 3.709991604325783*^9, 3.710066868562544*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq4", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq4", ",", - RowBox[{"{", + RowBox[{"Eq4", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq4", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.649441900454019*^9, 3.6494419029884357`*^9}, { 3.7096481750058613`*^9, 3.709648196975309*^9}}], @@ -3149,489 +3149,489 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k1", " ", - RowBox[{"theta1", "[", "t", "]"}]}], "+", - RowBox[{"c1", " ", + RowBox[{"k1", " ", + RowBox[{"theta1", "[", "t", "]"}]}], "+", + RowBox[{"c1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP1", "+", + RowBox[{"(", + RowBox[{"ISP1", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP1", "+", + RowBox[{"(", + RowBox[{"ISP1", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}]}], ")"}], " ", + SuperscriptBox["d1", "2"], " ", "mSP1"}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}]], "Output", - CellChangeTimes->{3.649441907035841*^9, 3.649442235781934*^9, - 3.650026351853695*^9, 3.650026437327095*^9, 3.6501139515691*^9, - 3.650114081436472*^9, 3.650114125027214*^9, 3.650121181630754*^9, - 3.6501254668447866`*^9, 3.650126351421269*^9, 3.6501264342223673`*^9, - 3.650126662116922*^9, 3.650127123045363*^9, 3.709648349946191*^9, - 3.7098939733623323`*^9, 3.709904501105431*^9, 3.70991223188218*^9, + CellChangeTimes->{3.649441907035841*^9, 3.649442235781934*^9, + 3.650026351853695*^9, 3.650026437327095*^9, 3.6501139515691*^9, + 3.650114081436472*^9, 3.650114125027214*^9, 3.650121181630754*^9, + 3.6501254668447866`*^9, 3.650126351421269*^9, 3.6501264342223673`*^9, + 3.650126662116922*^9, 3.650127123045363*^9, 3.709648349946191*^9, + 3.7098939733623323`*^9, 3.709904501105431*^9, 3.70991223188218*^9, 3.709991607844681*^9, 3.710066872120063*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq4", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq4", ",", RowBox[{ RowBox[{"theta1", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.649441926418256*^9, 3.64944194178972*^9}, { 3.7098939782495117`*^9, 3.7098939802637053`*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{ - FractionBox["1", - RowBox[{"ISP1", "+", + FractionBox["1", + RowBox[{"ISP1", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}]}]], - RowBox[{"(", + SuperscriptBox["d1", "2"], " ", "mSP1"}]}]], + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "k1"}], " ", - RowBox[{"theta1", "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "k1"}], " ", + RowBox[{"theta1", "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"c1", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"c1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"ISP1", " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"ISP1", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", - CellChangeTimes->{3.6501139516163483`*^9, 3.6501140814531193`*^9, - 3.6501141250632668`*^9, 3.650121181649961*^9, 3.650125466894619*^9, - 3.650126351454706*^9, 3.6501264342548857`*^9, 3.650126662157115*^9, - 3.650127123079134*^9, 3.709648353498505*^9, 3.709893982607676*^9, - 3.709904501156394*^9, 3.7099122319746513`*^9, 3.709991607949237*^9, + CellChangeTimes->{3.6501139516163483`*^9, 3.6501140814531193`*^9, + 3.6501141250632668`*^9, 3.650121181649961*^9, 3.650125466894619*^9, + 3.650126351454706*^9, 3.6501264342548857`*^9, 3.650126662157115*^9, + 3.650127123079134*^9, 3.709648353498505*^9, 3.709893982607676*^9, + 3.709904501156394*^9, 3.7099122319746513`*^9, 3.709991607949237*^9, 3.7100668721947823`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq5", " ", "=", " ", + RowBox[{"Eq5", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", + RowBox[{"D", "[", + RowBox[{"Lag", ",", RowBox[{ RowBox[{"theta2", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", "+", - RowBox[{"c2", "*", + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", "+", + RowBox[{"c2", "*", RowBox[{ - RowBox[{"theta2", "'"}], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"theta2", "'"}], "[", "t", "]"}]}]}], "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.649441974232287*^9, 3.649441974602599*^9}, { - 3.649442024415121*^9, 3.6494420297328167`*^9}, {3.6494422687274637`*^9, + 3.649442024415121*^9, 3.6494420297328167`*^9}, {3.6494422687274637`*^9, 3.649442279760858*^9}, {3.650026425804619*^9, 3.650026427138609*^9}, { - 3.7096483649009*^9, 3.709648365220821*^9}, {3.709893986529037*^9, + 3.7096483649009*^9, 3.709648365220821*^9}, {3.709893986529037*^9, 3.7098939986630163`*^9}, {3.709894594889039*^9, 3.7098945981436768`*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k2", " ", - RowBox[{"theta2", "[", "t", "]"}]}], "+", - RowBox[{"c2", " ", + RowBox[{"k2", " ", + RowBox[{"theta2", "[", "t", "]"}]}], "+", + RowBox[{"c2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", - RowBox[{"ISP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{"ISP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", CellChangeTimes->{ 3.649442031278104*^9, 3.649442281353739*^9, 3.650026351903562*^9, { - 3.650026428718976*^9, 3.650026437375152*^9}, 3.650113951691698*^9, - 3.650114081506241*^9, 3.650114125109868*^9, 3.6501211816981277`*^9, - 3.650125466995118*^9, 3.650126351524879*^9, 3.6501264343228188`*^9, - 3.650126662222809*^9, 3.650127123116081*^9, 3.709648368112361*^9, - 3.7098946055579844`*^9, 3.7099045012015133`*^9, 3.709912232028257*^9, + 3.650026428718976*^9, 3.650026437375152*^9}, 3.650113951691698*^9, + 3.650114081506241*^9, 3.650114125109868*^9, 3.6501211816981277`*^9, + 3.650125466995118*^9, 3.650126351524879*^9, 3.6501264343228188`*^9, + 3.650126662222809*^9, 3.650127123116081*^9, 3.709648368112361*^9, + 3.7098946055579844`*^9, 3.7099045012015133`*^9, 3.709912232028257*^9, 3.709991608051722*^9, 3.710066872251883*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq5", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq5", ",", - RowBox[{"{", + RowBox[{"Eq5", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq5", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.649442050675033*^9, 3.6494420532928762`*^9}, { 3.709648373091796*^9, 3.7096483936980247`*^9}}], @@ -3639,146 +3639,146 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k2", " ", - RowBox[{"theta2", "[", "t", "]"}]}], "+", - RowBox[{"c2", " ", + RowBox[{"k2", " ", + RowBox[{"theta2", "[", "t", "]"}]}], "+", + RowBox[{"c2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP2", "+", + RowBox[{"(", + RowBox[{"ISP2", "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP2", "+", + RowBox[{"(", + RowBox[{"ISP2", "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}]}], ")"}], " ", + SuperscriptBox["d2", "2"], " ", "mSP2"}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}]], "Output", - CellChangeTimes->{3.649442057869686*^9, 3.649442290890633*^9, - 3.650026354551691*^9, 3.650026439581772*^9, 3.650113954518001*^9, - 3.650114081534142*^9, 3.650114125129826*^9, 3.650121181727723*^9, - 3.650125467045177*^9, 3.6501263515581093`*^9, 3.650126435015355*^9, - 3.650126662243575*^9, 3.650127123150807*^9, 3.7096483991918497`*^9, - 3.709894614262093*^9, 3.709904506228737*^9, 3.7099122352551403`*^9, + CellChangeTimes->{3.649442057869686*^9, 3.649442290890633*^9, + 3.650026354551691*^9, 3.650026439581772*^9, 3.650113954518001*^9, + 3.650114081534142*^9, 3.650114125129826*^9, 3.650121181727723*^9, + 3.650125467045177*^9, 3.6501263515581093`*^9, 3.650126435015355*^9, + 3.650126662243575*^9, 3.650127123150807*^9, 3.7096483991918497`*^9, + 3.709894614262093*^9, 3.709904506228737*^9, 3.7099122352551403`*^9, 3.709991611016552*^9, 3.7100668766011467`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq5", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq5", ",", RowBox[{ RowBox[{"theta2", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.6494421764678802`*^9, 3.649442183039885*^9}, { 3.709894635218473*^9, 3.7098946388520184`*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{ - FractionBox["1", - RowBox[{"ISP2", "+", + FractionBox["1", + RowBox[{"ISP2", "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}]}]], - RowBox[{"(", + SuperscriptBox["d2", "2"], " ", "mSP2"}]}]], + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "k2"}], " ", - RowBox[{"theta2", "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "k2"}], " ", + RowBox[{"theta2", "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"c2", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"c2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"ISP2", " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"ISP2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", CellChangeTimes->{ - 3.650113954545787*^9, 3.650114081552614*^9, 3.65011412516045*^9, - 3.650121181746997*^9, 3.6501254670958147`*^9, 3.6501263515786343`*^9, - 3.650126435034376*^9, 3.650126662274653*^9, 3.6501271231820707`*^9, - 3.709648402930709*^9, {3.709894632465825*^9, 3.709894639761241*^9}, - 3.709904506291833*^9, 3.709912235339995*^9, 3.7099916111335163`*^9, + 3.650113954545787*^9, 3.650114081552614*^9, 3.65011412516045*^9, + 3.650121181746997*^9, 3.6501254670958147`*^9, 3.6501263515786343`*^9, + 3.650126435034376*^9, 3.650126662274653*^9, 3.6501271231820707`*^9, + 3.709648402930709*^9, {3.709894632465825*^9, 3.709894639761241*^9}, + 3.709904506291833*^9, 3.709912235339995*^9, 3.7099916111335163`*^9, 3.7100668766653957`*^9}] }, Open ]] }, Open ]] @@ -3911,4 +3911,3 @@ Cell[139379, 3713, 2762, 68, 197, "Output"] } ] *) - diff --git a/src/simulation/dynamics/HingedRigidBodies/_Documentation/secModelDescription.tex b/src/simulation/dynamics/HingedRigidBodies/_Documentation/secModelDescription.tex index e5bd94377c..28fa06d1a1 100644 --- a/src/simulation/dynamics/HingedRigidBodies/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/HingedRigidBodies/_Documentation/secModelDescription.tex @@ -4,7 +4,7 @@ \subsection{Introduction} The hinged rigid body class is an instantiation of the state effector abstract class. The state effector abstract class is a base class for modules that have dynamic states or degrees of freedom with respect to the rigid body hub. Examples of these would be reaction wheels, variable speed control moment gyroscopes, fuel slosh particles, etc. Since the state effectors are attached to the hub, the state effectors are directly affecting the hub as well as the hub is back affecting the state effectors. -Specifically, a hinged rigid body state effector is a rigid body that has a diagonal inertia with respect to its $\mathcal{S}_i$ frame as seen in Figure~\ref{fig:FlexFigure}. It is attached to the hub through a hinge with a linear torsional spring and linear damping term. An optional motor torque command can be used to actuate the panel. The dynamics of this multi-body problem have been derived and can be seen in Reference~\citenum{Allard2016rz}. The derivation is general for $N$ number of panels attached to the hub but does not allow for multiple interconnected panels. +Specifically, a hinged rigid body state effector is a rigid body that has a diagonal inertia with respect to its $\mathcal{S}_i$ frame as seen in Figure~\ref{fig:FlexFigure}. It is attached to the hub through a hinge with a linear torsional spring and linear damping term. An optional motor torque command can be used to actuate the panel. The dynamics of this multi-body problem have been derived and can be seen in Reference~\citenum{Allard2016rz}. The derivation is general for $N$ number of panels attached to the hub but does not allow for multiple interconnected panels. \begin{figure}[htbp] \centerline{ @@ -15,7 +15,7 @@ \subsection{Introduction} \subsection{Equations of Motion} -The following equations of motion (EOMs) are pulled from Reference~\citenum{Allard2016rz} for convenience. Equation~\eqref{eq:Rbddot3} is the spacecraft translational EOM, Equation~\eqref{eq:Final6} is the spacecraft rotational EOM, and Equation~\eqref{eq:solar_panel_final3} is the hinged rigid body rotational EOM. These are the coupled nonlinear EOMs that need to be integrated in the simulation. +The following equations of motion (EOMs) are pulled from Reference~\citenum{Allard2016rz} for convenience. Equation~\eqref{eq:Rbddot3} is the spacecraft translational EOM, Equation~\eqref{eq:Final6} is the spacecraft rotational EOM, and Equation~\eqref{eq:solar_panel_final3} is the hinged rigid body rotational EOM. These are the coupled nonlinear EOMs that need to be integrated in the simulation. \begin{multline} m_{\text{sc}} \ddot{\bm r}_{B/N}-m_{\text{sc}} [\tilde{\bm{c}}]\dot{\bm\omega}_{\cal B/N}+\sum_{i}^{N}m_{\text{sp}_i} d_i \bm{\hat{s}}_{i,3}\ddot{\theta}_i = \bm F_{\textnormal{ext}} - 2 m_{\text{sc}} [\tilde{\bm\omega}_{\cal B/N}]\bm{c}'\\ @@ -25,16 +25,16 @@ \subsection{Equations of Motion} \begin{multline} m_{\text{sc}}[\tilde{\bm{c}}]\ddot{\bm r}_{B/N}+[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} +\sum\limits_{i}^{N}\biggl\lbrace I_{s_i,2}\bm{\hat{h}}_{i,2}+m_{\text{sp}_i}d_i [\tilde{\bm{r}}_{S_i/B}] \bm{\hat{s}}_{i,3}\biggr\rbrace\ddot{\theta}_i = \\ - -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} + -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} - \sum\limits_{i}^{N}\biggl\lbrace\dot{\theta}_i [\bm{\tilde{\omega}}_{\cal B/N}] \left(I_{s_i,2} \bm{\hat{h}}_{i,2}+m_{\text{sp}_i} d_i [\tilde{\bm{r}}_{S_i/B}] \hat{\bm s}_{i,3}\right)\\ +m_{\text{sp}_i}d_i\dot{\theta}_i^2[\tilde{\bm{r}}_{S_i/B}] \bm{\hat{s}}_{i,1} \biggr\rbrace + \bm{L}_B \label{eq:Final6} \end{multline} \begin{multline} m_{\text{sp}_i} d_i \hat{\bm s}_{i,3}^{T} \ddot{\bm r}_{B/N}+ \biggl[\left(I_{s_{i,2}} + m_{\text{sp}_i}d_i^{2}\right) \hat{\bm s}_{i,2}^{T}-m_{\text{sp}_i} d_i \hat{\bm s}_{i,3}^{T} [\tilde{\bm r}_{H_i/B}]\biggr] \dot{\bm\omega}_{\cal B/N} \\ -+ \left( I_{s_{i,2}} + m_{\text{sp}_i} d_i^{2} \right) \ddot \theta_i ++ \left( I_{s_{i,2}} + m_{\text{sp}_i} d_i^{2} \right) \ddot \theta_i = u_i - k_i \theta_i - c_i \dot\theta_i + \hat{\bm s}_{i,2}^T \bm \tau_{\text{ext},H_i} + \left( I_{s_{i,3}} - I_{s_{i,1}} + m_{\text{sp}_i}d_i^{2}\right) \omega_{s_{i,3}} \omega_{s_{i,1}}\\ -- m_{\text{sp}_i} d_i \hat{\bm s}_{i,3}^{T} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}] \bm r_{H_i/B} +- m_{\text{sp}_i} d_i \hat{\bm s}_{i,3}^{T} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}] \bm r_{H_i/B} \label{eq:solar_panel_final3} \end{multline} where $u_i$ is the optional motor torque. @@ -54,12 +54,12 @@ \subsection{Back Substitution Method} \begin{multline} \Big[m_{\text{sc}}[\tilde{\bm{c}}] +\sum\limits_{i=1}^{N}\big( I_{s_{i,2}}\bm{\hat{s}}_{i,2}+m_{\text{sp}_i}d_i [\tilde{\bm{r}}_{S_{c,i}/B}] \bm{\hat{s}}_{i,3}\big)\bm a_{\theta_i}^T \Big]\ddot{\bm r}_{B/N}\\ +\Big[[I_{\text{sc},B}]+\sum\limits_{i=1}^{N}\big( I_{s_{i,2}}\bm{\hat{s}}_{i,2}+m_{\text{sp}_i}d_i [\tilde{\bm{r}}_{S_{c,i}/B}] \bm{\hat{s}}_{i,3}\big) \bm b_{\theta_i}^T\Big] \dot{\bm\omega}_{\cal B/N} -= --[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} += +-[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} \\ -\sum\limits_{i=1}^{N}\biggl\lbrace\big(\dot{\theta}_i [\bm{\tilde{\omega}}_{\cal B/N}] +c_{\theta_i} [I_{3\times3}]\big) \left(I_{s_{i,2}} \bm{\hat{s}}_{i,2}+m_{\text{sp}_i} d_i [\tilde{\bm{r}}_{S_{c,i}/B}] \hat{\bm s}_{i,3}\right) +m_{\text{sp}_i}d_i\dot{\theta}_i^2[\tilde{\bm{r}}_{S_{c,i}/B}] \bm{\hat{s}}_{i,1} \biggr\rbrace + \bm{L}_B \label{eq:Final9} -\end{multline} +\end{multline} With the following definitions: @@ -94,12 +94,12 @@ \subsection{Back Substitution Method} \label{eq:backSub} \end{equation} -Finally, the hinged rigid body model must make ``contributions" to the matrices defined in Equations~\eqref{eq:backSub}. These contributions are defined in the following equations: +Finally, the hinged rigid body model must make ``contributions" to the matrices defined in Equations~\eqref{eq:backSub}. These contributions are defined in the following equations: \begin{align} [A_{\textnormal{contr}}] &= m_{\text{sp}_i}d_i \bm{\hat{s}}_{i,3} \bm a_{\theta_i}^T \\ -[B_{\textnormal{contr}}] &= m_{\text{sp}_i}d_i \bm{\hat{s}}_{i,3} \bm b_{\theta_i}^T +[B_{\textnormal{contr}}] &= m_{\text{sp}_i}d_i \bm{\hat{s}}_{i,3} \bm b_{\theta_i}^T \\ [C_{\textnormal{contr}}] &= \big( I_{s_{i,2}}\bm{\hat{s}}_{i,2}+m_{\text{sp}_i}d_i [\tilde{\bm{r}}_{S_{c,i}/B}] \bm{\hat{s}}_{i,3}\big)\bm a_{\theta_i}^T \\ @@ -107,7 +107,7 @@ \subsection{Back Substitution Method} \\ \bm v_{\text{trans,contr}} &= -\Big(m_{\text{sp}_i}d_i \dot{\theta}_i^2 \bm{\hat{s}}_{i,1}+m_{\text{sp}_i}d_i c_{\theta_i} \bm{\hat{s}}_{i,3} \Big) \\ -\bm v_{\text{rot,contr}} &= -\biggl\lbrace\big(\dot{\theta}_i [\bm{\tilde{\omega}}_{\cal B/N}] +c_{\theta_i} [I_{3\times3}]\big) \left(I_{s_{i,2}} \bm{\hat{s}}_{i,2}+m_{\text{sp}_i} d_i [\tilde{\bm{r}}_{S_{c,i}/B}] \hat{\bm s}_{i,3}\right) +m_{\text{sp}_i}d_i\dot{\theta}_i^2[\tilde{\bm{r}}_{S_{c,i}/B}] \bm{\hat{s}}_{i,1} \biggr\rbrace +\bm v_{\text{rot,contr}} &= -\biggl\lbrace\big(\dot{\theta}_i [\bm{\tilde{\omega}}_{\cal B/N}] +c_{\theta_i} [I_{3\times3}]\big) \left(I_{s_{i,2}} \bm{\hat{s}}_{i,2}+m_{\text{sp}_i} d_i [\tilde{\bm{r}}_{S_{c,i}/B}] \hat{\bm s}_{i,3}\right) +m_{\text{sp}_i}d_i\dot{\theta}_i^2[\tilde{\bm{r}}_{S_{c,i}/B}] \bm{\hat{s}}_{i,1} \biggr\rbrace \end{align} The final equation that is needed is: diff --git a/src/simulation/dynamics/HingedRigidBodies/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/HingedRigidBodies/_Documentation/secModelFunctions.tex index 9f3f8c99bc..bfe51a29c7 100644 --- a/src/simulation/dynamics/HingedRigidBodies/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/HingedRigidBodies/_Documentation/secModelFunctions.tex @@ -24,4 +24,4 @@ \section{Model Assumptions and Limitations} \item The hinge does not have travel limits, therefore if the spring is not stiff enough it will unrealistically travel through bounds such as running into the spacecraft hub \item The EOMs are nonlinear equations of motion, therefore there can be inaccuracies (and divergence) that result from integration. Having a time step of $<= 0.10\ \text{sec}$ is recommended, but this also depends on the natural frequency of the system \item When trying to match the frequency of a physical appended body, note that the natural frequency of the coupled system will be different than the appending body flexing by itself -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/simulation/dynamics/HingedRigidBodies/_Documentation/secTest.tex b/src/simulation/dynamics/HingedRigidBodies/_Documentation/secTest.tex index bb44b43a39..b0dd6c6c80 100644 --- a/src/simulation/dynamics/HingedRigidBodies/_Documentation/secTest.tex +++ b/src/simulation/dynamics/HingedRigidBodies/_Documentation/secTest.tex @@ -2,7 +2,7 @@ \section{Test Description and Success Criteria} This test is located in \texttt{simulation/dynamics/HingedRigidBodies/UnitTest/\newline test\_hingedRigidBodyStateEffector.py}. In this integrated test there are two hinged rigid bodies connected to the spacecraft hub. Depending on the scenario, there are different success criteria. These are outlined in the following subsections: \subsection{Gravity and no damping scenario} -In this test the simulation is placed into orbit around Earth with point gravity and has no damping in the hinged rigid bodies. The following parameters are being tested. +In this test the simulation is placed into orbit around Earth with point gravity and has no damping in the hinged rigid bodies. The following parameters are being tested. \begin{itemize} \item Conservation of orbital angular momentum \item Conservation of orbital energy @@ -34,7 +34,7 @@ \subsection{No gravity with damping scenario} \subsection{Steady State Deflection Verification Scenario} -The BOE calculation for the steady state deflection can be seen in Fig.~\ref{fig:BOEThetaSS}. The resulting steady state deflection does not have a closed form solution so a root solving function must be used to converge on the solution. A Newton-Raphson method was chosen and the success criteria for this test is whether Basilisk gives the same results as this BOE calculation within a certain tolerance. The spacecraft has a constant force applied through the whole simulation with the hinged rigid bodies initially undeflected and they have damping. The force is always applied through the center of mass of the spacecraft and results in no rotation of the spacecraft. +The BOE calculation for the steady state deflection can be seen in Fig.~\ref{fig:BOEThetaSS}. The resulting steady state deflection does not have a closed form solution so a root solving function must be used to converge on the solution. A Newton-Raphson method was chosen and the success criteria for this test is whether Basilisk gives the same results as this BOE calculation within a certain tolerance. The spacecraft has a constant force applied through the whole simulation with the hinged rigid bodies initially undeflected and they have damping. The force is always applied through the center of mass of the spacecraft and results in no rotation of the spacecraft. \begin{figure}[htbp] \centerline{ @@ -65,9 +65,9 @@ \subsubsection{Frequency Test Description} \end{equation} \begin{equation} -m_{\text{sp}_i} d_i \hat{\bm s}_{i,3}^{T} \ddot{\bm r}_{B/N} -+ \left( I_{s_{i,2}} + m_{\text{sp}_i} d_i^{2} \right) \ddot \theta_i -= - k_i \theta_i - c_i \dot\theta_i + \hat{\bm s}_{i,2}^T \bm \tau_{\text{ext},H_i} +m_{\text{sp}_i} d_i \hat{\bm s}_{i,3}^{T} \ddot{\bm r}_{B/N} ++ \left( I_{s_{i,2}} + m_{\text{sp}_i} d_i^{2} \right) \ddot \theta_i += - k_i \theta_i - c_i \dot\theta_i + \hat{\bm s}_{i,2}^T \bm \tau_{\text{ext},H_i} \label{eq:solar_panel_final9} \end{equation} Next, assumptions \ref{item:4}-\ref{item:5} are applied: @@ -77,9 +77,9 @@ \subsubsection{Frequency Test Description} \end{equation} \begin{equation} -m_{\text{sp}_i} d_i \hat{\bm h}_{i,3}^{T} \ddot{\bm r}_{B/N} -+ \left( I_{s_{i,2}} + m_{\text{sp}_i} d_i^{2} \right) \ddot \theta_i -= - k_i \theta_i - c_i \dot\theta_i +m_{\text{sp}_i} d_i \hat{\bm h}_{i,3}^{T} \ddot{\bm r}_{B/N} ++ \left( I_{s_{i,2}} + m_{\text{sp}_i} d_i^{2} \right) \ddot \theta_i += - k_i \theta_i - c_i \dot\theta_i \label{eq:solar_panel_final10} \end{equation} Finally, knowing that the force is being directed along the $\hat{\bm b}_2$ axis and that the spacecraft will not rotate, the equations simplify to: @@ -89,9 +89,9 @@ \subsubsection{Frequency Test Description} \end{equation} \begin{equation} -m_{\text{sp}_i} d_i \ddot{y}_{B/N} -+ \left( I_{s_{i,2}} + m_{\text{sp}_i} d_i^{2} \right) \ddot \theta_i -= - k_i \theta_i - c_i \dot\theta_i +m_{\text{sp}_i} d_i \ddot{y}_{B/N} ++ \left( I_{s_{i,2}} + m_{\text{sp}_i} d_i^{2} \right) \ddot \theta_i += - k_i \theta_i - c_i \dot\theta_i \label{eq:solar_panel_final12} \end{equation} Converting these equations to state space: @@ -148,7 +148,7 @@ \subsubsection{Frequency Test Description} [\tilde{A}] = [M][A] \end{equation} -Finding the eigenvalues of $[\tilde{A}]$ will describe the coupled natural frequencies of the combined system. The integrated test for this scenario ensures that the analytical coupled frequency of oscillation matches the frequency obtained from the simulation. +Finding the eigenvalues of $[\tilde{A}]$ will describe the coupled natural frequencies of the combined system. The integrated test for this scenario ensures that the analytical coupled frequency of oscillation matches the frequency obtained from the simulation. \subsubsection{Max Deflection While Force is Being Applied} @@ -157,7 +157,7 @@ \subsubsection{Max Deflection While Force is Being Applied} \begin{equation} \theta_{\textnormal{max}} = 2 \theta_{\textnormal{SS}} \end{equation} -which uses the definition of $\theta_{\textnormal{SS}}$ from Fig.~\ref{fig:BOEThetaSS}. +which uses the definition of $\theta_{\textnormal{SS}}$ from Fig.~\ref{fig:BOEThetaSS}. \subsubsection{Max Deflection While Force is Not Being Applied} @@ -204,7 +204,7 @@ \section{Test Parameters} \caption{Spacecraft Hub Parameters} \label{tab:hub} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c | c | c |} % Column formatting, + \begin{tabular}{| c | c | c | c |} % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -226,7 +226,7 @@ \section{Test Parameters} \caption{Hinged Rigid Body 1 Parameters} \label{tab:panel1} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c | c | c |} % Column formatting, + \begin{tabular}{| c | c | c | c |} % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -260,7 +260,7 @@ \section{Test Parameters} \caption{Hinged Rigid Body 2 Parameters} \label{tab:panel2} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c | c | c |} % Column formatting, + \begin{tabular}{| c | c | c | c |} % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -294,7 +294,7 @@ \section{Test Parameters} \caption{Initial Conditions for Energy Momentum Conservation Scenarios} \label{tab:initial} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c | c | c |} % Column formatting, + \begin{tabular}{| c | c | c | c |} % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -307,7 +307,7 @@ \section{Test Parameters} (Panel 2) thetaDotInit & (Panel 2) Initial $\dot{\theta}$ & 0.0 & deg \\ \hline r\_CN\_NInit & Initial Position of S/C (gravity scenarios) & $\begin{bmatrix} - -4020339 & 7490567 & 5248299 + -4020339 & 7490567 & 5248299 \end{bmatrix}^T$ & m \\ \hline v\_CN\_NInit & Initial Velocity of S/C (gravity scenarios) & $\begin{bmatrix} @@ -315,7 +315,7 @@ \section{Test Parameters} \end{bmatrix}^T$ & m/s \\ \hline r\_CN\_NInit & Initial Position of S/C (no gravity) & $\begin{bmatrix} - 0.1 & -0.4 & 0.3 + 0.1 & -0.4 & 0.3 \end{bmatrix}^T$ & m \\ \hline v\_CN\_NInit & Initial Velocity of S/C (no gravity) & $\begin{bmatrix} @@ -337,7 +337,7 @@ \section{Test Parameters} \caption{Error Tolerance - Note: Relative Tolerance is $\textnormal{abs}(\frac{\textnormal{truth} - \textnormal{value}}{\textnormal{truth}}$)} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c |} % Column formatting, + \begin{tabular}{| c | c |} % Column formatting, \hline Test & Relative Tolerance \\ \hline @@ -352,7 +352,7 @@ \section{Test Parameters} Max deflection with force off & 5e-3 \\ \hline Lagrangian vs Basilisk comparison & 1e-10 \\ - \hline + \hline \end{tabular} \end{table} @@ -360,7 +360,7 @@ \section{Test Parameters} \section{Test Results} -The following figures show the conservation of the quantities described in the success criteria for each scenario. The conservation plots are all relative difference plots. All conservation plots show integration error which is the desired result. In the python test these values are automatically checked therefore when the tests pass, these values have all been confirmed to be conserved. +The following figures show the conservation of the quantities described in the success criteria for each scenario. The conservation plots are all relative difference plots. All conservation plots show integration error which is the desired result. In the python test these values are automatically checked therefore when the tests pass, these values have all been confirmed to be conserved. \subsection{Gravity with no damping scenario} \input{AutoTex/ChangeInOrbitalAngularMomentumGravity} @@ -396,7 +396,7 @@ \subsection{Frequency and Amplitude Verification Scenario} \caption{Frequency and Amplitude Test Results} \label{tab:freqAmpResults} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c | c | c |} % Column formatting, + \begin{tabular}{| c | c | c | c |} % Column formatting, \hline \textbf{Name} & \textbf{BOE Calculation} & \textbf{Basilisk Results} & \textbf{Relative Error} \\ \hline diff --git a/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.rst b/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.rst index 7da95c2581..21edfc9e9c 100644 --- a/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.rst +++ b/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.rst @@ -117,7 +117,3 @@ This section is to outline the steps needed to setup a Hinged Rigid Body State E #. Add the module to the task list:: unitTestSim.AddModelToTask(unitTaskName, panel1) - - - - diff --git a/src/simulation/dynamics/Integrators/_Documentation/AVS.sty b/src/simulation/dynamics/Integrators/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/Integrators/_Documentation/AVS.sty +++ b/src/simulation/dynamics/Integrators/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/Integrators/_Documentation/Basilisk-Integrators20170724.tex b/src/simulation/dynamics/Integrators/_Documentation/Basilisk-Integrators20170724.tex index 240896c253..0f0c4e57fc 100755 --- a/src/simulation/dynamics/Integrators/_Documentation/Basilisk-Integrators20170724.tex +++ b/src/simulation/dynamics/Integrators/_Documentation/Basilisk-Integrators20170724.tex @@ -92,7 +92,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/Integrators/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/Integrators/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/Integrators/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/Integrators/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/Integrators/_Documentation/secModelDescription.tex b/src/simulation/dynamics/Integrators/_Documentation/secModelDescription.tex index 208f527611..40fb509788 100644 --- a/src/simulation/dynamics/Integrators/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/Integrators/_Documentation/secModelDescription.tex @@ -19,7 +19,7 @@ \subsection{Overview} \begin{equation} \dot{\bm x} = \bm f(t, \bm x) \end{equation} -The initial conditions are specified through $\bm x_{0} = \bm x(t_{0})$. In integration time step is given through $h$. +The initial conditions are specified through $\bm x_{0} = \bm x(t_{0})$. In integration time step is given through $h$. \subsection{Implemented Integrators} @@ -29,7 +29,7 @@ \subsubsection{4th Order Runge Kutta - Default Integrator} \bm k_{1} &= \bm f\left(t_{n}, \bm x_{n}\right) \\ \bm k_{2} &= \bm f\left(t_{n} + \frac{h}{2}, \bm x_{n} + \frac{h}{2} \bm k_{1}\right) \\ \bm k_{3} &= \bm f\left(t_{n} + \frac{h}{2}, \bm x_{n} + \frac{h}{2} \bm k_{2}\right) \\ - \bm k_{4} &= \bm f\left(t_{n} + h, \bm x_{n} + h \bm k_{3}\right) + \bm k_{4} &= \bm f\left(t_{n} + h, \bm x_{n} + h \bm k_{3}\right) \end{align} The state at the next integration time $t_{n+1} = t_{n} + h$ is \begin{equation} @@ -43,7 +43,7 @@ \subsubsection{2nd Order Runge Kutta (Heun's Method)} A 2nd order Runge-Kutta method is implemented through Heun's method.\footnote{\url{http://goo.gl/SWdyBZ}} The 2 $k_{i}$ values are defined as \begin{align} \bm k_{1} &= \bm f(t_{n}, \bm x_{n}) \\ - \bm k_{2} &= \bm f(t_{n} + h, \bm x_{n} + h \bm k_{1}) + \bm k_{2} &= \bm f(t_{n} + h, \bm x_{n} + h \bm k_{1}) \end{align} The state at the next integration time $t_{n+1} = t_{n} + h$ is \begin{equation} @@ -56,7 +56,7 @@ \subsubsection{2nd Order Runge Kutta (Heun's Method)} \subsubsection{1st Order Runge Kutta (Euler's Method)} A first order Runge-Kutta method is implemented through Euler's method. The one $k_{1}$ value is defined as \begin{align} - \bm k_{1} &= \bm f(t_{n}, \bm x_{n}) + \bm k_{1} &= \bm f(t_{n}, \bm x_{n}) \end{align} The state at the next integration time $t_{n+1} = t_{n} + h$ is \begin{equation} diff --git a/src/simulation/dynamics/Integrators/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/Integrators/_Documentation/secModelFunctions.tex index 1c1b8e0e54..d8045c0fc7 100644 --- a/src/simulation/dynamics/Integrators/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/Integrators/_Documentation/secModelFunctions.tex @@ -16,6 +16,6 @@ \section{Model Assumptions and Limitations} \subsection{Assumptions} -The equations of motion class {\tt DynamicalObject} is assumed to respond to the method {\tt equationsOfMotion}. The integrator then integrates the system forward in time one BSK time step. +The equations of motion class {\tt DynamicalObject} is assumed to respond to the method {\tt equationsOfMotion}. The integrator then integrates the system forward in time one BSK time step. \subsection{Limitations} diff --git a/src/simulation/dynamics/Integrators/_Documentation/secTest.tex b/src/simulation/dynamics/Integrators/_Documentation/secTest.tex index 1a8acfd9d8..7c1765577d 100644 --- a/src/simulation/dynamics/Integrators/_Documentation/secTest.tex +++ b/src/simulation/dynamics/Integrators/_Documentation/secTest.tex @@ -118,8 +118,8 @@ \section{Test Results} \input{AutoTex/IntegratorsMsg-rkf78} \\ \hline ``euler'' & - \input{AutoTex/IntegratorsTestMsg-euler} & - \input{AutoTex/IntegratorsMsg-euler} + \input{AutoTex/IntegratorsTestMsg-euler} & + \input{AutoTex/IntegratorsMsg-euler} \\ \hline ``rk2'' & \input{AutoTex/IntegratorsTestMsg-rk2} diff --git a/src/simulation/dynamics/Integrators/_Documentation/secUserGuide.tex b/src/simulation/dynamics/Integrators/_Documentation/secUserGuide.tex index 0c09655b78..71fd80da9f 100644 --- a/src/simulation/dynamics/Integrators/_Documentation/secUserGuide.tex +++ b/src/simulation/dynamics/Integrators/_Documentation/secUserGuide.tex @@ -47,4 +47,4 @@ \subsection{Creating New Integration Methods} \begin{verbatim} Basilisk/simulation/dynamics/Integrators/ \end{verbatim} -The integrators must be created to function on a general state vector and be independent of the particular dynamics being integrated. Note that the default integrator is placed inside the {\tt\_GeneralModulesFiles} folder within the dynamics folder. \ No newline at end of file +The integrators must be created to function on a general state vector and be independent of the particular dynamics being integrated. Note that the default integrator is placed inside the {\tt\_GeneralModulesFiles} folder within the dynamics folder. diff --git a/src/simulation/dynamics/Integrators/svIntegratorEuler.rst b/src/simulation/dynamics/Integrators/svIntegratorEuler.rst index f3c9499eb9..c1b6383e11 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorEuler.rst +++ b/src/simulation/dynamics/Integrators/svIntegratorEuler.rst @@ -5,12 +5,3 @@ The module :download:`PDF Description ` contains further information on this module's function, how to run it, as well as testing. - - - - - - - - - diff --git a/src/simulation/dynamics/Integrators/svIntegratorRK2.rst b/src/simulation/dynamics/Integrators/svIntegratorRK2.rst index 74d67dd9c0..716a5f819c 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorRK2.rst +++ b/src/simulation/dynamics/Integrators/svIntegratorRK2.rst @@ -5,13 +5,3 @@ The module :download:`PDF Description ` contains further information on this module's function, how to run it, as well as testing. - - - - - - - - - - diff --git a/src/simulation/dynamics/Integrators/svIntegratorRKF45.rst b/src/simulation/dynamics/Integrators/svIntegratorRKF45.rst index 72b75e6d91..644bfff94a 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorRKF45.rst +++ b/src/simulation/dynamics/Integrators/svIntegratorRKF45.rst @@ -7,9 +7,3 @@ contains further information on this module's function, how to run it, as well as testing. The default ``absTol`` value is 1e-8, while the default ``relTol`` is 1e-4. - - - - - - diff --git a/src/simulation/dynamics/Integrators/svIntegratorRKF78.rst b/src/simulation/dynamics/Integrators/svIntegratorRKF78.rst index 9c507ff1c6..37cbe28131 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorRKF78.rst +++ b/src/simulation/dynamics/Integrators/svIntegratorRKF78.rst @@ -7,12 +7,3 @@ contains further information on this module's function, how to run it, as well as testing. The default ``absTol`` value is 1e-8, while the default ``relTol`` is 1e-4. - - - - - - - - - diff --git a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/AVS.sty b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/AVS.sty +++ b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/Basilisk-LINEARSPRINGMASSDAMPER-20180102.tex b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/Basilisk-LINEARSPRINGMASSDAMPER-20180102.tex index d28fd8c3d2..efce619092 100755 --- a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/Basilisk-LINEARSPRINGMASSDAMPER-20180102.tex +++ b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/Basilisk-LINEARSPRINGMASSDAMPER-20180102.tex @@ -90,7 +90,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/BasiliskReportMemo.cls index 1e869ba0c3..6256f116db 100755 --- a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/bibliography.bib b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/bibliography.bib index 9d7bddb4bd..145dace863 100755 --- a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/bibliography.bib @@ -6,7 +6,7 @@ @conference{Allard2016rz Note = {{P}aper No. AAS-16-156}, Title = {General Hinged Solar Panel Dynamics Approximating First-Order Spacecraft Flexing}, Year = {2016}} - + @book{schaub, Address = {Reston, VA}, Author = {Hanspeter Schaub and John L. Junkins}, diff --git a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secModelDescription.tex b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secModelDescription.tex index 8317a8ce73..fc51e9df7c 100644 --- a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secModelDescription.tex @@ -8,7 +8,7 @@ \section{Model Description} \includegraphics[]{Figures/Flex_Slosh_Figure} \caption{Frame and variable definitions used for formulation} \label{fig:Flex_Slosh_Figure} -\end{figure} +\end{figure} \subsubsection{Rigid Spacecraft Hub Translational Motion} @@ -25,19 +25,19 @@ \subsubsection{Rigid Spacecraft Hub Translational Motion} The definition of $\bm{c}$ can be seen in Eq. (\ref{eq:c}). \begin{equation} \bm{c} = \frac{1}{m_{\text{sc}}}\Big(m_{\text{\text{hub}}}\bm{r}_{B_{c}/B} + \sum_{j=1}^{N_{P}}m_j\bm{r}_{P_{c,j}/B}\Big) - \label{eq:c} + \label{eq:c} \end{equation} To find the inertial time derivative of $\bm{c}$, it is first necessary to find the time derivative of $\bm{c}$ with respect to the body frame. A time derivative of any vector, $\bm{v}$, with respect to the body frame is denoted by $\bm{v}'$; the inertial time derivative is labeled as $\dot{\bm{v}}$. The first and second body-relative time derivatives of $\bm{c}$ can be seen in Eqs. (\ref{eq:cprime}) and (\ref{eq:cdprime}). $\bm{r}_{P_{c,j}/B}$ is defined in the following \begin{equation} - \bm{r}_{P_{c,j}/B} = \bm{r}_{P_{j}/B} + \rho_j \hat{\bm p}_j + \bm{r}_{P_{c,j}/B} = \bm{r}_{P_{j}/B} + \rho_j \hat{\bm p}_j \end{equation} And, the first and second body time derivatives of $\bm{r}_{P_{c,j}/B}$ are \begin{align} - \bm{r}'_{P_{c,j}/B} &= \dot{\rho}_j \hat{\bm p}_j + \bm{r}'_{P_{c,j}/B} &= \dot{\rho}_j \hat{\bm p}_j \\ - \bm{r}''_{P_{c,j}/B} &= \ddot{\rho}_j \hat{\bm p}_j + \bm{r}''_{P_{c,j}/B} &= \ddot{\rho}_j \hat{\bm p}_j \end{align} $\bm{c}'$ and $\bm{c}''$ are defined in the following equations \begin{equation} @@ -60,7 +60,7 @@ \subsubsection{Rigid Spacecraft Hub Translational Motion} \end{equation} Substituting Eq.\eqref{eq:cdprime} into Eq.\eqref{eq:Rbddot} results in \begin{equation} - \ddot{\bm r}_{B/N} = \ddot{\bm r}_{C/N}-\frac{1}{m_{\text{sc}}}\sum_{j=1}^{N_{P}}m_j\ddot{\rho}_j \hat{\bm p}_j = + \ddot{\bm r}_{B/N} = \ddot{\bm r}_{C/N}-\frac{1}{m_{\text{sc}}}\sum_{j=1}^{N_{P}}m_j\ddot{\rho}_j \hat{\bm p}_j = - 2\bm\omega_{\cal B/N}\times\bm c' -\dot{\bm\omega}_{\cal B/N}\times\bm{c}-\bm\omega_{\cal B/N}\times\left(\bm\omega_{\cal B/N}\times\bm{c}\right) \label{eq:Rbddot2} @@ -130,17 +130,17 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} Finally, using tilde matrix and simplifying yields the modified Euler equation, which is the second EOM necessary to describe the motion of the spacecraft. \begin{multline} [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} \\- \sum\limits_{j=1}^{N_P}\biggl(m_j [\tilde{\bm r}_{P_{c,j}/B}]\bm{r}''_{P_{c,j}/B} - + m_j [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm{r}}_{P_{c,j}/B}] \bm{r}'_{P_{c,j}/B}\biggr) + + m_j [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm{r}}_{P_{c,j}/B}] \bm{r}'_{P_{c,j}/B}\biggr) + \bm{L}_B - m_{\text{sc}} [\tilde{\bm{c}}] \ddot{\bm r}_{B/N} \label{eq:Final5} \end{multline} Rearranging Eq.~\eqref{eq:Final5} to be in the same form as the previous sections results in \begin{multline} m_{\text{sc}}[\tilde{\bm{c}}]\ddot{\bm r}_{B/N}+[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \sum\limits_{j=1}^{N_P}m_j [\tilde{\bm r}_{P_{c,j}/B}] \hat{\bm p}_j \ddot{\rho}_j = \\ - -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} + -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} - \sum\limits_{j=1}^{N_P} m_j [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm{r}}_{P_{c,j}/B}] \bm{r}'_{P_{c,j}/B} + \bm{L}_B \label{eq:Final6} -\end{multline} +\end{multline} \subsubsection{Fuel Slosh Motion} Figure~\ref{fig:Flex_Slosh_Figure} shows that a single fuel slosh particle is free to move along its corresponding $\hat{\bm p}_j$ direction and this formulation is generalized to include $N_P$ number of fuel slosh particles. The derivation begins with Newton's law for each fuel slosh particle: @@ -157,27 +157,27 @@ \subsubsection{Fuel Slosh Motion} m_j \Big[\ddot{\bm{r}}_{B/N} + \ddot{\rho}_j \hat{\bm p}_j +2 \bm\omega_{\cal B/N} \times \bm{r}'_{P_{c,j}/B} + \dot{\bm\omega}_{\cal B/N} \times \bm{r}_{P_{c,j}/B} +\bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{P_{c,j}/B})\Big]\\ = \bm F_G + \bm F_C - k_j \rho_j \hat{\bm p}_j - c_j \dot{\rho_j} \hat{\bm p}_j \label{eq:sloshaccel3} -\end{multline} +\end{multline} Equation~\eqref{eq:sloshaccel3} is the dynamical equation for a fuel slosh particle, however, the constraint force, $\bm F_C$, is undefined. Since the fuel slosh particle is free to move in the $\hat{\bm p_j}$ direction, the component of $\bm F_C$ along the $\hat{\bm p_j}$ direction is zero. Leveraging this insight, Eq.~\eqref{eq:sloshaccel3} is projected into the $\hat{\bm p_j}$ direction by multiplying both sides of the equation by $\hat{\bm p_j}^T$. \begin{multline} m_j \Bigl(\hat{\bm p_j}^T \ddot{\bm{r}}_{B/N} + \ddot{\rho}_j +2 \hat{\bm p_j}^T \bm\omega_{\cal B/N} \times \bm{r}'_{P_{c,j}/B} + \hat{\bm p_j}^T \dot{\bm\omega}_{\cal B/N} \times \bm{r}_{P_{c,j}/B} + \hat{\bm p_j}^T \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{P_{c,j}/B})\Bigr)\\ = \hat{\bm p_j}^T \bm F_G - k_j \rho_j - c_j \dot{\rho_j} \label{eq:sloshaccel4} -\end{multline} +\end{multline} Moving the second order terms to the left hand side and introducing the tilde matrix notation yields the final equation needed to describe the motion of the spacecraft. \begin{multline} m_j \hat{\bm p_j}^T \ddot{\bm{r}}_{B/N} - m_j \hat{\bm p_j}^T [\tilde{\bm{r}}_{P_{c,j}/B}] \dot{\bm\omega}_{\cal B/N} + m_j \ddot{\rho}_j \\ - = \hat{\bm p_j}^T \bm F_G - k_j \rho_j - c_j \dot{\rho_j} - 2 m_j \hat{\bm p_j}^T [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{P_{c,j}/B} - m_j \hat{\bm p_j}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{P_{c,j}/B} + = \hat{\bm p_j}^T \bm F_G - k_j \rho_j - c_j \dot{\rho_j} - 2 m_j \hat{\bm p_j}^T [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{P_{c,j}/B} - m_j \hat{\bm p_j}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{P_{c,j}/B} \label{eq:sloshaccel5} -\end{multline} +\end{multline} \subsection{Derivation of Equations of Motion - Kane's Method} The choice of generalized coordinates and their respective generalized speeds are: \begin{equation} - \bm q = + \bm q = \begin{bmatrix} \bm r_{B/N}\\ \bm \sigma_{\cal{B/N}}\\ @@ -193,7 +193,7 @@ \subsection{Derivation of Equations of Motion - Kane's Method} \cdot\\ \dot{\rho}_{N_P} \end{bmatrix} -\end{equation} +\end{equation} The necessary velocities needed to be defined are as follows @@ -220,7 +220,7 @@ \subsection{Derivation of Equations of Motion - Kane's Method} \caption{Partial Velocity Table} \label{tab:hub} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline $r$ & $\bm v^{B_c}_{r}$ & $\bm \omega_{\textit{r}}^{\cal{B}}$ & $\bm v^{P_c}_{r}$ \\ \hline @@ -323,10 +323,10 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} Combining like terms results in the same equation seen in Eq.~\eqref{eq:Final6} found using Newtonian mechanics. \begin{multline} m_{\text{sc}}[\tilde{\bm{c}}]\ddot{\bm r}_{B/N}+[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \sum\limits_{j=1}^{N_P}m_j [\tilde{\bm r}_{P_{c,j}/B}] \hat{\bm p}_j \ddot{\rho}_j = \\ - -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} + -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} - \sum\limits_{j=1}^{N_P} m_j [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm{r}}_{P_{c,j}/B}] \bm{r}'_{P_{c,j}/B} + \bm{L}_B \label{eq:Final7} -\end{multline} +\end{multline} \subsubsection{Fuel Slosh Motion} Following the similar pattern for translational and rotational equations the generalized active forces are defined: @@ -334,7 +334,7 @@ \subsubsection{Fuel Slosh Motion} \begin{equation} \bm F_{7} = \bm v^{P_c}_{r} \cdot (\bm F_G + \bm F_C -k\rho_j \hat{\bm p}_j - c \dot{\rho} \hat{\bm p}_j) = \hat{\bm p_j}^T \bm F_G -k\rho_j - c \dot{\rho} \end{equation} -The generalized inertia forces are defined as: +The generalized inertia forces are defined as: \begin{equation} \bm F^*_{7} = \bm v^{P_c}_{r} \cdot (-m_j \ddot{\bm r}_{P_{c,j}/N}) = \hat{\bm p}_j^T (-m_j \ddot{\bm r}_{P_{c,j}/N}) @@ -343,15 +343,13 @@ \subsubsection{Fuel Slosh Motion} \begin{multline} \hat{\bm p_j}^T \bm F_G -k\rho_j - c \dot{\rho} - m_j \hat{\bm p}_j^T \Big[\ddot{\bm r}_{B/N} + \ddot{\rho}_j \hat{\bm p}_j +2 \bm\omega_{\cal B/N} \times \bm{r}'_{P_{c,j}/B} + \dot{\bm\omega}_{\cal B/N} \times \bm{r}_{P_{c,j}/B} \\ - +\bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{P_{c,j}/B})\Big] = 0 + +\bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{P_{c,j}/B})\Big] = 0 \end{multline} Rearranging and combining like terms results in: \begin{multline} m_j \hat{\bm p_j}^T \ddot{\bm{r}}_{B/N} - m_j \hat{\bm p_j}^T [\tilde{\bm{r}}_{P_{c,j}/B}] \dot{\bm\omega}_{\cal B/N} + m_j \ddot{\rho}_j \\ - = \hat{\bm p_j}^T \bm F_G - k_j \rho_j - c_j \dot{\rho_j} - 2 m_j \hat{\bm p_j}^T [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{P_{c,j}/B} - m_j \hat{\bm p_j}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{P_{c,j}/B} + = \hat{\bm p_j}^T \bm F_G - k_j \rho_j - c_j \dot{\rho_j} - 2 m_j \hat{\bm p_j}^T [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{P_{c,j}/B} - m_j \hat{\bm p_j}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{P_{c,j}/B} \label{eq:sloshaccel6} -\end{multline} +\end{multline} Which is identical to Eq.~\eqref{eq:sloshaccel5}! - - diff --git a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secModelFunctions.tex index 1621d0e674..9768d65461 100644 --- a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secModelFunctions.tex @@ -22,4 +22,4 @@ \section{Model Assumptions and Limitations} \item There are no travel limits for the fuel slosh therefore the particles could travel past the limits of the fuel tank boundary (this can be avoided by varying the mass and/or spring constant while considering the expected accelerations of the s/c to stay within the tank) \item The mass of the fuel slosh particles add to the total mass of the fuel tank \item This model could be used with other fuel slosh models like a pendulum based fuel slosh model -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secTest.tex b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secTest.tex index 6ba567dbaf..4e04083583 100644 --- a/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secTest.tex +++ b/src/simulation/dynamics/LinearSpringMassDamper/_Documentation/secTest.tex @@ -2,7 +2,7 @@ \section{Test Description and Success Criteria} This test is located in \texttt{simulation/dynamics/FuelTank/UnitTest/\newline test\_fuelSloshParticleStateEffector.py}. In this integrated test there are three fuel slosh particles connected to the spacecraft hub. Depending on the scenario, there are different success criteria. These are outlined in the following subsections: \subsection{Gravity integrated test} -In this test the simulation is placed into orbit around Earth with point gravity and has no damping in the fuel slosh particles. The following parameters are being tested. +In this test the simulation is placed into orbit around Earth with point gravity and has no damping in the fuel slosh particles. The following parameters are being tested. \begin{itemize} \item Conservation of orbital angular momentum \item Conservation of orbital energy @@ -29,13 +29,13 @@ \subsection{Damping with no gravity integrated test} \section{Test Parameters} -Since this is an integrated test, the inputs to the test are the physical parameters of the spacecraft along with the initial conditions of the states. These parameters are outlined in Tables~\ref{tab:hub}-~\ref{tab:initial}. Additionally, the error tolerances can be seen in Table~\ref{tab:errortol}. The error tolerance for the energy and momentum tests is 1e-10 and that was chosen to ensure cross platform success of the test, however it is expected to get conservation of energy and momentum down to 1e-14 or lower. +Since this is an integrated test, the inputs to the test are the physical parameters of the spacecraft along with the initial conditions of the states. These parameters are outlined in Tables~\ref{tab:hub}-~\ref{tab:initial}. Additionally, the error tolerances can be seen in Table~\ref{tab:errortol}. The error tolerance for the energy and momentum tests is 1e-10 and that was chosen to ensure cross platform success of the test, however it is expected to get conservation of energy and momentum down to 1e-14 or lower. \begin{table}[htbp] \caption{Spacecraft Hub Parameters} \label{tab:hub} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c | c | c |} % Column formatting, + \begin{tabular}{| c | c | c | c |} % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -57,7 +57,7 @@ \section{Test Parameters} \caption{Fuel Slosh Particle 1 Parameters} \label{tab:slosh1} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c | c | c |} % Column formatting, + \begin{tabular}{| c | c | c | c |} % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -80,7 +80,7 @@ \section{Test Parameters} \caption{Fuel Slosh Particle 2 Parameters} \label{tab:slosh2} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c | c | c |} % Column formatting, + \begin{tabular}{| c | c | c | c |} % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -103,7 +103,7 @@ \section{Test Parameters} \caption{Fuel Slosh Particle 3 Parameters} \label{tab:slosh3} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c | c | c |} % Column formatting, + \begin{tabular}{| c | c | c | c |} % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -126,7 +126,7 @@ \section{Test Parameters} \caption{Initial Conditions} \label{tab:initial} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c | c | c |} % Column formatting, + \begin{tabular}{| c | c | c | c |} % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -143,7 +143,7 @@ \section{Test Parameters} (Particle 3) rhoDotInit & (Particle 3) Initial $\dot{\rho}$ & 0.0 & m/s \\ \hline r\_CN\_NInit & Initial Position of S/C (gravity scenarios) & $\begin{bmatrix} - -4020339 & 7490567 & 5248299 + -4020339 & 7490567 & 5248299 \end{bmatrix}^T$ & m \\ \hline v\_CN\_NInit & Initial Velocity of S/C (gravity scenarios) & $\begin{bmatrix} @@ -151,7 +151,7 @@ \section{Test Parameters} \end{bmatrix}^T$ & m/s \\ \hline r\_CN\_NInit & Initial Position of S/C (no gravity) & $\begin{bmatrix} - 0.5 & 0.4 & -0.7 + 0.5 & 0.4 & -0.7 \end{bmatrix}^T$ & m \\ \hline v\_CN\_NInit & Initial Velocity of S/C (no gravity) & $\begin{bmatrix} @@ -173,12 +173,12 @@ \section{Test Parameters} \caption{Error Tolerance - Note: Relative Tolerance is $\textnormal{abs}(\frac{\textnormal{truth} - \textnormal{value}}{\textnormal{truth}}$)} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c |} % Column formatting, + \begin{tabular}{| c | c |} % Column formatting, \hline Test & Relative Tolerance \\ \hline Energy and Momentum Conservation & 1e-10 \\ - \hline + \hline \end{tabular} \end{table} @@ -186,7 +186,7 @@ \section{Test Parameters} \section{Test Results} -The following figures show the conservation of the quantities described in the success criteria for each scenario. The conservation plots are all relative difference plots. All conservation plots show integration error which is the desired result. In the python test these values are automatically checked therefore when the tests pass, these values have all been confirmed to be conserved. An additional note: the angular momentum plots are plotting the change in the components of the angular momentum vector in the inertial frame. The individual components are not labeled because the goal is for each component to show conservation therefore the individual components do not have separate information needing to be specified. +The following figures show the conservation of the quantities described in the success criteria for each scenario. The conservation plots are all relative difference plots. All conservation plots show integration error which is the desired result. In the python test these values are automatically checked therefore when the tests pass, these values have all been confirmed to be conserved. An additional note: the angular momentum plots are plotting the change in the components of the angular momentum vector in the inertial frame. The individual components are not labeled because the goal is for each component to show conservation therefore the individual components do not have separate information needing to be specified. \subsection{Gravity with no damping scenario} \begin{figure}[htbp] diff --git a/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.rst b/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.rst index e102f2c9f2..600a787e10 100644 --- a/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.rst +++ b/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.rst @@ -13,12 +13,3 @@ how to run it, as well as testing. Message Connection Descriptions ------------------------------- This state effector does not have any input or output messsages. - - - - - - - - - diff --git a/src/simulation/dynamics/MtbEffector/MtbEffector.h b/src/simulation/dynamics/MtbEffector/MtbEffector.h index 06345f4022..7823ea81ec 100644 --- a/src/simulation/dynamics/MtbEffector/MtbEffector.h +++ b/src/simulation/dynamics/MtbEffector/MtbEffector.h @@ -32,11 +32,11 @@ #include "architecture/utilities/bskLogging.h" #include "architecture/messaging/messaging.h" - + /*! @brief This module converts magnetic torque bar dipoles to body torques. */ class MtbEffector: public SysModel, public DynamicEffector { - + public: MtbEffector(); ~MtbEffector(); @@ -45,7 +45,7 @@ class MtbEffector: public SysModel, public DynamicEffector { void linkInStates(DynParamManager& states); void computeForceTorque(double integTime, double timeStep); void WriteOutputMessages(uint64_t CurrentClock); - + public: Message mtbOutMsg; //!< output message containing net torque produced by the torque bars in Body components StateData *hubSigma; //!< Hub/Inertial attitude represented by MRP @@ -53,7 +53,7 @@ class MtbEffector: public SysModel, public DynamicEffector { ReadFunctor magInMsg; //!< input msg for magnetic field data in inertial frame N ReadFunctor mtbParamsInMsg; //!< input msg for layout of magnetic torque bars BSKLogger bskLogger; //!< -- BSK Logging - + private: MTBCmdMsgPayload mtbCmdInMsgBuffer; //!< msg buffer or commanded mtb dipole array in the magnetic torque bar frame T MagneticFieldMsgPayload magInMsgBuffer; //!< msg buffer for magnetic field data in inertial frame N diff --git a/src/simulation/dynamics/MtbEffector/MtbEffector.rst b/src/simulation/dynamics/MtbEffector/MtbEffector.rst index 379f9dd052..a873a2fe93 100644 --- a/src/simulation/dynamics/MtbEffector/MtbEffector.rst +++ b/src/simulation/dynamics/MtbEffector/MtbEffector.rst @@ -4,9 +4,9 @@ This module converts magnetic torque bar dipoles to body torques. Message Connection Descriptions ------------------------------- -The following table lists all the module input and output messages. -The module msg connection is set by the user from python. -The msg type contains a link to the message structure definition, while the description +The following table lists all the module input and output messages. +The module msg connection is set by the user from python. +The msg type contains a link to the message structure definition, while the description provides information on what this message is used for. .. list-table:: Module I/O Messages @@ -33,4 +33,3 @@ provides information on what this message is used for. User Guide ---------- Note that the MTB input configuration message variable ``GtMatrix_B`` must be provided in a row major format. - diff --git a/src/simulation/dynamics/MtbEffector/_UnitTest/test_MtbEffector.py b/src/simulation/dynamics/MtbEffector/_UnitTest/test_MtbEffector.py index 7d69d358c5..cb9f54f80a 100644 --- a/src/simulation/dynamics/MtbEffector/_UnitTest/test_MtbEffector.py +++ b/src/simulation/dynamics/MtbEffector/_UnitTest/test_MtbEffector.py @@ -1,12 +1,12 @@ -# +# # 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 @@ -14,8 +14,8 @@ # 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 os @@ -37,7 +37,7 @@ def truthMagneticTorque(bField_N, sigmaBN, mtbCmds, GtMatrix, numMTB, maxDipole): - + temp = np.asarray(GtMatrix[0:3*numMTB]) GtMatrix = np.reshape(temp, (3, numMTB)) bField_N = np.asarray(bField_N) @@ -74,7 +74,7 @@ def test_MtbEffector(show_plots, accuracy, maxDipole): **Description of Variables Being Tested** - Here discuss what variables and states are being checked. + Here discuss what variables and states are being checked. """ [testResults, testMessage] = MtbEffectorTestFunction(show_plots, accuracy, maxDipole) assert testResults < 1, testMessage @@ -84,7 +84,7 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): """Call this routine directly to run the unit test.""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages - + # create simulation variable names simTaskName = "simTask" simProcessName = "simProcess" @@ -108,8 +108,8 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): earth = gravFactory.createEarth() earth.isCentralBody = True # ensure this is the central gravitational body mu = earth.mu - - + + # initialize spacecraft object and set properties scObject = spacecraft.Spacecraft() scObject.ModelTag = "bskTestSat" @@ -120,7 +120,7 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): scObject.hub.mHub = 10.0 # kg - spacecraft mass (arbitrary) scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) - + oe = orbitalMotion.ClassicElements() oe.a = 6778.14 * 1000. # meters oe.e = 0.0 @@ -137,8 +137,8 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): # add spacecraft object scSim.AddModelToTask(simTaskName, scObject) scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) - - + + # add magnetic field module magModule = magneticFieldWMM.MagneticFieldWMM() magModule.ModelTag = "WMM" @@ -147,15 +147,15 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): magModule.epochInMsg.subscribeTo(epochMsg) magModule.addSpacecraftToModel(scObject.scStateOutMsg) # this command can be repeated if multiple scSim.AddModelToTask(simTaskName, magModule) - - + + # add magnetic torque bar effector mtbEff = MtbEffector.MtbEffector() mtbEff.ModelTag = "MtbEff" scObject.addDynamicEffector(mtbEff) scSim.AddModelToTask(simTaskName, mtbEff) - - + + # mtbConfigData message mtbConfigParams = messaging.MTBArrayConfigMsgPayload() mtbConfigParams.numMTB = 3 @@ -167,30 +167,30 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): ] mtbConfigParams.maxMtbDipoles = [maxDipole]*4 mtbParamsInMsg = messaging.MTBArrayConfigMsg().write(mtbConfigParams) - - + + # MTBCmdMsgPayload, mtbCmdInMsg mtbCmdInMsgContainer = messaging.MTBCmdMsgPayload() mtbCmdInMsgContainer.mtbDipoleCmds = [0.2, 0.2, 0.2] mtbCmdInMsg = messaging.MTBCmdMsg().write(mtbCmdInMsgContainer) - - # subscribe to mesaages + + # subscribe to mesaages mtbEff.mtbCmdInMsg.subscribeTo(mtbCmdInMsg) mtbEff.mtbParamsInMsg.subscribeTo(mtbParamsInMsg) mtbEff.magInMsg.subscribeTo(magModule.envOutMsgs[0]) - - + + # Setup data logging before the simulation is initialized numDataPoints = 3600 samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) dataLog = scObject.scStateOutMsg.recorder(samplingTime) dataLogMag = magModule.envOutMsgs[0].recorder(samplingTime) dataLogMTB = mtbEff.mtbOutMsg.recorder(samplingTime) - + scSim.AddModelToTask(simTaskName, dataLogMTB) scSim.AddModelToTask(simTaskName, dataLogMag) scSim.AddModelToTask(simTaskName, dataLog) - + # initialize Simulation scSim.InitializeSimulation() @@ -204,7 +204,7 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): mtbData = dataLogMTB.mtbNetTorque_B dataMagField = dataLogMag.magField_N np.set_printoptions(precision=16) - + # plot net mtb torque if show_plots: plt.close("all") # clears out plots from earlier test runs @@ -248,13 +248,13 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): mtbConfigParams.numMTB, maxDipole ) - + testFailCount, testMessages = unitTestSupport.compareVector(mtbTauV, mtbTauTruth, accuracy, "mtbTorque_B", testFailCount, testMessages) - + if testFailCount == 0: print("PASSED: Mtb Effector") else: @@ -267,5 +267,3 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): 1e-12, 0.1 ) - - diff --git a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/AVS.sty b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/AVS.sty +++ b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/Basilisk-NHINGEDRIGIDBODYSTATEEFFECTOR-20180103.tex b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/Basilisk-NHINGEDRIGIDBODYSTATEEFFECTOR-20180103.tex index 4ff4f25338..8544a0c8ae 100755 --- a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/Basilisk-NHINGEDRIGIDBODYSTATEEFFECTOR-20180103.tex +++ b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/Basilisk-NHINGEDRIGIDBODYSTATEEFFECTOR-20180103.tex @@ -88,7 +88,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/BasiliskReportMemo.cls index 1e869ba0c3..6256f116db 100755 --- a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/bibliography.bib b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/bibliography.bib index 9d7bddb4bd..145dace863 100755 --- a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/bibliography.bib @@ -6,7 +6,7 @@ @conference{Allard2016rz Note = {{P}aper No. AAS-16-156}, Title = {General Hinged Solar Panel Dynamics Approximating First-Order Spacecraft Flexing}, Year = {2016}} - + @book{schaub, Address = {Reston, VA}, Author = {Hanspeter Schaub and John L. Junkins}, diff --git a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secModelDescription.tex b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secModelDescription.tex index b1dde64ce7..c797881300 100644 --- a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secModelDescription.tex @@ -16,7 +16,7 @@ \subsection{Derivation of Equations of Motion - Kane's Method} The choice of state variables and their respective chosen generalized speeds are: \begin{equation} - \bm X = + \bm X = \begin{bmatrix} \bm r_{B/N}\\ \bm \sigma_{\cal{B/N}}\\ @@ -32,7 +32,7 @@ \subsection{Derivation of Equations of Motion - Kane's Method} \cdot\\ \dot{\theta}_{N_p} \end{bmatrix} -\end{equation} +\end{equation} To create the partial velocity table, some velocities first need to be defined \begin{equation} @@ -72,14 +72,14 @@ \subsection{Derivation of Equations of Motion - Kane's Method} \bm c = \frac{1}{m_{sc}}\Big[ \sum_{i=1}^{N_p} m_p {\bm r}_{S_i/B}\Big] \end{equation} \begin{equation} - \dot{\bm c} = {\bm c}' - [\tilde{\bm c}]{\bm \omega}_{\cal{B/N}} + \dot{\bm c} = {\bm c}' - [\tilde{\bm c}]{\bm \omega}_{\cal{B/N}} \end{equation} \begin{equation} - {\bm c}' = \frac{m_p d}{m_{sc}} \sum_{i=1}^{N_p} \Big[\Big(\sum_{k=1}^{i} \dot{\theta}_k \Big) \bm{\hat{s}}_{i,3} + \sum_{n=1}^{i-1} 2\bm{\hat{s}}_{n,3}\Big(\sum_{k=1}^{n} \dot{\theta}_k \Big) \Big] + {\bm c}' = \frac{m_p d}{m_{sc}} \sum_{i=1}^{N_p} \Big[\Big(\sum_{k=1}^{i} \dot{\theta}_k \Big) \bm{\hat{s}}_{i,3} + \sum_{n=1}^{i-1} 2\bm{\hat{s}}_{n,3}\Big(\sum_{k=1}^{n} \dot{\theta}_k \Big) \Big] \label{eq:cprime1} \end{equation} \begin{equation} - {\bm c}' = \frac{m_p d}{m_{sc}} \Big[\sum_{i=1}^{N_p} \dot{\theta}_i \sum_{n=i}^{N_p} (2[N_p - n]+1 )\bm{\hat{s}}_{n,3}\Big] + {\bm c}' = \frac{m_p d}{m_{sc}} \Big[\sum_{i=1}^{N_p} \dot{\theta}_i \sum_{n=i}^{N_p} (2[N_p - n]+1 )\bm{\hat{s}}_{n,3}\Big] \label{eq:cprime2} \end{equation} Now the following partial velocity table can be created (here: $j = r-6$): @@ -88,7 +88,7 @@ \subsection{Derivation of Equations of Motion - Kane's Method} \caption{Partial Velocity Table} \label{tab:hub} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ |c | c | c | c | c | c |} % Column formatting, + \begin{tabular}{ |c | c | c | c | c | c |} % Column formatting, \hline $r$ & $\bm v^{C}_{r}$ & $\bm v^{B}_{r}$ & $\bm \omega_{\textit{r}}^{\cal{B}}$ & $\bm v^{S_i}_{r}$ & $\bm \omega_{\textit{r}}^{\cal{S}_\textit{i}}$ \\ \hline \hline @@ -149,7 +149,7 @@ \subsubsection{Hub Translational Motion} + 2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_i/B} + \dot{\bm\omega}_{\cal B/N} \times \bm{r}_{S_i/B} \\ + \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_i/B})\Big] = \bm F_{\text{ext}} \end{multline} -The body frame derivative can be written explicitly using the simplification used in Eqs. \ref{eq:cprime1} and \ref{eq:cprime2} (this simplification only works when $\bm{r}''_{S_i/B}$ is summed over all panels) +The body frame derivative can be written explicitly using the simplification used in Eqs. \ref{eq:cprime1} and \ref{eq:cprime2} (this simplification only works when $\bm{r}''_{S_i/B}$ is summed over all panels) \begin{equation} m_{\text{p}} \sum^{N_p}_{i=1} \bm{r}''_{S_i/B} = \sum^{N_p}_{i=1} \Big[ \ddot{\theta}_i \sum^{N_p}_{k=i} (2[N_p-k]+1) d m_{\text{p}} \bm{\hat{s}}_{k,3} + \Big(\sum^{i}_{k=1}\dot{\theta}_k\Big)^2 (2[N_p-i]+1) d m_{\text{p}} \bm{\hat{s}}_{i,1} \Big] \end{equation} @@ -210,7 +210,7 @@ \subsubsection{Hub Rotational Motion} - \sum\limits_{i=1}^{N_p}[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{p}_i,S_i}] \bm\omega_{\cal B/N} + \sum\limits_{i=1}^{N_p}\bigg( - \ddot{\theta}\sum\limits_{k=i}^{N_p}\Big[ [\tilde{{\bm r}}_{S_{k}/B}] + \sum\limits_{n=k+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big]m_{\text{p}} d \bm{\hat{s}}_{k,3} \\ - \Big( \sum\limits_{k=1}^{i} \dot{\theta}\Big)^2 \Big[[\tilde{{\bm r}}_{S_{i}/B}] + \sum\limits_{n=i+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big] m_{\text{p}} d \bm{\hat{s}}_{i,1} - - m_{\text{p}} [\tilde{{\bm r}}_{S_{i}/B}] \Big[ + - m_{\text{p}} [\tilde{{\bm r}}_{S_{i}/B}] \Big[ 2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_i/B} + \dot{\bm\omega}_{\cal B/N} \times \bm{r}_{S_i/B} \\ + \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_i/B})\Big] @@ -220,26 +220,26 @@ \subsubsection{Hub Rotational Motion} \begin{multline} \bm L_B - m_{\text{sc}}[\tilde{\bm{c}}]\ddot{\bm r}_{B/N} -[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} - \sum\limits_{i=1}^{N_p} \Big[\sum^{N_p}_{k=i}I_{s_{k,2}} \bm{\hat{s}}_{k,2} + \sum\limits_{k=i}^{N_p}\Big[ [\tilde{{\bm r}}_{S_{k}/B}] + \sum\limits_{n=k+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big]m_{\text{p}} d \bm{\hat{s}}_{k,3} \Big] \ddot{\theta}_i \\ - -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - + \sum\limits_{i=1}^{N_p}\bigg(- 2 m_{\text{p}} [\tilde{{\bm r}}_{S_{i}/B}] \Big[\bm\omega_{\cal B/N} \times \bm{r}'_{S_i/B}\Big] -\big(\sum^{i}_{k=1}\dot{\theta}_k\big)(I_{s_{i,3}}-I_{s_{i,1}})(\hat{\bm s}_{i,1}\hat{\bm s}_{i,3}^{T}+\hat{\bm s}_{i,3}\hat{\bm s}_{i,1}^{T}) \bm\omega_{\cal B/N} \\ + -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} + + \sum\limits_{i=1}^{N_p}\bigg(- 2 m_{\text{p}} [\tilde{{\bm r}}_{S_{i}/B}] \Big[\bm\omega_{\cal B/N} \times \bm{r}'_{S_i/B}\Big] -\big(\sum^{i}_{k=1}\dot{\theta}_k\big)(I_{s_{i,3}}-I_{s_{i,1}})(\hat{\bm s}_{i,1}\hat{\bm s}_{i,3}^{T}+\hat{\bm s}_{i,3}\hat{\bm s}_{i,1}^{T}) \bm\omega_{\cal B/N} \\ - \Big( \sum\limits_{k=1}^{i} \dot{\theta}\Big)^2 \Big[[\tilde{{\bm r}}_{S_{i}/B}] + \sum\limits_{n=i+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big] m_{\text{p}} d \bm{\hat{s}}_{i,1} - I_{s_{i,2}} \big(\sum^{i}_{k=1}\dot{\theta}_k\big) [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i,2} \bigg) = 0 \end{multline} Moving things to the correct sides \begin{multline} m_{\text{sc}}[\tilde{\bm{c}}]\ddot{\bm r}_{B/N} + [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \sum\limits_{i=1}^{N_p} \Big[\sum^{N_p}_{k=i}I_{s_{k,2}} \bm{\hat{s}}_{k,2} + \sum\limits_{k=i}^{N_p}\Big[ [\tilde{{\bm r}}_{S_{k}/B}] + \sum\limits_{n=k+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big]m_{\text{p}} d \bm{\hat{s}}_{k,3} \Big] \ddot{\theta}_i \\ - = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - \sum\limits_{i=1}^{N_p}\bigg(2 m_{\text{p}} [\tilde{{\bm r}}_{S_{i}/B}] \Big[\bm\omega_{\cal B/N} \times \bm{r}'_{S_i/B}\Big] + \big(\sum^{i}_{k=1}\dot{\theta}_k\big)(I_{s_{i,3}}-I_{s_{i,1}})(\hat{\bm s}_{i,1}\hat{\bm s}_{i,3}^{T}+\hat{\bm s}_{i,3}\hat{\bm s}_{i,1}^{T}) \bm\omega_{\cal B/N} \\ + = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - \sum\limits_{i=1}^{N_p}\bigg(2 m_{\text{p}} [\tilde{{\bm r}}_{S_{i}/B}] \Big[\bm\omega_{\cal B/N} \times \bm{r}'_{S_i/B}\Big] + \big(\sum^{i}_{k=1}\dot{\theta}_k\big)(I_{s_{i,3}}-I_{s_{i,1}})(\hat{\bm s}_{i,1}\hat{\bm s}_{i,3}^{T}+\hat{\bm s}_{i,3}\hat{\bm s}_{i,1}^{T}) \bm\omega_{\cal B/N} \\ + \Big( \sum\limits_{k=1}^{i} \dot{\theta}\Big)^2 \Big[[\tilde{{\bm r}}_{S_{i}/B}] + \sum\limits_{n=i+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big] m_{\text{p}} d \bm{\hat{s}}_{i,1} + I_{s_{i,2}} \big(\sum^{i}_{k=1}\dot{\theta}_k\big) [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i,2} \bigg) + \bm L_B \end{multline} \begin{multline} - m_{\text{sc}}[\tilde{\bm{c}}]\ddot{\bm r}_{B/N} + [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \sum\limits_{i=1}^{N_p} \Big[\sum^{N_p}_{k=i}I_{s_{k,2}} \bm{\hat{s}}_{k,2} + \sum\limits_{k=i}^{N_p}\Big[ [\tilde{{\bm r}}_{S_{k}/B}] + \sum\limits_{n=k+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big]m_{\text{p}} d \bm{\hat{s}}_{k,3} \Big] \ddot{\theta}_i + m_{\text{sc}}[\tilde{\bm{c}}]\ddot{\bm r}_{B/N} + [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \sum\limits_{i=1}^{N_p} \Big[\sum^{N_p}_{k=i}I_{s_{k,2}} \bm{\hat{s}}_{k,2} + \sum\limits_{k=i}^{N_p}\Big[ [\tilde{{\bm r}}_{S_{k}/B}] + \sum\limits_{n=k+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big]m_{\text{p}} d \bm{\hat{s}}_{k,3} \Big] \ddot{\theta}_i \\ = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - \sum\limits_{i=1}^{N_p}\bigg( - m_{\text{p}}\Big[[\bm{\tilde{r}}'_{S_i/B}] [\bm{\tilde{r}}_{S_i/B}] + [\bm{\tilde{r}}_{S_i/B}] [\bm{\tilde{r}}'_{S_i/B}]\Big] \bm\omega_{\cal B/N} \\ - +\big(\sum^{i}_{k=1}\dot{\theta}_k\big)(I_{s_{i,3}}-I_{s_{i,1}})(\hat{\bm s}_{i,1}\hat{\bm s}_{i,3}^{T}+\hat{\bm s}_{i,3}\hat{\bm s}_{i,1}^{T}) \bm\omega_{\cal B/N} + +\big(\sum^{i}_{k=1}\dot{\theta}_k\big)(I_{s_{i,3}}-I_{s_{i,1}})(\hat{\bm s}_{i,1}\hat{\bm s}_{i,3}^{T}+\hat{\bm s}_{i,3}\hat{\bm s}_{i,1}^{T}) \bm\omega_{\cal B/N} + m_{\text{p}} [\tilde{\bm\omega}_{\cal B/N}] [\tilde{{\bm r}}_{S_{i}/B}] \bm{r}'_{S_i/B} \\ - + \Big( \sum\limits_{k=1}^{i} \dot{\theta}\Big)^2 \Big[[\tilde{{\bm r}}_{S_{i}/B}] + + \Big( \sum\limits_{k=1}^{i} \dot{\theta}\Big)^2 \Big[[\tilde{{\bm r}}_{S_{i}/B}] + \sum\limits_{n=i+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big] m_{\text{p}} d \bm{\hat{s}}_{i,1} + I_{s_{i,2}} \big(\sum^{i}_{k=1}\dot{\theta}_k\big) [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i,2} \bigg) + \bm L_B \end{multline} @@ -258,9 +258,9 @@ \subsubsection{Panel Motions} \begin{multline} \bm F_{r} = \bm \omega_{\textit{r}}^{\cal{S}_\textit{j}} \cdot (-k_j (\theta_j-\theta_{j,0}) \bm{\hat{s}}_{j,2} - c_j \dot{\theta}_j \bm{\hat{s}}_{j,2} +k_{j+1} (\theta_{j+1}-\theta_{j+1,0}) \bm{\hat{s}}_{j,2} +c_{j+1} \dot{\theta}_{j+1} \bm{\hat{s}}_{j+1,2})\\ - + 2d\bm{\hat{s}}_{j,3}\cdot(\bm F_{ext,j+1} - m_{\textit{conn},j}\bm{\ddot{r}_{\textit{conn},j}}) = + + 2d\bm{\hat{s}}_{j,3}\cdot(\bm F_{ext,j+1} - m_{\textit{conn},j}\bm{\ddot{r}_{\textit{conn},j}}) = -k_j (\theta_j-\theta_{j,0}) - c_j \dot{\theta}_j + k_{j+1} (\theta_{j+1}-\theta_{j+1,0}) + c_{j+1} \dot{\theta}_{j+1}\\ - + 2d\bm{\hat{s}}_{j,3}\cdot(\bm F_{ext,j+1} - m_{\textit{conn},j}\bm{\ddot{r}_{\textit{conn},j}}) = \textit{K} + 2d\bm{\hat{s}}_{j,3}\cdot(\bm F_{ext,j+1} - m_{\textit{conn},j}\bm{\ddot{r}_{\textit{conn},j}}) + + 2d\bm{\hat{s}}_{j,3}\cdot(\bm F_{ext,j+1} - m_{\textit{conn},j}\bm{\ddot{r}_{\textit{conn},j}}) = \textit{K} + 2d\bm{\hat{s}}_{j,3}\cdot(\bm F_{ext,j+1} - m_{\textit{conn},j}\bm{\ddot{r}_{\textit{conn},j}}) \end{multline} Where $m_{\textit{conn},j}$ and $\bm{\ddot{r}_{\textit{conn},j}}$ are the mass and acceleration of the connected panels after the jth panel, and: \begin{equation} @@ -303,8 +303,8 @@ \subsubsection{Panel Motions} -I_{s_j,2} \sum^{N_p}_{i=1}\ddot{\theta}_i - \left( I_{s_{j,1}} - I_{s_{j,3}}\right) \omega_{s_{j,3}} \omega_{s_{j,1}}+ 2d\bm{\hat{s}}^{T}_{j,3}\bm F_{ext,j+1} -m_p d \hat{s}^{T}_{j,3}[\bm r''_{S_{j}/B} + \sum^{N_p}_{i=1} 2 \bm r''_{S_{i}/B}]\\ -m_p d \hat{s}^{T}_{j,3}\Big[2[\bm{\tilde{\omega}}_{\cal B/N}]\bm r'_{S_{j}/B} + [\bm{\tilde{\omega}}_{\cal B/N}][\bm{\tilde{\omega}}_{\cal B/N}]\bm r_{S_{j}/B}\\ - +\sum^{N_p}_{i=j+1}\Big(4[\bm{\tilde{\omega}}_{\cal B/N}]\bm r'_{S_{j}/B} - + 2[\bm{\tilde{\omega}}_{\cal B/N}][\bm{\tilde{\omega}}_{\cal B/N}]\bm r_{S_{j}/B} \Big)\Big] = 0 + +\sum^{N_p}_{i=j+1}\Big(4[\bm{\tilde{\omega}}_{\cal B/N}]\bm r'_{S_{j}/B} + + 2[\bm{\tilde{\omega}}_{\cal B/N}][\bm{\tilde{\omega}}_{\cal B/N}]\bm r_{S_{j}/B} \Big)\Big] = 0 \end{multline} The $\bm r''_{S_{j}/B}$ terms contain $\ddot{\theta}$ terms and thus need to be rewritten to a usable form. This is done by writing it out for several panels and finding a pattern, the result of this is shown next: \begin{multline} @@ -312,7 +312,7 @@ \subsubsection{Panel Motions} -I_{s_j,2} \sum^{N_p}_{i=1}\ddot{\theta}_i - \left( I_{s_{j,1}} - I_{s_{j,3}}\right) \omega_{s_{j,3}} \omega_{s_{j,1}}+ 2d\bm{\hat{s}}^{T}_{j,3}\bm F_{ext,j+1}\\ -m_p d \hat{s}^{T}_{j,3}\Big[2[\bm{\tilde{\omega}}_{\cal B/N}]\bm r'_{S_{j}/B} + [\bm{\tilde{\omega}}_{\cal B/N}][\bm{\tilde{\omega}}_{\cal B/N}]\bm r_{S_{j}/B}+\sum^{N_p}_{i=j+1}\Big(4[\bm{\tilde{\omega}}_{\cal B/N}]\bm r'_{S_{j}/B} + 2[\bm{\tilde{\omega}}_{\cal B/N}][\bm{\tilde{\omega}}_{\cal B/N}]\bm r_{S_{j}/B} \Big)\Big]\\ -m_p d^2 \hat{s}^{T}_{j,3}\sum^{N_p}_{i=1}\Big[\ddot{\theta}_i \sum^{N_p}_{k=i}(2\hat{s}_{k,3}+4\hat{s}_{k,3}(N_p-j) -H[k-j] 4\hat{s}_{k,3}(k-j)) - H[j-i]\hat{s}_{j,3} + \\ - \Big( \sum^{i}_{n=1} \dot{\theta}\Big)^2 (2\hat{s}_{i,1}+4\hat{s}_{i,1}(N_p-j) -H[i-j] 4\hat{s}_{i,1}(i-j) - \Big( \sum^{i}_{n=1} \dot{\theta}_i\Big)^2 \hat{s}_{j,1} ) \Big] = 0 + \Big( \sum^{i}_{n=1} \dot{\theta}\Big)^2 (2\hat{s}_{i,1}+4\hat{s}_{i,1}(N_p-j) -H[i-j] 4\hat{s}_{i,1}(i-j) - \Big( \sum^{i}_{n=1} \dot{\theta}_i\Big)^2 \hat{s}_{j,1} ) \Big] = 0 \end{multline} This finally leads to: \begin{multline} @@ -348,7 +348,7 @@ \subsection{Back Substitution Method} \begin{equation} \bm g_{j,1} = -[I_{s_j,2} \bm{\hat{s}}_{j,2}^T -m_p d \hat{s}^{T}_{j,3}[\tilde{{\bm r}}_{S_{i}/B}]-\sum^{N_p}_{i=j+1} 2 m_p d \hat{s}^{T}_{j,3}[\tilde{{\bm r}}_{S_{i}/B}] \end{equation} -Also defining the vector $\bm v$ +Also defining the vector $\bm v$ \begin{multline} \bm v_{j,1} = K + 2d\bm{\hat{s}}^{T}_{j,3}\bm F_{ext,j+1} - \left( I_{s_{j,1}} - I_{s_{j,3}}\right) \omega_{s_{j,3}} \omega_{s_{j,1}} - m_p d \hat{s}^{T}_{j,3}\Big[2[\bm{\tilde{\omega}}_{\cal B/N}]\bm r'_{S_{j}/B} + [\bm{\tilde{\omega}}_{\cal B/N}][\bm{\tilde{\omega}}_{\cal B/N}]\bm r_{S_{j}/B}\\ +\sum^{N_p}_{i=j+1}\Big(4[\bm{\tilde{\omega}}_{\cal B/N}]\bm r'_{S_{j}/B} + 2[\bm{\tilde{\omega}}_{\cal B/N}][\bm{\tilde{\omega}}_{\cal B/N}]\bm r_{S_{j}/B} \Big) \\ @@ -403,7 +403,7 @@ \subsection{Back Substitution Method} Combining like terms yields: \begin{multline} - \Bigg \lbrace m_{\textnormal{sc}}[I_{3\times 3}] + \sum^{N_p}_{i=1} \Big[\sum^{N_p}_{k=i} (2[N_p-k]+1) d m_{\text{p}} \bm{\hat{s}}_{k,3}\Big] e_{i}^T[F] \Bigg \rbrace \ddot{\bm r}_{B/N} + \Bigg \lbrace m_{\textnormal{sc}}[I_{3\times 3}] + \sum^{N_p}_{i=1} \Big[\sum^{N_p}_{k=i} (2[N_p-k]+1) d m_{\text{p}} \bm{\hat{s}}_{k,3}\Big] e_{i}^T[F] \Bigg \rbrace \ddot{\bm r}_{B/N} \\ + \Bigg \lbrace -m_{\textnormal{sc}} [\tilde{\bm{c}}] + \sum^{N_p}_{i=1} \Big[\sum^{N_p}_{k=i} (2[N_p-k]+1) d m_{\text{p}} \bm{\hat{s}}_{k,3}\Big] e_{i}^T[G]\Bigg \rbrace \dot{\bm\omega}_{\cal B/N}\\ = \bm F_{\textnormal{ext}} - 2 m_{\textnormal{sc}} [\tilde{\bm\omega}_{\cal B/N}]\bm{c}' @@ -431,25 +431,25 @@ \subsection{Back Substitution Method} \end{multline} With the following definitions: \begin{equation} - [A_{\text{contr}}] = \sum^{N_p}_{i=1} \Big[\sum^{N_p}_{k=i} (2[N_p-k]+1) d m_{\text{p}} \bm{\hat{s}}_{k,3}\Big] e_{i}^T[F] + [A_{\text{contr}}] = \sum^{N_p}_{i=1} \Big[\sum^{N_p}_{k=i} (2[N_p-k]+1) d m_{\text{p}} \bm{\hat{s}}_{k,3}\Big] e_{i}^T[F] \end{equation} \begin{equation} [B_{\text{contr}}] = \sum^{N_p}_{i=1} \Big[\sum^{N_p}_{k=i} (2[N_p-k]+1) d m_{\text{p}} \bm{\hat{s}}_{k,3}\Big] e_{i}^T[G] \end{equation} \begin{equation} \bm v_{\text{trans,contr}} = -\sum^{N_p}_{i=1} \Big[ \Big(\sum^{i}_{k=1}\dot{\theta}_k\Big)^2 (2[N_p-i]+1) d m_{\text{p}} \bm{\hat{s}}_{i,1} \Big]\\ - - \sum^{N_p}_{i=1} \Big[\sum^{N_p}_{k=i} (2[N_p-k]+1) d m_{\text{p}} \bm{\hat{s}}_{k,3} e_{i}^T \bm v \Big] + - \sum^{N_p}_{i=1} \Big[\sum^{N_p}_{k=i} (2[N_p-k]+1) d m_{\text{p}} \bm{\hat{s}}_{k,3} e_{i}^T \bm v \Big] \end{equation} \begin{equation} - [C_{\text{contr}}] = \sum\limits_{i=1}^{N_p} \Big[\sum^{N_p}_{k=i} (I_{s_{k,2}} \bm{\hat{s}}_{k,2} + \Big[ [\tilde{{\bm r}}_{S_{k}/B}] + \sum\limits_{n=k+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big]m_{\text{p}} d \bm{\hat{s}}_{k,3} ) \Big]e_{i}^T[F] + [C_{\text{contr}}] = \sum\limits_{i=1}^{N_p} \Big[\sum^{N_p}_{k=i} (I_{s_{k,2}} \bm{\hat{s}}_{k,2} + \Big[ [\tilde{{\bm r}}_{S_{k}/B}] + \sum\limits_{n=k+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big]m_{\text{p}} d \bm{\hat{s}}_{k,3} ) \Big]e_{i}^T[F] \end{equation} \begin{equation} [D_{\text{contr}}] = \sum\limits_{i=1}^{N_p} \Big[\sum^{N_p}_{k=i} (I_{s_{k,2}} \bm{\hat{s}}_{k,2} + \Big[ [\tilde{{\bm r}}_{S_{k}/B}] + \sum\limits_{n=k+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big]m_{\text{p}} d \bm{\hat{s}}_{k,3} )\Big] e_{i}^T[G] \end{equation} \begin{multline} [v_{\text{rot,contr}}] = - \sum\limits_{i=1}^{N_p}\bigg( m_{\text{p}} [\tilde{\bm\omega}_{\cal B/N}] [\tilde{{\bm r}}_{S_{i}/B}] \bm{r}'_{S_i/B} + \Big( \sum\limits_{k=1}^{i} \dot{\theta}\Big)^2 \Big[[\tilde{{\bm r}}_{S_{i}/B}] + \sum\limits_{n=i+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big] m_{\text{p}} d \bm{\hat{s}}_{i,1}\\ - + I_{s_{i,2}} \big(\sum^{i}_{k=1}\dot{\theta}_k\big) [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i,2} \bigg) - - \sum\limits_{i=1}^{N_p} \Big[\sum^{N_p}_{k=i} (I_{s_{k,2}} \bm{\hat{s}}_{k,2} + \Big[ [\tilde{{\bm r}}_{S_{k}/B}] + \sum\limits_{n=k+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big]m_{\text{p}} d \bm{\hat{s}}_{k,3} )\Big] e_{i}^T \bm v + + I_{s_{i,2}} \big(\sum^{i}_{k=1}\dot{\theta}_k\big) [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i,2} \bigg) + - \sum\limits_{i=1}^{N_p} \Big[\sum^{N_p}_{k=i} (I_{s_{k,2}} \bm{\hat{s}}_{k,2} + \Big[ [\tilde{{\bm r}}_{S_{k}/B}] + \sum\limits_{n=k+1}^{N_p}2[\tilde{{\bm r}}_{S_{n}/B}]\Big]m_{\text{p}} d \bm{\hat{s}}_{k,3} )\Big] e_{i}^T \bm v \end{multline} The full back substitution matrices then become: \begin{equation} @@ -495,5 +495,3 @@ \subsection{Back Substitution Method} \end{equation} Now Eq.~\eqref{eq:omegaDot} and ~\eqref{eq:rBNDDot} can be used to solve for $\dot{\bm\omega}_{\cal B/N}$ and $\ddot{\bm r}_{B/N}$. Once these second order state variables are solved for, Eq.~\eqref{eq:thetadot4} can be used to directly solve for $\ddot \theta_{i}$. This shows that the back substitution method can work seamlessly for interconnected bodies. - - diff --git a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secModelFunctions.tex index 29336c506d..90c076a2af 100644 --- a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secModelFunctions.tex @@ -20,4 +20,4 @@ \section{Model Assumptions and Limitations} \item The N hinged rigid body will always stay attached to the hub (the hinge does not have torque limits) \item The hinge does not have travel limits, therefore if the spring is not stiff enough it will unrealistically travel through bounds such as running into the spacecraft hub \item The EOMs are nonlinear equations of motion, therefore there can be inaccuracies (and divergence) that result from integration. Having a time step of $<= 0.10\ \text{sec}$ is recommended, but this also depends on the natural frequency of the system -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secTest.tex b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secTest.tex index a30a150ec8..097743d168 100644 --- a/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secTest.tex +++ b/src/simulation/dynamics/NHingedRigidBodies/_Documentation/secTest.tex @@ -2,7 +2,7 @@ \section{Test Description and Success Criteria} This test is located in \texttt{simulation/dynamics/nHingedRigidBodies/UnitTest/\newline test\_nHingedRigidBodyStateEffector.py}. In this integrated test there are two hinged rigid bodies connected to the spacecraft hub, one with 4 interconnected panels and one with 3 interconnected panels. Depending on the scenario, there are different success criteria. These are outlined in the following subsections: \subsection{Gravity integrated test} -In this test the simulation is placed into orbit around Earth with point gravity and has no damping in the hinged rigid bodies. The following parameters are being tested. +In this test the simulation is placed into orbit around Earth with point gravity and has no damping in the hinged rigid bodies. The following parameters are being tested. \begin{itemize} \item Conservation of orbital angular momentum \item Conservation of orbital energy @@ -27,7 +27,7 @@ \section{Test Parameters} \caption{Error Tolerance - Note: Relative Tolerance is $\textnormal{abs}(\frac{\textnormal{truth} - \textnormal{value}}{\textnormal{truth}}$)} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c |} % Column formatting, + \begin{tabular}{| c | c |} % Column formatting, \hline Test & Relative Tolerance \\ \hline @@ -38,7 +38,7 @@ \section{Test Parameters} \section{Test Results} -The following figures show the conservation of the quantities described in the success criteria for each scenario. The conservation plots are all relative difference plots. All conservation plots show integration error which is the desired result. In the python test these values are automatically checked therefore when the tests pass, these values have all been confirmed to be conserved. +The following figures show the conservation of the quantities described in the success criteria for each scenario. The conservation plots are all relative difference plots. All conservation plots show integration error which is the desired result. In the python test these values are automatically checked therefore when the tests pass, these values have all been confirmed to be conserved. \subsection{Gravity with no damping scenario} \input{AutoTex/ChangeInOrbitalAngularMomentumGravity} diff --git a/src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.rst b/src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.rst index a82ec3bf8b..e7b7fd473e 100644 --- a/src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.rst +++ b/src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.rst @@ -14,13 +14,3 @@ how to run it, as well as testing. Message Connection Descriptions ------------------------------- This state effector has no input or output messages. - - - - - - - - - - diff --git a/src/simulation/dynamics/RadiationPressure/_Documentation/AVS.sty b/src/simulation/dynamics/RadiationPressure/_Documentation/AVS.sty index 73e5dd7956..c02abd9dfe 100644 --- a/src/simulation/dynamics/RadiationPressure/_Documentation/AVS.sty +++ b/src/simulation/dynamics/RadiationPressure/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/RadiationPressure/_Documentation/Basilisk-RadiationPressure-20170712.tex b/src/simulation/dynamics/RadiationPressure/_Documentation/Basilisk-RadiationPressure-20170712.tex index 67226f7ac9..81de317564 100644 --- a/src/simulation/dynamics/RadiationPressure/_Documentation/Basilisk-RadiationPressure-20170712.tex +++ b/src/simulation/dynamics/RadiationPressure/_Documentation/Basilisk-RadiationPressure-20170712.tex @@ -73,7 +73,7 @@ \hline 1.2 & Updated on how to specify the SRP model & H. Schaub & June 12 2018\\ \hline - + \end{longtable} } @@ -83,7 +83,7 @@ \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/RadiationPressure/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/RadiationPressure/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/RadiationPressure/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/RadiationPressure/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/RadiationPressure/_Documentation/bibliography.bib b/src/simulation/dynamics/RadiationPressure/_Documentation/bibliography.bib index 385cf7eb5d..508a383069 100755 --- a/src/simulation/dynamics/RadiationPressure/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/RadiationPressure/_Documentation/bibliography.bib @@ -1,7 +1,7 @@ @book{vallado2001, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm Press}, year = {2001}, diff --git a/src/simulation/dynamics/RadiationPressure/_Documentation/secModelDescription.tex b/src/simulation/dynamics/RadiationPressure/_Documentation/secModelDescription.tex index 989bc2b60b..88f5c72036 100644 --- a/src/simulation/dynamics/RadiationPressure/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/RadiationPressure/_Documentation/secModelDescription.tex @@ -7,7 +7,7 @@ \subsection{Radiation Pressure Model} SF_{\mathrm{AU}} = 1372.5398 \bigg[\frac{W}{m^2}\bigg] \end{equation} \subsubsection{Cannonball Method} -The cannonball model assumes the spacecraft is a simple sphere. It is the default SRP model when the {\tt RadiationPressure} module is invoked. The radiation pressure at 1AU, $p_{SR}$, can be taken as the solar flux divided by the speed of light. +The cannonball model assumes the spacecraft is a simple sphere. It is the default SRP model when the {\tt RadiationPressure} module is invoked. The radiation pressure at 1AU, $p_{SR}$, can be taken as the solar flux divided by the speed of light. \begin{equation} p_{SR} = \frac{SF_{\mathrm{AU}}}{c} \bigg[\frac{N}{m^2}\bigg] \end{equation} @@ -41,10 +41,10 @@ \subsubsection{Table Look-up Method} Most important to the user of the table look-up method is the required input and format of data. Data must be recorded in XML format. As an example, see ../cube\_lookup.xml (in the radiation pressure folder). Additionally, a utility script called parseSRPLookup.py is provided there to read the XML input into numpy arrays. Experienced users are welcome to store their data in their own format and load it into equivalent numpy arrays as they see fit.\\\\ An example of using the provided python script to load data is shown in test\_radiationPressure.py. Note that this also requires import of the unitTestSupport library.\\ \subsubsection{Solar Eclipses} -Solar eclipses are are detected by the basilisk eclipse module. The effects of the eclipse are calculated into a shadow factor, $F_{\mathrm{s}}$, which is applied to the output forces and torques. +Solar eclipses are are detected by the basilisk eclipse module. The effects of the eclipse are calculated into a shadow factor, $F_{\mathrm{s}}$, which is applied to the output forces and torques. \begin{equation} \mathbf{F}_{\mathrm{out}} = F_{\mathrm{s}}\mathbf{F}_{\mathrm{full\_sun}} \end{equation} \begin{equation} \bm{\tau}_{\mathrm{out}} = F_{\mathrm{s}}\bm{\tau}_{\mathrm{full\_sun}} -\end{equation} \ No newline at end of file +\end{equation} diff --git a/src/simulation/dynamics/RadiationPressure/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/RadiationPressure/_Documentation/secModelFunctions.tex index 45bff77a30..23cdf0047e 100644 --- a/src/simulation/dynamics/RadiationPressure/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/RadiationPressure/_Documentation/secModelFunctions.tex @@ -8,7 +8,7 @@ \section{Model Functions} \item \textbf{Interface: Forces and Torques}: The code sends spacecraft force and torque contributions via computeForceTorque() which is called by the spacecraft. If using the cannonball method, the returned torque values are zero. \item \textbf{Interface: Sun Ephemeris}: The code receives Sun states (ephemeris information) via the Basilisk messaging system. \item \textbf{Interface: Solar Eclipse}: The code receives solar eclipse (shadow factor) information via the Basilisk messaging system. - + \end{itemize} @@ -21,4 +21,4 @@ \section{Model Assumptions and Limitations} \item \textbf{Radiation}: The radiation model is hard-coded to assume that the radiation comes from the Sun. It is not possible to model radiation pressure from other sources with this code. This applies to both the cannonball and look-up methods. The model has no time-varying radiation effects (solar storms, etc.). A more in-depth radiation model would be need if high-accuracy radiation pressure effect calculations are needed. \item \textbf{Eclipse}: The shadow factor applies a simple scaling factor to the output forces and torques. This assumes that all portions of the spacecraft are affected equally by the eclipse. This should, in most circumstances, be highly accurate. For exceptionally large $A_{\odot}$ spacecraft which also need highly accurate state calculations, this assumption could fail. \item \textbf{Tabulated Data Import} Currently, Basilisk includes a utility script to import data from XML files for use in radiation pressure calculations. While some users could learn to load data in other formats, this is currently a limitation to most users who have data in other forms. -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/simulation/dynamics/RadiationPressure/_Documentation/secTest.tex b/src/simulation/dynamics/RadiationPressure/_Documentation/secTest.tex index 6111aacbda..a3ba17d950 100644 --- a/src/simulation/dynamics/RadiationPressure/_Documentation/secTest.tex +++ b/src/simulation/dynamics/RadiationPressure/_Documentation/secTest.tex @@ -8,13 +8,13 @@ \subsection{Table Look-up Compared to Cannonball} This test compares the output \section{Test Parameters} -This section summarizes the error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. +This section summarizes the error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. \begin{table}[htbp] \caption{Error tolerance for each test.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline \textbf{Test} & \textbf{Tolerated Error} \\ \hline ``Cannonball" &\input{AutoTex/cannonballAccuracy} \\ \hline @@ -39,15 +39,15 @@ \section{Test Results} \caption{Test results.} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | c } % Column formatting, + \begin{tabular}{c | c | c } % Column formatting, \hline \textbf{Test} & \textbf{Pass/Fail} & \textbf{Notes} \\ \hline ``Cannonball" &\input{AutoTex/cannonballPassFail} &\input{AutoTex/cannonballFailMsg} \\ \hline Look-up &\input{AutoTex/lookupPassFail} &\input{AutoTex/lookupFailMsg} \\ \hline - Look-up (torque) & \input{AutoTex/lookupPassFail} &\input{AutoTex/lookupFailMsg} \\ \hline - Look-up With Eclipse &\input{AutoTex/lookupWithEclipsePassFail} &\input{AutoTex/lookupWithEclipseFailMsg}\\ \hline + Look-up (torque) & \input{AutoTex/lookupPassFail} &\input{AutoTex/lookupFailMsg} \\ \hline + Look-up With Eclipse &\input{AutoTex/lookupWithEclipsePassFail} &\input{AutoTex/lookupWithEclipseFailMsg}\\ \hline Look-up With Eclipse (torque) & \input{AutoTex/lookupWithEclipsePassFail} &\input{AutoTex/lookupWithEclipseFailMsg}\\ \hline ``Cannonball'' Look-up &\input{AutoTex/cannonballLookupPassFail} &\input{AutoTex/cannonballLookupFailMsg}\\ \hline ``Cannonball'' Look-up (torque)&\input{AutoTex/cannonballLookupPassFail} &\input{AutoTex/cannonballLookupFailMsg}\\ \hline \end{tabular} -\end{table} \ No newline at end of file +\end{table} diff --git a/src/simulation/dynamics/RadiationPressure/_Documentation/secUserGuide.tex b/src/simulation/dynamics/RadiationPressure/_Documentation/secUserGuide.tex index 32fe225c11..95e268976b 100644 --- a/src/simulation/dynamics/RadiationPressure/_Documentation/secUserGuide.tex +++ b/src/simulation/dynamics/RadiationPressure/_Documentation/secUserGuide.tex @@ -12,7 +12,7 @@ \subsection{Variable Definition and Code Description} \begin{table}[H] \caption{Definition and Explanation of Variables Used.} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ | m{5cm}| m{2cm} | m{1.5cm} | m{6cm} |} % Column formatting, + \begin{tabular}{ | m{5cm}| m{2cm} | m{1.5cm} | m{6cm} |} % Column formatting, \hline \textbf{Variable} & \textbf{LaTeX Equivalent} & \textbf{Variable Type} & \textbf{Notes} \\ \hline area & $A_{\odot}$ & double & [m2] Default setting: 0.0f. Required input for cannonball method to get any real output. This is the area to use when approximating the surface area of the spacecraft.\\ \hline @@ -36,4 +36,4 @@ \subsection{Code Diagram} After the inputs are given, radiation\_pressure.cpp calculates the effects of radiation pressure via one of the two methods. At the end, the eclipse factor scales the output forces and torques. -The spacecraft position and orientation states are obtained through the `spacecraft` state engine variables {\tt hubPosition} and {\tt hubSigma}. There is no longer a need to provide spacecraft states through an input message. \ No newline at end of file +The spacecraft position and orientation states are obtained through the `spacecraft` state engine variables {\tt hubPosition} and {\tt hubSigma}. There is no longer a need to provide spacecraft states through an input message. diff --git a/src/simulation/dynamics/RadiationPressure/_UnitTest/cannonballLookup.xml b/src/simulation/dynamics/RadiationPressure/_UnitTest/cannonballLookup.xml index b4c9adcc26..0e7a9ee08b 100644 --- a/src/simulation/dynamics/RadiationPressure/_UnitTest/cannonballLookup.xml +++ b/src/simulation/dynamics/RadiationPressure/_UnitTest/cannonballLookup.xml @@ -61446,4 +61446,4 @@ 0.0000000000000000 - \ No newline at end of file + diff --git a/src/simulation/dynamics/RadiationPressure/_UnitTest/cube_lookup.xml b/src/simulation/dynamics/RadiationPressure/_UnitTest/cube_lookup.xml index a065ca6a98..be46ef7372 100644 --- a/src/simulation/dynamics/RadiationPressure/_UnitTest/cube_lookup.xml +++ b/src/simulation/dynamics/RadiationPressure/_UnitTest/cube_lookup.xml @@ -3006,4 +3006,4 @@ 0.0000000000003093 - \ No newline at end of file + diff --git a/src/simulation/dynamics/RadiationPressure/cube_lookup.xml b/src/simulation/dynamics/RadiationPressure/cube_lookup.xml index a065ca6a98..be46ef7372 100644 --- a/src/simulation/dynamics/RadiationPressure/cube_lookup.xml +++ b/src/simulation/dynamics/RadiationPressure/cube_lookup.xml @@ -3006,4 +3006,4 @@ 0.0000000000003093 - \ No newline at end of file + diff --git a/src/simulation/dynamics/RadiationPressure/radiationPressure.h b/src/simulation/dynamics/RadiationPressure/radiationPressure.h index 718dab364a..56f17d43a1 100755 --- a/src/simulation/dynamics/RadiationPressure/radiationPressure.h +++ b/src/simulation/dynamics/RadiationPressure/radiationPressure.h @@ -60,7 +60,7 @@ class RadiationPressure: public SysModel, public DynamicEffector{ void addForceLookupBEntry(Eigen::Vector3d vec); void addTorqueLookupBEntry(Eigen::Vector3d vec); void addSHatLookupBEntry(Eigen::Vector3d vec); - + private: void computeCannonballModel(Eigen::Vector3d rSunB_B); void computeLookupModel(Eigen::Vector3d rSunB_B); diff --git a/src/simulation/dynamics/RadiationPressure/radiationPressure.rst b/src/simulation/dynamics/RadiationPressure/radiationPressure.rst index d7580dc972..50d5fd55ab 100644 --- a/src/simulation/dynamics/RadiationPressure/radiationPressure.rst +++ b/src/simulation/dynamics/RadiationPressure/radiationPressure.rst @@ -28,18 +28,3 @@ provides information on what this message is used for. * - sunEclipseInMsg - :ref:`EclipseMsgPayload` - (optional) sun eclipse input message - - - - - - - - - - - - - - - diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/AVS.sty b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/AVS.sty +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/bibliography.bib b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.rst b/src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.rst index ef852773e2..bafe41c40d 100644 --- a/src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.rst +++ b/src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.rst @@ -95,7 +95,7 @@ Model Assumptions and Limitations Assumptions ~~~~~~~~~~~ -The model assumes that the behavior of a thruster is represented by a first-order filter. Therefore, due to the simplicity of the model, some real-world behaviors cannot be simulated, such as overshoot or +The model assumes that the behavior of a thruster is represented by a first-order filter. Therefore, due to the simplicity of the model, some real-world behaviors cannot be simulated, such as overshoot or damping. The thruster module also assumes that the thruster always thrusts along its thrust directions axis. No dispersion is added to the thrust axis with respect to the nominal thruster axis. @@ -103,7 +103,7 @@ The thruster module also assumes that the thruster always thrusts along its thru Limitations ~~~~~~~~~~~ -One of the limitations of this model relates to the dynamic nature of this thruster implementation. The thrust is simulated through the thrust factor, which is updated by integrating a differencial equation. This means that it is not possible to reproduce on-off behavior, where the thruster goes from not thrusting to being at maximum thrust or vice-versa. Using this dynamic model, we would have to use infinite derivatives to +One of the limitations of this model relates to the dynamic nature of this thruster implementation. The thrust is simulated through the thrust factor, which is updated by integrating a differencial equation. This means that it is not possible to reproduce on-off behavior, where the thruster goes from not thrusting to being at maximum thrust or vice-versa. Using this dynamic model, we would have to use infinite derivatives to reproduce this behavior, which is not numerically feasible. To replicate this behavior, the user should use the older version of the thruster effector (:ref:`thrusterDynamicEffector`) with both on or off-ramps disabled. Another limitation is that the :math:`I_{sp}` used is constant throughout the simulation. This means that the mass flow rate of the thruster is constant - the thruster will lose mass as soon as the valve is open, independent of how much thrust force is being produced. If the user needs to change the :math:`I_{sp}` value of any of the thrusters, the simulation needs to be stop and restarted. @@ -122,7 +122,7 @@ This section contains conceptual overviews of the code and clear examples for th Module Setup ~~~~~~~~~~~~ -To use the thruster state effector module, the user first needs to create the thruster and populate it with the necessary information, such as thruster magnitude, minimum on time, etc. This can be done with the help +To use the thruster state effector module, the user first needs to create the thruster and populate it with the necessary information, such as thruster magnitude, minimum on time, etc. This can be done with the help of the :ref:`simIncludeThruster` Basilisk Python library. The code to create a generic thruster is shown below: .. code-block:: python diff --git a/src/simulation/dynamics/VSCMGs/_Documentation/AVS.sty b/src/simulation/dynamics/VSCMGs/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/VSCMGs/_Documentation/AVS.sty +++ b/src/simulation/dynamics/VSCMGs/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/VSCMGs/_Documentation/Basilisk-VSCMGSTATEEFFECTOR-20180718.tex b/src/simulation/dynamics/VSCMGs/_Documentation/Basilisk-VSCMGSTATEEFFECTOR-20180718.tex index 7cd03c8336..da5f25c4d8 100755 --- a/src/simulation/dynamics/VSCMGs/_Documentation/Basilisk-VSCMGSTATEEFFECTOR-20180718.tex +++ b/src/simulation/dynamics/VSCMGs/_Documentation/Basilisk-VSCMGSTATEEFFECTOR-20180718.tex @@ -88,7 +88,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/VSCMGs/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/VSCMGs/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/VSCMGs/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/VSCMGs/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/VSCMGs/_Documentation/bibliography.bib b/src/simulation/dynamics/VSCMGs/_Documentation/bibliography.bib index 77f26cf534..73ac3f0676 100755 --- a/src/simulation/dynamics/VSCMGs/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/VSCMGs/_Documentation/bibliography.bib @@ -15,7 +15,7 @@ @book{schaub Publisher = {{AIAA} Education Series}, Title = {Analytical Mechanics of Space Systems}, Year = {2014}} - + @article{OLSSON1998176, title = "Friction Models and Friction Compensation", journal = "European Journal of Control", @@ -28,7 +28,7 @@ @article{OLSSON1998176 url = "http://www.sciencedirect.com/science/article/pii/S094735809870113X", author = "H. Olsson and K.J. Å ström and C. Canudas de Wit and M. Gäfvert and P. Lischinsky", keywords = "Friction, Friction compensation, Models, Observers, Passivity"} - + @article{newRef, title = "Friction Models and Friction Compensation", journal = "European Journal of Control", @@ -41,13 +41,11 @@ @article{newRef url = "http://www.sciencedirect.com/science/article/pii/S094735809870113X", author = "H. Olsson and K.J. Å ström and C. Canudas de Wit and M. Gäfvert and P. Lischinsky", keywords = "Friction, Friction compensation, Models, Observers, Passivity"} - + @mastersthesis{alcorn2017, - Author = {John Alcorn}, + Author = {John Alcorn}, Title = {Fully-Couled Dynamical Jitter Modeling of Momentum Exchange Devices}, School = {University of Colorado at Boulder}, Year = {2017}, Address = {Engineering Center}, Month = {5}} - - diff --git a/src/simulation/dynamics/VSCMGs/_Documentation/secModelDescription.tex b/src/simulation/dynamics/VSCMGs/_Documentation/secModelDescription.tex index 1979841235..44278af92a 100644 --- a/src/simulation/dynamics/VSCMGs/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/VSCMGs/_Documentation/secModelDescription.tex @@ -7,8 +7,8 @@ \subsection{Introduction} The balanced wheels option is modeling the VSCMG as having their principle inertia axes aligned with spin axis, $\hat{\bm g}_s$, and the center of mass of the wheel is coincident with $\hat{\bm g}_s$. This results in the VSCMG not changing the mass properties of the spacecraft and results in simpler equations. The simple jitter option is approximating the jitter due to mass imbalances by applying an external force and torque to the spacecraft that is proportional to the wheel speeds squared. This is an approximation because in reality this is an internal force and torque. Finally, the fully-coupled mode is modeling VSCMG imbalance dynamics by modeling the static and dynamic imbalances as internal forces and torques which is physically realistic and allows for energy and momentum conservation. Figure \ref{fig:scplusVSCMG} shows the frame and variable definitions used for this problem. The formulation involves a rigid hub with its center of mass location labeled as point $B_c$, VSCMGs made of a gimbal and wheel whose center of mass locations are labeled as $G_{c_i}$ and $W_{c_i}$ respectively. The frames being used for this formulation are the body-fixed frame, \frameDefinition{B}, the gimbal frame of the $i^\text{th}$ gimbal, $\mathcal{G}_i:\{\hat{\bm g}_{s_i},\hat{\bm g}_{t_i},\hat{\bm g}_{g_i}\}$ which is also body-fixed, and the wheel-fixed frame of the $i^\text{th}$ RW, $\mathcal{W}_i:\{\hat{\bm g}_{s_i},\hat{\bm w}_{2_i},\hat{\bm w}_{3_i}\}$. The dynamics are modeled with respect to the $\mathcal{B}$ frame which can be generally oriented. The $\mathcal{W}_i$ frame is oriented such that the $\hat{\bm g}_{s_i}$ axis is aligned with the RW spin axis which is the same as the gimbal torque axis $\hat{\bm g}_{s_{i}}$, the $\hat{\bm w}_{2_i}$ axis is perpendicular to $\hat{\bm g}_{s_i}$ and points in the direction towards the RW center of mass $W_{c_i}$. The $\hat{\bm w}_{3_i}$ completes the right hand rule. The $\mathcal{M}_i$ frame is defined as being equal to the $\mathcal{W}_i$ frame at the beginning of the simulation and therefore the $\mathcal{W}_i$ and $\mathcal{G}_i$ frames are offset by an angle, $\theta_i$, about the $\hat{\bm g}_{s_i} = \hat{\bm g}_{s_i}$ axes. - -A few more key variables in Figure~\ref{fig:scplusVSCMG} need to be defined. The rigid spacecraft structure without the VSCMGs is called the hub. Point $B$ is the origin of the $\mathcal{B}$ frame and is a general body-fixed point that does not have to be identical to the total spacecraft center of mass, nor the rigid hub center of mass $B_{c}$. Point $W_i$ is the origin of the $\mathcal{W}_i$ frame and can also have any location relative to point $B$. Point $C$ is the center of mass of the total spacecraft system including the rigid hub and the VSCMGs. Due to the VSCMG imbalance, the vector $\bm c$, which points from point $B$ to point $C$, will vary as seen by a body-fixed observer. The scalar variable $d_i$ is the center of mass offset of the VSCMG, or the distance from the spin axis, $\hat{\bm g}_{\text{s}_i}$ to $W_{c_i}$. + +A few more key variables in Figure~\ref{fig:scplusVSCMG} need to be defined. The rigid spacecraft structure without the VSCMGs is called the hub. Point $B$ is the origin of the $\mathcal{B}$ frame and is a general body-fixed point that does not have to be identical to the total spacecraft center of mass, nor the rigid hub center of mass $B_{c}$. Point $W_i$ is the origin of the $\mathcal{W}_i$ frame and can also have any location relative to point $B$. Point $C$ is the center of mass of the total spacecraft system including the rigid hub and the VSCMGs. Due to the VSCMG imbalance, the vector $\bm c$, which points from point $B$ to point $C$, will vary as seen by a body-fixed observer. The scalar variable $d_i$ is the center of mass offset of the VSCMG, or the distance from the spin axis, $\hat{\bm g}_{\text{s}_i}$ to $W_{c_i}$. The key equations used to model the dynamics of VSCMGs are produced in a form to make use of back-substitution, a computationally efficent way to solve rigid body dynamics. An explaination of backsubstitution and the corresponding equations are highlighted below. For a more thorough explaination of back-substitution and the derivation of the equations of motion can be found in \href{http://hanspeterschaub.info/Papers/grads/JohnAlcorn.pdf}{Alcorn 2017}\cite{alcorn2017}. @@ -67,8 +67,8 @@ \subsection{Balanced VSCMG} \subsubsection{Equations of Motion} The balanced VSCMG equations of motion are provided here for the reader's convenience. Note that translation is not coupled with $\dot{\Omega}$ or $\ddot{\gamma}_i$. \begin{equation}\label{eq:vscmgTranslationSimpleBS_BACKSUBAPP} -m_\text{sc}\ddot{\bm r}_{B/N} - m_\text{sc}[\tilde{\bm{c}}]\dot{\bm \omega} -= \bm{F} - 2m_\text{sc}[\tilde{\bm{\omega}}]\bm{c}' - m_\text{sc}[\tilde{\bm{\omega}}]^2\bm{c} +m_\text{sc}\ddot{\bm r}_{B/N} - m_\text{sc}[\tilde{\bm{c}}]\dot{\bm \omega} += \bm{F} - 2m_\text{sc}[\tilde{\bm{\omega}}]\bm{c}' - m_\text{sc}[\tilde{\bm{\omega}}]^2\bm{c} \end{equation} The rotational equation of motion includes $\dot{\Omega}_i$ and $\ddot{\gamma}_i$ terms, and is thus coupled with VSCMG motion as seen below. \begin{multline} @@ -77,7 +77,7 @@ \subsubsection{Equations of Motion} +\sum\limits_{i=1}^{N}I_{\text{W}_{s_i}}\hat{\bm{g}}_{\text{s}_i} \dot{\Omega}_i \\ = \bm{L}_B - [I_{\text{sc},B}]'\bm{\omega} - [\tilde{\bm\omega}][I_{\text{sc},B}]\bm\omega --\sum\limits_{i=1}^{N} \Big[ +-\sum\limits_{i=1}^{N} \Big[ + I_{\text{W}_{t_i}}\Omega\dot{\gamma}\hat{\bm{g}}_{\text{t}_i} + \Omega_i\dot{\gamma}_i(I_{\text{W}_{s_i}}-I_{\text{W}_{t_i}})\hat{\bm{g}}_{\text{t}_i} \\+ [\tilde{\bm\omega}][I_{\text{G}_i,G_{c_i}}]\dot{\gamma}_i\hat{\bm{g}}_{\text{g}_i} + [\tilde{\bm{\omega}}][I_{\text{W}_i,W_{c_i}}]\bm{\omega}_{\mathcal{W}_i/\cal B} \Big] \label{eq:vscmgRotationSimpleBS_BACKSUBAPP} @@ -94,7 +94,7 @@ \subsubsection{Equations of Motion} I_{\text{W}_{s_i}}(\hat{\bm g}_{\text{s}_i}^T\dot{\bm\omega} + \dot{\Omega}_i) =-I_{\text{W}_{s_i}}\omega_t\dot{\gamma}_i + u_{\text{s}_i} \label{eq:vscmgWheelTorqueSimpleBS_BACKSUBAPP} -\end{equation} +\end{equation} \subsubsection{Modified EOM for Back-Substitution} To make use of back-substitution and define the back-substitution contribution matrices for a balanced VSCMG, the EOM must be arranged into the following form: @@ -102,7 +102,7 @@ \subsubsection{Modified EOM for Back-Substitution} m_{\text{\text{sc}}}[\tilde{\bm{c}}]\ddot{\bm r}_{B/N} + \Big[ [I_{\text{sc},B}] - \sum\limits_{i=1}^{N}\big(I_{\text{V}_{g_i}}\hat{\bm{g}}_{\text{g}_i}\hat{\bm g}_{\text{g}_i}^T\dot{\bm{\omega}} + I_{\text{W}_{s_i}}\hat{\bm{g}}_{\text{s}_i}\hat{\bm g}_{\text{s}_i}^T\big)\Big]\dot{\bm\omega} \\ = \bm{L}_B - [I_{\text{sc},B}]'\bm{\omega} - [\tilde{\bm\omega}][I_{\text{sc},B}]\bm\omega --\sum\limits_{i=1}^{N} \Big[ +-\sum\limits_{i=1}^{N} \Big[ \big( u_{\text{s}_i}-I_{\text{W}_{s_i}}\omega_t\dot{\gamma}_i \big) \hat{\bm{g}}_{\text{s}_i} + I_{\text{W}_{s_i}}\Omega\dot{\gamma}\hat{\bm{g}}_{\text{t}_i} \\+\big( u_{\text{g}_i} + (I_{\text{V}_{s_i}}-I_{\text{V}_{t_i}})\omega_s\omega_t + I_{\text{W}_{s_i}}\Omega_i\omega_t\big)\hat{\bm{g}}_{\text{g}_i}+ [\tilde{\bm\omega}][I_{\text{G}_i,G_{c_i}}]\dot{\gamma}_i\hat{\bm{g}}_{\text{g}_i} + [\tilde{\bm{\omega}}][I_{\text{W}_i,W_{c_i}}]\bm{\omega}_{\mathcal{W}_i/\cal B} \Big] \label{eq:vscmgRotationSimpleBS2_BACKSUBAPP} @@ -122,7 +122,7 @@ \subsubsection{Back-Substitution Contribution Matrices} \bm v_{\text{trans,contr}} = \bm 0 \end{gather} \begin{multline} -\bm v_{\text{rot,contr}} = -\sum\limits_{i=1}^{N} \Big[ +\bm v_{\text{rot,contr}} = -\sum\limits_{i=1}^{N} \Big[ \big( u_{\text{s}_i}-I_{\text{W}_{s_i}}\omega_t\dot{\gamma}_i \big) \hat{\bm{g}}_{\text{s}_i} + I_{\text{W}_{s_i}}\Omega\dot{\gamma}\hat{\bm{g}}_{\text{t}_i} +\big( u_{\text{g}_i} + (I_{\text{V}_{s_i}}-I_{\text{V}_{t_i}})\omega_s\omega_t + I_{\text{W}_{s_i}}\Omega_i\omega_t\big)\hat{\bm{g}}_{\text{g}_i}\\+ [\tilde{\bm\omega}][I_{\text{G}_i,G_{c_i}}]\dot{\gamma}_i\hat{\bm{g}}_{\text{g}_i} + [\tilde{\bm{\omega}}][I_{\text{W}_i,W_{c_i}}]\bm{\omega}_{\mathcal{W}_i/\cal B} \Big] \end{multline} @@ -132,18 +132,18 @@ \subsection{Imbalanced VSCMG} \subsubsection{Translational EOMs} The translational equation of motion for an imbalanced VSCMG is: \begin{multline} -\ddot{\bm r}_{B/N} - [\tilde{\bm{c}}]\dot{\bm \omega} +\ddot{\bm r}_{B/N} - [\tilde{\bm{c}}]\dot{\bm \omega} + \frac{1}{m_{\text{sc}}} \sum\limits_{i=1}^{N} \left[ m_{\text{G}_i} \left[\tilde{\hat{\bm g}}_{\text{g}_i} \right] \bm{r}_{G_{c_i}/G_i} - m_{\text{W}_i}d_ic\theta_i\hat{\bm{g}}_{\text{s}_i} + m_{\text{W}_i}\ell_i\hat{\bm{g}}_{\text{t}_i} \right] \ddot{\gamma}_i + \frac{1}{m_{\text{sc}}} \sum\limits_{i=1}^{N} \left[ m_{\text{W}_i}d_i\hat{\bm w}_{3_i} \right] \dot{\Omega}_i -\\= \ddot{\bm r}_{C/N} - 2[\tilde{\bm{\omega}}]\bm{c}' - [\tilde{\bm{\omega}}][\tilde{\bm{\omega}}]\bm{c} -- \frac{1}{m_{\text{sc}}} \sum\limits_{i=1}^{N} \Big[ +\\= \ddot{\bm r}_{C/N} - 2[\tilde{\bm{\omega}}]\bm{c}' - [\tilde{\bm{\omega}}][\tilde{\bm{\omega}}]\bm{c} +- \frac{1}{m_{\text{sc}}} \sum\limits_{i=1}^{N} \Big[ m_{\text{G}_i}\dot{\gamma}_i[\tilde{\hat{\bm g}}_{\text{g}_i}] \bm{r}_{G_{c_i}/B}' \\+ m_{\text{W}_i} \big[ \left(2d_i\dot{\gamma}_i\Omega_i\text{s}\theta_i - \ell_i\dot{\gamma}_i^2 \right)\hat{\bm{g}}_{\text{s}_i} - d_i\dot{\gamma}_i^2c\theta_i\hat{\bm{g}}_{\text{t}_i} - d_i\Omega_i^2 \hat{\bm{w}}_{2_i} \big] \Big] \end{multline} This equation represents 3 DOF and contains all second order states ($\ddot{\bm r}_{B/N}$, $\dot{\bm \omega}$, $\ddot{\gamma}_i$, $\dot{\Omega}_i$). Removing wheel imbalance terms and assuming a symmetrical VSCMG (i.e. $\bm{r}_{G_{c_i}/G_i} = \bm{0}$, $\ell_i = 0$, $d_i = 0$) gives the following balanced VSCMG transational equation of motion. \begin{equation} - m_\text{sc}\ddot{\bm r}_{B/N} - m_\text{sc}[\tilde{\bm{c}}]\dot{\bm \omega} - = \bm{F} - 2m_\text{sc}[\tilde{\bm{\omega}}]\bm{c}' - m_\text{sc}[\tilde{\bm{\omega}}]^2\bm{c} + m_\text{sc}\ddot{\bm r}_{B/N} - m_\text{sc}[\tilde{\bm{c}}]\dot{\bm \omega} + = \bm{F} - 2m_\text{sc}[\tilde{\bm{\omega}}]\bm{c}' - m_\text{sc}[\tilde{\bm{\omega}}]^2\bm{c} \label{eq:vscmgTranslationSimple} \end{equation} @@ -155,16 +155,16 @@ \subsubsection{Rotational EOMs} \begin{equation} \begin{split} m_{\text{\text{sc}}}[\tilde{\bm{c}}]\ddot{\bm r}&_{B/N} + [I_{\text{sc},B}]\dot{\bm\omega} - +\sum\limits_{i=1}^{N} \Big[ [I_{\text{G}_i,G_{c_i}}]\hat{\bm{g}}_{\text{g}_i} + m_{\text{G}_i}[\tilde{\bm{r}}_{G_{c_i}/B}][\tilde{\hat{\bm g}}_{\text{g}_i}]\bm{r}_{G_{c_i}/G_i} + [I_{\text{W}_i,W_{c_i}}]\hat{\bm{g}}_{\text{g}_i} + +\sum\limits_{i=1}^{N} \Big[ [I_{\text{G}_i,G_{c_i}}]\hat{\bm{g}}_{\text{g}_i} + m_{\text{G}_i}[\tilde{\bm{r}}_{G_{c_i}/B}][\tilde{\hat{\bm g}}_{\text{g}_i}]\bm{r}_{G_{c_i}/G_i} + [I_{\text{W}_i,W_{c_i}}]\hat{\bm{g}}_{\text{g}_i} \\ + m_{\text{W}_i}&[\tilde{\bm{r}}_{W_{c_i}/B}]( \ell_i\hat{\bm{g}}_{\text{t}_i} - d_ic\theta_i\hat{\bm{g}}_{\text{s}_i}) \Big] \ddot{\gamma}_i +\sum\limits_{i=1}^{N} \left[ [I_{\text{W}_i,W_{c_i}}]\hat{\bm{g}}_{\text{s}_i} + m_{\text{W}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm w}_{3_i} \right] \dot{\Omega}_i \\ =& \bm{L}_B - [I_{\text{sc},B}]'\bm{\omega} - [\tilde{\bm\omega}][I_{\text{sc},B}]\bm\omega - -\sum\limits_{i=1}^{N} \Big[ + -\sum\limits_{i=1}^{N} \Big[ [I_{\text{G}_i,G_{c_i}}]'\dot{\gamma}_i\hat{\bm{g}}_{\text{g}_i} + [\tilde{\bm\omega}][I_{\text{G}_i,G_{c_i}}]\dot{\gamma}_i\hat{\bm{g}}_{\text{g}_i} + m_{\text{G}_i}[\tilde{\bm{\omega}}][\tilde{\bm{r}}_{G_{c_i}/B}]\bm{r}'_{G_{c_i}/B} \\&+ m_{\text{G}_i}\dot{\gamma}_i[\tilde{\bm{r}}_{G_{c_i}/B}][\tilde{\hat{\bm g}}_{\text{g}_i}]\bm{r}'_{G_{c_i}/G_i} - + [I_{\text{W}_i,W_{c_i}}]\Omega\dot{\gamma}\hat{\bm{g}}_{\text{t}_i} + [I_{\text{W}_i,W_{c_i}}]'\bm{\omega}_{\mathcal{W}_i/\cal B} + [\tilde{\bm{\omega}}][I_{\text{W}_i,W_{c_i}}]\bm{\omega}_{\mathcal{W}_i/\cal B} + + [I_{\text{W}_i,W_{c_i}}]\Omega\dot{\gamma}\hat{\bm{g}}_{\text{t}_i} + [I_{\text{W}_i,W_{c_i}}]'\bm{\omega}_{\mathcal{W}_i/\cal B} + [\tilde{\bm{\omega}}][I_{\text{W}_i,W_{c_i}}]\bm{\omega}_{\mathcal{W}_i/\cal B} \\ &+ m_{\text{W}_i}[\tilde{\bm{\omega}}][\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B} + m_{\text{W}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\left[\left(2d_i\dot{\gamma}_i\Omega_i\text{s}\theta_i - \ell_i\dot{\gamma}_i^2\right)\hat{\bm{g}}_{\text{s}_i} - d_i\dot{\gamma}_i^2c\theta_i\hat{\bm{g}}_{\text{t}_i} - d_i\Omega_i^2 \hat{\bm{w}}_{2_i} \right] \Big] @@ -178,20 +178,20 @@ \subsubsection{Rotational EOMs} +\sum\limits_{i=1}^{N}I_{\text{W}_{s_i}}\hat{\bm{g}}_{\text{s}_i} \dot{\Omega}_i \\ = \bm{L}_B - [\tilde{\bm\omega}][I_{\text{sc},B}]\bm\omega --\sum\limits_{i=1}^{N} \Big[ +-\sum\limits_{i=1}^{N} \Big[ \omega_t\dot{\gamma}_i(I_{\text{V}_{s_i}}-I_{\text{V}_{t_i}}+I_{\text{V}_{g_i}}) \hat{\bm{g}}_{\text{s}_i} \\+ \big[ \omega_s\dot{\gamma}_i(I_{\text{V}_{s_i}}-I_{\text{V}_{t_i}}-I_{\text{V}_{g_i}}) + I_{\text{W}_{s_i}}\Omega_i(\dot{\gamma} + \omega_g) \big]\hat{\bm{g}}_{\text{t}_i} -\omega_tI_{\text{W}_{s_i}}\Omega_i\hat{\bm{g}}_{\text{g}_i} \Big] \label{eq:vscmgRotationSimple} \end{multline} -This equation agrees with that found in Eq. \ref{eq:vscmgRotationSimpleBS_BACKSUBAPP}. +This equation agrees with that found in Eq. \ref{eq:vscmgRotationSimpleBS_BACKSUBAPP}. \subsubsection{Gimbal Torque Equation} The VSCMG gimbal torque equation of motion is given by: \begin{multline} \hat{\bm g}_{\text{g}_i}^T \Bigg[ m_{\text{V}_i}[\tilde{\bm r}_{V_{c_i}/G_i}] \Bigg] \ddot{\bm r}_{B/N} + \hat{\bm g}_{\text{g}_i}^T \Bigg[ [I_{\text{V}_i,V_{c_i}}] + m_{\text{V}_i}[\tilde{\bm r}_{V_{c_i}/G_i}][\tilde{\bm{r}}_{V_{c_i}/B}]^T \Bigg] \dot{\bm{\omega}} -+ \hat{\bm g}_{\text{g}_i}^T \Bigg[ [I_{\text{G}_i,G_{c_i}}]\hat{\bm{g}}_{\text{g}_i} ++ \hat{\bm g}_{\text{g}_i}^T \Bigg[ [I_{\text{G}_i,G_{c_i}}]\hat{\bm{g}}_{\text{g}_i} \\+ [I_{\text{W}_i,W_{c_i}}]\hat{\bm{g}}_{\text{g}_i} + [P_i] \big(\ell_i\hat{\bm{g}}_{\text{t}_i} - d_ic\theta_i\hat{\bm{g}}_{\text{s}_i}\big) + [Q_i] [\tilde{\hat{\bm g}}_{\text{g}_i}]\bm{r}_{G_{c_i}/G_i} \Bigg] \ddot{\gamma}_i + \hat{\bm g}_{\text{g}_i}^T \Bigg[ [I_{\text{W}_i,W_{c_i}}]\hat{\bm{g}}_{\text{s}_i} + [P_i] d_i\hat{\bm w}_{3_i} \Bigg] \dot{\Omega}_i \\ = - \hat{\bm g}_{\text{g}_i}^T \Bigg[ \dot{\gamma}_i[Q_i][\tilde{\hat{\bm g}}_{\text{g}_i}]\bm{r}'_{G_{c_i}/G_i} + [P_i] \big[\left(2d_i\dot{\gamma}_i\Omega_i\text{s}\theta_i - \ell_i\dot{\gamma}_i^2\right)\hat{\bm{g}}_{\text{s}_i} - d_i\dot{\gamma}_i^2c\theta_i\hat{\bm{g}}_{\text{t}_i} - d_i\Omega_i^2 \hat{\bm{w}}_{2_i}\big] @@ -200,7 +200,7 @@ \subsubsection{Gimbal Torque Equation} \\ + m_{\text{W}_i}[\tilde{\bm{r}}_{W_{c_i}/V_{c_i}}]\big( 2[\tilde{\bm\omega}]\bm{r}'_{W_{c_i}/V_{c_i}} + [\tilde{\bm\omega}]^2\bm{r}_{W_{c_i}/V_{c_i}}\big) + m_{\text{V}_i}[\tilde{\bm r}_{V_{c_i}/G_i}]\big( 2[\tilde{\bm\omega}]\bm{r}'_{V_{c_i}/B} + [\tilde{\bm\omega}]^2\bm{r}_{V_{c_i}/B} \big) \Bigg] + u_{\text{g}_i} \label{eq:vscmgtorque} -\end{multline} +\end{multline} Where, \begin{gather} [I_{\text{V}_i,V_{c_i}}] = [I_{\text{G}_i,V_{c_i}}] + [I_{\text{W}_i,V_{c_i}}] @@ -229,18 +229,18 @@ \subsubsection{Wheel Torque Equation} \begin{multline} \left[ m_{\text{W}_i}d_i\hat{\bm w}_{3_i}^T \right] \ddot{\bm r}_{B/N} + \left[ \hat{\bm g}_{\text{s}_i}^T[I_{\text{W}_i,W_{c_i}}] + m_{\text{W}_i}d_i\hat{\bm g}_{\text{s}_i}^T[\tilde{\hat{\bm w}}_{2_i}][\tilde{\bm{r}}_{W_{c_i}/B}]^T \right] \dot{\bm\omega} \\ - +\left[ J_{12_i}s\theta_i + J_{13_i}c\theta_i - m_{\text{W}_i}d_i\ell_is\theta_i \right] \ddot{\gamma}_i + +\left[ J_{12_i}s\theta_i + J_{13_i}c\theta_i - m_{\text{W}_i}d_i\ell_is\theta_i \right] \ddot{\gamma}_i + \left[ J_{11_i} + m_{\text{W}_i}d_i^2 \right] \dot{\Omega}_i \\ =-\hat{\bm g}_{\text{s}_i}^T \Bigg[ [I_{\text{W}_i,W_{c_i}}]'\bm\omega_{\mathcal{W}_i/\cal N} + [\tilde{\bm{\omega}}][I_{\text{W}_i,W_{c_i}}]\bm\omega_{\mathcal{W}_i/\cal N} + m_{\text{W}_i}d_i[\tilde{\hat{\bm w}}_{2_i}] \Big[ 2[\tilde{\bm{r}}'_{W_{c_i}/B}]^T\bm\omega + [\tilde{\bm\omega}][\tilde{\bm\omega}]\bm{r}_{W_{c_i}/B} \Big] \Bigg] \\+(J_{13_i}s\theta_i - J_{12_i}c\theta_i)\Omega\dot{\gamma} - m_{\text{W}_i}d_i^2\dot{\gamma}_i^2c\theta_is\theta_i + u_{\text{s}_i} -\end{multline} +\end{multline} Removing imbalance terms gives (recall that for the simplified case $\theta_i = 0$), \begin{equation} I_{\text{W}_{s_i}}(\hat{\bm g}_{\text{s}_i}^T\dot{\bm\omega} + \dot{\Omega}_i) =-I_{\text{W}_{s_i}}\omega_t\dot{\gamma}_i + u_{\text{s}_i} -\end{equation} +\end{equation} which agrees with \eqref{eq:vscmgWheelTorqueSimpleBS_BACKSUBAPP}. \subsubsection{Modified EOM for Back-Substitution} @@ -288,7 +288,7 @@ \subsubsection{Modified EOM for Back-Substitution} \bm{v}_{\omega_i} &= [I_{\text{W}_i,W_{c_i}}]\hat{\bm{g}}_{\text{s}_i} + m_{\text{W}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm w}_{3_i} \\ \begin{split} -\bm{k}_{\omega_i} = +\bm{k}_{\omega_i} = [I_{\text{G}_i,G_{c_i}}]'\dot{\gamma}_i\hat{\bm{g}}_{\text{g}_i} + [\tilde{\bm\omega}][I_{\text{G}_i,G_{c_i}}]\dot{\gamma}_i\hat{\bm{g}}_{\text{g}_i} + m_{\text{G}_i}[\tilde{\bm{\omega}}][\tilde{\bm{r}}_{G_{c_i}/B}]\bm{r}'_{G_{c_i}/B} + m_{\text{G}_i}\dot{\gamma}_i[\tilde{\bm{r}}_{G_{c_i}/B}][\tilde{\hat{\bm g}}_{\text{g}_i}]\bm{r}'_{G_{c_i}/G_i} \\+ [I_{\text{W}_i,W_{c_i}}]\Omega\dot{\gamma}\hat{\bm{g}}_{\text{t}_i} + [I_{\text{W}_i,W_{c_i}}]'\bm{\omega}_{\mathcal{W}_i/\cal B} + [\tilde{\bm{\omega}}][I_{\text{W}_i,W_{c_i}}]\bm{\omega}_{\mathcal{W}_i/\cal B} + m_{\text{W}_i}[\tilde{\bm{\omega}}][\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B} \\+ m_{\text{W}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\left[\left(2d_i\dot{\gamma}_i\Omega_i\text{s}\theta_i - \ell_i\dot{\gamma}_i^2\right)\hat{\bm{g}}_{\text{s}_i} - d_i\dot{\gamma}_i^2c\theta_i\hat{\bm{g}}_{\text{t}_i} - d_i\Omega_i^2 \hat{\bm{w}}_{2_i} \right] @@ -297,10 +297,10 @@ \subsubsection{Modified EOM for Back-Substitution} \begin{multline} \left[ m_{\text{sc}}[\tilde{\bm c}]+ \sum\limits_{i=1}^{N}\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)\bm{a}_{\Omega_i}^T \right] \ddot{\bm r}_{B/N} -\\ -+ \left[ [I_{\text{sc},B}] + \sum\limits_{i=1}^{N}\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)\bm{b}_{\Omega_i}^T \right] \dot{\bm\omega}_{\cal B/N} -\\= -\sum\limits_{i=1}^{N}\Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big) \\ +\\ ++ \left[ [I_{\text{sc},B}] + \sum\limits_{i=1}^{N}\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)\bm{b}_{\Omega_i}^T \right] \dot{\bm\omega}_{\cal B/N} +\\= +\sum\limits_{i=1}^{N}\Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big) \\ -\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)c_{\Omega_i}\Big] \\ -[\tilde{\bm\omega}_{\cal B/N}][I_{\text{sc},B}]\bm\omega_{\cal B/N}- [I_{\text{sc},B}]'\bm\omega_{\cal B/N} + \bm{L}_B \end{multline} @@ -319,7 +319,7 @@ \subsubsection{Modified EOM for Back-Substitution} \\ + m_{\text{W}_i}[\tilde{\bm{r}}_{W_{c_i}/V_{c_i}}]\big( 2[\tilde{\bm\omega}]\bm{r}'_{W_{c_i}/V_{c_i}} + [\tilde{\bm\omega}]^2\bm{r}_{W_{c_i}/V_{c_i}}\big) + m_{\text{V}_i}[\tilde{\bm r}_{V_{c_i}/G_i}]\big( 2[\tilde{\bm\omega}]\bm{r}'_{V_{c_i}/B} + [\tilde{\bm\omega}]^2\bm{r}_{V_{c_i}/B} \big) \Bigg] + u_{\text{g}_i} \label{eq:vscmgtorqueMod} -\end{multline} +\end{multline} where, \begin{align} [I_{\text{V}_i,V_{c_i}}] &= [I_{\text{G}_i,V_{c_i}}] + [I_{\text{W}_i,V_{c_i}}] @@ -341,13 +341,13 @@ \subsubsection{Modified EOM for Back-Substitution} \begin{multline} \left[ m_{\text{W}_i}d_i\hat{\bm w}_{3_i}^T \right] \ddot{\bm r}_{B/N} + \left[ \hat{\bm g}_{\text{s}_i}^T[I_{\text{W}_i,W_{c_i}}] + m_{\text{W}_i}d_i\hat{\bm g}_{\text{s}_i}^T[\tilde{\hat{\bm w}}_{2_i}][\tilde{\bm{r}}_{W_{c_i}/B}]^T \right] \dot{\bm\omega} \\ -+\left[ J_{12_i}s\theta_i + J_{13_i}c\theta_i - m_{\text{W}_i}d_i\ell_is\theta_i \right] \ddot{\gamma}_i ++\left[ J_{12_i}s\theta_i + J_{13_i}c\theta_i - m_{\text{W}_i}d_i\ell_is\theta_i \right] \ddot{\gamma}_i + \left[ J_{11_i} + m_{\text{W}_i}d_i^2 \right] \dot{\Omega}_i \\ =-\hat{\bm g}_{\text{s}_i}^T \Bigg[ [I_{\text{W}_i,W_{c_i}}]'\bm\omega_{\mathcal{W}_i/\cal N} + [\tilde{\bm{\omega}}][I_{\text{W}_i,W_{c_i}}]\bm\omega_{\mathcal{W}_i/\cal N} + m_{\text{W}_i}d_i[\tilde{\hat{\bm w}}_{2_i}] \Big[ 2[\tilde{\bm{r}}'_{W_{c_i}/B}]^T\bm\omega + [\tilde{\bm\omega}][\tilde{\bm\omega}]\bm{r}_{W_{c_i}/B} \Big] \Bigg] \\+(J_{13_i}s\theta_i - J_{12_i}c\theta_i)\Omega\dot{\gamma} - m_{\text{W}_i}d_i^2\dot{\gamma}_i^2c\theta_is\theta_i + u_{\text{s}_i} -\end{multline} +\end{multline} \subsubsection{Back-Substitution Matrices for Imbalanced VSCMG} @@ -366,6 +366,3 @@ \subsubsection{Back-Substitution Matrices for Imbalanced VSCMG} \\ \bm{v}_\text{rot,contr} = - \sum\limits_{i=1}^{N}\Big[\bm{k}_{\omega_i} +\bm{u}_{\omega_i}d_{\gamma_i}+ \big(\bm{v}_{\omega_i} + \bm{u}_{\omega_i}c_{\gamma_i}\big)s_i\Big] \end{gather} - - - diff --git a/src/simulation/dynamics/VSCMGs/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/VSCMGs/_Documentation/secModelFunctions.tex index 949d3e4216..70ed3ce490 100644 --- a/src/simulation/dynamics/VSCMGs/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/VSCMGs/_Documentation/secModelFunctions.tex @@ -17,9 +17,9 @@ \section{Model Assumptions and Limitations} \item The VSCMG is considered a rigid body \item The spin axis is body fixed, therefore does not take into account bearing flexing \item There is no error placed on the torque when converting from the commanded torque to the applied torque - \item For balanced wheels and simple jitter mode the mass properties of the VSCMGs are assumed to be included in the mass and inertia of the rigid body hub, therefore there is zero contributions to the mass properties from the VSCMGs in the dynamics call. - \item For fully-coupled imbalanced VSCMGs mode the mass properties of the VSCMGs are assumed to not be included in the mass and inertia of the rigid body hub. - \item For balanced wheels and simple jitter mode the inertia matrix is assumed to be diagonal with one of it's principle inertia axis equal to the spin axis, and the center of mass of the VSCMG is coincident with the spin axis. + \item For balanced wheels and simple jitter mode the mass properties of the VSCMGs are assumed to be included in the mass and inertia of the rigid body hub, therefore there is zero contributions to the mass properties from the VSCMGs in the dynamics call. + \item For fully-coupled imbalanced VSCMGs mode the mass properties of the VSCMGs are assumed to not be included in the mass and inertia of the rigid body hub. + \item For balanced wheels and simple jitter mode the inertia matrix is assumed to be diagonal with one of it's principle inertia axis equal to the spin axis, and the center of mass of the VSCMG is coincident with the spin axis. \item For simple jitter, the parameters that define the static and dynamic imbalances are $U_s$ and $U_d$. \item For fully-coupled imbalanced wheels the inertia off-diagonal terms, $J_{12}$ and $J_{23}$ are equal to zero and the remaining inertia off-diagonal term $J_{13}$ is found through the setting the dynamic imbalance parameter $U_d$: $J_{13} = U_d$. The center of mass offset, $d$, is found using the static imbalance parameter $U_s$: $d = \frac{U_s}{m_{\text{rw}}}$ -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/simulation/dynamics/VSCMGs/_Documentation/secTest.tex b/src/simulation/dynamics/VSCMGs/_Documentation/secTest.tex index 3b222ef055..7288a02ba6 100644 --- a/src/simulation/dynamics/VSCMGs/_Documentation/secTest.tex +++ b/src/simulation/dynamics/VSCMGs/_Documentation/secTest.tex @@ -43,13 +43,13 @@ \subsection{Fully Coupled Jitter Scenario with Gravity- Integrated Test} \section{Test Parameters} -Since this is an integrated test, the inputs to the test are the physical parameters of the spacecraft along with the initial conditions of the states. These parameters are outlined in Tables~\ref{tab:hub}-~\ref{tab:initial}. Additionally, the error tolerances can be seen in Table~\ref{tab:errortol}. The energy-momentum conservation values will normally have an agreement down to 1e-14, but to ensure cross-platform agreement the tolerance was chose to be 1e-10. The position and attitude checks have a tolerance set to 1e-7 and is because 8 significant digits were chosen as the values being compared to. +Since this is an integrated test, the inputs to the test are the physical parameters of the spacecraft along with the initial conditions of the states. These parameters are outlined in Tables~\ref{tab:hub}-~\ref{tab:initial}. Additionally, the error tolerances can be seen in Table~\ref{tab:errortol}. The energy-momentum conservation values will normally have an agreement down to 1e-14, but to ensure cross-platform agreement the tolerance was chose to be 1e-10. The position and attitude checks have a tolerance set to 1e-7 and is because 8 significant digits were chosen as the values being compared to. \begin{table}[htbp] \caption{Spacecraft Hub Parameters for Energy Momentum Conservation Scenarios} \label{tab:hub} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -64,7 +64,7 @@ \section{Test Parameters} sigma\_BNInit & Initial MRP of $\cal{B}$ frame & $\begin{bmatrix} 0.0 & 0.0 & 0.0 \end{bmatrix}^T$ & - \\ - omega\_BN\_BInit & Initial Angular Velocity of $\cal{B}$ frame & $ + omega\_BN\_BInit & Initial Angular Velocity of $\cal{B}$ frame & $ \begin{bmatrix} 0.08 & & 0.01 & 0.0 \end{bmatrix}^T$ & rad/s \\ @@ -76,7 +76,7 @@ \section{Test Parameters} \caption{VSCMG Parameters Across All Tests} \label{tab:rw15} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -96,7 +96,7 @@ \section{Test Parameters} gamma & Gimbal Angle & 0.0 & rad \\ rGcG\_G & Gimbal Center of Mass in the $\cal{G}$ Frame & $\begin{bmatrix} 0.0001 & -0.02 & 0.1 \end{bmatrix}^T$ & m \\ - + \end{tabular} \end{table} @@ -104,7 +104,7 @@ \section{Test Parameters} \caption{VSCMG 1 Parameters Across All Tests} \label{tab:rw12} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -126,7 +126,7 @@ \section{Test Parameters} \caption{VSCMG 2 Parameters Across All Tests} \label{tab:rw13} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -148,7 +148,7 @@ \section{Test Parameters} \caption{VSCMG 3 Parameters Across All Tests} \label{tab:rw14} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -170,12 +170,12 @@ \section{Test Parameters} \caption{Initial Conditions for Fully-Coupled Jitter Scenario with Gravity} \label{tab:initial} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline r\_CN\_NInit & Initial Position of S/C & $\begin{bmatrix} - -4020339 & 7490567 & 5248299 + -4020339 & 7490567 & 5248299 \end{bmatrix}^T$ & m \\ v\_CN\_NInit & Initial Velocity of S/C & $\begin{bmatrix} -5199.78 & -3436.68 & 1041.58 @@ -188,7 +188,7 @@ \section{Test Parameters} \caption{Initial Conditions for Fully-Coupled Jitter Scenario without Gravity} \label{tab:initial2} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -206,7 +206,7 @@ \section{Test Parameters} \caption{VSCMG Wheel and Gimbal Torque for Simple Jitter and Fully-Coupled Jitter Scenario without Gravity Scenarios} \label{tab:initial3} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -224,14 +224,14 @@ \section{Test Parameters} \caption{Error Tolerance - Note: Relative Tolerance is $\textnormal{abs}(\frac{\textnormal{truth} - \textnormal{value}}{\textnormal{truth}}$)} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c |} % Column formatting, + \begin{tabular}{| c | c |} % Column formatting, \hline Test & Relative Tolerance \\ \hline Energy and Momentum Conservation & 1e-10 \\ \hline Position, Attitude Check & 1e-7 \\ - \hline + \hline \end{tabular} \end{table} @@ -271,12 +271,12 @@ \subsection{Balanced Wheels, Simple Jitter, Fully Coupled Jitter and Fully Coupl \caption{Test results.} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline \textbf{Test} & \textbf{Pass/Fail} \\ \hline Balanced Wheels & \input{AutoTeX/BalancedWheelsPassFail} \\ - Simple Jitter & \input{AutoTeX/JitterSimplePassFail} \\ - Fully Coupled Jitter & \input{AutoTeX/JitterFullyCoupledPassFail} \\ + Simple Jitter & \input{AutoTeX/JitterSimplePassFail} \\ + Fully Coupled Jitter & \input{AutoTeX/JitterFullyCoupledPassFail} \\ Fully Coupled Jitter + Gravity & \input{AutoTeX/JitterFullyCoupledGravityPassFail} \\ \hline \end{tabular} \end{table} diff --git a/src/simulation/dynamics/VSCMGs/_Documentation/secUserGuide.tex b/src/simulation/dynamics/VSCMGs/_Documentation/secUserGuide.tex index 33b171a6ee..194e0e20b1 100644 --- a/src/simulation/dynamics/VSCMGs/_Documentation/secUserGuide.tex +++ b/src/simulation/dynamics/VSCMGs/_Documentation/secUserGuide.tex @@ -25,7 +25,7 @@ \section{User Guide} \textit{VSCMG.rGcG\_G = [[0.0001],[-0.02],[0.1]]} \newline \textit{VSCMG.massW = 6.} \newline \textit{'VSCMG.massG = 6.} \newline - \textit{VSCMG.VSCMGModel = 0} + \textit{VSCMG.VSCMGModel = 0} \item Create an instantiation of a spacecraft: \newline \textit{scObject = spacecraft.Spacecraft()} \item Finally, add the VSCMG object to your spacecraft:\newline diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.rst b/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.rst index 6a16d8e399..e68cba3033 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.rst +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.rst @@ -1,4 +1,4 @@ Manager of states for Basilisk dynamical systems. Allows the state- effector models of a dynamic object to create, get, and update states -present in the model. \ No newline at end of file +present in the model. diff --git a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.h index e55a9d90e9..8375d75d20 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.h @@ -33,7 +33,7 @@ #include "architecture/utilities/avsEigenSupport.h" /** Container for gravitational body data - * + * * This class is designed to hold all of the information for a gravity * body. The nominal use-case has it initialized at the python level and * attached to dynamics using the AddGravityBody method. @@ -67,13 +67,13 @@ class GravBodyData void loadEphemeris(); /** Creates the following properies in the given statesIn object. - * + * * - [planetName].r_PN_N * - [planetName].v_PN_N * - [planetName].mu * - [planetName].J20002Pfix * - [planetName].J20002Pfix_dot - * + * * vr_PN_N`, `v_PN_N`, and `mu` are initialized to zero, while `J20002Pfix` and `J20002Pfix_dot` * are initialized to the values stored in `this->localPlanet`. This usually means that * `J20002Pfix` is initialized to the identity matrix and `J20002Pfix_dot` to zero. @@ -127,7 +127,7 @@ class GravityEffector : public SysModel void registerProperties(DynParamManager &statesIn); /** Calculate gravitational acceleration of s/c wrt inertial (no central body) or wrt central body - * + * * @param r_cF_N is position of center of mass of s/c wrt frame * @param rDot_cF_N is the derivative of above */ @@ -145,8 +145,8 @@ class GravityEffector : public SysModel /** Adds a `GravBodyData` associated with this effector */ void addGravBody(std::shared_ptr gravBody); - /** Called to modify property names to prepend them by the string stored in nameOfSpacecraftAttachedTo - * + /** Called to modify property names to prepend them by the string stored in nameOfSpacecraftAttachedTo + * * This can be used to make property names unique between different `GravityEffector` in a simulation * with multiple dynamic objects. */ diff --git a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.rst b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.rst index 1c549126e5..2b0654421f 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.rst +++ b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.rst @@ -33,7 +33,7 @@ provides information on what this message is used for. The gravity effector contains a list of ``GravBodyData`` objects which contain the planet mass and size properties etc. The following table lists the Spice planet ephemeris input message that can be connected to a ``GravBodyData`` object. -If no message is connected, then the planet has zero position and orientation information by default. +If no message is connected, then the planet has zero position and orientation information by default. .. list-table:: Module I/O Messages :widths: 25 25 50 diff --git a/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.cpp index 30914367ee..28ee8ccf76 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.cpp @@ -185,7 +185,7 @@ void HubEffector::updateEnergyMomContributions(double integTime, Eigen::Vector3d // - Find rotational energy contribution from the hub rotEnergyContr = 1.0/2.0*omegaLocal_BN_B.dot(IHubPntBc_P*omegaLocal_BN_B) + 1.0/2.0*mHub*rDot_BcB_B.dot(rDot_BcB_B); - + return; } @@ -208,4 +208,4 @@ void HubEffector::matchGravitytoVelocityState(Eigen::Vector3d v_CN_N) { this->gravVelocityState->setState(this->velocityState->getState()); this->gravVelocityBcState->setState(v_CN_N); -} \ No newline at end of file +} diff --git a/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.rst b/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.rst index c162e6f385..a78beb5bd9 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.rst +++ b/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.rst @@ -2,22 +2,3 @@ This class is an instantiation of the stateEffector abstract class and is for the hub of the s/c. The hub has 4 states: ``r_BN_N``, ``rDot_BN_N``, ``sigma_BN`` and ``omega_BN_B``. The hub utilizes the back-substitution method for calculating its derivatives using contributions from :ref:`stateEffector` and :ref:`dynamicEffector`. - - - - - - - - - - - - - - - - - - - diff --git a/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.rst b/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.rst index 188d40edb0..5525929abe 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.rst +++ b/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.rst @@ -5,13 +5,3 @@ The module :download:`PDF Description ` contains further information on this module's function, how to run it, as well as testing. - - - - - - - - - - diff --git a/src/simulation/dynamics/dragEffector/dragDynamicEffector.h b/src/simulation/dynamics/dragEffector/dragDynamicEffector.h index bc42fa962f..5b25fc8bbc 100644 --- a/src/simulation/dynamics/dragEffector/dragDynamicEffector.h +++ b/src/simulation/dynamics/dragEffector/dragDynamicEffector.h @@ -71,7 +71,7 @@ class DragDynamicEffector: public SysModel, public DynamicEffector { private: AtmoPropsMsgPayload atmoInData; - + }; diff --git a/src/simulation/dynamics/dragEffector/dragDynamicEffector.rst b/src/simulation/dynamics/dragEffector/dragDynamicEffector.rst index 8c66396460..89665730bf 100644 --- a/src/simulation/dynamics/dragEffector/dragDynamicEffector.rst +++ b/src/simulation/dynamics/dragEffector/dragDynamicEffector.rst @@ -19,4 +19,3 @@ provides information on what this message is used for. * - atmoDensInMsg - :ref:`AtmoPropsMsgPayload` - atmospheric density input message - diff --git a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/AVS.sty b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/AVS.sty +++ b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/Basilisk-DUALHINGEDRIGIDBODYSTATEEFFECTOR-20180102.tex b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/Basilisk-DUALHINGEDRIGIDBODYSTATEEFFECTOR-20180102.tex index 0ec4d10c66..091e962819 100755 --- a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/Basilisk-DUALHINGEDRIGIDBODYSTATEEFFECTOR-20180102.tex +++ b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/Basilisk-DUALHINGEDRIGIDBODYSTATEEFFECTOR-20180102.tex @@ -90,7 +90,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/bibliography.bib b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/bibliography.bib index 9d7bddb4bd..145dace863 100755 --- a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/bibliography.bib @@ -6,7 +6,7 @@ @conference{Allard2016rz Note = {{P}aper No. AAS-16-156}, Title = {General Hinged Solar Panel Dynamics Approximating First-Order Spacecraft Flexing}, Year = {2016}} - + @book{schaub, Address = {Reston, VA}, Author = {Hanspeter Schaub and John L. Junkins}, diff --git a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secModelDescription.tex b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secModelDescription.tex index 1794ade97a..fa463d97fc 100644 --- a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secModelDescription.tex @@ -10,7 +10,7 @@ \subsection{Problem Statement} \includegraphics[]{Figures/Flex_Slosh_Figure} \caption{Frame and variable definitions used for formulation} \label{fig:Flex_Slosh_Figure} -\end{figure} +\end{figure} There are six coordinate frames defined for this formulation. The inertial reference frame is indicated by \frameDefinition{N}. The body fixed coordinate frame, \frameDefinition{B}, which is anchored to the hub and can be oriented in any direction. The first solar panel frame, $\mathcal{S}_{i1}:\{\hat{\bm s}_{i1,1},\hat{\bm s}_{i1,2},\hat{\bm s}_{i1,3}\}$, is a frame with its origin located at its corresponding hinge location, $H_{i1}$. The $\mathcal{S}_{i1}$ frame is oriented such that $\hat{\bm{s}}_{i1,1}$ points antiparallel to the center of mass of the first solar panel, $S_{c,i1}$. The $\hat{\bm{s}}_{i1,2}$ axis is defined as the rotation axis that would yield a positive $\theta_{i1}$ using the right-hand rule. The distance from point $H_{i1}$ to point $S_{c,i1}$ is defined as $d_{i1}$. The total length of the first panel is $l_{i1}$ The hinge frame, $\mathcal{H}_{i1}:\{\hat{\bm h}_{i1,1}, \hat{\bm h}_{i1,2}, \hat{\bm h}_{i1,3} \}$, is a frame fixed with respect to the body frame, and is equivalent to the respective $\mathcal{S}_{i1}$ frame when the corresponding solar panel is undeflected. @@ -18,7 +18,7 @@ \subsection{Problem Statement} There are a few more key locations that need to be defined. Point $B$ is the origin of the body frame, and can have any location with respect to the hub. Point $B_c$ is the location of the center of mass of the rigid hub. -Using the variables and frames defined, the following section outlines the derivation of equations of motion for the spacecraft. +Using the variables and frames defined, the following section outlines the derivation of equations of motion for the spacecraft. \subsection{Derivation of Equations of Motion - Newtonian Mechanics} @@ -37,7 +37,7 @@ \subsubsection{Rigid Spacecraft Hub Translational Motion} The definition of $\bm{c}$ the location of the center of mass of the entire spacecraft, can be seen in Eq. (\ref{eq:c}). \begin{equation} \bm{c} = \frac{1}{m_{\text{sc}}}\Big[m_{\text{\text{hub}}}\bm{r}_{B_{c}/B} +\sum_{i=1}^{N_{S}}\big(m_{\text{sp}_{i1}}\bm{r}_{S_{c,i1}/B}+m_{\text{sp}_{i2}}\bm{r}_{S_{c,i2}/B}\big)\Big] - \label{eq:c} + \label{eq:c} \end{equation} To find the inertial time derivative of $\bm{c}$, it is first necessary to find the time derivative of $\bm{c}$ with respect to the body frame. A time derivative of any vector, $\bm{v}$, with respect to the body frame is denoted by $\bm{v}'$; the inertial time derivative is labeled as $\dot{\bm{v}}$. The first and second body-relative time derivatives of $\bm{c}$ can be seen in Eqs. (\ref{eq:cprime}) and (\ref{eq:cdprime}). \begin{align} @@ -104,7 +104,7 @@ \subsubsection{Rigid Spacecraft Hub Translational Motion} \begin{multline} m_\text{sc} \ddot{\bm r}_{B/N} -m_\text{sc} [\tilde{\bm{c}}] \dot{\bm\omega}_{\cal B/N} + \sum_{i=1}^{N_{S}}\bigg(\Big[m_{\text{sp}_{i1}}d_{i1} \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}}l_{i1} \bm{\hat{s}}_{i1,3}+m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}\Big]\ddot{\theta}_{i1} +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3}\ddot{\theta}_{i2}\bigg) \\ = \bm F - 2m_\text{sc} [\tilde{\bm\omega}_{\cal B/N}] \bm c'- m_\text{sc} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c}\\ - -\sum_{i=1}^{N_{S}}\bigg(m_{\text{sp}_{i1}}d_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} +m_{\text{sp}_{i2}}\Big[l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big]\bigg) + -\sum_{i=1}^{N_{S}}\bigg(m_{\text{sp}_{i1}}d_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} +m_{\text{sp}_{i2}}\Big[l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big]\bigg) \label{eq:Rbddot3} \end{multline} @@ -133,7 +133,7 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} I_{s_{i1,1}} & 0 & 0 \\ 0 & I_{s_{i1,2}} & 0 \\ 0 & 0 & I_{s_{i1,3}} - \end{bmatrix}} + \end{bmatrix}} \label{eq:IspMatrix} \end{equation} @@ -146,13 +146,13 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} I_{s_{i2,1}} & 0 & 0 \\ 0 & I_{s_{i2,2}} & 0 \\ 0 & 0 & I_{s_{i2,3}} - \end{bmatrix}} + \end{bmatrix}} \end{equation} Now the inertial time derivative of Eq. \eqref{eq:Hb2} is taken and yields \begin{multline} - \dot{\bm{H}}_{\text{sc},B} = [I_{\text{hub},B_c}] \dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{hub},B_c}] \bm\omega_{\cal B/N} + m_{\text{hub}} \bm{r}_{B_c/B}\times\ddot{\bm r}_{B_c/B}\\ +\sum\limits_{i=1}^{N_S} \biggl( [I'_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} + [I_{\text{sp}_{i1},S_{c,i1}}] \dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N}\\ + \dot{\bm{H}}_{\text{sc},B} = [I_{\text{hub},B_c}] \dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{hub},B_c}] \bm\omega_{\cal B/N} + m_{\text{hub}} \bm{r}_{B_c/B}\times\ddot{\bm r}_{B_c/B}\\ +\sum\limits_{i=1}^{N_S} \biggl( [I'_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} + [I_{\text{sp}_{i1},S_{c,i1}}] \dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N}\\ + \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}+ \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \ddot{\bm r}_{S_{c,i1}/B}\\ - + [I'_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} + [I_{\text{sp}_{i2},S_{c,i2}}] \dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N}\\ + + [I'_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} + [I_{\text{sp}_{i2},S_{c,i2}}] \dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N}\\ + \big(\ddot{\theta}_{i1} + \ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2}+ \bm\omega_{\cal B/N} \times \big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i1}} \bm{r}_{S_{c,i2}/B} \times \ddot{\bm r}_{S_{c,i2}/B}\biggr) \label{eq:Hbdot} \end{multline} @@ -170,16 +170,16 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} Incorporating Eqs.~\eqref{eq:rbddot} -~\eqref{eq:rsddot2} into Eq.~\eqref{eq:Hbdot} results in \begin{multline} - \dot{\bm{H}}_{\text{sc},B} = [I_{\text{hub},B_c}] \dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{hub},B_c}] \bm\omega_{\cal B/N} + m_{\text{hub}} \bm{r}_{B_c/B}\times ( \dot{\bm\omega}_{\cal B/N}\times \bm{r}_{B_c/B}) \\+ m_{\text{hub}} \bm{r}_{B_c/B}\times\Big[\bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{B_c/B})\Big] +\sum\limits_{i=1}^{N_S} \biggl( [I'_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} + [I_{\text{sp}_{i1},S_{c,i1}}] \dot{\bm\omega}_{\cal B/N} \\+ \bm\omega_{\cal B/N} \times [I_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} - + \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}+ \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} + \dot{\bm{H}}_{\text{sc},B} = [I_{\text{hub},B_c}] \dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{hub},B_c}] \bm\omega_{\cal B/N} + m_{\text{hub}} \bm{r}_{B_c/B}\times ( \dot{\bm\omega}_{\cal B/N}\times \bm{r}_{B_c/B}) \\+ m_{\text{hub}} \bm{r}_{B_c/B}\times\Big[\bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{B_c/B})\Big] +\sum\limits_{i=1}^{N_S} \biggl( [I'_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} + [I_{\text{sp}_{i1},S_{c,i1}}] \dot{\bm\omega}_{\cal B/N} \\+ \bm\omega_{\cal B/N} \times [I_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} + + \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}+ \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \bm{r}''_{S_{c,i1}/B}\\ + 2 m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \Big( \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big) +m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \Big( \dot{\bm\omega}_{\cal B/N} \times \bm{r}_{S_{c,i1}/B}\Big) \\+m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \Big[ \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_{c,i1}/B})\Big] - + [I'_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} + [I_{\text{sp}_{i2},S_{c,i2}}] \dot{\bm\omega}_{\cal B/N} \\+ \bm\omega_{\cal B/N} \times [I_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} + + [I'_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} + [I_{\text{sp}_{i2},S_{c,i2}}] \dot{\bm\omega}_{\cal B/N} \\+ \bm\omega_{\cal B/N} \times [I_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} + \big(\ddot{\theta}_{i1} + \ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2}+ \bm\omega_{\cal B/N} \times \big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} \\ +m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \bm{r}''_{S_{c,i2}/B} + 2 m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \Big(\bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B}\Big)\\ +m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \Big(\dot{\bm\omega}_{\cal B/N} \times \bm{r}_{S_{c,i2}/B}\Big) +m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \Big[\bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_{c,i2}/B})\Big]\biggr) \label{eq:Hbdot3} -\end{multline} +\end{multline} Applying the parallel axis theorem the following inertia tensor terms are defined as \begin{align} @@ -216,7 +216,7 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} \label{eq:iprime3} \end{equation} -Applying the same methodology for $[I'_{\text{sp}_{i2},S_{c,i2}}]$ and using the following definition: $\hat{\bm s}'_{i2,j} = \bm\omega_{\cal{S}_{\textit{i2}}/\cal{B}}\times\hat{\bm s}_{i2,j}=\big(\dot{\theta}_{i1} + \dot{\theta}_{i2} \big)\hat{\bm s}_{i2,2}\times\hat{\bm s}_{i2,j}$ results in +Applying the same methodology for $[I'_{\text{sp}_{i2},S_{c,i2}}]$ and using the following definition: $\hat{\bm s}'_{i2,j} = \bm\omega_{\cal{S}_{\textit{i2}}/\cal{B}}\times\hat{\bm s}_{i2,j}=\big(\dot{\theta}_{i1} + \dot{\theta}_{i2} \big)\hat{\bm s}_{i2,2}\times\hat{\bm s}_{i2,j}$ results in \begin{equation} [I'_{\text{sp}_{i2},S_{c,i2}}] = \big(\dot{\theta}_{i1}+\dot{\theta}_{i2}\big)(I_{s_{i2,3}}-I_{s_{i2,1}})(\hat{\bm s}_{i2,1}\hat{\bm s}_{i2,3}^{T}+\hat{\bm s}_{i2,3}\hat{\bm s}_{i2,1}^{T}) \label{eq:iprime4} @@ -224,13 +224,13 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} Substituting Eq.~\eqref{eq:iprime3} and Eq.~\eqref{eq:iprime4} into Eq.~\eqref{eq:Hbdot3} and using Eq.~\eqref{eq:IscB} to simplify results in Eq.~\eqref{eq:Hbdot4}. The Jacobi Identity, $(\bm a \times \bm b)\times \bm c = \bm a \times (\bm b\times \bm c) - \bm b \times (\bm a\times \bm c)$, is used to combine terms.\\ Factoring out $\dot{\bm\omega}_{\cal B/N}$ and, selectively, $\bm{\omega}_{\cal B/N}$ and utilizing the tilde matrix transforms Eq. \ref{eq:Hbdot3} into Eq. \ref{eq:Hbdot17} so that $[I_{\text{sc},B}]$ can be extracted. \begin{multline} - \dot{\bm{H}}_{\text{sc},B} = \bigg([I_{\text{hub},B_c}] - m_{\text{hub}} [\tilde{\bm{r}}_{B_c/B}] [\tilde{\bm{r}}_{B_c/B}] + + \dot{\bm{H}}_{\text{sc},B} = \bigg([I_{\text{hub},B_c}] - m_{\text{hub}} [\tilde{\bm{r}}_{B_c/B}] [\tilde{\bm{r}}_{B_c/B}] + \sum\limits_{i=1}^{N_S} \Big([I_{\text{sp}_{i1},S_{c,i1}}]+ [I_{\text{sp}_{i2},S_{c,i2}}] - m_{\text{sp}_{i1}} [\tilde{\bm{r}}_{S_{c,i1}/B}] [\tilde{\bm{r}}_{S_{c,i1}/B}] \\ - m_{\text{sp}_{i2}} [\tilde{\bm{r}}_{S_{c,i2}/B}] [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big)\bigg)\dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times \bigg([I_{\text{hub},B_c}] - m_{\text{hub}} [\tilde{\bm{r}}_{B_c/B}] [\tilde{\bm{r}}_{B_c/B}] + \\ - \sum\limits_{i=1}^{N_S}\Big( [I_{\text{sp}_{i1},S_{c,i1}}]+ [I_{\text{sp}_{i2},S_{c,i2}}] - m_{\text{sp}_{i1}} [\tilde{\bm{r}}_{S_{c,i1}/B}] [\tilde{\bm{r}}_{S_{c,i1}/B}] - m_{\text{sp}_{i2}} [\tilde{\bm{r}}_{S_{c,i2}/B}] [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big) \bigg) \bm\omega_{\cal B/N} \\ - +\sum\limits_{i=1}^{N_S} \biggl( [I'_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} + \sum\limits_{i=1}^{N_S}\Big( [I_{\text{sp}_{i1},S_{c,i1}}]+ [I_{\text{sp}_{i2},S_{c,i2}}] - m_{\text{sp}_{i1}} [\tilde{\bm{r}}_{S_{c,i1}/B}] [\tilde{\bm{r}}_{S_{c,i1}/B}] - m_{\text{sp}_{i2}} [\tilde{\bm{r}}_{S_{c,i2}/B}] [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big) \bigg) \bm\omega_{\cal B/N} \\ + +\sum\limits_{i=1}^{N_S} \biggl( [I'_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} + \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}+ \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \bm{r}''_{S_{c,i1}/B} \\ - + 2 m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \Big( \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big) + [I'_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} + + 2 m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \Big( \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big) + [I'_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} + \big(\ddot{\theta}_{i1} + \ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2}+ \\ \bm\omega_{\cal B/N} \times \big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \bm{r}''_{S_{c,i2}/B} + 2 m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \Big(\bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B}\Big)\biggr) @@ -238,10 +238,10 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} \end{multline} $[I_{\text{sc},B}]$ is substituted in from Eq. \ref{eq:IHubB} through Eq. \ref{eq:IscB}: \begin{multline} - \dot{\bm{H}}_{\text{sc},B} = [I_{\text{sc},B}]\dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sc},B}] \bm\omega_{\cal B/N} \\ - +\sum\limits_{i=1}^{N_S} \biggl( [I'_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} + \dot{\bm{H}}_{\text{sc},B} = [I_{\text{sc},B}]\dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sc},B}] \bm\omega_{\cal B/N} \\ + +\sum\limits_{i=1}^{N_S} \biggl( [I'_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} + \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}+ \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \bm{r}''_{S_{c,i1}/B} \\ - + 2 m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \Big( \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big) + [I'_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} + + 2 m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \Big( \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big) + [I'_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} + \big(\ddot{\theta}_{i1} + \ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2}+ \\ \bm\omega_{\cal B/N} \times \big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \bm{r}''_{S_{c,i2}/B} + 2 m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \Big(\bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B}\Big)\biggr) @@ -249,35 +249,35 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} \end{multline} Splitting the doubled terms: \begin{multline} - \dot{\bm{H}}_{\text{sc},B} = [I_{\text{sc},B}]\dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sc},B}] \bm\omega_{\cal B/N} \\ + \dot{\bm{H}}_{\text{sc},B} = [I_{\text{sc},B}]\dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sc},B}] \bm\omega_{\cal B/N} \\ +\sum\limits_{i=1}^{N_S} \biggl( [I'_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} + m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \Big( \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big) + \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}+ \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} \\ +m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \bm{r}''_{S_{c,i1}/B} + m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \Big( \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big) + [I'_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} \\ + m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \Big(\bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B}\Big) - + \big(\ddot{\theta}_{i1} + \ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} + + + \big(\ddot{\theta}_{i1} + \ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} + \bm\omega_{\cal B/N} \times \big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2}\\ +m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \bm{r}''_{S_{c,i2}/B} + m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \Big(\bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B}\Big)\biggr) \label{eq:Hbdot19} \end{multline} Using the Jacobi Identity again, followed by tilde matrix substitution: \begin{multline} - \dot{\bm{H}}_{\text{sc},B} = [I_{\text{sc},B}]\dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sc},B}] \bm\omega_{\cal B/N} \\ - +\sum\limits_{i=1}^{N_S} \biggl( [I'_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} - m_{\text{sp}_{i1}}\Big([\tilde{\bm{r}}_{S_{c,i1}/B}][\tilde{\bm{r'}}_{S_{c,i1}/B}] + [\tilde{\bm{r'}}_{S_{c,i1}/B}][\tilde{\bm{r}}_{S_{c,i1}/B}] \Big)\bm\omega_{\cal B/N} + \dot{\bm{H}}_{\text{sc},B} = [I_{\text{sc},B}]\dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sc},B}] \bm\omega_{\cal B/N} \\ + +\sum\limits_{i=1}^{N_S} \biggl( [I'_{\text{sp}_{i1},S_{c,i1}}] \bm\omega_{\cal B/N} - m_{\text{sp}_{i1}}\Big([\tilde{\bm{r}}_{S_{c,i1}/B}][\tilde{\bm{r'}}_{S_{c,i1}/B}] + [\tilde{\bm{r'}}_{S_{c,i1}/B}][\tilde{\bm{r}}_{S_{c,i1}/B}] \Big)\bm\omega_{\cal B/N} + \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}\\ - + \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \bm{r}''_{S_{c,i1}/B} + + \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \bm{r}''_{S_{c,i1}/B} + m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \Big( \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big) \\ - + [I'_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} - m_{\text{sp}_{i2}}\Big([\tilde{\bm{r}}_{S_{c,i2}/B}] [\tilde{\bm{r'}}_{S_{c,i2}/B}] + [\tilde{\bm{r'}}_{S_{c,i2}/B}] [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big) \bm\omega_{\cal B/N} + + [I'_{\text{sp}_{i2},S_{c,i2}}] \bm\omega_{\cal B/N} - m_{\text{sp}_{i2}}\Big([\tilde{\bm{r}}_{S_{c,i2}/B}] [\tilde{\bm{r'}}_{S_{c,i2}/B}] + [\tilde{\bm{r'}}_{S_{c,i2}/B}] [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big) \bm\omega_{\cal B/N} + \big(\ddot{\theta}_{i1} + \ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} \\ + \bm\omega_{\cal B/N} \times \big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \bm{r}''_{S_{c,i2}/B} + m_{\text{sp}_{i2}} \bm{r}_{S_{c,i2}/B} \times \Big(\bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B}\Big)\biggr) \label{eq:Hbdot20} \end{multline} -Factoring out $\bm\omega_{\cal B/N}$, and substituting in from Eq. \ref{eq:IprimeScB} leaves: +Factoring out $\bm\omega_{\cal B/N}$, and substituting in from Eq. \ref{eq:IprimeScB} leaves: \begin{multline} \dot{\bm{H}}_{\text{sc},B} = [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sc},B}] \bm\omega_{\cal B/N} + [I'_{\text{sc},B}] \bm\omega_{\cal B/N} + \sum\limits_{i=1}^{N_S} \bigg[ \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}\\ - + \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} + + \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \bm{r}''_{S_{c,i1}/B} +m_{\text{sp}_{i1}} \bm\omega_{\cal B/N} \times \left(\bm{r}_{S_{c,i1}/B} \times \bm{r}'_{S_{c,i1}/B}\right)\\ +\big(\ddot{\theta}_{i1}+\ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} @@ -288,9 +288,9 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} \end{multline} Eqs. (\ref{eq:Euler}) and (\ref{eq:Hbdot4}) are equated and yield \begin{multline} - \bm{L}_B+m_{\text{sc}}\ddot{\bm r}_{B/N}\times\bm{c} = [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sc},B}] \bm\omega_{\cal B/N} + [I'_{\text{sc},B}] \bm\omega_{\cal B/N} + \bm{L}_B+m_{\text{sc}}\ddot{\bm r}_{B/N}\times\bm{c} = [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \bm\omega_{\cal B/N} \times [I_{\text{sc},B}] \bm\omega_{\cal B/N} + [I'_{\text{sc},B}] \bm\omega_{\cal B/N} + \sum\limits_{i=1}^{N_S} \bigg[ \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}\\ - + \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} + + \bm\omega_{\cal B/N} \times \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} \bm{r}_{S_{c,i1}/B} \times \bm{r}''_{S_{c,i1}/B} +m_{\text{sp}_{i1}} \bm\omega_{\cal B/N} \times \left(\bm{r}_{S_{c,i1}/B} \times \bm{r}'_{S_{c,i1}/B}\right)\\ +\big(\ddot{\theta}_{i1}+\ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} @@ -302,7 +302,7 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} Finally, using tilde matrix and simplifying yields the modified Euler equation, which is the second EOM necessary to describe the motion of the spacecraft. \begin{multline} [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} - \sum\limits_{i=1}^{N_S} \bigg[ \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}\\ - + [\bm{\tilde{\omega}}_{\cal B/N}] \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} + + [\bm{\tilde{\omega}}_{\cal B/N}] \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{r}''_{S_{c,i1}/B} +m_{\text{sp}_{i1}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B}\\ +\big(\ddot{\theta}_{i1}+\ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} @@ -312,32 +312,32 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} + \bm{L}_B - m_{\text{sc}} [\tilde{\bm{c}}] \ddot{\bm r}_{B/N} \label{eq:Final5} \end{multline} -However, it is desirable to place the second order state variables on the left hand side of the equation. Performing some rearranging of Eq.~\eqref{eq:Final5} results in an intermediate step. +However, it is desirable to place the second order state variables on the left hand side of the equation. Performing some rearranging of Eq.~\eqref{eq:Final5} results in an intermediate step. \begin{multline} - m_{\text{sc}} [\tilde{\bm{c}}] \ddot{\bm r}_{B/N} + [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \sum\limits_{i=1}^{N_S} \bigg[ \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} + m_{\text{sc}} [\tilde{\bm{c}}] \ddot{\bm r}_{B/N} + [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \sum\limits_{i=1}^{N_S} \bigg[ \ddot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{r}''_{S_{c,i1}/B}\\ - +\big(\ddot{\theta}_{i1}+\ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} + +\big(\ddot{\theta}_{i1}+\ddot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{r}''_{S_{c,i2}/B}\bigg] = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} \\ - \sum\limits_{i=1}^{N_S} \bigg[ - [\bm{\tilde{\omega}}_{\cal B/N}] \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} + [\bm{\tilde{\omega}}_{\cal B/N}] \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B}\\ + [\bm{\tilde{\omega}}_{\cal B/N}] \big(\dot{\theta}_{i1}+\dot{\theta}_{i2}\big) I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{r}'_{S_{c,i2}/B}\bigg] - + \bm{L}_B + + \bm{L}_B \label{eq:Finalint} \end{multline} Then, the second order terms are factored out: \begin{multline} m_{\text{sc}} [\tilde{\bm{c}}] \ddot{\bm r}_{B/N} + [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \sum\limits_{i=1}^{N_S} \bigg[\Big( - I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} + I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} d_{i1} [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,3} + I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} \\ +m_{\text{sp}_{i2}} l_{i1} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}} d_{i2} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3}\Big) \ddot{\theta}_{i1} - +\Big(I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} + +\Big(I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}} [\tilde{\bm{r}}_{S_{c,i2}/B}] d_{i2}\bm{\hat{s}}_{i2,3}\Big) \ddot{\theta}_{i2} \bigg]\\ - = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} + = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} - \sum\limits_{i=1}^{N_S} \bigg[ - [\bm{\tilde{\omega}}_{\cal B/N}] \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} + [\bm{\tilde{\omega}}_{\cal B/N}] \dot{\theta}_{i1} I_{s_{i1,2}}\bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} d_{i1} \dot{\theta}_{i1}^2 [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,1} \\+m_{\text{sp}_{i2}} l_{i1} \dot{\theta}_{i1}^2 [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i1,1} +m_{\text{sp}_{i1}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B} @@ -354,30 +354,30 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} m_{\text{sc}} [\tilde{\bm{c}}] \ddot{\bm r}_{B/N} + [I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \sum\limits_{i=1}^{N_S} \bigg[ \big(I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}+m_{\text{sp}_{i1}}d_{i1} [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,3} + I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}}l_{i1} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i1,3}\\ +m_{\text{sp}_{i2}}d_{i2} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3}\big) \ddot{\theta}_{i1} - +\big( I_{s_{i2,2}}\bm{\hat{s}}_{i2,2}+m_{\text{sp}_{i2}} d_{i2} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3}\big)\ddot{\theta}_{i2}\bigg] + +\big( I_{s_{i2,2}}\bm{\hat{s}}_{i2,2}+m_{\text{sp}_{i2}} d_{i2} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3}\big)\ddot{\theta}_{i2}\bigg] \\ - = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} + = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} - \sum\limits_{i=1}^{N_S} \bigg[ - \dot{\theta}_{i1} I_{s_{i1,2}} [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i1,2} + \dot{\theta}_{i1} I_{s_{i1,2}} [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B} \\ +m_{\text{sp}_{i1}}d_{i1}\dot{\theta}_{i1}^2 [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,1} + \big(\dot{\theta}_{i1}+\dot{\theta}_{i2}\big) I_{s_{i2,2}}[\bm{\tilde{\omega}}_{\cal B/N}]\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{r}'_{S_{c,i2}/B} \\ +m_{\text{sp}_{i2}} [\tilde{\bm{r}}_{S_{c,i2}/B}] \big(l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\big)\bigg] - + \bm{L}_B + + \bm{L}_B \label{eq:Final6} \end{multline} \subsubsection{Dual Linked Solar Panel Motion} -The following section follows the same derivation seen in previous work\cite{Allard2016rz} and is summarized here for convenience. +The following section follows the same derivation seen in previous work\cite{Allard2016rz} and is summarized here for convenience. Let $\bm L_{H_{i1}} = L_{i1,1} \hat{\bm s}_{i1,1} + L_{i1,2} \hat{\bm s}_{i1,2} + L_{i1,3} \hat{\bm s}_{i1,3}$ be the total torque acting on the first solar panel at point $H_{i1}$. The corresponding hinge torque is given through \begin{equation} L_{i1,2} = - k_{i1} \theta_{i1} - c_{i1}\dot{\theta}_{i1} + k_{i2} \theta_{i2} + c_{i2} \dot\theta_{i2} + \hat{\bm s}_{i1,2} \cdot \bm \tau_{\text{ext}_{i1},H_{i1}} + \hat{\bm s}_{i1,2} \cdot \bm{r}_{H_{i2}/H_{i1}} \times \bm F_{1/2i} \label{eq:hingeTorque1} \end{equation} -Where $\bm F_{1/2i}$ is the reaction of solar panel 2 acting on solar panel 1. It is important to point out that $\bm F_{1/2i} = - \bm F_{2/1i}$. +Where $\bm F_{1/2i}$ is the reaction of solar panel 2 acting on solar panel 1. It is important to point out that $\bm F_{1/2i} = - \bm F_{2/1i}$. To define the $\bm F_{1/2i}$, $\bm F_{2/1i}$ needs to be defined. This is done performing the super particle theorem on the second solar panel: \begin{equation} @@ -385,7 +385,7 @@ \subsubsection{Dual Linked Solar Panel Motion} \end{equation} The sum of the external forces on solar panel 2, $\bm F_{\text{ext}_{i2}}$, is separate because it does not contribute to the reaction force at the joint. With this definition $\bm F_{1/2i}$ is defined as \begin{equation} - \bm F_{1/2i} = \bm F_{\text{ext}_{i2}} - m_{sp_{i2}} \ddot{\bm{r}}_{S_{c,i2}/N} + \bm F_{1/2i} = \bm F_{\text{ext}_{i2}} - m_{sp_{i2}} \ddot{\bm{r}}_{S_{c,i2}/N} \end{equation} Plugging this definition into Eq.~\eqref{eq:hingeTorque1} yields \begin{equation} @@ -401,7 +401,7 @@ \subsubsection{Dual Linked Solar Panel Motion} \begin{equation} \bm\omega_{\mathcal{S}_{i1}/\mathcal{N}} = \bm\omega_{\mathcal{S}_{i1}/\mathcal{H}_{i1}} + \bm\omega_{\mathcal{H}_{i1}/\mathcal{B}} + \bm\omega_{\cal B/N} \end{equation} -where $\bm\omega_{\mathcal{S}_{i1}/\mathcal{H}_{i1}} = \dot\theta_{i1} \hat{\bm s}_{i1,2}$. +where $\bm\omega_{\mathcal{S}_{i1}/\mathcal{H}_{i1}} = \dot\theta_{i1} \hat{\bm s}_{i1,2}$. Because the hinge frame $\mathcal{H}_{i1}$ is fixed relative to the body frame $\mathcal{B}$ the relative angular velocity vector is $\bm\omega_{\mathcal{H}_{i1}/\mathcal{B}} = \bm 0$. The body angular velocity vector is written in $\mathcal{S}_{i1}$-frame components as \begin{align} \bm\omega_{\cal B/N} &= ( \hat{\bm s}_{i1,1} \cdot \bm\omega_{\cal B/N}) \hat{\bm s}_{i1,1} @@ -417,7 +417,7 @@ \subsubsection{Dual Linked Solar Panel Motion} As $\hat{\bm s}_{i1,2}$ is a body-fixed vector, note that \begin{equation} \dot\omega_{s_{i1,2}} = \frac{\leftexp{B}\D}{\D t} \left( \bm\omega_{\cal B/N} \cdot \hat{\bm s}_{i1,2} \right) - = \frac{\leftexp{B}\D}{\D t} \left( \bm\omega_{\cal B/N}\right) \cdot \hat{\bm s}_{i1,2} = + = \frac{\leftexp{B}\D}{\D t} \left( \bm\omega_{\cal B/N}\right) \cdot \hat{\bm s}_{i1,2} = \dot{\bm\omega}_{\cal B/N} \cdot \hat{\bm s}_{i1,2} \end{equation} @@ -429,7 +429,7 @@ \subsubsection{Dual Linked Solar Panel Motion} \\ I_{s_{i1,3}} \dot\omega_{s_{i1,3}} &= - (I_{s_{i1,2}} - I_{s_{i1,1}}) \omega_{s_{i1,1}}(\omega_{s_{i1,2}}+ \dot\theta_{i1}) + L_{s_{i1,3}} \end{align} -where $\bm L_{S_{c,i1}} = L_{s_{i1,1}} \hat{\bm s}_{i1,1} + L_{s_{i1,2}} \hat{\bm s}_{i1,2} + L_{s_{i1,3}} \hat{\bm s}_{i1,3}$ is the net torque acting on the solar panel about its center of mass. The second differential equation is used to get the equations of motion of $\theta_{i1}$. The first and third equation could be used to back-solve for the structural hinge torques embedded in $L_{s_{i1,1}}$ and $L_{s_{i1,3}}$ if needed. +where $\bm L_{S_{c,i1}} = L_{s_{i1,1}} \hat{\bm s}_{i1,1} + L_{s_{i1,2}} \hat{\bm s}_{i1,2} + L_{s_{i1,3}} \hat{\bm s}_{i1,3}$ is the net torque acting on the solar panel about its center of mass. The second differential equation is used to get the equations of motion of $\theta_{i1}$. The first and third equation could be used to back-solve for the structural hinge torques embedded in $L_{s_{i1,1}}$ and $L_{s_{i1,3}}$ if needed. Let $\bm F_{S_{c,i1}}$ be the net force acting on the first solar panel. Using the superparticle theorem\cite{schaub} yields \begin{equation} @@ -437,7 +437,7 @@ \subsubsection{Dual Linked Solar Panel Motion} \end{equation} The torque about the solar panel center of mass can be related to the torque about the hinge point $H_{i1}$ using \begin{equation} - \bm L_{H_i1} = \bm L_{S_{c,i1}} + \bm r_{S_{c,i1}/H_{i1}} \times \bm F_{S_{c,i1}} + \bm L_{H_i1} = \bm L_{S_{c,i1}} + \bm r_{S_{c,i1}/H_{i1}} \times \bm F_{S_{c,i1}} \end{equation} Solving for the torque about $S_{c,i}$ yields \begin{equation} @@ -466,7 +466,7 @@ \subsubsection{Dual Linked Solar Panel Motion} \end{multline} The following definitions need to be defined: \begin{multline} - \ddot{\bm r}_{S_{c,i1}/N} = \ddot{\bm{r}}_{B/N} + \ddot{\bm r}_{S_{c,i1}/B} \\ + \ddot{\bm r}_{S_{c,i1}/N} = \ddot{\bm{r}}_{B/N} + \ddot{\bm r}_{S_{c,i1}/B} \\ = \ddot{\bm{r}}_{B/N} + \bm{r}''_{S_{c,i1}/B} + 2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B} + \dot{\bm\omega}_{\cal B/N} \times \bm{r}_{S_{c,i1}/B} + \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_{c,i1}/B}) \end{multline} \begin{multline} @@ -478,7 +478,7 @@ \subsubsection{Dual Linked Solar Panel Motion} \begin{multline} - L_{s_{i1,2}} = - k_{i1} \theta_{i1} - c_{i1}\dot{\theta}_{i1} + k_{i2} \theta_{i2} + c_{i2} \dot\theta_{i2} + \hat{\bm s}_{i1,2} \cdot \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \hat{\bm s}_{i1,3} \cdot \bm F_{\text{ext}_{i2}} + L_{s_{i1,2}} = - k_{i1} \theta_{i1} - c_{i1}\dot{\theta}_{i1} + k_{i2} \theta_{i2} + c_{i2} \dot\theta_{i2} + \hat{\bm s}_{i1,2} \cdot \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \hat{\bm s}_{i1,3} \cdot \bm F_{\text{ext}_{i2}} - m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3} \cdot \Big[\ddot{\bm{r}}_{B/N} \\ + \bm{r}''_{S_{c,i1}/B} + 2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B} + \dot{\bm\omega}_{\cal B/N} \times \bm{r}_{S_{c,i1}/B} + \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_{c,i1}/B})\Big]\\ @@ -507,28 +507,28 @@ \subsubsection{Dual Linked Solar Panel Motion} \begin{multline} \Big[m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3}^T + m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T \Big] \ddot{\bm{r}}_{B/N} + \Big[I_{s_{i1,2}} \hat{\bm s}_{i1,2}^T - m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3}^T [\tilde{\bm{r}}_{S_{c,i1}/B}] - m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big]\dot{\bm\omega}_{\cal B/N} \\ + I_{s_{i1,2}} \ddot\theta_{i1} + m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3}^T \bm{r}''_{S_{c,i1}/B} - + m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T \bm{r}''_{S_{c,i2}/B} + + m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T \bm{r}''_{S_{c,i2}/B} = - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}} - k_{i1} \theta_{i1} - c_{i1}\dot{\theta}_{i1} \\ - + k_{i2} \theta_{i2} + c_{i2} \dot\theta_{i2} + + k_{i2} \theta_{i2} + c_{i2} \dot\theta_{i2} + \hat{\bm s}_{i1,2} \cdot \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \hat{\bm s}_{i1,3} \cdot \bm F_{\text{ext}_{i2}} - m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3} \cdot \Big[2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B} \\ + \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_{c,i1}/B})\Big] - - m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3} \cdot \Big[ 2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B} + - m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3} \cdot \Big[ 2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B} + \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_{c,i2}/B})\Big] \end{multline} -Expanding the $\bm{r}''_{S_{c,i1}/B}$ and $\bm{r}''_{S_{c,i2}/B}$ terms, replacing cross products with the tilde matrix and again isolating the second order variables results in: +Expanding the $\bm{r}''_{S_{c,i1}/B}$ and $\bm{r}''_{S_{c,i2}/B}$ terms, replacing cross products with the tilde matrix and again isolating the second order variables results in: \begin{multline} \Big[m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3}^T + m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T \Big] \ddot{\bm{r}}_{B/N} + \Big[I_{s_{i1,2}} \hat{\bm s}_{i1,2}^T - m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3}^T [\tilde{\bm{r}}_{S_{c,i1}/B}] - m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big]\dot{\bm\omega}_{\cal B/N} \\ + \Big[I_{s_{i1,2}}+ m_{\text{sp}_{i1}} d_{i1}^2 + m_{sp_{i2}} l_{i1}^2 + m_{sp_{i2}} l_{i1} d_{i2} \hat{\bm s}_{i1,3}^T \bm{\hat{s}}_{i2,3} \Big] \ddot\theta_{i1} + \Big[m_{sp_{i2}} l_{i1} d_{i2} \hat{\bm s}_{i1,3}^T \bm{\hat{s}}_{i2,3} \Big] \ddot{\theta}_{i2}\\ - = - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}} - k_{i1} \theta_{i1} - c_{i1}\dot{\theta}_{i1} - + k_{i2} \theta_{i2} + c_{i2} \dot\theta_{i2} - + \hat{\bm s}_{i1,2}^T \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \hat{\bm s}_{i1,3}^T \bm F_{\text{ext}_{i2}}\\ + = - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}} - k_{i1} \theta_{i1} - c_{i1}\dot{\theta}_{i1} + + k_{i2} \theta_{i2} + c_{i2} \dot\theta_{i2} + + \hat{\bm s}_{i1,2}^T \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \hat{\bm s}_{i1,3}^T \bm F_{\text{ext}_{i2}}\\ - m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3}^T \Big[2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i1}/B} + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i1}/B}\Big] \\ - - m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + - m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i2}/B} + l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big] \label{eq:solar_panel_final10} \end{multline} @@ -540,7 +540,7 @@ \subsubsection{Dual Linked Solar Panel Motion} \end{equation} The relationship between the torque about the center of mass of the solar panel and about the hinge point is defined as: \begin{equation} - \bm L_{H_i2} = \bm L_{S_{c,i2}} + \bm r_{S_{c,i2}/H_{i2}} \times \bm F_{S_{c,i2}} + \bm L_{H_i2} = \bm L_{S_{c,i2}} + \bm r_{S_{c,i2}/H_{i2}} \times \bm F_{S_{c,i2}} \end{equation} The torque about $\hat{\bm s}_{i2,2}$ is the only torque that is required: \begin{equation} @@ -583,20 +583,20 @@ \subsubsection{Dual Linked Solar Panel Motion} \begin{multline} \Big[m_{\text{sp}_{i2}} d_{i2} \hat{\bm s}_{i2,3}^T\Big] \ddot{\bm{r}}_{B/N} + \Big[I_{s_{i2,2}} \hat{\bm s}_{i2,2}^T - m_{\text{sp}_{i2}} d_{i2} \hat{\bm s}_{i2,3}^T [\tilde{\bm{r}}_{S_{c,i2}/B}]\Big] \dot{\bm\omega}_{\cal B/N} + \Big[I_{s_{i2,2}} + m_{\text{sp}_{i2}} d_{i2}^2 + m_{\text{sp}_{i2}} l_{i1} d_{i2} \hat{\bm s}_{i2,3}^T \bm{\hat{s}}_{i1,3} \Big] \ddot\theta_{i1} \\ - + \Big[I_{s_{i2,2}} + m_{\text{sp}_{i2}} d_{i2}^2 \Big] \ddot\theta_{i2} + + \Big[I_{s_{i2,2}} + m_{\text{sp}_{i2}} d_{i2}^2 \Big] \ddot\theta_{i2} = - (I_{s_{i2,1}} - I_{s_{i2,3}}) \omega_{s_{i2,3}} \omega_{s_{i2,1}} - k_{i2} \theta_{i2} - c_{i2} \dot{\theta}_{i2} + \hat{\bm s}_{i2,2}^T \bm \tau_{\text{ext}_{i2},H_{i2}} \\ - m_{\text{sp}_{i2}} d_{i2} \hat{\bm s}_{i2,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i2}/B} + l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} \Big] \label{eq:sp2final} \end{multline} -Eq.~\eqref{eq:sp2final} is the last EOM needed to describe the motion of the spacecraft. The next section develops the back substitution method for interconnected panels and gives meaningful insight on how effectors connected to other effectors dynamically couple to the spacecraft. +Eq.~\eqref{eq:sp2final} is the last EOM needed to describe the motion of the spacecraft. The next section develops the back substitution method for interconnected panels and gives meaningful insight on how effectors connected to other effectors dynamically couple to the spacecraft. \subsection{Derivation of Equations of Motion - Kane's Method} The choice of state variables and their respective chosen generalized speeds are: \begin{equation} - \bm X = + \bm X = \begin{bmatrix} \bm r_{B/N}\\ \bm \sigma_{\cal{B/N}}\\ @@ -616,7 +616,7 @@ \subsection{Derivation of Equations of Motion - Kane's Method} \dot{\theta}_{N_S,1}\\ \dot{\theta}_{N_S,2} \end{bmatrix} -\end{equation} +\end{equation} The necessary velocities needed to be defined are as follows \begin{equation} @@ -654,7 +654,7 @@ \subsection{Derivation of Equations of Motion - Kane's Method} \caption{Partial Velocity Table} \label{tab:hub} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c | c | c | c } % Column formatting, \hline $r$ & $\bm v^{B_c}_{r}$ & $\bm \omega_{\textit{r}}^{\cal{B}}$ & $\bm v^{S_{c,i1}}_{r}$ & $\bm \omega_{r}^{\mathcal{S}_{i1}}$ & $\bm v^{S_{c,i2}}_{r}$ & $\bm \omega_{r}^{\mathcal{S}_{i2}}$ \\ \hline @@ -724,15 +724,15 @@ \subsubsection{Rigid Spacecraft Hub Translational Motion} \end{multline} Combining like terms results in: \begin{multline} - m_{\text{sc}} \ddot{\bm r}_{B/N} -m_{\textnormal{sc}} [\tilde{\bm{c}}]\dot{\bm\omega}_{\cal B/N} + m_{\text{sc}} \ddot{\bm r}_{B/N} -m_{\textnormal{sc}} [\tilde{\bm{c}}]\dot{\bm\omega}_{\cal B/N} + \sum\limits_{i}^{N_S} \bigg( m_{\text{sp}_{i1}} \Big[d_{i1} \bm{\hat{s}}_{i1,3} \ddot{\theta}_{i1} + d_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1}\Big] \\ + m_{\text{sp}_{i2}} \Big[(l_{i1} \bm{\hat{s}}_{i1,3} + d_{i2} \bm{\hat{s}}_{i2,3}) \ddot{\theta}_{i1} + d_{i2} \bm{\hat{s}}_{i2,3} \ddot{\theta}_{i2} + l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1} \Big]\bigg) \\ - = \bm F_{\text{ext}} - 2 m_{\textnormal{sc}}[\tilde{\bm\omega}_{\cal B/N}]\bm{c}' - m_{\textnormal{sc}} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} + = \bm F_{\text{ext}} - 2 m_{\textnormal{sc}}[\tilde{\bm\omega}_{\cal B/N}]\bm{c}' - m_{\textnormal{sc}} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} \end{multline} Rearranging and putting in final form: \begin{multline} - m_{\text{sc}} \ddot{\bm r}_{B/N} -m_{\textnormal{sc}} [\tilde{\bm{c}}]\dot{\bm\omega}_{\cal B/N} - + \sum\limits_{i}^{N_S} \bigg( \Big[m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3} + m_{\text{sp}_{i2}} l_{i1} \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3}\Big] \ddot{\theta}_{i1} + m_{\text{sc}} \ddot{\bm r}_{B/N} -m_{\textnormal{sc}} [\tilde{\bm{c}}]\dot{\bm\omega}_{\cal B/N} + + \sum\limits_{i}^{N_S} \bigg( \Big[m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3} + m_{\text{sp}_{i2}} l_{i1} \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3}\Big] \ddot{\theta}_{i1} + m_{\text{sp}_{i2}}d_{i2} \bm{\hat{s}}_{i2,3} \ddot{\theta}_{i2} \bigg) \\ = \bm F_{\text{ext}} - 2 m_{\textnormal{sc}}[\tilde{\bm\omega}_{\cal B/N}]\bm{c}' - m_{\textnormal{sc}} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} \\- \sum\limits_{i}^{N_S} \bigg( m_{\text{sp}_{i1}} d_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + m_{\text{sp}_{i2}} \Big[(l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1} \Big]\bigg) \end{multline} @@ -752,7 +752,7 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} \end{equation} \begin{multline} \bm F^*_{4-6} = [\bm \omega_{4-6}^{\cal{B}}]^T \bm T^*_{\text{hub}} + [\bm v^{B_c}_{4-6}]^T (-m_{\text{hub}} \ddot{\bm r}_{B_c/N}) + \sum\limits_{i}^{N_S}\bigg([\bm v^{S_{c,i1}}_{4-6}]^T (-m_{\text{sp}_{i1}} \ddot{\bm{r}}_{S_{c,{i1}}/N}) + [\bm \omega_{4-6}^{\mathcal{S}_{i1}}]^T \bm T^*_{\text{sp}_{i1}} \\ - + [\bm v^{S_{c,i1}}_{4-6}]^T (-m_{\text{sp}_{i1}} \ddot{\bm{r}}_{S_{c,{i1}}/N}) + [\bm \omega_{4-6}^{\mathcal{S}_{i1}}]^T \bm T^*_{\text{sp}_{i1}}\bigg) + + [\bm v^{S_{c,i1}}_{4-6}]^T (-m_{\text{sp}_{i1}} \ddot{\bm{r}}_{S_{c,{i1}}/N}) + [\bm \omega_{4-6}^{\mathcal{S}_{i1}}]^T \bm T^*_{\text{sp}_{i1}}\bigg) = -[I_{\text{hub},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{hub},B}] \bm\omega_{\cal B/N} - m_{\text{hub}} [\tilde{{\bm r}}_{B_c/B}] \ddot{\bm r}_{B_c/N} \\ + \sum\limits_{i}^{N_S}\bigg(- m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \ddot{\bm r}_{S_{c,i1}/N} -[I_{\text{sp}_{i1},S_{c,i1}}] \dot{\bm\omega}_{\mathcal{S}_{i1}/\mathcal{N}} -[\tilde{\bm \omega}_{\mathcal{S}_{i1}/\mathcal{N}}] [I_{\text{sp}_{i1},S_{c,i1}}] \bm \omega_{\mathcal{S}_{i1}/\mathcal{N}}\\ - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \ddot{\bm r}_{S_{c,i2}/N} -[I_{\text{sp}_{i2},S_{c,i2}}] \dot{\bm\omega}_{\mathcal{S}_{i2}/\mathcal{N}} -[\tilde{\bm \omega}_{\mathcal{S}_{i2}/\mathcal{N}}] [I_{\text{sp}_{i2},S_{c,i2}}] \bm \omega_{\mathcal{S}_{i2}/\mathcal{N}}\bigg) @@ -791,7 +791,7 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} Expand using those terms: \begin{multline} - \bm L_B -[I_{\text{hub},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{hub},B}] \bm\omega_{\cal B/N} - m_{\text{hub}} [\tilde{{\bm r}}_{B_c/B}] \ddot{\bm r}_{B_c/N} + \bm L_B -[I_{\text{hub},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{hub},B}] \bm\omega_{\cal B/N} - m_{\text{hub}} [\tilde{{\bm r}}_{B_c/B}] \ddot{\bm r}_{B_c/N} + \sum\limits_{i}^{N_S}\bigg(- m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \ddot{\bm r}_{S_{c,i1}/N} \\ -[I_{\text{sp}_{i1},S_{c,i1}}] \Big[\dot{\bm \omega}_{\cal{B/N}} + \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2}\Big] -[\tilde{\bm \omega}_{\mathcal{S}_{i1}/\mathcal{N}}] [I_{\text{sp}_{i1},S_{c,i1}}] \bm \omega_{\mathcal{S}_{i1}/\mathcal{N}}\\ - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \ddot{\bm r}_{S_{c,i2}/N} -[I_{\text{sp}_{i2},S_{c,i2}}] \Big[\dot{\bm \omega}_{\cal{B/N}} + (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2}\Big] \\ @@ -813,11 +813,11 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} \begin{multline} \bm L_B -[I_{\text{hub},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{hub},B}] \bm\omega_{\cal B/N} - m_{\text{hub}} [\tilde{{\bm r}}_{B_c/B}] \ddot{\bm r}_{B_c/N} + \sum\limits_{i}^{N_S}\bigg(- m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \ddot{\bm r}_{S_{c,i1}/N} \\ - -[I_{\text{sp}_{i1},S_{c,i1}}] \Big[\dot{\bm \omega}_{\cal{B/N}} + \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2}\Big] + -[I_{\text{sp}_{i1},S_{c,i1}}] \Big[\dot{\bm \omega}_{\cal{B/N}} + \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2}\Big] -[\tilde{\bm \omega}_{\cal{B/N}}] [I_{\text{sp}_{i1},S_{c,i1}}] \bm \omega_{\cal{B/N}} \\ -\bm \omega_{\cal{B/N}} \times [I_{\text{sp}_{i1},S_{c,i1}}] \dot{\theta}_{i1} \hat{\bm s}_{i1,2} -\dot{\theta}_{i1} \hat{\bm s}_{i1,2} \times [I_{\text{sp}_{i1},S_{c,i1}}] \Big[\bm \omega_{\cal{B/N}} + \dot{\theta}_{i1} \hat{\bm s}_{i1,2}\Big] - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \ddot{\bm r}_{S_{c,i2}/N} \\ - -[I_{\text{sp}_{i2},S_{c,i2}}] \Big[\dot{\bm \omega}_{\cal{B/N}} + (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2}\Big] + -[I_{\text{sp}_{i2},S_{c,i2}}] \Big[\dot{\bm \omega}_{\cal{B/N}} + (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2}\Big] -[\tilde{\bm \omega}_{\cal{B/N}}] [I_{\text{sp}_{i2},S_{c,i2}}] \bm \omega_{\cal{B/N}} \\ -\bm \omega_{\cal{B/N}} \times [I_{\text{sp}_{i2},S_{c,i2}}] (\dot{\theta}_{i1} + \dot{\theta}_{i2})\hat{\bm s}_{i2,2} -(\dot{\theta}_{i1} + \dot{\theta}_{i2})\hat{\bm s}_{i2,2} \times [I_{\text{sp}_{i2},S_{c,i2}}] \Big[\bm \omega_{\cal{B/N}} + (\dot{\theta}_{i1} + \dot{\theta}_{i2})\hat{\bm s}_{i2,2}\Big]\bigg) = 0 \end{multline} @@ -852,7 +852,7 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0 - \end{bmatrix}} {\vphantom{\bm \omega_{\cal{B/N}}}}^{\mathcal{S}_{i1}\!}{\bm \omega_{\cal{B/N}}} + \end{bmatrix}} {\vphantom{\bm \omega_{\cal{B/N}}}}^{\mathcal{S}_{i1}\!}{\bm \omega_{\cal{B/N}}} \end{equation} Which simplifies to: @@ -866,7 +866,7 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} 0 & 0 & I_{s_{i1,1}} \\ 0 & 0 & 0 \\ -I_{s_{i1,3}} & 0 & 0 - \end{bmatrix}} {\vphantom{\bm \omega_{\cal{B/N}}}}^{\mathcal{S}_{i1}\!}{\bm \omega_{\cal{B/N}}} + \end{bmatrix}} {\vphantom{\bm \omega_{\cal{B/N}}}}^{\mathcal{S}_{i1}\!}{\bm \omega_{\cal{B/N}}} \end{equation} Final simplification: @@ -900,7 +900,7 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} I_{s_{i1,1}} & 0 & 0 \\ 0 & I_{s_{i1,2}} & 0 \\ 0 & 0 & I_{s_{i1,3}} - \end{bmatrix}} {\vphantom{\bm \omega_{\cal{B/N}}}}^{\mathcal{S}_{i1}\!}{\bm \omega_{\cal{B/N}}} + \end{bmatrix}} {\vphantom{\bm \omega_{\cal{B/N}}}}^{\mathcal{S}_{i1}\!}{\bm \omega_{\cal{B/N}}} \end{equation} Which simplifies to: @@ -914,7 +914,7 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} 0 & 0 & I_{s_{i1,3}} \\ 0 & 0 & 0 \\ -I_{s_{i1,1}} & 0 & 0 - \end{bmatrix}} {\vphantom{\bm \omega_{\cal{B/N}}}}^{\mathcal{S}_{i1}\!}{\bm \omega_{\cal{B/N}}} + \end{bmatrix}} {\vphantom{\bm \omega_{\cal{B/N}}}}^{\mathcal{S}_{i1}\!}{\bm \omega_{\cal{B/N}}} \end{equation} Final simplification: @@ -946,9 +946,9 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} \bm L_B -[I_{\text{hub},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{hub},B}] \bm\omega_{\cal B/N} - m_{\text{hub}} [\tilde{{\bm r}}_{B_c/B}] \ddot{\bm r}_{B_c/N} \\ + \sum\limits_{i}^{N_S}\bigg(-[I_{\text{sp}_{i1},S_{c,i1}}] \dot{\bm \omega}_{\cal{B/N}} -[I_{\text{sp}_{i2},S_{c,i2}}] \dot{\bm \omega}_{\cal{B/N}} -[\tilde{\bm \omega}_{\cal{B/N}}] [I_{\text{sp}_{i1},S_{c,i1}}] \bm \omega_{\cal{B/N}}-[\tilde{\bm \omega}_{\cal{B/N}}] [I_{\text{sp}_{i2},S_{c,i2}}] \bm \omega_{\cal{B/N}}\\ -\dot{\theta}_{i1} (I_{s_{i1,3}} - I_{s_{i1,1}})( \hat{\bm s}_{i1,1} \hat{\bm s}_{i1,3}^T + \hat{\bm s}_{i1,3} \hat{\bm s}_{i1,1}^T) \bm \omega_{\cal{B/N}} -(\dot{\theta}_{i1} + \dot{\theta}_{i2})(I_{s_{i2,3}} - I_{s_{i2,1}}) (\hat{\bm s}_{i2,1} \hat{\bm s}_{i2,3}^T + \hat{\bm s}_{i2,3} \hat{\bm s}_{i2,1}^T) \bm \omega_{\cal{B/N}} \\ - - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \ddot{\bm r}_{S_{c,i1}/N} -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \ddot{\bm r}_{S_{c,i1}/N} -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} -I_{s_{i1,2}} \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2}\\ - - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \ddot{\bm r}_{S_{c,i2}/N} -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \ddot{\bm r}_{S_{c,i2}/N} -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} -I_{s_{i2,2}} (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2} \bigg) = 0 \end{multline} @@ -959,10 +959,10 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} + \sum\limits_{i}^{N_S}\bigg(-[I_{\text{sp}_{i1},S_{c,i1}}] \dot{\bm \omega}_{\cal{B/N}} -[I_{\text{sp}_{i2},S_{c,i2}}] \dot{\bm \omega}_{\cal{B/N}} -[\tilde{\bm \omega}_{\cal{B/N}}] [I_{\text{sp}_{i1},S_{c,i1}}] \bm \omega_{\cal{B/N}}-[\tilde{\bm \omega}_{\cal{B/N}}] [I_{\text{sp}_{i2},S_{c,i2}}] \bm \omega_{\cal{B/N}}\\ -\dot{\theta}_{i1} (I_{s_{i1,3}} - I_{s_{i1,1}})( \hat{\bm s}_{i1,1} \hat{\bm s}_{i1,3}^T + \hat{\bm s}_{i1,3} \hat{\bm s}_{i1,1}^T) \bm \omega_{\cal{B/N}} -(\dot{\theta}_{i1} + \dot{\theta}_{i2})(I_{s_{i2,3}} - I_{s_{i2,1}}) (\hat{\bm s}_{i2,1} \hat{\bm s}_{i2,3}^T + \hat{\bm s}_{i2,3} \hat{\bm s}_{i2,1}^T) \bm \omega_{\cal{B/N}} \\ - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \Big[\ddot{\bm r}_{B/N} + \ddot{\bm r}_{S_{c,i1}/B}\Big] - -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} -I_{s_{i1,2}} \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2}\\ - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \Big[\ddot{\bm r}_{B/N} + \ddot{\bm r}_{S_{c,i2}/B}\Big] - -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} -I_{s_{i2,2}} (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2} \bigg) = 0 \end{multline} @@ -975,10 +975,10 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \ddot{\bm r}_{B/N}- m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \ddot{\bm r}_{B/N}\\ -\dot{\theta}_{i1} (I_{s_{i1,3}} - I_{s_{i1,1}})( \hat{\bm s}_{i1,1} \hat{\bm s}_{i1,3}^T + \hat{\bm s}_{i1,3} \hat{\bm s}_{i1,1}^T) \bm \omega_{\cal{B/N}} -(\dot{\theta}_{i1} + \dot{\theta}_{i2})(I_{s_{i2,3}} - I_{s_{i2,1}}) (\hat{\bm s}_{i2,1} \hat{\bm s}_{i2,3}^T + \hat{\bm s}_{i2,3} \hat{\bm s}_{i2,1}^T) \bm \omega_{\cal{B/N}} \\ - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \Big[\bm{r}''_{S_{c,i1}/B} + 2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B} + \dot{\bm\omega}_{\cal B/N} \times \bm{r}_{S_{c,i1}/B} + \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_{c,i1}/B})\Big]\\ - -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} -I_{s_{i1,2}} \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2}\\ - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \Big[ \bm{r}''_{S_{c,i2}/B} + 2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B} + \dot{\bm\omega}_{\cal B/N} \times \bm{r}_{S_{c,i2}/B} + \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_{c,i2}/B})\Big]\\ - -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} -I_{s_{i2,2}} (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2} \bigg) = 0 \end{multline} @@ -986,7 +986,7 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} \begin{multline} \bm L_B -[I_{\text{hub},B}] \dot{\bm\omega}_{\cal B/N} + m_{\text{hub}} [\tilde{{\bm r}}_{B_c/B}] [\tilde{{\bm r}}_{B_c/B}] \bm{\dot{\omega}}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{hub},B}] \bm\omega_{\cal B/N} + m_{\text{hub}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{{\bm r}}_{B_c/B}] [\tilde{{\bm r}}_{B_c/B}]\bm\omega_{\cal B/N} \\ - - m_{\text{hub}} [\tilde{{\bm r}}_{B_c/B}] \ddot{\bm r}_{B/N} + - m_{\text{hub}} [\tilde{{\bm r}}_{B_c/B}] \ddot{\bm r}_{B/N} + \sum\limits_{i}^{N_S}\bigg(-[I_{\text{sp}_{i1},S_{c,i1}}] \dot{\bm \omega}_{\cal{B/N}} + m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] [\tilde{\bm{r}}_{S_{c,i1}/B}] \dot{\bm\omega}_{\cal B/N} -[I_{\text{sp}_{i2},S_{c,i2}}] \dot{\bm \omega}_{\cal{B/N}} \\ + m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] [\tilde{\bm{r}}_{S_{c,i2}/B}] \dot{\bm\omega}_{\cal B/N} -[\tilde{\bm \omega}_{\cal{B/N}}] [I_{\text{sp}_{i1},S_{c,i1}}] \bm \omega_{\cal{B/N}} @@ -996,71 +996,71 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \ddot{\bm r}_{B/N}\\ - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \ddot{\bm r}_{B/N} -\dot{\theta}_{i1} (I_{s_{i1,3}} - I_{s_{i1,1}})( \hat{\bm s}_{i1,1} \hat{\bm s}_{i1,3}^T + \hat{\bm s}_{i1,3} \hat{\bm s}_{i1,1}^T) \bm \omega_{\cal{B/N}} \\ - -(\dot{\theta}_{i1} + \dot{\theta}_{i2})(I_{s_{i2,3}} - I_{s_{i2,1}}) (\hat{\bm s}_{i2,1} \hat{\bm s}_{i2,3}^T + \hat{\bm s}_{i2,3} \hat{\bm s}_{i2,1}^T) \bm \omega_{\cal{B/N}} + -(\dot{\theta}_{i1} + \dot{\theta}_{i2})(I_{s_{i2,3}} - I_{s_{i2,1}}) (\hat{\bm s}_{i2,1} \hat{\bm s}_{i2,3}^T + \hat{\bm s}_{i2,3} \hat{\bm s}_{i2,1}^T) \bm \omega_{\cal{B/N}} - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \Big[\bm{r}''_{S_{c,i1}/B} + 2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big] \\ - -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} -I_{s_{i1,2}} \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2} - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \Big[ \bm{r}''_{S_{c,i2}/B} + 2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B}\Big] \\ - -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} -I_{s_{i2,2}} (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2} \bigg) = 0 \end{multline} Combining like terms using the parallel axis theorem: \begin{multline} - \bm L_B -[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - - m_{\text{sc}} [\tilde{\bm c}] \ddot{\bm r}_{B/N} + \bm L_B -[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} + - m_{\text{sc}} [\tilde{\bm c}] \ddot{\bm r}_{B/N} + \sum\limits_{i}^{N_S}\bigg( -\dot{\theta}_{i1} (I_{s_{i1,3}} - I_{s_{i1,1}})( \hat{\bm s}_{i1,1} \hat{\bm s}_{i1,3}^T + \hat{\bm s}_{i1,3} \hat{\bm s}_{i1,1}^T) \bm \omega_{\cal{B/N}} \\ - -2 m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \Big[\bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big] -(\dot{\theta}_{i1} + \dot{\theta}_{i2})(I_{s_{i2,3}} - I_{s_{i2,1}}) (\hat{\bm s}_{i2,1} \hat{\bm s}_{i2,3}^T + \hat{\bm s}_{i2,3} \hat{\bm s}_{i2,1}^T) \bm \omega_{\cal{B/N}} + -2 m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \Big[\bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big] -(\dot{\theta}_{i1} + \dot{\theta}_{i2})(I_{s_{i2,3}} - I_{s_{i2,1}}) (\hat{\bm s}_{i2,1} \hat{\bm s}_{i2,3}^T + \hat{\bm s}_{i2,3} \hat{\bm s}_{i2,1}^T) \bm \omega_{\cal{B/N}} \\ - 2 m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \Big[ \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B}\Big] - - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}''_{S_{c,i1}/B} - -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}''_{S_{c,i1}/B} + -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} -I_{s_{i1,2}} \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2} \\- m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \bm{r}''_{S_{c,i2}/B} - -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} -I_{s_{i2,2}} (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2} \bigg) = 0 \end{multline} Splitting the $-2 m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \Big[\bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big]$ term: \begin{multline} - \bm L_B -[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - - m_{\text{sc}} [\tilde{\bm c}] \ddot{\bm r}_{B/N} + \bm L_B -[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} + - m_{\text{sc}} [\tilde{\bm c}] \ddot{\bm r}_{B/N} + \sum\limits_{i}^{N_S}\bigg( -\dot{\theta}_{i1} (I_{s_{i1,3}} - I_{s_{i1,1}})( \hat{\bm s}_{i1,1} \hat{\bm s}_{i1,3}^T + \hat{\bm s}_{i1,3} \hat{\bm s}_{i1,1}^T) \bm \omega_{\cal{B/N}} \\ - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \Big[\bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i1}/B}\Big] + m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] [\tilde{{\bm r}}'_{S_{c,i1}/B}] \bm\omega_{\cal B/N} \\ - -(\dot{\theta}_{i1} + \dot{\theta}_{i2})(I_{s_{i2,3}} - I_{s_{i2,1}}) (\hat{\bm s}_{i2,1} \hat{\bm s}_{i2,3}^T + \hat{\bm s}_{i2,3} \hat{\bm s}_{i2,1}^T) \bm \omega_{\cal{B/N}} + -(\dot{\theta}_{i1} + \dot{\theta}_{i2})(I_{s_{i2,3}} - I_{s_{i2,1}}) (\hat{\bm s}_{i2,1} \hat{\bm s}_{i2,3}^T + \hat{\bm s}_{i2,3} \hat{\bm s}_{i2,1}^T) \bm \omega_{\cal{B/N}} - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \Big[ \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B}\Big]\\ + m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] [\tilde{{\bm r}}'_{S_{c,i2}/B}] \bm\omega_{\cal B/N} - - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}''_{S_{c,i1}/B} - -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}''_{S_{c,i1}/B} + -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} -I_{s_{i1,2}} \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2} \\ - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \bm{r}''_{S_{c,i2}/B} - -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} -I_{s_{i2,2}} (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2} \bigg) = 0 \end{multline} Using the Jacobi identity for simplification: \begin{multline} - \bm L_B -[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - - m_{\text{sc}} [\tilde{\bm c}] \ddot{\bm r}_{B/N} + \bm L_B -[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} + - m_{\text{sc}} [\tilde{\bm c}] \ddot{\bm r}_{B/N} + \sum\limits_{i}^{N_S}\bigg( -\dot{\theta}_{i1} (I_{s_{i1,3}} - I_{s_{i1,1}})( \hat{\bm s}_{i1,1} \hat{\bm s}_{i1,3}^T + \hat{\bm s}_{i1,3} \hat{\bm s}_{i1,1}^T) \bm \omega_{\cal{B/N}} \\ + m_{\text{sp}_{i1}} [\tilde{{\bm r}}'_{S_{c,i1}/B}] [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm\omega_{\cal B/N} + m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] [\tilde{{\bm r}}'_{S_{c,i1}/B}] \bm\omega_{\cal B/N} \\ - -(\dot{\theta}_{i1} + \dot{\theta}_{i2})(I_{s_{i2,3}} - I_{s_{i2,1}}) (\hat{\bm s}_{i2,1} \hat{\bm s}_{i2,3}^T + \hat{\bm s}_{i2,3} \hat{\bm s}_{i2,1}^T) \bm \omega_{\cal{B/N}} + -(\dot{\theta}_{i1} + \dot{\theta}_{i2})(I_{s_{i2,3}} - I_{s_{i2,1}}) (\hat{\bm s}_{i2,1} \hat{\bm s}_{i2,3}^T + \hat{\bm s}_{i2,3} \hat{\bm s}_{i2,1}^T) \bm \omega_{\cal{B/N}} + m_{\text{sp}_{i2}} [\tilde{{\bm r}}'_{S_{c,i2}/B}] [\tilde{{\bm r}}_{S_{c,i2}/B}] \bm\omega_{\cal B/N} \\ + m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] [\tilde{{\bm r}}'_{S_{c,i2}/B}] \bm\omega_{\cal B/N} - - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}''_{S_{c,i1}/B} - -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}''_{S_{c,i1}/B} + -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} -I_{s_{i1,2}} \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2} \\ - m_{\text{sp}_{i1}} [\tilde{\bm\omega}_{\cal B/N}] [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B} - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \bm{r}''_{S_{c,i2}/B} - -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} \\ - -I_{s_{i2,2}} (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2} + -I_{s_{i2,2}} (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2} - m_{\text{sp}_{i2}} [\tilde{\bm\omega}_{\cal B/N}] [\tilde{{\bm r}}_{S_{c,i2}/B}] \bm{r}'_{S_{c,i2}/B} \bigg) = 0 \end{multline} @@ -1069,12 +1069,12 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} \begin{multline} \bm L_B - m_{\text{sc}} [\tilde{\bm c}] \ddot{\bm r}_{B/N} -[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} + \sum\limits_{i}^{N_S}\bigg( - - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}''_{S_{c,i1}/B} + - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}''_{S_{c,i1}/B} \\ - -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} -I_{s_{i1,2}} \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2} - m_{\text{sp}_{i1}} [\tilde{\bm\omega}_{\cal B/N}] [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B} - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \bm{r}''_{S_{c,i2}/B}\\ - -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} -I_{s_{i2,2}} (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2} - m_{\text{sp}_{i2}} [\tilde{\bm\omega}_{\cal B/N}] [\tilde{{\bm r}}_{S_{c,i2}/B}] \bm{r}'_{S_{c,i2}/B} \bigg) = 0 \end{multline} @@ -1085,27 +1085,27 @@ \subsubsection{Rigid Spacecraft Hub Rotational Motion} \\ + \sum\limits_{i}^{N_S}\bigg( - m_{\text{sp}_{i1}} [\tilde{{\bm r}}_{S_{c,i1}/B}] \Big[d_{i1} \bm{\hat{s}}_{i1,3} \ddot{\theta}_{i1} + d_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} \Big] - -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + -[I_{\text{sp}_{i1},S_{c,i1}}] \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} \\-I_{s_{i1,2}} \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2} - m_{\text{sp}_{i1}} [\tilde{\bm\omega}_{\cal B/N}] [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B} \\ - m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \Big[(l_{i1} \bm{\hat{s}}_{i1,3} + d_{i2} \bm{\hat{s}}_{i2,3}) \ddot{\theta}_{i1} + d_{i2} \bm{\hat{s}}_{i2,3} \ddot{\theta}_{i2} + l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big]\\ - -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} + -[I_{\text{sp}_{i2},S_{c,i2}}] (\ddot{\theta}_{i1} + \ddot{\theta}_{i2})\hat{\bm s}_{i2,2} -I_{s_{i2,2}} (\dot{\theta}_{i1} + \dot{\theta}_{i2}) \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i2,2} - m_{\text{sp}_{i2}} [\tilde{\bm\omega}_{\cal B/N}] [\tilde{{\bm r}}_{S_{c,i2}/B}] \bm{r}'_{S_{c,i2}/B} \bigg) = 0 \end{multline} Moving the second order state derivatives to the left hand side: -\begin{multline} +\begin{multline} m_{\text{sc}} [\tilde{\bm c}] \ddot{\bm r}_{B/N} +[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} + \sum\limits_{i}^{N_S}\bigg(\Big[I_{s_{i1,2}} \hat{\bm s}_{i1,2}+ m_{\text{sp}_{i1}} d_{i1} [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,3} +I_{s_{i2,2}} \hat{\bm s}_{i2,2} \\ + m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] (l_{i1} \bm{\hat{s}}_{i1,3} + d_{i2} \bm{\hat{s}}_{i2,3}) \Big] \ddot{\theta}_{i1} +I_{s_{i2,2}}\hat{\bm s}_{i2,2} + m_{\text{sp}_{i2}} d_{i2} [\tilde{{\bm r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3} \ddot{\theta}_{i2} \bigg)= -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} \\ - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} - \sum\limits_{i}^{N_S}\bigg( - m_{\text{sp}_{i1}} d_{i1} \dot{\theta}_{i1}^2 [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,1} + m_{\text{sp}_{i1}} d_{i1} \dot{\theta}_{i1}^2 [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,1} +I_{s_{i1,2}} \dot{\theta}_{i1} [\tilde{\bm \omega}_{\cal{B/N}}] \hat{\bm s}_{i1,2} + m_{\text{sp}_{i1}} [\tilde{\bm\omega}_{\cal B/N}] [\tilde{{\bm r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B} \\ - + m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \Big[ l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big] + + m_{\text{sp}_{i2}} [\tilde{{\bm r}}_{S_{c,i2}/B}] \Big[ l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big] +I_{s_{i2,2}} (\dot{\theta}_{i1} + \dot{\theta}_{i2}) [\tilde{\bm \omega}_{\cal{B/N}}] \hat{\bm s}_{i2,2} \\ + m_{\text{sp}_{i2}} [\tilde{\bm\omega}_{\cal B/N}] [\tilde{{\bm r}}_{S_{c,i2}/B}] \bm{r}'_{S_{c,i2}/B} \bigg) + \bm L_B \end{multline} @@ -1134,7 +1134,7 @@ \subsubsection{Panel 1 Flexing Equation} This needs to be defined: \begin{equation} - \bm F_{1/2i} = \bm F_{\text{ext}_{i2}} - m_{sp_{i2}} \ddot{\bm{r}}_{S_{c,i2}/N} + \bm F_{1/2i} = \bm F_{\text{ext}_{i2}} - m_{sp_{i2}} \ddot{\bm{r}}_{S_{c,i2}/N} \end{equation} Plugging this in results in: @@ -1146,10 +1146,10 @@ \subsubsection{Panel 1 Flexing Equation} Simplifying: \begin{equation} - F_{7} = -k_{i1} \theta_{i1} - c_{i1} \dot{\theta}_{i1} + k_{i2} \theta_{i2} + c_{i2} \dot{\theta}_{i2} + \bm{\hat{s}}_{i1,2} \cdot \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \bm{\hat{s}}_{i1,3} \cdot \bm F_{\text{ext}_{i2}} - m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3} \cdot \ddot{\bm{r}}_{S_{c,i2}/N} + F_{7} = -k_{i1} \theta_{i1} - c_{i1} \dot{\theta}_{i1} + k_{i2} \theta_{i2} + c_{i2} \dot{\theta}_{i2} + \bm{\hat{s}}_{i1,2} \cdot \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \bm{\hat{s}}_{i1,3} \cdot \bm F_{\text{ext}_{i2}} - m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3} \cdot \ddot{\bm{r}}_{S_{c,i2}/N} \end{equation} -The generalized inertia forces are defined as: +The generalized inertia forces are defined as: \begin{multline} F^*_{7} = \bm \omega_{\textit{7}}^{\mathcal{S}_{i1}} \cdot \bm T^*_{\text{sp}_{i1}} + \bm v^{S_{c,i1}}_{7} \cdot (-m_{\text{sp}_{i1}} \ddot{\bm{r}}_{S_{c,i1}/N}) \\ @@ -1167,7 +1167,7 @@ \subsubsection{Panel 1 Flexing Equation} \begin{multline} -k_{i1} \theta_{i1} - c_{i1} \dot{\theta}_{i1} + k_{i2} \theta_{i2} + c_{i2} \dot{\theta}_{i2} + \bm{\hat{s}}_{i1,2} \cdot \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \bm{\hat{s}}_{i1,3} \cdot \bm F_{\text{ext}_{i2}} - m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3} \cdot \ddot{\bm{r}}_{S_{c,i2}/N} \\ - + \bm{\hat{s}}_{i1,2} \cdot \bigg(-[I_{\text{sp}_{i1},S_{c,i1}}] \Big[\dot{\bm \omega}_{\cal{B/N}} + \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2}\Big] + + \bm{\hat{s}}_{i1,2} \cdot \bigg(-[I_{\text{sp}_{i1},S_{c,i1}}] \Big[\dot{\bm \omega}_{\cal{B/N}} + \ddot{\theta}_{i1} \hat{\bm s}_{i1,2} + \dot{\theta}_{i1} \bm \omega_{\cal{B/N}} \times \hat{\bm s}_{i1,2}\Big] -[\tilde{\bm \omega}_{\cal{B/N}}] [I_{\text{sp}_{i1},S_{c,i1}}] \bm \omega_{\cal{B/N}}\\ -\bm \omega_{\cal{B/N}} \times [I_{\text{sp}_{i1},S_{c,i1}}] \dot{\theta}_{i1} \hat{\bm s}_{i1,2} -\dot{\theta}_{i1} \hat{\bm s}_{i1,2} \times [I_{\text{sp}_{i1},S_{c,i1}}] \bm \omega_{\cal{B/N}}\bigg) -m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3} \cdot \ddot{\bm{r}}_{S_{c,i1}/N} = 0 \end{multline} @@ -1176,7 +1176,7 @@ \subsubsection{Panel 1 Flexing Equation} \begin{multline} -k_{i1} \theta_{i1} - c_{i1} \dot{\theta}_{i1} + k_{i2} \theta_{i2} + c_{i2} \dot{\theta}_{i2} + \bm{\hat{s}}_{i1,2} \cdot \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \bm{\hat{s}}_{i1,3} \cdot \bm F_{\text{ext}_{i2}} - m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3} \cdot \ddot{\bm{r}}_{S_{c,i2}/N} \\ - + \bm{\hat{s}}_{i1,2} \cdot \bigg(-[I_{\text{sp}_{i1},S_{c,i1}}] \Big[\dot{\bm \omega}_{\cal{B/N}} + \ddot{\theta}_{i1} \hat{\bm s}_{i1,2}\Big] + + \bm{\hat{s}}_{i1,2} \cdot \bigg(-[I_{\text{sp}_{i1},S_{c,i1}}] \Big[\dot{\bm \omega}_{\cal{B/N}} + \ddot{\theta}_{i1} \hat{\bm s}_{i1,2}\Big] -[\tilde{\bm \omega}_{\cal{B/N}}] [I_{\text{sp}_{i1},S_{c,i1}}] \bm \omega_{\cal{B/N}} \bigg) -m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3} \cdot \ddot{\bm{r}}_{S_{c,i1}/N} = 0 \end{multline} @@ -1195,22 +1195,22 @@ \subsubsection{Panel 1 Flexing Equation} + \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_{c,i2}/B})\Big] = -k_{i1} \theta_{i1} - c_{i1} \dot{\theta}_{i1} + k_{i2} \theta_{i2} + c_{i2} \dot{\theta}_{i2} - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}} \\ + \bm{\hat{s}}_{i1,2} \cdot \bm \tau_{\text{ext}_{i1},H_{i1}} - + l_{i1} \bm{\hat{s}}_{i1,3} \cdot \bm F_{\text{ext}_{i2}} + + l_{i1} \bm{\hat{s}}_{i1,3} \cdot \bm F_{\text{ext}_{i2}} \end{multline} Combing like terms: \begin{multline} \Big[m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3}^T + m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3}^T \Big] \ddot{\bm{r}}_{B/N} + \Big[I_{s_{i1,2}} \bm{\hat{s}}_{i1,2}^T - m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3}^T [\tilde{\bm{r}}_{S_{c,i1}/B}] - m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3}^T [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big] \dot{\bm \omega}_{\cal{B/N}} \\ - + I_{s_{i1,2}} \ddot{\theta}_{i1} + m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3}^T \Big[ \bm{r}''_{S_{c,i1}/B} + + I_{s_{i1,2}} \ddot{\theta}_{i1} + m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3}^T \Big[ \bm{r}''_{S_{c,i1}/B} \Big] + m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3}^T \Big[ \bm{r}''_{S_{c,i2}/B}\Big] = -k_{i1} \theta_{i1} - c_{i1} \dot{\theta}_{i1} + k_{i2} \theta_{i2} + c_{i2} \dot{\theta}_{i2} \\ - - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}} + - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}} - 2 m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3}^T [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} - - m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i2}/B}\\ + - m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i2}/B}\\ - m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i1}/B} + \bm{\hat{s}}_{i1,2} \cdot \bm \tau_{\text{ext}_{i1},H_{i1}} - + l_{i1} \bm{\hat{s}}_{i1,3} \cdot \bm F_{\text{ext}_{i2}} + + l_{i1} \bm{\hat{s}}_{i1,3} \cdot \bm F_{\text{ext}_{i2}} \end{multline} Plugging in the $\bm{r}''_{S_{c,i1}/B}$ and $\bm{r}''_{S_{c,i2}/B}$ terms: @@ -1218,9 +1218,9 @@ \subsubsection{Panel 1 Flexing Equation} \begin{multline} \Big[m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3}^T + m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3}^T \Big] \ddot{\bm{r}}_{B/N} + \Big[I_{s_{i1,2}} \bm{\hat{s}}_{i1,2}^T - m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3}^T [\tilde{\bm{r}}_{S_{c,i1}/B}] - m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3}^T [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big] \dot{\bm \omega}_{\cal{B/N}} \\ + \Big[I_{s_{i1,2}} + m_{\text{sp}_{i1}} d_{i1}^2 + m_{sp_{i2}} l_{i1}^2 + m_{sp_{i2}} l_{i1} d_{i2} \bm{\hat{s}}_{i1,3}^T \bm{\hat{s}}_{i2,3} \Big] \ddot{\theta}_{i1} + \Big[m_{sp_{i2}} l_{i1} d_{i2} \bm{\hat{s}}_{i1,3}^T \bm{\hat{s}}_{i2,3} \Big]\ddot{\theta}_{i2} - = - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}}\\ -k_{i1} \theta_{i1} - c_{i1} \dot{\theta}_{i1} + = - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}}\\ -k_{i1} \theta_{i1} - c_{i1} \dot{\theta}_{i1} + k_{i2} \theta_{i2} + c_{i2} \dot{\theta}_{i2} + \bm{\hat{s}}_{i1,2}^T \bm \tau_{\text{ext}_{i1},H_{i1}} - + l_{i1} \bm{\hat{s}}_{i1,3}^T \bm F_{\text{ext}_{i2}} + + l_{i1} \bm{\hat{s}}_{i1,3}^T \bm F_{\text{ext}_{i2}} - m_{\text{sp}_{i1}} d_{i1} \bm{\hat{s}}_{i1,3}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i1}/B}\\ - m_{sp_{i2}} l_{i1} \bm{\hat{s}}_{i1,3}^T\Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i2}/B} + d_{i2} \big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2 \bm{\hat{s}}_{i2,1}\Big] \end{multline} @@ -1240,7 +1240,7 @@ \subsubsection{Panel 2 Flexing Equation} F_{8} = \bm{\hat{s}}_{i2,2} \cdot \Big[ (-k_{i2} \theta_{i2} - c_{i2} \dot{\theta}_{i2})\bm{\hat{s}}_{i2,2} + \bm \tau_{\text{ext}_{i2},H_{i2}} \Big] = -k_{i2} \theta_{i2} - c_{i2} \dot{\theta}_{i2} + \bm{\hat{s}}_{i2,2} \cdot \bm \tau_{\text{ext}_{i2},H_{i2}} \end{equation} -The generalized inertia forces are defined as: +The generalized inertia forces are defined as: \begin{multline} F^*_{8} = \bm \omega_{\textit{8}}^{\mathcal{S}_{i2}} \cdot \bm T^*_{\text{sp}_{i2}} + \bm v^{S_{c,i2}}_{8} \cdot (-m_{\text{sp}_{i2}} \ddot{\bm{r}}_{S_{c,i2}/N}) \\ @@ -1274,7 +1274,7 @@ \subsubsection{Panel 2 Flexing Equation} \begin{multline} I_{s_{i2,2}} \bm{\hat{s}}_{i2,2}^T \dot{\bm \omega}_{\cal{B/N}} + I_{s_{i2,2}} (\ddot{\theta}_{i1} + \ddot{\theta}_{i2}) + m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T \ddot{\bm{r}}_{S_{c,i2}/N} = - (I_{s_{i2,1}} - I_{s_{i2,3}}) \omega_{s_{i2,3}} \omega_{s_{i2,1}} \\-k_{i2} \theta_{i2} - c_{i2} \dot{\theta}_{i2} - + \bm{\hat{s}}_{i2,2} \cdot \bm \tau_{\text{ext}_{i2},H_{i2}} + + \bm{\hat{s}}_{i2,2} \cdot \bm \tau_{\text{ext}_{i2},H_{i2}} \end{multline} Plugging in accelerations: @@ -1282,16 +1282,16 @@ \subsubsection{Panel 2 Flexing Equation} \begin{multline} I_{s_{i2,2}} \bm{\hat{s}}_{i2,2}^T \dot{\bm \omega}_{\cal{B/N}} + I_{s_{i2,2}} (\ddot{\theta}_{i1} + \ddot{\theta}_{i2}) + m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T \Big[\ddot{\bm{r}}_{B/N} + \bm{r}''_{S_{c,i2}/B} + 2 \bm\omega_{\cal B/N} \times \bm{r}'_{S_{c,i2}/B} + \dot{\bm\omega}_{\cal B/N} \times \bm{r}_{S_{c,i2}/B} \\ + \bm\omega_{\cal B/N} \times (\bm\omega_{\cal B/N} \times \bm{r}_{S_{c,i2}/B})\Big] = - (I_{s_{i2,1}} - I_{s_{i2,3}}) \omega_{s_{i2,3}} \omega_{s_{i2,1}} -k_{i2} \theta_{i2} - c_{i2} \dot{\theta}_{i2} - + \bm{\hat{s}}_{i2,2} \cdot \bm \tau_{\text{ext}_{i2},H_{i2}} + + \bm{\hat{s}}_{i2,2} \cdot \bm \tau_{\text{ext}_{i2},H_{i2}} \end{multline} Moving common terms around: \begin{multline} - \Big[m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T\Big] \ddot{\bm{r}}_{B/N} + \Big[I_{s_{i2,2}} \bm{\hat{s}}_{i2,2}^T - m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big] \dot{\bm \omega}_{\cal{B/N}} + \Big[I_{s_{i2,2}}\Big] \ddot{\theta}_{i1} + \Big[I_{s_{i2,2}}\Big] \ddot{\theta}_{i2} \\+ m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T \Big[ \bm{r}''_{S_{c,i2}/B}\Big] + \Big[m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T\Big] \ddot{\bm{r}}_{B/N} + \Big[I_{s_{i2,2}} \bm{\hat{s}}_{i2,2}^T - m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big] \dot{\bm \omega}_{\cal{B/N}} + \Big[I_{s_{i2,2}}\Big] \ddot{\theta}_{i1} + \Big[I_{s_{i2,2}}\Big] \ddot{\theta}_{i2} \\+ m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T \Big[ \bm{r}''_{S_{c,i2}/B}\Big] = - (I_{s_{i2,1}} - I_{s_{i2,3}}) \omega_{s_{i2,3}} \omega_{s_{i2,1}} -k_{i2} \theta_{i2} - c_{i2} \dot{\theta}_{i2} + \bm{\hat{s}}_{i2,2} \cdot \bm \tau_{\text{ext}_{i2},H_{i2}} \\ - - m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + - m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i2}/B}\Big] \end{multline} @@ -1299,10 +1299,10 @@ \subsubsection{Panel 2 Flexing Equation} \begin{multline} \Big[m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T\Big] \ddot{\bm{r}}_{B/N} + \Big[I_{s_{i2,2}} \bm{\hat{s}}_{i2,2}^T - m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big] \dot{\bm \omega}_{\cal{B/N}} + \Big[I_{s_{i2,2}}+ m_{\text{sp}_{i2}} d_{i2}^2 + m_{\text{sp}_{i2}} l_{i1} d_{i2} \bm{\hat{s}}_{i2,3}^T \bm{\hat{s}}_{i1,3} \Big] \ddot{\theta}_{i1} \\ - + \Big[I_{s_{i2,2}} + m_{\text{sp}_{i2}} d_{i2}^2 \Big] \ddot{\theta}_{i2} + + \Big[I_{s_{i2,2}} + m_{\text{sp}_{i2}} d_{i2}^2 \Big] \ddot{\theta}_{i2} = - (I_{s_{i2,1}} - I_{s_{i2,3}}) \omega_{s_{i2,3}} \omega_{s_{i2,1}} -k_{i2} \theta_{i2} - c_{i2} \dot{\theta}_{i2} + \bm{\hat{s}}_{i2,2} \cdot \bm \tau_{\text{ext}_{i2},H_{i2}} \\ - - m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + - m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i2}/B} + l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1}\Big] \end{multline} @@ -1347,29 +1347,29 @@ \subsection{Back Substitution Method} 1\times 1 \end{bmatrix} \end{equation} -This system mass matrix shows that the all of the solar panel modes are fully coupled with the hub, and that the pairs of solar panels are fully coupled with one another. However, the pairs of solar panels are not directly coupled with other pairs of solar panels. To utilize this pattern in the system mass matrix, the following back-substitution is developed. +This system mass matrix shows that the all of the solar panel modes are fully coupled with the hub, and that the pairs of solar panels are fully coupled with one another. However, the pairs of solar panels are not directly coupled with other pairs of solar panels. To utilize this pattern in the system mass matrix, the following back-substitution is developed. First, Eq.~\eqref{eq:solar_panel_final10} and ~\eqref{eq:sp2final} are rearranged so that the second order state variables for the solar panel motions are isolated on the left hand side: \begin{multline} \Big[I_{s_{i1,2}}+ m_{\text{sp}_{i1}} d_{i1}^2 + m_{sp_{i2}} l_{i1}^2 + m_{sp_{i2}} l_{i1} d_{i2} \hat{\bm s}_{i1,3}^T \bm{\hat{s}}_{i2,3} \Big] \ddot\theta_{i1} + \Big[m_{sp_{i2}} l_{i1} d_{i2} \hat{\bm s}_{i1,3}^T \bm{\hat{s}}_{i2,3} \Big] \ddot{\theta}_{i2}=\\ -\Big[m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3}^T + m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T \Big] \ddot{\bm{r}}_{B/N} - \Big[I_{s_{i1,2}} \hat{\bm s}_{i1,2}^T - m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3}^T [\tilde{\bm{r}}_{S_{c,i1}/B}] - m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T [\tilde{\bm{r}}_{S_{c,i2}/B}] \Big]\dot{\bm\omega}_{\cal B/N} \\ - - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}} - k_{i1} \theta_{i1} - c_{i1}\dot{\theta}_{i1} - + k_{i2} \theta_{i2} + c_{i2} \dot\theta_{i2} - + \hat{\bm s}_{i1,2}^T \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \hat{\bm s}_{i1,3}^T \bm F_{\text{ext}_{i2}}\\ + - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}} - k_{i1} \theta_{i1} - c_{i1}\dot{\theta}_{i1} + + k_{i2} \theta_{i2} + c_{i2} \dot\theta_{i2} + + \hat{\bm s}_{i1,2}^T \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \hat{\bm s}_{i1,3}^T \bm F_{\text{ext}_{i2}}\\ - m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3}^T \Big[2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i1}/B} + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i1}/B}\Big] \\ - - m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + - m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i2}/B} + l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big] \label{eq:spMotion1} \end{multline} \begin{multline} - \Big[I_{s_{i2,2}} + m_{\text{sp}_{i2}} d_{i2}^2 + m_{\text{sp}_{i2}} l_{i1} d_{i2} \hat{\bm s}_{i2,3}^T \bm{\hat{s}}_{i1,3} \Big] \ddot\theta_{i1} - + \Big[I_{s_{i2,2}} + m_{\text{sp}_{i2}} d_{i2}^2 \Big] \ddot\theta_{i2} + \Big[I_{s_{i2,2}} + m_{\text{sp}_{i2}} d_{i2}^2 + m_{\text{sp}_{i2}} l_{i1} d_{i2} \hat{\bm s}_{i2,3}^T \bm{\hat{s}}_{i1,3} \Big] \ddot\theta_{i1} + + \Big[I_{s_{i2,2}} + m_{\text{sp}_{i2}} d_{i2}^2 \Big] \ddot\theta_{i2} = \\ -\Big[m_{\text{sp}_{i2}} d_{i2} \hat{\bm s}_{i2,3}^T\Big] \ddot{\bm{r}}_{B/N} - \Big[I_{s_{i2,2}} \hat{\bm s}_{i2,2}^T + m_{\text{sp}_{i2}} d_{i2} \hat{\bm s}_{i2,3}^T [\tilde{\bm{r}}_{S_{c,i2}/B}]\Big] \dot{\bm\omega}_{\cal B/N} - (I_{s_{i2,1}} - I_{s_{i2,3}}) \omega_{s_{i2,3}} \omega_{s_{i2,1}} \\ - - k_{i2} \theta_{i2} - c_{i2} \dot{\theta}_{i2} + \hat{\bm s}_{i2,2}^T \bm \tau_{\text{ext}_{i2},H_{i2}} + - k_{i2} \theta_{i2} - c_{i2} \dot{\theta}_{i2} + \hat{\bm s}_{i2,2}^T \bm \tau_{\text{ext}_{i2},H_{i2}} - m_{\text{sp}_{i2}} d_{i2} \hat{\bm s}_{i2,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} \\ + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i2}/B} + l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} \Big] \label{eq:sp3final} @@ -1399,31 +1399,31 @@ \subsection{Back Substitution Method} \end{subequations} Also defining the vector $\bm v_i$ as as $2\times1$ with the following components: \begin{multline} - v_{i1} = - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}} - k_{i1} \theta_{i1} - c_{i1}\dot{\theta}_{i1} - + k_{i2} \theta_{i2} + c_{i2} \dot\theta_{i2} - + \hat{\bm s}_{i1,2}^T \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \hat{\bm s}_{i1,3}^T \bm F_{\text{ext}_{i2}}\\ + v_{i1} = - (I_{s_{i1,1}} - I_{s_{i1,3}}) \omega_{s_{i1,3}} \omega_{s_{i1,1}} - k_{i1} \theta_{i1} - c_{i1}\dot{\theta}_{i1} + + k_{i2} \theta_{i2} + c_{i2} \dot\theta_{i2} + + \hat{\bm s}_{i1,2}^T \bm \tau_{\text{ext}_{i1},H_{i1}} + l_{i1} \hat{\bm s}_{i1,3}^T \bm F_{\text{ext}_{i2}}\\ - m_{\text{sp}_{i1}} d_{i1} \hat{\bm s}_{i1,3}^T \Big[2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i1}/B} + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i1}/B}\Big] \\ - - m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + - m_{sp_{i2}} l_{i1} \hat{\bm s}_{i1,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i2}/B} + l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big] \label{eq:solar_panel_final8} \end{multline} \begin{multline} v_{i2} = - (I_{s_{i2,1}} - I_{s_{i2,3}}) \omega_{s_{i2,3}} \omega_{s_{i2,1}} - - k_{i2} \theta_{i2} - c_{i2} \dot{\theta}_{i2} + \hat{\bm s}_{i2,2}^T \bm \tau_{\text{ext}_{i2},H_{i2}}\\ + - k_{i2} \theta_{i2} - c_{i2} \dot{\theta}_{i2} + \hat{\bm s}_{i2,2}^T \bm \tau_{\text{ext}_{i2},H_{i2}}\\ - m_{\text{sp}_{i2}} d_{i2} \hat{\bm s}_{i2,3}^T \Big[ 2 [\tilde{\bm\omega}_{\cal B/N}] \bm{r}'_{S_{c,i2}/B} + [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm{r}_{S_{c,i2}/B} + l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} \Big] \end{multline} Eqs. \eqref{eq:spMotion1} and \eqref{eq:sp3final} can now be re-written as: \begin{equation} - a_{i1,1} \ddot\theta_{i1} + a_{i1,2} \ddot{\theta}_{i2}=\bm f_{i1} \ddot{\bm{r}}_{B/N} + \bm g_{i1}\dot{\bm\omega}_{\cal B/N} + a_{i1,1} \ddot\theta_{i1} + a_{i1,2} \ddot{\theta}_{i2}=\bm f_{i1} \ddot{\bm{r}}_{B/N} + \bm g_{i1}\dot{\bm\omega}_{\cal B/N} +v_{i1} \label{eq:spMotion1Simple} \end{equation} \begin{equation} - a_{i2,1} \ddot\theta_{i1} + a_{i2,1} \ddot\theta_{i1} + a_{i2,2}\ddot\theta_{i2} = \bm f_{i2}\ddot{\bm{r}}_{B/N} + \bm g_{i2}\dot{\bm\omega}_{\cal B/N} + v_{i2} \label{eq:sp3finalSimple} \end{equation} @@ -1471,7 +1471,7 @@ \subsection{Back Substitution Method} \begin{multline} m_\text{sc} \ddot{\bm r}_{B/N} -m_\text{sc} [\tilde{\bm{c}}] \dot{\bm\omega}_{\cal B/N} + \sum_{i=1}^{N_{S}}\bigg(\Big[m_{\text{sp}_{i1}}d_{i1} \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}}l_{i1} \bm{\hat{s}}_{i1,3}+m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}\Big]\ddot{\theta}_{i1} +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3}\ddot{\theta}_{i2}\bigg) \\ = \bm F - 2m_\text{sc} [\tilde{\bm\omega}_{\cal B/N}] \bm c'- m_\text{sc} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c}\\ - -\sum_{i=1}^{N_{S}}\bigg(m_{\text{sp}_{i1}}d_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} +m_{\text{sp}_{i2}}\Big[l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big]\bigg) + -\sum_{i=1}^{N_{S}}\bigg(m_{\text{sp}_{i1}}d_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} +m_{\text{sp}_{i2}}\Big[l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big]\bigg) \label{eq:Rbddot4} \end{multline} @@ -1481,20 +1481,20 @@ \subsection{Back Substitution Method} +\big( I_{s_{i2,2}}\bm{\hat{s}}_{i2,2}+m_{\text{sp}_{i2}} d_{i2} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3}\big)\ddot{\theta}_{i2}\bigg] \\ = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} \\ - \sum\limits_{i=1}^{N_S} \bigg[ - \dot{\theta}_{i1} I_{s_{i1,2}} [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i1,2} + \dot{\theta}_{i1} I_{s_{i1,2}} [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B} +m_{\text{sp}_{i1}}d_{i1}\dot{\theta}_{i1}^2 [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,1}\\ + \big(\dot{\theta}_{i1}+\dot{\theta}_{i2}\big) I_{s_{i2,2}}[\bm{\tilde{\omega}}_{\cal B/N}]\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{r}'_{S_{c,i2}/B} \\+m_{\text{sp}_{i2}} [\tilde{\bm{r}}_{S_{c,i2}/B}] \big(l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\big)\bigg] - + \bm{L}_B + + \bm{L}_B \label{eq:Final7} \end{multline} Performing this substitution for translation yields: \begin{multline} m_\text{sc} \ddot{\bm r}_{B/N} -m_\text{sc} [\tilde{\bm{c}}] \dot{\bm\omega}_{\cal B/N} + \sum_{i=1}^{N_{S}}\bigg(\Big[m_{\text{sp}_{i1}}d_{i1} \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}}l_{i1} \bm{\hat{s}}_{i1,3}+m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}\Big]\Big(e_{i1}^T[F_i] \ddot{\bm r}_{B/N} + e_{i1}^T[G_i]\dot{\bm\omega}_{\cal B/N} \\ - + e_{i1}^T\bm v_i\Big) +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3}\Big(e_{i2}^T[F_i] \ddot{\bm r}_{B/N} + e_{i2}^T[G_i]\dot{\bm\omega}_{\cal B/N} + e_{i2}^T\bm v_i\Big)\bigg) + + e_{i1}^T\bm v_i\Big) +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3}\Big(e_{i2}^T[F_i] \ddot{\bm r}_{B/N} + e_{i2}^T[G_i]\dot{\bm\omega}_{\cal B/N} + e_{i2}^T\bm v_i\Big)\bigg) = \bm F - 2m_\text{sc} [\tilde{\bm\omega}_{\cal B/N}] \bm c'- m_\text{sc} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c}\\ - -\sum_{i=1}^{N_{S}}\bigg(m_{\text{sp}_{i1}}d_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} +m_{\text{sp}_{i2}}\Big[l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big]\bigg) + -\sum_{i=1}^{N_{S}}\bigg(m_{\text{sp}_{i1}}d_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} +m_{\text{sp}_{i2}}\Big[l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big]\bigg) \label{eq:Rbddot5} \end{multline} Combining like terms yields: @@ -1503,8 +1503,8 @@ \subsection{Back Substitution Method} +\Bigg \lbrace-m_\text{sc} [\tilde{\bm{c}}] + \sum_{i=1}^{N_{S}}\bigg[\Big(m_{\text{sp}_{i1}}d_{i1} \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}}l_{i1} \bm{\hat{s}}_{i1,3}+m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}\Big) e_{i1}^T[G_i] +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3} e_{i2}^T[G_i]\bigg] \Bigg \rbrace \dot{\bm\omega}_{\cal B/N} \\ = \bm F - 2m_\text{sc} [\tilde{\bm\omega}_{\cal B/N}] \bm c'- m_\text{sc} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} -\sum_{i=1}^{N_{S}}\bigg(m_{\text{sp}_{i1}}d_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} +m_{\text{sp}_{i2}}\Big[l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big]\\ - +\Big[m_{\text{sp}_{i1}}d_{i1} \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}}l_{i1} \bm{\hat{s}}_{i1,3}+m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}\Big]e_{i1}^T\bm v_i - +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3}e_{i2}^T\bm v_i \bigg) + +\Big[m_{\text{sp}_{i1}}d_{i1} \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}}l_{i1} \bm{\hat{s}}_{i1,3}+m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}\Big]e_{i1}^T\bm v_i + +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3}e_{i2}^T\bm v_i \bigg) \label{eq:Rbddot6} \end{multline} Substitution into the rotational equation of motion: @@ -1515,11 +1515,11 @@ \subsection{Back Substitution Method} \bigg] \\ = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} \\ - \sum\limits_{i=1}^{N_S} \bigg[ - \dot{\theta}_{i1} I_{s_{i1,2}} [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i1,2} + \dot{\theta}_{i1} I_{s_{i1,2}} [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B} +m_{\text{sp}_{i1}}d_{i1}\dot{\theta}_{i1}^2 [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,1}\\ + \big(\dot{\theta}_{i1}+\dot{\theta}_{i2}\big) I_{s_{i2,2}}[\bm{\tilde{\omega}}_{\cal B/N}]\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{r}'_{S_{c,i2}/B} \\+m_{\text{sp}_{i2}} [\tilde{\bm{r}}_{S_{c,i2}/B}] \big(l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\big)\bigg] - + \bm{L}_B + + \bm{L}_B \label{eq:Final7sub} \end{multline} And combining like terms yields: @@ -1533,16 +1533,16 @@ \subsection{Back Substitution Method} +m_{\text{sp}_{i2}}d_{i2} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3}\big)e_{i1}^T[G_i] +\big( I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}} d_{i2} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3}\big)e_{i2}^T[G_i] \bigg]\Bigg \rbrace \dot{\bm\omega}_{\cal B/N} \\ - = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} + = -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} - \sum\limits_{i=1}^{N_S} \bigg[ - \dot{\theta}_{i1} I_{s_{i1,2}} [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i1,2} + \dot{\theta}_{i1} I_{s_{i1,2}} [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B} \\ +m_{\text{sp}_{i1}}d_{i1}\dot{\theta}_{i1}^2 [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,1} + \big(\dot{\theta}_{i1}+\dot{\theta}_{i2}\big) I_{s_{i2,2}}[\bm{\tilde{\omega}}_{\cal B/N}]\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{r}'_{S_{c,i2}/B} \\+m_{\text{sp}_{i2}} [\tilde{\bm{r}}_{S_{c,i2}/B}] \big(l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\big) + \big(I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}+m_{\text{sp}_{i1}}d_{i1} [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,3} + I_{s_{i2,2}}\bm{\hat{s}}_{i2,2}\\ +m_{\text{sp}_{i2}}l_{i1} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i1,3}+m_{\text{sp}_{i2}}d_{i2} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3}\big)e_{i1}^T\bm v_i+\big( I_{s_{i2,2}}\bm{\hat{s}}_{i2,2}\\ +m_{\text{sp}_{i2}} d_{i2} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3}\big)e_{i2}^T\bm v_i\bigg] - + \bm{L}_B + + \bm{L}_B \label{eq:Final8} \end{multline} @@ -1553,8 +1553,8 @@ \subsection{Back Substitution Method} [B_{\text{contr}}] = \sum_{i=1}^{N_{S}}\bigg[\Big(m_{\text{sp}_{i1}}d_{i1} \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}}l_{i1}\bm{\hat{s}}_{i1,3}+m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}\Big) e_{i1}^T[G_i] +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3} e_{i2}^T[G_i]\bigg] \end{multline}\begin{multline} \bm v_{\text{trans,contr}} = -\sum_{i=1}^{N_{S}}\bigg(m_{\text{sp}_{i1}}d_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} +m_{\text{sp}_{i2}}\Big[l_{i1} \dot{\theta}_{i1}^2 \bm{\hat{s}}_{i1,1} + d_{i2}\big(\dot{\theta}_{i1} + \dot{\theta}_{i2}\big)^2\bm{\hat{s}}_{i2,1}\Big]\\ - +\Big[m_{\text{sp}_{i1}}d_{i1} \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}}l_{i1} \bm{\hat{s}}_{i1,3}+m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}\Big]e_{i1}^T\bm v_i - +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3}e_{i2}^T\bm v_i \bigg) + +\Big[m_{\text{sp}_{i1}}d_{i1} \bm{\hat{s}}_{i1,3} +m_{\text{sp}_{i2}}l_{i1} \bm{\hat{s}}_{i1,3}+m_{\text{sp}_{i2}} d_{i2}\bm{\hat{s}}_{i2,3}\Big]e_{i1}^T\bm v_i + +m_{\text{sp}_{i2}} d_{i2} \bm{\hat{s}}_{i2,3}e_{i2}^T\bm v_i \bigg) \end{multline}\begin{multline} [C_{\text{contr}}] = \sum\limits_{i=1}^{N_S} \bigg[ \big(I_{s_{i1,2}}\bm{\hat{s}}_{i1,2}+m_{\text{sp}_{i1}}d_{i1} [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,3} + I_{s_{i2,2}}\bm{\hat{s}}_{i2,2} +m_{\text{sp}_{i2}}l_{i1} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i1,3}\\ @@ -1565,7 +1565,7 @@ \subsection{Back Substitution Method} +m_{\text{sp}_{i2}}d_{i2} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3}\big)e_{i1}^T[G_i] +\big( I_{s_{i2,2}}\bm{\hat{s}}_{i2,2}+m_{\text{sp}_{i2}} d_{i2} [\tilde{\bm{r}}_{S_{c,i2}/B}] \bm{\hat{s}}_{i2,3}\big)e_{i2}^T[G_i] \bigg]\ \end{multline}\begin{multline} [v_{\text{rot,contr}}] = -\sum\limits_{i=1}^{N_S} \bigg[ - \dot{\theta}_{i1} I_{s_{i1,2}} [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i1,2} + \dot{\theta}_{i1} I_{s_{i1,2}} [\bm{\tilde{\omega}}_{\cal B/N}] \bm{\hat{s}}_{i1,2} +m_{\text{sp}_{i1}} [\bm{\tilde{\omega}}_{\cal B/N}] [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{r}'_{S_{c,i1}/B} +m_{\text{sp}_{i1}}d_{i1}\dot{\theta}_{i1}^2 [\tilde{\bm{r}}_{S_{c,i1}/B}] \bm{\hat{s}}_{i1,1} \\ + \big(\dot{\theta}_{i1}+\dot{\theta}_{i2}\big) I_{s_{i2,2}}[\bm{\tilde{\omega}}_{\cal B/N}]\bm{\hat{s}}_{i2,2} @@ -1613,6 +1613,4 @@ \subsection{Back Substitution Method} \label{eq:rBNDDot} \end{equation} -Now Eq.~\eqref{eq:omegaDot} and ~\eqref{eq:rBNDDot} can be used to solve for $\dot{\bm\omega}_{\cal B/N}$ and $\ddot{\bm r}_{B/N}$. Once these second order state variables are solved for, Eqs.~\eqref{eq:thetadot4} and ~\eqref{eq:thetadot5} can be used to directly solve for $\ddot \theta_{i1}$ and $\ddot \theta_{i2}$. This shows that the back substitution method can work seamlessly for interconnected bodies. For this problem the number of interconnected bodies was fixed to be 2, and resulted in an additional $2\times 2$ matrix inversion for each solar panel pair. This shows that for general interconnected bodies, this method would result in needing to invert a matrix based on the number of interconnected bodies. - - +Now Eq.~\eqref{eq:omegaDot} and ~\eqref{eq:rBNDDot} can be used to solve for $\dot{\bm\omega}_{\cal B/N}$ and $\ddot{\bm r}_{B/N}$. Once these second order state variables are solved for, Eqs.~\eqref{eq:thetadot4} and ~\eqref{eq:thetadot5} can be used to directly solve for $\ddot \theta_{i1}$ and $\ddot \theta_{i2}$. This shows that the back substitution method can work seamlessly for interconnected bodies. For this problem the number of interconnected bodies was fixed to be 2, and resulted in an additional $2\times 2$ matrix inversion for each solar panel pair. This shows that for general interconnected bodies, this method would result in needing to invert a matrix based on the number of interconnected bodies. diff --git a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secModelFunctions.tex index 4ccba8a303..a70de98eaa 100644 --- a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secModelFunctions.tex @@ -20,4 +20,4 @@ \section{Model Assumptions and Limitations} \item The dual hinged rigid body will always stay attached to the hub (the hinge does not have torque limits) \item The hinge does not have travel limits, therefore if the spring is not stiff enough it will unrealistically travel through bounds such as running into the spacecraft hub \item The EOMs are nonlinear equations of motion, therefore there can be inaccuracies (and divergence) that result from integration. Having a time step of $<= 0.10\ \text{sec}$ is recommended, but this also depends on the natural frequency of the system -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secTest.tex b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secTest.tex index 416add31c4..5fbe53e824 100644 --- a/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secTest.tex +++ b/src/simulation/dynamics/dualHingedRigidBodies/_Documentation/secTest.tex @@ -2,7 +2,7 @@ \section{Test Description and Success Criteria} This test is located in \texttt{simulation/dynamics/dualHingedRigidBodies/UnitTest/\newline test\_dualHingedRigidBodyStateEffector.py}. In this integrated test there are two dual hinged rigid bodies connected to the spacecraft hub. Depending on the scenario, there are different success criteria. These are outlined in the following subsections: \subsection{Gravity integrated test} -In this test the simulation is placed into orbit around Earth with point gravity and has no damping in the hinged rigid bodies. The following parameters are being tested. +In this test the simulation is placed into orbit around Earth with point gravity and has no damping in the hinged rigid bodies. The following parameters are being tested. \begin{itemize} \item Conservation of orbital angular momentum \item Conservation of orbital energy @@ -27,7 +27,7 @@ \section{Test Parameters} \caption{Error Tolerance - Note: Relative Tolerance is $\textnormal{abs}(\frac{\textnormal{truth} - \textnormal{value}}{\textnormal{truth}}$)} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c |} % Column formatting, + \begin{tabular}{| c | c |} % Column formatting, \hline Test & Relative Tolerance \\ \hline @@ -38,7 +38,7 @@ \section{Test Parameters} \section{Test Results} -The following figures show the conservation of the quantities described in the success criteria for each scenario. The conservation plots are all relative difference plots. All conservation plots show integration error which is the desired result. In the python test these values are automatically checked therefore when the tests pass, these values have all been confirmed to be conserved. +The following figures show the conservation of the quantities described in the success criteria for each scenario. The conservation plots are all relative difference plots. All conservation plots show integration error which is the desired result. In the python test these values are automatically checked therefore when the tests pass, these values have all been confirmed to be conserved. \subsection{Gravity with no damping scenario} \input{AutoTex/ChangeInOrbitalAngularMomentumGravity} diff --git a/src/simulation/dynamics/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.rst b/src/simulation/dynamics/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.rst index 12af19416d..8fdbaa7313 100644 --- a/src/simulation/dynamics/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.rst +++ b/src/simulation/dynamics/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.rst @@ -88,5 +88,3 @@ This section is to outline the steps needed to setup a Hinged Rigid Body State E #. Add the module to the task list:: unitTestSim.AddModelToTask(unitTaskName, panel1) - - diff --git a/src/simulation/dynamics/extForceTorque/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/extForceTorque/_Documentation/BasiliskReportMemo.cls index 7096e85b10..2c8157c432 100755 --- a/src/simulation/dynamics/extForceTorque/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/extForceTorque/_Documentation/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/extForceTorque/extForceTorque.rst b/src/simulation/dynamics/extForceTorque/extForceTorque.rst index a22ee83908..9fb117c155 100644 --- a/src/simulation/dynamics/extForceTorque/extForceTorque.rst +++ b/src/simulation/dynamics/extForceTorque/extForceTorque.rst @@ -34,4 +34,3 @@ provides information on what this message is used for. * - cmdForceInertialInMsg - :ref:`CmdForceInertialMsgPayload` - commanded force input msg in N frame - diff --git a/src/simulation/dynamics/facetDragEffector/_Documentation/AVS.sty b/src/simulation/dynamics/facetDragEffector/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/facetDragEffector/_Documentation/AVS.sty +++ b/src/simulation/dynamics/facetDragEffector/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/facetDragEffector/_Documentation/Basilisk-facet_drag-20190515.tex b/src/simulation/dynamics/facetDragEffector/_Documentation/Basilisk-facet_drag-20190515.tex index 0e63c85a34..ad89fb17bc 100755 --- a/src/simulation/dynamics/facetDragEffector/_Documentation/Basilisk-facet_drag-20190515.tex +++ b/src/simulation/dynamics/facetDragEffector/_Documentation/Basilisk-facet_drag-20190515.tex @@ -49,7 +49,7 @@ \newcommand{\subject}{FacetDrag} \newcommand{\status}{Released} \newcommand{\preparer}{A. Harris} -\newcommand{\summary}{The {\tt facetedDrag} class used to calculate drag forces acting on a spacecraft modeled as a collection of flat, angled facets. Spacecraft geometry is settable by the user. +\newcommand{\summary}{The {\tt facetedDrag} class used to calculate drag forces acting on a spacecraft modeled as a collection of flat, angled facets. Spacecraft geometry is settable by the user. In a given simulation, each spacecraft should have only one drag effector associated with it.} \begin{document} diff --git a/src/simulation/dynamics/facetDragEffector/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/facetDragEffector/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100755 --- a/src/simulation/dynamics/facetDragEffector/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/facetDragEffector/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/facetDragEffector/_Documentation/bibliography.bib b/src/simulation/dynamics/facetDragEffector/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/simulation/dynamics/facetDragEffector/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/facetDragEffector/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/simulation/dynamics/facetDragEffector/_Documentation/secModuleFunctions.tex b/src/simulation/dynamics/facetDragEffector/_Documentation/secModuleFunctions.tex index 21ca29e9ae..70e1027ce6 100644 --- a/src/simulation/dynamics/facetDragEffector/_Documentation/secModuleFunctions.tex +++ b/src/simulation/dynamics/facetDragEffector/_Documentation/secModuleFunctions.tex @@ -9,6 +9,6 @@ \section{Module Functions} \item \textbf {Subscribe to model-relevant information:} Each provided atmospheric model requires different input information to operate, such as current space weather conditions and spacecraft positions. This module automatically attempts to subscribe to the relevant messages for a specified model. \end{itemize} \section{Module Assumptions and Limitations} -This module is only intended for simple convex geometries that do not self-shadow; facets that are turned ``away'' from the flow are considered to be non-interacting, while facets that are turned ``into'' the flow are, regardless of other panel geometry. Additionally, specular reflection is not considered, so lift effects are not calculated. +This module is only intended for simple convex geometries that do not self-shadow; facets that are turned ``away'' from the flow are considered to be non-interacting, while facets that are turned ``into'' the flow are, regardless of other panel geometry. Additionally, specular reflection is not considered, so lift effects are not calculated. -Drag modeling is complex and subject to a variety of assumptions and limitations. For further details, an interested reader is pointed to ~\citenum{vallado2013}. \ No newline at end of file +Drag modeling is complex and subject to a variety of assumptions and limitations. For further details, an interested reader is pointed to ~\citenum{vallado2013}. diff --git a/src/simulation/dynamics/facetDragEffector/_Documentation/secTest.tex b/src/simulation/dynamics/facetDragEffector/_Documentation/secTest.tex index 63048f7e23..7d17d9144c 100644 --- a/src/simulation/dynamics/facetDragEffector/_Documentation/secTest.tex +++ b/src/simulation/dynamics/facetDragEffector/_Documentation/secTest.tex @@ -11,7 +11,7 @@ \subsubsection{setDensityMessage} \subsubsection{testDragForce} -This test verifies that the module correctly calculates the drag force given the model's assumptions. It also implicitly tests the compatibility of facetDrag and exponentialAtmosphere. +This test verifies that the module correctly calculates the drag force given the model's assumptions. It also implicitly tests the compatibility of facetDrag and exponentialAtmosphere. \subsubsection{testShadow} @@ -21,17 +21,17 @@ \subsection{Model-Specific Tests} \subsubsection{test\_unitFacetDrag.py} -This unit test runs setDensityMessage, testDragForce, and testShadow to verify the functionality of the module. +This unit test runs setDensityMessage, testDragForce, and testShadow to verify the functionality of the module. \section{Test Parameters} -The simulation tolerances are shown in Table~\ref{tab:errortol}. In each simulation the neutral density output message is checked relative to python computed true values. +The simulation tolerances are shown in Table~\ref{tab:errortol}. In each simulation the neutral density output message is checked relative to python computed true values. \begin{table}[htbp] \caption{Error tolerance for each test.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ \hline {\tt newDrag.forceExternal\_N} & \input{AutoTeX/toleranceValue} (relative) \\ \hline\hline \end{tabular} @@ -48,15 +48,12 @@ \section{Test Results} \caption{Test result for test\_unitFacetDrag.py} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline\hline - \textbf{Check} & \textbf{Pass/Fail} \\ + \textbf{Check} & \textbf{Pass/Fail} \\ \hline - 1 & \input{AutoTeX/unitTestPassFail} \\ + 1 & \input{AutoTeX/unitTestPassFail} \\ \hline \hline \end{tabular} \end{table} - - - diff --git a/src/simulation/dynamics/facetDragEffector/_Documentation/secUserGuide.tex b/src/simulation/dynamics/facetDragEffector/_Documentation/secUserGuide.tex index 8c9fc08f60..d03633ade5 100644 --- a/src/simulation/dynamics/facetDragEffector/_Documentation/secUserGuide.tex +++ b/src/simulation/dynamics/facetDragEffector/_Documentation/secUserGuide.tex @@ -35,4 +35,3 @@ \subsection{General Module Setup} scObject.ModelTag = "spacecraftBody" scObject.addDynamicEffector(newDrag) \end{verbatim} - diff --git a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h index 2bee2130ec..1d689099c7 100644 --- a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h +++ b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h @@ -80,4 +80,4 @@ class FacetDragDynamicEffector: public SysModel, public DynamicEffector { }; -#endif +#endif diff --git a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.rst b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.rst index 6121e8af21..2da51c2613 100644 --- a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.rst +++ b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.rst @@ -28,4 +28,3 @@ provides information on what this message is used for. * - atmoDensInMsg - :ref:`AtmoPropsMsgPayload` - input message for atmospheric density information - diff --git a/src/simulation/dynamics/gravityEffector/_Documentation/AVS.sty b/src/simulation/dynamics/gravityEffector/_Documentation/AVS.sty index 73e5dd7956..c02abd9dfe 100755 --- a/src/simulation/dynamics/gravityEffector/_Documentation/AVS.sty +++ b/src/simulation/dynamics/gravityEffector/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/gravityEffector/_Documentation/Basilisk-GravityEffector-20170712.tex b/src/simulation/dynamics/gravityEffector/_Documentation/Basilisk-GravityEffector-20170712.tex index 85447378ac..d9d5d3b81e 100755 --- a/src/simulation/dynamics/gravityEffector/_Documentation/Basilisk-GravityEffector-20170712.tex +++ b/src/simulation/dynamics/gravityEffector/_Documentation/Basilisk-GravityEffector-20170712.tex @@ -86,7 +86,7 @@ \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/gravityEffector/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/gravityEffector/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/gravityEffector/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/gravityEffector/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/gravityEffector/_Documentation/bibliography.bib b/src/simulation/dynamics/gravityEffector/_Documentation/bibliography.bib index fcbb8bad81..1c6ceefe0c 100755 --- a/src/simulation/dynamics/gravityEffector/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/gravityEffector/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, @@ -37,17 +37,17 @@ @book{scheeres2012 @book{schaub2014, auTHor = "Hanspeter Schaub and John L. Junkins", -Title = {Analytical Mechanics of Space Systems}, +Title = {Analytical Mechanics of Space Systems}, publisher = {AIAA Education Series}, year = {2014}, edition = 3, -} +} -@article{werner1996, - auTHor = "Werner, R. and Scheeres, D.", - Title = {Exterior gravitation of a polyhedron derived and compared with harmonic and mascon representations of asteroid 4769 Castalia}, +@article{werner1996, + auTHor = "Werner, R. and Scheeres, D.", + Title = {Exterior gravitation of a polyhedron derived and compared with harmonic and mascon representations of asteroid 4769 Castalia}, journal = "Celestial Mechanics and Dynamical Astronomy", volume={65}, pages={313-344}, - YEAR = 1996, -} + YEAR = 1996, +} diff --git a/src/simulation/dynamics/gravityEffector/_Documentation/secModelDescription.tex b/src/simulation/dynamics/gravityEffector/_Documentation/secModelDescription.tex index b30a57f9ee..2d316a7c60 100755 --- a/src/simulation/dynamics/gravityEffector/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/gravityEffector/_Documentation/secModelDescription.tex @@ -144,7 +144,7 @@ \subsection{Spherical harmonics gravity model} U(\mathbf{\bar r}) = \frac{\mu}{r} \sum_{l=0}^\infty \sum_{m=0}^l \bigg(\frac{R_{\text{ref}}}{r}\bigg)^l P_{l,m}[\sin(\phi)] \big[C_{l,m} \cos(m \lambda) + S_{l,m} \sin(m \lambda)\big] \end{equation} -Some coefficients have a very interesting interpretation. +Some coefficients have a very interesting interpretation. \begin{align} C_{0,0} &= 1\\ S_{l,0} &= 0 \quad \forall l \geq 0\\ @@ -383,9 +383,9 @@ \subsection{Polyhedral gravity model} \end{equation} The vector $\mathbf{r}_f$ extends from the evaluation point to any vertex on the face. The variable $\mathbf{n}_f$ is the outward-pointing normal vector of face $f$. The term $\omega_f$ is the signed solid angle subtended by the face $f$ when viewed from the evalution point. The variable $\mathbf{r}_e$ is a vector from the evaluation point to the initial vertex of edge $e$. The vector $\mathbf{n}_e$ is the normal of the edge lying on the face plane. The term $L_e$ corresponds to the potential of a 1D wire. - + Note that in Werner and Scheeres\cite{werner1996}, the double summation term of \eqref{eq:polyhedral_potential} was simplified to a single summation over all polyhedron's edges. Although that reduction is convenient for mathematical compactness, retaining the double summation simplifies the algorithmic implementation (so that there is no need to check for common edges between adjacent faces). - + In order to provide consistency with other gravity models, the density $\sigma$ is computed based on the polyhedron shape and the input gravity parameter $\mu$. The volume of a trisurface polyhedron is \begin{equation} V = \frac{1}{6}\sum_{f\in\text{faces}}|(\mathbf{r}^{f}_1\times\mathbf{r}^{f}_2)\cdot \mathbf{r}^{f}_3|, diff --git a/src/simulation/dynamics/gravityEffector/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/gravityEffector/_Documentation/secModelFunctions.tex index f3c7124fe7..7254889711 100755 --- a/src/simulation/dynamics/gravityEffector/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/gravityEffector/_Documentation/secModelFunctions.tex @@ -7,13 +7,13 @@ \section{Model Functions} \begin{itemize} \item \textbf{Simple Gravity}: The code can compute a gravity acceleration between two bodies according to Newton's law of universal gravitation given $\mu$ and the distance between the bodies. \item \textbf{Spherical Harmonics}: The code can compute gravity acceleration between two bodies using the more-complex method of spherical harmonics. To do this, it must be provided with the same inputs as for calculating simple gravity. In addition, it needs to be provided a "degree" of spherical harmonics to be used and spherical harmonics coefficients useful up to that degree. - \item \textbf{Polyhedral:} The code can compute gravity acceleration between two bodies using the polyhedral model. To do this, it must be provided with the same inputs as for calculating simple gravity. In addition, it needs to be provided with the vertexes positions and their assignment to faces. + \item \textbf{Polyhedral:} The code can compute gravity acceleration between two bodies using the polyhedral model. To do this, it must be provided with the same inputs as for calculating simple gravity. In addition, it needs to be provided with the vertexes positions and their assignment to faces. \end{itemize} \item \textbf{Multiple Body Effects}: The code can stack the effects of multiple gravity bodies on top of each other to determine the net effect on a spacecraft. The user must indicate in the spacecraft set-up which gravitational bodies should be taken into account. \item \textbf{Interface: Spacecraft States}: The code sends and receives spacecraft state information via the DynParamManager. \item \textbf{Interface: Energy Contributions}: The code sends spacecraft energy contributions via \\updateEnergyContributions() which is called by the spacecraft. \item \textbf{Interface: GravBody States}: The code outputs GravBody states(ephemeris information) via the Basilisk messaging system. - + \end{itemize} \section{Model Assumptions and Limitations} @@ -30,7 +30,5 @@ \subsection{Polyhedral gravity model} \begin{itemize} \item \textbf{Constant Density}: The polyhedron gravity computation assumes that the body has constant density. Consequently, this method does not account for spatial density variations that typically arise within the internal structure of bodies or in contact binary asteroids. \item \textbf{Shape Accuracy}: The polyhedral model assumes the body shape is described as a polyhedron which is an approximation of the continuous real shape. The resolution of the model can be augmented by increasing the number of vertexes and faces though, in turn, this may considerably slow down the gravity evaluation times. Let recall that the polyhedron gravity computation requires to loop over all faces and edges. - \item \textbf{Trisurface Polyhedron:} The implemented computation is case-specific for polyhedrons with faces composed of three vertexes. This reduces the possible polyhedrons to a single geometrical topology. However, the trisurface polyhedron is the standardized shape for small bodies. + \item \textbf{Trisurface Polyhedron:} The implemented computation is case-specific for polyhedrons with faces composed of three vertexes. This reduces the possible polyhedrons to a single geometrical topology. However, the trisurface polyhedron is the standardized shape for small bodies. \end{itemize} - - diff --git a/src/simulation/dynamics/gravityEffector/_Documentation/secTest.tex b/src/simulation/dynamics/gravityEffector/_Documentation/secTest.tex index a1e3fc39be..daae21a3ba 100755 --- a/src/simulation/dynamics/gravityEffector/_Documentation/secTest.tex +++ b/src/simulation/dynamics/gravityEffector/_Documentation/secTest.tex @@ -10,7 +10,7 @@ \subsection{Model Set-Up Verification} \end{itemize} \subsection{Independent Spherical Harmonics Check} This test compares the Basilisk gravity module spherical harmonics acceleration output to an independently formulated python solution. Gravity is measured at an arbitrary point. Note that the independent solution has singularities at the poles that lead to minor divergences in total acceleration. \subsection{Single-Body Gravity Calculations} This test compares calculated gravity values around the Earth with ephemeris data from the Hubble telescope. The simulation begins shortly after 0200 UTC May 1, 2012 and carries on for two hours, evaluating the gravitational acceleration at two second intervals. -\subsection{Multi-Body Gravity Calculations} This test checks whether gravity from multiple sources is correctly stacked when applied to a spacecraft. First, a planet is placed in space near a spacecraft. Second, a planet with half the mass of the first is placed the same distance from the spacecraft but in the opposite direction. The gravitational acceleration along that axis is seen to be cut in half for the spacecraft. Finally, a third planet identical to the second is added coincident with the second and the net gravitational acceleration on the spacecraft is seen to be zero. +\subsection{Multi-Body Gravity Calculations} This test checks whether gravity from multiple sources is correctly stacked when applied to a spacecraft. First, a planet is placed in space near a spacecraft. Second, a planet with half the mass of the first is placed the same distance from the spacecraft but in the opposite direction. The gravitational acceleration along that axis is seen to be cut in half for the spacecraft. Finally, a third planet identical to the second is added coincident with the second and the net gravitational acceleration on the spacecraft is seen to be zero. \section{Test Parameters} @@ -20,12 +20,12 @@ \section{Test Parameters} \caption{Error tolerance for each test. Note that relative tolerance = $\frac{truth - result}{truth}$} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | l } % Column formatting, + \begin{tabular}{ c | l } % Column formatting, \hline \textbf{Test} & \textbf{Tolerance} \\ \hline - Setup Test & \input{AutoTex/sphericalHarmonicsAccuracy} (Absolute) \\ - Independent Spherical Harmonics Check & \input{AutoTex/independentCheckAccuracy} (Relative) \\ - Single-Body Gravity & \input{AutoTex/singleBodyAccuracy} (Relative) \\ + Setup Test & \input{AutoTex/sphericalHarmonicsAccuracy} (Absolute) \\ + Independent Spherical Harmonics Check & \input{AutoTex/independentCheckAccuracy} (Relative) \\ + Single-Body Gravity & \input{AutoTex/singleBodyAccuracy} (Relative) \\ Multi-Body Gravity & \input{AutoTex/multiBodyAccuracy} (Relative for first check, absolute for second) \\ \hline \end{tabular} \end{table} @@ -40,12 +40,12 @@ \section{Test Results} \caption{Test results.} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | c } % Column formatting, + \begin{tabular}{c | c | c } % Column formatting, \hline \textbf{Test} & \textbf{Pass/Fail} & \textbf{Notes} \\ \hline Setup Test & \input{AutoTex/sphericalHarmonicsPassFail} & \input{AutoTex/sphericalHarmonicsFailMsg} \\ - Independent Spherical Harmonics Check & \input{AutoTex/independentCheckPassFail} & \input{AutoTex/independentCheckFailMsg} \\ - Single-Body Gravity & \input{AutoTex/singleBodyPassFail} & \input{AutoTex/singleBodyFailMsg} \\ + Independent Spherical Harmonics Check & \input{AutoTex/independentCheckPassFail} & \input{AutoTex/independentCheckFailMsg} \\ + Single-Body Gravity & \input{AutoTex/singleBodyPassFail} & \input{AutoTex/singleBodyFailMsg} \\ Multi-Body Gravity &\input{AutoTex/multiBodyPassFail} & \input{AutoTex/multiBodyFailMsg} \\ \hline \end{tabular} -\end{table} \ No newline at end of file +\end{table} diff --git a/src/simulation/dynamics/gravityEffector/_Documentation/secUserGuide.tex b/src/simulation/dynamics/gravityEffector/_Documentation/secUserGuide.tex index ba1e9d6e57..f2d8270ccd 100755 --- a/src/simulation/dynamics/gravityEffector/_Documentation/secUserGuide.tex +++ b/src/simulation/dynamics/gravityEffector/_Documentation/secUserGuide.tex @@ -4,8 +4,8 @@ \section{User Guide} \subsection{Using Central Bodies and Relative Dynamics} \subsubsection{Using Central Bodies} In simulations with multiple planetary bodies, using dynamics relative to a central body can improve accuracy. Generally, this is the right thing to do rather than using an absolute coordinate set. If a user has a gravBody called \verb|earth|, the central body flag should be set to True. -\verb|earth.isCentralBody = True| -The dynamics will then take care of themselves, but the user needs to be careful to input initial position and velocity values as \textit{relative to} the central body. This can be input from a set of Keplerian orbital elements using \verb|orbitalMotion.elem2rv| as in\\ \verb|Basilisk/tests/scenarios/scenarioBasicOrbit.py|. +\verb|earth.isCentralBody = True| +The dynamics will then take care of themselves, but the user needs to be careful to input initial position and velocity values as \textit{relative to} the central body. This can be input from a set of Keplerian orbital elements using \verb|orbitalMotion.elem2rv| as in\\ \verb|Basilisk/tests/scenarios/scenarioBasicOrbit.py|. The user should be aware that if spacecraft position and velocity are read back from a message log or plotted that the absolute position and velocity will be returned. It will take additional work to convert the outputs back to a relative form by subtracting out the central body positions and velocities. No rotation will be needed, though. It is critical that the relative position and velocities are given in a frame which is linearly translated but \textbf{not rotated} from the simulation inertial frame. There is no handling of rotated relative frames within the dynamics. The orbital element to position and velocity conversion in the section below can be used for relative dynamics inputs, as well. diff --git a/src/simulation/dynamics/gravityEffector/_UnitTest/EROS856Vert1708Fac.txt b/src/simulation/dynamics/gravityEffector/_UnitTest/EROS856Vert1708Fac.txt index 3ce700432c..b355f13877 100755 --- a/src/simulation/dynamics/gravityEffector/_UnitTest/EROS856Vert1708Fac.txt +++ b/src/simulation/dynamics/gravityEffector/_UnitTest/EROS856Vert1708Fac.txt @@ -2562,4 +2562,4 @@ 849 856 854 852 853 855 855 853 854 -854 856 855 \ No newline at end of file +854 856 855 diff --git a/src/simulation/dynamics/gravityEffector/_UnitTest/GGM03S.txt b/src/simulation/dynamics/gravityEffector/_UnitTest/GGM03S.txt index 6022af24f8..3c4a508b3f 100755 --- a/src/simulation/dynamics/gravityEffector/_UnitTest/GGM03S.txt +++ b/src/simulation/dynamics/gravityEffector/_UnitTest/GGM03S.txt @@ -1,4 +1,4 @@ -0.6378136300E+07, 0.3986004415E+15, 7.2921150E-5, 180, 180, 1, 0.0, 0.0 +0.6378136300E+07, 0.3986004415E+15, 7.2921150E-5, 180, 180, 1, 0.0, 0.0 0, 0, 1.000000000000E+00, 0.000000000000E+00, 0.00000E+00, 0.00000E+00 1, 0, 0.000000000000E+00, 0.000000000000E+00, 0.00000E+00, 0.00000E+00 1, 1, 0.000000000000E+00, 0.000000000000E+00, 0.00000E+00, 0.00000E+00 diff --git a/src/simulation/dynamics/gravityEffector/_UnitTest/Validation_code/computePolyAcc.c b/src/simulation/dynamics/gravityEffector/_UnitTest/Validation_code/computePolyAcc.c index fe059dad83..d55a5ea4c2 100755 --- a/src/simulation/dynamics/gravityEffector/_UnitTest/Validation_code/computePolyAcc.c +++ b/src/simulation/dynamics/gravityEffector/_UnitTest/Validation_code/computePolyAcc.c @@ -14,10 +14,10 @@ double dotProduct(double *a, double *b){ /* Assign variable to dot product output */ double c; - + /* Compute dot product */ c = a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; - + /* Return output */ return c; } @@ -26,7 +26,7 @@ double dotProduct(double *a, double *b){ void crossProduct(double *c, double *a, double *b){ /* Compute cross product for each component */ c[0] = a[1]*b[2] - a[2]*b[1]; - c[1] = a[2]*b[0] - a[0]*b[2]; + c[1] = a[2]*b[0] - a[0]*b[2]; c[2] = a[0]*b[1] - a[1]*b[0]; } @@ -34,19 +34,19 @@ void crossProduct(double *c, double *a, double *b){ double normVector(double *a){ /* Declare variable to loop through its 3 components */ int i_3D; - + /* Declare variable to store the norm */ double norm = 0.0; - + /* Loop through all its 3 components */ for (i_3D = 0; i_3D < 3; i_3D++){ /* Summatory add square of each vector component */ norm += pow(a[i_3D], 2.0); } - + /* Compute the square root of the root components summatory */ norm = pow(norm, 0.5); - + /* Return norm value */ return norm; } @@ -55,7 +55,7 @@ double normVector(double *a){ int intmin(int a, int b){ /* Define output number */ int c; - + /* If statement to check what number is lower */ if (a < b){ c = a; @@ -66,7 +66,7 @@ int intmin(int a, int b){ else{ c = a; } - + /* Return min value */ return c; } @@ -75,12 +75,12 @@ int intmin(int a, int b){ void dyadProduct(double c[3][3], double *a, double *b){ /* Declare auxiliary variables to loop */ int i, j; - + /* Loop through rows */ for (i = 0; i < 3; i++){ /*Loop through columns */ for (j = 0; j < 3; j++){ - c[i][j] = a[i]*b[j]; + c[i][j] = a[i]*b[j]; } } } @@ -91,13 +91,13 @@ void matrixvectorProduct(double *c, double a[3][3], double *b, double factor, int flag){ /* Define for loops auxiliary variable */ int i, j; - + /* Reset c values to zero (to not interfere with summatories if c is * filled) */ c[0] = 0.0; c[1] = 0.0; c[2] = 0.0; - + /* If statement to decide if the vector will multiply by the right side * or the left side */ if (flag == 1){ @@ -106,7 +106,7 @@ void matrixvectorProduct(double *c, double a[3][3], double *b, for (j = 0; j < 3; j++){ c[i] = c[i] + a[j][i]*b[j]; } - + /* Multiply vector value by multiplication factor */ c[i] = c[i] * factor; } @@ -118,7 +118,7 @@ void matrixvectorProduct(double *c, double a[3][3], double *b, c[i] = c[i] + a[i][j]*b[j]; /*printf("%lf \t %lf \t %lf \t %lf\n", c[i], a[i][j], b[j], factor);*/ } - + /* Multiply vector component value by multiplication factor */ c[i] = c[i] * factor; } @@ -128,107 +128,107 @@ void matrixvectorProduct(double *c, double a[3][3], double *b, /* The gateway function */ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) -{ +{ /* Macros for the input arguments */ #define r_IN prhs[0] #define xyz_IN prhs[1] #define order_IN prhs[2] #define rho_IN prhs[3] #define G_IN prhs[4] - + /* Macros for the output arguments */ #define dU_OUT plhs[0] #define lU_OUT plhs[1] - + /* Declare input arguments */ double *r, *xyz, *order, rho, G; - + /* Declare parameters for the input arguments */ int M_xyz, N_xyz; int m_xyz, n_xyz; double r_xyz_norm; int M_order, N_order; int m_order, n_order; - + /* Declare output arguments */ double *dU, *lU; - + /* Declare auxiliary variables in for loops */ int i_3D; - + int i, j, k; - + double ri_idx[3], rj_idx[3], rk_idx[3]; - + double norm_ri_idx, norm_rj_idx, norm_rk_idx; - + double r_edge1[3], r_edge2[3]; - + double nf_vec[3]; double norm_nf_vec; double nf_idx[3]; - + int i_min; - + double r1[3], r2[3], re[3]; double a, b; - + double r12[3]; double e; - + double n12_vec[3], n12[3]; double norm_n12_vec; - + double Le; double Ee[3][3]; double EereLe[3] = {0, 0, 0}; - + double dUe[3] = {0, 0, 0}; - + double cross_rj_rk[3]; double wf; - + double dot_nf_rf; double dot_nf_rf_wf; - + double dUf[3] = {0, 0, 0}; - + /* Get the pointers to the data of r, xyz and order */ r = mxGetPr(r_IN); xyz = mxGetPr(xyz_IN); order = mxGetPr(order_IN); - + /* Get the xyz dimensions (rows and columns) */ M_xyz = mxGetM(xyz_IN); N_xyz = mxGetN(xyz_IN); - + /* Get the order dimensions (rows and columns) */ M_order = mxGetM(order_IN); N_order = mxGetN(order_IN); - + /* Get the values for rho and G */ rho = mxGetScalar(rho_IN); G = mxGetScalar(G_IN); - + /* Create the output vector and scalar */ dU_OUT = mxCreateDoubleMatrix(3, 1, mxREAL); lU_OUT = mxCreateDoubleScalar(0); - + /* Get the pointer to the data of dU */ dU = mxGetPr(dU_OUT); - + /* Get lU and initialize it */ lU = mxGetPr(lU_OUT); lU[0] = 0.0; - + /* Loop through each facect */ for (m_order = 0; m_order < M_order; m_order++){ - /* Fill auxiliary variables with vertex order on each facet + /* Fill auxiliary variables with vertex order on each facet * (1 has to be substracted because C arrays starts in 0)*/ i = order[m_order] - 1; j = order[m_order + M_order] - 1; - k = order[m_order + 2*M_order] - 1; - + k = order[m_order + 2*M_order] - 1; + /* Loop through each 3D position */ for (i_3D = 0; i_3D < 3; i_3D++){ /* Compute vectors going from each vertex to the evaluation @@ -236,30 +236,30 @@ void mexFunction( int nlhs, mxArray *plhs[], ri_idx[i_3D] = xyz[i + M_xyz*i_3D] - r[i_3D]; rj_idx[i_3D] = xyz[j + M_xyz*i_3D] - r[i_3D]; rk_idx[i_3D] = xyz[k + M_xyz*i_3D] - r[i_3D]; - + /* Compute two edge vectors for a-posteriori normal * facet computation */ r_edge1[i_3D] = rj_idx[i_3D] - ri_idx[i_3D]; - r_edge2[i_3D] = rk_idx[i_3D] - rj_idx[i_3D]; + r_edge2[i_3D] = rk_idx[i_3D] - rj_idx[i_3D]; } - + /* Compute norm of vectors going from each vertex to the evaluation * point */ norm_ri_idx = normVector(ri_idx); norm_rj_idx = normVector(rj_idx); norm_rk_idx = normVector(rk_idx); - + /* Compute facet normal vector */ crossProduct(nf_vec, r_edge1, r_edge2); - + /* Compute facet normal vector norm */ norm_nf_vec = normVector(nf_vec); - + /* Compute normalized facet normal vector */ nf_idx[0] = nf_vec[0] / norm_nf_vec; nf_idx[1] = nf_vec[1] / norm_nf_vec; nf_idx[2] = nf_vec[2] / norm_nf_vec; - + /* Loop through each facet edge */ for (n_order = 0; n_order < N_order; n_order++){ /* Switch to determine edge being computed */ @@ -270,14 +270,14 @@ void mexFunction( int nlhs, mxArray *plhs[], * performed for that particular edge: one edge belongs * to two faces*/ i_min = intmin(i, j); - + /* Loop through 1,2,3 */ for (i_3D = 0; i_3D < 3; i_3D++){ r1[i_3D] = ri_idx[i_3D]; r2[i_3D] = rj_idx[i_3D]; re[i_3D] = xyz[i_min + M_xyz*i_3D] - r[i_3D]; } - + /* Assign norm */ a = norm_ri_idx; b = norm_rj_idx; @@ -288,14 +288,14 @@ void mexFunction( int nlhs, mxArray *plhs[], * performed for that particular edge: one edge belongs * to two faces*/ i_min = intmin(j, k); - + /* Loop through 1,2,3 */ for (i_3D = 0; i_3D < 3; i_3D++){ r1[i_3D] = rj_idx[i_3D]; r2[i_3D] = rk_idx[i_3D]; re[i_3D] = xyz[i_min + M_xyz*i_3D] - r[i_3D]; } - + /* Assign norm */ a = norm_rj_idx; b = norm_rk_idx; @@ -306,44 +306,44 @@ void mexFunction( int nlhs, mxArray *plhs[], * performed for that particular edge: one edge belongs * to two faces*/ i_min = intmin(i, k); - + /* Loop through 1,2,3 */ for (i_3D = 0; i_3D < 3; i_3D++){ r1[i_3D] = rk_idx[i_3D]; r2[i_3D] = ri_idx[i_3D]; re[i_3D] = xyz[i_min + M_xyz*i_3D] - r[i_3D]; } - + /* Assign norm */ a = norm_rk_idx; b = norm_ri_idx; break; } - + /* Compute along edge vector and norm */ r12[0] = r2[0] - r1[0]; r12[1] = r2[1] - r1[1]; r12[2] = r2[2] - r1[2]; e = normVector(r12); - + /* Compute normal vector to edge, n12_vec, and its norm */ crossProduct(n12_vec, r12, nf_idx); norm_n12_vec = normVector(n12_vec); - + /* Normalize normal vector to edge */ n12[0] = n12_vec[0] / norm_n12_vec; n12[1] = n12_vec[1] / norm_n12_vec; n12[2] = n12_vec[2] / norm_n12_vec; - + /* Dimensionless per-edge factor */ Le = log((a+b+e) / (a+b-e)); - + /* Compute dyad product between nf_idx and n12 */ dyadProduct(Ee, nf_idx, n12); - + /* Compute Le*Ee*re */ matrixvectorProduct(EereLe, Ee, re, Le, 0); - + /* Add current facet contribution to dUe */ dUe[0] = dUe[0] + EereLe[0]; dUe[1] = dUe[1] + EereLe[1]; @@ -352,37 +352,36 @@ void mexFunction( int nlhs, mxArray *plhs[], /* Compute auxiliary vector to compute then solid angle angle * per facet */ crossProduct(cross_rj_rk, rj_idx, rk_idx); - + /* Compute solid angle for the current facet. Dimensionless * per-face factor*/ wf = 2*atan2(dotProduct(ri_idx, cross_rj_rk), - norm_ri_idx*norm_rj_idx*norm_rk_idx + - norm_ri_idx*dotProduct(rj_idx, rk_idx) + - norm_rj_idx*dotProduct(rk_idx, ri_idx) + + norm_ri_idx*norm_rj_idx*norm_rk_idx + + norm_ri_idx*dotProduct(rj_idx, rk_idx) + + norm_rj_idx*dotProduct(rk_idx, ri_idx) + norm_rk_idx*dotProduct(ri_idx, rj_idx)); - + /* Compute auxiliary dot product for solid angle contribution. * rf is taken as ri */ dot_nf_rf = dotProduct(nf_idx, ri_idx); - + /* Auxiliary constant term to compute current solid angle facet * contribution */ dot_nf_rf_wf = dot_nf_rf*wf; - + /* Add current solid angle facet contribution to dUf */ dUf[0] = dUf[0] + nf_idx[0]*dot_nf_rf_wf; dUf[1] = dUf[1] + nf_idx[1]*dot_nf_rf_wf; dUf[2] = dUf[2] + nf_idx[2]*dot_nf_rf_wf; - + /* Update lU with solid angle */ lU[0] = lU[0] + wf; } - + /* Potential computation*/ dU[0] = G*rho*(-dUe[0] + dUf[0]); dU[1] = G*rho*(-dUe[1] + dUf[1]); dU[2] = G*rho*(-dUe[2] + dUf[2]); - + return; } - diff --git a/src/simulation/dynamics/gravityEffector/_UnitTest/Validation_code/polyhedralTestdataGenerator.m b/src/simulation/dynamics/gravityEffector/_UnitTest/Validation_code/polyhedralTestdataGenerator.m index 0e614b4ab4..ded4191360 100644 --- a/src/simulation/dynamics/gravityEffector/_UnitTest/Validation_code/polyhedralTestdataGenerator.m +++ b/src/simulation/dynamics/gravityEffector/_UnitTest/Validation_code/polyhedralTestdataGenerator.m @@ -23,7 +23,7 @@ % Define ode simulation options ode_options = odeset('RelTol',1E-12, 'AbsTol',1E-12); - + % LAUNCH RUNGE-KUTTA ALGORITHM ODE45 [t, x] = ode45(@(t,x) dynamics(t, x, omega, rho, G, xyz_poly,... order_poly), tspan, x0, ode_options); @@ -39,7 +39,7 @@ f(1:3,1) = x(4:6,1); acc = computePolyAcc(x(1:3,1), xyz_poly, order_poly, rho, G); - + % Obtain velocities time derivatives f(4:6) = acc - 2*cross(omega, x(4:6,1)) - cross(omega,cross(omega,x(1:3,1))); end @@ -66,13 +66,13 @@ xyz = zeros(vert, 3); order = zeros(facets, 3); vol_facet = zeros(facets, 1); - + % Loop through all vertices to fill vertex positions matrix reading % filename lines for i = 1:vert; xyz(i,:) = str2num(fgetl(fid)); end - + % Loop through all facets to fill facets topology matrix reading % filename lines. Also, compute single facet volume and store it. for i = 1:facets; @@ -80,23 +80,23 @@ vol_facet(i,1) = abs(dot(cross(xyz(order(i,1),:), xyz(order(i,2),:)),... xyz(order(i,3),:)))/6; end - + elseif strcmp(ext, '.tab') % Preallocate matrices and vector corresponding to vertex positions, % facets topology and facet polyhedra volume xyz = zeros(vert, 4); order = zeros(facets, 4); vol_facet = zeros(facets, 1); - + % Loop through all vertices to fill vertex positions matrix reading % filename lines for i = 1:vert; xyz(i,:) = str2num(fgetl(fid)); end - + % Extract only vertex positions, not vertex index numeration xyz = xyz(:, 2:4); - + % Loop through all facets to fill facets topology matrix reading % filename lines. Also, compute single facet volume and store it. for i = 1:facets; @@ -104,10 +104,10 @@ vol_facet(i,1) = abs(dot(cross(xyz(order(i,2),:), xyz(order(i,3),:)),... xyz(order(i,4),:)))/6; end - + % Extract only facets vertex index, not facet index numeration order = order(:, 2:4); - + else % Display information on screen if filename format is not adequate fprintf('Error, entered asteroid shape filename format is not .txt or .tab\n') @@ -119,5 +119,3 @@ % Close filename fclose(fid); end - - diff --git a/src/simulation/dynamics/gravityEffector/gravCoeffOps.py b/src/simulation/dynamics/gravityEffector/gravCoeffOps.py index 99feb26a25..f0a4828acd 100755 --- a/src/simulation/dynamics/gravityEffector/gravCoeffOps.py +++ b/src/simulation/dynamics/gravityEffector/gravCoeffOps.py @@ -20,8 +20,8 @@ from Basilisk import __path__ def loadGravFromFile( - fileName: str, - spherHarm: "SphericalHarmonicsGravityModel", + fileName: str, + spherHarm: "SphericalHarmonicsGravityModel", maxDeg: int = 2 ): @@ -57,7 +57,7 @@ def loadGravFromFileToList(fileName: str, maxDeg: int = 2): raise ValueError(f"Requested using Spherical Harmonics of degree {maxDeg}" f", but file '{fileName}' has maximum degree/order of" f"{min(maxDegreeFile, maxOrderFile)}") - + if not coefficientsNormalized: raise ValueError("Coefficients in given file are not normalized. This is " "not currently supported in Basilisk.") @@ -84,8 +84,8 @@ def loadGravFromFileToList(fileName: str, maxDeg: int = 2): slmRow.append(float(gravRow[3])) return [clmList, slmList, mu, radEquator] - - + + def loadPolyFromFile(fileName: str, poly: "PolyhedralGravityModel"): [vertList, faceList, _, _] = loadPolyFromFileToList(fileName) poly.xyzVertex = vertList diff --git a/src/simulation/dynamics/hingedRigidBodyMotor/_UnitTest/test_hingedRigidBodyMotor.py b/src/simulation/dynamics/hingedRigidBodyMotor/_UnitTest/test_hingedRigidBodyMotor.py index 04208a9792..a8e259bd69 100644 --- a/src/simulation/dynamics/hingedRigidBodyMotor/_UnitTest/test_hingedRigidBodyMotor.py +++ b/src/simulation/dynamics/hingedRigidBodyMotor/_UnitTest/test_hingedRigidBodyMotor.py @@ -1,12 +1,12 @@ -# +# # ISC License -# +# # Copyright (c) 2022, 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 @@ -14,8 +14,8 @@ # 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 pytest from Basilisk.architecture import messaging @@ -50,7 +50,7 @@ def test_hingedRigidBodyMotor(show_plots, K, P, sensedTheta, sensedThetaDot, ref accuracy (double): unit text accuracy **Description of Variables Being Tested** - + K and P are varied (note both must be set to positive values). The sensed hinged rigid body state is held constant while the reference is also varied to check positive and negative deltas. """ @@ -89,7 +89,7 @@ def hingedRigidBodyMotorTestFunction(show_plots, K, P, sensedTheta, sensedThetaD # subscribe input messages to module module.hingedBodyStateSensedInMsg.subscribeTo(hingedBodyStateSensedInMsg) module.hingedBodyStateReferenceInMsg.subscribeTo(hingedBodyStateReferenceInMsg) - + module.K = K module.P = P @@ -117,5 +117,3 @@ def hingedRigidBodyMotorTestFunction(show_plots, K, P, sensedTheta, sensedThetaD if __name__ == "__main__": test_hingedRigidBodyMotor(False, 5, 1, 1, .1, 1.2, .2, 1e-12) # first test case above - - diff --git a/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.h b/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.h index 04baf6640e..57b6399fc7 100644 --- a/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.h +++ b/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.h @@ -39,7 +39,7 @@ class HingedRigidBodyMotor: public SysModel { void UpdateState(uint64_t CurrentSimNanos); public: - + double K; //!< gain on theta double P; //!< gain on theta dot diff --git a/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.rst b/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.rst index b2501f5c71..e1619ebf50 100644 --- a/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.rst +++ b/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.rst @@ -4,9 +4,9 @@ Calculates a motor torque given a sensed and reference hinged rigid body state u Message Connection Descriptions ------------------------------- -The following table lists all the module input and output messages. -The module msg connection is set by the user from python. -The msg type contains a link to the message structure definition, while the description +The following table lists all the module input and output messages. +The module msg connection is set by the user from python. +The msg type contains a link to the message structure definition, while the description provides information on what this message is used for. .. list-table:: Module I/O Messages diff --git a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h index 487d95b291..d458fb9775 100644 --- a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h +++ b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h @@ -51,7 +51,7 @@ class linearTranslationOneDOFStateEffector : /** setter for `k` property */ void setK(double k); /** setter for `c` property */ - void setC(double c); + void setC(double c); /** setter for `rhoInit` property */ void setRhoInit(double rhoInit) {this->rhoInit = rhoInit;}; /** setter for `rhoDotInit` property */ diff --git a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.rst b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.rst index e3140dae87..a8bcf13cce 100644 --- a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.rst +++ b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.rst @@ -122,4 +122,3 @@ This section is to outline the steps needed to setup a Translating Body State Ef #. Add the module to the task list:: unitTestSim.AddModelToTask(unitTaskName, translatingBody) - diff --git a/src/simulation/dynamics/msmForceTorque/_UnitTest/Support/msmCheck.nb b/src/simulation/dynamics/msmForceTorque/_UnitTest/Support/msmCheck.nb index 08906aca8f..82e27997ed 100644 --- a/src/simulation/dynamics/msmForceTorque/_UnitTest/Support/msmCheck.nb +++ b/src/simulation/dynamics/msmForceTorque/_UnitTest/Support/msmCheck.nb @@ -26,87 +26,87 @@ Cell[BoxData[ Cell[BoxData[{ RowBox[{ - RowBox[{"spPosList", "=", - RowBox[{"{", "\[IndentingNewLine]", + RowBox[{"spPosList", "=", + RowBox[{"{", "\[IndentingNewLine]", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", - RowBox[{"1", ",", "2", ",", "3"}], "}"}], ",", " ", - RowBox[{"{", - RowBox[{"4", ",", "5", ",", "6"}], "}"}]}], "}"}], ",", - "\[IndentingNewLine]", - RowBox[{"{", + RowBox[{"{", + RowBox[{"1", ",", "2", ",", "3"}], "}"}], ",", " ", + RowBox[{"{", + RowBox[{"4", ",", "5", ",", "6"}], "}"}]}], "}"}], ",", + "\[IndentingNewLine]", + RowBox[{"{", RowBox[{ - RowBox[{"{", - RowBox[{"1", ",", "2", ",", "3"}], "}"}], ",", " ", - RowBox[{"{", - RowBox[{"4", ",", "5", ",", "6"}], "}"}], ",", - RowBox[{"{", - RowBox[{"14", ",", "5", ",", "6"}], "}"}]}], "}"}], ",", - "\[IndentingNewLine]", - RowBox[{"{", + RowBox[{"{", + RowBox[{"1", ",", "2", ",", "3"}], "}"}], ",", " ", + RowBox[{"{", + RowBox[{"4", ",", "5", ",", "6"}], "}"}], ",", + RowBox[{"{", + RowBox[{"14", ",", "5", ",", "6"}], "}"}]}], "}"}], ",", + "\[IndentingNewLine]", + RowBox[{"{", RowBox[{ - RowBox[{"{", - RowBox[{"1", ",", "2", ",", "3"}], "}"}], ",", " ", - RowBox[{"{", - RowBox[{"4", ",", "5", ",", "6"}], "}"}]}], "}"}]}], - "\[IndentingNewLine]", "}"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"1", ",", "2", ",", "3"}], "}"}], ",", " ", + RowBox[{"{", + RowBox[{"4", ",", "5", ",", "6"}], "}"}]}], "}"}]}], + "\[IndentingNewLine]", "}"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"rList", " ", "=", - RowBox[{"{", "\[IndentingNewLine]", + RowBox[{"rList", " ", "=", + RowBox[{"{", "\[IndentingNewLine]", RowBox[{ - RowBox[{"{", - RowBox[{"1", ",", "2"}], "}"}], ",", "\[IndentingNewLine]", - RowBox[{"{", - RowBox[{"1", ",", "2", ",", "1.5"}], "}"}], ",", "\[IndentingNewLine]", - - RowBox[{"{", - RowBox[{"1", ",", "2"}], "}"}]}], "\[IndentingNewLine]", "}"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"1", ",", "2"}], "}"}], ",", "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"1", ",", "2", ",", "1.5"}], "}"}], ",", "\[IndentingNewLine]", + + RowBox[{"{", + RowBox[{"1", ",", "2"}], "}"}]}], "\[IndentingNewLine]", "}"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"numSat", " ", "=", " ", "3"}], ";"}], "\[IndentingNewLine]", + RowBox[{"numSat", " ", "=", " ", "3"}], ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"numSpheres", " ", "=", " ", "7"}], ";"}]}], "Input", CellChangeTimes->{{3.824975502686029*^9, 3.824975611615081*^9}, { - 3.8249759992717857`*^9, 3.8249760017406397`*^9}, {3.824977222905002*^9, + 3.8249759992717857`*^9, 3.8249760017406397`*^9}, {3.824977222905002*^9, 3.824977238801915*^9}}, CellLabel-> "In[572]:=",ExpressionUUID->"cc2d9da0-b7a6-4796-9f86-e5fe32be1bbc"], Cell[BoxData[{ RowBox[{ - RowBox[{"rBNN", " ", "=", " ", - RowBox[{"{", "\[IndentingNewLine]", + RowBox[{"rBNN", " ", "=", " ", + RowBox[{"{", "\[IndentingNewLine]", RowBox[{ - RowBox[{"{", - RowBox[{"10", ",", "2", ",", "3"}], "}"}], ",", "\[IndentingNewLine]", - RowBox[{"{", + RowBox[{"{", + RowBox[{"10", ",", "2", ",", "3"}], "}"}], ",", "\[IndentingNewLine]", + RowBox[{"{", RowBox[{ - RowBox[{"-", "10"}], ",", - RowBox[{"-", "2"}], ",", "3"}], "}"}], ",", "\[IndentingNewLine]", - RowBox[{"{", - RowBox[{"1", ",", "1", ",", "0"}], "}"}]}], "\[IndentingNewLine]", - "}"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"-", "10"}], ",", + RowBox[{"-", "2"}], ",", "3"}], "}"}], ",", "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"1", ",", "1", ",", "0"}], "}"}]}], "\[IndentingNewLine]", + "}"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Sigma]BN", " ", "=", " ", - RowBox[{"{", "\[IndentingNewLine]", + RowBox[{"\[Sigma]BN", " ", "=", " ", + RowBox[{"{", "\[IndentingNewLine]", RowBox[{ - RowBox[{"{", - RowBox[{"0.1", ",", " ", "0.2", ",", " ", "0.3"}], "}"}], ",", - "\[IndentingNewLine]", - RowBox[{"{", + RowBox[{"{", + RowBox[{"0.1", ",", " ", "0.2", ",", " ", "0.3"}], "}"}], ",", + "\[IndentingNewLine]", + RowBox[{"{", RowBox[{ - RowBox[{"-", "0.1"}], ",", " ", "0.2", ",", " ", "0.3"}], "}"}], ",", - "\[IndentingNewLine]", - RowBox[{"{", - RowBox[{"0.1", ",", " ", "0.2", ",", " ", - RowBox[{"-", "0.3"}]}], "}"}]}], "\[IndentingNewLine]", "}"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"-", "0.1"}], ",", " ", "0.2", ",", " ", "0.3"}], "}"}], ",", + "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"0.1", ",", " ", "0.2", ",", " ", + RowBox[{"-", "0.3"}]}], "}"}]}], "\[IndentingNewLine]", "}"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"volt", " ", "=", " ", - RowBox[{"{", - RowBox[{"30000", ",", " ", + RowBox[{"volt", " ", "=", " ", + RowBox[{"{", + RowBox[{"30000", ",", " ", RowBox[{"-", "10000"}], ",", " ", "20000"}], "}"}]}], ";"}]}], "Input", CellChangeTimes->{{3.824975646186*^9, 3.824975647869066*^9}, { 3.82497585464361*^9, 3.824975964104961*^9}}, @@ -117,52 +117,52 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"rSNN", "=", - RowBox[{"{", "}"}]}], ";"}], "\[IndentingNewLine]", - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"rSNN", "=", + RowBox[{"{", "}"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"BN", " ", "=", " ", - RowBox[{"MRP2C", "[", - RowBox[{"\[Sigma]BN", "[", - RowBox[{"[", "i", "]"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"Print", "[", - RowBox[{"\"\\"", " ", ",", - RowBox[{"i", "-", "1"}]}], "]"}], ";", "\[IndentingNewLine]", - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"BN", " ", "=", " ", + RowBox[{"MRP2C", "[", + RowBox[{"\[Sigma]BN", "[", + RowBox[{"[", "i", "]"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"Print", "[", + RowBox[{"\"\\"", " ", ",", + RowBox[{"i", "-", "1"}]}], "]"}], ";", "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"Print", "[", + RowBox[{"Print", "[", RowBox[{ - RowBox[{"rBNN", "[", - RowBox[{"[", "i", "]"}], "]"}], "+", + RowBox[{"rBNN", "[", + RowBox[{"[", "i", "]"}], "]"}], "+", RowBox[{ - RowBox[{"Transpose", "[", "BN", "]"}], ".", - RowBox[{"spPosList", "[", - RowBox[{"[", - RowBox[{"i", ",", "j"}], "]"}], "]"}]}]}], "]"}], ";", - "\[IndentingNewLine]", - RowBox[{"AppendTo", "[", - RowBox[{"rSNN", ",", " ", + RowBox[{"Transpose", "[", "BN", "]"}], ".", + RowBox[{"spPosList", "[", + RowBox[{"[", + RowBox[{"i", ",", "j"}], "]"}], "]"}]}]}], "]"}], ";", + "\[IndentingNewLine]", + RowBox[{"AppendTo", "[", + RowBox[{"rSNN", ",", " ", RowBox[{ - RowBox[{"rBNN", "[", - RowBox[{"[", "i", "]"}], "]"}], "+", + RowBox[{"rBNN", "[", + RowBox[{"[", "i", "]"}], "]"}], "+", RowBox[{ - RowBox[{"Transpose", "[", "BN", "]"}], ".", - RowBox[{"spPosList", "[", - RowBox[{"[", - RowBox[{"i", ",", "j"}], "]"}], "]"}]}]}]}], "]"}], ";"}], - "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"j", ",", - RowBox[{"Length", "[", - RowBox[{"spPosList", "[", - RowBox[{"[", "i", "]"}], "]"}], "]"}]}], "}"}]}], "]"}], ";"}], - "\[IndentingNewLine]", ",", - RowBox[{"{", + RowBox[{"Transpose", "[", "BN", "]"}], ".", + RowBox[{"spPosList", "[", + RowBox[{"[", + RowBox[{"i", ",", "j"}], "]"}], "]"}]}]}]}], "]"}], ";"}], + "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"j", ",", + RowBox[{"Length", "[", + RowBox[{"spPosList", "[", + RowBox[{"[", "i", "]"}], "]"}], "]"}]}], "}"}]}], "]"}], ";"}], + "\[IndentingNewLine]", ",", + RowBox[{"{", RowBox[{"i", ",", "numSat"}], "}"}]}], "]"}]}], "Input", CellChangeTimes->{{3.8249759850522747`*^9, 3.824976285743181*^9}, { - 3.8249763322594967`*^9, 3.824976339020772*^9}, {3.82497637494724*^9, + 3.8249763322594967`*^9, 3.824976339020772*^9}, {3.82497637494724*^9, 3.824976390145536*^9}, {3.824976444391202*^9, 3.8249764590731373`*^9}, { 3.8249765364187527`*^9, 3.824976576960115*^9}}, CellLabel-> @@ -176,32 +176,32 @@ Cell[BoxData[ SequenceForm["SC", 0], Editable->False]], "Print", CellChangeTimes->{ - 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, - 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, + 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, + 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, 3.824976547302734*^9, 3.8249765779051247`*^9, 3.853071620595459*^9}, CellLabel-> "During evaluation of \ In[579]:=",ExpressionUUID->"17c614fa-61de-442e-8523-2fe732cf0c70"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{"11.`", ",", "4.`", ",", "5.999999999999999`"}], "}"}]], "Print", CellChangeTimes->{ - 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, - 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, + 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, + 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, 3.824976547302734*^9, 3.8249765779051247`*^9, 3.853071620597527*^9}, CellLabel-> "During evaluation of \ In[579]:=",ExpressionUUID->"a5fffab9-f128-4795-9c34-4a7f8ad46fde"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - "11.728531855955678`", ",", "8.218836565096952`", ",", + "11.728531855955678`", ",", "8.218836565096952`", ",", "8.944598337950136`"}], "}"}]], "Print", CellChangeTimes->{ - 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, - 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, + 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, + 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, 3.824976547302734*^9, 3.8249765779051247`*^9, 3.8530716206001043`*^9}, CellLabel-> "During evaluation of \ @@ -213,47 +213,47 @@ Cell[BoxData[ SequenceForm["SC", 1], Editable->False]], "Print", CellChangeTimes->{ - 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, - 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, + 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, + 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, 3.824976547302734*^9, 3.8249765779051247`*^9, 3.8530716206089153`*^9}, CellLabel-> "During evaluation of \ In[579]:=",ExpressionUUID->"9647f816-3bce-46ea-b9a2-c846a6529fff"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "10.600492459218222`"}], ",", "1.3419513696522003`", ",", + RowBox[{"-", "10.600492459218222`"}], ",", "1.3419513696522003`", ",", "4.571868267159125`"}], "}"}]], "Print", CellChangeTimes->{ - 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, - 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, + 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, + 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, 3.824976547302734*^9, 3.8249765779051247`*^9, 3.853071620611024*^9}, CellLabel-> "During evaluation of \ In[579]:=",ExpressionUUID->"a29c34f9-d63c-46a2-b149-5c10b5039972"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "11.71868267159126`"}], ",", "6.410280086180363`", ",", + RowBox[{"-", "11.71868267159126`"}], ",", "6.410280086180363`", ",", "4.820252385349336`"}], "}"}]], "Print", CellChangeTimes->{ - 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, - 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, + 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, + 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, 3.824976547302734*^9, 3.8249765779051247`*^9, 3.8530716206129637`*^9}, CellLabel-> "During evaluation of \ In[579]:=",ExpressionUUID->"3818bd2d-fa00-42ec-a82c-da93839636db"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "9.721144967682365`"}], ",", "13.120036934441364`", ",", + RowBox[{"-", "9.721144967682365`"}], ",", "13.120036934441364`", ",", RowBox[{"-", "2.3204062788550335`"}]}], "}"}]], "Print", CellChangeTimes->{ - 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, - 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, + 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, + 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, 3.824976547302734*^9, 3.8249765779051247`*^9, 3.853071620615046*^9}, CellLabel-> "During evaluation of \ @@ -265,34 +265,34 @@ Cell[BoxData[ SequenceForm["SC", 2], Editable->False]], "Print", CellChangeTimes->{ - 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, - 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, + 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, + 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, 3.824976547302734*^9, 3.8249765779051247`*^9, 3.853071620616901*^9}, CellLabel-> "During evaluation of \ In[579]:=",ExpressionUUID->"0ee27f94-cf74-44a6-aed9-103560382617"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"4.068328716528162`", ",", - RowBox[{"-", "0.8042474607571561`"}], ",", "1.1532779316712827`"}], + RowBox[{"{", + RowBox[{"4.068328716528162`", ",", + RowBox[{"-", "0.8042474607571561`"}], ",", "1.1532779316712827`"}], "}"}]], "Print", CellChangeTimes->{ - 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, - 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, + 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, + 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, 3.824976547302734*^9, 3.8249765779051247`*^9, 3.8530716206189117`*^9}, CellLabel-> "During evaluation of \ In[579]:=",ExpressionUUID->"3e8a16d4-6324-4e97-9113-dae1389a3557"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"8.453370267774698`", ",", - RowBox[{"-", "3.5660203139427518`"}], ",", "0.7737765466297306`"}], + RowBox[{"{", + RowBox[{"8.453370267774698`", ",", + RowBox[{"-", "3.5660203139427518`"}], ",", "0.7737765466297306`"}], "}"}]], "Print", CellChangeTimes->{ - 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, - 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, + 3.824976286520595*^9, 3.8249763398938513`*^9, {3.8249763779803457`*^9, + 3.824976401215128*^9}, {3.824976448948619*^9, 3.8249764596281147`*^9}, 3.824976547302734*^9, 3.8249765779051247`*^9, 3.8530716206208467`*^9}, CellLabel-> "During evaluation of \ @@ -308,36 +308,36 @@ Cell[BoxData["rSNN"], "Input", "In[581]:=",ExpressionUUID->"3552c920-1034-4f55-9e72-1eb5ded23668"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", - RowBox[{"11.`", ",", "4.`", ",", "5.999999999999999`"}], "}"}], ",", - RowBox[{"{", + RowBox[{"{", + RowBox[{"11.`", ",", "4.`", ",", "5.999999999999999`"}], "}"}], ",", + RowBox[{"{", RowBox[{ - "11.728531855955678`", ",", "8.218836565096952`", ",", - "8.944598337950136`"}], "}"}], ",", - RowBox[{"{", + "11.728531855955678`", ",", "8.218836565096952`", ",", + "8.944598337950136`"}], "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", "10.600492459218222`"}], ",", "1.3419513696522003`", ",", - "4.571868267159125`"}], "}"}], ",", - RowBox[{"{", + RowBox[{"-", "10.600492459218222`"}], ",", "1.3419513696522003`", ",", + "4.571868267159125`"}], "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", "11.71868267159126`"}], ",", "6.410280086180363`", ",", - "4.820252385349336`"}], "}"}], ",", - RowBox[{"{", + RowBox[{"-", "11.71868267159126`"}], ",", "6.410280086180363`", ",", + "4.820252385349336`"}], "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", "9.721144967682365`"}], ",", "13.120036934441364`", ",", - RowBox[{"-", "2.3204062788550335`"}]}], "}"}], ",", - RowBox[{"{", - RowBox[{"4.068328716528162`", ",", - RowBox[{"-", "0.8042474607571561`"}], ",", "1.1532779316712827`"}], - "}"}], ",", - RowBox[{"{", - RowBox[{"8.453370267774698`", ",", - RowBox[{"-", "3.5660203139427518`"}], ",", "0.7737765466297306`"}], + RowBox[{"-", "9.721144967682365`"}], ",", "13.120036934441364`", ",", + RowBox[{"-", "2.3204062788550335`"}]}], "}"}], ",", + RowBox[{"{", + RowBox[{"4.068328716528162`", ",", + RowBox[{"-", "0.8042474607571561`"}], ",", "1.1532779316712827`"}], + "}"}], ",", + RowBox[{"{", + RowBox[{"8.453370267774698`", ",", + RowBox[{"-", "3.5660203139427518`"}], ",", "0.7737765466297306`"}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{{3.824976232765617*^9, 3.8249762570017233`*^9}, - 3.8249764726406403`*^9, 3.824976549581284*^9, 3.824976579882965*^9, + CellChangeTimes->{{3.824976232765617*^9, 3.8249762570017233`*^9}, + 3.8249764726406403`*^9, 3.824976549581284*^9, 3.824976579882965*^9, 3.8530716206334057`*^9}, CellLabel-> "Out[581]=",ExpressionUUID->"827b3838-c7d1-4427-824b-025ee1feebec"] @@ -346,11 +346,11 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"MatrixForm", "[", - RowBox[{"Transpose", "[", - RowBox[{"MRP2C", "[", - RowBox[{"{", - RowBox[{"0.1", ",", " ", "0.2", ",", " ", "0.3"}], "}"}], "]"}], "]"}], + RowBox[{"MatrixForm", "[", + RowBox[{"Transpose", "[", + RowBox[{"MRP2C", "[", + RowBox[{"{", + RowBox[{"0.1", ",", " ", "0.2", ",", " ", "0.3"}], "}"}], "]"}], "]"}], "]"}]], "Input", CellChangeTimes->{{3.8249768444449863`*^9, 3.824976873009424*^9}}, CellLabel-> @@ -359,24 +359,24 @@ Cell[BoxData[ Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ - {"0.19975377039088937`", + {"0.19975377039088937`", RowBox[{"-", "0.6709756848261003`"}], "0.714065866420437`"}, {"0.9172052939365959`", "0.38442597722376104`", "0.10464758387196055`"}, { - RowBox[{"-", "0.34472145275469374`"}], "0.634041243459526`", + RowBox[{"-", "0.34472145275469374`"}], "0.634041243459526`", "0.6922129886118803`"} }, GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { - Offset[0.7]}, + Offset[0.7]}, Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { - Offset[0.4]}, + Offset[0.4]}, Offset[0.2]}}], "\[NoBreak]", ")"}], - Function[BoxForm`e$, + Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", - CellChangeTimes->{{3.824976850519701*^9, 3.8249768734080963`*^9}, + CellChangeTimes->{{3.824976850519701*^9, 3.8249768734080963`*^9}, 3.853071620649016*^9}, CellLabel-> "Out[582]//MatrixForm=",ExpressionUUID->"9f384698-631b-4bc6-9a43-\ @@ -385,8 +385,8 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ - RowBox[{"kc", " ", "=", " ", - RowBox[{"8.99", "*", + RowBox[{"kc", " ", "=", " ", + RowBox[{"8.99", "*", RowBox[{"10", "^", "9"}]}]}], ";"}]], "Input", CellChangeTimes->{{3.8249771644939413`*^9, 3.824977169563518*^9}}, CellLabel-> @@ -394,93 +394,93 @@ Cell[BoxData[ Cell[BoxData[{ RowBox[{ - RowBox[{"S", " ", "=", " ", - RowBox[{"IdentityMatrix", "[", "numSpheres", "]"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"S", " ", "=", " ", + RowBox[{"IdentityMatrix", "[", "numSpheres", "]"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"V", " ", "=", " ", - RowBox[{"Table", "[", - RowBox[{"0", ",", - RowBox[{"{", - RowBox[{"i", ",", " ", "numSpheres"}], "}"}]}], "]"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"V", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{"0", ",", + RowBox[{"{", + RowBox[{"i", ",", " ", "numSpheres"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"counter", " ", "=", " ", "1"}], ";"}], "\[IndentingNewLine]", + RowBox[{"counter", " ", "=", " ", "1"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ RowBox[{ - RowBox[{"S", "[", - RowBox[{"[", - RowBox[{"counter", ",", " ", "counter"}], "]"}], "]"}], " ", "=", - " ", - RowBox[{"kc", "/", - RowBox[{"rList", "[", - RowBox[{"[", - RowBox[{"c", ",", "k"}], "]"}], "]"}]}]}], ";", - "\[IndentingNewLine]", + RowBox[{"S", "[", + RowBox[{"[", + RowBox[{"counter", ",", " ", "counter"}], "]"}], "]"}], " ", "=", + " ", + RowBox[{"kc", "/", + RowBox[{"rList", "[", + RowBox[{"[", + RowBox[{"c", ",", "k"}], "]"}], "]"}]}]}], ";", + "\[IndentingNewLine]", RowBox[{ - RowBox[{"V", "[", - RowBox[{"[", "counter", "]"}], "]"}], " ", "=", " ", - RowBox[{"volt", "[", - RowBox[{"[", "c", "]"}], "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"counter", "++"}], ";"}], "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"k", ",", - RowBox[{"Length", "[", - RowBox[{"spPosList", "[", - RowBox[{"[", "c", "]"}], "]"}], "]"}]}], "}"}]}], "]"}], ";"}], - "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"c", ",", "numSat"}], "}"}]}], "]"}], - ";"}], "\[IndentingNewLine]", + RowBox[{"V", "[", + RowBox[{"[", "counter", "]"}], "]"}], " ", "=", " ", + RowBox[{"volt", "[", + RowBox[{"[", "c", "]"}], "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"counter", "++"}], ";"}], "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"k", ",", + RowBox[{"Length", "[", + RowBox[{"spPosList", "[", + RowBox[{"[", "c", "]"}], "]"}], "]"}]}], "}"}]}], "]"}], ";"}], + "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"c", ",", "numSat"}], "}"}]}], "]"}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"If", "[", + RowBox[{"If", "[", RowBox[{ - RowBox[{"i", "\[NotEqual]", "j"}], ",", "\[IndentingNewLine]", + RowBox[{"i", "\[NotEqual]", "j"}], ",", "\[IndentingNewLine]", RowBox[{ - RowBox[{"rij", " ", "=", " ", + RowBox[{"rij", " ", "=", " ", RowBox[{ - RowBox[{"rSNN", "[", - RowBox[{"[", "i", "]"}], "]"}], " ", "-", " ", - RowBox[{"rSNN", "[", - RowBox[{"[", "j", "]"}], "]"}]}]}], ";", "\[IndentingNewLine]", - + RowBox[{"rSNN", "[", + RowBox[{"[", "i", "]"}], "]"}], " ", "-", " ", + RowBox[{"rSNN", "[", + RowBox[{"[", "j", "]"}], "]"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{ - RowBox[{"S", "[", - RowBox[{"[", - RowBox[{"i", ",", "j"}], "]"}], "]"}], " ", "=", " ", - RowBox[{"kc", "/", - RowBox[{"Norm", "[", "rij", "]"}]}]}], ";", - "\[IndentingNewLine]", + RowBox[{"S", "[", + RowBox[{"[", + RowBox[{"i", ",", "j"}], "]"}], "]"}], " ", "=", " ", + RowBox[{"kc", "/", + RowBox[{"Norm", "[", "rij", "]"}]}]}], ";", + "\[IndentingNewLine]", RowBox[{ - RowBox[{"S", "[", - RowBox[{"[", - RowBox[{"j", ",", "i"}], "]"}], "]"}], " ", "=", " ", - RowBox[{"S", "[", - RowBox[{"[", - RowBox[{"i", ",", "j"}], "]"}], "]"}]}], ";"}]}], - "\[IndentingNewLine]", "]"}], ";"}], "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"j", ",", "numSpheres"}], "}"}]}], "]"}], ";"}], - "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"i", ",", "numSpheres"}], "}"}]}], "]"}], + RowBox[{"S", "[", + RowBox[{"[", + RowBox[{"j", ",", "i"}], "]"}], "]"}], " ", "=", " ", + RowBox[{"S", "[", + RowBox[{"[", + RowBox[{"i", ",", "j"}], "]"}], "]"}]}], ";"}]}], + "\[IndentingNewLine]", "]"}], ";"}], "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"j", ",", "numSpheres"}], "}"}]}], "]"}], ";"}], + "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"i", ",", "numSpheres"}], "}"}]}], "]"}], ";"}], "\[IndentingNewLine]"}], "Input", CellChangeTimes->{{3.8249772464915037`*^9, 3.824977403209012*^9}, { - 3.824977445972068*^9, 3.8249774755182323`*^9}, {3.824977512334091*^9, + 3.824977445972068*^9, 3.8249774755182323`*^9}, {3.824977512334091*^9, 3.82497752228195*^9}, {3.82498034587582*^9, 3.824980347329908*^9}, { - 3.824980386003771*^9, 3.824980421478362*^9}, {3.824981230742221*^9, + 3.824980386003771*^9, 3.824980421478362*^9}, {3.824981230742221*^9, 3.824981325392461*^9}}, CellLabel-> "In[584]:=",ExpressionUUID->"fe39be31-8850-46b1-9535-0e8409a53fe3"], @@ -496,40 +496,40 @@ Cell[BoxData[ Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ - {"8.99`*^9", "1.7301263066715791`*^9", "4.121919261069541`*^8", - "3.929776135706204`*^8", "3.727211175000547`*^8", + {"8.99`*^9", "1.7301263066715791`*^9", "4.121919261069541`*^8", + "3.929776135706204`*^8", "3.727211175000547`*^8", "9.242068746102201`*^8", "9.421834309979445`*^8"}, - {"1.7301263066715791`*^9", "4.495`*^9", "3.782129307381068`*^8", - "3.7653216671741223`*^8", "3.6369327719335747`*^8", + {"1.7301263066715791`*^9", "4.495`*^9", "3.782129307381068`*^8", + "3.7653216671741223`*^8", "3.6369327719335747`*^8", "6.344223183954302`*^8", "6.111661587808528`*^8"}, - {"4.121919261069541`*^8", "3.782129307381068`*^8", "8.99`*^9", - "1.730126306671579`*^9", "6.574139092902906`*^8", + {"4.121919261069541`*^8", "3.782129307381068`*^8", "8.99`*^9", + "1.730126306671579`*^9", "6.574139092902906`*^8", "5.909012672949091`*^8", "4.486241471633197`*^8"}, - {"3.929776135706204`*^8", "3.7653216671741223`*^8", - "1.730126306671579`*^9", "4.495`*^9", "8.99`*^8", + {"3.929776135706204`*^8", "3.7653216671741223`*^8", + "1.730126306671579`*^9", "4.495`*^9", "8.99`*^8", "5.067496909264666`*^8", "3.93175829475823`*^8"}, - {"3.727211175000547`*^8", "3.6369327719335747`*^8", - "6.574139092902906`*^8", "8.99`*^8", "5.993333333333333`*^9", + {"3.727211175000547`*^8", "3.6369327719335747`*^8", + "6.574139092902906`*^8", "8.99`*^8", "5.993333333333333`*^9", "4.517064628817205`*^8", "3.615397367641068`*^8"}, - {"9.242068746102201`*^8", "6.344223183954302`*^8", - "5.909012672949091`*^8", "5.067496909264666`*^8", + {"9.242068746102201`*^8", "6.344223183954302`*^8", + "5.909012672949091`*^8", "5.067496909264666`*^8", "4.517064628817205`*^8", "8.99`*^9", "1.7301263066715791`*^9"}, - {"9.421834309979445`*^8", "6.111661587808528`*^8", - "4.486241471633197`*^8", "3.93175829475823`*^8", + {"9.421834309979445`*^8", "6.111661587808528`*^8", + "4.486241471633197`*^8", "3.93175829475823`*^8", "3.615397367641068`*^8", "1.7301263066715791`*^9", "4.495`*^9"} }, GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { - Offset[0.7]}, + Offset[0.7]}, Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { - Offset[0.4]}, + Offset[0.4]}, Offset[0.2]}}], "\[NoBreak]", ")"}], - Function[BoxForm`e$, + Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", - CellChangeTimes->{{3.824977406336231*^9, 3.824977416971496*^9}, - 3.82498035708786*^9, 3.824980423933359*^9, 3.824981328486557*^9, + CellChangeTimes->{{3.824977406336231*^9, 3.824977416971496*^9}, + 3.82498035708786*^9, 3.824980423933359*^9, 3.824981328486557*^9, 3.8530716206681557`*^9}, CellLabel-> "Out[589]//MatrixForm=",ExpressionUUID->"3ffb2024-a47d-4438-9540-\ @@ -546,7 +546,7 @@ Cell[BoxData[ Cell[BoxData[ TagBox[ - RowBox[{"(", "\[NoBreak]", + RowBox[{"(", "\[NoBreak]", TagBox[GridBox[{ {"30000"}, {"30000"}, @@ -562,15 +562,15 @@ Cell[BoxData[ GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { - Offset[0.5599999999999999]}, + Offset[0.5599999999999999]}, Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { - Offset[0.4]}, + Offset[0.4]}, Offset[0.2]}}], Column], "\[NoBreak]", ")"}], - Function[BoxForm`e$, + Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", - CellChangeTimes->{3.8249774815291853`*^9, 3.824980380701638*^9, + CellChangeTimes->{3.8249774815291853`*^9, 3.824980380701638*^9, 3.8249804251330547`*^9, 3.853071620680859*^9}, CellLabel-> "Out[590]//MatrixForm=",ExpressionUUID->"91186968-5d18-4024-a8cf-\ @@ -580,7 +580,7 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"q", " ", "=", " ", + RowBox[{"q", " ", "=", " ", RowBox[{ RowBox[{"Inverse", "[", "S", "]"}], ".", "V"}]}]], "Input", CellChangeTimes->{{3.8249814084177322`*^9, 3.824981415458468*^9}}, @@ -588,11 +588,11 @@ Cell[BoxData[ "In[591]:=",ExpressionUUID->"1b45c4e8-2da1-48f2-b38f-2f8aadf212de"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"1.9993237214700567`*^-6", ",", "5.738612475123448`*^-6", ",", - RowBox[{"-", "1.0671493510706592`*^-6"}], ",", - RowBox[{"-", "2.510722990990354`*^-6"}], ",", - RowBox[{"-", "1.940443032839189`*^-6"}], ",", "1.3014825709942323`*^-6", + RowBox[{"{", + RowBox[{"1.9993237214700567`*^-6", ",", "5.738612475123448`*^-6", ",", + RowBox[{"-", "1.0671493510706592`*^-6"}], ",", + RowBox[{"-", "2.510722990990354`*^-6"}], ",", + RowBox[{"-", "1.940443032839189`*^-6"}], ",", "1.3014825709942323`*^-6", ",", "3.2313119388008995`*^-6"}], "}"}]], "Output", CellChangeTimes->{3.82498141592866*^9, 3.853071620688139*^9}, CellLabel-> @@ -602,9 +602,9 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Part", "[", - RowBox[{"q", ",", - RowBox[{"{", + RowBox[{"Part", "[", + RowBox[{"q", ",", + RowBox[{"{", RowBox[{"1", ",", "2"}], "}"}]}], "]"}]], "Input", CellChangeTimes->{{3.853071713122583*^9, 3.853071716537714*^9}, { 3.853072734160725*^9, 3.853072770710881*^9}}, @@ -612,8 +612,8 @@ Cell[BoxData[ "In[602]:=",ExpressionUUID->"ffab7b52-e8b0-494e-a7b1-a3b013e7960f"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"1.9993237214700567`*^-6", ",", "5.738612475123448`*^-6"}], + RowBox[{"{", + RowBox[{"1.9993237214700567`*^-6", ",", "5.738612475123448`*^-6"}], "}"}]], "Output", CellChangeTimes->{3.853072771093698*^9}, CellLabel-> @@ -623,19 +623,19 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Part", "[", - RowBox[{"q", ",", - RowBox[{"{", + RowBox[{"Part", "[", + RowBox[{"q", ",", + RowBox[{"{", RowBox[{"3", ",", "4", ",", "5"}], "}"}]}], "]"}]], "Input", CellChangeTimes->{{3.853071720503511*^9, 3.853071726506631*^9}}, CellLabel-> "In[597]:=",ExpressionUUID->"c2d092c5-063d-44d1-911f-e40f8b999e16"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "1.0671493510706592`*^-6"}], ",", - RowBox[{"-", "2.510722990990354`*^-6"}], ",", + RowBox[{"-", "1.0671493510706592`*^-6"}], ",", + RowBox[{"-", "2.510722990990354`*^-6"}], ",", RowBox[{"-", "1.940443032839189`*^-6"}]}], "}"}]], "Output", CellChangeTimes->{3.853071728272781*^9}, CellLabel-> @@ -645,17 +645,17 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Part", "[", - RowBox[{"q", ",", - RowBox[{"{", + RowBox[{"Part", "[", + RowBox[{"q", ",", + RowBox[{"{", RowBox[{"6", ",", "7"}], "}"}]}], "]"}]], "Input", CellChangeTimes->{{3.853071729310143*^9, 3.853071733401492*^9}}, CellLabel-> "In[598]:=",ExpressionUUID->"8db46d5c-2f56-4744-9ecc-c9b71bdd3aec"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"1.3014825709942323`*^-6", ",", "3.2313119388008995`*^-6"}], + RowBox[{"{", + RowBox[{"1.3014825709942323`*^-6", ",", "3.2313119388008995`*^-6"}], "}"}]], "Output", CellChangeTimes->{3.853071734034212*^9}, CellLabel-> @@ -666,102 +666,102 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"i0", " ", "=", " ", "0"}], ";"}], "\[IndentingNewLine]", + RowBox[{"i0", " ", "=", " ", "0"}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"netForceN", " ", "=", " ", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], ";", "\[IndentingNewLine]", - RowBox[{"netTorqueB", " ", "=", " ", - RowBox[{"{", + RowBox[{"netForceN", " ", "=", " ", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], ";", "\[IndentingNewLine]", + RowBox[{"netTorqueB", " ", "=", " ", + RowBox[{"{", RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"BN", " ", "=", " ", - RowBox[{"MRP2C", "[", - RowBox[{"\[Sigma]BN", "[", - RowBox[{"[", "c", "]"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"i1", " ", "=", " ", - RowBox[{"i0", "+", - RowBox[{"Length", "[", - RowBox[{"rList", "[", - RowBox[{"[", "c", "]"}], "]"}], "]"}]}]}], ";", - "\[IndentingNewLine]", "\[IndentingNewLine]", - RowBox[{"Do", "[", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"BN", " ", "=", " ", + RowBox[{"MRP2C", "[", + RowBox[{"\[Sigma]BN", "[", + RowBox[{"[", "c", "]"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"i1", " ", "=", " ", + RowBox[{"i0", "+", + RowBox[{"Length", "[", + RowBox[{"rList", "[", + RowBox[{"[", "c", "]"}], "]"}], "]"}]}]}], ";", + "\[IndentingNewLine]", "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"forceN", " ", "=", " ", - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], ";", - "\[IndentingNewLine]", - RowBox[{"Do", "[", "\[IndentingNewLine]", + RowBox[{"forceN", " ", "=", " ", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], ";", + "\[IndentingNewLine]", + RowBox[{"Do", "[", "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"If", "[", + RowBox[{"If", "[", RowBox[{ RowBox[{ - RowBox[{"i", "<", - RowBox[{"i0", "+", "1"}]}], " ", "||", " ", - RowBox[{"i", "\[GreaterEqual]", - RowBox[{"i1", "+", "1"}]}]}], ",", "\[IndentingNewLine]", + RowBox[{"i", "<", + RowBox[{"i0", "+", "1"}]}], " ", "||", " ", + RowBox[{"i", "\[GreaterEqual]", + RowBox[{"i1", "+", "1"}]}]}], ",", "\[IndentingNewLine]", RowBox[{ - RowBox[{"rijN", " ", "=", " ", + RowBox[{"rijN", " ", "=", " ", RowBox[{ - RowBox[{"rSNN", "[", - RowBox[{"[", "i", "]"}], "]"}], "-", - RowBox[{"rSNN", "[", - RowBox[{"[", "j", "]"}], "]"}]}]}], ";", - "\[IndentingNewLine]", - RowBox[{"rij", " ", "=", " ", - RowBox[{"Norm", "[", "rijN", "]"}]}], ";", - "\[IndentingNewLine]", "\[IndentingNewLine]", - RowBox[{"forceN", " ", "-=", " ", - RowBox[{"kc", " ", - RowBox[{"q", "[", - RowBox[{"[", "i", "]"}], "]"}], " ", - RowBox[{"q", "[", - RowBox[{"[", "j", "]"}], "]"}], " ", - RowBox[{"rijN", "/", - RowBox[{"rij", "^", "3"}]}]}]}], ";"}]}], - "\[IndentingNewLine]", "]"}], ";"}], "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"i", ",", "numSpheres"}], "}"}]}], "]"}], ";", - "\[IndentingNewLine]", - RowBox[{"Print", "[", - RowBox[{"c", ",", "j"}], "]"}], ";", "\[IndentingNewLine]", - RowBox[{"Print", "[", "forceN", "]"}], ";", "\[IndentingNewLine]", - RowBox[{"netForceN", " ", "+=", " ", "forceN"}], ";", - "\[IndentingNewLine]", - RowBox[{"netTorqueB", " ", "+=", " ", - RowBox[{"Cross", "[", + RowBox[{"rSNN", "[", + RowBox[{"[", "i", "]"}], "]"}], "-", + RowBox[{"rSNN", "[", + RowBox[{"[", "j", "]"}], "]"}]}]}], ";", + "\[IndentingNewLine]", + RowBox[{"rij", " ", "=", " ", + RowBox[{"Norm", "[", "rijN", "]"}]}], ";", + "\[IndentingNewLine]", "\[IndentingNewLine]", + RowBox[{"forceN", " ", "-=", " ", + RowBox[{"kc", " ", + RowBox[{"q", "[", + RowBox[{"[", "i", "]"}], "]"}], " ", + RowBox[{"q", "[", + RowBox[{"[", "j", "]"}], "]"}], " ", + RowBox[{"rijN", "/", + RowBox[{"rij", "^", "3"}]}]}]}], ";"}]}], + "\[IndentingNewLine]", "]"}], ";"}], "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"i", ",", "numSpheres"}], "}"}]}], "]"}], ";", + "\[IndentingNewLine]", + RowBox[{"Print", "[", + RowBox[{"c", ",", "j"}], "]"}], ";", "\[IndentingNewLine]", + RowBox[{"Print", "[", "forceN", "]"}], ";", "\[IndentingNewLine]", + RowBox[{"netForceN", " ", "+=", " ", "forceN"}], ";", + "\[IndentingNewLine]", + RowBox[{"netTorqueB", " ", "+=", " ", + RowBox[{"Cross", "[", RowBox[{ - RowBox[{"spPosList", "[", - RowBox[{"[", - RowBox[{"c", ",", - RowBox[{"j", "-", "i0"}]}], "]"}], "]"}], ",", " ", - RowBox[{"BN", ".", "forceN"}]}], "]"}]}], ";"}], - "\[IndentingNewLine]", "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"j", ",", - RowBox[{"i0", "+", "1"}], ",", " ", "i1"}], "}"}]}], "]"}], ";", - "\[IndentingNewLine]", - RowBox[{"Print", "[", - RowBox[{"\"\\"", ",", " ", "netForceN"}], "]"}], ";", - "\[IndentingNewLine]", - RowBox[{"Print", "[", - RowBox[{"\"\\"", ",", " ", "netTorqueB"}], "]"}], ";", - "\[IndentingNewLine]", "\[IndentingNewLine]", - RowBox[{"i0", "=", "i1"}], ";"}], "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"c", ",", " ", "numSat"}], "}"}]}], "]"}], + RowBox[{"spPosList", "[", + RowBox[{"[", + RowBox[{"c", ",", + RowBox[{"j", "-", "i0"}]}], "]"}], "]"}], ",", " ", + RowBox[{"BN", ".", "forceN"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]", "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"j", ",", + RowBox[{"i0", "+", "1"}], ",", " ", "i1"}], "}"}]}], "]"}], ";", + "\[IndentingNewLine]", + RowBox[{"Print", "[", + RowBox[{"\"\\"", ",", " ", "netForceN"}], "]"}], ";", + "\[IndentingNewLine]", + RowBox[{"Print", "[", + RowBox[{"\"\\"", ",", " ", "netTorqueB"}], "]"}], ";", + "\[IndentingNewLine]", "\[IndentingNewLine]", + RowBox[{"i0", "=", "i1"}], ";"}], "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"c", ",", " ", "numSat"}], "}"}]}], "]"}], ";"}], "\[IndentingNewLine]"}], "Input", CellChangeTimes->{{3.824981478482285*^9, 3.82498148499399*^9}, { - 3.824981566847728*^9, 3.824981940500107*^9}, {3.824982030286585*^9, + 3.824981566847728*^9, 3.824981940500107*^9}, {3.824982030286585*^9, 3.824982106356666*^9}, {3.824982145827045*^9, 3.8249821734727573`*^9}, { - 3.824982287210435*^9, 3.824982368783121*^9}, {3.824982455465069*^9, + 3.824982287210435*^9, 3.824982368783121*^9}, {3.824982455465069*^9, 3.824982459044075*^9}, {3.824982718626686*^9, 3.824982733840366*^9}, { - 3.824982848792452*^9, 3.824982854216054*^9}, {3.824983045171444*^9, + 3.824982848792452*^9, 3.824982854216054*^9}, {3.824983045171444*^9, 3.8249830455513697`*^9}, {3.824983112806713*^9, 3.8249831496223717`*^9}, { 3.824983187617732*^9, 3.8249832307576437`*^9}}, CellLabel-> @@ -775,7 +775,7 @@ Cell[BoxData[ SequenceForm[1, 1], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620700492*^9}, CellLabel-> @@ -783,12 +783,12 @@ Cell[BoxData[ In[592]:=",ExpressionUUID->"fa86e362-25c9-4b81-a60c-63519718e3a7"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - "0.00016936514755194854`", ",", "0.0006547883508395613`", ",", + "0.00016936514755194854`", ",", "0.0006547883508395613`", ",", "0.0004448286222728123`"}], "}"}]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.8530716207025213`*^9}, CellLabel-> @@ -801,7 +801,7 @@ Cell[BoxData[ SequenceForm[1, 2], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620705776*^9}, CellLabel-> @@ -809,12 +809,12 @@ Cell[BoxData[ In[592]:=",ExpressionUUID->"6345c9e6-dfc1-48d1-8a91-01e9812fe02c"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "0.00010454725955974693`"}], ",", "0.0008172636583773597`", + RowBox[{"-", "0.00010454725955974693`"}], ",", "0.0008172636583773597`", ",", "0.0004799775983669273`"}], "}"}]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620707502*^9}, CellLabel-> @@ -823,17 +823,17 @@ In[592]:=",ExpressionUUID->"f97a64db-7056-4596-96b0-a2aeff92d4e9"], Cell[BoxData[ InterpretationBox[ - RowBox[{"\<\"netForceN = \"\>", "\[InvisibleSpace]", - RowBox[{"{", + RowBox[{"\<\"netForceN = \"\>", "\[InvisibleSpace]", + RowBox[{"{", RowBox[{ - "0.00006481788799220161`", ",", "0.001472052009216921`", ",", + "0.00006481788799220161`", ",", "0.001472052009216921`", ",", "0.0009248062206397396`"}], "}"}]}], SequenceForm[ - "netForceN = ", {0.00006481788799220161, 0.001472052009216921, + "netForceN = ", {0.00006481788799220161, 0.001472052009216921, 0.0009248062206397396}], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.8530716207092247`*^9}, CellLabel-> @@ -842,18 +842,18 @@ In[592]:=",ExpressionUUID->"05f92300-252f-46d7-9258-02ba195b1569"], Cell[BoxData[ InterpretationBox[ - RowBox[{"\<\"netTorqueB = \"\>", "\[InvisibleSpace]", - RowBox[{"{", + RowBox[{"\<\"netTorqueB = \"\>", "\[InvisibleSpace]", + RowBox[{"{", RowBox[{ - RowBox[{"-", "0.0026819212957245675`"}], ",", "0.0029528846153595873`", - ",", + RowBox[{"-", "0.0026819212957245675`"}], ",", "0.0029528846153595873`", + ",", RowBox[{"-", "0.0006036867278413309`"}]}], "}"}]}], SequenceForm[ - "netTorqueB = ", {-0.0026819212957245675`, + "netTorqueB = ", {-0.0026819212957245675`, 0.0029528846153595873`, -0.0006036867278413309}], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.8530716207108803`*^9}, CellLabel-> @@ -866,7 +866,7 @@ Cell[BoxData[ SequenceForm[2, 3], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.8530716207132807`*^9}, CellLabel-> @@ -874,11 +874,11 @@ Cell[BoxData[ In[592]:=",ExpressionUUID->"a4379483-f578-4197-88cd-ed436c83d2d9"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"0.0002568836752980422`", ",", "6.588300664925413`*^-6", ",", + RowBox[{"{", + RowBox[{"0.0002568836752980422`", ",", "6.588300664925413`*^-6", ",", RowBox[{"-", "6.186795263680677`*^-6"}]}], "}"}]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620715007*^9}, CellLabel-> @@ -891,7 +891,7 @@ Cell[BoxData[ SequenceForm[2, 4], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620716696*^9}, CellLabel-> @@ -899,12 +899,12 @@ Cell[BoxData[ In[592]:=",ExpressionUUID->"4020b9dc-5517-4431-b3d3-d057df5a0277"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"0.0005149137614968912`", ",", - RowBox[{"-", "0.00009069958212017717`"}], ",", + RowBox[{"{", + RowBox[{"0.0005149137614968912`", ",", + RowBox[{"-", "0.00009069958212017717`"}], ",", RowBox[{"-", "2.842401850829052`*^-7"}]}], "}"}]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620718266*^9}, CellLabel-> @@ -917,7 +917,7 @@ Cell[BoxData[ SequenceForm[2, 5], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620719882*^9}, CellLabel-> @@ -925,12 +925,12 @@ Cell[BoxData[ In[592]:=",ExpressionUUID->"0e7e10a7-0045-4220-979e-5e352d5a6882"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"0.00030002169336654135`", ",", - RowBox[{"-", "0.00015643179581794145`"}], ",", "0.0001166953148740848`"}], + RowBox[{"{", + RowBox[{"0.00030002169336654135`", ",", + RowBox[{"-", "0.00015643179581794145`"}], ",", "0.0001166953148740848`"}], "}"}]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.8530716207214003`*^9}, CellLabel-> @@ -939,17 +939,17 @@ In[592]:=",ExpressionUUID->"ef718f1b-de7b-43c0-b1f7-c15450583465"], Cell[BoxData[ InterpretationBox[ - RowBox[{"\<\"netForceN = \"\>", "\[InvisibleSpace]", - RowBox[{"{", - RowBox[{"0.0010718191301614748`", ",", - RowBox[{"-", "0.0002405430772731932`"}], ",", + RowBox[{"\<\"netForceN = \"\>", "\[InvisibleSpace]", + RowBox[{"{", + RowBox[{"0.0010718191301614748`", ",", + RowBox[{"-", "0.0002405430772731932`"}], ",", "0.00011022427942532121`"}], "}"}]}], SequenceForm[ - "netForceN = ", {0.0010718191301614748`, -0.0002405430772731932, + "netForceN = ", {0.0010718191301614748`, -0.0002405430772731932, 0.00011022427942532121`}], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620723048*^9}, CellLabel-> @@ -958,17 +958,17 @@ In[592]:=",ExpressionUUID->"6cd12fb3-af38-4c99-b94a-f6a5f18121fb"], Cell[BoxData[ InterpretationBox[ - RowBox[{"\<\"netTorqueB = \"\>", "\[InvisibleSpace]", - RowBox[{"{", - RowBox[{"0.006883871045941627`", ",", - RowBox[{"-", "0.0020943789635838378`"}], ",", + RowBox[{"\<\"netTorqueB = \"\>", "\[InvisibleSpace]", + RowBox[{"{", + RowBox[{"0.006883871045941627`", ",", + RowBox[{"-", "0.0020943789635838378`"}], ",", RowBox[{"-", "0.006475443311145492`"}]}], "}"}]}], SequenceForm[ "netTorqueB = ", { 0.006883871045941627, -0.0020943789635838378`, -0.006475443311145492}], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620724647*^9}, CellLabel-> @@ -981,7 +981,7 @@ Cell[BoxData[ SequenceForm[3, 6], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620726247*^9}, CellLabel-> @@ -989,13 +989,13 @@ Cell[BoxData[ In[592]:=",ExpressionUUID->"96155523-8c86-4bee-b1d4-135d992f5dae"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "0.0005317202379214852`"}], ",", - RowBox[{"-", "0.00024935594543734944`"}], ",", + RowBox[{"-", "0.0005317202379214852`"}], ",", + RowBox[{"-", "0.00024935594543734944`"}], ",", RowBox[{"-", "0.0002856285662044265`"}]}], "}"}]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620727811*^9}, CellLabel-> @@ -1008,7 +1008,7 @@ Cell[BoxData[ SequenceForm[3, 7], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620730754*^9}, CellLabel-> @@ -1016,13 +1016,13 @@ Cell[BoxData[ In[592]:=",ExpressionUUID->"d9468372-9030-43f1-ae17-834ae9fda068"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "0.0006049167802321913`"}], ",", - RowBox[{"-", "0.0009821529865063783`"}], ",", + RowBox[{"-", "0.0006049167802321913`"}], ",", + RowBox[{"-", "0.0009821529865063783`"}], ",", RowBox[{"-", "0.0007494019338606343`"}]}], "}"}]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.8530716207326527`*^9}, CellLabel-> @@ -1031,18 +1031,18 @@ In[592]:=",ExpressionUUID->"4ff22d17-673d-4931-aab9-adf3ff0a6e86"], Cell[BoxData[ InterpretationBox[ - RowBox[{"\<\"netForceN = \"\>", "\[InvisibleSpace]", - RowBox[{"{", + RowBox[{"\<\"netForceN = \"\>", "\[InvisibleSpace]", + RowBox[{"{", RowBox[{ - RowBox[{"-", "0.0011366370181536765`"}], ",", - RowBox[{"-", "0.0012315089319437278`"}], ",", + RowBox[{"-", "0.0011366370181536765`"}], ",", + RowBox[{"-", "0.0012315089319437278`"}], ",", RowBox[{"-", "0.001035030500065061`"}]}], "}"}]}], SequenceForm[ "netForceN = ", {-0.0011366370181536765`, -0.0012315089319437278`, \ -0.001035030500065061}], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.8530716207345753`*^9}, CellLabel-> @@ -1051,16 +1051,16 @@ In[592]:=",ExpressionUUID->"c4cbf755-f98e-4981-b290-0a3ddb221ec1"], Cell[BoxData[ InterpretationBox[ - RowBox[{"\<\"netTorqueB = \"\>", "\[InvisibleSpace]", - RowBox[{"{", - RowBox[{"0.005816286242019339`", ",", "0.007876002422047416`", ",", + RowBox[{"\<\"netTorqueB = \"\>", "\[InvisibleSpace]", + RowBox[{"{", + RowBox[{"0.005816286242019339`", ",", "0.007876002422047416`", ",", RowBox[{"-", "0.009866120811810899`"}]}], "}"}]}], SequenceForm[ - "netTorqueB = ", {0.005816286242019339, + "netTorqueB = ", {0.005816286242019339, 0.007876002422047416, -0.009866120811810899}], Editable->False]], "Print", CellChangeTimes->{{3.82498229271428*^9, 3.824982311485003*^9}, { - 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, + 3.82498234469492*^9, 3.824982369522997*^9}, 3.824982460836672*^9, 3.8249827345734997`*^9, {3.8249831387192507`*^9, 3.8249831505288467`*^9}, { 3.824983208295813*^9, 3.824983233965189*^9}, 3.853071620736906*^9}, CellLabel-> @@ -1166,4 +1166,3 @@ Cell[39596, 1051, 767, 16, 24, "Print",ExpressionUUID->"69fa7b4f-7140-4f57-9543- } ] *) - diff --git a/src/simulation/dynamics/msmForceTorque/_UnitTest/test_msmForceTorque.py b/src/simulation/dynamics/msmForceTorque/_UnitTest/test_msmForceTorque.py index eb18ea692a..7f110be973 100644 --- a/src/simulation/dynamics/msmForceTorque/_UnitTest/test_msmForceTorque.py +++ b/src/simulation/dynamics/msmForceTorque/_UnitTest/test_msmForceTorque.py @@ -1,12 +1,12 @@ -# +# # 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 @@ -14,8 +14,8 @@ # 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 pytest from Basilisk.architecture import messaging @@ -168,5 +168,3 @@ def msmForceTorqueTestFunction(show_plots, accuracy): if __name__ == "__main__": test_msmForceTorque(False, 1e-4) - - diff --git a/src/simulation/dynamics/msmForceTorque/msmForceTorque.h b/src/simulation/dynamics/msmForceTorque/msmForceTorque.h index ce47b217c9..22ac697e59 100644 --- a/src/simulation/dynamics/msmForceTorque/msmForceTorque.h +++ b/src/simulation/dynamics/msmForceTorque/msmForceTorque.h @@ -46,7 +46,7 @@ class MsmForceTorque: public SysModel { private: void readMessages(); - + public: std::vector> scStateInMsgs; //!< vector of spacecraft state input messages std::vector> voltInMsgs; //!< vector of voltage input messages diff --git a/src/simulation/dynamics/msmForceTorque/msmForceTorque.rst b/src/simulation/dynamics/msmForceTorque/msmForceTorque.rst index 3a3124bce2..c4f1a77d85 100644 --- a/src/simulation/dynamics/msmForceTorque/msmForceTorque.rst +++ b/src/simulation/dynamics/msmForceTorque/msmForceTorque.rst @@ -4,9 +4,9 @@ This module uses the Multi-Sphere-Method (MSM) to evaluate the mutual electrosta Message Connection Descriptions ------------------------------- -The following table lists all the module input and output messages. -The module msg connection is set by the user from python. -The msg type contains a link to the message structure definition, while the description +The following table lists all the module input and output messages. +The module msg connection is set by the user from python. +The msg type contains a link to the message structure definition, while the description provides information on what this message is used for. .. _ModuleIO_MSM_FORCE_TORQUE: diff --git a/src/simulation/dynamics/reactionWheels/_Documentation/AVS.sty b/src/simulation/dynamics/reactionWheels/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/reactionWheels/_Documentation/AVS.sty +++ b/src/simulation/dynamics/reactionWheels/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/reactionWheels/_Documentation/Basilisk-REACTIONWHEELSTATEEFFECTOR-20170816.tex b/src/simulation/dynamics/reactionWheels/_Documentation/Basilisk-REACTIONWHEELSTATEEFFECTOR-20170816.tex index 2d3d8e66b6..2311cbb0c0 100755 --- a/src/simulation/dynamics/reactionWheels/_Documentation/Basilisk-REACTIONWHEELSTATEEFFECTOR-20170816.tex +++ b/src/simulation/dynamics/reactionWheels/_Documentation/Basilisk-REACTIONWHEELSTATEEFFECTOR-20170816.tex @@ -90,7 +90,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/reactionWheels/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/reactionWheels/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/reactionWheels/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/reactionWheels/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/reactionWheels/_Documentation/RWJitterBackSubstitution/Basilisk-RWJitterBackSub-20161221.tex b/src/simulation/dynamics/reactionWheels/_Documentation/RWJitterBackSubstitution/Basilisk-RWJitterBackSub-20161221.tex index 2f2bd98f47..fcd41b9ba1 100755 --- a/src/simulation/dynamics/reactionWheels/_Documentation/RWJitterBackSubstitution/Basilisk-RWJitterBackSub-20161221.tex +++ b/src/simulation/dynamics/reactionWheels/_Documentation/RWJitterBackSubstitution/Basilisk-RWJitterBackSub-20161221.tex @@ -68,7 +68,7 @@ \subsection{Equations of Motion} The translational equation of motion is not coupled with $\dot{\Omega}$ as seen in the equation below. \begin{equation} m_{\text{sc}} [I_{3 \times 3}]\ddot{\bm r}_{B/N} --m_{\text{sc}} [\tilde{\bm{c}}] \dot{\bm\omega}_{\cal B/N} +-m_{\text{sc}} [\tilde{\bm{c}}] \dot{\bm\omega}_{\cal B/N} = \bm F_{\text{ext}}- 2 m_{\text{sc}} [\tilde{\bm\omega}_{\cal B/N}] \bm c' -m_{\text{sc}} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} \label{eq:Rbddot35} @@ -137,7 +137,7 @@ \subsection{Equations of Motion} \begin{equation} \begin{split} m_{\text{sc}}[\tilde{\bm c}]\ddot{\bm r}_{B/N} +& [I_{\text{sc},B}]\dot{\bm\omega}_{\cal B/N} +\sum\limits_{i=1}^{N}\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)\dot{\Omega}_i - \\=& + \\=& \sum\limits_{i=1}^{N}\Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big)\Big] \\& -[\tilde{\bm\omega}_{\cal B/N}][I_{\text{sc},B}]\bm\omega_{\cal B/N}- [I_{\text{sc},B}]'\bm\omega_{\cal B/N} + \bm{L}_B \end{split} @@ -146,9 +146,9 @@ \subsection{Equations of Motion} The motor torque equation is (note that $J_{12_i} = J_{23_i} = 0$) \begin{multline} \big[m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T\big]\ddot{\bm{r}}_{B/N} + \big[(J_{11_i} + m_{\text{rw}_i} d_i^2)\hat{\bm g}_{\text{s}_i}^T + J_{13_i}\hat{\bm w}_{3_i}^T -m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T [\tilde{\bm r}_{W_i/B}]\big]\dot{\bm\omega}_{\cal B/N} + \big[J_{11_i} + m_{\text{rw}_i} d_i^2\big]\dot{\Omega}_i - \\= - J_{13_i} \omega_{w_{2_i}}\omega_{s_i} + \\= - J_{13_i} \omega_{w_{2_i}}\omega_{s_i} + \omega_{w_{2_i}} \omega_{w_{3_i}} (J_{22_i} - J_{33_i} - m_{\text{rw}_i} d_i^2 - ) + ) -m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm r_{W_i/B} + u_{s_i} \label{eq:motorTorqueFinal} \end{multline} @@ -159,7 +159,7 @@ \subsection{Derivation of Back-Substitution} Solve motor torque equation for $\dot{\Omega}_i$ in terms of $\ddot{\bm{r}}_{B/N}$ and $\dot{\bm\omega}_{\cal B/N}$ \begin{multline} \dot{\Omega}_i -= \frac{-m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T }{J_{11_i} + m_{\text{rw}_i} d_i^2}\ddot{\bm{r}}_{B/N} + \frac{-\big[(J_{11_i} + m_{\text{rw}_i} d_i^2)\hat{\bm g}_{\text{s}_i}^T + J_{13_i}\hat{\bm w}_{3_i}^T -m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T [\tilde{\bm r}_{W_i/B}]\big]}{J_{11_i} + m_{\text{rw}_i} d_i^2}\dot{\bm\omega}_{\cal B/N} += \frac{-m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T }{J_{11_i} + m_{\text{rw}_i} d_i^2}\ddot{\bm{r}}_{B/N} + \frac{-\big[(J_{11_i} + m_{\text{rw}_i} d_i^2)\hat{\bm g}_{\text{s}_i}^T + J_{13_i}\hat{\bm w}_{3_i}^T -m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T [\tilde{\bm r}_{W_i/B}]\big]}{J_{11_i} + m_{\text{rw}_i} d_i^2}\dot{\bm\omega}_{\cal B/N} \\ +\frac{1}{J_{11_i} + m_{\text{rw}_i} d_i^2}\left[\omega_{w_{2_i}} \omega_{w_{3_i}} (J_{22_i} - J_{33_i} - m_{\text{rw}_i} d_i^2 )-J_{13_i} \omega_{w_{2_i}}\omega_{s_i} -m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm r_{W_i/B} + u_{s_i} \right] @@ -186,16 +186,16 @@ \subsection{Derivation of Back-Substitution} \end{equation} Plugging the equation above into Eq.~\eqref{eq:rBddot} and multiplying both sides by $m_\text{sc}$, (plug $\dot{\Omega}_i$ into translation) \begin{multline} -\left[ [I_{3\times3}] +\frac{1}{m_{\text{sc}}}\sum\limits_{i=1}^{N}m_{\text{rw}_i}d_i\hat{\bm{w}}_{3_i}\bm{a}_{\Omega_i}^T \right] \ddot{\bm r}_{B/N} +\left[ -[\tilde{\bm{c}}] + \frac{1}{m_{\text{sc}}}\sum\limits_{i=1}^{N}m_{\text{rw}_i}d_i\hat{\bm{w}}_{3_i}\bm{b}_{\Omega_i}^T \right] \dot{\bm\omega}_{\cal B/N} +\left[ [I_{3\times3}] +\frac{1}{m_{\text{sc}}}\sum\limits_{i=1}^{N}m_{\text{rw}_i}d_i\hat{\bm{w}}_{3_i}\bm{a}_{\Omega_i}^T \right] \ddot{\bm r}_{B/N} +\left[ -[\tilde{\bm{c}}] + \frac{1}{m_{\text{sc}}}\sum\limits_{i=1}^{N}m_{\text{rw}_i}d_i\hat{\bm{w}}_{3_i}\bm{b}_{\Omega_i}^T \right] \dot{\bm\omega}_{\cal B/N} \\= \ddot{\bm r}_{C/N} - 2[\tilde{\bm\omega}_{\cal B/N}]\bm{c}'-[\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} + \frac{1}{m_{\text{sc}}}\sum\limits_{i=1}^{N}m_{\text{rw}_i}d_i\left(\Omega_i^2\hat{\bm{w}}_{2_i}-c_{\Omega_i}\hat{\bm{w}}_{3_i}\right) \end{multline} Moving on to rotation, (plug $\dot{\Omega}_i$ into rotation) \begin{multline} \left[ m_{\text{sc}}[\tilde{\bm c}]+ \sum\limits_{i=1}^{N}\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)\bm{a}_{\Omega_i}^T \right] \ddot{\bm r}_{B/N} -\\ -+ \left[ [I_{\text{sc},B}] + \sum\limits_{i=1}^{N}\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)\bm{b}_{\Omega_i}^T \right] \dot{\bm\omega}_{\cal B/N} -\\= -\sum\limits_{i=1}^{N}\Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big) \\ +\\ ++ \left[ [I_{\text{sc},B}] + \sum\limits_{i=1}^{N}\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)\bm{b}_{\Omega_i}^T \right] \dot{\bm\omega}_{\cal B/N} +\\= +\sum\limits_{i=1}^{N}\Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big) \\ -\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)c_{\Omega_i}\Big] \\ -[\tilde{\bm\omega}_{\cal B/N}][I_{\text{sc},B}]\bm\omega_{\cal B/N}- [I_{\text{sc},B}]'\bm\omega_{\cal B/N} + \bm{L}_B \end{multline} @@ -224,7 +224,7 @@ \subsection{Back-Substitution Contribution Matrices} \end{equation} \begin{multline} -\bm{v}_\text{rot,contr} = \sum\limits_{i=1}^{N} \Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big) \\ +\bm{v}_\text{rot,contr} = \sum\limits_{i=1}^{N} \Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big) \\ -\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)c_{\Omega_i} \Big] \end{multline} diff --git a/src/simulation/dynamics/reactionWheels/_Documentation/RWJitterBackSubstitution/BasiliskReportMemo.cls b/src/simulation/dynamics/reactionWheels/_Documentation/RWJitterBackSubstitution/BasiliskReportMemo.cls index 7096e85b10..2c8157c432 100755 --- a/src/simulation/dynamics/reactionWheels/_Documentation/RWJitterBackSubstitution/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/reactionWheels/_Documentation/RWJitterBackSubstitution/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/reactionWheels/_Documentation/Support/RW_BOE.nb b/src/simulation/dynamics/reactionWheels/_Documentation/Support/RW_BOE.nb index a0137b12b2..58cab68302 100644 --- a/src/simulation/dynamics/reactionWheels/_Documentation/Support/RW_BOE.nb +++ b/src/simulation/dynamics/reactionWheels/_Documentation/Support/RW_BOE.nb @@ -21,23 +21,23 @@ Notebook[{ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"A", " ", "=", " ", - RowBox[{"{", + RowBox[{"A", " ", "=", " ", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"I1", " ", "+", " ", "J_s"}], ",", " ", "J_s"}], "}"}], ",", - RowBox[{"{", + RowBox[{"I1", " ", "+", " ", "J_s"}], ",", " ", "J_s"}], "}"}], ",", + RowBox[{"{", RowBox[{"J_s", ",", " ", "J_s"}], "}"}]}], "}"}]}]], "Input", CellChangeTimes->{{3.712336311949378*^9, 3.712336347364161*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"I1", "+", "J_s"}], ",", "J_s"}], "}"}], ",", - RowBox[{"{", + RowBox[{"I1", "+", "J_s"}], ",", "J_s"}], "}"}], ",", + RowBox[{"{", RowBox[{"J_s", ",", "J_s"}], "}"}]}], "}"}]], "Output", CellChangeTimes->{3.712336348350506*^9}] }, Open ]], @@ -45,17 +45,17 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"b", " ", "=", " ", - RowBox[{"{", + RowBox[{"b", " ", "=", " ", + RowBox[{"{", RowBox[{ - RowBox[{"{", "0", "}"}], ",", + RowBox[{"{", "0", "}"}], ",", RowBox[{"{", "u_s", "}"}]}], "}"}]}]], "Input", CellChangeTimes->{{3.712336371066243*^9, 3.712336383356255*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", "0", "}"}], ",", + RowBox[{"{", "0", "}"}], ",", RowBox[{"{", "u_s", "}"}]}], "}"}]], "Output", CellChangeTimes->{3.712336385680353*^9}] }, Open ]], @@ -63,23 +63,23 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"A_t", " ", "=", " ", + RowBox[{"A_t", " ", "=", " ", RowBox[{ RowBox[{"Inverse", "[", "A", "]"}], ".", "b"}]}]], "Input", CellChangeTimes->{{3.7123363922972603`*^9, 3.7123364138304358`*^9}, { 3.7123364502578173`*^9, 3.712336450499956*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", - RowBox[{"-", - FractionBox["u_s", "I1"]}], "}"}], ",", - RowBox[{"{", + RowBox[{"{", + RowBox[{"-", + FractionBox["u_s", "I1"]}], "}"}], ",", + RowBox[{"{", FractionBox[ RowBox[{ - RowBox[{"(", - RowBox[{"I1", "+", "J_s"}], ")"}], " ", "u_s"}], + RowBox[{"(", + RowBox[{"I1", "+", "J_s"}], ")"}], " ", "u_s"}], RowBox[{"I1", " ", "J_s"}]], "}"}]}], "}"}]], "Output", CellChangeTimes->{3.7123364146740522`*^9, 3.712336451296796*^9}] }, Open ]] @@ -118,4 +118,3 @@ Cell[1870, 71, 372, 12, 104, "Output"] *) (* End of internal cache information *) - diff --git a/src/simulation/dynamics/reactionWheels/_Documentation/bibliography.bib b/src/simulation/dynamics/reactionWheels/_Documentation/bibliography.bib index 1ced21edd8..cc264a4ec5 100755 --- a/src/simulation/dynamics/reactionWheels/_Documentation/bibliography.bib +++ b/src/simulation/dynamics/reactionWheels/_Documentation/bibliography.bib @@ -24,7 +24,7 @@ @book{schaub Publisher = {{AIAA} Education Series}, Title = {Analytical Mechanics of Space Systems}, Year = {2014}} - + @article{OLSSON1998176, title = "Friction Models and Friction Compensation", journal = "European Journal of Control", @@ -36,4 +36,4 @@ @article{OLSSON1998176 doi = "https://doi.org/10.1016/S0947-3580(98)70113-X", url = "http://www.sciencedirect.com/science/article/pii/S094735809870113X", author = "H. Olsson and K.J. Å ström and C. Canudas de Wit and M. Gäfvert and P. Lischinsky", - keywords = "Friction, Friction compensation, Models, Observers, Passivity"} \ No newline at end of file + keywords = "Friction, Friction compensation, Models, Observers, Passivity"} diff --git a/src/simulation/dynamics/reactionWheels/_Documentation/secModelDescription.tex b/src/simulation/dynamics/reactionWheels/_Documentation/secModelDescription.tex index 7c0ff26fb5..5cb21ccec9 100644 --- a/src/simulation/dynamics/reactionWheels/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/reactionWheels/_Documentation/secModelDescription.tex @@ -2,10 +2,10 @@ \section{Model Description} \subsection{Introduction} -This module is modeling a reaction wheel connected to a rigid body hub. The reaction wheel model has three modes that can be ran: balanced wheels, simple jitter, and fully-coupled imbalanced wheels. The balanced wheels option is modeling the reaction wheels as having their principle inertia axes aligned with spin axis, $\hat{\bm g}_s$, and the center of mass of the wheel is coincident with $\hat{\bm g}_s$. This results in the reaction wheel not changing the mass properties of the spacecraft and results in simpler equations. The simple jitter option is approximating the jitter due to mass imbalances by applying an external force and torque to the spacecraft that is proportional to the wheel speeds squared. This is an approximation because in reality this is an internal force and torque. Finally, the fully-coupled mode is modeling reaction wheel imbalance dynamics by modeling the static and dynamic imbalances as internal forces and torques which is physically realistic and allows for energy and momentum conservation. +This module is modeling a reaction wheel connected to a rigid body hub. The reaction wheel model has three modes that can be ran: balanced wheels, simple jitter, and fully-coupled imbalanced wheels. The balanced wheels option is modeling the reaction wheels as having their principle inertia axes aligned with spin axis, $\hat{\bm g}_s$, and the center of mass of the wheel is coincident with $\hat{\bm g}_s$. This results in the reaction wheel not changing the mass properties of the spacecraft and results in simpler equations. The simple jitter option is approximating the jitter due to mass imbalances by applying an external force and torque to the spacecraft that is proportional to the wheel speeds squared. This is an approximation because in reality this is an internal force and torque. Finally, the fully-coupled mode is modeling reaction wheel imbalance dynamics by modeling the static and dynamic imbalances as internal forces and torques which is physically realistic and allows for energy and momentum conservation. + +Figure \ref{fig:scplusrw} shows the frame and variable definitions used for this problem. The formulation involves a rigid hub with its center of mass location labeled as point $B_c$, and $N_\text{rw}$ RWs with their center of mass locations labeled as $W_{c_i}$. The frames being used for this formulation are the body-fixed frame, \frameDefinition{B}, the motor frame of the $i^\text{th}$ RW, $\mathcal{M}_i:\{\hat{\bm m}_{s_i},\hat{\bm m}_{2_i},\hat{\bm m}_{3_i}\}$ which is also body-fixed, and the wheel-fixed frame of the $i^\text{th}$ RW, $\mathcal{W}_i:\{\hat{\bm g}_{s_i},\hat{\bm w}_{2_i},\hat{\bm w}_{3_i}\}$. The dynamics are modeled with respect to the $\mathcal{B}$ frame which can be generally oriented. The $\mathcal{W}_i$ frame is oriented such that the $\hat{\bm g}_{s_i}$ axis is aligned with the RW spin axis which is the same as the motor torque axis $\hat{\bm m}_{s_{i}}$, the $\hat{\bm w}_{2_i}$ axis is perpendicular to $\hat{\bm g}_{s_i}$ and points in the direction towards the RW center of mass $W_{c_i}$. The $\hat{\bm w}_{3_i}$ completes the right hand rule. The $\mathcal{M}_i$ frame is defined as being equal to the $\mathcal{W}_i$ frame at the beginning of the simulation and therefore the $\mathcal{W}_i$ and $\mathcal{M}_i$ frames are offset by an angle, $\theta_i$, about the $\hat{\bm m}_{s_i} = \hat{\bm g}_{s_i}$ axes. -Figure \ref{fig:scplusrw} shows the frame and variable definitions used for this problem. The formulation involves a rigid hub with its center of mass location labeled as point $B_c$, and $N_\text{rw}$ RWs with their center of mass locations labeled as $W_{c_i}$. The frames being used for this formulation are the body-fixed frame, \frameDefinition{B}, the motor frame of the $i^\text{th}$ RW, $\mathcal{M}_i:\{\hat{\bm m}_{s_i},\hat{\bm m}_{2_i},\hat{\bm m}_{3_i}\}$ which is also body-fixed, and the wheel-fixed frame of the $i^\text{th}$ RW, $\mathcal{W}_i:\{\hat{\bm g}_{s_i},\hat{\bm w}_{2_i},\hat{\bm w}_{3_i}\}$. The dynamics are modeled with respect to the $\mathcal{B}$ frame which can be generally oriented. The $\mathcal{W}_i$ frame is oriented such that the $\hat{\bm g}_{s_i}$ axis is aligned with the RW spin axis which is the same as the motor torque axis $\hat{\bm m}_{s_{i}}$, the $\hat{\bm w}_{2_i}$ axis is perpendicular to $\hat{\bm g}_{s_i}$ and points in the direction towards the RW center of mass $W_{c_i}$. The $\hat{\bm w}_{3_i}$ completes the right hand rule. The $\mathcal{M}_i$ frame is defined as being equal to the $\mathcal{W}_i$ frame at the beginning of the simulation and therefore the $\mathcal{W}_i$ and $\mathcal{M}_i$ frames are offset by an angle, $\theta_i$, about the $\hat{\bm m}_{s_i} = \hat{\bm g}_{s_i}$ axes. - A few more key variables in Figure~\ref{fig:scplusrw} need to be defined. The rigid spacecraft structure without the RWs is called the hub. Point $B$ is the origin of the $\mathcal{B}$ frame and is a general body-fixed point that does not have to be identical to the total spacecraft center of mass, nor the rigid hub center of mass $B_{c}$. Point $W_i$ is the origin of the $\mathcal{W}_i$ frame and can also have any location relative to point $B$. Point $C$ is the center of mass of the total spacecraft system including the rigid hub and the RWs. Due to the RW imbalance, the vector $\bm c$, which points from point $B$ to point $C$, will vary as seen by a body-fixed observer. The scalar variable $d_i$ is the center of mass offset of the RW, or the distance from the spin axis, $\hat{\bm g}_{\text{s}_i}$ to $W_{c_i}$. Finally, the inertial frame orientation is defined through $\frameDefinition{N}$, while the origin of the inertial frame is labeled as $N$. \begin{figure}[htbp] @@ -24,7 +24,7 @@ \subsubsection{Balanced Wheels} For balanced wheels, translational equation of motion is not coupled with $\dot{\Omega}$ as seen in the equation below. \begin{equation} m_{\text{sc}} [I_{3 \times 3}]\ddot{\bm r}_{B/N} --m_{\text{sc}} [\tilde{\bm{c}}] \dot{\bm\omega}_{\cal B/N} +-m_{\text{sc}} [\tilde{\bm{c}}] \dot{\bm\omega}_{\cal B/N} = \bm F_{\text{ext}}- 2 m_{\text{sc}} [\tilde{\bm\omega}_{\cal B/N}] \bm c' -m_{\text{sc}} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} \label{eq:Rbddot35} @@ -84,12 +84,12 @@ \subsubsection{Simple Jitter} For simple jitter, like balanced wheels, the translational equation of motion is not coupled with $\dot{\Omega}$ as seen in the equation below, however the jitter does apply a force on the spacecraft. \begin{equation} m_{\text{sc}} [I_{3 \times 3}]\ddot{\bm r}_{B/N} --m_{\text{sc}} [\tilde{\bm{c}}] \dot{\bm\omega}_{\cal B/N} +-m_{\text{sc}} [\tilde{\bm{c}}] \dot{\bm\omega}_{\cal B/N} = \bm F_{\text{ext}}- 2 m_{\text{sc}} [\tilde{\bm\omega}_{\cal B/N}] \bm c' -m_{\text{sc}} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} + U_{s_i}\Omega_i^2\hat{\bm{u}}_i \label{eq:Rbddot35} \end{equation} -The rotational equation of motion is very similar to the balanced wheels EOM but has two additional torques due to the reaction wheel imbalance. +The rotational equation of motion is very similar to the balanced wheels EOM but has two additional torques due to the reaction wheel imbalance. \begin{multline} m_{\text{sc}}[\tilde{\bm{c}}] \ddot{\bm r}_{B/N} +[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} @@ -150,7 +150,7 @@ \subsubsection{Fully-Coupled Jitter} \begin{equation} \begin{split} m_{\text{sc}}[\tilde{\bm c}]\ddot{\bm r}_{B/N} +& [I_{\text{sc},B}]\dot{\bm\omega}_{\cal B/N} +\sum\limits_{i=1}^{N}\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)\dot{\Omega}_i -\\=& +\\=& \sum\limits_{i=1}^{N}\Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big)\Big] \\& -[\tilde{\bm\omega}_{\cal B/N}][I_{\text{sc},B}]\bm\omega_{\cal B/N}- [I_{\text{sc},B}]'\bm\omega_{\cal B/N} + \bm{L}_B \end{split} @@ -159,9 +159,9 @@ \subsubsection{Fully-Coupled Jitter} The motor torque equation is (note that $J_{12_i} = J_{23_i} = 0$) \begin{multline} \big[m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T\big]\ddot{\bm{r}}_{B/N} + \big[(J_{11_i} + m_{\text{rw}_i} d_i^2)\hat{\bm g}_{\text{s}_i}^T + J_{13_i}\hat{\bm w}_{3_i}^T -m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T [\tilde{\bm r}_{W_i/B}]\big]\dot{\bm\omega}_{\cal B/N} + \big[J_{11_i} + m_{\text{rw}_i} d_i^2\big]\dot{\Omega}_i -\\= - J_{13_i} \omega_{w_{2_i}}\omega_{s_i} +\\= - J_{13_i} \omega_{w_{2_i}}\omega_{s_i} + \omega_{w_{2_i}} \omega_{w_{3_i}} (J_{22_i} - J_{33_i} - m_{\text{rw}_i} d_i^2 -) +) -m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm r_{W_i/B} + u_{s_i} \label{eq:motorTorqueFinal} \end{multline} @@ -169,7 +169,7 @@ \subsubsection{Fully-Coupled Jitter} The first step in the back-substitution method is to solve the motor torque equation for $\dot{\Omega}_i$ in terms of $\ddot{\bm{r}}_{B/N}$ and $\dot{\bm\omega}_{\cal B/N}$ \begin{multline} \dot{\Omega}_i -= \frac{-m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T }{J_{11_i} + m_{\text{rw}_i} d_i^2}\ddot{\bm{r}}_{B/N} + \frac{-\big[(J_{11_i} + m_{\text{rw}_i} d_i^2)\hat{\bm g}_{\text{s}_i}^T + J_{13_i}\hat{\bm w}_{3_i}^T -m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T [\tilde{\bm r}_{W_i/B}]\big]}{J_{11_i} + m_{\text{rw}_i} d_i^2}\dot{\bm\omega}_{\cal B/N} += \frac{-m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T }{J_{11_i} + m_{\text{rw}_i} d_i^2}\ddot{\bm{r}}_{B/N} + \frac{-\big[(J_{11_i} + m_{\text{rw}_i} d_i^2)\hat{\bm g}_{\text{s}_i}^T + J_{13_i}\hat{\bm w}_{3_i}^T -m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T [\tilde{\bm r}_{W_i/B}]\big]}{J_{11_i} + m_{\text{rw}_i} d_i^2}\dot{\bm\omega}_{\cal B/N} \\ +\frac{1}{J_{11_i} + m_{\text{rw}_i} d_i^2}\left[\omega_{w_{2_i}} \omega_{w_{3_i}} (J_{22_i} - J_{33_i} - m_{\text{rw}_i} d_i^2 )-J_{13_i} \omega_{w_{2_i}}\omega_{s_i} -m_{\text{rw}_i} d_i \hat{\bm{w}}_{3_i}^T [\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm\omega}_{\cal B/N}] \bm r_{W_i/B} + u_{s_i} \right] @@ -196,16 +196,16 @@ \subsubsection{Fully-Coupled Jitter} \end{equation} Plugging the equation above into Eq.~\eqref{eq:rBddot} and multiplying both sides by $m_\text{sc}$, (plug $\dot{\Omega}_i$ into translation) \begin{multline} -\left[m_\text{sc} [I_{3\times3}] +\sum\limits_{i=1}^{N}m_{\text{rw}_i}d_i\hat{\bm{w}}_{3_i}\bm{a}_{\Omega_i}^T \right] \ddot{\bm r}_{B/N} +\left[ -m_\text{sc} [\tilde{\bm{c}}] + \sum\limits_{i=1}^{N}m_{\text{rw}_i}d_i\hat{\bm{w}}_{3_i}\bm{b}_{\Omega_i}^T \right] \dot{\bm\omega}_{\cal B/N} +\left[m_\text{sc} [I_{3\times3}] +\sum\limits_{i=1}^{N}m_{\text{rw}_i}d_i\hat{\bm{w}}_{3_i}\bm{a}_{\Omega_i}^T \right] \ddot{\bm r}_{B/N} +\left[ -m_\text{sc} [\tilde{\bm{c}}] + \sum\limits_{i=1}^{N}m_{\text{rw}_i}d_i\hat{\bm{w}}_{3_i}\bm{b}_{\Omega_i}^T \right] \dot{\bm\omega}_{\cal B/N} \\= m_\text{sc} \ddot{\bm r}_{C/N} - 2 m_\text{sc} [\tilde{\bm\omega}_{\cal B/N}]\bm{c}'-m_\text{sc} [\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} + \sum\limits_{i=1}^{N}m_{\text{rw}_i}d_i\left(\Omega_i^2\hat{\bm{w}}_{2_i}-c_{\Omega_i}\hat{\bm{w}}_{3_i}\right) \end{multline} Moving on to rotation, (plug $\dot{\Omega}_i$ into rotation) \begin{multline} \left[ m_{\text{sc}}[\tilde{\bm c}]+ \sum\limits_{i=1}^{N}\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)\bm{a}_{\Omega_i}^T \right] \ddot{\bm r}_{B/N} -\\ -+ \left[ [I_{\text{sc},B}] + \sum\limits_{i=1}^{N}\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)\bm{b}_{\Omega_i}^T \right] \dot{\bm\omega}_{\cal B/N} -\\= -\sum\limits_{i=1}^{N}\Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big) \\ +\\ ++ \left[ [I_{\text{sc},B}] + \sum\limits_{i=1}^{N}\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)\bm{b}_{\Omega_i}^T \right] \dot{\bm\omega}_{\cal B/N} +\\= +\sum\limits_{i=1}^{N}\Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big) \\ -\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)c_{\Omega_i}\Big] \\ -[\tilde{\bm\omega}_{\cal B/N}][I_{\text{sc},B}]\bm\omega_{\cal B/N}- [I_{\text{sc},B}]'\bm\omega_{\cal B/N} + \bm{L}_B \end{multline} @@ -232,7 +232,7 @@ \subsubsection{Fully-Coupled Jitter} \end{equation} \begin{multline} -\bm{v}_\text{rot,contr} = \sum\limits_{i=1}^{N} \Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big) \\ +\bm{v}_\text{rot,contr} = \sum\limits_{i=1}^{N} \Big[ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]d_i \Omega_i^2\hat{\bm{w}}_{2_i}-[I_{\text{rw}_i,W_{c_i}}]'\Omega_i \hat{\bm{g}}_{s_i} -[\tilde{\bm\omega}_{\cal B/N}]\Big([I_{\text{rw}_i,W_{c_i}}]\Omega_i \hat{\bm{g}}_{s_i}+ m_{\text{rw}_i}[\tilde{\bm{r}}_{W_{c_i}/B}]\bm{r}'_{W_{c_i}/B}\Big) \\ -\Big([I_{\text{rw}_i,W_{c_i}}]\hat{\bm{g}}_{s_i} + m_{\text{rw}_i}d_i[\tilde{\bm{r}}_{W_{c_i}/B}]\hat{\bm{w}}_{3_i}\Big)c_{\Omega_i} \Big] \end{multline} @@ -255,7 +255,7 @@ \subsection{Friction Model} \label{eq:friction} \end{equation} -In Eq.~\eqref{eq:friction}, $\tau_{f}$ is the friction torque, $\tau_{\text{st}}$ is the static friction magnitude, $\tau_c$ is the coulomb friction magnitude, $\beta_{\text{st}}$ is the Stribeck coefficient which modifies the peakedness of the Stribeck curve, and $c_v$ is the viscous damping coefficient. These variables can also be seen in the variable descriptions in Fig.~\ref{fig:Stribeck}. +In Eq.~\eqref{eq:friction}, $\tau_{f}$ is the friction torque, $\tau_{\text{st}}$ is the static friction magnitude, $\tau_c$ is the coulomb friction magnitude, $\beta_{\text{st}}$ is the Stribeck coefficient which modifies the peakedness of the Stribeck curve, and $c_v$ is the viscous damping coefficient. These variables can also be seen in the variable descriptions in Fig.~\ref{fig:Stribeck}. The Stribeck function is only applicable when the reaction wheel is starting from rest. In contrast, when the reaction wheel starts from a non-zero speed, or has already broken free of static friction term, then the following equation is implemented: @@ -263,4 +263,4 @@ \subsection{Friction Model} \tau_{f} = - \tau_c \text{sgn} (\Omega) - c_v \Omega \label{eq:friction2} \end{equation} -This logic and math is implemented in the reaction wheel dynamics module. +This logic and math is implemented in the reaction wheel dynamics module. diff --git a/src/simulation/dynamics/reactionWheels/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/reactionWheels/_Documentation/secModelFunctions.tex index b9a9ec26d1..099e0e0aea 100644 --- a/src/simulation/dynamics/reactionWheels/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/reactionWheels/_Documentation/secModelFunctions.tex @@ -18,12 +18,12 @@ \section{Model Assumptions and Limitations} \item The reaction wheel is considered a rigid body \item The spin axis is body fixed, therefore does not take into account bearing flexing \item There is no error placed on the torque when converting from the commanded torque to the applied torque - \item For balanced wheels and simple jitter mode the mass properties of the reaction wheels are assumed to be included in the mass and inertia of the rigid body hub, therefore there is zero contributions to the mass properties from the reaction wheels in the dynamics call. - \item For fully-coupled imbalanced wheels mode the mass properties of the reaction wheels are assumed to not be included in the mass and inertia of the rigid body hub. - \item For balanced wheels and simple jitter mode the inertia matrix is assumed to be diagonal with one of it's principle inertia axis equal to the spin axis, and the center of mass of the reaction wheel is coincident with the spin axis. + \item For balanced wheels and simple jitter mode the mass properties of the reaction wheels are assumed to be included in the mass and inertia of the rigid body hub, therefore there is zero contributions to the mass properties from the reaction wheels in the dynamics call. + \item For fully-coupled imbalanced wheels mode the mass properties of the reaction wheels are assumed to not be included in the mass and inertia of the rigid body hub. + \item For balanced wheels and simple jitter mode the inertia matrix is assumed to be diagonal with one of it's principle inertia axis equal to the spin axis, and the center of mass of the reaction wheel is coincident with the spin axis. \item For simple jitter, the parameters that define the static and dynamic imbalances are $U_s$ and $U_d$. \item For fully-coupled imbalanced wheels the inertia off-diagonal terms, $J_{12}$ and $J_{23}$ are equal to zero and the remaining inertia off-diagonal term $J_{13}$ is found through the setting the dynamic imbalance parameter $U_d$: $J_{13} = U_d$. The center of mass offset, $d$, is found using the static imbalance parameter $U_s$: $d = \frac{U_s}{m_{\text{rw}}}$ - \item The friction model is modeling static, Coulomb, and viscous friction. Other higher order effects of friction are not included. + \item The friction model is modeling static, Coulomb, and viscous friction. Other higher order effects of friction are not included. \item The speed saturation model only has one boundary, whereas in some reaction wheels once the speed boundary has been passed, the torque is turned off and won't turn back on until it spins down to another boundary. This model only can turn off and turn on the torque and the same boundary \item The power saturation model assumes a fixed supplied power limit. This supplied power doesn't currently link to the dynamic power modules from the src/simulation/power folder. -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/simulation/dynamics/reactionWheels/_Documentation/secTest.tex b/src/simulation/dynamics/reactionWheels/_Documentation/secTest.tex index 027b6bdad1..2c10941eb0 100644 --- a/src/simulation/dynamics/reactionWheels/_Documentation/secTest.tex +++ b/src/simulation/dynamics/reactionWheels/_Documentation/secTest.tex @@ -33,7 +33,7 @@ \subsection{Fully Coupled Jitter Scenario - Integrated Test} \subsection{BOE Calculation Scenario - Integrated Test} -The BOE for this scenario can be seen in Figure~\ref{fig:BOE}. This involves a rigid body hub connected to a reaction wheel with the spin axis being aligned with both the hub's center of mass and the reaction wheel's center of mass. This problem assumes the hub and reaction wheel are fixed to rotate about the the spin axis and so it is a two degree of freedom problem. The analytical expressions for the angular velocity of the hub, $\omega_1$, the angle of the hub, $\theta$ and the reaction wheel speed, $\Omega$ are shown in Figure~\ref{fig:BOE}. The test sets up Basilisk so that the initial conditions constrain the spacecraft to rotate about the spin axis. The results confirm that the analytical expressions agree with the Basilisk simulation. +The BOE for this scenario can be seen in Figure~\ref{fig:BOE}. This involves a rigid body hub connected to a reaction wheel with the spin axis being aligned with both the hub's center of mass and the reaction wheel's center of mass. This problem assumes the hub and reaction wheel are fixed to rotate about the the spin axis and so it is a two degree of freedom problem. The analytical expressions for the angular velocity of the hub, $\omega_1$, the angle of the hub, $\theta$ and the reaction wheel speed, $\Omega$ are shown in Figure~\ref{fig:BOE}. The test sets up Basilisk so that the initial conditions constrain the spacecraft to rotate about the spin axis. The results confirm that the analytical expressions agree with the Basilisk simulation. \begin{figure}[htbp] \centerline{ @@ -135,13 +135,13 @@ \subsection{Update - Unit Test} \section{Test Parameters} -Since this is an integrated test, the inputs to the test are the physical parameters of the spacecraft along with the initial conditions of the states. These parameters are outlined in Tables~\ref{tab:hub}-~\ref{tab:initial}. Additionally, the error tolerances can be seen in Table~\ref{tab:errortol}. The error tolerances are different depending on the test. The energy-momentum conservation values will normally have an agreement down to 1e-14, but to ensure cross-platform agreement the tolerance was chose to be 1e-10. The position and attitude checks have a tolerance set to 1e-7 and is because 8 significant digits were chosen as the values being compared to. The BOE tests depend on the integration time step but as the time step gets smaller the accuracy gets better. So 1e-8 tolerance was chosen so that a larger time step could be used but still show agreement. The Friction tests give the same numerical outputs down to ~1e-15 between python and Basilisk, but 1e-10 was chosen to ensure cross platform agreement. Finally, the saturation and minimum torque tests have 1e-10 to ensure cross-platform success, but these values will typically agree to machine precision. +Since this is an integrated test, the inputs to the test are the physical parameters of the spacecraft along with the initial conditions of the states. These parameters are outlined in Tables~\ref{tab:hub}-~\ref{tab:initial}. Additionally, the error tolerances can be seen in Table~\ref{tab:errortol}. The error tolerances are different depending on the test. The energy-momentum conservation values will normally have an agreement down to 1e-14, but to ensure cross-platform agreement the tolerance was chose to be 1e-10. The position and attitude checks have a tolerance set to 1e-7 and is because 8 significant digits were chosen as the values being compared to. The BOE tests depend on the integration time step but as the time step gets smaller the accuracy gets better. So 1e-8 tolerance was chosen so that a larger time step could be used but still show agreement. The Friction tests give the same numerical outputs down to ~1e-15 between python and Basilisk, but 1e-10 was chosen to ensure cross platform agreement. Finally, the saturation and minimum torque tests have 1e-10 to ensure cross-platform success, but these values will typically agree to machine precision. \begin{table}[htbp] \caption{Spacecraft Hub Parameters for Energy Momentum Conservation Scenarios} \label{tab:hub} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -161,7 +161,7 @@ \section{Test Parameters} \caption{Reaction Wheel 1 Parameters for Energy Momentum Conservation Scenarios} \label{tab:rw1} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -181,7 +181,7 @@ \section{Test Parameters} \caption{Reaction Wheel 2 Parameters for Energy Momentum Conservation Scenarios} \label{tab:rw2} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -201,7 +201,7 @@ \section{Test Parameters} \caption{Reaction Wheel 3 Parameters for Energy Momentum Conservation Scenarios} \label{tab:rw3} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -221,7 +221,7 @@ \section{Test Parameters} \caption{Reaction wheel 1 parameters for friction tests} \label{tab:rwFriction} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -239,7 +239,7 @@ \section{Test Parameters} \caption{Reaction wheel 2 parameters for friction tests} \label{tab:rwFriction2} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -257,7 +257,7 @@ \section{Test Parameters} \caption{Initial Conditions for Energy Momentum Conservation Scenarios} \label{tab:initial} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -265,7 +265,7 @@ \section{Test Parameters} (RW 2) OmegaInit & (RW 2) Initial $\Omega$ & 200 & RPM \\ (RW 3) OmegaInit & (RW 3) Initial $\Omega$ & -150 & RPM \\ r\_CN\_NInit & Initial Position of S/C & $\begin{bmatrix} - -4020339 & 7490567 & 5248299 + -4020339 & 7490567 & 5248299 \end{bmatrix}^T$ & m \\ v\_CN\_NInit & Initial Velocity of S/C & $\begin{bmatrix} -5199.78 & -3436.68 & 1041.58 @@ -284,7 +284,7 @@ \section{Test Parameters} \caption{Error Tolerance - Note: Relative Tolerance is $\textnormal{abs}(\frac{\textnormal{truth} - \textnormal{value}}{\textnormal{truth}}$)} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c |} % Column formatting, + \begin{tabular}{| c | c |} % Column formatting, \hline Test & Relative Tolerance \\ \hline @@ -299,7 +299,7 @@ \section{Test Parameters} Saturation Tests & 1e-10 \\ \hline Minimum Torque & 1e-10 \\ - \hline + \hline \end{tabular} \end{table} @@ -355,11 +355,11 @@ \subsection{Simple Jitter, Saturation and Minimum Torque Tests Results} \caption{Test results.} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline \textbf{Test} & \textbf{Pass/Fail} \\ \hline Simple Jitter & \input{AutoTeX/JitterSimplePassFail} \\ - Saturation & \input{AutoTeX/saturationPassFail} \\ + Saturation & \input{AutoTeX/saturationPassFail} \\ Minimum Torque & \input{AutoTeX/minimumPassFail} \\ \hline \end{tabular} \end{table} diff --git a/src/simulation/dynamics/reactionWheels/_Documentation/secUserGuide.tex b/src/simulation/dynamics/reactionWheels/_Documentation/secUserGuide.tex index 1505752e24..940fbde134 100644 --- a/src/simulation/dynamics/reactionWheels/_Documentation/secUserGuide.tex +++ b/src/simulation/dynamics/reactionWheels/_Documentation/secUserGuide.tex @@ -6,7 +6,7 @@ \section{User Guide} \item Import the reactionWheelStateEffector class, the spacecraft class, and the simIncludeRW python module: \newline \textit{import reactionWheelStateEffector}, \textit{import spacecraft} and \textit{import simIncludeRW} \item Define an instantiation of a rwFactory: \newline \textit{rwFactory = simIncludeRW.rwFactory()} - \item Create a reaction wheel (Honeywell HR16 as an example): \newline + \item Create a reaction wheel (Honeywell HR16 as an example): \newline \textit{rwFactory.create()} \newline \textit{'Honeywell\_HR16'} \newline \textit{,[1,0,0]} \newline @@ -14,7 +14,7 @@ \section{User Guide} \textit{,rWB\_B = [0.1,0.,0.]} \newline \textit{,maxMomentum = varMaxMomentum} \newline \textit{,RWModel= varRWModel}) - \item To include stribeck friction effects, include \textit{ betaStatic } within the rwFactory.create() and assign it a non-zero value. By default, betaStatic is set to -1 and stribeck friction is ignored. + \item To include stribeck friction effects, include \textit{ betaStatic } within the rwFactory.create() and assign it a non-zero value. By default, betaStatic is set to -1 and stribeck friction is ignored. \item To limit reaction wheel power, include \textit{maxPower} within the rwFactory.create() and assign it to a non-zero value. By default, power limiting is ignored. \item Create an instantiation of a reaction wheel state effector: \newline \textit{rws = reactionWheelStateEffector.ReactionWheelStateEffector()} \item Create an instantiation of a spacecraft: \newline diff --git a/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_RWUpdate.py b/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_RWUpdate.py index d7cd22929c..03effde941 100644 --- a/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_RWUpdate.py +++ b/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_RWUpdate.py @@ -55,8 +55,8 @@ def test_RWUpdate(show_plots, accuracy): The objective of this script is to test the functionality of changing the reaction wheel (RW) characteristics while the simulation is running. It starts by testing the initial setup, and then does four additional tests: the first - two change the maximum allowed torque, the third one changes the maximum power limit, and the final one changes the - current wheel speeds and maximum allowed wheel speeds. All these tests rely on the fact that, when a maximum or + two change the maximum allowed torque, the third one changes the maximum power limit, and the final one changes the + current wheel speeds and maximum allowed wheel speeds. All these tests rely on the fact that, when a maximum or minimum value is surpassed, the applied torque is capped accordingly. As this test script is not parameterized, only one version of this script will run. diff --git a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/AVS.sty b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/AVS.sty +++ b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/Basilisk-SPACECRAFT-20170808.tex b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/Basilisk-SPACECRAFT-20170808.tex index 31efcb3a1c..7bd7597337 100755 --- a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/Basilisk-SPACECRAFT-20170808.tex +++ b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/Basilisk-SPACECRAFT-20170808.tex @@ -97,7 +97,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/BasiliskReportMemo.cls b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/Support/PlanarFlexibleDynamicsDerivation.nb b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/Support/PlanarFlexibleDynamicsDerivation.nb index 37b87f117c..20aba4294a 100644 --- a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/Support/PlanarFlexibleDynamicsDerivation.nb +++ b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/Support/PlanarFlexibleDynamicsDerivation.nb @@ -22,669 +22,669 @@ Notebook[{ Cell[CellGroupData[{ Cell["Planar Flexible Dynamics with two solar panels", "Section", CellChangeTimes->{{3.6476211926645107`*^9, 3.6476212207444983`*^9}, { - 3.647621411462695*^9, 3.647621435484638*^9}, {3.6476218320778303`*^9, + 3.647621411462695*^9, 3.647621435484638*^9}, {3.6476218320778303`*^9, 3.6476218344203043`*^9}, {3.649439747948255*^9, 3.649439759068905*^9}}], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Rhub", " ", "=", " ", - RowBox[{"{", + RowBox[{"Rhub", " ", "=", " ", + RowBox[{"{", RowBox[{ - RowBox[{"xHub", "[", "t", "]"}], ",", " ", + RowBox[{"xHub", "[", "t", "]"}], ",", " ", RowBox[{"yHub", "[", "t", "]"}]}], "}"}]}]], "Input", CellChangeTimes->{{3.6476077582392817`*^9, 3.64760777925314*^9}, { - 3.647615079246727*^9, 3.647615079629671*^9}, {3.647621147852172*^9, + 3.647615079246727*^9, 3.647615079629671*^9}, {3.647621147852172*^9, 3.647621148259598*^9}, {3.649439738791636*^9, 3.649439776241705*^9}, { - 3.7096453803292313`*^9, 3.709645383457893*^9}, {3.70964542147725*^9, + 3.7096453803292313`*^9, 3.709645383457893*^9}, {3.70964542147725*^9, 3.7096454318767157`*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"xHub", "[", "t", "]"}], ",", + RowBox[{"xHub", "[", "t", "]"}], ",", RowBox[{"yHub", "[", "t", "]"}]}], "}"}]], "Output", - CellChangeTimes->{3.7096454421264477`*^9, 3.7098930254953527`*^9, - 3.70990443182268*^9, 3.709912186034545*^9, 3.709991565698237*^9, + CellChangeTimes->{3.7096454421264477`*^9, 3.7098930254953527`*^9, + 3.70990443182268*^9, 3.709912186034545*^9, 3.709991565698237*^9, 3.71006682731984*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"VHub", " ", "=", " ", - RowBox[{"D", "[", + RowBox[{"VHub", " ", "=", " ", + RowBox[{"D", "[", RowBox[{"Rhub", ",", "t"}], "]"}]}]], "Input", CellChangeTimes->{{3.647608152658317*^9, 3.6476081672650843`*^9}, { - 3.6494398162615347`*^9, 3.649439828455247*^9}, {3.7096466475466137`*^9, + 3.6494398162615347`*^9, 3.649439828455247*^9}, {3.7096466475466137`*^9, 3.7096466482166*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], ",", + MultilineFunction->None], "[", "t", "]"}], ",", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], "}"}]], "Output", CellChangeTimes->{ - 3.647608169805852*^9, {3.647614323833295*^9, 3.64761434783421*^9}, - 3.647615089563908*^9, 3.647621228054256*^9, {3.649439822196238*^9, - 3.649439831862186*^9}, 3.650026329921008*^9, 3.650026435544567*^9, - 3.650113943872491*^9, 3.6501140768873177`*^9, 3.650114124028068*^9, - 3.650121181032531*^9, 3.6501254656908484`*^9, 3.650126350692739*^9, - 3.650126432976297*^9, 3.650126661304862*^9, 3.6501271224543867`*^9, - 3.709645396421109*^9, 3.7096454641959667`*^9, 3.7096466511650953`*^9, - 3.7098930294802837`*^9, 3.709904431894704*^9, 3.709912186140971*^9, + 3.647608169805852*^9, {3.647614323833295*^9, 3.64761434783421*^9}, + 3.647615089563908*^9, 3.647621228054256*^9, {3.649439822196238*^9, + 3.649439831862186*^9}, 3.650026329921008*^9, 3.650026435544567*^9, + 3.650113943872491*^9, 3.6501140768873177`*^9, 3.650114124028068*^9, + 3.650121181032531*^9, 3.6501254656908484`*^9, 3.650126350692739*^9, + 3.650126432976297*^9, 3.650126661304862*^9, 3.6501271224543867`*^9, + 3.709645396421109*^9, 3.7096454641959667`*^9, 3.7096466511650953`*^9, + 3.7098930294802837`*^9, 3.709904431894704*^9, 3.709912186140971*^9, 3.709991565807317*^9, 3.710066827425913*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP1Hub", " ", "=", " ", - RowBox[{"{", + RowBox[{"RSP1Hub", " ", "=", " ", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge1", "*", - RowBox[{"Cos", "[", + RowBox[{"Rhinge1", "*", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], - " ", "+", " ", - RowBox[{"d1", "*", " ", - RowBox[{"Cos", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], + " ", "+", " ", + RowBox[{"d1", "*", " ", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", - - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", " ", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", + + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", " ", RowBox[{ - RowBox[{"Rhinge1", "*", - RowBox[{"Sin", "[", + RowBox[{"Rhinge1", "*", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], - " ", "+", " ", - RowBox[{"d1", "*", " ", - RowBox[{"Sin", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta1"}], "]"}]}], + " ", "+", " ", + RowBox[{"d1", "*", " ", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", - + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH1", "+", " ", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]}]], "Input", CellChangeTimes->{{3.649439849206862*^9, 3.649439917339305*^9}, { - 3.649440015026457*^9, 3.649440104067552*^9}, {3.709645886898323*^9, + 3.649440015026457*^9, 3.649440104067552*^9}, {3.709645886898323*^9, 3.709645916521826*^9}, {3.7096459487470293`*^9, 3.709646004845886*^9}, { - 3.709646048343261*^9, 3.709646092722971*^9}, {3.709646222601664*^9, + 3.709646048343261*^9, 3.709646092722971*^9}, {3.709646222601664*^9, 3.709646380970314*^9}, {3.709893035295415*^9, 3.709893153573711*^9}, { 3.709904347427107*^9, 3.7099043599351263`*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ",", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]], "Output", - CellChangeTimes->{3.649440107978813*^9, 3.650026329938651*^9, - 3.650026435571876*^9, 3.650113943890592*^9, 3.650114076912006*^9, - 3.6501141240599203`*^9, 3.650121181063718*^9, 3.6501254657141533`*^9, - 3.65012635072224*^9, 3.650126432995634*^9, 3.650126661323832*^9, - 3.650127122474761*^9, 3.709646389923583*^9, 3.7098931733877287`*^9, - 3.7099043619306583`*^9, 3.709904431944851*^9, 3.709912186205373*^9, + CellChangeTimes->{3.649440107978813*^9, 3.650026329938651*^9, + 3.650026435571876*^9, 3.650113943890592*^9, 3.650114076912006*^9, + 3.6501141240599203`*^9, 3.650121181063718*^9, 3.6501254657141533`*^9, + 3.65012635072224*^9, 3.650126432995634*^9, 3.650126661323832*^9, + 3.650127122474761*^9, 3.709646389923583*^9, 3.7098931733877287`*^9, + 3.7099043619306583`*^9, 3.709904431944851*^9, 3.709912186205373*^9, 3.7099915659045143`*^9, 3.710066827499762*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP1", " ", "=", " ", + RowBox[{"RSP1", " ", "=", " ", RowBox[{"Rhub", " ", "+", " ", "RSP1Hub"}]}]], "Input", CellChangeTimes->{{3.709646395997994*^9, 3.709646412127426*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"xHub", "[", "t", "]"}]}], ",", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"xHub", "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", RowBox[{"yHub", "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.709646415182599*^9, 3.709893180581501*^9, - 3.709904365017598*^9, 3.709904432007655*^9, 3.7099121862900763`*^9, + CellChangeTimes->{3.709646415182599*^9, 3.709893180581501*^9, + 3.709904365017598*^9, 3.709904432007655*^9, 3.7099121862900763`*^9, 3.70999156597033*^9, 3.710066827566654*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP2Hub", " ", "=", " ", - RowBox[{"{", + RowBox[{"RSP2Hub", " ", "=", " ", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge2", "*", - RowBox[{"Cos", "[", + RowBox[{"Rhinge2", "*", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], - " ", "+", " ", - RowBox[{"d2", "*", " ", - RowBox[{"Cos", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], + " ", "+", " ", + RowBox[{"d2", "*", " ", + RowBox[{"Cos", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", - - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", " ", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", + + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", " ", RowBox[{ - RowBox[{"Rhinge2", "*", - RowBox[{"Sin", "[", + RowBox[{"Rhinge2", "*", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], - " ", "+", " ", - RowBox[{"d2", "*", " ", - RowBox[{"Sin", "[", + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "beta2"}], "]"}]}], + " ", "+", " ", + RowBox[{"d2", "*", " ", + RowBox[{"Sin", "[", RowBox[{ - RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", - + RowBox[{"theta", "[", "t", "]"}], " ", "+", " ", "thetaH2", "+", " ", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]}]], "Input", CellChangeTimes->{{3.709646463873159*^9, 3.709646543110506*^9}, { - 3.70989319908799*^9, 3.709893260993504*^9}, {3.709904369663701*^9, + 3.70989319908799*^9, 3.709893260993504*^9}, {3.709904369663701*^9, 3.709904375505327*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ",", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}]}], "}"}]], "Output", - CellChangeTimes->{{3.709646522302372*^9, 3.709646546358492*^9}, - 3.709893305041185*^9, 3.709904376632481*^9, 3.709904432100214*^9, + CellChangeTimes->{{3.709646522302372*^9, 3.709646546358492*^9}, + 3.709893305041185*^9, 3.709904376632481*^9, 3.709904432100214*^9, 3.709912186356444*^9, 3.709991566037938*^9, 3.710066827630702*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"RSP2", " ", "=", " ", + RowBox[{"RSP2", " ", "=", " ", RowBox[{"Rhub", " ", "+", " ", "RSP2Hub"}]}]], "Input", CellChangeTimes->{{3.709646526347693*^9, 3.709646553485578*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"xHub", "[", "t", "]"}]}], ",", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"xHub", "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}], "+", RowBox[{"yHub", "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.7096465578787622`*^9, 3.709893308250244*^9, - 3.709904379426956*^9, 3.70990443217269*^9, 3.709912186422678*^9, + CellChangeTimes->{3.7096465578787622`*^9, 3.709893308250244*^9, + 3.709904379426956*^9, 3.70990443217269*^9, 3.709912186422678*^9, 3.709991566104603*^9, 3.7100668277002287`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"VSP1", " ", "=", " ", - RowBox[{"D", "[", + RowBox[{"VSP1", " ", "=", " ", + RowBox[{"D", "[", RowBox[{"RSP1", ",", "t"}], "]"}]}]], "Input", CellChangeTimes->{{3.649440115855461*^9, 3.649440124808634*^9}, { 3.709646572311063*^9, 3.70964658152712*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ",", + MultilineFunction->None], "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.649440131399378*^9, 3.650026329966713*^9, - 3.650026435604875*^9, 3.650113943920773*^9, 3.650114076942313*^9, - 3.6501141240781507`*^9, 3.650121181083191*^9, 3.6501254657517776`*^9, - 3.650126350757868*^9, 3.6501264330338707`*^9, 3.6501266613621817`*^9, - 3.650127122516333*^9, 3.709646585406389*^9, 3.709893320897305*^9, - 3.70990438813186*^9, 3.70990443223833*^9, 3.709912186471583*^9, + CellChangeTimes->{3.649440131399378*^9, 3.650026329966713*^9, + 3.650026435604875*^9, 3.650113943920773*^9, 3.650114076942313*^9, + 3.6501141240781507`*^9, 3.650121181083191*^9, 3.6501254657517776`*^9, + 3.650126350757868*^9, 3.6501264330338707`*^9, 3.6501266613621817`*^9, + 3.650127122516333*^9, 3.709646585406389*^9, 3.709893320897305*^9, + 3.70990438813186*^9, 3.70990443223833*^9, 3.709912186471583*^9, 3.7099915661712847`*^9, 3.710066827767516*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"VSP2", " ", "=", " ", - RowBox[{"D", "[", + RowBox[{"VSP2", " ", "=", " ", + RowBox[{"D", "[", RowBox[{"RSP2", ",", "t"}], "]"}]}]], "Input", CellChangeTimes->{{3.649440262161566*^9, 3.64944026549428*^9}, { 3.709646616832868*^9, 3.709646621688724*^9}}], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ",", + MultilineFunction->None], "[", "t", "]"}]}], ",", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], "}"}]], "Output", - CellChangeTimes->{3.649440268747218*^9, 3.650026330022114*^9, - 3.650026435667726*^9, 3.650113943975575*^9, 3.650114076989862*^9, - 3.6501141241372833`*^9, 3.650121181143683*^9, 3.650125465840637*^9, - 3.650126350821439*^9, 3.650126433110935*^9, 3.650126661418356*^9, - 3.650127122571417*^9, 3.709646629412587*^9, 3.709893323420001*^9, - 3.7099043906789207`*^9, 3.7099044323057747`*^9, 3.70991218652265*^9, + CellChangeTimes->{3.649440268747218*^9, 3.650026330022114*^9, + 3.650026435667726*^9, 3.650113943975575*^9, 3.650114076989862*^9, + 3.6501141241372833`*^9, 3.650121181143683*^9, 3.650125465840637*^9, + 3.650126350821439*^9, 3.650126433110935*^9, 3.650126661418356*^9, + 3.650127122571417*^9, 3.709646629412587*^9, 3.709893323420001*^9, + 3.7099043906789207`*^9, 3.7099044323057747`*^9, 3.70991218652265*^9, 3.7099915662376337`*^9, 3.710066827832479*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"T", " ", "=", " ", + RowBox[{"T", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"1", "/", "2"}], "*", "mHub", "*", - RowBox[{"Dot", "[", - RowBox[{"VHub", ",", "VHub"}], "]"}]}], "+", + RowBox[{"1", "/", "2"}], "*", "mHub", "*", + RowBox[{"Dot", "[", + RowBox[{"VHub", ",", "VHub"}], "]"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "IHub", "*", + RowBox[{"1", "/", "2"}], "*", "IHub", "*", RowBox[{ RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}], "^", "2"}]}], "+", + RowBox[{"theta", "'"}], "[", "t", "]"}], "^", "2"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "mSP1", "*", - RowBox[{"Dot", "[", - RowBox[{"VSP1", ",", "VSP1"}], "]"}]}], "+", + RowBox[{"1", "/", "2"}], "*", "mSP1", "*", + RowBox[{"Dot", "[", + RowBox[{"VSP1", ",", "VSP1"}], "]"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "ISP1", "*", + RowBox[{"1", "/", "2"}], "*", "ISP1", "*", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}], "+", + RowBox[{"theta", "'"}], "[", "t", "]"}], "+", RowBox[{ - RowBox[{"theta1", "'"}], "[", "t", "]"}]}], ")"}], "^", "2"}]}], "+", - + RowBox[{"theta1", "'"}], "[", "t", "]"}]}], ")"}], "^", "2"}]}], "+", + RowBox[{ - RowBox[{"1", "/", "2"}], "*", "mSP2", "*", - RowBox[{"Dot", "[", - RowBox[{"VSP2", ",", "VSP2"}], "]"}]}], "+", + RowBox[{"1", "/", "2"}], "*", "mSP2", "*", + RowBox[{"Dot", "[", + RowBox[{"VSP2", ",", "VSP2"}], "]"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "*", "ISP2", "*", + RowBox[{"1", "/", "2"}], "*", "ISP2", "*", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}], "+", + RowBox[{"theta", "'"}], "[", "t", "]"}], "+", RowBox[{ - RowBox[{"theta2", "'"}], "[", "t", "]"}]}], ")"}], "^", + RowBox[{"theta2", "'"}], "[", "t", "]"}]}], ")"}], "^", "2"}]}]}]}]], "Input", CellChangeTimes->{{3.6476081740482817`*^9, 3.647608248074851*^9}, { - 3.6476082871491823`*^9, 3.647608323868623*^9}, {3.6476084126718693`*^9, + 3.6476082871491823`*^9, 3.647608323868623*^9}, {3.6476084126718693`*^9, 3.647608426646431*^9}, {3.647614434493647*^9, 3.647614437644298*^9}, { - 3.649440314162457*^9, 3.649440454966736*^9}, {3.650113501057988*^9, + 3.649440314162457*^9, 3.649440454966736*^9}, {3.650113501057988*^9, 3.650113561077476*^9}, {3.6501140530388117`*^9, 3.650114070671464*^9}, { - 3.650125417777068*^9, 3.650125455538785*^9}, {3.650126331537952*^9, + 3.650125417777068*^9, 3.650125455538785*^9}, {3.650126331537952*^9, 3.650126341438471*^9}, {3.650126402719391*^9, 3.650126428999959*^9}, { - 3.650126617109906*^9, 3.6501266521408863`*^9}, {3.709646659345621*^9, + 3.650126617109906*^9, 3.6501266521408863`*^9}, {3.709646659345621*^9, 3.709646783134551*^9}, {3.7098933386684723`*^9, 3.709893360494198*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - FractionBox["1", "2"], " ", "IHub", " ", + FractionBox["1", "2"], " ", "IHub", " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP1", " ", + FractionBox["1", "2"], " ", "ISP1", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP2", " ", + FractionBox["1", "2"], " ", "ISP2", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mHub", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mHub", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}], "2"], "+", SuperscriptBox[ RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}], - "+", + "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}]}]], "Output", - CellChangeTimes->{3.650126433152005*^9, 3.650126661451419*^9, - 3.6501271226026382`*^9, 3.709647565406519*^9, 3.709893450106464*^9, - 3.709904399733959*^9, 3.709904432371422*^9, 3.709912186587597*^9, + CellChangeTimes->{3.650126433152005*^9, 3.650126661451419*^9, + 3.6501271226026382`*^9, 3.709647565406519*^9, 3.709893450106464*^9, + 3.709904399733959*^9, 3.709904432371422*^9, 3.709912186587597*^9, 3.709991566302853*^9, 3.7100668278993692`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"V", " ", "=", " ", + RowBox[{"V", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"1", "/", "2"}], "k1", + RowBox[{"1", "/", "2"}], "k1", RowBox[{ - RowBox[{"(", - RowBox[{"theta1", "[", "t", "]"}], ")"}], "^", "2"}]}], "+", + RowBox[{"(", + RowBox[{"theta1", "[", "t", "]"}], ")"}], "^", "2"}]}], "+", RowBox[{ - RowBox[{"1", "/", "2"}], "k2", + RowBox[{"1", "/", "2"}], "k2", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{"theta2", "[", "t", "]"}], ")"}], "^", "2"}]}]}]}]], "Input", CellChangeTimes->{{3.647608457408671*^9, 3.647608497714343*^9}, { - 3.6494404974964943`*^9, 3.6494405178638973`*^9}, {3.709893455995377*^9, + 3.6494404974964943`*^9, 3.6494405178638973`*^9}, {3.709893455995377*^9, 3.709893465684599*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - FractionBox["1", "2"], " ", "k1", " ", + FractionBox["1", "2"], " ", "k1", " ", SuperscriptBox[ - RowBox[{"theta1", "[", "t", "]"}], "2"]}], "+", + RowBox[{"theta1", "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "k2", " ", + FractionBox["1", "2"], " ", "k2", " ", SuperscriptBox[ RowBox[{"theta2", "[", "t", "]"}], "2"]}]}]], "Output", CellChangeTimes->{ - 3.647608499924172*^9, 3.64761448705923*^9, 3.647615089616064*^9, - 3.647621228105358*^9, 3.649440521266033*^9, 3.650026330069805*^9, - 3.650026435698052*^9, 3.650113944040901*^9, 3.6501140770340567`*^9, - 3.6501141241886263`*^9, 3.650121181193581*^9, 3.65012546600323*^9, - 3.650126350893875*^9, 3.650126433184325*^9, 3.65012666148414*^9, + 3.647608499924172*^9, 3.64761448705923*^9, 3.647615089616064*^9, + 3.647621228105358*^9, 3.649440521266033*^9, 3.650026330069805*^9, + 3.650026435698052*^9, 3.650113944040901*^9, 3.6501140770340567`*^9, + 3.6501141241886263`*^9, 3.650121181193581*^9, 3.65012546600323*^9, + 3.650126350893875*^9, 3.650126433184325*^9, 3.65012666148414*^9, 3.650127122632825*^9, 3.7096475779352207`*^9, 3.709893468268532*^9, { - 3.709904403476265*^9, 3.709904432440607*^9}, 3.709912186653967*^9, + 3.709904403476265*^9, 3.709904432440607*^9}, 3.709912186653967*^9, 3.70999156637119*^9, 3.710066827966717*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Lag", " ", "=", " ", + RowBox[{"Lag", " ", "=", " ", RowBox[{"T", "-", "V"}]}]], "Input", CellChangeTimes->{{3.647608505756122*^9, 3.6476085105548487`*^9}, { 3.647614570390028*^9, 3.647614570501202*^9}}], @@ -692,626 +692,626 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", - FractionBox["1", "2"]}], " ", "k1", " ", + RowBox[{"-", + FractionBox["1", "2"]}], " ", "k1", " ", SuperscriptBox[ - RowBox[{"theta1", "[", "t", "]"}], "2"]}], "-", + RowBox[{"theta1", "[", "t", "]"}], "2"]}], "-", RowBox[{ - FractionBox["1", "2"], " ", "k2", " ", + FractionBox["1", "2"], " ", "k2", " ", SuperscriptBox[ - RowBox[{"theta2", "[", "t", "]"}], "2"]}], "+", + RowBox[{"theta2", "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "IHub", " ", + FractionBox["1", "2"], " ", "IHub", " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP1", " ", + FractionBox["1", "2"], " ", "ISP1", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "ISP2", " ", + FractionBox["1", "2"], " ", "ISP2", " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mHub", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mHub", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}], "2"], "+", SuperscriptBox[ RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}], - "+", + "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"], "+", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], ")"}]}]}]], "Output", CellChangeTimes->{ - 3.647608512913962*^9, 3.647614573588203*^9, 3.647615089649335*^9, - 3.647621228134879*^9, 3.649440526896332*^9, 3.6500263301004267`*^9, - 3.650026435722816*^9, 3.650113791252983*^9, 3.6501139440765142`*^9, - 3.650114077051147*^9, 3.650114124208144*^9, 3.650121181214005*^9, - 3.650125466055393*^9, 3.650126350927471*^9, 3.650126433224345*^9, - 3.650126661504125*^9, 3.650127122654311*^9, 3.709647615366233*^9, - 3.709893471758278*^9, {3.709904407003621*^9, 3.7099044325072527`*^9}, + 3.647608512913962*^9, 3.647614573588203*^9, 3.647615089649335*^9, + 3.647621228134879*^9, 3.649440526896332*^9, 3.6500263301004267`*^9, + 3.650026435722816*^9, 3.650113791252983*^9, 3.6501139440765142`*^9, + 3.650114077051147*^9, 3.650114124208144*^9, 3.650121181214005*^9, + 3.650125466055393*^9, 3.650126350927471*^9, 3.650126433224345*^9, + 3.650126661504125*^9, 3.650127122654311*^9, 3.709647615366233*^9, + 3.709893471758278*^9, {3.709904407003621*^9, 3.7099044325072527`*^9}, 3.709912186722497*^9, 3.7099915664381742`*^9, 3.710066828032853*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq1", " ", "=", " ", + RowBox[{"Eq1", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{ - RowBox[{"xHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"xHub", "[", "t", "]"}]}], "]"}], "-", "Tx"}], " ", "\[Equal]", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{ + RowBox[{"xHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"xHub", "[", "t", "]"}]}], "]"}], "-", "Tx"}], " ", "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.647614579441598*^9, 3.647614600924193*^9}, { - 3.647614866603059*^9, 3.647614887329845*^9}, {3.649440543327643*^9, + 3.647614866603059*^9, 3.647614887329845*^9}, {3.649440543327643*^9, 3.649440562004551*^9}, {3.649440749072403*^9, 3.6494407508768377`*^9}, { - 3.649440951552544*^9, 3.649440961562614*^9}, {3.709647639508596*^9, + 3.649440951552544*^9, 3.649440961562614*^9}, {3.709647639508596*^9, 3.709647646811626*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", "Tx"}], "+", - RowBox[{"mHub", " ", + RowBox[{"-", "Tx"}], "+", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{{3.647614875914831*^9, 3.647614889063602*^9}, - 3.647615089667774*^9, 3.647621228172324*^9, 3.6494405665702753`*^9, - 3.6494407535950603`*^9, 3.649440963907598*^9, 3.650026330119986*^9, - 3.650026435739256*^9, 3.6501137981351843`*^9, 3.6501139441069593`*^9, - 3.650114077086082*^9, 3.650114124242312*^9, 3.650121181242817*^9, - 3.650125466102409*^9, 3.650126350975716*^9, 3.650126433267352*^9, - 3.65012666153375*^9, 3.650127122695442*^9, 3.7096476604132757`*^9, - 3.70989348146432*^9, {3.709904409115304*^9, 3.709904432575056*^9}, + CellChangeTimes->{{3.647614875914831*^9, 3.647614889063602*^9}, + 3.647615089667774*^9, 3.647621228172324*^9, 3.6494405665702753`*^9, + 3.6494407535950603`*^9, 3.649440963907598*^9, 3.650026330119986*^9, + 3.650026435739256*^9, 3.6501137981351843`*^9, 3.6501139441069593`*^9, + 3.650114077086082*^9, 3.650114124242312*^9, 3.650121181242817*^9, + 3.650125466102409*^9, 3.650126350975716*^9, 3.650126433267352*^9, + 3.65012666153375*^9, 3.650127122695442*^9, 3.7096476604132757`*^9, + 3.70989348146432*^9, {3.709904409115304*^9, 3.709904432575056*^9}, 3.7099121867900543`*^9, 3.7099915665063963`*^9, 3.7100668281002493`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq1", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq1", ",", - RowBox[{"{", + RowBox[{"Eq1", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq1", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.647614906346649*^9, 3.647614911027494*^9}, { - 3.649440758351534*^9, 3.649440832552063*^9}, {3.709647671852079*^9, + 3.649440758351534*^9, 3.649440832552063*^9}, {3.709647671852079*^9, 3.7096476989589853`*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"mHub", " ", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "Tx"}]], "Output", CellChangeTimes->{ - 3.647614926214444*^9, 3.647615092999082*^9, 3.6476212284034767`*^9, - 3.649440684330357*^9, 3.649440807189921*^9, 3.6494408416192007`*^9, - 3.649440967306904*^9, 3.6500263368342657`*^9, 3.650026436202402*^9, - 3.6501138061520643`*^9, 3.65011394412993*^9, 3.650114077120083*^9, - 3.650114124276799*^9, 3.6501211812611856`*^9, 3.650125466152321*^9, - 3.6501263510091753`*^9, 3.650126433305233*^9, 3.650126661553743*^9, - 3.650127122727659*^9, 3.709647677516232*^9, 3.7096477099497433`*^9, - 3.709893503042837*^9, {3.7099044268732767`*^9, 3.709904443064567*^9}, + 3.647614926214444*^9, 3.647615092999082*^9, 3.6476212284034767`*^9, + 3.649440684330357*^9, 3.649440807189921*^9, 3.6494408416192007`*^9, + 3.649440967306904*^9, 3.6500263368342657`*^9, 3.650026436202402*^9, + 3.6501138061520643`*^9, 3.65011394412993*^9, 3.650114077120083*^9, + 3.650114124276799*^9, 3.6501211812611856`*^9, 3.650125466152321*^9, + 3.6501263510091753`*^9, 3.650126433305233*^9, 3.650126661553743*^9, + 3.650127122727659*^9, 3.709647677516232*^9, 3.7096477099497433`*^9, + 3.709893503042837*^9, {3.7099044268732767`*^9, 3.709904443064567*^9}, 3.709912195685472*^9, 3.709991574358623*^9, 3.7100668369914494`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq1", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq1", ",", RowBox[{ RowBox[{"xHub", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.6494409813785467`*^9, 3.649440988496232*^9}, { - 3.649441103290543*^9, 3.649441120586372*^9}, {3.70964771655809*^9, + 3.649441103290543*^9, 3.649441120586372*^9}, {3.70964771655809*^9, 3.709647717438199*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", - RowBox[{ - FractionBox["1", - RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], - RowBox[{"(", - RowBox[{"Tx", "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + RowBox[{ + FractionBox["1", + RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], + RowBox[{"(", + RowBox[{"Tx", "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", - CellChangeTimes->{3.650113944169817*^9, 3.650114077139044*^9, - 3.650114124304409*^9, 3.650121181294386*^9, 3.650125466202359*^9, - 3.650126351042288*^9, 3.650126433338122*^9, 3.65012666158496*^9, - 3.650127122765802*^9, 3.709647720541893*^9, 3.70989354325331*^9, - 3.709904443140407*^9, 3.709912195760069*^9, 3.709991574470261*^9, + CellChangeTimes->{3.650113944169817*^9, 3.650114077139044*^9, + 3.650114124304409*^9, 3.650121181294386*^9, 3.650125466202359*^9, + 3.650126351042288*^9, 3.650126433338122*^9, 3.65012666158496*^9, + 3.650127122765802*^9, 3.709647720541893*^9, 3.70989354325331*^9, + 3.709904443140407*^9, 3.709912195760069*^9, 3.709991574470261*^9, 3.710066837095171*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq2", " ", "=", " ", + RowBox[{"Eq2", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{ - RowBox[{"yHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"yHub", "[", "t", "]"}]}], "]"}], "-", "Ty"}], " ", "\[Equal]", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{ + RowBox[{"yHub", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"yHub", "[", "t", "]"}]}], "]"}], "-", "Ty"}], " ", "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.64944132623348*^9, 3.6494413532739983`*^9}, { 3.7096477376723948`*^9, 3.709647742814459*^9}}], @@ -1319,132 +1319,132 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", "Ty"}], "+", - RowBox[{"mHub", " ", + RowBox[{"-", "Ty"}], "+", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{{3.649441330224839*^9, 3.649441354551085*^9}, - 3.6500263369901237`*^9, 3.650026436270349*^9, 3.650113944223764*^9, - 3.65011407719658*^9, 3.650114124360874*^9, 3.6501211813409653`*^9, - 3.650125466295785*^9, 3.6501263511254063`*^9, 3.6501264334209547`*^9, - 3.6501266616626263`*^9, 3.650127122799155*^9, 3.7096477442006702`*^9, - 3.709893548368627*^9, 3.709904443200683*^9, 3.70991219582679*^9, + CellChangeTimes->{{3.649441330224839*^9, 3.649441354551085*^9}, + 3.6500263369901237`*^9, 3.650026436270349*^9, 3.650113944223764*^9, + 3.65011407719658*^9, 3.650114124360874*^9, 3.6501211813409653`*^9, + 3.650125466295785*^9, 3.6501263511254063`*^9, 3.6501264334209547`*^9, + 3.6501266616626263`*^9, 3.650127122799155*^9, 3.7096477442006702`*^9, + 3.709893548368627*^9, 3.709904443200683*^9, 3.70991219582679*^9, 3.7099915745328817`*^9, 3.7100668371544437`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq2", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq2", ",", - RowBox[{"{", + RowBox[{"Eq2", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq2", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.6494413919573603`*^9, 3.649441395319479*^9}, { 3.709647749463461*^9, 3.7096477703687468`*^9}}], @@ -1452,1008 +1452,1008 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"mHub", " ", + RowBox[{"mHub", " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"mSP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], "\[Equal]", "Ty"}]], "Output", - CellChangeTimes->{{3.649441384353636*^9, 3.649441404905581*^9}, - 3.650026343707588*^9, 3.65002643654138*^9, 3.650113944258304*^9, - 3.650114077232009*^9, 3.6501141243879232`*^9, 3.650121181377021*^9, - 3.6501254663447037`*^9, 3.650126351158494*^9, 3.6501264334502573`*^9, - 3.6501266616822643`*^9, 3.650127122819181*^9, 3.709647784487842*^9, - 3.709893574056983*^9, 3.7099044610384007`*^9, 3.709912208124202*^9, + CellChangeTimes->{{3.649441384353636*^9, 3.649441404905581*^9}, + 3.650026343707588*^9, 3.65002643654138*^9, 3.650113944258304*^9, + 3.650114077232009*^9, 3.6501141243879232`*^9, 3.650121181377021*^9, + 3.6501254663447037`*^9, 3.650126351158494*^9, 3.6501264334502573`*^9, + 3.6501266616822643`*^9, 3.650127122819181*^9, 3.709647784487842*^9, + 3.709893574056983*^9, 3.7099044610384007`*^9, 3.709912208124202*^9, 3.7099915860370693`*^9, 3.710066849681682*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq2", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq2", ",", RowBox[{ RowBox[{"yHub", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.649441467695437*^9, 3.649441470984462*^9}, { 3.709647777488635*^9, 3.709647783103689*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", - RowBox[{ - FractionBox["1", - RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], - RowBox[{"(", - RowBox[{"Ty", "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + RowBox[{ + FractionBox["1", + RowBox[{"mHub", "+", "mSP1", "+", "mSP2"}]], + RowBox[{"(", + RowBox[{"Ty", "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", - CellChangeTimes->{3.650113944303441*^9, 3.650114077264604*^9, - 3.650114124408894*^9, 3.65012118141026*^9, 3.650125466395176*^9, - 3.650126351191698*^9, 3.650126433471068*^9, 3.650126661717382*^9, - 3.650127122848145*^9, 3.7096477862767563`*^9, 3.709893604933693*^9, - 3.709904461095764*^9, 3.709912208203537*^9, 3.70999158617177*^9, + CellChangeTimes->{3.650113944303441*^9, 3.650114077264604*^9, + 3.650114124408894*^9, 3.65012118141026*^9, 3.650125466395176*^9, + 3.650126351191698*^9, 3.650126433471068*^9, 3.650126661717382*^9, + 3.650127122848145*^9, 3.7096477862767563`*^9, 3.709893604933693*^9, + 3.709904461095764*^9, 3.709912208203537*^9, 3.70999158617177*^9, 3.71006684977703*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq3", " ", "=", " ", + RowBox[{"Eq3", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{ - RowBox[{"theta", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", "-", " ", "Torque"}], + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{ + RowBox[{"theta", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", "-", " ", "Torque"}], " ", "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.649441490019846*^9, 3.649441526048011*^9}, { - 3.6494416329455757`*^9, 3.649441639953199*^9}, {3.709647999226737*^9, + 3.6494416329455757`*^9, 3.649441639953199*^9}, {3.709647999226737*^9, 3.709648001657591*^9}, {3.7098936614514217`*^9, 3.709893666760483*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"-", "Torque"}], "-", + RowBox[{"-", "Torque"}], "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "-", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", - RowBox[{"IHub", " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{"IHub", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"ISP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"ISP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"ISP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"ISP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ - RowBox[{"2", " ", - RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], - " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}], + " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{3.6494416430996*^9, 3.650026343837957*^9, - 3.6500264366053953`*^9, 3.6501139443755608`*^9, 3.6501140773356533`*^9, - 3.650114124465275*^9, 3.650121181475136*^9, 3.650125466495*^9, - 3.6501263512510633`*^9, 3.6501264335329323`*^9, 3.6501266617780237`*^9, - 3.650127122873707*^9, 3.7096478122779713`*^9, 3.709648036046701*^9, - 3.7098936714787397`*^9, 3.7099044611434526`*^9, 3.7099122082757*^9, + CellChangeTimes->{3.6494416430996*^9, 3.650026343837957*^9, + 3.6500264366053953`*^9, 3.6501139443755608`*^9, 3.6501140773356533`*^9, + 3.650114124465275*^9, 3.650121181475136*^9, 3.650125466495*^9, + 3.6501263512510633`*^9, 3.6501264335329323`*^9, 3.6501266617780237`*^9, + 3.650127122873707*^9, 3.7096478122779713`*^9, 3.709648036046701*^9, + 3.7098936714787397`*^9, 3.7099044611434526`*^9, 3.7099122082757*^9, 3.709991586226976*^9, 3.710066849868989*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq3", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq3", ",", - RowBox[{"{", + RowBox[{"Eq3", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq3", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.649441701396606*^9, 3.649441704434289*^9}, { 3.709647818203969*^9, 3.709647839090629*^9}}], @@ -2461,687 +2461,687 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", - RowBox[{"2", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + RowBox[{"2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"IHub", "+", "ISP1", "+", "ISP2"}], ")"}], " ", + RowBox[{"(", + RowBox[{"IHub", "+", "ISP1", "+", "ISP2"}], ")"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", - SuperscriptBox["Rhinge1", "2"], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", + SuperscriptBox["Rhinge1", "2"], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP2", " ", - SuperscriptBox["Rhinge2", "2"], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP2", " ", + SuperscriptBox["Rhinge2", "2"], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"ISP1", " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"ISP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"ISP2", " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"ISP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", - RowBox[{"Torque", "+", + MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"Torque", "+", RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}]}]], "Output", - CellChangeTimes->{3.649441711039947*^9, 3.650026348595045*^9, - 3.650026437159946*^9, 3.6501139490906878`*^9, 3.65011408062339*^9, - 3.650114124719529*^9, 3.650121181507895*^9, 3.650125466545499*^9, - 3.650126351285775*^9, 3.650126434062338*^9, 3.650126661823897*^9, - 3.650127122910791*^9, 3.709647853944254*^9, 3.70964806140898*^9, - 3.709893706843128*^9, 3.709904495809168*^9, 3.70991222827071*^9, + CellChangeTimes->{3.649441711039947*^9, 3.650026348595045*^9, + 3.650026437159946*^9, 3.6501139490906878`*^9, 3.65011408062339*^9, + 3.650114124719529*^9, 3.650121181507895*^9, 3.650125466545499*^9, + 3.650126351285775*^9, 3.650126434062338*^9, 3.650126661823897*^9, + 3.650127122910791*^9, 3.709647853944254*^9, 3.70964806140898*^9, + 3.709893706843128*^9, 3.709904495809168*^9, 3.70991222827071*^9, 3.7099916041330023`*^9, 3.710066868405623*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq3", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq3", ",", RowBox[{ RowBox[{"theta", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.649441744480874*^9, 3.649441755989689*^9}, { 3.7098939348877172`*^9, 3.709893936213263*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{ - RowBox[{"(", - RowBox[{"Torque", "-", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{"Torque", "-", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"ISP1", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"ISP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"ISP2", " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"ISP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}], "/", - RowBox[{"(", - RowBox[{"IHub", "+", "ISP1", "+", "ISP2", "+", + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}], "/", + RowBox[{"(", + RowBox[{"IHub", "+", "ISP1", "+", "ISP2", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", + SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", - RowBox[{"mSP1", " ", - SuperscriptBox["Rhinge1", "2"]}], "+", - RowBox[{"mSP2", " ", - SuperscriptBox["Rhinge2", "2"]}], "+", - RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", - RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}]}]}], "}"}], + SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", + RowBox[{"mSP1", " ", + SuperscriptBox["Rhinge1", "2"]}], "+", + RowBox[{"mSP2", " ", + SuperscriptBox["Rhinge2", "2"]}], "+", + RowBox[{"2", " ", "d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}], "+", + RowBox[{"2", " ", "d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", CellChangeTimes->{ - 3.650113949170033*^9, 3.650114080665533*^9, 3.650114124738514*^9, - 3.650121181527328*^9, 3.6501254665943823`*^9, 3.6501263513202057`*^9, - 3.65012643412017*^9, 3.650126661855892*^9, 3.650127122950385*^9, - 3.709647869432714*^9, 3.70964807076822*^9, {3.70989392998943*^9, - 3.709893938934258*^9}, 3.709904495937688*^9, 3.7099122283751574`*^9, + 3.650113949170033*^9, 3.650114080665533*^9, 3.650114124738514*^9, + 3.650121181527328*^9, 3.6501254665943823`*^9, 3.6501263513202057`*^9, + 3.65012643412017*^9, 3.650126661855892*^9, 3.650127122950385*^9, + 3.709647869432714*^9, 3.70964807076822*^9, {3.70989392998943*^9, + 3.709893938934258*^9}, 3.709904495937688*^9, 3.7099122283751574`*^9, 3.7099916042609997`*^9, 3.710066868501666*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq4", " ", "=", " ", + RowBox[{"Eq4", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", + RowBox[{"D", "[", + RowBox[{"Lag", ",", RowBox[{ RowBox[{"theta1", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", "+", - RowBox[{"c1", "*", + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", "+", + RowBox[{"c1", "*", RowBox[{ - RowBox[{"theta1", "'"}], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"theta1", "'"}], "[", "t", "]"}]}]}], "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.649441857154335*^9, 3.6494418688045387`*^9}, { - 3.649442193911736*^9, 3.649442218793474*^9}, {3.650026298715227*^9, + 3.649442193911736*^9, 3.649442218793474*^9}, {3.650026298715227*^9, 3.650026299976062*^9}, {3.7096481588067293`*^9, 3.709648159022726*^9}, { 3.709893944199317*^9, 3.7098939571022673`*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k1", " ", - RowBox[{"theta1", "[", "t", "]"}]}], "+", - RowBox[{"c1", " ", + RowBox[{"k1", " ", + RowBox[{"theta1", "[", "t", "]"}]}], "+", + RowBox[{"c1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", - RowBox[{"ISP1", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{"ISP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP1", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP1", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", "d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge1"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", "d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge1"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", - CellChangeTimes->{3.6494418700740137`*^9, 3.6494422226433496`*^9, - 3.6500263034503527`*^9, 3.650026348745247*^9, 3.650026437210599*^9, - 3.650113949253563*^9, 3.6501140807222767`*^9, 3.650114124804435*^9, - 3.6501211815963173`*^9, 3.650125466805504*^9, 3.650126351387097*^9, - 3.650126434183649*^9, 3.650126661922491*^9, 3.650127122999509*^9, - 3.709648163644412*^9, 3.709893959183962*^9, 3.709904495992156*^9, + CellChangeTimes->{3.6494418700740137`*^9, 3.6494422226433496`*^9, + 3.6500263034503527`*^9, 3.650026348745247*^9, 3.650026437210599*^9, + 3.650113949253563*^9, 3.6501140807222767`*^9, 3.650114124804435*^9, + 3.6501211815963173`*^9, 3.650125466805504*^9, 3.650126351387097*^9, + 3.650126434183649*^9, 3.650126661922491*^9, 3.650127122999509*^9, + 3.709648163644412*^9, 3.709893959183962*^9, 3.709904495992156*^9, 3.709912228430462*^9, 3.709991604325783*^9, 3.710066868562544*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq4", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq4", ",", - RowBox[{"{", + RowBox[{"Eq4", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq4", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.649441900454019*^9, 3.6494419029884357`*^9}, { 3.7096481750058613`*^9, 3.709648196975309*^9}}], @@ -3149,489 +3149,489 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k1", " ", - RowBox[{"theta1", "[", "t", "]"}]}], "+", - RowBox[{"c1", " ", + RowBox[{"k1", " ", + RowBox[{"theta1", "[", "t", "]"}]}], "+", + RowBox[{"c1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP1", "+", + RowBox[{"(", + RowBox[{"ISP1", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + SuperscriptBox["d1", "2"], " ", "mSP1"}], "+", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP1", "+", + RowBox[{"(", + RowBox[{"ISP1", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}]}], ")"}], " ", + SuperscriptBox["d1", "2"], " ", "mSP1"}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}]], "Output", - CellChangeTimes->{3.649441907035841*^9, 3.649442235781934*^9, - 3.650026351853695*^9, 3.650026437327095*^9, 3.6501139515691*^9, - 3.650114081436472*^9, 3.650114125027214*^9, 3.650121181630754*^9, - 3.6501254668447866`*^9, 3.650126351421269*^9, 3.6501264342223673`*^9, - 3.650126662116922*^9, 3.650127123045363*^9, 3.709648349946191*^9, - 3.7098939733623323`*^9, 3.709904501105431*^9, 3.70991223188218*^9, + CellChangeTimes->{3.649441907035841*^9, 3.649442235781934*^9, + 3.650026351853695*^9, 3.650026437327095*^9, 3.6501139515691*^9, + 3.650114081436472*^9, 3.650114125027214*^9, 3.650121181630754*^9, + 3.6501254668447866`*^9, 3.650126351421269*^9, 3.6501264342223673`*^9, + 3.650126662116922*^9, 3.650127123045363*^9, 3.709648349946191*^9, + 3.7098939733623323`*^9, 3.709904501105431*^9, 3.70991223188218*^9, 3.709991607844681*^9, 3.710066872120063*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq4", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq4", ",", RowBox[{ RowBox[{"theta1", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.649441926418256*^9, 3.64944194178972*^9}, { 3.7098939782495117`*^9, 3.7098939802637053`*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["theta1", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{ - FractionBox["1", - RowBox[{"ISP1", "+", + FractionBox["1", + RowBox[{"ISP1", "+", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1"}]}]], - RowBox[{"(", + SuperscriptBox["d1", "2"], " ", "mSP1"}]}]], + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "k1"}], " ", - RowBox[{"theta1", "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Sin", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "k1"}], " ", + RowBox[{"theta1", "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Sin", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"c1", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"c1", " ", RowBox[{ SuperscriptBox["theta1", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"ISP1", " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"ISP1", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d1", "2"], " ", "mSP1", " ", + SuperscriptBox["d1", "2"], " ", "mSP1", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", - RowBox[{"Cos", "[", - RowBox[{"beta1", "-", "thetaH1", "-", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", "Rhinge1", " ", + RowBox[{"Cos", "[", + RowBox[{"beta1", "-", "thetaH1", "-", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d1", " ", "mSP1", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH1", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d1", " ", "mSP1", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH1", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta1", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", - CellChangeTimes->{3.6501139516163483`*^9, 3.6501140814531193`*^9, - 3.6501141250632668`*^9, 3.650121181649961*^9, 3.650125466894619*^9, - 3.650126351454706*^9, 3.6501264342548857`*^9, 3.650126662157115*^9, - 3.650127123079134*^9, 3.709648353498505*^9, 3.709893982607676*^9, - 3.709904501156394*^9, 3.7099122319746513`*^9, 3.709991607949237*^9, + CellChangeTimes->{3.6501139516163483`*^9, 3.6501140814531193`*^9, + 3.6501141250632668`*^9, 3.650121181649961*^9, 3.650125466894619*^9, + 3.650126351454706*^9, 3.6501264342548857`*^9, 3.650126662157115*^9, + 3.650127123079134*^9, 3.709648353498505*^9, 3.709893982607676*^9, + 3.709904501156394*^9, 3.7099122319746513`*^9, 3.709991607949237*^9, 3.7100668721947823`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq5", " ", "=", " ", + RowBox[{"Eq5", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"D", "[", + RowBox[{"D", "[", RowBox[{ - RowBox[{"D", "[", - RowBox[{"Lag", ",", + RowBox[{"D", "[", + RowBox[{"Lag", ",", RowBox[{ RowBox[{"theta2", "'"}], "[", "t", "]"}]}], "]"}], ",", "t"}], "]"}], - "-", - RowBox[{"D", "[", - RowBox[{"Lag", ",", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", "+", - RowBox[{"c2", "*", + "-", + RowBox[{"D", "[", + RowBox[{"Lag", ",", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", "+", + RowBox[{"c2", "*", RowBox[{ - RowBox[{"theta2", "'"}], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"theta2", "'"}], "[", "t", "]"}]}]}], "\[Equal]", "0"}]}]], "Input", CellChangeTimes->{{3.649441974232287*^9, 3.649441974602599*^9}, { - 3.649442024415121*^9, 3.6494420297328167`*^9}, {3.6494422687274637`*^9, + 3.649442024415121*^9, 3.6494420297328167`*^9}, {3.6494422687274637`*^9, 3.649442279760858*^9}, {3.650026425804619*^9, 3.650026427138609*^9}, { - 3.7096483649009*^9, 3.709648365220821*^9}, {3.709893986529037*^9, + 3.7096483649009*^9, 3.709648365220821*^9}, {3.709893986529037*^9, 3.7098939986630163`*^9}, {3.709894594889039*^9, 3.7098945981436768`*^9}}], Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k2", " ", - RowBox[{"theta2", "[", "t", "]"}]}], "+", - RowBox[{"c2", " ", + RowBox[{"k2", " ", + RowBox[{"theta2", "[", "t", "]"}]}], "+", + RowBox[{"c2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], - "+", - RowBox[{"ISP2", " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{"ISP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ - FractionBox["1", "2"], " ", "mSP2", " ", - RowBox[{"(", + FractionBox["1", "2"], " ", "mSP2", " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", "d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + RowBox[{"-", "2"}], " ", "d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "-", - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "-", + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", - RowBox[{"2", " ", "d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "Rhinge2"}], " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + RowBox[{"2", " ", "d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "Rhinge2"}], " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"d2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"d2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ - RowBox[{"(", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], - "+", - RowBox[{"Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "+", - RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], ")"}], "2"]}], + "+", + RowBox[{"Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "+", + RowBox[{"theta", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", - RowBox[{"(", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "+", + MultilineFunction->None], "[", "t", "]"}], "+", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "+", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], + MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], ")"}]}]}], "\[Equal]", "0"}]], "Output", CellChangeTimes->{ 3.649442031278104*^9, 3.649442281353739*^9, 3.650026351903562*^9, { - 3.650026428718976*^9, 3.650026437375152*^9}, 3.650113951691698*^9, - 3.650114081506241*^9, 3.650114125109868*^9, 3.6501211816981277`*^9, - 3.650125466995118*^9, 3.650126351524879*^9, 3.6501264343228188`*^9, - 3.650126662222809*^9, 3.650127123116081*^9, 3.709648368112361*^9, - 3.7098946055579844`*^9, 3.7099045012015133`*^9, 3.709912232028257*^9, + 3.650026428718976*^9, 3.650026437375152*^9}, 3.650113951691698*^9, + 3.650114081506241*^9, 3.650114125109868*^9, 3.6501211816981277`*^9, + 3.650125466995118*^9, 3.650126351524879*^9, 3.6501264343228188`*^9, + 3.650126662222809*^9, 3.650127123116081*^9, 3.709648368112361*^9, + 3.7098946055579844`*^9, 3.7099045012015133`*^9, 3.709912232028257*^9, 3.709991608051722*^9, 3.710066872251883*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Eq5", " ", "=", - RowBox[{"FullSimplify", "[", - RowBox[{"Eq5", ",", - RowBox[{"{", + RowBox[{"Eq5", " ", "=", + RowBox[{"FullSimplify", "[", + RowBox[{"Eq5", ",", + RowBox[{"{", RowBox[{ - RowBox[{"mHub", ">", "0"}], ",", - RowBox[{"mSP1", ">", "0"}], ",", - RowBox[{"mSP2", ">", "0"}], ",", - RowBox[{"IHub", ">", "0"}], ",", - RowBox[{"ISP1", ">", "0"}], ",", + RowBox[{"mHub", ">", "0"}], ",", + RowBox[{"mSP1", ">", "0"}], ",", + RowBox[{"mSP2", ">", "0"}], ",", + RowBox[{"IHub", ">", "0"}], ",", + RowBox[{"ISP1", ">", "0"}], ",", RowBox[{"ISP2", ">", "0"}]}], "}"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.649442050675033*^9, 3.6494420532928762`*^9}, { 3.709648373091796*^9, 3.7096483936980247`*^9}}], @@ -3639,146 +3639,146 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ RowBox[{ - RowBox[{"k2", " ", - RowBox[{"theta2", "[", "t", "]"}]}], "+", - RowBox[{"c2", " ", + RowBox[{"k2", " ", + RowBox[{"theta2", "[", "t", "]"}]}], "+", + RowBox[{"c2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP2", "+", + RowBox[{"(", + RowBox[{"ISP2", "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", + SuperscriptBox["d2", "2"], " ", "mSP2"}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}]}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", + MultilineFunction->None], "[", "t", "]"}]}], "+", RowBox[{ - RowBox[{"(", - RowBox[{"ISP2", "+", + RowBox[{"(", + RowBox[{"ISP2", "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}]}], ")"}], " ", + SuperscriptBox["d2", "2"], " ", "mSP2"}]}], ")"}], " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}]}], "\[Equal]", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "+", RowBox[{ - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}]], "Output", - CellChangeTimes->{3.649442057869686*^9, 3.649442290890633*^9, - 3.650026354551691*^9, 3.650026439581772*^9, 3.650113954518001*^9, - 3.650114081534142*^9, 3.650114125129826*^9, 3.650121181727723*^9, - 3.650125467045177*^9, 3.6501263515581093`*^9, 3.650126435015355*^9, - 3.650126662243575*^9, 3.650127123150807*^9, 3.7096483991918497`*^9, - 3.709894614262093*^9, 3.709904506228737*^9, 3.7099122352551403`*^9, + CellChangeTimes->{3.649442057869686*^9, 3.649442290890633*^9, + 3.650026354551691*^9, 3.650026439581772*^9, 3.650113954518001*^9, + 3.650114081534142*^9, 3.650114125129826*^9, 3.650121181727723*^9, + 3.650125467045177*^9, 3.6501263515581093`*^9, 3.650126435015355*^9, + 3.650126662243575*^9, 3.650127123150807*^9, 3.7096483991918497`*^9, + 3.709894614262093*^9, 3.709904506228737*^9, 3.7099122352551403`*^9, 3.709991611016552*^9, 3.7100668766011467`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Solve", "[", - RowBox[{"Eq5", ",", + RowBox[{"Solve", "[", + RowBox[{"Eq5", ",", RowBox[{ RowBox[{"theta2", "''"}], "[", "t", "]"}]}], "]"}]], "Input", CellChangeTimes->{{3.6494421764678802`*^9, 3.649442183039885*^9}, { 3.709894635218473*^9, 3.7098946388520184`*^9}}], Cell[BoxData[ - RowBox[{"{", - RowBox[{"{", + RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ SuperscriptBox["theta2", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "\[Rule]", + MultilineFunction->None], "[", "t", "]"}], "\[Rule]", RowBox[{ - FractionBox["1", - RowBox[{"ISP2", "+", + FractionBox["1", + RowBox[{"ISP2", "+", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2"}]}]], - RowBox[{"(", + SuperscriptBox["d2", "2"], " ", "mSP2"}]}]], + RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "k2"}], " ", - RowBox[{"theta2", "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Sin", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + RowBox[{"-", "k2"}], " ", + RowBox[{"theta2", "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Sin", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", SuperscriptBox[ RowBox[{ SuperscriptBox["theta", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", - RowBox[{"c2", " ", + MultilineFunction->None], "[", "t", "]"}], "2"]}], "-", + RowBox[{"c2", " ", RowBox[{ SuperscriptBox["theta2", "\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"ISP2", " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"ISP2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", + MultilineFunction->None], "[", "t", "]"}]}], "-", RowBox[{ - SuperscriptBox["d2", "2"], " ", "mSP2", " ", + SuperscriptBox["d2", "2"], " ", "mSP2", " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", - RowBox[{"Cos", "[", - RowBox[{"beta2", "-", "thetaH2", "-", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", "Rhinge2", " ", + RowBox[{"Cos", "[", + RowBox[{"beta2", "-", "thetaH2", "-", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["theta", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "+", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Sin", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "+", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Sin", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["xHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}], "-", - RowBox[{"d2", " ", "mSP2", " ", - RowBox[{"Cos", "[", - RowBox[{"thetaH2", "+", - RowBox[{"theta", "[", "t", "]"}], "+", - RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", + MultilineFunction->None], "[", "t", "]"}]}], "-", + RowBox[{"d2", " ", "mSP2", " ", + RowBox[{"Cos", "[", + RowBox[{"thetaH2", "+", + RowBox[{"theta", "[", "t", "]"}], "+", + RowBox[{"theta2", "[", "t", "]"}]}], "]"}], " ", RowBox[{ SuperscriptBox["yHub", "\[Prime]\[Prime]", - MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], + MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}]}], "}"}], "}"}]], "Output", CellChangeTimes->{ - 3.650113954545787*^9, 3.650114081552614*^9, 3.65011412516045*^9, - 3.650121181746997*^9, 3.6501254670958147`*^9, 3.6501263515786343`*^9, - 3.650126435034376*^9, 3.650126662274653*^9, 3.6501271231820707`*^9, - 3.709648402930709*^9, {3.709894632465825*^9, 3.709894639761241*^9}, - 3.709904506291833*^9, 3.709912235339995*^9, 3.7099916111335163`*^9, + 3.650113954545787*^9, 3.650114081552614*^9, 3.65011412516045*^9, + 3.650121181746997*^9, 3.6501254670958147`*^9, 3.6501263515786343`*^9, + 3.650126435034376*^9, 3.650126662274653*^9, 3.6501271231820707`*^9, + 3.709648402930709*^9, {3.709894632465825*^9, 3.709894639761241*^9}, + 3.709904506291833*^9, 3.709912235339995*^9, 3.7099916111335163`*^9, 3.7100668766653957`*^9}] }, Open ]] }, Open ]] @@ -3911,4 +3911,3 @@ Cell[139379, 3713, 2762, 68, 197, "Output"] } ] *) - diff --git a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secModelDescription.tex b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secModelDescription.tex index 4f768b3c74..4212395b3d 100644 --- a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secModelDescription.tex +++ b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secModelDescription.tex @@ -30,14 +30,14 @@ \subsection{Equations of Motion} \label{eq:Rbddot3} \end{equation} -The following equation describes the rotational EOM if only rotational is enabled. This is similar to what can be seen in Reference~\cite{schaub} +The following equation describes the rotational EOM if only rotational is enabled. This is similar to what can be seen in Reference~\cite{schaub} \begin{equation} \Big[[I_{\text{sc},B}]+\sum\limits_{i=1}^{N} [D_{\textnormal{contr},i}]\Big] \dot{\bm\omega}_{\cal B/N} = - [\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} + \bm{L}_B +\sum\limits_{i=1}^{N} \bm v_{\textnormal{rot,contr},i} \label{eq:Final6} \end{equation} $[D_{\textnormal{contr},i}]$ and $\bm v_{\textnormal{rot},i}$ will be defined in the next set of equations. The $[\bm{\tilde{\omega}}_{\cal B/N}]$ matrix is the matrix representation of the cross product operation. -Finally, the two following equations describe the translational and rotational EOMs when the both rotational and translational equations are enabled. +Finally, the two following equations describe the translational and rotational EOMs when the both rotational and translational equations are enabled. \begin{multline} \Big(m_{\text{sc}} [I_{3\times3}] +\sum_{i=1}^{N}[A_{\textnormal{contr},i}]\Big)\ddot{\bm r}_{B/N}+\Big(-m_{\text{sc}} [\tilde{\bm{c}}] +\sum_{i=1}^{N}[B_{\textnormal{contr},i}]\Big) \dot{\bm\omega}_{\cal B/N} \\ @@ -50,10 +50,10 @@ \subsection{Equations of Motion} \begin{multline} \Big[m_{\text{sc}}[\tilde{\bm{c}}] +\sum\limits_{i=1}^{N}[C_{\textnormal{contr},i}]\Big]\ddot{\bm r}_{B/N}+\Big[[I_{\text{sc},B}]+\sum\limits_{i=1}^{N}[D_{\textnormal{contr},i}]\Big]\dot{\bm\omega}_{\cal B/N} \\ -= -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} += -[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N} + \bm{L}_B +\sum\limits_{i=1}^{N}\bm v_{\textnormal{rot,contr},i} \label{eq:Final9} -\end{multline} +\end{multline} Where $[A_{\textnormal{contr},i}]$, $[B_{\textnormal{contr},i}]$, $[C_{\textnormal{contr},i}]$, $[D_{\textnormal{contr},i}]$, $\bm v_{\textnormal{trans,contr},i}$, and $\bm v_{\textnormal{rot,contr},i}$, are the contributions from the stateEffectors using the back-substitution method seen in Reference~\cite{Allard2016rz} and also discussed more in detail in the hinged rigid body state effector document. The equations can now be organized into the following matrix representation: @@ -106,7 +106,7 @@ \subsubsection{Total Orbital Kinetic Energy} \subsubsection{Total Orbital Potential Energy} -The total orbital potential energy depends on what type of gravity model you are using. For simplicity, the orbital potential energy due to point gravity is included here but would need to be changed for spherical harmonics, etc. +The total orbital potential energy depends on what type of gravity model you are using. For simplicity, the orbital potential energy due to point gravity is included here but would need to be changed for spherical harmonics, etc. \begin{equation} V_{\text{orb}} = \frac{\mu}{\vert \bm r_{C/N} \vert} @@ -128,7 +128,7 @@ \subsubsection{Total Rotational and Deformational Kinetic Energy} + \sum\limits_{i}^{N}\Big(\frac{1}{2} \bm \omega_{\cal{E}_{\textit{i}}/\cal{N}}^T [I_{\text{eff},E_{c,i}}] \bm \omega_{\cal{E}_{\textit{i}}/\cal{N}} + \frac{1}{2} m_{\text{eff}} \dot{\bm r}_{E_{c,i}/C} \cdot \dot{\bm r}_{E_{c,i}/C}\Big) \end{multline} -Where $N$ is the number of state effectors attached to the hub, ``eff" is the current state effector which a frame specified as $\cal{E}_{\textit{i}}$ and a center of mass location labeled as point $E_{c,i}$. Expanding these terms similar to orbital kinetic energy results in +Where $N$ is the number of state effectors attached to the hub, ``eff" is the current state effector which a frame specified as $\cal{E}_{\textit{i}}$ and a center of mass location labeled as point $E_{c,i}$. Expanding these terms similar to orbital kinetic energy results in \begin{multline} T_{\text{rot}} = \frac{1}{2} \bm \omega_{\cal{B/N}}^T [I_{\text{hub},B_c}] \bm \omega_{\cal{B/N}} + \frac{1}{2} m_{\text{hub}} (\dot{\bm r}_{B_c/B} - \dot{\bm c}) \cdot (\dot{\bm r}_{B_c/B} - \dot{\bm c}) \\ + \sum\limits_{i}^{N}\Big[\frac{1}{2} \bm \omega_{\cal{E}_{\textit{i}}/\cal{N}}^T [I_{\text{eff},E_{c,i}}] \bm \omega_{\cal{E}_{\textit{i}}/\cal{N}} diff --git a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secModelFunctions.tex b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secModelFunctions.tex index 6a3d83e54c..5acd581ec9 100644 --- a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secModelFunctions.tex +++ b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secModelFunctions.tex @@ -1,6 +1,6 @@ \section{Model Functions} -This module is intended to be used as a model to represent a spacecraft that can be decomposed into a rigid body hub and has the ability to model state effectors such as reactions wheels, and flexing solar panels, etc attached to the hub. +This module is intended to be used as a model to represent a spacecraft that can be decomposed into a rigid body hub and has the ability to model state effectors such as reactions wheels, and flexing solar panels, etc attached to the hub. \begin{itemize} \item Updates the mass properties of the spacecraft by adding up all of the contributions to the mass properties from the hub and the state effectors @@ -17,9 +17,9 @@ \section{Model Assumptions and Limitations} \begin{itemize} \item Translational only mode cannot have state effectors attached to it that change the mass properties of the spacecraft. However, a state effector like a battery could be used since this does not change the mass properties of the spacecraft. - \item Rotational only mode can only have state effectors attached to it that do not change the mass properties of the spacecraft, i.e. balanced reaction wheels, and the $\cal{B}$ frame origin must coincident with the center of mass of the spacecraft. + \item Rotational only mode can only have state effectors attached to it that do not change the mass properties of the spacecraft, i.e. balanced reaction wheels, and the $\cal{B}$ frame origin must coincident with the center of mass of the spacecraft. \item State effectors that are changing the mass properties of the spacecraft are considered new bodies that are added to the mass properties of the spacecraft. For example, adding flexing solar panels to the sim would require subtracting the mass and inertia of the solar panels from the total spacecraft which would then represent the rigid body hub, and then when the solar panels are added to the sim, their mass inertia are programatically added back to the spacecraft. - \item The limitations of the sim are primarily based on what configuration you have set the spacecraft in (i.e. what state effectors and dynamic effectors you have attached to the spacecraft). Additionally you are limited to the current capability of Basilisk in regards to state effectors and dynamic effectors. + \item The limitations of the sim are primarily based on what configuration you have set the spacecraft in (i.e. what state effectors and dynamic effectors you have attached to the spacecraft). Additionally you are limited to the current capability of Basilisk in regards to state effectors and dynamic effectors. \item The accuracy of the simulation is based upon the integrator and integrator step size - \item As discussed in the description section, the body fixed frame $\cal{B}$ can be oriented generally and the origin of the $\cal{B}$ frame can be placed anywhere as long as it is fixed with respect to the body. This means that there no limitations from the rigid body hub perspective. -\end{itemize} \ No newline at end of file + \item As discussed in the description section, the body fixed frame $\cal{B}$ can be oriented generally and the origin of the $\cal{B}$ frame can be placed anywhere as long as it is fixed with respect to the body. This means that there no limitations from the rigid body hub perspective. +\end{itemize} diff --git a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secTest.tex b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secTest.tex index dbc685f3f3..a47faafd31 100644 --- a/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secTest.tex +++ b/src/simulation/dynamics/spacecraft/_Documentation/Spacecraft/secTest.tex @@ -4,7 +4,7 @@ \section{Test Description and Success Criteria} This test is located in \texttt{simulation/dynamics/SpacecraftDynamics/spacecraft/\_UnitTest/\newline test\_spacecraft.py}. Depending on the scenario, there are different success criteria. These are outlined in the following subsections: \subsection{Translation Only with Gravity Scenario} -In this test the simulation is placed into orbit around Earth with point gravity and the spacecraft only has translational equations being evaluated. The following parameters are being tested. +In this test the simulation is placed into orbit around Earth with point gravity and the spacecraft only has translational equations being evaluated. The following parameters are being tested. \begin{itemize} \item Conservation of orbital angular momentum \item Conservation of orbital energy @@ -22,7 +22,7 @@ \subsection{Rotational Only Scenario} \end{itemize} The calculations for both the switching of the MRPs and the BOE for rotational dynamics need to be further discussed. The following sections outline these checks. -\subsubsection{MRP switching test} +\subsubsection{MRP switching test} The MRP switching check needs to be discussed. In Basilisk the MRPs are switched to the shadow set in \textit{hubEffector} after one step of the integration using the method \textit{modifyStates()} which is available to all \textit{stateEffectors} that need to change their states to a different but equivalent form. The MRP switching adheres to the following equation\cite{schaub}: \begin{equation} @@ -36,7 +36,7 @@ \subsubsection{MRP switching test} \label{eq:ballin} \end{equation} -To check that the switch in the simulation is behaving the way it should, the following check was developed. If the switch happened at time $t_\textnormal{s}$, then there are two variables from the sim that will be used: $\bm \sigma(t_\textnormal{s-1})$ and $\bm \sigma(t_\textnormal{s})$. The intermediate MRP that is switched in the sim is not an output of the simulation, but we will call this variable $\bm \sigma_0(t_\textnormal{s})$. To check the switching the following math occurs: +To check that the switch in the simulation is behaving the way it should, the following check was developed. If the switch happened at time $t_\textnormal{s}$, then there are two variables from the sim that will be used: $\bm \sigma(t_\textnormal{s-1})$ and $\bm \sigma(t_\textnormal{s})$. The intermediate MRP that is switched in the sim is not an output of the simulation, but we will call this variable $\bm \sigma_0(t_\textnormal{s})$. To check the switching the following math occurs: \begin{equation} \bm \sigma_0(t_\textnormal{s}) \approx \bm \sigma(t_\textnormal{s-1}) + \frac{\bm \sigma(t_\textnormal{s-1}) - \bm \sigma(t_\textnormal{s-2})}{\Delta t} \Delta t @@ -109,7 +109,7 @@ \subsection{Translational and Rotational Scenario} \subsection{Translational BOE Calculation Scenario} -The translational BOE calculation can be seen in Figure~\ref{fig:BOETrans}. In this test a positive force is placed on the hub in the $\hat{\bm b}_1$ direction with no torque and no initial rotation of the spacecraft. This results in the 1 degree of freedom problem seen in Figure~\ref{fig:BOETrans}. The force is applied for some length of time, left off for a length of time, and then a negative force is applied ot the system for some length of time. The test is ensuring that Basilisk is giving the same results as the BOE calculation. +The translational BOE calculation can be seen in Figure~\ref{fig:BOETrans}. In this test a positive force is placed on the hub in the $\hat{\bm b}_1$ direction with no torque and no initial rotation of the spacecraft. This results in the 1 degree of freedom problem seen in Figure~\ref{fig:BOETrans}. The force is applied for some length of time, left off for a length of time, and then a negative force is applied ot the system for some length of time. The test is ensuring that Basilisk is giving the same results as the BOE calculation. \begin{figure}[htbp] \centerline{ @@ -134,13 +134,13 @@ \section{Test Parameters} Since this is an integrated test, the inputs to the test are the physical parameters of the spacecraft along with the initial conditions of the states. These parameters are outlined in Tables~\ref{tab:hub}-~\ref{tab:initial}. Additionally, the error tolerances can be seen in Table~\ref{tab:errortol}. -The energy/momentum conservation, rotational BOE, and translational BOE relative tolerance values were chosen to be 1e-10 to ensure cross-platform success. However, the expected relative tolerance for these three tests are $\approx$ 1e-15, which is about machine precision. This is because the integration time step was low enough that the integrator was introducing very small errors with respect to the exact solution. This gives great confidence in the simulations. The position and attitude relative tolerance values (including Point B Vs. Point C tests) were chosen because the values saved in python to compare to had 10 significant digits. Therefore, 1e-8 was chosen to ensure platform success. This agreement is very good and gives further confidence in the solution. Finally, the MRP switching relative tolerance was chosen to be 1e-5 because of the time step dependency of this test. Since the test involves a numerical difference the accuracy gets better the smaller the step size. For example with a step size of 0.001, the resulting relative accuracy was 1e-7 but if a step size of 0.001 is used, 1e-13 was the resulting relative accuracy. Therefore, 1e-5 was chosen to ensure cross platform success and to use a step size of 0.001 for speed. However, since as the step size goes down the simulation approaches the analytical solution, this tests gives very good confidence in the simulation. +The energy/momentum conservation, rotational BOE, and translational BOE relative tolerance values were chosen to be 1e-10 to ensure cross-platform success. However, the expected relative tolerance for these three tests are $\approx$ 1e-15, which is about machine precision. This is because the integration time step was low enough that the integrator was introducing very small errors with respect to the exact solution. This gives great confidence in the simulations. The position and attitude relative tolerance values (including Point B Vs. Point C tests) were chosen because the values saved in python to compare to had 10 significant digits. Therefore, 1e-8 was chosen to ensure platform success. This agreement is very good and gives further confidence in the solution. Finally, the MRP switching relative tolerance was chosen to be 1e-5 because of the time step dependency of this test. Since the test involves a numerical difference the accuracy gets better the smaller the step size. For example with a step size of 0.001, the resulting relative accuracy was 1e-7 but if a step size of 0.001 is used, 1e-13 was the resulting relative accuracy. Therefore, 1e-5 was chosen to ensure cross platform success and to use a step size of 0.001 for speed. However, since as the step size goes down the simulation approaches the analytical solution, this tests gives very good confidence in the simulation. \begin{table}[htbp] \caption{Spacecraft Hub Parameters} \label{tab:hub} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline @@ -160,18 +160,18 @@ \section{Test Parameters} \caption{Initial Conditions of the simulations} \label{tab:initial} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | p{2.25in} | c | c } % Column formatting, + \begin{tabular}{ c | p{2.25in} | c | c } % Column formatting, \hline \textbf{Name} & \textbf{Description} & \textbf{Value} & \textbf{Units} \\ \hline r\_CN\_NInit & Initial Position of S/C (gravity scenarios) & $\begin{bmatrix} - -4020339 & 7490567 & 5248299 + -4020339 & 7490567 & 5248299 \end{bmatrix}^T$ & m \\ v\_CN\_NInit & Initial Velocity of S/C (gravity scenarios) & $\begin{bmatrix} -5199.78 & -3436.68 & 1041.58 \end{bmatrix}^T$ & m/s \\ r\_CN\_NInit & Initial Position of S/C (no gravity) & $\begin{bmatrix} - 0.0 & 0.0 & 0.0 + 0.0 & 0.0 & 0.0 \end{bmatrix}^T$ & m \\ v\_CN\_NInit & Initial Velocity of S/C (no gravity) & $\begin{bmatrix} 0.0 & 0.0 & 0.0 @@ -196,7 +196,7 @@ \section{Test Parameters} \caption{Error Tolerance - Note: Relative Tolerance is $\textnormal{abs}(\frac{\textnormal{truth} - \textnormal{value}}{\textnormal{truth}}$)} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline \textbf{Test} & \textbf{Relative Tolerance} \\ \hline @@ -207,7 +207,7 @@ \section{Test Parameters} Rotational BOE & 1e-10 \\ Translational BOE & 1e-10 \\ Point B Vs. Point C & 1e-8 \\ - \hline + \hline \end{tabular} \end{table} @@ -318,4 +318,4 @@ \subsection{Dynamics Calculated About Point B Test} \caption{PointB Vs PointC Attitude} \label{fig:PointBVsPointCAttitude} \end{figure} -\clearpage \ No newline at end of file +\clearpage diff --git a/src/simulation/dynamics/spacecraft/spacecraft.rst b/src/simulation/dynamics/spacecraft/spacecraft.rst index 604fa3bbf8..34ed43f90e 100644 --- a/src/simulation/dynamics/spacecraft/spacecraft.rst +++ b/src/simulation/dynamics/spacecraft/spacecraft.rst @@ -118,23 +118,3 @@ This section is to outline the steps needed to setup a Spacecraft module in pyth * - r_BcB_B - double[3] - Center of mass location in B frame - - - - - - - - - - - - - - - - - - - - diff --git a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/AVS.sty b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/AVS.sty +++ b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/Basilisk-SPACECRAFTSYSTEM-20180712.tex b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/Basilisk-SPACECRAFTSYSTEM-20180712.tex index d9e45d177a..3a1987d120 100755 --- a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/Basilisk-SPACECRAFTSYSTEM-20180712.tex +++ b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/Basilisk-SPACECRAFTSYSTEM-20180712.tex @@ -90,7 +90,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/BasiliskReportMemo.cls b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secModelDescription.tex b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secModelDescription.tex index d77aaf9f41..fb59eedff3 100644 --- a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secModelDescription.tex +++ b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secModelDescription.tex @@ -2,39 +2,39 @@ \section{Progress Explanation and Current Status} -This module is IN PROGRESS and this section lists the current progress and status of the module as capability is added to the module this document needs to be updated. This document should always be describing the current status of the module. +This module is IN PROGRESS and this section lists the current progress and status of the module as capability is added to the module this document needs to be updated. This document should always be describing the current status of the module. Below is a list describing the limitations and status of the module \begin{itemize} \item The first major status that needs to be understood is that the HingedRigidBody stateEffector is the only stateEffector at this time that is ready to be used with the multi-spacecraft architecture. The reason that each stateEffector needs to be updated is because it was found with stateEffectors that have multiple degrees of freedom change the generalized interaction between and the individual stateEffectors. However, this change is relatively small to each stateEffector as long as the developer understands the steps to make the change. The architecture is proven to work with hingedRigidBodies and each stateEffector just needs to follow suit. - + So in other words, if a stateEffector wants the capability of being able to be simulated in the multi-spacecraft environment some modifications need to be implemented to the specific stateEffectors. The hingedRigidBody stateEffector acts as a detailed example on the method to convert a stateEffector to allow for the multi-spacecraft interaction. Below is a further on how to implement this change: \begin{itemize} \item The {\tt prependSpacecraftNameToStates()} method needs to be implemented for the corresponding stateEffector states just like hingedRigidBody does for its states. This allows for the stateEffector to know about the individual spacecraft units that it is attached to. \item The {\tt linkInStates} method needs to prepend the linked states from the spacecraft it is attached to, to get the corresponding state from the spacecraft. However, it should be noted that you can see the hingedRigidBody no longer needs access to the hubs primary state variables. This is because the backSubstitution methods now provide the stateEffectors with the appropriate Hub states in the method calls. Therefore there is not a need for stateEffectors to link to any of the Hubs state variables. \item The last step is to provide the contributions from the stateEffectors about point $P$ and frame $\cal P$. Again, this cannot be generalized because of multiple degree of freedom effectors so each effector has the job of being provide their contributions about point $P$ and frame $\cal P$. It should be noted that if the spacecraft is independent then point $B$ will be coincident with point $P$ and dcmPB will be identity. Therefore the math should work for both connected and unconnected spacecraft. Let me explain this in more detail with the hingedRigidBody: - - You can see that in the private variables in hingedRigidBody, that the variables have been changed to be point $P$ and $\cal P$ relative. Again, if single spacecraft is being implemented then frames $\cal B$ and $\cal P$ are coincident. In the stateEffector.h it shows that each stateEffector has access to these two crucial variables: r\_BP\_P and dcm\_BP. r\_BP\_P is defaulted to zero and dcm\_BP is defaulted to identity. With that knowledge, each stateEffector needs to use those variables and defined their contributions about about point $P$ and frame $\cal P$. about point $P$ and frame $\cal P$. The easiest way to implement this correctly, is to to change any internal variables (hopefully they are already indicated as private variables) to point $P$ and frame $\cal P$ relative. Therefore, values being set by python shouldn't be changed, but internal variables or intermediate variables should be changed. + + You can see that in the private variables in hingedRigidBody, that the variables have been changed to be point $P$ and $\cal P$ relative. Again, if single spacecraft is being implemented then frames $\cal B$ and $\cal P$ are coincident. In the stateEffector.h it shows that each stateEffector has access to these two crucial variables: r\_BP\_P and dcm\_BP. r\_BP\_P is defaulted to zero and dcm\_BP is defaulted to identity. With that knowledge, each stateEffector needs to use those variables and defined their contributions about about point $P$ and frame $\cal P$. about point $P$ and frame $\cal P$. The easiest way to implement this correctly, is to to change any internal variables (hopefully they are already indicated as private variables) to point $P$ and frame $\cal P$ relative. Therefore, values being set by python shouldn't be changed, but internal variables or intermediate variables should be changed. \item Once a model has been changed to allow for multi spacecraft, it can be tested by using the test\_multi-spacecraft and add the stateEffector to the spacecraft system just like hingedRigidBodies are. If energy and momentum is conserved the stateEffector most likely was changed correctly. Obviously all other tests should pass. \end{itemize} -The following table describes the current status of each stateEffector being implemented in the multi-spacecraft environment. This table should be updated as stateEffectors have this added capability. +The following table describes the current status of each stateEffector being implemented in the multi-spacecraft environment. This table should be updated as stateEffectors have this added capability. \begin{table}[htbp] \caption{Test results.} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | c } % Column formatting, + \begin{tabular}{c | c | c } % Column formatting, \hline \textbf{Test} & \textbf{Pass/Fail} & \\ \hline HingedRigidBodies & \textcolor{ForestGreen}{Implemented} & \textcolor{ForestGreen}{Tested} \\ - LinearSpringMassDamper & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ - SphericalPendulum & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ - FuelTank & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ - VSCMGs & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ - dualHingedRigidBodies & \textcolor{ForestGreen}{Implemented} & \textcolor{Red}{Not Tested} \\ - nHingedRigidBodies & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ + LinearSpringMassDamper & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ + SphericalPendulum & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ + FuelTank & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ + VSCMGs & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ + dualHingedRigidBodies & \textcolor{ForestGreen}{Implemented} & \textcolor{Red}{Not Tested} \\ + nHingedRigidBodies & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ \hline \end{tabular} \end{table} @@ -46,31 +46,31 @@ \section{Progress Explanation and Current Status} \caption{Test results.} \label{tab:results2} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | c } % Column formatting, + \begin{tabular}{c | c | c } % Column formatting, \hline \textbf{Test} & \textbf{Pass/Fail} & \\ \hline - Thrusters & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ - ExtForceTorque & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ - ExtPulsedForceTorque & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ - dragEffector & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ - radiationPressure & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ + Thrusters & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ + ExtForceTorque & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ + ExtPulsedForceTorque & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ + dragEffector & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ + radiationPressure & \textcolor{Red}{Not Implemented} & \textcolor{Red}{Not Tested} \\ \hline \end{tabular} \end{table} \item On further evaluation, there is a bug believed to be in {\tt spacecraftSystem} with the multi-connected hubs. The attached hubEffectors needs to provide their contribution to the primary spacecraft about point $P$ and $\cal P$. Just like the other stateEffectors the hubEffector needs to be changed. This should be a small change, but nees to be done! The tests in this document do not show this bug because the contributions are not being added in {\tt spacecraftSystem}. So to be clear there is a bug in both {\tt spacecraftSystem} (because the attached hubs are not being looped over) and in the hubEffector (not providing contributions about point $P$ and $\cal P$) This is BUG that needs to be fixed. - + \item The docking and detachment of {\tt spacecraftUnit} needs to be implemented in the {\tt spacecraftSystem} class. Currently the docking and detaching could be done using python (by stopping the sim at a specific time step and dock/detach from python) but this is not the intended use of this multi-spacecraft architecture. This NEEDS to be implemented in the {\tt spacecraftSystem} class that would allow for a message to be written out to the system which would allow for detachment and docking of the {\tt spacecraftUnit}. The kinematics of the docking locations would need to be taken into account in this docking/detaching method. (DockingData is the struct that holds the docking information of each dock) However, once this method is added, the rest of architecture would allow for the docking and detaching to happen seamlessly. The docking and detaching method should be called at the end of the {\tt SpacecraftSystem::integrateState} method. -\item The interaction between sensors and environment models has yet to be tested for the multi-spacecraft architecture. - +\item The interaction between sensors and environment models has yet to be tested for the multi-spacecraft architecture. + \end{itemize} \section{Model Description} \subsection{Introduction} -A detailed description of this architecture can be seen in: \url{http://hanspeterschaub.info/Papers/grads/CodyAllard.pdf}. Figure~\ref{fig:FlexSloshFigure} shows a schematic of what this module is intended to to do: simulate multiple spacecraft units at a time that can dock and detach throughout the simulation. +A detailed description of this architecture can be seen in: \url{http://hanspeterschaub.info/Papers/grads/CodyAllard.pdf}. Figure~\ref{fig:FlexSloshFigure} shows a schematic of what this module is intended to to do: simulate multiple spacecraft units at a time that can dock and detach throughout the simulation. \begin{figure}[htbp] \centerline{ diff --git a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secModelFunctions.tex b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secModelFunctions.tex index 107443f898..4854d438cc 100644 --- a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secModelFunctions.tex +++ b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secModelFunctions.tex @@ -4,4 +4,4 @@ \section{Model Functions} \section{Model Assumptions and Limitations} -Please see the first section describing current progress of this module. \ No newline at end of file +Please see the first section describing current progress of this module. diff --git a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secTest.tex b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secTest.tex index e83269fac0..3f7a96703f 100644 --- a/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secTest.tex +++ b/src/simulation/dynamics/spacecraftSystem/_Documentation/SpacecraftSystem/secTest.tex @@ -2,7 +2,7 @@ \section{Test Description and Success Criteria} This test is located in \texttt{simulation/dynamics/spacecraftDynamics/\_UnitTest/\newline -test\_multiSpacecraft.py}. +test\_multiSpacecraft.py}. \subsection{SCConnected Test} This test ensures that a spacecraft system with hingedRigidBodies attached to it conserves energy and momentum. @@ -129,4 +129,4 @@ \subsection{SCConnectedAndUnconnected Test} \label{fig:ChangeInRotationalEnergy2} \end{figure} -\clearpage \ No newline at end of file +\clearpage diff --git a/src/simulation/dynamics/spacecraftSystem/spacecraftSystem.rst b/src/simulation/dynamics/spacecraftSystem/spacecraftSystem.rst index a3fe589261..e08b02391a 100644 --- a/src/simulation/dynamics/spacecraftSystem/spacecraftSystem.rst +++ b/src/simulation/dynamics/spacecraftSystem/spacecraftSystem.rst @@ -1,7 +1,7 @@ .. warning:: This module allows for multiple spacecraft units (mother craft and a docked daughter craft, etc.) to be simulated as an integrated dynamical system. See `Dr. Cody Allard's dissertation `__ for more information. However, this is still work in progress and not all effectors are compatible with this manner of doing the dynamics. Use :ref:`spacecraft` to create a spacecraft simulation object unless you are familiar what this expanded spacecraft dynamics module provides. - + Executive Summary ----------------- @@ -41,6 +41,3 @@ provides information on what this message is used for. * - scEnergyMomentumOutMsg - :ref:`SCEnergyMomentumMsgPayload` - spacecraft element energy and momentum output message - - - diff --git a/src/simulation/dynamics/sphericalPendulum/_Documentation/AVS.sty b/src/simulation/dynamics/sphericalPendulum/_Documentation/AVS.sty index 73e5dd7956..c02abd9dfe 100644 --- a/src/simulation/dynamics/sphericalPendulum/_Documentation/AVS.sty +++ b/src/simulation/dynamics/sphericalPendulum/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/dynamics/sphericalPendulum/_Documentation/Basilisk-SPHERICALPENDULUM-20180518.tex b/src/simulation/dynamics/sphericalPendulum/_Documentation/Basilisk-SPHERICALPENDULUM-20180518.tex index 3b2417dd56..658d680a32 100755 --- a/src/simulation/dynamics/sphericalPendulum/_Documentation/Basilisk-SPHERICALPENDULUM-20180518.tex +++ b/src/simulation/dynamics/sphericalPendulum/_Documentation/Basilisk-SPHERICALPENDULUM-20180518.tex @@ -91,7 +91,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/dynamics/sphericalPendulum/_Documentation/BasiliskReportMemo.cls b/src/simulation/dynamics/sphericalPendulum/_Documentation/BasiliskReportMemo.cls index 1e869ba0c3..6256f116db 100644 --- a/src/simulation/dynamics/sphericalPendulum/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/dynamics/sphericalPendulum/_Documentation/BasiliskReportMemo.cls @@ -115,4 +115,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/dynamics/sphericalPendulum/_Documentation/references.bib b/src/simulation/dynamics/sphericalPendulum/_Documentation/references.bib index 4370122e9b..05dacf79dd 100644 --- a/src/simulation/dynamics/sphericalPendulum/_Documentation/references.bib +++ b/src/simulation/dynamics/sphericalPendulum/_Documentation/references.bib @@ -243,4 +243,4 @@ @article{akyildiz2006 pages={2135--2149}, year={2006}, publisher={Elsevier} -} \ No newline at end of file +} diff --git a/src/simulation/dynamics/sphericalPendulum/_Documentation/secModelDescription.tex b/src/simulation/dynamics/sphericalPendulum/_Documentation/secModelDescription.tex index 88d851b535..f774a714a4 100644 --- a/src/simulation/dynamics/sphericalPendulum/_Documentation/secModelDescription.tex +++ b/src/simulation/dynamics/sphericalPendulum/_Documentation/secModelDescription.tex @@ -9,17 +9,17 @@ \subsection{Problem Statement} \includegraphics[width=13cm]{Figures/spacecraft.pdf} \caption{Frame and variable definitions used for formulation} \label{fig:Slosh_Figure} -\end{figure} +\end{figure} -There are four coordinate frames defined for this formulation. The inertial reference frame is indicated by \frameDefinition{N}. The body fixed coordinate frame, \frameDefinition{B}, which is anchored to the hub and can be oriented in any direction. The initial pendulum frame, $\mathcal{P}_{0,j}:\{\hat{\bm p}_{0_j,1},\hat{\bm p}_{0_j,2},\hat{\bm p}_{0_j,3}\}$, is a frame with its origin located at tank geometrical center, $T$. The $\mathcal{P}_{0,j}$ frame is a fixed frame respect to the body frame, oriented such that $\hat{\bm{p}}_{0_j,1}$ points to the fuel slosh mass in its initial position, $P_{j}$. The constant distance from point $T$ to point $P_{j}$ is defined as $l_j$. +There are four coordinate frames defined for this formulation. The inertial reference frame is indicated by \frameDefinition{N}. The body fixed coordinate frame, \frameDefinition{B}, which is anchored to the hub and can be oriented in any direction. The initial pendulum frame, $\mathcal{P}_{0,j}:\{\hat{\bm p}_{0_j,1},\hat{\bm p}_{0_j,2},\hat{\bm p}_{0_j,3}\}$, is a frame with its origin located at tank geometrical center, $T$. The $\mathcal{P}_{0,j}$ frame is a fixed frame respect to the body frame, oriented such that $\hat{\bm{p}}_{0_j,1}$ points to the fuel slosh mass in its initial position, $P_{j}$. The constant distance from point $T$ to point $P_{j}$ is defined as $l_j$. There are a few more key locations that need to be defined. Point $B$ is the origin of the body frame, and can have any location with respect to the hub. Point $B_c$ is the location of the center of mass of the rigid hub. $P_{c,j}$ is the instantaneous position of the fuel slosh mass $m_j$. $\bm{d}$ is vector from the center of the body reference system to the tank geometrical center. $\bm{l_j}$ is the vector from $T$ to $P_{c,j}$. -Figure~\ref{fig:Slosh_Detailed} provides further detail of the fuel slosh parameters and reference frames. As seen in Figure~\ref{fig:Slosh_Figure}, an individual slosh particle is free to move in every direction while connected by rigid weightless rod to the geometrical center of the tank. A linear damper effect is considered using a damping matrix, $D$. The variables, $\varphi_j$ and $\vartheta_j$ are state variables and quantify the angular displacement from initial position for the corresponding slosh mass. +Figure~\ref{fig:Slosh_Detailed} provides further detail of the fuel slosh parameters and reference frames. As seen in Figure~\ref{fig:Slosh_Figure}, an individual slosh particle is free to move in every direction while connected by rigid weightless rod to the geometrical center of the tank. A linear damper effect is considered using a damping matrix, $D$. The variables, $\varphi_j$ and $\vartheta_j$ are state variables and quantify the angular displacement from initial position for the corresponding slosh mass. \begin{figure}[ht] \centering - + \includegraphics[width=13cm]{Figures/referencesystems.pdf} \caption{Further detail of fuel slosh and reference frames} \label{fig:Slosh_Detailed} @@ -44,7 +44,7 @@ \subsubsection{Rigid Spacecraft Hub Translational Motion} The definition of $\bm{c}$ can be seen in Eq. (\ref{eq:c}). \begin{equation} \bm{c} = \frac{1}{m_{\text{sc}}}\Big(m_{\text{\text{hub}}}\bm{r}_{B_{c}/B} +\sum_{j=1}^{N_{P}}m_j\bm{r}_{P_{c,j}/B}\Big) - \label{eq:c} + \label{eq:c} \end{equation} To find the inertial time derivative of $\bm{c}$, it is first necessary to find the time derivative of $\bm{c}$ with respect to the body frame. A time derivative of any vector, $\bm{v}$, with respect to the body frame is denoted by $\bm{v}'$; the inertial time derivative is labeled as $\dot{\bm{v}}$. The first and second body-relative time derivatives of $\bm{c}$ can be seen in Eqs. (\ref{eq:cprime}) and (\ref{eq:cdprime}). \begin{align} @@ -56,7 +56,7 @@ \subsubsection{Rigid Spacecraft Hub Translational Motion} \end{align} Remembering that the derivative of $d$ is null respect to the body frame, the first and second body time derivatives of $\bm{r}_{P_{c,j}/B}$ are \begin{equation} - \bm{r}_{P_{c,j}/B} = + \bm{r}_{P_{c,j}/B} = l_j \leftidx{^{\mathcal{P}_{0,j}}} {\begin{bmatrix} @@ -69,9 +69,9 @@ \subsubsection{Rigid Spacecraft Hub Translational Motion} \end{equation} \begin{equation} - \bm{r}'_{P_{c,j}/B} + \bm{r}'_{P_{c,j}/B} = - l_j + l_j \leftidx{^{\mathcal{P}_{0,j}}} {\begin{bmatrix} -\dot{\varphi}_j\sin(\varphi_j)\cos(\vartheta_j)-\dot{\vartheta}_j\cos(\varphi_j)\sin(\vartheta_j) \\ @@ -82,7 +82,7 @@ \subsubsection{Rigid Spacecraft Hub Translational Motion} \end{equation} \begin{equation} - \bm{r}''_{P_{c,j}/B} + \bm{r}''_{P_{c,j}/B} = l_j \leftidx{^{\mathcal{P}_{0,j}}} @@ -105,7 +105,7 @@ \subsubsection{Rigid Spacecraft Hub Translational Motion} \begin{multline} \bm{c}'' = \frac{1}{m_{\text{sc}}}\sum_{j=1}^{N_{P}}m_j l_j \bigg[ - \Big(-\ddot{\varphi}_j\sin(\varphi_j)\cos(\vartheta_j)-\ddot{\vartheta}_j\cos(\varphi_j)\sin(\vartheta_j)-\dot{\varphi}_j^2\cos(\varphi_j)\cos(\vartheta_j)-\dot{\vartheta}_j^2\cos(\varphi_j)\cos(\vartheta_j)\\+2\dot{\varphi}_j\dot{\vartheta}_j\sin(\varphi_j)\sin(\vartheta_j) \Big)\bm{\hat{p}}_{0_j,1} + \Big(-\ddot{\varphi}_j\sin(\varphi_j)\cos(\vartheta_j)-\ddot{\vartheta}_j\cos(\varphi_j)\sin(\vartheta_j)-\dot{\varphi}_j^2\cos(\varphi_j)\cos(\vartheta_j)-\dot{\vartheta}_j^2\cos(\varphi_j)\cos(\vartheta_j)\\+2\dot{\varphi}_j\dot{\vartheta}_j\sin(\varphi_j)\sin(\vartheta_j) \Big)\bm{\hat{p}}_{0_j,1} +\Big(\ddot{\varphi}_j\cos(\varphi_j)\cos(\vartheta_j)-\ddot{\vartheta}_j\sin(\varphi_j)\sin(\vartheta_j)-\dot{\varphi}_j^2\sin(\varphi_j)\cos(\vartheta_j)\\-\dot{\vartheta}_j^2\sin(\varphi_j)\cos(\vartheta_j)-2\dot{\varphi}_j\dot{\vartheta}_j\cos(\varphi_j)\sin(\vartheta_j) \Big)\bm{\hat{p}}_{0_j,2} +\Big(-\ddot{\vartheta}_j\cos(\vartheta_j)+\dot{\vartheta}_j^2\sin(\vartheta_j) \Big)\bm{\hat{p}}_{0_j,3} \bigg] @@ -126,7 +126,7 @@ \subsubsection{Rigid Spacecraft Hub Translational Motion} \begin{multline} \ddot{\bm r}_{B/N} = \ddot{\bm r}_{C/N}-\frac{1}{m_{\text{sc}}}\sum_{j=1}^{N_{P}}m_j l_j \bigg[ - \Big(-\ddot{\varphi}_j\sin(\varphi_j)\cos(\vartheta_j)-\ddot{\vartheta}_j\cos(\varphi_j)\sin(\vartheta_j)-\dot{\varphi}_j^2\cos(\varphi_j)\cos(\vartheta_j)\\-\dot{\vartheta}_j^2\cos(\varphi_j)\cos(\vartheta_j)+2\dot{\varphi}_j\dot{\vartheta}_j\sin(\varphi_j)\sin(\vartheta_j) \Big)\bm{\hat{p}}_{0_j,1} + \Big(-\ddot{\varphi}_j\sin(\varphi_j)\cos(\vartheta_j)-\ddot{\vartheta}_j\cos(\varphi_j)\sin(\vartheta_j)-\dot{\varphi}_j^2\cos(\varphi_j)\cos(\vartheta_j)\\-\dot{\vartheta}_j^2\cos(\varphi_j)\cos(\vartheta_j)+2\dot{\varphi}_j\dot{\vartheta}_j\sin(\varphi_j)\sin(\vartheta_j) \Big)\bm{\hat{p}}_{0_j,1} +\Big(\ddot{\varphi}_j\cos(\varphi_j)\cos(\vartheta_j)-\ddot{\vartheta}_j\sin(\varphi_j)\sin(\vartheta_j)\\-\dot{\varphi}_j^2\sin(\varphi_j)\cos(\vartheta_j)-\dot{\vartheta}_j^2\sin(\varphi_j)\cos(\vartheta_j)-2\dot{\varphi}_j\dot{\vartheta}_j\cos(\varphi_j)\sin(\vartheta_j) \Big)\bm{\hat{p}}_{0_j,2} +\Big(-\ddot{\vartheta}_j\cos(\vartheta_j)\\+\dot{\vartheta}_j^2\sin(\vartheta_j) \Big)\bm{\hat{p}}_{0_j,3} \bigg] @@ -271,7 +271,7 @@ \subsubsection{Fuel Slosh Motion} \begin{equation} \bm{H}_{T,j}= m_j \bm{l_j}\times \bm{\dot{l}_j} \end{equation} -Deriving this equation we obtain: +Deriving this equation we obtain: \begin{equation} \bm{\dot{H}}_{T,j}=m_j\bm{l_j}\times \bm{\ddot{l_j}} \label{eq:dotH_T_particle} @@ -284,7 +284,7 @@ \subsubsection{Fuel Slosh Motion} $\bm{l_j}$ prime and second derivative are equal to \eqref{eq:rPcjBprime} and \eqref{eq:rPcjBprimeprime} because the $\bm{d}$ vector is constant in body frame. The previous results are reported here: \begin{equation} - \bm{l_j} = + \bm{l_j} = l_j \leftidx{^{\mathcal{P}_{0,j}}} {\begin{bmatrix} @@ -296,9 +296,9 @@ \subsubsection{Fuel Slosh Motion} \end{equation} \begin{equation} - \bm{l_j}' + \bm{l_j}' = - l_j + l_j \leftidx{^{\mathcal{P}_{0,j}}} {\begin{bmatrix} -\dot{\varphi}_j\sin(\varphi_j)\cos(\vartheta_j)-\dot{\vartheta}_j\cos(\varphi_j)\sin(\vartheta_j) \\ @@ -309,7 +309,7 @@ \subsubsection{Fuel Slosh Motion} \end{equation} \begin{equation} - \bm{l_j}'' + \bm{l_j}'' = l_j \leftidx{^{\mathcal{P}_{0,j}}} @@ -367,7 +367,7 @@ \subsubsection{Fuel Slosh Motion} \end{bmatrix}} \label{eq:FS3} \end{multline} -Using the results obtained in eq. \eqref{eq:FS3}, project eq. \eqref{eq:FSE2} on the axis of rotation of $\varphi$ and $\vartheta$. The $\varphi$ rotation axis is $\bm{\hat{p}}_{0_j,3}$, while we will call $\bm{\hat{p}'}_{0_j,2}$ the $\vartheta$ rotation axis. +Using the results obtained in eq. \eqref{eq:FS3}, project eq. \eqref{eq:FSE2} on the axis of rotation of $\varphi$ and $\vartheta$. The $\varphi$ rotation axis is $\bm{\hat{p}}_{0_j,3}$, while we will call $\bm{\hat{p}'}_{0_j,2}$ the $\vartheta$ rotation axis. \begin{figure}[ht] \centering @@ -430,9 +430,9 @@ \subsubsection{Fuel Slosh Motion} \subsection{Back-substitution Method} -The equations presented in the previous sections result in $2N_P + 6$ coupled differential equations. Therefore, if the EOMs were placed into state space form, a system mass matrix of size $2N_P + 6$ would need to be inverted to numerically integrate the EOMs. This can result in a computationally expensive simulation. The computation effort to numerically invert an $N\times N$ matrix scales with $N^{3}$. In the following section, the EOMs are manipulated using a back-substitution method to increase the computational efficiency. +The equations presented in the previous sections result in $2N_P + 6$ coupled differential equations. Therefore, if the EOMs were placed into state space form, a system mass matrix of size $2N_P + 6$ would need to be inverted to numerically integrate the EOMs. This can result in a computationally expensive simulation. The computation effort to numerically invert an $N\times N$ matrix scales with $N^{3}$. In the following section, the EOMs are manipulated using a back-substitution method to increase the computational efficiency. -This manipulation involves inverting twice a ($3\times 3$) matrix, the $A^{-1}$ and $(D-CA^{-1}B)^{-1}$ matrices as it is shown in eqs. \eqref{eq:finalsystemomega} and \eqref{eq:finalsystemr}. Then the system is completely solved back substituting for fuel slosh and translational motions. The derivation of the back-substitution method can be seen in the following sections. +This manipulation involves inverting twice a ($3\times 3$) matrix, the $A^{-1}$ and $(D-CA^{-1}B)^{-1}$ matrices as it is shown in eqs. \eqref{eq:finalsystemomega} and \eqref{eq:finalsystemr}. Then the system is completely solved back substituting for fuel slosh and translational motions. The derivation of the back-substitution method can be seen in the following sections. \\ \subsubsection{Fuel Slosh Motion} @@ -445,13 +445,13 @@ \subsubsection{Fuel Slosh Motion} \begin{equation} \ddot{\varphi}_j =\frac{1}{m_j l_j^2 \cos^2(\vartheta_j)}\Big(m_{j}\bm{\hat{p}}_{0_j,3}^T [\bm{\tilde{l}_j}]( [\bm{\tilde{l}_j}]+ [\bm{\tilde{d}}]) \bm{\dot{\omega}}_{B/N} -m_{j} \bm{\hat{p}}_{0_j,3}^{T} [\bm{\tilde{l}_j}] \bm{\ddot{r}}_{B/N}+a_{\varphi_j} \Big) \label{eq:BSM2} -\end{equation} +\end{equation} Writing it this way instead \begin{equation} \ddot{\varphi}_j = \bm a_{\varphi_j}^T \ddot{\bm{r}}_{B/N} + \bm b_{\varphi_j}^T \dot{\bm\omega}_{\cal B/N} +c_{\varphi_j} \label{eq:BSM3} \end{equation} -Where +Where \begin{equation} \bm a_{\varphi_j}^{T} = -\frac{\bm{\hat{p}}_{0_j,3}^{T} [\bm{\tilde{l}_j}]}{ l_j^2 \cos^2(\vartheta_j)} @@ -466,11 +466,11 @@ \subsubsection{Fuel Slosh Motion} \begin{multline} c_{\varphi_j} = \frac{1}{m_j l_j^2 \cos^2(\vartheta_j)}\Big\{-m_{j} \bm{\hat{p}}_{0_j,3}^{T} [\bm{\tilde{l}_j}][\bm{\tilde{\omega}}_{\cal B/N}][\bm{\tilde{\omega}}_{\cal B/N}] \bm{d}+\bm{\hat{p}}_{0_j,3}^T \bm{L}_{T,j}\\+2m_j l_j^2\dot{\varphi}_j\dot{\vartheta}\cos(\vartheta_j)\sin(\vartheta_j)- m_j \bm{\hat{p}}_{0_j,3}^T [\bm{\tilde{l}_j}]\bigg[2[\bm{\tilde{\omega}}_{\cal B/N}] \bm{l_j'}+[\bm{\tilde{\omega}}_{\cal B/N}][\bm{\tilde{\omega}}_{\cal B/N}] \bm{l_j}\bigg]\Big\} \label{eq:BSM6} -\end{multline} +\end{multline} Doing the same for eq. \eqref{eq:FSE7} \begin{multline} - \ddot{\vartheta}_j + \ddot{\vartheta}_j =\frac{1}{m_j l_j^2}\Big\{m_{j}\bm{\hat{p}}_{0_j,2}^{'T} [\bm{\tilde{l}_j}]( [\bm{\tilde{l}_j}] + [\bm{\tilde{d}}])\bm{\dot{\omega}}_{B/N} -m_{j} \bm{\hat{p}}_{0_j,2}^{'T} [\bm{\tilde{l}_j}] \bm{\ddot{r}}_{B/N}-m_{j} \bm{\hat{p}}_{0_j,2}^{'T}[\bm{\tilde{l}_j}][\bm{\tilde{\omega}}_{\cal B/N}][\bm{\tilde{\omega}}_{\cal B/N}] \bm{d}\\+\bm{\hat{p}}_{0_j,2}^{'T} \bm{L}_{T,j}-m_j l_j^2\dot{\varphi}_j^2\cos(\vartheta_j)\sin(\vartheta_j)- m_j \bm{\hat{p}}_{0_j,2}^{'T} [\bm{\tilde{l}_j}]\bigg[2[\bm{\tilde{\omega}}_{\cal B/N}] \bm{l_j'}+[\bm{\tilde{\omega}}_{\cal B/N}][\bm{\tilde{\omega}}_{\cal B/N}] \bm{l_j}\bigg]\Big\} \label{eq:BSM7} @@ -544,11 +544,11 @@ \subsubsection{Translation} \Big(\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)\bm b_{\varphi_j}^T +\Big(\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1}+\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\cos(\vartheta_j)\bm{\hat{p}}_{0_j,3}\Big)\bm b_{\vartheta_j}^T \bigg] \Big\}\dot{\bm\omega}_{\cal B/N} \\= \ddot{\bm r}_{C/N} - 2[\tilde{\bm\omega}_{\cal B/N}] \bm c' -[\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} - -\frac{1}{m_{\text{sc}}}\sum_{j=1}^{N_{P}}m_j l_j \bigg[ + -\frac{1}{m_{\text{sc}}}\sum_{j=1}^{N_{P}}m_j l_j \bigg[ \Big(-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\-\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)\dot{\varphi}_j^2 +\Big(-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}-\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\sin(\vartheta_j)\bm{\hat{p}}_{0_j,3} \Big)\dot{\vartheta}_j^2 \\+ \Big(2\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1} -2\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)\dot{\varphi}_j\dot{\vartheta}_j - -\Big(\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)c_{\varphi_j} - \Big(\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1}+\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\cos(\vartheta_j)\bm{\hat{p}}_{0_j,3}\Big) c_{\vartheta_j} + -\Big(\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)c_{\varphi_j} - \Big(\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1}+\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\cos(\vartheta_j)\bm{\hat{p}}_{0_j,3}\Big) c_{\vartheta_j} \bigg] \label{eq:Rbddot6} \end{multline} @@ -561,22 +561,22 @@ \subsubsection{Translation} \Big(\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)\bm b_{\varphi_j}^T +\Big(\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1}+\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\cos(\vartheta_j)\bm{\hat{p}}_{0_j,3}\Big)\bm b_{\vartheta_j}^T \bigg] \Big\}\dot{\bm\omega}_{\cal B/N} \\= m_{\text{sc}}\ddot{\bm r}_{C/N} - 2m_{\text{sc}}[\tilde{\bm\omega}_{\cal B/N}] \bm c' -m_{\text{sc}}[\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} - -\sum_{j=1}^{N_{P}}m_j l_j \bigg[ + -\sum_{j=1}^{N_{P}}m_j l_j \bigg[ \Big(-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\-\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)\dot{\varphi}_j^2 +\Big(-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}-\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\sin(\vartheta_j)\bm{\hat{p}}_{0_j,3} \Big)\dot{\vartheta}_j^2 \\+ \Big(2\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1} -2\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)\dot{\varphi}_j\dot{\vartheta}_j - -\Big(\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)c_{\varphi_j} - \Big(\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1}+\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\cos(\vartheta_j)\bm{\hat{p}}_{0_j,3}\Big) c_{\vartheta_j} + -\Big(\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)c_{\varphi_j} - \Big(\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1}+\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\cos(\vartheta_j)\bm{\hat{p}}_{0_j,3}\Big) c_{\vartheta_j} \bigg] \label{eq:Rbddot7} \end{multline} \subsubsection{Rotation} -Same thing for rotation: +Same thing for rotation: \begin{multline} m_{\text{sc}}[\tilde{\bm{c}}]\ddot{\bm r}_{B/N}+[I_{\text{sc},B}] \dot{\bm\omega}_{\cal B/N} - \sum\limits_{j=1}^{N_P}m_j l_j [\tilde{\bm{r}}_{P_{c,j}/B}] \bigg[ - \Big(\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)(\bm a_{\varphi_j}^T \ddot{\bm{r}}_{B/N} + \bm b_{\varphi_j}^T \dot{\bm\omega}_{\cal B/N}+c_{\varphi_j}) + \Big(\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)(\bm a_{\varphi_j}^T \ddot{\bm{r}}_{B/N} + \bm b_{\varphi_j}^T \dot{\bm\omega}_{\cal B/N}+c_{\varphi_j}) +\Big(\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1}+\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}\\+\cos(\vartheta_j)\bm{\hat{p}}_{0_j,3} \Big)(\bm a_{\vartheta_j}^T \ddot{\bm r}_{B/N} + \bm b_{\vartheta_j}^T \dot{\bm\omega}_{\cal B/N} + c_{\vartheta_j}) \bigg] = \\ \bm{L}_B-[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} @@ -602,7 +602,7 @@ \subsubsection{Rotation} +\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\cos(\vartheta_j)\bm{\hat{p}}_{0_j,3} \Big)\bm b_{\vartheta_j}^T \bigg] \Big\}\dot{\bm\omega}_{\cal B/N} - \\= + \\= \bm{L}_B-[\bm{\tilde{\omega}}_{\cal B/N}] [I_{\text{sc},B}] \bm\omega_{\cal B/N} - [I'_{\text{sc},B}] \bm\omega_{\cal B/N}- \sum\limits_{j=1}^{N_P} m_j \Big\{[\tilde{\bm\omega}_{\cal B/N}] [\tilde{\bm{r}}_{P_{c,j}/B}] \bm{r}'_{P_{c,j}/B} \\+l_j[\tilde{\bm{r}}_{P_{c,j}/B}]\bigg[ @@ -648,11 +648,11 @@ \subsubsection{Remaining Back-substitution Steps} \begin{multline} \bm v_{\text{trans}} = m_{\text{sc}}\ddot{\bm r}_{C/N} - 2m_{\text{sc}}[\tilde{\bm\omega}_{\cal B/N}] \bm c' -m_{\text{sc}}[\tilde{\bm\omega}_{\cal B/N}][\tilde{\bm\omega}_{\cal B/N}]\bm{c} - \\-\sum_{j=1}^{N_{P}}m_j l_j \bigg[ + \\-\sum_{j=1}^{N_{P}}m_j l_j \bigg[ \Big(-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}-\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)\dot{\varphi}_j^2 +\Big(-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\-\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\sin(\vartheta_j)\bm{\hat{p}}_{0_j,3} \Big)\dot{\vartheta}_j^2 + \Big(2\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1}\\ -2\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)\dot{\varphi}_j\dot{\vartheta}_j - -\Big(\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)c_{\varphi_j}\\ - \Big(\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1}+\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\cos(\vartheta_j)\bm{\hat{p}}_{0_j,3}\Big) c_{\vartheta_j} + -\Big(\sin(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,1}-\cos(\varphi_j)\cos(\vartheta_j)\bm{\hat{p}}_{0_j,2}\Big)c_{\varphi_j}\\ - \Big(\cos(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,1}+\sin(\varphi_j)\sin(\vartheta_j)\bm{\hat{p}}_{0_j,2}+\cos(\vartheta_j)\bm{\hat{p}}_{0_j,3}\Big) c_{\vartheta_j} \bigg] \end{multline} @@ -694,7 +694,7 @@ \subsubsection{Remaining Back-substitution Steps} \label{eq:finalsystemr} \end{equation} -Now the other state variables can be solved using Eqs. \eqref{eq:BSM3} and \eqref{eq:BSM9}: +Now the other state variables can be solved using Eqs. \eqref{eq:BSM3} and \eqref{eq:BSM9}: \begin{equation} \ddot{\varphi}_j = \bm a_{\varphi_j}^T \ddot{\bm{r}}_{B/N} + \bm b_{\varphi_j}^T \dot{\bm\omega}_{\cal B/N} +c_{\varphi_j} @@ -759,7 +759,7 @@ \subsection{Reference System Change} \includegraphics[width=13cm]{Figures/referencesystemsP0.pdf} \caption{$\mathcal{P}_{0,j}^{\text{new}}$ frame definition} \label{fig:newreferencesystem} -\end{figure} +\end{figure} At this point is easy to see that the new value of $\varphi_j$ and $\vartheta_j$ are equal to 0. To compute the new value of $\dot{\varphi}_j$ and $\dot{\vartheta}_j$, Eq. \eqref{eq:ljprime} is reversed and it yields: \begin{equation} \dot{\varphi}=\frac{\bm{l}_j[2]}{l_j} @@ -769,4 +769,4 @@ \subsection{Reference System Change} \dot{\vartheta}=-\frac{\bm{l}_j[3]}{l_j} \label{eq:RSC2} \end{equation} -The integration can continue using these new values and the new reference systems. This would lead to discontinuities on $\varphi_j$, $\theta_j$, $\varphi_j$ and $\vartheta_j$ but not on the vectors $\bm{l}_j$ and $\bm{l}_{j}'$. \ No newline at end of file +The integration can continue using these new values and the new reference systems. This would lead to discontinuities on $\varphi_j$, $\theta_j$, $\varphi_j$ and $\vartheta_j$ but not on the vectors $\bm{l}_j$ and $\bm{l}_{j}'$. diff --git a/src/simulation/dynamics/sphericalPendulum/_Documentation/secModelFunctions.tex b/src/simulation/dynamics/sphericalPendulum/_Documentation/secModelFunctions.tex index af83aa37ca..e71839e993 100644 --- a/src/simulation/dynamics/sphericalPendulum/_Documentation/secModelFunctions.tex +++ b/src/simulation/dynamics/sphericalPendulum/_Documentation/secModelFunctions.tex @@ -14,6 +14,6 @@ \section{Model Assumptions and Limitations} \begin{itemize} \item The mass is assumed to be concentrated at the pendulum meaning that the mass is considered a point mass - \item The link is considered massless: the link being the length between the rotation point and the mass + \item The link is considered massless: the link being the length between the rotation point and the mass \item Is derived in such a manner that does not require constraints to be met -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/simulation/dynamics/sphericalPendulum/_Documentation/secTest.tex b/src/simulation/dynamics/sphericalPendulum/_Documentation/secTest.tex index 0db213c01d..9b146e7769 100644 --- a/src/simulation/dynamics/sphericalPendulum/_Documentation/secTest.tex +++ b/src/simulation/dynamics/sphericalPendulum/_Documentation/secTest.tex @@ -1,6 +1,6 @@ \section{Test Description and Success Criteria} This test is located in \texttt{simulation/dynamics/sphericalPendulum/UnitTest/\newline -test\_sphericalPendulum.py}. In this integrated test there are two spherical pendulums connected to the spacecraft hub. There are two scenarios being tested: one with the time step being 0.01 and one with the time step 0.001. Both tests should show energy and momentum conservation but the 0.001 should show less integration error. +test\_sphericalPendulum.py}. In this integrated test there are two spherical pendulums connected to the spacecraft hub. There are two scenarios being tested: one with the time step being 0.01 and one with the time step 0.001. Both tests should show energy and momentum conservation but the 0.001 should show less integration error. \section{Test Parameters} @@ -76,18 +76,18 @@ \section{Test Parameters} \caption{Error Tolerance - Note: Relative Tolerance is $\textnormal{abs}(\frac{\textnormal{truth} - \textnormal{value}}{\textnormal{truth}}$)} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c |} % Column formatting, + \begin{tabular}{| c | c |} % Column formatting, \hline Test & Relative Tolerance \\ \hline Energy and Momentum Conservation & 1e-8 \\ - \hline + \hline \end{tabular} \end{table} \section{Test Results} -The following figures show the conservation of the quantities described in the success criteria for each scenario. The conservation plots are all relative difference plots. All conservation plots show integration error which is the desired result. In the python test these values are automatically checked therefore when the tests pass, these values have all been confirmed to be conserved. An additional note: the angular momentum plots are plotting the change in the components of the angular momentum vector in the inertial frame. The individual components are not labeled because the goal is for each component to show conservation therefore the individual components do not have separate information needing to be specified. +The following figures show the conservation of the quantities described in the success criteria for each scenario. The conservation plots are all relative difference plots. All conservation plots show integration error which is the desired result. In the python test these values are automatically checked therefore when the tests pass, these values have all been confirmed to be conserved. An additional note: the angular momentum plots are plotting the change in the components of the angular momentum vector in the inertial frame. The individual components are not labeled because the goal is for each component to show conservation therefore the individual components do not have separate information needing to be specified. \subsection{Time Step = 0.01} \begin{figure}[htbp] diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/spinningBodyNDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/spinningBodyNDOFStateEffector.cpp index 196ff5a2dd..edfe98bd60 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/spinningBodyNDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/spinningBodyNDOFStateEffector.cpp @@ -30,7 +30,7 @@ SpinningBodyNDOFStateEffector::SpinningBodyNDOFStateEffector() this->effProps.IEffPntB_B.fill(0.0); this->effProps.rEffPrime_CB_B.fill(0.0); this->effProps.IEffPrimePntB_B.fill(0.0); - + this->nameOfThetaState = "spinningBodyTheta" + std::to_string(SpinningBodyNDOFStateEffector::effectorID); this->nameOfThetaDotState = "spinningBodyThetaDot" + std::to_string(SpinningBodyNDOFStateEffector::effectorID); SpinningBodyNDOFStateEffector::effectorID++; diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py index a00455db57..bafe34b9da 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py @@ -166,7 +166,7 @@ def spinningBody(show_plots, cmdTorque, lock, thetaRef): # 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() diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.rst b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.rst index 66827d7890..f5351604d3 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.rst +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.rst @@ -120,4 +120,3 @@ This section is to outline the steps needed to setup a Spinning Body State Effec #. Add the module to the task list:: unitTestSim.AddModelToTask(unitTaskName, spinningBody) - diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/_UnitTest/test_spinningBodyTwoDOFStateEffector.py b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/_UnitTest/test_spinningBodyTwoDOFStateEffector.py index 92a85d376a..cd80abc153 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/_UnitTest/test_spinningBodyTwoDOFStateEffector.py +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/_UnitTest/test_spinningBodyTwoDOFStateEffector.py @@ -192,7 +192,7 @@ def spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, th # 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() diff --git a/src/simulation/environment/ExponentialAtmosphere/_Documentation/AVS.sty b/src/simulation/environment/ExponentialAtmosphere/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/environment/ExponentialAtmosphere/_Documentation/AVS.sty +++ b/src/simulation/environment/ExponentialAtmosphere/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/environment/ExponentialAtmosphere/_Documentation/Basilisk-atmosphere-20190221.tex b/src/simulation/environment/ExponentialAtmosphere/_Documentation/Basilisk-atmosphere-20190221.tex index e3a5befc12..18622c665f 100755 --- a/src/simulation/environment/ExponentialAtmosphere/_Documentation/Basilisk-atmosphere-20190221.tex +++ b/src/simulation/environment/ExponentialAtmosphere/_Documentation/Basilisk-atmosphere-20190221.tex @@ -50,8 +50,8 @@ \newcommand{\status}{Released} \newcommand{\preparer}{A. Harris} \newcommand{\summary}{The {\tt Atmosphere} class used to calculate temperature / density above a body using multiple models. - This class is used to hold relevant atmospheric properties and to compute the density for a given set of spacecraft -relative to a specified planet. Planetary parameters, including position and input message, are settable by the user. + This class is used to hold relevant atmospheric properties and to compute the density for a given set of spacecraft +relative to a specified planet. Planetary parameters, including position and input message, are settable by the user. Internal support is provided for Venus, Earth, and Mars. In a given simulation, each planet of interest should have only one Atmosphere model associated with it linked to the spacecraft in orbit about that body. } diff --git a/src/simulation/environment/ExponentialAtmosphere/_Documentation/BasiliskReportMemo.cls b/src/simulation/environment/ExponentialAtmosphere/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100755 --- a/src/simulation/environment/ExponentialAtmosphere/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/environment/ExponentialAtmosphere/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/environment/ExponentialAtmosphere/_Documentation/bibliography.bib b/src/simulation/environment/ExponentialAtmosphere/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/simulation/environment/ExponentialAtmosphere/_Documentation/bibliography.bib +++ b/src/simulation/environment/ExponentialAtmosphere/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/simulation/environment/ExponentialAtmosphere/_Documentation/secModuleDescription.tex b/src/simulation/environment/ExponentialAtmosphere/_Documentation/secModuleDescription.tex index 950ebcb2a7..3a9018f1ba 100644 --- a/src/simulation/environment/ExponentialAtmosphere/_Documentation/secModuleDescription.tex +++ b/src/simulation/environment/ExponentialAtmosphere/_Documentation/secModuleDescription.tex @@ -4,7 +4,7 @@ \section{Model Description} \subsection{General Module Function} The purpose of this module is to implement an exponential neutral atmospheric density and temperature model. By invoking the atmosphere module, the default values are set such that this basic exponential atmosphere model returns zero for all inputs. Initial values can be specified using the {\tt setSimPlanetEnvironment.exponentialAtmosphere()} macro. -The reach of the model controlled by setting the variables {\tt envMinReach} and {\tt envMaxReach} to positive values. These values are the altitude above the planets surfaces assuming an equatorial radius and spherical shape. The default values are -1 which turns of this checking where the atmosphere model as unbounded reach. +The reach of the model controlled by setting the variables {\tt envMinReach} and {\tt envMaxReach} to positive values. These values are the altitude above the planets surfaces assuming an equatorial radius and spherical shape. The default values are -1 which turns of this checking where the atmosphere model as unbounded reach. \subsection{Exponential Atmosphere} Under the assumption of an isothermal atmosphere, an expression for atmospheric density can be readily arrived at by combining the equations of hydrostatic equilibrium with the ideal gas law and integrating, yielding: @@ -13,4 +13,4 @@ \subsection{Exponential Atmosphere} \rho = \rho_0 e^{\frac{-h}{h_0}} \end{equation} where $\rho_0$ is the density at the planet's surface, $h$ is the altitude, and $h_0$ is the atmospheric scale height derived from the weighted-average behavior of the species that make up the atmosphere. A list of these parameters -for atmospheric bodies in the solar system is available from NASA \href{https://nssdc.gsfc.nasa.gov/planetary/planetfact.html}{here}. For more detail, see Reference ~\citenum{vallado2013}. +for atmospheric bodies in the solar system is available from NASA \href{https://nssdc.gsfc.nasa.gov/planetary/planetfact.html}{here}. For more detail, see Reference ~\citenum{vallado2013}. diff --git a/src/simulation/environment/ExponentialAtmosphere/_Documentation/secModuleFunctions.tex b/src/simulation/environment/ExponentialAtmosphere/_Documentation/secModuleFunctions.tex index ce79c91bbc..9ea5191bf5 100644 --- a/src/simulation/environment/ExponentialAtmosphere/_Documentation/secModuleFunctions.tex +++ b/src/simulation/environment/ExponentialAtmosphere/_Documentation/secModuleFunctions.tex @@ -6,9 +6,9 @@ \section{Module Functions} \begin{itemize} \item \textbf{Compute atmospheric density and temperature}: Each of the provided models is fundamentally intended to compute the neutral atmospheric density and temperature for a spacecraft relative to a body. These parameters are stored in the AtmoPropsSimMsg struct. Supporting parameters needed by each model, such as planet-relative position, are also computed. \item \textbf{Communicate neutral density and temperature}: This module interfaces with modules that subscribe to neutral density messages via the messaging system. - \item \textbf {Subscribe to model-relevant information:} Each provided atmospheric model requires different input information to operate, such as current space weather conditions and spacecraft positions. This module automatically attempts to subscribe to the relevant messages for a specified model. + \item \textbf {Subscribe to model-relevant information:} Each provided atmospheric model requires different input information to operate, such as current space weather conditions and spacecraft positions. This module automatically attempts to subscribe to the relevant messages for a specified model. \item \textbf{Support for multiple spacecraft and model types} Only one Atmosphere module is required for each planet, and can support an arbitrary number of spacecraft. Output messages for individual spacecraft are automatically named based on the environment type. \end{itemize} \section{Module Assumptions and Limitations} -Individual atmospheric models are complex and have their own assumptions. For details about tradeoffs in atmospheric modeling, the reader is pointed to ~\citenum{vallado2013}. \ No newline at end of file +Individual atmospheric models are complex and have their own assumptions. For details about tradeoffs in atmospheric modeling, the reader is pointed to ~\citenum{vallado2013}. diff --git a/src/simulation/environment/ExponentialAtmosphere/_Documentation/secTest.tex b/src/simulation/environment/ExponentialAtmosphere/_Documentation/secTest.tex index 25d2220f10..e57d98fb6f 100644 --- a/src/simulation/environment/ExponentialAtmosphere/_Documentation/secTest.tex +++ b/src/simulation/environment/ExponentialAtmosphere/_Documentation/secTest.tex @@ -11,14 +11,14 @@ \subsection{test\_unitTestAtmosphere.py} This unit test only runs the atmosphere Basilisk module with two fixed spacecraft state input messages. The simulation option {\tt useDefault} checks if the default atmosphere parameters for an Earth-based exponential atmosphere module are used, or if the exponential model information is setup manually. The option {\tt useMinReach} dictates if the minimum altitude check is performed, while the option {\tt useMaxReach} checks if the maximum reach check is performed. The option {\tt usePlanetEphemeris} checks if a planet state input message should be created. All permutations are checked. \section{Test Parameters} -The simulation tolerances are shown in Table~\ref{tab:errortol}. In each simulation the neutral density output message is checked relative to python computed true values. +The simulation tolerances are shown in Table~\ref{tab:errortol}. In each simulation the neutral density output message is checked relative to python computed true values. \begin{table}[htbp] \caption{Error tolerance for each test.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ \hline {\tt neutralDensity} & \input{AutoTeX/toleranceValue} (relative) \\ \hline\hline @@ -36,11 +36,11 @@ \section{Test Results} \caption{Test result for test\_integratedTestAtmosphere.py} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline\hline - \textbf{Check} & \textbf{Pass/Fail} \\ + \textbf{Check} & \textbf{Pass/Fail} \\ \hline - 1 & \input{AutoTeX/passFail} \\ + 1 & \input{AutoTeX/passFail} \\ \hline \hline \end{tabular} @@ -51,29 +51,26 @@ \section{Test Results} \caption{Test result for test\_unitTestAtmosphere.py} \label{tab:results2} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | c | c | c } % Column formatting, + \begin{tabular}{c | c | c | c | c } % Column formatting, \hline\hline - {\tt useDefault} & {\tt useMinReach} & {\tt useMaxReach} & {\tt usePlanetEphemeris} & \textbf{Pass/Fail} \\ + {\tt useDefault} & {\tt useMinReach} & {\tt useMaxReach} & {\tt usePlanetEphemeris} & \textbf{Pass/Fail} \\ \hline - False & False & False & False & \input{AutoTeX/unitTestPassFailFalseFalseFalseFalse} \\ - False & False & False & True & \input{AutoTeX/unitTestPassFailFalseFalseFalseTrue} \\ - False & False & True & False & \input{AutoTeX/unitTestPassFailFalseFalseTrueFalse} \\ - False & False & True & True & \input{AutoTeX/unitTestPassFailFalseFalseTrueTrue} \\ - False & True & False & False & \input{AutoTeX/unitTestPassFailFalseTrueFalseFalse} \\ - False & True & False & True & \input{AutoTeX/unitTestPassFailFalseTrueFalseTrue} \\ - False & True & True & False & \input{AutoTeX/unitTestPassFailFalseTrueTrueFalse} \\ - False & True & True & True & \input{AutoTeX/unitTestPassFailFalseTrueTrueTrue} \\ - True & False & False & False & \input{AutoTeX/unitTestPassFailFalseFalseFalseFalse} \\ - True & False & False & True & \input{AutoTeX/unitTestPassFailTrueFalseFalseTrue} \\ - True & False & True & False & \input{AutoTeX/unitTestPassFailTrueFalseTrueFalse} \\ - True & False & True & True & \input{AutoTeX/unitTestPassFailTrueFalseTrueTrue} \\ - True & True & False & False & \input{AutoTeX/unitTestPassFailTrueTrueFalseFalse} \\ - True & True & False & True & \input{AutoTeX/unitTestPassFailTrueTrueFalseTrue} \\ - True & True & True & False & \input{AutoTeX/unitTestPassFailTrueTrueTrueFalse} \\ - True & True & True & True & \input{AutoTeX/unitTestPassFailTrueTrueTrueTrue} \\ + False & False & False & False & \input{AutoTeX/unitTestPassFailFalseFalseFalseFalse} \\ + False & False & False & True & \input{AutoTeX/unitTestPassFailFalseFalseFalseTrue} \\ + False & False & True & False & \input{AutoTeX/unitTestPassFailFalseFalseTrueFalse} \\ + False & False & True & True & \input{AutoTeX/unitTestPassFailFalseFalseTrueTrue} \\ + False & True & False & False & \input{AutoTeX/unitTestPassFailFalseTrueFalseFalse} \\ + False & True & False & True & \input{AutoTeX/unitTestPassFailFalseTrueFalseTrue} \\ + False & True & True & False & \input{AutoTeX/unitTestPassFailFalseTrueTrueFalse} \\ + False & True & True & True & \input{AutoTeX/unitTestPassFailFalseTrueTrueTrue} \\ + True & False & False & False & \input{AutoTeX/unitTestPassFailFalseFalseFalseFalse} \\ + True & False & False & True & \input{AutoTeX/unitTestPassFailTrueFalseFalseTrue} \\ + True & False & True & False & \input{AutoTeX/unitTestPassFailTrueFalseTrueFalse} \\ + True & False & True & True & \input{AutoTeX/unitTestPassFailTrueFalseTrueTrue} \\ + True & True & False & False & \input{AutoTeX/unitTestPassFailTrueTrueFalseFalse} \\ + True & True & False & True & \input{AutoTeX/unitTestPassFailTrueTrueFalseTrue} \\ + True & True & True & False & \input{AutoTeX/unitTestPassFailTrueTrueTrueFalse} \\ + True & True & True & True & \input{AutoTeX/unitTestPassFailTrueTrueTrueTrue} \\ \hline\hline \end{tabular} \end{table} - - - diff --git a/src/simulation/environment/ExponentialAtmosphere/_Documentation/secUserGuide.tex b/src/simulation/environment/ExponentialAtmosphere/_Documentation/secUserGuide.tex index 7af388117d..9fba6bbe15 100644 --- a/src/simulation/environment/ExponentialAtmosphere/_Documentation/secUserGuide.tex +++ b/src/simulation/environment/ExponentialAtmosphere/_Documentation/secUserGuide.tex @@ -38,4 +38,3 @@ \subsection{Planet Ephemeris Information} \subsection{Setting the Model Reach} By default the model doesn't perform any checks on the altitude to see if the specified atmosphere model should be used. This is set through the parameters {\tt envMinReach} and {\tt envMaxReach}. Their default values are -1. If these are set to positive values, then if the altitude is smaller than {\tt envMinReach} or larger than {\tt envMaxReach}, the density is set to zero. - diff --git a/src/simulation/environment/MsisAtmosphere/_Documentation/AVS.sty b/src/simulation/environment/MsisAtmosphere/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/environment/MsisAtmosphere/_Documentation/AVS.sty +++ b/src/simulation/environment/MsisAtmosphere/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/environment/MsisAtmosphere/_Documentation/Basilisk-atmosphere-20190221.tex b/src/simulation/environment/MsisAtmosphere/_Documentation/Basilisk-atmosphere-20190221.tex index e3a5befc12..18622c665f 100644 --- a/src/simulation/environment/MsisAtmosphere/_Documentation/Basilisk-atmosphere-20190221.tex +++ b/src/simulation/environment/MsisAtmosphere/_Documentation/Basilisk-atmosphere-20190221.tex @@ -50,8 +50,8 @@ \newcommand{\status}{Released} \newcommand{\preparer}{A. Harris} \newcommand{\summary}{The {\tt Atmosphere} class used to calculate temperature / density above a body using multiple models. - This class is used to hold relevant atmospheric properties and to compute the density for a given set of spacecraft -relative to a specified planet. Planetary parameters, including position and input message, are settable by the user. + This class is used to hold relevant atmospheric properties and to compute the density for a given set of spacecraft +relative to a specified planet. Planetary parameters, including position and input message, are settable by the user. Internal support is provided for Venus, Earth, and Mars. In a given simulation, each planet of interest should have only one Atmosphere model associated with it linked to the spacecraft in orbit about that body. } diff --git a/src/simulation/environment/MsisAtmosphere/_Documentation/BasiliskReportMemo.cls b/src/simulation/environment/MsisAtmosphere/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100755 --- a/src/simulation/environment/MsisAtmosphere/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/environment/MsisAtmosphere/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/environment/MsisAtmosphere/_Documentation/bibliography.bib b/src/simulation/environment/MsisAtmosphere/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/simulation/environment/MsisAtmosphere/_Documentation/bibliography.bib +++ b/src/simulation/environment/MsisAtmosphere/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/simulation/environment/MsisAtmosphere/_Documentation/secModuleFunctions.tex b/src/simulation/environment/MsisAtmosphere/_Documentation/secModuleFunctions.tex index 96f122189a..6c181df7eb 100644 --- a/src/simulation/environment/MsisAtmosphere/_Documentation/secModuleFunctions.tex +++ b/src/simulation/environment/MsisAtmosphere/_Documentation/secModuleFunctions.tex @@ -6,12 +6,12 @@ \section{Module Functions} \begin{itemize} \item \textbf{Compute atmospheric density and temperature}: Each of the provided models is fundamentally intended to compute the neutral atmospheric density and temperature for a spacecraft relative to a body. These parameters are stored in the AtmoPropsMsgPayload struct. Supporting parameters needed by each model, such as planet-relative position, are also computed. \item \textbf{Communicate neutral density and temperature}: This module interfaces with modules that subscribe to neutral density messages via the messaging system. - \item \textbf {Subscribe to model-relevant information:} Each provided atmospheric model requires different input information to operate, such as current space weather conditions and spacecraft positions. This module automatically attempts to subscribe to the relevant messages for a specified model. + \item \textbf {Subscribe to model-relevant information:} Each provided atmospheric model requires different input information to operate, such as current space weather conditions and spacecraft positions. This module automatically attempts to subscribe to the relevant messages for a specified model. \item \textbf{Support for multiple spacecraft} Only one NRLMSISE-00 atmosphere model is needed to compute densities for several spacecraft. \item \textbf{Support dynamic space-weather coupled density forecasting}: A primary benefit of the NRLMSISE-00 model is its ability to provide forecasts of neutral density in response to space weather events, changing atmospheric conditions, and the like that are not captured in simpler models. \end{itemize} \section{Module Assumptions and Limitations} -Individual atmospheric models are complex and have their own assumptions. At present, all non-exponential models are Earth-specific. For details about tradeoffs in atmospheric modeling, the reader is pointed to ~\citenum{vallado2013}. +Individual atmospheric models are complex and have their own assumptions. At present, all non-exponential models are Earth-specific. For details about tradeoffs in atmospheric modeling, the reader is pointed to ~\citenum{vallado2013}. -NRLMSISE-00, specifically, is highly dependent on ``space weather parameters'' such as $Ap$, $Kp$, $F10.7$, among others. The outputs of this model can vary greatly with the selected parameters. \ No newline at end of file +NRLMSISE-00, specifically, is highly dependent on ``space weather parameters'' such as $Ap$, $Kp$, $F10.7$, among others. The outputs of this model can vary greatly with the selected parameters. diff --git a/src/simulation/environment/MsisAtmosphere/_Documentation/secTest.tex b/src/simulation/environment/MsisAtmosphere/_Documentation/secTest.tex index ab839b9ef1..bceb2ed064 100644 --- a/src/simulation/environment/MsisAtmosphere/_Documentation/secTest.tex +++ b/src/simulation/environment/MsisAtmosphere/_Documentation/secTest.tex @@ -4,7 +4,7 @@ \section{Test Description and Success Criteria} This section describes the specific unit tests conducted on this module. \subsection{General Functionality} -The unit test check the neutral density calculation for a single spacecraft at a particular epoch time. The epoch is either specified through an epoch input message, or set directly by specifying the {\tt epochDoy} day of year value. +The unit test check the neutral density calculation for a single spacecraft at a particular epoch time. The epoch is either specified through an epoch input message, or set directly by specifying the {\tt epochDoy} day of year value. \subsection{Model-Specific Tests} @@ -14,17 +14,17 @@ \subsubsection{test\_unitTestNrlmsise00.py} This integrated test evaluates the NRLMSISE-00 model at a given point in an orbit with zero'd (i.e., nonphysical) space weather inputs and verifies its outputs against the outputs of the base C model with identical inputs. This is a comprehensive test of the NRLMSISE-00 implementation in BSK, as it checks out the end-to-end functionality of the space weather messaging system, the geodetic conversion, and the interface to NRLMSISE-00 itself. \section{Test Parameters} -The simulation tolerances are shown in Table~\ref{tab:errortol}. In each simulation the neutral density output message is checked relative to python computed true values. +The simulation tolerances are shown in Table~\ref{tab:errortol}. In each simulation the neutral density output message is checked relative to python computed true values. \begin{table}[htbp] \caption{Error tolerance for each test.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ \hline {Output Neutral Mass Density} & {\input{AutoTeX/unitTestToleranceValue} (relative) } \\ - {Output Temperature} & \input{AutoTeX/unitTestToleranceValue} (relative) \\ + {Output Temperature} & \input{AutoTeX/unitTestToleranceValue} (relative) \\ \hline\hline \end{tabular} \end{table} @@ -40,19 +40,17 @@ \section{Test Results} \caption{Test result for test\_unitMsisAtmosphere.py} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | c} % Column formatting, + \begin{tabular}{c | c | c} % Column formatting, \hline\hline - {\tt orbitCase} & {\tt setEpoch} & \textbf{Pass/Fail} \\ + {\tt orbitCase} & {\tt setEpoch} & \textbf{Pass/Fail} \\ \hline - LPO & Direct & \input{AutoTeX/unitTestPassFailLPODirect} \\ - LTO & Direct & \input{AutoTeX/unitTestPassFailLTODirect} \\ - LPO & Msg & \input{AutoTeX/unitTestPassFailLPOMsg} \\ - LTO & Msg & \input{AutoTeX/unitTestPassFailLTOMsg} \\ - LPO & Default & \input{AutoTeX/unitTestPassFailLPODefault} \\ - LTO & Default & \input{AutoTeX/unitTestPassFailLTODefault} \\ + LPO & Direct & \input{AutoTeX/unitTestPassFailLPODirect} \\ + LTO & Direct & \input{AutoTeX/unitTestPassFailLTODirect} \\ + LPO & Msg & \input{AutoTeX/unitTestPassFailLPOMsg} \\ + LTO & Msg & \input{AutoTeX/unitTestPassFailLTOMsg} \\ + LPO & Default & \input{AutoTeX/unitTestPassFailLPODefault} \\ + LTO & Default & \input{AutoTeX/unitTestPassFailLTODefault} \\ \hline \hline \end{tabular} \end{table} - - diff --git a/src/simulation/environment/MsisAtmosphere/_Documentation/secUserGuide.tex b/src/simulation/environment/MsisAtmosphere/_Documentation/secUserGuide.tex index 0f1e75b499..0160f3553c 100644 --- a/src/simulation/environment/MsisAtmosphere/_Documentation/secUserGuide.tex +++ b/src/simulation/environment/MsisAtmosphere/_Documentation/secUserGuide.tex @@ -22,7 +22,7 @@ \subsection{General Module Setup} \item {\bfseries Direct Setting:} The module variable {\tt epochDoy} can be set directly as well. However, note that if the input message is specified, the {\tt epochDoy} value is not used. \begin{verbatim} newAtmo.epochDoy = 1 - \end{verbatim} + \end{verbatim} \end{enumerate} The model can then be added to a task like other simModels. Each Atmosphere calculates atmospheric parameters based on the output state messages for a set of spacecraft. @@ -56,4 +56,4 @@ \subsection{NRLMSISE-00 atmosphere user guide} "ap_3_-42", "ap_3_-45", "ap_3_-48","ap_3_-51","ap_3_-54", "ap_3_-57","f107_1944_0","f107_24_-24" ] -\end{verbatim} \ No newline at end of file +\end{verbatim} diff --git a/src/simulation/environment/MsisAtmosphere/_UnitTest/truthOutputs.txt b/src/simulation/environment/MsisAtmosphere/_UnitTest/truthOutputs.txt index b860761e23..826136c2f1 100644 --- a/src/simulation/environment/MsisAtmosphere/_UnitTest/truthOutputs.txt +++ b/src/simulation/environment/MsisAtmosphere/_UnitTest/truthOutputs.txt @@ -8,4 +8,4 @@ 2.0309373610327914e+05 6.5549485610765977e-44 1027.318465 -167.31647306 \ No newline at end of file +167.31647306 diff --git a/src/simulation/environment/MsisAtmosphere/msisAtmosphere.rst b/src/simulation/environment/MsisAtmosphere/msisAtmosphere.rst index fabab3346a..4c8674a97b 100644 --- a/src/simulation/environment/MsisAtmosphere/msisAtmosphere.rst +++ b/src/simulation/environment/MsisAtmosphere/msisAtmosphere.rst @@ -55,5 +55,3 @@ Regarding the vector ``swDataInMsgs``, the order of these 23 messages must follo 20 - ap_3_-57 21 - f107_1944_0 22 - f107_24_-24 - - diff --git a/src/simulation/environment/MsisAtmosphere/nrlmsise-00.c b/src/simulation/environment/MsisAtmosphere/nrlmsise-00.c index 7b5d2f6841..6f7c183b2e 100644 --- a/src/simulation/environment/MsisAtmosphere/nrlmsise-00.c +++ b/src/simulation/environment/MsisAtmosphere/nrlmsise-00.c @@ -207,7 +207,7 @@ double dnet (double dd, double dm, double zhm, double xmm, double xm) { return dd; if (dd==0) return dm; - } + } ylog = a * log(dm/dd); if (ylog<-10) return dd; @@ -240,7 +240,7 @@ void splini (double *xa, double *ya, double *y2a, int n, double x, double *y) { if (khi<(n-1)) { if (xswc[12]* \ (p[83]*plg[0][1]+p[84]*plg[0][3]+p[85]*plg[0][5])* \ cos(sr*(input->sec-p[75])); - } + } } } @@ -818,7 +818,7 @@ double globe7(double *p, struct nrlmsise_input *input, struct nrlmsise_flags *fl /* ------------------------------------------------------------------- */ double glob7s(double *p, struct nrlmsise_input *input, struct nrlmsise_flags *flags) { -/* VERSION OF GLOBE FOR LOWER ATMOSPHERE 10/26/99 +/* VERSION OF GLOBE FOR LOWER ATMOSPHERE 10/26/99 */ double pset=2.0; double t[14]; @@ -884,7 +884,7 @@ double glob7s(double *p, struct nrlmsise_input *input, struct nrlmsise_flags *fl if ((int) flags->sw[9]) { if (flags->sw[9]==1) t[8] = apdf * (p[32] + p[45] * plg[0][2] * flags->swc[2]); - if (flags->sw[9]==-1) + if (flags->sw[9]==-1) t[8]=(p[50]*apt[0] + p[96]*plg[0][2] * apt[0]*flags->swc[2]); } @@ -995,7 +995,7 @@ void gtd7(struct nrlmsise_input *input, struct nrlmsise_flags *flags, struct nrl if (input->alt>zmix) dmc = 1.0 - (zn2[0]-input->alt)/(zn2[0] - zmix); dz28=soutput.d[2]; - + /**** N2 density ****/ dmr=soutput.d[2] / dm28m - 1.0; output->d[2]=densm(input->alt,dm28m,xmm, &tz, mn3, zn3, meso_tn3, meso_tgn3, mn2, zn2, meso_tn2, meso_tgn2); @@ -1047,7 +1047,7 @@ void gtd7d(struct nrlmsise_input *input, struct nrlmsise_flags *flags, struct nr if ((int) flags->sw[0]) output->d[5]=output->d[5]/1000; } - + /* ------------------------------------------------------------------- */ @@ -1085,7 +1085,7 @@ void ghp7(struct nrlmsise_input *input, struct nrlmsise_flags *flags, struct nrl cl2 = cl*cl; if (input->doy<182) cd = (1.0 - (double) input->doy) / 91.25; - else + else cd = ((double) input->doy) / 91.25 - 3.0; ca = 0; if ((pl > -1.11) && (pl<=-0.23)) @@ -1139,7 +1139,7 @@ void ghp7(struct nrlmsise_input *input, struct nrlmsise_flags *flags, struct nrl void gts7(struct nrlmsise_input *input, struct nrlmsise_flags *flags, struct nrlmsise_output *output) { /* Thermospheric portion of NRLMSISE-00 * See GTD7 for more extensive comments - * alt > 72.5 km! + * alt > 72.5 km! */ double za; int i, j; @@ -1173,7 +1173,7 @@ void gts7(struct nrlmsise_input *input, struct nrlmsise_flags *flags, struct nrl double hc216, hcc232; za = pdl[1][15]; zn1[0] = za; - for (j=0;j<9;j++) + for (j=0;j<9;j++) output->d[j]=0; /* TINF VARIATIONS NOT IMPORTANT BELOW ZA OR ZN1(1) */ @@ -1228,7 +1228,7 @@ void gts7(struct nrlmsise_input *input, struct nrlmsise_flags *flags, struct nrl dd=output->d[2]; /* Turbopause */ zh28=pdm[2][2]*zhf; - zhm28=pdm[2][3]*pdl[1][5]; + zhm28=pdm[2][3]*pdl[1][5]; xmd=28.0-xmm; /* Mixed density at Zlb */ b28=densu(zh28,db28,tinf,tlb,xmd,(alpha[2]-1.0),&tz,ptm[5],s,mn1, zn1,meso_tn1,meso_tgn1); diff --git a/src/simulation/environment/MsisAtmosphere/nrlmsise-00.h b/src/simulation/environment/MsisAtmosphere/nrlmsise-00.h index 9f5c31a860..aa608e1e73 100644 --- a/src/simulation/environment/MsisAtmosphere/nrlmsise-00.h +++ b/src/simulation/environment/MsisAtmosphere/nrlmsise-00.h @@ -6,7 +6,7 @@ * 20041227 * * The NRLMSISE-00 model was developed by Mike Picone, Alan Hedin, and - * Doug Drob. They also wrote a NRLMSISE-00 distribution package in + * Doug Drob. They also wrote a NRLMSISE-00 distribution package in * FORTRAN which is available at * http://uap-www.nrl.navy.mil/models_web/msis/msis_home.htm * @@ -28,12 +28,12 @@ struct nrlmsise_flags { double sw[24]; //!< variable double swc[24]; //!< variable }; -/* +/* * Switches: to turn on and off particular variations use these switches. * 0 is off, 1 is on, and 2 is main effects off but cross terms on. * - * Standard values are 0 for switch 0 and 1 for switches 1 to 23. The - * array "switches" needs to be set accordingly by the calling program. + * Standard values are 0 for switch 0 and 1 for switches 1 to 23. The + * array "switches" needs to be set accordingly by the calling program. * The arrays sw and swc are set internally. * * switches[i]: @@ -77,10 +77,10 @@ struct ap_array { * 2 : 3 hr AP index for 3 hrs before current time * 3 : 3 hr AP index for 6 hrs before current time * 4 : 3 hr AP index for 9 hrs before current time - * 5 : Average of eight 3 hr AP indicies from 12 to 33 hrs + * 5 : Average of eight 3 hr AP indicies from 12 to 33 hrs + * prior to current time + * 6 : Average of eight 3 hr AP indicies from 36 to 57 hrs * prior to current time - * 6 : Average of eight 3 hr AP indicies from 36 to 57 hrs - * prior to current time */ /*! NRL MSISE input structure */ @@ -98,9 +98,9 @@ struct nrlmsise_input { struct ap_array *ap_a; /*!< see above */ }; /* - * NOTES ON INPUT VARIABLES: + * NOTES ON INPUT VARIABLES: * UT, Local Time, and Longitude are used independently in the - * model and are not of equal importance for every situation. + * model and are not of equal importance for every situation. * For the most physically realistic calculation these three * variables should be consistent (lst=sec/3600 + g_long/15). * The Equation of Time departures from the above formula @@ -128,20 +128,20 @@ struct nrlmsise_output { double d[9]; /*!< densities */ double t[2]; /*!< temperatures */ }; -/* +/* * OUTPUT VARIABLES: * d[0] - HE NUMBER DENSITY(CM-3) * d[1] - O NUMBER DENSITY(CM-3) * d[2] - N2 NUMBER DENSITY(CM-3) * d[3] - O2 NUMBER DENSITY(CM-3) - * d[4] - AR NUMBER DENSITY(CM-3) + * d[4] - AR NUMBER DENSITY(CM-3) * d[5] - TOTAL MASS DENSITY(GM/CM3) [includes d[8] in td7d] * d[6] - H NUMBER DENSITY(CM-3) * d[7] - N NUMBER DENSITY(CM-3) * d[8] - Anomalous oxygen NUMBER DENSITY(CM-3) * t[0] - EXOSPHERIC TEMPERATURE * t[1] - TEMPERATURE AT ALT - * + * * * O, H, and N are set to zero below 72.5 km * @@ -149,7 +149,7 @@ struct nrlmsise_output { * altitudes below 120 km. The 120 km gradient is left at global * average value for altitudes below 72 km. * - * d[5], TOTAL MASS DENSITY, is NOT the same for subroutines GTD7 + * d[5], TOTAL MASS DENSITY, is NOT the same for subroutines GTD7 * and GTD7D * * SUBROUTINE GTD7 -- d[5] is the sum of the mass densities of the diff --git a/src/simulation/environment/MsisAtmosphere/nrlmsise-00_data.c b/src/simulation/environment/MsisAtmosphere/nrlmsise-00_data.c index 0175e6cfc2..a4a8a74085 100644 --- a/src/simulation/environment/MsisAtmosphere/nrlmsise-00_data.c +++ b/src/simulation/environment/MsisAtmosphere/nrlmsise-00_data.c @@ -6,7 +6,7 @@ * 20041227 * * The NRLMSISE-00 model was developed by Mike Picone, Alan Hedin, and - * Doug Drob. They also wrote a NRLMSISE-00 distribution package in + * Doug Drob. They also wrote a NRLMSISE-00 distribution package in * FORTRAN which is available at * http://uap-www.nrl.navy.mil/models_web/msis/msis_home.htm * @@ -304,7 +304,7 @@ double pd[9][150] = { -2.22432E-03,-3.93895E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00 + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00 }, /* HOT O DENSITY */ { 6.04050E-02, 1.57034E+00, 2.99387E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-1.51018E+00, @@ -318,7 +318,7 @@ double pd[9][150] = { -9.45934E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, @@ -377,7 +377,7 @@ double pdl[2][25] = { 4.63830E+00, 1.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 1.28840E+00, 3.10302E-02, 1.18339E-01 }, + 0.00000E+00, 0.00000E+00, 1.28840E+00, 3.10302E-02, 1.18339E-01 }, { 1.00000E+00, 7.00000E-01, 1.15020E+00, 3.44689E+00, 1.28840E+00, 1.00000E+00, 1.08738E+00, 1.22947E+00, 1.10016E+00, 7.34129E-01, 1.15241E+00, 2.22784E+00, 7.95046E-01, 4.01612E+00, 4.47749E+00, @@ -707,7 +707,7 @@ double pma[10][100] = { 5.11923E-02, 3.61225E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 } }; - + /* SEMIANNUAL MULT SAM */ double sam[100] = { 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, @@ -731,10 +731,9 @@ double sam[100] = { 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00 }; - - + + /* MIDDLE ATMOSPHERE AVERAGES */ double pavgm[10] = { 2.61000E+02, 2.64000E+02, 2.29000E+02, 2.17000E+02, 2.17000E+02, 2.23000E+02, 2.86760E+02,-2.93940E+00, 2.50000E+00, 0.00000E+00 }; - diff --git a/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.h b/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.h index 1107211ac6..224968f819 100644 --- a/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.h +++ b/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.h @@ -33,10 +33,10 @@ class TabularAtmosphere: public AtmosphereBase { private: - + // pulls density and temperature from atmospheric table at requested altitude, performs linear interpolation if necessary void evaluateAtmosphereModel(AtmoPropsMsgPayload *msg, double currentTime); - + int altList_length; // length of list of altitude values extracted from the atmosphere table int rhoList_length; // length of list of density values extracted from the atmosphere table int tempList_length; // length of list of temperature values extracted from the atmosphere table diff --git a/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.rst b/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.rst index d60cb9640d..d28402f6b7 100644 --- a/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.rst +++ b/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.rst @@ -17,7 +17,7 @@ The ``tabularAtmosphere`` module handles the following behavior: #. Iterates through the list until the requested altitude is greater than the previous value in the list and less than the next value. #. Will interpolate between the altitude and return the interpolated density and temperature. - + Module Assumptions and Limitations ---------------------------------- Returns density = 0 kg/m^3 and temperature = 0 K when altitude is outside range of provided data @@ -31,4 +31,3 @@ User Guide Required variables are ``altList``, ``rhoList``, and ``tempList``, each a standard vector of doubles. The lists must be sorted corresponding to ascending altitude, and be of the same nonzero length. Altitude must be provided in meters, density in kg/m^3, and temperature in Kelvin. - diff --git a/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.rst b/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.rst index d7b069a15e..869823ca2f 100644 --- a/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.rst +++ b/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.rst @@ -31,5 +31,3 @@ provides information on what this message is used for. * - epochInMsg - :ref:`EpochMsgPayload` - (optional) epoch date/time input message - - diff --git a/src/simulation/environment/albedo/albedo.h b/src/simulation/environment/albedo/albedo.h index 4b083a3f41..a97a4172b2 100644 --- a/src/simulation/environment/albedo/albedo.h +++ b/src/simulation/environment/albedo/albedo.h @@ -84,7 +84,7 @@ class Albedo : public SysModel { ReadFunctor spacecraftStateInMsg; //!< input message name for spacecraft data std::vector> planetInMsgs; //!< vector of planet data input data - BSKLogger bskLogger; //!< BSK Logging + BSKLogger bskLogger; //!< BSK Logging int defaultNumLat; //!< [-] default number of latitude grid points int defaultNumLon; //!< [-] default number of longitude grid points Eigen::Vector3d nHat_B_default; //!< [-] default value for unit normal of the instrument (spacecraft body) diff --git a/src/simulation/environment/dentonFluxModel/_UnitTest/test_dentonFluxModel.py b/src/simulation/environment/dentonFluxModel/_UnitTest/test_dentonFluxModel.py index 08c6aa5968..ca192bed75 100644 --- a/src/simulation/environment/dentonFluxModel/_UnitTest/test_dentonFluxModel.py +++ b/src/simulation/environment/dentonFluxModel/_UnitTest/test_dentonFluxModel.py @@ -1,12 +1,12 @@ -# +# # 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 diff --git a/src/simulation/environment/dentonFluxModel/dentonFluxModel.h b/src/simulation/environment/dentonFluxModel/dentonFluxModel.h index 741ac80aab..0b35fbb4f7 100644 --- a/src/simulation/environment/dentonFluxModel/dentonFluxModel.h +++ b/src/simulation/environment/dentonFluxModel/dentonFluxModel.h @@ -43,7 +43,7 @@ class DentonFluxModel: public SysModel { // Methods void Reset(uint64_t CurrentSimNanos) override; void UpdateState(uint64_t CurrentSimNanos) override; - + /* public variables */ int numOutputEnergies = -1; //!< number of energy bins used in the output message std::string kpIndex = ""; //!< Kp index @@ -72,10 +72,10 @@ class DentonFluxModel: public SysModel { //!< Electron Flux: double mean_e_flux[MAX_NUM_KPS][MAX_NUM_ENERGIES][MAX_NUM_LOCAL_TIMES]; - + //!< Ion Flux: double mean_i_flux[MAX_NUM_KPS][MAX_NUM_ENERGIES][MAX_NUM_LOCAL_TIMES]; - + //!< Fill average centre energies, normalized by satellite double enElec[40] = {1.034126, 1.346516, 1.817463, 2.399564, 3.161048, 4.153217, 5.539430, 7.464148, @@ -87,7 +87,7 @@ class DentonFluxModel: public SysModel { 2069.619628, 2703.301269, 3540.124511, 4639.775390, 6069.347656, 7957.457519, 10436.841796, 13677.195312, 17923.560546, 23488.560546, 30782.000000, 40326.937500}; - + double enProt[40] = { 1.816424, 2.284231, 2.904752, 3.639589, 4.483188, 5.671049, 7.343667, 9.450922, 11.934194, 15.105951, 19.372854, 24.943658, diff --git a/src/simulation/environment/dentonFluxModel/dentonFluxModel.rst b/src/simulation/environment/dentonFluxModel/dentonFluxModel.rst index 8866679897..8462e3c8e3 100644 --- a/src/simulation/environment/dentonFluxModel/dentonFluxModel.rst +++ b/src/simulation/environment/dentonFluxModel/dentonFluxModel.rst @@ -9,9 +9,9 @@ flux data. Message Connection Descriptions ------------------------------- -The following table lists all the module input and output messages. -The module msg connection is set by the user from python. -The msg type contains a link to the message structure definition, while the description +The following table lists all the module input and output messages. +The module msg connection is set by the user from python. +The msg type contains a link to the message structure definition, while the description provides information on what this message is used for. .. _ModuleIO_Denton_Flux_Model: diff --git a/src/simulation/environment/eclipse/_Documentation/Images/conical_shadow.svg b/src/simulation/environment/eclipse/_Documentation/Images/conical_shadow.svg index bc5a110bb2..bcebf1b88d 100644 --- a/src/simulation/environment/eclipse/_Documentation/Images/conical_shadow.svg +++ b/src/simulation/environment/eclipse/_Documentation/Images/conical_shadow.svg @@ -28,7 +28,7 @@ - Produced by OmniGraffle 7.12 + Produced by OmniGraffle 7.12 2019-12-31 22:49:28 +0000 diff --git a/src/simulation/environment/eclipse/_Documentation/Images/diskModel.svg b/src/simulation/environment/eclipse/_Documentation/Images/diskModel.svg index 7c4d3fdcb4..17669bbdda 100644 --- a/src/simulation/environment/eclipse/_Documentation/Images/diskModel.svg +++ b/src/simulation/environment/eclipse/_Documentation/Images/diskModel.svg @@ -8,7 +8,7 @@ - Produced by OmniGraffle 7.12 + Produced by OmniGraffle 7.12 2019-12-31 22:46:21 +0000 diff --git a/src/simulation/environment/eclipse/eclipse.h b/src/simulation/environment/eclipse/eclipse.h index f436baccb1..713d3ccf44 100755 --- a/src/simulation/environment/eclipse/eclipse.h +++ b/src/simulation/environment/eclipse/eclipse.h @@ -38,13 +38,13 @@ class Eclipse: public SysModel { public: Eclipse(); ~Eclipse(); - + void Reset(uint64_t CurrenSimNanos); void UpdateState(uint64_t CurrentSimNanos); void writeOutputMessages(uint64_t CurrentClock); void addSpacecraftToModel(Message *tmpScMsg); void addPlanetToModel(Message *tmpSpMsg); - + public: ReadFunctor sunInMsg; //!< sun ephemeris input message name std::vector> planetInMsgs; //!< A vector of planet incoming state message names ordered by the sequence in which planet are added to the module diff --git a/src/simulation/environment/ephemerisConverter/_Documentation/AVS.sty b/src/simulation/environment/ephemerisConverter/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/environment/ephemerisConverter/_Documentation/AVS.sty +++ b/src/simulation/environment/ephemerisConverter/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/environment/ephemerisConverter/_Documentation/Basilisk-EPHEMERIS_CONVERTER20170712.tex b/src/simulation/environment/ephemerisConverter/_Documentation/Basilisk-EPHEMERIS_CONVERTER20170712.tex index d9ef11c5bc..88f6396236 100755 --- a/src/simulation/environment/ephemerisConverter/_Documentation/Basilisk-EPHEMERIS_CONVERTER20170712.tex +++ b/src/simulation/environment/ephemerisConverter/_Documentation/Basilisk-EPHEMERIS_CONVERTER20170712.tex @@ -92,7 +92,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/environment/ephemerisConverter/_Documentation/BasiliskReportMemo.cls b/src/simulation/environment/ephemerisConverter/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/environment/ephemerisConverter/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/environment/ephemerisConverter/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/environment/ephemerisConverter/_Documentation/bibliography.bib b/src/simulation/environment/ephemerisConverter/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/simulation/environment/ephemerisConverter/_Documentation/bibliography.bib +++ b/src/simulation/environment/ephemerisConverter/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/simulation/environment/ephemerisConverter/_Documentation/secModelDescription.tex b/src/simulation/environment/ephemerisConverter/_Documentation/secModelDescription.tex index 166291613b..a690d07c26 100644 --- a/src/simulation/environment/ephemerisConverter/_Documentation/secModelDescription.tex +++ b/src/simulation/environment/ephemerisConverter/_Documentation/secModelDescription.tex @@ -23,5 +23,3 @@ \subsection{I/O} \item v\_BdyZero\_N[3]: the velocity of orbital body in the inertial frame \item timeTag: the vehicle Time-tag for state \end{itemize} - - diff --git a/src/simulation/environment/ephemerisConverter/_Documentation/secModelFunctions.tex b/src/simulation/environment/ephemerisConverter/_Documentation/secModelFunctions.tex index 12fb5196d7..aeec790912 100644 --- a/src/simulation/environment/ephemerisConverter/_Documentation/secModelFunctions.tex +++ b/src/simulation/environment/ephemerisConverter/_Documentation/secModelFunctions.tex @@ -5,4 +5,4 @@ \section{Model Functions} \section{Model Assumptions and Limitations} -There are no direct assumptions and limitations in this module which is simply a necessary piece for the code architecture. \ No newline at end of file +There are no direct assumptions and limitations in this module which is simply a necessary piece for the code architecture. diff --git a/src/simulation/environment/ephemerisConverter/_Documentation/secTest.tex b/src/simulation/environment/ephemerisConverter/_Documentation/secTest.tex index ca4ad8a1fc..818d07995d 100644 --- a/src/simulation/environment/ephemerisConverter/_Documentation/secTest.tex +++ b/src/simulation/environment/ephemerisConverter/_Documentation/secTest.tex @@ -7,7 +7,7 @@ \section{Test Description and Success Criteria} \subsection{Validation success criteria } -The criteria for a successful testing of this module is driven by the correct copy of the spice messages. This is done simply by comparing the messages before the copy with the outcome of the copied message. The error tolerance is at $\epsilon =10^{-5}$ which corresponds to 12 significant digits. This gives a healthy margin from machine precision all the while getting all of the physical information from the ephemerides. +The criteria for a successful testing of this module is driven by the correct copy of the spice messages. This is done simply by comparing the messages before the copy with the outcome of the copied message. The error tolerance is at $\epsilon =10^{-5}$ which corresponds to 12 significant digits. This gives a healthy margin from machine precision all the while getting all of the physical information from the ephemerides. \section{Test Parameters} @@ -35,4 +35,3 @@ \subsection{Pass/Fail results} \end{center} Both components of the test pass. The copy is therefore a properly executed. - diff --git a/src/simulation/environment/ephemerisConverter/_Documentation/secUserGuide.tex b/src/simulation/environment/ephemerisConverter/_Documentation/secUserGuide.tex index 7d7fde4595..c0b5036378 100644 --- a/src/simulation/environment/ephemerisConverter/_Documentation/secUserGuide.tex +++ b/src/simulation/environment/ephemerisConverter/_Documentation/secUserGuide.tex @@ -7,4 +7,4 @@ \section{User Guide} \item[-] \texttt{EphemObject.ModelTag = 'EphemData'} \item[-] \texttt{EphemObject.addSpiceInputMsg(spiceInMsg)} \end{itemize} -The output messages are then stored in the vector of output messages called {\tt ephemOutMsgs}. \ No newline at end of file +The output messages are then stored in the vector of output messages called {\tt ephemOutMsgs}. diff --git a/src/simulation/environment/ephemerisConverter/ephemerisConverter.h b/src/simulation/environment/ephemerisConverter/ephemerisConverter.h index 06c05eb6c4..eeafe09981 100755 --- a/src/simulation/environment/ephemerisConverter/ephemerisConverter.h +++ b/src/simulation/environment/ephemerisConverter/ephemerisConverter.h @@ -35,7 +35,7 @@ class EphemerisConverter: public SysModel { public: EphemerisConverter(); ~EphemerisConverter(); - + void UpdateState(uint64_t CurrentSimNanos); void Reset(uint64_t CurrentSimNanos); void readInputMessages(); //!< class method diff --git a/src/simulation/environment/groundLocation/groundLocation.h b/src/simulation/environment/groundLocation/groundLocation.h index 0a013ef93e..3ef0817734 100644 --- a/src/simulation/environment/groundLocation/groundLocation.h +++ b/src/simulation/environment/groundLocation/groundLocation.h @@ -48,7 +48,7 @@ class GroundLocation: public SysModel { void addSpacecraftToModel(Message *tmpScMsg); void specifyLocation(double lat, double longitude, double alt); void specifyLocationPCPF(Eigen::Vector3d& r_LP_P_Loc); - + private: void updateInertialPositions(); void computeAccess(); diff --git a/src/simulation/environment/groundMapping/groundMapping.rst b/src/simulation/environment/groundMapping/groundMapping.rst index 40a243cc5c..b933053792 100644 --- a/src/simulation/environment/groundMapping/groundMapping.rst +++ b/src/simulation/environment/groundMapping/groundMapping.rst @@ -11,9 +11,9 @@ also assumes the spacecraft instrument has a spherical field-of-view (FOV). Message Connection Descriptions ------------------------------- -The following table lists all the module input and output messages. -The module msg connection is set by the user from python. -The msg type contains a link to the message structure definition, while the description +The following table lists all the module input and output messages. +The module msg connection is set by the user from python. +The msg type contains a link to the message structure definition, while the description provides information on what this message is used for. .. list-table:: Module I/O Messages diff --git a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/AVS.sty b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/AVS.sty +++ b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/BasiliskReportMemo.cls b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100755 --- a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secModuleDescription.tex b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secModuleDescription.tex index c5005ed882..28ea4f1a4e 100644 --- a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secModuleDescription.tex +++ b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secModuleDescription.tex @@ -2,11 +2,11 @@ \section{Model Description} \subsection{General Module Behavior} -The purpose of this module is to implement a magnetic field model that rotates with a planet fixed frame \frameDefinition{P}. Here $\hat{\bm p}_{3}$ is the typical positive rotation axis and $\hat{\bm p}_{1}$ and $\hat{\bm p}_{2}$ span the planet's equatorial plane. +The purpose of this module is to implement a magnetic field model that rotates with a planet fixed frame \frameDefinition{P}. Here $\hat{\bm p}_{3}$ is the typical positive rotation axis and $\hat{\bm p}_{1}$ and $\hat{\bm p}_{2}$ span the planet's equatorial plane. -{\tt MagneticFieldCenteredDipole} is a child of the {\tt MagneticFieldBase} base class and provides a simple centered dipole magnetic field model. +{\tt MagneticFieldCenteredDipole} is a child of the {\tt MagneticFieldBase} base class and provides a simple centered dipole magnetic field model. By invoking the magnetic field module, the default values are set such that the dipole parameters are zeroed and the magnetic field output is a zero vector. -The reach of the model controlled by setting the variables {\tt envMinReach} and {\tt envMaxReach} to positive values. These values are the radial distance from the planet center. The default values are -1 which turns off this checking where the atmosphere model as unbounded reach. +The reach of the model controlled by setting the variables {\tt envMinReach} and {\tt envMaxReach} to positive values. These values are the radial distance from the planet center. The default values are -1 which turns off this checking where the atmosphere model as unbounded reach. There are a multitude of magnetic field models.\footnote{\url { https://geomag.colorado.edu/geomagnetic-and-electric-field-models.html}} The goal with Basilisk is to provide a simple and consistent interface to a range of models. The list of models is expected to grow over time. @@ -17,7 +17,7 @@ \subsection{Planet Centric Spacecraft Position Vector} \begin{equation} \bm r_{B/P} = \bm r_{B/O} - \bm r_{P/O} \end{equation} -If no planet ephemeris message is specified, then the planet position vector $\bm r_{P/O}$ is set to zero. +If no planet ephemeris message is specified, then the planet position vector $\bm r_{P/O}$ is set to zero. Let $[PN]$ be the direction cosine matrix\cite{schaub} that relates the rotating planet-fixed frame relative to an inertial frame \frameDefinition{N}. The simulation provides the spacecraft position vector in inertial frame components. The planet centric position vector is then written in Earth-fixed frame components using \begin{equation} @@ -32,7 +32,7 @@ \subsection{Centered Dipole Magnetic Field Model} \begin{equation} V(\bm r_{B/P}) = \frac{\bm m \cdot \bm r_{B/P}}{|\bm r_{B/P}|^{3}} \end{equation} -with the dipole vector being defined in Earth fixed frame $\cal E$ coordinates as +with the dipole vector being defined in Earth fixed frame $\cal E$ coordinates as \begin{equation} \bm m = \leftexp{E}{\begin{bmatrix} g_{1}^{1} \\ h_{1}^{1} \\ g_{1}^{0} @@ -43,7 +43,7 @@ \subsection{Centered Dipole Magnetic Field Model} \bm B(\bm r_{B/P}) = - \nabla V(\bm r_{B/P}) = \frac{3 (\bm m \cdot \bm r_{B/P}) \bm r_{B/P} - \bm r_{B/P} \cdot \bm r_{B/P} \bm m}{|\bm r_{B/P}|^{5}} = \frac{1}{|\bm r_{B/P}|^{3}} \left ( 3 (\bm m \cdot \hat{\bm r}) \hat{\bm r} - \bm m \right) \end{equation} -where +where \begin{equation} \hat{\bm r} = \frac{\bm r_{B/P}}{|\bm r_{B/P}|} \end{equation} diff --git a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secModuleFunctions.tex b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secModuleFunctions.tex index fbf8905ea3..e493dc5000 100644 --- a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secModuleFunctions.tex +++ b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secModuleFunctions.tex @@ -4,10 +4,10 @@ \section{Module Functions} This module will: \begin{itemize} - \item \textbf{Compute magnetic field vector}: Each of the provided models is fundamentally intended to compute the planetary magnetic vector for a spacecraft. - \item \textbf {Subscribe to model-relevant information:} Each provided magnetic field model requires different input information to operate, such as spacecraft positions or time. This module automatically attempts to subscribe to the relevant messages for a specified model. + \item \textbf{Compute magnetic field vector}: Each of the provided models is fundamentally intended to compute the planetary magnetic vector for a spacecraft. + \item \textbf {Subscribe to model-relevant information:} Each provided magnetic field model requires different input information to operate, such as spacecraft positions or time. This module automatically attempts to subscribe to the relevant messages for a specified model. \item \textbf{Support for multiple spacecraft and model types} Only one magnetic field module is required for each planet, and can support an arbitrary number of spacecraft. Output messages for individual spacecraft are automatically named based on the environment type. \end{itemize} \section{Module Assumptions and Limitations} -Individual magnetic field models are complex and have their own assumptions. The reader is referred to the cited literature to learn more about the model limitations and challenges. \ No newline at end of file +Individual magnetic field models are complex and have their own assumptions. The reader is referred to the cited literature to learn more about the model limitations and challenges. diff --git a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secTest.tex b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secTest.tex index 37ae45c80b..a1a6be5b84 100644 --- a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secTest.tex +++ b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secTest.tex @@ -1,20 +1,20 @@ % !TEX root = ./Basilisk-magFieldDipole-20190309.tex \section{Test Description and Success Criteria} -This section describes the specific unit tests conducted on this module. +This section describes the specific unit tests conducted on this module. This unit test only runs the magnetic field Basilisk module with two fixed spacecraft state input messages. The simulation option {\tt useDefault} checks if the module default settings are used that lead to a zero magnetic field vector, or if the centered dipole parameters are setup manually. The option {\tt useMinReach} dictates if the minimum orbit radius check is performed, while the option {\tt useMaxReach} checks if the maximum reach check is performed. The option {\tt usePlanetEphemeris} checks if a planet state input message should be created. All permutations are checked. \section{Test Parameters} -The simulation tolerances are shown in Table~\ref{tab:errortol}. In each simulation the neutral density output message is checked relative to python computed true values. +The simulation tolerances are shown in Table~\ref{tab:errortol}. In each simulation the neutral density output message is checked relative to python computed true values. \begin{table}[htbp] \caption{Error tolerance for each test.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ \hline - {\tt magneticField vector} & \input{AutoTeX/unitTestToleranceValue} (relative) \\ + {\tt magneticField vector} & \input{AutoTeX/unitTestToleranceValue} (relative) \\ \hline\hline \end{tabular} \end{table} @@ -31,29 +31,26 @@ \section{Test Results} \caption{Test result for {\tt test\_unitTestMagneticField.py}} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | c | c | c } % Column formatting, + \begin{tabular}{c | c | c | c | c } % Column formatting, \hline\hline - {\tt useDefault} & {\tt useMinReach} & {\tt useMaxReach} & {\tt usePlanetEphemeris} & \textbf{Pass/Fail} \\ + {\tt useDefault} & {\tt useMinReach} & {\tt useMaxReach} & {\tt usePlanetEphemeris} & \textbf{Pass/Fail} \\ \hline - False & False & False & False & \input{AutoTeX/unitTestPassFailFalseFalseFalseFalse} \\ - False & False & False & True & \input{AutoTeX/unitTestPassFailFalseFalseFalseTrue} \\ - False & False & True & False & \input{AutoTeX/unitTestPassFailFalseFalseTrueFalse} \\ - False & False & True & True & \input{AutoTeX/unitTestPassFailFalseFalseTrueTrue} \\ - False & True & False & False & \input{AutoTeX/unitTestPassFailFalseTrueFalseFalse} \\ - False & True & False & True & \input{AutoTeX/unitTestPassFailFalseTrueFalseTrue} \\ - False & True & True & False & \input{AutoTeX/unitTestPassFailFalseTrueTrueFalse} \\ - False & True & True & True & \input{AutoTeX/unitTestPassFailFalseTrueTrueTrue} \\ - True & False & False & False & \input{AutoTeX/unitTestPassFailFalseFalseFalseFalse} \\ - True & False & False & True & \input{AutoTeX/unitTestPassFailTrueFalseFalseTrue} \\ - True & False & True & False & \input{AutoTeX/unitTestPassFailTrueFalseTrueFalse} \\ - True & False & True & True & \input{AutoTeX/unitTestPassFailTrueFalseTrueTrue} \\ - True & True & False & False & \input{AutoTeX/unitTestPassFailTrueTrueFalseFalse} \\ - True & True & False & True & \input{AutoTeX/unitTestPassFailTrueTrueFalseTrue} \\ - True & True & True & False & \input{AutoTeX/unitTestPassFailTrueTrueTrueFalse} \\ - True & True & True & True & \input{AutoTeX/unitTestPassFailTrueTrueTrueTrue} \\ + False & False & False & False & \input{AutoTeX/unitTestPassFailFalseFalseFalseFalse} \\ + False & False & False & True & \input{AutoTeX/unitTestPassFailFalseFalseFalseTrue} \\ + False & False & True & False & \input{AutoTeX/unitTestPassFailFalseFalseTrueFalse} \\ + False & False & True & True & \input{AutoTeX/unitTestPassFailFalseFalseTrueTrue} \\ + False & True & False & False & \input{AutoTeX/unitTestPassFailFalseTrueFalseFalse} \\ + False & True & False & True & \input{AutoTeX/unitTestPassFailFalseTrueFalseTrue} \\ + False & True & True & False & \input{AutoTeX/unitTestPassFailFalseTrueTrueFalse} \\ + False & True & True & True & \input{AutoTeX/unitTestPassFailFalseTrueTrueTrue} \\ + True & False & False & False & \input{AutoTeX/unitTestPassFailFalseFalseFalseFalse} \\ + True & False & False & True & \input{AutoTeX/unitTestPassFailTrueFalseFalseTrue} \\ + True & False & True & False & \input{AutoTeX/unitTestPassFailTrueFalseTrueFalse} \\ + True & False & True & True & \input{AutoTeX/unitTestPassFailTrueFalseTrueTrue} \\ + True & True & False & False & \input{AutoTeX/unitTestPassFailTrueTrueFalseFalse} \\ + True & True & False & True & \input{AutoTeX/unitTestPassFailTrueTrueFalseTrue} \\ + True & True & True & False & \input{AutoTeX/unitTestPassFailTrueTrueTrueFalse} \\ + True & True & True & True & \input{AutoTeX/unitTestPassFailTrueTrueTrueTrue} \\ \hline\hline \end{tabular} \end{table} - - - diff --git a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secUserGuide.tex b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secUserGuide.tex index 87a6cace01..f9fd29861d 100644 --- a/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secUserGuide.tex +++ b/src/simulation/environment/magneticFieldCenteredDipole/_Documentation/secUserGuide.tex @@ -10,9 +10,9 @@ \subsection{General Module Setup} magModule = magneticFieldCenteredDipole.MagneticFieldCenteredDipole() magModule = "CenteredDipole" \end{verbatim} -By default the model the dipole parameters are zeroed, resulting in a zeroed magnetic field message. +By default the model the dipole parameters are zeroed, resulting in a zeroed magnetic field message. -The model can be added to a task like other simModels. +The model can be added to a task like other simModels. \begin{verbatim} unitTestSim.AddModelToTask(unitTaskName, testModule) \end{verbatim} @@ -46,13 +46,12 @@ \subsection{Centered Dipole Magnetic Parameters} \end{verbatim} where $g_{1}^{0}$, $g_{1}^{1}$ and $h_{1}^{1}$ are the first three expansion terms of the IGRF spherical harmonics model.\footnote{\url{https://www.ngdc.noaa.gov/IAGA/vmod/igrf.html}} -The python support file {\tt simSetPlanetEnvironment.py} contains a helper function called +The python support file {\tt simSetPlanetEnvironment.py} contains a helper function called \begin{verbatim} centeredDipoleMagField() -\end{verbatim} +\end{verbatim} which helps setup common NASA centered dipole models for a range of planets that contain global magnetic fields. This possible planet names includes mercury, earth, jupiter, saturn, uranus and neptune. The function is then called with \begin{verbatim} simSetPlanetEnvironment.centeredDipoleMagField(testModule, "jupiter") \end{verbatim} to setup the named planets dipole model. - diff --git a/src/simulation/environment/magneticFieldCenteredDipole/_UnitTest/Support/magFieldModels.nb b/src/simulation/environment/magneticFieldCenteredDipole/_UnitTest/Support/magFieldModels.nb index ffb4d288eb..524e5c7f01 100644 --- a/src/simulation/environment/magneticFieldCenteredDipole/_UnitTest/Support/magFieldModels.nb +++ b/src/simulation/environment/magneticFieldCenteredDipole/_UnitTest/Support/magFieldModels.nb @@ -21,20 +21,20 @@ Notebook[{ Cell[CellGroupData[{ Cell["Magnetic Field Models", "Title", - CellChangeTimes->{{3.7611602395386744`*^9, + CellChangeTimes->{{3.7611602395386744`*^9, 3.761160242506854*^9}},ExpressionUUID->"b3741299-3e65-425c-9750-\ 98a2f5725af8"], Cell[CellGroupData[{ Cell["Simple Dipole Models", "Section", - CellChangeTimes->{{3.7611602460144176`*^9, + CellChangeTimes->{{3.7611602460144176`*^9, 3.7611602512741127`*^9}},ExpressionUUID->"dde990d1-6cd6-4ed8-bd65-\ f522b6c03d22"], Cell[BoxData[ RowBox[{ - RowBox[{"req", " ", "=", " ", + RowBox[{"req", " ", "=", " ", RowBox[{"6371.2", "*", "1000"}]}], ";"}]], "Input", CellChangeTimes->{{3.761171685497162*^9, 3.761171704817787*^9}, { 3.761222714141919*^9, 3.7612227190511703`*^9}}, @@ -43,103 +43,103 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell["Spacecraft Design", "Subsection", - CellChangeTimes->{{3.761161263796702*^9, + CellChangeTimes->{{3.761161263796702*^9, 3.761161267289089*^9}},ExpressionUUID->"24867fba-8e3d-40df-863f-\ ea8de3e0c6f3"], Cell["\<\ This field assumes a north geomagnetic pole of 78.5\:02da N and 69.7\:02daW\ \>", "Text", - CellChangeTimes->{{3.761161216773966*^9, + CellChangeTimes->{{3.761161216773966*^9, 3.761161240043467*^9}},ExpressionUUID->"94accf41-55f4-4829-83e7-\ 9ecfd8816b6c"], Cell[BoxData[ RowBox[{ - RowBox[{"magFieldGrifin", "[", "rVec_", "]"}], ":=", - RowBox[{"Block", "[", + RowBox[{"magFieldGrifin", "[", "rVec_", "]"}], ":=", + RowBox[{"Block", "[", RowBox[{ - RowBox[{"{", - RowBox[{"rMag", ",", "\[Lambda]", ",", "\[Phi]", ",", "BfieldNED"}], - "}"}], ",", "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"rMag", ",", "\[Lambda]", ",", "\[Phi]", ",", "BfieldNED"}], + "}"}], ",", "\[IndentingNewLine]", RowBox[{ - RowBox[{"rMag", " ", "=", " ", - RowBox[{"Norm", "[", "rVec", "]"}]}], ";", "\[IndentingNewLine]", - RowBox[{"If", " ", "[", + RowBox[{"rMag", " ", "=", " ", + RowBox[{"Norm", "[", "rVec", "]"}]}], ";", "\[IndentingNewLine]", + RowBox[{"If", " ", "[", RowBox[{ RowBox[{ RowBox[{ - RowBox[{"rVec", "[", - RowBox[{"[", "1", "]"}], "]"}], "\[NotEqual]", "0"}], " ", "&&", - " ", + RowBox[{"rVec", "[", + RowBox[{"[", "1", "]"}], "]"}], "\[NotEqual]", "0"}], " ", "&&", + " ", RowBox[{ - RowBox[{"rVec", "[", - RowBox[{"[", "2", "]"}], "]"}], " ", "\[NotEqual]", " ", "0"}]}], - ",", "\[IndentingNewLine]", - RowBox[{"\[Lambda]", " ", "=", " ", - RowBox[{"ArcTan", "[", + RowBox[{"rVec", "[", + RowBox[{"[", "2", "]"}], "]"}], " ", "\[NotEqual]", " ", "0"}]}], + ",", "\[IndentingNewLine]", + RowBox[{"\[Lambda]", " ", "=", " ", + RowBox[{"ArcTan", "[", RowBox[{ - RowBox[{"rVec", "[", - RowBox[{"[", "1", "]"}], "]"}], ",", - RowBox[{"rVec", "[", - RowBox[{"[", "2", "]"}], "]"}]}], "]"}]}], "\[IndentingNewLine]", - ",", " ", - RowBox[{"\[Lambda]", " ", "=", " ", "0"}]}], "]"}], ";", - "\[IndentingNewLine]", - RowBox[{"\[Phi]", " ", "=", " ", - RowBox[{"ArcSin", "[", + RowBox[{"rVec", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", + RowBox[{"rVec", "[", + RowBox[{"[", "2", "]"}], "]"}]}], "]"}]}], "\[IndentingNewLine]", + ",", " ", + RowBox[{"\[Lambda]", " ", "=", " ", "0"}]}], "]"}], ";", + "\[IndentingNewLine]", + RowBox[{"\[Phi]", " ", "=", " ", + RowBox[{"ArcSin", "[", RowBox[{ - RowBox[{"rVec", "[", - RowBox[{"[", "3", "]"}], "]"}], "/", "rMag"}], "]"}]}], ";", - "\[IndentingNewLine]", "\[IndentingNewLine]", - RowBox[{"BfieldNED", " ", "=", " ", + RowBox[{"rVec", "[", + RowBox[{"[", "3", "]"}], "]"}], "/", "rMag"}], "]"}]}], ";", + "\[IndentingNewLine]", "\[IndentingNewLine]", + RowBox[{"BfieldNED", " ", "=", " ", RowBox[{ - RowBox[{"-", + RowBox[{"-", RowBox[{ - RowBox[{"(", - RowBox[{"req", "/", "rMag"}], ")"}], "^", "3."}]}], + RowBox[{"(", + RowBox[{"req", "/", "rMag"}], ")"}], "^", "3."}]}], RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", - RowBox[{"Cos", "[", "\[Phi]", "]"}]}], ",", " ", + RowBox[{"-", + RowBox[{"Cos", "[", "\[Phi]", "]"}]}], ",", " ", RowBox[{ - RowBox[{"Sin", "[", "\[Phi]", "]"}], - RowBox[{"Cos", "[", "\[Lambda]", "]"}]}], ",", " ", + RowBox[{"Sin", "[", "\[Phi]", "]"}], + RowBox[{"Cos", "[", "\[Lambda]", "]"}]}], ",", " ", RowBox[{ - RowBox[{"Sin", "[", "\[Phi]", "]"}], - RowBox[{"Sin", "[", "\[Lambda]", "]"}]}]}], "}"}], ",", - RowBox[{"{", - RowBox[{"0", ",", - RowBox[{"Sin", "[", "\[Lambda]", "]"}], ",", - RowBox[{"-", - RowBox[{"Cos", "[", "\[Lambda]", "]"}]}]}], "}"}], ",", + RowBox[{"Sin", "[", "\[Phi]", "]"}], + RowBox[{"Sin", "[", "\[Lambda]", "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", + RowBox[{"Sin", "[", "\[Lambda]", "]"}], ",", + RowBox[{"-", + RowBox[{"Cos", "[", "\[Lambda]", "]"}]}]}], "}"}], ",", RowBox[{ - RowBox[{"-", "2"}], - RowBox[{"{", + RowBox[{"-", "2"}], + RowBox[{"{", RowBox[{ - RowBox[{"Sin", "[", "\[Phi]", "]"}], ",", " ", + RowBox[{"Sin", "[", "\[Phi]", "]"}], ",", " ", RowBox[{ - RowBox[{"Cos", "[", "\[Phi]", "]"}], - RowBox[{"Cos", "[", "\[Lambda]", "]"}]}], " ", ",", + RowBox[{"Cos", "[", "\[Phi]", "]"}], + RowBox[{"Cos", "[", "\[Lambda]", "]"}]}], " ", ",", RowBox[{ - RowBox[{"Cos", "[", "\[Phi]", "]"}], - RowBox[{"Sin", "[", "\[Lambda]", "]"}]}]}], "}"}]}]}], "}"}], - " ", ".", - RowBox[{"{", - RowBox[{"29000", ",", "1900", ",", - RowBox[{"-", "5530"}]}], "}"}]}]}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"Return", "[", "BfieldNED", "]"}]}]}], "\[IndentingNewLine]", + RowBox[{"Cos", "[", "\[Phi]", "]"}], + RowBox[{"Sin", "[", "\[Lambda]", "]"}]}]}], "}"}]}]}], "}"}], + " ", ".", + RowBox[{"{", + RowBox[{"29000", ",", "1900", ",", + RowBox[{"-", "5530"}]}], "}"}]}]}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"Return", "[", "BfieldNED", "]"}]}]}], "\[IndentingNewLine]", "]"}]}]], "Input", CellChangeTimes->{{3.761160791841695*^9, 3.761160808834175*^9}, { - 3.761160839591168*^9, 3.761160918446228*^9}, {3.761161011372511*^9, - 3.761161129370013*^9}, {3.7611611824817123`*^9, 3.761161204966709*^9}, + 3.761160839591168*^9, 3.761160918446228*^9}, {3.761161011372511*^9, + 3.761161129370013*^9}, {3.7611611824817123`*^9, 3.761161204966709*^9}, 3.761161309030128*^9, {3.761161351692739*^9, 3.761161357843279*^9}, { - 3.761161527079445*^9, 3.7611615555829906`*^9}, 3.761161596470673*^9, - 3.761161691091264*^9, {3.761167300474043*^9, 3.761167305922627*^9}, + 3.761161527079445*^9, 3.7611615555829906`*^9}, 3.761161596470673*^9, + 3.761161691091264*^9, {3.761167300474043*^9, 3.761167305922627*^9}, 3.761171696011424*^9}, CellLabel-> "In[216]:=",ExpressionUUID->"6878ffd4-baee-4dba-af5f-d5f1a57ad4db"], @@ -147,21 +147,21 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Norm", "[", - RowBox[{"rVecE", " ", "=", " ", + RowBox[{"Norm", "[", + RowBox[{"rVecE", " ", "=", " ", RowBox[{ - RowBox[{"{", - RowBox[{"5000.", ",", "5000", ",", "2500"}], "}"}], "1000"}]}], + RowBox[{"{", + RowBox[{"5000.", ",", "5000", ",", "2500"}], "}"}], "1000"}]}], "]"}]], "Input", CellChangeTimes->{{3.761161320039953*^9, 3.761161336281125*^9}, { - 3.76116145307368*^9, 3.76116147163428*^9}, {3.76122273763161*^9, + 3.76116145307368*^9, 3.76116147163428*^9}, {3.76122273763161*^9, 3.761222738195681*^9}}, CellLabel-> "In[212]:=",ExpressionUUID->"82ee7af1-ff7d-4fa6-9eda-c7b8fa079dde"], Cell[BoxData["7.5`*^6"], "Output", CellChangeTimes->{{3.761161338233626*^9, 3.76116135930447*^9}, { - 3.761161454909383*^9, 3.761161472378436*^9}, 3.761161599569516*^9, + 3.761161454909383*^9, 3.761161472378436*^9}, 3.761161599569516*^9, 3.761161692801365*^9, 3.761170031763824*^9, 3.761222739752166*^9}, CellLabel-> "Out[212]=",ExpressionUUID->"b70a4575-74cc-47ce-b7d6-2b6793485e15"] @@ -170,19 +170,19 @@ Cell[BoxData["7.5`*^6"], "Output", Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"\[Lambda]", " ", "=", " ", - RowBox[{"ArcTan", "[", + RowBox[{"\[Lambda]", " ", "=", " ", + RowBox[{"ArcTan", "[", RowBox[{ - RowBox[{"rVecE", "[", - RowBox[{"[", "1", "]"}], "]"}], ",", - RowBox[{"rVecE", "[", + RowBox[{"rVecE", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", + RowBox[{"rVecE", "[", RowBox[{"[", "2", "]"}], "]"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.7611701531834517`*^9, 3.761170154271412*^9}}, CellLabel-> "In[213]:=",ExpressionUUID->"a5cdc0a9-ea0b-4a27-8828-b6531d15c1ef"], Cell[BoxData["0.7853981633974483`"], "Output", - CellChangeTimes->{3.761170154877943*^9, 3.7611707692472754`*^9, + CellChangeTimes->{3.761170154877943*^9, 3.7611707692472754`*^9, 3.761222742071471*^9}, CellLabel-> "Out[213]=",ExpressionUUID->"ae15ec63-8a0d-42bc-924f-f82f2b55cd97"] @@ -191,18 +191,18 @@ Cell[BoxData["0.7853981633974483`"], "Output", Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"\[Phi]", " ", "=", " ", - RowBox[{"ArcSin", "[", + RowBox[{"\[Phi]", " ", "=", " ", + RowBox[{"ArcSin", "[", RowBox[{ - RowBox[{"rVecE", "[", - RowBox[{"[", "3", "]"}], "]"}], "/", + RowBox[{"rVecE", "[", + RowBox[{"[", "3", "]"}], "]"}], "/", RowBox[{"Norm", "[", "rVecE", "]"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.761170170514616*^9, 3.761170179119265*^9}}, CellLabel-> "In[214]:=",ExpressionUUID->"12eb5faa-31f8-422b-a7f5-bc8b3abe23cb"], Cell[BoxData["0.3398369094541219`"], "Output", - CellChangeTimes->{{3.761170175504636*^9, 3.761170179522511*^9}, + CellChangeTimes->{{3.761170175504636*^9, 3.761170179522511*^9}, 3.761170769989255*^9, 3.761222742838048*^9}, CellLabel-> "Out[214]=",ExpressionUUID->"b142b309-917e-482a-b6ea-ac0724521052"] @@ -211,33 +211,33 @@ Cell[BoxData["0.3398369094541219`"], "Output", Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"TE", "=", + RowBox[{"TE", "=", RowBox[{ - RowBox[{"Euler2", "[", + RowBox[{"Euler2", "[", RowBox[{ - RowBox[{"-", "90"}], " ", "Degree"}], "]"}], ".", - RowBox[{"Euler2", "[", - RowBox[{"-", "\[Phi]"}], "]"}], ".", + RowBox[{"-", "90"}], " ", "Degree"}], "]"}], ".", + RowBox[{"Euler2", "[", + RowBox[{"-", "\[Phi]"}], "]"}], ".", RowBox[{"Euler3", "[", "\[Lambda]", "]"}]}]}]], "Input", CellLabel-> "In[215]:=",ExpressionUUID->"e0915515-3d9a-494d-84c9-83100a17d07d"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "0.23570226039551584`"}], ",", - RowBox[{"-", "0.2357022603955158`"}], ",", "0.9428090415820634`"}], - "}"}], ",", - RowBox[{"{", + RowBox[{"-", "0.23570226039551584`"}], ",", + RowBox[{"-", "0.2357022603955158`"}], ",", "0.9428090415820634`"}], + "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", "0.7071067811865475`"}], ",", "0.7071067811865476`", ",", - "0.`"}], "}"}], ",", - RowBox[{"{", + RowBox[{"-", "0.7071067811865475`"}], ",", "0.7071067811865476`", ",", + "0.`"}], "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", "0.6666666666666667`"}], ",", - RowBox[{"-", "0.6666666666666666`"}], ",", + RowBox[{"-", "0.6666666666666667`"}], ",", + RowBox[{"-", "0.6666666666666666`"}], ",", RowBox[{"-", "0.3333333333333333`"}]}], "}"}]}], "}"}]], "Output", CellChangeTimes->{3.7612212089566717`*^9, 3.761222745377109*^9}, CellLabel-> @@ -247,21 +247,21 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"BT1", "=", + RowBox[{"BT1", "=", RowBox[{"magFieldGrifin", "[", "rVecE", "]"}]}]], "Input", CellChangeTimes->{{3.761161339911333*^9, 3.761161343292625*^9}, { - 3.761221140240713*^9, 3.761221144456441*^9}, {3.7612212184765472`*^9, + 3.761221140240713*^9, 3.761221144456441*^9}, {3.7612212184765472`*^9, 3.761221218657753*^9}}, CellLabel-> "In[217]:=",ExpressionUUID->"a59826de-fc49-44d6-ab96-ce59de9b2e96"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"17285.57278076451`", ",", - RowBox[{"-", "3220.725714981534`"}], ",", "8884.811240310806`"}], + RowBox[{"{", + RowBox[{"17285.57278076451`", ",", + RowBox[{"-", "3220.725714981534`"}], ",", "8884.811240310806`"}], "}"}]], "Output", - CellChangeTimes->{{3.7611613437218857`*^9, 3.7611613599891567`*^9}, - 3.7611616010414667`*^9, 3.761161693707308*^9, 3.761167317210127*^9, + CellChangeTimes->{{3.7611613437218857`*^9, 3.7611613599891567`*^9}, + 3.7611616010414667`*^9, 3.761161693707308*^9, 3.761167317210127*^9, 3.761221145494506*^9, 3.761221220245243*^9, 3.76122275902842*^9}, CellLabel-> "Out[217]=",ExpressionUUID->"e9f7e22f-f7bd-4e21-902b-df1436e5d56d"] @@ -270,7 +270,7 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"BE1", " ", "=", " ", + RowBox[{"BE1", " ", "=", " ", RowBox[{ RowBox[{"Transpose", "[", "TE", "]"}], ".", "BT1"}]}]], "Input", CellChangeTimes->{{3.76122121308971*^9, 3.761221234574098*^9}}, @@ -278,10 +278,10 @@ Cell[BoxData[ "In[218]:=",ExpressionUUID->"31093fc9-15dd-4b86-8d46-502acb8cd725"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "7720.0590767926005`"}], ",", - RowBox[{"-", "12274.85306360327`"}], ",", "13335.390559859321`"}], + RowBox[{"-", "7720.0590767926005`"}], ",", + RowBox[{"-", "12274.85306360327`"}], ",", "13335.390559859321`"}], "}"}]], "Output", CellChangeTimes->{3.7612212353785257`*^9, 3.7612227650866327`*^9}, CellLabel-> @@ -290,9 +290,9 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ - RowBox[{"rVecEnorth", " ", "=", + RowBox[{"rVecEnorth", " ", "=", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{"0", ",", "0", ",", "7000"}], "}"}], "1000"}]}], ";"}]], "Input",\ CellChangeTimes->{{3.761170962459869*^9, 3.7611709741036797`*^9}, { @@ -303,22 +303,22 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"BT2", "=", + RowBox[{"BT2", "=", RowBox[{"magFieldGrifin", "[", "rVecEnorth", "]"}]}]], "Input", - CellChangeTimes->{{3.761161339911333*^9, 3.761161343292625*^9}, - 3.7611614466741877`*^9, {3.761161477220595*^9, 3.76116148242339*^9}, + CellChangeTimes->{{3.761161339911333*^9, 3.761161343292625*^9}, + 3.7611614466741877`*^9, {3.761161477220595*^9, 3.76116148242339*^9}, 3.761170979635881*^9, {3.761223340256003*^9, 3.76122334148577*^9}}, CellLabel-> "In[238]:=",ExpressionUUID->"07fcd2a8-58d6-4596-b437-b114ab723d6e"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "1432.5942244438575`"}], ",", - RowBox[{"-", "4169.603190091859`"}], ",", "43731.82369354933`"}], + RowBox[{"-", "1432.5942244438575`"}], ",", + RowBox[{"-", "4169.603190091859`"}], ",", "43731.82369354933`"}], "}"}]], "Output", - CellChangeTimes->{3.761161483823567*^9, 3.7611615587364597`*^9, - 3.761161602297242*^9, 3.761161694471571*^9, 3.7611673091512814`*^9, + CellChangeTimes->{3.761161483823567*^9, 3.7611615587364597`*^9, + 3.761161602297242*^9, 3.761161694471571*^9, 3.7611673091512814`*^9, 3.761170980305799*^9, 3.7612228047086697`*^9, 3.761223342014791*^9}, CellLabel-> "Out[238]=",ExpressionUUID->"9653e53a-98c5-486f-a10e-779e5bbfa2ff"] @@ -327,31 +327,31 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"TE", "=", + RowBox[{"TE", "=", RowBox[{ - RowBox[{"Euler2", "[", + RowBox[{"Euler2", "[", RowBox[{ - RowBox[{"-", "90"}], " ", "Degree"}], "]"}], ".", - RowBox[{"Euler2", "[", + RowBox[{"-", "90"}], " ", "Degree"}], "]"}], ".", + RowBox[{"Euler2", "[", RowBox[{ - RowBox[{"-", "90."}], " ", "Degree"}], "]"}], ".", + RowBox[{"-", "90."}], " ", "Degree"}], "]"}], ".", RowBox[{"Euler3", "[", "0", "]"}]}]}]], "Input", CellChangeTimes->{{3.761223328018552*^9, 3.761223333193447*^9}}, CellLabel-> "In[237]:=",ExpressionUUID->"c6b679f5-cee4-42bb-851d-9a40de1be838"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "1.`"}], ",", "0.`", ",", "6.123233995736766`*^-17"}], - "}"}], ",", - RowBox[{"{", - RowBox[{"0.`", ",", "1.`", ",", "0.`"}], "}"}], ",", - RowBox[{"{", + RowBox[{"-", "1.`"}], ",", "0.`", ",", "6.123233995736766`*^-17"}], + "}"}], ",", + RowBox[{"{", + RowBox[{"0.`", ",", "1.`", ",", "0.`"}], "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", "6.123233995736766`*^-17"}], ",", "0.`", ",", + RowBox[{"-", "6.123233995736766`*^-17"}], ",", "0.`", ",", RowBox[{"-", "1.`"}]}], "}"}]}], "}"}]], "Output", CellChangeTimes->{3.761223334088695*^9}, CellLabel-> @@ -361,7 +361,7 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"BE2", "=", + RowBox[{"BE2", "=", RowBox[{ RowBox[{"Transpose", "[", "TE", "]"}], ".", "BT2"}]}]], "Input", CellChangeTimes->{{3.761223344268951*^9, 3.761223364415888*^9}}, @@ -369,9 +369,9 @@ Cell[BoxData[ "In[239]:=",ExpressionUUID->"fadc9278-7193-4c0b-b42c-87323b693e8d"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"1432.5942244438547`", ",", - RowBox[{"-", "4169.603190091859`"}], ",", + RowBox[{"{", + RowBox[{"1432.5942244438547`", ",", + RowBox[{"-", "4169.603190091859`"}], ",", RowBox[{"-", "43731.82369354933`"}]}], "}"}]], "Output", CellChangeTimes->{3.761223365065268*^9}, CellLabel-> @@ -383,7 +383,7 @@ Cell[CellGroupData[{ Cell["centered dipole", "Subsection", CellChangeTimes->{{3.761161280026561*^9, 3.7611612854660807`*^9}, { - 3.761162314126008*^9, + 3.761162314126008*^9, 3.761162316453082*^9}},ExpressionUUID->"8f6aa3f4-1f72-4525-96ae-\ 4c0ba2c6e956"], @@ -394,48 +394,48 @@ Cell[BoxData[ Cell[BoxData[ RowBox[{ - RowBox[{"centeredDipole", "[", "rVecE_", "]"}], ":=", - RowBox[{"Block", "[", + RowBox[{"centeredDipole", "[", "rVecE_", "]"}], ":=", + RowBox[{"Block", "[", RowBox[{ - RowBox[{"{", - RowBox[{"rMag", ",", "m", ",", "rHat", ",", "BE"}], "}"}], ",", - "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"rMag", ",", "m", ",", "rHat", ",", "BE"}], "}"}], ",", + "\[IndentingNewLine]", RowBox[{ - RowBox[{"rMag", " ", "=", " ", - RowBox[{"Norm", "[", "rVecE", " ", "]"}]}], ";", "\[IndentingNewLine]", - - RowBox[{"rHat", " ", "=", " ", - RowBox[{"rVecE", " ", "/", "rMag"}]}], ";", "\[IndentingNewLine]", - RowBox[{"m", " ", "=", " ", + RowBox[{"rMag", " ", "=", " ", + RowBox[{"Norm", "[", "rVecE", " ", "]"}]}], ";", "\[IndentingNewLine]", + + RowBox[{"rHat", " ", "=", " ", + RowBox[{"rVecE", " ", "/", "rMag"}]}], ";", "\[IndentingNewLine]", + RowBox[{"m", " ", "=", " ", RowBox[{ - RowBox[{"req", "^", "3"}], " ", - RowBox[{"{", + RowBox[{"req", "^", "3"}], " ", + RowBox[{"{", RowBox[{ - RowBox[{"-", "1900"}], ",", " ", "5530", ",", - RowBox[{"-", "29000"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"BE", " ", "=", " ", + RowBox[{"-", "1900"}], ",", " ", "5530", ",", + RowBox[{"-", "29000"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"BE", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"3", " ", - RowBox[{"Outer", "[", - RowBox[{"Times", ",", " ", "rHat", ",", " ", "rHat"}], "]"}]}], - "-", " ", - RowBox[{"IdentityMatrix", "[", "3", "]"}]}], ")"}], ".", "m"}], "/", - RowBox[{"rMag", "^", "3"}]}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"Return", "[", "BE", "]"}], ";"}]}], "\[IndentingNewLine]", + RowBox[{"3", " ", + RowBox[{"Outer", "[", + RowBox[{"Times", ",", " ", "rHat", ",", " ", "rHat"}], "]"}]}], + "-", " ", + RowBox[{"IdentityMatrix", "[", "3", "]"}]}], ")"}], ".", "m"}], "/", + RowBox[{"rMag", "^", "3"}]}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"Return", "[", "BE", "]"}], ";"}]}], "\[IndentingNewLine]", "]"}]}]], "Input", CellChangeTimes->{{3.76116983137292*^9, 3.761170024538576*^9}, { 3.7611700698615923`*^9, 3.761170071016591*^9}, 3.761170765715787*^9, { - 3.761171122072507*^9, 3.7611711969322557`*^9}, {3.761171320030284*^9, + 3.761171122072507*^9, 3.7611711969322557`*^9}, {3.761171320030284*^9, 3.761171352475054*^9}, {3.7611717012337933`*^9, 3.7611717021939373`*^9}, { - 3.761218335231361*^9, 3.761218344753099*^9}, {3.761218436512929*^9, - 3.7612184391472597`*^9}, 3.761221499254425*^9, {3.7612215509824133`*^9, + 3.761218335231361*^9, 3.761218344753099*^9}, {3.761218436512929*^9, + 3.7612184391472597`*^9}, 3.761221499254425*^9, {3.7612215509824133`*^9, 3.761221595348377*^9}, {3.761222474734695*^9, 3.761222475321272*^9}, { - 3.761222623492916*^9, 3.761222626775672*^9}, {3.7612227750527687`*^9, + 3.761222623492916*^9, 3.761222626775672*^9}, {3.7612227750527687`*^9, 3.761222787927289*^9}}, CellLabel-> "In[221]:=",ExpressionUUID->"8ba81c4b-319a-4ca6-8478-32dd93a469ed"], @@ -443,7 +443,7 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"BE", "=", + RowBox[{"BE", "=", RowBox[{"centeredDipole", "[", "rVecE", "]"}]}]], "Input", CellChangeTimes->{{3.761170036539823*^9, 3.7611700392762823`*^9}, { 3.761170224954677*^9, 3.761170226032494*^9}}, @@ -451,17 +451,17 @@ Cell[BoxData[ "In[222]:=",ExpressionUUID->"64c7ef8a-3115-4700-8014-b69fe63b8f36"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "7720.059076792601`"}], ",", - RowBox[{"-", "12274.853063603268`"}], ",", "13335.390559859323`"}], + RowBox[{"-", "7720.059076792601`"}], ",", + RowBox[{"-", "12274.853063603268`"}], ",", "13335.390559859323`"}], "}"}]], "Output", CellChangeTimes->{ - 3.7611700396903152`*^9, 3.7611700750677433`*^9, 3.7611702264696293`*^9, - 3.76117077209907*^9, 3.761171182925062*^9, {3.7611713350845346`*^9, - 3.761171357007045*^9}, 3.761218352802743*^9, 3.761218487281652*^9, - 3.761221501677948*^9, 3.7612216014089813`*^9, 3.7612224794708147`*^9, - 3.761222596095995*^9, 3.761222628713689*^9, {3.7612227799843683`*^9, + 3.7611700396903152`*^9, 3.7611700750677433`*^9, 3.7611702264696293`*^9, + 3.76117077209907*^9, 3.761171182925062*^9, {3.7611713350845346`*^9, + 3.761171357007045*^9}, 3.761218352802743*^9, 3.761218487281652*^9, + 3.761221501677948*^9, 3.7612216014089813`*^9, 3.7612224794708147`*^9, + 3.761222596095995*^9, 3.761222628713689*^9, {3.7612227799843683`*^9, 3.761222790893427*^9}}, CellLabel-> "Out[222]=",ExpressionUUID->"0d6a2877-d47a-414d-b09a-b90d4a5c5507"] @@ -470,18 +470,18 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"BE", "=", + RowBox[{"BE", "=", RowBox[{"centeredDipole", "[", "rVecEnorth", "]"}]}]], "Input", CellChangeTimes->{{3.761170036539823*^9, 3.7611700392762823`*^9}, { - 3.761170224954677*^9, 3.761170226032494*^9}, {3.761218494531393*^9, + 3.761170224954677*^9, 3.761170226032494*^9}, {3.761218494531393*^9, 3.7612185024320374`*^9}}, CellLabel-> "In[226]:=",ExpressionUUID->"f50b92a1-7846-4430-ac31-0c53c26d145c"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"1432.594224443858`", ",", - RowBox[{"-", "4169.60319009186`"}], ",", + RowBox[{"{", + RowBox[{"1432.594224443858`", ",", + RowBox[{"-", "4169.60319009186`"}], ",", RowBox[{"-", "43731.82369354934`"}]}], "}"}]], "Output", CellChangeTimes->{ 3.761218503806026*^9, 3.761222489022039*^9, 3.761222629831821*^9, { @@ -498,7 +498,7 @@ Cell[BoxData["rVecE"], "Input", "In[240]:=",ExpressionUUID->"c16762c2-d5c7-48c2-a496-baf8b29070b9"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{"5.`*^6", ",", "5000000", ",", "2500000"}], "}"}]], "Output", CellChangeTimes->{3.761242327940855*^9}, CellLabel-> @@ -510,64 +510,64 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell["Testing", "Section", - CellChangeTimes->{{3.761242448595797*^9, + CellChangeTimes->{{3.761242448595797*^9, 3.761242451028562*^9}},ExpressionUUID->"8af01b86-4351-4f0f-b5dc-\ 3849b8cd0f6b"], Cell[BoxData[ RowBox[{ - RowBox[{"centeredDipole", "[", "rVecE_", "]"}], ":=", - RowBox[{"Block", "[", + RowBox[{"centeredDipole", "[", "rVecE_", "]"}], ":=", + RowBox[{"Block", "[", RowBox[{ - RowBox[{"{", - RowBox[{"rMag", ",", "m", ",", "rHat", ",", "BE"}], "}"}], ",", - "\[IndentingNewLine]", + RowBox[{"{", + RowBox[{"rMag", ",", "m", ",", "rHat", ",", "BE"}], "}"}], ",", + "\[IndentingNewLine]", RowBox[{ - RowBox[{"rMag", " ", "=", " ", - RowBox[{"Norm", "[", "rVecE", " ", "]"}]}], ";", "\[IndentingNewLine]", - - RowBox[{"rHat", " ", "=", " ", - RowBox[{"rVecE", " ", "/", "rMag"}]}], ";", "\[IndentingNewLine]", - RowBox[{"m", " ", "=", " ", + RowBox[{"rMag", " ", "=", " ", + RowBox[{"Norm", "[", "rVecE", " ", "]"}]}], ";", "\[IndentingNewLine]", + + RowBox[{"rHat", " ", "=", " ", + RowBox[{"rVecE", " ", "/", "rMag"}]}], ";", "\[IndentingNewLine]", + RowBox[{"m", " ", "=", " ", RowBox[{ - RowBox[{"req", "^", "3"}], " ", - RowBox[{"{", + RowBox[{"req", "^", "3"}], " ", + RowBox[{"{", RowBox[{ - RowBox[{"-", "2318"}], ",", " ", "5817", ",", - RowBox[{"-", "30926"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"BE", " ", "=", " ", + RowBox[{"-", "2318"}], ",", " ", "5817", ",", + RowBox[{"-", "30926"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"BE", " ", "=", " ", RowBox[{ RowBox[{ - RowBox[{"(", + RowBox[{"(", RowBox[{ - RowBox[{"3", " ", - RowBox[{"Outer", "[", - RowBox[{"Times", ",", " ", "rHat", ",", " ", "rHat"}], "]"}]}], - "-", " ", - RowBox[{"IdentityMatrix", "[", "3", "]"}]}], ")"}], ".", "m"}], "/", - RowBox[{"rMag", "^", "3"}]}]}], ";", "\[IndentingNewLine]", - "\[IndentingNewLine]", - RowBox[{"Return", "[", "BE", "]"}], ";"}]}], "\[IndentingNewLine]", + RowBox[{"3", " ", + RowBox[{"Outer", "[", + RowBox[{"Times", ",", " ", "rHat", ",", " ", "rHat"}], "]"}]}], + "-", " ", + RowBox[{"IdentityMatrix", "[", "3", "]"}]}], ")"}], ".", "m"}], "/", + RowBox[{"rMag", "^", "3"}]}]}], ";", "\[IndentingNewLine]", + "\[IndentingNewLine]", + RowBox[{"Return", "[", "BE", "]"}], ";"}]}], "\[IndentingNewLine]", "]"}]}]], "Input", CellChangeTimes->{{3.76116983137292*^9, 3.761170024538576*^9}, { 3.7611700698615923`*^9, 3.761170071016591*^9}, 3.761170765715787*^9, { - 3.761171122072507*^9, 3.7611711969322557`*^9}, {3.761171320030284*^9, + 3.761171122072507*^9, 3.7611711969322557`*^9}, {3.761171320030284*^9, 3.761171352475054*^9}, {3.7611717012337933`*^9, 3.7611717021939373`*^9}, { - 3.761218335231361*^9, 3.761218344753099*^9}, {3.761218436512929*^9, - 3.7612184391472597`*^9}, 3.761221499254425*^9, {3.7612215509824133`*^9, + 3.761218335231361*^9, 3.761218344753099*^9}, {3.761218436512929*^9, + 3.7612184391472597`*^9}, 3.761221499254425*^9, {3.7612215509824133`*^9, 3.761221595348377*^9}, {3.761222474734695*^9, 3.761222475321272*^9}, { - 3.761222623492916*^9, 3.761222626775672*^9}, {3.7612227750527687`*^9, + 3.761222623492916*^9, 3.761222626775672*^9}, {3.7612227750527687`*^9, 3.761222787927289*^9}, {3.761242469380966*^9, 3.761242477952352*^9}}, CellLabel->"In[1]:=",ExpressionUUID->"465a8117-cb2c-4f70-98fb-0b7de1f74832"], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"r0N", " ", "=", " ", - RowBox[{"{", + RowBox[{"r0N", " ", "=", " ", + RowBox[{"{", RowBox[{ - RowBox[{"-", "4857276.10161231"}], ",", " ", "1842048.9943826", ",", " ", + RowBox[{"-", "4857276.10161231"}], ",", " ", "1842048.9943826", ",", " ", "4023899.27495707"}], "}"}]}]], "Input", CellChangeTimes->{{3.761242330423923*^9, 3.7612423401398277`*^9}, { 3.761242393133493*^9, 3.7612423935465*^9}}, @@ -575,11 +575,11 @@ Cell[BoxData[ "In[248]:=",ExpressionUUID->"bfbaa73d-c39e-4728-a2d7-75366c1839a4"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "4.85727610161231`*^6"}], ",", "1.8420489943826`*^6", ",", + RowBox[{"-", "4.85727610161231`*^6"}], ",", "1.8420489943826`*^6", ",", "4.02389927495707`*^6"}], "}"}]], "Output", - CellChangeTimes->{3.7612423410066357`*^9, 3.761242399435523*^9, + CellChangeTimes->{3.7612423410066357`*^9, 3.761242399435523*^9, 3.761242480106926*^9}, CellLabel-> "Out[248]=",ExpressionUUID->"c95c6af3-4271-4861-9b0e-6982d2ac6d90"] @@ -589,18 +589,18 @@ Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ - RowBox[{"centeredDipole", "[", "r0N", "]"}], "/", + RowBox[{"centeredDipole", "[", "r0N", "]"}], "/", RowBox[{"10", "^", "9"}]}]], "Input", CellChangeTimes->{{3.7612423491235533`*^9, 3.761242397416885*^9}}, CellLabel-> "In[249]:=",ExpressionUUID->"cad30bf0-4220-4397-9748-952550f5a086"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"0.00003363477860608024`", ",", + RowBox[{"{", + RowBox[{"0.00003363477860608024`", ",", RowBox[{"-", "0.000017256541744194185`"}], ",", "2.076305539569021`*^-6"}], "}"}]], "Output", - CellChangeTimes->{{3.761242354714856*^9, 3.761242401295356*^9}, + CellChangeTimes->{{3.761242354714856*^9, 3.761242401295356*^9}, 3.761242480917715*^9}, CellLabel-> "Out[249]=",ExpressionUUID->"8ad74d48-3530-4307-8f59-7226453770cf"] @@ -609,23 +609,23 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"r1N", " ", "=", " ", - RowBox[{"{", + RowBox[{"r1N", " ", "=", " ", + RowBox[{"{", RowBox[{ RowBox[{"-", "4878712.87028477"}], ",", " ", "1850178.56687341", " ", ",", "4041658.07559224"}], "}"}]}]], "Input", CellChangeTimes->{{3.761242330423923*^9, 3.7612423401398277`*^9}, { - 3.761242393133493*^9, 3.7612423935465*^9}, {3.7612425021153927`*^9, + 3.761242393133493*^9, 3.7612423935465*^9}, {3.7612425021153927`*^9, 3.761242520890764*^9}}, CellLabel-> "In[250]:=",ExpressionUUID->"59ed12dd-0cc5-4307-97e8-73af33cb4954"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "4.87871287028477`*^6"}], ",", "1.85017856687341`*^6", ",", + RowBox[{"-", "4.87871287028477`*^6"}], ",", "1.85017856687341`*^6", ",", "4.04165807559224`*^6"}], "}"}]], "Output", - CellChangeTimes->{3.7612423410066357`*^9, 3.761242399435523*^9, + CellChangeTimes->{3.7612423410066357`*^9, 3.761242399435523*^9, 3.761242480106926*^9, 3.7612425227724733`*^9}, CellLabel-> "Out[250]=",ExpressionUUID->"ee5f63c1-26f2-408f-bba9-df981ac83101"] @@ -635,7 +635,7 @@ Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ - RowBox[{"centeredDipole", "[", "r1N", "]"}], "/", + RowBox[{"centeredDipole", "[", "r1N", "]"}], "/", RowBox[{"10", "^", "9"}]}]], "Input", CellChangeTimes->{{3.7612423491235533`*^9, 3.761242397416885*^9}, { 3.761242503692486*^9, 3.761242503756741*^9}}, @@ -643,11 +643,11 @@ Cell[BoxData[ "In[251]:=",ExpressionUUID->"55711612-2f8c-40e9-be0f-1315ef2955e9"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"0.0000331933563466706`", ",", + RowBox[{"{", + RowBox[{"0.0000331933563466706`", ",", RowBox[{"-", "0.00001703006718535939`"}], ",", "2.0490561411640843`*^-6"}], "}"}]], "Output", - CellChangeTimes->{{3.761242354714856*^9, 3.761242401295356*^9}, + CellChangeTimes->{{3.761242354714856*^9, 3.761242401295356*^9}, 3.761242480917715*^9, 3.761242523618688*^9}, CellLabel-> "Out[251]=",ExpressionUUID->"2f8095a1-ca87-401f-93c1-46dab0d0d486"] @@ -656,16 +656,16 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"centeredDipole", "[", - RowBox[{"{", + RowBox[{"centeredDipole", "[", + RowBox[{"{", RowBox[{"0", ",", "0", ",", "7000000"}], "}"}], "]"}]], "Input", CellChangeTimes->{{3.7614053014397087`*^9, 3.761405324161359*^9}}, CellLabel->"In[4]:=",ExpressionUUID->"0e4e5587-c304-4741-a0b5-e7e0806670d1"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"1747.7649538215064`", ",", - RowBox[{"-", "4386.000317678905`"}], ",", + RowBox[{"{", + RowBox[{"1747.7649538215064`", ",", + RowBox[{"-", "4386.000317678905`"}], ",", RowBox[{"-", "46636.21998436921`"}]}], "}"}]], "Output", CellChangeTimes->{{3.7614053094228497`*^9, 3.761405325290168*^9}}, CellLabel->"Out[4]=",ExpressionUUID->"133ac853-2d3e-42cc-9ef3-3030e38bd509"] @@ -675,7 +675,7 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell["Conversion", "Section", - CellChangeTimes->{{3.761485245067087*^9, + CellChangeTimes->{{3.761485245067087*^9, 3.761485246052724*^9}},ExpressionUUID->"0a12f6b4-054b-4db5-a66c-\ c240612b0be4"], @@ -683,31 +683,31 @@ Cell[CellGroupData[{ Cell["Nasa Earth", "Subsection", CellChangeTimes->{{3.761485906779035*^9, 3.761485907535144*^9}, { - 3.761500602368753*^9, + 3.761500602368753*^9, 3.761500603263917*^9}},ExpressionUUID->"9fae1077-8004-449e-bff3-\ d4f23b6cf454"], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"m", "=", - RowBox[{"0.306", " ", - RowBox[{"10", "^", + RowBox[{"m", "=", + RowBox[{"0.306", " ", + RowBox[{"10", "^", RowBox[{"-", "4", " "}]}]}]}]], "Input", CellChangeTimes->{{3.7614852505830097`*^9, 3.761485267833507*^9}, { - 3.761485329016069*^9, 3.761485333035672*^9}, {3.761485364176938*^9, + 3.761485329016069*^9, 3.761485333035672*^9}, {3.761485364176938*^9, 3.7614853642408323`*^9}, {3.761485455278775*^9, 3.761485460525774*^9}, { - 3.761485494119502*^9, 3.76148549530617*^9}, {3.761487468936984*^9, + 3.761485494119502*^9, 3.76148549530617*^9}, {3.761487468936984*^9, 3.761487492129184*^9}, 3.761500627326701*^9}, CellLabel-> "In[197]:=",ExpressionUUID->"073ef848-ba46-4a1f-acb3-9a3435c9d850"], Cell[BoxData["0.0000306`"], "Output", CellChangeTimes->{ - 3.761485268273548*^9, {3.761485335550934*^9, 3.76148536462856*^9}, - 3.7614854610495996`*^9, 3.761485495758309*^9, {3.7614874739745083`*^9, - 3.761487500239915*^9}, 3.7614984424302797`*^9, 3.761498528260454*^9, - 3.761499067111679*^9, 3.761500640112195*^9, 3.761500850980777*^9, + 3.761485268273548*^9, {3.761485335550934*^9, 3.76148536462856*^9}, + 3.7614854610495996`*^9, 3.761485495758309*^9, {3.7614874739745083`*^9, + 3.761487500239915*^9}, 3.7614984424302797`*^9, 3.761498528260454*^9, + 3.761499067111679*^9, 3.761500640112195*^9, 3.761500850980777*^9, 3.7615008881541653`*^9}, CellLabel-> "Out[197]=",ExpressionUUID->"a4d87ae3-a234-4ba8-9c92-1d188ae498cd"] @@ -715,20 +715,20 @@ Cell[BoxData["0.0000306`"], "Output", Cell[BoxData[{ RowBox[{ - RowBox[{"\[Lambda]", " ", "=", " ", - RowBox[{"70.8", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[Lambda]", " ", "=", " ", + RowBox[{"70.8", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"tilt", " ", "=", " ", - RowBox[{"11.2", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"tilt", " ", "=", " ", + RowBox[{"11.2", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Theta]m", " ", "=", " ", - RowBox[{"\[Pi]", "-", "tilt"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[Theta]m", " ", "=", " ", + RowBox[{"\[Pi]", "-", "tilt"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Alpha]m", " ", "=", + RowBox[{"\[Alpha]m", " ", "=", RowBox[{ - RowBox[{"-", "\[Lambda]"}], " ", "+", + RowBox[{"-", "\[Lambda]"}], " ", "+", RowBox[{"180.0", " ", "Degree"}]}]}], ";"}]}], "Input", - CellChangeTimes->{{3.761487377493517*^9, 3.761487449999037*^9}, + CellChangeTimes->{{3.761487377493517*^9, 3.761487449999037*^9}, 3.7614875956160583`*^9, {3.761498635815434*^9, 3.761498650595413*^9}}, CellLabel-> "In[198]:=",ExpressionUUID->"71d0fc76-bd12-4288-8510-e85e63d884f5"], @@ -736,31 +736,31 @@ Cell[BoxData[{ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"mVec", " ", "=", " ", - RowBox[{"m", - RowBox[{"{", + RowBox[{"mVec", " ", "=", " ", + RowBox[{"m", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Sin", "[", "\[Theta]m", "]"}], - RowBox[{"Cos", "[", "\[Alpha]m", "]"}]}], ",", + RowBox[{"Sin", "[", "\[Theta]m", "]"}], + RowBox[{"Cos", "[", "\[Alpha]m", "]"}]}], ",", RowBox[{ - RowBox[{"Sin", "[", "\[Theta]m", "]"}], - RowBox[{"Sin", "[", "\[Alpha]m", "]"}]}], ",", + RowBox[{"Sin", "[", "\[Theta]m", "]"}], + RowBox[{"Sin", "[", "\[Alpha]m", "]"}]}], ",", RowBox[{"Cos", "[", "\[Theta]m", "]"}]}], "}"}]}]}]], "Input", CellChangeTimes->{{3.761487603612254*^9, 3.761487625448086*^9}}, CellLabel-> "In[202]:=",ExpressionUUID->"cef24ec4-0ab8-4375-bb20-c320a4f44c0f"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "1.9546423128749864`*^-6"}], ",", "5.612968146364803`*^-6", - ",", + RowBox[{"-", "1.9546423128749864`*^-6"}], ",", "5.612968146364803`*^-6", + ",", RowBox[{"-", "0.00003001722775368526`"}]}], "}"}]], "Output", CellChangeTimes->{ - 3.7614876262390213`*^9, 3.761498442632904*^9, {3.76149852564215*^9, - 3.7614985539129763`*^9}, {3.761498626084391*^9, 3.7614986535584517`*^9}, - 3.761499067260521*^9, 3.761500640263329*^9, 3.761500851132387*^9, + 3.7614876262390213`*^9, 3.761498442632904*^9, {3.76149852564215*^9, + 3.7614985539129763`*^9}, {3.761498626084391*^9, 3.7614986535584517`*^9}, + 3.761499067260521*^9, 3.761500640263329*^9, 3.761500851132387*^9, 3.761500888330052*^9}, CellLabel-> "Out[202]=",ExpressionUUID->"19d7a7e6-1a0b-4fa6-aacd-fe7ccfe93d75"] @@ -769,21 +769,21 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"mVec", " ", + RowBox[{"mVec", " ", RowBox[{"10", "^", "9"}]}]], "Input", - CellChangeTimes->{{3.761487652444598*^9, 3.7614876872354317`*^9}, - 3.76149906412269*^9, {3.761500631639925*^9, 3.761500636042306*^9}, + CellChangeTimes->{{3.761487652444598*^9, 3.7614876872354317`*^9}, + 3.76149906412269*^9, {3.761500631639925*^9, 3.761500636042306*^9}, 3.761500885727289*^9}, CellLabel-> "In[203]:=",ExpressionUUID->"9642c044-160e-4c14-aba5-ce23a758f5b8"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "1954.6423128749864`"}], ",", "5612.968146364803`", ",", + RowBox[{"-", "1954.6423128749864`"}], ",", "5612.968146364803`", ",", RowBox[{"-", "30017.22775368526`"}]}], "}"}]], "Output", - CellChangeTimes->{{3.761487680970297*^9, 3.7614876881990232`*^9}, - 3.761498442737115*^9, 3.761499067478917*^9, {3.761500637251795*^9, + CellChangeTimes->{{3.761487680970297*^9, 3.7614876881990232`*^9}, + 3.761498442737115*^9, 3.761499067478917*^9, {3.761500637251795*^9, 3.7615006404602137`*^9}, 3.761500851332005*^9, 3.7615008884428263`*^9}, CellLabel-> "Out[203]=",ExpressionUUID->"62d3ddc5-7335-400c-8fa1-8b26a58215e5"] @@ -794,30 +794,30 @@ Cell[CellGroupData[{ Cell["Nasa Mercury", "Subsection", CellChangeTimes->{{3.761485906779035*^9, 3.761485907535144*^9}, { - 3.761500602368753*^9, + 3.761500602368753*^9, 3.761500609313586*^9}},ExpressionUUID->"e515bff6-a084-4176-bae0-\ 3e7edc4f0260"], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"m", "=", - RowBox[{"0.002", " ", - RowBox[{"10", "^", + RowBox[{"m", "=", + RowBox[{"0.002", " ", + RowBox[{"10", "^", RowBox[{"-", "4", " "}]}]}]}]], "Input", CellChangeTimes->{{3.7614852505830097`*^9, 3.761485267833507*^9}, { - 3.761485329016069*^9, 3.761485333035672*^9}, {3.761485364176938*^9, + 3.761485329016069*^9, 3.761485333035672*^9}, {3.761485364176938*^9, 3.7614853642408323`*^9}, {3.761485455278775*^9, 3.761485460525774*^9}, { - 3.761485494119502*^9, 3.76148549530617*^9}, {3.761487468936984*^9, + 3.761485494119502*^9, 3.76148549530617*^9}, {3.761487468936984*^9, 3.761487492129184*^9}, {3.761500619903819*^9, 3.7615006226571827`*^9}}, CellLabel-> "In[206]:=",ExpressionUUID->"267823b2-b1da-47f0-8ecd-374ccd2dbe79"], Cell[BoxData["2.0000000000000002`*^-7"], "Output", CellChangeTimes->{ - 3.761485268273548*^9, {3.761485335550934*^9, 3.76148536462856*^9}, - 3.7614854610495996`*^9, 3.761485495758309*^9, {3.7614874739745083`*^9, - 3.761487500239915*^9}, 3.7614984424302797`*^9, 3.761498528260454*^9, + 3.761485268273548*^9, {3.761485335550934*^9, 3.76148536462856*^9}, + 3.7614854610495996`*^9, 3.761485495758309*^9, {3.7614874739745083`*^9, + 3.761487500239915*^9}, 3.7614984424302797`*^9, 3.761498528260454*^9, 3.761499067111679*^9, 3.761500833215107*^9, 3.761500901164176*^9}, CellLabel-> "Out[206]=",ExpressionUUID->"ca6113cf-7520-4173-8773-cdf3c1cbb702"] @@ -825,20 +825,20 @@ Cell[BoxData["2.0000000000000002`*^-7"], "Output", Cell[BoxData[{ RowBox[{ - RowBox[{"\[Lambda]", " ", "=", " ", - RowBox[{"0.0", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[Lambda]", " ", "=", " ", + RowBox[{"0.0", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"tilt", " ", "=", " ", - RowBox[{"0.0", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"tilt", " ", "=", " ", + RowBox[{"0.0", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Theta]m", " ", "=", " ", - RowBox[{"\[Pi]", "-", "tilt"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[Theta]m", " ", "=", " ", + RowBox[{"\[Pi]", "-", "tilt"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Alpha]m", " ", "=", + RowBox[{"\[Alpha]m", " ", "=", RowBox[{ - RowBox[{"-", "\[Lambda]"}], " ", "+", + RowBox[{"-", "\[Lambda]"}], " ", "+", RowBox[{"180.0", " ", "Degree"}]}]}], ";"}]}], "Input", - CellChangeTimes->{{3.761487377493517*^9, 3.761487449999037*^9}, + CellChangeTimes->{{3.761487377493517*^9, 3.761487449999037*^9}, 3.7614875956160583`*^9, {3.761498635815434*^9, 3.761498650595413*^9}, { 3.761500652540107*^9, 3.7615006593145323`*^9}}, CellLabel-> @@ -847,31 +847,31 @@ Cell[BoxData[{ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"mVec", " ", "=", " ", - RowBox[{"m", - RowBox[{"{", + RowBox[{"mVec", " ", "=", " ", + RowBox[{"m", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Sin", "[", "\[Theta]m", "]"}], - RowBox[{"Cos", "[", "\[Alpha]m", "]"}]}], ",", + RowBox[{"Sin", "[", "\[Theta]m", "]"}], + RowBox[{"Cos", "[", "\[Alpha]m", "]"}]}], ",", RowBox[{ - RowBox[{"Sin", "[", "\[Theta]m", "]"}], - RowBox[{"Sin", "[", "\[Alpha]m", "]"}]}], ",", + RowBox[{"Sin", "[", "\[Theta]m", "]"}], + RowBox[{"Sin", "[", "\[Alpha]m", "]"}]}], ",", RowBox[{"Cos", "[", "\[Theta]m", "]"}]}], "}"}]}]}]], "Input", CellChangeTimes->{{3.761487603612254*^9, 3.761487625448086*^9}}, CellLabel-> "In[211]:=",ExpressionUUID->"c37b706b-089d-4d3b-b317-544c0f6e24ba"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{"-", "2.4492935982947066`*^-23"}], ",", "2.9995195653237153`*^-39", - ",", + ",", RowBox[{"-", "2.0000000000000002`*^-7"}]}], "}"}]], "Output", CellChangeTimes->{ - 3.7614876262390213`*^9, 3.761498442632904*^9, {3.76149852564215*^9, - 3.7614985539129763`*^9}, {3.761498626084391*^9, 3.7614986535584517`*^9}, - 3.761499067260521*^9, 3.7615006623472633`*^9, 3.761500839487708*^9, + 3.7614876262390213`*^9, 3.761498442632904*^9, {3.76149852564215*^9, + 3.7614985539129763`*^9}, {3.761498626084391*^9, 3.7614986535584517`*^9}, + 3.761499067260521*^9, 3.7615006623472633`*^9, 3.761500839487708*^9, 3.761500901322061*^9}, CellLabel-> "Out[211]=",ExpressionUUID->"5adb1a6e-5d06-4dd8-a28e-5d4672682f2b"] @@ -880,22 +880,22 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"mVec", " ", + RowBox[{"mVec", " ", RowBox[{"10", "^", "9"}]}]], "Input", - CellChangeTimes->{{3.761487652444598*^9, 3.7614876872354317`*^9}, - 3.76149906412269*^9, 3.7615006451847486`*^9, {3.761500891657308*^9, + CellChangeTimes->{{3.761487652444598*^9, 3.7614876872354317`*^9}, + 3.76149906412269*^9, 3.7615006451847486`*^9, {3.761500891657308*^9, 3.7615008977846813`*^9}}, CellLabel-> "In[212]:=",ExpressionUUID->"c00b10d7-eae6-4882-9343-8bdde95d8cae"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{"-", "2.4492935982947064`*^-14"}], ",", "2.9995195653237154`*^-30", - ",", + ",", RowBox[{"-", "200.00000000000003`"}]}], "}"}]], "Output", - CellChangeTimes->{{3.761487680970297*^9, 3.7614876881990232`*^9}, - 3.761498442737115*^9, 3.761499067478917*^9, 3.7615006947782173`*^9, + CellChangeTimes->{{3.761487680970297*^9, 3.7614876881990232`*^9}, + 3.761498442737115*^9, 3.761499067478917*^9, 3.7615006947782173`*^9, 3.761500842041223*^9, {3.76150089261574*^9, 3.761500901436981*^9}}, CellLabel-> "Out[212]=",ExpressionUUID->"d7e7f4e0-2a9a-42c4-8d0e-d01e4344f41e"] @@ -906,32 +906,32 @@ Cell[CellGroupData[{ Cell["Nasa Jupiter", "Subsection", CellChangeTimes->{{3.761485906779035*^9, 3.761485907535144*^9}, { - 3.761500602368753*^9, 3.761500609313586*^9}, {3.7615009126813726`*^9, + 3.761500602368753*^9, 3.761500609313586*^9}, {3.7615009126813726`*^9, 3.761500913902342*^9}},ExpressionUUID->"2a68b5b0-99d5-4a5c-bcdb-\ 83b2d6e7580c"], Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"m", "=", - RowBox[{"4.30", " ", - RowBox[{"10", "^", + RowBox[{"m", "=", + RowBox[{"4.30", " ", + RowBox[{"10", "^", RowBox[{"-", "4", " "}]}]}]}]], "Input", CellChangeTimes->{{3.7614852505830097`*^9, 3.761485267833507*^9}, { - 3.761485329016069*^9, 3.761485333035672*^9}, {3.761485364176938*^9, + 3.761485329016069*^9, 3.761485333035672*^9}, {3.761485364176938*^9, 3.7614853642408323`*^9}, {3.761485455278775*^9, 3.761485460525774*^9}, { - 3.761485494119502*^9, 3.76148549530617*^9}, {3.761487468936984*^9, - 3.761487492129184*^9}, {3.761500619903819*^9, 3.7615006226571827`*^9}, + 3.761485494119502*^9, 3.76148549530617*^9}, {3.761487468936984*^9, + 3.761487492129184*^9}, {3.761500619903819*^9, 3.7615006226571827`*^9}, 3.7615009235851717`*^9}, CellLabel-> "In[213]:=",ExpressionUUID->"eb23de5d-a0a0-46c9-93a6-bf3543f1b8bd"], Cell[BoxData["0.00043`"], "Output", CellChangeTimes->{ - 3.761485268273548*^9, {3.761485335550934*^9, 3.76148536462856*^9}, - 3.7614854610495996`*^9, 3.761485495758309*^9, {3.7614874739745083`*^9, - 3.761487500239915*^9}, 3.7614984424302797`*^9, 3.761498528260454*^9, - 3.761499067111679*^9, 3.761500833215107*^9, 3.761500901164176*^9, + 3.761485268273548*^9, {3.761485335550934*^9, 3.76148536462856*^9}, + 3.7614854610495996`*^9, 3.761485495758309*^9, {3.7614874739745083`*^9, + 3.761487500239915*^9}, 3.7614984424302797`*^9, 3.761498528260454*^9, + 3.761499067111679*^9, 3.761500833215107*^9, 3.761500901164176*^9, 3.761500937802195*^9}, CellLabel-> "Out[213]=",ExpressionUUID->"660a4fa2-607b-44bf-969a-73fc7988d9d7"] @@ -939,22 +939,22 @@ Cell[BoxData["0.00043`"], "Output", Cell[BoxData[{ RowBox[{ - RowBox[{"\[Lambda]", " ", "=", " ", - RowBox[{"200.1", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[Lambda]", " ", "=", " ", + RowBox[{"200.1", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"tilt", " ", "=", " ", - RowBox[{"9.4", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"tilt", " ", "=", " ", + RowBox[{"9.4", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Theta]m", " ", "=", " ", - RowBox[{"\[Pi]", "-", "tilt"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"\[Theta]m", " ", "=", " ", + RowBox[{"\[Pi]", "-", "tilt"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"\[Alpha]m", " ", "=", + RowBox[{"\[Alpha]m", " ", "=", RowBox[{ - RowBox[{"-", "\[Lambda]"}], " ", "+", + RowBox[{"-", "\[Lambda]"}], " ", "+", RowBox[{"180.0", " ", "Degree"}]}]}], ";"}]}], "Input", - CellChangeTimes->{{3.761487377493517*^9, 3.761487449999037*^9}, + CellChangeTimes->{{3.761487377493517*^9, 3.761487449999037*^9}, 3.7614875956160583`*^9, {3.761498635815434*^9, 3.761498650595413*^9}, { - 3.761500652540107*^9, 3.7615006593145323`*^9}, {3.76150092711406*^9, + 3.761500652540107*^9, 3.7615006593145323`*^9}, {3.76150092711406*^9, 3.761500933807798*^9}}, CellLabel-> "In[214]:=",ExpressionUUID->"b2d095d6-02cf-4ae4-b7a2-8c7caa5f3740"], @@ -962,30 +962,30 @@ Cell[BoxData[{ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"mVec", " ", "=", " ", - RowBox[{"m", - RowBox[{"{", + RowBox[{"mVec", " ", "=", " ", + RowBox[{"m", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Sin", "[", "\[Theta]m", "]"}], - RowBox[{"Cos", "[", "\[Alpha]m", "]"}]}], ",", + RowBox[{"Sin", "[", "\[Theta]m", "]"}], + RowBox[{"Cos", "[", "\[Alpha]m", "]"}]}], ",", RowBox[{ - RowBox[{"Sin", "[", "\[Theta]m", "]"}], - RowBox[{"Sin", "[", "\[Alpha]m", "]"}]}], ",", + RowBox[{"Sin", "[", "\[Theta]m", "]"}], + RowBox[{"Sin", "[", "\[Alpha]m", "]"}]}], ",", RowBox[{"Cos", "[", "\[Theta]m", "]"}]}], "}"}]}]}]], "Input", CellChangeTimes->{{3.761487603612254*^9, 3.761487625448086*^9}}, CellLabel-> "In[218]:=",ExpressionUUID->"928f96e2-6e9f-4131-933c-0802f524a106"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"0.00006595274311434638`", ",", - RowBox[{"-", "0.000024135276629798853`"}], ",", + RowBox[{"{", + RowBox[{"0.00006595274311434638`", ",", + RowBox[{"-", "0.000024135276629798853`"}], ",", RowBox[{"-", "0.0004242260294909968`"}]}], "}"}]], "Output", CellChangeTimes->{ - 3.7614876262390213`*^9, 3.761498442632904*^9, {3.76149852564215*^9, - 3.7614985539129763`*^9}, {3.761498626084391*^9, 3.7614986535584517`*^9}, - 3.761499067260521*^9, 3.7615006623472633`*^9, 3.761500839487708*^9, + 3.7614876262390213`*^9, 3.761498442632904*^9, {3.76149852564215*^9, + 3.7614985539129763`*^9}, {3.761498626084391*^9, 3.7614986535584517`*^9}, + 3.761499067260521*^9, 3.7615006623472633`*^9, 3.761500839487708*^9, 3.761500901322061*^9, 3.761500937951996*^9}, CellLabel-> "Out[218]=",ExpressionUUID->"c511927c-7187-4fe4-ba5e-0e5b646693d0"] @@ -994,22 +994,22 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"mVec", " ", + RowBox[{"mVec", " ", RowBox[{"10", "^", "9"}]}]], "Input", - CellChangeTimes->{{3.761487652444598*^9, 3.7614876872354317`*^9}, - 3.76149906412269*^9, 3.7615006451847486`*^9, {3.761500891657308*^9, + CellChangeTimes->{{3.761487652444598*^9, 3.7614876872354317`*^9}, + 3.76149906412269*^9, 3.7615006451847486`*^9, {3.761500891657308*^9, 3.7615008977846813`*^9}}, CellLabel-> "In[219]:=",ExpressionUUID->"223e75bf-9891-40bb-b5ea-9f3fd058c2b9"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"65952.74311434638`", ",", - RowBox[{"-", "24135.276629798853`"}], ",", + RowBox[{"{", + RowBox[{"65952.74311434638`", ",", + RowBox[{"-", "24135.276629798853`"}], ",", RowBox[{"-", "424226.0294909968`"}]}], "}"}]], "Output", - CellChangeTimes->{{3.761487680970297*^9, 3.7614876881990232`*^9}, - 3.761498442737115*^9, 3.761499067478917*^9, 3.7615006947782173`*^9, - 3.761500842041223*^9, {3.76150089261574*^9, 3.761500901436981*^9}, + CellChangeTimes->{{3.761487680970297*^9, 3.7614876881990232`*^9}, + 3.761498442737115*^9, 3.761499067478917*^9, 3.7615006947782173`*^9, + 3.761500842041223*^9, {3.76150089261574*^9, 3.761500901436981*^9}, 3.761500938072484*^9}, CellLabel-> "Out[219]=",ExpressionUUID->"0f9c3c3b-2ea5-4f5e-96ef-32772b20cd57"] @@ -1019,7 +1019,7 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell["Markley", "Subsection", - CellChangeTimes->{{3.761485912316471*^9, + CellChangeTimes->{{3.761485912316471*^9, 3.761485913542615*^9}},ExpressionUUID->"3cbb0b12-df47-4f51-9035-\ 291193113a0a"], @@ -1027,21 +1027,21 @@ Cell[CellGroupData[{ Cell[BoxData[{ RowBox[{ - RowBox[{"X", " ", "=", " ", + RowBox[{"X", " ", "=", " ", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "1669.05"}], ",", "5077.99", ",", - RowBox[{"-", "29554.63"}]}], "}"}], - RowBox[{"10", "^", - RowBox[{"-", "9"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"-", "1669.05"}], ",", "5077.99", ",", + RowBox[{"-", "29554.63"}]}], "}"}], + RowBox[{"10", "^", + RowBox[{"-", "9"}]}]}]}], ";"}], "\[IndentingNewLine]", RowBox[{"Norm", "[", "X", "]"}]}], "Input", - CellChangeTimes->{{3.761486629464964*^9, 3.7614866347171707`*^9}, + CellChangeTimes->{{3.761486629464964*^9, 3.7614866347171707`*^9}, 3.761487004380302*^9, {3.761487581432239*^9, 3.7614875831748247`*^9}}, CellLabel->"In[88]:=",ExpressionUUID->"a302490f-ac53-4907-888b-023d9d859b5d"], Cell[BoxData["0.00003003411168620607`"], "Output", - CellChangeTimes->{3.761486635125711*^9, 3.761487005055622*^9, + CellChangeTimes->{3.761486635125711*^9, 3.761487005055622*^9, 3.761487519681553*^9, 3.761487584434936*^9}, CellLabel->"Out[89]=",ExpressionUUID->"b2127fc8-1ce4-4b3a-a95c-d01b40b482dc"] }, Open ]], @@ -1049,27 +1049,27 @@ Cell[BoxData["0.00003003411168620607`"], "Output", Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"mVec", "=", - RowBox[{"X", " ", + RowBox[{"mVec", "=", + RowBox[{"X", " ", RowBox[{"6371200", "^", "3"}]}]}]], "Input", CellChangeTimes->{{3.7614853807884693`*^9, 3.7614854190063066`*^9}, { - 3.761485500309349*^9, 3.761485500880908*^9}, {3.7614855903086157`*^9, + 3.761485500309349*^9, 3.761485500880908*^9}, {3.7614855903086157`*^9, 3.761485590715323*^9}, {3.761485691125106*^9, 3.76148570486821*^9}, { - 3.7614857423969793`*^9, 3.761485742882346*^9}, {3.7614857825716133`*^9, - 3.761485786035236*^9}, 3.761486505001438*^9, {3.761486564161539*^9, - 3.761486569643054*^9}, 3.7614866506057997`*^9, {3.761487513032995*^9, + 3.7614857423969793`*^9, 3.761485742882346*^9}, {3.7614857825716133`*^9, + 3.761485786035236*^9}, 3.761486505001438*^9, {3.761486564161539*^9, + 3.761486569643054*^9}, 3.7614866506057997`*^9, {3.761487513032995*^9, 3.761487514533421*^9}}, CellLabel->"In[90]:=",ExpressionUUID->"51718717-a0b0-44a9-8ec4-09c8aacd6a46"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "4.316513088819217`*^14"}], ",", "1.3132746352651565`*^15", - ",", + RowBox[{"-", "4.316513088819217`*^14"}], ",", "1.3132746352651565`*^15", + ",", RowBox[{"-", "7.64344670502436`*^15"}]}], "}"}]], "Output", - CellChangeTimes->{{3.761485399434606*^9, 3.7614854193532763`*^9}, - 3.761485501721862*^9, 3.7614855915107403`*^9, {3.761485693834992*^9, - 3.7614857054306707`*^9}, 3.761485786405075*^9, 3.7614858387120857`*^9, + CellChangeTimes->{{3.761485399434606*^9, 3.7614854193532763`*^9}, + 3.761485501721862*^9, 3.7614855915107403`*^9, {3.761485693834992*^9, + 3.7614857054306707`*^9}, 3.761485786405075*^9, 3.7614858387120857`*^9, 3.761486570265689*^9, 3.761486651480625*^9, 3.7614870082770844`*^9, { 3.761487516052458*^9, 3.761487522555139*^9}, 3.7614875856500998`*^9}, CellLabel->"Out[90]=",ExpressionUUID->"a57edddf-b98a-416a-ab74-7ee52f619d20"] @@ -1078,17 +1078,17 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"mHat", " ", "=", " ", + RowBox[{"mHat", " ", "=", " ", RowBox[{"Normalize", "[", "mVec", "]"}]}]], "Input", CellChangeTimes->{{3.76148682992353*^9, 3.7614868367735357`*^9}}, CellLabel->"In[91]:=",ExpressionUUID->"18b5f3d3-09bd-4088-8777-b05736bc0034"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"-", "0.055571811726549375`"}], ",", "0.1690740865937512`", ",", + RowBox[{"-", "0.055571811726549375`"}], ",", "0.1690740865937512`", ",", RowBox[{"-", "0.9840354297401682`"}]}], "}"}]], "Output", - CellChangeTimes->{3.7614868371898317`*^9, 3.761487010309086*^9, + CellChangeTimes->{3.7614868371898317`*^9, 3.761487010309086*^9, 3.761487523619685*^9, 3.761487587303526*^9}, CellLabel->"Out[91]=",ExpressionUUID->"ed1ec7a7-721a-482a-85d4-e94563884a82"] }, Open ]], @@ -1096,13 +1096,13 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"m", " ", "=", " ", + RowBox[{"m", " ", "=", " ", RowBox[{"Norm", "[", "mVec", "]"}]}]], "Input", CellChangeTimes->{{3.7614866703357067`*^9, 3.7614866732659187`*^9}}, CellLabel->"In[92]:=",ExpressionUUID->"201e6f3a-7a1c-4155-a705-a3e60bb8645a"], Cell[BoxData["7.767450717747622`*^15"], "Output", - CellChangeTimes->{3.761486673856699*^9, 3.7614870117305717`*^9, + CellChangeTimes->{3.761486673856699*^9, 3.7614870117305717`*^9, 3.761487524685995*^9, 3.761487588470326*^9}, CellLabel->"Out[92]=",ExpressionUUID->"85d481e4-8872-4bd3-82e3-b1e53b45399f"] }, Open ]], @@ -1110,16 +1110,16 @@ Cell[BoxData["7.767450717747622`*^15"], "Output", Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"\[Theta]m", " ", "=", " ", - RowBox[{"ArcCos", "[", - RowBox[{"mHat", "[", + RowBox[{"\[Theta]m", " ", "=", " ", + RowBox[{"ArcCos", "[", + RowBox[{"mHat", "[", RowBox[{"[", "3", "]"}], "]"}], "]"}]}]], "Input", CellChangeTimes->{{3.7614867767896147`*^9, 3.761486796372303*^9}, { 3.76148684098139*^9, 3.761486842028701*^9}}, CellLabel->"In[82]:=",ExpressionUUID->"5336f261-5b45-4d67-9f1e-fad753f81b1e"], Cell[BoxData["2.9626668033157566`"], "Output", - CellChangeTimes->{3.761486797543551*^9, 3.761486842547388*^9, + CellChangeTimes->{3.761486797543551*^9, 3.761486842547388*^9, 3.7614870127114887`*^9, 3.761487526509305*^9}, CellLabel->"Out[82]=",ExpressionUUID->"92a524e0-98c9-4ce8-9f49-c2643c98f232"] }, Open ]], @@ -1132,7 +1132,7 @@ Cell[BoxData[ CellLabel->"In[83]:=",ExpressionUUID->"49452801-fa5a-4861-bb52-d34eb82a2c4e"], Cell[BoxData["169.74830393350803`"], "Output", - CellChangeTimes->{{3.761486801414482*^9, 3.761486804765642*^9}, + CellChangeTimes->{{3.761486801414482*^9, 3.761486804765642*^9}, 3.761487013611184*^9, 3.7614875276415787`*^9}, CellLabel->"Out[83]=",ExpressionUUID->"929db8d2-6c0f-4cfe-9681-ff70ca01e131"] }, Open ]], @@ -1140,18 +1140,18 @@ Cell[BoxData["169.74830393350803`"], "Output", Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"\[Alpha]m", "=", - RowBox[{"ArcTan", "[", + RowBox[{"\[Alpha]m", "=", + RowBox[{"ArcTan", "[", RowBox[{ - RowBox[{"mHat", "[", - RowBox[{"[", "1", "]"}], "]"}], ",", - RowBox[{"mHat", "[", + RowBox[{"mHat", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", + RowBox[{"mHat", "[", RowBox[{"[", "2", "]"}], "]"}]}], "]"}]}]], "Input", CellChangeTimes->{{3.761486812305539*^9, 3.761486847491053*^9}}, CellLabel->"In[84]:=",ExpressionUUID->"6a83d756-dcd0-44cd-851f-e24c0e216308"], Cell[BoxData["1.888355938670324`"], "Output", - CellChangeTimes->{3.761486847956828*^9, 3.761487014628314*^9, + CellChangeTimes->{3.761486847956828*^9, 3.761487014628314*^9, 3.761487528392893*^9}, CellLabel->"Out[84]=",ExpressionUUID->"c22beab2-42dc-4f56-b7be-5c01d0382ea2"] }, Open ]], @@ -1164,7 +1164,7 @@ Cell[BoxData[ CellLabel->"In[85]:=",ExpressionUUID->"102de6ce-b2d7-4730-a420-531932c94b11"], Cell[BoxData["108.19482550427449`"], "Output", - CellChangeTimes->{3.761486851997253*^9, 3.761487017214855*^9, + CellChangeTimes->{3.761486851997253*^9, 3.761487017214855*^9, 3.7614875290911283`*^9}, CellLabel->"Out[85]=",ExpressionUUID->"7edd182a-fb21-4563-8337-1861bd3ed965"] }, Open ]], @@ -1174,13 +1174,13 @@ Cell[CellGroupData[{ Cell[BoxData[ RowBox[{"108.195", "-", "180"}]], "Input", CellChangeTimes->{{3.761487058878826*^9, 3.761487061469287*^9}, { - 3.7614871478992434`*^9, 3.761487149920554*^9}, {3.761487326111589*^9, + 3.7614871478992434`*^9, 3.761487149920554*^9}, {3.761487326111589*^9, 3.7614873288187532`*^9}}, CellLabel->"In[86]:=",ExpressionUUID->"830571ac-6c0d-4e11-ac23-22d6a58db659"], Cell[BoxData[ RowBox[{"-", "71.805`"}]], "Output", - CellChangeTimes->{3.761487062037758*^9, 3.76148715044484*^9, + CellChangeTimes->{3.761487062037758*^9, 3.76148715044484*^9, 3.761487330250849*^9, 3.761487529709*^9}, CellLabel->"Out[86]=",ExpressionUUID->"19953b8a-7e76-4118-9d09-060ba0d460a0"] }, Open ]], @@ -1404,4 +1404,3 @@ Cell[42295, 1194, 192, 2, 34, "Output",ExpressionUUID->"a267ce42-401f-45bf-90c9- } ] *) - diff --git a/src/simulation/environment/magneticFieldCenteredDipole/magneticFieldCenteredDipole.rst b/src/simulation/environment/magneticFieldCenteredDipole/magneticFieldCenteredDipole.rst index d27f84297c..d1c8e70c47 100644 --- a/src/simulation/environment/magneticFieldCenteredDipole/magneticFieldCenteredDipole.rst +++ b/src/simulation/environment/magneticFieldCenteredDipole/magneticFieldCenteredDipole.rst @@ -7,4 +7,3 @@ For more information on this module see this :download:`PDF Description >12) number of degrees. It is also used in many + * generally and supports models even with a large (>>12) number of degrees. It is also used in many * other geomagnetic models distributed by NCEI. * * REUSE NOTES @@ -89,7 +89,7 @@ * does everything necessary to compute the geomagnetic elements from a given * geodetic point in space and magnetic model adjusted for the appropriate * date. These functions are the external functions necessary to create a - * program that uses or calculates the magnetic field. + * program that uses or calculates the magnetic field. ****************************************************************************** ******************************************************************************/ @@ -125,7 +125,7 @@ CALLS: MAG_AllocateLegendreFunctionMemory(NumTerms); ( For storing the ALF fu int NumTerms; MAGtype_MagneticResults MagneticResultsSph, MagneticResultsGeo, MagneticResultsSphVar, MagneticResultsGeoVar; - NumTerms = ((TimedMagneticModel->nMax + 1) * (TimedMagneticModel->nMax + 2) / 2); + NumTerms = ((TimedMagneticModel->nMax + 1) * (TimedMagneticModel->nMax + 2) / 2); LegendreFunction = MAG_AllocateLegendreFunctionMemory(NumTerms); /* For storing the ALF functions */ SphVariables = MAG_AllocateSphVarMemory(TimedMagneticModel->nMax); MAG_ComputeSphericalHarmonicVariables(Ellip, CoordSpherical, TimedMagneticModel->nMax, SphVariables); /* Compute Spherical Harmonic variables */ @@ -149,13 +149,13 @@ void MAG_Gradient(MAGtype_Ellipsoid Ellip, MAGtype_CoordGeodetic CoordGeodetic, coordinate system as the directions in which the gradients are taken. These variables represent a Cartesian coordinate system where the Earth's center is the origin, 'z' points up toward the North (rotational) pole and 'x' points toward - the prime meridian. 'y' points toward longitude = 90 degrees East. + the prime meridian. 'y' points toward longitude = 90 degrees East. The gradient is preformed along a local Cartesian coordinate system with the - origin at CoordGeodetic. 'z' points down toward the Earth's core, x points + origin at CoordGeodetic. 'z' points down toward the Earth's core, x points North, tangent to the local longitude line, and 'y' points East, tangent to the local latitude line.*/ double phiDelta = 0.01, /*DeltaY = 0.01,*/ hDelta = -1, x[2], y[2], z[2], distance; - + MAGtype_CoordSpherical AdjCoordSpherical; MAGtype_CoordGeodetic AdjCoordGeodetic; MAGtype_GeoMagneticElements GeomagneticElements, AdjGeoMagneticElements[2]; @@ -187,16 +187,16 @@ void MAG_Gradient(MAGtype_Ellipsoid Ellip, MAGtype_CoordGeodetic CoordGeodetic, AdjCoordGeodetic = MAG_CoordGeodeticAssign(CoordGeodetic); /*Gradient along y*/ - + /*It is perhaps noticeable that the method here for calculation is substantially different than that for the gradient along x. As we near the North pole the longitude lines approach each other, and the calculation that works well for latitude lines becomes unstable when 0.01 degrees represents sufficiently small numbers, and fails to function correctly at all at the North Pole*/ - + MAG_GeodeticToSpherical(Ellip, CoordGeodetic, &AdjCoordSpherical); MAG_GradY(Ellip, AdjCoordSpherical, CoordGeodetic, TimedMagneticModel, GeomagneticElements, &(Gradient->GradLambda)); - + /*Gradient along z*/ AdjCoordGeodetic.HeightAboveEllipsoid = CoordGeodetic.HeightAboveEllipsoid + hDelta; AdjCoordGeodetic.HeightAboveGeoid = CoordGeodetic.HeightAboveGeoid + hDelta; @@ -259,7 +259,7 @@ int MAG_robustReadMagneticModel_Large(char *filename, char *filenameSV, MAGtype_ if (NULL == fgets(line, MAXLINELENGTH, MODELFILE)) { return 0; - } + } do { if(NULL == fgets(line, MAXLINELENGTH, MODELFILE)) @@ -345,8 +345,8 @@ int MAG_robustReadMagModels(char *filename, MAGtype_MagneticModel *(*magneticmod /****************************************************************************** ********************************User Interface******************************** - * This grouping consists of functions which interact with the directly with - * the user and are generally specific to the XXX_point.c, XXX_grid.c, and + * This grouping consists of functions which interact with the directly with + * the user and are generally specific to the XXX_point.c, XXX_grid.c, and * XXX_file.c programs. They deal with input from and output to the user. ******************************************************************************/ @@ -677,7 +677,7 @@ CALLS: MAG_DMSstringToDegree(buffer, &CoordGeodetic->lambda); (The program uses int i, j, a, b, c, done = 0; double lat_bound[2] = {LAT_BOUND_MIN, LAT_BOUND_MAX}; double lon_bound[2] = {LON_BOUND_MIN, LON_BOUND_MAX}; - int alt_bound[2] = {ALT_BOUND_MIN, NO_ALT_MAX}; + int alt_bound[2] = {ALT_BOUND_MIN, NO_ALT_MAX}; char* Qstring = malloc(sizeof(char) * 1028); strcpy(buffer, ""); /*Clear the input */ strcpy(Qstring, "\nPlease enter latitude\nNorth latitude positive, For example:\n30, 30, 30 (D,M,S) or 30.508 (Decimal Degrees) (both are north)\n"); @@ -685,7 +685,7 @@ CALLS: MAG_DMSstringToDegree(buffer, &CoordGeodetic->lambda); (The program uses strcpy(buffer, ""); /*Clear the input*/ strcpy(Qstring,"\nPlease enter longitude\nEast longitude positive, West negative. For example:\n-100.5 or -100, 30, 0 for 100.5 degrees west\n"); MAG_GetDeg(Qstring, &CoordGeodetic->lambda, lon_bound); - + strcpy(Qstring,"\nPlease enter height above mean sea level (in kilometers):\n[For height above WGS-84 ellipsoid prefix E, for example (E20.1)]\n"); if(MAG_GetAltitude(Qstring, Geoid, CoordGeodetic, alt_bound, FALSE)==USER_GAVE_UP) return FALSE; @@ -751,7 +751,7 @@ CALLS: MAG_DMSstringToDegree(buffer, &CoordGeodetic->lambda); (The program uses strcpy(buffer, ""); printf("\nError encountered, please re-enter as MM/DD/YYYY, MM DD YYYY, or as YYYY.yyy:\n"); while (NULL ==fgets(buffer, 40, stdin)) { - printf("\nError encountered, please re-enter as MM/DD/YYYY, MM DD YYYY, or as YYYY.yyy:\n"); + printf("\nError encountered, please re-enter as MM/DD/YYYY, MM DD YYYY, or as YYYY.yyy:\n"); } i = -1; } @@ -998,7 +998,7 @@ CALLS : none { char ans[20]; strcpy(ans, ""); - + switch(control) { case 1:/* Horizontal Field strength low */ do { @@ -1007,7 +1007,7 @@ CALLS : none printf(" (https://www.ngdc.noaa.gov/geomag/WMM/data/MIL-PRF-89500B.pdf). Compass\n"); printf(" accuracy may be degraded in this region.\n"); printf("Press enter to continue...\n"); - } while(NULL == fgets(ans, 20, stdin)); + } while(NULL == fgets(ans, 20, stdin)); break; case 2:/* Horizontal Field strength very low */ do { @@ -1109,8 +1109,8 @@ CALLS : none /****************************************************************************** ********************************Memory and File Processing******************** - * This grouping consists of functions that read coefficient files into the - * memory, allocate memory, free memory or print models into coefficient files. + * This grouping consists of functions that read coefficient files into the + * memory, allocate memory, free memory or print models into coefficient files. ******************************************************************************/ @@ -1226,14 +1226,14 @@ CALLS : none MagneticModel->epoch = 0; MagneticModel->nMax = 0; MagneticModel->nMaxSecVar = 0; - + for(i=0; iMain_Field_Coeff_G[i] = 0; MagneticModel->Main_Field_Coeff_H[i] = 0; MagneticModel->Secular_Var_Coeff_G[i] = 0; MagneticModel->Secular_Var_Coeff_H[i] = 0; } - + return MagneticModel; } /*MAG_AllocateModelMemory*/ @@ -1252,7 +1252,7 @@ void MAG_AssignHeaderValues(MAGtype_MagneticModel *model, char values[][MAXLINEL { /* MAGtype_Date releasedate; */ strcpy(model->ModelName, values[MODELNAME]); - /* releasedate.Year = 0; + /* releasedate.Year = 0; releasedate.Day = 0; releasedate.Month = 0; releasedate.DecimalYear = 0; @@ -1604,7 +1604,7 @@ void MAG_PrintSHDFFormat(char *filename, MAGtype_MagneticModel *(*MagneticModel) fprintf(SHDF_file, "%%ExtStaticDeg: 0\n"); fprintf(SHDF_file, "%%ExtSecVarDeg: 0\n"); fprintf(SHDF_file, "%%Normalization: Schmidt semi-normailized\n"); - fprintf(SHDF_file, "%%SpatBasFunc: spherical harmonics\n"); + fprintf(SHDF_file, "%%SpatBasFunc: spherical harmonics\n"); fprintf(SHDF_file, "# To synthesize the field for a given date:\n"); fprintf(SHDF_file, "# Use the sub-model of the epoch corresponding to each date\n"); fprintf(SHDF_file, "#\n#\n#\n#\n# I/E, n, m, Gnm, Hnm, SV-Gnm, SV-Hnm\n#\n"); @@ -1657,7 +1657,7 @@ int MAG_readMagneticModel(char *filename, MAGtype_MagneticModel * MagneticModel) int i, icomp, m, n, EOF_Flag = 0, index; double epoch, gnm, hnm, dgnm, dhnm; MAG_COF_File = fopen(filename, "r"); - + if(MAG_COF_File == NULL) { MAG_Error(20); @@ -1851,7 +1851,7 @@ int MAG_readMagneticModel_SHDF(char *filename, MAGtype_MagneticModel *(*magnetic int n, m; double gnm, hnm, dgnm, dhnm, cutoff; int index; - + FILE *stream; ptrreset = line; stream = fopen(filename, READONLYMODE); @@ -1978,8 +1978,8 @@ char *MAG_Trim(char *str) /****************************************************************************** *************Conversions, Transformations, and other Calculations************** * This grouping consists of functions that perform unit conversions, coordinate - * transformations and other simple or straightforward calculations that are - * usually easily replicable with a typical scientific calculator. + * transformations and other simple or straightforward calculations that are + * usually easily replicable with a typical scientific calculator. ******************************************************************************/ @@ -2065,7 +2065,7 @@ void MAG_CalculateGradientElements(MAGtype_MagneticResults GradResults, MAGtype_ GradElements->X = GradResults.Bx; GradElements->Y = GradResults.By; GradElements->Z = GradResults.Bz; - + GradElements->H = (GradElements->X * MagneticElements.X + GradElements->Y * MagneticElements.Y) / MagneticElements.H; GradElements->F = (GradElements->X * MagneticElements.X + GradElements->Y * MagneticElements.Y + GradElements->Z * MagneticElements.Z) / MagneticElements.F; GradElements->Decl = 180.0 / M_PI * (MagneticElements.X * GradElements->Y - MagneticElements.Y * GradElements->X) / (MagneticElements.H * MagneticElements.H); @@ -2111,7 +2111,7 @@ void MAG_CartesianToGeodetic(MAGtype_Ellipsoid Ellip, double x, double y, double y is defined as the direction from the core toward 90 degrees east longitude along * the equator z is defined as the direction from the core out the geographic north pole*/ - + double modified_b,r,e,f,p,q,d,v,g,t,zlong,rlat; /* @@ -2136,12 +2136,12 @@ void MAG_CartesianToGeodetic(MAGtype_Ellipsoid Ellip, double x, double y, double q= 2.0 * (e*e - f*f); d= p*p*p + q*q; - if( d >= 0.0 ) + if( d >= 0.0 ) { v= pow( (sqrt( d ) - q), (1.0 / 3.0) ) - pow( (sqrt( d ) + q), (1.0 / 3.0) ); - } - else + } + else { v= 2.0 * sqrt( -p ) * cos( acos( q/(p * sqrt( -p )) ) / 3.0 ); @@ -2158,7 +2158,7 @@ void MAG_CartesianToGeodetic(MAGtype_Ellipsoid Ellip, double x, double y, double rlat =atan( (Ellip.a*(1.0 - t*t)) / (2.0*modified_b*t) ); CoordGeodetic->phi = RAD2DEG(rlat); - + /* * 5.0 compute height above ellipsoid */ @@ -2175,7 +2175,7 @@ void MAG_CartesianToGeodetic(MAGtype_Ellipsoid Ellip, double x, double y, double { CoordGeodetic->lambda-=360; } - + } MAGtype_CoordGeodetic MAG_CoordGeodeticAssign(MAGtype_CoordGeodetic CoordGeodetic) @@ -2410,7 +2410,7 @@ MAGtype_GeoMagneticElements MAG_GeoMagneticElementsAssign(MAGtype_GeoMagneticEle MAGtype_GeoMagneticElements MAG_GeoMagneticElementsScale(MAGtype_GeoMagneticElements Elements, double factor) { - /*This function scales all the geomagnetic elements to scale a vector use + /*This function scales all the geomagnetic elements to scale a vector use MAG_MagneticResultsScale*/ MAGtype_GeoMagneticElements product; product.X = Elements.X * factor; @@ -2434,31 +2434,31 @@ MAGtype_GeoMagneticElements MAG_GeoMagneticElementsScale(MAGtype_GeoMagneticElem MAGtype_GeoMagneticElements MAG_GeoMagneticElementsSubtract(MAGtype_GeoMagneticElements minuend, MAGtype_GeoMagneticElements subtrahend) { - /*This algorithm does not result in the difference of F being derived from + /*This algorithm does not result in the difference of F being derived from the Pythagorean theorem. This function should be used for computing residuals or changes in elements.*/ MAGtype_GeoMagneticElements difference; difference.X = minuend.X - subtrahend.X; difference.Y = minuend.Y - subtrahend.Y; difference.Z = minuend.Z - subtrahend.Z; - + difference.H = minuend.H - subtrahend.H; difference.F = minuend.F - subtrahend.F; difference.Decl = minuend.Decl - subtrahend.Decl; difference.Incl = minuend.Incl - subtrahend.Incl; - + difference.Xdot = minuend.Xdot - subtrahend.Xdot; difference.Ydot = minuend.Ydot - subtrahend.Ydot; difference.Zdot = minuend.Zdot - subtrahend.Zdot; - + difference.Hdot = minuend.Hdot - subtrahend.Hdot; difference.Fdot = minuend.Fdot - subtrahend.Fdot; difference.Decldot = minuend.Decldot - subtrahend.Decldot; difference.Incldot = minuend.Incldot - subtrahend.Incldot; - + difference.GV = minuend.GV - subtrahend.GV; difference.GVdot = minuend.GVdot - subtrahend.GVdot; - + return difference; } @@ -2683,10 +2683,10 @@ void MAG_SphericalToCartesian(MAGtype_CoordSpherical CoordSpherical, double *x, { double radphi; double radlambda; - + radphi = CoordSpherical.phig * (M_PI / 180); radlambda = CoordSpherical.lambda * (M_PI / 180); - + *x = CoordSpherical.r * cos(radphi) * cos(radlambda); *y = CoordSpherical.r * cos(radphi) * sin(radlambda); *z = CoordSpherical.r * sin(radphi); @@ -2695,10 +2695,10 @@ void MAG_SphericalToCartesian(MAGtype_CoordSpherical CoordSpherical, double *x, void MAG_SphericalToGeodetic(MAGtype_Ellipsoid Ellip, MAGtype_CoordSpherical CoordSpherical, MAGtype_CoordGeodetic *CoordGeodetic) { - /*This converts spherical coordinates back to geodetic coordinates. It is not used in the WMM but + /*This converts spherical coordinates back to geodetic coordinates. It is not used in the WMM but may be necessary for some applications, such as geomagnetic coordinates*/ double x,y,z; - + MAG_SphericalToCartesian(CoordSpherical, &x,&y,&z); MAG_CartesianToGeodetic(Ellip, x,y,z,CoordGeodetic); } @@ -2969,8 +2969,8 @@ CALLS : none /****************************************************************************** ********************************Spherical Harmonics*************************** - * This grouping consists of functions that together take gauss coefficients - * and return a magnetic vector for an input location in spherical coordinates + * This grouping consists of functions that together take gauss coefficients + * and return a magnetic vector for an input location in spherical coordinates ******************************************************************************/ int MAG_AssociatedLegendreFunction(MAGtype_CoordSpherical CoordSpherical, int nMax, MAGtype_LegendreFunction *LegendreFunction) @@ -3092,7 +3092,7 @@ void MAG_GradY(MAGtype_Ellipsoid Ellip, MAGtype_CoordSpherical CoordSpherical, M int NumTerms; MAGtype_MagneticResults GradYResultsSph, GradYResultsGeo; - NumTerms = ((TimedMagneticModel->nMax + 1) * (TimedMagneticModel->nMax + 2) / 2); + NumTerms = ((TimedMagneticModel->nMax + 1) * (TimedMagneticModel->nMax + 2) / 2); LegendreFunction = MAG_AllocateLegendreFunctionMemory(NumTerms); /* For storing the ALF functions */ SphVariables = MAG_AllocateSphVarMemory(TimedMagneticModel->nMax); MAG_ComputeSphericalHarmonicVariables(Ellip, CoordSpherical, TimedMagneticModel->nMax, SphVariables); /* Compute Spherical Harmonic variables */ @@ -3100,7 +3100,7 @@ void MAG_GradY(MAGtype_Ellipsoid Ellip, MAGtype_CoordSpherical CoordSpherical, M MAG_GradYSummation(LegendreFunction, TimedMagneticModel, *SphVariables, CoordSpherical, &GradYResultsSph); /* Accumulate the spherical harmonic coefficients*/ MAG_RotateMagneticVector(CoordSpherical, CoordGeodetic, GradYResultsSph, &GradYResultsGeo); /* Map the computed Magnetic fields to Geodetic coordinates */ MAG_CalculateGradientElements(GradYResultsGeo, GeoMagneticElements, GradYElements); /* Calculate the Geomagnetic elements, Equation 18 , WMM Technical report */ - + MAG_FreeLegendreMemory(LegendreFunction); MAG_FreeSphVarMemory(SphVariables); } @@ -3761,8 +3761,8 @@ CALLS : none /****************************************************************************** *************************************Geoid************************************ - * This grouping consists of functions that make calculations to adjust - * ellipsoid height to height above the geoid (Height above MSL). + * This grouping consists of functions that make calculations to adjust + * ellipsoid height to height above the geoid (Height above MSL). ****************************************************************************** ******************************************************************************/ @@ -3889,8 +3889,8 @@ int MAG_GetGeoidHeight(double Latitude, return TRUE; } /*MAG_GetGeoidHeight*/ -void MAG_EquivalentLatLon(double lat, double lon, double *repairedLat, double *repairedLon) -/*This function takes a latitude and longitude that are ordinarily out of range +void MAG_EquivalentLatLon(double lat, double lon, double *repairedLat, double *repairedLon) +/*This function takes a latitude and longitude that are ordinarily out of range and gives in range values that are equivalent on the Earth's surface. This is required to get correct values for the geoid function.*/ { @@ -3907,9 +3907,9 @@ void MAG_EquivalentLatLon(double lat, double lon, double *repairedLat, double * *repairedLon = *repairedLon+180; } *repairedLat = 90 - colat; - if (*repairedLon > 360) + if (*repairedLon > 360) *repairedLon-=360; - if (*repairedLon < -180) + if (*repairedLon < -180) *repairedLon+=360; } @@ -4027,7 +4027,7 @@ void MAG_GetDeg(char* Query_String, double* latitude, double bounds[2]) { /*Gets a degree value from the user using the standard input*/ char buffer[64], Error_Message[255]; int done, i, j; - + printf("%s", Query_String); while (NULL == fgets(buffer, 64, stdin)){ printf("%s", Query_String); @@ -4089,12 +4089,12 @@ int MAG_GetAltitude(char* Query_String, MAGtype_Geoid *Geoid, MAGtype_CoordGeode double value; done = 0; if(bounds[1] != NO_ALT_MAX){ - UpBoundOn = TRUE; + UpBoundOn = TRUE; } else { UpBoundOn = FALSE; } printf("%s", Query_String); - + while(!done) { strcpy(buffer, ""); @@ -4153,4 +4153,3 @@ int MAG_GetAltitude(char* Query_String, MAGtype_Geoid *Geoid, MAGtype_CoordGeode } return 0; } - diff --git a/src/simulation/environment/magneticFieldWMM/README-includingWMM.txt b/src/simulation/environment/magneticFieldWMM/README-includingWMM.txt index 29d5ab5ea7..e51c6a9bb1 100644 --- a/src/simulation/environment/magneticFieldWMM/README-includingWMM.txt +++ b/src/simulation/environment/magneticFieldWMM/README-includingWMM.txt @@ -1,8 +1,8 @@ -To include the WMM code, note that the following lines had to be changed to replace +To include the WMM code, note that the following lines had to be changed to replace *(*magneticmodels)[] -With +With *(*magneticmodels)[1] @@ -11,4 +11,4 @@ With - MAG_PrintSHDFFormat() ======== -Append 'f' float suffix to each value in 'float GeoidHeightBuffer' in EGM9615.h \ No newline at end of file +Append 'f' float suffix to each value in 'float GeoidHeightBuffer' in EGM9615.h diff --git a/src/simulation/environment/magneticFieldWMM/_Documentation/AVS.sty b/src/simulation/environment/magneticFieldWMM/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/environment/magneticFieldWMM/_Documentation/AVS.sty +++ b/src/simulation/environment/magneticFieldWMM/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/environment/magneticFieldWMM/_Documentation/BasiliskReportMemo.cls b/src/simulation/environment/magneticFieldWMM/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100755 --- a/src/simulation/environment/magneticFieldWMM/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/environment/magneticFieldWMM/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/environment/magneticFieldWMM/_Documentation/secModuleDescription.tex b/src/simulation/environment/magneticFieldWMM/_Documentation/secModuleDescription.tex index c35f85cab4..224f4ff999 100644 --- a/src/simulation/environment/magneticFieldWMM/_Documentation/secModuleDescription.tex +++ b/src/simulation/environment/magneticFieldWMM/_Documentation/secModuleDescription.tex @@ -4,9 +4,9 @@ \section{Model Description} \subsection{General Module Behavior} The purpose of this WMM magnetic field module is to implement a magnetic field model that rotates with an Earth-fixed frame \frameDefinition{P}. Note that this model is specific to Earth, and is not applicable to other planets. The Earth-fixed frame has $\hat{\bm p}_{3}$ being the typical positive rotation axis and $\hat{\bm p}_{1}$ and $\hat{\bm p}_{2}$ span the Earth's equatorial plane where $\hat{\bm p}_{1}$ points towards the prime meridian. -{\tt MagneticFieldWMM} is a child of the {\tt MagneticFieldBase} base class and provides an Earth specific magnetic field model based on the \href{https://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml}{World Magnetic Model}. +{\tt MagneticFieldWMM} is a child of the {\tt MagneticFieldBase} base class and provides an Earth specific magnetic field model based on the \href{https://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml}{World Magnetic Model}. By invoking the magnetic field module, the default epoch values are set to the BSK default epoch value of Jan 01 2019 00:00:00. -The reach of the model controlled by setting the variables {\tt envMinReach} and {\tt envMaxReach} to positive values. These values are the radial distance from the planet center. The default values are -1 which turns off this checking where the magnetic model as unbounded reach. +The reach of the model controlled by setting the variables {\tt envMinReach} and {\tt envMaxReach} to positive values. These values are the radial distance from the planet center. The default values are -1 which turns off this checking where the magnetic model as unbounded reach. There are a multitude of magnetic field models.\footnote{\url { https://geomag.colorado.edu/geomagnetic-and-electric-field-models.html}} The goal with Basilisk is to provide a simple and consistent interface to a range of models. The list of models is expected to grow over time. @@ -17,7 +17,7 @@ \subsection{Planet Centric Spacecraft Position Vector} \begin{equation} \bm r_{B/P} = \bm r_{B/O} - \bm r_{P/O} \end{equation} -If no planet ephemeris message is specified, then the planet position vector $\bm r_{P/O}$ is set to zero. +If no planet ephemeris message is specified, then the planet position vector $\bm r_{P/O}$ is set to zero. Let $[PN]$ be the direction cosine matrix\cite{schaub} that relates the rotating planet-fixed frame relative to an inertial frame \frameDefinition{N}. The simulation provides the spacecraft position vector in inertial frame components. The planet centric position vector is then written in Earth-fixed frame components using \begin{equation} @@ -28,7 +28,7 @@ \subsection{Planet Centric Spacecraft Position Vector} \subsection{World Magnetic Model --- WMM} -The World Magnetic Model (WMM) is a large spatial-scale representation of the Earth's magnetic field. It consists of a degree and order 12 spherical harmonic expansion of the magnetic potential of the geomagnetic main field generated in the Earth's core. Apart from the 168 spherical-harmonic "Gauss" coefficients, the model also has an equal number of spherical-harmonic Secular-Variation (SV) coefficients predicting the temporal evolution of the field over the upcoming five-year epoch. +The World Magnetic Model (WMM) is a large spatial-scale representation of the Earth's magnetic field. It consists of a degree and order 12 spherical harmonic expansion of the magnetic potential of the geomagnetic main field generated in the Earth's core. Apart from the 168 spherical-harmonic "Gauss" coefficients, the model also has an equal number of spherical-harmonic Secular-Variation (SV) coefficients predicting the temporal evolution of the field over the upcoming five-year epoch. Updated model coefficients are released at 5-year intervals, with WMM2015 (released Dec 15, 2014) supposed to last until December 31, 2019. However, due to extraordinarily large and erratic movements of the north magnetic pole, an out-of-cycle update (WMM2015v2) was released in February 2019 (delayed by a few weeks due to the U.S. federal government shutdown) to accurately model the magnetic field above 55\dg north latitude until the end of 2019. The next regular update (WMM2020) will occur in late 2019. diff --git a/src/simulation/environment/magneticFieldWMM/_Documentation/secModuleFunctions.tex b/src/simulation/environment/magneticFieldWMM/_Documentation/secModuleFunctions.tex index af62636d34..febb07df7f 100644 --- a/src/simulation/environment/magneticFieldWMM/_Documentation/secModuleFunctions.tex +++ b/src/simulation/environment/magneticFieldWMM/_Documentation/secModuleFunctions.tex @@ -4,10 +4,10 @@ \section{Module Functions} This module will: \begin{itemize} - \item \textbf{Compute magnetic field vector}: Each of the provided models is fundamentally intended to compute the planetary magnetic vector for a spacecraft. - \item \textbf {Subscribe to model-relevant information:} Each provided magnetic field model requires different input information to operate, such as spacecraft positions or time. This module automatically attempts to subscribe to the relevant messages for a specified model. + \item \textbf{Compute magnetic field vector}: Each of the provided models is fundamentally intended to compute the planetary magnetic vector for a spacecraft. + \item \textbf {Subscribe to model-relevant information:} Each provided magnetic field model requires different input information to operate, such as spacecraft positions or time. This module automatically attempts to subscribe to the relevant messages for a specified model. \item \textbf{Support for multiple spacecraft and model types} Only one magnetic field module is required for each planet, and can support an arbitrary number of spacecraft. Output messages for individual spacecraft are automatically named based on the environment type. \end{itemize} \section{Module Assumptions and Limitations} -This WMM field is specific to the Earth, and valid for a 5 year duration. See the WMM documentation on more information on this model. \ No newline at end of file +This WMM field is specific to the Earth, and valid for a 5 year duration. See the WMM documentation on more information on this model. diff --git a/src/simulation/environment/magneticFieldWMM/_Documentation/secTest.tex b/src/simulation/environment/magneticFieldWMM/_Documentation/secTest.tex index 9d3ebd69ba..aa72d0778c 100644 --- a/src/simulation/environment/magneticFieldWMM/_Documentation/secTest.tex +++ b/src/simulation/environment/magneticFieldWMM/_Documentation/secTest.tex @@ -1,20 +1,20 @@ % !TEX root = ./Basilisk-magFieldWMM-20190618.tex \section{Test Description and Success Criteria} -The WMM software provides a PDF with 12 sample locations and magnetic field values that should be returned in the NED frame. +The WMM software provides a PDF with 12 sample locations and magnetic field values that should be returned in the NED frame. The unit test runs the magnetic field Basilisk module with two fixed spacecraft state input messages. Their locations are identical and set to the WMM test locations. The simulation option {\tt useDefault} checks if the module epoch time value default settings are used, or if the epoch time in terms of a decimal year is specified directly. The option {\tt useMsg} determines if the epoch time is read in through a message. If this message is available, it is supposed to over-rule the epochYear variable. The option {\tt useMinReach} dictates if the minimum orbit radius check is performed, while the option {\tt useMaxReach} checks if the maximum reach check is performed. The option {\tt usePlanetEphemeris} checks if a planet state input message should be created. All permutations are checked. \section{Test Parameters} -The simulation tolerances are shown in Table~\ref{tab:errortol}. In each simulation the neutral density output message is checked relative to python computed true values. +The simulation tolerances are shown in Table~\ref{tab:errortol}. In each simulation the neutral density output message is checked relative to python computed true values. \begin{table}[htbp] \caption{Error tolerance for each test.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ \hline - {\tt magneticField vector} & \input{AutoTeX/unitTestToleranceValue} (nT, relative) \\ + {\tt magneticField vector} & \input{AutoTeX/unitTestToleranceValue} (nT, relative) \\ \hline\hline \end{tabular} \end{table} @@ -24,8 +24,3 @@ \section{Test Parameters} \section{Test Results} Over 200 test permutations are run by the unit test. All are expected to pass. - - - - - diff --git a/src/simulation/environment/magneticFieldWMM/_Documentation/secUserGuide.tex b/src/simulation/environment/magneticFieldWMM/_Documentation/secUserGuide.tex index e754385052..0723f95a06 100644 --- a/src/simulation/environment/magneticFieldWMM/_Documentation/secUserGuide.tex +++ b/src/simulation/environment/magneticFieldWMM/_Documentation/secUserGuide.tex @@ -24,13 +24,13 @@ \subsection{General Module Setup} epMsg = messaging.EpochMsg().write(epochMsgData) magModule.epochInMsg.subscribeTo(epMsg) \end{verbatim} -If the user wants to set the WMM epoch directly, this is done by defining the {\tt epochDate} variable in terms of a decimal year format required by WMM. +If the user wants to set the WMM epoch directly, this is done by defining the {\tt epochDate} variable in terms of a decimal year format required by WMM. \begin{verbatim} magModule.epochDate = decimalYear \end{verbatim} -Not that the epoch message, if available, over-writes the information of setting {\tt epochDate}. +Not that the epoch message, if available, over-writes the information of setting {\tt epochDate}. -The model can be added to a task like other simModels. +The model can be added to a task like other simModels. \begin{verbatim} unitTestSim.AddModelToTask(unitTaskName, testModule) \end{verbatim} @@ -52,5 +52,3 @@ \subsection{Planet Ephemeris Information} \subsection{Setting the Model Reach} By default the model doesn't perform any checks on the altitude to see if the specified magnetic field model should be used. This is set through the parameters {\tt envMinReach} and {\tt envMaxReach}. Their default values are -1. If these are set to positive values, then if the spacecraft orbit radius is smaller than {\tt envMinReach} or larger than {\tt envMaxReach}, the magnetic field vector is set to zero. - - diff --git a/src/simulation/environment/magneticFieldWMM/magneticFieldWMM.rst b/src/simulation/environment/magneticFieldWMM/magneticFieldWMM.rst index a367b703c9..3ac1e7ae38 100644 --- a/src/simulation/environment/magneticFieldWMM/magneticFieldWMM.rst +++ b/src/simulation/environment/magneticFieldWMM/magneticFieldWMM.rst @@ -7,4 +7,3 @@ For more information on this module see this For more information on this module The module is a sub-class of the :ref:`magneticFieldBase` base class. See that class for the nominal messages used and general instructions. - diff --git a/src/simulation/environment/planetEphemeris/Support/planetOrientation.nb b/src/simulation/environment/planetEphemeris/Support/planetOrientation.nb index 366db411da..48e042f746 100644 --- a/src/simulation/environment/planetEphemeris/Support/planetOrientation.nb +++ b/src/simulation/environment/planetEphemeris/Support/planetOrientation.nb @@ -26,44 +26,44 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"TN", " ", "=", " ", + RowBox[{"TN", " ", "=", " ", RowBox[{ - RowBox[{"Euler2", "[", - RowBox[{"-", "DEC"}], "]"}], ".", + RowBox[{"Euler2", "[", + RowBox[{"-", "DEC"}], "]"}], ".", RowBox[{"Euler3", "[", "RAN", "]"}]}]}]], "Input", - CellChangeTimes->{{3.765454285416875*^9, 3.765454312645727*^9}, + CellChangeTimes->{{3.765454285416875*^9, 3.765454312645727*^9}, 3.76545442492126*^9}, CellLabel->"In[7]:=",ExpressionUUID->"c5c4d2e5-81f4-44bf-afb0-dccb873abdab"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Cos", "[", "DEC", "]"}], " ", - RowBox[{"Cos", "[", "RAN", "]"}]}], ",", + RowBox[{"Cos", "[", "DEC", "]"}], " ", + RowBox[{"Cos", "[", "RAN", "]"}]}], ",", RowBox[{ - RowBox[{"Cos", "[", "DEC", "]"}], " ", - RowBox[{"Sin", "[", "RAN", "]"}]}], ",", - RowBox[{"Sin", "[", "DEC", "]"}]}], "}"}], ",", - RowBox[{"{", + RowBox[{"Cos", "[", "DEC", "]"}], " ", + RowBox[{"Sin", "[", "RAN", "]"}]}], ",", + RowBox[{"Sin", "[", "DEC", "]"}]}], "}"}], ",", + RowBox[{"{", RowBox[{ - RowBox[{"-", - RowBox[{"Sin", "[", "RAN", "]"}]}], ",", - RowBox[{"Cos", "[", "RAN", "]"}], ",", "0"}], "}"}], ",", - RowBox[{"{", + RowBox[{"-", + RowBox[{"Sin", "[", "RAN", "]"}]}], ",", + RowBox[{"Cos", "[", "RAN", "]"}], ",", "0"}], "}"}], ",", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"-", - RowBox[{"Cos", "[", "RAN", "]"}]}], " ", - RowBox[{"Sin", "[", "DEC", "]"}]}], ",", + RowBox[{"-", + RowBox[{"Cos", "[", "RAN", "]"}]}], " ", + RowBox[{"Sin", "[", "DEC", "]"}]}], ",", RowBox[{ - RowBox[{"-", - RowBox[{"Sin", "[", "DEC", "]"}]}], " ", - RowBox[{"Sin", "[", "RAN", "]"}]}], ",", + RowBox[{"-", + RowBox[{"Sin", "[", "DEC", "]"}]}], " ", + RowBox[{"Sin", "[", "RAN", "]"}]}], ",", RowBox[{"Cos", "[", "DEC", "]"}]}], "}"}]}], "}"}]], "Output", - CellChangeTimes->{{3.7654543052874727`*^9, 3.7654543149305687`*^9}, + CellChangeTimes->{{3.7654543052874727`*^9, 3.7654543149305687`*^9}, 3.765454426618465*^9}, CellLabel->"Out[7]=",ExpressionUUID->"4821bd12-4f69-44bf-87fe-7e449ec2f564"] }, Open ]], @@ -80,36 +80,36 @@ Cell[BoxData[ RowBox[{"(", "\[NoBreak]", GridBox[{ { RowBox[{ - RowBox[{"Cos", "[", "DEC", "]"}], " ", - RowBox[{"Cos", "[", "RAN", "]"}]}], + RowBox[{"Cos", "[", "DEC", "]"}], " ", + RowBox[{"Cos", "[", "RAN", "]"}]}], RowBox[{ - RowBox[{"Cos", "[", "DEC", "]"}], " ", - RowBox[{"Sin", "[", "RAN", "]"}]}], + RowBox[{"Cos", "[", "DEC", "]"}], " ", + RowBox[{"Sin", "[", "RAN", "]"}]}], RowBox[{"Sin", "[", "DEC", "]"}]}, { - RowBox[{"-", - RowBox[{"Sin", "[", "RAN", "]"}]}], + RowBox[{"-", + RowBox[{"Sin", "[", "RAN", "]"}]}], RowBox[{"Cos", "[", "RAN", "]"}], "0"}, { RowBox[{ - RowBox[{"-", - RowBox[{"Cos", "[", "RAN", "]"}]}], " ", - RowBox[{"Sin", "[", "DEC", "]"}]}], + RowBox[{"-", + RowBox[{"Cos", "[", "RAN", "]"}]}], " ", + RowBox[{"Sin", "[", "DEC", "]"}]}], RowBox[{ - RowBox[{"-", - RowBox[{"Sin", "[", "DEC", "]"}]}], " ", - RowBox[{"Sin", "[", "RAN", "]"}]}], + RowBox[{"-", + RowBox[{"Sin", "[", "DEC", "]"}]}], " ", + RowBox[{"Sin", "[", "RAN", "]"}]}], RowBox[{"Cos", "[", "DEC", "]"}]} }, GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { - Offset[0.7]}, + Offset[0.7]}, Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { - Offset[0.4]}, + Offset[0.4]}, Offset[0.2]}}], "\[NoBreak]", ")"}], - Function[BoxForm`e$, + Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", CellChangeTimes->{3.765454321303668*^9, 3.765454427563768*^9}, CellLabel-> @@ -121,40 +121,40 @@ Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ - RowBox[{"Transpose", "[", "TN", "]"}], ".", - RowBox[{"{", + RowBox[{"Transpose", "[", "TN", "]"}], ".", + RowBox[{"{", RowBox[{"1", ",", "0", ",", "0"}], "}"}]}]], "Input", CellChangeTimes->{{3.765454344839806*^9, 3.765454366029573*^9}, { 3.765455907168633*^9, 3.765455908839137*^9}, 3.76545618953152*^9}, CellLabel->"In[12]:=",ExpressionUUID->"7edac139-27ce-47ea-948a-a0f7c74198f1"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Cos", "[", "DEC", "]"}], " ", - RowBox[{"Cos", "[", "RAN", "]"}]}], ",", + RowBox[{"Cos", "[", "DEC", "]"}], " ", + RowBox[{"Cos", "[", "RAN", "]"}]}], ",", RowBox[{ - RowBox[{"Cos", "[", "DEC", "]"}], " ", - RowBox[{"Sin", "[", "RAN", "]"}]}], ",", + RowBox[{"Cos", "[", "DEC", "]"}], " ", + RowBox[{"Sin", "[", "RAN", "]"}]}], ",", RowBox[{"Sin", "[", "DEC", "]"}]}], "}"}]], "Output", - CellChangeTimes->{{3.765454348932735*^9, 3.7654543664130783`*^9}, + CellChangeTimes->{{3.765454348932735*^9, 3.7654543664130783`*^9}, 3.7654544282707787`*^9, 3.7654559094489737`*^9, 3.765456190122552*^9}, CellLabel->"Out[12]=",ExpressionUUID->"90aca915-3903-4093-b667-397131b1faa3"] }, Open ]], Cell[BoxData[ RowBox[{ - RowBox[{"eHatN", "[", - RowBox[{"RAN_", ",", " ", "DEC_"}], "]"}], ":=", " ", - RowBox[{"{", + RowBox[{"eHatN", "[", + RowBox[{"RAN_", ",", " ", "DEC_"}], "]"}], ":=", " ", + RowBox[{"{", RowBox[{ RowBox[{ - RowBox[{"Cos", "[", "DEC", "]"}], " ", - RowBox[{"Cos", "[", "RAN", "]"}]}], ",", + RowBox[{"Cos", "[", "DEC", "]"}], " ", + RowBox[{"Cos", "[", "RAN", "]"}]}], ",", RowBox[{ - RowBox[{"Cos", "[", "DEC", "]"}], " ", - RowBox[{"Sin", "[", "RAN", "]"}]}], ",", + RowBox[{"Cos", "[", "DEC", "]"}], " ", + RowBox[{"Sin", "[", "RAN", "]"}]}], ",", RowBox[{"Sin", "[", "DEC", "]"}]}], "}"}]}]], "Input", CellChangeTimes->{{3.765456169641238*^9, 3.765456186316567*^9}}, CellLabel->"In[15]:=",ExpressionUUID->"85f896b7-449c-437d-9fac-bab7fcd9ce84"], @@ -162,8 +162,8 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"CForm", "[", - RowBox[{"eHatN", "[", + RowBox[{"CForm", "[", + RowBox[{"eHatN", "[", RowBox[{"RAN", ",", "DEC"}], "]"}], "]"}]], "Input", CellChangeTimes->{{3.7654559110219507`*^9, 3.765455915357334*^9}, { 3.765456206807485*^9, 3.7654562100464573`*^9}}, @@ -178,17 +178,17 @@ Cell["List(Cos(DEC)*Cos(RAN),Cos(DEC)*Sin(RAN),Sin(DEC))", "Output", Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"eHatN", "[", + RowBox[{"eHatN", "[", RowBox[{ - RowBox[{"272.76", " ", "Degree"}], ",", " ", + RowBox[{"272.76", " ", "Degree"}], ",", " ", RowBox[{"67.16", " ", "Degree"}]}], "]"}]], "Input", CellChangeTimes->{{3.7654562717426662`*^9, 3.7654562839052277`*^9}}, CellLabel->"In[17]:=",ExpressionUUID->"1b585cf2-1304-4360-a71f-9ead827f30e0"], Cell[BoxData[ - RowBox[{"{", - RowBox[{"0.018690814168902073`", ",", - RowBox[{"-", "0.3877088083617989`"}], ",", "0.9215923900425705`"}], + RowBox[{"{", + RowBox[{"0.018690814168902073`", ",", + RowBox[{"-", "0.3877088083617989`"}], ",", "0.9215923900425705`"}], "}"}]], "Output", CellChangeTimes->{3.765456284425082*^9}, CellLabel->"Out[17]=",ExpressionUUID->"31076b61-bee2-4b5a-bb50-968bd78addcd"] @@ -235,4 +235,3 @@ Cell[6108, 187, 282, 6, 68, "Output",ExpressionUUID->"31076b61-bee2-4b5a-bb50-96 } ] *) - diff --git a/src/simulation/environment/planetEphemeris/_Documentation/AVS.sty b/src/simulation/environment/planetEphemeris/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/environment/planetEphemeris/_Documentation/AVS.sty +++ b/src/simulation/environment/planetEphemeris/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/environment/planetEphemeris/_Documentation/Basilisk-planetEphemeris-20190422.tex b/src/simulation/environment/planetEphemeris/_Documentation/Basilisk-planetEphemeris-20190422.tex index 0a05ffb065..06cb6267d5 100755 --- a/src/simulation/environment/planetEphemeris/_Documentation/Basilisk-planetEphemeris-20190422.tex +++ b/src/simulation/environment/planetEphemeris/_Documentation/Basilisk-planetEphemeris-20190422.tex @@ -17,8 +17,8 @@ % sec_user_guide.tex % % NOTE: if the TeX document is reading in auto-generated TeX snippets from the AutoTeX folder, then -% pytest must first be run for the unit test of this module. This process creates the required unit test results -%. that are read into this document. +% pytest must first be run for the unit test of this module. This process creates the required unit test results +%. that are read into this document. % %-Some rules about referencing within the document: %1. If writing the user guide, assume the module description is present @@ -93,7 +93,7 @@ - + \input{secModuleDescription.tex} %This section includes mathematical models, code description, etc. \input{secModuleFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/environment/planetEphemeris/_Documentation/BasiliskReportMemo.cls b/src/simulation/environment/planetEphemeris/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100755 --- a/src/simulation/environment/planetEphemeris/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/environment/planetEphemeris/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/environment/planetEphemeris/_Documentation/bibliography.bib b/src/simulation/environment/planetEphemeris/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/simulation/environment/planetEphemeris/_Documentation/bibliography.bib +++ b/src/simulation/environment/planetEphemeris/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/simulation/environment/planetEphemeris/_Documentation/secModuleDescription.tex b/src/simulation/environment/planetEphemeris/_Documentation/secModuleDescription.tex index 55605253dc..077ae4521e 100644 --- a/src/simulation/environment/planetEphemeris/_Documentation/secModuleDescription.tex +++ b/src/simulation/environment/planetEphemeris/_Documentation/secModuleDescription.tex @@ -11,13 +11,13 @@ \section{Model Description} -The purpose of this module is to create planetary ephemeris messages where the planetary motion is modeled through classical orbit elements. The module output messages are illustrated in Figure~\ref{fig:moduleImg}. Note that there are no input messages. The planetary translational motion, and optionally the rotational motion, are specified through the module parameters directly. +The purpose of this module is to create planetary ephemeris messages where the planetary motion is modeled through classical orbit elements. The module output messages are illustrated in Figure~\ref{fig:moduleImg}. Note that there are no input messages. The planetary translational motion, and optionally the rotational motion, are specified through the module parameters directly. \subsection{Specifying the required planetary translational information} The first step is to specify the planet names that are to be modeled. This is done by creating a C++ vector of strings and setting them using the module {\tt setPlanetNames()} method. The length of this vector determines how many output messages are created in the vector {\tt planetOutMsgs}. The planet name string provided is used to fill the output message {\tt PlanetName} variable. -Next the translational planetary motion is specified through a C++ vector of classical orbit element structures called {\tt planetElements}. The true anomaly provided is assumes to be the anomaly angle at epoch. This is the same time as the zero'th simulation time step. +Next the translational planetary motion is specified through a C++ vector of classical orbit element structures called {\tt planetElements}. The true anomaly provided is assumes to be the anomaly angle at epoch. This is the same time as the zero'th simulation time step. @@ -40,4 +40,4 @@ \subsection{Specifying the optional planetary orientational information} \end{equation} From these states the planet's DCM $[PN]$ and DCM rate $[\dot{PN}]$ are evaluated. -If the planet orientation information is computed, then the output message {\tt computeOrient} is set to +1. If not, then {\tt computeOrient} is 0. If the orientation is not specified, then the planet DCM is set to the identity matrix with $[PN] = [I_{3\times 3}]$. The DCM rate is set to zero. If any orientation states are not specified, then the module will output this default zero orientation attitude. \ No newline at end of file +If the planet orientation information is computed, then the output message {\tt computeOrient} is set to +1. If not, then {\tt computeOrient} is 0. If the orientation is not specified, then the planet DCM is set to the identity matrix with $[PN] = [I_{3\times 3}]$. The DCM rate is set to zero. If any orientation states are not specified, then the module will output this default zero orientation attitude. diff --git a/src/simulation/environment/planetEphemeris/_Documentation/secModuleFunctions.tex b/src/simulation/environment/planetEphemeris/_Documentation/secModuleFunctions.tex index dfed576839..a11df6c048 100644 --- a/src/simulation/environment/planetEphemeris/_Documentation/secModuleFunctions.tex +++ b/src/simulation/environment/planetEphemeris/_Documentation/secModuleFunctions.tex @@ -8,4 +8,4 @@ \section{Module Functions} \end{itemize} \section{Module Assumptions and Limitations} -The module assumes the heliocentric motion is unperturbed. The optional rotational motion is assumed to be an inertially fixed rotation of the planet. \ No newline at end of file +The module assumes the heliocentric motion is unperturbed. The optional rotational motion is assumed to be an inertially fixed rotation of the planet. diff --git a/src/simulation/environment/planetEphemeris/_Documentation/secTest.tex b/src/simulation/environment/planetEphemeris/_Documentation/secTest.tex index 15a4fa0774..84c7ea5c1b 100644 --- a/src/simulation/environment/planetEphemeris/_Documentation/secTest.tex +++ b/src/simulation/environment/planetEphemeris/_Documentation/secTest.tex @@ -1,7 +1,7 @@ % !TEX root = ./Basilisk-planetEphemeris-20190422.tex \section{Test Description and Success Criteria} -The unit test configures the module to model general orbital motions of Earth and Venus. In each case the translational motion is compute for 3 times steps from 0 to 1 second using a 0.5 second time step. The planet orientation information is only set if the appropriate simulation parameter is set. The following sub-sections discuss these flags. If none of these flags are set, then the module default orientation behavior is expected. If all the flags are set, then a constant rotation is set. If only a partial set of orientation information is provided then the reset routine will force the module to throw an error message and only output a default constant zero orientation of the planet. +The unit test configures the module to model general orbital motions of Earth and Venus. In each case the translational motion is compute for 3 times steps from 0 to 1 second using a 0.5 second time step. The planet orientation information is only set if the appropriate simulation parameter is set. The following sub-sections discuss these flags. If none of these flags are set, then the module default orientation behavior is expected. If all the flags are set, then a constant rotation is set. If only a partial set of orientation information is provided then the reset routine will force the module to throw an error message and only output a default constant zero orientation of the planet. \subsection{{\tt setRAN}} This flag specifies if a set of right ascension angles are specified in the unit test. @@ -24,16 +24,16 @@ \section{Test Parameters} \caption{Error tolerance for each test.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c } % Column formatting, + \begin{tabular}{ c | c } % Column formatting, \hline\hline - \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ + \textbf{Output Value Tested} & \textbf{Tolerated Error} \\ \hline - {\tt J2000Current} & \input{AutoTeX/toleranceValue} s \\ - {\tt PositionVector} & \input{AutoTeX/toleranceValue} m \\ - {\tt VelocityVector} & \input{AutoTeX/toleranceValue} m/s \\ - {\tt J20002Pfix} & \input{AutoTeX/toleranceValue} \\ - {\tt J20002Pfix\_dot} & $10^{-10}$ rad/s \\ - {\tt computeOrient} & \input{AutoTeX/toleranceValue} \\ + {\tt J2000Current} & \input{AutoTeX/toleranceValue} s \\ + {\tt PositionVector} & \input{AutoTeX/toleranceValue} m \\ + {\tt VelocityVector} & \input{AutoTeX/toleranceValue} m/s \\ + {\tt J20002Pfix} & \input{AutoTeX/toleranceValue} \\ + {\tt J20002Pfix\_dot} & $10^{-10}$ rad/s \\ + {\tt computeOrient} & \input{AutoTeX/toleranceValue} \\ \hline\hline \end{tabular} \end{table} @@ -48,27 +48,26 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | c | c | c } % Column formatting, + \begin{tabular}{c | c | c | c | c } % Column formatting, \hline\hline - {\tt setRAN} & {\tt setDEC} & {\tt setLST} & {\tt setRate} &\textbf{Pass/Fail} \\ + {\tt setRAN} & {\tt setDEC} & {\tt setLST} & {\tt setRate} &\textbf{Pass/Fail} \\ \hline - True & True & True & True & \input{AutoTeX/passFailTrueTrueTrueTrue} \\ - True & True & True & False & \input{AutoTeX/passFailTrueTrueTrueFalse} \\ - True & True & False & True & \input{AutoTeX/passFailTrueTrueFalseTrue} \\ - True & True & False & False & \input{AutoTeX/passFailTrueTrueFalseFalse} \\ - True & False & True & True & \input{AutoTeX/passFailTrueFalseTrueTrue} \\ - True & False & True & False & \input{AutoTeX/passFailTrueFalseTrueFalse} \\ - True & False & False & True & \input{AutoTeX/passFailTrueFalseFalseTrue} \\ - True & False & False & False & \input{AutoTeX/passFailTrueFalseFalseFalse} \\ - False & True & True & True & \input{AutoTeX/passFailFalseTrueTrueTrue} \\ - False & True & True & False & \input{AutoTeX/passFailFalseTrueTrueFalse} \\ - False & True & False & True & \input{AutoTeX/passFailFalseTrueFalseTrue} \\ - False & True & False & False & \input{AutoTeX/passFailFalseTrueFalseFalse} \\ - False & False & True & True & \input{AutoTeX/passFailFalseFalseTrueTrue} \\ - False & False & True & False & \input{AutoTeX/passFailFalseFalseTrueFalse} \\ - False & False & False & True & \input{AutoTeX/passFailFalseFalseFalseTrue} \\ - False & False & False & False & \input{AutoTeX/passFailFalseFalseFalseFalse} \\ + True & True & True & True & \input{AutoTeX/passFailTrueTrueTrueTrue} \\ + True & True & True & False & \input{AutoTeX/passFailTrueTrueTrueFalse} \\ + True & True & False & True & \input{AutoTeX/passFailTrueTrueFalseTrue} \\ + True & True & False & False & \input{AutoTeX/passFailTrueTrueFalseFalse} \\ + True & False & True & True & \input{AutoTeX/passFailTrueFalseTrueTrue} \\ + True & False & True & False & \input{AutoTeX/passFailTrueFalseTrueFalse} \\ + True & False & False & True & \input{AutoTeX/passFailTrueFalseFalseTrue} \\ + True & False & False & False & \input{AutoTeX/passFailTrueFalseFalseFalse} \\ + False & True & True & True & \input{AutoTeX/passFailFalseTrueTrueTrue} \\ + False & True & True & False & \input{AutoTeX/passFailFalseTrueTrueFalse} \\ + False & True & False & True & \input{AutoTeX/passFailFalseTrueFalseTrue} \\ + False & True & False & False & \input{AutoTeX/passFailFalseTrueFalseFalse} \\ + False & False & True & True & \input{AutoTeX/passFailFalseFalseTrueTrue} \\ + False & False & True & False & \input{AutoTeX/passFailFalseFalseTrueFalse} \\ + False & False & False & True & \input{AutoTeX/passFailFalseFalseFalseTrue} \\ + False & False & False & False & \input{AutoTeX/passFailFalseFalseFalseFalse} \\ \hline\hline \end{tabular} \end{table} - diff --git a/src/simulation/environment/planetEphemeris/planetEphemeris.rst b/src/simulation/environment/planetEphemeris/planetEphemeris.rst index 668ce424bd..098d028e17 100644 --- a/src/simulation/environment/planetEphemeris/planetEphemeris.rst +++ b/src/simulation/environment/planetEphemeris/planetEphemeris.rst @@ -31,4 +31,3 @@ provides information on what this message is used for. * - planetOutMsgs - :ref:`SpicePlanetStateMsgPayload` - vector of planet spice state output messages - diff --git a/src/simulation/environment/solarFlux/solarFlux.h b/src/simulation/environment/solarFlux/solarFlux.h index dea6490f43..2415dbb73a 100755 --- a/src/simulation/environment/solarFlux/solarFlux.h +++ b/src/simulation/environment/solarFlux/solarFlux.h @@ -35,7 +35,7 @@ class SolarFlux: public SysModel { public: SolarFlux(){}; ~SolarFlux(){}; - + void Reset(uint64_t CurrentSimNanos) override; void UpdateState(uint64_t CurrentSimNanos) override; void writeMessages(uint64_t CurrentSimNanos); diff --git a/src/simulation/environment/solarFlux/solarFlux.rst b/src/simulation/environment/solarFlux/solarFlux.rst index 02355db686..6033db3a87 100644 --- a/src/simulation/environment/solarFlux/solarFlux.rst +++ b/src/simulation/environment/solarFlux/solarFlux.rst @@ -79,4 +79,3 @@ The names below are only special in that they are useful defaults and are actual sim.AddModelToTask(task.Name, sf) dataLog = sf.solarFluxOutMsg.recorder() - diff --git a/src/simulation/environment/spacecraftLocation/_UnitTest/Support/spacecraftLocation.nb b/src/simulation/environment/spacecraftLocation/_UnitTest/Support/spacecraftLocation.nb index 96604f437c..21f6c1c31c 100644 --- a/src/simulation/environment/spacecraftLocation/_UnitTest/Support/spacecraftLocation.nb +++ b/src/simulation/environment/spacecraftLocation/_UnitTest/Support/spacecraftLocation.nb @@ -26,30 +26,30 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell["Test nearest point calculation", "Section", - CellChangeTimes->{{3.822174322711443*^9, + CellChangeTimes->{{3.822174322711443*^9, 3.8221743289859123`*^9}},ExpressionUUID->"fdef4746-3e75-4778-95b7-\ bbfd8ae8cf69"], Cell[BoxData[{ RowBox[{ - RowBox[{"rL", " ", "=", " ", - RowBox[{"{", - RowBox[{"1", ",", "2", ",", "3"}], "}"}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"rL", " ", "=", " ", + RowBox[{"{", + RowBox[{"1", ",", "2", ",", "3"}], "}"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"d", " ", "=", " ", - RowBox[{"{", - RowBox[{"1", ",", "1", ",", + RowBox[{"d", " ", "=", " ", + RowBox[{"{", + RowBox[{"1", ",", "1", ",", RowBox[{"-", "0.5"}]}], "}"}]}], ";"}]}], "Input", CellChangeTimes->{{3.822158903786858*^9, 3.822158924107624*^9}, { - 3.8221593428879147`*^9, 3.822159342951026*^9}, {3.822159389509151*^9, + 3.8221593428879147`*^9, 3.822159342951026*^9}, {3.822159389509151*^9, 3.822159390317012*^9}}, CellLabel->"In[66]:=",ExpressionUUID->"f2027347-ba3c-4676-bcf6-a420db02c146"], Cell[BoxData[ RowBox[{ - RowBox[{"nearest", "[", "t_", "]"}], ":=", - RowBox[{"rL", " ", "+", " ", + RowBox[{"nearest", "[", "t_", "]"}], ":=", + RowBox[{"rL", " ", "+", " ", RowBox[{"t", " ", "d"}]}]}]], "Input", CellChangeTimes->{{3.8221590085630903`*^9, 3.8221590225619287`*^9}}, CellLabel->"In[68]:=",ExpressionUUID->"799b13ff-caa8-4166-adce-2597dfe09e4c"], @@ -57,12 +57,12 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"tt", " ", "=", " ", + RowBox[{"tt", " ", "=", " ", RowBox[{ - RowBox[{"-", - RowBox[{"Dot", "[", - RowBox[{"rL", ",", "d"}], "]"}]}], "/", - RowBox[{"Dot", "[", + RowBox[{"-", + RowBox[{"Dot", "[", + RowBox[{"rL", ",", "d"}], "]"}]}], "/", + RowBox[{"Dot", "[", RowBox[{"d", ",", "d"}], "]"}]}]}]], "Input", CellChangeTimes->{{3.822159025270268*^9, 3.8221590402439938`*^9}, { 3.82217542397547*^9, 3.822175431353751*^9}}, @@ -84,11 +84,11 @@ Cell[BoxData[ CellLabel->"In[70]:=",ExpressionUUID->"a0a2447e-11cd-4147-ad67-b3052092a6fa"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ - "0.33333333333333337`", ",", "1.3333333333333335`", ",", + "0.33333333333333337`", ",", "1.3333333333333335`", ",", "3.3333333333333335`"}], "}"}]], "Output", - CellChangeTimes->{3.82215904688062*^9, 3.8221593613339443`*^9, + CellChangeTimes->{3.82215904688062*^9, 3.8221593613339443`*^9, 3.82215939270897*^9, 3.822175436959484*^9}, CellLabel->"Out[70]=",ExpressionUUID->"6e5aa9c4-823b-49e8-a85c-af947db09a12"] }, Open ]], @@ -96,43 +96,43 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Manipulate", "[", "\[IndentingNewLine]", + RowBox[{"Manipulate", "[", "\[IndentingNewLine]", RowBox[{ - RowBox[{"Show", "[", "\[IndentingNewLine]", + RowBox[{"Show", "[", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ParametricPlot3D", "[", + RowBox[{"ParametricPlot3D", "[", RowBox[{ - RowBox[{"rL", " ", "+", " ", - RowBox[{"t", " ", "d"}]}], ",", - RowBox[{"{", - RowBox[{"t", ",", - RowBox[{"-", "10"}], ",", "10"}], "}"}]}], "]"}], ",", - "\[IndentingNewLine]", - RowBox[{"Graphics3D", "[", - RowBox[{"{", - RowBox[{"Red", ",", " ", - RowBox[{"PointSize", "[", "0.02", "]"}], ",", " ", - RowBox[{"Point", "[", - RowBox[{"nearest", "[", "tt", "]"}], "]"}]}], "}"}], "]"}], ",", - "\[IndentingNewLine]", - RowBox[{"Graphics3D", "[", - RowBox[{"{", + RowBox[{"rL", " ", "+", " ", + RowBox[{"t", " ", "d"}]}], ",", + RowBox[{"{", + RowBox[{"t", ",", + RowBox[{"-", "10"}], ",", "10"}], "}"}]}], "]"}], ",", + "\[IndentingNewLine]", + RowBox[{"Graphics3D", "[", + RowBox[{"{", + RowBox[{"Red", ",", " ", + RowBox[{"PointSize", "[", "0.02", "]"}], ",", " ", + RowBox[{"Point", "[", + RowBox[{"nearest", "[", "tt", "]"}], "]"}]}], "}"}], "]"}], ",", + "\[IndentingNewLine]", + RowBox[{"Graphics3D", "[", + RowBox[{"{", RowBox[{ - RowBox[{"PointSize", "[", "0.02", "]"}], ",", " ", - RowBox[{"Point", "[", - RowBox[{"nearest", "[", "tUser", "]"}], "]"}]}], "}"}], "]"}], ",", - "\[IndentingNewLine]", - RowBox[{"Graphics3D", "[", - RowBox[{"{", - RowBox[{"Line", "[", - RowBox[{"{", + RowBox[{"PointSize", "[", "0.02", "]"}], ",", " ", + RowBox[{"Point", "[", + RowBox[{"nearest", "[", "tUser", "]"}], "]"}]}], "}"}], "]"}], ",", + "\[IndentingNewLine]", + RowBox[{"Graphics3D", "[", + RowBox[{"{", + RowBox[{"Line", "[", + RowBox[{"{", RowBox[{ - RowBox[{"{", - RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", - RowBox[{"nearest", "[", "tUser", "]"}]}], "}"}], "]"}], "}"}], - "]"}]}], "\[IndentingNewLine]", "]"}], "\[IndentingNewLine]", ",", - RowBox[{"{", - RowBox[{"tUser", ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", + RowBox[{"nearest", "[", "tUser", "]"}]}], "}"}], "]"}], "}"}], + "]"}]}], "\[IndentingNewLine]", "]"}], "\[IndentingNewLine]", ",", + RowBox[{"{", + RowBox[{"tUser", ",", RowBox[{"-", "10"}], ",", "10"}], "}"}]}], "]"}]], "Input", CellChangeTimes->{{3.8221591948655033`*^9, 3.822159225290412*^9}, { 3.822159257385582*^9, 3.82215935271294*^9}}, @@ -141,35 +141,35 @@ Cell[BoxData[ Cell[BoxData[ TagBox[ StyleBox[ - DynamicModuleBox[{$CellContext`tUser$$ = -6.75, Typeset`show$$ = True, - Typeset`bookmarkList$$ = {}, Typeset`bookmarkMode$$ = "Menu", - Typeset`animator$$, Typeset`animvar$$ = 1, Typeset`name$$ = + DynamicModuleBox[{$CellContext`tUser$$ = -6.75, Typeset`show$$ = True, + Typeset`bookmarkList$$ = {}, Typeset`bookmarkMode$$ = "Menu", + Typeset`animator$$, Typeset`animvar$$ = 1, Typeset`name$$ = "\"untitled\"", Typeset`specs$$ = {{ Hold[$CellContext`tUser$$], -10, 10}}, Typeset`size$$ = { - 360., {176., 180.}}, Typeset`update$$ = 0, Typeset`initDone$$, - Typeset`skipInitDone$$ = True}, + 360., {176., 180.}}, Typeset`update$$ = 0, Typeset`initDone$$, + Typeset`skipInitDone$$ = True}, DynamicBox[Manipulate`ManipulateBoxes[ - 1, StandardForm, "Variables" :> {$CellContext`tUser$$ = -10}, - "ControllerVariables" :> {}, + 1, StandardForm, "Variables" :> {$CellContext`tUser$$ = -10}, + "ControllerVariables" :> {}, "OtherVariables" :> { - Typeset`show$$, Typeset`bookmarkList$$, Typeset`bookmarkMode$$, - Typeset`animator$$, Typeset`animvar$$, Typeset`name$$, + Typeset`show$$, Typeset`bookmarkList$$, Typeset`bookmarkMode$$, + Typeset`animator$$, Typeset`animvar$$, Typeset`name$$, Typeset`specs$$, Typeset`size$$, Typeset`update$$, Typeset`initDone$$, Typeset`skipInitDone$$}, "Body" :> Show[ ParametricPlot3D[$CellContext`rL + $CellContext`t $CellContext`d, \ -{$CellContext`t, -10, 10}], - Graphics3D[{Red, - PointSize[0.02], +{$CellContext`t, -10, 10}], + Graphics3D[{Red, + PointSize[0.02], Point[ - $CellContext`nearest[$CellContext`tt]]}], + $CellContext`nearest[$CellContext`tt]]}], Graphics3D[{ - PointSize[0.02], + PointSize[0.02], Point[ - $CellContext`nearest[$CellContext`tUser$$]]}], + $CellContext`nearest[$CellContext`tUser$$]]}], Graphics3D[{ - Line[{{0, 0, 0}, - $CellContext`nearest[$CellContext`tUser$$]}]}]], - "Specifications" :> {{$CellContext`tUser$$, -10, 10}}, "Options" :> {}, + Line[{{0, 0, 0}, + $CellContext`nearest[$CellContext`tUser$$]}]}]], + "Specifications" :> {{$CellContext`tUser$$, -10, 10}}, "Options" :> {}, "DefaultOptions" :> {}], ImageSizeCache->{405., {221., 227.}}, SingleEvaluation->True], @@ -192,27 +192,27 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell["Spacecraft Access Test", "Section", - CellChangeTimes->{{3.822174334530141*^9, + CellChangeTimes->{{3.822174334530141*^9, 3.822174347325515*^9}},ExpressionUUID->"d63a2579-ef26-4244-a50a-\ 5ad0f8ec2113"], Cell[BoxData[{ RowBox[{ - RowBox[{"r", "=", " ", - RowBox[{"reqEarth", " ", "+", " ", "1000."}]}], - ";"}], "\[IndentingNewLine]", + RowBox[{"r", "=", " ", + RowBox[{"reqEarth", " ", "+", " ", "1000."}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"angle", " ", "=", " ", - RowBox[{"90.", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{"angle", " ", "=", " ", + RowBox[{"90.", " ", "Degree"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"zScale", " ", "=", " ", - RowBox[{"{", + RowBox[{"zScale", " ", "=", " ", + RowBox[{"{", RowBox[{"1", ",", "2"}], "}"}]}], ";"}]}], "Input", CellChangeTimes->{{3.822174360306006*^9, 3.822174390029777*^9}, { - 3.82217443414504*^9, 3.822174434497575*^9}, {3.822174518488579*^9, - 3.8221745221769123`*^9}, 3.822174628975865*^9, {3.822175556715019*^9, + 3.82217443414504*^9, 3.822174434497575*^9}, {3.822174518488579*^9, + 3.8221745221769123`*^9}, 3.822174628975865*^9, {3.822175556715019*^9, 3.822175567341251*^9}, {3.822176618043816*^9, 3.822176622839868*^9}, { - 3.8221766569480457`*^9, 3.822176684808957*^9}, {3.82217676057128*^9, + 3.8221766569480457`*^9, 3.822176684808957*^9}, {3.82217676057128*^9, 3.822176771102263*^9}, {3.822177243182062*^9, 3.822177259589284*^9}}, CellLabel-> "In[179]:=",ExpressionUUID->"8620891f-ecd9-4497-9f4a-f595865f7484"], @@ -220,48 +220,48 @@ Cell[BoxData[{ Cell[CellGroupData[{ Cell[BoxData[{ - RowBox[{"rB", " ", "=", " ", - RowBox[{"r", " ", - RowBox[{"{", - RowBox[{"1", ",", "0"}], "}"}]}]}], "\[IndentingNewLine]", - RowBox[{"rS", " ", "=", " ", - RowBox[{"r", - RowBox[{"{", + RowBox[{"rB", " ", "=", " ", + RowBox[{"r", " ", + RowBox[{"{", + RowBox[{"1", ",", "0"}], "}"}]}]}], "\[IndentingNewLine]", + RowBox[{"rS", " ", "=", " ", + RowBox[{"r", + RowBox[{"{", RowBox[{ - RowBox[{"Cos", "[", "angle", "]"}], ",", " ", - RowBox[{"Sin", "[", "angle", "]"}]}], "}"}]}]}], "\[IndentingNewLine]", - RowBox[{"rSB", " ", "=", " ", + RowBox[{"Cos", "[", "angle", "]"}], ",", " ", + RowBox[{"Sin", "[", "angle", "]"}]}], "}"}]}]}], "\[IndentingNewLine]", + RowBox[{"rSB", " ", "=", " ", RowBox[{"rS", " ", "-", " ", "rB"}]}]}], "Input", CellChangeTimes->{{3.822174496700551*^9, 3.822174532588408*^9}, { - 3.8221745864199543`*^9, 3.822174586600226*^9}, {3.822175173089787*^9, + 3.8221745864199543`*^9, 3.822174586600226*^9}, {3.822175173089787*^9, 3.822175203727586*^9}}, CellLabel-> "In[182]:=",ExpressionUUID->"bbe6ea74-e820-4443-8bb3-72b4ec165a67"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{"7378.1366`", ",", "0.`"}], "}"}]], "Output", CellChangeTimes->{{3.8221751747406187`*^9, 3.822175204153243*^9}, { - 3.8221755602738934`*^9, 3.822175570271946*^9}, {3.8221766599812727`*^9, + 3.8221755602738934`*^9, 3.822175570271946*^9}, {3.8221766599812727`*^9, 3.8221766672288027`*^9}, {3.822177246002091*^9, 3.822177261981621*^9}}, CellLabel-> "Out[182]=",ExpressionUUID->"ba359a35-0a34-4234-aade-36933f3591eb"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{"4.5178056854309675`*^-13", ",", "7378.1366`"}], "}"}]], "Output", CellChangeTimes->{{3.8221751747406187`*^9, 3.822175204153243*^9}, { - 3.8221755602738934`*^9, 3.822175570271946*^9}, {3.8221766599812727`*^9, + 3.8221755602738934`*^9, 3.822175570271946*^9}, {3.8221766599812727`*^9, 3.8221766672288027`*^9}, {3.822177246002091*^9, 3.822177261983465*^9}}, CellLabel-> "Out[183]=",ExpressionUUID->"187bd340-3dca-49b7-ae1b-98cc9f090588"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{ RowBox[{"-", "7378.1366`"}], ",", "7378.1366`"}], "}"}]], "Output", CellChangeTimes->{{3.8221751747406187`*^9, 3.822175204153243*^9}, { - 3.8221755602738934`*^9, 3.822175570271946*^9}, {3.8221766599812727`*^9, + 3.8221755602738934`*^9, 3.822175570271946*^9}, {3.8221766599812727`*^9, 3.8221766672288027`*^9}, {3.822177246002091*^9, 3.822177261985334*^9}}, CellLabel-> "Out[184]=",ExpressionUUID->"46765db8-50c8-4046-bcf2-f99306ccfd70"] @@ -270,18 +270,18 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"param", " ", "=", " ", + RowBox[{"param", " ", "=", " ", RowBox[{ - RowBox[{"-", - RowBox[{"Dot", "[", + RowBox[{"-", + RowBox[{"Dot", "[", RowBox[{ - RowBox[{"rSB", " ", "zScale"}], ",", " ", - RowBox[{"rB", " ", "zScale"}]}], "]"}]}], "/", - RowBox[{"Dot", "[", + RowBox[{"rSB", " ", "zScale"}], ",", " ", + RowBox[{"rB", " ", "zScale"}]}], "]"}]}], "/", + RowBox[{"Dot", "[", RowBox[{ - RowBox[{"rSB", " ", "zScale"}], ",", " ", + RowBox[{"rSB", " ", "zScale"}], ",", " ", RowBox[{"rSB", " ", "zScale"}]}], "]"}]}]}]], "Input", - CellChangeTimes->{{3.822175186162149*^9, 3.8221752282650414`*^9}, + CellChangeTimes->{{3.822175186162149*^9, 3.8221752282650414`*^9}, 3.8221753096253357`*^9, {3.8221754844830637`*^9, 3.822175489595668*^9}, { 3.822176789580037*^9, 3.822176802781786*^9}}, CellLabel-> @@ -289,9 +289,9 @@ Cell[BoxData[ Cell[BoxData["0.2`"], "Output", CellChangeTimes->{ - 3.8221752505672817`*^9, 3.822175310128345*^9, {3.8221754787795763`*^9, + 3.8221752505672817`*^9, 3.822175310128345*^9, {3.8221754787795763`*^9, 3.822175490269907*^9}, {3.822175560303719*^9, 3.822175570280826*^9}, { - 3.82217665999083*^9, 3.822176667258267*^9}, {3.822176793964128*^9, + 3.82217665999083*^9, 3.822176667258267*^9}, {3.822176793964128*^9, 3.8221768034367847`*^9}, {3.822177246013507*^9, 3.822177262017005*^9}}, CellLabel-> "Out[185]=",ExpressionUUID->"8d8ed489-322f-49e1-986a-1a08299db0b1"] @@ -300,26 +300,26 @@ Cell[BoxData["0.2`"], "Output", Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"rClose", " ", "=", " ", + RowBox[{"rClose", " ", "=", " ", RowBox[{ - RowBox[{"(", - RowBox[{"rB", " ", "+", " ", + RowBox[{"(", + RowBox[{"rB", " ", "+", " ", RowBox[{"param", " ", "rSB"}]}], " ", ")"}], " ", "zScale"}]}]], "Input",\ CellChangeTimes->{{3.822175260143683*^9, 3.822175269282509*^9}, { - 3.822176810180956*^9, 3.822176812229183*^9}, {3.8221768997760572`*^9, + 3.822176810180956*^9, 3.822176812229183*^9}, {3.8221768997760572`*^9, 3.822176922710826*^9}, 3.8221769751363573`*^9}, CellLabel-> "In[186]:=",ExpressionUUID->"1ee4b984-76b2-4e2b-9dbd-ee8ec55bd388"], Cell[BoxData[ - RowBox[{"{", + RowBox[{"{", RowBox[{"5902.50928`", ",", "2951.25464`"}], "}"}]], "Output", CellChangeTimes->{ - 3.822175269898757*^9, 3.822175311366249*^9, {3.8221754795467367`*^9, + 3.822175269898757*^9, 3.822175311366249*^9, {3.8221754795467367`*^9, 3.82217549153915*^9}, {3.822175560308742*^9, 3.822175570307267*^9}, { 3.8221766600171423`*^9, 3.822176667263468*^9}, 3.8221768148167057`*^9, { - 3.822176902037444*^9, 3.8221769757248096`*^9}, {3.8221772460437117`*^9, + 3.822176902037444*^9, 3.8221769757248096`*^9}, {3.8221772460437117`*^9, 3.8221772620225487`*^9}}, CellLabel-> "Out[186]=",ExpressionUUID->"7247e693-8e31-4941-8038-4b6cc051c7e9"] @@ -328,60 +328,60 @@ Cell[BoxData[ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Show", "[", "\[IndentingNewLine]", + RowBox[{"Show", "[", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ParametricPlot", "[", + RowBox[{"ParametricPlot", "[", RowBox[{ - RowBox[{"reqEarth", + RowBox[{"reqEarth", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"Cos", "[", "t", "]"}], ",", " ", - RowBox[{"Sin", "[", "t", "]"}]}], "}"}], "/", "zScale"}]}], ",", " ", - RowBox[{"{", - RowBox[{"t", ",", "0", ",", - RowBox[{"2", " ", "\[Pi]"}]}], "}"}]}], "]"}], "\[IndentingNewLine]", - ",", " ", - RowBox[{"Graphics", "[", - RowBox[{"{", + RowBox[{"Cos", "[", "t", "]"}], ",", " ", + RowBox[{"Sin", "[", "t", "]"}]}], "}"}], "/", "zScale"}]}], ",", " ", + RowBox[{"{", + RowBox[{"t", ",", "0", ",", + RowBox[{"2", " ", "\[Pi]"}]}], "}"}]}], "]"}], "\[IndentingNewLine]", + ",", " ", + RowBox[{"Graphics", "[", + RowBox[{"{", RowBox[{ - RowBox[{"PointSize", "[", "0.02", "]"}], ",", "Blue", ",", " ", - RowBox[{"Point", "[", "rB", "]"}]}], "}"}], "]"}], - "\[IndentingNewLine]", ",", - RowBox[{"Graphics", "[", - RowBox[{"{", + RowBox[{"PointSize", "[", "0.02", "]"}], ",", "Blue", ",", " ", + RowBox[{"Point", "[", "rB", "]"}]}], "}"}], "]"}], + "\[IndentingNewLine]", ",", + RowBox[{"Graphics", "[", + RowBox[{"{", RowBox[{ - RowBox[{"PointSize", "[", "0.02", "]"}], ",", "Green", ",", " ", - RowBox[{"Point", "[", "rS", "]"}]}], "}"}], "]"}], - "\[IndentingNewLine]", ",", " ", - RowBox[{"Graphics", "[", - RowBox[{"{", + RowBox[{"PointSize", "[", "0.02", "]"}], ",", "Green", ",", " ", + RowBox[{"Point", "[", "rS", "]"}]}], "}"}], "]"}], + "\[IndentingNewLine]", ",", " ", + RowBox[{"Graphics", "[", + RowBox[{"{", RowBox[{ - RowBox[{"PointSize", "[", "0.02", "]"}], ",", " ", "Red", ",", " ", - RowBox[{"Point", "[", - RowBox[{"rClose", " ", "/", "zScale"}], "]"}]}], "}"}], "]"}], - "\[IndentingNewLine]", ",", - RowBox[{"Graphics", "[", - RowBox[{"{", - RowBox[{"Line", "[", - RowBox[{"{", - RowBox[{"rB", ",", " ", "rS"}], "}"}], "]"}], "}"}], "]"}], - "\[IndentingNewLine]", ",", - RowBox[{"PlotRange", "\[Rule]", "All"}]}], "\[IndentingNewLine]", + RowBox[{"PointSize", "[", "0.02", "]"}], ",", " ", "Red", ",", " ", + RowBox[{"Point", "[", + RowBox[{"rClose", " ", "/", "zScale"}], "]"}]}], "}"}], "]"}], + "\[IndentingNewLine]", ",", + RowBox[{"Graphics", "[", + RowBox[{"{", + RowBox[{"Line", "[", + RowBox[{"{", + RowBox[{"rB", ",", " ", "rS"}], "}"}], "]"}], "}"}], "]"}], + "\[IndentingNewLine]", ",", + RowBox[{"PlotRange", "\[Rule]", "All"}]}], "\[IndentingNewLine]", "]"}]], "Input", CellChangeTimes->{{3.822174454384317*^9, 3.822174490890959*^9}, { - 3.8221745408306503`*^9, 3.822174561695527*^9}, {3.822174593814293*^9, + 3.8221745408306503`*^9, 3.822174561695527*^9}, {3.822174593814293*^9, 3.82217466328443*^9}, {3.822176574486981*^9, 3.822176628774406*^9}, { - 3.822176692873987*^9, 3.822176695965844*^9}, {3.822176775973424*^9, + 3.822176692873987*^9, 3.822176695965844*^9}, {3.822176775973424*^9, 3.8221767782008047`*^9}, {3.822176830713275*^9, 3.822176889759289*^9}, { 3.822176931471197*^9, 3.822176944634376*^9}, 3.822176979643404*^9}, CellLabel-> "In[187]:=",ExpressionUUID->"b7b6c923-5d38-42c6-847a-e4db43ba5336"], Cell[BoxData[ - GraphicsBox[{{{{}, {}, + GraphicsBox[{{{{}, {}, TagBox[ - {RGBColor[0.368417, 0.506779, 0.709798], AbsoluteThickness[1.6], + {RGBColor[0.368417, 0.506779, 0.709798], AbsoluteThickness[1.6], Opacity[1.], FaceForm[Opacity[0.3]], LineBox[CompressedData[" 1:eJw123c8V+/7OHAqKZJIIaOMNxVaQmhcGsooKUmUREZk7wiZibL3Xq/XMbLH ieLKSISEJISMrEIoWnzP5/H4/fzj8Xy8zjn3ed33dV33df/xEjK0vmS8hoGB @@ -682,11 +682,11 @@ ha+IBXunnUaLANhcT1nQNbtS7aAStv7nLP+F8uD92Ks7vRVRSnOshWGKhDVm tA/JaYeRa8779VbKgiVb+A94S6CtvKrk/34P969w//nD3nz4/38v1+kt3nbf zeLF/wHiP7GO "]]}, - Annotation[#, "Charting`Private`Tag$61644#1"]& ]}, {}}, - {RGBColor[0, 0, 1], PointSize[0.02], PointBox[{7378.1366, 0.}]}, - {RGBColor[0, 1, 0], PointSize[0.02], - PointBox[{4.5178056854309675`*^-13, 7378.1366}]}, - {RGBColor[1, 0, 0], PointSize[0.02], PointBox[{5902.50928, 1475.62732}]}, + Annotation[#, "Charting`Private`Tag$61644#1"]& ]}, {}}, + {RGBColor[0, 0, 1], PointSize[0.02], PointBox[{7378.1366, 0.}]}, + {RGBColor[0, 1, 0], PointSize[0.02], + PointBox[{4.5178056854309675`*^-13, 7378.1366}]}, + {RGBColor[1, 0, 0], PointSize[0.02], PointBox[{5902.50928, 1475.62732}]}, LineBox[{{7378.1366, 0.}, {4.5178056854309675`*^-13, 7378.1366}}]}, Axes->{True, True}, AxesLabel->{None, None}, @@ -699,25 +699,25 @@ zeLF/wHiP7GO ImagePadding->All, Method->{ "DefaultGraphicsInteraction" -> { - "Version" -> 1.2, "TrackMousePosition" -> {True, False}, + "Version" -> 1.2, "TrackMousePosition" -> {True, False}, "Effects" -> { - "Highlight" -> {"ratio" -> 2}, "HighlightPoint" -> {"ratio" -> 2}, + "Highlight" -> {"ratio" -> 2}, "HighlightPoint" -> {"ratio" -> 2}, "Droplines" -> { - "freeformCursorMode" -> True, - "placement" -> {"x" -> "All", "y" -> "None"}}}}, "ScalingFunctions" -> + "freeformCursorMode" -> True, + "placement" -> {"x" -> "All", "y" -> "None"}}}}, "ScalingFunctions" -> None}, PlotRange->All, PlotRangeClipping->True, PlotRangePadding->{{ - Scaled[0.05], + Scaled[0.05], Scaled[0.05]}, { - Scaled[0.05], + Scaled[0.05], Scaled[0.05]}}, Ticks->{Automatic, Automatic}]], "Output", CellChangeTimes->{{3.8221744839274282`*^9, 3.8221744917964354`*^9}, { 3.8221745623413887`*^9, 3.822174632595334*^9}, 3.822174664101468*^9, { - 3.822175560351336*^9, 3.8221755703574867`*^9}, {3.822176592109603*^9, - 3.822176629425507*^9}, {3.822176660054246*^9, 3.822176703789441*^9}, + 3.822175560351336*^9, 3.8221755703574867`*^9}, {3.822176592109603*^9, + 3.822176629425507*^9}, {3.822176660054246*^9, 3.822176703789441*^9}, 3.822176779823431*^9, {3.8221768335605516`*^9, 3.8221769801741333`*^9}, { 3.8221772460808077`*^9, 3.822177262069371*^9}}, CellLabel-> @@ -727,63 +727,63 @@ zeLF/wHiP7GO Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Show", "[", "\[IndentingNewLine]", + RowBox[{"Show", "[", "\[IndentingNewLine]", RowBox[{ - RowBox[{"ParametricPlot", "[", + RowBox[{"ParametricPlot", "[", RowBox[{ - RowBox[{"reqEarth", - RowBox[{"{", + RowBox[{"reqEarth", + RowBox[{"{", RowBox[{ - RowBox[{"Cos", "[", "t", "]"}], ",", " ", - RowBox[{"Sin", "[", "t", "]"}]}], "}"}]}], ",", " ", - RowBox[{"{", - RowBox[{"t", ",", "0", ",", - RowBox[{"2", " ", "\[Pi]"}]}], "}"}]}], "]"}], "\[IndentingNewLine]", - ",", " ", - RowBox[{"Graphics", "[", - RowBox[{"{", + RowBox[{"Cos", "[", "t", "]"}], ",", " ", + RowBox[{"Sin", "[", "t", "]"}]}], "}"}]}], ",", " ", + RowBox[{"{", + RowBox[{"t", ",", "0", ",", + RowBox[{"2", " ", "\[Pi]"}]}], "}"}]}], "]"}], "\[IndentingNewLine]", + ",", " ", + RowBox[{"Graphics", "[", + RowBox[{"{", RowBox[{ - RowBox[{"PointSize", "[", "0.02", "]"}], ",", "Blue", ",", " ", - RowBox[{"Point", "[", - RowBox[{"rB", " ", "zScale"}], "]"}]}], "}"}], "]"}], - "\[IndentingNewLine]", ",", - RowBox[{"Graphics", "[", - RowBox[{"{", + RowBox[{"PointSize", "[", "0.02", "]"}], ",", "Blue", ",", " ", + RowBox[{"Point", "[", + RowBox[{"rB", " ", "zScale"}], "]"}]}], "}"}], "]"}], + "\[IndentingNewLine]", ",", + RowBox[{"Graphics", "[", + RowBox[{"{", RowBox[{ - RowBox[{"PointSize", "[", "0.02", "]"}], ",", "Green", ",", " ", - RowBox[{"Point", "[", - RowBox[{"rS", " ", "zScale"}], "]"}]}], "}"}], "]"}], - "\[IndentingNewLine]", ",", " ", - RowBox[{"Graphics", "[", - RowBox[{"{", + RowBox[{"PointSize", "[", "0.02", "]"}], ",", "Green", ",", " ", + RowBox[{"Point", "[", + RowBox[{"rS", " ", "zScale"}], "]"}]}], "}"}], "]"}], + "\[IndentingNewLine]", ",", " ", + RowBox[{"Graphics", "[", + RowBox[{"{", RowBox[{ - RowBox[{"PointSize", "[", "0.02", "]"}], ",", " ", "Red", ",", " ", - RowBox[{"Point", "[", "rClose", "]"}]}], "}"}], "]"}], - "\[IndentingNewLine]", ",", - RowBox[{"Graphics", "[", - RowBox[{"{", - RowBox[{"Line", "[", - RowBox[{"{", + RowBox[{"PointSize", "[", "0.02", "]"}], ",", " ", "Red", ",", " ", + RowBox[{"Point", "[", "rClose", "]"}]}], "}"}], "]"}], + "\[IndentingNewLine]", ",", + RowBox[{"Graphics", "[", + RowBox[{"{", + RowBox[{"Line", "[", + RowBox[{"{", RowBox[{ - RowBox[{"rB", " ", "zScale"}], ",", " ", - RowBox[{"rS", " ", "zScale"}]}], "}"}], "]"}], "}"}], "]"}], - "\[IndentingNewLine]", ",", - RowBox[{"PlotRange", "\[Rule]", "All"}]}], "\[IndentingNewLine]", + RowBox[{"rB", " ", "zScale"}], ",", " ", + RowBox[{"rS", " ", "zScale"}]}], "}"}], "]"}], "}"}], "]"}], + "\[IndentingNewLine]", ",", + RowBox[{"PlotRange", "\[Rule]", "All"}]}], "\[IndentingNewLine]", "]"}]], "Input", CellChangeTimes->{{3.822174454384317*^9, 3.822174490890959*^9}, { - 3.8221745408306503`*^9, 3.822174561695527*^9}, {3.822174593814293*^9, + 3.8221745408306503`*^9, 3.822174561695527*^9}, {3.822174593814293*^9, 3.82217466328443*^9}, {3.822176574486981*^9, 3.822176628774406*^9}, { - 3.822176692873987*^9, 3.822176695965844*^9}, {3.822176775973424*^9, + 3.822176692873987*^9, 3.822176695965844*^9}, {3.822176775973424*^9, 3.8221767782008047`*^9}, {3.822176830713275*^9, 3.822176889759289*^9}, { - 3.822176931471197*^9, 3.822176944634376*^9}, {3.822176979643404*^9, + 3.822176931471197*^9, 3.822176944634376*^9}, {3.822176979643404*^9, 3.82217703139292*^9}}, CellLabel-> "In[188]:=",ExpressionUUID->"d45a7a7b-1120-4017-a224-404fb8d8de4c"], Cell[BoxData[ - GraphicsBox[{{{{}, {}, + GraphicsBox[{{{{}, {}, TagBox[ - {RGBColor[0.368417, 0.506779, 0.709798], AbsoluteThickness[1.6], + {RGBColor[0.368417, 0.506779, 0.709798], AbsoluteThickness[1.6], Opacity[1.], FaceForm[Opacity[0.3]], LineBox[CompressedData[" 1:eJw1m3c81e/7x6mkSCJFGWVEhZZQNC4NRUpKEiWREdmzCBkhyt5773nWO4or IylKkYRSRlYhlCbf+/N4/H7+8Xg+nHPeb/d9X9f1enKOuIndGbNFbGxszovZ @@ -1083,11 +1083,11 @@ PvE8YXr48rJVoyZov2Xb2TzCFx8kq3LtvILiIf7Ok4QfTj1Z8++bEX4rvTS/ wmI3CqpO7NTB1k1uez8T7rsVf36D7wmU1xlqYRujYJFl7tvUjCMoMOX7dDVh MdoqkR2+quiwV1Puv8/D/SvffnK3ryz+/+fl2n1lXtzy8Hj0P1y5578= "]]}, - Annotation[#, "Charting`Private`Tag$61674#1"]& ]}, {}}, - {RGBColor[0, 0, 1], PointSize[0.02], PointBox[{7378.1366, 0.}]}, - {RGBColor[0, 1, 0], PointSize[0.02], - PointBox[{4.5178056854309675`*^-13, 14756.2732}]}, - {RGBColor[1, 0, 0], PointSize[0.02], PointBox[{5902.50928, 2951.25464}]}, + Annotation[#, "Charting`Private`Tag$61674#1"]& ]}, {}}, + {RGBColor[0, 0, 1], PointSize[0.02], PointBox[{7378.1366, 0.}]}, + {RGBColor[0, 1, 0], PointSize[0.02], + PointBox[{4.5178056854309675`*^-13, 14756.2732}]}, + {RGBColor[1, 0, 0], PointSize[0.02], PointBox[{5902.50928, 2951.25464}]}, LineBox[{{7378.1366, 0.}, {4.5178056854309675`*^-13, 14756.2732}}]}, Axes->{True, True}, AxesLabel->{None, None}, @@ -1100,19 +1100,19 @@ MdoqkR2+quiwV1Puv8/D/SvffnK3ryz+/+fl2n1lXtzy8Hj0P1y5578= ImagePadding->All, Method->{ "DefaultGraphicsInteraction" -> { - "Version" -> 1.2, "TrackMousePosition" -> {True, False}, + "Version" -> 1.2, "TrackMousePosition" -> {True, False}, "Effects" -> { - "Highlight" -> {"ratio" -> 2}, "HighlightPoint" -> {"ratio" -> 2}, + "Highlight" -> {"ratio" -> 2}, "HighlightPoint" -> {"ratio" -> 2}, "Droplines" -> { - "freeformCursorMode" -> True, - "placement" -> {"x" -> "All", "y" -> "None"}}}}, "ScalingFunctions" -> + "freeformCursorMode" -> True, + "placement" -> {"x" -> "All", "y" -> "None"}}}}, "ScalingFunctions" -> None}, PlotRange->All, PlotRangeClipping->True, PlotRangePadding->{{ - Scaled[0.05], + Scaled[0.05], Scaled[0.05]}, { - Scaled[0.05], + Scaled[0.05], Scaled[0.05]}}, Ticks->{Automatic, Automatic}]], "Output", CellChangeTimes->{{3.822177001754611*^9, 3.8221770323883333`*^9}, { @@ -1186,4 +1186,3 @@ Cell[36278, 782, 19745, 338, 472, "Output",ExpressionUUID->"59711d05-fe77-4086-b } ] *) - diff --git a/src/simulation/environment/spiceInterface/_Documentation/AVS.sty b/src/simulation/environment/spiceInterface/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/environment/spiceInterface/_Documentation/AVS.sty +++ b/src/simulation/environment/spiceInterface/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/environment/spiceInterface/_Documentation/Basilisk-SPICE_INTERFACE20170712.tex b/src/simulation/environment/spiceInterface/_Documentation/Basilisk-SPICE_INTERFACE20170712.tex index 185f7f0b1f..068eb0e504 100755 --- a/src/simulation/environment/spiceInterface/_Documentation/Basilisk-SPICE_INTERFACE20170712.tex +++ b/src/simulation/environment/spiceInterface/_Documentation/Basilisk-SPICE_INTERFACE20170712.tex @@ -90,7 +90,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/environment/spiceInterface/_Documentation/BasiliskReportMemo.cls b/src/simulation/environment/spiceInterface/_Documentation/BasiliskReportMemo.cls index 7c17bc4226..c0aff19cf3 100755 --- a/src/simulation/environment/spiceInterface/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/environment/spiceInterface/_Documentation/BasiliskReportMemo.cls @@ -120,4 +120,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/environment/spiceInterface/_Documentation/bibliography.bib b/src/simulation/environment/spiceInterface/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/simulation/environment/spiceInterface/_Documentation/bibliography.bib +++ b/src/simulation/environment/spiceInterface/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/simulation/environment/spiceInterface/_Documentation/secModelDescription.tex b/src/simulation/environment/spiceInterface/_Documentation/secModelDescription.tex index 4e906cdd95..a2c957370a 100644 --- a/src/simulation/environment/spiceInterface/_Documentation/secModelDescription.tex +++ b/src/simulation/environment/spiceInterface/_Documentation/secModelDescription.tex @@ -19,5 +19,4 @@ \subsection{Output message} \subsection{Citation} -More information on Spice can be read by following the following link:\href{ \underline{https://naif.jpl.nasa.gov/naif/spiceconcept.html}}{The Navigation and Ancillary Information Facility}. - +More information on Spice can be read by following the following link:\href{ \underline{https://naif.jpl.nasa.gov/naif/spiceconcept.html}}{The Navigation and Ancillary Information Facility}. diff --git a/src/simulation/environment/spiceInterface/_Documentation/secModelFunctions.tex b/src/simulation/environment/spiceInterface/_Documentation/secModelFunctions.tex index 5ac21656cc..cca7ad00cc 100644 --- a/src/simulation/environment/spiceInterface/_Documentation/secModelFunctions.tex +++ b/src/simulation/environment/spiceInterface/_Documentation/secModelFunctions.tex @@ -8,7 +8,7 @@ \section{Model Functions} \item \textbf{Get Planet Data}: Pulls the desired planet's ephemeris from the Spice kernel. \item \textbf{Get current time string}: Pull the current time string. \item \textbf{Write Spice message}: This module then writes out the spice ephemeris message - \item \textbf{Read Optional Epoch Message}: This this message name is specified, then the epoch date and time information is pulled from this message. This makes it simple to synchronize the epoch information across multiple modules using this one epoch message. + \item \textbf{Read Optional Epoch Message}: This this message name is specified, then the epoch date and time information is pulled from this message. This makes it simple to synchronize the epoch information across multiple modules using this one epoch message. \end{itemize} @@ -16,9 +16,9 @@ \section{Model Assumptions and Limitations} \subsection{Assumptions} -Spice interface reads extremely precise ephemeris data, which we compare with JPL's Horizons data. Depending on the celestial body, the accuracy may vary. +Spice interface reads extremely precise ephemeris data, which we compare with JPL's Horizons data. Depending on the celestial body, the accuracy may vary. There are no direct assumptions made while using this module. A user must simply make sure to be comparing the write data by assuring the the frames, times, and loaded kernels (mars vs mars barycenter) are the same. \subsection{Limitations} -The limitations come directly from the kernels that are available to be loaded. These will limit the planets that can be tracked. \ No newline at end of file +The limitations come directly from the kernels that are available to be loaded. These will limit the planets that can be tracked. diff --git a/src/simulation/environment/spiceInterface/_Documentation/secTest.tex b/src/simulation/environment/spiceInterface/_Documentation/secTest.tex index a5d52ce0b5..aaf951be87 100644 --- a/src/simulation/environment/spiceInterface/_Documentation/secTest.tex +++ b/src/simulation/environment/spiceInterface/_Documentation/secTest.tex @@ -5,17 +5,17 @@ \subsection{Sub Tests} This test is located in {\tt simulation/environment/spice/\_UnitTest/test\_unitSpice.py}. In order to get good coverage of all the outputs of Spice, the test is broken up into several parts: \par \begin{enumerate} -\item \underline{Time Increment Check} The check goes through the simulation time advancement check. The steps are verified to be consistent. -\item \underline{GPS Time Check} At a specific UTC time, the simulation calculates GPS time. We therefore recalculate the expected GPS time at that date and compare it with the simulation for accuracy. +\item \underline{Time Increment Check} The check goes through the simulation time advancement check. The steps are verified to be consistent. +\item \underline{GPS Time Check} At a specific UTC time, the simulation calculates GPS time. We therefore recalculate the expected GPS time at that date and compare it with the simulation for accuracy. \item \underline{Julian Day Check} Similarly, we independently calculate the Julian date given the epoch of the simulation, and compare it to the simulation's Julian date. \item \underline{Mars Position Check} The position for Mars computed by Spice is compared to JPL Horizons ephemeris for the same epoch and propagation time. \item \underline{Earth Position Check} The position for Earth computed by Spice is compared to JPL Horizons ephemeris for the same epoch and propagation time. \item \underline{Sun Position Check} The position for the Sun computed by Spice is compared to JPL Horizons ephemeris for the same epoch and propagation time. -\end{enumerate} +\end{enumerate} \subsection{Test Success Criteria} -In order to thoroughly test the spice ephemeris module, the test was parametrized studying multiple dates. Through all of the tests, the error tolerances drive the success criteria, and are explained in the next section. +In order to thoroughly test the spice ephemeris module, the test was parametrized studying multiple dates. Through all of the tests, the error tolerances drive the success criteria, and are explained in the next section. \underline{Dates studied}: @@ -23,20 +23,20 @@ \subsection{Test Success Criteria} \underline{Truth Data}: -The truth data was taken from JPL's Horizons database. It provides highly accurate ephemerides for solar system objects ( 734640 asteroids, 3477 comets, 178 planetary satellites, 8 planets, the Sun, L1, L2, select spacecraft, and system barycenters ). +The truth data was taken from JPL's Horizons database. It provides highly accurate ephemerides for solar system objects ( 734640 asteroids, 3477 comets, 178 planetary satellites, 8 planets, the Sun, L1, L2, select spacecraft, and system barycenters ). \section{Test Parameters} \underline{Error Tolerance}: -We give ourselves certain levels or tolerance for each of the tests. These are summarized in table \ref{tab:errortol}. +We give ourselves certain levels or tolerance for each of the tests. These are summarized in table \ref{tab:errortol}. \begin{table}[htbp] \caption{Error Tolerance} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{| c | c | c | c | c |} % Column formatting, + \begin{tabular}{| c | c | c | c | c |} % Column formatting, \hline Test & Time Increment &GPS Time& Julian Day & Body Positions \\ \hline @@ -47,18 +47,18 @@ \section{Test Parameters} \end{tabular} \end{table} -The time increment error tolerance is taken at 1 ms generically. The GPS time error is relatively high: this is due to floating point approximations. The Julian Date time error is given by $0.1/(24*3600)$, which is a 0.1 second error over a day. The Body position error tolerance is set to a quarter kilometer generically. +The time increment error tolerance is taken at 1 ms generically. The GPS time error is relatively high: this is due to floating point approximations. The Julian Date time error is given by $0.1/(24*3600)$, which is a 0.1 second error over a day. The Body position error tolerance is set to a quarter kilometer generically. \section{Test Results} \subsection{Pass/Fail results} -When running pytest, we came to notice that the time checks were failing for two of the days. This is due to the fact that they are Sunday mornings, which is the end of a GPS week. The seconds therefore jump from 604800 to 0. Since we understand the source of this error, and in order to make pytest pass, we skip the time checks of two days. Their other tests passed, and all 22 other dates, being that they are not Sundays, pass the time checks as desired. +When running pytest, we came to notice that the time checks were failing for two of the days. This is due to the fact that they are Sunday mornings, which is the end of a GPS week. The seconds therefore jump from 604800 to 0. Since we understand the source of this error, and in order to make pytest pass, we skip the time checks of two days. Their other tests passed, and all 22 other dates, being that they are not Sundays, pass the time checks as desired. \begin{table}[htbp] \caption{Test Parameters} \label{tab:parameters} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | c | c | c | c | c} % Column formatting, + \begin{tabular}{c | c | c | c | c | c | c} % Column formatting, \hline Date & Time Increment &GPS Time& Julian Day & Mars Position & Earth Position & Sun Position \\ \hline @@ -93,13 +93,8 @@ \subsection{Pass/Fail results} \subsection{Ephemeris precision} -From these tests, we can also plot out the precision of the planet ephemeris. This is done in figure \ref{fig:EphemMars}. We notice that Mars has the highest error by orders of magnitude. This is expected, and the errors are still bounded by 200m, which is well beyond the precision needed. We can also look more closely are the precision for Earth and the Sun, seen in figures \ref{fig:EphemEarth} and \ref{fig:EphemSun} respectively. The Earth and Sun's positions are known very precisely. +From these tests, we can also plot out the precision of the planet ephemeris. This is done in figure \ref{fig:EphemMars}. We notice that Mars has the highest error by orders of magnitude. This is expected, and the errors are still bounded by 200m, which is well beyond the precision needed. We can also look more closely are the precision for Earth and the Sun, seen in figures \ref{fig:EphemEarth} and \ref{fig:EphemSun} respectively. The Earth and Sun's positions are known very precisely. \input{AutoTex/EphemMars} \input{AutoTex/EphemEarth} \input{AutoTex/EphemSun} - - - - - diff --git a/src/simulation/navigation/pinholeCamera/pinholeCamera.rst b/src/simulation/navigation/pinholeCamera/pinholeCamera.rst index 39514fc625..f5ad1bac95 100755 --- a/src/simulation/navigation/pinholeCamera/pinholeCamera.rst +++ b/src/simulation/navigation/pinholeCamera/pinholeCamera.rst @@ -4,7 +4,7 @@ Executive Summary This module simulates a camera pinhole model that computes landmark pixels as seen by a spacecraft. It reads the planet's ephemeris and spacecraft state messages as inputs. It outputs a vector of landmark messages. -The module can also account for Sun's lighting constraint by setting a mask angle. +The module can also account for Sun's lighting constraint by setting a mask angle. diff --git a/src/simulation/navigation/planetHeading/planetHeading.h b/src/simulation/navigation/planetHeading/planetHeading.h index e9f4087132..581589e2cd 100755 --- a/src/simulation/navigation/planetHeading/planetHeading.h +++ b/src/simulation/navigation/planetHeading/planetHeading.h @@ -35,7 +35,7 @@ class PlanetHeading: public SysModel { public: PlanetHeading(); ~PlanetHeading(){}; - + void UpdateState(uint64_t CurrentSimNanos) override; void Reset(uint64_t CurrentSimNanos) override; void writeMessages(uint64_t CurrentSimNanos); diff --git a/src/simulation/navigation/planetHeading/planetHeading.rst b/src/simulation/navigation/planetHeading/planetHeading.rst index ce1d19c2f7..470d7f02a6 100644 --- a/src/simulation/navigation/planetHeading/planetHeading.rst +++ b/src/simulation/navigation/planetHeading/planetHeading.rst @@ -78,4 +78,3 @@ The user can only instantiate this module and add it to a task. sim.AddModelToTask(task.Name, ph) dataLog = ph.planetHeadingOutMsg.recorder() - diff --git a/src/simulation/navigation/planetNav/planetNav.rst b/src/simulation/navigation/planetNav/planetNav.rst index 2e0a54708c..1ecab5919b 100644 --- a/src/simulation/navigation/planetNav/planetNav.rst +++ b/src/simulation/navigation/planetNav/planetNav.rst @@ -23,9 +23,9 @@ The user can set the noise levels of each of these parameters independently. Message Connection Descriptions ------------------------------- -The following table lists all the module input and output messages. -The module msg connection is set by the user from python. -The msg type contains a link to the message structure definition, while the description +The following table lists all the module input and output messages. +The module msg connection is set by the user from python. +The msg type contains a link to the message structure definition, while the description provides information on what this message is used for. .. list-table:: Module I/O Messages @@ -56,4 +56,4 @@ depend on velocity or attitude error should depend on angular velocity. planetNavigation.PMatrix = pMatrix planetNavigation.crossTrans = True planetNavigation.crossAtt = False - scSim.AddModelToTask(simTaskName, planetNavigation) \ No newline at end of file + scSim.AddModelToTask(simTaskName, planetNavigation) diff --git a/src/simulation/navigation/simpleNav/_Documentation/AVS.sty b/src/simulation/navigation/simpleNav/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/navigation/simpleNav/_Documentation/AVS.sty +++ b/src/simulation/navigation/simpleNav/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/navigation/simpleNav/_Documentation/Basilisk-SIMPLE_NAV20170712.tex b/src/simulation/navigation/simpleNav/_Documentation/Basilisk-SIMPLE_NAV20170712.tex index af2c278e7b..e919e8f263 100755 --- a/src/simulation/navigation/simpleNav/_Documentation/Basilisk-SIMPLE_NAV20170712.tex +++ b/src/simulation/navigation/simpleNav/_Documentation/Basilisk-SIMPLE_NAV20170712.tex @@ -49,7 +49,7 @@ \newcommand{\status}{Initial Document} \newcommand{\preparer}{S. Piggott} \newcommand{\summary}{ - This is a report documenting the results of the Simple Navigation Model unit test created for + This is a report documenting the results of the Simple Navigation Model unit test created for the AVS Basilisk Simulation as part of the EMM project. It also includes a description of the module.} \begin{document} @@ -92,7 +92,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/navigation/simpleNav/_Documentation/BasiliskReportMemo.cls b/src/simulation/navigation/simpleNav/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/navigation/simpleNav/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/navigation/simpleNav/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/navigation/simpleNav/_Documentation/bibliography.bib b/src/simulation/navigation/simpleNav/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/simulation/navigation/simpleNav/_Documentation/bibliography.bib +++ b/src/simulation/navigation/simpleNav/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/simulation/navigation/simpleNav/_Documentation/secModelDescription.tex b/src/simulation/navigation/simpleNav/_Documentation/secModelDescription.tex index f9c3282b78..b8d9f8ec34 100644 --- a/src/simulation/navigation/simpleNav/_Documentation/secModelDescription.tex +++ b/src/simulation/navigation/simpleNav/_Documentation/secModelDescription.tex @@ -1,42 +1,42 @@ \section{Model Description} -The Simple Navigation model in the AVS Basilisk simulation is used to generate -a stand-in for the real navigation system. Its use-case is to provide realistic -navigation signals that are right at the spec of what ADCS claims for navigation -system performance. For a typical spacecraft navigation system, the spec will -almost always be at least a factor of two lower in performance than the expected -nominal capabilities of the on-board navigation system. Therefore, we require -the existence of a model that can provide spec-level navigation errors so that -the functionality of the guidance and control subsystems can be verified as +The Simple Navigation model in the AVS Basilisk simulation is used to generate +a stand-in for the real navigation system. Its use-case is to provide realistic +navigation signals that are right at the spec of what ADCS claims for navigation +system performance. For a typical spacecraft navigation system, the spec will +almost always be at least a factor of two lower in performance than the expected +nominal capabilities of the on-board navigation system. Therefore, we require +the existence of a model that can provide spec-level navigation errors so that +the functionality of the guidance and control subsystems can be verified as acceptable in the presence of navigation errors that are right at the spec. \\ -The noise present in the simple navigation is designed to mimic the error signals -that will be observed in the real navigation system. The true "noise" present -in a vehicle's navigation system is always a combination of bias, white noise, -and brown noise (or random walk). In order to provide this, a second-order -Gauss-Markov process model was added to the simulation utilities that allows -the user to configure a random walk process. The output of this model when -applied to the vehicle position can be observed in Figure~\ref{fig:SimpleNavPos}, +The noise present in the simple navigation is designed to mimic the error signals +that will be observed in the real navigation system. The true "noise" present +in a vehicle's navigation system is always a combination of bias, white noise, +and brown noise (or random walk). In order to provide this, a second-order +Gauss-Markov process model was added to the simulation utilities that allows +the user to configure a random walk process. The output of this model when +applied to the vehicle position can be observed in Figure~\ref{fig:SimpleNavPos}, and the attitude can be observed in Figure~\ref{fig:SimpleNavAtt}. \input{AutoTeX/SimpleNavPos.tex} \input{AutoTeX/SimpleNavAtt.tex} In this figure, the nominal position that the error was being distributed about - was [10000.0, 0.0, 0.0] in meters and the error was bounded such that the -absolute value of the error was less than 1000 m. The random walk had a white + was [10000.0, 0.0, 0.0] in meters and the error was bounded such that the +absolute value of the error was less than 1000 m. The random walk had a white noise standard deviation of 5 meters. -In the model, the vehicle position, velocity, attitude, attitude rate, -accumulated delta-velocity, and sun-pointing vector have errors applied -separately. Arguably the sun-pointing vector should just use the vehicle -attitude and the Sun position to get is pointing accuracy, but the Sun can be +In the model, the vehicle position, velocity, attitude, attitude rate, +accumulated delta-velocity, and sun-pointing vector have errors applied +separately. Arguably the sun-pointing vector should just use the vehicle +attitude and the Sun position to get is pointing accuracy, but the Sun can be sensed independently of this and that is why there is a separate error source used for it. -The top-level model relies on a lower-level utility module called GaussMarkov -that supplies the random walk process that is added to the truth states. -Since the simpleNav model exercises this utility completely, the unit test -documented here was also used to test the GaussMarkov model both in terms of -functionality and in terms of code coverage. The coverage results are included -in a table in the results. \ No newline at end of file +The top-level model relies on a lower-level utility module called GaussMarkov +that supplies the random walk process that is added to the truth states. +Since the simpleNav model exercises this utility completely, the unit test +documented here was also used to test the GaussMarkov model both in terms of +functionality and in terms of code coverage. The coverage results are included +in a table in the results. diff --git a/src/simulation/navigation/simpleNav/_Documentation/secModelFunctions.tex b/src/simulation/navigation/simpleNav/_Documentation/secModelFunctions.tex index 92f4a00358..6d5cbace5d 100644 --- a/src/simulation/navigation/simpleNav/_Documentation/secModelFunctions.tex +++ b/src/simulation/navigation/simpleNav/_Documentation/secModelFunctions.tex @@ -11,14 +11,14 @@ \section{Model Functions} \item[-] The $\Delta $V \end{itemize} -The user can set the noise levels on each of these parameters independently. +The user can set the noise levels on each of these parameters independently. \section{Model Assumptions and Limitations} -The direct assumption behind the simple nav module, is that the random walk produced by a Gauss-Markov process can accurately model the uncertainties of a real sensor. -Generally, real sensor noise is a combination of bias, white noise, -and brown noise. In order to provide this, a second-order -Gauss-Markov process model was added to the simulation utilities that allows -the user to configure a random walk process. \ No newline at end of file +The direct assumption behind the simple nav module, is that the random walk produced by a Gauss-Markov process can accurately model the uncertainties of a real sensor. +Generally, real sensor noise is a combination of bias, white noise, +and brown noise. In order to provide this, a second-order +Gauss-Markov process model was added to the simulation utilities that allows +the user to configure a random walk process. diff --git a/src/simulation/navigation/simpleNav/_Documentation/secTest.tex b/src/simulation/navigation/simpleNav/_Documentation/secTest.tex index 7a6a80733d..ec428b284d 100644 --- a/src/simulation/navigation/simpleNav/_Documentation/secTest.tex +++ b/src/simulation/navigation/simpleNav/_Documentation/secTest.tex @@ -11,19 +11,19 @@ \subsection{Test location} \subsection{Subtests} -\noindent This unit test is designed to functionally test the simulation model -outputs as well as get complete code path coverage. The test design is broken +\noindent This unit test is designed to functionally test the simulation model +outputs as well as get complete code path coverage. The test design is broken up into three main parts:\\ \begin{enumerate} -\item{\underline{Error Bound Enforcement}: The simulation is run for 2.4 hours and the +\item{\underline{Error Bound Enforcement}: The simulation is run for 2.4 hours and the error bounds for all of the signals are tested. This test length is long enough to see both the walk in the signal and the noise, all the while not being so long as to slow down the test. The test ensures that the bounds are crossed no more than 30\% of the time.} -\item{\underline{Error Bound Usage}: The error signals are checked for all of the model - parameters over the course of the simulation to ensure that the error gets +\item{\underline{Error Bound Usage}: The error signals are checked for all of the model + parameters over the course of the simulation to ensure that the error gets to at least 80\% of its maximum error bound at least once, ensuring that noise is indeed properly introduced.} -\item{\underline{Corner Case Check}: The simulation is intentionally given bad inputs to +\item{\underline{Corner Case Check}: The simulation is intentionally given bad inputs to ensure that it alerts the user and does not crash.} \end{enumerate} @@ -47,7 +47,7 @@ \subsection{Test success criteria} \begin{tabular}{|c||c|c|c|c|c|c|} \hline Variable & Position & Velocity & Attitude & Rates & $\Delta$ V & Sun Position \\ \hline \hline -Associated $\sigma$ & 5 (m)& 0.035 (m/s)& $\frac{1}{360}$ (deg) & 0.05 (deg/s) & 1 (deg) & 0.1 (deg) \\ \hline +Associated $\sigma$ & 5 (m)& 0.035 (m/s)& $\frac{1}{360}$ (deg) & 0.05 (deg/s) & 1 (deg) & 0.1 (deg) \\ \hline \end{tabular} \end{table} @@ -58,7 +58,7 @@ \subsection{Test success criteria} \begin{tabular}{|c||c|c|c|c|c|c|} \hline Variable & Position & Velocity & Attitude & Rates & $\Delta$ V & Sun Position \\ \hline \hline -Associated bounds & 1000 (m)& 1 (m/s)& 0.29 (deg) & 1.15 (deg/s) & 5 (deg) & 3.03 (deg) \\ \hline +Associated bounds & 1000 (m)& 1 (m/s)& 0.29 (deg) & 1.15 (deg/s) & 5 (deg) & 3.03 (deg) \\ \hline \end{tabular} \end{table} @@ -81,20 +81,20 @@ \subsection{Pass/Fail} The test results are explained below and summarized in Table~\ref{tab:results}. \begin{enumerate} - \item{Error Bound Enforcement: We only want to violate the error bound a - statistically small number of times as most bounds are specified 3-sigma. + \item{Error Bound Enforcement: We only want to violate the error bound a + statistically small number of times as most bounds are specified 3-sigma. All signals remained inside their bounds more than 1-sigma (~70\%) of the time. } - \item{Error Bound Usage: As stated above, we want to ensure that the random - walk process is effectively utilizing the error bound that it has been - given and not remaining mired near zero. All error signals cross up above + \item{Error Bound Usage: As stated above, we want to ensure that the random + walk process is effectively utilizing the error bound that it has been + given and not remaining mired near zero. All error signals cross up above 80\% of their error bound at least once.} \end{enumerate} -\subsection{Corner case test} +\subsection{Corner case test} Corner Case Usage: All errors/warnings were stimulated and the simulation still ran without incident. The expected error message is not automatically validated within pytest, but the test can not pass -if the corner case test did break the simulation. In that way it is a partially automated test. +if the corner case test did break the simulation. In that way it is a partially automated test. \subsection{Summary of Test Results} @@ -118,14 +118,14 @@ \subsection{Summary of Test Results} \subsection{Test Coverage} -The method coverage for all of the methods included in the simple\_nav +The method coverage for all of the methods included in the simple\_nav module are tabulated in Tables~\ref{tab:cov_met} and \ref{tab:cov_met2}. \begin{table}[htbp] \caption{Simple Navigation Test Analysis Results} \label{tab:cov_met} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | r | r | r} % Column formatting, + \begin{tabular}{c | r | r | r} % Column formatting, \hline Method Name & Unit Test Coverage (\%) & Runtime Self (\%) & Runtime Children (\%) \\ \hline @@ -140,7 +140,7 @@ \subsection{Test Coverage} \caption{GaussMarkov Test Analysis Results} \label{tab:cov_met2} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | r | r | r} % Column formatting, + \begin{tabular}{c | r | r | r} % Column formatting, \hline Method Name & Unit Test Coverage (\%) & Runtime Self (\%) & Runtime Children (\%) \\ \hline @@ -154,20 +154,18 @@ \subsection{Test Coverage} \hline \end{tabular} \end{table} -For all of the code this test was designed for, the coverage percentage is -100\%. The CPU usage of the model is higher than would be ideal although this -might just be a symptom of the level of simplicity present in the overall -simulation. The majority of the computations are coming from two pieces of the -GaussMarkov code. - -The first is the random number generator. The model is using -one of the simplest random number generators in the standard template library. -That is still a relatively expensive operation as random numbers are costly and -we generate a new random number for each state. The second factor is in the -state and noise propagation. Those are being performed with a matrix -multiplication that is an $n^2$ operation. We could save some computations -here in the future if we took away the cross-correlation capability from some -of the states which would definitely be easy and accurate. It would just take +For all of the code this test was designed for, the coverage percentage is +100\%. The CPU usage of the model is higher than would be ideal although this +might just be a symptom of the level of simplicity present in the overall +simulation. The majority of the computations are coming from two pieces of the +GaussMarkov code. + +The first is the random number generator. The model is using +one of the simplest random number generators in the standard template library. +That is still a relatively expensive operation as random numbers are costly and +we generate a new random number for each state. The second factor is in the +state and noise propagation. Those are being performed with a matrix +multiplication that is an $n^2$ operation. We could save some computations +here in the future if we took away the cross-correlation capability from some +of the states which would definitely be easy and accurate. It would just take some more code. - - diff --git a/src/simulation/navigation/simpleNav/_Documentation/secUserGuide.tex b/src/simulation/navigation/simpleNav/_Documentation/secUserGuide.tex index ed42de20aa..eef3b35280 100644 --- a/src/simulation/navigation/simpleNav/_Documentation/secUserGuide.tex +++ b/src/simulation/navigation/simpleNav/_Documentation/secUserGuide.tex @@ -8,4 +8,4 @@ \section{User Guide} \item[-] \texttt{sNavObject.PMatrix = sim\_model.DoubleVector(pMatrix)}: Add the matrix of standard deviation values \item[-] \texttt{sNavObject.crossTrans = True} \item[-] \texttt{sNavObject.crossAtt = False} -\end{itemize} \ No newline at end of file +\end{itemize} diff --git a/src/simulation/onboardDataHandling/_GeneralModuleFiles/dataStorageUnitBase.rst b/src/simulation/onboardDataHandling/_GeneralModuleFiles/dataStorageUnitBase.rst index 036f8cbdbe..09d96e12b5 100644 --- a/src/simulation/onboardDataHandling/_GeneralModuleFiles/dataStorageUnitBase.rst +++ b/src/simulation/onboardDataHandling/_GeneralModuleFiles/dataStorageUnitBase.rst @@ -42,4 +42,4 @@ provides information on what this message is used for. User Guide ---------- - The user can connect to the output message using ``storageUnitDataOutMsg`` in Python. -- The input message (data nodes) are provided by calling the method ``addDataNodeToModel()`` \ No newline at end of file +- The input message (data nodes) are provided by calling the method ``addDataNodeToModel()`` diff --git a/src/simulation/onboardDataHandling/_doc.rst b/src/simulation/onboardDataHandling/_doc.rst index cd60596a01..d7ad5cd412 100644 --- a/src/simulation/onboardDataHandling/_doc.rst +++ b/src/simulation/onboardDataHandling/_doc.rst @@ -1 +1 @@ -This folder contains modules related to the generation and consumption of data onboard the spacecraft. Base classes are provided to related generate data nodes. Another base class is provided to create data storage devices such as a simple storage unit. \ No newline at end of file +This folder contains modules related to the generation and consumption of data onboard the spacecraft. Base classes are provided to related generate data nodes. Another base class is provided to create data storage devices such as a simple storage unit. diff --git a/src/simulation/onboardDataHandling/instrument/mappingInstrument/_UnitTest/test_mappingInstrument.py b/src/simulation/onboardDataHandling/instrument/mappingInstrument/_UnitTest/test_mappingInstrument.py index 3223b2a481..9ac887d2ee 100644 --- a/src/simulation/onboardDataHandling/instrument/mappingInstrument/_UnitTest/test_mappingInstrument.py +++ b/src/simulation/onboardDataHandling/instrument/mappingInstrument/_UnitTest/test_mappingInstrument.py @@ -1,12 +1,12 @@ -# +# # ISC License -# +# # Copyright (c) 2022, 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 @@ -14,8 +14,8 @@ # 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 @@ -111,5 +111,3 @@ def mappingInstrumentTestFunction(): if __name__ == "__main__": test_mappingInstrument() - - diff --git a/src/simulation/onboardDataHandling/instrument/mappingInstrument/mappingInstrument.rst b/src/simulation/onboardDataHandling/instrument/mappingInstrument/mappingInstrument.rst index 8a471241ef..32e6e247ca 100644 --- a/src/simulation/onboardDataHandling/instrument/mappingInstrument/mappingInstrument.rst +++ b/src/simulation/onboardDataHandling/instrument/mappingInstrument/mappingInstrument.rst @@ -11,9 +11,9 @@ are not accessible. Message Connection Descriptions ------------------------------- -The following table lists all the module input and output messages. -The module msg connection is set by the user from python. -The msg type contains a link to the message structure definition, while the description +The following table lists all the module input and output messages. +The module msg connection is set by the user from python. +The msg type contains a link to the message structure definition, while the description provides information on what this message is used for. .. list-table:: Module I/O Messages diff --git a/src/simulation/onboardDataHandling/storageUnit/simpleStorageUnit.h b/src/simulation/onboardDataHandling/storageUnit/simpleStorageUnit.h index c98056abc3..545b3856d5 100644 --- a/src/simulation/onboardDataHandling/storageUnit/simpleStorageUnit.h +++ b/src/simulation/onboardDataHandling/storageUnit/simpleStorageUnit.h @@ -39,5 +39,3 @@ class SimpleStorageUnit: public DataStorageUnitBase { }; #endif //BASILISK_SIMPLESTORAGEUNIT_H - - diff --git a/src/simulation/power/ReactionWheelPower/_Documentation/Images/PowerRW.svg b/src/simulation/power/ReactionWheelPower/_Documentation/Images/PowerRW.svg index 64b0a365e5..978cc41803 100644 --- a/src/simulation/power/ReactionWheelPower/_Documentation/Images/PowerRW.svg +++ b/src/simulation/power/ReactionWheelPower/_Documentation/Images/PowerRW.svg @@ -24,7 +24,7 @@ - Produced by OmniGraffle 7.12.1 + Produced by OmniGraffle 7.12.1 2020-01-23 17:09:40 +0000 diff --git a/src/simulation/power/_GeneralModuleFiles/powerNodeBase.h b/src/simulation/power/_GeneralModuleFiles/powerNodeBase.h index 29dff512aa..6d21403fbf 100644 --- a/src/simulation/power/_GeneralModuleFiles/powerNodeBase.h +++ b/src/simulation/power/_GeneralModuleFiles/powerNodeBase.h @@ -45,7 +45,7 @@ class PowerNodeBase: public SysModel { protected: void writeMessages(uint64_t CurrentClock); - bool readMessages(); + bool readMessages(); virtual void evaluatePowerModel(PowerNodeUsageMsgPayload *powerUsageMsg)=0; //!< Virtual void method used to compute module-wise power usage/generation. virtual void customReset(uint64_t CurrentClock); //!< Custom Reset method, similar to customSelfInit. virtual void customWriteMessages(uint64_t CurrentClock);//!< custom Write method, similar to customSelfInit. diff --git a/src/simulation/power/_GeneralModuleFiles/powerStorageBase.rst b/src/simulation/power/_GeneralModuleFiles/powerStorageBase.rst index e2037104dd..a600f533ee 100644 --- a/src/simulation/power/_GeneralModuleFiles/powerStorageBase.rst +++ b/src/simulation/power/_GeneralModuleFiles/powerStorageBase.rst @@ -43,4 +43,4 @@ User Guide - The base class behavior requires the initial energy storage to be specified through ``storedCharge_Init``. - The integration time step is evaluated as the time between module calls. - The user must set the output message name variable ``batPowerOutMsg`` -- The input message names are provided by calling the method ``addPowerNodeToModel(msg)`` \ No newline at end of file +- The input message names are provided by calling the method ``addPowerNodeToModel(msg)`` diff --git a/src/simulation/power/_doc.rst b/src/simulation/power/_doc.rst index 1a8bf24cad..7a9d921fea 100644 --- a/src/simulation/power/_doc.rst +++ b/src/simulation/power/_doc.rst @@ -1 +1 @@ -This folder contains modules related to the electrical power generation and consumption. Base classes are provided to related generate power nodes. Another base class is provided to create power storage devices such as a battery module. \ No newline at end of file +This folder contains modules related to the electrical power generation and consumption. Base classes are provided to related generate power nodes. Another base class is provided to create power storage devices such as a battery module. diff --git a/src/simulation/sensors/camera/camera.h b/src/simulation/sensors/camera/camera.h index 568a12f9e7..18545af1e1 100644 --- a/src/simulation/sensors/camera/camera.h +++ b/src/simulation/sensors/camera/camera.h @@ -41,7 +41,7 @@ class Camera: public SysModel { public: Camera(); ~Camera(); - + void UpdateState(uint64_t currentSimNanos) override; void Reset(uint64_t currentSimNanos) override; void hsvAdjust(const cv::Mat&, cv::Mat &mDst); @@ -60,7 +60,7 @@ class Camera: public SysModel { std::string saveDir{}; //!< The name of the directory to save images uint64_t sensorTimeTag{}; //!< [ns] Current time tag for sensor out int32_t saveImages{}; //!< [-] 1 to save images to file for debugging - + /*! Camera parameters */ char parentName[MAX_STRING_LENGTH]{}; //!< [-] Name of the parent body to which the camera should be attached int cameraIsOn{}; //!< [-] Is the camera currently taking images diff --git a/src/simulation/sensors/camera/camera.rst b/src/simulation/sensors/camera/camera.rst index ced7751362..6f67bb03a8 100644 --- a/src/simulation/sensors/camera/camera.rst +++ b/src/simulation/sensors/camera/camera.rst @@ -5,7 +5,7 @@ codebase. Although images are provided by the visualization, they are renders of the Unity engine and are not necessarily representative of a camera. The module reads in an image from a file, or in the simulation as a pointer to image data, then corrupts it according to -input parameters. +input parameters. Module Assumptions and Limitations ---------------------------------- diff --git a/src/simulation/sensors/coarseSunSensor/_Documentation/AVS.sty b/src/simulation/sensors/coarseSunSensor/_Documentation/AVS.sty index a57e094317..f2f1a14acb 100644 --- a/src/simulation/sensors/coarseSunSensor/_Documentation/AVS.sty +++ b/src/simulation/sensors/coarseSunSensor/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red \definecolor{colorPA}{rgb}{1,0,1} % Bright purple @@ -94,5 +94,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/sensors/coarseSunSensor/_Documentation/Basilisk-CoarseSunSensor-20170803.tex b/src/simulation/sensors/coarseSunSensor/_Documentation/Basilisk-CoarseSunSensor-20170803.tex index d924d1702c..a16fe5d950 100755 --- a/src/simulation/sensors/coarseSunSensor/_Documentation/Basilisk-CoarseSunSensor-20170803.tex +++ b/src/simulation/sensors/coarseSunSensor/_Documentation/Basilisk-CoarseSunSensor-20170803.tex @@ -23,7 +23,7 @@ %In order to import some of these sections into a document in a different directory: %\usepackage{import} -%Then, the sections are called with \subimport{relative path}{file} in order to \input{file} using the right relative path. +%Then, the sections are called with \subimport{relative path}{file} in order to \input{file} using the right relative path. %\import{full path}{file} can also be used if absolute paths are preferred over relative paths. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -105,7 +105,7 @@ - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/sensors/coarseSunSensor/_Documentation/BasiliskReportMemo.cls b/src/simulation/sensors/coarseSunSensor/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/sensors/coarseSunSensor/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/sensors/coarseSunSensor/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/sensors/coarseSunSensor/_Documentation/Support/notes.nb b/src/simulation/sensors/coarseSunSensor/_Documentation/Support/notes.nb index 33bc516ff5..fa1b1efaa2 100644 --- a/src/simulation/sensors/coarseSunSensor/_Documentation/Support/notes.nb +++ b/src/simulation/sensors/coarseSunSensor/_Documentation/Support/notes.nb @@ -21,38 +21,38 @@ Notebook[{ Cell[CellGroupData[{ Cell[BoxData[ - RowBox[{"Plot", "[", + RowBox[{"Plot", "[", RowBox[{ - RowBox[{"{", + RowBox[{"{", RowBox[{ - RowBox[{"Cos", "[", - RowBox[{"x", " ", "Degree"}], "]"}], "\[IndentingNewLine]", ",", + RowBox[{"Cos", "[", + RowBox[{"x", " ", "Degree"}], "]"}], "\[IndentingNewLine]", ",", RowBox[{ - RowBox[{"Cos", "[", - RowBox[{"x", " ", "Degree"}], "]"}], - RowBox[{"(", - RowBox[{"1", "-", - RowBox[{"Exp", "[", + RowBox[{"Cos", "[", + RowBox[{"x", " ", "Degree"}], "]"}], + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"Exp", "[", RowBox[{ - RowBox[{"-", + RowBox[{"-", RowBox[{ - RowBox[{"Cos", "[", - RowBox[{"x", " ", "Degree"}], "]"}], "^", "2"}]}], "/", ".1"}], - "]"}]}], ")"}]}]}], "\[IndentingNewLine]", "}"}], ",", - RowBox[{"{", - RowBox[{"x", ",", " ", - RowBox[{"-", "90"}], ",", "90"}], "}"}], "\[IndentingNewLine]", ",", " ", - RowBox[{"AxesLabel", "\[Rule]", - RowBox[{"{", - RowBox[{"\"\<\[Phi]\>\"", ",", "\"\\""}], "}"}]}]}], + RowBox[{"Cos", "[", + RowBox[{"x", " ", "Degree"}], "]"}], "^", "2"}]}], "/", ".1"}], + "]"}]}], ")"}]}]}], "\[IndentingNewLine]", "}"}], ",", + RowBox[{"{", + RowBox[{"x", ",", " ", + RowBox[{"-", "90"}], ",", "90"}], "}"}], "\[IndentingNewLine]", ",", " ", + RowBox[{"AxesLabel", "\[Rule]", + RowBox[{"{", + RowBox[{"\"\<\[Phi]\>\"", ",", "\"\\""}], "}"}]}]}], "]"}]], "Input", CellChangeTimes->{{3.708348070275147*^9, 3.708348140257758*^9}, { - 3.708348319117258*^9, 3.7083483211430073`*^9}, {3.708349257935976*^9, + 3.708348319117258*^9, 3.7083483211430073`*^9}, {3.708349257935976*^9, 3.7083492996618347`*^9}},ExpressionUUID->"8c8a8fe2-4258-46b1-8049-\ 3fdc92e87df8"], Cell[BoxData[ - GraphicsBox[{{{}, {}, + GraphicsBox[{{{}, {}, TagBox[ {RGBColor[0.368417, 0.506779, 0.709798], AbsoluteThickness[1.6], Opacity[ 1.], LineBox[CompressedData[" @@ -146,7 +146,7 @@ T9Ffm0cj4Wj6QpeNDx397hRmFkedAgl7mqlOIFmva5rmuZxjcGRt3SGFIDpa 6d9j3BjlB5V/9iWJh9FREOSKJOfsBfHGxUq+cDqa+Pta40WOE/zvfAD9/3zg P4GvN7Y= "]]}, - Annotation[#, "Charting`Private`Tag$5978#1"]& ], + Annotation[#, "Charting`Private`Tag$5978#1"]& ], TagBox[ {RGBColor[0.880722, 0.611041, 0.142051], AbsoluteThickness[1.6], Opacity[ 1.], LineBox[CompressedData[" @@ -313,42 +313,42 @@ SA== AspectRatio->NCache[GoldenRatio^(-1), 0.6180339887498948], Axes->{True, True}, AxesLabel->{ - FormBox["\"\[Phi]\"", TraditionalForm], + FormBox["\"\[Phi]\"", TraditionalForm], FormBox["\"Signal\"", TraditionalForm]}, AxesOrigin->{0, 0}, DisplayFunction->Identity, Frame->{{False, False}, {False, False}}, FrameLabel->{{None, None}, {None, None}}, - FrameTicks->{{Automatic, - Charting`ScaledFrameTicks[{Identity, Identity}]}, {Automatic, + FrameTicks->{{Automatic, + Charting`ScaledFrameTicks[{Identity, Identity}]}, {Automatic, Charting`ScaledFrameTicks[{Identity, Identity}]}}, GridLines->{None, None}, GridLinesStyle->Directive[ GrayLevel[0.5, 0.4]], ImagePadding->All, Method->{ - "DefaultBoundaryStyle" -> Automatic, "DefaultMeshStyle" -> - AbsolutePointSize[6], "ScalingFunctions" -> None, + "DefaultBoundaryStyle" -> Automatic, "DefaultMeshStyle" -> + AbsolutePointSize[6], "ScalingFunctions" -> None, "CoordinatesToolOptions" -> {"DisplayFunction" -> ({ (Identity[#]& )[ - Part[#, 1]], + Part[#, 1]], (Identity[#]& )[ Part[#, 2]]}& ), "CopiedValueFunction" -> ({ (Identity[#]& )[ - Part[#, 1]], + Part[#, 1]], (Identity[#]& )[ Part[#, 2]]}& )}}, PlotRange->{{-90, 90}, {0., 0.9999999707682925}}, PlotRangeClipping->True, PlotRangePadding->{{ - Scaled[0.02], + Scaled[0.02], Scaled[0.02]}, { - Scaled[0.05], + Scaled[0.05], Scaled[0.05]}}, Ticks->{Automatic, Automatic}]], "Output", CellChangeTimes->{ 3.708348087141313*^9, 3.7083481409081507`*^9, 3.708348327563547*^9, { - 3.70834927689005*^9, + 3.70834927689005*^9, 3.708349300445147*^9}},ExpressionUUID->"d7089512-c637-488a-a128-\ 88384cb35932"] }, Open ]] @@ -381,4 +381,3 @@ Cell[1648, 53, 16826, 299, 253, "Output", "ExpressionUUID" -> \ *) (* End of internal cache information *) - diff --git a/src/simulation/sensors/coarseSunSensor/_Documentation/bibliography.bib b/src/simulation/sensors/coarseSunSensor/_Documentation/bibliography.bib index 3d8df08944..3603ad3eb0 100755 --- a/src/simulation/sensors/coarseSunSensor/_Documentation/bibliography.bib +++ b/src/simulation/sensors/coarseSunSensor/_Documentation/bibliography.bib @@ -1,26 +1,26 @@ -@article{pines1973, -auTHor = "Samuel Pines", -Title = {Uniform Representation of the Gravitational Potential and its derivatives}, +@article{pines1973, +auTHor = "Samuel Pines", +Title = {Uniform Representation of the Gravitational Potential and its derivatives}, journal = "AIAA Journal", volume={11}, number={11}, pages={1508-1511}, -YEAR = 1973, -} +YEAR = 1973, +} -@article{lundberg1988, -auTHor = "Lundberg, J. and Schutz, B.", -Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, +@article{lundberg1988, +auTHor = "Lundberg, J. and Schutz, B.", +Title = {Recursion Formulas of Legendre Functions for Use with Nonsingular Geopotential Models}, journal = "Journal of Guidance AIAA", volume={11}, number={1}, pages={31-38}, -YEAR = 1988, -} +YEAR = 1988, +} @book{vallado2013, - author = {David Vallado}, + author = {David Vallado}, title = {Fundamentals of Astrodynamics and Applications}, publisher = {Microcosm press}, year = {2013}, @@ -28,7 +28,7 @@ @book{vallado2013 } @book{scheeres2012, - author = {Daniel Scheeres}, + author = {Daniel Scheeres}, title = {Orbital Motion in Strongly Perturbed Environments}, publisher = {Springer}, year = {2012}, diff --git a/src/simulation/sensors/coarseSunSensor/_Documentation/secModelDescription.tex b/src/simulation/sensors/coarseSunSensor/_Documentation/secModelDescription.tex index be74adbf3a..3a7b931847 100644 --- a/src/simulation/sensors/coarseSunSensor/_Documentation/secModelDescription.tex +++ b/src/simulation/sensors/coarseSunSensor/_Documentation/secModelDescription.tex @@ -18,7 +18,7 @@ \subsection{Single CSS module} \subsubsection{I/O Messages} First, let us discuss the input and output messages of the individual CSS sensor module. The two required input messages are of the type {\tt SpicePlanetStateMsgPayload} and {\tt SCStatesMsgPayload}. The first message is used to determine the sun's location relative to the inertial frame $\leftexp{N}{\bm r}_{\sun/\mathcal{N}}$. The second message is used to read in the spacecraft inertial position vector relative to the same inertial frame $\leftexp{N}{\bm r}_{\cal B/N}$. Finally, the last message is optional. If it is connected, it provides the sun shadow parameter indicating if the spacecraft is in a planet's shadow. -One output message of an individual CSS unit creates a message containing the simulated CSS sensor. The second output message is of type {\tt CSSConfigLogMsgPayload} that logs the CSS configuration states. +One output message of an individual CSS unit creates a message containing the simulated CSS sensor. The second output message is of type {\tt CSSConfigLogMsgPayload} that logs the CSS configuration states. Note, if using a CSS constellation class of CSS devices then the CSS message connections must be made before copying them into the CSS device vector in the constellation class. @@ -37,10 +37,10 @@ \subsubsection{CSS Signal Simulation} \begin{equation} \hat \gamma = \hat{\bm n} \cdot \hat{\bm s} = \cos\phi \end{equation} -where $\phi$ is the CSS sunlight incidence angle. -This is the normalized CSS signal where 1 is returned if the sensor is looking straight at the sun. If the sensor axis $\hat{\bm n}$ is more than the field of view half-angle (set through {\tt fov}) from the sun axis, then a 0 signal is simulated. This {\tt fov} variable is the angle from $\hat{\bm n}$ beyond which the CSS signal is set to zero. +where $\phi$ is the CSS sunlight incidence angle. +This is the normalized CSS signal where 1 is returned if the sensor is looking straight at the sun. If the sensor axis $\hat{\bm n}$ is more than the field of view half-angle (set through {\tt fov}) from the sun axis, then a 0 signal is simulated. This {\tt fov} variable is the angle from $\hat{\bm n}$ beyond which the CSS signal is set to zero. -The Kelly parameter allows for the CSS signal to pinch towards zero for larger incidence angles as illustrated in Figure~\ref{fig:kelly}. The amount of signal distortion is set through $\kappa$, where the Kelly factor $p_{\kappa}$ is then computed as +The Kelly parameter allows for the CSS signal to pinch towards zero for larger incidence angles as illustrated in Figure~\ref{fig:kelly}. The amount of signal distortion is set through $\kappa$, where the Kelly factor $p_{\kappa}$ is then computed as \begin{equation} f_{\kappa} = 1 - e^{-\hat\gamma^{2}/\kappa} \end{equation} @@ -62,9 +62,9 @@ \subsubsection{CSS Signal Simulation} \end{equation} and the shadow factor due to eclipse is given as $f_s$. So the output curve adjusted for light intensity is: \begin{equation} - \gamma_{\textrm{li}} = \gamma_{\kappa} f_{\mathrm{sunDist}} f_s + \gamma_{\textrm{li}} = \gamma_{\kappa} f_{\mathrm{sunDist}} f_s \end{equation} -If the spacecraft is outside of a planet's shadow, $f_s$ is 1. If it is within the shadow, then it is $0\le f_{s} < 1$. +If the spacecraft is outside of a planet's shadow, $f_s$ is 1. If it is within the shadow, then it is $0\le f_{s} < 1$. This curve has now accounted for light intensity, but not for the magnitude or units of the sensor electronics output. To do that, a scale factor, $f_{\mathrm{scale}}$ is factored into the equation: \begin{equation} \gamma_{\mathrm{clean}} = f_{\mathrm{scale}} \gamma_{\textrm{li}} @@ -74,7 +74,7 @@ \subsubsection{CSS Signal Simulation} Next, Gaussian noise and sensor can be added to the signal The normalized bias is set through {\tt SenBias}, while the normalized noise is set through {\tt SenNoiseStd}. Let $ n$ be the normalized sensor noise and $b$ be the normalized sensor bias. Note that these values are non-dimensional and apply to the unscaled output. Therefore, they will also be scaled when a scale factor is applied to the output. The noisy and appropriately scaled output of the sensor is: \begin{equation} - \gamma_{\mathrm{noise}} = (\gamma_{\textrm{li}} + n + b) f_{\mathrm{scale}} + \gamma_{\mathrm{noise}} = (\gamma_{\textrm{li}} + n + b) f_{\mathrm{scale}} \end{equation} This indicates that sensor noise is not a function of light incidence angle or light intensity. Now, the noise or bias could have caused the sensor to produce a value less than 0 or higher than is possible for the hardware. To prevent this, saturation values are input as and treated as max/min values: @@ -97,7 +97,4 @@ \subsubsection{CSS Signal Simulation} % \label{fig:moduleDiagramConstellation} %\end{figure} \subsection{Constellation or Array of CSS Modules} -The {\tt CSSConstellation} class is defined as a convenience such that the Basilisk update function is called on a group of CSS modules. Here the CSS BSK modules are created and configured as before, but are then provided to the {\tt CSSConstellation} class object as a list of CSS in python. The CSS can be configured individually, and must each be connected with the required input messages. However, the {\tt CSSConstellation} will gather all the CSS outputs and write a single CSS Constellation output message of the type {\tt CSSArraySensorMsgPayload}. - - - +The {\tt CSSConstellation} class is defined as a convenience such that the Basilisk update function is called on a group of CSS modules. Here the CSS BSK modules are created and configured as before, but are then provided to the {\tt CSSConstellation} class object as a list of CSS in python. The CSS can be configured individually, and must each be connected with the required input messages. However, the {\tt CSSConstellation} will gather all the CSS outputs and write a single CSS Constellation output message of the type {\tt CSSArraySensorMsgPayload}. diff --git a/src/simulation/sensors/coarseSunSensor/_Documentation/secModelFunctions.tex b/src/simulation/sensors/coarseSunSensor/_Documentation/secModelFunctions.tex index 82f6efafd5..784ddee2c8 100644 --- a/src/simulation/sensors/coarseSunSensor/_Documentation/secModelFunctions.tex +++ b/src/simulation/sensors/coarseSunSensor/_Documentation/secModelFunctions.tex @@ -23,4 +23,4 @@ \section{Model Assumptions and Limitations} \item \textbf{Conical Symmetry}: The module assumes that sensor readings and bias are the same in every direction from normal. In reality, there may be different biases or errors depending on which direction the sun vector is from the sensor normal vector. This limits the use of this module to sensors that are symmetric or only slightly asymmetric. \item \textbf{Albedo}: Currently, the model is hard-coded to zero-out any albedo input, so it is limited to cases where albedo is not important. \item \textbf{Cosine Behavior}: The model assumes the sensor to have a nominal cosine behavior. This is distorted by the Kelly factor. To improve behavior closer to reality where housing and spacecraft glint might cause slightly non-conical non-cosine behaviors, a look-up table of sensor outputs compared to sun headings could be implemented in future work. -\end{enumerate} \ No newline at end of file +\end{enumerate} diff --git a/src/simulation/sensors/coarseSunSensor/_Documentation/secTest.tex b/src/simulation/sensors/coarseSunSensor/_Documentation/secTest.tex index 79ad07dde1..b58dd20578 100644 --- a/src/simulation/sensors/coarseSunSensor/_Documentation/secTest.tex +++ b/src/simulation/sensors/coarseSunSensor/_Documentation/secTest.tex @@ -39,9 +39,9 @@ \section{Test Parameters} \caption{Parameters for each test. Note that relative tolerance is $\frac{truth - output}{truth}$} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c | c | c | c | c | c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c | c | c | c | c | c | c | c | c } % Column formatting, \hline\hline - \rot{\textbf{Test}}& \rot{\textbf{useConstellation}}& \rot{\textbf{visibilityFactor}}& \rot{\textbf{fov}}& \rot{\textbf{kelly}}& \rot{\textbf{scaleFactor}}& \rot{\textbf{bias}}& \rot{\textbf{noiseStd}}& \rot{\textbf{albedoValue}}& \rot{\textbf{saturation}}&\rot{\textbf{errTol}}&\rot{\textbf{sunDistInput}}\\ + \rot{\textbf{Test}}& \rot{\textbf{useConstellation}}& \rot{\textbf{visibilityFactor}}& \rot{\textbf{fov}}& \rot{\textbf{kelly}}& \rot{\textbf{scaleFactor}}& \rot{\textbf{bias}}& \rot{\textbf{noiseStd}}& \rot{\textbf{albedoValue}}& \rot{\textbf{saturation}}&\rot{\textbf{errTol}}&\rot{\textbf{sunDistInput}}\\ \hline\hline 1 & \input{AutoTex/plainUseConstellation}&\input{AutoTex/plainVisibilityFactor}&\input{AutoTex/plainFov}&\input{AutoTex/plainKelly}&\input{AutoTex/plainScaleFactor}&\input{AutoTex/plainBias}&\input{AutoTex/plainNoiseStd}&\input{AutoTex/plainAlbedoValue}&\input{AutoTex/plainMaxSaturation},\input{AutoTex/plainMinSaturation}&\input{AutoTex/plainErrTol}&\input{AutoTex/plainLocation} \\ \hline 2 & \input{AutoTex/eclipseUseConstellation}&\input{AutoTex/eclipseVisibilityFactor}&\input{AutoTex/eclipseFov}&\input{AutoTex/eclipseKelly}&\input{AutoTex/eclipseScaleFactor}&\input{AutoTex/eclipseBias}&\input{AutoTex/eclipseNoiseStd}&\input{AutoTex/eclipseAlbedoValue}&\input{AutoTex/eclipseMaxSaturation},\input{AutoTex/eclipseMinSaturation}&\input{AutoTex/eclipseErrTol}&\input{AutoTex/eclipseLocation} \\ \hline @@ -68,7 +68,7 @@ \section{Test Results} \caption{Test results.} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c } % Column formatting, + \begin{tabular}{ c | c | c } % Column formatting, \hline \textbf{Test} & \textbf{Pass/Fail} & \textbf{Notes} \\ \hline 1 &\input{AutoTex/plainPassedText} &\input{AutoTex/plainPassFailMsg} \\ \hline @@ -84,7 +84,7 @@ \section{Test Results} 11 &\input{AutoTex/cleanCombinedPassedText} &\input{AutoTex/cleanCombinedPassFailMsg} \\ \hline 12 &\input{AutoTex/combinedPassedText} &\input{AutoTex/combinedPassFailMsg} \\ \hline 13 &\input{AutoTex/constellationPassedText} &\input{AutoTex/constellationPassFailMsg} \\ \hline - + \end{tabular} \end{table} diff --git a/src/simulation/sensors/coarseSunSensor/_Documentation/secUserGuide.tex b/src/simulation/sensors/coarseSunSensor/_Documentation/secUserGuide.tex index 04ab145822..8fe174cea0 100644 --- a/src/simulation/sensors/coarseSunSensor/_Documentation/secUserGuide.tex +++ b/src/simulation/sensors/coarseSunSensor/_Documentation/secUserGuide.tex @@ -8,27 +8,27 @@ \subsubsection{Direct Method} \begin{equation} {\tt nHat\_B} \equiv \leftexp{B}{\hat{\bm n}} \end{equation} -It is possible to set these vectors directly. However, there are some convenience functions that make this process easier. +It is possible to set these vectors directly. However, there are some convenience functions that make this process easier. \subsubsection{Via a Common Sensor Platform} Multiple CSS devices are often arranged together on a single CSS platform. The orientation of the body-fixed platform frame $\frameDefinition{P}$ relative to the body frame $\frameDefinition{B}$ is given through $[PB]$. In the CSS module, the DCM is specified through the variable {\tt dcm\_PB}. Assume the DCM {\tt dcm\_PB} is set directly via Python. Two angles then define the orientation of the sensor normal axis $\hat{\bm n}$. The elevation angle is $\phi$ and the azimuth angle is $\theta$. These are an Euler angle sequence with the order 3-(-2). The elevation angle is a positive 3-axis rotation, while the azimuth is a minus 2-axis rotation. The helper function $$ -{\tt setUnitDirectionVectorWithPerturbation}(\theta_{p}, \phi_{p}) +{\tt setUnitDirectionVectorWithPerturbation}(\theta_{p}, \phi_{p}) $$ where $\theta_{p}$ and $\phi_{p}$ are CSS heading perturbation can can be specified. The Euler angles implemented are then \begin{align} \phi_{\text{actual}} &= \phi_{\text{true}} + \phi_{p} \\ - \theta_{\text{actual}} &= \theta_{\text{true}} + \theta_{p} + \theta_{\text{actual}} &= \theta_{\text{true}} + \theta_{p} \end{align} -To setup un-perturbed CSS sensor axes simple set these perturbation angles to zero. +To setup un-perturbed CSS sensor axes simple set these perturbation angles to zero. Instead of setting the DCM {\tt dcm\_PB} variable directly, this can also be set via the helper function $$ {\tt setBodyToPlatformDCM}(\psi, \theta, \phi) $$ -where $(\psi, \theta, \phi)$ are classical $3-2-1$ Euler angles that map from the body frame $\cal B$ to the platform frame $\cal P$. +where $(\psi, \theta, \phi)$ are classical $3-2-1$ Euler angles that map from the body frame $\cal B$ to the platform frame $\cal P$. @@ -38,7 +38,7 @@ \subsection{CSS Field-of-View} $$ {\tt fov} $$ -This angle is the angle from the bore-sight axis to the edge of the field of view, and is expressed in terms of radians. +This angle is the angle from the bore-sight axis to the edge of the field of view, and is expressed in terms of radians. It defaults to a value of 60 degrees. @@ -50,14 +50,14 @@ \subsection{CSS Output Scale} $$ {\tt scaleFactor} $$ -is the scale factor $\alpha$ which scales the output to the desired range of values, as well as the desired units. For example, if the maximum sun signal ($\hat{\bm n}$ points directly at sun) should yield 1 mA, then the scale factor is set to this value. +is the scale factor $\alpha$ which scales the output to the desired range of values, as well as the desired units. For example, if the maximum sun signal ($\hat{\bm n}$ points directly at sun) should yield 1 mA, then the scale factor is set to this value. If not set by the user, this parameter has a default value of 1.0. \subsection{Specifying CSS Sensor Noise} -Three types of CSS signal corruptions can be simulated. If not specified, all these corruptions are zeroed. +Three types of CSS signal corruptions can be simulated. If not specified, all these corruptions are zeroed. The Kelly corruption parameter $\kappa$ is set by specifying the variable $$ @@ -68,13 +68,13 @@ \subsection{Specifying CSS Sensor Noise} $$ {\tt SenNoiseStd} $$ -is set to a non-zero value. This is the standard deviation of normalized gaussian noise. Note that this noise magnitude is in terms of normalized units as it is added to the 0-1 nominal signal. +is set to a non-zero value. This is the standard deviation of normalized gaussian noise. Note that this noise magnitude is in terms of normalized units as it is added to the 0-1 nominal signal. Next, to simulate a signal bias, the variable $$ {\tt SenBias} $$ -is set to a non-zero value. This constant bias of the normalized gaussian noise. +is set to a non-zero value. This constant bias of the normalized gaussian noise. Finally, to set saturation values, the variables $$ @@ -86,12 +86,12 @@ \subsection{Specifying CSS Sensor Noise} are used. minOutput is 0 by default and maxOutput is 1,000,000 by default. \subsection{Connecting Messages} -If the {\tt EclipseSimMsg} is connected, then the solar eclipse information is taking into account. The eclipse info provides 0 if the spacecraft is fully in a planet's shadow, 1 if in free space fully exposed to the, and a value between (0,1) if in the penumbra region. The cosine sensor value $\hat\gamma$ is scaled by this eclipse value. If the message is not connected, then this value default to 1, thus simulating a spacecraft that is fully exposed to the sun. +If the {\tt EclipseSimMsg} is connected, then the solar eclipse information is taking into account. The eclipse info provides 0 if the spacecraft is fully in a planet's shadow, 1 if in free space fully exposed to the, and a value between (0,1) if in the penumbra region. The cosine sensor value $\hat\gamma$ is scaled by this eclipse value. If the message is not connected, then this value default to 1, thus simulating a spacecraft that is fully exposed to the sun. If the optional albedo message is connected, then its information is included in the CSS sensor data evaluation. \subsection{Auxiliary Parameters} -The following module variables can be set, but they are not currently used by the CSS module itself. These are used however in the Vizard visualization. +The following module variables can be set, but they are not currently used by the CSS module itself. These are used however in the Vizard visualization. \begin{itemize} \item {\tt r\_B} -- position vector of the sensor relative to the body frame $\cal B$. If not set, this defaults to a zero vector. \item {\tt CSSGroupID} -- positive integer ID number if the CSS unit belongs to a group of CSS units. If not set, this defaults to group 0. @@ -100,14 +100,13 @@ \subsection{Auxiliary Parameters} \subsection{Setting Up CSS Modules} \subsubsection{Individual CSS Units} -It is possible to add Individual CSS units to the Basilisk simulation. This is done by invoking instances of the {\tt CoarseSunSensor()} class from python, configuring the required states, and then adding each to the BSK evaluation stack. Each {\tt CoarseSunSensor} class has it's own {\tt UpdateState()} method that get's evaluated each update period. +It is possible to add Individual CSS units to the Basilisk simulation. This is done by invoking instances of the {\tt CoarseSunSensor()} class from python, configuring the required states, and then adding each to the BSK evaluation stack. Each {\tt CoarseSunSensor} class has it's own {\tt UpdateState()} method that get's evaluated each update period. This setup is convenient if only 1-2 CSS units have to be modeled, but can be cumbersome if a larger cluster of CSS units must be administered. When setup this way, each CSS unit will output an individual CSS output message. \subsubsection{Array or Constellation of CSS Units} -An alternate method to setup a series of CSS units is to use the {\tt CSSConstellation()} class. This class is able to store a series of CSS {\tt CoarseSunSensor} objects, and engage the update routine on all of them at once. This way only the {\tt CSSConstellation} module needs to be added to the BSK update stack. In this method the +An alternate method to setup a series of CSS units is to use the {\tt CSSConstellation()} class. This class is able to store a series of CSS {\tt CoarseSunSensor} objects, and engage the update routine on all of them at once. This way only the {\tt CSSConstellation} module needs to be added to the BSK update stack. In this method the {\tt CSSConstellation} module outputs a single CSS sensor message which contains an array of doubles with the CSS sensor signal. Here the individual CSS units {\tt CSS1}, {\tt CSS2}, etc. are setup and configured first. Next, they are added to the {\tt CSSConstellation} through the python command $$ {\tt cssConstellation.sensorList = coarse\_sun\_sensor.CSSVector([CSS1, CSS2, ..., CSS8])} $$ - diff --git a/src/simulation/sensors/coarseSunSensor/_UnitTest/test_CSSConfig.py b/src/simulation/sensors/coarseSunSensor/_UnitTest/test_CSSConfig.py index 9a4821d86e..091e58f6c5 100644 --- a/src/simulation/sensors/coarseSunSensor/_UnitTest/test_CSSConfig.py +++ b/src/simulation/sensors/coarseSunSensor/_UnitTest/test_CSSConfig.py @@ -187,4 +187,3 @@ def run(show_plots, accuracy): False, # show_plots 1e-12 # accuracy ) - diff --git a/src/simulation/sensors/hingedRigidBodyMotorSensor/_UnitTest/test_hingedRigidBodyMotorSensor.py b/src/simulation/sensors/hingedRigidBodyMotorSensor/_UnitTest/test_hingedRigidBodyMotorSensor.py index 441548516e..54e15f1a9f 100644 --- a/src/simulation/sensors/hingedRigidBodyMotorSensor/_UnitTest/test_hingedRigidBodyMotorSensor.py +++ b/src/simulation/sensors/hingedRigidBodyMotorSensor/_UnitTest/test_hingedRigidBodyMotorSensor.py @@ -1,12 +1,12 @@ -# +# # ISC License -# +# # Copyright (c) 2022, 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 @@ -14,8 +14,8 @@ # 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 matplotlib.pyplot as plt import numpy as np @@ -52,7 +52,7 @@ def test_hingedRigidBodyMotorSensor(show_plots, thetaNoiseStd, thetaDotNoiseStd, accuracy (double): absolute accuracy value used in the validation tests **Description of Variables Being Tested** - + The python evaluated sensed value is compared against the module output. """ @@ -66,7 +66,7 @@ def hingedRigidBodyMotorSensorTestFunction(show_plots, thetaNoiseStd, thetaDotNo testMessages = [] unitTaskName = "unitTask" unitProcessName = "TestProcess" - + timeStep = 0.5 totalTime = 10.0 @@ -82,7 +82,7 @@ def hingedRigidBodyMotorSensorTestFunction(show_plots, thetaNoiseStd, thetaDotNo # Configure blank module input messages hingedRigidBodyMotorSensorInMsgData = messaging.HingedRigidBodyMsgPayload() - + # set up fake input message hingedRigidBodyMotorSensorInMsgData.theta = trueTheta; hingedRigidBodyMotorSensorInMsgData.thetaDot = trueThetaDot; @@ -95,7 +95,7 @@ def hingedRigidBodyMotorSensorTestFunction(show_plots, thetaNoiseStd, thetaDotNo # set up output message recorder objects dataLog = module.hingedRigidBodyMotorSensorOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) - + # set up variables in sensor module.thetaNoiseStd = thetaNoiseStd module.thetaDotNoiseStd = thetaDotNoiseStd @@ -113,11 +113,11 @@ def hingedRigidBodyMotorSensorTestFunction(show_plots, thetaNoiseStd, thetaDotNo # pull module data and make sure it is correct sensedTheta = dataLog.theta[-1] sensedThetaDot = dataLog.thetaDot[-1] - + # add bias to test values biasTheta = trueTheta+thetaBias biasThetaDot = trueThetaDot+thetaDotBias - + # discretize test values if thetaLSB > 0: discTheta = round(biasTheta/thetaLSB)*thetaLSB @@ -127,7 +127,7 @@ def hingedRigidBodyMotorSensorTestFunction(show_plots, thetaNoiseStd, thetaDotNo discThetaDot = round(biasThetaDot/thetaDotLSB)*thetaDotLSB else: discThetaDot = biasThetaDot - + print(sensedTheta) print(sensedThetaDot) print(trueTheta) @@ -141,7 +141,7 @@ def hingedRigidBodyMotorSensorTestFunction(show_plots, thetaNoiseStd, thetaDotNo if not unitTestSupport.isDoubleEqual(sensedThetaDot, discThetaDot, accuracy): testMessages.append("Failed thetaDot bias.") testFailCount += 1 - + # check discretization if abs(thetaLSB) > accuracy: if not unitTestSupport.isDoubleEqual(sensedTheta, discTheta, accuracy): @@ -155,7 +155,7 @@ def hingedRigidBodyMotorSensorTestFunction(show_plots, thetaNoiseStd, thetaDotNo print("PASSED: " + module.ModelTag) else: print(testMessages) - + if show_plots: thetaVals = trueTheta*np.ones(int(totalTime/timeStep)+1) thetaDotVals = trueThetaDot*np.ones(int(totalTime/timeStep)+1) @@ -181,7 +181,7 @@ def hingedRigidBodyMotorSensorTestFunction(show_plots, thetaNoiseStd, thetaDotNo plt.close("all") return [testFailCount, "".join(testMessages)] - + if __name__ == "__main__": test_hingedRigidBodyMotorSensor( diff --git a/src/simulation/sensors/hingedRigidBodyMotorSensor/hingedRigidBodyMotorSensor.h b/src/simulation/sensors/hingedRigidBodyMotorSensor/hingedRigidBodyMotorSensor.h index c9651af4f9..0bd50696a2 100644 --- a/src/simulation/sensors/hingedRigidBodyMotorSensor/hingedRigidBodyMotorSensor.h +++ b/src/simulation/sensors/hingedRigidBodyMotorSensor/hingedRigidBodyMotorSensor.h @@ -37,7 +37,7 @@ class HingedRigidBodyMotorSensor: public SysModel { void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); - + void setRNGSeed(unsigned int newSeed); //!< for setting the seed public: @@ -48,7 +48,7 @@ class HingedRigidBodyMotorSensor: public SysModel { double thetaDotBias; //!< [rad/s] bias added to true theta dot double thetaLSB; //!< [rad] discretization for theta double thetaDotLSB; //!< [rad/s] discretization for theta dot - + ReadFunctor hingedRigidBodyMotorSensorInMsg; //!< input message for true rigid body state (theta, theta dot) Message hingedRigidBodyMotorSensorOutMsg; //!< output message for sensed rigid body state diff --git a/src/simulation/sensors/hingedRigidBodyMotorSensor/hingedRigidBodyMotorSensor.rst b/src/simulation/sensors/hingedRigidBodyMotorSensor/hingedRigidBodyMotorSensor.rst index 5f04fcc40c..23fe59a395 100644 --- a/src/simulation/sensors/hingedRigidBodyMotorSensor/hingedRigidBodyMotorSensor.rst +++ b/src/simulation/sensors/hingedRigidBodyMotorSensor/hingedRigidBodyMotorSensor.rst @@ -6,9 +6,9 @@ measured state. Message Connection Descriptions ------------------------------- -The following table lists all the module input and output messages. -The module msg connection is set by the user from python. -The msg type contains a link to the message structure definition, while the description +The following table lists all the module input and output messages. +The module msg connection is set by the user from python. +The msg type contains a link to the message structure definition, while the description provides information on what this message is used for. .. list-table:: Module I/O Messages diff --git a/src/simulation/sensors/imuSensor/_Documentation/AVS.sty b/src/simulation/sensors/imuSensor/_Documentation/AVS.sty index 73e5dd7956..c02abd9dfe 100644 --- a/src/simulation/sensors/imuSensor/_Documentation/AVS.sty +++ b/src/simulation/sensors/imuSensor/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/sensors/imuSensor/_Documentation/Basilisk-IMU-20170712.tex b/src/simulation/sensors/imuSensor/_Documentation/Basilisk-IMU-20170712.tex index c43def8e61..e1c3978806 100755 --- a/src/simulation/sensors/imuSensor/_Documentation/Basilisk-IMU-20170712.tex +++ b/src/simulation/sensors/imuSensor/_Documentation/Basilisk-IMU-20170712.tex @@ -82,7 +82,7 @@ \hline 1.2 & Rewrote for new IMU implementation and test & S. Carnahan & 20170914 \\ \hline - + \end{longtable} } @@ -92,7 +92,7 @@ \tableofcontents %Autogenerate the table of contents ~\\ \hrule ~\\ %Makes the line under table of contents - + \input{secModelDescription.tex} %This section includes mathematical models, code description, etc. \input{secModelFunctions.tex} %This includes a concise list of what the module does. It also includes model assumptions and limitations diff --git a/src/simulation/sensors/imuSensor/_Documentation/BasiliskCorruptions.tex b/src/simulation/sensors/imuSensor/_Documentation/BasiliskCorruptions.tex index d9c7438c6f..ddaef233ed 100644 --- a/src/simulation/sensors/imuSensor/_Documentation/BasiliskCorruptions.tex +++ b/src/simulation/sensors/imuSensor/_Documentation/BasiliskCorruptions.tex @@ -122,7 +122,7 @@ \section{Desired Corruptions} \begin{enumerate} \item Max \item Min - \end{enumerate} + \end{enumerate} \item Discretization \begin{enumerate} \item Max/min only = 2 bit diff --git a/src/simulation/sensors/imuSensor/_Documentation/BasiliskReportMemo.cls b/src/simulation/sensors/imuSensor/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/sensors/imuSensor/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/sensors/imuSensor/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/sensors/imuSensor/_Documentation/secModelDescription.tex b/src/simulation/sensors/imuSensor/_Documentation/secModelDescription.tex index 8283b768d9..cb163c8da7 100644 --- a/src/simulation/sensors/imuSensor/_Documentation/secModelDescription.tex +++ b/src/simulation/sensors/imuSensor/_Documentation/secModelDescription.tex @@ -18,7 +18,7 @@ \subsubsection{Angular Rates} Where $\cal{P}$ is the sensor platform frame, $\cal{B}$ is the vehicle body frame, and $\cal{N}$ is the inertial frame. [PB] is the direction cosine matrix from $\cal{B}$ to $\cal{P}$. This allows for an arbitrary angular offset between $\cal{B}$ and $\cal{P}$ and allows for that offset to be time-varying. $\leftexp{B}{\bm{\omega}_{B/N}}$ is provided by the spacecraft output message from the most recent dynamics integration. \subsubsection{Angular Displacement} -The IMU also outputs the angular displacement accumulated between IMU calls. In order to avoid complexities having to do with the relative timestep between the dynamics process and the IMU calls, this is not calculated in the same way as an IMU works physically. In this way, also, the dynamics do not have to be run at a fast enough rate for a physical IMU angular accumulation to be simulated. +The IMU also outputs the angular displacement accumulated between IMU calls. In order to avoid complexities having to do with the relative timestep between the dynamics process and the IMU calls, this is not calculated in the same way as an IMU works physically. In this way, also, the dynamics do not have to be run at a fast enough rate for a physical IMU angular accumulation to be simulated. The modified Rodriguez parameter (MRP) is recorded for the last time (1) the IMU was called. Once the new MRP is received, both are converted to DCMs and the step-PRV is computed as follows The current MRP is always provided by the spacecraft message from the most recent dynamics integration. \begin{equation} [PN]_2 = [PB][BN]_2 diff --git a/src/simulation/sensors/imuSensor/_Documentation/secModelFunctions.tex b/src/simulation/sensors/imuSensor/_Documentation/secModelFunctions.tex index 67ae18f2ba..6c1be36f41 100644 --- a/src/simulation/sensors/imuSensor/_Documentation/secModelFunctions.tex +++ b/src/simulation/sensors/imuSensor/_Documentation/secModelFunctions.tex @@ -18,8 +18,8 @@ \section{Model Assumptions and Limitations} \begin{itemize} \item \textbf{Error Inputs}: Because the error models rely on user inputs, these inputs are the most likely source of error in IMU output. Instrument bias would have to be measured experimentally or an educated guess would have to be made. The Guass-Markov noise model has well-known assumptions and is generally accepted to be a good model for this application. \item \textbf{Error Integration}: Errors for integrated values (DV and PRV) are calculated as acceleration and angular velocity errors multiplied by the IMU time step. If the IMU timestep matches the dynamics process rate, this is possibly a good assumption. However, if the IMU is run slower than the dynamics process, then the velocity errors may not be related to the instantaneous acceleration errors at the sampling time. - + \item \textbf{Integral Saturation}: Because the DV and PRV output values are calculated only at the IMU time step and not actually by integrating rates multiple times between calls, their saturation values are taken as the time integral of the rate saturation values. This misses some possibilities with varying accelerations between IMU time steps. Furthermore, the PRV is taken to be the integral of the angular rate over the time step. This should be a good approximation if the attitude of the spacecraft doesn't change "too much" over a relevant time step. - - \item \textbf{IMU Rate Limitation}: As with a real IMU, this model will only be run at a finite speed. It is limited in that it cannot correctly capture dynamics that are happening much faster than the IMU is called. -\end{itemize} \ No newline at end of file + + \item \textbf{IMU Rate Limitation}: As with a real IMU, this model will only be run at a finite speed. It is limited in that it cannot correctly capture dynamics that are happening much faster than the IMU is called. +\end{itemize} diff --git a/src/simulation/sensors/imuSensor/_Documentation/secTest.tex b/src/simulation/sensors/imuSensor/_Documentation/secTest.tex index 34a5fb2140..3349cab7f8 100644 --- a/src/simulation/sensors/imuSensor/_Documentation/secTest.tex +++ b/src/simulation/sensors/imuSensor/_Documentation/secTest.tex @@ -73,18 +73,18 @@ \subsection{Test Descriptions} \subitem \textbf{Success Criteria}: The outputs match to acceptable tolerance and are visually confirmed to be capped. \item \underline{Discretization} The IMU is run with all clean inputs, i.e. nonzero accelerations and angular accelerations of the spacecraft and this is compared to the truth values generated in python. Outputs are discretized. \subitem \textbf{Success Criteria}: The outputs match to acceptable tolerance and are visually confirmed to be discretized. Note. Two points in time always fail this test. This has to do with the python generated and c++ generated values being ever-so-slightly off and not discretizing at the same point. They match at the next timesteps and have been ignored for the test. -\end{enumerate} +\end{enumerate} As an additional check, $[PB]$ is calculated separately for the truth values and $yaw$, $pitch$, and $roll$ are fed to the IMU which calculates this value independently. In this way, the multiple set-up options for the IMU are validated. \section{Test Parameters} -This section summarizes the specific error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. +This section summarizes the specific error tolerances for each test. Error tolerances are determined based on whether the test results comparison should be exact or approximate due to integration or other reasons. Error tolerances for each test are summarized in table \ref{tab:errortol}. \begin{table}[H] \caption{Error tolerance for each test. Note that tolerances are relative $\frac{truth-output}{truth}$} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ c | c | c | c | c | c | c | c | c | c } % Column formatting, + \begin{tabular}{ c | c | c | c | c | c | c | c | c | c } % Column formatting, \hline \rot{\textbf{Test}} & \rot{\textbf{Tolerance}} &\rot{\textbf{GyroLSB}}& \rot{\textbf{AccelLSB}}& \rot{\textbf{RotMax}}&\rot{\textbf{TransMax}}&\rot{\textbf{RotNoise}}&\rot{\textbf{TransNoise}}&\rot{\textbf{RotBias}}&\rot{\textbf{TransBias}} \\ \hline Clean & \input{AutoTex/cleanaccuracy} & \input{AutoTex/cleangyroLSB}& \input{AutoTex/cleanaccelLSB}& \input{AutoTex/cleanrotMax}& \input{AutoTex/cleantransMax}& \input{AutoTex/cleanrotNoise}& \input{AutoTex/cleantransNoise}& \input{AutoTex/cleanrotBias}& \input{AutoTex/cleanTransBias} \\ \hline @@ -105,7 +105,7 @@ \section{Test Results} \caption{Test results} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c } % Column formatting, + \begin{tabular}{c | c } % Column formatting, \hline \textbf{Test} &\textbf{Pass/Fail} \\ \hline Clean & \input{AutoTex/cleanpassFail} \\ \hline @@ -157,4 +157,4 @@ \section{Test Results} \input{AutoTex/AccelNoise} \input{AutoTex/DRnoise} -\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. \ No newline at end of file +\pagebreak %needed to keep images/paragraphs in the right place. Cannot \usepackage{float} here because it is not used in the AutoTex implementation. diff --git a/src/simulation/sensors/imuSensor/_Documentation/secUserGuide.tex b/src/simulation/sensors/imuSensor/_Documentation/secUserGuide.tex index 6ca35e7a5d..051e441914 100644 --- a/src/simulation/sensors/imuSensor/_Documentation/secUserGuide.tex +++ b/src/simulation/sensors/imuSensor/_Documentation/secUserGuide.tex @@ -1,5 +1,5 @@ \section{User Guide} -This section contains conceptual overviews of the code and clear examples for the prospective user. +This section contains conceptual overviews of the code and clear examples for the prospective user. \subsection{Code Diagram} The diagram in Fig. \ref{img:codeFlow} demonstrates the basic logic of the IMU module. There is additional code that deals with auxiliary functions. An example of IMU use is given in test\_imu\_sensor.py in the imu\_sensor \_UnitTest folder. Application of each IMU function follows a simple, linear progression until realistic IMU outputs are achieved and sent out via the messaging system. @@ -16,7 +16,7 @@ \subsection{Variable Definitions} \caption{Definition and Explanation of Variables Used.} \label{tab:errortol} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{ | m{3cm}| m{3cm} | m{3cm} | m{6cm} |} % Column formatting, + \begin{tabular}{ | m{3cm}| m{3cm} | m{3cm} | m{6cm} |} % Column formatting, \hline \textbf{Variable} & \textbf{LaTeX Equivalent} & \textbf{Variable Type} & \textbf{Notes} \\ \hline InputStateMsg&N/A & string & Default setting: "inertial\_state\_output". This is the message from which the IMU receives spacecraft inertial data.\\ \hline @@ -36,4 +36,4 @@ \subsection{Variable Definitions} gyroLSB & (LSB) & double & Default: 0.0. This is the discretization value (least significant bit) for acceleration. Zero indicates no discretization. \\ \hline \end{tabular} \label{tabular:vars} -\end{table} \ No newline at end of file +\end{table} diff --git a/src/simulation/sensors/imuSensor/imuSensor.rst b/src/simulation/sensors/imuSensor/imuSensor.rst index d4f1a03650..df77939c3b 100644 --- a/src/simulation/sensors/imuSensor/imuSensor.rst +++ b/src/simulation/sensors/imuSensor/imuSensor.rst @@ -30,4 +30,3 @@ provides information on what this message is used for. * - sensorOutMsg - :ref:`IMUSensorMsgPayload` - output message name for IMU output data - diff --git a/src/simulation/sensors/simpleMassProps/simpleMassProps.h b/src/simulation/sensors/simpleMassProps/simpleMassProps.h index 6ca03cb780..602000047d 100644 --- a/src/simulation/sensors/simpleMassProps/simpleMassProps.h +++ b/src/simulation/sensors/simpleMassProps/simpleMassProps.h @@ -43,7 +43,7 @@ class SimpleMassProps: public SysModel { ReadFunctor scMassPropsInMsg; //!< sc mass properties input msg Message vehicleConfigOutMsg; //!< vehicle configuration output msg - + BSKLogger bskLogger; //!< -- BSK Logging diff --git a/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.rst b/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.rst index 9f3f0b6152..d614db7413 100644 --- a/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.rst +++ b/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.rst @@ -37,4 +37,4 @@ simpleVoltEstimator.SimpleVoltEstimator()``. The random walk bound is specified the standard deviation is specified using ``sVoltObject.PMatrix``. Note that the input for the walk bound and standard deviation must be a list to work for the :ref:`gauss_markov` module. -If no walk bound or standard deviation is specified, then the voltage measurement will not be corrupted with noise. \ No newline at end of file +If no walk bound or standard deviation is specified, then the voltage measurement will not be corrupted with noise. diff --git a/src/simulation/sensors/starTracker/_Documentation/AVS.sty b/src/simulation/sensors/starTracker/_Documentation/AVS.sty index 73e5dd7956..c02abd9dfe 100644 --- a/src/simulation/sensors/starTracker/_Documentation/AVS.sty +++ b/src/simulation/sensors/starTracker/_Documentation/AVS.sty @@ -1,6 +1,6 @@ % % AVS.sty -% +% % provides common packages used to edit LaTeX papers within the AVS lab % and creates some common mathematical macros % @@ -19,7 +19,7 @@ % % define Track changes macros and colors -% +% \definecolor{colorHPS}{rgb}{1,0,0} % Red \definecolor{COLORHPS}{rgb}{1,0,0} % Red %\definecolor{colorPA}{rgb}{1,0,1} % Magenta @@ -98,5 +98,3 @@ % define the oe orbit element symbol for the TeX math environment \renewcommand{\OE}{{o\hspace*{-2pt}e}} \renewcommand{\oe}{{\bm o\hspace*{-2pt}\bm e}} - - diff --git a/src/simulation/sensors/starTracker/_Documentation/Basilisk-star_tracker-20161101.tex b/src/simulation/sensors/starTracker/_Documentation/Basilisk-star_tracker-20161101.tex index d2b571214b..aec4cdaa2d 100755 --- a/src/simulation/sensors/starTracker/_Documentation/Basilisk-star_tracker-20161101.tex +++ b/src/simulation/sensors/starTracker/_Documentation/Basilisk-star_tracker-20161101.tex @@ -55,7 +55,7 @@ \section{{\tt test\textunderscore star\textunderscore tracker} Test Description} \item \underline{Structure to Body Transformation} The check verifies that the module correctly applied the structure to body frame transformation to the attitude coordinate output. \item \underline{Process Noise} The check verifies that the Gauss-Markov model applies noise of appropriate mean and standard deviation to the attitude coordinate output. This check does not consider bias random walk. \item \underline{Bias Random Walk Bounds} The check verifies that the Gauss-Markov model correctly applies bias random walk to the attitude coordinate output. Specified walk bounds are validated. -\end{enumerate} +\end{enumerate} \section{Test Parameters} @@ -85,7 +85,7 @@ \section{Test Results} \caption{Test results.} \label{tab:results} \centering \fontsize{10}{10}\selectfont - \begin{tabular}{c | c | c | c | c | c } % Column formatting, + \begin{tabular}{c | c | c | c | c | c } % Column formatting, \hline & Attitude I/O & Time Stamp I/O & T\_str2Bdy & Process Noise & Bias Walk Bounds \\ \hline diff --git a/src/simulation/sensors/starTracker/_Documentation/BasiliskReportMemo.cls b/src/simulation/sensors/starTracker/_Documentation/BasiliskReportMemo.cls index 569e0c6039..e2ee1590a3 100755 --- a/src/simulation/sensors/starTracker/_Documentation/BasiliskReportMemo.cls +++ b/src/simulation/sensors/starTracker/_Documentation/BasiliskReportMemo.cls @@ -97,4 +97,3 @@ % % Miscellaneous definitions % - diff --git a/src/simulation/simSynch/simSynch.h b/src/simulation/simSynch/simSynch.h index 9406f5e6ce..78f67efcfd 100755 --- a/src/simulation/simSynch/simSynch.h +++ b/src/simulation/simSynch/simSynch.h @@ -35,10 +35,10 @@ class ClockSynch: public SysModel { public: ClockSynch(); ~ClockSynch(); - + void Reset(uint64_t currentSimNanos); void UpdateState(uint64_t currentSimNanos); - + public: double accelFactor; //!< [-] Factor used to accelerate sim-time relative to clock SynchClockMsgPayload outputData; //!< [-] Output data for the synch module @@ -50,7 +50,7 @@ class ClockSynch: public SysModel { private: bool timeInitialized; //!< [-] Flag that the module has been reset std::chrono::high_resolution_clock::time_point startTime; //! [-] first time stamp of pass through data - uint64_t startSimTimeNano; //!< [ns] Previous simulation time observed + uint64_t startSimTimeNano; //!< [ns] Previous simulation time observed }; diff --git a/src/simulation/simSynch/simSynch.rst b/src/simulation/simSynch/simSynch.rst index 736ac4bec5..5546be549a 100644 --- a/src/simulation/simSynch/simSynch.rst +++ b/src/simulation/simSynch/simSynch.rst @@ -2,4 +2,3 @@ The clock synchronization module is used to slave the simulation to realtime. The module is controlled by specifying an acceleration factor which can be adjusted dynamically if the timeInitialized factor is also reset dynamically. - diff --git a/src/simulation/thermal/motorThermal/motorThermal.rst b/src/simulation/thermal/motorThermal/motorThermal.rst index c7f8a3d078..38c4533ac2 100644 --- a/src/simulation/thermal/motorThermal/motorThermal.rst +++ b/src/simulation/thermal/motorThermal/motorThermal.rst @@ -1,7 +1,7 @@ Executive Summary ----------------- -Module that computes the motor temperature. It takes into account mechanical power efficiency, as well as friction, +Module that computes the motor temperature. It takes into account mechanical power efficiency, as well as friction, as sources of heat. There is also a dissipative term due to the temperature gradient between the motor and the air surrounding it. @@ -36,7 +36,7 @@ power :math:`P_{mec}` is given by: .. math:: P_{mec}=\Omega \times u_s -where :math:`\Omega` is the motor angular velocity and :math:`u_s` represents the applied torque. Given the mechanical efficiency :math:`\eta`, we can convert +where :math:`\Omega` is the motor angular velocity and :math:`u_s` represents the applied torque. Given the mechanical efficiency :math:`\eta`, we can convert mechanical power into thermal power as follows: .. math:: @@ -48,7 +48,7 @@ Similarly to the mechanical power, the friction dissipation is given by: P_{f}=\Omega \times \tau_f where :math:`\tau_f` represents the friction torque. The absolute value of these two terms added together represents the thermal power generation. As for the power -dissipation, we assume a temperature gradient between the motor and the surrounding environment. For simplicity, a constant ambient temperature is considered. The +dissipation, we assume a temperature gradient between the motor and the surrounding environment. For simplicity, a constant ambient temperature is considered. The thermal power dissipation is calculated using the temperature difference between the motor and the environment, scaled by the ambient's thermal resistance :math:`R_{ambient}`: .. math:: @@ -128,4 +128,4 @@ A sample setup is done using: thermalModel.ambientTemperature = 20 # [Celsius] thermalModel.efficiency = 0.7 thermalModel.ambientThermalResistance = 5 # Air Thermal Resistance - thermalModel.motorHeatCapacity = 50 # Motor Heat Capacity \ No newline at end of file + thermalModel.motorHeatCapacity = 50 # Motor Heat Capacity diff --git a/src/simulation/thermal/sensorThermal/sensorThermal.rst b/src/simulation/thermal/sensorThermal/sensorThermal.rst index 675bb7f531..e41d795546 100644 --- a/src/simulation/thermal/sensorThermal/sensorThermal.rst +++ b/src/simulation/thermal/sensorThermal/sensorThermal.rst @@ -160,4 +160,4 @@ The relevant messages are then connected to the module: sensorThermalModel.sunInMsg.subscribeTo(sunMsg) sensorThermalModel.stateInMsg.subscribeTo(scStateMsg) sensorThermalModel.sensorStatusInMsg.subscribeTo(sensorStatusMsg) - unitTestSim.AddModelToTask(unitTaskName, sensorThermalModel) \ No newline at end of file + unitTestSim.AddModelToTask(unitTaskName, sensorThermalModel) diff --git a/src/simulation/vizard/dataFileToViz/_UnitTest/test_dataFileToViz.py b/src/simulation/vizard/dataFileToViz/_UnitTest/test_dataFileToViz.py index 5d9639aaa4..16ecdb5636 100644 --- a/src/simulation/vizard/dataFileToViz/_UnitTest/test_dataFileToViz.py +++ b/src/simulation/vizard/dataFileToViz/_UnitTest/test_dataFileToViz.py @@ -347,7 +347,7 @@ def run(show_plots, convertPosUnits, attType, checkThruster, checkRW, verbose): # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() - unitTestSim.ConfigureStopTime(simulationTime) + unitTestSim.ConfigureStopTime(simulationTime) # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -458,4 +458,3 @@ def run(show_plots, convertPosUnits, attType, checkThruster, checkRW, verbose): ) if os.path.exists(dataFileName): os.remove(dataFileName) - diff --git a/src/simulation/vizard/dataFileToViz/dataFileToViz.h b/src/simulation/vizard/dataFileToViz/dataFileToViz.h index c544240b7f..2a83e67d5c 100755 --- a/src/simulation/vizard/dataFileToViz/dataFileToViz.h +++ b/src/simulation/vizard/dataFileToViz/dataFileToViz.h @@ -1,10 +1,10 @@ /* Copyright (c) 2016, Autonomous Vehicle Systems Lab, Univeristy 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 @@ -12,7 +12,7 @@ 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. - + */ diff --git a/src/simulation/vizard/dataFileToViz/dataFileToViz.rst b/src/simulation/vizard/dataFileToViz/dataFileToViz.rst index e109a2c725..12e441c11c 100644 --- a/src/simulation/vizard/dataFileToViz/dataFileToViz.rst +++ b/src/simulation/vizard/dataFileToViz/dataFileToViz.rst @@ -134,4 +134,3 @@ Next, the RW position, spin axis direction, the wheel speed and the maximum moto testModule.appendUMax(0.5) Repeat the above steps for each wheel. - diff --git a/src/tests/test_bskPrinciples.py b/src/tests/test_bskPrinciples.py index 96706ef4ca..5d08ad613c 100644 --- a/src/tests/test_bskPrinciples.py +++ b/src/tests/test_bskPrinciples.py @@ -82,4 +82,4 @@ def test_scenarioBskPrinciples(show_plots, bskScript): test_scenarioBskPrinciples( False, # show_plots 'bsk-4' - ) \ No newline at end of file + ) diff --git a/src/tests/test_deprecated.py b/src/tests/test_deprecated.py index 6bf5a52950..ba5a1a308f 100644 --- a/src/tests/test_deprecated.py +++ b/src/tests/test_deprecated.py @@ -17,7 +17,7 @@ # r""" -This test demonstrates how to deprecate functions, classes, attributes, +This test demonstrates how to deprecate functions, classes, attributes, and properties defined in Python code. To learn how to deprecate C++ code exposed to users through SWIG, see: @@ -42,7 +42,7 @@ def myFun1(self): @property def myProperty(self): return self.myPropertyInner * 2 - + @myProperty.setter def myProperty(self, value: int): self.myPropertyInner = value / 2 diff --git a/src/tests/test_orbitalMotion.py b/src/tests/test_orbitalMotion.py index 1c64844a9b..b2b122ebe9 100644 --- a/src/tests/test_orbitalMotion.py +++ b/src/tests/test_orbitalMotion.py @@ -281,4 +281,4 @@ def test_orbitalMotion(show_plots): if __name__ == "__main__": - test_orbitalMotion(False) \ No newline at end of file + test_orbitalMotion(False) diff --git a/src/tests/test_rbkRoutine.py b/src/tests/test_rbkRoutine.py index b203cc964d..a4f522580b 100755 --- a/src/tests/test_rbkRoutine.py +++ b/src/tests/test_rbkRoutine.py @@ -1004,4 +1004,4 @@ def test_rigidBodyKinematics(show_plots): if __name__ == "__main__": - test_rigidBodyKinematics(False) \ No newline at end of file + test_rigidBodyKinematics(False) diff --git a/src/tests/test_scenarioAlbedo.py b/src/tests/test_scenarioAlbedo.py index 7b565b376d..0e6bbe2339 100644 --- a/src/tests/test_scenarioAlbedo.py +++ b/src/tests/test_scenarioAlbedo.py @@ -83,4 +83,3 @@ def test_scenarioAlbedo(show_plots, albedoData, multipleInstrument, multiplePlan # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttGuideHyperbolic.py b/src/tests/test_scenarioAttGuideHyperbolic.py index e448568d27..b46a3ce74f 100644 --- a/src/tests/test_scenarioAttGuideHyperbolic.py +++ b/src/tests/test_scenarioAttGuideHyperbolic.py @@ -83,5 +83,3 @@ def test_bskAttGuide_Hyperbolic(show_plots, useAltBodyFrame): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - - diff --git a/src/tests/test_scenarioAttLocPoint.py b/src/tests/test_scenarioAttLocPoint.py index 6a183fda77..3695c2b4bb 100755 --- a/src/tests/test_scenarioAttLocPoint.py +++ b/src/tests/test_scenarioAttLocPoint.py @@ -78,6 +78,3 @@ def test_bskAttitudeGuidance(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - - - diff --git a/src/tests/test_scenarioAttitudeConstrainedManeuver.py b/src/tests/test_scenarioAttitudeConstrainedManeuver.py index 41d7927b7b..9a0b4fd595 100644 --- a/src/tests/test_scenarioAttitudeConstrainedManeuver.py +++ b/src/tests/test_scenarioAttitudeConstrainedManeuver.py @@ -85,4 +85,3 @@ def test_bskAttitudeConstrainedManeuver(show_plots, use2SunSensors, starTrackerF # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttitudeConstraintViolation.py b/src/tests/test_scenarioAttitudeConstraintViolation.py index e6a35e84b7..5f08e2457d 100644 --- a/src/tests/test_scenarioAttitudeConstraintViolation.py +++ b/src/tests/test_scenarioAttitudeConstraintViolation.py @@ -52,7 +52,7 @@ # of the multiple test runs for this test. -@pytest.mark.parametrize("use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCase", [(False, 20, 70, 0), +@pytest.mark.parametrize("use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCase", [(False, 20, 70, 0), (True, 20, 70, 0), (True, 20, 70, 1), (True, 20, 70, 2), @@ -87,4 +87,3 @@ def test_bskAttitudeConstraintViolation(show_plots, use2SunSensors, starTrackerF # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttitudeFeedback.py b/src/tests/test_scenarioAttitudeFeedback.py index 3b17351165..726e77c495 100644 --- a/src/tests/test_scenarioAttitudeFeedback.py +++ b/src/tests/test_scenarioAttitudeFeedback.py @@ -84,4 +84,3 @@ def test_bskAttitudeFeedback(show_plots, useUnmodeledTorque, useIntGain, useKnow # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttitudeFeedback2T_TH.py b/src/tests/test_scenarioAttitudeFeedback2T_TH.py index 6444ded552..a1dd5637c5 100755 --- a/src/tests/test_scenarioAttitudeFeedback2T_TH.py +++ b/src/tests/test_scenarioAttitudeFeedback2T_TH.py @@ -81,4 +81,3 @@ def test_bskAttitudeFeedback2T_TH(show_plots, useDVThrusters): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttitudeFeedback2T_stateEffTH.py b/src/tests/test_scenarioAttitudeFeedback2T_stateEffTH.py index 1831a38077..11fb237f8e 100644 --- a/src/tests/test_scenarioAttitudeFeedback2T_stateEffTH.py +++ b/src/tests/test_scenarioAttitudeFeedback2T_stateEffTH.py @@ -81,4 +81,3 @@ def test_bskAttitudeFeedback2T_stateEffTH(show_plots, useDVThrusters): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttitudeFeedbackNoEarth.py b/src/tests/test_scenarioAttitudeFeedbackNoEarth.py index adbb9fd811..bbea9d4fe9 100644 --- a/src/tests/test_scenarioAttitudeFeedbackNoEarth.py +++ b/src/tests/test_scenarioAttitudeFeedbackNoEarth.py @@ -84,4 +84,3 @@ def test_bskAttitudeFeedbackNoEarth(show_plots, useUnmodeledTorque, useIntGain, # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttitudeFeedbackRW.py b/src/tests/test_scenarioAttitudeFeedbackRW.py index 17e84baab5..a0e626a68c 100644 --- a/src/tests/test_scenarioAttitudeFeedbackRW.py +++ b/src/tests/test_scenarioAttitudeFeedbackRW.py @@ -81,4 +81,3 @@ def test_bskAttitudeFeedbackRW(show_plots, useJitterSimple, useRWVoltageIO): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttitudeFeedbackRWPower.py b/src/tests/test_scenarioAttitudeFeedbackRWPower.py index 584dc458e2..0cc69ffcaf 100644 --- a/src/tests/test_scenarioAttitudeFeedbackRWPower.py +++ b/src/tests/test_scenarioAttitudeFeedbackRWPower.py @@ -90,4 +90,3 @@ def test_bskAttitudeFeedbackRW(show_plots, useRwPowerGeneration): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttitudeGG.py b/src/tests/test_scenarioAttitudeGG.py index 6cd79f625b..448904704c 100644 --- a/src/tests/test_scenarioAttitudeGG.py +++ b/src/tests/test_scenarioAttitudeGG.py @@ -79,4 +79,3 @@ def test_scenarioAttitudeGG(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttitudeGuidance.py b/src/tests/test_scenarioAttitudeGuidance.py index 109ca793ce..ba3d674e3c 100755 --- a/src/tests/test_scenarioAttitudeGuidance.py +++ b/src/tests/test_scenarioAttitudeGuidance.py @@ -81,6 +81,3 @@ def test_bskAttitudeGuidance(show_plots, useAltBodyFrame): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - - - diff --git a/src/tests/test_scenarioAttitudePointing.py b/src/tests/test_scenarioAttitudePointing.py index f2ccc0d4e7..a8bda9d0bb 100644 --- a/src/tests/test_scenarioAttitudePointing.py +++ b/src/tests/test_scenarioAttitudePointing.py @@ -78,4 +78,3 @@ def test_bskAttitudePointing(show_plots, useLargeTumble): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttitudePointingPy.py b/src/tests/test_scenarioAttitudePointingPy.py index fe44cbde34..bd90549824 100755 --- a/src/tests/test_scenarioAttitudePointingPy.py +++ b/src/tests/test_scenarioAttitudePointingPy.py @@ -70,4 +70,3 @@ def test_bskAttitudePointingPD(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioAttitudePrescribed.py b/src/tests/test_scenarioAttitudePrescribed.py index ee92b36599..d8ee30fff8 100755 --- a/src/tests/test_scenarioAttitudePrescribed.py +++ b/src/tests/test_scenarioAttitudePrescribed.py @@ -78,6 +78,3 @@ def test_bskAttitudeGuidance(show_plots, useAltBodyFrame): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - - - diff --git a/src/tests/test_scenarioAttitudeSteering.py b/src/tests/test_scenarioAttitudeSteering.py index 5ebf25cf13..74e3f1265e 100644 --- a/src/tests/test_scenarioAttitudeSteering.py +++ b/src/tests/test_scenarioAttitudeSteering.py @@ -82,4 +82,3 @@ def test_bskAttitudeFeedbackRW(show_plots, simCase): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioBskLog.py b/src/tests/test_scenarioBskLog.py index 0af3a795fa..030c3fe9cf 100644 --- a/src/tests/test_scenarioBskLog.py +++ b/src/tests/test_scenarioBskLog.py @@ -93,4 +93,3 @@ def test_scenarioBskLog(show_plots, case): # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioCSS.py b/src/tests/test_scenarioCSS.py index cfe6a471b4..fcbdc84666 100644 --- a/src/tests/test_scenarioCSS.py +++ b/src/tests/test_scenarioCSS.py @@ -79,4 +79,3 @@ def test_bskAttitudeFeedback(show_plots, useCSSConstellation, usePlatform, useEc # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioCSSFilters.py b/src/tests/test_scenarioCSSFilters.py index 5b10456216..1e248ad2c6 100755 --- a/src/tests/test_scenarioCSSFilters.py +++ b/src/tests/test_scenarioCSSFilters.py @@ -71,5 +71,3 @@ def test_Filters(show_plots, FilterType, simTime): # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - - diff --git a/src/tests/test_scenarioCentralBody.py b/src/tests/test_scenarioCentralBody.py index 00adf051bd..94e3ed720e 100644 --- a/src/tests/test_scenarioCentralBody.py +++ b/src/tests/test_scenarioCentralBody.py @@ -92,4 +92,3 @@ def test_scenarioCentralBody(show_plots, useCentral): # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioCustomGravBody.py b/src/tests/test_scenarioCustomGravBody.py index 4b162642de..57d0a96ba1 100644 --- a/src/tests/test_scenarioCustomGravBody.py +++ b/src/tests/test_scenarioCustomGravBody.py @@ -57,5 +57,3 @@ def test_simplePowerDemo(show_plots): assert testFailCount < 1, testMessages return - - diff --git a/src/tests/test_scenarioDataDemo.py b/src/tests/test_scenarioDataDemo.py index ea0f8739bf..bc4ef9b9cd 100644 --- a/src/tests/test_scenarioDataDemo.py +++ b/src/tests/test_scenarioDataDemo.py @@ -55,5 +55,3 @@ def test_simpleDataDemo(show_plots): assert testFailCount < 1, testMessages return - - diff --git a/src/tests/test_scenarioDataToViz.py b/src/tests/test_scenarioDataToViz.py index 4193e40007..db70192feb 100644 --- a/src/tests/test_scenarioDataToViz.py +++ b/src/tests/test_scenarioDataToViz.py @@ -60,5 +60,3 @@ def test_simplePowerDemo(show_plots, attType): assert testFailCount < 1, testMessages return - - diff --git a/src/tests/test_scenarioDebrisReorbitET.py b/src/tests/test_scenarioDebrisReorbitET.py index cff8493521..f3cf0d977f 100644 --- a/src/tests/test_scenarioDebrisReorbitET.py +++ b/src/tests/test_scenarioDebrisReorbitET.py @@ -84,4 +84,4 @@ def test_scenarioDebrisReorbitET(show_plots): if __name__ == "__main__": test_scenarioDebrisReorbitET( False # show_plots - ) \ No newline at end of file + ) diff --git a/src/tests/test_scenarioDragRendezvous.py b/src/tests/test_scenarioDragRendezvous.py index 0c2bc444d4..0becb4a0e8 100644 --- a/src/tests/test_scenarioDragRendezvous.py +++ b/src/tests/test_scenarioDragRendezvous.py @@ -66,4 +66,3 @@ def test_scenarioDragRendezvous(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioDragSensitivity.py b/src/tests/test_scenarioDragSensitivity.py index 48e1844aab..d2a3fb144c 100644 --- a/src/tests/test_scenarioDragSensitivity.py +++ b/src/tests/test_scenarioDragSensitivity.py @@ -62,4 +62,3 @@ def test_scenarioDragSensitivity(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioFormationBasic.py b/src/tests/test_scenarioFormationBasic.py index 418c1336a4..1f7c8f1d4c 100644 --- a/src/tests/test_scenarioFormationBasic.py +++ b/src/tests/test_scenarioFormationBasic.py @@ -79,4 +79,3 @@ def test_scenarioFormationBasic(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioFormationReconfig.py b/src/tests/test_scenarioFormationReconfig.py index 942a705959..888253eebc 100644 --- a/src/tests/test_scenarioFormationReconfig.py +++ b/src/tests/test_scenarioFormationReconfig.py @@ -150,7 +150,7 @@ def test_scenarioFormationReconfig(show_plots, useRefAttitude): testFailCount, testMessages = unitTestSupport.compareArrayRelative( trueVel2, dataVel2, accuracy, "deputy v2_BN_N Vector", testFailCount, testMessages) - + testFailCount, testMessages = unitTestSupport.compareArray( trueAttErr, dataAttErr, accuracy, "deputy attitude Error", testFailCount, testMessages) diff --git a/src/tests/test_scenarioGroundMapping.py b/src/tests/test_scenarioGroundMapping.py index 4681ba18e3..286d2f76d6 100644 --- a/src/tests/test_scenarioGroundMapping.py +++ b/src/tests/test_scenarioGroundMapping.py @@ -53,5 +53,3 @@ def test_scenarioGroundMapping(show_plots): assert testFailCount < 1, testMessages return - - diff --git a/src/tests/test_scenarioHingedRigidBody.py b/src/tests/test_scenarioHingedRigidBody.py index db51269283..20228945ba 100644 --- a/src/tests/test_scenarioHingedRigidBody.py +++ b/src/tests/test_scenarioHingedRigidBody.py @@ -87,4 +87,3 @@ def test_scenarioOrbitManeuver(doUnitTests, show_plots): # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioHohmann.py b/src/tests/test_scenarioHohmann.py index 8ae375f4d0..300a2e8fee 100755 --- a/src/tests/test_scenarioHohmann.py +++ b/src/tests/test_scenarioHohmann.py @@ -79,6 +79,3 @@ def test_bskAttitudeGuidance(show_plots, rFirst, rSecond): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - - - diff --git a/src/tests/test_scenarioLagrangePointOrbit.py b/src/tests/test_scenarioLagrangePointOrbit.py index 51413f6bfc..8fcffeb592 100644 --- a/src/tests/test_scenarioLagrangePointOrbit.py +++ b/src/tests/test_scenarioLagrangePointOrbit.py @@ -92,4 +92,4 @@ def test_scenarioLagrangePointOrbit(lagrangePoint, nOrbits, timestep, showPlots) 1, # Number of Moon orbits 300, # Timestep (seconds) False # Show plots - ) \ No newline at end of file + ) diff --git a/src/tests/test_scenarioMagneticFieldCenteredDipole.py b/src/tests/test_scenarioMagneticFieldCenteredDipole.py index d8306bff89..6e076202eb 100644 --- a/src/tests/test_scenarioMagneticFieldCenteredDipole.py +++ b/src/tests/test_scenarioMagneticFieldCenteredDipole.py @@ -85,4 +85,3 @@ def test_scenarioMagneticField(show_plots, orbitCase, planetCase): # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioMomentumDumping.py b/src/tests/test_scenarioMomentumDumping.py index ba3e0128e8..3e38405584 100644 --- a/src/tests/test_scenarioMomentumDumping.py +++ b/src/tests/test_scenarioMomentumDumping.py @@ -82,4 +82,3 @@ def test_bskMomentumDumping(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioOrbitMultiBody.py b/src/tests/test_scenarioOrbitMultiBody.py index 3d083b20c6..4d8d2f3206 100644 --- a/src/tests/test_scenarioOrbitMultiBody.py +++ b/src/tests/test_scenarioOrbitMultiBody.py @@ -83,4 +83,3 @@ def test_scenarioOrbitMultiBodyCopy(show_plots, scCase): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioPowerDemo.py b/src/tests/test_scenarioPowerDemo.py index a4d318c0ac..6d50f55992 100644 --- a/src/tests/test_scenarioPowerDemo.py +++ b/src/tests/test_scenarioPowerDemo.py @@ -55,5 +55,3 @@ def test_simplePowerDemo(show_plots): assert testFailCount < 1, testMessages return - - diff --git a/src/tests/test_scenarioRotatingPanel.py b/src/tests/test_scenarioRotatingPanel.py index 9f64f5f489..ecf5a545e9 100644 --- a/src/tests/test_scenarioRotatingPanel.py +++ b/src/tests/test_scenarioRotatingPanel.py @@ -79,4 +79,3 @@ def test_scenarioAttitudeGG(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioSatelliteConstellation.py b/src/tests/test_scenarioSatelliteConstellation.py index d8be094ce1..104e5b98db 100644 --- a/src/tests/test_scenarioSatelliteConstellation.py +++ b/src/tests/test_scenarioSatelliteConstellation.py @@ -73,4 +73,3 @@ def test_bskAttitudePointing(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioSmallBodyLandmarks.py b/src/tests/test_scenarioSmallBodyLandmarks.py index 3743d1d30f..0d1e13fbd9 100644 --- a/src/tests/test_scenarioSmallBodyLandmarks.py +++ b/src/tests/test_scenarioSmallBodyLandmarks.py @@ -95,4 +95,4 @@ def test_scenarioSmallBodyLandmarks(show_plots, useBatch): test_scenarioSmallBodyLandmarks( False, # show_plots False # useBatch - ) \ No newline at end of file + ) diff --git a/src/tests/test_scenarioSpacecraftLocation.py b/src/tests/test_scenarioSpacecraftLocation.py index 65bd4206d1..9ac157bf86 100644 --- a/src/tests/test_scenarioSpacecraftLocation.py +++ b/src/tests/test_scenarioSpacecraftLocation.py @@ -92,4 +92,4 @@ def test_scenarioBasicOrbit(show_plots): 'LEO', # orbit Case (LEO, GTO, GEO) False, # useSphericalHarmonics 'Mars' # planetCase (Earth, Mars) - ) \ No newline at end of file + ) diff --git a/src/tests/test_scenarioStationKeepingMultiSat.py b/src/tests/test_scenarioStationKeepingMultiSat.py index 43a8c88c79..61efd8e8b3 100644 --- a/src/tests/test_scenarioStationKeepingMultiSat.py +++ b/src/tests/test_scenarioStationKeepingMultiSat.py @@ -67,4 +67,4 @@ def test_scenarioStationKeepingMultiSat(show_plots, numberSpacecraft, relativeNa False, # show_plots 3, # numberSpacecraft False # relativeNavigation - ) \ No newline at end of file + ) diff --git a/src/tests/test_scenarioTAM.py b/src/tests/test_scenarioTAM.py index 5e0f2cdf64..2da340dd36 100644 --- a/src/tests/test_scenarioTAM.py +++ b/src/tests/test_scenarioTAM.py @@ -126,4 +126,3 @@ def test_scenarioTAM(show_plots, orbitCase, planetCase, useBias, useBounds): # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/tests/test_scenarioTAMcomparison.py b/src/tests/test_scenarioTAMcomparison.py index 544566296c..d2bdf86c98 100644 --- a/src/tests/test_scenarioTAMcomparison.py +++ b/src/tests/test_scenarioTAMcomparison.py @@ -96,4 +96,4 @@ def test_scenarioTAMcomparison(show_plots, orbitCase, useBias1, useBias2, useBou False, # useBias2 False, # useBounds1 False # useBounds2 - ) \ No newline at end of file + ) diff --git a/src/tests/test_scenarioVizPoint.py b/src/tests/test_scenarioVizPoint.py index bcefc08d90..3e2a27db57 100644 --- a/src/tests/test_scenarioVizPoint.py +++ b/src/tests/test_scenarioVizPoint.py @@ -88,4 +88,3 @@ def test_scenarioViz(show_plots, missionType): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found assert testFailCount < 1, testMessages - diff --git a/src/utilities/SpherePlot.py b/src/utilities/SpherePlot.py index 3b5a655070..40874107ee 100644 --- a/src/utilities/SpherePlot.py +++ b/src/utilities/SpherePlot.py @@ -112,4 +112,3 @@ def plotSpheres(posDataL_N, posDataF_N, attDataL_N, attDataF_N, spPosListLeader_ def NormalizeData(data): return (data - np.min(data)) / (np.max(data) - np.min(data)) - diff --git a/src/utilities/macros.py b/src/utilities/macros.py index ddd26d8f8a..54e91565c0 100755 --- a/src/utilities/macros.py +++ b/src/utilities/macros.py @@ -62,4 +62,3 @@ def day2nano(time): # variable to convert RPM to radians per second rpm2radsec = 2.0 * math.pi/60. - diff --git a/src/utilities/simSetPlanetEnvironment.py b/src/utilities/simSetPlanetEnvironment.py index c8eb177396..33d70d8685 100644 --- a/src/utilities/simSetPlanetEnvironment.py +++ b/src/utilities/simSetPlanetEnvironment.py @@ -130,5 +130,3 @@ def convertToIgrfDipoleCoefficients(nominalField, tilt, longitudeOfTilt, magFiel magFieldModule.g10 = nominalField*np.cos(theta_m) return - - diff --git a/src/utilities/simulationProgessBar.py b/src/utilities/simulationProgessBar.py index f5a34b3e35..a94c93cfd7 100644 --- a/src/utilities/simulationProgessBar.py +++ b/src/utilities/simulationProgessBar.py @@ -34,4 +34,3 @@ def markComplete(self): def close(self): self.p.close() - diff --git a/supportData/AtmosphereData/NRLMSISE00Nominal.txt b/supportData/AtmosphereData/NRLMSISE00Nominal.txt index ad516fb937..14c068cab6 100644 --- a/supportData/AtmosphereData/NRLMSISE00Nominal.txt +++ b/supportData/AtmosphereData/NRLMSISE00Nominal.txt @@ -13,21 +13,21 @@ Optional input parameters: Sunspot number(Rz12) =not specified Ionospheric index(IG12) =not specified Upper limit for Electron content = not specified -F peak model = -Ne Topside = -foF2 Storm model = -Bottomside Thickness = -F1 occurrence probability = -D-Region Ne = -Topside Te = -Ion Composition = +F peak model = +Ne Topside = +foF2 Storm model = +Bottomside Thickness = +F1 occurrence probability = +D-Region Ne = +Topside Te = +Ion Composition = Selected output parameters: 1 Height, km 2 Mass_density, g/cm-3 3 Temperature_neutral, K -1 2 3 +1 2 3 0.0 1.233E-03 286.4 0.5 1.168E-03 284.8 1.0 1.108E-03 282.7 diff --git a/supportData/AtmosphereData/support/EarthGRAM_bsk_namelist.txt b/supportData/AtmosphereData/support/EarthGRAM_bsk_namelist.txt index b2739d8d06..7b565f1b8f 100644 --- a/supportData/AtmosphereData/support/EarthGRAM_bsk_namelist.txt +++ b/supportData/AtmosphereData/support/EarthGRAM_bsk_namelist.txt @@ -6,7 +6,7 @@ nprpath = EarthGRAMNominal.txt conpath = null rrapath = C:\GRAMs\EarthGRAM2016\Version2.0\RRAdata\ - rralist = null + rralist = null profile = null h1 = 200.0 phi1 = 40.0 @@ -97,24 +97,24 @@ nmax = maximum number of positions (including initial one; 0 means read trajectory input file) delt = time increment between positions (real seconds) iopt = trajectory option (0 = no trajectory data; > 0 for trajectory data) -iaux = auxiliary profile option (0 = no auxiliary profile; > 0 for auxiliary profile data) +iaux = auxiliary profile option (0 = no auxiliary profile; > 0 for auxiliary profile data) iprt = "output.txt" output option (0 = no "output.txt" output; > 0 = "output.txt" output) inpr = "special.txt" output option (0 = no "special.txt" output; > 0 = "special.txt" output) icon = "species.txt" output option (0 = no "species.txt" output; > 0 = "species.txt" output) -NCEPyr = y1y2 to use NCEP climatology for period-of-record (POR) from year - y1 through year y2 (e.g. NCEPyr=9008 for POR = 1990 through - 2008). NCEP monthly climatology is determined by input value +NCEPyr = y1y2 to use NCEP climatology for period-of-record (POR) from year + y1 through year y2 (e.g. NCEPyr=9008 for POR = 1990 through + 2008). NCEP monthly climatology is determined by input value of month (mn) in initial time input -NCEPhr = Code for UT hour of day if NCEP climatology is used: 1=00 UT, - 2=06UT, 3=12UT, 4=18UT, 5=all times of day combined, or 0 to +NCEPhr = Code for UT hour of day if NCEP climatology is used: 1=00 UT, + 2=06UT, 3=12UT, 4=18UT, 5=all times of day combined, or 0 to use NCEP time-of-day based on input UTC hour (ihro) nr1 = first starting random number (1 to 9 * 10**8) mc = number of Monte Carlo runs -rpscale = random perturbation scale for density, temperature and pressure; +rpscale = random perturbation scale for density, temperature and pressure; nominal=1.0, max=2.0, min=0.1 -ruscale = random perturbation scale for horizontal winds; nominal=1.0, +ruscale = random perturbation scale for horizontal winds; nominal=1.0, maximum=2.0, minimum=0.1 -rwscale = random perturbation scale for vertical winds; nominal=1.0, +rwscale = random perturbation scale for vertical winds; nominal=1.0, maximum=2.0, minimum=0.1 iurra = unit number for Range Reference Atmosphere (RRA) data (0 if none; iurra > 0 for RRA data) iyrrra = 1 for 1983 RRAs, 2 for 2006 RRAs, 3 for 2013 RRAs @@ -124,7 +124,7 @@ sitelim = lat-lon radius (deg) from RRA site, outside which RRA data are cannot be used simultaneously. sitenear = lat-lon radius (deg) from RRA site, inside which RRA data is used with full weight of 1 (smooth transition of weight factor - from 1 to 0 between sitenear and sitelim). Also used, with a + from 1 to 0 between sitenear and sitelim). Also used, with a similar meaning, for auxiliary profile input. initpert = Use 1 for user-selected initial perturbations or 0 (default) for GRAM-derived, random initial perturbation values @@ -134,13 +134,13 @@ rtinit = initial temperature perturbation value (% of mean). Note - initial ruinit = initial eastward velocity perturbation (m/s) rvinit = initial northward velocity perturbation (m/s) rwinit = initial upward velocity perturbation (m/s) -patchy = not equal 0 for patchiness; 0 to suppress patchiness in +patchy = not equal 0 for patchiness; 0 to suppress patchiness in perturbation model -itherm = 1 for MET (Jacchia), 2 for MSIS, or 3 for JB2008 thermosphere -z0in = surface roughness (z0) for sigma-w model [ < 0 to use 1-by-1 deg +itherm = 1 for MET (Jacchia), 2 for MSIS, or 3 for JB2008 thermosphere +z0in = surface roughness (z0) for sigma-w model [ < 0 to use 1-by-1 deg lat-lon surface data, from new file atmosdat_E07.txt; = 0 for - speed-dependent z0 over water; or enter a value between 1.0e-5 - and 3 for user-specified z0 value ]. For more information, + speed-dependent z0 over water; or enter a value between 1.0e-5 + and 3 for user-specified z0 value ]. For more information, see file README7.txt. -ibltest = unit number for BL model output file (bltest.txt), or 0 for no +ibltest = unit number for BL model output file (bltest.txt), or 0 for no BL model output diff --git a/supportData/AtmosphereData/support/JupiterGRAM_bsk_namelist.txt b/supportData/AtmosphereData/support/JupiterGRAM_bsk_namelist.txt index 5edaa5c9b5..b83bcd33da 100644 --- a/supportData/AtmosphereData/support/JupiterGRAM_bsk_namelist.txt +++ b/supportData/AtmosphereData/support/JupiterGRAM_bsk_namelist.txt @@ -1,9 +1,9 @@ $INPUT - SpicePath = '\spice' - ListFileName = 'C:\GRAMs\GRAM_Suite_1.3\Jupiter\IOfiles\JupiterGRAMLIST' - ColumnFileName = 'C:\GRAMs\GRAM_Suite_1.3\Jupiter\IOfiles\JupiterGRAMNominal' - EastLongitudePositive = 1 - + SpicePath = '\spice' + ListFileName = 'C:\GRAMs\GRAM_Suite_1.3\Jupiter\IOfiles\JupiterGRAMLIST' + ColumnFileName = 'C:\GRAMs\GRAM_Suite_1.3\Jupiter\IOfiles\JupiterGRAMNominal' + EastLongitudePositive = 1 + TimeFrame = 1 TimeScale = 1 Month = 3 @@ -13,14 +13,14 @@ Minute = 30 Seconds = 0.0 - TrajectoryFileName = 'null' - NumberOfPositions = 2001 - InitialHeight = 0.0 - InitialLatitude = 22.0 - InitialLongitude = 48.0 - DeltaHeight = 1.0 - DeltaLatitude = 0. - DeltaLongitude = 0. + TrajectoryFileName = 'null' + NumberOfPositions = 2001 + InitialHeight = 0.0 + InitialLatitude = 22.0 + InitialLongitude = 48.0 + DeltaHeight = 1.0 + DeltaLatitude = 0. + DeltaLongitude = 0. DeltaTime = 0. AuxiliaryAtmosphereFileName = 'null' @@ -30,49 +30,49 @@ FastModeOn = 0 ExtraPrecision = 0 - $END + $END Explanation of variables: SpicePath = Path to NAIF Spice data ListFileName = List file name - ColumnFileName = Output file name + ColumnFileName = Output file name EastLongitudePositive = 0 for input and output West longitudes positive 1 for East longitudes positive TimeFrame = 0 Planet event time (PET) 1 for time input as Earth-receive time (ERT) - + TimeScale = 0 for Terrestrial (Dynamical) Time (TDT) 1 for time input as Coordinated Universal Time (UTC) 2 for Barycentric Dynamical Time (TDB) - Month = month of year - Day = day of month + Month = month of year + Day = day of month Year = year (4-digit, or 1970-2069 can be 2-digit) - Hour = hour of day (meaning controlled by TimeFrame and TimeScale) - Minute = minute of hour (meaning controlled by TimeFrame and TimeScale) - Seconds = seconds of minute (meaning controlled by TimeFrame and TimeScale) + Hour = hour of day (meaning controlled by TimeFrame and TimeScale) + Minute = minute of hour (meaning controlled by TimeFrame and TimeScale) + Seconds = seconds of minute (meaning controlled by TimeFrame and TimeScale) - TrajectoryFileName = (Optional) Trajectory input file name - If present, then the values below are ignored - NumberOfPositions = number of positions to evaluate - InitialHeight = initial height (km) - InitialLatitude = initial latitude (N positive), degrees - InitialLongitude = initial longitude, degrees + TrajectoryFileName = (Optional) Trajectory input file name + If present, then the values below are ignored + NumberOfPositions = number of positions to evaluate + InitialHeight = initial height (km) + InitialLatitude = initial latitude (N positive), degrees + InitialLongitude = initial longitude, degrees (depends on EastLongitudePositive) DeltaHeight = height increment (km) between steps - DeltaLatitude = latitude increment (deg) between steps + DeltaLatitude = latitude increment (deg) between steps DeltaLongitude = longitude increment (deg) between steps - (depends on EastLongitudePositive) + (depends on EastLongitudePositive) DeltaTime = time increment (seconds) between steps. - - AuxiliaryAtmosphereFileName = (Optional) auxiliary profile input file name + + AuxiliaryAtmosphereFileName = (Optional) auxiliary profile input file name InnerRadius = Lat-lon radius within which weight for auxiliary profile is 1.0 - (Use InnerRadius = 0.0 for no profile input) - OuterRadius = Lat-lon radius beyond which weight for auxiliary profile is 0.0 - + (Use InnerRadius = 0.0 for no profile input) + OuterRadius = Lat-lon radius beyond which weight for auxiliary profile is 0.0 + FastModeOn = Flags use of faster ephemeris computations (less accurate) 0 Most accurate ephemeris computations are used 1 Faster computations with slight loss in accuracy - ExtraPrecision = For the new column output format, this parameter + ExtraPrecision = For the new column output format, this parameter adds precision to all outputs. diff --git a/supportData/AtmosphereData/support/MarsGRAM_bsk_namelist.txt b/supportData/AtmosphereData/support/MarsGRAM_bsk_namelist.txt index 5eaa23073b..a559360741 100644 --- a/supportData/AtmosphereData/support/MarsGRAM_bsk_namelist.txt +++ b/supportData/AtmosphereData/support/MarsGRAM_bsk_namelist.txt @@ -81,24 +81,24 @@ TRAJFL = (Optional) Trajectory input file. File contains time (sec) relative to start time, height (km), latitude (deg), longitude (deg W if LonEW=0, deg E if LonEW=1, see below) - profile = (Optional) auxiliary profile input file name + profile = (Optional) auxiliary profile input file name WaveFile = (Optional) file for time-dependent wave coefficient data. See file description under parameter iuwave, below. - DATADIR = Directory for COSPAR data and topographic height data + DATADIR = Directory for COSPAR data and topographic height data GCMDIR = Directory for GCM binary data files - IERT = 1 for time input as Earth-Receive time (ERT) or 0 Mars-event + IERT = 1 for time input as Earth-Receive time (ERT) or 0 Mars-event time (MET) - IUTC = 1 for time input as Coordinated Universal Time (UTC), or 0 + IUTC = 1 for time input as Coordinated Universal Time (UTC), or 0 for Terrestrial (Dynamical) Time (TT) MONTH = (Integer) month of year MDAY = (Integer) day of month MYEAR = (Integer) year (4-digit; 1970-2069 can be 2-digit) NPOS = max # positions to evaluate (0 = read data from trajectory input file) - IHR = Hour of day (ERT or MET, controlled by IERT and UTC or TT, + IHR = Hour of day (ERT or MET, controlled by IERT and UTC or TT, controlled by IUTC) IMIN = minute of hour (meaning controlled by IERT and IUTC) - SEC = seconds of minute (meaning controlled by IERT and IUTC). + SEC = seconds of minute (meaning controlled by IERT and IUTC). IHR:IMIN:SEC is time for initial position to be evaluated LonEW = 0 for input and output West longitudes positive; 1 for East longitudes positive @@ -118,8 +118,8 @@ DUSTLAT = Latitude (degrees) for center of dust storm DUSTLON = Longitude (degrees) (West positive if LonEW=0, or East positive if LonEW = 1) for center of dust storm - MapYear = 1 or 2 for TES mapping year 1 or 2 GCM input data, or 0 for - Mars-GRAM 2001 GCM input data sets + MapYear = 1 or 2 for TES mapping year 1 or 2 GCM input data, or 0 for + Mars-GRAM 2001 GCM input data sets F107 = 10.7 cm solar flux (10**-22 W/cm**2 at 1 AU) NR1 = starting random number (0 < NR1 < 30000) NVARX = x-code for plotable output (1=hgt above MOLA areoid). @@ -142,7 +142,7 @@ negative offset decreases density. ibougher = 0 for no Ls-dependent (Bougher) height offset term; 1 means add Ls-dependent (Bougher) term, -A*Sin(Ls) (km), - to constant term (zoffset) [offset amplitude A = 2.5 for + to constant term (zoffset) [offset amplitude A = 2.5 for MapYear=0 or 0.5 for MapYear > 0]; 2 means use global mean height offset from data file hgtoffst.dat; 3 means use daily average height offset at local position; 4 means @@ -154,7 +154,7 @@ if LonEW = 0, Eastward positive if LonEW = 1) DELTIME = time increment (sec) between steps deltaTEX = adjustment for exospheric temperature (K) - profnear = Lat-lon radius (degrees) within which weight for auxiliary + profnear = Lat-lon radius (degrees) within which weight for auxiliary profile is 1.0 (Use profnear = 0.0 for no profile input) proffar = Lat-lon radius (degrees) beyond which weight for auxiliary profile is 0.0 @@ -166,31 +166,31 @@ NMONTE = number of Monte Carlo runs iup = 0 for no LIST and graphics output, or unit number for output WaveA0 = Mean term of longitude-dependent wave multiplier for density - WaveDate = Julian date for (primary) peak(s) of wave (0 for no traveling - component) + WaveDate = Julian date for (primary) peak(s) of wave (0 for no traveling + component) WaveA1 = Amplitude of wave-1 component of longitude-dependent wave multiplier for density Wavephi1 = Phase of wave-1 component of longitude-dependent wave multiplier (longitude, with West positive if LonEW = 0, East positive if LonEW = 1) - phi1dot = Rate of longitude movement (degrees per day) for wave-1 - component (Westward positive if LonEW = 0, Eastward + phi1dot = Rate of longitude movement (degrees per day) for wave-1 + component (Westward positive if LonEW = 0, Eastward positive if LonEW = 1) WaveA2 = Amplitude of wave-2 component of longitude-dependent wave multiplier for density Wavephi2 = Phase of wave-2 component of longitude-dependent wave multiplier (longitude, with West positive if LonEW = 0, East positive if LonEW = 1) - phi2dot = Rate of longitude movement (degrees per day) for wave-2 - component (Westward positive if LonEW = 0, Eastward + phi2dot = Rate of longitude movement (degrees per day) for wave-2 + component (Westward positive if LonEW = 0, Eastward positive if LonEW = 1) WaveA3 = Amplitude of wave-3 component of longitude-dependent wave multiplier for density Wavephi3 = Phase of wave-3 component of longitude-dependent wave multiplier (longitude, with West positive if LonEW = 0, East positive if LonEW = 1) - phi3dot = Rate of longitude movement (degrees per day) for wave-3 - component (Westward positive if LonEW = 0, Eastward + phi3dot = Rate of longitude movement (degrees per day) for wave-3 + component (Westward positive if LonEW = 0, Eastward positive if LonEW = 1) iuwave = Unit number for (Optional) time-dependent wave coefficient data file "WaveFile" (or 0 for none). @@ -202,8 +202,8 @@ corlmin = minimum relative step size for perturbation updates (0.0-1.0); 0.0 means always update perturbations, x.x means only update perturbations when corlim > x.x - ipclat = 1 for Planeto-centric latitude and height input, + ipclat = 1 for Planeto-centric latitude and height input, 0 for Planeto-graphic latitude and height input requa = Equatorial radius (km) for reference ellipsoid - rpole = Polar radius (km) for reference ellipsoid + rpole = Polar radius (km) for reference ellipsoid idaydata = 1 for daily max/min data output; 0 for none diff --git a/supportData/AtmosphereData/support/NeptuneGRAM_bsk_namelist.txt b/supportData/AtmosphereData/support/NeptuneGRAM_bsk_namelist.txt index 457406851e..5120d4a7f4 100644 --- a/supportData/AtmosphereData/support/NeptuneGRAM_bsk_namelist.txt +++ b/supportData/AtmosphereData/support/NeptuneGRAM_bsk_namelist.txt @@ -1,9 +1,9 @@ $INPUT - SpicePath = '\spice' - ListFileName = 'C:\GRAMs\GRAM_Suite_1.3\Neptune\IOfiles\NeptuneGRAMLIST' - ColumnFileName = 'C:\GRAMs\GRAM_Suite_1.3\Neptune\IOfiles\NeptuneGRAMNominal' - EastLongitudePositive = 0 - + SpicePath = '\spice' + ListFileName = 'C:\GRAMs\GRAM_Suite_1.3\Neptune\IOfiles\NeptuneGRAMLIST' + ColumnFileName = 'C:\GRAMs\GRAM_Suite_1.3\Neptune\IOfiles\NeptuneGRAMNominal' + EastLongitudePositive = 0 + TimeFrame = 1 TimeScale = 1 Month = 8 @@ -13,90 +13,90 @@ Minute = 45 Seconds = 0.0 - InitialRandomSeed = 1234 - DensityPerturbationScale = 1.0 - EWWindPerturbationScale = 1.0 - NSWindPerturbationScale = 1.0 + InitialRandomSeed = 1234 + DensityPerturbationScale = 1.0 + EWWindPerturbationScale = 1.0 + NSWindPerturbationScale = 1.0 MinimumRelativeStepSize = 0.0 - TrajectoryFileName = 'null' - NumberOfPositions = 2001 - InitialHeight = 0.0 - InitialLatitude = 22.0 - InitialLongitude = 48.0 - DeltaHeight = 1.0 - DeltaLatitude = 0. - DeltaLongitude = 0. + TrajectoryFileName = 'null' + NumberOfPositions = 2001 + InitialHeight = 0.0 + InitialLatitude = 22.0 + InitialLongitude = 48.0 + DeltaHeight = 1.0 + DeltaLatitude = 0. + DeltaLongitude = 0. DeltaTime = 0. AuxiliaryAtmosphereFileName = 'null' InnerRadius = 0.0 OuterRadius = 0.0 - NumberOfMonteCarloRuns = 1 - - MinMaxFactor = 0.0 + NumberOfMonteCarloRuns = 1 + + MinMaxFactor = 0.0 ComputeMinMaxFactor = 1 DinitrogenMoleFraction = 0.0 - + FastModeOn = 0 ExtraPrecision = 0 - UseLegacyOutputs = 0 - DensityPrintScale = 0 + UseLegacyOutputs = 0 + DensityPrintScale = 0 - $END + $END Explanation of variables: SpicePath = Path to NAIF Spice data ListFileName = List file name - ColumnFileName = Output file name + ColumnFileName = Output file name EastLongitudePositive = 0 for input and output West longitudes positive 1 for East longitudes positive TimeFrame = 0 Planet event time (PET) 1 for time input as Earth-receive time (ERT) - + TimeScale = 0 for Terrestrial (Dynamical) Time (TDT) 1 for time input as Coordinated Universal Time (UTC) 2 for Barycentric Dynamical Time (TDB) - Month = month of year - Day = day of month + Month = month of year + Day = day of month Year = year (4-digit, or 1970-2069 can be 2-digit) - Hour = hour of day (meaning controlled by TimeFrame and TimeScale) - Minute = minute of hour (meaning controlled by TimeFrame and TimeScale) - Seconds = seconds of minute (meaning controlled by TimeFrame and TimeScale) - - InitialRandomSeed = starting random number (0 - 2^24 = 16,777,216) - DensityPerturbationScale = random perturbation scale factor for density (0 - 2) - EWWindPerturbationScale = random perturbation scale factor for east/west winds (0 - 2) - NSWindPerturbationScale = random perturbation scale factor for north/south winds (0 - 2) - PerturbationScales = sets all perturbation scale factors (0 - 2) + Hour = hour of day (meaning controlled by TimeFrame and TimeScale) + Minute = minute of hour (meaning controlled by TimeFrame and TimeScale) + Seconds = seconds of minute (meaning controlled by TimeFrame and TimeScale) + + InitialRandomSeed = starting random number (0 - 2^24 = 16,777,216) + DensityPerturbationScale = random perturbation scale factor for density (0 - 2) + EWWindPerturbationScale = random perturbation scale factor for east/west winds (0 - 2) + NSWindPerturbationScale = random perturbation scale factor for north/south winds (0 - 2) + PerturbationScales = sets all perturbation scale factors (0 - 2) MinimumRelativeStepSize = Minimum relative step size for perturbations(0 - 1) - 0.0 means always update perturbations, + 0.0 means always update perturbations, x.x means only update perturbations when relative step size > x.x - TrajectoryFileName = (Optional) Trajectory input file name - If present, then the values below are ignored - NumberOfPositions = number of positions to evaluate - InitialHeight = initial height (km) - InitialLatitude = initial latitude (N positive), degrees - InitialLongitude = initial longitude, degrees + TrajectoryFileName = (Optional) Trajectory input file name + If present, then the values below are ignored + NumberOfPositions = number of positions to evaluate + InitialHeight = initial height (km) + InitialLatitude = initial latitude (N positive), degrees + InitialLongitude = initial longitude, degrees (depends on EastLongitudePositive) DeltaHeight = height increment (km) between steps - DeltaLatitude = latitude increment (deg) between steps + DeltaLatitude = latitude increment (deg) between steps DeltaLongitude = longitude increment (deg) between steps - (depends on EastLongitudePositive) + (depends on EastLongitudePositive) DeltaTime = time increment (seconds) between steps. - - AuxiliaryAtmosphereFileName = (Optional) auxiliary profile input file name + + AuxiliaryAtmosphereFileName = (Optional) auxiliary profile input file name InnerRadius = Lat-lon radius within which weight for auxiliary profile is 1.0 - (Use InnerRadius = 0.0 for no profile input) - OuterRadius = Lat-lon radius beyond which weight for auxiliary profile is 0.0 - - NumberOfMonteCarloRuns = the number of Monte Carlo runs + (Use InnerRadius = 0.0 for no profile input) + OuterRadius = Lat-lon radius beyond which weight for auxiliary profile is 0.0 + + NumberOfMonteCarloRuns = the number of Monte Carlo runs - MinMaxFactor = Factor (-1. to +1. to vary between minimum and + MinMaxFactor = Factor (-1. to +1. to vary between minimum and maximuum allowed mean profiles ComputeMinMaxFactor = 0 to use Fminmax input value "as is" 1 to automatically adjust input the factor for @@ -106,15 +106,14 @@ FastModeOn = Flags use of faster ephemeris computations (less accurate) 0 Most accurate ephemeris computations are used 1 Faster computations with slight loss in accuracy - ExtraPrecision = For the new column output format, this parameter + ExtraPrecision = For the new column output format, this parameter adds precision to all outputs. UseLegacyOutputs = Flags which outputs to generate. 0 Use the new output formats. - 1 Use output formats closely matching those of the + 1 Use output formats closely matching those of the legacy NeptuneGram. DensityPrintScale = For legacy outputs only. 0 regular SI units 1 log-base-10 scale 2 percentage deviations from Mean model - 3 SI units with density in kg/km**3 - \ No newline at end of file + 3 SI units with density in kg/km**3 diff --git a/supportData/AtmosphereData/support/README.txt b/supportData/AtmosphereData/support/README.txt index 8b2032efec..da7ef46327 100644 --- a/supportData/AtmosphereData/support/README.txt +++ b/supportData/AtmosphereData/support/README.txt @@ -7,7 +7,7 @@ Overview =============================================================================== The directory supportData/AtmosphereData provides a number of data files to be used by the TabularAtmosphere module. This module interpolates -from a user-provided table of altitude, density, and temperature. +from a user-provided table of altitude, density, and temperature. Any user-supplied data that is in ascending order and units of meters, kg/m^3, and Kelvin will work. @@ -31,12 +31,12 @@ The 1976 version of the US Standard Atmosphere is defined in a report of the same name from NOAA, NASA, and the USAF, currently available at the link below: https://www.ngdc.noaa.gov/stp/space-weather/online-publications/... miscellaneous/us-standard-atmosphere-1976/... - us-standard-atmosphere_st76-1562_noaa.pdf + us-standard-atmosphere_st76-1562_noaa.pdf The provided data table was generated using the webapp available at: https://www.digitaldutch.com/atmoscalc/table.htm -Temperature and Density were output in Kelvin and kilograms/cubic meter, +Temperature and Density were output in Kelvin and kilograms/cubic meter, respectively. A minimum altitude of -5000 m and max of 86000 m were used with a 500 meter increment. @@ -49,11 +49,11 @@ EarthGRAMNominal.txt =============================================================================== The 2016 version of the Global Reference Atmospheric Model for Earth, or Earth-GRAM2016, was used to generate this data. GRAM software is under General -Public Release from NASA but must be requested for download: +Public Release from NASA but must be requested for download: https://software.nasa.gov/software/MFS-32780-2 GRAM is run with numerous required inputs included in a namelist file. The file -used to generate the data included here is provided in this directory as +used to generate the data included here is provided in this directory as EarthGRAM_bsk_namelist.txt, which also includes brief descriptions of each input variable. See the GRAM documentation for more info. @@ -70,12 +70,12 @@ change the value of mc in the namelist file to equal N and re-run. =============================================================================== NRLMSISE00Nominal.txt =============================================================================== -The 2000 version of the US Naval Research Laboratory mass spectrometer and +The 2000 version of the US Naval Research Laboratory mass spectrometer and incoherent radar (exosphere), or NRLMSISE-00, was used to generate this data. The sourcecode is available for download and Basilisk also has functionality to run MSIS-in-the-loop for more precise density values. As an additional option, this data file is a single nominal data table for a columnar atmosphere. The -data was generated using the webapp available at: +data was generated using the webapp available at: https://ccmc.gsfc.nasa.gov/modelweb/models/nrlmsise00.php The inputs, also listed in the header of the data file, are: date of 4/1/21 at @@ -88,19 +88,19 @@ grams/cubic centimeter, and 3 is temperature in Kelvin. =============================================================================== MarsGRAMNominal.txt =============================================================================== -The 2010 version of the Global Reference Atmospheric Model for Mars, or +The 2010 version of the Global Reference Atmospheric Model for Mars, or Mars-GRAM2010, was used to generate this data. GRAM software is under General -Public Release from NASA but must be requested for download: +Public Release from NASA but must be requested for download: https://software.nasa.gov/software/MFS-33158-1 GRAM is run with numerous required inputs included in a namelist file. The file -used to generate the data included here is provided in this directory as +used to generate the data included here is provided in this directory as MarsGRAM_bsk_namelist.txt, which also includes brief descriptions of each input variable. See the GRAM documentation for more info. In the provided file, HgtMOLA is altitude in kilometers, Denkgm3 is the mean density in kilograms/cubic meter, and Temp is the avg. temperature in Kelvin. -Perturbation values are also included, but for this nominal output the mean +Perturbation values are also included, but for this nominal output the mean values are more appropriate, & these are what's read in by the python function. The provided data is a single columnar (only altitude varies) atmosphere table. @@ -111,13 +111,13 @@ change the value of NMONTE in the namelist file to equal N and re-run. =============================================================================== VenusGRAMNominal.csv =============================================================================== -The 2021 version of the Global Reference Atmospheric Model for Venus, or +The 2021 version of the Global Reference Atmospheric Model for Venus, or Venus-GRAM2021, was used to generate this data. GRAM software is under General -Public Release from NASA but must be requested for download: +Public Release from NASA but must be requested for download: https://software.nasa.gov/software/MFS-33888-1 GRAM is run with numerous required inputs included in a namelist file. The file -used to generate the data included here is provided in this directory as +used to generate the data included here is provided in this directory as VenusGRAM_bsk_namelist.txt, which also includes brief descriptions of each input variable. See the GRAM documentation for more info. @@ -136,13 +136,13 @@ and re-run. =============================================================================== TitanGRAMNominal.csv =============================================================================== -The 2021b version of the Global Reference Atmospheric Model for Titan, or +The 2021b version of the Global Reference Atmospheric Model for Titan, or Titan-GRAM2021b, was used to generate this data. GRAM software is under General -Public Release from NASA but must be requested for download: +Public Release from NASA but must be requested for download: https://software.nasa.gov/software/MFS-33888-1 GRAM is run with numerous required inputs included in a namelist file. The file -used to generate the data included here is provided in this directory as +used to generate the data included here is provided in this directory as TitanGRAM_bsk_namelist.txt, which also includes brief descriptions of each input variable. See the GRAM documentation for more info. @@ -160,13 +160,13 @@ and re-run. =============================================================================== NeptuneGRAMNominal.csv =============================================================================== -The 2019c version of the Global Reference Atmospheric Model for Neptune, or +The 2019c version of the Global Reference Atmospheric Model for Neptune, or Neptune-GRAM2019c, was used to generate this data. GRAM software is under -General Public Release from NASA but must be requested for download: +General Public Release from NASA but must be requested for download: https://software.nasa.gov/software/MFS-33888-1 GRAM is run with numerous required inputs included in a namelist file. The file -used to generate the data included here is provided in this directory as +used to generate the data included here is provided in this directory as NeptuneGRAM_bsk_namelist.txt, which also includes brief descriptions of each input variable. See the GRAM documentation for more info. @@ -184,13 +184,13 @@ and re-run. =============================================================================== UranusGRAMNominal.csv =============================================================================== -The 2021a version of the Global Reference Atmospheric Model for Uranus, or +The 2021a version of the Global Reference Atmospheric Model for Uranus, or Uranus-GRAM2021a, was used to generate this data. GRAM software is under -General Public Release from NASA but must be requested for download: +General Public Release from NASA but must be requested for download: https://software.nasa.gov/software/MFS-33888-1 GRAM is run with numerous required inputs included in a namelist file. The file -used to generate the data included here is provided in this directory as +used to generate the data included here is provided in this directory as UranusGRAM_bsk_namelist.txt, which also includes brief descriptions of each input variable. See the GRAM documentation for more info. @@ -208,13 +208,13 @@ and re-run. =============================================================================== JupiterGRAMNominal.csv =============================================================================== -The 2021 version of the Global Reference Atmospheric Model for Jupiter, or +The 2021 version of the Global Reference Atmospheric Model for Jupiter, or Jupiter-GRAM2021, was used to generate this data. GRAM software is under -General Public Release from NASA but must be requested for download: +General Public Release from NASA but must be requested for download: https://software.nasa.gov/software/MFS-33888-1 GRAM is run with numerous required inputs included in a namelist file. The file -used to generate the data included here is provided in this directory as +used to generate the data included here is provided in this directory as JupiterGRAM_bsk_namelist.txt, which also includes brief descriptions of each input variable. See the GRAM documentation for more info. @@ -225,4 +225,4 @@ also included, but for this nominal output the mean values are more appropriate, and these are what is read in by the python function. JupiterGRAM does not currently include a perturbation model for use in Monte -Carlo analyses. \ No newline at end of file +Carlo analyses. diff --git a/supportData/AtmosphereData/support/TitanGRAM_bsk_namelist.txt b/supportData/AtmosphereData/support/TitanGRAM_bsk_namelist.txt index 040e551203..416d5931b5 100644 --- a/supportData/AtmosphereData/support/TitanGRAM_bsk_namelist.txt +++ b/supportData/AtmosphereData/support/TitanGRAM_bsk_namelist.txt @@ -1,9 +1,9 @@ $INPUT - SpicePath = '\spice' - ListFileName = 'C:\GRAMs\GRAM_Suite_1.3\Titan\IOfiles\TitanGRAMLIST' - ColumnFileName = 'C:\GRAMs\GRAM_Suite_1.3\Titan\IOfiles\TitanGRAMNominal' - EastLongitudePositive = 0 - + SpicePath = '\spice' + ListFileName = 'C:\GRAMs\GRAM_Suite_1.3\Titan\IOfiles\TitanGRAMLIST' + ColumnFileName = 'C:\GRAMs\GRAM_Suite_1.3\Titan\IOfiles\TitanGRAMNominal' + EastLongitudePositive = 0 + TimeFrame = 1 TimeScale = 1 Month = 8 @@ -13,113 +13,112 @@ Minute = 45 Seconds = 0.0 - InitialRandomSeed = 1234 - DensityPerturbationScale = 1.0 - EWWindPerturbationScale = 1.0 - NSWindPerturbationScale = 1.0 + InitialRandomSeed = 1234 + DensityPerturbationScale = 1.0 + EWWindPerturbationScale = 1.0 + NSWindPerturbationScale = 1.0 MinimumRelativeStepSize = 0.0 - TrajectoryFileName = 'null' - NumberOfPositions = 2001 - InitialHeight = 0.0 - InitialLatitude = 22.0 - InitialLongitude = 48.0 - DeltaHeight = 0.5 - DeltaLatitude = 0. - DeltaLongitude = 0. + TrajectoryFileName = 'null' + NumberOfPositions = 2001 + InitialHeight = 0.0 + InitialLatitude = 22.0 + InitialLongitude = 48.0 + DeltaHeight = 0.5 + DeltaLatitude = 0. + DeltaLongitude = 0. DeltaTime = 0. AuxiliaryAtmosphereFileName = 'null' InnerRadius = 0.0 OuterRadius = 0.0 - NumberOfMonteCarloRuns = 1 - + NumberOfMonteCarloRuns = 1 + ModelType = 1 - MinMaxFactor = 0.0 + MinMaxFactor = 0.0 ComputeMinMaxFactor = 1 MethaneMoleFraction = 0.0 - + FastModeOn = 0 ExtraPrecision = 0 - UseLegacyOutputs = 0 - DensityPrintScale = 0 + UseLegacyOutputs = 0 + DensityPrintScale = 0 - $END + $END Explanation of variables: SpicePath = Path to NAIF Spice data ListFileName = List file name - ColumnFileName = Output file name + ColumnFileName = Output file name EastLongitudePositive = 0 for input and output West longitudes positive 1 for East longitudes positive TimeFrame = 0 Planet event time (PET) 1 for time input as Earth-receive time (ERT) - + TimeScale = 0 for Terrestrial (Dynamical) Time (TDT) 1 for time input as Coordinated Universal Time (UTC) 2 for Barycentric Dynamical Time (TDB) - Month = month of year - Day = day of month + Month = month of year + Day = day of month Year = year (4-digit, or 1970-2069 can be 2-digit) - Hour = hour of day (meaning controlled by TimeFrame and TimeScale) - Minute = minute of hour (meaning controlled by TimeFrame and TimeScale) - Seconds = seconds of minute (meaning controlled by TimeFrame and TimeScale) - - InitialRandomSeed = starting random number (0 - 2^24 = 16,777,216) - DensityPerturbationScale = random perturbation scale factor for density (0 - 2) - EWWindPerturbationScale = random perturbation scale factor for east/west winds (0 - 2) - NSWindPerturbationScale = random perturbation scale factor for north/south winds (0 - 2) - PerturbationScales = sets all perturbation scale factors (0 - 2) + Hour = hour of day (meaning controlled by TimeFrame and TimeScale) + Minute = minute of hour (meaning controlled by TimeFrame and TimeScale) + Seconds = seconds of minute (meaning controlled by TimeFrame and TimeScale) + + InitialRandomSeed = starting random number (0 - 2^24 = 16,777,216) + DensityPerturbationScale = random perturbation scale factor for density (0 - 2) + EWWindPerturbationScale = random perturbation scale factor for east/west winds (0 - 2) + NSWindPerturbationScale = random perturbation scale factor for north/south winds (0 - 2) + PerturbationScales = sets all perturbation scale factors (0 - 2) MinimumRelativeStepSize = Minimum relative step size for perturbations(0 - 1) - 0.0 means always update perturbations, + 0.0 means always update perturbations, x.x means only update perturbations when relative step size > x.x - TrajectoryFileName = (Optional) Trajectory input file name - If present, then the values below are ignored - NumberOfPositions = number of positions to evaluate - InitialHeight = initial height (km) - InitialLatitude = initial latitude (N positive), degrees - InitialLongitude = initial longitude, degrees + TrajectoryFileName = (Optional) Trajectory input file name + If present, then the values below are ignored + NumberOfPositions = number of positions to evaluate + InitialHeight = initial height (km) + InitialLatitude = initial latitude (N positive), degrees + InitialLongitude = initial longitude, degrees (depends on EastLongitudePositive) DeltaHeight = height increment (km) between steps - DeltaLatitude = latitude increment (deg) between steps + DeltaLatitude = latitude increment (deg) between steps DeltaLongitude = longitude increment (deg) between steps - (depends on EastLongitudePositive) + (depends on EastLongitudePositive) DeltaTime = time increment (seconds) between steps. - - AuxiliaryAtmosphereFileName = (Optional) auxiliary profile input file name + + AuxiliaryAtmosphereFileName = (Optional) auxiliary profile input file name InnerRadius = Lat-lon radius within which weight for auxiliary profile is 1.0 - (Use InnerRadius = 0.0 for no profile input) - OuterRadius = Lat-lon radius beyond which weight for auxiliary profile is 0.0 - - NumberOfMonteCarloRuns = the number of Monte Carlo runs + (Use InnerRadius = 0.0 for no profile input) + OuterRadius = Lat-lon radius beyond which weight for auxiliary profile is 0.0 + + NumberOfMonteCarloRuns = the number of Monte Carlo runs ModelType = 1 to use the Yelle Min/Max data model 2 to use the Titan GCM model - MinMaxFactor = Factor (-1. to +1. to vary between minimum and + MinMaxFactor = Factor (-1. to +1. to vary between minimum and maximuum allowed mean profiles ComputeMinMaxFactor = 0 to use MinMaxFactor input value "as is" 1 to automatically adjust input the factor for seasonal, latitude, and time-of-day effects - MethaneMoleFraction = Methane mole fraction override (percent) 1.0 to 5.0. + MethaneMoleFraction = Methane mole fraction override (percent) 1.0 to 5.0. Input = 0.0 (default) means use modelled values. FastModeOn = Flags use of faster ephemeris computations (less accurate) 0 Most accurate ephemeris computations are used 1 Faster computations with slight loss in accuracy - ExtraPrecision = For the new column output format, this parameter + ExtraPrecision = For the new column output format, this parameter adds precision to all outputs. UseLegacyOutputs = Flags which outputs to generate. 0 Use the new output formats. - 1 Use output formats closely matching those of the + 1 Use output formats closely matching those of the legacy NeptuneGram. DensityPrintScale = For legacy outputs only. 0 regular SI units 1 log-base-10 scale 2 percentage deviations from Mean model - 3 SI units with density in kg/km**3 - \ No newline at end of file + 3 SI units with density in kg/km**3 diff --git a/supportData/AtmosphereData/support/UranusGRAM_bsk_namelist.txt b/supportData/AtmosphereData/support/UranusGRAM_bsk_namelist.txt index 1662a77263..839b2c79f8 100644 --- a/supportData/AtmosphereData/support/UranusGRAM_bsk_namelist.txt +++ b/supportData/AtmosphereData/support/UranusGRAM_bsk_namelist.txt @@ -1,9 +1,9 @@ $INPUT - SpicePath = '\spice' - ListFileName = 'C:\GRAMs\GRAM_Suite_1.3\Uranus\IOfiles\UranusGRAMLIST' - ColumnFileName = 'C:\GRAMs\GRAM_Suite_1.3\Uranus\IOfiles\UranusGRAMNominal' - EastLongitudePositive = 0 - + SpicePath = '\spice' + ListFileName = 'C:\GRAMs\GRAM_Suite_1.3\Uranus\IOfiles\UranusGRAMLIST' + ColumnFileName = 'C:\GRAMs\GRAM_Suite_1.3\Uranus\IOfiles\UranusGRAMNominal' + EastLongitudePositive = 0 + TimeFrame = 1 TimeScale = 1 Month = 3 @@ -13,82 +13,81 @@ Minute = 30 Seconds = 0.0 - InitialRandomSeed = 1234 - DensityPerturbationScale = 1.0 + InitialRandomSeed = 1234 + DensityPerturbationScale = 1.0 MinimumRelativeStepSize = 0.0 - TrajectoryFileName = 'null' - NumberOfPositions = 2001 - InitialHeight = 0.0 - InitialLatitude = 22.0 - InitialLongitude = 48.0 - DeltaHeight = 1.0 - DeltaLatitude = 0. - DeltaLongitude = 0. + TrajectoryFileName = 'null' + NumberOfPositions = 2001 + InitialHeight = 0.0 + InitialLatitude = 22.0 + InitialLongitude = 48.0 + DeltaHeight = 1.0 + DeltaLatitude = 0. + DeltaLongitude = 0. DeltaTime = 0. AuxiliaryAtmosphereFileName = 'null' InnerRadius = 0.0 OuterRadius = 0.0 - NumberOfMonteCarloRuns = 1 - + NumberOfMonteCarloRuns = 1 + FastModeOn = 0 ExtraPrecision = 0 - $END + $END Explanation of variables: SpicePath = Path to NAIF Spice data ListFileName = List file name - ColumnFileName = Output file name + ColumnFileName = Output file name EastLongitudePositive = 0 for input and output West longitudes positive 1 for East longitudes positive TimeFrame = 0 Planet event time (PET) 1 for time input as Earth-receive time (ERT) - + TimeScale = 0 for Terrestrial (Dynamical) Time (TDT) 1 for time input as Coordinated Universal Time (UTC) 2 for Barycentric Dynamical Time (TDB) - Month = month of year - Day = day of month + Month = month of year + Day = day of month Year = year (4-digit, or 1970-2069 can be 2-digit) - Hour = hour of day (meaning controlled by TimeFrame and TimeScale) - Minute = minute of hour (meaning controlled by TimeFrame and TimeScale) - Seconds = seconds of minute (meaning controlled by TimeFrame and TimeScale) + Hour = hour of day (meaning controlled by TimeFrame and TimeScale) + Minute = minute of hour (meaning controlled by TimeFrame and TimeScale) + Seconds = seconds of minute (meaning controlled by TimeFrame and TimeScale) - InitialRandomSeed = starting random number (0 - 2^24 = 16,777,216) - DensityPerturbationScale = random perturbation scale factor for density (0 - 2) - PerturbationScales = sets all perturbation scale factors (0 - 2) + InitialRandomSeed = starting random number (0 - 2^24 = 16,777,216) + DensityPerturbationScale = random perturbation scale factor for density (0 - 2) + PerturbationScales = sets all perturbation scale factors (0 - 2) MinimumRelativeStepSize = Minimum relative step size for perturbations(0 - 1) - 0.0 means always update perturbations, + 0.0 means always update perturbations, x.x means only update perturbations when relative step size > x.x - TrajectoryFileName = (Optional) Trajectory input file name - If present, then the values below are ignored - NumberOfPositions = number of positions to evaluate - InitialHeight = initial height (km) - InitialLatitude = initial latitude (N positive), degrees - InitialLongitude = initial longitude, degrees + TrajectoryFileName = (Optional) Trajectory input file name + If present, then the values below are ignored + NumberOfPositions = number of positions to evaluate + InitialHeight = initial height (km) + InitialLatitude = initial latitude (N positive), degrees + InitialLongitude = initial longitude, degrees (depends on EastLongitudePositive) DeltaHeight = height increment (km) between steps - DeltaLatitude = latitude increment (deg) between steps + DeltaLatitude = latitude increment (deg) between steps DeltaLongitude = longitude increment (deg) between steps - (depends on EastLongitudePositive) + (depends on EastLongitudePositive) DeltaTime = time increment (seconds) between steps. - - AuxiliaryAtmosphereFileName = (Optional) auxiliary profile input file name + + AuxiliaryAtmosphereFileName = (Optional) auxiliary profile input file name InnerRadius = Lat-lon radius within which weight for auxiliary profile is 1.0 - (Use InnerRadius = 0.0 for no profile input) - OuterRadius = Lat-lon radius beyond which weight for auxiliary profile is 0.0 - - NumberOfMonteCarloRuns = the number of Monte Carlo runs + (Use InnerRadius = 0.0 for no profile input) + OuterRadius = Lat-lon radius beyond which weight for auxiliary profile is 0.0 + + NumberOfMonteCarloRuns = the number of Monte Carlo runs FastModeOn = Flags use of faster ephemeris computations (less accurate) 0 Most accurate ephemeris computations are used 1 Faster computations with slight loss in accuracy - ExtraPrecision = For the new column output format, this parameter + ExtraPrecision = For the new column output format, this parameter adds precision to all outputs. - \ No newline at end of file diff --git a/supportData/AtmosphereData/support/VenusGRAM_bsk_namelist.txt b/supportData/AtmosphereData/support/VenusGRAM_bsk_namelist.txt index 3592b2e259..546f3181cd 100644 --- a/supportData/AtmosphereData/support/VenusGRAM_bsk_namelist.txt +++ b/supportData/AtmosphereData/support/VenusGRAM_bsk_namelist.txt @@ -1,9 +1,9 @@ $INPUT - SpicePath = '\spice' - ListFileName = 'C:\GRAMs\GRAM_Suite_1.3\Venus\IOfiles\venusGRAMLIST' - ColumnFileName = 'C:\GRAMs\GRAM_Suite_1.3\Venus\IOfiles\VenusGRAMNominal' - EastLongitudePositive = 0 - + SpicePath = '\spice' + ListFileName = 'C:\GRAMs\GRAM_Suite_1.3\Venus\IOfiles\venusGRAMLIST' + ColumnFileName = 'C:\GRAMs\GRAM_Suite_1.3\Venus\IOfiles\VenusGRAMNominal' + EastLongitudePositive = 0 + TimeFrame = 1 TimeScale = 1 Month = 3 @@ -13,97 +13,96 @@ Minute = 30 Seconds = 0.0 - InitialRandomSeed = 1234 - DensityPerturbationScale = 1.0 - EWWindPerturbationScale = 1.0 - NSWindPerturbationScale = 1.0 + InitialRandomSeed = 1234 + DensityPerturbationScale = 1.0 + EWWindPerturbationScale = 1.0 + NSWindPerturbationScale = 1.0 MinimumRelativeStepSize = 0.0 - TrajectoryFileName = 'null' - NumberOfPositions = 401 - InitialHeight = 0.0 - InitialLatitude = 22.0 - InitialLongitude = 48.0 - DeltaHeight = 0.5 - DeltaLatitude = 0. - DeltaLongitude = 0. + TrajectoryFileName = 'null' + NumberOfPositions = 401 + InitialHeight = 0.0 + InitialLatitude = 22.0 + InitialLongitude = 48.0 + DeltaHeight = 0.5 + DeltaLatitude = 0. + DeltaLongitude = 0. DeltaTime = 0. AuxiliaryAtmosphereFileName = 'null' InnerRadius = 0.0 OuterRadius = 0.0 - NumberOfMonteCarloRuns = 1 - + NumberOfMonteCarloRuns = 1 + FastModeOn = 0 ExtraPrecision = 0 - UseLegacyOutputs = 0 - DensityPrintScale = 0 + UseLegacyOutputs = 0 + DensityPrintScale = 0 - $END + $END Explanation of variables: SpicePath = Path to NAIF Spice data ListFileName = List file name - ColumnFileName = Output file name + ColumnFileName = Output file name EastLongitudePositive = 0 for input and output West longitudes positive 1 for East longitudes positive TimeFrame = 0 Planet event time (PET) 1 for time input as Earth-receive time (ERT) - + TimeScale = 0 for Terrestrial (Dynamical) Time (TDT) 1 for time input as Coordinated Universal Time (UTC) 2 for Barycentric Dynamical Time (TDB) - Month = month of year - Day = day of month + Month = month of year + Day = day of month Year = year (4-digit, or 1970-2069 can be 2-digit) - Hour = hour of day (meaning controlled by TimeFrame and TimeScale) - Minute = minute of hour (meaning controlled by TimeFrame and TimeScale) - Seconds = seconds of minute (meaning controlled by TimeFrame and TimeScale) - - InitialRandomSeed = starting random number (0 - 2^24 = 16,777,216) - DensityPerturbationScale = random perturbation scale factor for density (0 - 2) - EWWindPerturbationScale = random perturbation scale factor for east/west winds (0 - 2) - NSWindPerturbationScale = random perturbation scale factor for north/south winds (0 - 2) - PerturbationScales = sets all perturbation scale factors (0 - 2) + Hour = hour of day (meaning controlled by TimeFrame and TimeScale) + Minute = minute of hour (meaning controlled by TimeFrame and TimeScale) + Seconds = seconds of minute (meaning controlled by TimeFrame and TimeScale) + + InitialRandomSeed = starting random number (0 - 2^24 = 16,777,216) + DensityPerturbationScale = random perturbation scale factor for density (0 - 2) + EWWindPerturbationScale = random perturbation scale factor for east/west winds (0 - 2) + NSWindPerturbationScale = random perturbation scale factor for north/south winds (0 - 2) + PerturbationScales = sets all perturbation scale factors (0 - 2) MinimumRelativeStepSize = Minimum relative step size for perturbations(0 - 1) - 0.0 means always update perturbations, + 0.0 means always update perturbations, x.x means only update perturbations when relative step size > x.x - TrajectoryFileName = (Optional) Trajectory input file name - If present, then the values below are ignored - NumberOfPositions = number of positions to evaluate - InitialHeight = initial height (km) - InitialLatitude = initial latitude (N positive), degrees - InitialLongitude = initial longitude, degrees + TrajectoryFileName = (Optional) Trajectory input file name + If present, then the values below are ignored + NumberOfPositions = number of positions to evaluate + InitialHeight = initial height (km) + InitialLatitude = initial latitude (N positive), degrees + InitialLongitude = initial longitude, degrees (depends on EastLongitudePositive) DeltaHeight = height increment (km) between steps - DeltaLatitude = latitude increment (deg) between steps + DeltaLatitude = latitude increment (deg) between steps DeltaLongitude = longitude increment (deg) between steps - (depends on EastLongitudePositive) + (depends on EastLongitudePositive) DeltaTime = time increment (seconds) between steps. - - AuxiliaryAtmosphereFileName = (Optional) auxiliary profile input file name + + AuxiliaryAtmosphereFileName = (Optional) auxiliary profile input file name InnerRadius = Lat-lon radius within which weight for auxiliary profile is 1.0 - (Use InnerRadius = 0.0 for no profile input) - OuterRadius = Lat-lon radius beyond which weight for auxiliary profile is 0.0 - - NumberOfMonteCarloRuns = the number of Monte Carlo runs + (Use InnerRadius = 0.0 for no profile input) + OuterRadius = Lat-lon radius beyond which weight for auxiliary profile is 0.0 + + NumberOfMonteCarloRuns = the number of Monte Carlo runs FastModeOn = Flags use of faster ephemeris computations (less accurate) 0 Most accurate ephemeris computations are used 1 Faster computations with slight loss in accuracy - ExtraPrecision = For the new column output format, this parameter + ExtraPrecision = For the new column output format, this parameter adds precision to all outputs. UseLegacyOutputs = Flags which outputs to generate. 0 Use the new output formats. - 1 Use output formats closely matching those of the + 1 Use output formats closely matching those of the legacy NeptuneGram. DensityPrintScale = For legacy outputs only. 0 regular SI units 1 log-base-10 scale 2 percentage deviations from Mean model - 3 SI units with density in kg/km**3 - \ No newline at end of file + 3 SI units with density in kg/km**3 diff --git a/supportData/EphemerisData/de-403-masses.tpc b/supportData/EphemerisData/de-403-masses.tpc index e5ffebca92..242b54f420 100755 --- a/supportData/EphemerisData/de-403-masses.tpc +++ b/supportData/EphemerisData/de-403-masses.tpc @@ -6,12 +6,12 @@ the value of the AU to the list of items input to the kernel pool. Use this at your own risk, for now. -The masses for the sun and barycenter given in this file are +The masses for the sun and barycenter given in this file are derived from the masses used for the integration of the planetary ephemerides DE-403. The value of the AU given in this ephemeris is. AU = 149597870.693 km - + In the ephemeris the values of the masses are given as ratios to of Solar GM to barycenter GM. These values are given here. @@ -33,7 +33,7 @@ trajectories. \begindata -AU = 149597870.693 +AU = 149597870.693 BODY1_GM = 22032.080 BODY2_GM = 324858.599 @@ -54,7 +54,7 @@ The masses of the earth and moon are taken from DE-403. \begindata -BODY199_GM = 22032.080 +BODY199_GM = 22032.080 BODY299_GM = 324858.599 BODY301_GM = 4902.799 BODY399_GM = 398600.436 diff --git a/supportData/EphemerisData/naif0011.tls b/supportData/EphemerisData/naif0011.tls index 64d0b7780f..0abaf6605c 100644 --- a/supportData/EphemerisData/naif0011.tls +++ b/supportData/EphemerisData/naif0011.tls @@ -12,19 +12,19 @@ Modifications: 2012, Jan. 5 NJB Modified file to account for the leapsecond that will occur on June 30, 2012. - + 2008, Jul. 7 NJB Modified file to account for the leapsecond that will occur on December 31, 2008. - + 2005, Aug. 3 NJB Modified file to account for the leapsecond that will occur on December 31, 2005. - + 1998, Jul 17 WLT Modified file to account for the leapsecond that will occur on December 31, 1998. - + 1997, Feb 22 WLT Modified file to account for the leapsecond that will occur on June 30, 1997. - + 1995, Dec 14 KSZ Corrected date of last leapsecond from 1-1-95 to 1-1-96. @@ -40,19 +40,19 @@ Modifications: 1992, Mar. 6 HAN Modified file to account for the leapsecond on June 30, 1992. -1990, Oct. 8 HAN Modified file to account for the leapsecond on - Dec. 31, 1990. +1990, Oct. 8 HAN Modified file to account for the leapsecond on + Dec. 31, 1990. Explanation: ------------ -The contents of this file are used by the routine DELTET to compute the +The contents of this file are used by the routine DELTET to compute the time difference -[1] DELTA_ET = ET - UTC - -the increment to be applied to UTC to give ET. +[1] DELTA_ET = ET - UTC + +the increment to be applied to UTC to give ET. The difference between UTC and TAI, @@ -68,17 +68,17 @@ is declared. Combining [1] and [2] gives The difference (ET - TAI) is periodic, and is given by -[4] ET - TAI = DELTA_T_A + K sin E +[4] ET - TAI = DELTA_T_A + K sin E -where DELTA_T_A and K are constant, and E is the eccentric anomaly of the -heliocentric orbit of the Earth-Moon barycenter. Equation [4], which ignores +where DELTA_T_A and K are constant, and E is the eccentric anomaly of the +heliocentric orbit of the Earth-Moon barycenter. Equation [4], which ignores small-period fluctuations, is accurate to about 0.000030 seconds. -The eccentric anomaly E is given by +The eccentric anomaly E is given by [5] E = M + EB sin M -where M is the mean anomaly, which in turn is given by +where M is the mean anomaly, which in turn is given by [6] M = M + M t 0 1 @@ -96,7 +96,7 @@ Thus, in order to compute DELTA_ET, the following items are necessary. The numbers, and the formulation, are taken from the following sources. - 1) Moyer, T.D., Transformation from Proper Time on Earth to + 1) Moyer, T.D., Transformation from Proper Time on Earth to Coordinate Time in Solar System Barycentric Space-Time Frame of Reference, Parts 1 and 2, Celestial Mechanics 23 (1981), 33-56 and 57-68. @@ -105,7 +105,7 @@ The numbers, and the formulation, are taken from the following sources. Reference System on Algorithms for Computing Time Differences and Clock Rates, JPL IOM 314.5--942, 1 October 1985. -The variable names used above are consistent with those used in the +The variable names used above are consistent with those used in the Astronomical Almanac. \begindata @@ -116,26 +116,26 @@ DELTET/EB = 1.671D-2 DELTET/M = ( 6.239996D0 1.99096871D-7 ) DELTET/DELTA_AT = ( 10, @1972-JAN-1 - 11, @1972-JUL-1 - 12, @1973-JAN-1 - 13, @1974-JAN-1 - 14, @1975-JAN-1 - 15, @1976-JAN-1 - 16, @1977-JAN-1 - 17, @1978-JAN-1 - 18, @1979-JAN-1 - 19, @1980-JAN-1 - 20, @1981-JUL-1 - 21, @1982-JUL-1 - 22, @1983-JUL-1 - 23, @1985-JUL-1 - 24, @1988-JAN-1 + 11, @1972-JUL-1 + 12, @1973-JAN-1 + 13, @1974-JAN-1 + 14, @1975-JAN-1 + 15, @1976-JAN-1 + 16, @1977-JAN-1 + 17, @1978-JAN-1 + 18, @1979-JAN-1 + 19, @1980-JAN-1 + 20, @1981-JUL-1 + 21, @1982-JUL-1 + 22, @1983-JUL-1 + 23, @1985-JUL-1 + 24, @1988-JAN-1 25, @1990-JAN-1 - 26, @1991-JAN-1 + 26, @1991-JAN-1 27, @1992-JUL-1 28, @1993-JUL-1 29, @1994-JUL-1 - 30, @1996-JAN-1 + 30, @1996-JAN-1 31, @1997-JUL-1 32, @1999-JAN-1 33, @2006-JAN-1 @@ -144,4 +144,3 @@ DELTET/DELTA_AT = ( 10, @1972-JAN-1 36, @2015-JUL-1 ) \begintext - diff --git a/supportData/EphemerisData/naif0012.tls b/supportData/EphemerisData/naif0012.tls index e1afdee1b6..caa6a4d139 100644 --- a/supportData/EphemerisData/naif0012.tls +++ b/supportData/EphemerisData/naif0012.tls @@ -15,19 +15,19 @@ Modifications: 2012, Jan. 5 NJB Modified file to account for the leapsecond that will occur on June 30, 2012. - + 2008, Jul. 7 NJB Modified file to account for the leapsecond that will occur on December 31, 2008. - + 2005, Aug. 3 NJB Modified file to account for the leapsecond that will occur on December 31, 2005. - + 1998, Jul 17 WLT Modified file to account for the leapsecond that will occur on December 31, 1998. - + 1997, Feb 22 WLT Modified file to account for the leapsecond that will occur on June 30, 1997. - + 1995, Dec 14 KSZ Corrected date of last leapsecond from 1-1-95 to 1-1-96. @@ -43,19 +43,19 @@ Modifications: 1992, Mar. 6 HAN Modified file to account for the leapsecond on June 30, 1992. -1990, Oct. 8 HAN Modified file to account for the leapsecond on - Dec. 31, 1990. +1990, Oct. 8 HAN Modified file to account for the leapsecond on + Dec. 31, 1990. Explanation: ------------ -The contents of this file are used by the routine DELTET to compute the +The contents of this file are used by the routine DELTET to compute the time difference -[1] DELTA_ET = ET - UTC - -the increment to be applied to UTC to give ET. +[1] DELTA_ET = ET - UTC + +the increment to be applied to UTC to give ET. The difference between UTC and TAI, @@ -71,17 +71,17 @@ is declared. Combining [1] and [2] gives The difference (ET - TAI) is periodic, and is given by -[4] ET - TAI = DELTA_T_A + K sin E +[4] ET - TAI = DELTA_T_A + K sin E -where DELTA_T_A and K are constant, and E is the eccentric anomaly of the -heliocentric orbit of the Earth-Moon barycenter. Equation [4], which ignores +where DELTA_T_A and K are constant, and E is the eccentric anomaly of the +heliocentric orbit of the Earth-Moon barycenter. Equation [4], which ignores small-period fluctuations, is accurate to about 0.000030 seconds. -The eccentric anomaly E is given by +The eccentric anomaly E is given by [5] E = M + EB sin M -where M is the mean anomaly, which in turn is given by +where M is the mean anomaly, which in turn is given by [6] M = M + M t 0 1 @@ -99,7 +99,7 @@ Thus, in order to compute DELTA_ET, the following items are necessary. The numbers, and the formulation, are taken from the following sources. - 1) Moyer, T.D., Transformation from Proper Time on Earth to + 1) Moyer, T.D., Transformation from Proper Time on Earth to Coordinate Time in Solar System Barycentric Space-Time Frame of Reference, Parts 1 and 2, Celestial Mechanics 23 (1981), 33-56 and 57-68. @@ -108,7 +108,7 @@ The numbers, and the formulation, are taken from the following sources. Reference System on Algorithms for Computing Time Differences and Clock Rates, JPL IOM 314.5--942, 1 October 1985. -The variable names used above are consistent with those used in the +The variable names used above are consistent with those used in the Astronomical Almanac. \begindata @@ -119,34 +119,32 @@ DELTET/EB = 1.671D-2 DELTET/M = ( 6.239996D0 1.99096871D-7 ) DELTET/DELTA_AT = ( 10, @1972-JAN-1 - 11, @1972-JUL-1 - 12, @1973-JAN-1 - 13, @1974-JAN-1 - 14, @1975-JAN-1 - 15, @1976-JAN-1 - 16, @1977-JAN-1 - 17, @1978-JAN-1 - 18, @1979-JAN-1 - 19, @1980-JAN-1 - 20, @1981-JUL-1 - 21, @1982-JUL-1 - 22, @1983-JUL-1 - 23, @1985-JUL-1 - 24, @1988-JAN-1 + 11, @1972-JUL-1 + 12, @1973-JAN-1 + 13, @1974-JAN-1 + 14, @1975-JAN-1 + 15, @1976-JAN-1 + 16, @1977-JAN-1 + 17, @1978-JAN-1 + 18, @1979-JAN-1 + 19, @1980-JAN-1 + 20, @1981-JUL-1 + 21, @1982-JUL-1 + 22, @1983-JUL-1 + 23, @1985-JUL-1 + 24, @1988-JAN-1 25, @1990-JAN-1 - 26, @1991-JAN-1 + 26, @1991-JAN-1 27, @1992-JUL-1 28, @1993-JUL-1 29, @1994-JUL-1 - 30, @1996-JAN-1 + 30, @1996-JAN-1 31, @1997-JUL-1 32, @1999-JAN-1 33, @2006-JAN-1 34, @2009-JAN-1 35, @2012-JUL-1 - 36, @2015-JUL-1 + 36, @2015-JUL-1 37, @2017-JAN-1 ) \begintext - - diff --git a/supportData/EphemerisData/pck00010.tpc b/supportData/EphemerisData/pck00010.tpc index efa020922d..96b1e3787c 100755 --- a/supportData/EphemerisData/pck00010.tpc +++ b/supportData/EphemerisData/pck00010.tpc @@ -1,12 +1,12 @@ KPL/PCK - - + + P_constants (PcK) SPICE kernel file =========================================================================== By: Nat Bachman (NAIF) 2011 October 21 - - + + Purpose -------------------------------------------------------- @@ -27,28 +27,28 @@ Purpose File Organization -------------------------------------------------------- - + The contents of this file are as follows. - + Introductory Information: -- Purpose -- File Organization - + -- Version description - + -- Disclaimer - + -- Sources - + -- Explanatory notes - + -- Body numbers and names - + PcK Data: - + Orientation Data ---------------- @@ -60,8 +60,8 @@ File Organization for the epochs 2012 -- Orientation constants for satellites - - -- Orientation constants for asteroids + + -- Orientation constants for asteroids Davida Eros @@ -72,8 +72,8 @@ File Organization Pallas Steins Vesta - - -- Orientation constants for comets + + -- Orientation constants for comets 19P/Borrelly 9P/Tempel 1 @@ -100,24 +100,24 @@ File Organization --------------- -- Radii of Sun, planets, and Pluto - + -- Radii of satellites, where available - - -- Radii of asteroids + + -- Radii of asteroids Ceres Davida Eros Gaspra - Ida + Ida Itokawa Lutetia Mathilde Steins Toutatis Vesta - - -- Radii of comets + + -- Radii of comets 19P/Borrelly 81P/Wild 2 @@ -128,13 +128,13 @@ File Organization Version Description -------------------------------------------------------- - + This file was created on October 21, 2011 at NASA's Navigation and Ancillary Information Facility (NAIF), located at the Jet Propulsion Laboratory, Pasadena, CA. The previous version of the file was - + pck00009.tpc That file was published March 3 2010. @@ -155,12 +155,12 @@ Version Description Pallene Polydeuces Steins - - - + + + Disclaimer -------------------------------------------------------- - + Applicability of Data This P_constants file may not contain the parameter values that @@ -173,8 +173,8 @@ File Modifications by Users values or add/delete parameters. NAIF requests that you update the "by line," date, version description section, and file name if you modify this file. - - A user-modified file should be thoroughly tested before + + A user-modified file should be thoroughly tested before being published or otherwise distributed. P_constants files must conform to the standards described @@ -239,7 +239,7 @@ Known Limitations and Caveats TAI = International Atomic Time UT1 = Greenwich hour angle of computed mean sun - 12h UT1R = Regularized UT1 - GMST = Greenwich mean sidereal time + GMST = Greenwich mean sidereal time These kernels are available from the NAIF web site @@ -250,7 +250,7 @@ Known Limitations and Caveats ftp://naif.jpl.nasa.gov/pub/naif/generic_kernels/pck or via anonymous ftp from the server - + naif.jpl.nasa.gov The kernels are in the path @@ -265,7 +265,7 @@ Known Limitations and Caveats file's coverage begin time. The second and third dates are, respectively, the file's coverage end time and the epoch of the last datum. - + These binary PCK files are very accurate (error < 0.1 microradian) for epochs preceding the epoch of the last datum. For later epochs, the error rises to several microradians. @@ -298,7 +298,7 @@ Known Limitations and Caveats These files are available on the NAIF web site (see URLs above) and in the NAIF server's ftp area. The lunar frame kernel is located in the path - + pub/naif/generic_kernels/fk/satellites and has a name of the form @@ -328,16 +328,16 @@ Known Limitations and Caveats epoch 2012.0 was selected as a representative datum, and the planetocentric longitude and latitude of this location have been associated with the keywords - + BODY399_N_GEOMAG_CTR_DIPOLE_LON BODY399_N_GEOMAG_CTR_DIPOLE_LAT - + Values for the earth's north geomagnetic centered dipole are presented in comments as a discrete time series for the time range 1945-2000. For details concerning the geomagnetic field model from which these values were derived, including a discussion of the model's accuracy, see [9] and [11]. - + Prime meridian offsets ---------------------- @@ -356,8 +356,8 @@ Known Limitations and Caveats specified here should do so by creating a constant-offset frame (also called a "TK" frame) specification. See the Frames Required Reading frames.req for details. - - The Mars prime meridian offset given by [5] is provided for + + The Mars prime meridian offset given by [5] is provided for informational purposes only. @@ -367,11 +367,11 @@ Known Limitations and Caveats SPICE Toolkits prior to version N0057 cannot make use of trigonometric polynomial terms in the formulas for orientation of the planets. - + The second nutation precession angle (M2) for Mars is represented by a quadratic polynomial in the 2006 IAU report. The SPICELIB subroutine BODEUL can not handle this term (which is extremely - small), so we truncate the polynomial to a linear one. The + small), so we truncate the polynomial to a linear one. The resulting orientation error has a maximum magnitude of less than 0.0032 degrees over the time span 1996-2015 and less than 0.0082 degrees over the time span 1986-2025. @@ -379,34 +379,34 @@ Known Limitations and Caveats Sources and References -------------------------------------------------------- - + The sources for the constants listed in this file are: [1] Archinal, B.A., A'Hearn, M.F., Bowell, E., Conrad, A., - Consolmagno, G.J., Courtin, R., Fukushima, T., + Consolmagno, G.J., Courtin, R., Fukushima, T., Hestroffer, D., Hilton, J.L., Krasinsky, G.A., Neumann, G., Oberst, J., Seidelmann, P.K., Stooke, P., - Tholen, D.J., Thomas, P.C., and Williams, I.P. - "Report of the IAU Working Group on Cartographic Coordinates + Tholen, D.J., Thomas, P.C., and Williams, I.P. + "Report of the IAU Working Group on Cartographic Coordinates and Rotational Elements: 2009." [2] Archinal, B.A., A'Hearn, M.F., Conrad, A., - Consolmagno, G.J., Courtin, R., Fukushima, T., + Consolmagno, G.J., Courtin, R., Fukushima, T., Hestroffer, D., Hilton, J.L., Krasinsky, G.A., Neumann, G., Oberst, J., Seidelmann, P.K., Stooke, P., - Tholen, D.J., Thomas, P.C., and Williams, I.P. + Tholen, D.J., Thomas, P.C., and Williams, I.P. "Erratum to: Reports of the IAU Working Group on Cartographic Coordinates and Rotational Elements: 2006 & 2009." - [3] Seidelmann, P.K., Archinal, B.A., A'Hearn, M.F., + [3] Seidelmann, P.K., Archinal, B.A., A'Hearn, M.F., Conrad, A., Consolmagno, G.J., Hestroffer, D., Hilton, J.L., Krasinsky, G.A., Neumann, G., - Oberst, J., Stooke, P., Tedesco, E.F., Tholen, D.J., - and Thomas, P.C. "Report of the IAU/IAG Working Group + Oberst, J., Stooke, P., Tedesco, E.F., Tholen, D.J., + and Thomas, P.C. "Report of the IAU/IAG Working Group on cartographic coordinates and rotational elements: 2006." - + [4] Nautical Almanac Office, United States Naval Observatory and H.M. Nautical Almanac Office, Rutherford Appleton Laboratory (2010). "The Astronomical Almanac for @@ -417,28 +417,28 @@ Sources and References Conventions," presentation to the Mars Express Data Archive Working Group, Dec. 14, 2001. - [6] Russell, C.T. and Luhmann, J.G. (1990). "Earth: Magnetic + [6] Russell, C.T. and Luhmann, J.G. (1990). "Earth: Magnetic Field and Magnetosphere." . Originally published in "Encyclopedia of Planetary Sciences," J.H. Shirley and R.W. Fainbridge, eds. Chapman and Hall, New York, pp 208-211. - [7] Russell, C.T. (1971). "Geophysical Coordinate + [7] Russell, C.T. (1971). "Geophysical Coordinate Transformations," Cosmic Electrodynamics 2 184-186. NAIF document 181.0. - + [8] ESA/ESTEC Space Environment Information System (SPENVIS) (2003). Web page: "Dipole approximations of the geomagnetic field." . - + [9] International Association of Geomagnetism and Aeronomy and International Union of Geodesy and Geophysics (2004). Web page: "The 9th Generation International Geomagnetic Reference Field." . - + [10] Davies, M.E., Abalakin, V.K., Bursa, M., Hunt, G.E., and Lieske, J.H. (1989). "Report of the IAU/IAG/COSPAR Working Group on Cartographic Coordinates and Rotational @@ -447,7 +447,7 @@ Sources and References 187-204. [11] International Association of Geomagnetism and Aeronomy - Web page: "International Geomagnetic Reference Field." + Web page: "International Geomagnetic Reference Field." Discussion URL: http://www.ngdc.noaa.gov/IAGA/vmod/igrf.html @@ -456,35 +456,35 @@ Sources and References http://www.ngdc.noaa.gov/IAGA/vmod/igrf11coeffs.txt - - Most values are from [1]. All exceptions are + + Most values are from [1]. All exceptions are commented where they occur in this file. The exceptions are: - - + + -- Radii for the Sun are from [4]. -- Prime meridian constant (W0) terms for Pluto, Charon, and Ida are from [2]. - + -- The second nutation precession angle (M2) for Mars is represented by a quadratic polynomial in the 2000 IAU report. The SPICELIB subroutine BODEUL can not handle this term (which is extremely small), so we truncate the polynomial to a linear one. - + -- Earth north geomagnetic centered dipole values are from [11]. The values were also computed from the 11th generation IGRF by Nat Bachman. - + "Old values" listed are from the SPICE P_constants file pck00009.tpc dated March 3, 2010. Most of these values came from the 2006 IAU report [3]. - - - - + + + + Explanatory Notes -------------------------------------------------------- @@ -499,12 +499,12 @@ Explanatory Notes radii are equal. The SPICE Toolkit routines that use this file are documented in - the SPICE "Required Reading" file pck.req. They are also + the SPICE "Required Reading" file pck.req. They are also documented in the "PCK" SPICE tutorial, which is available on the NAIF web site. File Format - + A terse description of the PCK file format is given here. See the SPICE "Required Reading" files pck.req and kernel.req for a detailed explanation of the SPICE text kernel file format. The @@ -515,7 +515,7 @@ File Format KPL/PCK - This string identifies the file as a text kernel containing PCK + This string identifies the file as a text kernel containing PCK data. This file consists of a series of comment blocks and data blocks. @@ -539,7 +539,7 @@ File Format with each kernel variable name. Kernel variable names are case-sensitive and are limited to - 32 characters in length. + 32 characters in length. Numeric values may be integer or floating point. String values are normally limited to 80 characters in length; however, SPICE @@ -553,13 +553,13 @@ File Format The list must be bracketed by parentheses. Example: BODY399_RADII = ( 6378.1366 6378.1366 6356.7519 ) - + Any blanks preceding or following keyword names, values and equal signs are ignored. - + Assignments may be spread over multiple lines, for example: - BODY399_RADII = ( 6378.1366 + BODY399_RADII = ( 6378.1366 6378.1366 6356.7519 ) @@ -578,14 +578,14 @@ Time systems and reference frames file, we use the names "J2000 TDB" and "J2000" for this epoch. The name "J2000.0" is equivalent. - SPICE documentation refers to the time system used in this file - as either "ET" or "TDB." SPICE software makes no distinction + SPICE documentation refers to the time system used in this file + as either "ET" or "TDB." SPICE software makes no distinction between TDB and the time system associated with the independent variable of the JPL planetary ephemerides T_eph. - + The inertial reference frame used for the rotational elements in this file is identified by [1] as the ICRF (International - Celestial Reference Frame). + Celestial Reference Frame). The SPICE PCK software that reads this file uses the label "J2000" to refer to the ICRF; this is actually a mislabeling which has @@ -599,26 +599,26 @@ Time systems and reference frames accuracy level of the formulas in this file. Orientation models - + All of the orientation models use three Euler angles to describe the orientation of the coordinate axes of the "Body Equator and Prime Meridian" system with respect to an inertial system. By default, the inertial system is the ICRF (labeled as "J2000"), but other inertial frames can be specified in the file. See the PCK Required Reading for details. - + The first two angles, in order, are the ICRF right ascension and declination (henceforth RA and DEC) of the north pole of a body as a function of time. The third angle is the prime meridian location (represented by "W"), which is expressed as a rotation about the north pole, and is also a function of time. - + For each body, the expressions for the north pole's right ascension and declination, as well as prime meridian location, are sums (as far as the models that appear in this file are concerned) of quadratic polynomials and trigonometric polynomials, where the independent variable is time. - + In this file, the time arguments in expressions always refer to Barycentric Dynamical Time (TDB), measured in centuries or days past a reference epoch. By default, the reference epoch is the @@ -632,20 +632,20 @@ Orientation models In this file, we call the arguments of these trigonometric terms "nutation precession angles." - Example: 2009 IAU Model for orientation of Jupiter. Note that - these values are used as an example only; see the data area below + Example: 2009 IAU Model for orientation of Jupiter. Note that + these values are used as an example only; see the data area below for current values. Right ascension --------------- - - alpha = 268.056595 - 0.006499 T + 0.000117 sin(Ja) + + alpha = 268.056595 - 0.006499 T + 0.000117 sin(Ja) 0 + 0.000938 sin(Jb) + 0.001432 sin(Jc) + 0.000030 sin(Jd) + 0.002150 sin(Je) Declination ----------- - + delta = 64.495303 + 0.002413 T + 0.000050 cos(Ja) 0 + 0.000404 cos(Jb) + 0.000617 cos(Jc) - 0.000013 cos(Jd) + 0.000926 cos(Je) @@ -654,24 +654,24 @@ Orientation models -------------- W = 284.95 + 870.5366420 d - + Here T represents centuries past J2000 ( TDB ), - + d represents days past J2000 ( TDB ). Ja-Je are nutation precession angles. - In this file, the polynomials' coefficients above are assigned + In this file, the polynomials' coefficients above are assigned to kernel variable names (left-hand-side symbols) as follows BODY599_POLE_RA = ( 268.056595 -0.006499 0. ) BODY599_POLE_DEC = ( 64.495303 0.002413 0. ) BODY599_PM = ( 284.95 870.5360000 0. ) - and the trigonometric polynomials' coefficients are assigned + and the trigonometric polynomials' coefficients are assigned as follows BODY599_NUT_PREC_RA = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.000117 @@ -690,7 +690,7 @@ Orientation models 0.0 0.0 0.0 - 0.0 ) + 0.0 ) Note the number "599"; this is the NAIF ID code for Jupiter. @@ -709,7 +709,7 @@ Orientation models J8 = 113.35 + 6070.0 T J9 = 146.64 + 182945.8 T - J10 = 49.24 + 90274.4 T + J10 = 49.24 + 90274.4 T Ja = 99.360714 + 4850.4046 T Jb = 175.895369 + 1191.9605 T @@ -723,7 +723,7 @@ Orientation models J1-J10 and Ja-Je are the nutation precession angles. The angles J9 and J10 are equal to 2*J1 and 2*J2, respectively. - + Angles J9 and J10 are not present in [1]; they have been added to fit the terms 2*J1 and 2*J2, which appear in the orientation models of several satellites, into a form that can be accepted @@ -731,7 +731,7 @@ Orientation models The assignment of the nutation precession angles for the Jupiter system is as follows: - + BODY5_NUT_PREC_ANGLES = ( 73.32 91472.9 24.62 45137.2 283.90 4850.7 @@ -739,9 +739,9 @@ Orientation models 119.90 262.1 229.80 64.3 352.25 2382.6 - 113.35 6070.0 + 113.35 6070.0 146.64 182945.8 - 49.24 90274.4 + 49.24 90274.4 99.360714 4850.4046 175.895369 1191.9605 300.323162 262.5475 @@ -750,9 +750,9 @@ Orientation models You'll see an additional symbol grouped with the ones listed above; it is - + BODY599_LONG_AXIS - + This is a deprecated feature; see the note on "Prime meridian offsets" under "Known Limitations and Caveats" above. @@ -760,35 +760,35 @@ Orientation models to that for Jupiter. Example: 2006 IAU values for Io. Again, these values are used as an example only; see the data area below for current values. - + Right ascension --------------- alpha = 268.05 - 0.009 T + 0.094 sin(J3) + 0.024 sin(J4) - 0 + 0 Declination ----------- delta = 64.50 + 0.003 T + 0.040 cos(J3) + 0.011 cos(J4) - 0 - + 0 + Prime meridian -------------- W = 200.39 + 203.4889538 d - 0.085 sin(J3) - 0.022 sin(J4) - + d represents days past J2000. - + J3 and J4 are nutation precession angles. - + The polynomial terms are assigned to symbols by the statements - + BODY501_POLE_RA = ( 268.05 -0.009 0. ) BODY501_POLE_DEC = ( 64.50 0.003 0. ) BODY501_PM = ( 200.39 203.4889538 0. ) - + The coefficients of the trigonometric terms are assigned to symbols by the statements @@ -797,22 +797,22 @@ Orientation models BODY501_NUT_PREC_PM = ( 0. 0. -0.085 -0.022 ) 501 is the NAIF ID code for Io. - + SPICE software expects the models for satellite orientation to follow the form of the model shown here: the polynomial portions of the - RA, DEC, and W expressions are expected to be quadratic, the - trigonometric terms for RA and W (satellite prime meridian) are expected - to be linear combinations of sines of nutation precession angles, the - trigonometric terms for DEC are expected to be linear combinations of - cosines of nutation precession angles, and the polynomials for the + RA, DEC, and W expressions are expected to be quadratic, the + trigonometric terms for RA and W (satellite prime meridian) are expected + to be linear combinations of sines of nutation precession angles, the + trigonometric terms for DEC are expected to be linear combinations of + cosines of nutation precession angles, and the polynomials for the nutation precession angles themselves are expected to be linear. - + Eventually, the software will handle more complex expressions, we expect. - - + + Shape models - + There is only one kind of shape model supported by the SPICE Toolkit software at present: the triaxial ellipsoid. The 2009 IAU report [1] does not use any other models, except in the case of @@ -825,17 +825,17 @@ Shape models the largest equatorial radius (the length of the semi-axis containing the prime meridian), the second number is the smaller equatorial radius, and the third is the polar radius. - + Example: Radii of the Earth. - + BODY399_RADII = ( 6378.1366 6378.1366 6356.7519 ) - - + + Body Numbers and Names -------------------------------------------------------- - - + + The following NAIF body ID codes and body names appear in this file. See the NAIF IDs Required Reading file naif_ids.req for a detailed discussion and a complete list of ID codes and names. @@ -852,60 +852,60 @@ Body Numbers and Names 9 Pluto barycenter 10 Sun - + 199 Mercury - - + + 299 Venus - - + + 399 Earth - + 301 Moon - - + + 499 Mars - + 401 Phobos 402 Deimos - - + + 599 Jupiter - + 501 Io 502 Europa 503 Ganymede 504 Callisto 505 Amalthea 506 Himalia 507 Elara 508 Pasiphae 509 Sinope 510 Lysithea 511 Carme 512 Ananke 513 Leda 514 Thebe 515 Adrastea 516 Metis - - + + 699 Saturn - + 601 Mimas 602 Enceladus 603 Tethys 604 Dione 605 Rhea 606 Titan 607 Hyperion 608 Iapetus 609 Phoebe 610 Janus 611 Epimetheus 612 Helene 613 Telesto 614 Calypso 615 Atlas 616 Prometheus 617 Pandora 618 Pan 632 Methone 633 Pallene 634 Polydeuces 635 Daphnis 649 Anthe - - + + 799 Uranus - + 701 Ariel 702 Umbriel 703 Titania 704 Oberon 705 Miranda 706 Cordelia 707 Ophelia 708 Bianca 709 Cressida 710 Desdemona 711 Juliet 712 Portia 713 Rosalind 714 Belinda 715 Puck - - + + 899 Neptune - + 801 Triton 802 Nereid 803 Naiad 804 Thalassa 805 Despina 806 Galatea 807 Larissa 808 Proteus - - + + 999 Pluto - + 901 Charon - - + + 1000005 Comet 19P/Borrelly 1000036 Comet Halley 1000093 Comet 9P/Tempel 1 @@ -924,33 +924,33 @@ Body Numbers and Names 2025143 Asteroid Itokawa 2431010 Asteroid Ida 9511010 Asteroid Gaspra - - + + Orientation Constants for the Sun and Planets -------------------------------------------------------- - + Sun - + Old values: Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY10_POLE_RA = ( 286.13 0. 0. ) BODY10_POLE_DEC = ( 63.87 0. 0. ) BODY10_PM = ( 84.176 14.18440 0. ) BODY10_LONG_AXIS = ( 0. ) \begintext - + Mercury - + Old values: - + Values are from the 2006 IAU report. body199_pole_ra = ( 281.01 -0.033 0. ) @@ -959,13 +959,13 @@ Mercury Current values: - + \begindata BODY199_POLE_RA = ( 281.0097 -0.0328 0. ) BODY199_POLE_DEC = ( 61.4143 -0.0049 0. ) BODY199_PM = ( 329.5469 6.1385025 0. ) - + BODY199_LONG_AXIS = ( 0. ) BODY199_NUT_PREC_RA = ( 0. 0. 0. 0. 0. ) @@ -976,13 +976,13 @@ Mercury -0.00104581 -0.00010280 -0.00002364 - -0.00000532 ) - \begintext + -0.00000532 ) + \begintext The linear coefficients have been scaled up from degrees/day to degrees/century, because the SPICELIB PCK reader expects these units. The original constants were: - + 174.791086 4.092335 349.582171 8.184670 164.373257 12.277005 @@ -992,43 +992,43 @@ Mercury \begindata - BODY1_NUT_PREC_ANGLES = ( 174.791086 0.14947253587500003E+06 + BODY1_NUT_PREC_ANGLES = ( 174.791086 0.14947253587500003E+06 349.582171 0.29894507175000006E+06 - 164.373257 0.44841760762500006E+06 - 339.164343 0.59789014350000012E+06 + 164.373257 0.44841760762500006E+06 + 339.164343 0.59789014350000012E+06 153.955429 0.74736267937499995E+06 ) \begintext - - + + Venus - + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - + \begindata - + BODY299_POLE_RA = ( 272.76 0. 0. ) BODY299_POLE_DEC = ( 67.16 0. 0. ) BODY299_PM = ( 160.20 -1.4813688 0. ) - + BODY299_LONG_AXIS = ( 0. ) - + \begintext Earth - + Old values: - + Values are unchanged in the 2009 report. - + Current values: - - \begindata - + + \begindata + BODY399_POLE_RA = ( 0. -0.641 0. ) BODY399_POLE_DEC = ( 90. -0.557 0. ) BODY399_PM = ( 190.147 360.9856235 0. ) @@ -1042,7 +1042,7 @@ Earth The linear coefficients have been scaled up from degrees/day to degrees/century, because the SPICELIB PCK reader expects these units. The original constants were: - + 125.045D0 -0.0529921D0 250.089D0 -0.1059842D0 260.008D0 13.0120009D0 @@ -1055,29 +1055,29 @@ Earth 15.134D0 -0.1589763D0 119.743D0 0.0036096D0 239.961D0 0.1643573D0 - 25.053D0 12.9590088D0 + 25.053D0 12.9590088D0 \begindata - + BODY3_NUT_PREC_ANGLES = ( 125.045 -1935.5364525000 250.089 -3871.0729050000 - 260.008 475263.3328725000 + 260.008 475263.3328725000 176.625 487269.6299850000 357.529 35999.0509575000 311.589 964468.4993100000 134.963 477198.8693250000 276.617 12006.3007650000 - 34.226 63863.5132425000 + 34.226 63863.5132425000 15.134 -5806.6093575000 119.743 131.8406400000 - 239.961 6003.1503825000 + 239.961 6003.1503825000 25.053 473327.7964200000 ) \begintext - + Earth north geomagnetic centered dipole: @@ -1088,7 +1088,7 @@ Earth Reference Field" and "International Geomagnetic Reference Field." See references [6], [8], and [9] for details. - Coordinates are planetocentric. + Coordinates are planetocentric. Data source Lat Lon ----------- ----- ------ @@ -1099,11 +1099,11 @@ Earth DGRF 1965 78.53 290.15 DGRF 1970 78.59 289.82 DGRF 1975 78.69 289.53 - DGRF 1980 78.81 289.24 + DGRF 1980 78.81 289.24 DGRF 1985 78.97 289.10 DGRF 1990 79.13 288.89 IGRF 1995 79.30 288.59 - IGRF 2000 79.54 288.43 + IGRF 2000 79.54 288.43 Original values: @@ -1124,7 +1124,7 @@ Earth by Nat Bachman from constants provided by [11]. \begindata - + BODY399_N_GEOMAG_CTR_DIPOLE_LON = ( 287.62 ) BODY399_N_GEOMAG_CTR_DIPOLE_LAT = ( 80.13 ) @@ -1132,23 +1132,23 @@ Earth - + Mars - + Old values: - + Values are unchanged in the 2009 IAU report. - + Current values: - + \begindata - + BODY499_POLE_RA = ( 317.68143 -0.1061 0. ) BODY499_POLE_DEC = ( 52.88650 -0.0609 0. ) BODY499_PM = ( 176.630 350.89198226 0. ) \begintext - + Source [5] specifies the following value for the lambda_a term (BODY499_LONG_AXIS ) for Mars. This term is the POSITIVE EAST LONGITUDE, measured from the prime meridian, of the meridian @@ -1156,54 +1156,54 @@ Mars (CAUTION: previous values were POSITIVE WEST.) body499_long_axis = ( 252. ) - + We list this lambda_a value for completeness. The IAU report [1] gives equal values for both equatorial radii, so the lambda_a offset does not apply to the IAU model. - + The 2003 IAU report defines M2, the second nutation precession angle, by: - + 2 192.93 + 1128.4096700 d + 8.864 T - + We truncate the M2 series to a linear expression, because the PCK software cannot handle the quadratic term. - + Again, the linear terms are scaled by 36525.0: - + -0.4357640000000000 --> -15916.28010000000 1128.409670000000 --> 41215163.19675000 -1.8151000000000000E-02 --> -662.9652750000000 - + We also introduce a fourth nutation precession angle, which is the pi/2-complement of the third angle. This angle is used in computing the prime meridian location for Deimos. See the discussion of this angle below in the section containing orientation constants for Deimos. - + \begindata BODY4_NUT_PREC_ANGLES = ( 169.51 -15916.2801 192.93 41215163.19675 53.47 -662.965275 36.53 662.965275 ) - + \begintext - - + + Jupiter - + Old values: - + The rotation rate is from the 2006 IAU report; all other values are unchanged in the 2009 report. body599_pm = ( 284.95 870.5366420 0. ) - + Current values: - + The number of nutation precession angles is 15. The ninth and tenth are twice the first and second, respectively. The eleventh through fifteenth correspond to angles JA-JE in @@ -1211,13 +1211,13 @@ Jupiter report. \begindata - - + + BODY599_POLE_RA = ( 268.056595 -0.006499 0. ) BODY599_POLE_DEC = ( 64.495303 0.002413 0. ) BODY599_PM = ( 284.95 870.5360000 0. ) BODY599_LONG_AXIS = ( 0. ) - + BODY599_NUT_PREC_RA = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.000117 0.000938 0.001432 @@ -1234,7 +1234,7 @@ Jupiter 0.0 0.0 0.0 - 0.0 ) + 0.0 ) BODY5_NUT_PREC_ANGLES = ( 73.32 91472.9 @@ -1244,21 +1244,21 @@ Jupiter 119.90 262.1 229.80 64.3 352.25 2382.6 - 113.35 6070.0 + 113.35 6070.0 146.64 182945.8 - 49.24 90274.4 + 49.24 90274.4 99.360714 4850.4046 175.895369 1191.9605 300.323162 262.5475 114.012305 6070.2476 49.511251 64.3000 ) \begintext - - + + Saturn - + Old values: - + Values are from the 2006 IAU report. @@ -1266,29 +1266,29 @@ Saturn body699_pole_dec = ( 83.537 -0.004 0. ) body699_pm = ( 38.90 810.7939024 0. ) body699_long_axis = ( 0. ) - - - The first seven angles given here are the angles S1 + + + The first seven angles given here are the angles S1 through S7 from the 2000 report; the eighth and ninth angles are 2*S1 and 2*S2, respectively. - - + + body6_nut_prec_angles = ( 353.32 75706.7 - 28.72 75706.7 - 177.40 -36505.5 - 300.00 -7225.9 + 28.72 75706.7 + 177.40 -36505.5 + 300.00 -7225.9 316.45 506.2 - 345.20 -1016.3 + 345.20 -1016.3 29.80 -52.1 706.64 151413.4 57.44 151413.4 ) Current values: - + The change from the previous set of values is the - removal of S7. This causes BODY6_NUT_PREC_ANGLES + removal of S7. This causes BODY6_NUT_PREC_ANGLES elements that formerly corresponded to 2*S1 and 2*S1 to be shifted toward the start of the array. @@ -1298,131 +1298,131 @@ Saturn BODY699_POLE_DEC = ( 83.537 -0.004 0. ) BODY699_PM = ( 38.90 810.7939024 0. ) BODY699_LONG_AXIS = ( 0. ) - + \begintext - - The first six angles given here are the angles S1 + + The first six angles given here are the angles S1 through S6 from the 2009 report; the seventh and eigth angles are 2*S1 and 2*S2, respectively. - - + + \begindata BODY6_NUT_PREC_ANGLES = ( 353.32 75706.7 - 28.72 75706.7 - 177.40 -36505.5 - 300.00 -7225.9 + 28.72 75706.7 + 177.40 -36505.5 + 300.00 -7225.9 316.45 506.2 - 345.20 -1016.3 + 345.20 -1016.3 706.64 151413.4 57.44 151413.4 ) \begintext - - + + Uranus - + Old values: - + Values are unchanged in the 2009 IAU report. - + Current values: - + \begindata - + BODY799_POLE_RA = ( 257.311 0. 0. ) BODY799_POLE_DEC = ( -15.175 0. 0. ) BODY799_PM = ( 203.81 -501.1600928 0. ) BODY799_LONG_AXIS = ( 0. ) - + \begintext - - The first 16 angles given here are the angles U1 + + The first 16 angles given here are the angles U1 through U16 from the 2000 report; the 17th and 18th angles are 2*U11 and 2*U12, respectively. - + \begindata - + BODY7_NUT_PREC_ANGLES = ( 115.75 54991.87 141.69 41887.66 135.03 29927.35 - 61.77 25733.59 + 61.77 25733.59 249.32 24471.46 - 43.86 22278.41 - 77.66 20289.42 - 157.36 16652.76 - 101.81 12872.63 + 43.86 22278.41 + 77.66 20289.42 + 157.36 16652.76 + 101.81 12872.63 138.64 8061.81 - 102.23 -2024.22 - 316.41 2863.96 - 304.01 -51.94 - 308.71 -93.17 - 340.82 -75.32 - 259.14 -504.81 + 102.23 -2024.22 + 316.41 2863.96 + 304.01 -51.94 + 308.71 -93.17 + 340.82 -75.32 + 259.14 -504.81 204.46 -4048.44 632.82 5727.92 ) - + \begintext - - - + + + Neptune - + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - \begindata - + \begindata + BODY899_POLE_RA = ( 299.36 0. 0. ) BODY899_POLE_DEC = ( 43.46 0. 0. ) BODY899_PM = ( 253.18 536.3128492 0. ) BODY899_LONG_AXIS = ( 0. ) - BODY899_NUT_PREC_RA = ( 0.70 0. 0. 0. 0. 0. 0. 0. ) + BODY899_NUT_PREC_RA = ( 0.70 0. 0. 0. 0. 0. 0. 0. ) BODY899_NUT_PREC_DEC = ( -0.51 0. 0. 0. 0. 0. 0. 0. ) BODY899_NUT_PREC_PM = ( -0.48 0. 0. 0. 0. 0. 0. 0. ) \begintext - + The 2000 report defines the nutation precession angles - + N, N1, N2, ... , N7 - + and also uses the multiples of N1 and N7 - + 2*N1 - + and - + 2*N7, 3*N7, ..., 9*N7 - + In this file, we treat the angles and their multiples as separate angles. In the kernel variable - + BODY8_NUT_PREC_ANGLES - + the order of the angles is - + N, N1, N2, ... , N7, 2*N1, 2*N7, 3*N7, ..., 9*N7 - + Each angle is defined by a linear polynomial, so two consecutive array elements are allocated for each angle. The first term of each pair is the constant term, the second is the linear term. - - \begindata + + \begindata BODY8_NUT_PREC_ANGLES = ( 357.85 52.316 323.92 62606.6 - 220.51 55064.2 + 220.51 55064.2 354.27 46564.5 - 75.31 26109.4 + 75.31 26109.4 35.36 14325.4 - 142.61 2824.6 - 177.85 52.316 + 142.61 2824.6 + 177.85 52.316 647.840 125213.200 355.700 104.632 533.550 156.948 @@ -1432,20 +1432,20 @@ Neptune 1244.950 366.212 1422.800 418.528 1600.650 470.844 ) - + \begintext - - - + + + Orientation Constants for the Dwarf Planet Pluto -------------------------------------------------------- - + Pluto - + Old values: - - Values are from the 2006 IAU report. + + Values are from the 2006 IAU report. body999_pole_ra = ( 312.993 0. 0. ) body999_pole_dec = ( 6.163 0. 0. ) @@ -1458,206 +1458,206 @@ Pluto Due to the new definition of planetocentric coordinates for small bodies, and to the reclassification of Pluto as a dwarf planet, Pluto's north pole direction has been - inverted. + inverted. - The PM constant W0 is from [2]. + The PM constant W0 is from [2]. \begindata - + BODY999_POLE_RA = ( 132.993 0. 0. ) BODY999_POLE_DEC = ( -6.163 0. 0. ) BODY999_PM = ( 302.695 56.3625225 0. ) BODY999_LONG_AXIS = ( 0. ) \begintext - - - - + + + + Orientation constants for the satellites -------------------------------------------------------- - - + + Satellites of Earth - + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + New values: - + \begindata - + BODY301_POLE_RA = ( 269.9949 0.0031 0. ) BODY301_POLE_DEC = ( 66.5392 0.0130 0. ) BODY301_PM = ( 38.3213 13.17635815 -1.4D-12 ) BODY301_LONG_AXIS = ( 0. ) - + BODY301_NUT_PREC_RA = ( -3.8787 -0.1204 0.0700 -0.0172 0.0 0.0072 0.0 0.0 0.0 -0.0052 0.0 0.0 0.0043 ) - + BODY301_NUT_PREC_DEC = ( 1.5419 0.0239 -0.0278 0.0068 0.0 -0.0029 0.0009 0.0 - 0.0 0.0008 0.0 0.0 + 0.0 0.0008 0.0 0.0 -0.0009 ) - + BODY301_NUT_PREC_PM = ( 3.5610 0.1208 -0.0642 0.0158 0.0252 -0.0066 -0.0047 -0.0046 0.0028 0.0052 0.0040 0.0019 -0.0044 ) \begintext - - + + Satellites of Mars - - + + Phobos - + Old values: - + Values are unchanged in the 2009 IAU report. - + Current values: - + The quadratic prime meridian term is scaled by 1/36525**2: - + 8.864000000000000 ---> 6.6443009930565219E-09 - + \begindata - + BODY401_POLE_RA = ( 317.68 -0.108 0. ) BODY401_POLE_DEC = ( 52.90 -0.061 0. ) BODY401_PM = ( 35.06 1128.8445850 6.6443009930565219E-09 ) - + BODY401_LONG_AXIS = ( 0. ) - + BODY401_NUT_PREC_RA = ( 1.79 0. 0. 0. ) BODY401_NUT_PREC_DEC = ( -1.08 0. 0. 0. ) BODY401_NUT_PREC_PM = ( -1.42 -0.78 0. 0. ) \begintext - - + + Deimos - + Old values: - + Values are unchanged in the 2009 IAU report. - + New values: - + The Deimos prime meridian expression is: - - + + 2 W = 79.41 + 285.1618970 d - 0.520 T - 2.58 sin M 3 - + + 0.19 cos M . 3 - - + + At the present time, the PCK kernel software (the routine BODEUL in particular) cannot handle the cosine term directly, but we can represent it as - + 0.19 sin M 4 - + where - + M = 90.D0 - M 4 3 - + Therefore, the nutation precession angle assignments for Phobos and Deimos contain four coefficients rather than three. - + The quadratic prime meridian term is scaled by 1/36525**2: - + -0.5200000000000000 ---> -3.8978300049519307E-10 - + \begindata - + BODY402_POLE_RA = ( 316.65 -0.108 0. ) BODY402_POLE_DEC = ( 53.52 -0.061 0. ) BODY402_PM = ( 79.41 285.1618970 -3.897830D-10 ) BODY402_LONG_AXIS = ( 0. ) - + BODY402_NUT_PREC_RA = ( 0. 0. 2.98 0. ) BODY402_NUT_PREC_DEC = ( 0. 0. -1.78 0. ) BODY402_NUT_PREC_PM = ( 0. 0. -2.58 0.19 ) \begintext - - - - + + + + Satellites of Jupiter - - + + Io - + Old values: - + Values are unchanged in the 2009 IAU report. - + Current values: - + \begindata - + BODY501_POLE_RA = ( 268.05 -0.009 0. ) BODY501_POLE_DEC = ( 64.50 0.003 0. ) BODY501_PM = ( 200.39 203.4889538 0. ) BODY501_LONG_AXIS = ( 0. ) - + BODY501_NUT_PREC_RA = ( 0. 0. 0.094 0.024 ) BODY501_NUT_PREC_DEC = ( 0. 0. 0.040 0.011 ) BODY501_NUT_PREC_PM = ( 0. 0. -0.085 -0.022 ) \begintext - - - + + + Europa - - + + Old values: - Values are unchanged in the 2009 IAU report. + Values are unchanged in the 2009 IAU report. Current values: - - \begindata - + + \begindata + BODY502_POLE_RA = ( 268.08 -0.009 0. ) BODY502_POLE_DEC = ( 64.51 0.003 0. ) BODY502_PM = ( 36.022 101.3747235 0. ) BODY502_LONG_AXIS = ( 0. ) - + BODY502_NUT_PREC_RA = ( 0. 0. 0. 1.086 0.060 0.015 0.009 ) BODY502_NUT_PREC_DEC = ( 0. 0. 0. 0.468 0.026 0.007 0.002 ) BODY502_NUT_PREC_PM = ( 0. 0. 0. -0.980 -0.054 -0.014 -0.008 ) - + \begintext - - + + Ganymede - + Old values: - + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY503_POLE_RA = ( 268.20 -0.009 0. ) BODY503_POLE_DEC = ( 64.57 0.003 0. ) BODY503_PM = ( 44.064 50.3176081 0. ) @@ -1666,130 +1666,130 @@ Satellites of Jupiter BODY503_NUT_PREC_RA = ( 0. 0. 0. -0.037 0.431 0.091 ) BODY503_NUT_PREC_DEC = ( 0. 0. 0. -0.016 0.186 0.039 ) BODY503_NUT_PREC_PM = ( 0. 0. 0. 0.033 -0.389 -0.082 ) - + \begintext - - + + Callisto - + Old values: Values are unchanged in the 2009 IAU report. - + Current values: - - + + \begindata - + BODY504_POLE_RA = ( 268.72 -0.009 0. ) BODY504_POLE_DEC = ( 64.83 0.003 0. ) BODY504_PM = ( 259.51 21.5710715 0. ) BODY504_LONG_AXIS = ( 0. ) - + BODY504_NUT_PREC_RA = ( 0. 0. 0. 0. -0.068 0.590 0. 0.010 ) BODY504_NUT_PREC_DEC = ( 0. 0. 0. 0. -0.029 0.254 0. -0.004 ) BODY504_NUT_PREC_PM = ( 0. 0. 0. 0. 0.061 -0.533 0. -0.009 ) - + \begintext - - + + Amalthea - - + + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - + \begindata - + BODY505_POLE_RA = ( 268.05 -0.009 0. ) BODY505_POLE_DEC = ( 64.49 0.003 0. ) BODY505_PM = ( 231.67 722.6314560 0. ) BODY505_LONG_AXIS = ( 0. ) - + BODY505_NUT_PREC_RA = ( -0.84 0. 0. 0. 0. 0. 0. 0. 0.01 0. ) BODY505_NUT_PREC_DEC = ( -0.36 0. 0. 0. 0. 0. 0. 0. 0. 0. ) BODY505_NUT_PREC_PM = ( 0.76 0. 0. 0. 0. 0. 0. 0. -0.01 0. ) - + \begintext - - + + Thebe - - + + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - + \begindata - + BODY514_POLE_RA = ( 268.05 -0.009 0. ) BODY514_POLE_DEC = ( 64.49 0.003 0. ) BODY514_PM = ( 8.56 533.7004100 0. ) BODY514_LONG_AXIS = ( 0. ) - + BODY514_NUT_PREC_RA = ( 0. -2.11 0. 0. 0. 0. 0. 0. 0. 0.04 ) BODY514_NUT_PREC_DEC = ( 0. -0.91 0. 0. 0. 0. 0. 0. 0. 0.01 ) BODY514_NUT_PREC_PM = ( 0. 1.91 0. 0. 0. 0. 0. 0. 0. -0.04 ) - + \begintext - - + + Adrastea - + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - - \begindata - + + \begindata + BODY515_POLE_RA = ( 268.05 -0.009 0. ) BODY515_POLE_DEC = ( 64.49 0.003 0. ) BODY515_PM = ( 33.29 1206.9986602 0. ) BODY515_LONG_AXIS = ( 0. ) \begintext - - + + Metis - + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - + \begindata BODY516_POLE_RA = ( 268.05 -0.009 0. ) BODY516_POLE_DEC = ( 64.49 0.003 0. ) BODY516_PM = ( 346.09 1221.2547301 0. ) BODY516_LONG_AXIS = ( 0. ) - + \begintext - - - + + + Satellites of Saturn - - + + Mimas - + Old values: - + Values are from the 2006 IAU report. - + body601_pole_ra = ( 40.66 -0.036 0. ) body601_pole_dec = ( 83.52 -0.004 0. ) body601_pm = ( 337.46 381.9945550 0. ) body601_long_axis = ( 0. ) - + body601_nut_prec_ra = ( 0. 0. 13.56 0. 0. 0. 0. 0. 0. ) body601_nut_prec_dec = ( 0. 0. -1.53 0. 0. 0. 0. 0. 0. ) body601_nut_prec_pm = ( 0. 0. -13.48 0. -44.85 0. 0. 0. 0. ) @@ -1798,26 +1798,26 @@ Satellites of Saturn Current values: \begindata - + BODY601_POLE_RA = ( 40.66 -0.036 0. ) BODY601_POLE_DEC = ( 83.52 -0.004 0. ) BODY601_PM = ( 333.46 381.9945550 0. ) BODY601_LONG_AXIS = ( 0. ) - + BODY601_NUT_PREC_RA = ( 0. 0. 13.56 0. 0. 0. 0. 0. ) BODY601_NUT_PREC_DEC = ( 0. 0. -1.53 0. 0. 0. 0. 0. ) BODY601_NUT_PREC_PM = ( 0. 0. -13.48 0. -44.85 0. 0. 0. ) \begintext - - + + Enceladus - - + + Old values: - - Values are from the 2006 IAU report. - + + Values are from the 2006 IAU report. + body602_pole_ra = ( 40.66 -0.036 0. ) body602_pole_dec = ( 83.52 -0.004 0. ) body602_pm = ( 2.82 262.7318996 0. ) @@ -1825,58 +1825,58 @@ Satellites of Saturn Current values: - + \begindata - + BODY602_POLE_RA = ( 40.66 -0.036 0. ) BODY602_POLE_DEC = ( 83.52 -0.004 0. ) BODY602_PM = ( 6.32 262.7318996 0. ) BODY602_LONG_AXIS = ( 0. ) \begintext - - - + + + Tethys - - + + Old values: - - Values are from the 2006 IAU report. - + + Values are from the 2006 IAU report. + body603_pole_ra = ( 40.66 -0.036 0. ) body603_pole_dec = ( 83.52 -0.004 0. ) body603_pm = ( 10.45 190.6979085 0. ) body603_long_axis = ( 0. ) - + body603_nut_prec_ra = ( 0. 0. 0. 9.66 0. 0. 0. 0. 0. ) body603_nut_prec_dec = ( 0. 0. 0. -1.09 0. 0. 0. 0. 0. ) body603_nut_prec_pm = ( 0. 0. 0. -9.60 2.23 0. 0. 0. 0. ) Current values: - + \begindata - + BODY603_POLE_RA = ( 40.66 -0.036 0. ) BODY603_POLE_DEC = ( 83.52 -0.004 0. ) BODY603_PM = ( 8.95 190.6979085 0. ) BODY603_LONG_AXIS = ( 0. ) - + BODY603_NUT_PREC_RA = ( 0. 0. 0. 9.66 0. 0. 0. 0. ) BODY603_NUT_PREC_DEC = ( 0. 0. 0. -1.09 0. 0. 0. 0. ) BODY603_NUT_PREC_PM = ( 0. 0. 0. -9.60 2.23 0. 0. 0. ) \begintext - - + + Dione - - + + Old values: - - Values are from the 2006 IAU report. - + + Values are from the 2006 IAU report. + body604_pole_ra = ( 40.66 -0.036 0. ) body604_pole_dec = ( 83.52 -0.004 0. ) body604_pm = ( 357.00 131.5349316 0. ) @@ -1884,68 +1884,68 @@ Satellites of Saturn Current values: - + \begindata - + BODY604_POLE_RA = ( 40.66 -0.036 0. ) BODY604_POLE_DEC = ( 83.52 -0.004 0. ) BODY604_PM = ( 357.6 131.5349316 0. ) BODY604_LONG_AXIS = ( 0. ) \begintext - - - + + + Rhea - - + + Old values: - + Values are from the 2009 IAU report. body605_pole_ra = ( 40.38 -0.036 0. ) body605_pole_dec = ( 83.55 -0.004 0. ) body605_pm = ( 235.16 79.6900478 0. ) body605_long_axis = ( 0. ) - + body605_nut_prec_ra = ( 0. 0. 0. 0. 0. 3.10 0. 0. 0. ) body605_nut_prec_dec = ( 0. 0. 0. 0. 0. -0.35 0. 0. 0. ) body605_nut_prec_pm = ( 0. 0. 0. 0. 0. -3.08 0. 0. 0. ) - + Current values: - + Data values are unchanged in the 2009 IAU report. However the kernel variable contents have changed due to removal of the angle S7. \begindata - + BODY605_POLE_RA = ( 40.38 -0.036 0. ) BODY605_POLE_DEC = ( 83.55 -0.004 0. ) BODY605_PM = ( 235.16 79.6900478 0. ) BODY605_LONG_AXIS = ( 0. ) - + BODY605_NUT_PREC_RA = ( 0. 0. 0. 0. 0. 3.10 0. 0. ) BODY605_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. -0.35 0. 0. ) BODY605_NUT_PREC_PM = ( 0. 0. 0. 0. 0. -3.08 0. 0. ) - + \begintext - - - + + + Titan - - + + Old values: - - Values are from the 2006 IAU report. + + Values are from the 2006 IAU report. BODY606_POLE_RA = ( 36.41 -0.036 0. ) BODY606_POLE_DEC = ( 83.94 -0.004 0. ) BODY606_PM = ( 189.64 22.5769768 0. ) BODY606_LONG_AXIS = ( 0. ) - + BODY606_NUT_PREC_RA = ( 0. 0. 0. 0. 0. 0. 2.66 0. 0 ) BODY606_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. 0. -0.30 0. 0 ) BODY606_NUT_PREC_PM = ( 0. 0. 0. 0. 0. 0. -2.64 0. 0 ) @@ -1955,35 +1955,35 @@ Satellites of Saturn Note removal of dependence on the nutation precession angles. - + \begindata - + BODY606_POLE_RA = ( 39.4827 0. 0. ) BODY606_POLE_DEC = ( 83.4279 0. 0. ) BODY606_PM = ( 186.5855 22.5769768 0. ) BODY606_LONG_AXIS = ( 0. ) - + BODY606_NUT_PREC_RA = ( 0. 0. 0. 0. 0. 0. 0. 0 ) BODY606_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. 0. 0. 0 ) BODY606_NUT_PREC_PM = ( 0. 0. 0. 0. 0. 0. 0. 0 ) \begintext - - - + + + Hyperion - + The IAU report does not give an orientation model for Hyperion. Hyperion's rotation is in chaotic and is not predictable for long periods. - + Iapetus - - + + Old values: - - Values are from the 2006 IAU report. + + Values are from the 2006 IAU report. body608_pole_ra = ( 318.16 -3.949 0. ) body608_pole_dec = ( 75.03 -1.143 0. ) @@ -1992,43 +1992,43 @@ Satellites of Saturn Current values: - + \begindata - + BODY608_POLE_RA = ( 318.16 -3.949 0. ) BODY608_POLE_DEC = ( 75.03 -1.143 0. ) BODY608_PM = ( 355.2 4.5379572 0. ) BODY608_LONG_AXIS = ( 0. ) \begintext - - - + + + Phoebe - + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - - \begindata - + + \begindata + BODY609_POLE_RA = ( 356.90 0. 0. ) BODY609_POLE_DEC = ( 77.80 0. 0. ) BODY609_PM = ( 178.58 931.639 0. ) BODY609_LONG_AXIS = ( 0. ) \begintext - - + + Janus - - + + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: @@ -2036,81 +2036,81 @@ Satellites of Saturn Data values are unchanged in the 2009 IAU report. However the kernel variable contents have changed due to removal of the angle S7. - + \begindata BODY610_POLE_RA = ( 40.58 -0.036 0. ) BODY610_POLE_DEC = ( 83.52 -0.004 0. ) BODY610_PM = ( 58.83 518.2359876 0. ) BODY610_LONG_AXIS = ( 0. ) - + BODY610_NUT_PREC_RA = ( 0. -1.623 0. 0. 0. 0. 0. 0.023 ) BODY610_NUT_PREC_DEC = ( 0. -0.183 0. 0. 0. 0. 0. 0.001 ) BODY610_NUT_PREC_PM = ( 0. 1.613 0. 0. 0. 0. 0. -0.023 ) - + \begintext - - - + + + Epimetheus - - + + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - + Data values are unchanged in the 2009 IAU report. However the kernel variable contents have changed due to removal of the angle S7. - \begindata - + \begindata + BODY611_POLE_RA = ( 40.58 -0.036 0. ) BODY611_POLE_DEC = ( 83.52 -0.004 0. ) BODY611_PM = ( 293.87 518.4907239 0. ) BODY611_LONG_AXIS = ( 0. ) - + BODY611_NUT_PREC_RA = ( -3.153 0. 0. 0. 0. 0. 0.086 0. ) BODY611_NUT_PREC_DEC = ( -0.356 0. 0. 0. 0. 0. 0.005 0. ) BODY611_NUT_PREC_PM = ( 3.133 0. 0. 0. 0. 0. -0.086 0. ) \begintext - - - + + + Helene - - + + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - - \begindata - + + \begindata + BODY612_POLE_RA = ( 40.85 -0.036 0. ) BODY612_POLE_DEC = ( 83.34 -0.004 0. ) BODY612_PM = ( 245.12 131.6174056 0. ) BODY612_LONG_AXIS = ( 0. ) \begintext - - - + + + Telesto - - + + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - - \begindata - + + \begindata + BODY613_POLE_RA = ( 50.51 -0.036 0. ) BODY613_POLE_DEC = ( 84.06 -0.004 0. ) BODY613_PM = ( 56.88 190.6979332 0. ) @@ -2118,275 +2118,275 @@ Satellites of Saturn \begintext - - + + Calypso - - + + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY614_POLE_RA = ( 36.41 -0.036 0. ) BODY614_POLE_DEC = ( 85.04 -0.004 0. ) BODY614_PM = ( 153.51 190.6742373 0. ) BODY614_LONG_AXIS = ( 0. ) - + \begintext - - - + + + Atlas - - + + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY615_POLE_RA = ( 40.58 -0.036 0. ) - BODY615_POLE_DEC = ( 83.53 -0.004 0. ) + BODY615_POLE_DEC = ( 83.53 -0.004 0. ) BODY615_PM = ( 137.88 598.3060000 0. ) BODY615_LONG_AXIS = ( 0. ) \begintext - - - + + + Prometheus - - + + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY616_POLE_RA = ( 40.58 -0.036 ) BODY616_POLE_DEC = ( 83.53 -0.004 ) BODY616_PM = ( 296.14 587.289000 ) BODY616_LONG_AXIS = ( 0. ) - + \begintext - - - + + + Pandora - - + + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY617_POLE_RA = ( 40.58 -0.036 0. ) BODY617_POLE_DEC = ( 83.53 -0.004 0. ) BODY617_PM = ( 162.92 572.7891000 0. ) BODY617_LONG_AXIS = ( 0. ) - + \begintext - - - + + + Pan - - + + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY618_POLE_RA = ( 40.6 -0.036 0. ) BODY618_POLE_DEC = ( 83.5 -0.004 0. ) BODY618_PM = ( 48.8 626.0440000 0. ) BODY618_LONG_AXIS = ( 0. ) \begintext - - - - - + + + + + Satellites of Uranus - - - + + + Ariel - + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - \begindata + \begindata BODY701_POLE_RA = ( 257.43 0. 0. ) BODY701_POLE_DEC = ( -15.10 0. 0. ) BODY701_PM = ( 156.22 -142.8356681 0. ) BODY701_LONG_AXIS = ( 0. ) - + BODY701_NUT_PREC_RA = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.29 ) - + BODY701_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.28 ) - + BODY701_NUT_PREC_PM = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.05 0.08 ) \begintext - - - + + + Umbriel - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - - \begindata - + + \begindata + BODY702_POLE_RA = ( 257.43 0. 0. ) BODY702_POLE_DEC = ( -15.10 0. 0. ) BODY702_PM = ( 108.05 -86.8688923 0. ) BODY702_LONG_AXIS = ( 0. ) - - BODY702_NUT_PREC_RA = ( 0. 0. 0. 0. 0. + + BODY702_NUT_PREC_RA = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.21 ) - - BODY702_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. + + BODY702_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.20 ) - - BODY702_NUT_PREC_PM = ( 0. 0. 0. 0. 0. + + BODY702_NUT_PREC_PM = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. -0.09 0. 0.06 ) \begintext - - - + + + Titania - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY703_POLE_RA = ( 257.43 0. 0. ) BODY703_POLE_DEC = ( -15.10 0. 0. ) BODY703_PM = ( 77.74 -41.3514316 0. ) BODY703_LONG_AXIS = ( 0. ) - - BODY703_NUT_PREC_RA = ( 0. 0. 0. 0. 0. + + BODY703_NUT_PREC_RA = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.29 ) - - BODY703_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. + + BODY703_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.28 ) - - BODY703_NUT_PREC_PM = ( 0. 0. 0. 0. 0. + + BODY703_NUT_PREC_PM = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.08 ) \begintext - - - + + + Oberon - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY704_POLE_RA = ( 257.43 0. 0. ) BODY704_POLE_DEC = ( -15.10 0. 0. ) BODY704_PM = ( 6.77 -26.7394932 0. ) BODY704_LONG_AXIS = ( 0. ) - - - BODY704_NUT_PREC_RA = ( 0. 0. 0. 0. 0. - 0. 0. 0. 0. 0. + + + BODY704_NUT_PREC_RA = ( 0. 0. 0. 0. 0. + 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.16 ) - - BODY704_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. - 0. 0. 0. 0. 0. + + BODY704_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. + 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.16 ) - - BODY704_NUT_PREC_PM = ( 0. 0. 0. 0. 0. - 0. 0. 0. 0. 0. + + BODY704_NUT_PREC_PM = ( 0. 0. 0. 0. 0. + 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.04 ) \begintext - - - + + + Miranda - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY705_POLE_RA = ( 257.43 0. 0. ) BODY705_POLE_DEC = ( -15.08 0. 0. ) BODY705_PM = ( 30.70 -254.6906892 0. ) BODY705_LONG_AXIS = ( 0. ) - - BODY705_NUT_PREC_RA = ( 0. 0. 0. 0. 0. - 0. 0. 0. 0. 0. - 4.41 0. 0. 0. 0. + + BODY705_NUT_PREC_RA = ( 0. 0. 0. 0. 0. + 0. 0. 0. 0. 0. + 4.41 0. 0. 0. 0. 0. -0.04 0. ) - - BODY705_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. - 0. 0. 0. 0. 0. - 4.25 0. 0. 0. 0. + + BODY705_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. + 0. 0. 0. 0. 0. + 4.25 0. 0. 0. 0. 0. -0.02 0. ) - - BODY705_NUT_PREC_PM = ( 0. 0. 0. 0. 0. - 0. 0. 0. 0. 0. - 1.15 -1.27 0. 0. 0. + + BODY705_NUT_PREC_PM = ( 0. 0. 0. 0. 0. + 0. 0. 0. 0. 0. + 1.15 -1.27 0. 0. 0. 0. -0.09 0.15 ) \begintext - - - + + + Cordelia - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY706_POLE_RA = ( 257.31 0. 0. ) BODY706_POLE_DEC = ( -15.18 0. 0. ) BODY706_PM = ( 127.69 -1074.5205730 0. ) BODY706_LONG_AXIS = ( 0. ) - + BODY706_NUT_PREC_RA = ( -0.15 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. @@ -2395,33 +2395,33 @@ Satellites of Uranus BODY706_NUT_PREC_DEC = ( 0.14 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. - 0. 0. 0. ) + 0. 0. 0. ) BODY706_NUT_PREC_PM = ( -0.04 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. - 0. 0. 0. ) - + 0. 0. 0. ) + \begintext - - + + Ophelia - - + + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY707_POLE_RA = ( 257.31 0. 0. ) BODY707_POLE_DEC = ( -15.18 0. 0. ) BODY707_PM = ( 130.35 -956.4068150 0. ) BODY707_LONG_AXIS = ( 0. ) - + BODY707_NUT_PREC_RA = ( 0. -0.09 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. @@ -2436,26 +2436,26 @@ Satellites of Uranus 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + \begintext - - - + + + Bianca - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY708_POLE_RA = ( 257.31 0. 0. ) BODY708_POLE_DEC = ( -15.18 0. 0. ) BODY708_PM = ( 105.46 -828.3914760 0. ) BODY708_LONG_AXIS = ( 0. ) - + BODY708_NUT_PREC_RA = ( 0. 0. -0.16 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. @@ -2472,25 +2472,25 @@ Satellites of Uranus 0. 0. 0. ) \begintext - - - + + + Cressida - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - - + + BODY709_POLE_RA = ( 257.31 0. 0. ) BODY709_POLE_DEC = ( -15.18 0. 0. ) BODY709_PM = ( 59.16 -776.5816320 0. ) BODY709_LONG_AXIS = ( 0. ) - + BODY709_NUT_PREC_RA = ( 0. 0. 0. -0.04 0. 0. 0. 0. 0. 0. @@ -2511,569 +2511,569 @@ Satellites of Uranus \begintext - - - + + + Desdemona - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - - \begindata - + + \begindata + BODY710_POLE_RA = ( 257.31 0. 0. ) BODY710_POLE_DEC = ( -15.18 0. 0. ) BODY710_PM = ( 95.08 -760.0531690 0. ) BODY710_LONG_AXIS = ( 0. ) - - BODY710_NUT_PREC_RA = ( 0. 0. 0. 0. -0.17 + + BODY710_NUT_PREC_RA = ( 0. 0. 0. 0. -0.17 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - BODY710_NUT_PREC_DEC = ( 0. 0. 0. 0. 0.16 + BODY710_NUT_PREC_DEC = ( 0. 0. 0. 0. 0.16 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - BODY710_NUT_PREC_PM = ( 0. 0. 0. 0. -0.04 + BODY710_NUT_PREC_PM = ( 0. 0. 0. 0. -0.04 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) \begintext - - - + + + Juliet - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY711_POLE_RA = ( 257.31 0. 0. ) BODY711_POLE_DEC = ( -15.18 0. 0. ) BODY711_PM = ( 302.56 -730.1253660 0. ) BODY711_LONG_AXIS = ( 0. ) - - BODY711_NUT_PREC_RA = ( 0. 0. 0. 0. 0. + + BODY711_NUT_PREC_RA = ( 0. 0. 0. 0. 0. -0.06 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - - BODY711_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. + + BODY711_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. 0.06 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - - BODY711_NUT_PREC_PM = ( 0. 0. 0. 0. 0. + + BODY711_NUT_PREC_PM = ( 0. 0. 0. 0. 0. -0.02 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + \begintext - - - + + + Portia - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY712_POLE_RA = ( 257.31 0. 0. ) BODY712_POLE_DEC = ( -15.18 0. 0. ) BODY712_PM = ( 25.03 -701.4865870 0. ) BODY712_LONG_AXIS = ( 0. ) - - BODY712_NUT_PREC_RA = ( 0. 0. 0. 0. 0. + + BODY712_NUT_PREC_RA = ( 0. 0. 0. 0. 0. 0. -0.09 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - BODY712_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. + BODY712_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. 0. 0.09 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - BODY712_NUT_PREC_PM = ( 0. 0. 0. 0. 0. + BODY712_NUT_PREC_PM = ( 0. 0. 0. 0. 0. 0. -0.02 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) \begintext - - - + + + Rosalind - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY713_POLE_RA = ( 257.31 0. 0. ) BODY713_POLE_DEC = ( -15.18 0. 0. ) BODY713_PM = ( 314.90 -644.6311260 0. ) BODY713_LONG_AXIS = ( 0. ) - - BODY713_NUT_PREC_RA = ( 0. 0. 0. 0. 0. + + BODY713_NUT_PREC_RA = ( 0. 0. 0. 0. 0. 0. 0. -0.29 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - BODY713_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. + BODY713_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. 0. 0. 0.28 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - BODY713_NUT_PREC_PM = ( 0. 0. 0. 0. 0. + BODY713_NUT_PREC_PM = ( 0. 0. 0. 0. 0. 0. 0. -0.08 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + \begintext - - - + + + Belinda - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - - \begindata - + + \begindata + BODY714_POLE_RA = ( 257.31 0. 0. ) BODY714_POLE_DEC = ( -15.18 0. 0. ) BODY714_PM = ( 297.46 -577.3628170 0. ) BODY714_LONG_AXIS = ( 0. ) - - BODY714_NUT_PREC_RA = ( 0. 0. 0. 0. 0. + + BODY714_NUT_PREC_RA = ( 0. 0. 0. 0. 0. 0. 0. 0. -0.03 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - BODY714_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. + BODY714_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. 0. 0. 0. 0.03 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - BODY714_NUT_PREC_PM = ( 0. 0. 0. 0. 0. + BODY714_NUT_PREC_PM = ( 0. 0. 0. 0. 0. 0. 0. 0. -0.01 0. 0. 0. 0. 0. 0. 0. 0. 0. ) \begintext - - - + + + Puck - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata - + BODY715_POLE_RA = ( 257.31 0. 0. ) BODY715_POLE_DEC = ( -15.18 0. 0. ) BODY715_PM = ( 91.24 -472.5450690 0. ) BODY715_LONG_AXIS = ( 0. ) - - BODY715_NUT_PREC_RA = ( 0. 0. 0. 0. 0. - 0. 0. 0. 0. -0.33 + + BODY715_NUT_PREC_RA = ( 0. 0. 0. 0. 0. + 0. 0. 0. 0. -0.33 0. 0. 0. 0. 0. 0. 0. 0. ) - BODY715_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. + BODY715_NUT_PREC_DEC = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.31 0. 0. 0. 0. 0. 0. 0. 0. ) - BODY715_NUT_PREC_PM = ( 0. 0. 0. 0. 0. + BODY715_NUT_PREC_PM = ( 0. 0. 0. 0. 0. 0. 0. 0. 0. -0.09 0. 0. 0. 0. 0. 0. 0. 0. ) - + \begintext - - - - + + + + Satellites of Neptune - - + + Triton - + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - + \begindata BODY801_POLE_RA = ( 299.36 0. 0. ) BODY801_POLE_DEC = ( 41.17 0. 0. ) BODY801_PM = ( 296.53 -61.2572637 0. ) BODY801_LONG_AXIS = ( 0. ) - - - BODY801_NUT_PREC_RA = ( 0. 0. 0. 0. - 0. 0. 0. -32.35 - 0. -6.28 -2.08 -0.74 - -0.28 -0.11 -0.07 -0.02 + + + BODY801_NUT_PREC_RA = ( 0. 0. 0. 0. + 0. 0. 0. -32.35 + 0. -6.28 -2.08 -0.74 + -0.28 -0.11 -0.07 -0.02 -0.01 ) - - - BODY801_NUT_PREC_DEC = ( 0. 0. 0. 0. - 0. 0. 0. 22.55 - 0. 2.10 0.55 0.16 + + + BODY801_NUT_PREC_DEC = ( 0. 0. 0. 0. + 0. 0. 0. 22.55 + 0. 2.10 0.55 0.16 0.05 0.02 0.01 0. 0. ) - - - BODY801_NUT_PREC_PM = ( 0. 0. 0. 0. - 0. 0. 0. 22.25 - 0. 6.73 2.05 0.74 + + + BODY801_NUT_PREC_PM = ( 0. 0. 0. 0. + 0. 0. 0. 22.25 + 0. 6.73 2.05 0.74 0.28 0.11 0.05 0.02 0.01 ) - + \begintext - - - - + + + + Nereid - + Old values: - - Values are from the 1988 IAU report [10]. Note that this + + Values are from the 1988 IAU report [10]. Note that this rotation model pre-dated the 1989 Voyager 2 Neptune encounter. - + body802_pole_ra = ( 273.48 0. 0. ) body802_pole_dec = ( 67.22 0. 0. ) body802_pm = ( 237.22 0.9996465 0. ) body802_long_axis = ( 0. ) - - + + The report seems to have a typo: in the nut_prec_ra expression, where the report gives -0.51 sin 3N3, we use -0.51 3N2. - + body802_nut_prec_ra = ( 0. -17.81 0. 0. 0. 0. 0. 0. 0. 2.56 -0.51 0.11 -0.03 ) - + body802_nut_prec_dec = ( 0. -6.67 0. 0. 0. 0. 0. 0. 0. 0.47 -0.07 0.01 ) - + body802_nut_prec_pm = ( 0. 16.48 0. 0. 0. 0. 0. 0. 0. -2.57 0.51 -0.11 0.02 ) - - - + + + Current values: - + The 2009 report [1] states that values for Nereid are not given because Nereid is not in synchronous rotation with Neptune (notes following table 2). - - - + + + Naiad - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - - + + \begindata - + BODY803_POLE_RA = ( 299.36 0. 0. ) BODY803_POLE_DEC = ( 43.36 0. 0. ) BODY803_PM = ( 254.06 +1222.8441209 0. ) BODY803_LONG_AXIS = ( 0. ) - - + + BODY803_NUT_PREC_RA = ( 0.70 -6.49 0. 0. 0. 0. 0. 0. 0.25 0. 0. 0. 0. 0. 0. 0. 0. ) - + BODY803_NUT_PREC_DEC = ( -0.51 -4.75 0. 0. 0. 0. 0. 0. 0.09 0. 0. 0. 0. 0. 0. 0. 0. ) - + BODY803_NUT_PREC_PM = ( -0.48 4.40 0. 0. 0. 0. 0. 0. -0.27 0. 0. 0. 0. 0. 0. 0. 0. ) - + \begintext - - - - + + + + Thalassa - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata BODY804_POLE_RA = ( 299.36 0. 0. ) BODY804_POLE_DEC = ( 43.45 0. 0. ) - BODY804_PM = ( 102.06 1155.7555612 0. ) + BODY804_PM = ( 102.06 1155.7555612 0. ) BODY804_LONG_AXIS = ( 0. ) - - + + BODY804_NUT_PREC_RA = ( 0.70 0. -0.28 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - - + + BODY804_NUT_PREC_DEC = ( -0.51 0. -0.21 0. 0. 0. 0. 0. 0. 0. 0. 0. - 0. 0. 0. 0. + 0. 0. 0. 0. 0. ) - + BODY804_NUT_PREC_PM = ( -0.48 0. 0.19 0. 0. 0. 0. 0. 0. 0. 0. 0. - 0. 0. 0. 0. + 0. 0. 0. 0. 0. ) - + \begintext - - - + + + Despina - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - - + + \begindata - + BODY805_POLE_RA = ( 299.36 0. 0. ) BODY805_POLE_DEC = ( 43.45 0. 0. ) BODY805_PM = ( 306.51 +1075.7341562 0. ) BODY805_LONG_AXIS = ( 0. ) - - + + BODY805_NUT_PREC_RA = ( 0.70 0. 0. -0.09 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + BODY805_NUT_PREC_DEC = ( -0.51 0. 0. -0.07 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + BODY805_NUT_PREC_PM = ( -0.49 0. 0. 0.06 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) \begintext - - - + + + Galatea - - + + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - - + + \begindata - + BODY806_POLE_RA = ( 299.36 0. 0. ) BODY806_POLE_DEC = ( 43.43 0. 0. ) BODY806_PM = ( 258.09 839.6597686 0. ) BODY806_LONG_AXIS = ( 0. ) - - + + BODY806_NUT_PREC_RA = ( 0.70 0. 0. 0. -0.07 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + BODY806_NUT_PREC_DEC = ( -0.51 0. 0. 0. -0.05 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + BODY806_NUT_PREC_PM = ( -0.48 0. 0. 0. 0.05 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. - 0. ) + 0. ) \begintext - + Larissa - - + + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata BODY807_POLE_RA = ( 299.36 0. 0. ) BODY807_POLE_DEC = ( 43.41 0. 0. ) BODY807_PM = ( 179.41 +649.0534470 0. ) BODY807_LONG_AXIS = ( 0. ) - - + + BODY807_NUT_PREC_RA = ( 0.70 0. 0. 0. 0. -0.27 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + BODY807_NUT_PREC_DEC = ( -0.51 0. 0. 0. 0. -0.20 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + BODY807_NUT_PREC_PM = ( -0.48 0. 0. 0. 0. 0.19 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) \begintext - - - + + + Proteus - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata BODY808_POLE_RA = ( 299.27 0. 0. ) BODY808_POLE_DEC = ( 42.91 0. 0. ) BODY808_PM = ( 93.38 +320.7654228 0. ) BODY808_LONG_AXIS = ( 0. ) - - + + BODY808_NUT_PREC_RA = ( 0.70 0. 0. 0. 0. 0. -0.05 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + BODY808_NUT_PREC_DEC = ( -0.51 0. 0. 0. 0. 0. -0.04 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + BODY808_NUT_PREC_PM = ( -0.48 0. 0. 0. 0. 0. 0.04 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. ) - + \begintext - - - - - + + + + + Satellites of Pluto - + Charon - + Old values: - - Values are from the 2006 IAU report. - + + Values are from the 2006 IAU report. + body901_pole_ra = ( 312.993 0. 0. ) body901_pole_dec = ( 6.163 0. 0. ) body901_pm = ( 57.305 -56.3625225 0. ) body901_long_axis = ( 0. ) - + Current values: - + Due to the new definition of planetocentric coordinates for small bodies, and to the reclassification of Pluto as a dwarf planet, Charon's north pole direction has been - inverted. + inverted. - The PM constant W0 is from [2]. + The PM constant W0 is from [2]. \begindata - + BODY901_POLE_RA = ( 132.993 0. 0. ) BODY901_POLE_DEC = ( -6.163 0. 0. ) BODY901_PM = ( 122.695 56.3625225 0. ) BODY901_LONG_AXIS = ( 0. ) \begintext - - - + + + Orientation constants for Selected Comets and Asteroids -------------------------------------------------------- @@ -3082,14 +3082,14 @@ Orientation constants for Selected Comets and Asteroids Ceres Current values: - + \begindata BODY2000001_POLE_RA = ( 291. 0. 0. ) BODY2000001_POLE_DEC = ( 59. 0. 0. ) BODY2000001_PM = ( 170.90 952.1532 0. ) BODY2000001_LONG_AXIS = ( 0. ) - + \begintext @@ -3097,14 +3097,14 @@ Ceres Pallas Current values: - + \begindata BODY2000002_POLE_RA = ( 33. 0. 0. ) BODY2000002_POLE_DEC = ( -3. 0. 0. ) BODY2000002_PM = ( 38. 1105.8036 0. ) BODY2000002_LONG_AXIS = ( 0. ) - + \begintext @@ -3112,8 +3112,8 @@ Pallas Vesta Old values: - - Values are from the 2009 IAU report. + + Values are from the 2009 IAU report. body2000004_pole_ra = ( 301. 0. 0. ) body2000004_pole_dec = ( 41. 0. 0. ) @@ -3121,14 +3121,14 @@ Vesta body2000004_long_axis = ( 0. ) Current values: - + \begindata BODY2000004_POLE_RA = ( 305.8 0. 0. ) BODY2000004_POLE_DEC = ( 41.4 0. 0. ) BODY2000004_PM = ( 292. 1617.332776 0. ) BODY2000004_LONG_AXIS = ( 0. ) - + \begintext @@ -3136,14 +3136,14 @@ Vesta Lutetia Current values: - + \begindata BODY2000021_POLE_RA = ( 52. 0. 0. ) BODY2000021_POLE_DEC = ( 12. 0. 0. ) BODY2000021_PM = ( 94. 1057.7515 0. ) BODY2000021_LONG_AXIS = ( 0. ) - + \begintext @@ -3151,23 +3151,23 @@ Lutetia Ida Old values: - + BODY2431010_POLE_RA = ( 168.76 0. 0. ) BODY2431010_POLE_DEC = ( -2.88 0. 0. ) BODY2431010_PM = ( 265.95 +1864.6280070 0. ) BODY2431010_LONG_AXIS = ( 0. ) Current values: - - The PM constant W0 is from [2]. + + The PM constant W0 is from [2]. \begindata - + BODY2431010_POLE_RA = ( 168.76 0. 0. ) BODY2431010_POLE_DEC = ( -2.88 0. 0. ) BODY2431010_PM = ( 274.05 +1864.6280070 0. ) BODY2431010_LONG_AXIS = ( 0. ) - + \begintext @@ -3175,18 +3175,18 @@ Ida Eros Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata BODY2000433_POLE_RA = ( 11.35 0. 0. ) BODY2000433_POLE_DEC = ( 17.22 0. 0. ) BODY2000433_PM = ( 326.07 1639.38864745 0. ) BODY2000433_LONG_AXIS = ( 0. ) - + \begintext @@ -3194,14 +3194,14 @@ Eros Davida Current values: - + \begindata BODY2000511_POLE_RA = ( 297. 0. 0. ) BODY2000511_POLE_DEC = ( 5. 0. 0. ) BODY2000511_PM = ( 268.1 1684.4193549 0. ) BODY2000511_LONG_AXIS = ( 0. ) - + \begintext @@ -3209,13 +3209,13 @@ Davida Gaspra Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - - \begindata - + + \begindata + BODY9511010_POLE_RA = ( 9.47 0. 0. ) BODY9511010_POLE_DEC = ( 26.70 0. 0. ) BODY9511010_PM = ( 83.67 1226.9114850 0. ) @@ -3228,9 +3228,9 @@ Gaspra Steins Current values: - - \begindata - + + \begindata + BODY2002867_POLE_RA = ( 90. 0. 0. ) BODY2002867_POLE_DEC = ( -62. 0. 0. ) BODY2002867_PM = ( 93.94 1428.852332 0. ) @@ -3243,18 +3243,18 @@ Steins Itokawa Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata BODY2025143_POLE_RA = ( 90.53 0. 0. ) BODY2025143_POLE_DEC = ( -66.30 0. 0. ) BODY2025143_PM = ( 000.0 712.143 0. ) BODY2025143_LONG_AXIS = ( 0. ) - + \begintext @@ -3263,18 +3263,18 @@ Itokawa Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata BODY1000093_POLE_RA = ( 294. 0. 0. ) BODY1000093_POLE_DEC = ( 73. 0. 0. ) BODY1000093_PM = ( 252.63 212.064 0. ) BODY1000093_LONG_AXIS = ( 0. ) - + \begintext @@ -3282,18 +3282,18 @@ Itokawa 19P/Borrelly Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - + \begindata BODY1000005_POLE_RA = ( 218.5 0. 0. ) BODY1000005_POLE_DEC = ( -12.5 0. 0. ) BODY1000005_PM = ( 000. 390.0 0. ) BODY1000005_LONG_AXIS = ( 0. ) - + \begintext @@ -3301,239 +3301,239 @@ Itokawa - + Radii of Sun and Planets -------------------------------------------------------- - - + + Sun - + \begindata - + BODY10_RADII = ( 696000. 696000. 696000. ) - + \begintext - - + + Mercury - + Old values: - + Values are unchanged in the 2009 IAU report. - + Current values: - + \begindata - + BODY199_RADII = ( 2439.7 2439.7 2439.7 ) - + \begintext - - + + Venus - + Old values: - + Values are unchanged in the 2009 IAU report. - + Current values: - + \begindata - + BODY299_RADII = ( 6051.8 6051.8 6051.8 ) - + \begintext - - + + Earth - + Old values: - + Values are from the 2006 IAU report. - + body399_radii = ( 6378.14 6378.14 6356.75 ) Current values: - + \begindata - + BODY399_RADII = ( 6378.1366 6378.1366 6356.7519 ) - + \begintext - - + + Mars - - + + Old values: Values are from the 2006 IAU report. body499_radii = ( 3397. 3397. 3375. ) - + Current values: The 2009 IAU report gives separate values for the north and south polar radii: north: 3373.19 - south: 3379.21 + south: 3379.21 The report provides the average of these values as well, which we use as the polar radius for the triaxial model. - + \begindata - + BODY499_RADII = ( 3396.19 3396.19 3376.20 ) - + \begintext - - - + + + Jupiter - + Old values: - + Values are unchanged in the 2009 IAU report. - + Current values: - + \begindata - + BODY599_RADII = ( 71492 71492 66854 ) - + \begintext - - - + + + Saturn - + Old values: - + Values are unchanged in the 2009 IAU report. - + Current values: - + \begindata - + BODY699_RADII = ( 60268 60268 54364 ) - + \begintext - - - + + + Uranus - + Old values: - + Values are unchanged in the 2009 IAU report. - + Current values: - + \begindata - + BODY799_RADII = ( 25559 25559 24973 ) - + \begintext - - - + + + Neptune - + Old values: - + Values are unchanged in the 2009 IAU report. - + Current values: - + (Values are for the 1 bar pressure level.) - + \begindata - + BODY899_RADII = ( 24764 24764 24341 ) - + \begintext - - + + Radii of the Dwarf Planet Pluto -------------------------------------------------------- - + Pluto - + Old values: - + Values are unchanged in the 2009 IAU report. - + Current values: - + \begindata - + BODY999_RADII = ( 1195 1195 1195 ) - + \begintext - + Radii of Satellites -------------------------------------------------------- - - + + Moon - + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - + \begindata - + BODY301_RADII = ( 1737.4 1737.4 1737.4 ) - + \begintext - - - + + + Satellites of Mars - + Old values: - - Values are from the 2006 IAU report. - + + Values are from the 2006 IAU report. + body401_radii = ( 13.4 11.2 9.2 ) body402_radii = ( 7.5 6.1 5.2 ) Current values: - + \begindata - + BODY401_RADII = ( 13.0 11.4 9.1 ) BODY402_RADII = ( 7.8 6.0 5.1 ) - + \begintext - - - + + + Satellites of Jupiter - + Old values: - + Values are unchanged in the 2009 IAU report, - except for those of Europa, Ganymede, Callisto, + except for those of Europa, Ganymede, Callisto, and Metis. For Metis, now all three radii are provided. - + body502_radii = ( 1564.13 1561.23 1560.93 ) body503_radii = ( 2632.4 2632.29 2632.35 ) body504_radii = ( 2409.4 2409.2 2409.3 ) - The value for the second radius for body 516 is not given in + The value for the second radius for body 516 is not given in 2003 IAU report. The values given are: body516_radii = ( 30 --- 20 ) @@ -3546,25 +3546,25 @@ Satellites of Jupiter Current values: - + Note that for Ganymede and Callisto only mean radii are provided. \begindata - + BODY501_RADII = ( 1829.4 1819.4 1815.7 ) BODY502_RADII = ( 1562.6 1560.3 1559.5 ) BODY503_RADII = ( 2631.2 2631.2 2631.2 ) BODY504_RADII = ( 2410.3 2410.3 2410.3 ) BODY505_RADII = ( 125 73 64 ) - + \begintext - + Only mean radii are available in the 2003 IAU report for bodies 506-513. - + \begindata - + BODY506_RADII = ( 85 85 85 ) BODY507_RADII = ( 40 40 40 ) BODY508_RADII = ( 18 18 18 ) @@ -3576,18 +3576,18 @@ Satellites of Jupiter BODY514_RADII = ( 58 49 42 ) BODY515_RADII = ( 10 8 7 ) BODY516_RADII = ( 30 20 17 ) - + \begintext - - - + + + Satellites of Saturn - - + + Old values: - + Values are from the 2006 IAU report. - + body601_radii = ( 207.4 196.8 190.6 ) body602_radii = ( 256.6 251.4 248.3 ) body603_radii = ( 540.4 531.1 527.5 ) @@ -3599,33 +3599,33 @@ Satellites of Saturn body609_radii = ( 108.6 107.7 101.5 ) body610_radii = ( 97.0 95.0 77.0 ) body611_radii = ( 69.0 55.0 55.0 ) - - + + Only the first equatorial radius for Helene (body 612) is given in the 2006 IAU report: - + body612_radii = ( 17.5 --- --- ) - + The mean radius is 16km; we use this radius for all three axes, as we do for the satellites for which only the mean radius is available. - + body612_radii = ( 17.5 17.5 17.5 ) body613_radii = ( 15 12.5 7.5 ) body614_radii = ( 15.0 8.0 8.0 ) body615_radii = ( 18.5 17.2 13.5 ) body616_radii = ( 74.0 50.0 34.0 ) body617_radii = ( 55.0 44.0 31.0 ) - + For Pan, only a mean radius is given in the 2006 report. - + body618_radii = ( 10 10 10 ) - - - + + + Current values: - + \begindata - + BODY601_RADII = ( 207.8 196.7 190.6 ) BODY602_RADII = ( 256.6 251.4 248.3 ) BODY603_RADII = ( 538.4 528.3 526.3 ) @@ -3636,13 +3636,13 @@ Satellites of Saturn BODY608_RADII = ( 745.7 745.7 712.1 ) BODY609_RADII = ( 109.4 108.5 101.8 ) BODY610_RADII = ( 101.5 92.5 76.3 ) - BODY611_RADII = ( 64.9 57.0 53.1 ) + BODY611_RADII = ( 64.9 57.0 53.1 ) BODY612_RADII = ( 21.7 19.1 13.0 ) BODY613_RADII = ( 16.3 11.8 10.0 ) BODY614_RADII = ( 15.1 11.5 7.0 ) BODY615_RADII = ( 20.4 17.7 9.4 ) BODY616_RADII = ( 67.8 39.7 29.7 ) - BODY617_RADII = ( 52.0 40.5 32.0 ) + BODY617_RADII = ( 52.0 40.5 32.0 ) BODY618_RADII = ( 17.2 15.7 10.4 ) BODY632_RADII = ( 1.6 1.6 1.6 ) @@ -3650,33 +3650,33 @@ Satellites of Saturn BODY634_RADII = ( 1.5 1.2 1.0 ) BODY635_RADII = ( 4.3 4.1 3.2 ) BODY649_RADII = ( 1 1 1 ) - + \begintext - - - + + + Satellites of Uranus - + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - + \begindata - + BODY701_RADII = ( 581.1 577.9 577.7 ) BODY702_RADII = ( 584.7 584.7 584.7 ) BODY703_RADII = ( 788.9 788.9 788.9 ) BODY704_RADII = ( 761.4 761.4 761.4 ) BODY705_RADII = ( 240.4 234.2 232.9 ) - + \begintext - + The 2000 report gives only mean radii for satellites 706--715. - + \begindata - + BODY706_RADII = ( 13 13 13 ) BODY707_RADII = ( 15 15 15 ) BODY708_RADII = ( 21 21 21 ) @@ -3687,66 +3687,66 @@ Satellites of Uranus BODY713_RADII = ( 27 27 27 ) BODY714_RADII = ( 33 33 33 ) BODY715_RADII = ( 77 77 77 ) - + \begintext - - - - + + + + Satellites of Neptune - - + + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - + The 2009 report gives mean radii only for bodies 801-806. - + \begindata - + BODY801_RADII = ( 1352.6 1352.6 1352.6 ) BODY802_RADII = ( 170 170 170 ) BODY803_RADII = ( 29 29 29 ) BODY804_RADII = ( 40 40 40 ) BODY805_RADII = ( 74 74 74 ) BODY806_RADII = ( 79 79 79 ) - - \begintext - + + \begintext + The second equatorial radius for Larissa is not given in the 2009 report. The available values are: - + BODY807_RADII = ( 104 --- 89 ) - + For use within the SPICE system, we use only the mean radius. \begindata - + BODY807_RADII = ( 96 96 96 ) BODY808_RADII = ( 218 208 201 ) - + \begintext - - - - + + + + Satellites of Pluto - - + + Old values: - - Values are unchanged in the 2009 IAU report. - + + Values are unchanged in the 2009 IAU report. + Current values: - + \begindata - + BODY901_RADII = ( 605 605 605 ) - + \begintext - + Radii for Selected Comets and Asteroids @@ -3759,16 +3759,16 @@ Radii for Selected Comets and Asteroids Ceres Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: \begindata - + BODY2000001_RADII = ( 487.3 487.3 454.7 ) - + \begintext @@ -3777,16 +3777,16 @@ Vesta Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: \begindata - + BODY2000004_RADII = ( 289. 280. 229. ) - + \begintext @@ -3798,27 +3798,27 @@ Lutetia \begindata - + BODY2000021_RADII = ( 62.0 50.5 46.5 ) - + \begintext - + Ida - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - - + + \begindata - + BODY2431010_RADII = ( 26.8 12.0 7.6 ) - + \begintext @@ -3827,80 +3827,80 @@ Mathilde Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: \begindata - + BODY2000253_RADII = ( 33. 24. 23. ) - + \begintext - + Eros - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - - + + \begindata - + BODY2000433_RADII = ( 17.0 5.5 5.5 ) - + \begintext Davida - + Current values: - - + + \begindata - + BODY2000511_RADII = ( 180. 147. 127. ) - + \begintext Gaspra - + Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: - - + + \begindata - + BODY9511010_RADII = ( 9.1 5.2 4.4 ) - + \begintext - + Steins - + Current values: - - + + \begindata - + BODY2002867_RADII = ( 3.24 2.73 2.04 ) - + \begintext @@ -3909,34 +3909,34 @@ Toutatis Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: \begindata - + BODY2004179_RADII = ( 2.13 1.015 0.85 ) - + \begintext - + Itokawa Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: \begindata - + BODY2025143_RADII = ( 0.535 0.294 0.209 ) - + \begintext @@ -3947,32 +3947,32 @@ Kleopatra Values are from the 2003 report. - + body2000216_radii = ( 108.5 47 40.5 ) - + Current values: - - + + No values are provided in the 2009 report. - - + + Halley Old values: - - Values are unchanged in the 2009 IAU report. + + Values are unchanged in the 2009 IAU report. Current values: \begindata - + BODY1000036_RADII = ( 8.0 4.0 4.0 ) - + \begintext @@ -3981,18 +3981,18 @@ Halley Old values: - - The effective radius is unchanged in the 2009 IAU report. + + The effective radius is unchanged in the 2009 IAU report. Current values: - The value in the data assignment below is the + The value in the data assignment below is the "effective radius." - + According to [1]: - The maximum and minimum radii are not properly + The maximum and minimum radii are not properly the values of the principal semi-axes, they are half the maximum and minimum values of the diameter. Due to the large deviations from a @@ -4001,9 +4001,9 @@ Halley orthogonal to each other. \begindata - + BODY1000093_RADII = ( 3.0 3.0 3.0 ) - + \begintext @@ -4017,10 +4017,10 @@ Halley Current values: - The value in the data assignment below is the + The value in the data assignment below is the "effective radius." - The first principal axis length is + The first principal axis length is 3.5 km @@ -4028,7 +4028,7 @@ Halley by [1]. \begindata - + BODY1000005_RADII = ( 4.22 4.22 4.22 ) \begintext @@ -4046,16 +4046,13 @@ Halley \begindata - + BODY1000107_RADII = ( 2.7 1.9 1.5 ) - + \begintext - + =========================================================================== End of file pck00010.tpc =========================================================================== - - - diff --git a/supportData/LocalGravData/GGM03S-J2-only.txt b/supportData/LocalGravData/GGM03S-J2-only.txt index e8b773232d..525ced8274 100755 --- a/supportData/LocalGravData/GGM03S-J2-only.txt +++ b/supportData/LocalGravData/GGM03S-J2-only.txt @@ -1,4 +1,4 @@ -0.6378136300E+07, 0.3986004415E+15, 7.2921150E-5, 180, 180, 1, 0.0, 0.0 +0.6378136300E+07, 0.3986004415E+15, 7.2921150E-5, 180, 180, 1, 0.0, 0.0 0, 0, 1.000000000000E+00, 0.000000000000E+00, 0.00000E+00, 0.00000E+00 1, 0, 0.000000000000E+00, 0.000000000000E+00, 0.00000E+00, 0.00000E+00 1, 1, 0.000000000000E+00, 0.000000000000E+00, 0.00000E+00, 0.00000E+00 diff --git a/supportData/LocalGravData/GGM03S.txt b/supportData/LocalGravData/GGM03S.txt index 6022af24f8..3c4a508b3f 100755 --- a/supportData/LocalGravData/GGM03S.txt +++ b/supportData/LocalGravData/GGM03S.txt @@ -1,4 +1,4 @@ -0.6378136300E+07, 0.3986004415E+15, 7.2921150E-5, 180, 180, 1, 0.0, 0.0 +0.6378136300E+07, 0.3986004415E+15, 7.2921150E-5, 180, 180, 1, 0.0, 0.0 0, 0, 1.000000000000E+00, 0.000000000000E+00, 0.00000E+00, 0.00000E+00 1, 0, 0.000000000000E+00, 0.000000000000E+00, 0.00000E+00, 0.00000E+00 1, 1, 0.000000000000E+00, 0.000000000000E+00, 0.00000E+00, 0.00000E+00 diff --git a/supportData/LocalGravData/GGM2BData.txt b/supportData/LocalGravData/GGM2BData.txt index 93649ef0ea..2a71ba3607 100755 --- a/supportData/LocalGravData/GGM2BData.txt +++ b/supportData/LocalGravData/GGM2BData.txt @@ -1,3319 +1,3319 @@ - 3.3970000000000000E+06, 4.2828371901284001E+13, 7.3999999999999996E-05, 80, 80, 1, 0.0000000000000000E+00, 0.0000000000000000E+00 - 2, 0,-8.7450547081842009E-04, 0.0000000000000000E+00, 1.2103113782184000E-10, 0.0000000000000000E+00 - 2, 1, 1.3938449166781359E-10, 1.7044280642328221E-10, 7.3039927797648007E-11, 7.3266295432547008E-11 - 2, 2,-8.4177519807822603E-05, 4.9605348841412452E-05, 3.3991945147726000E-11, 3.4592569163869000E-11 - 3, 0,-1.1886910646015641E-05, 0.0000000000000000E+00, 9.8471786784139995E-11, 0.0000000000000000E+00 - 3, 1, 3.9053442315700724E-06, 2.5139324037413419E-05, 7.1449688816484996E-11, 7.2254867814010005E-11 - 3, 2,-1.5863411026265399E-05, 8.4857987158792132E-06, 5.9676026032878996E-11, 5.9797382572260005E-11 - 3, 3, 3.5338541142774030E-05, 2.5113984262622799E-05, 3.9949037676474000E-11, 4.0462374542868001E-11 - 4, 0, 5.1257987175465586E-06, 0.0000000000000000E+00, 1.0329911830041000E-10, 0.0000000000000000E+00 - 4, 1, 4.2271575054702128E-06, 3.7413215027228718E-06, 9.3004765290544999E-11, 9.4423002643425008E-11 - 4, 2,-1.0253884110275679E-06,-8.9622951629187374E-06, 7.3213784191804998E-11, 7.3314619138911000E-11 - 4, 3, 6.4461288728918093E-06,-2.7297790313231990E-07, 5.0594702258703002E-11, 5.1248160693008003E-11 - 4, 4, 9.6384334824044650E-08,-1.2861361694339760E-05, 2.9964623548633001E-11, 3.0045838133870998E-11 - 5, 0,-1.7242068505338999E-06, 0.0000000000000000E+00, 1.0935027490925000E-10, 0.0000000000000000E+00 - 5, 1, 4.9155252614409601E-07, 2.1179750719200639E-06, 1.0291694202540000E-10, 1.0455199159658000E-10 - 5, 2,-4.3015486989529303E-06,-1.1283599363068411E-06, 8.8105287844057999E-11, 8.7282038965633995E-11 - 5, 3, 3.3106878341316730E-06, 2.3024139448590119E-07, 6.8215826754134999E-11, 6.8675139409518995E-11 - 5, 4,-4.6889658986047850E-06,-3.2997722093047299E-06, 5.0256560862804000E-11, 4.9993069474561002E-11 - 5, 5,-4.3640801168293771E-06, 3.8656154098344251E-06, 3.2926957261939999E-11, 3.2808375832549997E-11 - 6, 0, 1.3448267510621481E-06, 0.0000000000000000E+00, 1.1989849952729999E-10, 0.0000000000000000E+00 - 6, 1, 1.7926701548423649E-06,-1.5234122376020040E-06, 1.1430564902046000E-10, 1.1578885420138000E-10 - 6, 2, 8.7185360387643135E-07, 1.4592318487313359E-06, 1.0264522918987000E-10, 1.0233400253426000E-10 - 6, 3, 9.5816746766165918E-07, 3.2022418522315923E-07, 8.4682052808222002E-11, 8.5144731447337001E-11 - 6, 4, 1.0503162831989909E-06, 2.6170003356585492E-06, 6.7176945044346996E-11, 6.6934853873167006E-11 - 6, 5, 1.6880399538737750E-06, 1.5853762547290531E-06, 4.8482587746359003E-11, 4.8320327459045001E-11 - 6, 6, 2.7768427868541821E-06, 7.5170270469872722E-07, 2.9588792491855000E-11, 3.0111321390980003E-11 - 7, 0, 1.0566966079621890E-06, 0.0000000000000000E+00, 1.3053898393156999E-10, 0.0000000000000000E+00 - 7, 1, 1.3714218129129991E-06,-2.3256794873473699E-07, 1.2668612509928999E-10, 1.2845746108779001E-10 - 7, 2, 2.8033764459323890E-06,-6.5173659805380695E-07, 1.1687908987197001E-10, 1.1629864769762000E-10 - 7, 3, 8.7336552189929800E-07,-4.0675705536590699E-07, 1.0138864653051000E-10, 1.0200867730236000E-10 - 7, 4, 2.4566369021535799E-06,-4.6224291975934719E-07, 8.4182046514154006E-11, 8.3789545340149998E-11 - 7, 5,-2.1930690076858139E-07,-1.3515575270662830E-06, 6.7293023283346998E-11, 6.7061084496940997E-11 - 7, 6,-6.0515930653241116E-07,-1.8832018842058270E-06, 5.0024296240089002E-11, 5.0354050499176000E-11 - 7, 7, 3.8833580001392989E-07,-1.7838625631807321E-06, 3.0365097497653002E-11, 3.0158240731334000E-11 - 8, 0, 1.4442117457414669E-07, 0.0000000000000000E+00, 1.4391555197484001E-10, 0.0000000000000000E+00 - 8, 1,-1.2904533441152021E-07, 7.4942490031393760E-07, 1.4035512994475001E-10, 1.4238986448210000E-10 - 8, 2, 1.8100214820539061E-06, 4.9096335865112556E-07, 1.3258296928176000E-10, 1.3177642777485001E-10 - 8, 3,-1.2207784549871089E-06,-1.3230871310063700E-06, 1.1803251372697001E-10, 1.1844597503660000E-10 - 8, 4, 1.5866931019861380E-06, 1.2166283194289211E-07, 1.0234551564058000E-10, 1.0210100863050001E-10 - 8, 5,-2.8183735255990001E-06,-1.5681467772892050E-06, 8.5387347630881000E-11, 8.5126826010749003E-11 - 8, 6,-9.5599877920939645E-07,-1.7626582142154601E-06, 6.8834697153848007E-11, 6.9560301830679008E-11 - 8, 7,-4.2471893831892682E-07, 1.6540404534478641E-06, 5.1036963686811000E-11, 5.0905844119266998E-11 - 8, 8,-3.1783805703279631E-07,-2.3919729926160170E-07, 3.0246036449331002E-11, 3.0306724975438997E-11 - 9, 0,-2.8745593244289322E-07, 0.0000000000000000E+00, 1.5944139470497001E-10, 0.0000000000000000E+00 - 9, 1, 4.1743298827892820E-07,-4.9042273171692781E-07, 1.5470877245763001E-10, 1.5677087657617000E-10 - 9, 2, 1.1385495484935660E-06, 3.7160680928003798E-07, 1.4963319721882999E-10, 1.4894233754269999E-10 - 9, 3,-1.0035199183447020E-06,-9.9277210221517757E-07, 1.3545466665846999E-10, 1.3621130548164001E-10 - 9, 4, 3.2079095125258889E-07, 1.6143044902574391E-06, 1.2095501179368999E-10, 1.2057094744733001E-10 - 9, 5,-2.2970171000700119E-06,-1.4984195288745289E-06, 1.0442727621315001E-10, 1.0430687092711999E-10 - 9, 6, 8.2020744110463016E-07, 5.5634131644082519E-07, 8.6819995795880997E-11, 8.7611819598515998E-11 - 9, 7,-5.9128904319118457E-07, 8.8413812925584658E-07, 7.1358830660606002E-11, 7.1061774488430003E-11 - 9, 8, 1.1992792300607110E-06,-1.8558235185992101E-07, 5.2995668209201000E-11, 5.2929182004139998E-11 - 9, 9,-1.1923241516342979E-06,-6.1225374183571644E-07, 2.9106076211082999E-11, 2.9395530609045002E-11 - 10, 0, 7.2586970184033398E-07, 0.0000000000000000E+00, 1.7539119019022001E-10, 0.0000000000000000E+00 - 10, 1, 9.2212003081492357E-07, 2.1871853125250699E-07, 1.7260613065107000E-10, 1.7490034816170001E-10 - 10, 2,-2.1698263972956060E-09,-1.1128765713995101E-06, 1.6661149525105999E-10, 1.6563430085192001E-10 - 10, 3,-2.9275595477379353E-07, 4.4574364384326100E-07, 1.5549353531419001E-10, 1.5608224719629999E-10 - 10, 4,-1.2112906007513811E-06,-4.9501526389811057E-08, 1.4018964991696999E-10, 1.4011950998712999E-10 - 10, 5, 3.9839975982150670E-07,-1.0683891523420349E-06, 1.2456925121774000E-10, 1.2447089194251000E-10 - 10, 6, 6.9499178396903159E-07, 1.0970214333048109E-06, 1.0718511577723000E-10, 1.0816817612797000E-10 - 10, 7, 2.9911814611399940E-07,-6.2740746269725919E-07, 9.1117241757156008E-11, 9.0615755019302006E-11 - 10, 8, 5.6556749448508493E-07, 7.9979384590180545E-07, 7.5255834664691999E-11, 7.5217730035947998E-11 - 10, 9,-1.5064865665416510E-06,-1.4044666157341789E-06, 5.6093715084827000E-11, 5.6089381349223998E-11 - 10, 10,-2.4277051735088178E-07, 7.6193623951414320E-07, 3.0465236758491000E-11, 3.0569101480682999E-11 - 11, 0,-2.6649003866535410E-07, 0.0000000000000000E+00, 1.9412621885494000E-10, 0.0000000000000000E+00 - 11, 1,-8.1519392894443437E-07,-2.1751302948038430E-07, 1.9125131244509001E-10, 1.9370687404446999E-10 - 11, 2,-3.2104320936602609E-07,-9.8790735061236510E-07, 1.8661216509076001E-10, 1.8558323281450999E-10 - 11, 3,-1.2892409568787179E-06, 7.7876627497924496E-07, 1.7539826276664999E-10, 1.7608951498311000E-10 - 11, 4,-1.5791909175600400E-06,-6.1657941677816327E-07, 1.6222588482032000E-10, 1.6215407995167999E-10 - 11, 5, 1.3703809961827519E-06, 8.6845829243624667E-07, 1.4551458457398999E-10, 1.4558131054602001E-10 - 11, 6,-2.4320602261743078E-07, 2.6287975442268369E-08, 1.2872598115413999E-10, 1.3000944402768999E-10 - 11, 7, 6.3312547465221262E-07,-8.8124580630580818E-07, 1.1251635285127000E-10, 1.1171292457530000E-10 - 11, 8,-1.1526216305062039E-06, 8.0289109851342910E-07, 9.4849314070212001E-11, 9.4759494365052999E-11 - 11, 9,-4.2887218833226868E-07,-4.0266826541827498E-07, 7.8491792268899000E-11, 7.8833465257989999E-11 - 11, 10, 4.1248741283895690E-07, 1.9449054488648321E-06, 5.7429923668925998E-11, 5.7204755236884002E-11 - 11, 11,-5.9129125196024682E-08,-3.2013461955227138E-07, 2.8669458192070001E-11, 2.8703820604629998E-11 - 12, 0, 2.5901010973447811E-07, 0.0000000000000000E+00, 2.1544212220396000E-10, 0.0000000000000000E+00 - 12, 1,-1.1232760242154771E-06,-4.6597501385511642E-08, 2.1240222807557999E-10, 2.1517552677388000E-10 - 12, 2,-9.3180509514092405E-08, 5.3208625826598304E-07, 2.0836388521206001E-10, 2.0730039832943999E-10 - 12, 3,-1.4328437159904669E-06, 3.3094328849604699E-07, 1.9790381030890000E-10, 1.9863260618291999E-10 - 12, 4,-9.8989762948794809E-08, 1.2477384606767650E-07, 1.8521062384222000E-10, 1.8506816515755000E-10 - 12, 5, 7.3841719085192487E-07, 9.8313706759767646E-07, 1.6957326958723000E-10, 1.6957690960913000E-10 - 12, 6,-4.4199660810539768E-07,-1.6138324546284839E-06, 1.5136857109862001E-10, 1.5288392827742000E-10 - 12, 7, 3.9950015269219688E-07,-1.2420962147609649E-07, 1.3592081848620999E-10, 1.3511281006034000E-10 - 12, 8,-1.6103036177638721E-06,-3.3390142111388809E-07, 1.1789375872111001E-10, 1.1786734461664001E-10 - 12, 9, 7.2319162327703644E-07, 4.4834612822914618E-07, 9.9879585325839996E-11, 1.0049027768587001E-10 - 12, 10, 5.4275397597080196E-07, 1.3521153280272900E-06, 8.4332707588165004E-11, 8.3844073261327006E-11 - 12, 11, 7.8624501327329339E-07,-1.6864131081620689E-06, 6.2473070064380999E-11, 6.3135152971843006E-11 - 12, 12,-1.0856558755016089E-08,-8.8551160240718884E-08, 3.7956014392819003E-11, 3.7611187752070002E-11 - 13, 0,-4.8952404831803390E-07, 0.0000000000000000E+00, 2.3922705382994002E-10, 0.0000000000000000E+00 - 13, 1,-5.6648398354796893E-07, 2.2788663120968919E-07, 2.3567363500466001E-10, 2.3878585276678001E-10 - 13, 2, 1.9841104168070469E-07, 8.4834357183256708E-07, 2.3277554281839000E-10, 2.3170622185943001E-10 - 13, 3, 2.1767220813221301E-07, 1.7831477048833361E-07, 2.2242693420350000E-10, 2.2314084792430999E-10 - 13, 4, 1.8222961512093351E-07, 6.8437925412020000E-07, 2.1033196052609000E-10, 2.1049042014077001E-10 - 13, 5,-2.1635837716555661E-09,-4.0358226801550021E-07, 1.9467991713149999E-10, 1.9501144632263999E-10 - 13, 6, 2.0889697822006498E-08,-9.2963519094536122E-07, 1.7718448092050001E-10, 1.7896489279944000E-10 - 13, 7,-7.5633516483173801E-07, 4.1816002573524902E-07, 1.6048616227869000E-10, 1.5934076308326999E-10 - 13, 8,-2.3166748437483840E-07, 1.2030332905113710E-09, 1.4249650274048000E-10, 1.4266366612095000E-10 - 13, 9, 1.1327419797014090E-06, 7.6091981962187019E-07, 1.2376915902365999E-10, 1.2469998624707001E-10 - 13, 10,-1.8420690773034191E-07,-6.3834053349287647E-07, 1.0643120864673000E-10, 1.0566216959408000E-10 - 13, 11, 7.2152491396419184E-07,-8.5419916802722339E-07, 8.9076403459725997E-11, 8.9676279954839007E-11 - 13, 12,-1.4713308304428070E-06,-3.5633994441531591E-07, 6.5055201193692995E-11, 6.5129690420097996E-11 - 13, 13, 4.7249117577621590E-07, 8.4684144373715813E-07, 3.6320246678525002E-11, 3.6828348268609003E-11 - 14, 0, 3.0219271864045722E-07, 0.0000000000000000E+00, 2.6556530076336000E-10, 0.0000000000000000E+00 - 14, 1, 7.5309548515849434E-07, 3.0659939197390379E-07, 2.6233129402536999E-10, 2.6556182531491001E-10 - 14, 2, 4.4431057610504009E-07,-5.2539256378857698E-07, 2.5932099596772002E-10, 2.5802064520266001E-10 - 14, 3, 4.2086425064021988E-07,-3.1613141949167798E-07, 2.4997785445991998E-10, 2.5076161843620999E-10 - 14, 4, 2.1086607659015601E-07,-9.6126046924893883E-07, 2.3771365012418000E-10, 2.3784292426187999E-10 - 14, 5,-4.7256009813144700E-07,-9.5209040368264479E-07, 2.2291973969368000E-10, 2.2331186086574999E-10 - 14, 6,-6.8240005934222495E-08,-8.1255730199729201E-08, 2.0456642237235999E-10, 2.0692637694372000E-10 - 14, 7,-4.3833797637763461E-07, 2.5344830630813670E-07, 1.8866572025066999E-10, 1.8730060109863999E-10 - 14, 8, 8.2760862524879536E-07, 3.4542413026249739E-07, 1.6899410807335000E-10, 1.6907860932119999E-10 - 14, 9, 2.5206510270110481E-07, 1.1412035615165890E-06, 1.5033736238848001E-10, 1.5179467349326000E-10 - 14, 10,-4.0006125111822032E-08,-1.5805411521501649E-06, 1.3268506472902000E-10, 1.3209264822394001E-10 - 14, 11,-9.1756400631668835E-07, 2.0181206830551511E-07, 1.1265659621480000E-10, 1.1338872230048000E-10 - 14, 12,-4.6098789395114132E-07,-2.8123541924842100E-07, 9.5933862469390002E-11, 9.6244721746418006E-11 - 14, 13, 8.6159647817875325E-07, 2.0166474791487490E-06, 7.4316359088339006E-11, 7.4854791147524003E-11 - 14, 14, 4.0566294949909653E-09,-7.4397446155337985E-07, 3.8421806471019999E-11, 3.8721148521822999E-11 - 15, 0, 4.9338119977619251E-07, 0.0000000000000000E+00, 2.9517746279730999E-10, 0.0000000000000000E+00 - 15, 1, 2.3504085774446549E-07, 2.5814277194372382E-07, 2.9153422873027998E-10, 2.9510973190729001E-10 - 15, 2,-3.5877664452026991E-07,-7.7775370918244422E-07, 2.8937289735560998E-10, 2.8809597971141002E-10 - 15, 3,-5.6235428831306517E-07,-2.9119276531665789E-07, 2.7991900779889000E-10, 2.8061418388809002E-10 - 15, 4,-7.3107741433901826E-07,-8.2291945418793019E-07, 2.6859288097318999E-10, 2.6869042589351999E-10 - 15, 5,-9.4418999262905104E-07,-4.5309344036893439E-07, 2.5320088176142001E-10, 2.5374084143311002E-10 - 15, 6,-4.6569424928889101E-08, 6.3181043403389908E-07, 2.3538004318833002E-10, 2.3808694212132000E-10 - 15, 7, 9.3252509300585759E-07, 3.3367910077558299E-07, 2.1874533651858999E-10, 2.1704461108567000E-10 - 15, 8, 1.1713557431122270E-06, 4.3397393082264647E-07, 1.9895169713842000E-10, 1.9935016997159999E-10 - 15, 9,-3.1361197239010152E-07,-3.6130776998697968E-07, 1.7843735281586000E-10, 1.8027480683255001E-10 - 15, 10,-1.6617979177073590E-07,-1.4989148710418020E-07, 1.6089851425946000E-10, 1.6020667736964000E-10 - 15, 11,-7.4449581533426630E-07, 6.0499732095853779E-07, 1.4050475077078000E-10, 1.4113581034136000E-10 - 15, 12, 1.0125729414718300E-06, 7.0744643143345193E-07, 1.2077765476258000E-10, 1.2113372705228000E-10 - 15, 13, 2.1632945779220320E-07, 6.9692452639138542E-07, 1.0355725991470000E-10, 1.0471853347515001E-10 - 15, 14, 1.4287311615973200E-07,-1.3675794263071820E-06, 7.4065406404841999E-11, 7.3892787879709996E-11 - 15, 15,-3.2983911020273521E-07, 1.4679301357791700E-07, 3.9262982920572003E-11, 3.8815308015795001E-11 - 16, 0, 6.2492272251973242E-07, 0.0000000000000000E+00, 3.2805690113790010E-10, 0.0000000000000000E+00 - 16, 1,-2.3576455388861381E-07,-5.5301461123008231E-07, 3.2467108947652001E-10, 3.2840534860225002E-10 - 16, 2,-4.8128515175366109E-07,-8.3798338110112221E-08, 3.2223553208065998E-10, 3.2088216250179002E-10 - 16, 3,-5.2884584512722868E-07, 4.3179535117818742E-07, 3.1338928892497998E-10, 3.1432050775531001E-10 - 16, 4,-7.7524880882120419E-07,-4.1236194296459131E-08, 3.0177887171033002E-10, 3.0235677774235001E-10 - 16, 5,-3.3156669814606880E-07, 5.9447709629048816E-07, 2.8723444746464998E-10, 2.8801243692789999E-10 - 16, 6, 1.4873519233098461E-07, 2.1995105650212950E-07, 2.6860427150630999E-10, 2.7201072932818998E-10 - 16, 7, 3.9540191879011490E-07, 2.9924170216304151E-07, 2.5266259649062001E-10, 2.5097762688375000E-10 - 16, 8, 2.4726774880144362E-07, 3.8194759694030728E-07, 2.3126097274353999E-10, 2.3205851951551000E-10 - 16, 9,-3.9083254186985018E-07,-9.2091247731411718E-07, 2.1087090709721001E-10, 2.1309604245963001E-10 - 16, 10,-4.9887892959809643E-07, 3.6222033033373808E-07, 1.9170274792531999E-10, 1.9102897234817000E-10 - 16, 11,-8.0503692776153824E-08,-3.0833185246732417E-07, 1.7102041766089001E-10, 1.7187789196071001E-10 - 16, 12, 4.4116504364058009E-07, 6.4720068684209609E-07, 1.5147887036299000E-10, 1.5226342447604000E-10 - 16, 13,-9.6676684214209105E-08,-5.9086298237427453E-07, 1.2965348593317001E-10, 1.3095008462812001E-10 - 16, 14,-2.2346863154783541E-07,-9.2646534797515104E-07, 1.1023249349617000E-10, 1.1033431106696000E-10 - 16, 15,-8.3320558932387890E-07, 3.7522848734954839E-07, 7.9994374080275996E-11, 7.9815215662268004E-11 - 16, 16, 2.1693483565260650E-07, 1.0572655635748210E-07, 4.4719179009241003E-11, 4.4297873663697003E-11 - 17, 0,-5.4709542986930998E-08, 0.0000000000000000E+00, 3.6486938511132998E-10, 0.0000000000000000E+00 - 17, 1,-3.7399137381517438E-07,-6.3489366161239067E-07, 3.6139366386669997E-10, 3.6555218681949001E-10 - 17, 2,-2.0004915070624970E-07, 1.4769821786575250E-07, 3.5904127896492010E-10, 3.5769757037661011E-10 - 17, 3,-4.4054829757514781E-08, 1.9182219078222199E-07, 3.5064014324423999E-10, 3.5126578586621010E-10 - 17, 4, 4.5617658067346931E-07,-1.7248528799716611E-08, 3.3926128392930001E-10, 3.3982901241850999E-10 - 17, 5, 7.0352057270776358E-07, 2.9716758199903928E-07, 3.2468741422718010E-10, 3.2513551110133001E-10 - 17, 6, 6.2735688086993120E-07,-3.2424361898797991E-07, 3.0601686597374010E-10, 3.0972440072692999E-10 - 17, 7, 3.3067607545801760E-07,-2.4963319407555199E-07, 2.8950999985072002E-10, 2.8759668614058998E-10 - 17, 8,-3.7638667604706180E-07,-1.9171388957514899E-07, 2.6798109429521002E-10, 2.6869443710152001E-10 - 17, 9,-2.0630975795240439E-07,-3.2050677363791618E-07, 2.4565365760960002E-10, 2.4851552404129003E-10 - 17, 10,-5.5645632584498266E-07, 2.8962148266298910E-07, 2.2656892831538001E-10, 2.2570862491793000E-10 - 17, 11, 3.5243509929602669E-07, 3.5354260317607713E-08, 2.0412432326884999E-10, 2.0487607979741001E-10 - 17, 12, 2.0025673973076780E-07,-1.3079093383117499E-08, 1.8339435658969001E-10, 1.8454434218630000E-10 - 17, 13,-4.8612342952491358E-07,-7.7483339436755765E-07, 1.6198903492658001E-10, 1.6326363756298999E-10 - 17, 14,-3.8008064670123599E-07,-4.4794294782534088E-07, 1.3980184549476999E-10, 1.3977109236774000E-10 - 17, 15,-3.3804523377146830E-07, 4.2775478707706719E-07, 1.1880084629867000E-10, 1.1846428801309000E-10 - 17, 16, 1.1049431287614231E-06, 4.9309080047218621E-07, 8.4458348846849004E-11, 8.4040825824180008E-11 - 17, 17, 7.4847480457838662E-08, 2.8783514545524541E-07, 4.6320910087850003E-11, 4.6044632780380997E-11 - 18, 0,-3.1250064729225039E-07, 0.0000000000000000E+00, 4.0647151544338010E-10, 0.0000000000000000E+00 - 18, 1,-1.8296324000412531E-07,-5.7769470567911306E-07, 4.0182053213840002E-10, 4.0613395819612002E-10 - 18, 2,-1.0412207107656000E-07, 4.5344370424782177E-08, 4.0091840702135999E-10, 3.9916212618639010E-10 - 18, 3,-2.9939186240254088E-07,-4.5784264171329838E-07, 3.9111971648737002E-10, 3.9213646782313002E-10 - 18, 4, 8.7501712948002529E-07, 1.1649861097786980E-07, 3.8110333143449998E-10, 3.8149708418869998E-10 - 18, 5, 1.9313905997574150E-07,-6.2249966128910834E-07, 3.6571291034253002E-10, 3.6677764161835998E-10 - 18, 6, 2.0688943917981149E-07,-6.8437607860474299E-07, 3.4697265703707002E-10, 3.5152607038042010E-10 - 18, 7,-2.0217971073446040E-07,-1.6356127281011639E-07, 3.3079412662467999E-10, 3.2849516979069998E-10 - 18, 8, 5.2056773524533092E-08,-2.6360703691128459E-07, 3.0821050140840002E-10, 3.0911912357847998E-10 - 18, 9, 7.7969026722603824E-08, 2.2552919560177530E-07, 2.8548535497369002E-10, 2.8846431362759998E-10 - 18, 10, 6.6329434317302129E-07,-9.4810465055948729E-08, 2.6476790067956001E-10, 2.6398470495955001E-10 - 18, 11, 2.6059961095133892E-07,-3.1345741040837309E-07, 2.4179754271710999E-10, 2.4254330369547999E-10 - 18, 12, 6.8911605863124401E-08,-1.0399482958457250E-07, 2.1927104697961000E-10, 2.2060181574120001E-10 - 18, 13,-3.1271776039499141E-07,-2.5122404024047290E-07, 1.9702549436515000E-10, 1.9801809485136001E-10 - 18, 14,-3.5413778761107888E-07, 2.1245035795232279E-07, 1.7435835357738001E-10, 1.7446770355651000E-10 - 18, 15, 4.0427231994548502E-07, 7.2717527650255237E-07, 1.5073384274190999E-10, 1.5057492391765001E-10 - 18, 16, 4.2540458121371041E-07, 1.9491720533030481E-07, 1.2840222181280000E-10, 1.2749581817833001E-10 - 18, 17,-6.6185211446951153E-09,-1.1055839853856400E-06, 9.2880725482521999E-11, 9.2084105750115004E-11 - 18, 18, 4.9076799412949712E-07, 3.0810258604566902E-07, 5.3443957066628999E-11, 5.3472074408286003E-11 - 19, 0, 5.9593471480895979E-09, 0.0000000000000000E+00, 4.5245741196867011E-10, 0.0000000000000000E+00 - 19, 1, 4.7984786635487607E-08, 5.0606657054152741E-07, 4.4709204533812002E-10, 4.5182610404806003E-10 - 19, 2,-5.2810415618861903E-08, 8.7643122788538723E-08, 4.4666089453549010E-10, 4.4526615944716010E-10 - 19, 3,-5.4861024791885132E-08,-1.9930153462648481E-07, 4.3757055597421999E-10, 4.3792789594428999E-10 - 19, 4, 1.4263562742559671E-07, 2.5044457390705712E-07, 4.2660285644026998E-10, 4.2717024220667010E-10 - 19, 5,-2.9276850316067513E-07,-4.3895607578845078E-07, 4.1214652696741998E-10, 4.1326737715955010E-10 - 19, 6,-2.7765506917946031E-07,-1.4472759335102671E-07, 3.9203776243683999E-10, 3.9750433422089011E-10 - 19, 7,-4.8276286290870036E-07, 2.0107368970881271E-07, 3.7669164889519010E-10, 3.7403156471358002E-10 - 19, 8, 4.2212800997513992E-07,-1.4536614945671280E-07, 3.5255605664897998E-10, 3.5434298597904999E-10 - 19, 9, 2.2422934903485960E-07, 3.8573458502090812E-07, 3.2881766074467010E-10, 3.3270013700692998E-10 - 19, 10, 5.9754731714712053E-07,-1.0349337739528020E-07, 3.0770856996466001E-10, 3.0695011263143010E-10 - 19, 11, 2.6748761625195819E-08,-3.9238448990827910E-07, 2.8331503232646010E-10, 2.8393091220607998E-10 - 19, 12,-1.1353985029612160E-07,-1.5548311289263989E-08, 2.5947446619016998E-10, 2.6083737630754998E-10 - 19, 13,-2.0231048781492040E-07,-2.9076960333033691E-08, 2.3568136690029002E-10, 2.3687764768677002E-10 - 19, 14, 1.8370489788655959E-07, 5.0561428737075958E-07, 2.1267880380456999E-10, 2.1271931494252001E-10 - 19, 15, 8.5469937506493990E-07, 4.5122259786035062E-07, 1.8759663496207000E-10, 1.8754393534591001E-10 - 19, 16,-1.5178564199107839E-08,-1.3538949530565789E-07, 1.6367988101766000E-10, 1.6249570081939000E-10 - 19, 17,-1.4691049019410600E-07,-4.2595464334983788E-07, 1.3905860915571000E-10, 1.3786940077562999E-10 - 19, 18,-6.7605472888485071E-07, 6.4337251024823409E-07, 9.8584004670604003E-11, 9.8376567566550997E-11 - 19, 19,-4.2895397427418769E-07,-8.0085248199827081E-07, 5.4420332957366998E-11, 5.4816334366905998E-11 - 20, 0, 2.9881260537936598E-07, 0.0000000000000000E+00, 5.0184663007148015E-10, 0.0000000000000000E+00 - 20, 1, 1.8580341470164049E-07, 5.5166467757316584E-07, 4.9919952052003007E-10, 5.0467931410559008E-10 - 20, 2, 6.7077484529405142E-08,-2.3854889220809512E-07, 4.9652043698732999E-10, 4.9477800571342004E-10 - 20, 3, 4.2629483620741551E-07, 2.0444588490030209E-07, 4.8921251982749000E-10, 4.9054971284786009E-10 - 20, 4,-3.5522136272433662E-07, 2.1259491309390880E-07, 4.7657854594275000E-10, 4.7779056899115007E-10 - 20, 5, 1.6924862463125410E-07, 1.1045333324553961E-08, 4.6333840475570002E-10, 4.6479758232154010E-10 - 20, 6, 2.0998547236303271E-07, 3.7356644384240269E-07, 4.4289799342539998E-10, 4.4858874167258998E-10 - 20, 7, 7.0244713844181063E-08, 3.7898258389081973E-08, 4.2687270596393011E-10, 4.2425284453157999E-10 - 20, 8, 3.2136235256075510E-07, 3.0592253100467350E-07, 4.0288449827823999E-10, 4.0434545266766999E-10 - 20, 9, 6.5192996480357300E-08, 1.7864588537008961E-07, 3.7724888098863002E-10, 3.8189531242642001E-10 - 20, 10,-4.2083149137409152E-07,-1.0435247651158750E-08, 3.5570227006743001E-10, 3.5466936557795010E-10 - 20, 11,-1.6114641023949900E-07, 2.7220617299528919E-08, 3.2991960684076998E-10, 3.3070174759012002E-10 - 20, 12,-4.7994489225668577E-07,-1.4795540504238811E-07, 3.0414458499437000E-10, 3.0610234070543998E-10 - 20, 13, 1.8109737849233900E-08, 3.4234017008717649E-07, 2.7934920709935001E-10, 2.8050870203489010E-10 - 20, 14, 2.9344108002153201E-07, 2.1671360097310811E-07, 2.5444946864621000E-10, 2.5455916228636002E-10 - 20, 15, 6.0208708189420820E-08,-2.9039462561300382E-07, 2.2927240159561999E-10, 2.2926338492610000E-10 - 20, 16, 2.8414172567249571E-08,-2.7761158323091630E-07, 2.0339971859272001E-10, 2.0208547506292999E-10 - 20, 17,-5.1094548886474557E-07,-3.4292939751667268E-07, 1.7731480238238001E-10, 1.7567791351635001E-10 - 20, 18, 8.9167321908967871E-08, 7.1919662036032088E-07, 1.4970632314231001E-10, 1.5002809557524000E-10 - 20, 19, 1.7333602608012730E-07,-3.2472457615893582E-08, 1.0807864590361000E-10, 1.0850544460582000E-10 - 20, 20,-4.6655014134911748E-07, 1.0747723767163330E-07, 6.1889870171316002E-11, 6.1881103359350995E-11 - 21, 0, 1.2347586822151001E-07, 0.0000000000000000E+00, 5.6092145547486002E-10, 0.0000000000000000E+00 - 21, 1,-1.4966915629923551E-07,-4.2489275995067721E-07, 5.5400146306094002E-10, 5.5948244875631003E-10 - 21, 2,-9.7221100336762922E-08,-5.5509403173538134E-07, 5.5559866749161015E-10, 5.5353506895829009E-10 - 21, 3,-1.5552364970400649E-07, 2.4876536599425881E-07, 5.4445577957016006E-10, 5.4525681021975004E-10 - 21, 4,-4.0097666230990929E-07,-1.3425496908502699E-07, 5.3519179232478005E-10, 5.3652444095995008E-10 - 21, 5, 3.2870712432165809E-07, 1.5936242888488789E-07, 5.1852943652637005E-10, 5.2000039742390003E-10 - 21, 6, 6.9079939553383232E-08, 2.7488485498799867E-07, 4.9981092505811000E-10, 5.0714328273240007E-10 - 21, 7, 2.3320857354459750E-07,-3.0863895600893129E-07, 4.8248339531331006E-10, 4.7975408264332006E-10 - 21, 8, 5.1570907658035643E-08, 3.0886698315441479E-07, 4.5794180553710011E-10, 4.6023982925519998E-10 - 21, 9,-1.8008202114286151E-07,-9.4958964606421974E-08, 4.3188529209723011E-10, 4.3672251035467998E-10 - 21, 10,-2.7938955389263798E-07, 1.1820439495967310E-07, 4.0861710491605002E-10, 4.0802852538335999E-10 - 21, 11,-2.8796500726804299E-07,-1.5823916152800439E-07, 3.8192843799357998E-10, 3.8242625999403001E-10 - 21, 12, 2.5731362205597250E-07, 6.4541115316012470E-08, 3.5479112611681001E-10, 3.5647543697996001E-10 - 21, 13, 8.3708645895450682E-09, 1.0116864535512461E-07, 3.2836010572066999E-10, 3.2931642025641997E-10 - 21, 14, 1.1134161192369960E-07,-3.9795307952092191E-07, 3.0182862717165000E-10, 3.0142690991002002E-10 - 21, 15,-3.6470794043294531E-07,-5.5740534092566497E-07, 2.7431998248750002E-10, 2.7465546207263001E-10 - 21, 16,-2.8556691172751940E-07, 5.5442768395247233E-09, 2.4864100787316002E-10, 2.4739973849435998E-10 - 21, 17,-2.8525352941551792E-07,-3.0837599867540438E-07, 2.2024612655286001E-10, 2.1846283807876001E-10 - 21, 18, 2.8317542188568368E-07, 5.2143158077609181E-07, 1.9151199655527999E-10, 1.9189295501394001E-10 - 21, 19, 1.2069812234460869E-07,-7.8399759298237206E-07, 1.6264619656004000E-10, 1.6424303374405000E-10 - 21, 20,-1.4923345109938631E-07,-3.0137302609478931E-07, 1.1617409847869000E-10, 1.1608810333569000E-10 - 21, 21, 6.9990179109415883E-07, 3.3555798221123139E-07, 6.3777843344463999E-11, 6.3514837029015004E-11 - 22, 0,-1.1264028279608120E-07, 0.0000000000000000E+00, 6.2169060766361004E-10, 0.0000000000000000E+00 - 22, 1,-4.0208656684925192E-07,-1.1826221354554750E-07, 6.1965153364098003E-10, 6.2615300164572006E-10 - 22, 2, 1.0381732434953100E-07, 7.4421453554010241E-08, 6.1619571586371009E-10, 6.1437823674776015E-10 - 22, 3,-3.7677474476696823E-07, 2.0395835626638869E-08, 6.1004989636936010E-10, 6.1121172491412008E-10 - 22, 4, 1.8933663793100171E-07,-3.8145043638466721E-07, 5.9621059310135002E-10, 5.9724675802340004E-10 - 22, 5, 7.3369812805457375E-08, 3.0105913448950862E-08, 5.8380002824166002E-10, 5.8534510562074007E-10 - 22, 6,-2.8170834311131842E-07,-5.2344037555341106E-07, 5.6006184352243010E-10, 5.6811654838524008E-10 - 22, 7, 2.6177231161258758E-07, 5.6295008574609990E-09, 5.4735297948348001E-10, 5.4358506739729000E-10 - 22, 8,-3.7895979428661201E-07,-1.9937087713119211E-07, 5.1845594288649999E-10, 5.2055382967662007E-10 - 22, 9,-1.9429436459151098E-09,-3.0284890893445682E-08, 4.9273472447378003E-10, 4.9842158159693000E-10 - 22, 10,-2.4257445288973869E-07, 2.8902128641733269E-07, 4.6836846575480003E-10, 4.6714925278314001E-10 - 22, 11,-9.3715993038994807E-08,-1.5687894061080690E-07, 4.3998301463812002E-10, 4.4035229977679999E-10 - 22, 12, 3.7559620252873468E-07,-8.9063527407004070E-08, 4.1083535556573011E-10, 4.1329956920568002E-10 - 22, 13,-1.7940313993881491E-07,-1.6316979104796981E-07, 3.8327527467533010E-10, 3.8412764269703002E-10 - 22, 14,-1.7537860033055239E-07,-5.0922917414207737E-07, 3.5463882450144001E-10, 3.5481368759858998E-10 - 22, 15,-4.0193319616745418E-07, 2.8477577528598978E-07, 3.2574983269088001E-10, 3.2589798949287999E-10 - 22, 16,-2.0310502853512480E-07,-2.8750227028928629E-08, 2.9777160193187002E-10, 2.9643383144971998E-10 - 22, 17, 3.4627242633572500E-07, 2.1188159428630770E-07, 2.6924788530568001E-10, 2.6773927227122998E-10 - 22, 18,-1.1210259579021690E-07, 6.0333782848045594E-08, 2.3782341617990002E-10, 2.3795103410093000E-10 - 22, 19, 3.7629022242182029E-07,-4.4970927422294212E-07, 2.0736735553430001E-10, 2.0894773988851999E-10 - 22, 20,-4.3042697908639468E-07, 9.2797413487135024E-08, 1.7661613167423001E-10, 1.7733446902182000E-10 - 22, 21, 2.7695836337858037E-07, 5.6231669757848327E-07, 1.2631250616197999E-10, 1.2595236295029000E-10 - 22, 22,-1.4347583619729300E-07,-3.1326857604805262E-07, 6.9629579226962003E-11, 6.9528756326033998E-11 - 23, 0, 4.7326423023179462E-08, 0.0000000000000000E+00, 6.9514404455145001E-10, 0.0000000000000000E+00 - 23, 1, 2.4374408042259629E-07, 7.3494112778137450E-08, 6.8743829520618005E-10, 6.9375960268094010E-10 - 23, 2, 1.9843225742036479E-07, 4.6791638769017139E-07, 6.9006399300259002E-10, 6.8766546507300006E-10 - 23, 3, 9.9820782624612770E-08,-1.3263849456797489E-07, 6.7785554674036007E-10, 6.7869323133403003E-10 - 23, 4, 4.4503832725168122E-07, 6.6478968884065293E-08, 6.6922174058685010E-10, 6.7067286057720006E-10 - 23, 5,-3.1144276467443448E-07, 1.9519929635273781E-07, 6.5126628872723010E-10, 6.5300927885387002E-10 - 23, 6, 3.6992172443827102E-07,-5.0612553610889074E-07, 6.3188830582267007E-10, 6.4149439834482005E-10 - 23, 7,-2.8487071785549231E-07, 2.9122642661273411E-07, 6.1413147156428008E-10, 6.1044040213774007E-10 - 23, 8,-2.2579336399057979E-07,-2.7709320642165849E-07, 5.8925387976550001E-10, 5.9194463829239007E-10 - 23, 9, 2.7359227298597801E-08, 1.0347913896544291E-07, 5.5785816316293006E-10, 5.6459337667553007E-10 - 23, 10,-9.5790812490446192E-08,-1.4405809987966211E-07, 5.3586962653184008E-10, 5.3508630406274009E-10 - 23, 11, 3.2113423215861948E-07,-3.2797563249356841E-07, 5.0446832601535004E-10, 5.0469166421631008E-10 - 23, 12, 7.3832375707856811E-08,-1.2217476464311631E-08, 4.7417662077922001E-10, 4.7669127302433001E-10 - 23, 13,-2.3140846208712029E-07,-8.4294987746199942E-08, 4.4442532592368999E-10, 4.4544261667375011E-10 - 23, 14,-1.0224728887818180E-07,-3.1770480727318979E-07, 4.1444578033871011E-10, 4.1391850023898002E-10 - 23, 15,-2.3223458029351269E-07, 7.1651363037084832E-07, 3.8305576227990002E-10, 3.8375209257075999E-10 - 23, 16, 4.4498981004519351E-07, 1.0929769880668740E-07, 3.5351146204446999E-10, 3.5173149973274999E-10 - 23, 17, 3.6670408070543000E-07, 7.6096781011156177E-07, 3.2251074277606001E-10, 3.2117057284674000E-10 - 23, 18,-1.7642610439993799E-07,-5.9165257242252639E-07, 2.9126447139445998E-10, 2.9164307450757001E-10 - 23, 19, 4.1926005181402358E-07, 5.6605058404973177E-08, 2.5755571420528000E-10, 2.5985479383052000E-10 - 23, 20,-4.8862524724287533E-07, 4.5045545106160693E-08, 2.2637066371170001E-10, 2.2669893360440999E-10 - 23, 21, 4.9604626390305803E-07, 6.1353338185901423E-07, 1.9428987756624001E-10, 1.9363369423127999E-10 - 23, 22, 1.5353129714946131E-07,-2.0327835563482890E-07, 1.3719233472675999E-10, 1.3690091114198000E-10 - 23, 23,-5.2692420718805256E-07, 2.7878783895039369E-07, 7.5362044269102005E-11, 7.5331515616826995E-11 - 24, 0, 2.6873303650461098E-07, 0.0000000000000000E+00, 7.7182446151493005E-10, 0.0000000000000000E+00 - 24, 1, 1.9057622150479901E-07, 1.6690861715652519E-07, 7.6790671154419009E-10, 7.7503580713019002E-10 - 24, 2, 1.7280977634451560E-08,-1.3850302464473820E-07, 7.6654204116784007E-10, 7.6428108494813003E-10 - 24, 3, 3.2921208881991352E-07,-2.9176290248854039E-07, 7.5830076837382007E-10, 7.5977830689642005E-10 - 24, 4, 5.9218617620265157E-08, 1.1739532630109240E-07, 7.4513892186620003E-10, 7.4698267772589002E-10 - 24, 5,-3.5012008481587548E-07,-1.5733433810897421E-07, 7.3135906527225008E-10, 7.3413088551381010E-10 - 24, 6, 3.1642695265316869E-07, 2.9276330519769568E-08, 7.0655222559914004E-10, 7.1720256827696007E-10 - 24, 7,-3.9264498823450741E-07, 5.1879797911087025E-07, 6.9432846044614006E-10, 6.9002327097593007E-10 - 24, 8, 1.8933598874193869E-07, 1.1078479731924260E-07, 6.6276844743168009E-10, 6.6609220109415005E-10 - 24, 9, 1.2527409470495221E-07, 3.2842814568059190E-07, 6.3575495049220010E-10, 6.4312111341184006E-10 - 24, 10,-1.3450041125559309E-07,-3.0140135882258529E-07, 6.0736843489246001E-10, 6.0679577452778007E-10 - 24, 11, 1.0843022480946701E-08, 3.0915504653432879E-07, 5.7875627748271005E-10, 5.7925345837186004E-10 - 24, 12,-3.1255874794963632E-07,-3.0429281336725132E-07, 5.4378373731387000E-10, 5.4609935260132008E-10 - 24, 13, 7.5599798100225250E-08,-2.7849631860020731E-08, 5.1360702190987009E-10, 5.1495325892458001E-10 - 24, 14, 2.6270393549553840E-08, 3.9848536131460611E-07, 4.8077142508029000E-10, 4.8094993769846003E-10 - 24, 15, 2.1284441845550539E-07,-4.2522142616756123E-08, 4.4779655643690010E-10, 4.4854152251518998E-10 - 24, 16, 6.2126031993091466E-07, 2.8516896440125381E-07, 4.1609027341372002E-10, 4.1444023338576999E-10 - 24, 17,-1.0668238261122960E-07,-1.5743112235114869E-08, 3.8298057311606010E-10, 3.8170659501541001E-10 - 24, 18, 3.1846210628267689E-07,-5.7222896549097009E-07, 3.4922869260153002E-10, 3.4934033965158002E-10 - 24, 19,-2.5065325932672343E-07, 4.2087828662401751E-07, 3.1565513488044000E-10, 3.1754515510757999E-10 - 24, 20,-9.3603719702306670E-08,-1.4150417744732959E-07, 2.8066997368318002E-10, 2.8152979527744002E-10 - 24, 21, 5.0245562931673793E-09, 4.9620199514226188E-07, 2.4723388648750000E-10, 2.4604678129472002E-10 - 24, 22,-1.6939863171226771E-07,-6.9209106756932092E-07, 2.1099215652271000E-10, 2.1020286601959999E-10 - 24, 23,-5.8779889641222291E-07, 4.1208495271270067E-08, 1.4599724461299000E-10, 1.4602346682201001E-10 - 24, 24, 2.8572403319338679E-07, 1.0611983382256000E-07, 7.7481299402831007E-11, 7.7794732165366008E-11 - 25, 0,-1.3915637435630249E-07, 0.0000000000000000E+00, 8.6195340975259007E-10, 0.0000000000000000E+00 - 25, 1,-7.7127738215862221E-08,-1.7543650003502259E-08, 8.5318896866255006E-10, 8.6097616139176004E-10 - 25, 2,-1.7403806696811899E-07,-3.5958046277900300E-07, 8.5662046572052011E-10, 8.5392874905001003E-10 - 25, 3, 1.0709464053995960E-07,-1.4852699810844249E-07, 8.4451062522032008E-10, 8.4495247223287005E-10 - 25, 4,-6.1577275010244243E-07, 1.6531831359390281E-07, 8.3429405274330011E-10, 8.3613264241993008E-10 - 25, 5, 2.7870987142976032E-07,-3.2513243257401430E-07, 8.1713461927319006E-10, 8.1920410443996006E-10 - 25, 6,-1.5069102202669251E-07, 3.5398705137297831E-07, 7.9394057467795008E-10, 8.0646218338521010E-10 - 25, 7, 1.2745235340382241E-07, 2.5722461498610012E-07, 7.7887508506762008E-10, 7.7358362605685015E-10 - 25, 8, 2.5188727311854308E-07, 1.4987704009305059E-07, 7.5013837707236015E-10, 7.5361878626318006E-10 - 25, 9, 8.8548350659178343E-08, 2.0918083387298929E-07, 7.1687764466770011E-10, 7.2533327616075001E-10 - 25, 10, 2.0459340707431701E-07,-2.5755712172913070E-07, 6.9268878214916015E-10, 6.9200954567793005E-10 - 25, 11,-4.0208525901293581E-07, 4.2193640157022459E-07, 6.5725104115827006E-10, 6.5738309288535008E-10 - 25, 12,-3.4108004488017230E-07,-2.2236094344922479E-07, 6.2550862951063007E-10, 6.2815016437989005E-10 - 25, 13,-3.3913589664341351E-09, 2.7928595832259351E-07, 5.8930761667783002E-10, 5.8977086458119003E-10 - 25, 14,-6.0081500302783250E-08, 2.6920677556667728E-07, 5.5677786690828015E-10, 5.5647262099777000E-10 - 25, 15, 4.3084430524138863E-07,-1.7103728129146550E-07, 5.1995466644648003E-10, 5.2113781641361001E-10 - 25, 16,-7.2389572160777240E-08, 2.4780679724823610E-07, 4.8676251842699999E-10, 4.8479282742677008E-10 - 25, 17,-4.0812396698157091E-07,-7.9124914881614433E-07, 4.5072257745514998E-10, 4.4979404989055010E-10 - 25, 18, 1.3039868082469749E-07, 3.7143279380405901E-08, 4.1472252015723002E-10, 4.1528899881160002E-10 - 25, 19,-6.7379262122205607E-07,-3.2745841267601822E-09, 3.7867016962360002E-10, 3.8072410364819998E-10 - 25, 20, 5.7252513267242740E-07, 6.8581143753309572E-08, 3.4448327501239002E-10, 3.4496723848186002E-10 - 25, 21,-6.7333893396917305E-07, 1.8939175435100679E-07, 3.0699475037708001E-10, 3.0691003959778999E-10 - 25, 22, 2.9662492388194308E-07,-4.3844233143149069E-07, 2.6937606789095002E-10, 2.6831876012246002E-10 - 25, 23,-4.7106093096747168E-07, 4.7197829147459941E-07, 2.3104022556810000E-10, 2.3092676676266999E-10 - 25, 24, 3.3585277049000689E-07, 7.1057634707613332E-08, 1.6316345114651999E-10, 1.6303192085638999E-10 - 25, 25, 1.0811211693401540E-07,-2.9679761935162980E-07, 1.1259297247996000E-10, 1.1203314497462000E-10 - 26, 0,-1.7628448725189109E-07, 0.0000000000000000E+00, 9.5646179902204006E-10, 0.0000000000000000E+00 - 26, 1,-1.9213755004800671E-07,-3.8315999458061602E-07, 9.5393576191029013E-10, 9.6246263298386002E-10 - 26, 2,-2.0175674235327581E-07, 1.4205464154316201E-08, 9.5104253749955998E-10, 9.4875807231161001E-10 - 26, 3,-1.4909954219721411E-07, 1.2804480855375901E-07, 9.4432404533318011E-10, 9.4632587013837014E-10 - 26, 4,-3.6897114728500608E-07, 2.1670066344322219E-07, 9.2833128156653011E-10, 9.3137878050170002E-10 - 26, 5, 2.0844964305891050E-07, 2.2188350902353279E-07, 9.1576821157264004E-10, 9.1881776625466012E-10 - 26, 6,-3.2567066782868078E-07, 6.5494767739468042E-08, 8.8770564922389013E-10, 9.0247779114555007E-10 - 26, 7, 1.1641740455968710E-07,-2.3553518937132420E-07, 8.7619089318085003E-10, 8.7064543605862008E-10 - 26, 8, 8.6298428878060334E-08, 2.0973525309858609E-08, 8.4330800855063004E-10, 8.4754326992811008E-10 - 26, 9,-7.2025186638855130E-08,-5.0879344685424231E-07, 8.1148208572013006E-10, 8.2152218295000003E-10 - 26, 10, 1.4853026076295599E-07, 3.8012335713704092E-08, 7.8343084717166002E-10, 7.8257018538250001E-10 - 26, 11,-4.7167856753693068E-07,-1.9898237971716059E-07, 7.5033330375978006E-10, 7.5005160937906007E-10 - 26, 12, 2.2527623574358020E-07,-6.9385296018421442E-08, 7.1113488614674008E-10, 7.1440532141405001E-10 - 26, 13, 5.1145042075150622E-08,-1.0433026848759240E-08, 6.7797537840471008E-10, 6.7969053771201003E-10 - 26, 14, 1.2480011835258409E-07,-8.9757900507323339E-08, 6.3854884342428003E-10, 6.3804149624024002E-10 - 26, 15, 9.0209718719096993E-08,-5.4776152943267738E-08, 6.0310513679445009E-10, 6.0399924463423007E-10 - 26, 16,-5.0820149849512781E-07,-8.4248120896246461E-08, 5.6541225832593001E-10, 5.6317442114490004E-10 - 26, 17,-2.4657849080845568E-07,-3.7013988183926381E-08, 5.2829209190483001E-10, 5.2626042462608999E-10 - 26, 18,-2.3290580440167000E-07, 5.3888816219307701E-07, 4.8910563301772005E-10, 4.8906483677148007E-10 - 26, 19,-2.5970273573784249E-08,-1.6653485548744791E-07, 4.4985121425657999E-10, 4.5273714064118001E-10 - 26, 20, 1.5281466506169339E-07, 3.5290024176266420E-07, 4.1259624893995010E-10, 4.1349844288603998E-10 - 26, 21,-3.1201209295350659E-07,-5.5170201263527412E-07, 3.7539326322068999E-10, 3.7520822414308001E-10 - 26, 22, 4.0844874037237719E-07, 4.5750616882189957E-08, 3.3456391414699010E-10, 3.3354784033926998E-10 - 26, 23,-2.4275552862587361E-07, 2.2709480739465239E-07, 2.9386651172722002E-10, 2.9348739633092002E-10 - 26, 24, 7.3201075677302809E-07,-2.8419292894155149E-09, 2.5267197253394001E-10, 2.5303632791328003E-10 - 26, 25, 2.3151452106338080E-07,-4.4013918971703292E-07, 1.8075273296128000E-10, 1.8214235046023001E-10 - 26, 26,-1.1007952809534780E-07, 4.4794368613723358E-07, 1.0243326452524000E-10, 1.0216275506207000E-10 - 27, 0,-2.2429133670940241E-07, 0.0000000000000000E+00, 1.0688135493884001E-09, 0.0000000000000000E+00 - 27, 1, 5.6344813457961528E-08,-4.2301208758475378E-08, 1.0602470832988000E-09, 1.0683181594241999E-09 - 27, 2,-6.5365457782954078E-10, 2.6901356144063480E-07, 1.0637220835864999E-09, 1.0601972347623999E-09 - 27, 3, 1.2627092149005500E-07, 3.1206623710871861E-07, 1.0509895742518999E-09, 1.0510936256283000E-09 - 27, 4, 3.4424611315322562E-07, 8.5273684327321170E-08, 1.0400982743925000E-09, 1.0427789970397000E-09 - 27, 5, 1.0222231504240270E-07, 2.8166455549002169E-07, 1.0208581479378000E-09, 1.0239737794769001E-09 - 27, 6, 2.6324849166294649E-07,-3.2786198801969991E-07, 9.9729508036308003E-10, 1.0134023842680000E-09 - 27, 7,-9.7211844450005550E-10,-1.1172236487931330E-07, 9.8075960393982001E-10, 9.7517090158288013E-10 - 27, 8,-3.4547432643060329E-07, 6.5907866839411809E-08, 9.5051781070319007E-10, 9.5510104009309010E-10 - 27, 9,-5.6074328514253417E-08,-3.8503746456282642E-07, 9.1430290798289006E-10, 9.2557478196193007E-10 - 27, 10,-3.0969727349976250E-07, 3.6473955062594962E-08, 8.8763013121982011E-10, 8.8667550402064009E-10 - 27, 11,-2.0254335758159719E-07,-2.1423176515356401E-07, 8.5014752295159003E-10, 8.5057628394169010E-10 - 27, 12, 6.0235142746719770E-08,-8.8460963500726847E-09, 8.1196405789325003E-10, 8.1510451808690007E-10 - 27, 13,-7.4459935135365112E-08,-1.0590711498148070E-07, 7.7323646383894011E-10, 7.7401466084491007E-10 - 27, 14, 1.7173555188447300E-07,-4.1929659570348682E-08, 7.3548128924106002E-10, 7.3540610428632011E-10 - 27, 15,-4.5952114701047252E-08, 8.8451767953049423E-08, 6.9161061131437002E-10, 6.9314612367975009E-10 - 27, 16,-1.1942685337678140E-07,-2.2375526433632609E-07, 6.5652105946070015E-10, 6.5394948423620002E-10 - 27, 17, 2.4581814647318210E-07, 3.4807209403586919E-07, 6.1311019500296007E-10, 6.1213461204599009E-10 - 27, 18,-2.0923914984464690E-08, 1.5127172664997909E-07, 5.7337101791642008E-10, 5.7291214942788001E-10 - 27, 19, 7.5026181480599506E-07, 1.2894024808243501E-07, 5.3013677185143005E-10, 5.3336161989875008E-10 - 27, 20,-2.5269557479531271E-07,-5.4762440538989093E-08, 4.9066690405182002E-10, 4.9167385355396000E-10 - 27, 21, 2.5817973151245719E-07,-3.1039157162723509E-07, 4.5106100732553999E-10, 4.4960237268220002E-10 - 27, 22,-1.9717193142012019E-07, 3.9791824063853078E-07, 4.1012278999698001E-10, 4.0910415240501010E-10 - 27, 23, 1.2012853555960479E-07,-2.7057833944663443E-07, 3.6599245562414011E-10, 3.6544114400724002E-10 - 27, 24, 1.6193138480660450E-07, 4.6200158477059414E-09, 3.2142993958500999E-10, 3.2101346973354001E-10 - 27, 25,-1.5582978599656841E-07,-5.7300191246279213E-07, 2.7824565374532001E-10, 2.7818286189845999E-10 - 27, 26,-2.5066768488364009E-07, 3.5159123462776440E-07, 1.9575759027658001E-10, 1.9579089507761000E-10 - 27, 27, 4.9198392164878586E-07,-8.9847065707103033E-08, 1.1208664642179001E-10, 1.1119147652428000E-10 - 28, 0, 3.7688556434609083E-08, 0.0000000000000000E+00, 1.1888554271315999E-09, 0.0000000000000000E+00 - 28, 1, 1.5783700928634909E-07, 1.0124148940442520E-07, 1.1820781772946000E-09, 1.1929555320500000E-09 - 28, 2, 3.1017655862825947E-07, 2.4849703134892952E-07, 1.1829012343091999E-09, 1.1805570026167000E-09 - 28, 3, 1.9313510743590381E-07,-2.7948778667318201E-07, 1.1727549826926000E-09, 1.1749988807797000E-09 - 28, 4, 4.0313724493949629E-07, 1.3956437835763860E-08, 1.1592924430640000E-09, 1.1619190536040999E-09 - 28, 5,-4.8400167777080632E-08,-3.6928288502624842E-08, 1.1427210127532001E-09, 1.1469585357633001E-09 - 28, 6, 4.5375184117556701E-07,-2.7356671662511559E-07, 1.1132589984090000E-09, 1.1324303814092000E-09 - 28, 7,-1.6823684799278519E-07, 2.0162811309386700E-07, 1.1025843788036001E-09, 1.0962536060899999E-09 - 28, 8,-2.4710103415387870E-07, 4.2320818316738424E-09, 1.0656528737516000E-09, 1.0712311667570000E-09 - 28, 9, 1.0710413315087720E-07, 4.2063510832961999E-07, 1.0330440157332001E-09, 1.0443155342090001E-09 - 28, 10,-8.2870261985355222E-08, 1.5609810208320571E-08, 1.0009367938067001E-09, 1.0008198591898999E-09 - 28, 11, 2.2512128014213121E-07, 2.7273955283408457E-07, 9.6472099955680000E-10, 9.6411046443387014E-10 - 28, 12, 8.9524747741376861E-08,-2.0457909203691569E-08, 9.2214004215855010E-10, 9.2600197880948003E-10 - 28, 13,-1.2787142340077051E-07, 4.2619780444182797E-08, 8.8202561545839011E-10, 8.8399409953010002E-10 - 28, 14, 8.9051930235575579E-08, 1.2467634316901960E-07, 8.4011535643316008E-10, 8.3978871109370006E-10 - 28, 15,-7.9032732095102051E-08, 1.3383638181939430E-07, 7.9702276014992007E-10, 7.9856153991781011E-10 - 28, 16, 1.7615849583248601E-07, 5.3584124607366892E-08, 7.5343860356357008E-10, 7.5127731871870009E-10 - 28, 17, 1.8637754507548990E-07, 3.6837259317905662E-07, 7.1261298317508007E-10, 7.1142659302964009E-10 - 28, 18, 2.5127844343473148E-07,-3.5746993951465539E-07, 6.6584527636534005E-10, 6.6562334706439004E-10 - 28, 19, 3.1908355132650678E-07, 2.3285527480023169E-08, 6.2211795888892007E-10, 6.2517618690299008E-10 - 28, 20,-2.9898302714105848E-07,-3.9848909033499608E-07, 5.7856572425524006E-10, 5.7943520893803004E-10 - 28, 21, 2.2935452901297270E-07, 1.3419423339194680E-07, 5.3584146956246004E-10, 5.3465666365097009E-10 - 28, 22,-3.6507274474573752E-07, 1.0399280108543280E-07, 4.9128373725735002E-10, 4.9080511616411003E-10 - 28, 23, 3.9683452222884022E-07, 9.8249634583056008E-08, 4.4683410322354002E-10, 4.4723368747057011E-10 - 28, 24,-2.7695608240874400E-07, 2.0151109007883150E-07, 4.0007687899146002E-10, 3.9842232273392002E-10 - 28, 25,-6.1515040223357603E-08,-1.0472751263353350E-07, 3.5234393073152010E-10, 3.5230593217385010E-10 - 28, 26,-3.2090104339849368E-07, 5.5288152018396721E-07, 3.0367314820848010E-10, 3.0375180079987002E-10 - 28, 27, 5.5931795695475683E-08, 3.5884049270198863E-08, 2.0719243549205999E-10, 2.0685862082665000E-10 - 28, 28,-2.0401164017886030E-07,-5.6962748687958568E-07, 1.1919183251375999E-10, 1.1977316838928000E-10 - 29, 0, 9.3702846930364048E-08, 0.0000000000000000E+00, 1.3269895580359001E-09, 0.0000000000000000E+00 - 29, 1, 7.1348817189583361E-08, 2.0184784435221219E-07, 1.3155461535670000E-09, 1.3251171354550999E-09 - 29, 2, 8.0655382026096740E-08, 1.7215589308587210E-09, 1.3212209487921000E-09, 1.3177394521570001E-09 - 29, 3, 9.6836190539040183E-08,-3.0816637609178689E-07, 1.3065372768155000E-09, 1.3077588404191000E-09 - 29, 4,-1.8680363369921000E-07, 4.9832426947715548E-08, 1.2951001636531000E-09, 1.2987855613128999E-09 - 29, 5,-1.2983945214640070E-07,-1.6917412913966779E-07, 1.2753942240684999E-09, 1.2799248359441001E-09 - 29, 6,-9.0422606023952042E-08, 9.4101032299517958E-09, 1.2467736070723001E-09, 1.2681568018185000E-09 - 29, 7,-2.5192143983259911E-07, 1.7464531562866890E-07, 1.2351661514365999E-09, 1.2269344661147001E-09 - 29, 8,-4.2698625836007993E-08,-8.9228824142963896E-09, 1.1979545264541000E-09, 1.2048748008240000E-09 - 29, 9, 1.0183111147610990E-07, 3.2809139697703049E-07, 1.1597588921124999E-09, 1.1738330374847001E-09 - 29, 10, 1.3883747261810520E-07,-3.1141217839033688E-08, 1.1315980039220000E-09, 1.1312136461913000E-09 - 29, 11, 2.9691958578528659E-07, 4.5742929551007871E-08, 1.0897283264328001E-09, 1.0890766484020999E-09 - 29, 12,-1.3427538075769189E-07, 1.0946619933814260E-07, 1.0477105685632999E-09, 1.0506675691901000E-09 - 29, 13, 1.7506798719067500E-07, 1.7575001570676170E-08, 1.0039468704104000E-09, 1.0046035654782999E-09 - 29, 14, 7.7837043194376414E-08, 1.3395399821071559E-07, 9.5959851019895002E-10, 9.5870365130894005E-10 - 29, 15, 5.2109845392902077E-08, 1.3384672333717210E-07, 9.1214098941280008E-10, 9.1376609155229003E-10 - 29, 16, 1.4794412969211669E-07, 1.4792180554770010E-08, 8.6775123351595004E-10, 8.6517475737255015E-10 - 29, 17,-3.3745554673021112E-08,-7.0045295120893890E-08, 8.1879191105107002E-10, 8.1817758566270015E-10 - 29, 18, 2.1418855086524331E-08,-9.4109030401293733E-08, 7.7463151777979015E-10, 7.7425022762524002E-10 - 29, 19,-1.9709952597879409E-07,-1.3505178388051149E-08, 7.2280809848853001E-10, 7.2639905013149008E-10 - 29, 20,-1.3550431404915160E-07,-8.6822470688634973E-08, 6.7899196912890006E-10, 6.8018441254815006E-10 - 29, 21,-1.5030267724421009E-07, 2.6509414861913419E-07, 6.3106634313102007E-10, 6.3061530257303007E-10 - 29, 22, 7.6578317345497392E-08,-2.0680632146692670E-08, 5.8426835986540006E-10, 5.8384046818594003E-10 - 29, 23, 9.6924765746032127E-08, 3.1356707917145092E-07, 5.3670014366337006E-10, 5.3621904299521004E-10 - 29, 24,-1.4005500648212799E-07,-3.0469382582476341E-07, 4.8959770155743999E-10, 4.8737437835855004E-10 - 29, 25, 1.3633949923828659E-07, 2.7162645618812211E-07, 4.3895921296437011E-10, 4.3882983204924002E-10 - 29, 26, 1.6797261963368699E-08, 2.6538690568921410E-07, 3.8508550765224999E-10, 3.8510496209569001E-10 - 29, 27, 4.7933639268462252E-07, 1.7349828302600341E-08, 3.3003016303705999E-10, 3.2953878250756998E-10 - 29, 28,-6.3045061240640739E-08,-2.1918930385312251E-07, 2.2589465685087999E-10, 2.2557285739724001E-10 - 29, 29,-1.3377254772957059E-07, 1.7717530296083961E-07, 1.3447847912214001E-10, 1.3335017312970001E-10 - 30, 0, 1.1145099036532529E-08, 0.0000000000000000E+00, 1.4728978136750000E-09, 0.0000000000000000E+00 - 30, 1,-1.4654000712290799E-07,-1.1060675702898149E-09, 1.4709044466601000E-09, 1.4823720918823001E-09 - 30, 2,-1.8998886339668401E-07,-4.2452925690369157E-08, 1.4671155028397001E-09, 1.4638162238225000E-09 - 30, 3,-3.5270525533382182E-08, 8.3431426027651058E-09, 1.4608721979792001E-09, 1.4632320295891000E-09 - 30, 4,-2.5787459990413612E-07, 2.1727062750581659E-07, 1.4407045088016000E-09, 1.4453987585056000E-09 - 30, 5,-9.1733739134503762E-08, 1.4967249746880319E-07, 1.4283622679380999E-09, 1.4328344220871001E-09 - 30, 6,-1.3497060122031410E-07, 3.6547714193666440E-07, 1.3921557620139000E-09, 1.4156952846847000E-09 - 30, 7,-8.1965937969819072E-08, 3.0984720440641203E-08, 1.3845794726063999E-09, 1.3763428270698000E-09 - 30, 8, 5.5483344343330702E-08, 9.4218169495485086E-08, 1.3441882432999001E-09, 1.3503448635867001E-09 - 30, 9,-7.5045838407190513E-08,-1.4151056355603200E-08, 1.3046072862255000E-09, 1.3207016246568000E-09 - 30, 10, 5.6994847624024508E-08,-2.4088907113550531E-07, 1.2743909196812000E-09, 1.2735202280074000E-09 - 30, 11, 1.2792396133672280E-07,-1.2025161217794699E-07, 1.2331078806424000E-09, 1.2314816930304000E-09 - 30, 12,-2.9831960575819447E-07, 1.2135602798176551E-07, 1.1846102979878999E-09, 1.1890826119050001E-09 - 30, 13, 1.0057243261066830E-07, 5.6318659207429081E-08, 1.1406458849735000E-09, 1.1421407233890000E-09 - 30, 14,-1.3508582102988071E-07, 4.3938012671177198E-08, 1.0915163435781999E-09, 1.0920306711788001E-09 - 30, 15, 1.2018251166455231E-07,-2.5325824294089910E-08, 1.0420760889225000E-09, 1.0439208944607000E-09 - 30, 16, 7.1709898974243120E-08, 5.8193859688772242E-08, 9.9451736800682008E-10, 9.9171744130412006E-10 - 30, 17,-1.4436063445509550E-07,-1.7785010375395631E-07, 9.4275577413583000E-10, 9.4174197623852006E-10 - 30, 18,-7.2068360221388700E-08, 1.1056105864779130E-08, 8.9190955484790005E-10, 8.9122552472219010E-10 - 30, 19,-2.5431419252392298E-07, 6.2142940580670450E-08, 8.4167431328825006E-10, 8.4437605404986006E-10 - 30, 20, 1.3779199405606101E-07, 1.7379509825482029E-07, 7.8866736332075002E-10, 7.8986435597044009E-10 - 30, 21,-2.6415079885543231E-08, 7.8131348871576513E-08, 7.4174231027921008E-10, 7.4020877325964011E-10 - 30, 22, 2.0178394500258259E-07,-2.0456347280951610E-08, 6.8885380042866001E-10, 6.8750924497837004E-10 - 30, 23,-1.5461409754925761E-07, 9.5584411231037844E-08, 6.3768823637270002E-10, 6.3775682826442010E-10 - 30, 24, 9.0729028915191664E-08,-2.3643420072473279E-07, 5.8713053094652003E-10, 5.8486705790154008E-10 - 30, 25,-1.5661849280927401E-07, 2.7613822597114260E-07, 5.3593035466434009E-10, 5.3679032600515004E-10 - 30, 26, 8.1106286134173940E-08,-3.3710802783892099E-07, 4.7829512025673005E-10, 4.7858912337595001E-10 - 30, 27, 2.8782192752951719E-07,-2.4198363506992220E-07, 4.2217257189487011E-10, 4.2041180763929001E-10 - 30, 28,-2.6001883652509370E-07,-3.0907353476336140E-07, 3.5896293857996998E-10, 3.5924057352975002E-10 - 30, 29, 2.6054562193385649E-08, 7.8234963744760744E-08, 2.4506604590468999E-10, 2.4482135722474002E-10 - 30, 30, 8.1528975676899342E-08,-1.7506214508672120E-08, 1.4465517791511999E-10, 1.4484225410005001E-10 - 31, 0, 1.1287170200725720E-07, 0.0000000000000000E+00, 1.6471924768841000E-09, 0.0000000000000000E+00 - 31, 1,-6.1861543402132000E-08,-8.2210099378316832E-08, 1.6333504227131000E-09, 1.6453070327146000E-09 - 31, 2, 8.5025844206685978E-08, 1.3393507846428070E-08, 1.6417410137102999E-09, 1.6369777839567001E-09 - 31, 3,-3.2918743216093827E-08, 5.3573643937370072E-08, 1.6239900542143999E-09, 1.6249407227327000E-09 - 31, 4,-3.0693415303256343E-08, 7.5885563450514509E-09, 1.6137969354955999E-09, 1.6183516861331000E-09 - 31, 5, 9.5050316393459869E-08, 1.7269202062704191E-07, 1.5893905730041000E-09, 1.5948727330663001E-09 - 31, 6, 6.2956324201030600E-08, 1.1345288369187270E-07, 1.5608907033138001E-09, 1.5885858896495001E-09 - 31, 7, 2.3776715964544131E-07,-1.4083684406008409E-07, 1.5461803301184001E-09, 1.5362354795595999E-09 - 31, 8,-1.9589863816685069E-09, 5.8341997617833577E-08, 1.5096117307078000E-09, 1.5185677410121999E-09 - 31, 9,-1.3343953784693900E-07,-3.7881113773361707E-08, 1.4643730487618001E-09, 1.4815367210514999E-09 - 31, 10,-9.2896662432773253E-08,-2.1821071986276781E-07, 1.4350630809088000E-09, 1.4345169617844000E-09 - 31, 11,-1.8542966166663171E-07, 1.9907916992782680E-07, 1.3895376811342999E-09, 1.3901197588579999E-09 - 31, 12,-8.7291074490033562E-08,-4.1802616371664442E-08, 1.3406392705659000E-09, 1.3447036962871000E-09 - 31, 13, 3.8305025124044411E-08, 1.4128537412595329E-07, 1.2919809789707999E-09, 1.2942278761715999E-09 - 31, 14,-2.8077518866457709E-08,-1.1716735262040720E-07, 1.2430213235248000E-09, 1.2412992477176000E-09 - 31, 15, 8.7898315698478590E-08, 1.0936478913889840E-08, 1.1868873743987001E-09, 1.1896042643973999E-09 - 31, 16,-1.3012232974639761E-07, 1.1169673736089499E-09, 1.1374475459075999E-09, 1.1334609656880999E-09 - 31, 17,-4.4201481315247582E-08, 5.1536615322554183E-08, 1.0817462629711000E-09, 1.0803589497200999E-09 - 31, 18, 4.1257417256225502E-08, 1.5114884345532379E-07, 1.0258649297142999E-09, 1.0256545091810001E-09 - 31, 19, 7.4906796232949318E-08, 9.4773927006365372E-08, 9.6952871392541011E-10, 9.7464869858914000E-10 - 31, 20, 4.1356034912852518E-07, 1.3257915418875509E-07, 9.1817444528070012E-10, 9.1881895474822011E-10 - 31, 21,-1.1142387105448830E-08, 2.7003529896595820E-08, 8.6184780344969005E-10, 8.6017222759691009E-10 - 31, 22, 1.2393827079206459E-07,-1.5723930552253350E-07, 8.0935089222459011E-10, 8.0851273933216005E-10 - 31, 23,-1.3879734508950121E-07,-1.6740283644819311E-07, 7.5096269053905008E-10, 7.5138268561116003E-10 - 31, 24, 2.9885747457820662E-08, 1.1444246279427569E-07, 6.9895322200752001E-10, 6.9581473424738010E-10 - 31, 25, 2.6975995832007031E-09,-8.8502450553843264E-09, 6.4248636797120015E-10, 6.4212503659584010E-10 - 31, 26, 1.7125913226196099E-07,-4.8176107290494673E-08, 5.8456147791565000E-10, 5.8464977496990001E-10 - 31, 27,-1.0794395833441440E-07,-1.2083786428134781E-07, 5.2323267220033007E-10, 5.2279715720796006E-10 - 31, 28,-3.9968578077814322E-07,-1.3145215370927559E-07, 4.6160336598058998E-10, 4.6071039856543010E-10 - 31, 29,-5.1366359009729553E-08, 2.1177654546929961E-07, 3.9436388327854001E-10, 3.9412131292079002E-10 - 31, 30, 4.8898788635706623E-08, 2.0043669526703350E-07, 2.6880779865564999E-10, 2.6960192930992002E-10 - 31, 31,-1.8770411750075860E-07, 1.0366944991723379E-07, 1.6362851055910000E-10, 1.6474116873511000E-10 - 32, 0, 1.4199164415066540E-07, 0.0000000000000000E+00, 1.8298436720472000E-09, 0.0000000000000000E+00 - 32, 1,-6.9008301003050204E-08,-1.8450978973317849E-07, 1.8260817903867000E-09, 1.8382254288500000E-09 - 32, 2, 1.3571452247432001E-07, 1.8175786246893730E-07, 1.8233966952835000E-09, 1.8200173262958000E-09 - 32, 3,-1.5237958885822189E-07, 3.8103547315772572E-08, 1.8160345621225000E-09, 1.8174848646909999E-09 - 32, 4, 2.0243538484911449E-07, 6.5322199228227312E-08, 1.7941587633740000E-09, 1.8008880242029000E-09 - 32, 5, 9.2088695039827188E-08,-6.6707883167499961E-08, 1.7791935573582999E-09, 1.7869448944820001E-09 - 32, 6, 4.2216120461878249E-09,-2.0955403475299489E-07, 1.7390225177629001E-09, 1.7698601693920000E-09 - 32, 7, 1.5998118485636949E-07,-1.7096554939386519E-07, 1.7357410653359000E-09, 1.7253415786337000E-09 - 32, 8,-2.3903048512526019E-07,-3.2727968772223782E-08, 1.6869215294400000E-09, 1.6959189281984001E-09 - 32, 9,-7.2496327817676861E-09,-9.8134727164149334E-08, 1.6486842367227001E-09, 1.6679677449938999E-09 - 32, 10,-1.8918870004901241E-08, 1.1086350047943871E-07, 1.6098861804508000E-09, 1.6103471282871000E-09 - 32, 11,-5.6222017412753783E-08, 8.3747710742040034E-08, 1.5689713960391001E-09, 1.5673754359267000E-09 - 32, 12, 6.8589675912041462E-08, 2.2521676724264161E-08, 1.5144005307817001E-09, 1.5181474176307999E-09 - 32, 13, 4.9231679841695092E-08, 1.6695087709746542E-08, 1.4623672256946999E-09, 1.4649511084137000E-09 - 32, 14, 1.0886511036796810E-08,-1.8631661731997840E-07, 1.4091023929133001E-09, 1.4092468004167000E-09 - 32, 15, 5.1757708843862072E-08, 7.4475120792676999E-08, 1.3508289303736999E-09, 1.3541419312219000E-09 - 32, 16,-1.8517999711912979E-07,-4.9124912903018692E-08, 1.2961065415977001E-09, 1.2927338519270001E-09 - 32, 17, 1.0058940344279920E-08,-2.1331055075556621E-08, 1.2372418011536999E-09, 1.2363661840900000E-09 - 32, 18, 7.7932779460410839E-08, 1.9222315559475072E-09, 1.1781557689048999E-09, 1.1775141435250999E-09 - 32, 19, 2.1076515801856930E-07,-1.1451202158152390E-07, 1.1165212450293999E-09, 1.1198523417108001E-09 - 32, 20, 1.6275293059884601E-07, 3.6307007193329441E-08, 1.0597530451814001E-09, 1.0613237913239999E-09 - 32, 21,-8.7015122326974829E-08,-2.2006564172550320E-07, 1.0023970797554000E-09, 1.0003848408843000E-09 - 32, 22,-1.3141983548181709E-07,-6.5706171114170411E-08, 9.4062246686334006E-10, 9.3931166653792005E-10 - 32, 23,-2.8311552278668531E-08, 1.3422778859965351E-07, 8.8318230012003006E-10, 8.8361103445924005E-10 - 32, 24,-1.4042290961879799E-08, 1.5566011198718880E-07, 8.2369476260654008E-10, 8.1847544970919002E-10 - 32, 25, 9.2227713951455806E-08,-1.2956988000446940E-07, 7.6346105341627009E-10, 7.6423633211170011E-10 - 32, 26, 2.5640567067651571E-08,-6.8537066416396170E-08, 7.0065449110951007E-10, 7.0166199959709010E-10 - 32, 27,-6.2037265088381082E-08,-1.0238839881171219E-07, 6.4008417010413004E-10, 6.3969660546482003E-10 - 32, 28,-2.5102619712448360E-08, 2.7652198344327742E-07, 5.7198148303264006E-10, 5.7151854423029008E-10 - 32, 29, 6.4823341316124624E-08, 1.8604827587785851E-07, 5.0707742720734002E-10, 5.0516076262031009E-10 - 32, 30, 1.5376018182582620E-07,-8.0341209408402635E-08, 4.2881745501903002E-10, 4.3247744620836010E-10 - 32, 31, 1.1600838970022609E-07, 4.0321705631569283E-08, 2.9173442168702998E-10, 2.9412068488415999E-10 - 32, 32, 9.8814690692256247E-08, 7.7107073389659512E-08, 1.7521546474470999E-10, 1.7581011302767999E-10 - 33, 0,-9.3041161995794731E-08, 0.0000000000000000E+00, 2.0420957927936998E-09, 0.0000000000000000E+00 - 33, 1,-3.9155619476503921E-08,-9.5472951755738484E-08, 2.0314257436830001E-09, 2.0458872611762000E-09 - 33, 2, 6.0495095617281389E-08, 2.0388444611356801E-07, 2.0360944782539002E-09, 2.0312744451671000E-09 - 33, 3,-1.2635136701065109E-07,-1.1722570873242300E-07, 2.0204967038969000E-09, 2.0236256121439000E-09 - 33, 4, 9.3039604687992252E-08, 3.0724477493342641E-08, 2.0067193482865000E-09, 2.0114907630335001E-09 - 33, 5,-1.5065698613803810E-07, 5.6365977329365531E-08, 1.9835893857804001E-09, 1.9897633689870002E-09 - 33, 6,-1.4655889148091281E-07,-2.5737195557073528E-07, 1.9474165826865002E-09, 1.9822769220588999E-09 - 33, 7,-8.2933999034545262E-08, 2.9017505639063501E-07, 1.9363848432174000E-09, 1.9258638821784998E-09 - 33, 8,-1.1613016656396209E-07,-1.5174951296085281E-07, 1.8938563716785001E-09, 1.9055326847702998E-09 - 33, 9, 8.9989753058879369E-08, 1.0568950686966560E-07, 1.8443758090054000E-09, 1.8654899358336001E-09 - 33, 10, 5.9820199909105688E-09,-1.0939147720901480E-08, 1.8149230854104000E-09, 1.8152859880700000E-09 - 33, 11, 1.6713370934542750E-07,-1.6807661092338261E-07, 1.7613313380300000E-09, 1.7591409406433999E-09 - 33, 12, 1.7300030910776801E-07,-4.3788526045533312E-08, 1.7116987335214000E-09, 1.7157773756716000E-09 - 33, 13,-2.8840622092309700E-09,-1.9834544529064040E-08, 1.6525657760202000E-09, 1.6543362285702000E-09 - 33, 14, 5.8856684519502652E-08,-1.8654676706815480E-08, 1.5970753777042999E-09, 1.5963209452595001E-09 - 33, 15,-1.2161177303677400E-07, 1.0667577514684720E-07, 1.5345870289086001E-09, 1.5374883019192000E-09 - 33, 16, 2.4639424579856639E-08,-5.9001777810272968E-08, 1.4753247646557000E-09, 1.4724772567582000E-09 - 33, 17, 4.3606539106569017E-08, 2.4765101992831768E-08, 1.4117760489003000E-09, 1.4110893972557999E-09 - 33, 18,-1.2190341059056400E-07, 4.9682588850084671E-08, 1.3488741602557000E-09, 1.3477505629597000E-09 - 33, 19, 2.0067276583451190E-07,-2.1213398184407039E-07, 1.2824597618667999E-09, 1.2861093116400000E-09 - 33, 20,-1.6804639835150579E-07,-3.7630183697907902E-08, 1.2188859681510000E-09, 1.2209298729294999E-09 - 33, 21,-6.7127255683312121E-08,-2.3457283311142829E-07, 1.1590997498802000E-09, 1.1573541133060000E-09 - 33, 22,-5.3737111456736953E-08, 1.1984305878437651E-07, 1.0938325849425000E-09, 1.0923215606811000E-09 - 33, 23,-5.3045917259878153E-09, 1.1866956881209659E-07, 1.0271920741488000E-09, 1.0274781199747000E-09 - 33, 24, 1.5343563704200659E-07, 1.3675531038335189E-07, 9.6849442138469008E-10, 9.6289475703231014E-10 - 33, 25, 4.3550430286284293E-08,-5.8632154795252263E-08, 8.9832960676870006E-10, 8.9844149752293012E-10 - 33, 26,-3.7299578526325522E-08,-1.2038348936563821E-07, 8.3536388643211003E-10, 8.3404207071878011E-10 - 33, 27,-4.8200608425042718E-08, 4.6056492187872341E-08, 7.6750064636291010E-10, 7.6722574220750010E-10 - 33, 28,-5.1790687097832173E-08, 1.9390406026864819E-07, 7.0077122262765006E-10, 6.9927789310119007E-10 - 33, 29, 1.6008323713881899E-07, 1.1772951094374950E-08, 6.2740125651839003E-10, 6.2640646526186003E-10 - 33, 30, 1.0982620523421721E-07,-1.6664622274891639E-07, 5.5346738794537001E-10, 5.5603172828650006E-10 - 33, 31,-1.6505664906862919E-07,-3.0499920741683240E-09, 4.7120305392721007E-10, 4.7462531134952008E-10 - 33, 32,-1.1726664325540211E-08,-4.0000930060880048E-07, 3.2169717829822999E-10, 3.2291334690067001E-10 - 33, 33, 4.5174402586025787E-08,-1.0160378841311640E-07, 1.9751890422947000E-10, 2.0004525952135000E-10 - 34, 0,-1.0632887732392461E-07, 0.0000000000000000E+00, 2.2774455538373000E-09, 0.0000000000000000E+00 - 34, 1, 7.1415401897142285E-08,-4.9939191989393053E-08, 2.2623094640862998E-09, 2.2769233599152000E-09 - 34, 2,-1.0503626578362790E-07, 4.8735766498620773E-08, 2.2715426121178000E-09, 2.2653834431027002E-09 - 34, 3, 8.7426691656420210E-08,-1.6024833581982230E-07, 2.2521775759588000E-09, 2.2539927384797001E-09 - 34, 4,-2.6225259217101110E-08, 1.0118550795607580E-07, 2.2384354292012000E-09, 2.2451768677573002E-09 - 34, 5,-1.4176810909249629E-07,-5.5390063209572447E-08, 2.2133852651424999E-09, 2.2221181830959000E-09 - 34, 6,-1.0099938127383279E-08, 2.8211816113405839E-08, 2.1738036832624999E-09, 2.2139543291405000E-09 - 34, 7,-1.7057769281099299E-07, 2.3369066516504980E-07, 2.1684910445339001E-09, 2.1546410670495000E-09 - 34, 8, 2.2798882443566892E-08, 3.7760690705900123E-08, 2.1182463142377002E-09, 2.1301537206400998E-09 - 34, 9, 4.0136265384011702E-08, 1.2008488518940759E-07, 2.0715916670615001E-09, 2.0953958354617998E-09 - 34, 10,-2.6202192746739029E-08,-1.0944880314150940E-07, 2.0341551100690000E-09, 2.0335611672965002E-09 - 34, 11, 1.1353548626252819E-07,-2.1864018671670321E-07, 1.9858780431187998E-09, 1.9845525213063000E-09 - 34, 12,-7.3683620428852475E-08,-4.6168471565229713E-08, 1.9239764124736999E-09, 1.9274717398329001E-09 - 34, 13,-4.7579850021928781E-08,-1.2081902098228350E-07, 1.8706417695154999E-09, 1.8732171088457001E-09 - 34, 14, 4.1492210791192372E-08, 7.4710644016103333E-08, 1.8037532549055999E-09, 1.8031895442318999E-09 - 34, 15,-1.0997482077328889E-07, 7.6542996763054433E-08, 1.7405561858017999E-09, 1.7451716521645000E-09 - 34, 16, 2.3015340938087661E-07,-2.0105444760175092E-08, 1.6778245628197999E-09, 1.6727586653511000E-09 - 34, 17,-7.8588215654433869E-08, 7.3834738497796672E-08, 1.6078835197818001E-09, 1.6068409021771000E-09 - 34, 18,-3.4445364411745752E-08,-9.4177461860616565E-08, 1.5406561966247999E-09, 1.5390351562932000E-09 - 34, 19, 2.6527544728216930E-08, 1.9789558549789850E-07, 1.4685332769099000E-09, 1.4730881900627000E-09 - 34, 20,-1.7884929132645501E-07,-7.6271520420408341E-08, 1.4004784262042000E-09, 1.4026764956785999E-09 - 34, 21, 4.3553071302506183E-08, 1.3735028836628479E-07, 1.3339258973346000E-09, 1.3305843221517999E-09 - 34, 22,-5.6480634178509722E-08, 1.2819809899443320E-07, 1.2658428942326000E-09, 1.2643718030349000E-09 - 34, 23, 1.2445272972257441E-07,-5.9915928409727871E-08, 1.1931692272158999E-09, 1.1939985102325000E-09 - 34, 24, 1.1122139510232851E-07,-2.8476237200761750E-08, 1.1273280639248001E-09, 1.1203735892750000E-09 - 34, 25,-1.2252918749598119E-07,-8.9601300097711192E-08, 1.0574802636429000E-09, 1.0576717015847999E-09 - 34, 26, 6.1971254618533661E-08,-1.1079395498102480E-07, 9.8196411577357002E-10, 9.8279322299333001E-10 - 34, 27,-9.7315257931023134E-08, 1.2697196806340269E-07, 9.1403619957925004E-10, 9.1259819858949004E-10 - 34, 28, 7.8424659353231952E-08,-2.4000607220929259E-08, 8.4096249691276005E-10, 8.3931922540460011E-10 - 34, 29, 1.1085126465880940E-07, 5.5038737301195307E-08, 7.6813026120130007E-10, 7.6713406072977007E-10 - 34, 30, 8.2084920456087813E-09,-2.3302196924809060E-07, 6.8506332146900009E-10, 6.8883561978433003E-10 - 34, 31,-2.6560926919225028E-07, 1.1935975196949980E-07, 6.0658517134915015E-10, 6.1197667761620008E-10 - 34, 32, 1.3459906830385611E-07, 9.7368420389679973E-08, 5.1827891163423002E-10, 5.1990373600401004E-10 - 34, 33,-3.7864802372635139E-07, 5.5371199259954147E-08, 3.4934093333419999E-10, 3.5232444640827999E-10 - 34, 34, 1.2899929461847120E-08, 5.6821952047653063E-08, 2.1344810624042000E-10, 2.1300130706911001E-10 - 35, 0, 3.4024288438553658E-08, 0.0000000000000000E+00, 2.5324457788890002E-09, 0.0000000000000000E+00 - 35, 1, 7.1144467291398490E-08, 1.5644291900712211E-07, 2.5268609134136001E-09, 2.5422194194264001E-09 - 35, 2,-4.0982271089152203E-08,-5.8861114697072292E-08, 2.5255934604169998E-09, 2.5209660431461000E-09 - 35, 3, 7.1213409593184471E-08,-5.8377933040032508E-08, 2.5155768702520999E-09, 2.5186733474825000E-09 - 35, 4,-1.7929286223617721E-07, 8.1282437672105923E-08, 2.4914216405970002E-09, 2.4999222794892999E-09 - 35, 5, 7.5843741150327531E-08,-2.1312335468172129E-07, 2.4743462846533999E-09, 2.4848714716618001E-09 - 35, 6, 1.6604305607685219E-07, 1.8345312607409969E-07, 2.4247212648326999E-09, 2.4687382815254998E-09 - 35, 7, 3.4327777754990443E-08,-1.0628024552178880E-07, 2.4262558972308001E-09, 2.4123480514337001E-09 - 35, 8, 1.8581591771855159E-07, 8.6471564877470363E-08, 2.3693792621926998E-09, 2.3829510198630002E-09 - 35, 9,-3.9190194801627102E-08,-4.4365185019594807E-08, 2.3201660247167000E-09, 2.3492859516327001E-09 - 35, 10,-5.7246798699595002E-08,-6.8053676100103979E-08, 2.2828472403134001E-09, 2.2844584093076998E-09 - 35, 11,-5.5866176479524723E-08, 3.4259711168244792E-08, 2.2303004116213001E-09, 2.2275031342149998E-09 - 35, 12,-1.5404403505834219E-07, 4.9679959830521061E-08, 2.1680489983270002E-09, 2.1747918519153002E-09 - 35, 13,-6.1235114141357142E-08, 1.5237402884358111E-08, 2.1057361019158998E-09, 2.1064963779645001E-09 - 35, 14,-5.7581877683415301E-08, 1.0354562488116990E-07, 2.0445785040103998E-09, 2.0432624494563002E-09 - 35, 15, 6.8613484166470630E-08,-1.7424767158547829E-07, 1.9670087751257999E-09, 1.9704574436114999E-09 - 35, 16, 1.1821256289905221E-07, 6.9364285159169428E-08, 1.9052301376389001E-09, 1.9017570041860998E-09 - 35, 17,-1.4970719875700659E-07,-7.9225958874616963E-08, 1.8274619407674000E-09, 1.8271857259028001E-09 - 35, 18, 1.1065358799911551E-07,-6.7618308230379339E-08, 1.7553062979314999E-09, 1.7538614559809001E-09 - 35, 19,-1.0512687252784650E-07, 8.1662940248482451E-08, 1.6784920004596000E-09, 1.6840780255742001E-09 - 35, 20, 1.3408159324450509E-07,-1.0208799230801030E-07, 1.6045991231690001E-09, 1.6063458251434001E-09 - 35, 21, 6.8286897314518651E-08, 2.2539568227638071E-07, 1.5314837516506000E-09, 1.5304905928782999E-09 - 35, 22, 6.2016696429319921E-08,-1.0509548000082010E-07, 1.4569450666850999E-09, 1.4551035708741000E-09 - 35, 23, 3.6318691474751922E-08, 5.2156564624231831E-08, 1.3835072832607001E-09, 1.3831107640437000E-09 - 35, 24,-1.4111667891907559E-07,-4.8081363517502137E-10, 1.3085678129336001E-09, 1.3016227843542000E-09 - 35, 25,-5.8129176179972783E-08,-4.2444888159175283E-08, 1.2307270539300001E-09, 1.2304479773704000E-09 - 35, 26, 6.5991573606637773E-09, 3.8245564679828872E-08, 1.1565625973312000E-09, 1.1550229004460000E-09 - 35, 27,-9.5783609026223032E-08, 1.5875449157598520E-09, 1.0754454138995001E-09, 1.0753711751556999E-09 - 35, 28, 1.5153677193595010E-07,-9.4181481113138865E-09, 1.0007695792587000E-09, 1.0000023684061000E-09 - 35, 29,-1.3530417563280879E-07,-5.3988464378971132E-08, 9.2166079123506007E-10, 9.1979251146602006E-10 - 35, 30, 8.8716100802109765E-08,-1.2639213420519021E-07, 8.4030148130901007E-10, 8.4230512978404008E-10 - 35, 31,-5.8321007488984718E-08, 2.8790343528139821E-08, 7.5131557908504002E-10, 7.5621606993206011E-10 - 35, 32, 2.5531808292070540E-07, 2.2268803718401721E-07, 6.6673909172445009E-10, 6.7118012944764003E-10 - 35, 33,-1.1265421756798219E-07,-2.0937342975520031E-07, 5.6720772653031003E-10, 5.7013095607282005E-10 - 35, 34, 1.8927628048535371E-07, 3.3038614834541960E-07, 3.8444751425462010E-10, 3.8252455070643002E-10 - 35, 35, 1.0998534272364330E-07, 2.0758703611699020E-09, 2.3408565153264002E-10, 2.3554523442473000E-10 - 36, 0, 1.7352345802700461E-07, 0.0000000000000000E+00, 2.8212502342747002E-09, 0.0000000000000000E+00 - 36, 1,-9.2007856008371074E-08,-3.0952448936565900E-08, 2.8165760555997999E-09, 2.8341361669071998E-09 - 36, 2, 1.0530738484892290E-07,-1.2561437941773701E-07, 2.8156693630046001E-09, 2.8084066376673000E-09 - 36, 3,-1.1771852403467630E-07, 5.2985466285570571E-08, 2.8040600931308998E-09, 2.8077959235237999E-09 - 36, 4,-1.5313565372215140E-07,-1.9696456884033421E-08, 2.7794738298442998E-09, 2.7902523778882999E-09 - 36, 5, 8.1541689411796423E-08, 8.3350902398881584E-09, 2.7597827763457999E-09, 2.7696544366136001E-09 - 36, 6,-1.1711756774511030E-07, 8.9199564709527033E-09, 2.7106762252863001E-09, 2.7606503382563999E-09 - 36, 7, 1.0130108131381110E-07,-1.2028864718113240E-07, 2.7086044675120001E-09, 2.6913942238664001E-09 - 36, 8, 6.5650983855326063E-08,-4.8429664321144141E-08, 2.6547682023264998E-09, 2.6700785829480000E-09 - 36, 9,-4.6052228595997172E-08,-6.4211098547860040E-08, 2.5977036237053000E-09, 2.6256733035864999E-09 - 36, 10, 7.7793593779078320E-08, 3.1354655672475727E-08, 2.5646202665267000E-09, 2.5631662766898998E-09 - 36, 11,-7.5941886551186470E-08, 4.0196531745606541E-08, 2.5037258880649000E-09, 2.5007888665080001E-09 - 36, 12,-3.2144990107619147E-08, 8.1338662401160021E-09, 2.4413301866691998E-09, 2.4445154670620001E-09 - 36, 13, 1.6000633386291669E-07, 1.7591776430693591E-07, 2.3719714813867001E-09, 2.3770332546191000E-09 - 36, 14,-1.0071815566570820E-07,-4.4889184763151842E-08, 2.3039043299996000E-09, 2.3035417799446002E-09 - 36, 15, 2.4731013684687929E-07,-9.7021649942600052E-08, 2.2291022661302999E-09, 2.2346773320167001E-09 - 36, 16,-1.4505743301733001E-07,-6.3431374593812647E-09, 2.1529048355355999E-09, 2.1490099912252000E-09 - 36, 17,-1.4215542344097820E-08,-1.1551738332249510E-07, 2.0799579513551002E-09, 2.0784931712010999E-09 - 36, 18,-2.1878663531801221E-08, 1.5112603824599780E-07, 1.9955687212862001E-09, 1.9937278599323001E-09 - 36, 19,-1.0375681711073870E-07,-7.0982295220271862E-08, 1.9149031539097000E-09, 1.9189511390907002E-09 - 36, 20, 1.9031547678995510E-07, 7.8676120875159915E-08, 1.8355185718101000E-09, 1.8366357181594000E-09 - 36, 21, 7.8982338009706250E-08,-4.1706629246173578E-08, 1.7556468326195000E-09, 1.7518924448673000E-09 - 36, 22, 4.3102414379775842E-08,-1.0495857453142480E-07, 1.6743456866282000E-09, 1.6724694508701999E-09 - 36, 23, 1.0009537462456780E-08, 9.0614876441402204E-08, 1.5920635995864001E-09, 1.5925734617636001E-09 - 36, 24,-1.3926800128973271E-07,-5.7741876862596088E-08, 1.5172168181862001E-09, 1.5088235512165999E-09 - 36, 25, 3.8683102481227643E-08, 1.1124132178198440E-07, 1.4281235040165001E-09, 1.4290182355881000E-09 - 36, 26, 1.1689001881252919E-08, 7.5888958007031610E-08, 1.3477070755178000E-09, 1.3460045869286001E-09 - 36, 27, 5.8596021799722803E-08,-7.0293939734974430E-08, 1.2654315623703001E-09, 1.2640738704014000E-09 - 36, 28, 1.5076159933213461E-08, 1.9367461081613100E-07, 1.1787580763497000E-09, 1.1770232024983000E-09 - 36, 29,-1.6405335298419641E-07,-1.6850176167800510E-07, 1.0971741557322999E-09, 1.0955640431368001E-09 - 36, 30,-7.4254465455895331E-08, 2.0300350645333650E-07, 1.0085384598780999E-09, 1.0105255204069000E-09 - 36, 31,-1.3240823573794790E-07,-1.5088055722480529E-07, 9.2245194722944007E-10, 9.2485717966927003E-10 - 36, 32, 2.0674945258381269E-07, 1.5594694820138731E-08, 8.2699261936268003E-10, 8.2857322468686009E-10 - 36, 33, 5.2539661504874101E-08,-3.0051816790934441E-07, 7.3102868525657007E-10, 7.3820876170409002E-10 - 36, 34,-6.9262542472925794E-08, 1.5200865678709570E-07, 6.2706320086444002E-10, 6.2632484924636004E-10 - 36, 35, 1.6192670968369651E-07,-2.1443667465336801E-07, 4.1859681991775998E-10, 4.2012936428306001E-10 - 36, 36,-1.0044992897759140E-07,-1.8993104225368830E-07, 2.5956753106882997E-10, 2.6072805184818002E-10 - 37, 0,-6.3741046289057150E-08, 0.0000000000000000E+00, 3.1518095948558001E-09, 0.0000000000000000E+00 - 37, 1,-1.0611777630949591E-07,-1.8632051812706290E-07, 3.1313665100694001E-09, 3.1493456619447998E-09 - 37, 2, 5.6009815733753203E-08,-2.2446207229091391E-08, 3.1456716068961002E-09, 3.1392285172910999E-09 - 37, 3,-1.1842092807006070E-07, 6.2856719350806839E-08, 3.1196750839364001E-09, 3.1211905555981998E-09 - 37, 4, 1.0005914273016900E-07, 2.0014500256915651E-09, 3.1079892460496000E-09, 3.1177072583829998E-09 - 37, 5, 7.2264811073958769E-08, 1.2268383979687680E-07, 3.0732015841466999E-09, 3.0854297562281002E-09 - 37, 6,-1.8449611937997360E-07,-1.1901884817943060E-07, 3.0304744601702001E-09, 3.0869327640330999E-09 - 37, 7, 1.7391050683384140E-08,-2.1457643848063579E-08, 3.0231504961451998E-09, 3.0059997125841998E-09 - 37, 8,-2.5558233195870249E-07,-8.8206213756179182E-08, 2.9698723627135999E-09, 2.9865953479774001E-09 - 37, 9,-1.0620157770467470E-07,-4.8196020181185413E-08, 2.9084951863985999E-09, 2.9425286240098000E-09 - 37, 10,-2.7260453085020112E-09, 5.3806932218296873E-08, 2.8705471149994998E-09, 2.8722537546449999E-09 - 37, 11, 7.4034346997526069E-08,-5.1020913506620543E-08, 2.8128484716105999E-09, 2.8105727604382002E-09 - 37, 12, 7.5141166617708072E-08,-8.7603429403916834E-08, 2.7401206776844001E-09, 2.7458468160325000E-09 - 37, 13, 7.9209639967711802E-08, 3.9366188437952202E-08, 2.6722678019848001E-09, 2.6776668063549000E-09 - 37, 14,-8.1065694344038970E-09,-2.1589652872834841E-07, 2.5966373810077001E-09, 2.5976079093861998E-09 - 37, 15, 4.6533599379558117E-08, 3.9027411416846491E-08, 2.5174525917276999E-09, 2.5221189694953000E-09 - 37, 16,-1.3461496804365910E-07, 7.2253579465040884E-09, 2.4408709701621002E-09, 2.4353147508286999E-09 - 37, 17, 6.9995333268277029E-08, 1.1342404950751620E-07, 2.3510721607072001E-09, 2.3521985170203000E-09 - 37, 18,-1.0608247963127480E-07, 1.5337671225312510E-07, 2.2730067741842998E-09, 2.2700143196357001E-09 - 37, 19, 1.1576736332664570E-07,-7.7702062297068632E-08, 2.1761985016015002E-09, 2.1808174654418998E-09 - 37, 20, 7.5075036055401461E-08, 8.7057509818666982E-08, 2.0951774108844000E-09, 2.0960194485450999E-09 - 37, 21,-1.4994230264514201E-08,-1.1213963263696450E-07, 2.0078114800580001E-09, 2.0057597175989001E-09 - 37, 22, 4.4451651515338033E-08, 4.7873051414468217E-08, 1.9187409988851998E-09, 1.9154298505054001E-09 - 37, 23,-1.2391483089678131E-07, 3.2318312861779412E-08, 1.8305766991071000E-09, 1.8307982256192001E-09 - 37, 24, 4.1417378852924142E-08,-1.9111012269063280E-08, 1.7468341758596000E-09, 1.7377625961983000E-09 - 37, 25, 7.7326621210856984E-08, 5.2528174298109912E-08, 1.6568779891453000E-09, 1.6579351381770999E-09 - 37, 26,-5.2153906481778981E-08,-8.1641095176670622E-08, 1.5632739812481999E-09, 1.5613619366946000E-09 - 37, 27, 1.3504069301523100E-07,-7.9539227844172039E-08, 1.4757992560850001E-09, 1.4748827876534999E-09 - 37, 28,-1.7512993499676440E-07, 1.2705491672337340E-08, 1.3872685679039001E-09, 1.3830462701841000E-09 - 37, 29, 7.1470482599722573E-08,-1.0656217876793470E-07, 1.2921460400488000E-09, 1.2906265564860000E-09 - 37, 30,-5.2602301530057247E-08, 3.1382247811784079E-07, 1.2015701581562999E-09, 1.2028856410909999E-09 - 37, 31, 1.3582137005274120E-07,-1.3247281712944691E-07, 1.1065078362981000E-09, 1.1085860009932001E-09 - 37, 32,-4.4326791130363873E-09, 9.9824161505700014E-08, 1.0115744145062000E-09, 1.0155971288600001E-09 - 37, 33,-2.5639009814894070E-07,-1.6915326937568349E-07, 9.0477544788566002E-10, 9.1192406200485004E-10 - 37, 34,-1.2681810372529520E-07, 4.6625600984098993E-08, 8.0985947613249005E-10, 8.0621423404090010E-10 - 37, 35, 2.2560076674541489E-07, 7.1822320072323871E-08, 6.8792871261185004E-10, 6.8503447028885004E-10 - 37, 36,-3.1953116760542538E-07,-1.4063251761484349E-07, 4.4585230865913999E-10, 4.4635432166606002E-10 - 37, 37,-7.1841285233563025E-08, 9.5724111685223512E-08, 2.8739188630525001E-10, 2.8579275289196999E-10 - 38, 0,-1.5405998574156460E-07, 0.0000000000000000E+00, 3.5101065566660001E-09, 0.0000000000000000E+00 - 38, 1,-1.8725150453835798E-08,-9.1583448966180298E-09, 3.4929417912518999E-09, 3.5121980732936002E-09 - 38, 2,-1.6539765166918122E-08, 1.1561106188445949E-07, 3.5022925680956999E-09, 3.4953427309793000E-09 - 38, 3,-9.1462668167888710E-08,-1.0809767167287560E-08, 3.4808627780116000E-09, 3.4852439813047002E-09 - 38, 4, 1.7191699432976051E-07, 5.9753324011347201E-08, 3.4603743534709998E-09, 3.4715540854127999E-09 - 38, 5,-5.3649166964855931E-08,-3.4281153285803283E-08, 3.4347947240453998E-09, 3.4491925401947000E-09 - 38, 6, 9.9768326570849770E-08,-1.2193059030373480E-07, 3.3744710356326000E-09, 3.4399842470546001E-09 - 38, 7,-5.1102547958078612E-08, 1.5558891893238221E-07, 3.3847062437060001E-09, 3.3654375004525000E-09 - 38, 8,-7.5344401017860512E-08, 7.1912481545413382E-08, 3.3128572905057000E-09, 3.3334488422402000E-09 - 38, 9,-2.8346125209541060E-08, 1.4022018233298871E-07, 3.2610938199923002E-09, 3.2981285973107002E-09 - 38, 10,-5.0959358077642622E-08, 4.1315098657821473E-09, 3.2125213642478002E-09, 3.2150175489171002E-09 - 38, 11, 4.3019144239984183E-08,-9.7559862884989775E-08, 3.1569194688073001E-09, 3.1535092112091000E-09 - 38, 12,-1.7129305870774869E-08,-8.6008156272069162E-08, 3.0792989576717999E-09, 3.0861404139879000E-09 - 38, 13,-5.4530742921943897E-08,-8.6074144792288493E-08, 3.0068513110446998E-09, 3.0072913095337999E-09 - 38, 14, 3.3067505299285671E-08,-2.9537328778986109E-08, 2.9294791709741999E-09, 2.9270130300493999E-09 - 38, 15,-7.4733537259586694E-08,-7.2978104750845752E-08, 2.8391997913145000E-09, 2.8443019352731998E-09 - 38, 16,-5.7136955760144062E-08, 2.2557039578988329E-08, 2.7587533069528001E-09, 2.7541353796084001E-09 - 38, 17, 6.7089470986150363E-08, 7.4411490243961630E-08, 2.6650552753709000E-09, 2.6642121700641001E-09 - 38, 18,-3.4841800828916690E-08,-9.0175956331260211E-08, 2.5723509532211000E-09, 2.5722458776120999E-09 - 38, 19, 1.2084635693765930E-07,-6.5001652205626020E-08, 2.4814144642592002E-09, 2.4831388951254001E-09 - 38, 20, 1.4193912751912891E-08,-3.5504919380974737E-08, 2.3809926330058999E-09, 2.3815803991133001E-09 - 38, 21,-1.9782468811196401E-08,-7.4871070662501524E-08, 2.2934732582701999E-09, 2.2909626971486999E-09 - 38, 22,-9.7071896873553566E-08, 6.8387130079837683E-08, 2.1947603016042999E-09, 2.1920364436354001E-09 - 38, 23,-4.6197364882310861E-08,-1.0098875200363020E-07, 2.0968954790551002E-09, 2.0978787079216000E-09 - 38, 24, 5.9689555035904404E-08, 4.0277524559842673E-08, 2.0093758272000000E-09, 1.9979850216606000E-09 - 38, 25,-3.0551847724494910E-08,-3.8968966243183198E-08, 1.9084477966142001E-09, 1.9088303289307999E-09 - 38, 26, 3.0444961811857902E-08,-1.1656011692344250E-07, 1.8142207375464001E-09, 1.8111685589541001E-09 - 38, 27,-5.6550019523513930E-09, 1.0425749062421010E-07, 1.7105549142549999E-09, 1.7112560393004001E-09 - 38, 28,-1.2783776427157491E-07,-1.2301460525967901E-07, 1.6171299149957000E-09, 1.6172402426568000E-09 - 38, 29, 1.5421865195310449E-07, 2.0412514096121941E-07, 1.5186360970305000E-09, 1.5164788456649000E-09 - 38, 30,-1.3929808499773699E-07,-9.0603406745824274E-08, 1.4153572832958001E-09, 1.4167699683205999E-09 - 38, 31, 3.0879633377199652E-07, 1.6708915299764249E-08, 1.3175180903696000E-09, 1.3208616389976000E-09 - 38, 32,-1.2075088880431910E-07, 2.7990428119870291E-08, 1.2143425758723000E-09, 1.2177273475295001E-09 - 38, 33, 8.2116489399665752E-08, 5.2458046954483246E-09, 1.1112410252017000E-09, 1.1161244972367000E-09 - 38, 34,-4.1154829117260563E-08, 1.8964669066459271E-07, 1.0020742058895000E-09, 9.9616530576910008E-10 - 38, 35, 1.3762911451233519E-07, 4.7481109769087227E-08, 8.8873157584254009E-10, 8.8827714042599009E-10 - 38, 36,-1.3108468771968021E-07,-1.6947905597121030E-07, 7.5614331553341002E-10, 7.5550873771000009E-10 - 38, 37, 7.2116642160146979E-08, 3.1912563596010080E-07, 5.1412734291246003E-10, 5.1375671856824999E-10 - 38, 38, 1.3659382787784400E-07, 6.6003276570958289E-09, 3.4552796305070999E-10, 3.4265571384370999E-10 - 39, 0, 5.3300296518817847E-08, 0.0000000000000000E+00, 3.9035173386167000E-09, 0.0000000000000000E+00 - 39, 1, 1.3292094229078051E-07, 1.1341140325640750E-07, 3.8996077885533002E-09, 3.9205733281837000E-09 - 39, 2, 1.5266323043110379E-09, 8.9367826520247134E-08, 3.8965916997159997E-09, 3.8885742674248004E-09 - 39, 3, 1.1544697443314651E-08,-2.2416832893452210E-08, 3.8874839944472002E-09, 3.8913183880570998E-09 - 39, 4,-1.3943081704577660E-08, 3.7147422269354681E-08, 3.8520163299310000E-09, 3.8663601468312997E-09 - 39, 5,-1.1318742467645000E-07,-1.8968885023875070E-07, 3.8362107326404003E-09, 3.8509071265778998E-09 - 39, 6, 7.5952483998406942E-08,-5.3033608789881273E-09, 3.7658559212065998E-09, 3.8359738669917997E-09 - 39, 7,-7.9199947776868484E-08,-1.4270267969447910E-09, 3.7805803600366997E-09, 3.7579211356360998E-09 - 39, 8, 1.3404990452028629E-07, 2.3597949828772129E-08, 3.7049471690602001E-09, 3.7293811556327002E-09 - 39, 9, 1.0291991059300560E-07, 1.6248764726980340E-08, 3.6439985953520000E-09, 3.6839368214963001E-09 - 39, 10, 8.7014607494870442E-08, 8.8965744828152320E-09, 3.6055242256543002E-09, 3.6049556891345999E-09 - 39, 11, 5.1496874808932923E-08,-2.8020956341249119E-08, 3.5349497695779998E-09, 3.5285404546092001E-09 - 39, 12,-8.1395430631224550E-08,-2.3161127924717901E-08, 3.4623081831655999E-09, 3.4663919881001002E-09 - 39, 13,-2.5737302795979229E-08,-9.0315047807128653E-08, 3.3750667660068000E-09, 3.3808609266695000E-09 - 39, 14,-2.3273029493648630E-08, 5.7524655688520962E-08, 3.2968065431810002E-09, 3.2958792831586000E-09 - 39, 15,-4.4139258526118877E-08, 2.5426920730707720E-08, 3.2003841874628000E-09, 3.2087716818489000E-09 - 39, 16, 6.5978882881334969E-08,-2.1611023131836550E-08, 3.1135077515176999E-09, 3.1082959803441001E-09 - 39, 17, 4.3832121189280372E-08, 1.9101381207184771E-08, 3.0154721637971001E-09, 3.0148821970058999E-09 - 39, 18, 5.0596479080772032E-08,-1.8299313978655771E-07, 2.9155822949350001E-09, 2.9127453986996999E-09 - 39, 19,-7.6265510349001780E-08, 7.1684168850240922E-08, 2.8106916990741000E-09, 2.8184756296819000E-09 - 39, 20,-9.7369681679613324E-09,-1.0481196667219839E-07, 2.7130522307760001E-09, 2.7135824096223001E-09 - 39, 21,-2.6240002446651179E-08, 6.9020629660380632E-08, 2.6065503291009000E-09, 2.6031762556958000E-09 - 39, 22,-6.6994970937120112E-08,-1.1029185616235130E-08, 2.5098415742327000E-09, 2.5068897563658002E-09 - 39, 23, 8.9542132971258069E-08,-1.1968974262989280E-09, 2.4006212631559998E-09, 2.3986153763043000E-09 - 39, 24,-1.7041397913174399E-08,-1.1366060449154550E-08, 2.3016541039886999E-09, 2.2903638687493000E-09 - 39, 25,-5.1927520782573713E-08,-8.6672589942709914E-08, 2.1949088503061001E-09, 2.1966350850529998E-09 - 39, 26,-1.7081179165595821E-08,-3.7483879524688449E-09, 2.0910013985964999E-09, 2.0868739776894002E-09 - 39, 27,-8.1205253651218911E-08, 1.2890360457968621E-07, 1.9846683933008001E-09, 1.9847330640971000E-09 - 39, 28, 1.9062945992975649E-07,-5.5199168562401373E-08, 1.8757307177143999E-09, 1.8731148148633001E-09 - 39, 29,-4.5299166976803431E-08, 2.2098597271240089E-07, 1.7746898737068001E-09, 1.7723317346903001E-09 - 39, 30, 9.1090715649129804E-08,-2.5138211429178671E-07, 1.6630955783419001E-09, 1.6641865173641001E-09 - 39, 31, 6.4382994407110405E-08, 1.5852724149788030E-07, 1.5531692484911999E-09, 1.5553167500657000E-09 - 39, 32,-1.1029173844851870E-07,-2.4387905056924291E-07, 1.4455072701698999E-09, 1.4501550843618000E-09 - 39, 33, 1.7112465675371320E-07, 1.1879831889164030E-07, 1.3318157930413999E-09, 1.3380250605958001E-09 - 39, 34,-4.7767683725477561E-08,-6.6998431683726150E-08, 1.2241834948446000E-09, 1.2224634062290000E-09 - 39, 35, 1.4071716839129849E-07, 9.0275121736172926E-08, 1.0974807815217999E-09, 1.0965931031676001E-09 - 39, 36,-4.6308462864084007E-08,-6.4841144849370293E-08, 9.7802057825835013E-10, 9.8026146250936010E-10 - 39, 37,-4.8600314891556532E-08, 9.0444427821184413E-08, 8.3616341401812012E-10, 8.3512057977080015E-10 - 39, 38, 2.1996393733324770E-07,-1.5314143963627541E-07, 5.6308308094327009E-10, 5.6484166197498009E-10 - 39, 39,-5.1597207682441097E-08,-2.2609837027806909E-07, 3.6651947269690002E-10, 3.6272443115121001E-10 - 40, 0, 1.1431642571856310E-07, 0.0000000000000000E+00, 4.3565370006819999E-09, 0.0000000000000000E+00 - 40, 1, 3.1647660364933687E-08,-1.2315860111016490E-08, 4.3397110308841001E-09, 4.3604376870271002E-09 - 40, 2,-3.3315541741577042E-08,-9.2243360499731132E-08, 4.3488845862202002E-09, 4.3407501716164003E-09 - 40, 3, 1.8701581254687180E-08,-1.0205576486770170E-08, 4.3259698092749996E-09, 4.3282855894414001E-09 - 40, 4,-9.0846175675476103E-08, 5.2977246889948087E-08, 4.3030535433193000E-09, 4.3186024586383997E-09 - 40, 5, 5.0177116883188513E-08,-1.2588819373650590E-07, 4.2698941148760001E-09, 4.2871641305583001E-09 - 40, 6,-1.1217077677067280E-07, 7.8338895154592315E-08, 4.2108068708246004E-09, 4.2901306322554002E-09 - 40, 7, 2.1718041200904621E-08,-4.0624066538334683E-08, 4.2117722322596999E-09, 4.1879109869461004E-09 - 40, 8, 5.0790711991283391E-08,-7.5782032686181212E-08, 4.1491999548176997E-09, 4.1725101234980999E-09 - 40, 9, 5.4145072508608202E-08,-1.1988569027936910E-07, 4.0694632537465999E-09, 4.1157574532163999E-09 - 40, 10, 5.2353949617784162E-09,-1.1620105031857129E-07, 4.0383542100396002E-09, 4.0367892494077997E-09 - 40, 11, 9.2282346940739848E-08,-6.2717455970037551E-08, 3.9626860609048003E-09, 3.9554209508821003E-09 - 40, 12,-1.5666361601992489E-09, 1.3994069734621220E-08, 3.8809162934788000E-09, 3.8854532636673004E-09 - 40, 13, 9.2899519041465608E-09, 3.2590936643575061E-08, 3.7973103734763996E-09, 3.8006691686245001E-09 - 40, 14, 9.2560115363515330E-09, 5.1913782086675288E-08, 3.7033245847340999E-09, 3.7037445643064999E-09 - 40, 15,-9.7810519581017463E-08,-2.6810746744726531E-08, 3.6085468076240998E-09, 3.6160348021295999E-09 - 40, 16, 1.3511476653449830E-07,-1.5392095664508581E-08, 3.5114471030298999E-09, 3.5050676225453000E-09 - 40, 17,-7.4151417465336532E-08,-3.1231163152071722E-08, 3.4056334029001000E-09, 3.4066743913918998E-09 - 40, 18, 6.4803210962662469E-08, 5.7378266470393187E-08, 3.2994032789215998E-09, 3.2977408335367000E-09 - 40, 19,-1.0626163688162100E-07, 7.6350083673886074E-09, 3.1861232408072999E-09, 3.1917200216411999E-09 - 40, 20, 4.4546605643986383E-08,-3.6682748646973609E-09, 3.0791514750688000E-09, 3.0814103421329000E-09 - 40, 21, 1.2842102102267159E-08, 7.4625674210358361E-08, 2.9699040079105998E-09, 2.9652559232456001E-09 - 40, 22, 3.7840203669831671E-08,-3.3115310471431442E-08, 2.8535396864201002E-09, 2.8494261143211001E-09 - 40, 23, 8.6159655778415223E-08, 7.8277455659345510E-08, 2.7447592972053998E-09, 2.7465912469088001E-09 - 40, 24, 4.5210104573282512E-08,-5.9931342692569679E-11, 2.6323757785026999E-09, 2.6193318264278999E-09 - 40, 25,-7.5734740408267544E-08,-4.7314574994362781E-08, 2.5153037673779001E-09, 2.5177476699028998E-09 - 40, 26, 2.7047726198364879E-08, 1.2100929081666580E-07, 2.4052234072232000E-09, 2.4020288985008000E-09 - 40, 27, 4.6492553931192713E-09,-1.7294019656837769E-08, 2.2889893662475000E-09, 2.2870912467736000E-09 - 40, 28, 1.7634915939065211E-07, 1.1870827729518299E-07, 2.1755001129646001E-09, 2.1729526055816002E-09 - 40, 29,-8.9324278875407614E-08,-1.4283544336856740E-07, 2.0556201490151001E-09, 2.0549673373012001E-09 - 40, 30, 1.5463854684447579E-07,-6.1857317940726088E-08, 1.9451147633541998E-09, 1.9457327269791000E-09 - 40, 31,-2.1632223442408729E-07,-1.1714023769748050E-08, 1.8220712123244000E-09, 1.8269589031553999E-09 - 40, 32, 9.9384010028091922E-08,-5.6395574493421521E-08, 1.7050031014447999E-09, 1.7074599268152001E-09 - 40, 33,-7.3948282093979442E-08, 1.2896122476894400E-07, 1.5866996929663999E-09, 1.5931441551130000E-09 - 40, 34, 5.8934031083069974E-09,-2.1542192002439329E-07, 1.4696614302963999E-09, 1.4643536255201001E-09 - 40, 35,-7.5750529510708973E-08, 8.2091206778419993E-08, 1.3451562762663000E-09, 1.3458498668878001E-09 - 40, 36,-1.8911941880542261E-08,-6.1202906971342760E-08, 1.2078348743287000E-09, 1.2086268739590999E-09 - 40, 37,-1.9229968816438468E-08, 1.1666544956875890E-07, 1.0776357825042000E-09, 1.0789382738425000E-09 - 40, 38, 1.0377193592993050E-07,-4.2416003480273973E-08, 9.2587790001143015E-10, 9.2242454195647009E-10 - 40, 39,-1.3635697439148091E-07,-8.8373941074185891E-09, 6.1081355645544000E-10, 6.1268885767370008E-10 - 40, 40,-2.7101100513344118E-07, 8.5026569722694621E-08, 4.0312688286402001E-10, 4.0375879551846002E-10 - 41, 0, 7.9871997105759519E-08, 0.0000000000000000E+00, 4.8536815681199003E-09, 0.0000000000000000E+00 - 41, 1,-4.1043764165845303E-08,-6.6196153339955633E-08, 4.8350442304172997E-09, 4.8576261290102997E-09 - 41, 2, 4.9179249652509597E-08,-1.1013450301150640E-07, 4.8464471250358002E-09, 4.8361660933811004E-09 - 41, 3,-8.2312573526092829E-08, 6.8022061285030301E-08, 4.8193323635516000E-09, 4.8272399868962998E-09 - 41, 4, 3.8617443674673813E-08,-9.1090743411112681E-09, 4.7941319663762997E-09, 4.8143969273559002E-09 - 41, 5, 1.1298131071200550E-07, 8.5119144101046612E-08, 4.7629069930133004E-09, 4.7839494658635999E-09 - 41, 6,-4.4944522954060142E-08, 4.0965479663067913E-08, 4.6917213823435002E-09, 4.7854970542008003E-09 - 41, 7, 7.6006661332476222E-08, 1.1381524181573270E-07, 4.7075929055475000E-09, 4.6794528491369004E-09 - 41, 8, 9.6423737198694305E-09,-6.0828607290153702E-08, 4.6269108707493000E-09, 4.6545254011342998E-09 - 41, 9,-7.0898379878253088E-08,-1.9501451859426580E-08, 4.5583211900231000E-09, 4.6095121746958002E-09 - 41, 10,-7.9036251409471309E-08,-6.9977959671344712E-08, 4.5070592297337997E-09, 4.5100744624033997E-09 - 41, 11,-2.5332751859082841E-08,-7.2000149507426119E-09, 4.4442375164739003E-09, 4.4389303759007002E-09 - 41, 12,-8.3779834167687820E-08, 1.6107004253650731E-08, 4.3452726949190003E-09, 4.3513190941461002E-09 - 41, 13, 4.3832520518013943E-08, 2.3343830710948490E-09, 4.2625274257462999E-09, 4.2696727072243004E-09 - 41, 14,-5.2387436526443118E-08, 5.4724464684394042E-08, 4.1623555086222001E-09, 4.1654643831298999E-09 - 41, 15, 9.2140921844138248E-08,-5.7358728216130062E-08, 4.0582122175180001E-09, 4.0648087523495997E-09 - 41, 16,-3.9377891900435567E-08,-4.5074798336150379E-09, 3.9593746797143000E-09, 3.9549729871267000E-09 - 41, 17, 1.2925635359866230E-08,-9.9882784338163854E-08, 3.8413671255967002E-09, 3.8418836748286002E-09 - 41, 18,-1.0515459741048680E-07, 1.1322243917026700E-07, 3.7324291185028998E-09, 3.7273197797948002E-09 - 41, 19, 9.1774661613064660E-09, 1.6920261273236539E-08, 3.6092848113467000E-09, 3.6106592870390999E-09 - 41, 20, 3.9632674215208358E-08, 5.8044464641165463E-08, 3.4912428471816998E-09, 3.4916299331804000E-09 - 41, 21, 1.0178759866291400E-07, 7.6103953767527410E-09, 3.3733667038349000E-09, 3.3692622207384001E-09 - 41, 22, 6.2787577838306609E-08,-3.0790208864839737E-08, 3.2477543490810001E-09, 3.2465811906238000E-09 - 41, 23,-5.1007543902693572E-08,-3.2075425395985773E-08, 3.1239895203908999E-09, 3.1229016286254000E-09 - 41, 24,-3.3047895443993861E-08,-8.3378097064279612E-08, 3.0133697193626998E-09, 2.9994836997292001E-09 - 41, 25, 1.6129623177286039E-08,-6.0558536238174062E-08, 2.8769574419418000E-09, 2.8778316869239999E-09 - 41, 26,-1.5002838254841810E-08, 9.4476752114376612E-08, 2.7602697723983999E-09, 2.7532163243356001E-09 - 41, 27, 1.2136530532977621E-07,-6.5520567463968040E-08, 2.6342152990620998E-09, 2.6327775530200002E-09 - 41, 28,-8.7029992414525354E-08, 5.3360448749159223E-08, 2.5080191352496999E-09, 2.5064825622519000E-09 - 41, 29,-4.3867782895983341E-08,-9.9875722628776192E-08, 2.3851639274241000E-09, 2.3820578441980002E-09 - 41, 30,-1.4172518633341620E-08, 1.0689722703554319E-07, 2.2524692656559002E-09, 2.2558155685412001E-09 - 41, 31,-7.5750796045780452E-08,-1.0859777542615310E-07, 2.1337986275345002E-09, 2.1367787455794998E-09 - 41, 32, 1.0990067595226921E-07, 1.5469110921301549E-07, 2.0003607840018002E-09, 2.0034216939689999E-09 - 41, 33,-1.4856416031271959E-07,-2.8927228186120179E-08, 1.8714821224508999E-09, 1.8768802546121001E-09 - 41, 34, 1.3058340922204921E-07,-7.6364501320176865E-08, 1.7490305712510000E-09, 1.7434026707973999E-09 - 41, 35,-1.8715593868844011E-07, 4.7505811923597722E-08, 1.6146633741532000E-09, 1.6099521675505000E-09 - 41, 36, 2.2862299407007860E-08,-5.0383898653871342E-08, 1.4804011412513000E-09, 1.4812444875604000E-09 - 41, 37, 8.6528038465078909E-08, 9.5184082798041005E-08, 1.3293936610316000E-09, 1.3279845352863001E-09 - 41, 38, 2.0697875088996049E-08,-1.1672670545989641E-07, 1.1894639017349000E-09, 1.1919128642639000E-09 - 41, 39,-5.5167197888632668E-08,-2.2343651058036722E-08, 1.0074087999986000E-09, 1.0052862319106000E-09 - 41, 40, 9.6016911042050594E-08, 1.4806586193813390E-07, 6.5744510608772002E-10, 6.6164464139373005E-10 - 41, 41, 9.4297429823070367E-08, 1.5006966941898189E-07, 4.4525777036122002E-10, 4.4435036662742010E-10 - 42, 0,-1.7137556396439459E-08, 0.0000000000000000E+00, 5.4017689957910000E-09, 0.0000000000000000E+00 - 42, 1,-3.1085555217210388E-08,-2.0747224762365502E-08, 5.3919944402276000E-09, 5.4180817577411000E-09 - 42, 2, 5.8839602375953784E-09, 8.9574906805196080E-08, 5.3919900463163998E-09, 5.3836798217462004E-09 - 42, 3,-1.1164560247900809E-07, 2.4325083281381339E-08, 5.3767897371886001E-09, 5.3838267222679997E-09 - 42, 4, 1.3329878375203610E-07, 4.4277868919749381E-08, 5.3406497578784996E-09, 5.3581788877020997E-09 - 42, 5, 4.3066671070897467E-08,-4.1939709906210613E-08, 5.3165493991593000E-09, 5.3362312097032999E-09 - 42, 6, 5.1967204239964987E-08,-1.8241878220335150E-08, 5.2324096996497999E-09, 5.3308773213974002E-09 - 42, 7, 7.1622019173744901E-08,-1.9140139720980339E-08, 5.2557894105964003E-09, 5.2262701388934997E-09 - 42, 8, 5.8311147114710313E-08,-1.1124493316667720E-08, 5.1644959036402002E-09, 5.1964986726831999E-09 - 42, 9,-1.9001578573782611E-08, 5.1369916410424927E-08, 5.0951492005025998E-09, 5.1483100277250001E-09 - 42, 10,-1.3435375341139611E-08, 3.9066754006329112E-08, 5.0443578158561998E-09, 5.0471194850878002E-09 - 42, 11,-1.0622266046405760E-07, 9.8714521861853553E-08, 4.9692106940743997E-09, 4.9627628936765996E-09 - 42, 12,-2.9156055296528991E-08, 8.4347934667656576E-09, 4.8769781477038997E-09, 4.8854080920972999E-09 - 42, 13, 3.8083233571807552E-10, 4.2895604594033143E-09, 4.7727788188854000E-09, 4.7812726538014003E-09 - 42, 14,-6.7339003570920022E-08,-9.1892312588592614E-08, 4.6845083362737003E-09, 4.6813027472068001E-09 - 42, 15, 9.7413956855713262E-08, 7.8081651424869584E-09, 4.5586221276299996E-09, 4.5695766565388003E-09 - 42, 16,-1.4850282586143070E-07, 1.5002549465408500E-08, 4.4568519372082000E-09, 4.4530662002780996E-09 - 42, 17, 3.4946457205504107E-08, 3.8312943419232140E-08, 4.3343998306176000E-09, 4.3331500845737000E-09 - 42, 18,-7.2182285726355242E-08, 3.2160647815427140E-08, 4.2106541872980000E-09, 4.2068768426671999E-09 - 42, 19, 7.1998416423079081E-08,-4.8876387429211362E-08, 4.0810072603513002E-09, 4.0879165511319998E-09 - 42, 20, 2.9311588475205901E-08, 1.2032268815139301E-08, 3.9522381333464003E-09, 3.9514996085960003E-09 - 42, 21, 3.2595107644244431E-08,-7.6504405037729211E-08, 3.8251726729373999E-09, 3.8219149401141002E-09 - 42, 22,-2.4316022333933020E-08,-7.7247148133218600E-08, 3.6931637739063002E-09, 3.6886428418410001E-09 - 42, 23,-5.7432782183942077E-08,-9.5532279488757758E-09, 3.5566938045292002E-09, 3.5544464213345000E-09 - 42, 24,-6.0236532348569551E-08,-1.6040599693174530E-08, 3.4302527149196998E-09, 3.4144162123027002E-09 - 42, 25, 2.8551297372567519E-08, 7.0413032253180312E-08, 3.2928332494825000E-09, 3.2952253565486001E-09 - 42, 26, 6.0661485042093622E-09,-9.0879720173483327E-08, 3.1538767760076000E-09, 3.1467205091784000E-09 - 42, 27, 4.1464973076392673E-08,-5.3325049336206543E-08, 3.0242511392858001E-09, 3.0203544105009999E-09 - 42, 28,-1.3537264895830711E-07,-3.6903252119608633E-08, 2.8872191368341000E-09, 2.8845737395957000E-09 - 42, 29, 3.9770580211219934E-09,-2.0070711819112278E-08, 2.7513056856339001E-09, 2.7475759492551000E-09 - 42, 30,-8.9197805112631992E-08, 1.0456360005424260E-07, 2.6128816276740998E-09, 2.6150498290280002E-09 - 42, 31, 9.7777844196337413E-08, 3.3107553848855677E-08, 2.4714268726078998E-09, 2.4765731864935001E-09 - 42, 32, 1.8825796918954612E-09, 1.0264434234318540E-07, 2.3409550811963999E-09, 2.3461074740369999E-09 - 42, 33, 9.9393773922422288E-08,-1.8640078621906360E-07, 2.1928626166884001E-09, 2.2023186871975001E-09 - 42, 34, 6.6586344005882209E-10, 3.7258492673236861E-08, 2.0616812516398999E-09, 2.0567962568017000E-09 - 42, 35,-1.2443040460240540E-07,-2.7676663690450339E-08, 1.9202524458838999E-09, 1.9188358014647998E-09 - 42, 36, 8.2552359862342170E-08, 6.7052447146477134E-08, 1.7731663976384999E-09, 1.7735083532542000E-09 - 42, 37, 1.7102320914112520E-08, 1.8757472258333540E-08, 1.6295031133838001E-09, 1.6281429392958000E-09 - 42, 38, 7.0150903623199802E-09,-1.3743831882202600E-07, 1.4655651166083001E-09, 1.4650036680467001E-09 - 42, 39,-1.2087952580800200E-08, 9.4783993450608066E-10, 1.3074796912351000E-09, 1.3060615085414000E-09 - 42, 40,-1.9957656394902161E-08, 1.4256046084825890E-07, 1.1026687291226999E-09, 1.1046314904318000E-09 - 42, 41, 7.4550314984643260E-08,-1.5665030746893270E-07, 7.2278290660806008E-10, 7.2342710005829003E-10 - 42, 42, 7.5785726183163979E-08,-1.0699873237628981E-07, 4.9554836860219007E-10, 4.9557047908597002E-10 - 43, 0, 3.7471842745203072E-08, 0.0000000000000000E+00, 6.0253203365899999E-09, 0.0000000000000000E+00 - 43, 1, 1.0940925204293170E-07,-4.6518061672389871E-08, 5.9976895823990000E-09, 6.0234860218583997E-09 - 43, 2,-2.7502886464458809E-08, 1.5472351168609571E-07, 6.0179621376051998E-09, 6.0039704322354998E-09 - 43, 3,-5.6634453941805927E-08, 1.6876664552559672E-08, 5.9801392714298998E-09, 5.9889309015686002E-09 - 43, 4, 5.8773884977798221E-08, 7.1322115880910800E-08, 5.9577858996015999E-09, 5.9825408365380001E-09 - 43, 5,-5.7211615963801272E-08,-9.2086521843098551E-08, 5.9165128726402001E-09, 5.9407997363232997E-09 - 43, 6,-2.8183841998379459E-08,-5.2794268166107312E-08, 5.8420115787497001E-09, 5.9533229416897002E-09 - 43, 7,-1.5109491761491501E-08,-5.0671082746500437E-08, 5.8558192559899001E-09, 5.8227415908299003E-09 - 43, 8,-4.7414250585318121E-09,-4.9040619502658312E-08, 5.7715348354562004E-09, 5.8074328828668001E-09 - 43, 9, 5.6927781297715258E-08, 3.6408462864318533E-08, 5.6830874887439000E-09, 5.7462247327744000E-09 - 43, 10, 5.7090865770674262E-08, 2.7299785311151939E-08, 5.6447428163057996E-09, 5.6465016456502997E-09 - 43, 11, 1.1870731193889190E-08, 9.2961007340671258E-09, 5.5577279140890003E-09, 5.5469883506171003E-09 - 43, 12, 9.0647226275979881E-08, 8.0403021621620174E-09, 5.4628005183092003E-09, 5.4687146500335001E-09 - 43, 13,-4.9130097206984682E-08, 1.6668650379244290E-09, 5.3558597587657000E-09, 5.3617160536506003E-09 - 43, 14, 2.6217930541455741E-09,-2.3469552074918950E-08, 5.2478958771784004E-09, 5.2490360718109002E-09 - 43, 15,-6.4975709624341304E-09, 7.5480018508543451E-08, 5.1306220592409996E-09, 5.1408447084330001E-09 - 43, 16,-8.2163284341294182E-08,-8.3266185026719411E-08, 5.0086790890655001E-09, 5.0011388733263998E-09 - 43, 17, 5.7724632602488273E-09, 1.0443482734608840E-07, 4.8846864130076001E-09, 4.8851691959271998E-09 - 43, 18, 1.7307091838774720E-08,-2.4085970491631841E-08, 4.7488667563965998E-09, 4.7442833413172997E-09 - 43, 19,-1.0235890752631530E-08,-8.2465781317579683E-08, 4.6096148408216999E-09, 4.6141102332403997E-09 - 43, 20,-3.4230932223467940E-09,-6.2505704725977740E-08, 4.4749728649339998E-09, 4.4706615183261000E-09 - 43, 21,-3.7149548149485632E-08,-1.0040409706186210E-07, 4.3314022021663000E-09, 4.3258399379140000E-09 - 43, 22,-6.3001129932942259E-08, 3.7076901982693737E-08, 4.1899461044727997E-09, 4.1870402057841003E-09 - 43, 23,-1.0723202701120561E-07, 3.9852254590481537E-08, 4.0415547815925003E-09, 4.0415303353987996E-09 - 43, 24,-2.0456039931410390E-08,-6.2013653216909018E-09, 3.9030030874306999E-09, 3.8877934163141004E-09 - 43, 25, 4.3550529045118801E-08, 5.8145896737270153E-08, 3.7523004527496998E-09, 3.7540894531077999E-09 - 43, 26, 3.5068242081835120E-08,-8.7607428576049401E-08, 3.6111490223843002E-09, 3.6032725038277002E-09 - 43, 27,-3.5308164199753332E-08, 1.0433673431789490E-07, 3.4540331619642999E-09, 3.4514819998214001E-09 - 43, 28,-8.4334891546169632E-08,-3.9205747691987771E-08, 3.3160779997766999E-09, 3.3130222274571000E-09 - 43, 29, 1.8714465002736638E-08, 1.4882653996220990E-07, 3.1644492899173999E-09, 3.1645709535368000E-09 - 43, 30, 2.8212679299797081E-08,-3.7809465777908942E-08, 3.0149745451310000E-09, 3.0156963244294000E-09 - 43, 31, 1.4028441313329869E-07, 6.5448088623924091E-09, 2.8649602485608001E-09, 2.8712861416482001E-09 - 43, 32,-3.6540133060966673E-08,-6.5907204135334644E-08, 2.7150883176357999E-09, 2.7156520007794999E-09 - 43, 33, 1.2489740204139020E-07,-6.6456338416773170E-08, 2.5690642997434999E-09, 2.5780375855160000E-09 - 43, 34,-1.8086846948942510E-07, 1.9156392792913430E-08, 2.4163837359313000E-09, 2.4097968046182998E-09 - 43, 35, 1.1616167578339940E-07, 1.2850018242570830E-08, 2.2638842264652999E-09, 2.2632897512463001E-09 - 43, 36, 2.0183663109732231E-08, 1.1238488442878350E-07, 2.1097672308466000E-09, 2.1102942732254001E-09 - 43, 37, 3.8750508202634828E-08,-5.8657085637391073E-08, 1.9526270099558999E-09, 1.9499845154016002E-09 - 43, 38,-6.3244673688973899E-08, 2.9648542185686360E-08, 1.8014170643809000E-09, 1.8012004698190999E-09 - 43, 39,-7.4635923836723781E-08, 5.6817100944223847E-08, 1.6074015599984000E-09, 1.6071725484010000E-09 - 43, 40, 9.9875709287124122E-09, 6.3582318152205199E-08, 1.4370887165619999E-09, 1.4398700189888999E-09 - 43, 41, 8.7613415840494159E-08,-3.4307045255616830E-08, 1.2093636312690000E-09, 1.2083688335381000E-09 - 43, 42,-1.4901347533796320E-07,-5.1742819938211677E-08, 7.9214314553688009E-10, 7.9125757888886006E-10 - 43, 43,-1.1566099896650800E-07, 3.1341189801872529E-09, 5.4833082654205001E-10, 5.4392920757779999E-10 - 44, 0, 8.5524329039863840E-08, 0.0000000000000000E+00, 6.6967911266635003E-09, 0.0000000000000000E+00 - 44, 1, 8.3850034826364461E-08, 1.5996922284384351E-09, 6.6921769702013998E-09, 6.7198343561653999E-09 - 44, 2,-4.7383539368391223E-08,-1.1429095402858490E-08, 6.6883143523976998E-09, 6.6732824625369002E-09 - 44, 3, 8.6781111274101510E-08,-2.8057002814233372E-08, 6.6740431346801001E-09, 6.6817647116594997E-09 - 44, 4, 4.2635426081388002E-08, 1.4930471391427640E-08, 6.6260675815080003E-09, 6.6501261123059003E-09 - 44, 5, 2.2842110478575401E-08,-5.0239429063942463E-08, 6.6040546069081997E-09, 6.6321629981050997E-09 - 44, 6,-1.0455791021996280E-07, 5.6878315588005877E-08, 6.4969623988667996E-09, 6.6275980438470004E-09 - 44, 7,-1.0353451878592160E-07, 8.9155653753268193E-09, 6.5427071871772004E-09, 6.5037381445638004E-09 - 44, 8,-5.0553512133621493E-08, 5.6768249086620301E-08, 6.4296897676176003E-09, 6.4687020771260002E-09 - 44, 9, 1.2305416456194189E-08, 6.1245464372566503E-09, 6.3536110003647001E-09, 6.4258131447114000E-09 - 44, 10, 2.8702632299081250E-08,-4.5126972323724348E-08, 6.2961207193928998E-09, 6.3015589180575000E-09 - 44, 11, 5.8117779943790087E-08,-1.4470231349996529E-07, 6.2203240012459999E-09, 6.2136338755930998E-09 - 44, 12, 3.8619109743098307E-08, 2.4118475635070741E-09, 6.1072018040489998E-09, 6.1121853427524998E-09 - 44, 13,-2.6946599755023951E-08,-5.4019276646471498E-08, 6.0044769113906999E-09, 6.0135615078900004E-09 - 44, 14, 5.2781117875213477E-08, 5.6331461585802138E-08, 5.8807324455479003E-09, 5.8850082457388002E-09 - 44, 15,-4.1425424068052802E-08, 3.1662857675478171E-08, 5.7593421629291998E-09, 5.7688481156160001E-09 - 44, 16, 1.1381667483408540E-07,-1.2975366283877720E-08, 5.6334362629907003E-09, 5.6276983260458998E-09 - 44, 17, 1.4242528464987590E-08,-1.6798474881229641E-08, 5.4893503352855996E-09, 5.4902030904824004E-09 - 44, 18, 5.4584165288354497E-08,-2.4186520246601400E-08, 5.3561859806542000E-09, 5.3516099812798004E-09 - 44, 19,-5.4594231664254258E-09,-5.6998281404251623E-09, 5.1976103758004998E-09, 5.2023578839582001E-09 - 44, 20,-7.3487034811141202E-08,-4.4555136940010121E-08, 5.0533992178279000E-09, 5.0546728027307998E-09 - 44, 21,-4.1189532928050653E-08, 6.1390113598152640E-08, 4.8998086155248999E-09, 4.8970884757648000E-09 - 44, 22, 9.0384080620170283E-09, 2.5931095822628839E-08, 4.7457262232640001E-09, 4.7394251235608999E-09 - 44, 23,-5.5985903118507084E-09,-3.7199475467357619E-10, 4.5885032375039002E-09, 4.5876877437054997E-09 - 44, 24, 9.9954947413291572E-08, 2.2522892183270111E-08, 4.4358484517249004E-09, 4.4193957154857998E-09 - 44, 25,-3.6462773242078573E-08,-2.8243651922494430E-08, 4.2687147304105004E-09, 4.2727670760407001E-09 - 44, 26,-6.6483391857885573E-09,-5.3173451362795557E-08, 4.1173224604217999E-09, 4.1065085469750002E-09 - 44, 27,-6.1870101571911623E-08, 4.7515448598952557E-08, 3.9562647355410996E-09, 3.9487816005029996E-09 - 44, 28,-8.8922453061693363E-09, 4.3288168671051990E-09, 3.7867868384792999E-09, 3.7855055998343004E-09 - 44, 29,-9.2701585348725802E-09, 7.8290546289377349E-08, 3.6383446165422001E-09, 3.6341138598297999E-09 - 44, 30, 1.0070698114291090E-07,-5.4711399155709687E-08, 3.4669046939539999E-09, 3.4726619431384000E-09 - 44, 31, 6.4874257654950712E-09, 5.6696368420009374E-09, 3.3056866723733998E-09, 3.3138185451482999E-09 - 44, 32,-3.9507976334654172E-08,-1.1480721203835839E-07, 3.1467440936905999E-09, 3.1491574942707001E-09 - 44, 33,-1.0068234422754279E-07, 6.8675085783054870E-08, 2.9779664968550001E-09, 2.9867996820106001E-09 - 44, 34,-5.3958221083960017E-08,-3.1219946448202773E-08, 2.8293649252367000E-09, 2.8230152861647001E-09 - 44, 35, 9.3310092392370870E-08, 8.5294851784949574E-08, 2.6522784888352999E-09, 2.6495105856711999E-09 - 44, 36, 3.2480357689670010E-09,-5.3999135982482587E-08, 2.4880675232694001E-09, 2.4899582891293998E-09 - 44, 37, 6.9878965021720259E-08,-4.3287315369110178E-08, 2.3223134535626002E-09, 2.3190886684744000E-09 - 44, 38,-1.0698171160101250E-07, 4.4964159828214733E-08, 2.1510266392474998E-09, 2.1479233009433999E-09 - 44, 39, 5.2216479475137063E-08, 3.5305334520983051E-08, 1.9704809727963002E-09, 1.9720561064276999E-09 - 44, 40, 1.0020580028866940E-07, 1.3256528091229689E-08, 1.7668689530747999E-09, 1.7705626007434000E-09 - 44, 41,-3.1576137868590563E-08,-5.9021595782638833E-08, 1.5829046390807000E-09, 1.5844586416789000E-09 - 44, 42, 5.8575368170156192E-08,-3.1841242105327440E-08, 1.3317231176191001E-09, 1.3312949749083000E-09 - 44, 43,-6.8395396577632181E-10, 1.0253780798043329E-07, 8.6658888976186010E-10, 8.7255632690104003E-10 - 44, 44, 5.0469092624835337E-08, 1.9157201867454991E-07, 6.0076663153271001E-10, 6.2869135601547004E-10 - 45, 0, 7.0816882022473439E-09, 0.0000000000000000E+00, 7.4580287026931005E-09, 0.0000000000000000E+00 - 45, 1,-1.3296976582754729E-08, 6.6534684936877042E-08, 7.4466498150470999E-09, 7.4720824806187997E-09 - 45, 2,-7.5859988876592400E-08,-5.9366772604899227E-08, 7.4501616340816998E-09, 7.4352565233383004E-09 - 45, 3,-3.1288336452636682E-08, 1.3994387397176570E-09, 7.4245913400217996E-09, 7.4321326792748997E-09 - 45, 4, 4.4801136225444247E-08,-1.3327022127010760E-08, 7.3839487144407003E-09, 7.4158219024254999E-09 - 45, 5, 5.5782576243611607E-08, 1.4549561383987430E-08, 7.3489053927805998E-09, 7.3763819455607999E-09 - 45, 6, 3.0991118828510422E-08, 4.4211678959369152E-08, 7.2523837884696003E-09, 7.3907447173556996E-09 - 45, 7, 3.9204071905402442E-08, 9.0555599872658693E-08, 7.2809829768070000E-09, 7.2401196669200000E-09 - 45, 8, 1.2025282909251610E-09, 2.5768597646997800E-08, 7.1819329447856002E-09, 7.2260509398192002E-09 - 45, 9,-4.1689886151007030E-08, 6.8031942837560232E-08, 7.0831509865671003E-09, 7.1528980839210004E-09 - 45, 10,-9.4904206100350488E-08,-9.3789931341861924E-08, 7.0443395178597996E-09, 7.0452051485728996E-09 - 45, 11,-3.4612560624975093E-08,-2.9884315131893713E-08, 6.9408932581039999E-09, 6.9309721015835996E-09 - 45, 12,-1.0900885608370560E-07,-1.2408142589623951E-08, 6.8387741348567996E-09, 6.8461954018499001E-09 - 45, 13,-6.1089332147779283E-08,-2.0836777079938710E-08, 6.7114240666236999E-09, 6.7215859510143998E-09 - 45, 14,-4.2805435245291702E-10, 6.3067690646383739E-08, 6.6012587136810003E-09, 6.6012059351700001E-09 - 45, 15,-1.0093677530041090E-08,-1.1425541970500419E-07, 6.4506402340688997E-09, 6.4631806765927998E-09 - 45, 16, 7.2495705193027511E-08, 3.5989321277322470E-08, 6.3307528932015998E-09, 6.3226914074112999E-09 - 45, 17, 8.5104685390003286E-09,-6.2755562162705310E-08, 6.1691160315479996E-09, 6.1735979979674001E-09 - 45, 18, 6.0500033515889764E-08,-3.1282446374317503E-08, 6.0242032162173004E-09, 6.0194344580088003E-09 - 45, 19, 2.3247088768756169E-08,-8.9575735474308089E-09, 5.8650547729307998E-09, 5.8678391036225002E-09 - 45, 20,-3.1738876652873501E-09,-2.9464093916811920E-08, 5.6994710766570002E-09, 5.6965666440148003E-09 - 45, 21, 4.7755250994173034E-09, 5.5020158294225963E-08, 5.5397086671470001E-09, 5.5365841056229000E-09 - 45, 22, 5.7477301802602071E-08,-1.0082689121919501E-08, 5.3667964387384999E-09, 5.3653548171760003E-09 - 45, 23, 4.3160171732370847E-08,-4.2464669436463657E-08, 5.1983816440709998E-09, 5.1971506119605000E-09 - 45, 24,-8.6949120978425074E-09, 2.2566936447842442E-08, 5.0369979958771003E-09, 5.0171581758793996E-09 - 45, 25, 4.8261549571777717E-08,-6.3145875372852851E-08, 4.8512516802564998E-09, 4.8564161990824004E-09 - 45, 26,-1.3781083666783460E-08, 2.2072431906074442E-08, 4.6863688586216000E-09, 4.6747605755306999E-09 - 45, 27,-4.6343005056650343E-08,-3.7589942916291363E-09, 4.5106581504901997E-09, 4.5047958927596003E-09 - 45, 28, 7.8148187644440609E-08, 3.7010142461081297E-08, 4.3333489829245001E-09, 4.3314819787330002E-09 - 45, 29,-1.6483929470657571E-08,-1.5000358657275531E-08, 4.1545428797124001E-09, 4.1533458908685004E-09 - 45, 30, 3.6480077514276101E-08,-4.3632243838042597E-08, 3.9866718282299000E-09, 3.9923025173569003E-09 - 45, 31,-9.6047661469233808E-08,-3.0622063205300112E-08, 3.8021254499815003E-09, 3.8112213389371000E-09 - 45, 32,-3.0754307390408008E-08, 4.6331145159682542E-08, 3.6302118576173001E-09, 3.6350899529596999E-09 - 45, 33,-9.7621921347122933E-08, 6.5918633199269810E-08, 3.4494136181665000E-09, 3.4626210934545001E-09 - 45, 34, 5.0261918034275291E-08, 1.8605801408433561E-08, 3.2794599207675002E-09, 3.2719353617325999E-09 - 45, 35,-3.0535133943112557E-08, 2.2722675079391121E-08, 3.1048439504128998E-09, 3.1052041109103002E-09 - 45, 36, 3.0922367051313182E-08,-7.9941687861232612E-08, 2.9129752966724999E-09, 2.9144912771194000E-09 - 45, 37,-1.1239119887232200E-07, 5.1675783240508652E-08, 2.7412540626955001E-09, 2.7361891459877000E-09 - 45, 38,-2.9880709851304671E-08,-3.7096821704852383E-08, 2.5558368756345000E-09, 2.5539872496561999E-09 - 45, 39, 9.0423983289436129E-08,-1.6802746784832989E-08, 2.3602893136554000E-09, 2.3618425830073000E-09 - 45, 40, 2.0771503371137012E-09,-5.0700473636701027E-08, 2.1646963324294999E-09, 2.1727054598575000E-09 - 45, 41,-9.1701384497857444E-08,-1.2205899401810210E-07, 1.9440690819096001E-09, 1.9486359397646999E-09 - 45, 42,-4.2217657939705328E-08, 7.9912712116935674E-08, 1.7440517978280999E-09, 1.7457564627471000E-09 - 45, 43,-1.0311655878126590E-08,-9.3263166085774407E-08, 1.4644645667373000E-09, 1.4607127673898001E-09 - 45, 44, 2.7671029844338330E-08,-9.2947130324559163E-09, 9.6365618254225018E-10, 9.4757371820821998E-10 - 45, 45, 1.2207329320214070E-07,-1.3131956375791811E-07, 6.6941480365647006E-10, 6.7304176718417009E-10 - 46, 0, 7.5987367097924869E-08, 0.0000000000000000E+00, 8.3053668021114993E-09, 0.0000000000000000E+00 - 46, 1,-2.5915102522951452E-08, 2.7471214407565709E-08, 8.2770329627909006E-09, 8.3085952455978004E-09 - 46, 2,-1.9650576990196731E-08, 3.9340574054676623E-08, 8.2935601618923005E-09, 8.2811004224602007E-09 - 46, 3,-4.9506857631354971E-08, 4.3105646467268897E-08, 8.2545022014127999E-09, 8.2663253103632005E-09 - 46, 4,-1.5782825623008170E-09, 2.6671432486938089E-08, 8.2249948762118002E-09, 8.2568385378070002E-09 - 46, 5, 1.7502450665273479E-08, 3.8338098547694833E-09, 8.1764869684272998E-09, 8.2097395500283004E-09 - 46, 6, 1.5814069559538240E-08,-5.2784424123614487E-08, 8.0768459768524001E-09, 8.2321338637521996E-09 - 46, 7, 4.7768073377267318E-09, 2.4809146636188650E-08, 8.1157869331198994E-09, 8.0623846076595004E-09 - 46, 8, 7.6139431128016741E-08,-1.8629588565592900E-08, 8.0005635501464993E-09, 8.0513361440412995E-09 - 46, 9, 5.0854819120122321E-08, 1.4526835821870110E-08, 7.9035811196245999E-09, 7.9836323962919007E-09 - 46, 10, 2.8101249933856340E-08,-1.0860961990170790E-08, 7.8520745044879995E-09, 7.8527797304451999E-09 - 46, 11,-3.8525417399715807E-08, 6.0365548089748078E-08, 7.7639377001531004E-09, 7.7457987582069005E-09 - 46, 12,-1.2884142131903131E-07, 7.0348185116721761E-08, 7.6334810099850997E-09, 7.6375618554772993E-09 - 46, 13, 4.5803597678275481E-08, 7.2435849038390361E-08, 7.5171720607438005E-09, 7.5287487687497998E-09 - 46, 14,-1.0104159353350031E-07, 4.1296463071040362E-08, 7.3794326306473999E-09, 7.3782727633698998E-09 - 46, 15, 1.1226059133059190E-07,-3.4913567682375041E-09, 7.2385174862943001E-09, 7.2555621037341000E-09 - 46, 16,-8.4220283407263993E-08, 2.8839133016602710E-08, 7.0886097450461000E-09, 7.0829212922666001E-09 - 46, 17, 1.3135319549772810E-08,-6.1969901416681331E-08, 6.9404049115801998E-09, 6.9397199259124001E-09 - 46, 18,-4.0650277090905087E-08, 4.7434479194609827E-08, 6.7663352154121997E-09, 6.7622209130460996E-09 - 46, 19,-6.2657249028578554E-08,-4.0167648416253761E-09, 6.6002041007711996E-09, 6.6075417667100003E-09 - 46, 20, 5.0371007082258712E-08, 4.2177565690707537E-08, 6.4299179707206996E-09, 6.4210622424914996E-09 - 46, 21,-1.6105832537482660E-08,-1.7033245141205689E-08, 6.2482293398193003E-09, 6.2410612892534001E-09 - 46, 22, 3.2107379207394822E-08,-5.1936442954534707E-08, 6.0697023633325999E-09, 6.0659292712318000E-09 - 46, 23,-3.2155530442314387E-08,-3.8418176892369747E-08, 5.8807757271295003E-09, 5.8772103219672001E-09 - 46, 24,-6.0076479141730190E-08,-6.9435840605643939E-08, 5.7089132032935996E-09, 5.6842981082902999E-09 - 46, 25,-9.3487152177013471E-10,-4.0034290714103322E-08, 5.5086613020177999E-09, 5.5107917866121003E-09 - 46, 26,-1.2871097130107059E-08, 3.3678749728784060E-08, 5.3257582177330002E-09, 5.3112750558406003E-09 - 46, 27, 1.4742517463960399E-08,-3.2339406308604701E-08, 5.1328301390456000E-09, 5.1295395801021000E-09 - 46, 28, 3.6138205300870661E-09, 7.6558797730435231E-08, 4.9432053643993997E-09, 4.9406249626915998E-09 - 46, 29,-5.0684867347837111E-08,-8.2175164374130581E-08, 4.7522559179426003E-09, 4.7491590473171004E-09 - 46, 30,-2.5515291809644268E-08,-4.1661066427301202E-08, 4.5535319748714999E-09, 4.5625543867611003E-09 - 46, 31,-9.0500065907009915E-09, 5.6963804443523647E-08, 4.3731306340205001E-09, 4.3834219327166004E-09 - 46, 32, 2.9220048537814631E-08, 5.7653353868610242E-08, 4.1731565593020999E-09, 4.1797976021918996E-09 - 46, 33, 2.3061921547064679E-08, 1.2546490744583380E-08, 3.9833142184629997E-09, 3.9967508441705002E-09 - 46, 34, 6.6128195828951323E-08,-2.8573074141846971E-08, 3.7997396486597001E-09, 3.7919849748662001E-09 - 46, 35,-3.3832021781669332E-08,-6.8719335366843424E-08, 3.6004341793315001E-09, 3.5984050251703999E-09 - 46, 36,-8.7462068973516514E-09, 5.9510942291544211E-08, 3.4126128539801999E-09, 3.4106272054872000E-09 - 46, 37,-1.2365684344913969E-07, 3.8299187959994572E-08, 3.2036725152979001E-09, 3.2010442915127002E-09 - 46, 38, 5.1064888612002412E-08, 3.3655627843064681E-08, 3.0146194318339002E-09, 3.0128347459582000E-09 - 46, 39,-2.7508648084207650E-08, 8.6354543160772033E-09, 2.8068162942188001E-09, 2.8049091317473000E-09 - 46, 40,-7.3551417071972269E-08,-3.1395531557854011E-08, 2.5987342664421001E-09, 2.5974583012214001E-09 - 46, 41,-1.7118756362328690E-08,-7.3069559603513099E-08, 2.3863584388643998E-09, 2.3866820021184001E-09 - 46, 42,-6.1193128608632982E-08, 1.6768805906150490E-07, 2.1426151552006999E-09, 2.1433554615214002E-09 - 46, 43, 5.0418991655279243E-08,-3.8740377358284123E-08, 1.9242012357268000E-09, 1.9221222195688999E-09 - 46, 44,-8.9823856959702204E-09, 5.4720559910589031E-08, 1.6119932297693000E-09, 1.6093645421290000E-09 - 46, 45, 1.7942334640991878E-08, 2.0342167500590872E-08, 1.0496166669352001E-09, 1.0463613103364999E-09 - 46, 46,-1.5459355307663359E-07,-6.1676338688784440E-08, 7.5474634607639003E-10, 7.5357071817490003E-10 - 47, 0, 1.5614301951303130E-08, 0.0000000000000000E+00, 9.2308285386458997E-09, 0.0000000000000000E+00 - 47, 1, 2.3609025745226040E-08,-1.0388620820807531E-07, 9.2076274203086001E-09, 9.2396467641682004E-09 - 47, 2, 2.9646393062244212E-09, 5.6639972735709062E-08, 9.2209014635769004E-09, 9.2012985938049996E-09 - 47, 3, 5.2704918819669973E-08,-1.3794245411362680E-09, 9.1850330783676998E-09, 9.1956304559128994E-09 - 47, 4, 8.1851609676822241E-08, 6.6168165171316149E-08, 9.1395365208593003E-09, 9.1812674381112007E-09 - 47, 5,-3.7402201275496719E-09,-3.0625354541469449E-09, 9.1023524941452000E-09, 9.1397019071456001E-09 - 47, 6,-2.0413793975060600E-09,-1.7822421928175191E-08, 8.9786650744105007E-09, 9.1561738623577006E-09 - 47, 7,-2.6096874623112950E-08,-1.1144131567663740E-07, 9.0341807865165008E-09, 8.9893406411892002E-09 - 47, 8, 1.3818880298297440E-08,-3.3102612010965580E-08, 8.9034487705110007E-09, 8.9593703233408001E-09 - 47, 9, 1.9372544607452760E-08,-6.8453890897754640E-08, 8.8090015453294001E-09, 8.9032607234428006E-09 - 47, 10, 3.8836813477129413E-08,-2.5974803105918101E-08, 8.7471499664884008E-09, 8.7553638384027994E-09 - 47, 11, 1.1656363575311950E-08, 4.2575337716427072E-08, 8.6594585695016992E-09, 8.6432890994529999E-09 - 47, 12, 6.5057914860563920E-08,-1.9250824541312060E-08, 8.5209180846839000E-09, 8.5338647648507007E-09 - 47, 13, 1.1995568065751530E-08, 1.1701764231398260E-07, 8.3930540500798007E-09, 8.4038738114510994E-09 - 47, 14, 2.1456691067849820E-08,-4.7139713071279618E-08, 8.2562222470633005E-09, 8.2658063054626996E-09 - 47, 15, 1.7524858871249200E-08, 6.1399267272348140E-08, 8.0945197387208007E-09, 8.1090450669894001E-09 - 47, 16,-1.1249604113748950E-08,-1.7713757486640639E-08, 7.9540008397590994E-09, 7.9482242627827005E-09 - 47, 17,-2.1454349663442841E-09, 3.7108029914748192E-08, 7.7714460942300997E-09, 7.7722428631594994E-09 - 47, 18,-5.6041038707306787E-08, 7.6481633006867533E-08, 7.6146607215393994E-09, 7.6017653321920005E-09 - 47, 19, 5.2653009475560343E-08,-4.9927921104807953E-08, 7.4117373549378004E-09, 7.4148504143118004E-09 - 47, 20,-6.4701501854907848E-09, 5.3749854165845902E-08, 7.2407911223855998E-09, 7.2373446592245002E-09 - 47, 21, 1.7267930904419589E-08,-4.9606225509938342E-08, 7.0399300938140999E-09, 7.0347284193213998E-09 - 47, 22,-6.4399127634675531E-08,-2.0592778131918201E-08, 6.8489628718932999E-09, 6.8399643948823996E-09 - 47, 23,-6.1265781995959141E-08, 5.3791358775832221E-08, 6.6477347069731003E-09, 6.6454594249420003E-09 - 47, 24,-6.0731889860468238E-08,-5.4278119146964663E-08, 6.4527605107044001E-09, 6.4329855759990997E-09 - 47, 25,-2.4833438094629802E-08, 2.7271716452670110E-08, 6.2406126502433996E-09, 6.2496242354982003E-09 - 47, 26, 1.1324794090940210E-09,-1.9916398660419091E-08, 6.0434797129407003E-09, 6.0281453541346998E-09 - 47, 27, 2.5162579317479261E-08,-1.2885048781083690E-08, 5.8355751913820001E-09, 5.8246923313632002E-09 - 47, 28,-6.7332273243277863E-08, 1.1681170810699170E-08, 5.6266282430951002E-09, 5.6253680925565001E-09 - 47, 29,-9.5616818536305902E-09,-1.1275646948486110E-07, 5.4194248607901999E-09, 5.4184690227782999E-09 - 47, 30,-4.8655336502165643E-08, 4.4966992296574312E-08, 5.2059312873363000E-09, 5.2142584359664004E-09 - 47, 31, 3.2232108668396122E-08,-8.7299695372077168E-09, 4.9972272251233997E-09, 5.0105637649557002E-09 - 47, 32, 5.9907918671449982E-08, 1.3308402668684691E-08, 4.8018449526676996E-09, 4.8072219596262003E-09 - 47, 33, 4.1878874831374902E-08,-2.0952992929531140E-08, 4.5765461146223003E-09, 4.5916670420600997E-09 - 47, 34,-8.5950044430708048E-09,-1.0430897798193800E-07, 4.3870289540180000E-09, 4.3767005856342002E-09 - 47, 35, 2.8934072637661000E-08,-3.4453397542848681E-08, 4.1708675369820000E-09, 4.1666793774728996E-09 - 47, 36,-2.6326153823111859E-08, 5.3982163209910893E-08, 3.9541947445926999E-09, 3.9593431640908003E-09 - 47, 37, 5.7671174914856917E-08,-6.3325244439165243E-09, 3.7536429688758004E-09, 3.7477016175814996E-09 - 47, 38, 1.7891910708106471E-08, 6.3276885864226668E-08, 3.5259242235726000E-09, 3.5213813158972001E-09 - 47, 39, 7.0311880456086892E-08,-4.8770149122363111E-08, 3.3102469147118002E-09, 3.3146232634387998E-09 - 47, 40,-1.6027597113483761E-08, 3.3173933296197851E-08, 3.0842952047202999E-09, 3.0856281930724001E-09 - 47, 41,-1.6628981842619978E-08, 5.5436772513276471E-08, 2.8591424278772998E-09, 2.8621177236146000E-09 - 47, 42, 5.7797659469229983E-09, 5.8593961667630912E-08, 2.6255271476004000E-09, 2.6284674566479999E-09 - 47, 43, 1.3859206998216579E-07, 2.9646485641977078E-09, 2.3606925021418998E-09, 2.3599348575995999E-09 - 47, 44,-1.9102331435626339E-08, 9.8336413661396019E-10, 2.1066412056284000E-09, 2.1320734112382002E-09 - 47, 45,-2.0243580815915009E-08, 1.1855552840696251E-08, 1.7772211344349000E-09, 1.7722137056639000E-09 - 47, 46,-5.0494110664638594E-09,-5.9960208767685802E-08, 1.1542744415681999E-09, 1.1463360009904001E-09 - 47, 47, 3.9016634733085787E-08, 1.4013084125490451E-07, 8.2554790793524015E-10, 8.2321638999531012E-10 - 48, 0,-6.3750241134827699E-08, 0.0000000000000000E+00, 1.0259158823852000E-08, 0.0000000000000000E+00 - 48, 1,-1.9762579165320080E-08,-7.1935622896234082E-08, 1.0227434500870000E-08, 1.0260174772260000E-08 - 48, 2,-6.7394142368719490E-08,-3.6465683234284137E-08, 1.0247048485258000E-08, 1.0225478723472000E-08 - 48, 3, 3.1501252776408332E-08,-1.7251413095420339E-08, 1.0199245844961000E-08, 1.0218295733749000E-08 - 48, 4, 8.7113213298670963E-08, 1.4822951110727410E-08, 1.0160260144403001E-08, 1.0203590205489000E-08 - 48, 5,-1.8438600405829319E-08,-2.7758658523185819E-08, 1.0117431386801001E-08, 1.0154562989101001E-08 - 48, 6, 2.2167200974868909E-10, 6.3690413352121522E-08, 9.9862282610952995E-09, 1.0180139382934001E-08 - 48, 7, 2.4290147206032891E-08, 4.1319260120170911E-09, 1.0047557428616999E-08, 9.9915392197516997E-09 - 48, 8,-5.4724685141463572E-08, 1.6965723165524921E-08, 9.9107987845498996E-09, 9.9732403628553006E-09 - 48, 9,-2.6484662888791281E-08, 1.9297319491991659E-08, 9.8033840686030001E-09, 9.9003938883756002E-09 - 48, 10,-2.7412167214822730E-08,-2.7440515466141559E-08, 9.7547149601343005E-09, 9.7531942114681995E-09 - 48, 11,-1.6168441375188821E-08,-8.6346984004787953E-08, 9.6414494346891005E-09, 9.6271409352103004E-09 - 48, 12, 1.0729327872635720E-07,-4.5846866530718681E-08, 9.5150367698897006E-09, 9.5208076526378992E-09 - 48, 13,-4.4650408335363178E-09,-5.8636496229109702E-08, 9.3623656821377002E-09, 9.3751614646196007E-09 - 48, 14, 5.7722413946988117E-08, 3.5568091087407127E-08, 9.2253822589712007E-09, 9.2305020835311005E-09 - 48, 15,-9.7569047018103591E-10, 1.9261911766125070E-08, 9.0542953642801006E-09, 9.0690846300744000E-09 - 48, 16, 7.3595596989960582E-08,-3.3891614405056541E-08, 8.8941124524424000E-09, 8.8878111195082004E-09 - 48, 17,-1.9339901087533729E-08, 5.4579615546871717E-08, 8.7139475490963003E-09, 8.7182790827953004E-09 - 48, 18, 3.1243176920553012E-08,-3.6656109603871737E-08, 8.5227963813165998E-09, 8.5183016863941999E-09 - 48, 19, 7.9703081102276183E-08, 5.6803461722788712E-09, 8.3363891014164998E-09, 8.3354061522043001E-09 - 48, 20,-1.0017600934077501E-09,-6.5581858147522963E-08, 8.1250259546058996E-09, 8.1194737621496994E-09 - 48, 21, 2.5460022420247709E-08, 3.3248858049502239E-09, 7.9329433777085007E-09, 7.9286994805400992E-09 - 48, 22,-3.4140412149492301E-08,-4.9924233757748613E-09, 7.7055574536742999E-09, 7.7061365617312998E-09 - 48, 23,-3.8433009085633043E-08, 6.4918412890670270E-08, 7.4998058409392004E-09, 7.4983001726151007E-09 - 48, 24, 2.3229396021088109E-08, 3.3357193677674663E-08, 7.2920992655258999E-09, 7.2677772249955000E-09 - 48, 25, 6.0057041403143620E-09, 6.0435785326037792E-08, 7.0565639448915996E-09, 7.0667455942011997E-09 - 48, 26, 3.6040279595973920E-08,-5.8091946101487402E-08, 6.8499275139059998E-09, 6.8310556003603997E-09 - 48, 27,-1.2785461164684380E-08, 4.5379759717073297E-08, 6.6187889797227998E-09, 6.6071729314955997E-09 - 48, 28,-4.2663598426513397E-08,-9.8472372395266584E-09, 6.3950221129029000E-09, 6.3883472698864002E-09 - 48, 29, 7.5456155750597337E-09, 4.6154199826600671E-08, 6.1702809230552998E-09, 6.1665175099044000E-09 - 48, 30,-3.8421961280628443E-08, 8.7862016596813804E-09, 5.9379454638974002E-09, 5.9462037251829003E-09 - 48, 31, 9.6022131165956568E-08,-2.6932469663205360E-08, 5.7123621706353999E-09, 5.7202095346676001E-09 - 48, 32,-1.7141580496130462E-08, 1.1830337989638490E-08, 5.4886250401256001E-09, 5.4954383413700004E-09 - 48, 33,-2.9458560564814932E-09,-6.5394256683156383E-08, 5.2636669894201000E-09, 5.2828392681144002E-09 - 48, 34,-3.6433567418369152E-08,-3.9284748074609782E-08, 5.0359505301791002E-09, 5.0280768764745997E-09 - 48, 35,-1.0809840538152090E-08, 1.7315051787370481E-08, 4.8138350268272999E-09, 4.8148664059107002E-09 - 48, 36,-4.4599978135483233E-08, 9.5828716675089686E-09, 4.5817070373405998E-09, 4.5795801176617996E-09 - 48, 37, 8.6617633062467059E-08,-9.6995202952304684E-09, 4.3531094481756998E-09, 4.3429920926809996E-09 - 48, 38, 2.1414062279445689E-10,-6.9569369141697913E-08, 4.1245953616188003E-09, 4.1217433916614997E-09 - 48, 39, 4.0488623371387732E-08, 3.0377216890451748E-08, 3.8712817046300004E-09, 3.8713115094175998E-09 - 48, 40,-2.8874987367371591E-08,-3.5603438761853533E-08, 3.6425082155666002E-09, 3.6433611942538998E-09 - 48, 41, 5.1180716729067438E-11, 4.0577891870018696E-09, 3.3928034203435000E-09, 3.3964307776526002E-09 - 48, 42, 1.2344088160826949E-07, 2.9193576849276589E-08, 3.1491706441176999E-09, 3.1481296425514998E-09 - 48, 43, 1.6155109205826309E-08,-1.1546514332246049E-08, 2.8913096110682001E-09, 2.8932509211199001E-09 - 48, 44,-3.9025840695115792E-08,-1.4568607931726140E-07, 2.5881587356603000E-09, 2.6125641206578002E-09 - 48, 45, 3.7749362680766513E-08, 6.3010509122220051E-08, 2.3417356387534002E-09, 2.3349401924075001E-09 - 48, 46,-4.6298426798492982E-08,-2.6796519091463439E-08, 1.9463723322822000E-09, 1.9483855424161000E-09 - 48, 47,-5.5160436640001762E-08,-2.5423993864680618E-09, 1.2506965804749000E-09, 1.2536668715835000E-09 - 48, 48, 1.2771364094735550E-07,-3.2473999675112073E-08, 9.0062428330639009E-10, 9.0967562717278009E-10 - 49, 0,-2.4331222300097099E-08, 0.0000000000000000E+00, 1.1376179369261000E-08, 0.0000000000000000E+00 - 49, 1,-4.2165875083441413E-08, 6.9326640864386959E-08, 1.1359843304109999E-08, 1.1389886009650000E-08 - 49, 2,-4.0787318991428403E-08,-1.4485001510092071E-08, 1.1363565419930001E-08, 1.1340011617514001E-08 - 49, 3,-3.5424948755476082E-08, 4.5842173279045727E-08, 1.1330082156481000E-08, 1.1346037802739000E-08 - 49, 4,-1.6375765390998910E-08,-1.7169242814551180E-08, 1.1271771427673000E-08, 1.1316080487770000E-08 - 49, 5,-1.4320990741186261E-08,-6.1774980609956594E-09, 1.1240444905649000E-08, 1.1279948552072000E-08 - 49, 6, 4.6681329671375091E-08,-3.7779336068049242E-08, 1.1087003414497000E-08, 1.1295710155314000E-08 - 49, 7, 4.5394108538369321E-08, 7.0940107173856414E-08, 1.1166156033258000E-08, 1.1102773417093000E-08 - 49, 8, 9.6480660913728943E-09, 5.0978431524877972E-08, 1.1010356398125000E-08, 1.1078317364130000E-08 - 49, 9, 1.4046426722771679E-08, 9.1241896197010133E-08, 1.0900801938244999E-08, 1.1010219687807999E-08 - 49, 10,-7.2810281401773550E-09, 5.8187739726673613E-08, 1.0842716413072999E-08, 1.0849207957937001E-08 - 49, 11, 1.3885509441237339E-08,-3.5712294965069268E-09, 1.0736515242617000E-08, 1.0717518155846000E-08 - 49, 12,-3.6000634499406313E-08, 4.3808379732011712E-08, 1.0590753497960999E-08, 1.0595124450052001E-08 - 49, 13,-1.5741494034640448E-08,-8.3960012704327122E-08, 1.0444121921213000E-08, 1.0455996788047000E-08 - 49, 14,-6.6257106865837234E-08, 1.2424856734831949E-07, 1.0283612196586001E-08, 1.0284152884216000E-08 - 49, 15,-1.7857708005444810E-08,-1.3708029051145311E-08, 1.0114064174704001E-08, 1.0132512104993000E-08 - 49, 16, 4.1730099054416340E-08, 3.2514403827523411E-08, 9.9360630176381995E-09, 9.9274827920424995E-09 - 49, 17,-2.6757273808305139E-08,-1.8634423688545310E-08, 9.7471987275226995E-09, 9.7478761893825008E-09 - 49, 18, 4.8511057886281938E-08,-5.7418690964179212E-08, 9.5533125260720992E-09, 9.5373120350762000E-09 - 49, 19,-3.7159318539262001E-09,-9.1865210832788195E-09, 9.3340512023346999E-09, 9.3365833432671004E-09 - 49, 20,-3.6481146158293942E-08,-2.8303741666207851E-08, 9.1325273095475003E-09, 9.1203090201938999E-09 - 49, 21,-3.4098631562893672E-08, 1.2958362047488291E-08, 8.8961204509837992E-09, 8.8945829004208006E-09 - 49, 22,-2.4433338455858041E-08, 2.5669387356113341E-08, 8.6886262763832996E-09, 8.6784422100524004E-09 - 49, 23, 4.1673657485320868E-08,-1.5824983437764751E-08, 8.4379056919616993E-09, 8.4333171924361996E-09 - 49, 24, 2.0141023271968491E-08, 3.4757300406351107E-08, 8.2303972345257007E-09, 8.2012809433585995E-09 - 49, 25, 1.9459015678262761E-08, 2.0867319047319191E-08, 7.9674066046015992E-09, 7.9776138892338007E-09 - 49, 26, 3.8516365473709210E-08,-2.6967572635483520E-08, 7.7447888305233002E-09, 7.7248547457273993E-09 - 49, 27,-3.9670960758801481E-08, 2.5448201469517290E-08, 7.4994672109480000E-09, 7.4845038477497994E-09 - 49, 28, 4.9114401080034791E-08, 2.4976605015527830E-09, 7.2472278623296003E-09, 7.2450107445372999E-09 - 49, 29, 1.8999776588324139E-10, 9.3229817405744326E-08, 7.0047146898463999E-09, 7.0099113831286996E-09 - 49, 30, 3.5213090409624863E-08,-3.1789697680608412E-08, 6.7560970034973003E-09, 6.7679909079842998E-09 - 49, 31, 5.2590207307942507E-08, 1.6147468022602829E-08, 6.5056204434223998E-09, 6.5261445380567996E-09 - 49, 32,-5.6956279664886933E-08,-2.8996340502817620E-08, 6.2647249865473998E-09, 6.2759487891727001E-09 - 49, 33, 2.0967857635557160E-08,-2.1375051060920139E-08, 6.0174728638906997E-09, 6.0408312414718002E-09 - 49, 34,-3.8317462542831381E-08, 4.4978986938763873E-08, 5.7924878830633999E-09, 5.7808457274777002E-09 - 49, 35,-2.4824498682147762E-09, 1.2600487390317820E-08, 5.5254733311894002E-09, 5.5235420152104998E-09 - 49, 36,-7.0874264428959780E-09,-2.5978606803980960E-08, 5.2870107116159002E-09, 5.2923336509363999E-09 - 49, 37, 7.8179489380899363E-09, 6.4085105242359808E-09, 5.0369044414813003E-09, 5.0299460885605996E-09 - 49, 38,-2.5967217013353081E-08,-8.1934023027526323E-08, 4.7830285360344001E-09, 4.7826947662573002E-09 - 49, 39,-4.9024037418913212E-08, 7.2450723366859531E-08, 4.5292568299986003E-09, 4.5284039859686997E-09 - 49, 40,-6.2710985808800299E-09,-3.8441943156038417E-08, 4.2525557913590000E-09, 4.2583760966264000E-09 - 49, 41,-1.7518318450902211E-08, 1.2185790948282380E-08, 4.0047226259989999E-09, 4.0120944980209002E-09 - 49, 42, 2.5884031949442859E-08,-1.8558769655220920E-08, 3.7321887465186000E-09, 3.7354870708992002E-09 - 49, 43,-2.7887373814444430E-08,-7.8158379468500980E-08, 3.4666188389970000E-09, 3.4680804852004001E-09 - 49, 44,-4.5700968787527781E-08,-4.3979653544466968E-08, 3.1754799563285999E-09, 3.1971139297916000E-09 - 49, 45,-7.7171616902294059E-08, 6.6129588102413759E-08, 2.8732081196556999E-09, 2.8597516693852000E-09 - 49, 46, 4.6336356352455108E-08,-8.8822820888147182E-08, 2.5794752660813999E-09, 2.5718741719767002E-09 - 49, 47,-2.9101571862767379E-08, 6.2689944831698471E-08, 2.1508586841130998E-09, 2.1515733579103002E-09 - 49, 48, 1.4530398741670231E-08, 3.8905623266605453E-08, 1.3750623808999000E-09, 1.3684604552421000E-09 - 49, 49,-1.0120468302635470E-07,-9.5048582292414182E-08, 1.0126462086787999E-09, 1.0282894004119000E-09 - 50, 0, 5.5518316183221382E-08, 0.0000000000000000E+00, 1.2612468155256000E-08, 0.0000000000000000E+00 - 50, 1,-8.9146895385472082E-09, 3.5516031480898243E-08, 1.2585260976620999E-08, 1.2615934275477001E-08 - 50, 2, 3.2929035769255971E-08, 5.1659029521868433E-08, 1.2596207503556001E-08, 1.2570950553236001E-08 - 50, 3,-2.5459565398316901E-08, 1.6146472722275330E-08, 1.2549626899989999E-08, 1.2571624935515001E-08 - 50, 4,-1.4219443845740400E-08,-9.5051567762534860E-09, 1.2492683117462001E-08, 1.2554152562496000E-08 - 50, 5,-4.3864879845661912E-08, 1.7470830600592629E-08, 1.2450794948039000E-08, 1.2501744220969000E-08 - 50, 6, 4.9009673284828942E-08,-3.3186764017648152E-08, 1.2300140517216000E-08, 1.2533637339213000E-08 - 50, 7,-1.2112504086960641E-08,-3.2168636180768721E-08, 1.2375917727130999E-08, 1.2306316228101001E-08 - 50, 8, 4.8350478013820593E-08,-3.8834493493167811E-09, 1.2226698492290001E-08, 1.2298928415111000E-08 - 50, 9, 3.9596880587795677E-08,-1.3518411780585161E-08, 1.2097370020938001E-08, 1.2208398335796000E-08 - 50, 10,-2.7647989802063939E-08,-2.4268348504982711E-08, 1.2052247506341000E-08, 1.2050404450118000E-08 - 50, 11, 1.6424064699651550E-08, 7.8566205443877140E-08, 1.1927882651679000E-08, 1.1901405587369000E-08 - 50, 12,-5.4725277778664452E-08,-1.5712145980743479E-08, 1.1777404518012999E-08, 1.1788098098493999E-08 - 50, 13, 3.1639905362049999E-09, 8.4168853825395041E-08, 1.1613046777446000E-08, 1.1633911071162001E-08 - 50, 14,-5.8205650492157793E-08, 7.6394323676880621E-08, 1.1454819144122999E-08, 1.1459061855300000E-08 - 50, 15, 3.3743293316659063E-08, 2.3929876088189739E-08, 1.1261702168767000E-08, 1.1285362518115999E-08 - 50, 16,-2.2172186144013370E-08, 4.1873047424387163E-08, 1.1091395581999000E-08, 1.1085111108443000E-08 - 50, 17,-6.6914992091900333E-09,-8.1120443195919680E-08, 1.0876626054391000E-08, 1.0873927189378000E-08 - 50, 18,-4.2969424993123567E-08, 3.9802656858974972E-08, 1.0676145891736000E-08, 1.0669740888923999E-08 - 50, 19,-4.9307579503998542E-08, 5.2911081209436636E-09, 1.0442592991032000E-08, 1.0445980381447001E-08 - 50, 20,-5.7844168993620931E-08,-1.3151307173354939E-08, 1.0224906853397001E-08, 1.0212035849105999E-08 - 50, 21,-3.9428044790698978E-08, 4.5053087140207853E-08, 9.9859825312458000E-09, 9.9790049262438992E-09 - 50, 22, 2.2083169662579771E-08, 5.5110709458346997E-09, 9.7361887700577006E-09, 9.7352078487261998E-09 - 50, 23,-2.9476540976816099E-09,-2.4761393210521561E-08, 9.4982956872500995E-09, 9.4995110894743008E-09 - 50, 24,-2.3167380785832789E-08,-2.1843917543260049E-08, 9.2440201357062005E-09, 9.2194071051455999E-09 - 50, 25,-1.5149295783642759E-08,-2.9214677020141229E-08, 8.9924127846586001E-09, 8.9980849813800994E-09 - 50, 26, 3.1525034017933167E-08,-4.3600221408865610E-09, 8.7342212064386002E-09, 8.7130134857807994E-09 - 50, 27,-1.9637544899872369E-09,-2.9879265242401342E-08, 8.4760377710076998E-09, 8.4609933894069001E-09 - 50, 28, 3.7760142501198012E-08, 2.2345745664451401E-08, 8.2066091322914004E-09, 8.1988083560779002E-09 - 50, 29,-3.3379962195498513E-08,-1.6147016813120740E-08, 7.9421688710552002E-09, 7.9353685966617998E-09 - 50, 30, 5.5626822040388273E-08,-6.4943913200274539E-08, 7.6734419666758004E-09, 7.6834380684175005E-09 - 50, 31,-1.3183561849283790E-08,-1.6651967391241049E-08, 7.4017027038639999E-09, 7.4216959967212001E-09 - 50, 32, 3.3014021158889700E-08,-1.4714356587998761E-09, 7.1356661886079996E-09, 7.1519633529866001E-09 - 50, 33,-3.0397622393980882E-08, 6.0367040462299743E-08, 6.8696019566927003E-09, 6.8903694740381998E-09 - 50, 34,-1.3876658697445199E-08, 5.4049742497374843E-08, 6.6209958933704002E-09, 6.6116360906557002E-09 - 50, 35,-1.6061381334190741E-08, 1.1444145848826811E-08, 6.3513537602618004E-09, 6.3485801285423001E-09 - 50, 36, 3.8460501215838288E-08, 2.0165185707331649E-08, 6.0665389006140998E-09, 6.0668124779912001E-09 - 50, 37,-2.2900232600051399E-08,-1.7093082517859330E-08, 5.8167434664348001E-09, 5.8072074021301003E-09 - 50, 38,-1.3077862187704181E-08, 3.5588033623319080E-08, 5.5345901821055998E-09, 5.5270463641465003E-09 - 50, 39,-3.0798378590850622E-08, 4.4230857411351352E-08, 5.2543621173034999E-09, 5.2526951397174001E-09 - 50, 40, 5.7326599062175503E-08,-3.0659745719022300E-08, 4.9773256570935999E-09, 4.9775515581820001E-09 - 50, 41,-1.0191812185934289E-08, 9.3692087639537567E-09, 4.6780253086252997E-09, 4.6844040319575999E-09 - 50, 42,-1.3088127341329980E-08,-4.2668539015090678E-08, 4.4067259307626001E-09, 4.4123946598855000E-09 - 50, 43,-6.4205845483577124E-08, 2.8128874072328060E-08, 4.1074892888302001E-09, 4.1096505415733998E-09 - 50, 44, 1.7331881280579619E-08, 4.5832344442830367E-08, 3.8218082246977001E-09, 3.8188993954170004E-09 - 50, 45,-5.4902599234380482E-08, 2.8644337935898591E-08, 3.5096507956676999E-09, 3.5082207062754999E-09 - 50, 46, 7.7656580301768369E-08,-1.3772036351399739E-10, 3.1591518045200002E-09, 3.1541705664860002E-09 - 50, 47,-8.3828906326495605E-08,-3.1106957533502582E-08, 2.8480769154245000E-09, 2.8436174225307999E-09 - 50, 48, 8.5158050861342004E-08, 6.3548965974157246E-09, 2.3573487915759000E-09, 2.3519304944844998E-09 - 50, 49, 2.7467922569274171E-08,-1.3942102845328860E-08, 1.4738128149217000E-09, 1.4571884878495001E-09 - 50, 50,-6.1739297428029809E-08, 7.7790588732529842E-08, 1.1460656731844000E-09, 1.1547962420840001E-09 - 51, 0,-1.4040563887360571E-08, 0.0000000000000000E+00, 1.3945716248327000E-08, 0.0000000000000000E+00 - 51, 1,-2.5807225837876931E-08,-3.9502450870680833E-08, 1.3922458161797000E-08, 1.3958685077531000E-08 - 51, 2,-1.0426422018221650E-08,-2.4233504694116291E-08, 1.3931888682031000E-08, 1.3896683671589999E-08 - 51, 3, 4.1289145072110287E-08,-3.4950981016614383E-08, 1.3887312785153001E-08, 1.3907996037498000E-08 - 51, 4, 3.1731977629578743E-08,-1.1510435654774860E-08, 1.3826095874057000E-08, 1.3875429080391999E-08 - 51, 5,-2.0374747295268731E-08, 1.8336636626905089E-08, 1.3783494558751000E-08, 1.3830731541838000E-08 - 51, 6,-1.1842505205910181E-08, 1.8899193568128690E-08, 1.3612179725785001E-08, 1.3865360316225000E-08 - 51, 7,-4.6910135166158802E-08,-3.9728889660761183E-09, 1.3702321141793999E-08, 1.3626172324043000E-08 - 51, 8,-5.3334369559299073E-08, 4.1863687795779367E-09, 1.3532775415876000E-08, 1.3617044485129000E-08 - 51, 9, 4.1061575767198101E-09,-3.4744177361346277E-08, 1.3407462144696000E-08, 1.3530653388556999E-08 - 51, 10,-2.4682452132809930E-08,-5.0630330784990428E-08, 1.3342909491249000E-08, 1.3352462795288000E-08 - 51, 11,-1.9516148426216509E-08,-2.7020393197178559E-09, 1.3231266120217001E-08, 1.3209250839618001E-08 - 51, 12, 2.8497967307574241E-08,-6.4052101808591779E-08, 1.3062073616355000E-08, 1.3066243377362000E-08 - 51, 13,-5.1042199599653271E-08, 6.8485976934800199E-08, 1.2901219292467000E-08, 1.2923574598862000E-08 - 51, 14, 2.4822824666958461E-08,-3.7680007439771747E-08, 1.2720036429506000E-08, 1.2730543109734999E-08 - 51, 15, 5.6901186025051113E-08, 3.3954835076779923E-08, 1.2529993368020000E-08, 1.2552197013346000E-08 - 51, 16, 4.7821133590500438E-08,-1.0500741483096220E-08, 1.2338485198274999E-08, 1.2333138069272000E-08 - 51, 17, 2.0888245398168282E-08,-2.9670983222235501E-08, 1.2119471783788000E-08, 1.2129945644914001E-08 - 51, 18,-3.3494304644702041E-08, 2.7629980493199201E-08, 1.1895508326657000E-08, 1.1890062846390000E-08 - 51, 19,-2.4963244811027319E-08, 2.2213790963872059E-08, 1.1665092293416000E-08, 1.1670762022595000E-08 - 51, 20,-7.0543879987064577E-09, 1.1757663548559910E-08, 1.1419209107276000E-08, 1.1408285323303999E-08 - 51, 21, 5.5155275750198432E-09, 1.6349508815493969E-08, 1.1172357414194000E-08, 1.1170509238601001E-08 - 51, 22, 4.6871183054180362E-08, 2.4760657365816029E-08, 1.0912166471584001E-08, 1.0905998928139999E-08 - 51, 23,-4.7674186708854723E-08,-1.4813259309489511E-09, 1.0648749528536000E-08, 1.0642256541364000E-08 - 51, 24,-6.4295535384703913E-09,-5.1854019646573983E-08, 1.0399350603098000E-08, 1.0367394097318999E-08 - 51, 25,-4.7085096350693112E-08,-3.7467561377730562E-08, 1.0088974784827999E-08, 1.0104216837876000E-08 - 51, 26, 7.4858689380451180E-09, 5.3314459228113828E-09, 9.8515933754459994E-09, 9.8230601461401997E-09 - 51, 27,-1.3505131178303649E-08,-1.9852236137281241E-08, 9.5487814060388997E-09, 9.5324225138090007E-09 - 51, 28,-3.5192140516645377E-08, 1.6920317449100161E-08, 9.2680049264411994E-09, 9.2685540030330995E-09 - 51, 29,-3.5806469551034971E-08,-2.0193105116909719E-08, 8.9749842700656001E-09, 8.9782932053812005E-09 - 51, 30,-6.8474702779974490E-10,-2.5076180409702710E-08, 8.6836060320119992E-09, 8.7046577680005008E-09 - 51, 31,-2.7433494685854750E-08,-6.7320675345394953E-08, 8.4036354629553002E-09, 8.4189743297253003E-09 - 51, 32, 3.2706290050604787E-08, 4.3010231385512021E-08, 8.1152479897636993E-09, 8.1239423267122993E-09 - 51, 33, 1.5338866864197060E-08, 8.2100122128362807E-09, 7.8157061477371996E-09, 7.8457333589560001E-09 - 51, 34, 4.7999191903997334E-09, 3.6916920243981263E-08, 7.5488699445890007E-09, 7.5385089402637007E-09 - 51, 35, 6.3995124607836612E-08,-4.1549355480916761E-09, 7.2615152692307003E-09, 7.2581348021912001E-09 - 51, 36, 6.3904682931446001E-09,-3.7784328066977954E-09, 6.9659974923164998E-09, 6.9673662595021998E-09 - 51, 37, 1.5992212894221629E-08, 1.1973753049613980E-08, 6.6653620491225004E-09, 6.6562132996134001E-09 - 51, 38,-3.5635931632274437E-08, 5.8339751611619868E-08, 6.3842849686042996E-09, 6.3874017042059001E-09 - 51, 39, 2.4002563695187451E-08,-6.6061487225216589E-08, 6.0746526455409000E-09, 6.0668954088178996E-09 - 51, 40, 7.8430934246473202E-08,-1.1274386844345250E-08, 5.7747862268753000E-09, 5.7717580395886996E-09 - 51, 41,-5.2848798399566121E-08,-3.2427449885975748E-08, 5.4653006214722003E-09, 5.4753314324117000E-09 - 51, 42, 2.0655749951342261E-09, 2.5624789120520449E-08, 5.1456306816257998E-09, 5.1470935699979000E-09 - 51, 43,-3.4224859595931832E-08, 1.3403953523998609E-08, 4.8496545262178000E-09, 4.8523150632151000E-09 - 51, 44, 9.3323835413447984E-08,-1.2249138182468249E-09, 4.5165986241474000E-09, 4.5273585948158001E-09 - 51, 45,-3.5977175181172987E-08, 3.4007573246534718E-09, 4.2069357100146999E-09, 4.2070319493107000E-09 - 51, 46, 4.5427812368221218E-08, 9.9185446152200238E-09, 3.8633560279165000E-09, 3.8656984788233999E-09 - 51, 47,-2.0109373300981450E-08,-6.1566528039729332E-08, 3.4846145725162999E-09, 3.4809278346783999E-09 - 51, 48, 3.9671130193403781E-08, 4.8539180962100123E-08, 3.1360518635293999E-09, 3.1356668144526998E-09 - 51, 49,-9.6295313432649808E-09,-4.8216941875681473E-08, 2.5959235180557999E-09, 2.5945687918626001E-09 - 51, 50,-8.2743608048787692E-08,-9.2491009408480114E-09, 1.7341447906936999E-09, 1.7134538211476000E-09 - 51, 51, 6.5513195149778882E-08,-1.2609497913451269E-08, 1.2981863627650000E-09, 1.2829247811058000E-09 - 52, 0,-4.6998043401862638E-08, 0.0000000000000000E+00, 1.5404544057491000E-08, 0.0000000000000000E+00 - 52, 1,-7.3125989249548971E-08, 1.3193446981940500E-08, 1.5355458908091001E-08, 1.5380442371931001E-08 - 52, 2,-3.9162440267123392E-08,-3.0872193571645832E-08, 1.5377486327465001E-08, 1.5355649373651001E-08 - 52, 3,-1.0380686456439971E-08,-3.7915243521455307E-08, 1.5308886829942001E-08, 1.5337388338010001E-08 - 52, 4, 2.9543026038369640E-08,-1.0176165347588820E-08, 1.5264759325287000E-08, 1.5332139407892001E-08 - 52, 5, 2.1364265555602461E-08,-3.3221184222899608E-08, 1.5204789197676001E-08, 1.5252091581735001E-08 - 52, 6,-5.0212916735820923E-09,-3.8084824156320584E-09, 1.5044532727070000E-08, 1.5312244123198001E-08 - 52, 7, 2.4097352531784110E-09, 1.8867550334226561E-08, 1.5118752720155999E-08, 1.5044261558046001E-08 - 52, 8,-2.4699904721744941E-08, 2.5120589995064679E-08, 1.4955748942836001E-08, 1.5046176846718001E-08 - 52, 9,-6.2582358849204104E-08, 2.6980792694704350E-08, 1.4814258072064000E-08, 1.4945554935076001E-08 - 52, 10, 1.9006405217994720E-08, 2.9767073874791761E-09, 1.4759758841719000E-08, 1.4761826850137000E-08 - 52, 11,-7.6623875539400899E-08,-3.5848238837530522E-08, 1.4634272094994999E-08, 1.4603339772872001E-08 - 52, 12, 3.2480711743471650E-08,-4.8000496597705417E-09, 1.4462695526376000E-08, 1.4470007133795000E-08 - 52, 13,-4.5011134897040232E-08,-2.8345539286740270E-08, 1.4277867055759001E-08, 1.4307202994433001E-08 - 52, 14, 4.5519445887034017E-08,-3.1377455745158911E-08, 1.4109590931620999E-08, 1.4115666996183999E-08 - 52, 15,-1.5734315541709421E-08,-2.7031519649153559E-08, 1.3888253810004000E-08, 1.3915623331237000E-08 - 52, 16, 4.9300208757767551E-08,-4.9808563646618357E-08, 1.3699828824666999E-08, 1.3700043501956000E-08 - 52, 17, 6.4400954141947939E-09,-6.1332286620946051E-09, 1.3468956240437000E-08, 1.3466801618682999E-08 - 52, 18,-1.8891032342919400E-08,-2.8948337399279530E-08, 1.3242661615892000E-08, 1.3225512501545000E-08 - 52, 19, 4.6431841068943923E-08,-5.1366181075607742E-08, 1.2984041640578001E-08, 1.2981668963135001E-08 - 52, 20, 4.2906868844164661E-08,-1.4568512056101220E-08, 1.2741604577147000E-08, 1.2721476335797000E-08 - 52, 21, 4.3849106348132181E-08,-2.7487078212542670E-08, 1.2460761994191000E-08, 1.2452199157794000E-08 - 52, 22, 2.2129641912034591E-08, 6.0857862853891006E-09, 1.2196405554660000E-08, 1.2192243092300000E-08 - 52, 23,-2.7291304341374759E-08,-2.6816692934692270E-08, 1.1906471267927000E-08, 1.1902359551474000E-08 - 52, 24,-5.7325459793046452E-08, 2.0998148353659600E-08, 1.1640675458794000E-08, 1.1611380651188000E-08 - 52, 25,-8.8543516677016567E-09,-1.6492631853621202E-08, 1.1330775831498001E-08, 1.1341739058568000E-08 - 52, 26,-2.8298020291289359E-08, 4.0144029279420067E-08, 1.1041952326965000E-08, 1.1017870001994000E-08 - 52, 27,-4.8630297725006997E-09, 1.6679124128243080E-09, 1.0759961518346000E-08, 1.0731542367285000E-08 - 52, 28,-5.4708722719589192E-08, 1.2937493059482590E-08, 1.0430582232006999E-08, 1.0421081663719000E-08 - 52, 29, 9.2450643784572275E-09, 4.4347180119491128E-08, 1.0135478407770999E-08, 1.0131438194527000E-08 - 52, 30,-2.5366126298359571E-08, 5.1725472523159881E-09, 9.8084404403930001E-09, 9.8249622642011996E-09 - 52, 31, 1.3767739772072860E-08, 1.8768516178763520E-08, 9.5053538318778997E-09, 9.5257153357151994E-09 - 52, 32, 5.4227866572063518E-09, 4.5000864495489661E-08, 9.1965356789198998E-09, 9.2149514276115994E-09 - 52, 33, 3.2968499233536213E-08,-8.6751857754775080E-10, 8.8789481015368003E-09, 8.9070543959285003E-09 - 52, 34, 2.4486825471471770E-08,-2.3811771190500061E-08, 8.5869415131601993E-09, 8.5708923118769006E-09 - 52, 35, 1.0527840310867370E-08,-3.3082861291913313E-08, 8.2705281040128998E-09, 8.2723914962043993E-09 - 52, 36,-1.8757070788135729E-08,-1.2182129829691790E-08, 7.9595412575527000E-09, 7.9625629779519007E-09 - 52, 37, 1.3727245216935850E-08,-1.0521111213145380E-08, 7.6498053575904995E-09, 7.6330385306567994E-09 - 52, 38,-4.4175843590176847E-08,-1.8981307305971281E-08, 7.3140427849498001E-09, 7.3109488864421999E-09 - 52, 39, 3.1903296115492062E-08,-4.7119939331212922E-08, 7.0086401127670004E-09, 7.0059690868939999E-09 - 52, 40,-4.7331716245612263E-08, 3.8104193011362067E-08, 6.6647314412094003E-09, 6.6666998073779999E-09 - 52, 41,-4.5447411640280422E-08,-7.0018698154348914E-08, 6.3399951174455999E-09, 6.3485069467117001E-09 - 52, 42,-5.3922308897598753E-08, 8.0875190816934851E-08, 6.0094873435955997E-09, 6.0118296370318997E-09 - 52, 43, 4.6428064903514002E-08,-6.3339090052245010E-08, 5.6641678216327004E-09, 5.6539673572952004E-09 - 52, 44, 4.4192270666107401E-08, 1.8836035822316309E-08, 5.3399792069535997E-09, 5.3353590793936999E-09 - 52, 45,-4.8120400099369153E-08,-2.7835380437460419E-08, 4.9729589830321999E-09, 4.9776257999947001E-09 - 52, 46,-6.6899199735992966E-09,-6.1489118191177341E-08, 4.6296076136027004E-09, 4.6399820733583999E-09 - 52, 47, 1.7392768758844169E-08,-1.9114839621822230E-08, 4.2593407900264002E-09, 4.2573999643498000E-09 - 52, 48,-6.8888508194770691E-08, 5.2472384269907173E-08, 3.8322444326906999E-09, 3.8377341665740002E-09 - 52, 49, 2.0389298546465718E-08,-4.9792721449842987E-08, 3.4626303645120002E-09, 3.4692936678980002E-09 - 52, 50,-8.1668415940390269E-08, 4.1129950546979502E-08, 2.8991974827547001E-09, 2.8845648423191000E-09 - 52, 51, 2.7881484013889271E-08, 6.4810513354435554E-08, 1.8509552229195999E-09, 1.8541412713019000E-09 - 52, 52,-2.7400623323859001E-08,-6.5761673085644410E-08, 1.4267747922295000E-09, 1.4206157800908001E-09 - 53, 0,-1.6691019592369758E-08, 0.0000000000000000E+00, 1.6927106251376000E-08, 0.0000000000000000E+00 - 53, 1,-1.5666102397789839E-08, 3.5566327968041421E-08, 1.6910938362479999E-08, 1.6941301068388000E-08 - 53, 2,-1.4954443187894230E-09, 2.6854082131425410E-08, 1.6908066954479000E-08, 1.6862938489965001E-08 - 53, 3,-3.2251236580078892E-08, 6.5728992936600433E-09, 1.6863078134422999E-08, 1.6890562337403999E-08 - 53, 4,-9.3863432195234914E-09, 3.4991411194196657E-08, 1.6781329807560000E-08, 1.6845565041945999E-08 - 53, 5,-3.0712799637772112E-08,-1.0582243302834779E-08, 1.6752310920891000E-08, 1.6802167896249999E-08 - 53, 6,-2.3553763041211711E-08,-3.9126633818855677E-08, 1.6544157519902000E-08, 1.6831305948303000E-08 - 53, 7, 4.0104995854707551E-08,-2.4112294204052810E-08, 1.6668794470065000E-08, 1.6572664574909999E-08 - 53, 8, 2.6431132552206969E-08, 1.3608243955304149E-08, 1.6457339225048000E-08, 1.6546754588499000E-08 - 53, 9, 3.7004888408355613E-08, 1.3661920051187479E-09, 1.6335772058480000E-08, 1.6479280263643999E-08 - 53, 10, 5.1230315465902433E-08, 2.7687070539827611E-08, 1.6247981061418000E-08, 1.6262873849342999E-08 - 53, 11, 1.8340782129712009E-09,-1.0336489964999799E-08, 1.6143034972543998E-08, 1.6111193514044999E-08 - 53, 12,-6.2052775075369532E-08, 6.8850250959663399E-08, 1.5946273175166999E-08, 1.5959521297399999E-08 - 53, 13, 2.0460460601186171E-08,-6.1855946586193251E-08, 1.5776240198435999E-08, 1.5790048930125999E-08 - 53, 14,-6.2425954044582042E-09, 1.9837492840787200E-08, 1.5571195897709000E-08, 1.5596068212924000E-08 - 53, 15,-6.9601572151115273E-09, 7.9164254347134379E-09, 1.5368529270133000E-08, 1.5388188739381001E-08 - 53, 16,-9.2892990909516953E-09,-6.9883458666884686E-09, 1.5152139445960001E-08, 1.5148976128229000E-08 - 53, 17,-4.8668807783869368E-09, 1.1532899564587510E-08, 1.4925076052202999E-08, 1.4925648295847000E-08 - 53, 18,-2.0537468213377050E-08,-6.1370744313838980E-08, 1.4677497871454000E-08, 1.4654445390924000E-08 - 53, 19, 9.6975161538131114E-09,-2.9024852370221798E-10, 1.4418681162664000E-08, 1.4410420982192001E-08 - 53, 20, 2.2703555087580229E-08,-1.9281952576269900E-08, 1.4148451313920001E-08, 1.4131204577637000E-08 - 53, 21, 2.7869736922404999E-08,-3.3167635854992492E-08, 1.3863981647431000E-08, 1.3864188251345000E-08 - 53, 22,-1.5409270683423080E-08,-2.9204187712910168E-08, 1.3571785908323001E-08, 1.3568612916676000E-08 - 53, 23, 7.2927962340971284E-09, 2.5346054928181650E-08, 1.3286421038518000E-08, 1.3282409197042000E-08 - 53, 24,-1.1673750352504509E-08,-9.4429903721035365E-09, 1.2993091999689000E-08, 1.2956350133522999E-08 - 53, 25,-1.5844787546178970E-09, 3.7764159239042003E-08, 1.2665994448452001E-08, 1.2686319172582000E-08 - 53, 26, 7.8433956440027791E-09, 2.8567991100722241E-08, 1.2370775676354000E-08, 1.2339647642680999E-08 - 53, 27, 2.8723603650376988E-09, 2.3794926127698481E-08, 1.2042953298298000E-08, 1.2023592688317000E-08 - 53, 28,-1.5065759696539081E-08, 4.3597313937908353E-08, 1.1728051829145999E-08, 1.1718409433701000E-08 - 53, 29, 4.5722543860343827E-08,-1.2570866714267290E-08, 1.1378968518334000E-08, 1.1382787665965000E-08 - 53, 30,-3.2657372209199011E-08, 1.1295809029677430E-08, 1.1056853467901001E-08, 1.1079036185759000E-08 - 53, 31, 6.2565668631466469E-08, 4.2217634282961531E-08, 1.0713107396564000E-08, 1.0737768485607000E-08 - 53, 32, 2.8766150712279080E-08, 2.0837229500576491E-09, 1.0396425000506000E-08, 1.0410230206497000E-08 - 53, 33,-1.8879650637938542E-08, 2.0658673297021081E-08, 1.0051639522024000E-08, 1.0083605061998999E-08 - 53, 34,-4.1190753717460562E-08,-4.8163347805704682E-08, 9.7337769044394996E-09, 9.7238763506490003E-09 - 53, 35,-5.5230420819605743E-08, 1.6631180477438700E-10, 9.3958171746913006E-09, 9.3899321439999999E-09 - 53, 36, 1.2979922422761800E-08,-1.1324425121402990E-08, 9.0617342908561999E-09, 9.0592573081305001E-09 - 53, 37,-3.2126589158522942E-08,-8.0698840294973879E-09, 8.7339429563066002E-09, 8.7166715336931996E-09 - 53, 38, 2.1769774829099241E-08, 3.8218859146666479E-09, 8.3796778835900993E-09, 8.3751673621492005E-09 - 53, 39, 3.4298154117308892E-08,-9.2248762677727159E-09, 8.0214680810334995E-09, 8.0134047177718997E-09 - 53, 40,-8.0785510927542902E-08, 1.1507172316275580E-08, 7.6882464256036006E-09, 7.6893778703007004E-09 - 53, 41, 7.1393641780884312E-08, 3.7640346461293821E-08, 7.3089652919441004E-09, 7.3209813195835999E-09 - 53, 42,-4.9654165937185422E-08,-1.9909290496673799E-09, 6.9660439676264000E-09, 6.9676531280277000E-09 - 53, 43, 1.1101972682468910E-07, 8.8740828524389599E-09, 6.6001721595170999E-09, 6.6018269270909000E-09 - 53, 44,-3.5459649991908157E-08, 1.5132009128550950E-08, 6.2222653172873002E-09, 6.2228974174248997E-09 - 53, 45, 4.9121650219101452E-08,-2.6337973614609719E-08, 5.8668514208298003E-09, 5.8704653074254002E-09 - 53, 46,-2.0202586161871480E-08, 3.0033066282118739E-09, 5.4707664378853003E-09, 5.4798988195103998E-09 - 53, 47, 3.5392682181632917E-08, 1.7741904918560880E-08, 5.1012145933616996E-09, 5.1069694251553003E-09 - 53, 48,-6.7414254293143934E-08,-3.3033971734706463E-08, 4.6953855636049999E-09, 4.6820383315528003E-09 - 53, 49, 8.4262361165395812E-08, 3.5386699332998772E-08, 4.2322433676568998E-09, 4.2302224734801001E-09 - 53, 50,-6.1889922005387833E-08, 7.4290427012850204E-09, 3.8203131176994997E-09, 3.8324320650784004E-09 - 53, 51, 3.6218637647528652E-08, 9.9883624817707802E-09, 3.1677051483488000E-09, 3.1630006035320998E-09 - 53, 52, 9.4627620407400127E-08,-6.7933921620453271E-08, 2.0294236591710000E-09, 2.0052670215972002E-09 - 53, 53,-6.4701085775140611E-08, 3.0103050032314503E-08, 1.5752857413314999E-09, 1.5789111023553999E-09 - 54, 0, 4.3926231289027848E-08, 0.0000000000000000E+00, 1.8569097828866999E-08, 0.0000000000000000E+00 - 54, 1, 1.2008783927256970E-08, 4.2223927127308491E-08, 1.8527204101258001E-08, 1.8553643818324999E-08 - 54, 2, 7.0721778187372190E-08, 1.4556630222110619E-08, 1.8540335114558999E-08, 1.8502233992147999E-08 - 54, 3, 5.0819804558331778E-08, 6.3753171488066512E-08, 1.8468396854807000E-08, 1.8503641607654001E-08 - 54, 4,-4.5444717490999721E-08, 2.4100147675497140E-08, 1.8405381281501001E-08, 1.8484675604941999E-08 - 54, 5,-1.7671380390997081E-08, 3.1896528569287528E-09, 1.8353669811885001E-08, 1.8405221521708000E-08 - 54, 6,-4.6023216069339603E-08, 2.6165267500894481E-08, 1.8159968009433000E-08, 1.8469271654298000E-08 - 54, 7, 4.3196250016040070E-09,-4.9795966203032011E-08, 1.8256723018717999E-08, 1.8170611671806999E-08 - 54, 8,-2.8565227288765071E-09, 9.0807265433742420E-09, 1.8069605217972999E-08, 1.8174360696093000E-08 - 54, 9, 3.5778312217087382E-08,-2.5491524087210481E-08, 1.7915488649706000E-08, 1.8062762202385001E-08 - 54, 10,-9.0250277755164568E-09, 1.3219702757062211E-08, 1.7866604256746999E-08, 1.7868136525361001E-08 - 54, 11, 5.2751760058383094E-09, 6.8078249497292209E-10, 1.7714789304404999E-08, 1.7675607996958000E-08 - 54, 12,-3.8354913258106977E-08,-4.6295397867577959E-09, 1.7548712785658999E-08, 1.7555205782096999E-08 - 54, 13, 3.7734928592909871E-08, 2.7734951045143369E-08, 1.7333201605602999E-08, 1.7354756221362999E-08 - 54, 14,-4.1300802134345663E-08, 1.9162059350642602E-08, 1.7157211518801000E-08, 1.7164068479365001E-08 - 54, 15, 6.3531836160298633E-09, 3.1690672606606553E-08, 1.6911267488018000E-08, 1.6947969076741000E-08 - 54, 16,-2.6533856826950400E-08, 3.2689845798466007E-08, 1.6710326588823999E-08, 1.6705386710552001E-08 - 54, 17, 6.3847421477796457E-09, 3.5476438521018430E-08, 1.6457274807736999E-08, 1.6458703877052000E-08 - 54, 18, 2.9457193534100790E-08,-9.0053222874719414E-09, 1.6216825540185001E-08, 1.6196779236924001E-08 - 54, 19,-6.2958073111271251E-08,-1.0892345287920950E-08, 1.5927565156457999E-08, 1.5929283556617002E-08 - 54, 20, 3.1377467824839969E-09, 1.5773711905280180E-08, 1.5664792427287999E-08, 1.5645921355492999E-08 - 54, 21,-6.2671446988217480E-08, 3.4691725769198473E-08, 1.5360103902737999E-08, 1.5352981583470998E-08 - 54, 22,-4.1647315232253212E-08, 3.0534599352053147E-08, 1.5061505393082001E-08, 1.5060710829965000E-08 - 54, 23, 2.2193135458412811E-08,-2.5389799084360941E-08, 1.4746906757499000E-08, 1.4746863669600999E-08 - 54, 24, 2.5841093437669669E-08,-2.4814116718005610E-08, 1.4457637138008001E-08, 1.4423808349317999E-08 - 54, 25, 2.0868551734792011E-08, 1.7696057564614001E-08, 1.4097723194406999E-08, 1.4123875056221001E-08 - 54, 26, 5.0548419143542632E-08,-2.1998531847733900E-08, 1.3804742772501000E-08, 1.3772005749499999E-08 - 54, 27, 1.5357905368815240E-09, 1.6401679484838179E-08, 1.3456309368959000E-08, 1.3423789838842999E-08 - 54, 28, 1.0042633471445060E-08,-3.6734959695416671E-08, 1.3108471120688001E-08, 1.3104099456414000E-08 - 54, 29,-2.2556636069232699E-08,-1.5692683795595120E-09, 1.2767976135315000E-08, 1.2765804538491001E-08 - 54, 30, 3.7943330926536668E-08,-2.3211084483853620E-08, 1.2392261870229001E-08, 1.2415513552097999E-08 - 54, 31, 1.7965066655535941E-08,-2.1952950141070021E-08, 1.2060428024627001E-08, 1.2079556476534999E-08 - 54, 32, 3.8241477061397297E-08,-8.4353849684386816E-09, 1.1687641752715000E-08, 1.1716439493583001E-08 - 54, 33,-3.0495857681777542E-08, 1.7699225827790901E-08, 1.1343265730240001E-08, 1.1378775162033000E-08 - 54, 34,-4.4975769991237369E-09,-6.9519469040710451E-09, 1.0997165580651000E-08, 1.0990368233738000E-08 - 54, 35,-1.5923034361737749E-08, 4.4077387687169632E-08, 1.0635705907618000E-08, 1.0637549971156000E-08 - 54, 36,-1.0143969383951411E-08, 2.2585998261804580E-08, 1.0267418791527999E-08, 1.0279539616608001E-08 - 54, 37,-1.5926314244131182E-08,-2.6543053317284340E-08, 9.9282286380283992E-09, 9.9113253618795996E-09 - 54, 38, 7.0379785462622013E-09, 1.6539475858318622E-08, 9.5600542349769997E-09, 9.5481115751477007E-09 - 54, 39, 2.2843731988396330E-08, 4.2417810100264172E-08, 9.1708027824298995E-09, 9.1668072760559007E-09 - 54, 40, 1.6737942477516070E-08,-1.0395016022966039E-08, 8.7886432221734008E-09, 8.7875437783335995E-09 - 54, 41, 3.3759743587776683E-08, 5.2252867723500112E-08, 8.4213967693429996E-09, 8.4392032220404001E-09 - 54, 42, 6.5788129576289293E-09,-6.4474262502821200E-08, 8.0223514685597998E-09, 8.0259706560391000E-09 - 54, 43, 4.8989325907204272E-08, 5.0841173313952951E-08, 7.6475294472649999E-09, 7.6476070808533005E-09 - 54, 44,-3.7290913064637728E-08,-4.5201177079201752E-08, 7.2634414894627997E-09, 7.2336291296214003E-09 - 54, 45, 5.4855305925634923E-08, 9.0262906458967249E-10, 6.8356402847410996E-09, 6.8386095805118999E-09 - 54, 46,-4.0292437380047853E-08,-3.9202232531853606E-09, 6.4468827503164003E-09, 6.4592892132743998E-09 - 54, 47, 4.1085250062970322E-08, 3.3799518536846038E-08, 6.0152716273831997E-09, 6.0277930406485003E-09 - 54, 48,-2.0721760942411529E-08, 1.3236234765934920E-08, 5.6321854185535999E-09, 5.6095572808763996E-09 - 54, 49,-8.8057281168314134E-11, 4.0803002647829001E-08, 5.1691979743948004E-09, 5.1651473620730998E-09 - 54, 50,-1.9754983881252422E-08,-8.0973487380146494E-08, 4.6603293034134997E-09, 4.6657131000811999E-09 - 54, 51, 1.8546289446097100E-08, 4.9335150932487057E-08, 4.2219595354321997E-09, 4.2103802030160003E-09 - 54, 52, 5.6815670289527632E-08,-3.4894938614014332E-08, 3.4400218030889001E-09, 3.4528656872436000E-09 - 54, 53,-7.4771215181302731E-08,-3.0461013444809292E-08, 2.2060144548953000E-09, 2.1906629866961999E-09 - 54, 54, 3.7194903778081370E-08, 4.8179833074196091E-08, 1.7488972605114001E-09, 1.7603759371869000E-09 - 55, 0, 1.4848553196557169E-08, 0.0000000000000000E+00, 2.0255852162828001E-08, 0.0000000000000000E+00 - 55, 1,-1.4058680232162451E-08,-3.4121066246251741E-08, 2.0223726645290999E-08, 2.0246889653405001E-08 - 55, 2, 2.6671590937996058E-10,-5.2375271382274752E-08, 2.0213023301817001E-08, 2.0193107413634000E-08 - 55, 3, 1.7479439344560511E-08, 1.7671698679507189E-08, 2.0162159047806999E-08, 2.0195997921686001E-08 - 55, 4,-1.6232198893426281E-08,-3.0768856418781013E-08, 2.0090223154436002E-08, 2.0154461504795999E-08 - 55, 5, 1.2491194101443860E-09, 8.3006511625410902E-09, 2.0041945930315001E-08, 2.0086883670250000E-08 - 55, 6, 2.9546032548126461E-08, 2.7956920139482280E-08, 1.9825853918638999E-08, 2.0141294901611002E-08 - 55, 7,-6.2934723803025273E-09, 5.5289281214079848E-09, 1.9941849102990001E-08, 1.9846067752288999E-08 - 55, 8,-2.8699604371125241E-08, 3.3020725983130329E-09, 1.9728569951087999E-08, 1.9834614494127001E-08 - 55, 9,-2.0114994683042280E-10, 2.8976341698270351E-08, 1.9590335533256001E-08, 1.9731144875840001E-08 - 55, 10,-4.1892554273674942E-08,-2.7527005140683249E-08, 1.9509826615854000E-08, 1.9521090044603000E-08 - 55, 11,-3.1690494713281462E-08, 1.2398804275623759E-08, 1.9374846063292000E-08, 1.9347350665292999E-08 - 55, 12, 2.5812554277739821E-08,-6.1806826407082310E-08, 1.9175910239386000E-08, 1.9189962450077001E-08 - 55, 13,-2.2304345023907789E-08, 2.8416596899984020E-08, 1.8991103575785000E-08, 1.9017353490529999E-08 - 55, 14, 1.6368014467583660E-08,-9.9109789184268809E-09, 1.8770783265897000E-08, 1.8786507551038002E-08 - 55, 15,-7.9280371320809353E-10, 5.7235755682308870E-09, 1.8565520315429000E-08, 1.8587794789385001E-08 - 55, 16, 3.7519218913759983E-08, 2.4135902934264331E-09, 1.8323852990558000E-08, 1.8312473319311000E-08 - 55, 17, 3.0564895086805630E-08,-1.6781151368446430E-08, 1.8082952022139001E-08, 1.8085509187686000E-08 - 55, 18, 6.7344323797808717E-09, 4.1021444845145523E-08, 1.7816456407324001E-08, 1.7799021249295001E-08 - 55, 19,-2.2214234476372539E-08, 2.0680173720233402E-09, 1.7536413466870000E-08, 1.7542332267924001E-08 - 55, 20, 1.6052408128393140E-08, 2.1700891663986469E-08, 1.7244237649420000E-08, 1.7230464691730001E-08 - 55, 21, 1.8403081882094350E-08, 8.2710599665939083E-09, 1.6949797904022999E-08, 1.6945316908438001E-08 - 55, 22, 8.3646738693678966E-09, 6.1478809221210555E-08, 1.6632448060628001E-08, 1.6625203132026999E-08 - 55, 23, 8.2019882925412967E-09, 1.9500420525503821E-09, 1.6318284764157998E-08, 1.6307617685641999E-08 - 55, 24, 2.1085316689551190E-08, 4.7283409137414814E-09, 1.6001896439118000E-08, 1.5966013746327000E-08 - 55, 25,-2.4855216013693911E-08,-4.4899706894485623E-08, 1.5640748640691000E-08, 1.5669137750885999E-08 - 55, 26, 2.3109032210127469E-08, 1.3427998748024830E-08, 1.5323358244500999E-08, 1.5284045224914999E-08 - 55, 27,-3.3069769476958191E-08,-4.6824943394393022E-08, 1.4975034817417000E-08, 1.4942665362973999E-08 - 55, 28,-1.4806325832099880E-08,-1.2152113069977260E-08, 1.4592238128570000E-08, 1.4590902039271999E-08 - 55, 29,-6.8087291176056211E-08,-3.4052741707478109E-09, 1.4241440755162000E-08, 1.4243040173908001E-08 - 55, 30,-1.7703627645540731E-08, 1.2777650262624090E-08, 1.3864585499441000E-08, 1.3881434381983000E-08 - 55, 31,-2.2530898560832640E-08,-5.3537321388998152E-08, 1.3478716267959000E-08, 1.3509100201105000E-08 - 55, 32,-2.4260286387374811E-08,-2.6321663663961160E-08, 1.3128748591137000E-08, 1.3146487826332000E-08 - 55, 33,-2.2815566601572171E-08, 1.7596417593771229E-08, 1.2724287956589000E-08, 1.2767950955395999E-08 - 55, 34, 2.7723247133203460E-08, 5.8653335485771483E-08, 1.2388043059693000E-08, 1.2374273423599000E-08 - 55, 35, 1.3326506650698069E-08, 8.0248811749605771E-08, 1.1990237387740000E-08, 1.1989355532615000E-08 - 55, 36, 3.0284425469817513E-08, 2.5544260213064941E-08, 1.1608954210159001E-08, 1.1607738598316001E-08 - 55, 37,-4.9923156277013099E-09, 1.4389704726578880E-08, 1.1230750840839000E-08, 1.1212949246840999E-08 - 55, 38, 5.6736176497116823E-08,-2.7966911140070791E-08, 1.0847020109660000E-08, 1.0839160377946999E-08 - 55, 39, 1.1734236676501950E-08,-2.2729261446815979E-08, 1.0445571651644000E-08, 1.0435233527582000E-08 - 55, 40, 1.3063950959503171E-08,-6.3495321759772328E-09, 1.0027440587830999E-08, 1.0029416801496000E-08 - 55, 41,-6.0493167978568252E-08,-2.5592644443257859E-08, 9.6140134928287000E-09, 9.6302727751747995E-09 - 55, 42, 6.3287282201381362E-08,-3.6519512963966728E-08, 9.2301820390914993E-09, 9.2375422611580992E-09 - 55, 43,-8.2097749359317121E-08,-1.0044070806770400E-08, 8.7983321262273996E-09, 8.7845399926094998E-09 - 55, 44,-2.8286364381684259E-08,-1.7516756461603471E-08, 8.3955193291789998E-09, 8.3801710744392997E-09 - 55, 45,-3.0409410590102023E-08, 2.3862032000979189E-08, 7.9456051844270002E-09, 7.9545387008854994E-09 - 55, 46,-1.3749244330251250E-08,-8.3976645580965546E-09, 7.5041886879347993E-09, 7.5163053591610003E-09 - 55, 47, 2.7127207716497369E-09, 5.1386942076275433E-08, 7.0926868166237996E-09, 7.0889151663259000E-09 - 55, 48,-2.1853760565014311E-08,-3.2412205573666843E-08, 6.6268981714796001E-09, 6.6152521068145001E-09 - 55, 49,-4.4266785150122721E-10,-8.4213337921105733E-09, 6.1840670473314004E-09, 6.1872596309603002E-09 - 55, 50, 1.2732568684082210E-08, 3.2676249140723782E-08, 5.6920485654712003E-09, 5.6897322203955002E-09 - 55, 51,-1.6354635258231271E-08, 1.1478872642360911E-08, 5.1419461809925997E-09, 5.1298451795178001E-09 - 55, 52, 2.4702648434862821E-08,-3.6279101597158313E-08, 4.6349125317610001E-09, 4.6186407342457004E-09 - 55, 53, 8.7170999765779012E-09,-1.4905022368989959E-08, 3.7874377166505998E-09, 3.7833637523087000E-09 - 55, 54,-2.9323227972810941E-08, 8.3961247778335282E-08, 2.4120683398160999E-09, 2.4041981003412999E-09 - 55, 55,-2.2086028018406869E-08,-5.1715417671369101E-08, 1.9584778322491000E-09, 1.9367436382694999E-09 - 56, 0,-1.1932734881648851E-08, 0.0000000000000000E+00, 2.1985156404556999E-08, 0.0000000000000000E+00 - 56, 1,-7.0871225166908844E-08, 3.8641681074455722E-08, 2.1956792689355001E-08, 2.1976639175697000E-08 - 56, 2,-3.3603114782430217E-08,-4.7810606698333273E-08, 2.1946697327076999E-08, 2.1903165965034001E-08 - 56, 3,-3.3107320973744453E-08,-7.7656091678670951E-08, 2.1880925603969000E-08, 2.1923700060446999E-08 - 56, 4, 5.0032577285218290E-09,-5.6364326409146962E-08, 2.1803996657043001E-08, 2.1877474267659000E-08 - 56, 5,-4.1777382095886552E-08,-2.3067342111362549E-08, 2.1755998480754999E-08, 2.1810388736132001E-08 - 56, 6, 1.4020291048802770E-08,-6.8608717175948577E-09, 2.1539480545025000E-08, 2.1854211341193000E-08 - 56, 7, 7.5007086637424902E-09,-2.2723812623338340E-09, 2.1649705562556999E-08, 2.1554573471426999E-08 - 56, 8,-3.1252495535718451E-08, 2.8481841194410902E-08, 2.1438946072389001E-08, 2.1542472546963999E-08 - 56, 9,-3.7267895693270583E-08, 2.7542771131781479E-08, 2.1285216937377999E-08, 2.1432872316448001E-08 - 56, 10, 2.0871073005265238E-09, 9.7938082753904023E-09, 2.1215728272085001E-08, 2.1223157180087001E-08 - 56, 11,-1.5843361391065881E-08, 3.7504777927324182E-08, 2.1072289333262001E-08, 2.1028783103100999E-08 - 56, 12, 2.2483076910337769E-08, 1.8187539859300521E-08, 2.0879859194790000E-08, 2.0880535052400999E-08 - 56, 13,-4.0230969929810147E-08,-1.1531456784441490E-08, 2.0664578500316998E-08, 2.0702668498395000E-08 - 56, 14, 3.5750146104206479E-09, 8.5054419800780475E-09, 2.0467211265449000E-08, 2.0477747946656001E-08 - 56, 15,-1.9876841997635318E-09,-2.8213920063283041E-09, 2.0220759374663999E-08, 2.0254922731416001E-08 - 56, 16, 4.3726535566161811E-08, 8.7421769261966627E-09, 2.0006900750149001E-08, 2.0010149302870001E-08 - 56, 17,-1.4453560619039120E-09, 1.2478197972131130E-08, 1.9737743708595999E-08, 1.9732324746761000E-08 - 56, 18,-3.9915110550761733E-09, 3.2846884773427170E-09, 1.9494619331896001E-08, 1.9474736222203999E-08 - 56, 19, 9.1004280092814276E-09,-1.1343487087107269E-08, 1.9184262875335000E-08, 1.9188754298184001E-08 - 56, 20, 5.9467812860131087E-09,-4.9051441307418334E-10, 1.8911052585471999E-08, 1.8886014475064999E-08 - 56, 21, 4.0464759336384773E-09,-7.1913020853953971E-08, 1.8587344387629001E-08, 1.8577307493416002E-08 - 56, 22, 2.0179510382623661E-08,-2.0974262127494669E-08, 1.8276812016907001E-08, 1.8273422018760999E-08 - 56, 23, 2.8219825636949951E-08, 2.4809049714962229E-09, 1.7934727636155000E-08, 1.7935539239868998E-08 - 56, 24,-8.8109793002204936E-09, 3.7236044785029432E-08, 1.7627583049245000E-08, 1.7591464769288998E-08 - 56, 25,-1.8687261149443490E-08,-1.2841939650031820E-08, 1.7250173272153001E-08, 1.7276229355774999E-08 - 56, 26,-2.4872429987073328E-08, 8.4491242196578232E-09, 1.6932577524130999E-08, 1.6891691832879001E-08 - 56, 27,-1.0400714598861700E-09, 2.3027757067652129E-09, 1.6561936149474001E-08, 1.6525087429106999E-08 - 56, 28, 5.9634217450002092E-09, 1.1238310074177161E-08, 1.6188613742723999E-08, 1.6175370085601000E-08 - 56, 29,-3.1170168903600581E-08, 1.3229671714463281E-08, 1.5790836494433000E-08, 1.5801098397898000E-08 - 56, 30,-2.2352617740292561E-09, 4.2704496018687501E-08, 1.5413813141221001E-08, 1.5450092561044001E-08 - 56, 31, 8.5790000981193572E-09, 3.1481391012758888E-09, 1.5025516854391999E-08, 1.5046680269698000E-08 - 56, 32,-2.7030611553292609E-08, 1.5205317543233561E-08, 1.4632814561005999E-08, 1.4655282492149001E-08 - 56, 33, 1.4394403067515250E-08, 2.5560662073070188E-08, 1.4249031437071000E-08, 1.4283050777318000E-08 - 56, 34,-9.3467365009604126E-09, 1.2773264421065540E-08, 1.3860555072631000E-08, 1.3841972549442000E-08 - 56, 35, 4.1855439251139450E-09,-2.0392997489899030E-08, 1.3473205004291000E-08, 1.3466093027460000E-08 - 56, 36, 4.9531426593927456E-09, 9.7498403503221794E-09, 1.3042881309044001E-08, 1.3051647478372000E-08 - 56, 37,-2.5591660178739770E-08,-1.8711256485878950E-08, 1.2667093498425000E-08, 1.2639074602813000E-08 - 56, 38, 1.5322101548935169E-08,-2.2656447180228591E-08, 1.2237554426427000E-08, 1.2232316901880000E-08 - 56, 39,-3.5472632100994032E-08,-1.0413181972359199E-08, 1.1827394762823000E-08, 1.1821384072159999E-08 - 56, 40,-1.4287698737672810E-08, 7.8674679510912097E-09, 1.1399125650416999E-08, 1.1392782895444999E-08 - 56, 41,-1.7404832757375981E-08,-3.6160379984470752E-08, 1.0946401488173000E-08, 1.0961986652321999E-08 - 56, 42,-2.4155919176973871E-08, 6.1534629543356289E-08, 1.0516869819515000E-08, 1.0525514553906001E-08 - 56, 43,-5.7955662029208043E-08,-4.0353626301948603E-08, 1.0101174719006000E-08, 1.0096384942736000E-08 - 56, 44, 3.3684164125439371E-08, 6.7358317295381854E-08, 9.6397181785035993E-09, 9.6115996978769997E-09 - 56, 45,-5.8165632480347277E-08, 2.9008239805475479E-09, 9.1864609091498005E-09, 9.1912650018491005E-09 - 56, 46, 8.0045538387696621E-08, 4.6678779795698002E-08, 8.7098037714007004E-09, 8.7192661081898001E-09 - 56, 47,-3.6829432969901267E-08, 7.2071325153694886E-09, 8.2390902242930000E-09, 8.2425294888297004E-09 - 56, 48, 1.9956809546781731E-08,-1.9519964262933199E-08, 7.7926342124519996E-09, 7.7765958734867001E-09 - 56, 49, 5.1503280793249212E-08, 1.4271643703674220E-08, 7.2761024396559001E-09, 7.2721976378025004E-09 - 56, 50,-4.4689067536612654E-09,-3.0807798627173160E-08, 6.8270213503857001E-09, 6.8031809535278000E-09 - 56, 51, 2.5790077262080708E-08,-3.2111733449172441E-08, 6.2660511083078003E-09, 6.2543259247193999E-09 - 56, 52, 1.8554812559999950E-09,-1.9993621847970261E-08, 5.6510965105985996E-09, 5.6333768467284999E-09 - 56, 53,-3.3530514139558771E-08,-2.9119325437705201E-08, 5.0937154010530003E-09, 5.0920173731619000E-09 - 56, 54, 6.5078969513961821E-09, 1.5740387258520900E-09, 4.1448019018495996E-09, 4.1524387261614999E-09 - 56, 55, 1.2738552004149509E-07,-4.5105162324429591E-08, 2.6312346058907002E-09, 2.6419309184124999E-09 - 56, 56,-2.1919178373421420E-08, 6.5491351160329483E-08, 2.1591342723745001E-09, 2.1622117609756001E-09 - 57, 0,-2.1243891495712241E-08, 0.0000000000000000E+00, 2.3723297372618001E-08, 0.0000000000000000E+00 - 57, 1, 4.1531225927031417E-10, 1.9873894089602499E-08, 2.3674945324655001E-08, 2.3697058550305000E-08 - 57, 2, 4.3417614569583733E-09, 5.5426278662686363E-08, 2.3672478352049000E-08, 2.3641963850214002E-08 - 57, 3, 7.2763017113695284E-09, 1.9890550601183790E-08, 2.3598340412454001E-08, 2.3637159315577999E-08 - 57, 4, 2.8352091228297479E-08, 1.0671273735474990E-08, 2.3521892663164001E-08, 2.3611735718708999E-08 - 57, 5,-4.9042245170945766E-10,-8.4411285598917464E-09, 2.3473240193197000E-08, 2.3510375388814001E-08 - 57, 6,-1.0691815038230210E-08,-8.3303307735296533E-09, 2.3248172847385001E-08, 2.3587241918998999E-08 - 57, 7,-2.5327314704575740E-08,-2.5739017197925340E-08, 2.3351382093979001E-08, 2.3259959720911000E-08 - 57, 8,-1.3070032685694571E-08, 1.9360114067206479E-08, 2.3154205639407999E-08, 2.3251685459037999E-08 - 57, 9,-1.1660168794217701E-08,-1.6806979599113609E-08, 2.2982874895444001E-08, 2.3134434973918000E-08 - 57, 10, 3.0903583289763373E-08, 4.2775007179188453E-08, 2.2910988079111999E-08, 2.2932350255206999E-08 - 57, 11, 3.2685671623017221E-08, 2.2829537397021192E-08, 2.2762313506967999E-08, 2.2735649214227999E-08 - 57, 12, 3.1556048497262832E-08, 2.0189978026834209E-08, 2.2568466940405998E-08, 2.2579073990731001E-08 - 57, 13,-8.9845618401068049E-09, 2.7880872789146231E-08, 2.2362806542388999E-08, 2.2392020650459000E-08 - 57, 14, 2.4442288598510530E-08, 1.3110467479758710E-08, 2.2150992037771001E-08, 2.2174612015469998E-08 - 57, 15, 5.4949748479115807E-10, 1.3578456246447679E-08, 2.1920337850445000E-08, 2.1941539376602999E-08 - 57, 16,-2.6828947713793100E-09, 5.9707039337331236E-09, 2.1692641217018999E-08, 2.1687562885678999E-08 - 57, 17, 9.4349779495163113E-10, 1.4042789694091400E-08, 2.1432764048446001E-08, 2.1435737403336999E-08 - 57, 18,-6.6511240894019173E-09, 5.7368744895617897E-09, 2.1167391440240000E-08, 2.1139264029751000E-08 - 57, 19, 1.5910348372004039E-08,-9.3713262858116040E-09, 2.0887494229586999E-08, 2.0888712545183998E-08 - 57, 20,-5.0618387304031450E-09,-1.0019795285925850E-08, 2.0577774187195001E-08, 2.0555168605901998E-08 - 57, 21,-2.6967703819319900E-08,-3.1315575291156651E-08, 2.0271116949638999E-08, 2.0274641989795999E-08 - 57, 22,-3.5344404747113392E-08,-5.5929039885978847E-08, 1.9940026621490001E-08, 1.9939235544622001E-08 - 57, 23,-5.6686179805740703E-08,-1.1844040442389340E-08, 1.9620586371417999E-08, 1.9616127313655998E-08 - 57, 24,-1.7020233517005648E-08,-1.7125111627194060E-09, 1.9285914073699001E-08, 1.9255915406117001E-08 - 57, 25, 3.6562394595858423E-08, 1.5692134192673700E-09, 1.8911498894308999E-08, 1.8949453480397001E-08 - 57, 26,-4.8634076130040093E-08, 9.1834642026123978E-09, 1.8586539695158998E-08, 1.8546672447039001E-08 - 57, 27, 4.6130375797864207E-08, 3.2556492901524107E-08, 1.8218886967526000E-08, 1.8182996100629000E-08 - 57, 28, 5.4689481576062600E-09, 4.6599773260688833E-08, 1.7824076125944001E-08, 1.7815010716331000E-08 - 57, 29, 3.6745930719475888E-08,-1.5119394103802420E-08, 1.7444341521385000E-08, 1.7437643377325000E-08 - 57, 30, 7.7900543131802443E-09,-8.5013810612921877E-09, 1.7027576788149999E-08, 1.7063067177360001E-08 - 57, 31, 1.8504594464247911E-08,-3.8052555878795892E-09, 1.6651207625169000E-08, 1.6678442029561001E-08 - 57, 32, 3.5475679313498653E-08, 3.3085574203563761E-08, 1.6232064783924000E-08, 1.6258424187359001E-08 - 57, 33, 1.4726033412290410E-08,-2.2567664250473270E-08, 1.5822817988690999E-08, 1.5867304571437000E-08 - 57, 34,-4.3056322345365841E-08,-5.9240379288160653E-09, 1.5446828795036001E-08, 1.5440330106886000E-08 - 57, 35, 1.4552897679796521E-08,-7.3424145857240362E-08, 1.5009154952245998E-08, 1.5018115568861001E-08 - 57, 36,-4.9282339815758892E-08,-8.8800324746045985E-09, 1.4603695856140001E-08, 1.4612910040736001E-08 - 57, 37,-1.5213122638216710E-10,-3.0109872919255591E-08, 1.4179450142875000E-08, 1.4154322627231000E-08 - 57, 38,-4.1249824189076837E-08, 1.7627589382168950E-08, 1.3762891028947001E-08, 1.3741743100561000E-08 - 57, 39,-3.9129168006424116E-09, 1.2749240438007280E-09, 1.3308823502511000E-08, 1.3296543003873000E-08 - 57, 40,-1.8941950991629269E-08, 2.8549012200682050E-08, 1.2870180059541000E-08, 1.2869834126193000E-08 - 57, 41,-2.2113855580287060E-09, 7.4195491414365042E-09, 1.2401650584109000E-08, 1.2420263432962000E-08 - 57, 42, 5.1678198013408821E-09, 6.2487645468054121E-08, 1.1937273480416999E-08, 1.1943646432406001E-08 - 57, 43,-1.4575337182998440E-08, 1.5424311548276471E-09, 1.1488146078210000E-08, 1.1474538535383999E-08 - 57, 44, 1.3216504009769249E-09, 3.0557632023929763E-08, 1.1037865301216000E-08, 1.1024537342361000E-08 - 57, 45, 5.8638050373894228E-08,-4.4585190276121127E-08, 1.0515604943803000E-08, 1.0520307222247001E-08 - 57, 46, 5.3921909888683872E-08, 6.1019881645414481E-08, 1.0044849476281000E-08, 1.0060493817069999E-08 - 57, 47,-1.5886993766328458E-08,-6.3739364556579840E-08, 9.5410539885890003E-09, 9.5368363621357004E-09 - 57, 48, 3.2355174011437509E-09, 1.9891566410252481E-08, 9.0391020766838004E-09, 9.0276274365445998E-09 - 57, 49,-1.6635479735933390E-08,-1.4990982908636769E-08, 8.5409750732316004E-09, 8.5333367860444994E-09 - 57, 50,-1.0138335766194510E-08,-1.1696632609939420E-08, 7.9917296316221995E-09, 7.9871772283609007E-09 - 57, 51, 1.3666576278193531E-09,-7.5584952917716056E-09, 7.4776629089633003E-09, 7.4809758882437004E-09 - 57, 52, 2.4540445362470070E-09, 5.0538999901756037E-08, 6.8541846302563997E-09, 6.8528435843003998E-09 - 57, 53,-5.4284527060827491E-08,-6.2108950701615569E-09, 6.2178652950754000E-09, 6.1940934031118997E-09 - 57, 54,-1.3647378052313160E-08, 6.6573613126338164E-08, 5.6010102890217002E-09, 5.6054316172836997E-09 - 57, 55,-1.3048754707484809E-08,-3.0174525200948582E-08, 4.5248170728788996E-09, 4.5464605419020997E-09 - 57, 56,-6.6601689101884101E-08,-9.2417367195420845E-08, 2.8874929321069001E-09, 2.9056229980127001E-09 - 57, 57, 7.6691153671404511E-08, 4.5059601692362497E-08, 2.4192648659456000E-09, 2.4342878064930998E-09 - 58, 0, 1.2047513260901750E-09, 0.0000000000000000E+00, 2.5409261746237998E-08, 0.0000000000000000E+00 - 58, 1, 1.8823657610989730E-08,-1.1569274748816460E-08, 2.5377061354694999E-08, 2.5380480338766999E-08 - 58, 2, 6.6137421776539104E-09,-1.1193037962724350E-08, 2.5342932216723999E-08, 2.5324338895112001E-08 - 58, 3, 3.8047284062618281E-08, 4.1794160984964171E-08, 2.5281831512899999E-08, 2.5331959013757001E-08 - 58, 4, 5.5737543807119143E-08, 2.9800174574788202E-08, 2.5210031315450001E-08, 2.5265972050298001E-08 - 58, 5, 2.5055862915150868E-09, 3.0012350802429448E-10, 2.5148287694420000E-08, 2.5200756809446999E-08 - 58, 6, 1.1726187366599299E-08, 9.3511389743938401E-09, 2.4932861229808000E-08, 2.5234312022540999E-08 - 58, 7,-6.1739502710402631E-09, 3.2225289379238009E-09, 2.5037202896420999E-08, 2.4936245734929001E-08 - 58, 8,-2.2337933506376671E-08, 1.7903454140434040E-08, 2.4800735861965999E-08, 2.4926931901567000E-08 - 58, 9,-2.0217869838145092E-09,-2.1435894067361030E-10, 2.4666603221280999E-08, 2.4800119863225999E-08 - 58, 10, 3.7303900856690607E-08,-1.8138281549426479E-08, 2.4576047667074001E-08, 2.4584718418618999E-08 - 58, 11, 2.5380019538382131E-09,-1.2625375602838040E-09, 2.4443156214503999E-08, 2.4398175117338998E-08 - 58, 12, 1.9190552835155001E-09,-2.5326884839777791E-08, 2.4222183149681001E-08, 2.4248307174367001E-08 - 58, 13, 4.3349935657125041E-08,-5.5039857107314157E-09, 2.4024721454196998E-08, 2.4062054591908001E-08 - 58, 14, 2.6400208518652658E-09,-5.5199832398728522E-09, 2.3815698157629999E-08, 2.3839124279540999E-08 - 58, 15, 2.2896118562855858E-08,-6.7167294601887563E-09, 2.3580258963418999E-08, 2.3608599548070999E-08 - 58, 16,-1.8814575065865669E-08, 1.1681224069162469E-08, 2.3354468235455001E-08, 2.3360845706181999E-08 - 58, 17,-1.2918549972594160E-08,-3.6447320615214430E-09, 2.3111805653418000E-08, 2.3093224009617999E-08 - 58, 18, 1.7610499348107371E-08, 1.9426009155064079E-08, 2.2841010223468000E-08, 2.2811994505252001E-08 - 58, 19, 1.7472656472487431E-09, 1.6190316469249129E-08, 2.2553307433727001E-08, 2.2550086888657001E-08 - 58, 20,-1.8627153910916749E-08, 1.3661725487086760E-08, 2.2263483820704001E-08, 2.2245610550079999E-08 - 58, 21,-2.0974739804714060E-08, 1.6783131812981899E-08, 2.1938602917671999E-08, 2.1929371744731001E-08 - 58, 22,-3.0029943880694499E-09, 2.2584725860829389E-08, 2.1626963054113001E-08, 2.1630591878246001E-08 - 58, 23,-4.5605096042276997E-08, 6.1863124735647972E-08, 2.1285970733616000E-08, 2.1281081725883998E-08 - 58, 24,-1.1172942407539200E-08, 4.3210625972034012E-09, 2.0970597471557000E-08, 2.0947114236464000E-08 - 58, 25, 2.4901249666543930E-08, 3.9213295773485751E-08, 2.0584933848324001E-08, 2.0620356595468000E-08 - 58, 26,-3.5074484593944872E-08,-2.3026039630124899E-08, 2.0273148076289002E-08, 2.0230791843364001E-08 - 58, 27, 2.4936154099689351E-08,-2.6145367340125850E-09, 1.9893773961820999E-08, 1.9855202717656999E-08 - 58, 28,-3.4510547743000031E-08,-3.3592497547361561E-08, 1.9512726534076001E-08, 1.9494154725341999E-08 - 58, 29, 1.9908993362642989E-08,-2.9174410008416009E-08, 1.9113510813227999E-08, 1.9110783818848001E-08 - 58, 30,-1.2495981031819341E-08,-3.1093920172654300E-08, 1.8712553701312000E-08, 1.8733063658064999E-08 - 58, 31,-1.8705067364975680E-08,-5.4283131902508492E-08, 1.8312465602296000E-08, 1.8324285752821000E-08 - 58, 32, 2.5565181696173861E-08,-1.6915591003628391E-08, 1.7908685229686001E-08, 1.7939607917385000E-08 - 58, 33,-2.6439019069162840E-08, 3.0067905518930157E-08, 1.7466011770793000E-08, 1.7515446106870000E-08 - 58, 34,-1.5246597035486791E-08,-1.9786593599663089E-08, 1.7082823988812000E-08, 1.7082370712763001E-08 - 58, 35, 1.2793002485075790E-09, 9.3491926090917502E-10, 1.6661035929050999E-08, 1.6667416675635999E-08 - 58, 36,-4.6691491419155172E-08, 2.3502395496399610E-08, 1.6210830156671999E-08, 1.6224075687680001E-08 - 58, 37, 1.3087725087608799E-08, 2.8977138289796159E-08, 1.5815631184190000E-08, 1.5791498559058000E-08 - 58, 38,-4.1402226929529901E-08, 1.5078509727562809E-08, 1.5339812882953001E-08, 1.5329527115366000E-08 - 58, 39, 4.4214687863619512E-08,-7.6762173132433868E-09, 1.4904978256429001E-08, 1.4890704309938999E-08 - 58, 40,-3.0990227692192272E-09,-1.4164079832113559E-08, 1.4428662602413000E-08, 1.4426732845557000E-08 - 58, 41, 2.0990087804697419E-08, 2.5570376925147250E-09, 1.3963713351700001E-08, 1.3981539721727000E-08 - 58, 42, 3.3703528780648941E-08,-1.4487124682240640E-08, 1.3476132544077001E-08, 1.3493229506713999E-08 - 58, 43, 1.4021584496293430E-08,-1.2582895918467361E-08, 1.2993917406591000E-08, 1.2981216354442000E-08 - 58, 44, 3.2567836769240513E-08,-3.6295402617081532E-08, 1.2507263701137000E-08, 1.2503388732073000E-08 - 58, 45, 4.2916409929425503E-08,-1.8036956983188720E-08, 1.2017784030296001E-08, 1.2015111284432000E-08 - 58, 46,-7.1144538258152400E-08, 3.2872365133388802E-08, 1.1468782545102000E-08, 1.1480444056564999E-08 - 58, 47, 9.5521659354754124E-09,-6.3597445424838331E-08, 1.0980238766690001E-08, 1.0974063725761000E-08 - 58, 48,-4.7810928558461138E-08, 7.4243998925716279E-09, 1.0433666967160001E-08, 1.0412322453846000E-08 - 58, 49,-1.1742980318201830E-08,-1.1000940432308250E-08, 9.8853171003749997E-09, 9.8899645211655995E-09 - 58, 50,-9.3381588076665514E-09, 2.5630718719408970E-08, 9.3589944577075998E-09, 9.3634677968674003E-09 - 58, 51, 1.6319329828924680E-09, 1.4570838989938131E-08, 8.7417316940462007E-09, 8.7643736185063000E-09 - 58, 52, 1.8271757614159821E-09, 5.8032807572724532E-08, 8.1871039188667002E-09, 8.2055978038569993E-09 - 58, 53, 2.5546615691538040E-08, 3.7894814319004807E-08, 7.5218518820381008E-09, 7.5151255663560996E-09 - 58, 54, 1.5635265611709542E-08, 2.6840692699293371E-08, 6.8107433028128002E-09, 6.8286442871962998E-09 - 58, 55, 1.9961030200840940E-08, 2.8679403475494671E-09, 6.1405474838749998E-09, 6.1432889481565001E-09 - 58, 56, 2.6822056952802661E-08,-6.4701730814571170E-09, 5.0027924114675003E-09, 5.0035768672705001E-09 - 58, 57,-4.8004650079194133E-08, 7.5216170477841512E-08, 3.2165849315731002E-09, 3.1940294951165000E-09 - 58, 58, 2.9862851975805060E-09,-7.1622010981229529E-08, 2.6379560770613000E-09, 2.6687755088234001E-09 - 59, 0,-7.5091581591094091E-08, 0.0000000000000000E+00, 2.7000923212774001E-08, 0.0000000000000000E+00 - 59, 1,-1.9038071578597198E-09,-2.3483280674136780E-08, 2.6987277320081000E-08, 2.6990264894905000E-08 - 59, 2, 2.0318680730819010E-08,-4.9375869395951407E-08, 2.6930978249876001E-08, 2.6908173315937000E-08 - 59, 3, 2.3185171040539781E-08,-1.7057122867018701E-08, 2.6882940045801001E-08, 2.6929971560227999E-08 - 59, 4,-2.0501298053190071E-08,-7.1577687868231148E-09, 2.6793465493122001E-08, 2.6849988185269000E-08 - 59, 5, 4.1831845702114471E-09, 9.6073912894566784E-09, 2.6745577311574000E-08, 2.6794166173867000E-08 - 59, 6, 3.5221516357942591E-08, 1.6661748674528059E-09, 2.6520039963649999E-08, 2.6810610090656999E-08 - 59, 7, 3.2000972955992507E-08, 3.8829937775706441E-09, 2.6619140531743002E-08, 2.6525164457805999E-08 - 59, 8, 2.5430958332904771E-08,-6.7540867044082994E-09, 2.6406154409368000E-08, 2.6490132020154001E-08 - 59, 9,-1.4493939798645620E-08, 1.5891521408192990E-08, 2.6245269290411001E-08, 2.6384018383178001E-08 - 59, 10, 1.2874284962696370E-09, 4.9885131653795834E-09, 2.6164567572310001E-08, 2.6170328451440001E-08 - 59, 11,-3.2806495147442693E-08,-9.4803948887888155E-09, 2.6005632718034002E-08, 2.5988580768550002E-08 - 59, 12,-7.8136808023975637E-09,-3.0069108831890848E-08, 2.5819667627380001E-08, 2.5837530956773998E-08 - 59, 13, 4.0228082095788072E-08,-1.2762531030684760E-08, 2.5604951492456000E-08, 2.5639735110728999E-08 - 59, 14,-5.9028641583681418E-09, 1.1634597752272309E-08, 2.5420988372020000E-08, 2.5433203308209000E-08 - 59, 15,-3.2508033424529872E-08,-3.7505592204065928E-08, 2.5181749597636000E-08, 2.5189998330581000E-08 - 59, 16,-5.2234996422991854E-09, 2.8758974462518200E-09, 2.4965112692263999E-08, 2.4952981431446001E-08 - 59, 17,-1.2010233571955460E-08,-2.1938299757070819E-08, 2.4711455963296999E-08, 2.4708853344390000E-08 - 59, 18, 5.1868305551150567E-09, 4.2225453930158641E-09, 2.4456675584854999E-08, 2.4424099947832001E-08 - 59, 19, 5.8747968733477918E-10, 1.4066593209169590E-08, 2.4171145961779000E-08, 2.4170943008490999E-08 - 59, 20,-3.7582430819860744E-09, 7.0336643826736384E-09, 2.3887859577633999E-08, 2.3867265716805000E-08 - 59, 21,-7.8952599520638866E-09, 2.4664626335780549E-08, 2.3574487048164000E-08, 2.3575481548620000E-08 - 59, 22,-4.6400764232985528E-10, 4.1486787053903252E-08, 2.3249649863381001E-08, 2.3260204781574999E-08 - 59, 23, 1.8873261728867758E-08, 1.0337249402667921E-08, 2.2945319208940000E-08, 2.2936792681962001E-08 - 59, 24, 1.7712504152170731E-08, 2.1450436035769050E-08, 2.2619683250668000E-08, 2.2580426627553000E-08 - 59, 25,-2.2263659735897861E-08, 2.6412184918243660E-08, 2.2255972220170000E-08, 2.2284965632852000E-08 - 59, 26, 1.6413954634663330E-08,-2.5609734872547449E-08, 2.1921872262399000E-08, 2.1887537717174000E-08 - 59, 27,-1.9828682667067850E-08,-1.0496627129445150E-08, 2.1571650618858001E-08, 2.1527990550728000E-08 - 59, 28,-2.8285367913793619E-08,-3.2686185070609621E-08, 2.1170029710212999E-08, 2.1169722239048002E-08 - 59, 29,-1.4577962014205940E-08, 2.1734115584931960E-09, 2.0797950041609001E-08, 2.0793353880392999E-08 - 59, 30,-8.5797045139896771E-09,-3.1044197978555687E-08, 2.0375030961257000E-08, 2.0417610858072999E-08 - 59, 31, 2.5368981154946951E-09, 2.5025684864256909E-08, 1.9992556819309000E-08, 2.0015091007162001E-08 - 59, 32,-3.0376217549835529E-10,-1.1263180700285919E-08, 1.9576949972493000E-08, 1.9617077456989000E-08 - 59, 33, 1.9964445388565680E-08,-1.4656666546230770E-08, 1.9174075210812999E-08, 1.9207373410291001E-08 - 59, 34, 3.3952555621562273E-08, 8.8164171379139259E-09, 1.8753864823126002E-08, 1.8741807364301001E-08 - 59, 35,-1.7056905050651480E-08, 2.5554551543429871E-08, 1.8337509794384001E-08, 1.8343284650267000E-08 - 59, 36, 3.5816639882684607E-08, 2.3489390627850799E-08, 1.7898804121317999E-08, 1.7907014301213999E-08 - 59, 37,-2.0323626740639240E-08, 4.6491441134576053E-08, 1.7470465353956000E-08, 1.7436415094340999E-08 - 59, 38, 5.1771774762910912E-08,-3.5850292843105938E-08, 1.7029420978892999E-08, 1.7010435891389999E-08 - 59, 39, 2.8797940652478778E-08, 1.6873684517769471E-08, 1.6535799025125001E-08, 1.6526782316852000E-08 - 59, 40, 6.8679205939191540E-09,-2.5002977481646728E-08, 1.6080619365857000E-08, 1.6083213398295000E-08 - 59, 41,-3.1340550209916322E-08, 4.9162597200635843E-09, 1.5585198208150999E-08, 1.5603850895531000E-08 - 59, 42,-3.4660745619997700E-08,-4.7308379329145953E-08, 1.5111770885507000E-08, 1.5121670119886001E-08 - 59, 43,-8.8810821045887877E-09,-6.3306309091624832E-09, 1.4603008020586000E-08, 1.4603073155339000E-08 - 59, 44, 6.4757674880955480E-10,-5.3427916352357057E-08, 1.4091389098351001E-08, 1.4083162920375000E-08 - 59, 45,-1.4954267628976579E-08, 2.8488296800838760E-08, 1.3579304538970000E-08, 1.3576869769574000E-08 - 59, 46,-1.9036033461663869E-08,-3.1268568793447031E-08, 1.3059727285976000E-08, 1.3068523869076000E-08 - 59, 47, 3.9656198555560764E-09, 4.2271568047756767E-09, 1.2487121723272001E-08, 1.2484310043066000E-08 - 59, 48,-2.0641280280147881E-08, 3.5031907135038968E-08, 1.1965798214329000E-08, 1.1949638558530000E-08 - 59, 49, 2.1222103459619591E-08, 8.5315373284775782E-09, 1.1365625453443000E-08, 1.1368593713595000E-08 - 59, 50, 1.1327130843346560E-08, 3.4548304209638693E-08, 1.0806897340129001E-08, 1.0809534453270001E-08 - 59, 51, 3.7628158169680752E-08,-2.8592771040614770E-08, 1.0215717750091000E-08, 1.0222993229136001E-08 - 59, 52, 9.7025198985422034E-09, 1.0715546914694281E-08, 9.5627499912546006E-09, 9.5844750039766005E-09 - 59, 53, 7.3038998467914648E-09, 1.4802828937142580E-08, 8.9677625706929992E-09, 8.9872272848040000E-09 - 59, 54, 1.9530101503351350E-08,-5.1370145254809817E-08, 8.2323622508636007E-09, 8.2416931669464997E-09 - 59, 55, 3.0875791679519418E-08, 2.5066639639273108E-09, 7.4739674132983006E-09, 7.4823957772288998E-09 - 59, 56, 2.8809792820601490E-08,-1.2008492805136421E-09, 6.7675602980312996E-09, 6.7736552853342996E-09 - 59, 57,-3.2873226969815460E-08,-1.1065316936877790E-08, 5.4883500004618000E-09, 5.4670945877038996E-09 - 59, 58, 5.3696086964138218E-08, 4.7224842380252853E-08, 3.4511797508372999E-09, 3.4660728182546001E-09 - 59, 59,-5.5454066056719202E-08, 3.1334102107489401E-08, 2.9810671321072999E-09, 2.9622354608023002E-09 - 60, 0,-3.3108636764790851E-08, 0.0000000000000000E+00, 2.8484388185223001E-08, 0.0000000000000000E+00 - 60, 1, 6.3245531296261832E-11, 1.3490441539497419E-09, 2.8443329521459999E-08, 2.8447544192618999E-08 - 60, 2, 1.9917628107598070E-08, 1.7868898713223569E-08, 2.8397649380840000E-08, 2.8390852549471998E-08 - 60, 3,-5.3664678607413780E-09,-4.7914590742039280E-09, 2.8338111801216000E-08, 2.8381547978614999E-08 - 60, 4,-1.9702723680909059E-08,-1.4700802313100581E-08, 2.8262961865673999E-08, 2.8322298694695001E-08 - 60, 5,-3.1372550271836148E-08, 5.4229768058367593E-09, 2.8201232313754000E-08, 2.8236015149041999E-08 - 60, 6, 2.7005349245876980E-08,-1.8300971074691819E-08, 2.8000059026256001E-08, 2.8269895107368001E-08 - 60, 7, 6.8339248381042761E-09,-2.9258135055096260E-09, 2.8062680221880001E-08, 2.7979634037254000E-08 - 60, 8, 4.1861779483395059E-09,-2.7012099850646041E-08, 2.7872740519355000E-08, 2.7959475866759999E-08 - 60, 9, 2.4093985319316018E-09, 4.7922791286457374E-09, 2.7710235280044002E-08, 2.7821894041123001E-08 - 60, 10,-1.5008138379742071E-08, 1.2585563615914220E-08, 2.7624203564456001E-08, 2.7651156791209999E-08 - 60, 11, 4.0929390562970649E-09, 1.7236051224280698E-08, 2.7470935511719001E-08, 2.7444890558176001E-08 - 60, 12, 1.1122976597496199E-08, 2.0862382491447250E-08, 2.7291410670863998E-08, 2.7312547039346999E-08 - 60, 13, 1.9375664288297401E-08, 3.1833057034099230E-08, 2.7085083605709999E-08, 2.7108642366823999E-08 - 60, 14,-7.6025086486352066E-09, 5.1858593199798893E-08, 2.6895812549041000E-08, 2.6912087605975001E-08 - 60, 15, 2.0211657793549460E-08,-2.3676682501497068E-08, 2.6668155520655999E-08, 2.6685643349404999E-08 - 60, 16,-1.9360838497187169E-08,-3.3400658435871957E-08, 2.6451153077456000E-08, 2.6443927012372999E-08 - 60, 17, 2.9353412859014520E-08, 2.5009634169745061E-08, 2.6221687958131001E-08, 2.6209751788840002E-08 - 60, 18,-2.7767092525729980E-08,-3.2434893251114942E-08, 2.5961715583733999E-08, 2.5943456436469000E-08 - 60, 19, 1.0414563404275889E-08,-4.5462551687539688E-09, 2.5690908400504000E-08, 2.5698924825759000E-08 - 60, 20,-1.1057579273098140E-08, 1.6959288955175360E-08, 2.5416306094685001E-08, 2.5397756442497001E-08 - 60, 21, 3.3209209507735702E-08,-1.5981218452971819E-08, 2.5124424662745001E-08, 2.5115876269842001E-08 - 60, 22, 1.2478825021207209E-08,-3.6244843463215112E-08, 2.4808876623690000E-08, 2.4811292727183000E-08 - 60, 23, 2.4340366484305301E-08,-3.7137298386868547E-08, 2.4501035754701998E-08, 2.4500843188617001E-08 - 60, 24, 6.9303493912016028E-09,-1.3315350467132830E-08, 2.4196713476991999E-08, 2.4173079353563999E-08 - 60, 25,-6.8193795670069533E-09, 1.1697190004464080E-08, 2.3830137078506001E-08, 2.3874397218769000E-08 - 60, 26,-7.7822578175871859E-09, 5.4713411424861923E-08, 2.3531430850172001E-08, 2.3499716866371001E-08 - 60, 27,-3.0587836825738218E-08,-1.5604018594999539E-08, 2.3173773401254999E-08, 2.3133740980976001E-08 - 60, 28, 3.3586045157243922E-08, 2.8615082870361401E-08, 2.2805295787060000E-08, 2.2802407144463000E-08 - 60, 29, 4.2172744534238094E-09, 3.5869153661210820E-08, 2.2421936374379000E-08, 2.2427676731406002E-08 - 60, 30, 3.0185643205040271E-08, 2.8032185975427322E-08, 2.2035819796305998E-08, 2.2066404666394999E-08 - 60, 31,-8.1002103258777091E-09, 3.2656442856011377E-08, 2.1638667242554001E-08, 2.1669483956988999E-08 - 60, 32,-4.2162348396286048E-08,-2.0099365896839279E-08, 2.1253256444635001E-08, 2.1274800952894999E-08 - 60, 33, 2.5400824161614969E-08, 1.0319427932579880E-08, 2.0839934779352999E-08, 2.0872032863766001E-08 - 60, 34, 1.3035789685759499E-08,-2.5766316510051400E-08, 2.0448753535302999E-08, 2.0442423784029999E-08 - 60, 35, 5.5282965374147284E-09, 1.2923405054370780E-08, 2.0001488166994999E-08, 2.0012716302767001E-08 - 60, 36,-1.2742321344060860E-08, 1.1381796973302179E-09, 1.9588907871742001E-08, 1.9604792979722001E-08 - 60, 37,-1.6661615216549660E-09,-2.1232802026805081E-08, 1.9168606057335000E-08, 1.9146891325671000E-08 - 60, 38, 3.0021625016959630E-08,-2.5850736439286800E-09, 1.8705485839393001E-08, 1.8689295317022999E-08 - 60, 39,-1.4648412475004110E-08,-2.9522087276308460E-08, 1.8244573706194002E-08, 1.8249014411821001E-08 - 60, 40, 1.5473206744825189E-08, 1.5330230901398549E-08, 1.7753275130222999E-08, 1.7751220232629001E-08 - 60, 41,-1.3721112616586730E-08, 1.8260290052167121E-08, 1.7286869375621001E-08, 1.7307275693365999E-08 - 60, 42,-1.9402335532355100E-08,-8.5448126017970379E-09, 1.6778289050945001E-08, 1.6797155619090999E-08 - 60, 43,-7.4084173924363597E-10, 1.0594094195039840E-08, 1.6291618132478999E-08, 1.6297796304134002E-08 - 60, 44,-2.4931723796387840E-08, 2.7318223017861991E-08, 1.5751221484791999E-08, 1.5771171611261000E-08 - 60, 45,-2.1382153577429429E-08, 3.3875496356341523E-08, 1.5232482774720999E-08, 1.5216384248514999E-08 - 60, 46, 3.2993927467381813E-08,-3.8992381867388593E-08, 1.4687771301365001E-08, 1.4702431330182001E-08 - 60, 47,-1.9201224723136971E-08, 3.9756671452646441E-08, 1.4150910802410001E-08, 1.4149023319085000E-08 - 60, 48, 3.9387678175265283E-08, 2.6107738260247449E-08, 1.3564126992324000E-08, 1.3527874163635000E-08 - 60, 49, 2.1849854251892091E-08, 6.2413103119478465E-08, 1.2990460942510999E-08, 1.2996993944737000E-08 - 60, 50,-3.1633782337288500E-09,-3.1661047792882230E-08, 1.2371410712090000E-08, 1.2378080019510000E-08 - 60, 51, 9.2632542761942713E-09, 3.5166982217625498E-09, 1.1771356439167000E-08, 1.1761810530674000E-08 - 60, 52,-2.4168316693313160E-08,-5.2756977043446752E-09, 1.1129002331473000E-08, 1.1143284981364000E-08 - 60, 53,-4.2070791251197377E-09,-3.2588993781587271E-09, 1.0439201168250000E-08, 1.0461511686001999E-08 - 60, 54, 1.1708264116049810E-08,-3.0161967265025688E-09, 9.8131600495227007E-09, 9.8114236742132006E-09 - 60, 55,-5.9913661584365440E-08,-2.1727304165026729E-09, 8.9925217384418008E-09, 9.0028805597320004E-09 - 60, 56,-3.4349111983262941E-08,-1.8121887940579761E-08, 8.2092972623061005E-09, 8.2008904795931002E-09 - 60, 57,-1.1491508456620339E-08, 1.9407185362639842E-09, 7.4154937094794997E-09, 7.4236965081321003E-09 - 60, 58,-2.0323657779749828E-08, 6.8802901325972923E-08, 6.0105320635522002E-09, 5.9646458613258996E-09 - 60, 59,-1.8435731254147809E-08,-6.7674814100362951E-08, 3.8214866103512997E-09, 3.7967938009894003E-09 - 60, 60, 3.5480644283013412E-08, 3.2671657999570567E-08, 3.2209938672415000E-09, 3.2475151691014000E-09 - 61, 0,-2.9130180219896170E-08, 0.0000000000000000E+00, 2.9781164522348000E-08, 0.0000000000000000E+00 - 61, 1, 1.5529868065833130E-08, 3.2834716479107740E-08, 2.9759975821052999E-08, 2.9754601137635002E-08 - 61, 2, 1.4149065639100971E-08,-5.1303556170471017E-09, 2.9678775857431999E-08, 2.9678280567000999E-08 - 61, 3, 1.6457750563703200E-08,-1.7658601641076590E-08, 2.9642005411167001E-08, 2.9685911626282999E-08 - 61, 4,-2.9831405430521712E-09,-1.1442665281632510E-08, 2.9552617868877001E-08, 2.9595328059670000E-08 - 61, 5,-3.3143261645858673E-08, 5.1267789933400538E-09, 2.9504475495845001E-08, 2.9541086969886001E-08 - 61, 6,-3.0168647462827941E-08,-3.7057223003806122E-10, 2.9294441951336001E-08, 2.9537333638019000E-08 - 61, 7,-6.4657091527999573E-09,-1.3880826635212660E-08, 2.9367116239522999E-08, 2.9281259286876999E-08 - 61, 8,-1.0663425428489990E-08,-9.3900374284642398E-09, 2.9152965818455000E-08, 2.9243170665305000E-08 - 61, 9, 3.0074241388433618E-09,-2.1231270239997931E-08, 2.9017654329396000E-08, 2.9127611857311000E-08 - 61, 10, 1.9930166846039130E-08,-2.3660012429757289E-08, 2.8921761563387000E-08, 2.8930112690830001E-08 - 61, 11,-6.9389008014433722E-09, 1.4593403859407450E-08, 2.8782943015170002E-08, 2.8760012979838999E-08 - 61, 12, 3.6514408191221033E-08, 1.4760965337687591E-08, 2.8591000512554001E-08, 2.8616966171759002E-08 - 61, 13, 3.9344717569271672E-08,-8.9846692011105705E-09, 2.8407088712825999E-08, 2.8431340336653001E-08 - 61, 14, 1.0063751701860420E-08,-2.4265074674296419E-08, 2.8217856557009000E-08, 2.8231547242653001E-08 - 61, 15, 1.0756445290579779E-08, 2.6615225288869419E-08, 2.8009728841290001E-08, 2.8018313436471998E-08 - 61, 16, 4.3158058429207126E-09,-1.7219579638812131E-08, 2.7798009123186001E-08, 2.7797129499802999E-08 - 61, 17,-7.0136574273283194E-09, 1.0368676022094290E-08, 2.7579893312517999E-08, 2.7562820875746999E-08 - 61, 18, 1.9780118079406259E-08, 1.5873613629791469E-08, 2.7339215073494001E-08, 2.7321670634627000E-08 - 61, 19, 3.6585976473783750E-09, 3.8486624377278551E-08, 2.7082949929027998E-08, 2.7082454966826001E-08 - 61, 20,-1.2104457686059771E-08, 3.8810671014025192E-08, 2.6816762317364999E-08, 2.6799458625986999E-08 - 61, 21,-2.0169405665127871E-08,-1.4027611613523641E-08, 2.6535272918719999E-08, 2.6538050929126999E-08 - 61, 22,-4.2652611167724140E-08,-2.9095642112579291E-08, 2.6242615552645001E-08, 2.6246638373177002E-08 - 61, 23, 8.9313465924684437E-10, 9.5200986060911763E-09, 2.5949087199543000E-08, 2.5944784252622000E-08 - 61, 24,-3.4078779112168460E-09,-3.7299217412466573E-08, 2.5664062431959001E-08, 2.5636128197291999E-08 - 61, 25, 4.0695776472107601E-08,-1.7135515675758431E-08, 2.5321996843710999E-08, 2.5355975312404999E-08 - 61, 26,-2.6037754530014981E-09, 1.0193130700765800E-08, 2.5027615851228001E-08, 2.4985557454081000E-08 - 61, 27, 9.2865456393948503E-09, 2.6780787089180259E-08, 2.4690301683027002E-08, 2.4661725965614001E-08 - 61, 28, 2.5554012749534961E-08,-1.4537228785195599E-08, 2.4335072132418999E-08, 2.4312955218539000E-08 - 61, 29, 2.5280999273120370E-08,-3.5637875215108240E-08, 2.3981249553552000E-08, 2.3984508619547001E-08 - 61, 30, 1.7582074742540351E-08, 2.9899262696523020E-10, 2.3590875442902999E-08, 2.3624057151017999E-08 - 61, 31,-1.9461396102115349E-08, 4.7193195834991026E-09, 2.3237939660633001E-08, 2.3252598256900002E-08 - 61, 32, 1.9286211346271760E-08, 2.0751588640982350E-08, 2.2837000296677999E-08, 2.2863737882031000E-08 - 61, 33,-1.5850408859360691E-09, 9.2103801168139082E-09, 2.2454528995757001E-08, 2.2488213095407000E-08 - 61, 34,-1.4160166665933209E-08, 1.6323826558519009E-08, 2.2066170956941000E-08, 2.2070417020504000E-08 - 61, 35,-1.9594216975646720E-08,-2.0211202389776729E-08, 2.1658068831209002E-08, 2.1658253551518001E-08 - 61, 36,-3.0488919401607970E-09,-1.7035973644770961E-08, 2.1221749084774001E-08, 2.1239313561716000E-08 - 61, 37,-2.7772349810590451E-08,-6.0257207365959393E-09, 2.0843263810329998E-08, 2.0814544446880999E-08 - 61, 38, 4.2172410190415502E-10, 5.5811510020746159E-09, 2.0398163224175999E-08, 2.0367963674340001E-08 - 61, 39,-1.1375231310601739E-08,-2.9951172321742539E-09, 1.9913796140790002E-08, 1.9920977592014001E-08 - 61, 40,-1.3969940626541909E-08, 2.0834768995022729E-08, 1.9464564924184000E-08, 1.9463704440782998E-08 - 61, 41, 1.6011818928376410E-09,-2.0980672613247059E-08, 1.8966002081239999E-08, 1.8977946592149999E-08 - 61, 42, 3.0729257136870482E-08, 4.2547884423009183E-09, 1.8498067316009999E-08, 1.8513827209368000E-08 - 61, 43,-1.4520699531900530E-08,-5.5374200285790552E-08, 1.7986525028163999E-08, 1.7993779743662000E-08 - 61, 44,-2.0015657687847040E-08, 2.2249604901400101E-08, 1.7461607144120001E-08, 1.7513785622205000E-08 - 61, 45, 2.4953631732441051E-08,-4.8949003577835138E-09, 1.6937135211987000E-08, 1.6935406189246999E-08 - 61, 46, 1.1233947058348081E-08, 1.3245605516922701E-08, 1.6391851544655001E-08, 1.6391430604012001E-08 - 61, 47,-9.6766872945171317E-09, 8.6435694090469070E-09, 1.5840862517631001E-08, 1.5840398739222001E-08 - 61, 48, 1.4072970334352799E-08, 7.2205829690304423E-09, 1.5289600281169002E-08, 1.5261373427984999E-08 - 61, 49,-5.7078183596301883E-08, 8.5056731039286548E-09, 1.4645072353037001E-08, 1.4642456157803000E-08 - 61, 50, 7.1930503799837444E-09,-4.1268630993802851E-08, 1.4071889019193000E-08, 1.4080954015543001E-08 - 61, 51,-4.7711693604012778E-08, 3.8042974530092843E-08, 1.3406531767408000E-08, 1.3405268228532000E-08 - 61, 52,-7.2881536601116533E-09,-3.5168580491916767E-08, 1.2772429218752001E-08, 1.2779785135331999E-08 - 61, 53,-3.2908535821920413E-08,-9.8534605160237535E-11, 1.2096298629322000E-08, 1.2109173691288999E-08 - 61, 54, 1.4528655891089940E-08, 2.1259484567587670E-08, 1.1375768484231000E-08, 1.1392398365321000E-08 - 61, 55,-3.6668880715493373E-08, 2.3869613556866231E-08, 1.0692837388740000E-08, 1.0700973547027000E-08 - 61, 56,-2.4389956927733500E-08, 7.4085640226178401E-08, 9.8366175566569001E-09, 9.8328246147547992E-09 - 61, 57, 1.1679374141286490E-08, 2.4537594708907748E-08, 8.9909480451632004E-09, 8.9619428914987003E-09 - 61, 58,-2.2395473238179290E-08, 2.6334614456163300E-09, 8.0945856244875994E-09, 8.1461175118100001E-09 - 61, 59, 5.1684992799776827E-08, 3.3593287010680960E-09, 6.4916289100676999E-09, 6.5110065464841002E-09 - 61, 60,-6.7508690586049999E-08, 3.3375087509244801E-08, 4.0281571094318998E-09, 4.0409157653684004E-09 - 61, 61, 3.6241042619557372E-08,-4.8316793410786897E-08, 3.5422447267278999E-09, 3.5464042630630999E-09 - 62, 0,-1.5237515803023880E-08, 0.0000000000000000E+00, 3.0899384610665003E-08, 0.0000000000000000E+00 - 62, 1,-1.8531919623790820E-08,-3.8890275894473930E-08, 3.0857463237215000E-08, 3.0849004371518003E-08 - 62, 2, 2.6108324327238651E-08,-1.9842457978630911E-08, 3.0778774811834997E-08, 3.0789573675870001E-08 - 62, 3, 4.2407549595154972E-08,-1.6179649107590579E-08, 3.0732644533909001E-08, 3.0776422310528002E-08 - 62, 4,-7.7063873401823322E-09, 1.2432801385891009E-08, 3.0662884762134998E-08, 3.0695472092522001E-08 - 62, 5, 3.2581293698565353E-08,-5.6766528212745827E-09, 3.0595970727672997E-08, 3.0632027935106002E-08 - 62, 6,-1.4658916811607160E-08, 5.9576260707113587E-08, 3.0420128411111999E-08, 3.0627102329291000E-08 - 62, 7,-3.9890929756252423E-09,-6.3333303134135598E-09, 3.0451327110902997E-08, 3.0382279564940999E-08 - 62, 8, 9.5311643639130883E-09,-1.1989756675781740E-08, 3.0275864932213997E-08, 3.0345055804840998E-08 - 62, 9, 1.2603488219635900E-08, 1.2334063269542271E-09, 3.0122831701590998E-08, 3.0223560113748999E-08 - 62, 10,-3.3586311941422273E-08,-4.1395503370154489E-09, 3.0040185550076000E-08, 3.0056930640781000E-08 - 62, 11, 6.1661044849132983E-09,-2.3408994990373239E-08, 2.9890953250714997E-08, 2.9873583825149000E-08 - 62, 12, 4.8757776369615850E-09,-1.5201455089911658E-08, 2.9729227463772999E-08, 2.9756335277573001E-08 - 62, 13, 1.5883538421322579E-08,-4.2983444724078311E-09, 2.9533057620480002E-08, 2.9561253907258999E-08 - 62, 14, 1.3433005727536640E-08,-1.1422657934616931E-08, 2.9385186996825001E-08, 2.9379089108769999E-08 - 62, 15,-1.2029071301056259E-08,-2.7557318117520870E-08, 2.9164588495436001E-08, 2.9159745329716001E-08 - 62, 16, 7.7882338370000420E-09,-2.5350499849952258E-08, 2.8975974592533000E-08, 2.8984176230942001E-08 - 62, 17, 4.9838941593198823E-09, 2.7034807211267029E-09, 2.8754348089131999E-08, 2.8746828345186001E-08 - 62, 18,-1.7288389745254151E-08, 1.2489286978839300E-08, 2.8541383087340999E-08, 2.8526236242896001E-08 - 62, 19, 1.2281820432965430E-08, 2.4847339811281221E-08, 2.8309658046831999E-08, 2.8301636850533001E-08 - 62, 20,-1.0337069721714379E-09,-1.9970529921677881E-08, 2.8049109418860999E-08, 2.8030746564452998E-08 - 62, 21,-2.3690547845982088E-08, 1.2102811683585121E-08, 2.7792538691541998E-08, 2.7786638629250999E-08 - 62, 22,-1.5988580605384198E-08,-8.4821073918670880E-09, 2.7519644877601999E-08, 2.7511107420759998E-08 - 62, 23,-3.2092207693364531E-08, 1.5436539399695551E-08, 2.7238959567649999E-08, 2.7239027009662001E-08 - 62, 24,-1.1143510954642609E-09,-2.5457776578700371E-08, 2.6961121347761001E-08, 2.6947822161920000E-08 - 62, 25,-9.1111602875161020E-09,-2.4785466123360571E-08, 2.6658076771343999E-08, 2.6701549565392998E-08 - 62, 26, 1.5086398695480401E-08,-1.7569935717630292E-08, 2.6375550939418999E-08, 2.6342858500637001E-08 - 62, 27, 1.1961857516279410E-08, 2.9798041182599981E-08, 2.6067827077262001E-08, 2.6033490635882999E-08 - 62, 28,-8.3799048164308070E-09,-3.0645532958038268E-08, 2.5732466066547999E-08, 2.5725612434688001E-08 - 62, 29,-1.5772310985773290E-09,-4.9885344027723107E-08, 2.5401315152775999E-08, 2.5396678759136000E-08 - 62, 30, 1.4967123397842400E-09, 1.4874585323265660E-08, 2.5047512996626000E-08, 2.5077816077718001E-08 - 62, 31, 5.9851869110575440E-09,-2.3356982632812680E-08, 2.4699854074199999E-08, 2.4711363432541000E-08 - 62, 32,-1.7785868028923150E-08, 4.0816476569953163E-08, 2.4342725900309000E-08, 2.4363780210493000E-08 - 62, 33,-2.5858098267769959E-08, 6.7459020995067190E-09, 2.3963515784113000E-08, 2.3985497687817999E-08 - 62, 34,-7.9590917717540688E-10,-1.4623329643685180E-08, 2.3612072059608001E-08, 2.3608974189382000E-08 - 62, 35, 2.9509128667746738E-08,-2.4503406605707760E-08, 2.3210662682604000E-08, 2.3216582152943000E-08 - 62, 36,-3.6209574607675588E-09,-4.5808145535708322E-09, 2.2810415937261002E-08, 2.2830908141270999E-08 - 62, 37,-2.0162433359424589E-08, 1.1428781780599639E-08, 2.2429910548439001E-08, 2.2395218417138001E-08 - 62, 38,-2.5236457595484909E-08, 5.7275103076928867E-08, 2.2029043752547000E-08, 2.1993991989529001E-08 - 62, 39, 1.1712322362681351E-08, 3.2980073222288307E-08, 2.1556503510032001E-08, 2.1557378158389001E-08 - 62, 40, 1.6212797664427780E-08, 2.2758704808720628E-08, 2.1104574539712999E-08, 2.1112398235205001E-08 - 62, 41, 3.5410149297695009E-09, 1.3804061048909900E-08, 2.0652618973037000E-08, 2.0661906460539999E-08 - 62, 42, 1.3384187922083369E-08,-8.7289276407054339E-09, 2.0160632242992001E-08, 2.0172836066718001E-08 - 62, 43, 8.6505787374064707E-11,-4.3873275702569212E-08, 1.9702914912705999E-08, 1.9705208799417000E-08 - 62, 44,-6.3056408848101623E-09,-3.3079824827468210E-09, 1.9165140202643998E-08, 1.9199752705036000E-08 - 62, 45, 2.2754650441098491E-08, 5.1390233117784097E-09, 1.8672642852935999E-08, 1.8672366525642999E-08 - 62, 46, 3.0819811511665142E-09,-1.4273101957530629E-08, 1.8110297051000999E-08, 1.8118374631736999E-08 - 62, 47,-7.5348848260711023E-09,-1.8025989986679691E-08, 1.7569075345234000E-08, 1.7553686548600002E-08 - 62, 48, 1.3809380157297630E-08,-3.2442066624646798E-08, 1.7017668052383999E-08, 1.6983181361529000E-08 - 62, 49, 3.1318184977251897E-08,-6.2576428901603979E-08, 1.6419489663281001E-08, 1.6420485474881001E-08 - 62, 50, 8.9179426240111851E-10, 3.8099888193077211E-10, 1.5771831598760000E-08, 1.5782294022313002E-08 - 62, 51,-2.7848743062215911E-08, 9.2974945957549845E-09, 1.5170113705013000E-08, 1.5174378372706000E-08 - 62, 52, 9.7184651794905158E-09, 3.1420874280204581E-08, 1.4471893156177000E-08, 1.4471749897231000E-08 - 62, 53,-1.7804636772243061E-08, 5.1381699601684186E-09, 1.3815917374777000E-08, 1.3826510558010001E-08 - 62, 54, 2.1683751028216750E-08, 7.6022286441440682E-10, 1.3107099205432000E-08, 1.3117702719641000E-08 - 62, 55,-7.9774227183647397E-09, 2.7134453637222849E-08, 1.2363941714681000E-08, 1.2344971695433000E-08 - 62, 56, 5.0374589917884743E-08, 2.6010517162053260E-08, 1.1624747797484999E-08, 1.1649282902021001E-08 - 62, 57, 5.6780152753494277E-08,-1.2392053686854310E-08, 1.0714694293530001E-08, 1.0703691238204000E-08 - 62, 58, 1.0037816453422260E-08,-6.5769348915798304E-10, 9.7858082848445002E-09, 9.7997385850099002E-09 - 62, 59,-6.9343878286298350E-09, 2.2798363187571119E-09, 8.8443742798667000E-09, 8.8441752363808005E-09 - 62, 60,-2.7079507981401400E-08,-2.2071484646482510E-08, 7.0724819995894001E-09, 7.0469681831366998E-09 - 62, 61, 1.6646941052809011E-08, 3.8330530122948437E-08, 4.3856613360477998E-09, 4.3659226255275003E-09 - 62, 62,-1.0215911116872690E-08,-1.6276937368969710E-08, 3.9353058949744998E-09, 3.9810735624987004E-09 - 63, 0,-7.5943791443928821E-08, 0.0000000000000000E+00, 3.1788788081735000E-08, 0.0000000000000000E+00 - 63, 1, 2.7691886944838320E-08, 2.0596816742636831E-08, 3.1769147861156999E-08, 3.1753619461662002E-08 - 63, 2, 2.9715992967965741E-08,-1.7168373104857661E-08, 3.1649463056010000E-08, 3.1653987824499997E-08 - 63, 3, 3.6873414498162663E-08,-2.5481699502382870E-08, 3.1641472269514002E-08, 3.1677429317383001E-08 - 63, 4,-8.6167308610847245E-09,-3.2519014195806118E-08, 3.1538272163824002E-08, 3.1564099509351999E-08 - 63, 5, 4.0651639086864612E-08,-1.8003473493273741E-08, 3.1501102349586000E-08, 3.1526921087187999E-08 - 63, 6, 2.1585136644879010E-08, 1.7570149368159169E-09, 3.1314597281618001E-08, 3.1482860321529002E-08 - 63, 7,-1.1290846936463471E-08, 1.7467714563693679E-08, 3.1357124657597000E-08, 3.1291249729190000E-08 - 63, 8, 2.3337178975136330E-08, 3.2342114375696358E-10, 3.1163002352456001E-08, 3.1225726436892001E-08 - 63, 9,-2.2275180302268201E-08, 2.7953733130668779E-08, 3.1043261825079001E-08, 3.1122871550337001E-08 - 63, 10,-1.2816806413405159E-08,-3.0267227144410613E-08, 3.0937953653035003E-08, 3.0951764968447003E-08 - 63, 11, 2.5801605711006790E-08,-8.7939024430990625E-09, 3.0805558925134999E-08, 3.0804259107503002E-08 - 63, 12,-1.8389379773050990E-08, 1.8646342369911689E-09, 3.0649847639131001E-08, 3.0669072099572003E-08 - 63, 13,-8.4856291370786422E-09,-1.5355320220804460E-08, 3.0469816249391998E-08, 3.0505246756316003E-08 - 63, 14,-5.7087279085419957E-10, 1.5452907644911679E-08, 3.0318948805765997E-08, 3.0324007902354998E-08 - 63, 15,-2.1011376973990970E-08,-1.3767017009901371E-08, 3.0128529621199997E-08, 3.0127756213171999E-08 - 63, 16,-2.2581395192308731E-09,-1.0872859509997521E-08, 2.9945159980149001E-08, 2.9952971532521002E-08 - 63, 17, 1.8134489519029020E-08, 1.8866335532876371E-08, 2.9758296026211000E-08, 2.9744052489077001E-08 - 63, 18, 1.4017258991706780E-08, 4.9816702576110223E-09, 2.9545388272229000E-08, 2.9524824703803999E-08 - 63, 19,-1.1532560068250120E-08,-1.0690957080519140E-08, 2.9343677311349000E-08, 2.9349568233097001E-08 - 63, 20,-1.6216888705302001E-08,-1.3947410885202230E-08, 2.9094802913210999E-08, 2.9078167145358002E-08 - 63, 21, 7.4627265733165043E-09,-1.3996209166237691E-08, 2.8852221598228001E-08, 2.8856326439609999E-08 - 63, 22, 4.0529018113074132E-08, 3.5932579945092962E-08, 2.8606627835357001E-08, 2.8611769118975999E-08 - 63, 23,-7.4140290180824763E-09,-7.5820510940248867E-09, 2.8351243201945001E-08, 2.8348410100406000E-08 - 63, 24,-8.2268754280088796E-09,-3.6127920586659491E-08, 2.8099316590913001E-08, 2.8085033998541999E-08 - 63, 25,-2.1304690264748699E-08,-1.4957233990301941E-08, 2.7818781583144000E-08, 2.7857253430978001E-08 - 63, 26, 1.7685548783221130E-08, 1.9841475566080699E-08, 2.7561225141064000E-08, 2.7533904333766000E-08 - 63, 27,-1.1455959894545491E-08,-1.6962190771507431E-08, 2.7263035016433000E-08, 2.7238975300826000E-08 - 63, 28, 1.6142319292065671E-08,-1.9732834012873990E-09, 2.6965648379599002E-08, 2.6952200979715999E-08 - 63, 29,-2.1866482064642398E-09, 2.4083746321721769E-08, 2.6658685034848000E-08, 2.6655820652441001E-08 - 63, 30, 3.1877162289184529E-09,-6.1278281164164450E-09, 2.6327267024295999E-08, 2.6353310828262000E-08 - 63, 31,-1.0248441393665220E-08,-2.0206976321949530E-09, 2.6022900751786999E-08, 2.6030111430457000E-08 - 63, 32,-2.7234181345502660E-08,-2.1901469298433940E-08, 2.5663574980624000E-08, 2.5692830419268000E-08 - 63, 33,-1.3681752825881160E-08,-9.8587973153148841E-09, 2.5337937480627999E-08, 2.5360669600910000E-08 - 63, 34, 2.4508437244074708E-09,-1.1639560393034890E-08, 2.4983011719764000E-08, 2.4992605983647001E-08 - 63, 35, 2.6985356025638891E-08,-1.3436037276956600E-08, 2.4642417107931999E-08, 2.4636312223850999E-08 - 63, 36, 8.5539703263784263E-09,-3.8432582882081003E-08, 2.4256750449971000E-08, 2.4281763669311001E-08 - 63, 37, 4.4998393615860222E-11,-1.6988121256300739E-09, 2.3896296009102000E-08, 2.3874720146918001E-08 - 63, 38, 2.3426928395581240E-09, 1.9971330821829790E-08, 2.3522683351386999E-08, 2.3488141269059000E-08 - 63, 39, 2.1915572602372571E-08, 3.3133479841437401E-08, 2.3094047136546998E-08, 2.3099717186653002E-08 - 63, 40,-1.4030583486448660E-08, 4.1440567158597643E-09, 2.2667659459006001E-08, 2.2659117922604001E-08 - 63, 41, 1.0604175304375370E-08, 3.9141296193608923E-08, 2.2227935811114000E-08, 2.2237735576680001E-08 - 63, 42,-6.2309782507947553E-09,-1.2809717854969799E-08, 2.1774631481078999E-08, 2.1795705376317999E-08 - 63, 43, 1.8283900942308952E-08,-1.1825406610819371E-08, 2.1315640728736000E-08, 2.1314607641428001E-08 - 63, 44,-1.4350276195345560E-08,-1.4573448661338410E-08, 2.0831227182817999E-08, 2.0879333662931999E-08 - 63, 45,-1.0734452180132770E-08,-5.6054527630036539E-09, 2.0339503774517000E-08, 2.0339808806058000E-08 - 63, 46, 1.7038843727042671E-08, 8.4339169316858392E-11, 1.9831564217717999E-08, 1.9837057528383000E-08 - 63, 47,-1.4760539493376309E-08,-3.3815780174300388E-08, 1.9269223588422000E-08, 1.9270992591362999E-08 - 63, 48,-1.6937755172712510E-08, 2.5692447630744990E-09, 1.8743670418483000E-08, 1.8708822726706000E-08 - 63, 49, 1.2180462333865850E-08, 1.2432215099747449E-08, 1.8150753431939001E-08, 1.8147877004548999E-08 - 63, 50,-1.2255077769720080E-08,-2.4908844982229320E-08, 1.7585995615198998E-08, 1.7573175683667001E-08 - 63, 51, 5.6630020038144770E-09,-2.4457413764056681E-08, 1.6892509909201000E-08, 1.6899643662990999E-08 - 63, 52,-2.4661508290094039E-08,-2.2921855890018601E-08, 1.6283753946873001E-08, 1.6278810908301000E-08 - 63, 53, 1.1637836156486369E-08, 1.0189376128409431E-08, 1.5543452408284000E-08, 1.5574108708275999E-08 - 63, 54, 1.2442202938712420E-08,-6.5595671661389934E-09, 1.4899962000260999E-08, 1.4890626887338000E-08 - 63, 55, 7.2766949901697839E-09,-1.1520326893344109E-08, 1.4157789654027000E-08, 1.4132495738149000E-08 - 63, 56, 3.0654557296558080E-08,-6.3128240915831324E-08, 1.3364697751280000E-08, 1.3374943369799000E-08 - 63, 57, 4.4031401037442877E-08,-5.6483314238897011E-08, 1.2631825445683001E-08, 1.2626457665838000E-08 - 63, 58,-5.3451733517514187E-08,-5.9259095870344957E-08, 1.1583037117071000E-08, 1.1619449922422999E-08 - 63, 59,-1.2320226727700480E-08, 2.5111463673349749E-08, 1.0656144427685000E-08, 1.0647130038886000E-08 - 63, 60,-5.4728023000676557E-08,-4.2272795648675733E-08, 9.6325076384051996E-09, 9.6246810610475995E-09 - 63, 61,-4.2589592191794903E-08, 4.3703780610828117E-08, 7.5674953593446006E-09, 7.5357656199930002E-09 - 63, 62,-2.0580522002390040E-08,-1.5820159253095019E-08, 4.6548999034524001E-09, 4.6554384786422000E-09 - 63, 63,-2.5449001938065469E-08,-6.4648636006584637E-09, 4.3945918986273000E-09, 4.3940231470794001E-09 - 64, 0,-7.4449382843840351E-08, 0.0000000000000000E+00, 3.2469647070265998E-08, 0.0000000000000000E+00 - 64, 1,-3.2134535975874391E-08,-1.6485777335694871E-08, 3.2454745257944002E-08, 3.2434977864908997E-08 - 64, 2,-3.7492543129441683E-09, 1.3989217029388180E-08, 3.2309370985183002E-08, 3.2315465877613002E-08 - 64, 3, 1.1949846982847500E-08,-8.4259339503889063E-10, 3.2322092531969003E-08, 3.2348452853436997E-08 - 64, 4,-2.2267209979357219E-08, 1.2605373709994350E-08, 3.2213208295114999E-08, 3.2228604597233001E-08 - 64, 5, 3.3873868285291153E-10,-1.8065489002329500E-08, 3.2179315900339997E-08, 3.2203679019807001E-08 - 64, 6, 8.9096702309749915E-09,-4.9286018744709774E-09, 3.2002661684858001E-08, 3.2153042213845998E-08 - 64, 7,-3.5382643338429680E-09,-3.4717499330702748E-09, 3.2030776901566998E-08, 3.1967560910291000E-08 - 64, 8,-1.1774387069060490E-08, 1.1748360948792639E-08, 3.1867526158036998E-08, 3.1906991329027999E-08 - 64, 9,-1.4973808522952140E-08, 1.1302390381946730E-08, 3.1735480267309999E-08, 3.1803750370269003E-08 - 64, 10, 1.3055379067334780E-08,-4.0030665164994817E-08, 3.1642994183024000E-08, 3.1659068654116000E-08 - 64, 11, 3.2870122447696898E-10, 1.8224206276661419E-08, 3.1505427943118999E-08, 3.1507104462411001E-08 - 64, 12,-5.9965209405000724E-09, 1.3853505417795390E-09, 3.1372222098865000E-08, 3.1398588638998998E-08 - 64, 13, 9.0410021707575163E-09,-7.5510374028616690E-09, 3.1195451212704999E-08, 3.1227732514210999E-08 - 64, 14,-3.7209465964807762E-08,-4.2987766684075811E-10, 3.1071665425003001E-08, 3.1072639410598001E-08 - 64, 15,-4.4379149684779142E-08, 2.3299596621458512E-08, 3.0881130019812000E-08, 3.0867672556243000E-08 - 64, 16,-1.7626855847295689E-08, 6.2157194776472199E-09, 3.0724785778560001E-08, 3.0736764269151002E-08 - 64, 17, 3.9374075132516584E-09, 6.7448865385620587E-09, 3.0538770599093002E-08, 3.0529263149843003E-08 - 64, 18, 2.5398015896857329E-08,-2.0608960605843340E-08, 3.0361043499961998E-08, 3.0338782061530003E-08 - 64, 19, 1.7961973559312631E-09,-1.0707300577863900E-08, 3.0164194599927000E-08, 3.0160125008877001E-08 - 64, 20, 1.1344068421875490E-08,-1.0337467631336110E-08, 2.9953307109461001E-08, 2.9933095266775999E-08 - 64, 21,-1.6329697188216859E-08,-1.1599086132585630E-08, 2.9717169632819001E-08, 2.9711173180534001E-08 - 64, 22,-1.1826052197356130E-08,-6.0245486094387539E-10, 2.9500063566672999E-08, 2.9499005015553001E-08 - 64, 23, 9.7865609250484410E-09,-2.1254358112118091E-09, 2.9264116813437001E-08, 2.9264873545304999E-08 - 64, 24, 1.3326889233735801E-08,-1.8353970354225579E-08, 2.9023518703788000E-08, 2.9023226755794000E-08 - 64, 25, 1.6039271648389301E-09, 1.1577484419109340E-09, 2.8789624809696001E-08, 2.8819499471532000E-08 - 64, 26, 6.7419341769582543E-09, 4.3803129147250249E-09, 2.8544963657049998E-08, 2.8523093276426999E-08 - 64, 27,-4.2572441442897538E-08, 1.6447796974506020E-08, 2.8290720643729001E-08, 2.8264355486304000E-08 - 64, 28, 1.8019192646612490E-08, 3.5679029733359628E-08, 2.8007028532088002E-08, 2.7989249478314999E-08 - 64, 29, 1.2862418179503711E-08, 3.2413425183845530E-09, 2.7739637556347002E-08, 2.7735946195360999E-08 - 64, 30, 2.7932509454073880E-08,-4.1264183964135603E-08, 2.7432543622417000E-08, 2.7453969274017000E-08 - 64, 31, 1.2080390048981080E-08,-8.2059721167175321E-09, 2.7161311684665000E-08, 2.7164010755888001E-08 - 64, 32,-1.6945265954974461E-08, 3.8943986894285802E-09, 2.6839206279085000E-08, 2.6864706129276000E-08 - 64, 33,-1.4229095853173079E-08, 2.9762120309613048E-09, 2.6533169138690999E-08, 2.6545277767147000E-08 - 64, 34, 3.1612763298350502E-08, 3.8557911234177184E-09, 2.6230895148950001E-08, 2.6232214116561000E-08 - 64, 35,-2.2895209443881589E-08,-2.5036190661753769E-08, 2.5888134173403999E-08, 2.5891978370895998E-08 - 64, 36,-2.6383351436709791E-08, 1.3499765909527250E-08, 2.5568506439187000E-08, 2.5591057885523999E-08 - 64, 37, 9.6902701371694182E-09,-7.4978812404463664E-09, 2.5228676667123999E-08, 2.5195888942415002E-08 - 64, 38, 3.2302734095519121E-08, 5.7763257225849601E-09, 2.4883869343010999E-08, 2.4856156158987000E-08 - 64, 39, 9.2014524965311663E-09,-4.3343440936829571E-08, 2.4474637107729001E-08, 2.4486062752519001E-08 - 64, 40, 1.2500707896537740E-08,-2.3071652648993489E-08, 2.4099171573020000E-08, 2.4100888224358999E-08 - 64, 41,-3.4261154811809672E-08, 9.9700017766030358E-09, 2.3680485481563999E-08, 2.3693965793871001E-08 - 64, 42,-6.0097416286279361E-09,-7.7594686952157254E-09, 2.3258614679426001E-08, 2.3290563334430001E-08 - 64, 43, 5.5981717673728683E-09,-7.1833355198882459E-09, 2.2841530991026999E-08, 2.2843955303707000E-08 - 64, 44,-4.5141217304534722E-09,-1.6580659027018880E-08, 2.2351969213548002E-08, 2.2428047600589002E-08 - 64, 45, 2.7892950242233871E-08,-4.8742945589521472E-09, 2.1945910674227001E-08, 2.1938482556991001E-08 - 64, 46, 2.2514602818244649E-08, 8.6996466927462183E-09, 2.1436575485056000E-08, 2.1433600306314001E-08 - 64, 47,-1.2479549898600280E-08, 9.3920250571008670E-10, 2.0936577631763999E-08, 2.0936050781890001E-08 - 64, 48,-2.4785510931200299E-08, 1.2907566793828089E-08, 2.0400519048963002E-08, 2.0371707463557999E-08 - 64, 49, 1.6972193536732529E-08,-3.7873617031007326E-09, 1.9859587691329999E-08, 1.9835745473420999E-08 - 64, 50,-3.0299127083561782E-08,-3.0484224224850741E-08, 1.9282627399332000E-08, 1.9280460707405001E-08 - 64, 51,-1.3252912803313040E-08, 3.9640373769328083E-09, 1.8699299221339000E-08, 1.8676703115534001E-08 - 64, 52,-3.3698528811638562E-08,-5.5677655380967583E-08, 1.8005047130137000E-08, 1.7999458714196001E-08 - 64, 53,-9.2669226579688220E-09, 1.1495155773228341E-08, 1.7370031009717998E-08, 1.7390555619792001E-08 - 64, 54, 4.6573109109871371E-09,-2.2315904896199261E-08, 1.6643101150645999E-08, 1.6653925027734000E-08 - 64, 55,-2.6882956434639729E-08,-2.3961277104260370E-08, 1.5975990249501000E-08, 1.5956654675246001E-08 - 64, 56,-6.5945074783442799E-10,-2.6234657020469080E-08, 1.5201177801869999E-08, 1.5197785454174000E-08 - 64, 57,-3.7246900749063552E-08, 3.7724029179669770E-08, 1.4404426139175000E-08, 1.4421710479838001E-08 - 64, 58,-6.8754005123588049E-08,-5.3971610062573252E-08, 1.3602106798003000E-08, 1.3612084954101001E-08 - 64, 59,-4.8033710371110173E-09, 6.2434348030641530E-08, 1.2514549758802000E-08, 1.2523246322021999E-08 - 64, 60, 2.3488664348531219E-08, 2.1329346999039952E-09, 1.1534245859858999E-08, 1.1561520155938000E-08 - 64, 61,-4.0000339574457222E-08, 1.7497056396858609E-08, 1.0380153150478001E-08, 1.0386882768325001E-08 - 64, 62, 3.9607914432958911E-08,-2.2609578386970621E-08, 8.2293858627941996E-09, 8.1684242860042996E-09 - 64, 63,-2.6201067067444270E-08,-8.3667973718990166E-09, 5.3011083001746000E-09, 5.2136201295475996E-09 - 64, 64, 4.0556285026030066E-09,-9.9324223921493445E-09, 4.8974032083497996E-09, 4.7700597193751002E-09 - 65, 0,-5.0373649385218523E-08, 0.0000000000000000E+00, 3.2979711252249997E-08, 0.0000000000000000E+00 - 65, 1, 6.1440539320017990E-09,-7.3607481231004046E-09, 3.2915689104917000E-08, 3.2887523177812002E-08 - 65, 2,-1.4635090586904640E-08,-5.1984539933336634E-09, 3.2773276880178002E-08, 3.2772701922588001E-08 - 65, 3, 6.2562626963645239E-09,-6.8179442273111503E-09, 3.2781366171998998E-08, 3.2806763529898999E-08 - 65, 4, 3.3705657305245552E-08, 2.1351986563289321E-08, 3.2693583668430999E-08, 3.2691936836181997E-08 - 65, 5,-1.9246751793774729E-08,-2.7083970664319688E-08, 3.2640846277649001E-08, 3.2656796641295997E-08 - 65, 6,-4.5288519985819274E-09, 1.9039640424468279E-08, 3.2498506571673998E-08, 3.2614136814249001E-08 - 65, 7,-2.5659830547821631E-08, 1.1022507125668020E-08, 3.2491301226676998E-08, 3.2441564911273997E-08 - 65, 8, 7.9347509260707208E-09, 3.5511712100806723E-08, 3.2357779931031997E-08, 3.2388709082018997E-08 - 65, 9, 1.5401658170595368E-08, 5.3594821677913671E-09, 3.2224977307155999E-08, 3.2279866585059003E-08 - 65, 10,-2.5809139451387951E-09,-8.3292193719512204E-09, 3.2141810598765000E-08, 3.2150837887446000E-08 - 65, 11, 1.4100032511657780E-09,-2.2518063783763170E-08, 3.2005628284506002E-08, 3.2006608753955002E-08 - 65, 12, 4.3118968323305323E-09,-1.1618847909717671E-08, 3.1892843409960002E-08, 3.1907768738308998E-08 - 65, 13, 6.8293793248579268E-09,-1.0878535838740319E-08, 3.1723134332280002E-08, 3.1751737073412998E-08 - 65, 14,-1.1081929376683840E-08,-1.1895911150363309E-08, 3.1611283303060003E-08, 3.1605936837869002E-08 - 65, 15, 9.7889811481527806E-09, 1.1904730172792501E-08, 3.1435402549105002E-08, 3.1433129893354002E-08 - 65, 16,-3.1444998525805007E-08,-2.8509849941932471E-09, 3.1290894003443002E-08, 3.1297677607640002E-08 - 65, 17, 1.6247572368627081E-08, 4.5265727055545253E-09, 3.1137010448426997E-08, 3.1124521124550999E-08 - 65, 18, 4.1676774145368996E-09, 2.7571868182857240E-09, 3.0963654606114001E-08, 3.0938638148955998E-08 - 65, 19,-3.9612755821232552E-08, 2.5208021030743451E-08, 3.0800560935773999E-08, 3.0794345136491999E-08 - 65, 20, 2.4577179208149300E-09,-1.4391668058398869E-08, 3.0591855000919001E-08, 3.0579658758844003E-08 - 65, 21,-9.6027204302834200E-09,-1.7409428699283549E-08, 3.0398393354832003E-08, 3.0385960229433001E-08 - 65, 22,-1.3545664642899019E-09, 8.3126594875122052E-09, 3.0180801346034999E-08, 3.0192046277791997E-08 - 65, 23, 8.2090969518756101E-09, 5.2765129898844763E-09, 2.9986479087893001E-08, 2.9983628914561001E-08 - 65, 24, 3.0660605977806902E-08,-3.6280898587319020E-08, 2.9770550723870000E-08, 2.9764911345755001E-08 - 65, 25,-2.4574278556145440E-08, 1.1973212969424260E-08, 2.9558480919908998E-08, 2.9583263797398999E-08 - 65, 26,-1.3646774177153830E-08, 1.9519863897160289E-08, 2.9338331335536998E-08, 2.9324100088597000E-08 - 65, 27,-3.1764100657395569E-09, 9.6565869752210594E-09, 2.9099156917231999E-08, 2.9081287193371001E-08 - 65, 28,-2.6050493131592851E-09, 2.8153271783866631E-08, 2.8852536408737001E-08, 2.8842513724126001E-08 - 65, 29, 1.9295938033527311E-08,-1.3381067137353790E-08, 2.8602970268677000E-08, 2.8600560982178001E-08 - 65, 30, 3.3409535004024399E-09,-1.8397873749283459E-08, 2.8338379299195000E-08, 2.8356843307657001E-08 - 65, 31, 1.6279693651416600E-08, 2.7030649219729581E-09, 2.8087474574533999E-08, 2.8098559723041999E-08 - 65, 32, 2.8202449046635931E-09,-1.5206448188486401E-08, 2.7804555284990998E-08, 2.7826173376331001E-08 - 65, 33,-1.5181683809517251E-08,-1.1246193249582661E-08, 2.7538398774413000E-08, 2.7541358921519000E-08 - 65, 34, 1.2475890277340461E-09,-1.0434873744912081E-08, 2.7238177485005000E-08, 2.7250753359791000E-08 - 65, 35, 3.9654905508864327E-09,-7.7723999802645494E-09, 2.6952737718756999E-08, 2.6957420890022999E-08 - 65, 36, 5.2017553859623486E-09, 1.7749107417294802E-08, 2.6652017561657001E-08, 2.6687185762796998E-08 - 65, 37,-1.6850716526981921E-08, 1.6665128322498810E-08, 2.6357085872437000E-08, 2.6343617050841999E-08 - 65, 38,-2.7203615343007070E-08, 1.2988587498351540E-08, 2.6052223606177999E-08, 2.6030785618225000E-08 - 65, 39, 2.3386662955670499E-08,-7.1719720833468350E-09, 2.5674472170111000E-08, 2.5686142255226000E-08 - 65, 40,-3.7129383033347820E-09,-3.0563488672475012E-08, 2.5346308995019999E-08, 2.5341196549413000E-08 - 65, 41,-7.8853995174648540E-09,-1.6984244807583050E-08, 2.4968920088791001E-08, 2.4985023018826001E-08 - 65, 42,-1.4020203210659219E-08, 6.4842609462148248E-09, 2.4588982653832001E-08, 2.4597605870739000E-08 - 65, 43, 1.4769575954496240E-08,-1.2399029796778120E-08, 2.4203564679586001E-08, 2.4215974545331999E-08 - 65, 44, 3.1050160585356162E-08, 1.3375359606085030E-08, 2.3769369219086999E-08, 2.3817917108554001E-08 - 65, 45, 1.0781936561002179E-08, 2.9811617410850281E-09, 2.3384503354282000E-08, 2.3375145743232999E-08 - 65, 46, 5.2390036686861051E-10,-1.8310423713543740E-08, 2.2948406939992999E-08, 2.2936407198830001E-08 - 65, 47,-1.2110801421473970E-09, 2.7016685001210180E-08, 2.2450453123629000E-08, 2.2456081230169999E-08 - 65, 48, 3.1916534239582331E-09, 1.7788682233757571E-08, 2.1988649979848001E-08, 2.1971128745011002E-08 - 65, 49, 1.5238141773081521E-08, 1.6273399730965211E-08, 2.1425192131214999E-08, 2.1426921713955001E-08 - 65, 50, 3.6261009575424311E-08, 2.2138198381060049E-08, 2.0945124995160001E-08, 2.0938721502526999E-08 - 65, 51,-3.9446887953643409E-10, 2.7898937929553481E-08, 2.0342451300917999E-08, 2.0334217672456001E-08 - 65, 52,-3.6958133701971453E-08,-1.6167801717580831E-08, 1.9758559372128001E-08, 1.9740903442491001E-08 - 65, 53,-5.0835342477612115E-10, 3.6715874633191400E-09, 1.9077280683014999E-08, 1.9075835670897001E-08 - 65, 54,-2.5527075284464231E-08, 1.0008427383058449E-08, 1.8461478061087999E-08, 1.8456927917505001E-08 - 65, 55,-1.6940959074174489E-08, 1.1678890022092431E-10, 1.7712458839258998E-08, 1.7697153706285002E-08 - 65, 56, 2.8881330745768340E-08, 1.4622436658367160E-08, 1.7038181773529001E-08, 1.7042579149056001E-08 - 65, 57,-2.4254486119916029E-08, 1.6180550426335621E-08, 1.6253776195238000E-08, 1.6254276494611999E-08 - 65, 58,-1.6510498842722010E-08, 2.7629609662246990E-08, 1.5409911518840002E-08, 1.5452109316826999E-08 - 65, 59,-3.3803670117320393E-08, 2.7052110067149541E-08, 1.4607732409283000E-08, 1.4637694536487999E-08 - 65, 60, 4.9117828285767292E-08,-1.8497742505408230E-08, 1.3457034906313999E-08, 1.3455441972366001E-08 - 65, 61,-4.1991808355709383E-09,-1.8661023655929389E-08, 1.2468022749053000E-08, 1.2481068574772999E-08 - 65, 62, 2.5437974304450860E-08, 4.0496461171912171E-08, 1.1221733875019001E-08, 1.1280588790957000E-08 - 65, 63,-8.7848509700082287E-09,-3.7692957462359342E-08, 8.9091627052292004E-09, 8.9541616399045002E-09 - 65, 64, 1.5262114405323779E-08,-4.1958447869487269E-10, 5.4604902088895997E-09, 5.5929574630289000E-09 - 65, 65,-3.5922044167734411E-08, 2.8889954922094178E-09, 5.3791672584085003E-09, 5.4171355160032002E-09 - 66, 0,-2.2511520008357951E-08, 0.0000000000000000E+00, 3.3273731046016998E-08, 0.0000000000000000E+00 - 66, 1,-1.6650898506102991E-09,-3.9069590310602421E-10, 3.3195301571880998E-08, 3.3172480414124997E-08 - 66, 2,-5.3228575683501458E-09,-1.6755231490103439E-08, 3.3039955998171000E-08, 3.3029147505341003E-08 - 66, 3, 2.5622026366651851E-08, 7.1007004943716289E-09, 3.3063751620458001E-08, 3.3081038693060002E-08 - 66, 4,-3.1719259730059319E-08, 2.7114090583255869E-08, 3.2966017876793001E-08, 3.2957109755877002E-08 - 66, 5, 9.4272380599092478E-09,-2.3271167086174029E-08, 3.2930306186909999E-08, 3.2934076617078001E-08 - 66, 6,-1.3213214638993780E-08,-5.4621151430621639E-09, 3.2786305095313998E-08, 3.2880626856153998E-08 - 66, 7,-1.5758364019074409E-08, 3.6587875409956439E-09, 3.2783046103074001E-08, 3.2733189009153002E-08 - 66, 8, 2.5988498687538911E-08, 3.4245514516507097E-08, 3.2643852400326998E-08, 3.2676277415602003E-08 - 66, 9, 1.1201192054131471E-08, 1.0527868825641421E-08, 3.2538721900150997E-08, 3.2579000285398001E-08 - 66, 10,-1.3505846195285391E-08, 4.7669780607262324E-09, 3.2441535892558997E-08, 3.2447833173654999E-08 - 66, 11, 1.3980382427165259E-09,-8.7763576590849478E-10, 3.2331022291643999E-08, 3.2336426210894001E-08 - 66, 12,-5.8152968995804009E-09,-1.1594443720085560E-08, 3.2208772188250003E-08, 3.2224268080954000E-08 - 66, 13, 1.2113962392989080E-08, 1.7907297220382910E-08, 3.2075376778471998E-08, 3.2098338074496001E-08 - 66, 14, 8.0736809527317541E-09, 1.5532053708287700E-08, 3.1954389911102001E-08, 3.1951340010435997E-08 - 66, 15,-4.6397092381545069E-09, 3.3628148822002881E-09, 3.1804243765363003E-08, 3.1794875915975001E-08 - 66, 16,-8.7048008115685909E-09, 5.0624127547366181E-10, 3.1667847509788000E-08, 3.1676042307069003E-08 - 66, 17,-2.3214176814475130E-08, 3.0592643580949898E-09, 3.1529026687547998E-08, 3.1522041396386001E-08 - 66, 18,-1.3406947591252130E-08, 9.7283019657205285E-09, 3.1384482638225998E-08, 3.1355974194568998E-08 - 66, 19,-2.0000254659363359E-09, 8.2691117767756523E-09, 3.1220311623407998E-08, 3.1221532725506003E-08 - 66, 20,-1.7885627099356461E-08, 3.0497500095823490E-09, 3.1044674341998003E-08, 3.1039699072091999E-08 - 66, 21, 2.2070174374221470E-08,-4.5669178798035738E-09, 3.0857789661096999E-08, 3.0859699358352998E-08 - 66, 22, 3.0942259867676909E-09, 5.6005857272690547E-09, 3.0683067234510001E-08, 3.0686357859409997E-08 - 66, 23, 1.8327731091204601E-08,-1.9035057160541920E-08, 3.0495017737359000E-08, 3.0496778997722002E-08 - 66, 24, 2.2157235068500461E-08,-1.2562941517218090E-08, 3.0315291670028003E-08, 3.0310928751131000E-08 - 66, 25,-1.6101902268019710E-08, 7.5698207618363516E-09, 3.0123724053191998E-08, 3.0140236184933001E-08 - 66, 26,-2.2455108028381420E-08, 1.4441135047716390E-08, 2.9924956058463998E-08, 2.9919744422990999E-08 - 66, 27,-1.5081279638113360E-08, 8.1868002362437970E-09, 2.9727221602679999E-08, 2.9714407333618000E-08 - 66, 28, 1.1023013299868539E-08, 1.4538303036551440E-08, 2.9499270352943000E-08, 2.9488003793567000E-08 - 66, 29,-1.0336228434864701E-08,-1.8292661258702660E-08, 2.9286884874299999E-08, 2.9291194873146000E-08 - 66, 30,-3.6935775432822280E-09, 1.1936359590318340E-08, 2.9043209403379001E-08, 2.9058749533816001E-08 - 66, 31,-1.4738287090717410E-08,-1.6552063053243689E-08, 2.8844598437595000E-08, 2.8837940680626999E-08 - 66, 32,-1.0627479323609780E-08,-2.5383918818776141E-08, 2.8576932484616001E-08, 2.8592839168774999E-08 - 66, 33, 4.3893191378796191E-09,-3.6950371923427688E-09, 2.8354547419022000E-08, 2.8355325399671999E-08 - 66, 34, 7.5619769970966567E-09,-1.6463823287467760E-08, 2.8086151315681000E-08, 2.8091186578258001E-08 - 66, 35,-1.9457408572877029E-09, 6.9126440116157973E-09, 2.7821533986248999E-08, 2.7833162503592001E-08 - 66, 36, 5.8793340758330233E-09, 4.4776671464012873E-09, 2.7585321777701998E-08, 2.7612307315484999E-08 - 66, 37,-2.0732252273474818E-08, 5.1821866305416353E-09, 2.7297501079734001E-08, 2.7282875119288001E-08 - 66, 38, 7.0261452235977683E-09, 2.0143832158110662E-08, 2.7045735851203001E-08, 2.7035258341198999E-08 - 66, 39, 1.7102912650890920E-08,-1.4652716040433541E-08, 2.6690322697840001E-08, 2.6699660423459999E-08 - 66, 40,-3.4308004111736622E-08,-8.0207709984275157E-09, 2.6388962316604002E-08, 2.6400530231806000E-08 - 66, 41, 1.0276085800734230E-09,-8.7967945900204299E-09, 2.6068806186732001E-08, 2.6077084532547001E-08 - 66, 42, 1.0046035536738761E-09,-2.9759587688352611E-08, 2.5727156912785001E-08, 2.5749216109249999E-08 - 66, 43,-8.1556571406130910E-09,-9.8381395749664541E-09, 2.5374793733236001E-08, 2.5395019986192999E-08 - 66, 44, 2.0597780580480098E-09,-1.6079537004986279E-09, 2.4987385606820999E-08, 2.5062594631576998E-08 - 66, 45, 4.8122618414413097E-10, 1.1663773580019380E-08, 2.4642215305004000E-08, 2.4628937945690999E-08 - 66, 46,-3.3876165105028000E-09, 7.8230422616169222E-09, 2.4242450974169000E-08, 2.4243944509366999E-08 - 66, 47, 6.8412649450404349E-10,-6.9230753712942944E-09, 2.3824772102063999E-08, 2.3829566493564000E-08 - 66, 48,-6.4101798339056083E-09, 3.6224466255426157E-08, 2.3385194027353999E-08, 2.3357377574975000E-08 - 66, 49,-3.3831669373333787E-08, 2.0746096782214280E-08, 2.2921851847542001E-08, 2.2904243330710001E-08 - 66, 50,-1.8078863349351720E-08, 1.7879099454058009E-08, 2.2404473503114001E-08, 2.2419985744352000E-08 - 66, 51, 1.4903526135906159E-08,-7.3201833188761900E-08, 2.1910325099385999E-08, 2.1904052684951001E-08 - 66, 52, 7.7525366370604461E-08, 7.1373128539888214E-09, 2.1334854247133001E-08, 2.1316021540771999E-08 - 66, 53, 1.3754236407925550E-08, 6.0449606759056230E-09, 2.0747571957369001E-08, 2.0757073347928000E-08 - 66, 54,-4.1283091106582676E-09,-2.0937030210493141E-08, 2.0089961725015000E-08, 2.0098439849574000E-08 - 66, 55, 5.6967820170768514E-09,-2.3838914174551059E-08, 1.9463991577173999E-08, 1.9472010412401000E-08 - 66, 56,-1.6480363419492661E-09, 2.0735196435222211E-08, 1.8733610634736999E-08, 1.8756245623440001E-08 - 66, 57, 6.5208399314248863E-09, 9.5555516927442503E-09, 1.8105212900815999E-08, 1.8097113729740000E-08 - 66, 58,-5.4735245973153859E-09,-1.2596737529274191E-09, 1.7233612401602999E-08, 1.7289639541572000E-08 - 66, 59, 1.0776500690148420E-08,-2.7508573062808149E-08, 1.6471538944238999E-08, 1.6472322166167000E-08 - 66, 60, 8.1623380947365579E-09,-2.2709291942720719E-08, 1.5630244228200000E-08, 1.5586456889189001E-08 - 66, 61,-6.0863558223073611E-08,-2.3143456865729212E-08, 1.4376453968533001E-08, 1.4361085371578001E-08 - 66, 62, 2.3049304066902740E-08, 1.1278883433419030E-08, 1.3374383900751999E-08, 1.3430295927202999E-08 - 66, 63, 5.2946531095619091E-08,-4.7795605926934243E-08, 1.2098305972187999E-08, 1.2114939185846000E-08 - 66, 64,-2.8958395621563808E-08, 2.1151728593225900E-08, 9.4249162964531006E-09, 9.4790772096174005E-09 - 66, 65, 4.6351473973913762E-08,-2.5555721526976060E-08, 6.0854439494489997E-09, 5.9452043754958996E-09 - 66, 66,-1.3547574785928200E-08, 2.9564983621979921E-08, 6.0383888040812999E-09, 6.0210619948290000E-09 - 67, 0,-8.2042364988053092E-08, 0.0000000000000000E+00, 3.3433566347576000E-08, 0.0000000000000000E+00 - 67, 1, 3.4809886172156630E-09,-2.0242575809330559E-08, 3.3275570709149002E-08, 3.3258298034792998E-08 - 67, 2,-6.1922785732092011E-09,-1.6341600634354181E-08, 3.3126847209147003E-08, 3.3110806614570997E-08 - 67, 3,-1.2175396570356269E-08, 2.4396092798539398E-10, 3.3158685814215001E-08, 3.3172942657999000E-08 - 67, 4, 4.7382759195032523E-09, 3.4426038832712421E-09, 3.3066769181493998E-08, 3.3045353073404000E-08 - 67, 5,-7.0945334894264350E-09,-8.7845956515856489E-09, 3.3022241032643998E-08, 3.3033829248762001E-08 - 67, 6,-1.3880352526323480E-08, 7.3178114560709044E-09, 3.2903678456613000E-08, 3.2972323866611002E-08 - 67, 7,-2.2874512834602740E-08, 1.1392386866221661E-08, 3.2880650367835998E-08, 3.2844853729586003E-08 - 67, 8,-7.0627659232543019E-10, 8.6425591623508795E-09, 3.2762067441995997E-08, 3.2786616875848000E-08 - 67, 9, 1.3497846251707780E-08, 7.6351439056457655E-09, 3.2661966152728002E-08, 3.2695405384711998E-08 - 67, 10, 2.4165076634525370E-08,-1.2413367149951170E-08, 3.2576599782072001E-08, 3.2578707630318998E-08 - 67, 11, 1.0828139472109140E-08,-1.5270751308120601E-08, 3.2461225014802997E-08, 3.2465613022022003E-08 - 67, 12,-4.5729876137577209E-09,-1.4605400507895610E-08, 3.2368714133836002E-08, 3.2380083114071998E-08 - 67, 13, 1.0842741781351791E-08,-9.9649907268058963E-10, 3.2226535242504000E-08, 3.2248388544813997E-08 - 67, 14, 6.7663962114929613E-09, 2.1374906969419409E-08, 3.2144161785173001E-08, 3.2133262385881997E-08 - 67, 15,-2.8234164166358279E-08,-3.8585341394953269E-09, 3.1988730890858997E-08, 3.1976560174646998E-08 - 67, 16,-1.9653679309134410E-08,-1.3859175863098960E-08, 3.1877014085802002E-08, 3.1886413874855003E-08 - 67, 17,-3.7334415072929383E-09, 1.0045754838628440E-08, 3.1747285706535997E-08, 3.1741623063099001E-08 - 67, 18,-1.6494325876283350E-09, 4.8839874008194711E-11, 3.1625970919780000E-08, 3.1597697302998001E-08 - 67, 19, 3.6884764560975749E-09, 2.6563367537715231E-08, 3.1473561733847002E-08, 3.1488960749907000E-08 - 67, 20, 2.5908570138293060E-08, 2.3387833017761730E-09, 3.1316740656646998E-08, 3.1318313115498997E-08 - 67, 21, 9.3071446621351160E-09,-5.4530866671707521E-09, 3.1162500292637997E-08, 3.1155678467185000E-08 - 67, 22,-7.4227114676430093E-09,-2.0066504872185870E-08, 3.1002422888271002E-08, 3.1003130934778002E-08 - 67, 23,-6.4879610717041083E-09, 1.6030125090129539E-08, 3.0833100837497998E-08, 3.0840019251131001E-08 - 67, 24, 1.9970817171213452E-08, 6.6964669050006451E-09, 3.0681037844729002E-08, 3.0678025854933002E-08 - 67, 25,-3.0641949005002991E-08, 2.9629315405706691E-08, 3.0522229943025997E-08, 3.0531549859736003E-08 - 67, 26, 5.6833031108645419E-09,-1.4663264320471030E-08, 3.0334035722600002E-08, 3.0326883706454002E-08 - 67, 27,-1.6262268385028759E-08,-2.8639813051531721E-08, 3.0155844787522999E-08, 3.0146270859329997E-08 - 67, 28, 5.1976766917456631E-09, 2.1174842434001800E-08, 2.9969240410601999E-08, 2.9950162807438001E-08 - 67, 29, 1.3784722957208250E-08, 6.0465133895953267E-09, 2.9767977724981000E-08, 2.9767054694013999E-08 - 67, 30,-3.8579051269073221E-08, 9.7175075812296886E-09, 2.9563757826316002E-08, 2.9570209729226000E-08 - 67, 31, 1.2178729256049900E-08,-2.2915884987181369E-08, 2.9374396665900999E-08, 2.9371743145572999E-08 - 67, 32,-1.7934635512840681E-09,-1.1236163069335729E-08, 2.9153796483741001E-08, 2.9158746319230001E-08 - 67, 33, 1.7571290432680191E-08,-1.4518563433997900E-08, 2.8953147188860000E-08, 2.8950743132173999E-08 - 67, 34, 1.7000159203982740E-08,-1.1989028496605740E-09, 2.8717691511685000E-08, 2.8721351586433999E-08 - 67, 35, 5.5176979192551283E-09,-2.5187862081642861E-08, 2.8473821475605999E-08, 2.8492353745944000E-08 - 67, 36, 1.2268635209375070E-08, 1.6007559263140670E-08, 2.8273402790176999E-08, 2.8301238594593001E-08 - 67, 37,-3.4681867812801901E-09,-7.7906855478179653E-09, 2.8028889160470001E-08, 2.8013796248238999E-08 - 67, 38, 5.2923507173436967E-11, 7.2676030436194689E-09, 2.7809628904658001E-08, 2.7809936655988000E-08 - 67, 39, 1.0363255171171960E-08,-1.4761141556242850E-08, 2.7500561870533001E-08, 2.7509888573489001E-08 - 67, 40, 8.5923714648108362E-09,-9.0468847845579834E-09, 2.7236422504640001E-08, 2.7242715685528999E-08 - 67, 41, 2.4479981442525219E-08,-4.6961593373167618E-09, 2.6952679513628001E-08, 2.6951347115754000E-08 - 67, 42,-2.6643483756935178E-08, 1.9529128967951639E-08, 2.6668705035892999E-08, 2.6678687975895001E-08 - 67, 43, 1.4762164864903990E-08, 5.2292889461143484E-09, 2.6347258140097999E-08, 2.6373274879949000E-08 - 67, 44,-8.0801490422467358E-09,-6.1244874856815208E-09, 2.6000786035738001E-08, 2.6083846852914001E-08 - 67, 45, 9.2782423554687217E-09, 1.6439363181945240E-08, 2.5724435435401000E-08, 2.5722137080558998E-08 - 67, 46, 2.3494960127012481E-08, 1.3895714685920609E-08, 2.5349787562312001E-08, 2.5348754629088000E-08 - 67, 47, 2.6003655400198571E-08,-1.5358438415360071E-08, 2.5002673520702000E-08, 2.5009906609216999E-08 - 67, 48,-1.3483794694802049E-08, 2.5085260180078231E-08, 2.4636388430020999E-08, 2.4597432429021001E-08 - 67, 49, 1.2026609978649870E-08, 7.2249842144973379E-09, 2.4175507490815001E-08, 2.4161760360456999E-08 - 67, 50,-4.1676817350043917E-09, 2.1964521950254719E-09, 2.3789119626680000E-08, 2.3794605736520002E-08 - 67, 51, 1.9836118602524762E-08,-2.0417089758260611E-08, 2.3259268205548998E-08, 2.3237963489673999E-08 - 67, 52,-2.8742622036199501E-08, 3.4696313883168781E-08, 2.2784329877867001E-08, 2.2774444638862999E-08 - 67, 53, 6.6794235737935409E-09,-2.2937580799672769E-08, 2.2224025741716000E-08, 2.2220299751910002E-08 - 67, 54,-2.7453628691544889E-09,-2.1527961420639020E-08, 2.1669496607992001E-08, 2.1682981801452000E-08 - 67, 55, 2.2778803397719551E-08,-1.0944493880214780E-08, 2.1021363788823000E-08, 2.1006071049218000E-08 - 67, 56, 5.4849274963716293E-09, 1.1598478837465610E-08, 2.0427642188317001E-08, 2.0447238491892001E-08 - 67, 57,-9.5188573150943268E-10,-7.2396081226709243E-09, 1.9720069943945000E-08, 1.9719044218080001E-08 - 67, 58,-1.6157726991064779E-08,-1.4291178878468390E-09, 1.9038235506568001E-08, 1.9074856301279998E-08 - 67, 59,-1.2457972772208059E-08, 4.5025747276896881E-08, 1.8223026978353000E-08, 1.8233993652396000E-08 - 67, 60,-2.9093693326444970E-08, 2.8910816244521800E-08, 1.7449254608513999E-08, 1.7416848200942001E-08 - 67, 61,-2.2721673729746590E-08, 2.7612280745647371E-08, 1.6587886315171000E-08, 1.6549516991304000E-08 - 67, 62,-2.4620275319967232E-08, 4.6760687913147191E-08, 1.5271094141913000E-08, 1.5282498577876999E-08 - 67, 63, 3.2026611502940362E-08,-2.8203260181717331E-08, 1.4358619499893000E-08, 1.4341672091596000E-08 - 67, 64, 1.3089454999768500E-08, 2.9928416840258883E-08, 1.2893890423049999E-08, 1.2886361197024001E-08 - 67, 65, 3.9299839801992962E-08, 6.8953295337926541E-08, 1.0154238484613001E-08, 1.0185446086705000E-08 - 67, 66,-3.0235816266102032E-08,-4.5519797221910937E-08, 6.5686372578950999E-09, 6.5817780778550997E-09 - 67, 67, 4.7305692250117892E-08, 1.7927704183023691E-08, 6.5323168795116001E-09, 6.5632751757363002E-09 - 68, 0,-1.4060437580433089E-08, 0.0000000000000000E+00, 3.3410242671013000E-08, 0.0000000000000000E+00 - 68, 1, 1.1856638668738259E-08,-7.1624936313842979E-09, 3.3235480211711002E-08, 3.3224586433359997E-08 - 68, 2,-2.1351118173655570E-08,-1.0771461661547650E-08, 3.3074142666075999E-08, 3.3050683374992998E-08 - 68, 3, 2.5759527114722210E-08,-1.3913162885903369E-08, 3.3119115594318001E-08, 3.3131646969549998E-08 - 68, 4, 9.8343907231008246E-09, 5.6391931488591852E-09, 3.3018854388203001E-08, 3.2988243729244002E-08 - 68, 5, 5.8287856800555752E-11,-7.1190413465649487E-09, 3.2992098482644998E-08, 3.2996330327199999E-08 - 68, 6,-5.1145729003170758E-09, 2.2481835537070411E-08, 3.2872475708754998E-08, 3.2920174599839003E-08 - 68, 7,-9.4662662329646602E-09, 7.0864787179928262E-09, 3.2854513861606003E-08, 3.2823085390979999E-08 - 68, 8, 9.2295708717322816E-09,-8.2235743235046974E-09, 3.2738314720635997E-08, 3.2757578594947001E-08 - 68, 9,-9.8879533757480279E-09,-1.1890069702573460E-08, 3.2651625549525002E-08, 3.2671727395693002E-08 - 68, 10,-1.7329742177763550E-08, 3.3865143344422112E-10, 3.2567138196419000E-08, 3.2566049369245002E-08 - 68, 11,-4.4200911192072299E-09,-1.8279083749978782E-08, 3.2462181530507003E-08, 3.2467779169328003E-08 - 68, 12,-1.1743697238529180E-08,-1.1961577071004141E-08, 3.2375822479880998E-08, 3.2379938118900003E-08 - 68, 13, 6.4824018927447910E-09, 1.9110502531640581E-09, 3.2250097989317001E-08, 3.2272771784505998E-08 - 68, 14, 9.8102118616334437E-09, 4.5571563281183448E-09, 3.2163155679449002E-08, 3.2150564977687999E-08 - 68, 15, 2.3689701830813219E-08, 2.5054875408146831E-08, 3.2028902736620003E-08, 3.2025224113039997E-08 - 68, 16,-2.2459983786357640E-08,-9.6463501292197249E-09, 3.1924583905702002E-08, 3.1925939279823000E-08 - 68, 17,-3.2647560735597141E-09, 8.1585679044529714E-09, 3.1820875844587001E-08, 3.1813623892457999E-08 - 68, 18,-6.2907884183331483E-09, 8.7543591965110668E-09, 3.1700083174898999E-08, 3.1670164935778003E-08 - 68, 19, 1.6952073194400529E-09, 1.4988210977705639E-08, 3.1572026629759998E-08, 3.1593585599270002E-08 - 68, 20,-3.6329819818853048E-09,-1.8728960472695258E-08, 3.1419138243939999E-08, 3.1430655663873002E-08 - 68, 21, 4.8086139156727379E-09,-2.6973926521039381E-08, 3.1295204650505999E-08, 3.1284833630023002E-08 - 68, 22,-5.8364478763023630E-09,-6.0987388424415090E-09, 3.1148726841796002E-08, 3.1149023275689002E-08 - 68, 23,-8.5665897475464837E-09,-2.0659245162033999E-09, 3.0997807114560000E-08, 3.1007351879002999E-08 - 68, 24, 2.6619401571512350E-08,-1.1548026320364069E-09, 3.0868237218892003E-08, 3.0863417239398001E-08 - 68, 25, 5.5202498561049240E-09,-3.7842728813904929E-09, 3.0722661112076003E-08, 3.0738978072252997E-08 - 68, 26,-1.5133208447093580E-08,-8.8898209047251597E-09, 3.0566704870736003E-08, 3.0564564274887003E-08 - 68, 27,-5.9330008366678093E-09,-2.5294616180126899E-08, 3.0413850815306999E-08, 3.0408378498997000E-08 - 68, 28,-1.9752080985181830E-08,-1.3391758342743960E-08, 3.0251528819538000E-08, 3.0239043059648001E-08 - 68, 29,-1.4484064117091589E-08,-1.4737038773033699E-08, 3.0085878951002003E-08, 3.0083242523737997E-08 - 68, 30, 3.2161862546334901E-09, 9.3591906107526821E-09, 2.9900929388471999E-08, 2.9901212358730999E-08 - 68, 31, 1.2009248849353381E-08, 7.3127937489536764E-09, 2.9747571898527000E-08, 2.9742594093068999E-08 - 68, 32,-2.9998357558461322E-08,-2.7356819771891920E-08, 2.9543099766883001E-08, 2.9550077923991000E-08 - 68, 33,-8.8397763095839246E-09,-1.0311359553040620E-08, 2.9391863639476998E-08, 2.9380302034330999E-08 - 68, 34, 6.7226859030169502E-09, 3.9990800946378602E-09, 2.9180021003598001E-08, 2.9174734942864001E-08 - 68, 35,-4.1152756508177641E-09,-1.1110832811384190E-08, 2.8970462622015001E-08, 2.9000164628400999E-08 - 68, 36,-6.7112938842462440E-10, 4.7499419579773067E-09, 2.8806774620441002E-08, 2.8818546648202001E-08 - 68, 37, 2.6949727238917689E-08, 2.4209948350929770E-08, 2.8584261472165001E-08, 2.8571587254184999E-08 - 68, 38, 1.7970956197969700E-09,-7.0075040218608679E-10, 2.8402938759647000E-08, 2.8417676376261001E-08 - 68, 39,-7.4941724921979443E-09,-1.6239072403863311E-08, 2.8121887580127002E-08, 2.8125344387016999E-08 - 68, 40,-2.5290110553892220E-08,-1.9386605063586750E-08, 2.7890021927229001E-08, 2.7894654840073001E-08 - 68, 41,-1.1104586684538780E-08,-3.1088151274537260E-08, 2.7642187135207999E-08, 2.7635268688811999E-08 - 68, 42,-1.7327354894841760E-08, 3.2750772325457660E-08, 2.7389672741132002E-08, 2.7401404841657999E-08 - 68, 43, 1.2779547416731461E-08,-8.4421212017224894E-10, 2.7123648310522999E-08, 2.7140120190977999E-08 - 68, 44,-2.0858458630566221E-08, 7.5317085386708845E-09, 2.6830547597246999E-08, 2.6884256409151000E-08 - 68, 45, 1.2501149175553739E-08,-6.7543118094289182E-09, 2.6567091413083001E-08, 2.6564624317922001E-08 - 68, 46,-3.0730634707344910E-09,-5.4898695548705640E-09, 2.6258788022218001E-08, 2.6261506086092000E-08 - 68, 47, 1.3435546532733470E-08,-2.9540369917196819E-08, 2.5925270897042000E-08, 2.5938153420077000E-08 - 68, 48,-2.4665389481321891E-09, 2.1143807147039820E-08, 2.5632808344766001E-08, 2.5586248402447999E-08 - 68, 49, 5.7214605732993510E-09,-1.1526735307805800E-08, 2.5256401439338000E-08, 2.5241382490464998E-08 - 68, 50,-2.3450888797539340E-08, 1.1733218289063900E-08, 2.4880171124643998E-08, 2.4883812660461000E-08 - 68, 51, 1.1918236047817149E-08,-1.6039784331926739E-08, 2.4485538214993999E-08, 2.4465010583511999E-08 - 68, 52, 2.2139904517736691E-08,-1.7114731407393179E-09, 2.3984642762676001E-08, 2.3974150215736001E-08 - 68, 53,-4.0088202002270763E-09,-1.7210698789309419E-08, 2.3544806516809001E-08, 2.3526998674230999E-08 - 68, 54,-5.2492071034749392E-08,-8.2692556042887146E-09, 2.3020180736477001E-08, 2.3027775785981999E-08 - 68, 55, 2.8048688257383870E-08,-1.3481960618371690E-08, 2.2464307960936999E-08, 2.2471488240150999E-08 - 68, 56, 1.6710417109442980E-09, 2.1293365698639000E-08, 2.1880648092398000E-08, 2.1886446013949000E-08 - 68, 57,-1.5575914489410860E-09, 1.3133246934454410E-08, 2.1345661562048000E-08, 2.1326053350179000E-08 - 68, 58,-4.1063302404071117E-08,-2.2002012233996929E-08, 2.0592991156088001E-08, 2.0591722491324000E-08 - 68, 59,-5.2371022647453807E-08, 7.5891500024720301E-09, 2.0008073526401999E-08, 2.0015528447306999E-08 - 68, 60,-1.8369954220722671E-08, 1.1346223649464280E-08, 1.9154567818499001E-08, 1.9116624857515001E-08 - 68, 61,-8.7610152835638234E-09,-2.2388817620117669E-08, 1.8411916583256001E-08, 1.8357081720373999E-08 - 68, 62, 1.0852957387856650E-08, 3.5199914455287002E-08, 1.7514793075201999E-08, 1.7490901485237001E-08 - 68, 63, 1.7375091341850531E-08,-1.0517123461232960E-08, 1.6187598805324999E-08, 1.6145436265080000E-08 - 68, 64,-2.2117086381405499E-08, 4.4181004491535403E-09, 1.5213841234579001E-08, 1.5284400406959999E-08 - 68, 65,-1.2591807752617281E-08, 3.4444590324765779E-09, 1.3756171616210000E-08, 1.3781488962193000E-08 - 68, 66, 5.6267561134636053E-08,-3.2922367632368193E-08, 1.0813613297599999E-08, 1.0821356569034000E-08 - 68, 67,-5.6281853797891613E-08,-7.4157272687444503E-09, 6.8623851389823000E-09, 6.7772877010763001E-09 - 68, 68, 1.5340438050081701E-08,-2.7600451997187540E-08, 7.2645260838079002E-09, 7.2576128594877999E-09 - 69, 0,-1.1388687392688910E-07, 0.0000000000000000E+00, 3.3289257840933999E-08, 0.0000000000000000E+00 - 69, 1,-1.9474905102782161E-08,-2.2996972065424459E-11, 3.3029549528823002E-08, 3.3027320396641003E-08 - 69, 2, 1.1543579001510020E-08,-5.3598276115665196E-09, 3.2863721694128998E-08, 3.2842605772387998E-08 - 69, 3, 1.9154249287331271E-08, 2.6954410925302940E-08, 3.2934051647911001E-08, 3.2950220812841997E-08 - 69, 4, 3.7522184613455634E-09, 3.4974030177431971E-09, 3.2816336798347997E-08, 3.2777654297025000E-08 - 69, 5, 1.3175646197640680E-08,-1.2139007125138160E-08, 3.2809589432068997E-08, 3.2812436117976003E-08 - 69, 6,-6.8437704138621421E-09,-2.0870379336569551E-09, 3.2688697967237003E-08, 3.2722452037136001E-08 - 69, 7, 1.6312239010579641E-09, 1.0253608127094630E-08, 3.2671385952128999E-08, 3.2645216354873997E-08 - 69, 8, 4.7962853207595322E-09, 8.3196410844159710E-09, 3.2564492525407997E-08, 3.2579793797207003E-08 - 69, 9, 2.5781332171137370E-08,-1.5168288599107709E-09, 3.2482557273989001E-08, 3.2494042209709998E-08 - 69, 10, 2.4474994211827548E-08,-5.0917861729704490E-09, 3.2410977410490001E-08, 3.2410467323776998E-08 - 69, 11,-7.6396714493846004E-09,-2.0840798182327289E-08, 3.2300103908163998E-08, 3.2305677248234001E-08 - 69, 12,-5.8823174872236864E-09,-1.9356360365775790E-08, 3.2238968849753000E-08, 3.2239663260872997E-08 - 69, 13, 2.9344148413498541E-08, 2.6341152970443169E-08, 3.2114803319260998E-08, 3.2125485834028997E-08 - 69, 14, 2.3644664575164061E-09, 5.8863241601789476E-09, 3.2049886084829002E-08, 3.2043723600350999E-08 - 69, 15,-1.9835584985785540E-08,-6.1684767456697723E-09, 3.1917953746683998E-08, 3.1909803343920999E-08 - 69, 16,-5.0449778428580269E-09,-1.0542077678569430E-08, 3.1838435340273000E-08, 3.1836338758660997E-08 - 69, 17,-5.4585381826407514E-10, 2.4055939926443610E-08, 3.1741054198262001E-08, 3.1734421845638999E-08 - 69, 18,-1.1224222778328339E-08, 2.4443187452286050E-08, 3.1640296396747002E-08, 3.1612024210607000E-08 - 69, 19, 2.3207958864514452E-08, 1.7013733876648240E-08, 3.1526843524581998E-08, 3.1552222503101997E-08 - 69, 20,-5.7387304636730300E-10, 1.5042947115555200E-08, 3.1389044021790000E-08, 3.1409214625462001E-08 - 69, 21, 7.1112940863084728E-09,-3.0063898824257552E-08, 3.1279326814258003E-08, 3.1271788914502002E-08 - 69, 22,-1.7411591897696369E-08,-4.8907412244071658E-09, 3.1163884244329999E-08, 3.1156494996500000E-08 - 69, 23,-5.6386842701846970E-09, 6.0625423579077358E-09, 3.1015452620337002E-08, 3.1031224254809997E-08 - 69, 24, 1.2508621050872820E-08, 1.5095176886667571E-08, 3.0915254979078003E-08, 3.0913261761661000E-08 - 69, 25,-5.0985199160502377E-09, 4.7271858830435061E-08, 3.0793775197651997E-08, 3.0800994261343000E-08 - 69, 26,-7.6563323510355326E-09,-3.6692329486713410E-09, 3.0653840388350001E-08, 3.0653632538238999E-08 - 69, 27,-4.3868902441823366E-09, 1.0491562360961860E-08, 3.0508179516176000E-08, 3.0502650357051002E-08 - 69, 28, 1.6624505433732999E-09, 2.0631655801347919E-08, 3.0376595153609003E-08, 3.0369820732310999E-08 - 69, 29,-9.4254817748248356E-09,-1.0542086250563129E-08, 3.0231689435798000E-08, 3.0229190271045997E-08 - 69, 30, 1.3929732891855919E-08, 6.9287680209072622E-09, 3.0074366490395998E-08, 3.0074869196942000E-08 - 69, 31,-3.3434595237403001E-09,-2.6181409737363849E-08, 2.9938236743405000E-08, 2.9929967876783997E-08 - 69, 32, 2.0135387528712528E-08,-1.7704377101805259E-08, 2.9766674629752998E-08, 2.9764613017617999E-08 - 69, 33,-3.2014831035342220E-09,-8.0580154096427952E-09, 2.9628662878871002E-08, 2.9617212229282999E-08 - 69, 34, 7.8902409830861555E-09, 2.4260225944466882E-08, 2.9453992962703000E-08, 2.9441427443960000E-08 - 69, 35,-5.1426332402852748E-09,-5.1699046438865558E-09, 2.9265206274929002E-08, 2.9296608316613000E-08 - 69, 36, 2.7131818784367050E-08,-3.9639983713556960E-08, 2.9132514942538001E-08, 2.9135342561733999E-08 - 69, 37,-1.1692465759273261E-08, 6.8597360384490937E-10, 2.8938156990080001E-08, 2.8919604438280000E-08 - 69, 38,-1.3510230292102179E-08,-6.0247296255564357E-09, 2.8792367045272000E-08, 2.8803025705586000E-08 - 69, 39,-2.2146111663420819E-08, 2.8260255250294889E-08, 2.8550264247447000E-08, 2.8552621899981998E-08 - 69, 40,-4.5998540975413773E-09, 8.9854926177874857E-09, 2.8356909829578002E-08, 2.8366626657880000E-08 - 69, 41,-1.3890971571109691E-08,-1.6366384409786701E-08, 2.8140529649633999E-08, 2.8123020467104001E-08 - 69, 42,-1.7659384429110600E-08,-7.3850916928753303E-09, 2.7926485587398999E-08, 2.7942923172572001E-08 - 69, 43, 4.0434339906986183E-09, 1.3804056684096390E-08, 2.7694307830685001E-08, 2.7713009592981000E-08 - 69, 44,-9.2140592911599718E-09,-1.4418396625911290E-08, 2.7474370529723001E-08, 2.7491893894797001E-08 - 69, 45, 1.9837282237159310E-08, 1.4287508116837360E-08, 2.7237058632017001E-08, 2.7239117504748999E-08 - 69, 46,-9.2405068779750918E-09, 1.2936600213232450E-08, 2.6966614824054001E-08, 2.6962067654474001E-08 - 69, 47,-4.2127091487640952E-10, 8.2993503630751269E-09, 2.6711456242605999E-08, 2.6729807871302999E-08 - 69, 48,-2.4714969808106420E-08,-2.7323205582386541E-08, 2.6426391339557000E-08, 2.6385261282640999E-08 - 69, 49,-1.9233156083936049E-08,-2.2254173703064320E-09, 2.6118851935810999E-08, 2.6118801117064000E-08 - 69, 50,-1.7778738120161168E-08,-2.2482181188083081E-08, 2.5823191189415000E-08, 2.5829596388686999E-08 - 69, 51, 5.1518240697247402E-08,-1.5019422746881260E-08, 2.5416115096797001E-08, 2.5398848512483000E-08 - 69, 52, 2.5901349021544452E-09, 3.0458253153510162E-08, 2.5057481548528999E-08, 2.5044442402097999E-08 - 69, 53, 1.3960566671437180E-08,-9.5326483414303820E-09, 2.4582559863742999E-08, 2.4590100751397001E-08 - 69, 54,-1.1062156850735790E-08,-2.7417551763642001E-08, 2.4188116366427001E-08, 2.4200803878420001E-08 - 69, 55, 3.6608059713997861E-08,-1.3332755802820329E-08, 2.3662083786909001E-08, 2.3664869541931000E-08 - 69, 56,-9.1780995339035209E-09, 1.2330375522769300E-08, 2.3199393686144999E-08, 2.3212487450628998E-08 - 69, 57, 7.1582524351817886E-09,-7.6319582872955547E-09, 2.2640453994083000E-08, 2.2609456491054001E-08 - 69, 58,-1.4203885832727210E-08,-6.1511602741994788E-09, 2.2063180730279999E-08, 2.2054082177058000E-08 - 69, 59,-3.8863419022903910E-09,-1.4651119818567420E-09, 2.1360870531170000E-08, 2.1379113314734999E-08 - 69, 60,-1.5202642869749030E-08, 1.1856108117300580E-08, 2.0805752625187999E-08, 2.0765401091493001E-08 - 69, 61, 1.6550072259894189E-08,-1.5650088870784611E-08, 1.9929351043108999E-08, 1.9907382661004001E-08 - 69, 62, 1.8595671792097341E-09,-1.2019765629813010E-08, 1.9213263641044999E-08, 1.9251542189161001E-08 - 69, 63, 8.2229963314416278E-10,-7.7074677260288156E-09, 1.8390828912351999E-08, 1.8442853549776999E-08 - 69, 64,-2.8295721454597661E-08,-4.8136725907073757E-08, 1.6946739108215999E-08, 1.6977625211579001E-08 - 69, 65, 1.2123244010607710E-08,-8.6058649231401289E-09, 1.6073972063511000E-08, 1.6140827963063001E-08 - 69, 66,-2.6142341365549049E-08, 1.7926726805536629E-08, 1.4501229221739999E-08, 1.4540356333820000E-08 - 69, 67,-5.9965898921057310E-08, 2.4387429854293038E-08, 1.1470004728841000E-08, 1.1519384493185999E-08 - 69, 68,-2.1671136714669381E-08, 1.8850663863821788E-08, 7.3071140319775001E-09, 7.3460650890707001E-09 - 69, 69,-1.0383571238487469E-08,-8.4236539141892482E-09, 7.8673899825607005E-09, 7.8766571263402000E-09 - 70, 0,-2.8603563716425270E-08, 0.0000000000000000E+00, 3.3036358618039999E-08, 0.0000000000000000E+00 - 70, 1, 1.1923057265959440E-08,-2.3132840022126140E-08, 3.2754762122827998E-08, 3.2761682975584997E-08 - 70, 2,-1.6928177120201739E-08,-4.1713719279455724E-09, 3.2592841077081002E-08, 3.2576811072719999E-08 - 70, 3,-6.9010609245023119E-09,-4.7557734681538362E-08, 3.2655945152190999E-08, 3.2677263690263001E-08 - 70, 4,-6.8272368808751896E-09,-5.4607384715999674E-09, 3.2549457687285001E-08, 3.2507068350722002E-08 - 70, 5,-1.9336399949220781E-10,-2.7250150441747082E-09, 3.2539017183759000E-08, 3.2543807043927001E-08 - 70, 6, 4.4975395227936719E-09, 1.1661068420153800E-08, 3.2436185111276997E-08, 3.2456368615608003E-08 - 70, 7,-1.2019353197566890E-08, 1.1150701883847111E-08, 3.2408293910402002E-08, 3.2387304287722000E-08 - 70, 8, 6.8766476637806308E-09,-1.9034075968686030E-09, 3.2321801238236999E-08, 3.2337227822023998E-08 - 70, 9, 1.7277966048657530E-09,-1.8920219271599011E-08, 3.2236337985173999E-08, 3.2237971573286002E-08 - 70, 10,-2.5900083435111949E-10,-7.4913936693846679E-09, 3.2181979874522998E-08, 3.2179253567185998E-08 - 70, 11,-8.5245117521978992E-09,-2.5558035499327660E-08, 3.2064810772616003E-08, 3.2072395239354002E-08 - 70, 12,-1.7536754616242268E-08, 4.4962945424551696E-09, 3.2019128395321002E-08, 3.2019245577000001E-08 - 70, 13, 9.0115833813112283E-09, 1.5913338803171490E-08, 3.1902284087127001E-08, 3.1910857713347998E-08 - 70, 14, 2.0473272160462221E-08,-1.7288587686398790E-09, 3.1831882998986997E-08, 3.1826970692439999E-08 - 70, 15, 1.5880157366049359E-08, 1.6837768769751021E-08, 3.1719870439469003E-08, 3.1712647221808002E-08 - 70, 16,-8.0595499204450670E-09,-1.9446524666630890E-08, 3.1644721308376999E-08, 3.1638698533718000E-08 - 70, 17,-3.9367599857539848E-09, 1.6547853726037169E-09, 3.1567571866572000E-08, 3.1561534488425997E-08 - 70, 18,-7.7662621646509915E-09, 6.1420449570649934E-09, 3.1464625139540003E-08, 3.1441427909358999E-08 - 70, 19,-4.4910420668357208E-09, 2.1216015538631501E-08, 3.1383293778730998E-08, 3.1404902394845001E-08 - 70, 20, 2.6748651956661329E-08, 5.6792159307969828E-11, 3.1238636136058997E-08, 3.1262545351616000E-08 - 70, 21, 1.6742744053191691E-08,-1.0229597981398389E-08, 3.1143320640075997E-08, 3.1143095786917997E-08 - 70, 22, 9.5268440318647903E-09,-1.8512780913010250E-08, 3.1038433789517999E-08, 3.1036045964768997E-08 - 70, 23,-1.8266544198705539E-08, 3.1814321520416013E-08, 3.0908756105865001E-08, 3.0929918115760003E-08 - 70, 24,-8.2728325871809838E-09,-8.6183410004348951E-10, 3.0819107899202002E-08, 3.0818794326930002E-08 - 70, 25, 1.0130032858544400E-08,-5.8551813813940454E-09, 3.0707502423646003E-08, 3.0720631916265998E-08 - 70, 26, 3.3951932591009959E-09,-1.0578194974233620E-08, 3.0594663827963998E-08, 3.0602442145603998E-08 - 70, 27, 6.2214513703575664E-10,-2.8111399308176150E-08, 3.0481372827243001E-08, 3.0476292322883998E-08 - 70, 28,-1.8140539283510899E-08, 6.7992677859043479E-10, 3.0351169774461000E-08, 3.0350608576226997E-08 - 70, 29,-7.9075242241582574E-09, 1.1163992765071691E-08, 3.0245796025812998E-08, 3.0243769590772002E-08 - 70, 30,-1.3703801400170180E-08,-4.7161360203330443E-09, 3.0101290563962999E-08, 3.0103684257123999E-08 - 70, 31, 1.4003221372656730E-08, 1.5608092689935791E-08, 2.9991514958059001E-08, 2.9984440769634002E-08 - 70, 32,-1.6183549948463541E-08,-8.5080454180460340E-09, 2.9847433396170000E-08, 2.9834612963326002E-08 - 70, 33,-4.4786813657509232E-11,-2.9539150290257782E-09, 2.9736470484629001E-08, 2.9718999485217000E-08 - 70, 34, 1.4649739859033490E-08,-6.8507166460511294E-09, 2.9584126151458999E-08, 2.9568697540975000E-08 - 70, 35,-4.8081095299319731E-09,-1.4196644634685930E-08, 2.9436884360330000E-08, 2.9456173833964001E-08 - 70, 36, 6.9734105182903511E-10, 2.3805974252019039E-09, 2.9333068015122000E-08, 2.9323209995958000E-08 - 70, 37, 1.4615749786218570E-08, 1.7996265642523489E-08, 2.9149643124900998E-08, 2.9130915657676000E-08 - 70, 38,-3.1045989942545083E-08,-3.1320380048511530E-09, 2.9039014049285999E-08, 2.9051995280454001E-08 - 70, 39, 1.3148790281025351E-09, 1.6524798249333390E-09, 2.8818213028894000E-08, 2.8815070762879998E-08 - 70, 40,-1.3765722280346450E-08,-1.1534449477680619E-08, 2.8645743722837001E-08, 2.8641239597942000E-08 - 70, 41,-2.6111420022190232E-09, 7.3295059439551202E-09, 2.8468727799599000E-08, 2.8450107093946000E-08 - 70, 42,-1.1233762199402850E-08, 3.7699825858005492E-09, 2.8270280458084002E-08, 2.8276407758293999E-08 - 70, 43, 1.5769352596786721E-08,-1.1733717053254920E-08, 2.8079623536358001E-08, 2.8097440831092000E-08 - 70, 44,-9.3263217941864955E-09,-3.8010309117161091E-08, 2.7906503580290999E-08, 2.7889076797400000E-08 - 70, 45,-2.6401922213000899E-08,-1.0075860468347820E-08, 2.7677426278146000E-08, 2.7685539074631000E-08 - 70, 46, 1.1397124399644710E-08, 7.1278540214887320E-09, 2.7453481065677001E-08, 2.7462038876953000E-08 - 70, 47,-3.2075150947700420E-09,-1.8995829864121581E-08, 2.7229113067225000E-08, 2.7233527673057001E-08 - 70, 48,-2.1069454357050590E-08,-1.2870489658920280E-08, 2.6984331527436999E-08, 2.6957146583148000E-08 - 70, 49, 6.1762788302812462E-10, 2.4216868961622859E-08, 2.6732565609375999E-08, 2.6721918719371000E-08 - 70, 50,-2.2646450355798781E-08,-3.0382098438612532E-09, 2.6501322974567998E-08, 2.6503816954415001E-08 - 70, 51, 3.4426938830612930E-08,-3.3791449016225977E-08, 2.6196476585579001E-08, 2.6173471088411999E-08 - 70, 52,-4.7716242251803453E-08, 2.2562131911969629E-08, 2.5820733659258999E-08, 2.5812212875877001E-08 - 70, 53, 1.9937872600147260E-08,-3.2185599251037740E-09, 2.5490115775333000E-08, 2.5493672821989000E-08 - 70, 54,-1.3612300490370350E-08,-3.6031498495872218E-09, 2.5082406824579000E-08, 2.5100125883696001E-08 - 70, 55, 1.0079611623376240E-08,-1.5288087240950561E-08, 2.4673371032729999E-08, 2.4672787078322000E-08 - 70, 56, 1.0422612037056009E-08,-2.1918919236026019E-08, 2.4278073950657999E-08, 2.4267332600443999E-08 - 70, 57,-3.0158829872559971E-09,-2.1111548350054959E-08, 2.3851679201385998E-08, 2.3814336129879001E-08 - 70, 58, 1.4236432290351169E-08,-3.0007715221803049E-09, 2.3232516668796001E-08, 2.3245248925013001E-08 - 70, 59,-4.7165889902010462E-08,-1.5279460471512019E-11, 2.2762049187365000E-08, 2.2778956480098001E-08 - 70, 60,-1.9180703947653840E-08, 1.3749945641599169E-08, 2.2057378376204001E-08, 2.2039137180895000E-08 - 70, 61,-2.3048864121345989E-08, 3.5243726398225883E-08, 2.1534837234819002E-08, 2.1555460430171001E-08 - 70, 62, 1.6206555395291540E-08, 3.0222864022735292E-08, 2.0661514450857000E-08, 2.0684928569439002E-08 - 70, 63, 3.7354428350443083E-08,-2.0023548038207220E-08, 2.0022251842652999E-08, 2.0047719340333998E-08 - 70, 64,-1.2854662706360769E-10,-2.4650292634864379E-08, 1.9164259238250001E-08, 1.9117188336741001E-08 - 70, 65, 1.7472670958113491E-08, 1.8479864678455521E-08, 1.7746278876955000E-08, 1.7784450774821000E-08 - 70, 66,-1.4809281282693381E-08,-1.8813863583771640E-09, 1.6965468643746999E-08, 1.6954298439692000E-08 - 70, 67,-4.5133348623732204E-09, 2.8233612944585691E-08, 1.5367562652975001E-08, 1.5376706270083001E-08 - 70, 68, 6.8159336960863941E-08, 2.8549064858643569E-08, 1.1971334169210001E-08, 1.1992273570997000E-08 - 70, 69,-1.6589479214403129E-09, 3.7090077381570487E-08, 7.5680077660476996E-09, 7.6075363248372001E-09 - 70, 70,-2.6369438738045919E-09, 1.6323937086419450E-09, 8.6296054671129004E-09, 8.5936833185489998E-09 - 71, 0,-9.9669551926998209E-08, 0.0000000000000000E+00, 3.2722180312853999E-08, 0.0000000000000000E+00 - 71, 1,-1.6972003167125830E-08, 1.9829175912852659E-08, 3.2343398901947001E-08, 3.2355587798100998E-08 - 71, 2, 1.6410402991795750E-09, 1.4659266878294910E-08, 3.2178386520336002E-08, 3.2185749514866003E-08 - 71, 3, 2.4048888325664680E-08, 8.1143011875860034E-09, 3.2265996093558003E-08, 3.2287484987933998E-08 - 71, 4, 3.2714330064309511E-09,-2.6906149250308480E-09, 3.2136787846569998E-08, 3.2095410956967000E-08 - 71, 5, 1.0171848540962650E-08,-2.0776583995093090E-08, 3.2154014635038999E-08, 3.2155707176770002E-08 - 71, 6,-3.8367363613918274E-09, 1.3581771673407720E-08, 3.2043342973699003E-08, 3.2048745536286999E-08 - 71, 7, 3.1573302838679109E-09,-3.4454021587589530E-10, 3.2012588095870999E-08, 3.2001373985132001E-08 - 71, 8, 2.0521284352012259E-09, 9.6729511633649327E-09, 3.1928919288600998E-08, 3.1948397377237002E-08 - 71, 9,-1.3961870462699331E-08,-4.7344132336446522E-09, 3.1852131650347998E-08, 3.1849134607665002E-08 - 71, 10, 5.3782795028767539E-09,-2.1406386686778549E-08, 3.1802501797486999E-08, 3.1801407173451002E-08 - 71, 11,-4.9874586051620124E-09,-7.1600169774440699E-09, 3.1683313031173999E-08, 3.1689428285498997E-08 - 71, 12,-7.6633620504809635E-09, 3.7084112938933061E-09, 3.1649983842422997E-08, 3.1650581552353003E-08 - 71, 13, 2.7826732434214562E-08, 2.3312831331857679E-08, 3.1531728065111998E-08, 3.1539763671516997E-08 - 71, 14,-5.0927725395165751E-09,-3.6585749246244840E-09, 3.1491610897606998E-08, 3.1492798129552000E-08 - 71, 15, 1.5938846748911190E-08,-4.9420806144194544E-09, 3.1368540716649000E-08, 3.1357911026917003E-08 - 71, 16,-9.9075591615217204E-09,-4.6602890813109066E-09, 3.1332557030018002E-08, 3.1319175501138001E-08 - 71, 17,-7.8319674580579210E-09, 1.6491562634654959E-09, 3.1252295463175998E-08, 3.1239586295388997E-08 - 71, 18,-2.1016841004483651E-08,-2.5160294006566361E-09, 3.1160796309237003E-08, 3.1154439744085999E-08 - 71, 19,-1.4004550385970339E-08, 2.2457998927491969E-08, 3.1106851573634003E-08, 3.1125079993379003E-08 - 71, 20,-1.0224141971645081E-08, 7.6484765605507042E-09, 3.0964580497934001E-08, 3.0996359544639002E-08 - 71, 21, 2.9756774379517159E-08,-2.0506890009622570E-08, 3.0883240806770000E-08, 3.0884442686678998E-08 - 71, 22,-6.3419035703899323E-09,-1.5094330683236520E-08, 3.0794116017189999E-08, 3.0786555686601999E-08 - 71, 23, 6.4336129052406133E-09,-1.8356740998683499E-08, 3.0681351822984998E-08, 3.0703352292623003E-08 - 71, 24, 1.7598099628476890E-09,-2.9849119148757509E-10, 3.0602277920157001E-08, 3.0600609766588999E-08 - 71, 25,-3.1065712260445409E-10, 2.3400921025419790E-08, 3.0526743343221000E-08, 3.0541794046417998E-08 - 71, 26,-5.9930109456382523E-09,-1.9963760619881219E-08, 3.0405992883274997E-08, 3.0415278093546002E-08 - 71, 27,-1.7070470307427180E-09, 2.3157791194600560E-08, 3.0304271930509998E-08, 3.0308560917187997E-08 - 71, 28,-5.1361783257442134E-09,-4.0596491729985868E-09, 3.0200990259988998E-08, 3.0203395793211997E-08 - 71, 29,-2.1168834032021471E-08, 6.5723034144021624E-09, 3.0099685618860000E-08, 3.0101396406144000E-08 - 71, 30, 7.3959309096106614E-10, 5.3618943184920433E-09, 2.9993830481220999E-08, 2.9993499155011999E-08 - 71, 31, 2.2587894740109781E-08,-1.9681518909220922E-09, 2.9889234490202000E-08, 2.9883663891722002E-08 - 71, 32, 3.9637875866237476E-09,-1.9127237364091679E-08, 2.9767714085910999E-08, 2.9759439529323002E-08 - 71, 33, 1.8166797460546251E-08, 1.3824708342100070E-08, 2.9678826259123000E-08, 2.9656081114481000E-08 - 71, 34, 5.1837615324224992E-09, 8.9601994733948320E-09, 2.9540546559500000E-08, 2.9529437447896001E-08 - 71, 35,-1.9786395152533160E-08,-6.6575281071534936E-09, 2.9430751431623000E-08, 2.9438180617663999E-08 - 71, 36,-1.4000539013219580E-08,-1.9795667831988011E-08, 2.9336826907829998E-08, 2.9326938224310000E-08 - 71, 37,-2.4101638818299810E-08,-1.4269176741802449E-08, 2.9194320493058000E-08, 2.9181486051643001E-08 - 71, 38, 1.7378614074190311E-08,-3.0612642511839229E-09, 2.9094404854645999E-08, 2.9093088105969999E-08 - 71, 39, 1.3248124472054650E-08, 1.1106773542292081E-08, 2.8917997794760999E-08, 2.8921239608741001E-08 - 71, 40,-1.6798687786015549E-08,-2.2465708140776568E-09, 2.8790395860105000E-08, 2.8779378832863000E-08 - 71, 41,-3.1774220766311701E-08,-2.6914495772114439E-09, 2.8623376466681000E-08, 2.8613466981595998E-08 - 71, 42,-1.7876086568889071E-08, 5.6763295010055950E-09, 2.8481281942429999E-08, 2.8484660560318999E-08 - 71, 43, 1.9785656197862938E-09, 4.5080278025986258E-10, 2.8304157549488001E-08, 2.8312283182580999E-08 - 71, 44,-3.1594992291649627E-08, 2.5122146068178159E-09, 2.8197573226636002E-08, 2.8150978925395001E-08 - 71, 45, 5.1413684254885692E-09,-7.1265583036399699E-09, 2.7991919144613999E-08, 2.8002582607057001E-08 - 71, 46,-5.8581394222992342E-09, 8.7127693993753621E-09, 2.7800930091451000E-08, 2.7810503387818998E-08 - 71, 47, 2.3720012877269321E-08, 1.0684440477625681E-08, 2.7639485622009998E-08, 2.7648188802092000E-08 - 71, 48,-3.1844358069399347E-08,-2.2850767290228900E-08, 2.7402353856235000E-08, 2.7392767555700999E-08 - 71, 49, 9.2027595224473864E-09,-5.5691612414827853E-09, 2.7222121992588001E-08, 2.7229880939779999E-08 - 71, 50,-5.8195281279188491E-08, 4.0330838593115671E-09, 2.6994608583896000E-08, 2.6995813927083998E-08 - 71, 51, 3.3818658089504201E-08,-2.1109563865031301E-09, 2.6737787019683999E-08, 2.6733646300088000E-08 - 71, 52,-1.1807119779700420E-08,-4.3668667649641291E-09, 2.6445948079744999E-08, 2.6451246647251001E-08 - 71, 53, 2.0028043136214541E-08,-2.5319117048019139E-08, 2.6113119429121001E-08, 2.6114337097425001E-08 - 71, 54,-2.6927973889641291E-08, 9.3392357392173318E-09, 2.5834723676456000E-08, 2.5839750837433002E-08 - 71, 55,-9.5190611288506274E-09,-9.8649387834764429E-11, 2.5412091256660001E-08, 2.5416549306797001E-08 - 71, 56, 2.9698432586222858E-09, 1.6396699295273731E-08, 2.5127825150558001E-08, 2.5110824112931001E-08 - 71, 57,-2.2932561074105520E-08,-1.1228975245819830E-09, 2.4718784175073000E-08, 2.4694244095744999E-08 - 71, 58,-9.7654235363317687E-09,-3.0182747552584530E-08, 2.4245545000834001E-08, 2.4249099353825001E-08 - 71, 59,-2.4790257069595590E-08, 1.0220527043897430E-08, 2.3709210331838999E-08, 2.3730126264625999E-08 - 71, 60,-2.4134647301549351E-08, 1.1800138264930850E-08, 2.3243044866268999E-08, 2.3223216425783000E-08 - 71, 61, 2.6037040120129541E-08,-3.7037359850218130E-08, 2.2567866425664001E-08, 2.2577127408746001E-08 - 71, 62,-2.6786780288590279E-08, 1.9658084963050931E-08, 2.2134719438118001E-08, 2.2184300833106000E-08 - 71, 63,-3.0922936149157599E-09,-1.0592030247184380E-08, 2.1327976147104000E-08, 2.1338216532359001E-08 - 71, 64, 2.1775137372322929E-09, 2.0211488351192349E-08, 2.0697653465394001E-08, 2.0603354202599001E-08 - 71, 65,-1.2027501868100529E-08, 4.8052328818708468E-09, 1.9801720536824998E-08, 1.9854726615041999E-08 - 71, 66,-2.2921343393367021E-08,-1.4390435672782921E-08, 1.8409167498283000E-08, 1.8415956294903001E-08 - 71, 67, 1.3798342131329170E-08, 1.0368002686855321E-08, 1.7660340391174999E-08, 1.7642119879012999E-08 - 71, 68, 3.5639111635703982E-08, 4.6533588272512723E-09, 1.5912758513000999E-08, 1.5890336529468000E-08 - 71, 69, 6.1606342042180863E-08,-4.1156264184690961E-08, 1.2713933886716000E-08, 1.2685672586418000E-08 - 71, 70, 5.1293175883268118E-08, 2.9043610365190490E-08, 7.9147192010652001E-09, 7.9197036554027994E-09 - 71, 71,-6.6330531868963483E-09, 1.6537157821882598E-08, 9.3016579416225998E-09, 9.3083667157918994E-09 - 72, 0,-1.2400559997920529E-08, 0.0000000000000000E+00, 3.2304239816966999E-08, 0.0000000000000000E+00 - 72, 1, 8.0861192619732272E-09,-2.5170975454308920E-08, 3.1924985186672001E-08, 3.1949216608337997E-08 - 72, 2,-3.9391354669143189E-09,-5.3083824684429883E-09, 3.1766961869286998E-08, 3.1790273402163997E-08 - 72, 3,-6.2821444142459321E-09,-2.7983538172296119E-08, 3.1849458105724002E-08, 3.1873688486909000E-08 - 72, 4,-4.8904146793067710E-09,-2.5543799442943352E-10, 3.1727310199551000E-08, 3.1687965796262002E-08 - 72, 5,-8.4558626964249657E-09,-4.1843415452525244E-09, 3.1739358005362002E-08, 3.1738503672771000E-08 - 72, 6, 1.0750555071196580E-08, 1.5684858688265550E-08, 3.1642553985997001E-08, 3.1640770804799002E-08 - 72, 7, 2.4797188144752130E-09, 8.6955877876330657E-09, 3.1611211443357997E-08, 3.1603334442440999E-08 - 72, 8, 2.2223212062467300E-10, 6.1619823653171178E-09, 3.1537015407688003E-08, 3.1559228481545998E-08 - 72, 9, 1.3738809088981059E-08,-1.6070868603763529E-08, 3.1459897071127000E-08, 3.1454424057842002E-08 - 72, 10, 1.9970066349137920E-09, 4.4002196043969317E-09, 3.1423021994733000E-08, 3.1424143058249000E-08 - 72, 11,-6.1677571933725112E-09,-2.0265427132374409E-08, 3.1304980120674999E-08, 3.1307616782961001E-08 - 72, 12,-1.4180166323424979E-08,-1.0644402165157641E-08, 3.1285842088277002E-08, 3.1293492279146000E-08 - 72, 13, 2.2302177334952979E-08, 1.0550011525181380E-08, 3.1161032126826998E-08, 3.1165979827772999E-08 - 72, 14, 1.5366841919806931E-08,-1.2005088149135060E-08, 3.1127942780806003E-08, 3.1129978289248002E-08 - 72, 15, 3.1513980674151540E-09, 1.2418446931200630E-08, 3.1008127522071000E-08, 3.0994599982702997E-08 - 72, 16, 5.1300086631206253E-09,-5.1187053086573986E-09, 3.0981325629469000E-08, 3.0967432912810999E-08 - 72, 17,-1.3927324441700581E-08, 1.7826594669930088E-08, 3.0921395395613997E-08, 3.0904855278085001E-08 - 72, 18,-1.6793166246903090E-08, 7.4183914734368198E-09, 3.0817800456626999E-08, 3.0821578102235997E-08 - 72, 19, 1.5981339487919782E-08, 1.4681156451832240E-08, 3.0800247382441001E-08, 3.0813984073528001E-08 - 72, 20, 1.1837174149207389E-08, 3.9933533931539537E-09, 3.0644483874903003E-08, 3.0668375652492003E-08 - 72, 21, 3.1978412055190453E-08,-2.1218228098805071E-08, 3.0569815095000998E-08, 3.0572556942338999E-08 - 72, 22, 4.8208195055958381E-09,-2.0690035692550370E-08, 3.0489324536977999E-08, 3.0480766337005000E-08 - 72, 23,-6.6165346163196112E-09, 2.5198456371908448E-08, 3.0381157796900997E-08, 3.0399078406688998E-08 - 72, 24,-2.3319063280668671E-08, 5.0338620885428616E-09, 3.0318475350628001E-08, 3.0318993216337002E-08 - 72, 25, 4.8940500819204342E-09, 1.6585772746213190E-09, 3.0229421891480999E-08, 3.0242369072943998E-08 - 72, 26, 1.0008997001815649E-08,-7.7318467345780506E-09, 3.0153216071560998E-08, 3.0165958105731000E-08 - 72, 27, 4.7823421503767693E-09, 1.7840796900349469E-09, 3.0059339104376002E-08, 3.0065444810714002E-08 - 72, 28,-6.3562096876581640E-09, 7.5201297610048883E-11, 2.9964403267831999E-08, 2.9969544481763002E-08 - 72, 29,-1.3846442903533530E-08, 1.3063687414270490E-08, 2.9887604657632002E-08, 2.9890352761244998E-08 - 72, 30,-1.5650544312812840E-08, 3.0621322411119319E-09, 2.9785519205768002E-08, 2.9784947913298000E-08 - 72, 31, 2.6074970236616001E-08,-4.2479903649993244E-09, 2.9714618991458000E-08, 2.9713141899179000E-08 - 72, 32,-1.9886452766260940E-08,-1.4973579568647251E-08, 2.9595409343458998E-08, 2.9589489936551000E-08 - 72, 33,-5.2222999469853411E-09, 1.9322927683885319E-08, 2.9536721085513001E-08, 2.9512286851710000E-08 - 72, 34, 8.1622293437080507E-09, 5.4126598841253181E-09, 2.9413910267122999E-08, 2.9411388660240999E-08 - 72, 35,-1.9365710153314760E-08, 2.0945486332794931E-09, 2.9336754474282001E-08, 2.9320732102412999E-08 - 72, 36,-9.3271509527984338E-09, 7.4898289199419246E-09, 2.9282300316578002E-08, 2.9274525433795999E-08 - 72, 37, 1.1340633849627111E-08, 2.0502288699662019E-08, 2.9116794102917000E-08, 2.9109555181106001E-08 - 72, 38,-3.2549823652681770E-08,-1.2080609919362490E-08, 2.9105258443081000E-08, 2.9085088619871000E-08 - 72, 39, 4.3673968816460578E-09,-5.5246725573929090E-09, 2.8875816553183001E-08, 2.8882145151355001E-08 - 72, 40,-1.2914207380117560E-08,-1.8789353730746981E-09, 2.8789117568813000E-08, 2.8768352251284000E-08 - 72, 41, 2.3933716566861770E-10,-1.3060933193229850E-08, 2.8642429328760999E-08, 2.8646561995746000E-08 - 72, 42,-1.2194058585675750E-08, 5.3287840693302753E-09, 2.8511772302484999E-08, 2.8512778451277000E-08 - 72, 43, 2.4260411797904182E-08, 1.9351790994771169E-08, 2.8388727886737001E-08, 2.8388800954703000E-08 - 72, 44,-1.6229856825341321E-08,-6.4610891546807622E-09, 2.8281559128629000E-08, 2.8220768070339999E-08 - 72, 45,-5.6359066767091946E-09, 2.4986308895013620E-09, 2.8105611723965001E-08, 2.8116332614123998E-08 - 72, 46,-1.0687980840826080E-09, 2.5339961588514579E-08, 2.7953548064463001E-08, 2.7956170730341000E-08 - 72, 47,-7.6809531123382634E-09, 5.7748563312877850E-09, 2.7789915627347999E-08, 2.7792544730344001E-08 - 72, 48, 4.5393678115381311E-10,-3.2291470919922321E-08, 2.7599373842615000E-08, 2.7603489913287000E-08 - 72, 49, 2.0535209074818830E-09, 1.0545622624835560E-08, 2.7449648349050000E-08, 2.7447774632485000E-08 - 72, 50,-1.8679363635243709E-08,-1.8243923500786710E-08, 2.7295188933876000E-08, 2.7286088955480999E-08 - 72, 51, 4.4692291710314912E-08, 6.1218812463269743E-09, 2.7074290668637000E-08, 2.7079014028224000E-08 - 72, 52, 1.3605728755701820E-08, 1.1128405282782170E-08, 2.6822848282553002E-08, 2.6833493614060000E-08 - 72, 53,-8.4977076744883624E-09,-1.3326906916445950E-08, 2.6588780604505001E-08, 2.6601745236206000E-08 - 72, 54,-1.7858468017709110E-08, 9.1179610420679758E-09, 2.6307805386175999E-08, 2.6308179441832999E-08 - 72, 55, 7.0308975170499301E-09, 1.1064831997947520E-09, 2.6003229285678000E-08, 2.6008606006124000E-08 - 72, 56, 4.4601868723281169E-10, 2.0182616818735980E-10, 2.5739525091861999E-08, 2.5733415390970000E-08 - 72, 57,-1.2531450217643670E-09,-3.4717037273111938E-08, 2.5431469852234999E-08, 2.5414301146349001E-08 - 72, 58, 2.8779819477405450E-08, 1.4160473820741290E-08, 2.5031629811488999E-08, 2.5044180052901000E-08 - 72, 59,-2.9979959732671387E-08,-1.7603353798346799E-08, 2.4642282720645001E-08, 2.4674016806966001E-08 - 72, 60, 5.1741634477703881E-09, 1.3684359824363790E-08, 2.4103639736884001E-08, 2.4106617264768000E-08 - 72, 61,-9.7817214437971637E-09, 5.2303435451107023E-09, 2.3737987428077001E-08, 2.3708823738826001E-08 - 72, 62, 1.0908869399353859E-08, 1.9859815297657580E-08, 2.3026463818941999E-08, 2.3056461671347001E-08 - 72, 63, 1.6414926909048469E-08,-1.2547451780279930E-08, 2.2767047869682998E-08, 2.2695518737244000E-08 - 72, 64, 3.1876779365586422E-08,-1.2061135625039171E-09, 2.1832078983849999E-08, 2.1733207980593999E-08 - 72, 65, 4.4707014123123871E-08,-2.0621106375592171E-08, 2.1308814381471002E-08, 2.1257994652184002E-08 - 72, 66,-2.9241167935939869E-08,-3.7366723313258573E-08, 2.0537511647816001E-08, 2.0564915642871999E-08 - 72, 67,-2.8994127712788319E-08,-8.5690219739166553E-09, 1.9067485169466001E-08, 1.9072805295757001E-08 - 72, 68, 1.3585103235974300E-08,-7.5335129350855655E-09, 1.8381432835969999E-08, 1.8395246372969001E-08 - 72, 69, 4.5490984189121958E-09, 3.5216462280055030E-09, 1.6674229842645000E-08, 1.6722088897935998E-08 - 72, 70,-3.6951704237315931E-08,-8.4436021284281197E-09, 1.2930460086922000E-08, 1.2938401572436000E-08 - 72, 71, 1.4014721254529831E-09, 7.6701154851347941E-09, 8.2022312571959003E-09, 8.1691909974875993E-09 - 72, 72, 3.0109512089084069E-08,-5.9484322581029043E-09, 9.9302426770602995E-09, 9.9496308124608005E-09 - 73, 0,-5.4741093665270662E-08, 0.0000000000000000E+00, 3.1853110861192998E-08, 0.0000000000000000E+00 - 73, 1,-2.5693317519154569E-08, 1.8926017985384959E-08, 3.1392400512161002E-08, 3.1419047380892998E-08 - 73, 2,-8.0922326403307147E-09, 3.2789664716711121E-09, 3.1214459238027999E-08, 3.1286429980711000E-08 - 73, 3, 2.3331695127011891E-08, 1.5244645000583749E-08, 3.1331848540543998E-08, 3.1353134605871000E-08 - 73, 4, 2.0560672357391488E-09,-5.3440071906492728E-09, 3.1176339803461003E-08, 3.1150317263379002E-08 - 73, 5, 2.1036617688507812E-08,-2.5948452960461820E-08, 3.1230316037523001E-08, 3.1215929847339997E-08 - 73, 6,-3.5450443043776600E-09, 4.7565374325580562E-09, 3.1102715328379999E-08, 3.1100426956771999E-08 - 73, 7,-1.0825912688910680E-08, 8.8992170377227170E-09, 3.1079128807921999E-08, 3.1076388485386001E-08 - 73, 8, 1.2202191756818301E-08, 1.3828337127515269E-10, 3.0995700765555001E-08, 3.1032265912444998E-08 - 73, 9,-2.4721929170621640E-08,-1.4232064399891731E-08, 3.0928898140520998E-08, 3.0918036408212002E-08 - 73, 10, 3.8304752833644743E-09,-1.7037736969687330E-08, 3.0893419809058001E-08, 3.0899319952890999E-08 - 73, 11,-4.1014454401116401E-09,-1.3312221623789280E-09, 3.0756703722018001E-08, 3.0758146578916999E-08 - 73, 12,-9.2048790452939965E-09, 6.9878922614575126E-09, 3.0761810980272997E-08, 3.0771200949956002E-08 - 73, 13, 2.0171674662805600E-08, 1.1970064958384629E-08, 3.0607016967128001E-08, 3.0614833617862998E-08 - 73, 14,-1.1564044861758020E-08,-7.5947855393786930E-09, 3.0625435329757001E-08, 3.0629960461702000E-08 - 73, 15, 5.4762781081225561E-09,-8.2168640102559375E-09, 3.0467754004323997E-08, 3.0447063755049998E-08 - 73, 16, 5.3768934513216186E-09, 2.9149710208360549E-09, 3.0495412943951001E-08, 3.0473157885388999E-08 - 73, 17,-3.3024394527175541E-09,-1.0211270185853780E-08, 3.0419541754220000E-08, 3.0391662738098002E-08 - 73, 18,-2.1073279695128969E-08, 2.6906342069222108E-09, 3.0325933783487998E-08, 3.0354848387682997E-08 - 73, 19,-3.2108539387134749E-09, 2.2396627403926751E-08, 3.0342044262041999E-08, 3.0342879545047001E-08 - 73, 20,-5.3190341515864328E-09, 2.3202686860515431E-08, 3.0181428443743000E-08, 3.0205229514873002E-08 - 73, 21, 1.8542022380512279E-08, 9.0634840293188165E-09, 3.0101129619334997E-08, 3.0109349715164003E-08 - 73, 22, 4.4409278756620576E-09,-3.1566610529249421E-09, 3.0040335917309002E-08, 3.0026370195219997E-08 - 73, 23,-1.0532146277374580E-08, 4.1447345397356967E-09, 2.9946632610083003E-08, 2.9963072222219998E-08 - 73, 24,-1.3253913072054650E-08,-1.2614778301445610E-08, 2.9879209273120000E-08, 2.9880359488098003E-08 - 73, 25, 1.5410644927128019E-08, 8.3743960752836795E-09, 2.9865271732220000E-08, 2.9883926012912998E-08 - 73, 26,-3.1482346640230860E-09,-2.0788514546403189E-08, 2.9717624500683999E-08, 2.9730962858476999E-08 - 73, 27, 1.2198662639287141E-08, 1.7062074299846991E-08, 2.9669154558144001E-08, 2.9679172835920999E-08 - 73, 28, 1.3029895157416510E-08,-1.4326692678052160E-08, 2.9564480438613000E-08, 2.9571075850954999E-08 - 73, 29,-1.2769099818438640E-08, 1.8576959117096880E-08, 2.9507646399117999E-08, 2.9510085360810001E-08 - 73, 30,-2.3426554312624860E-09, 6.9522948784894772E-09, 2.9415366872248999E-08, 2.9414646442784999E-08 - 73, 31, 1.8543160489504450E-08,-8.5016620132344634E-09, 2.9350312188769000E-08, 2.9357633054978999E-08 - 73, 32,-4.8622132824035226E-09,-1.3540584118015830E-09, 2.9254614942096000E-08, 2.9251687447468000E-08 - 73, 33, 8.7505934499858853E-09,-1.7755772274068879E-08, 2.9202958718748001E-08, 2.9175044473426000E-08 - 73, 34, 2.9549936808802122E-09, 1.4320146835805880E-08, 2.9092981194592001E-08, 2.9104014991340999E-08 - 73, 35,-2.7746161904575618E-08, 1.9342711423253679E-08, 2.9053720602177001E-08, 2.9012116900272001E-08 - 73, 36,-5.7314244005564442E-09,-8.7650271232197934E-09, 2.8989865845984999E-08, 2.9000012402945000E-08 - 73, 37, 1.6822268415222970E-09, 4.1129030362602073E-09, 2.8876973110971000E-08, 2.8883860227333000E-08 - 73, 38,-6.1822889155722671E-09,-1.6225511562355602E-08, 2.8858412858609001E-08, 2.8816307667862999E-08 - 73, 39,-7.5209593092276301E-09,-3.9112606686174787E-09, 2.8694942228440999E-08, 2.8711805825626999E-08 - 73, 40,-2.7517460483207380E-08,-1.0214540035513699E-09, 2.8611789914119999E-08, 2.8588246437973999E-08 - 73, 41,-4.3396022448906552E-09,-8.7098777174848236E-09, 2.8497171970786001E-08, 2.8519644023844000E-08 - 73, 42,-9.5261180864394542E-09,-1.3530735779118541E-08, 2.8415864603307999E-08, 2.8406426609902000E-08 - 73, 43,-2.9596129258802169E-09,-1.1558351879537550E-08, 2.8310172495309000E-08, 2.8298208463797000E-08 - 73, 44,-1.9725479802088840E-08,-7.2172154836892608E-09, 2.8249075045048998E-08, 2.8203917850255999E-08 - 73, 45, 1.2109535796542291E-08,-2.0259807776412699E-09, 2.8094231998956999E-08, 2.8103967009766999E-08 - 73, 46,-1.1305474581606660E-08, 2.9162116167722511E-09, 2.8007169396252999E-08, 2.7996800109840002E-08 - 73, 47, 1.7591304519975371E-08, 2.5562850421019249E-08, 2.7892298445972000E-08, 2.7893317322759002E-08 - 73, 48,-1.5241509582782481E-08,-3.6210054636252202E-08, 2.7697226358652000E-08, 2.7714698612800001E-08 - 73, 49, 1.4673587515577570E-08,-4.0942166371660022E-10, 2.7644091541226000E-08, 2.7653354967144998E-08 - 73, 50, 3.3063910496673180E-09, 6.0306834735311729E-09, 2.7426962599196999E-08, 2.7413501880650000E-08 - 73, 51, 3.9793276665634052E-08, 8.5835067168987234E-09, 2.7303183643632998E-08, 2.7320311544823998E-08 - 73, 52,-1.5882375084015339E-08,-9.7961432962044899E-09, 2.7049610866690999E-08, 2.7061694730803000E-08 - 73, 53, 7.9113005303859590E-09,-7.2576117741929739E-09, 2.6863917447584999E-08, 2.6874668587755002E-08 - 73, 54,-2.8303185352910790E-08, 2.9437657299182630E-08, 2.6652697990482999E-08, 2.6646421761030001E-08 - 73, 55, 2.0877114712311380E-08, 1.8084463184397121E-08, 2.6353661826062000E-08, 2.6353267637363001E-08 - 73, 56, 1.0093269685306939E-08,-3.1322259745319729E-09, 2.6170562463766000E-08, 2.6159838362128001E-08 - 73, 57,-1.8199346497611261E-08,-1.2341428361233509E-08, 2.5871886797413001E-08, 2.5869204372138000E-08 - 73, 58,-2.7227314818093759E-08,-8.9894781637201285E-09, 2.5550142874913000E-08, 2.5553228082713999E-08 - 73, 59,-1.9554747623991011E-08,-2.6291038423612571E-08, 2.5191041291585001E-08, 2.5225162112812002E-08 - 73, 60, 4.2308201643079419E-10,-1.0897321138695080E-08, 2.4810677234645999E-08, 2.4815703310126999E-08 - 73, 61,-1.5303138052917889E-08, 7.4345536795173124E-09, 2.4349278354494999E-08, 2.4354206496032999E-08 - 73, 62,-1.6595563708347159E-08, 2.0524931155827349E-08, 2.4033225826939000E-08, 2.4023169937890000E-08 - 73, 63,-5.7194974429583177E-10,-1.3086725437076040E-08, 2.3471215370364999E-08, 2.3455322341739999E-08 - 73, 64,-2.0040041280622340E-09, 6.4320600673165623E-09, 2.3028610019652999E-08, 2.3055597583575001E-08 - 73, 65,-8.5518149983290422E-09, 1.7825477299461139E-08, 2.2233103872789000E-08, 2.2211620033390001E-08 - 73, 66,-1.4407254661105271E-08,-6.1659955876943662E-10, 2.1795780124255999E-08, 2.1857549960167999E-08 - 73, 67,-1.5906940974271650E-09, 2.2667132676949479E-08, 2.0905520432346998E-08, 2.0972755309152001E-08 - 73, 68, 3.7404434051274757E-08,-1.4045722279351511E-09, 1.9584819659375000E-08, 1.9578043735913999E-08 - 73, 69,-1.6133961635387780E-09, 1.1483182599622450E-08, 1.8873305708993001E-08, 1.8910258912401998E-08 - 73, 70,-3.2270022345185597E-08, 4.1734216780398537E-08, 1.6890499482160000E-08, 1.6924658578063999E-08 - 73, 71,-6.2517853081282672E-08, 4.3988808207129403E-08, 1.3431841150515000E-08, 1.3492724824515000E-08 - 73, 72, 5.4059865046867321E-09,-2.9657209661693879E-08, 7.9816395768636996E-09, 7.9589388354222005E-09 - 73, 73, 3.1531773877331532E-08, 1.7744208338832571E-08, 1.0566635178122999E-08, 1.0592024893628001E-08 - 74, 0, 1.0075095302618919E-09, 0.0000000000000000E+00, 3.1332401432589002E-08, 0.0000000000000000E+00 - 74, 1, 9.7280744073455260E-09,-1.9688685572869588E-08, 3.0899355935951000E-08, 3.0935517224389002E-08 - 74, 2, 5.8382050022705834E-09,-1.0376476318903401E-08, 3.0726928896512997E-08, 3.0823567797935003E-08 - 74, 3,-6.2393781927674953E-09,-3.1434745408575618E-08, 3.0838500982598997E-08, 3.0858579736032999E-08 - 74, 4, 6.7706886520845637E-10, 3.8470563461167074E-09, 3.0690669716231999E-08, 3.0673180067371999E-08 - 74, 5,-2.1302668974750321E-08, 3.8760204531147372E-09, 3.0736735289011001E-08, 3.0718292192408998E-08 - 74, 6, 1.8887448284673311E-09, 4.6548054931092217E-09, 3.0621151705821999E-08, 3.0620902030527003E-08 - 74, 7, 8.8910521172329863E-09,-1.1987991401491929E-09, 3.0603063176234002E-08, 3.0594676049820003E-08 - 74, 8, 5.1231791832347462E-09, 6.6857252258708036E-09, 3.0523370781407998E-08, 3.0565353116815001E-08 - 74, 9,-6.8177226329305461E-09,-1.7828236732168929E-08, 3.0460446452600997E-08, 3.0447826638579998E-08 - 74, 10, 1.6799790586647100E-09,-5.0062154013370718E-11, 3.0437571419136000E-08, 3.0443240068849002E-08 - 74, 11,-1.0502091304239840E-08,-2.7282584798622119E-08, 3.0294476344195001E-08, 3.0297420594524999E-08 - 74, 12,-1.1051955307534880E-08,-9.1042587957634570E-09, 3.0324540586312997E-08, 3.0338789144491003E-08 - 74, 13, 1.3706727146467690E-08, 6.8259934514329181E-09, 3.0151231589690998E-08, 3.0151656208619001E-08 - 74, 14,-6.5828114269428559E-10,-1.4102816776234701E-08, 3.0180179957077003E-08, 3.0185577886888002E-08 - 74, 15, 2.0929859994879220E-09, 1.4702473374405839E-08, 3.0025242910728002E-08, 3.0005095205481997E-08 - 74, 16,-7.3396905117352449E-09,-8.0998450215158468E-10, 3.0053250634358001E-08, 3.0032947713774000E-08 - 74, 17,-6.4586224604997291E-09, 1.6780956621800262E-08, 3.0005671853427002E-08, 2.9971872801758001E-08 - 74, 18,-2.5576044026497250E-08,-3.6137738492460249E-09, 2.9889430524349997E-08, 2.9922902402253000E-08 - 74, 19, 4.5026250913407378E-09, 8.8063779908964879E-09, 2.9937538808448001E-08, 2.9942168296745001E-08 - 74, 20, 1.4420627817618549E-08, 3.0187285064099519E-09, 2.9764118302831000E-08, 2.9773448095308000E-08 - 74, 21, 2.8610205089438201E-08,-2.9439727799811380E-08, 2.9681514925355001E-08, 2.9694810899638998E-08 - 74, 22, 3.7771142276217457E-09,-7.3144262868252003E-09, 2.9623851430434000E-08, 2.9611772183968000E-08 - 74, 23,-2.0439583641830281E-09, 5.2570086718028024E-09, 2.9531301101329001E-08, 2.9542757915560001E-08 - 74, 24,-3.4697069030418351E-08, 3.8737020804707541E-10, 2.9479263325078000E-08, 2.9479190797049000E-08 - 74, 25, 1.2710262753414379E-08, 2.1872265728607611E-09, 2.9436416491409999E-08, 2.9438293378712999E-08 - 74, 26, 9.9311429338501156E-09, 3.3528708643665978E-10, 2.9359852837604000E-08, 2.9372544354913999E-08 - 74, 27, 1.4960304177396081E-09, 2.6371128749702250E-08, 2.9279556956110001E-08, 2.9295217073261999E-08 - 74, 28,-1.9166422256374419E-09,-2.0441602759535269E-08, 2.9209492015937999E-08, 2.9214690647255999E-08 - 74, 29,-1.3485586208902279E-08, 1.4290709851165180E-08, 2.9154536003892000E-08, 2.9152438795154002E-08 - 74, 30,-1.5281871399872789E-08,-2.2157672593337750E-09, 2.9068548742639000E-08, 2.9065990081161999E-08 - 74, 31, 2.8490905907943262E-08,-3.5812345661718962E-09, 2.9022697212027002E-08, 2.9036167808250002E-08 - 74, 32,-1.1471024477268380E-08,-5.7786076336430004E-09, 2.8929060361668000E-08, 2.8927585836335999E-08 - 74, 33, 5.9391419943654012E-09, 1.4948147833063799E-08, 2.8911960129134998E-08, 2.8880018548081000E-08 - 74, 34, 1.1808556071112700E-08, 1.7663456463909311E-08, 2.8795147314267998E-08, 2.8816085971700000E-08 - 74, 35,-1.6215338381765901E-08, 2.3461507678399788E-08, 2.8793754987838001E-08, 2.8732883155273999E-08 - 74, 36, 7.3663137674168382E-09, 7.1967417856108246E-09, 2.8760600287301999E-08, 2.8789274769660000E-08 - 74, 37, 1.0555380051232289E-08,-9.4501969568586484E-09, 2.8604811296367001E-08, 2.8630617621010001E-08 - 74, 38,-1.6126661586148269E-08,-1.6874806556435301E-08, 2.8721422837285999E-08, 2.8647793678032999E-08 - 74, 39,-1.0821070428213859E-08, 1.8643884486158760E-08, 2.8434574302589000E-08, 2.8450844015982001E-08 - 74, 40,-2.8903992676261608E-08,-2.4635873452906769E-09, 2.8424655485779001E-08, 2.8400784452689001E-08 - 74, 41,-3.6501829178763288E-09,-1.5494328822211251E-08, 2.8277665981059000E-08, 2.8308219791283000E-08 - 74, 42, 7.4281523917726178E-09,-7.3143502808522306E-09, 2.8240905644524999E-08, 2.8227811130474999E-08 - 74, 43, 4.0090046755752222E-09, 1.3706598682103540E-08, 2.8152996678439999E-08, 2.8139098006765000E-08 - 74, 44,-1.6456097903566950E-08,-5.2517252028764690E-09, 2.8087533877809998E-08, 2.8048085040038001E-08 - 74, 45, 1.1410400145279960E-08,-1.2198872455205670E-08, 2.7972686839029999E-08, 2.7981623081776999E-08 - 74, 46, 5.0215385266106488E-09, 6.8312529776234621E-09, 2.7884717914952999E-08, 2.7871909927184999E-08 - 74, 47, 1.0218386521398239E-08, 1.4956354486089900E-08, 2.7781426246518000E-08, 2.7770431257513999E-08 - 74, 48,-1.9385473035965770E-08,-1.4032042799955039E-08, 2.7643699799397001E-08, 2.7659029835348001E-08 - 74, 49, 6.0798499458002649E-09,-1.1252952225701209E-08, 2.7544467154658000E-08, 2.7543157289043999E-08 - 74, 50,-4.0467447236503293E-08, 1.2866875283719640E-08, 2.7488391798228001E-08, 2.7465451571146001E-08 - 74, 51, 1.3058369595418580E-08, 2.5235260033298021E-08, 2.7295463468578000E-08, 2.7309768442138000E-08 - 74, 52,-9.4925285851321308E-10,-3.0016630154808312E-08, 2.7144144157270001E-08, 2.7149329060281002E-08 - 74, 53,-1.1717606258201639E-09,-1.3144062485990970E-08, 2.6986783052470001E-08, 2.7003106357902000E-08 - 74, 54,-4.2979502454087718E-09, 1.3835523394905591E-08, 2.6816683057475999E-08, 2.6802417190518999E-08 - 74, 55,-1.3964582121447690E-08, 1.1442276484206331E-08, 2.6589819407297001E-08, 2.6583418619510999E-08 - 74, 56,-7.4355996700606504E-09,-2.3892213010518999E-08, 2.6423057985282002E-08, 2.6420826132911999E-08 - 74, 57, 8.5893820495418265E-09, 4.6287818157601543E-09, 2.6196746281063999E-08, 2.6203034626917001E-08 - 74, 58, 1.4223319749053999E-08,-7.2525356592532598E-09, 2.5967234592157000E-08, 2.5972330051369000E-08 - 74, 59,-3.1560945760425668E-08,-1.6233989775165059E-08, 2.5641711714729001E-08, 2.5671006480948001E-08 - 74, 60,-1.0044742383552881E-08, 1.1633309733253759E-08, 2.5342912231703000E-08, 2.5344633813982001E-08 - 74, 61,-1.3719551806473151E-08, 3.6576239767861932E-09, 2.5032642804470999E-08, 2.5036660605096000E-08 - 74, 62,-7.7568104335235130E-09,-9.1145960807675060E-09, 2.4504442441604999E-08, 2.4499502769383000E-08 - 74, 63, 1.4158160495001469E-09,-6.4996803071943812E-09, 2.4342666166959999E-08, 2.4342475490696999E-08 - 74, 64,-9.3909834045483080E-09,-4.8242148396467982E-08, 2.3597821106513000E-08, 2.3636689854764001E-08 - 74, 65, 3.4032045911106451E-08, 7.4997005562898456E-09, 2.3476821068320002E-08, 2.3387712156629000E-08 - 74, 66,-3.8764835942804832E-08,-3.4562456110417973E-08, 2.2637880817535000E-08, 2.2685028716510001E-08 - 74, 67,-3.2394347205966272E-08,-5.0447308844694773E-08, 2.2168936584136000E-08, 2.2158228692076001E-08 - 74, 68,-1.4533345021218889E-08,-1.5283116324058440E-08, 2.1512722190631001E-08, 2.1516337035062000E-08 - 74, 69, 1.9694781066984122E-08, 4.6987237855780159E-09, 1.9969597049261001E-08, 1.9998366688941000E-08 - 74, 70, 8.1001982648263366E-09, 5.0194938793936178E-09, 1.9431571324358000E-08, 1.9443651300339000E-08 - 74, 71,-1.3499424263447450E-08,-3.7384219772171420E-08, 1.7443061840033000E-08, 1.7425027572625001E-08 - 74, 72, 4.3766182611447093E-08,-5.1102401895543206E-09, 1.3268874046908000E-08, 1.3244484317584000E-08 - 74, 73, 1.1643052517374630E-08,-4.8503962606024993E-09, 8.2427942304512004E-09, 8.2052739066543001E-09 - 74, 74, 3.9416412373467242E-08,-7.8497399090510311E-08, 1.1089099053353999E-08, 1.1072578996428999E-08 - 75, 0,-4.6896353147918351E-08, 0.0000000000000000E+00, 3.0790196465616998E-08, 0.0000000000000000E+00 - 75, 1,-2.2592220188599031E-08, 1.4004550230924031E-08, 3.0308720035565001E-08, 3.0342892968273003E-08 - 75, 2,-1.1489675614779261E-08, 1.2488378151695839E-09, 3.0090876019701002E-08, 3.0255677507342999E-08 - 75, 3, 2.3917301221307231E-08, 2.7167552255778501E-08, 3.0261687851638999E-08, 3.0269818855956003E-08 - 75, 4, 2.2431502654085531E-09,-1.2452216208437420E-08, 3.0059938794054000E-08, 3.0068701100105997E-08 - 75, 5, 2.4557419415764809E-08,-1.8021397424405839E-08, 3.0161977311540000E-08, 3.0121060431760997E-08 - 75, 6, 1.4593249956870580E-08,-3.1237499023377250E-09, 2.9991293984441998E-08, 3.0007496030171000E-08 - 75, 7,-5.6494043952309296E-10, 8.3606665288044001E-09, 3.0002355425987001E-08, 2.9986005637715998E-08 - 75, 8, 1.7205734816053740E-08,-8.7007436653910335E-09, 2.9893813224326002E-08, 2.9959779539497999E-08 - 75, 9,-7.1861227215401604E-09,-1.6008310943955761E-08, 2.9846942035006003E-08, 2.9828450422923999E-08 - 75, 10, 8.4184668515083113E-09, 3.3229660845952100E-10, 2.9819558779293998E-08, 2.9832552342006999E-08 - 75, 11,-1.3480374292945201E-08,-1.5826380406286020E-09, 2.9646844908046002E-08, 2.9650162256657001E-08 - 75, 12, 5.5657154819099028E-10, 7.7777717029185979E-09, 2.9704000512095999E-08, 2.9720030310418001E-08 - 75, 13, 1.7238187266363859E-08, 1.9611179251118052E-08, 2.9475578483170000E-08, 2.9494204821185000E-08 - 75, 14,-1.1014627571655960E-08,-5.3122128393950564E-09, 2.9567874138436001E-08, 2.9576271275326001E-08 - 75, 15, 1.8123628394921099E-08,-9.1636833905229820E-09, 2.9351857375967000E-08, 2.9324139033560001E-08 - 75, 16, 8.5615909432337770E-09, 5.7857900040811383E-10, 2.9447240155291000E-08, 2.9420227530740000E-08 - 75, 17,-1.3543572009815250E-08,-1.0930028987987230E-08, 2.9363918241421001E-08, 2.9317144188804001E-08 - 75, 18,-2.1945493454236149E-08,-2.7195403767574349E-10, 2.9268941781789000E-08, 2.9332339306699999E-08 - 75, 19, 1.0041680689885469E-08, 2.2221091019352519E-08, 2.9339729157776999E-08, 2.9318650425820000E-08 - 75, 20, 4.7540706040427858E-10, 9.8099131966942408E-09, 2.9165313154904999E-08, 2.9167760668130000E-08 - 75, 21, 3.3473641591781647E-08, 5.0901818605137374E-09, 2.9039425733817001E-08, 2.9069954150142000E-08 - 75, 22, 3.0905734568768900E-08,-1.1630735277352811E-08, 2.9021776634184001E-08, 2.9002298442249001E-08 - 75, 23,-1.2077910599331690E-08, 1.3782249641757650E-08, 2.8919382712053001E-08, 2.8934201274638000E-08 - 75, 24,-2.3454153833176751E-08, 3.4096962487687702E-09, 2.8872310715345001E-08, 2.8869008859858999E-08 - 75, 25, 7.2552602308290104E-09, 1.0196033044098790E-08, 2.8917346045988001E-08, 2.8947230633014001E-08 - 75, 26, 1.4954735791116050E-09,-8.2357350824665586E-10, 2.8719478009692999E-08, 2.8729571647766999E-08 - 75, 27, 1.0166408650948139E-08, 1.3011408311723590E-09, 2.8698352076590998E-08, 2.8724793638669001E-08 - 75, 28, 5.4859435063085232E-09,-2.5433844057488710E-08, 2.8584993429908001E-08, 2.8588566140959001E-08 - 75, 29,-2.1237084854367151E-08, 2.9148021009514070E-08, 2.8569706825657001E-08, 2.8568015944225001E-08 - 75, 30,-2.6135990199764919E-09, 1.4861830179316009E-08, 2.8459188262275001E-08, 2.8453968903437001E-08 - 75, 31, 1.3028227968488190E-08,-6.4457592785768308E-09, 2.8423052087117000E-08, 2.8440940705947001E-08 - 75, 32,-2.1611515301590511E-08,-3.3878461271873708E-09, 2.8329937779834001E-08, 2.8328073366292000E-08 - 75, 33, 4.4491836586969890E-09,-1.1404533462432310E-09, 2.8313203386331001E-08, 2.8282360195086002E-08 - 75, 34,-1.2966968824587161E-08, 3.8648224673189872E-09, 2.8198380665509999E-08, 2.8239545863871999E-08 - 75, 35,-3.2700622916454551E-08, 8.1795421393296579E-09, 2.8212586931190001E-08, 2.8128125693264001E-08 - 75, 36,-3.1736313896168232E-09,-3.1533093391336070E-09, 2.8188526999179999E-08, 2.8242999191411000E-08 - 75, 37, 2.0433933002914222E-08, 1.7970738355989550E-08, 2.8041349906848000E-08, 2.8100698809307001E-08 - 75, 38,-1.9430804820971561E-08,-8.1717147649672835E-09, 2.8170082651116999E-08, 2.8078218633378000E-08 - 75, 39,-4.8658462139157183E-09,-8.7584994060815594E-09, 2.7956991256435000E-08, 2.7980667876684001E-08 - 75, 40,-4.2498272957354219E-09, 1.9664993544376920E-08, 2.7898861520281001E-08, 2.7886766517764002E-08 - 75, 41, 4.7380510101968991E-09,-1.1881286744552161E-08, 2.7819450303963001E-08, 2.7874914447313000E-08 - 75, 42,-1.2302054753439581E-08,-9.7409571670223080E-09, 2.7788172648337000E-08, 2.7768005241990000E-08 - 75, 43, 1.0568085129256759E-08,-9.1245776790206231E-10, 2.7752728441382000E-08, 2.7732458347114999E-08 - 75, 44,-6.5832878759307027E-09,-1.9399965598599942E-09, 2.7702764258272000E-08, 2.7711945353532999E-08 - 75, 45, 1.1582704133514700E-08,-1.0226266918236140E-08, 2.7631218848474999E-08, 2.7638743366526999E-08 - 75, 46,-3.2627902505987739E-09, 9.2232465562029622E-09, 2.7628471319504999E-08, 2.7604648667289001E-08 - 75, 47, 4.1950998728464543E-08, 2.0187849948557899E-08, 2.7533159531270999E-08, 2.7522010448756000E-08 - 75, 48, 9.7818771333576663E-09,-3.8310433309344022E-08, 2.7441544613863000E-08, 2.7455968755336999E-08 - 75, 49, 8.8023201980156809E-09,-1.0344179675831800E-08, 2.7453661783078998E-08, 2.7449487803124000E-08 - 75, 50, 4.6929200000558797E-10,-9.0915621079922396E-09, 2.7240998321179000E-08, 2.7226039545250002E-08 - 75, 51, 4.7166738224088268E-08, 2.1482290312565371E-08, 2.7287645446410999E-08, 2.7305205509202000E-08 - 75, 52,-1.1299759886721820E-08,-1.8723605672709010E-08, 2.6979537991242999E-08, 2.6979934744044001E-08 - 75, 53,-1.0464448973376690E-08, 8.3269792256715692E-09, 2.6952933078306001E-08, 2.6973710180132999E-08 - 75, 54,-1.9924729532869841E-08, 1.9737346783039450E-08, 2.6756951518010000E-08, 2.6749720810639999E-08 - 75, 55, 3.1779917119005317E-08, 3.2921909620133463E-08, 2.6602210933736000E-08, 2.6580830853626001E-08 - 75, 56, 3.9027133981582889E-09,-2.5435481377500840E-08, 2.6450512067214999E-08, 2.6448432825298001E-08 - 75, 57,-1.2612163164246850E-09, 1.0425085082161961E-08, 2.6265692451217002E-08, 2.6280010776014000E-08 - 75, 58,-1.6384850120440630E-08, 9.5760975006338343E-09, 2.6045493631161999E-08, 2.6039110269743000E-08 - 75, 59, 2.8223034543075930E-08,-5.8152635322268044E-09, 2.5782502127589999E-08, 2.5811925979322999E-08 - 75, 60, 7.0794721625292634E-09,-2.0928410389576309E-08, 2.5527217418160999E-08, 2.5533856050238000E-08 - 75, 61,-1.0646680294221530E-08,-8.7918602535505631E-09, 2.5227481623730001E-08, 2.5261393158642002E-08 - 75, 62,-2.3171923154297691E-08, 1.0343211168529460E-08, 2.5001919141784000E-08, 2.4976898813550999E-08 - 75, 63, 2.7593746524158100E-08,-3.9576713898458146E-09, 2.4645724003286001E-08, 2.4682933309256998E-08 - 75, 64, 3.1148780354186972E-08, 2.7251641718048100E-08, 2.4263285894677001E-08, 2.4417726770759999E-08 - 75, 65,-1.3058503363408430E-08,-7.2923895868523184E-09, 2.3849893822348998E-08, 2.3818480853252002E-08 - 75, 66, 4.9739139505425745E-10,-3.3073378539145731E-08, 2.3740229430299000E-08, 2.3770751648578000E-08 - 75, 67, 3.8476879717568003E-09, 4.0793960126823598E-09, 2.2861140038572001E-08, 2.2838024459891000E-08 - 75, 68,-6.6363768381986002E-09, 4.1510073368538271E-09, 2.2641153814067001E-08, 2.2635679652402000E-08 - 75, 69,-3.1667830476878962E-08,-1.9875903626184519E-08, 2.1754531113504001E-08, 2.1733972794247001E-08 - 75, 70,-1.5655056517275641E-08,-2.4115044540884171E-09, 2.0490838265395999E-08, 2.0486947089326999E-08 - 75, 71, 2.8279821168558261E-08,-1.0590150252496721E-08, 1.9855564205973001E-08, 1.9860801041017998E-08 - 75, 72, 2.1800540481196129E-08,-3.6040807302936321E-09, 1.7330633425846000E-08, 1.7358854313588002E-08 - 75, 73,-1.0906481447157671E-08, 1.4697366851891760E-08, 1.2993485547474000E-08, 1.2985913840682999E-08 - 75, 74, 3.5346406472764548E-09, 9.0478146947524686E-09, 6.6768082652238002E-09, 6.6879501223241000E-09 - 75, 75, 3.3081630082587631E-08,-6.3413289624522398E-09, 1.0411508025864999E-08, 1.0532915055619001E-08 - 76, 0,-1.5275562265008611E-08, 0.0000000000000000E+00, 3.0213832972279998E-08, 0.0000000000000000E+00 - 76, 1, 1.3542100665971810E-08,-8.2180330560394501E-09, 2.9787275279016000E-08, 2.9829917197802998E-08 - 76, 2, 5.0657700631733980E-09,-9.5117019631845240E-09, 2.9572646109747001E-08, 2.9759119175477999E-08 - 76, 3,-1.1936117971844610E-08,-3.5031302786457520E-08, 2.9735483258741999E-08, 2.9737757561228999E-08 - 76, 4,-1.0124385549915980E-08, 1.0792848761168861E-09, 2.9545616487865999E-08, 2.9567263804015999E-08 - 76, 5,-2.1737994856806980E-08, 6.6173065051489138E-09, 2.9636168275081999E-08, 2.9587918993118999E-08 - 76, 6,-5.3327992829587652E-09, 7.2786983083775231E-09, 2.9477948754060001E-08, 2.9503449963428999E-08 - 76, 7, 1.9008737776440359E-09,-7.0126705605203524E-09, 2.9494534317601999E-08, 2.9468908569063000E-08 - 76, 8, 3.2313581266268849E-09, 1.2032812174656250E-08, 2.9390412070953001E-08, 2.9465568095594999E-08 - 76, 9,-1.4764992785650390E-08,-6.9117363195682176E-09, 2.9350734583190999E-08, 2.9329079692246000E-08 - 76, 10, 1.4672751109051880E-10,-6.2719168248767139E-09, 2.9337017057111000E-08, 2.9350291774507000E-08 - 76, 11,-2.7338324615655138E-09,-1.8424891800560789E-08, 2.9156837809429001E-08, 2.9165682081747999E-08 - 76, 12,-1.1149644082276730E-08,-1.9971241189445680E-08, 2.9240254395145000E-08, 2.9262056569741001E-08 - 76, 13, 1.3468297907092810E-08,-9.3353076777423194E-09, 2.8992702726839000E-08, 2.8999632626066999E-08 - 76, 14,-4.8838757883831708E-09,-2.4683766421580301E-08, 2.9099446433667000E-08, 2.9107252157761000E-08 - 76, 15,-9.5458664072040122E-09, 1.5820445586331520E-08, 2.8887345300986001E-08, 2.8862914782523002E-08 - 76, 16,-6.3690280879167703E-09, 2.6425281006155160E-08, 2.8976913791827000E-08, 2.8953180713045000E-08 - 76, 17, 1.4498520996867471E-09, 6.6349195754510533E-09, 2.8921089722593001E-08, 2.8872071641382001E-08 - 76, 18,-2.2379111291654961E-08,-1.9976689360796441E-09, 2.8804032299175000E-08, 2.8863516858389000E-08 - 76, 19, 1.8196016771931870E-09,-2.3222529412256052E-09, 2.8893780186233001E-08, 2.8888615974581999E-08 - 76, 20, 1.2541618121940850E-08, 7.8055666050250177E-09, 2.8704978574429000E-08, 2.8692124032576000E-08 - 76, 21, 2.2051692938196101E-08,-2.0353664992427840E-08, 2.8594445468194001E-08, 2.8624619789605001E-08 - 76, 22, 3.7925707272445038E-09,-1.1713769923490910E-09, 2.8555454642585001E-08, 2.8541364970902001E-08 - 76, 23, 9.9024830706730526E-09, 5.5749438117867420E-09, 2.8467643017073000E-08, 2.8473214981196999E-08 - 76, 24,-5.6185400353265432E-08,-3.4578222543005391E-09, 2.8416281688525000E-08, 2.8413750911178999E-08 - 76, 25, 1.3078069608496070E-08, 1.3853921065570429E-08, 2.8425571308711998E-08, 2.8420469881208000E-08 - 76, 26, 7.0858919667888109E-09,-6.8741098596236528E-09, 2.8331442608140999E-08, 2.8339202093717001E-08 - 76, 27, 6.6077856813822496E-09, 3.1872819814188651E-08, 2.8252137691644999E-08, 2.8277524778382000E-08 - 76, 28,-4.7934972727196860E-09,-2.1554736627160090E-08, 2.8188439215301000E-08, 2.8187154594209000E-08 - 76, 29,-6.9561640400408213E-09, 9.9739093367577158E-09, 2.8154662143283999E-08, 2.8152147527939000E-08 - 76, 30,-1.7298850399053620E-08,-1.0449126853525500E-08, 2.8064297799181999E-08, 2.8052796814519000E-08 - 76, 31, 2.0427945666550649E-08,-5.1559623423232746E-09, 2.8036941325918002E-08, 2.8057472109155001E-08 - 76, 32, 4.6188133452199043E-09, 2.7630723894285118E-09, 2.7937736519744999E-08, 2.7937671569765000E-08 - 76, 33, 1.5473241247945410E-08, 1.8377877833733592E-08, 2.7964452496715000E-08, 2.7932870329807999E-08 - 76, 34, 1.8419218673326231E-08, 2.4203589807593202E-08, 2.7828198594611001E-08, 2.7880115882923999E-08 - 76, 35,-1.1576977790954119E-08, 3.0562695163727928E-08, 2.7899227259010999E-08, 2.7804483780965000E-08 - 76, 36, 1.6842641027547720E-08,-5.2895763194769646E-09, 2.7875632669953999E-08, 2.7949014755679000E-08 - 76, 37, 4.9878334002134014E-09,-1.8508351080782401E-08, 2.7722342349578000E-08, 2.7802775432280001E-08 - 76, 38,-5.8796709181566803E-09,-1.3300991622500540E-08, 2.7925329498394000E-08, 2.7812256481462001E-08 - 76, 39,-2.7255118627242899E-08, 1.7495493331454369E-08, 2.7600031185089000E-08, 2.7621717752444000E-08 - 76, 40,-4.2764383478624343E-08, 1.7328158872592910E-08, 2.7630575172532000E-08, 2.7616694948181000E-08 - 76, 41,-7.5639421613343147E-09,-1.2853264487589751E-08, 2.7462086890615002E-08, 2.7514630589406999E-08 - 76, 42,-4.9785954983282444E-09,-2.0985136562176680E-08, 2.7514874271147000E-08, 2.7494643172457001E-08 - 76, 43, 3.1629980923831900E-09, 4.0956341521482748E-09, 2.7428053391917998E-08, 2.7413107625753001E-08 - 76, 44,-9.7600780864094518E-09, 1.2881658770767079E-08, 2.7408412548273000E-08, 2.7431158045340999E-08 - 76, 45, 1.4562974744161569E-08,-1.2923541118449271E-08, 2.7349188810848000E-08, 2.7348520343827000E-08 - 76, 46, 2.8031007045234328E-09,-4.9700332234777708E-11, 2.7321945105937002E-08, 2.7300615505810000E-08 - 76, 47, 1.0563844804791500E-08, 3.3954909654783391E-08, 2.7244519040886001E-08, 2.7241717254358001E-08 - 76, 48,-5.7270565241558473E-09,-3.5131714347238557E-08, 2.7185385446366999E-08, 2.7189775060950001E-08 - 76, 49, 9.9755836942707195E-09,-2.6604252510247409E-08, 2.7123944250694999E-08, 2.7103728423121000E-08 - 76, 50,-3.2393406975226197E-08, 3.8186460952231392E-08, 2.7146845130828998E-08, 2.7131370276116001E-08 - 76, 51, 8.2363042876565837E-10, 1.8654777173338611E-08, 2.6975910788483999E-08, 2.6979899101007999E-08 - 76, 52,-5.4549961151072283E-09,-4.2051928579463380E-08, 2.6945223394714001E-08, 2.6934631726367999E-08 - 76, 53, 4.3328811794342242E-10,-6.4473774956132926E-09, 2.6780326202199000E-08, 2.6786485643535000E-08 - 76, 54, 7.7791880523864997E-09, 5.5462469156639319E-09, 2.6714178823743999E-08, 2.6711177546643999E-08 - 76, 55,-6.8513337728391641E-09, 2.2994083159371320E-09, 2.6561786397354999E-08, 2.6542042079835000E-08 - 76, 56,-1.6368328545898921E-08,-1.1352469519772731E-08, 2.6474331949048999E-08, 2.6470710832151001E-08 - 76, 57, 1.9502980844022588E-08, 9.8518619349957040E-09, 2.6313065950274001E-08, 2.6321710175306001E-08 - 76, 58,-1.0428224595543189E-09, 2.2733436814127639E-09, 2.6193730194258999E-08, 2.6202553714786000E-08 - 76, 59, 3.3076171807908240E-09,-6.5903650132201812E-09, 2.5890180703211001E-08, 2.5892436416364999E-08 - 76, 60, 1.4122752076930229E-08,-5.9114365282621552E-09, 2.5805991862962001E-08, 2.5803283978064001E-08 - 76, 61,-3.4255838664772141E-09,-5.2510041396373119E-09, 2.5450255956757001E-08, 2.5490475831870000E-08 - 76, 62,-7.4038269195788133E-09,-1.0721046719932759E-09, 2.5206584818110999E-08, 2.5211939806423000E-08 - 76, 63, 5.6962998814675104E-09,-2.2062841195169371E-08, 2.5060593846447999E-08, 2.5086675304051000E-08 - 76, 64, 1.0337872067318880E-08,-1.9947530879960030E-08, 2.4488599691390001E-08, 2.4556140496827999E-08 - 76, 65, 2.4819528792992309E-09, 1.3017093633185060E-08, 2.4504494277821001E-08, 2.4468589136304999E-08 - 76, 66,-3.5220416438795301E-08,-4.2238387100520158E-08, 2.3955662593722001E-08, 2.3972642286038001E-08 - 76, 67,-3.0661429493481528E-08,-3.6404084136808712E-08, 2.3854461612106001E-08, 2.3776014991893999E-08 - 76, 68,-3.9833577415097362E-08, 3.1373686013475949E-08, 2.3096088315052001E-08, 2.3106193023874001E-08 - 76, 69, 2.2162033547726120E-08,-2.2308442943132020E-08, 2.2735238470133998E-08, 2.2725235230212999E-08 - 76, 70,-1.9414412729604591E-08,-1.8240199445376290E-08, 2.2132285443050000E-08, 2.2105760915454001E-08 - 76, 71, 8.6782042079213286E-09,-4.7104721658484278E-09, 2.0521651231786001E-08, 2.0530786170286000E-08 - 76, 72, 7.6652339064003030E-09,-2.8300589690111488E-09, 2.0168134609693001E-08, 2.0164165473630001E-08 - 76, 73,-9.1348519390794130E-08, 7.5511220796638508E-09, 1.7314298573442001E-08, 1.7272712442422999E-08 - 76, 74,-2.0155617172204422E-09, 1.2023358233764180E-08, 1.2164788317635000E-08, 1.2158339209383999E-08 - 76, 75, 1.4435192685666169E-08, 2.5476954128332320E-08, 6.7067335513985998E-09, 6.8163025056430999E-09 - 76, 76,-1.7399521075397470E-08,-8.3679785269692319E-08, 1.1761886531710000E-08, 1.1694608989496999E-08 - 77, 0,-3.3175417267974147E-08, 0.0000000000000000E+00, 2.9620193578158000E-08, 0.0000000000000000E+00 - 77, 1,-2.3866987117812741E-08, 1.1039511430776550E-08, 2.9176390576995000E-08, 2.9214201793273001E-08 - 77, 2,-5.6504285348803869E-09, 2.9688927761926800E-09, 2.8897087456398999E-08, 2.9162464418748001E-08 - 77, 3, 1.9502158111920871E-08, 2.6586603502872751E-08, 2.9137537019366999E-08, 2.9120515790572002E-08 - 77, 4,-6.7737358711472307E-10,-7.6647813847472818E-09, 2.8880553023988002E-08, 2.8936273394312001E-08 - 77, 5, 2.4440475506094458E-08,-2.1718342682824999E-08, 2.9035156898091999E-08, 2.8954511880831001E-08 - 77, 6, 9.6718891732388609E-09,-1.0553399144575810E-09, 2.8808003428403000E-08, 2.8859336307799999E-08 - 77, 7,-2.5042239603513932E-10, 4.1402396035280771E-09, 2.8871645263911000E-08, 2.8823722284535001E-08 - 77, 8, 1.0019704173778510E-08,-1.8574257904051629E-08, 2.8720329085235001E-08, 2.8830531266207002E-08 - 77, 9,-5.3437911644649539E-09,-2.0954378216032671E-08, 2.8705729995842000E-08, 2.8673697008764000E-08 - 77, 10, 5.6073647359257583E-09, 4.5312772316525807E-09, 2.8680599497888000E-08, 2.8704353685767999E-08 - 77, 11,-1.5431932759667848E-08, 9.6923452772768300E-09, 2.8470596450096001E-08, 2.8481195029829001E-08 - 77, 12, 6.2253866165856333E-09, 1.3749722484469730E-08, 2.8577320128683000E-08, 2.8602570403414999E-08 - 77, 13, 1.5801847470957461E-08, 4.0176406856820637E-08, 2.8262707626823001E-08, 2.8295619490897000E-08 - 77, 14,-7.2133863810242424E-09,-1.2001667211665551E-10, 2.8434044854929001E-08, 2.8442703747652001E-08 - 77, 15, 2.8633786159681789E-08,-5.6078084156026958E-09, 2.8146766806993000E-08, 2.8113241410807001E-08 - 77, 16, 9.2223040601533342E-09,-2.1901115013133540E-09, 2.8304986908043000E-08, 2.8280327421007000E-08 - 77, 17,-6.5702753710968109E-09,-3.7504291049909100E-09, 2.8198810975666999E-08, 2.8137898181160999E-08 - 77, 18,-3.3936853691972102E-08,-3.9321007996800506E-09, 2.8108124016739001E-08, 2.8202520781963000E-08 - 77, 19, 9.7217939237696369E-09, 1.5459851261354921E-08, 2.8217637321957999E-08, 2.8176218141439001E-08 - 77, 20,-6.7837325866714813E-09, 1.4812011165582341E-08, 2.8023898566457000E-08, 2.8002438039974002E-08 - 77, 21, 4.4190917660587362E-08, 5.1673639214872533E-09, 2.7836016932860000E-08, 2.7887501919490999E-08 - 77, 22, 3.8384270534673912E-08,-2.4648567299249159E-08, 2.7863000144345002E-08, 2.7841865528972002E-08 - 77, 23,-1.5172212218229539E-08, 7.9174985239677096E-09, 2.7726570202112999E-08, 2.7737918935979001E-08 - 77, 24,-1.5091349711111090E-08, 5.6789638076045476E-09, 2.7712219590799000E-08, 2.7704840980214999E-08 - 77, 25, 1.2483172531881880E-08, 1.7627913133629489E-08, 2.7787358406349999E-08, 2.7839527406585001E-08 - 77, 26, 1.2661709780083051E-08, 4.9849294596781847E-09, 2.7551691054276000E-08, 2.7553539702346000E-08 - 77, 27, 2.5926873419532669E-09, 1.1128680458941290E-08, 2.7524806383454999E-08, 2.7565219159127000E-08 - 77, 28, 9.9625249234237912E-10,-2.7998652220863599E-08, 2.7393450260574000E-08, 2.7390253674555000E-08 - 77, 29,-3.3245345226563961E-08, 3.0347494878684523E-08, 2.7397667612583999E-08, 2.7405050070292001E-08 - 77, 30,-2.3264179601590362E-09, 1.1124559242777140E-08, 2.7263580356506999E-08, 2.7247056829359000E-08 - 77, 31, 9.9654950230004841E-09, 2.8374134453619601E-09, 2.7242173888758001E-08, 2.7263474767456001E-08 - 77, 32,-3.0620307644790332E-08, 4.6602642351817102E-09, 2.7119628045595001E-08, 2.7116614509471001E-08 - 77, 33,-1.7102091694009751E-09, 1.3189180633136450E-08, 2.7121711366707999E-08, 2.7090635162792000E-08 - 77, 34,-3.7013703900785947E-08, 1.3897274299832561E-09, 2.6984183616044999E-08, 2.7054887241173001E-08 - 77, 35,-4.0009848430695690E-08, 1.3288754608315410E-08, 2.7020591214252001E-08, 2.6911644364921999E-08 - 77, 36,-4.6233234567264471E-10,-7.0350701349119657E-09, 2.7035732501414000E-08, 2.7141613956525999E-08 - 77, 37, 1.8054556974615259E-08, 2.2171706132734130E-08, 2.6819830655878999E-08, 2.6947333096885002E-08 - 77, 38,-3.0210532855865199E-08,-7.4505211127095023E-09, 2.7092578081327998E-08, 2.6969316682231001E-08 - 77, 39, 8.9682069096164098E-10,-3.7757932970086457E-08, 2.6793410433097001E-08, 2.6816834724755001E-08 - 77, 40, 9.6648602092708163E-09, 6.6247824680071693E-09, 2.6752720649952999E-08, 2.6772238605457999E-08 - 77, 41, 2.1735393822840308E-08,-5.0720001114834133E-09, 2.6668289671138000E-08, 2.6739491804974998E-08 - 77, 42, 1.3767800851798921E-09, 5.5773107379982709E-09, 2.6671749509795000E-08, 2.6650434126438001E-08 - 77, 43, 1.1148562980873960E-08,-1.2986159400614080E-08, 2.6666317711823001E-08, 2.6663164277087001E-08 - 77, 44, 2.9619569317739052E-10, 4.1047726241601984E-09, 2.6634461988921001E-08, 2.6715675630275001E-08 - 77, 45,-3.9331790474298220E-09,-6.8735071970498514E-09, 2.6642092273277999E-08, 2.6627512976715000E-08 - 77, 46, 2.7172208592959601E-09, 1.0700062690193120E-08, 2.6718362353081999E-08, 2.6677832841400001E-08 - 77, 47, 4.6420626102513601E-08, 1.8709103669224872E-08, 2.6590950428269000E-08, 2.6599986862024999E-08 - 77, 48, 1.3111536260332990E-08,-3.4281223981577357E-08, 2.6666260169894000E-08, 2.6652518875213001E-08 - 77, 49, 7.5377140284312358E-10, 6.6545419309829426E-09, 2.6634023304814001E-08, 2.6604875192968001E-08 - 77, 50,-1.0523138935234350E-09,-2.1221151812756989E-08, 2.6542190632284001E-08, 2.6534647336292001E-08 - 77, 51, 2.7920949126953781E-08, 5.5253300949398177E-09, 2.6669459757218999E-08, 2.6650996492051002E-08 - 77, 52, 2.1865979719953751E-08,-2.0618971497344029E-08, 2.6301373584400001E-08, 2.6284241823912001E-08 - 77, 53,-2.7853088968286620E-08, 2.2145484523373081E-08, 2.6457884780373000E-08, 2.6463802889769000E-08 - 77, 54,-1.3557659475893080E-08, 1.7374493724990901E-08, 2.6150955767417001E-08, 2.6157374508517001E-08 - 77, 55, 1.9470344318422431E-08, 1.6447231786795762E-08, 2.6197948015183000E-08, 2.6158353981445001E-08 - 77, 56, 1.0273807462544781E-09,-2.4024139629688431E-08, 2.6008307265337000E-08, 2.6008539114835999E-08 - 77, 57,-1.8540320002195619E-09, 1.2018832230790190E-08, 2.5939503635904999E-08, 2.5962777208196001E-08 - 77, 58,-1.8772060070030891E-08, 2.0039726655070001E-08, 2.5736040141591999E-08, 2.5744218541357999E-08 - 77, 59, 4.0663977766531342E-08,-4.1600296163430312E-08, 2.5530352639672999E-08, 2.5523354083617001E-08 - 77, 60, 3.9306813456847252E-09,-1.9969167926295411E-08, 2.5344856082812998E-08, 2.5345901313289001E-08 - 77, 61,-4.1622337109332867E-08,-5.5046481987865780E-09, 2.5099685030480999E-08, 2.5141250982497001E-08 - 77, 62,-1.2448489605019870E-08,-6.9067892780718631E-09, 2.4863249162087000E-08, 2.4868988503687999E-08 - 77, 63, 4.5852839944717212E-08,-1.1252617509579430E-08, 2.4821484608242001E-08, 2.4895166263469002E-08 - 77, 64, 5.3578197763863217E-08,-3.7389764862516912E-08, 2.4412043337677001E-08, 2.4550698096371000E-08 - 77, 65, 1.7895694066166530E-08,-9.1403813720807074E-09, 2.4212040694448999E-08, 2.4294290667630001E-08 - 77, 66,-3.0732396298079967E-08,-1.5497918181919450E-09, 2.4217499790107001E-08, 2.4205311012691998E-08 - 77, 67, 4.2652856640314052E-08,-4.3052342995357923E-08, 2.3749531270038002E-08, 2.3718161340608001E-08 - 77, 68,-1.1407124511349361E-08, 2.0760845737049769E-08, 2.3713328790076001E-08, 2.3751707719240000E-08 - 77, 69,-2.1048696726471920E-08,-2.8766692588555371E-08, 2.2925636601356000E-08, 2.2932551932868001E-08 - 77, 70,-2.0901254918761260E-08, 1.3632007112972620E-08, 2.3063686957014001E-08, 2.3038320360697000E-08 - 77, 71, 4.9097185798932417E-08,-4.1448751395953247E-08, 2.1969475356902999E-08, 2.1978833865975000E-08 - 77, 72, 1.0406061832125910E-08,-2.6883803117298302E-08, 2.1007701984061000E-08, 2.0990076335877001E-08 - 77, 73,-9.3703314047004525E-09,-2.3876118906691889E-08, 2.0450987158327000E-08, 2.0450312727740001E-08 - 77, 74, 4.2478267694708333E-08, 5.1333310765161993E-09, 1.6962893498422999E-08, 1.6969142755027001E-08 - 77, 75, 4.2763313137673183E-08,-2.8093520976808132E-08, 9.4171491707272995E-09, 9.4311062016354992E-09 - 77, 76, 3.1027345584730982E-08,-4.5329929440797703E-08, 3.2977544914492999E-09, 3.0714463924657001E-09 - 77, 77, 4.1241019263909587E-08,-1.8342859868567531E-08, 9.6701433469026008E-09, 9.6752409383881003E-09 - 78, 0,-1.3651871576431630E-08, 0.0000000000000000E+00, 2.9022725966607999E-08, 0.0000000000000000E+00 - 78, 1, 2.0007574890190509E-08,-1.1917359750880859E-08, 2.8650773011152999E-08, 2.8692933121429002E-08 - 78, 2, 5.5015226881157541E-09,-1.3307175585156750E-08, 2.8375669371087999E-08, 2.8654852115496999E-08 - 78, 3,-1.5846114998914331E-08,-3.4654647377657671E-08, 2.8604263887904001E-08, 2.8574996280185001E-08 - 78, 4,-2.3430980031731121E-09,-3.0856491033797910E-09, 2.8366786884037001E-08, 2.8434317864359999E-08 - 78, 5,-2.5664356989469272E-08, 1.1227343688026411E-08, 2.8505205054163001E-08, 2.8415304140668999E-08 - 78, 6,-4.2329063954188410E-09, 6.1357015388518117E-09, 2.8292349507894999E-08, 2.8355855306800001E-08 - 78, 7, 3.5175015623792748E-09,-2.2474940557799511E-09, 2.8361007175515001E-08, 2.8296902789315001E-08 - 78, 8, 3.3632024253883828E-10, 1.0006639045488590E-08, 2.8217022016065999E-08, 2.8335591968731999E-08 - 78, 9,-1.4031289397781801E-08,-5.5193672283405140E-09, 2.8212179614964001E-08, 2.8178404344152000E-08 - 78, 10, 4.6561493893151328E-09,-3.5010840421413310E-09, 2.8200131239640000E-08, 2.8223225424240001E-08 - 78, 11, 1.1124683888028340E-09,-2.3469940179413101E-08, 2.7991536966383001E-08, 2.8006762262960000E-08 - 78, 12,-1.4868007306046500E-08,-1.4929135650882432E-08, 2.8117503222743001E-08, 2.8145023889503000E-08 - 78, 13, 1.6474788686008270E-08,-1.4784459967321050E-08, 2.7799112012734001E-08, 2.7810522528038999E-08 - 78, 14,-8.4387914942751674E-09,-2.2806227401399440E-08, 2.7976763396315999E-08, 2.7984629023416000E-08 - 78, 15,-1.9225095190515649E-08, 1.8802105113413839E-08, 2.7702842030478002E-08, 2.7676972362551001E-08 - 78, 16, 1.7127843935318650E-09, 3.2626403374281277E-08, 2.7845636813494998E-08, 2.7828023715371000E-08 - 78, 17,-7.5622432795513458E-09, 1.1509899004014700E-09, 2.7770337032011001E-08, 2.7713000044629999E-08 - 78, 18,-1.9684626514578369E-08,-5.5569923673689877E-09, 2.7659378061241999E-08, 2.7738312861105000E-08 - 78, 19, 8.7851044052141443E-10, 4.4328039045677106E-09, 2.7769446102113000E-08, 2.7759401781888999E-08 - 78, 20, 1.4960110205560601E-08, 2.8467961695772671E-09, 2.7571636843659000E-08, 2.7537996665482000E-08 - 78, 21, 2.2260973857672711E-08,-1.0197133124881559E-08, 2.7420034535393000E-08, 2.7465324812645999E-08 - 78, 22, 1.6918113270808030E-08, 9.2180801400990337E-09, 2.7398984059580001E-08, 2.7389075629542000E-08 - 78, 23, 1.8363442573372010E-08, 5.9259472384364023E-09, 2.7302305808382000E-08, 2.7299076302856001E-08 - 78, 24,-6.5168973335790371E-08,-5.6186570044578471E-09, 2.7254924458824999E-08, 2.7249172486569001E-08 - 78, 25,-8.0020149689858571E-10, 1.7566254567495910E-08, 2.7295617661643000E-08, 2.7291038905248001E-08 - 78, 26,-8.2456842487408573E-10,-1.7370921843393679E-08, 2.7182930720313999E-08, 2.7183913504590999E-08 - 78, 27, 1.3054074184234781E-08, 2.5832528357660160E-08, 2.7097578374793001E-08, 2.7132947110281002E-08 - 78, 28,-2.1954569479845840E-09,-1.7595252584785489E-08, 2.7018945840947000E-08, 2.7009506264643000E-08 - 78, 29,-7.7154740621029854E-09, 1.7392123093596870E-08, 2.7002930004162999E-08, 2.7004558784840000E-08 - 78, 30,-1.3427583783324160E-08,-1.6218206792996189E-08, 2.6891741011854001E-08, 2.6870120249029000E-08 - 78, 31, 1.4463264750810081E-08,-3.7505707209113497E-09, 2.6879151750868000E-08, 2.6904251218031999E-08 - 78, 32,-6.1223042481017193E-09, 9.9449932172918519E-10, 2.6742388387190999E-08, 2.6746287460614001E-08 - 78, 33, 2.0489562195719131E-08, 6.7566468630021109E-09, 2.6807911128987000E-08, 2.6782331857702999E-08 - 78, 34, 2.7571395955110069E-08, 1.9055959971506420E-08, 2.6630852073035000E-08, 2.6702834815196001E-08 - 78, 35,-1.3564025060229139E-08, 2.2978240346277951E-08, 2.6767236534122999E-08, 2.6662547973572999E-08 - 78, 36, 2.9617739040252459E-08,-1.0265219513765370E-09, 2.6720320922733999E-08, 2.6831082668030001E-08 - 78, 37, 1.4328040037043590E-08,-1.1875507753171301E-08, 2.6606710992597999E-08, 2.6740312311114001E-08 - 78, 38, 4.5525139866867448E-09,-9.0210705706669276E-09, 2.6785268182268001E-08, 2.6653404632869001E-08 - 78, 39,-4.4208460736287958E-08, 3.2766453071359660E-08, 2.6475817289000001E-08, 2.6494106890838000E-08 - 78, 40,-5.4775320369152988E-08, 1.9907894410641531E-08, 2.6488068720746000E-08, 2.6491453703792001E-08 - 78, 41,-1.9457995138341308E-08,-3.6877927764556069E-09, 2.6309439970051002E-08, 2.6358210127909000E-08 - 78, 42,-1.9122347312174541E-08,-2.1770803826649540E-08, 2.6408151436312001E-08, 2.6378795380037000E-08 - 78, 43,-3.8034186892750508E-10, 1.6092632991879600E-08, 2.6276071671249999E-08, 2.6286027155563001E-08 - 78, 44, 1.6272779595474851E-08, 2.2786917071128530E-08, 2.6334279548309999E-08, 2.6399877272483000E-08 - 78, 45, 1.7758047251864851E-08,-1.9161221480271919E-08, 2.6277064669265999E-08, 2.6256127706455999E-08 - 78, 46,-8.6432470414735680E-09,-6.3100591625703029E-09, 2.6304824810059999E-08, 2.6277632396778999E-08 - 78, 47, 1.1972538323825741E-08, 4.0590540684757391E-08, 2.6217035242341000E-08, 2.6231422792506999E-08 - 78, 48,-7.2490151276142393E-09,-5.2431032020474942E-08, 2.6240072695935999E-08, 2.6219045747103000E-08 - 78, 49, 9.8093091502996847E-09,-4.3141485986115851E-08, 2.6182608167615000E-08, 2.6140723190875999E-08 - 78, 50,-1.4989609756122451E-08, 5.1926730280767347E-08, 2.6271838034858000E-08, 2.6269793548220998E-08 - 78, 51,-7.9977581382417977E-09, 4.2362178988934302E-08, 2.6146242119058001E-08, 2.6129696649207002E-08 - 78, 52,-3.9298114215323109E-09,-5.7885360953354813E-08, 2.6187464138127000E-08, 2.6175050760403001E-08 - 78, 53, 1.2580059385347289E-08, 6.2664858835279148E-09, 2.5998582355149998E-08, 2.5986094815756000E-08 - 78, 54, 6.8087934100149373E-09, 2.5189629199644882E-08, 2.6035891685100001E-08, 2.6045882430394999E-08 - 78, 55,-2.1999688002496730E-08, 3.0189019107963272E-08, 2.5907400192572001E-08, 2.5875076080633999E-08 - 78, 56,-2.4758465288994892E-08, 8.1324017208831476E-10, 2.5880377881324999E-08, 2.5872704469655001E-08 - 78, 57, 1.4321935089203969E-08, 2.1154986103067481E-08, 2.5800892100797999E-08, 2.5800732794415000E-08 - 78, 58,-1.5044642459842980E-09, 4.1961252310966311E-09, 2.5713873840670999E-08, 2.5736153508446001E-08 - 78, 59, 2.7366093240144619E-08, 1.3547739175822880E-08, 2.5440983614354000E-08, 2.5414914296013000E-08 - 78, 60, 2.9279456595598089E-08,-2.6905744844189551E-08, 2.5503863192235001E-08, 2.5516499087904998E-08 - 78, 61, 1.1466439837640130E-08,-1.4141888252610170E-08, 2.4991111100146999E-08, 2.5029117600117999E-08 - 78, 62,-1.9796134744631280E-08, 2.1843938416468611E-10, 2.5107105989984999E-08, 2.5147989395649001E-08 - 78, 63,-8.7975778397654290E-09, 4.9167262526171497E-09, 2.4712965385141001E-08, 2.4736303794077001E-08 - 78, 64, 2.3532511325432969E-09, 2.9411359430649239E-09, 2.4571651309509999E-08, 2.4527535533070001E-08 - 78, 65,-3.5698843207339763E-08, 1.9204131108677311E-08, 2.4419318339087999E-08, 2.4494257754065999E-08 - 78, 66, 1.7817849411510959E-08,-3.9074902985096453E-09, 2.4252341141734000E-08, 2.4203502614197999E-08 - 78, 67, 5.2506177763955721E-10,-1.7844752125445780E-08, 2.4154972851436001E-08, 2.4125126292405001E-08 - 78, 68,-2.2841657154720451E-08, 1.9268193534068030E-08, 2.3652196465328000E-08, 2.3688446774712000E-08 - 78, 69,-2.3859852421256960E-09,-2.6496448788204721E-09, 2.3687380076543999E-08, 2.3700008761092001E-08 - 78, 70,-1.1958960176482331E-08,-2.8552579453117799E-08, 2.2954447894777000E-08, 2.2927978665686001E-08 - 78, 71, 3.8418648990361743E-08, 4.3855516171037893E-08, 2.2852725861751000E-08, 2.2832843990616999E-08 - 78, 72, 4.3838338903220726E-09,-6.1185921710597652E-09, 2.2033994231110000E-08, 2.2043202436203001E-08 - 78, 73,-8.0089805519322734E-08, 2.0196353740244259E-08, 2.0662450350928001E-08, 2.0664842994675001E-08 - 78, 74, 3.9981913982536377E-08,-1.8126404443645410E-08, 2.0540893705184999E-08, 2.0503913158330001E-08 - 78, 75,-4.4786922638042567E-08, 6.0581028295660345E-08, 1.7101239829210000E-08, 1.7104287001046001E-08 - 78, 76,-3.2203280490699882E-08,-1.2866571278288149E-07, 1.1740315174219000E-08, 1.1586663215419001E-08 - 78, 77,-3.4304873140560701E-08, 1.5206917869573420E-10, 3.4608847496430000E-09, 3.5072885577901998E-09 - 78, 78, 3.1916464057956610E-08, 1.7407733855812471E-08, 1.0603238277694999E-08, 1.0612151701529000E-08 - 79, 0,-2.6580030737536651E-08, 0.0000000000000000E+00, 2.8406033670986000E-08, 0.0000000000000000E+00 - 79, 1,-2.1141447252752409E-08, 1.0080122064768180E-08, 2.8038956558164000E-08, 2.8074335482151001E-08 - 79, 2,-1.3778279671509890E-09, 2.0941879284064319E-09, 2.7690567042505001E-08, 2.8045204385897000E-08 - 79, 3, 1.5752887151062088E-08, 2.7808062591184521E-08, 2.8005627672105000E-08, 2.7952126415059000E-08 - 79, 4,-2.5989916649176110E-09,-1.0014623816836661E-08, 2.7694253516184999E-08, 2.7798980364870999E-08 - 79, 5, 2.3849844931274459E-08,-2.2324311550613210E-08, 2.7898860549656000E-08, 2.7773880629867000E-08 - 79, 6, 1.2693918750351830E-08, 5.8662141671555628E-09, 2.7610978710242001E-08, 2.7706405947611999E-08 - 79, 7, 7.2622025907125383E-10, 6.1289679035864528E-09, 2.7740791201895001E-08, 2.7645389877345999E-08 - 79, 8, 6.2083459977640278E-09,-1.7592246180823199E-08, 2.7538248195141000E-08, 2.7697720471981999E-08 - 79, 9,-7.3729197241414301E-09,-1.3561340768086210E-08, 2.7570194820370001E-08, 2.7520042088585999E-08 - 79, 10, 3.3595386898662121E-09, 1.8483558688248771E-09, 2.7539197431145000E-08, 2.7573801554545001E-08 - 79, 11,-1.0074489446158050E-08, 8.1008957312218816E-09, 2.7307841248052002E-08, 2.7323126010209000E-08 - 79, 12, 8.1007918180071237E-09, 1.4077498800945890E-08, 2.7453343148622999E-08, 2.7483648208023000E-08 - 79, 13, 1.8069577841352691E-08, 5.1489202272910697E-08, 2.7065290294430999E-08, 2.7108884172578001E-08 - 79, 14, 8.1681676241915424E-10, 3.3668686509473910E-09, 2.7301723132253001E-08, 2.7310255626198001E-08 - 79, 15, 3.2017184378222612E-08,-1.1526148860022730E-08, 2.6948164728272999E-08, 2.6913386545771999E-08 - 79, 16, 3.7791328496748159E-09, 1.7097575134615780E-09, 2.7158646039274001E-08, 2.7143137093342999E-08 - 79, 17,-2.2704745380625439E-09,-2.9707056947370810E-09, 2.7027177309864999E-08, 2.6957176212445999E-08 - 79, 18,-4.1347583258882343E-08,-2.5641666062873002E-09, 2.6944842960034001E-08, 2.7059125015556999E-08 - 79, 19, 1.1113670901972480E-08, 8.3861788625615397E-09, 2.7069165784810002E-08, 2.7015936534416001E-08 - 79, 20, 2.0909369680074049E-10, 1.6351909855960989E-08, 2.6868165644142000E-08, 2.6824578831494000E-08 - 79, 21, 4.0304227516878183E-08, 4.0713203039800800E-10, 2.6611386264838000E-08, 2.6682011608180999E-08 - 79, 22, 2.4926830354468559E-08,-3.1187816409330242E-08, 2.6681984838412999E-08, 2.6663814106980999E-08 - 79, 23,-2.0960135677567920E-08, 1.0110876428822050E-08, 2.6497616664130000E-08, 2.6503109331887001E-08 - 79, 24,-2.5687447039431621E-08, 5.4501015736764983E-09, 2.6525681211605000E-08, 2.6514722640449999E-08 - 79, 25, 1.3896953755161469E-08, 1.2662760846587479E-08, 2.6597766123193000E-08, 2.6672381504904999E-08 - 79, 26, 1.0374944404687450E-08, 1.9040072461729770E-08, 2.6348505591400999E-08, 2.6340584320789999E-08 - 79, 27, 6.4902426225915351E-09, 1.0446895561494700E-08, 2.6285353236776000E-08, 2.6342753690812001E-08 - 79, 28,-3.6019150179790861E-09,-2.7443632143430341E-08, 2.6144450094775999E-08, 2.6135247243162001E-08 - 79, 29,-3.8140445458125997E-08, 2.9251802728910129E-08, 2.6145935360195001E-08, 2.6161718395696000E-08 - 79, 30, 8.2678515949939995E-10, 2.9762342108525880E-09, 2.5988044466050000E-08, 2.5958825985356000E-08 - 79, 31, 1.4533424578181500E-08, 4.9407812622841399E-09, 2.5967999749512000E-08, 2.5989766198848000E-08 - 79, 32,-3.3850440008485593E-08, 1.5470106395207130E-08, 2.5798441442357000E-08, 2.5795594954953999E-08 - 79, 33, 5.1482492244217673E-09, 1.7852610682086740E-08, 2.5801743058525000E-08, 2.5773196305063000E-08 - 79, 34,-5.0686057899307728E-08, 3.7483570955424440E-09, 2.5628624204639001E-08, 2.5713655815303001E-08 - 79, 35,-3.3805971674011822E-08, 6.6812094486717069E-09, 2.5657745596432002E-08, 2.5547252433485001E-08 - 79, 36,-1.0435712971752500E-08,-4.2322757653386844E-09, 2.5697343667523000E-08, 2.5840542728893999E-08 - 79, 37, 1.7427425191684930E-08, 1.7389995649766131E-08, 2.5416991345587999E-08, 2.5610403347492999E-08 - 79, 38,-1.9044806554282330E-08,-1.2842381759673790E-09, 2.5778214508578999E-08, 2.5639146831217002E-08 - 79, 39, 8.0549666023742409E-09,-5.3764757974955427E-08, 2.5384889358506000E-08, 2.5397895518933001E-08 - 79, 40, 2.0270332155353359E-08, 1.1271045831613349E-08, 2.5336416758496001E-08, 2.5399561725193000E-08 - 79, 41, 3.7857369162168423E-08,-8.8911425205243866E-09, 2.5235611477268000E-08, 2.5288543715860001E-08 - 79, 42, 1.2389077098640749E-08, 1.6523461188663089E-08, 2.5250439262593001E-08, 2.5228422532365000E-08 - 79, 43, 8.1925126641079528E-09,-3.6181668092325492E-08, 2.5193015143690002E-08, 2.5238994207966001E-08 - 79, 44,-4.0324254736891724E-09, 8.4480013773497379E-09, 2.5215084032786999E-08, 2.5342812682218001E-08 - 79, 45,-3.5876386976691451E-09,-9.6768720508613127E-09, 2.5250716618122999E-08, 2.5200294101511001E-08 - 79, 46, 5.3668257793554629E-09, 4.4538413182462928E-09, 2.5391350216885001E-08, 2.5344426076545000E-08 - 79, 47, 6.7852277040644681E-08, 6.9335781903548050E-09, 2.5172586106689001E-08, 2.5214882580532000E-08 - 79, 48, 1.5956480925978358E-08,-2.7957869552652088E-08, 2.5469307736683000E-08, 2.5411998392650000E-08 - 79, 49,-6.4981800647078387E-09, 2.4928400471582379E-08, 2.5253529642021001E-08, 2.5205430869683000E-08 - 79, 50,-1.1486294039335800E-08,-2.8299550866745701E-08, 2.5446267710243000E-08, 2.5441052560100000E-08 - 79, 51, 1.5985331395678501E-08,-2.3242000248460959E-08, 2.5432180905823999E-08, 2.5369499852719001E-08 - 79, 52, 2.4518970713275470E-08,-3.1043028031664022E-08, 2.5103497486590001E-08, 2.5071275194269000E-08 - 79, 53,-3.9248704786471621E-08, 3.4093821139579131E-08, 2.5370305965605999E-08, 2.5344228906529999E-08 - 79, 54, 5.0033326799261269E-09, 4.2821373026314309E-09, 2.4913997768510002E-08, 2.4932822390549000E-08 - 79, 55, 9.3098384361993808E-09,-2.1436417595878769E-10, 2.5193469068756999E-08, 2.5129465621074000E-08 - 79, 56,-1.7909341351874820E-09,-3.9828410101996712E-08, 2.4819396219342999E-08, 2.4822346468001000E-08 - 79, 57, 1.5628256617125029E-08, 2.5934674432528089E-08, 2.4897227971736001E-08, 2.4932612225959999E-08 - 79, 58,-1.2456951740396420E-08, 2.0287981019799670E-08, 2.4609326924388000E-08, 2.4665653928733999E-08 - 79, 59, 5.0259401777492157E-08,-6.6100280141439520E-08, 2.4416476214743000E-08, 2.4367304540979998E-08 - 79, 60, 6.0923734035883318E-09,-1.2660929361926320E-08, 2.4213544302976999E-08, 2.4233794305003999E-08 - 79, 61,-4.5160894518078632E-08, 1.9672451463032159E-08, 2.3948442609383999E-08, 2.3968781912805999E-08 - 79, 62, 2.4321133263496842E-08,-1.2325140064759700E-08, 2.3530197667136999E-08, 2.3584724123424000E-08 - 79, 63, 6.0517473672512681E-08,-3.1732355109649897E-08, 2.3854644455448001E-08, 2.3936181934036001E-08 - 79, 64, 4.2562104356480792E-08,-5.3090015026228662E-08, 2.3112945127134999E-08, 2.3155291348904999E-08 - 79, 65, 3.8771705558373337E-08,-2.6500876877895741E-08, 2.3135301526009000E-08, 2.3392141334152999E-08 - 79, 66,-6.0744235543600940E-08,-2.6248392167649691E-08, 2.3068607687516000E-08, 2.2904489429946001E-08 - 79, 67,-2.6048881406844131E-08,-5.2226362665936691E-08, 2.2814841884041000E-08, 2.2914609399711001E-08 - 79, 68, 1.0890522127926011E-08, 2.4664285766552631E-08, 2.2451494135044000E-08, 2.2513812431631002E-08 - 79, 69,-2.1936907986687819E-08,-5.9110052787580798E-08, 2.2300693863597001E-08, 2.2372484011166001E-08 - 79, 70, 7.4666285039832773E-09, 9.2233343248650326E-09, 2.2268459269770000E-08, 2.2285827794734999E-08 - 79, 71, 4.1903761185084702E-08,-3.8798729843688747E-08, 2.1173757575620999E-08, 2.1128708171571999E-08 - 79, 72,-1.0966433703587360E-08,-3.7281832449681112E-08, 2.1956656229110999E-08, 2.1890014738789999E-08 - 79, 73,-2.3252182118600310E-08,-1.4921015165695480E-08, 1.9270072124999001E-08, 1.9256931746078001E-08 - 79, 74, 4.5947638667768872E-08, 2.4350363400316529E-08, 1.8794815287880000E-08, 1.8831895642319999E-08 - 79, 75,-3.3686739590678911E-09, 4.5706895926528217E-08, 1.8080066997501999E-08, 1.8153565688750000E-08 - 79, 76,-5.5910246120074843E-08, 9.3301033479905452E-08, 1.1514528615365000E-08, 1.1227534488311999E-08 - 79, 77, 6.1828489376380253E-09,-2.1854205513144799E-08, 1.0985811985810001E-08, 1.1089028933726000E-08 - 79, 78,-1.8979255881771220E-08, 4.7261929386014762E-08, 5.2218607737287998E-09, 5.2306901425724999E-09 - 79, 79, 2.5286185979705590E-09, 1.1264824975435439E-07, 8.8019736257302006E-09, 8.7229168559538994E-09 - 80, 0,-5.5581652244845550E-09, 0.0000000000000000E+00, 2.7809496063867998E-08, 0.0000000000000000E+00 - 80, 1, 1.6105404539531880E-08,-9.4305524346008953E-09, 2.7519346076780001E-08, 2.7555955196302001E-08 - 80, 2, 6.6996778257555003E-09,-1.2000398675073469E-08, 2.7183681699991999E-08, 2.7537900349047001E-08 - 80, 3,-1.4913771135925640E-08,-3.3984412455782507E-08, 2.7479546367451002E-08, 2.7411407030261000E-08 - 80, 4,-1.8616359355659361E-09, 2.2723162227291298E-09, 2.7194259162854001E-08, 2.7307608266155000E-08 - 80, 5,-2.1787117213659941E-08, 1.4311698457650620E-08, 2.7378715730623001E-08, 2.7246596828772001E-08 - 80, 6,-5.0324291407620092E-09, 2.0349773484661131E-10, 2.7109758485564001E-08, 2.7215513479652001E-08 - 80, 7, 5.4258286154019111E-09,-7.1629094284259131E-09, 2.7240288050471001E-08, 2.7128537628625001E-08 - 80, 8,-3.2432500567627532E-09, 5.5941061898149527E-09, 2.7049710146881000E-08, 2.7214908246617001E-08 - 80, 9,-1.3575229751083810E-08,-6.7346379720056380E-09, 2.7096295419965001E-08, 2.7040936621722001E-08 - 80, 10, 6.0338767165288471E-09,-5.5257907517891201E-09, 2.7075113004724001E-08, 2.7107923349240001E-08 - 80, 11,-8.0050257870553730E-10,-2.1938171865205221E-08, 2.6856104922334999E-08, 2.6874854898280999E-08 - 80, 12,-1.1320904524980449E-08,-1.6199449390020519E-08, 2.7009458341087000E-08, 2.7039347909023999E-08 - 80, 13, 1.7856216845155880E-08,-2.4186366001015391E-08, 2.6638795911337001E-08, 2.6656841757622000E-08 - 80, 14,-1.3772590643255320E-08,-2.4486738625583279E-08, 2.6870985988385999E-08, 2.6879132960532002E-08 - 80, 15,-2.5127653175995361E-08, 2.4953602836504131E-08, 2.6547151541928002E-08, 2.6525689347758001E-08 - 80, 16, 7.1008597667007362E-09, 3.4237594353959652E-08, 2.6730563215834999E-08, 2.6722210946258999E-08 - 80, 17,-1.3559531624407820E-08, 1.1962331638381260E-09, 2.6637731031683000E-08, 2.6574938222193999E-08 - 80, 18,-1.3313068257378230E-08,-1.3933135231347740E-08, 2.6535367421886002E-08, 2.6624524118733001E-08 - 80, 19,-6.5665171820662933E-10, 5.9986956470296269E-09, 2.6646175640487998E-08, 2.6635993231359002E-08 - 80, 20, 5.3256747647972054E-09,-1.1645879334670830E-09, 2.6450682188929998E-08, 2.6401796120763001E-08 - 80, 21, 1.7859384685587529E-08,-3.4484342445663418E-10, 2.6259060994827001E-08, 2.6317371629006001E-08 - 80, 22, 2.8517546646613449E-08, 1.4926530250858179E-08, 2.6255313313102000E-08, 2.6250803366181000E-08 - 80, 23, 2.1786189155315910E-08, 6.3492490207413779E-09, 2.6140837202441999E-08, 2.6131784602296001E-08 - 80, 24,-7.3642518997790722E-08,-2.5008483921826949E-09, 2.6107218025356001E-08, 2.6097389639324001E-08 - 80, 25,-4.1976675398835593E-09, 2.5472837659481589E-08, 2.6144164981169000E-08, 2.6144751663473999E-08 - 80, 26,-2.4839953626677899E-09,-1.8557992993411669E-08, 2.6034728347152000E-08, 2.6027232119138000E-08 - 80, 27, 1.3524678562542650E-08, 3.2027234412455680E-08, 2.5928877676997000E-08, 2.5974641403621000E-08 - 80, 28, 1.1788759692830061E-09,-1.9463450344046001E-08, 2.5835049889098001E-08, 2.5819394433451999E-08 - 80, 29,-3.0031703377047017E-11, 2.6392469990709931E-08, 2.5830759358036999E-08, 2.5832789715556999E-08 - 80, 30,-1.0905476474323560E-08,-1.7408655887830911E-08, 2.5686415555287999E-08, 2.5656734556563999E-08 - 80, 31, 4.8453046289875611E-09,-3.1553917918727250E-09, 2.5688273872605999E-08, 2.5715210332136001E-08 - 80, 32,-3.8797294062137374E-09, 1.5105918278718890E-09, 2.5498649206474001E-08, 2.5507118386917000E-08 - 80, 33, 1.4467562566795161E-08, 1.0532264311497570E-08, 2.5593752154250001E-08, 2.5576536590028999E-08 - 80, 34, 2.9957090174492722E-08, 1.7175081443761351E-08, 2.5362397230453002E-08, 2.5436381678179000E-08 - 80, 35,-2.6679638290762940E-08, 3.0050347935913090E-08, 2.5547156670594000E-08, 2.5453393274060001E-08 - 80, 36, 4.7882139135036427E-08,-1.0253981210654091E-08, 2.5452522397769001E-08, 2.5580116288200001E-08 - 80, 37, 2.1335310056201359E-08,-8.6270163459696745E-09, 2.5413192402642001E-08, 2.5591048446484999E-08 - 80, 38, 1.1733655877873139E-08,-1.2837065152313779E-08, 2.5456439543442999E-08, 2.5329192465855002E-08 - 80, 39,-4.2358818553315502E-08, 4.8205354525143243E-08, 2.5232587517348000E-08, 2.5241410488021999E-08 - 80, 40,-6.1925688280558291E-08, 2.1163233395031960E-08, 2.5150162704904001E-08, 2.5178303747426001E-08 - 80, 41,-1.4778028047205430E-08, 6.9861928407038001E-09, 2.4992407410166001E-08, 2.5015343549103001E-08 - 80, 42,-7.4516381551750495E-09,-2.9622241535938709E-08, 2.5086985758967001E-08, 2.5055965193180000E-08 - 80, 43,-4.6417899111812908E-10, 1.5184071707212271E-08, 2.4883531079576002E-08, 2.4926641537090999E-08 - 80, 44, 2.9630914123460569E-08, 3.0829745426810592E-08, 2.5022316721629999E-08, 2.5091762787643001E-08 - 80, 45, 1.3815009729208870E-08,-1.6022633320064719E-08, 2.4903750544597000E-08, 2.4857355119025999E-08 - 80, 46,-3.7622113836815749E-09,-9.5516683889124168E-09, 2.4984648571695999E-08, 2.4966318521517000E-08 - 80, 47,-8.3395782281606974E-09, 3.9326146365600977E-08, 2.4832086979760000E-08, 2.4867299812931999E-08 - 80, 48,-7.4165402895904980E-09,-5.3105370572886257E-08, 2.4929651777814001E-08, 2.4876579760528000E-08 - 80, 49, 9.6883903202945099E-09,-4.5311088372339381E-08, 2.4834830510448000E-08, 2.4774479927988000E-08 - 80, 50,-1.8597550344267569E-10, 6.8631082966286260E-08, 2.5002684299732999E-08, 2.5000273073624001E-08 - 80, 51,-5.2323803606734123E-09, 6.2384243796602041E-08, 2.4894622105521000E-08, 2.4863412956598000E-08 - 80, 52,-9.9280364590895888E-09,-3.9659759888552218E-08, 2.4928036474941000E-08, 2.4919418748146000E-08 - 80, 53, 3.0974627118803122E-08, 2.5145406028728331E-09, 2.4738901736350999E-08, 2.4700843511163000E-08 - 80, 54, 1.4324249654997550E-08, 2.7212568876220099E-08, 2.4830717089246000E-08, 2.4845304072938000E-08 - 80, 55,-3.4611647087559340E-08, 6.0636291133516465E-08, 2.4714583506211999E-08, 2.4664294087304000E-08 - 80, 56,-2.1896688456436420E-08, 2.5306620135170022E-08, 2.4681681759214000E-08, 2.4663796391865999E-08 - 80, 57, 1.6976934247191510E-08, 4.0812804128376697E-09, 2.4680641222375999E-08, 2.4679170243585000E-08 - 80, 58, 7.5350958316826669E-09, 1.6848844518199869E-08, 2.4526954355442001E-08, 2.4572033163790001E-08 - 80, 59, 2.7187534803989398E-08, 1.6049933859608012E-08, 2.4347664997237001E-08, 2.4300460253319001E-08 - 80, 60, 4.1789798980955653E-08,-3.0848877348280398E-08, 2.4410903627827999E-08, 2.4433955891687000E-08 - 80, 61, 1.0290268731821001E-08,-2.6461204206885849E-08, 2.3760307817292000E-08, 2.3775097641050000E-08 - 80, 62,-3.9933564935121653E-08,-7.9262343324545450E-09, 2.4167778438042001E-08, 2.4243240233636001E-08 - 80, 63,-2.6030744450060770E-08, 3.5246868459676597E-08, 2.3274157181616001E-08, 2.3281655738052000E-08 - 80, 64,-5.9943048904649453E-09, 5.3467015227894519E-09, 2.3745340681378000E-08, 2.3574343800997001E-08 - 80, 65,-7.2882879079013380E-08, 3.3028631779662417E-08, 2.3047894729124001E-08, 2.3176469694489999E-08 - 80, 66, 1.8872555283807299E-08, 3.5338553550096107E-08, 2.3444245174544000E-08, 2.3296178594182002E-08 - 80, 67, 5.4838625638220832E-08, 2.8538880314539471E-08, 2.3019120951375000E-08, 2.3023603816566999E-08 - 80, 68,-3.1178521587207417E-08,-3.0311470601815053E-11, 2.2727595990178002E-08, 2.2690790335318000E-08 - 80, 69,-2.6750142767822221E-08, 3.8383509799345972E-08, 2.2751761393620999E-08, 2.2728060015139999E-08 - 80, 70,-2.3386308516903710E-08,-1.9836141583977470E-08, 2.2090263259638001E-08, 2.2100660196533999E-08 - 80, 71, 2.9529758800279319E-08, 7.5624987216249083E-08, 2.2291530115080002E-08, 2.2233574050017000E-08 - 80, 72,-1.0032595911956839E-08,-1.9420146713508260E-08, 2.1022219368817000E-08, 2.1012363609914001E-08 - 80, 73,-3.2735606783809041E-08, 3.4309383774046480E-10, 2.1548877188606001E-08, 2.1527273614317999E-08 - 80, 74, 4.9461442700933757E-08,-1.7576578428478611E-08, 1.8981785612109000E-08, 1.9027076906016000E-08 - 80, 75,-8.4857257257284652E-08, 3.6022191229826377E-08, 1.8957114546149999E-08, 1.8939813872601999E-08 - 80, 76,-1.1068756994004329E-09, 4.2966533959804681E-08, 1.8182909273002999E-08, 1.8095040449487001E-08 - 80, 77,-7.4473875387362241E-08, 4.2482015215949883E-08, 1.4787068274503000E-08, 1.4833713082563000E-08 - 80, 78,-5.5783185324809871E-08,-4.0326401796641487E-08, 1.2148658100906000E-08, 1.2200276523601000E-08 - 80, 79, 3.8147798704151063E-08,-1.9721419218429551E-08, 3.8064906705878004E-09, 3.8718549642204002E-09 - 80, 80, 4.0582099786720437E-08,-5.3860308941804763E-08, 5.8013789517889999E-09, 5.7638703929097002E-09 + 3.3970000000000000E+06, 4.2828371901284001E+13, 7.3999999999999996E-05, 80, 80, 1, 0.0000000000000000E+00, 0.0000000000000000E+00 + 2, 0,-8.7450547081842009E-04, 0.0000000000000000E+00, 1.2103113782184000E-10, 0.0000000000000000E+00 + 2, 1, 1.3938449166781359E-10, 1.7044280642328221E-10, 7.3039927797648007E-11, 7.3266295432547008E-11 + 2, 2,-8.4177519807822603E-05, 4.9605348841412452E-05, 3.3991945147726000E-11, 3.4592569163869000E-11 + 3, 0,-1.1886910646015641E-05, 0.0000000000000000E+00, 9.8471786784139995E-11, 0.0000000000000000E+00 + 3, 1, 3.9053442315700724E-06, 2.5139324037413419E-05, 7.1449688816484996E-11, 7.2254867814010005E-11 + 3, 2,-1.5863411026265399E-05, 8.4857987158792132E-06, 5.9676026032878996E-11, 5.9797382572260005E-11 + 3, 3, 3.5338541142774030E-05, 2.5113984262622799E-05, 3.9949037676474000E-11, 4.0462374542868001E-11 + 4, 0, 5.1257987175465586E-06, 0.0000000000000000E+00, 1.0329911830041000E-10, 0.0000000000000000E+00 + 4, 1, 4.2271575054702128E-06, 3.7413215027228718E-06, 9.3004765290544999E-11, 9.4423002643425008E-11 + 4, 2,-1.0253884110275679E-06,-8.9622951629187374E-06, 7.3213784191804998E-11, 7.3314619138911000E-11 + 4, 3, 6.4461288728918093E-06,-2.7297790313231990E-07, 5.0594702258703002E-11, 5.1248160693008003E-11 + 4, 4, 9.6384334824044650E-08,-1.2861361694339760E-05, 2.9964623548633001E-11, 3.0045838133870998E-11 + 5, 0,-1.7242068505338999E-06, 0.0000000000000000E+00, 1.0935027490925000E-10, 0.0000000000000000E+00 + 5, 1, 4.9155252614409601E-07, 2.1179750719200639E-06, 1.0291694202540000E-10, 1.0455199159658000E-10 + 5, 2,-4.3015486989529303E-06,-1.1283599363068411E-06, 8.8105287844057999E-11, 8.7282038965633995E-11 + 5, 3, 3.3106878341316730E-06, 2.3024139448590119E-07, 6.8215826754134999E-11, 6.8675139409518995E-11 + 5, 4,-4.6889658986047850E-06,-3.2997722093047299E-06, 5.0256560862804000E-11, 4.9993069474561002E-11 + 5, 5,-4.3640801168293771E-06, 3.8656154098344251E-06, 3.2926957261939999E-11, 3.2808375832549997E-11 + 6, 0, 1.3448267510621481E-06, 0.0000000000000000E+00, 1.1989849952729999E-10, 0.0000000000000000E+00 + 6, 1, 1.7926701548423649E-06,-1.5234122376020040E-06, 1.1430564902046000E-10, 1.1578885420138000E-10 + 6, 2, 8.7185360387643135E-07, 1.4592318487313359E-06, 1.0264522918987000E-10, 1.0233400253426000E-10 + 6, 3, 9.5816746766165918E-07, 3.2022418522315923E-07, 8.4682052808222002E-11, 8.5144731447337001E-11 + 6, 4, 1.0503162831989909E-06, 2.6170003356585492E-06, 6.7176945044346996E-11, 6.6934853873167006E-11 + 6, 5, 1.6880399538737750E-06, 1.5853762547290531E-06, 4.8482587746359003E-11, 4.8320327459045001E-11 + 6, 6, 2.7768427868541821E-06, 7.5170270469872722E-07, 2.9588792491855000E-11, 3.0111321390980003E-11 + 7, 0, 1.0566966079621890E-06, 0.0000000000000000E+00, 1.3053898393156999E-10, 0.0000000000000000E+00 + 7, 1, 1.3714218129129991E-06,-2.3256794873473699E-07, 1.2668612509928999E-10, 1.2845746108779001E-10 + 7, 2, 2.8033764459323890E-06,-6.5173659805380695E-07, 1.1687908987197001E-10, 1.1629864769762000E-10 + 7, 3, 8.7336552189929800E-07,-4.0675705536590699E-07, 1.0138864653051000E-10, 1.0200867730236000E-10 + 7, 4, 2.4566369021535799E-06,-4.6224291975934719E-07, 8.4182046514154006E-11, 8.3789545340149998E-11 + 7, 5,-2.1930690076858139E-07,-1.3515575270662830E-06, 6.7293023283346998E-11, 6.7061084496940997E-11 + 7, 6,-6.0515930653241116E-07,-1.8832018842058270E-06, 5.0024296240089002E-11, 5.0354050499176000E-11 + 7, 7, 3.8833580001392989E-07,-1.7838625631807321E-06, 3.0365097497653002E-11, 3.0158240731334000E-11 + 8, 0, 1.4442117457414669E-07, 0.0000000000000000E+00, 1.4391555197484001E-10, 0.0000000000000000E+00 + 8, 1,-1.2904533441152021E-07, 7.4942490031393760E-07, 1.4035512994475001E-10, 1.4238986448210000E-10 + 8, 2, 1.8100214820539061E-06, 4.9096335865112556E-07, 1.3258296928176000E-10, 1.3177642777485001E-10 + 8, 3,-1.2207784549871089E-06,-1.3230871310063700E-06, 1.1803251372697001E-10, 1.1844597503660000E-10 + 8, 4, 1.5866931019861380E-06, 1.2166283194289211E-07, 1.0234551564058000E-10, 1.0210100863050001E-10 + 8, 5,-2.8183735255990001E-06,-1.5681467772892050E-06, 8.5387347630881000E-11, 8.5126826010749003E-11 + 8, 6,-9.5599877920939645E-07,-1.7626582142154601E-06, 6.8834697153848007E-11, 6.9560301830679008E-11 + 8, 7,-4.2471893831892682E-07, 1.6540404534478641E-06, 5.1036963686811000E-11, 5.0905844119266998E-11 + 8, 8,-3.1783805703279631E-07,-2.3919729926160170E-07, 3.0246036449331002E-11, 3.0306724975438997E-11 + 9, 0,-2.8745593244289322E-07, 0.0000000000000000E+00, 1.5944139470497001E-10, 0.0000000000000000E+00 + 9, 1, 4.1743298827892820E-07,-4.9042273171692781E-07, 1.5470877245763001E-10, 1.5677087657617000E-10 + 9, 2, 1.1385495484935660E-06, 3.7160680928003798E-07, 1.4963319721882999E-10, 1.4894233754269999E-10 + 9, 3,-1.0035199183447020E-06,-9.9277210221517757E-07, 1.3545466665846999E-10, 1.3621130548164001E-10 + 9, 4, 3.2079095125258889E-07, 1.6143044902574391E-06, 1.2095501179368999E-10, 1.2057094744733001E-10 + 9, 5,-2.2970171000700119E-06,-1.4984195288745289E-06, 1.0442727621315001E-10, 1.0430687092711999E-10 + 9, 6, 8.2020744110463016E-07, 5.5634131644082519E-07, 8.6819995795880997E-11, 8.7611819598515998E-11 + 9, 7,-5.9128904319118457E-07, 8.8413812925584658E-07, 7.1358830660606002E-11, 7.1061774488430003E-11 + 9, 8, 1.1992792300607110E-06,-1.8558235185992101E-07, 5.2995668209201000E-11, 5.2929182004139998E-11 + 9, 9,-1.1923241516342979E-06,-6.1225374183571644E-07, 2.9106076211082999E-11, 2.9395530609045002E-11 + 10, 0, 7.2586970184033398E-07, 0.0000000000000000E+00, 1.7539119019022001E-10, 0.0000000000000000E+00 + 10, 1, 9.2212003081492357E-07, 2.1871853125250699E-07, 1.7260613065107000E-10, 1.7490034816170001E-10 + 10, 2,-2.1698263972956060E-09,-1.1128765713995101E-06, 1.6661149525105999E-10, 1.6563430085192001E-10 + 10, 3,-2.9275595477379353E-07, 4.4574364384326100E-07, 1.5549353531419001E-10, 1.5608224719629999E-10 + 10, 4,-1.2112906007513811E-06,-4.9501526389811057E-08, 1.4018964991696999E-10, 1.4011950998712999E-10 + 10, 5, 3.9839975982150670E-07,-1.0683891523420349E-06, 1.2456925121774000E-10, 1.2447089194251000E-10 + 10, 6, 6.9499178396903159E-07, 1.0970214333048109E-06, 1.0718511577723000E-10, 1.0816817612797000E-10 + 10, 7, 2.9911814611399940E-07,-6.2740746269725919E-07, 9.1117241757156008E-11, 9.0615755019302006E-11 + 10, 8, 5.6556749448508493E-07, 7.9979384590180545E-07, 7.5255834664691999E-11, 7.5217730035947998E-11 + 10, 9,-1.5064865665416510E-06,-1.4044666157341789E-06, 5.6093715084827000E-11, 5.6089381349223998E-11 + 10, 10,-2.4277051735088178E-07, 7.6193623951414320E-07, 3.0465236758491000E-11, 3.0569101480682999E-11 + 11, 0,-2.6649003866535410E-07, 0.0000000000000000E+00, 1.9412621885494000E-10, 0.0000000000000000E+00 + 11, 1,-8.1519392894443437E-07,-2.1751302948038430E-07, 1.9125131244509001E-10, 1.9370687404446999E-10 + 11, 2,-3.2104320936602609E-07,-9.8790735061236510E-07, 1.8661216509076001E-10, 1.8558323281450999E-10 + 11, 3,-1.2892409568787179E-06, 7.7876627497924496E-07, 1.7539826276664999E-10, 1.7608951498311000E-10 + 11, 4,-1.5791909175600400E-06,-6.1657941677816327E-07, 1.6222588482032000E-10, 1.6215407995167999E-10 + 11, 5, 1.3703809961827519E-06, 8.6845829243624667E-07, 1.4551458457398999E-10, 1.4558131054602001E-10 + 11, 6,-2.4320602261743078E-07, 2.6287975442268369E-08, 1.2872598115413999E-10, 1.3000944402768999E-10 + 11, 7, 6.3312547465221262E-07,-8.8124580630580818E-07, 1.1251635285127000E-10, 1.1171292457530000E-10 + 11, 8,-1.1526216305062039E-06, 8.0289109851342910E-07, 9.4849314070212001E-11, 9.4759494365052999E-11 + 11, 9,-4.2887218833226868E-07,-4.0266826541827498E-07, 7.8491792268899000E-11, 7.8833465257989999E-11 + 11, 10, 4.1248741283895690E-07, 1.9449054488648321E-06, 5.7429923668925998E-11, 5.7204755236884002E-11 + 11, 11,-5.9129125196024682E-08,-3.2013461955227138E-07, 2.8669458192070001E-11, 2.8703820604629998E-11 + 12, 0, 2.5901010973447811E-07, 0.0000000000000000E+00, 2.1544212220396000E-10, 0.0000000000000000E+00 + 12, 1,-1.1232760242154771E-06,-4.6597501385511642E-08, 2.1240222807557999E-10, 2.1517552677388000E-10 + 12, 2,-9.3180509514092405E-08, 5.3208625826598304E-07, 2.0836388521206001E-10, 2.0730039832943999E-10 + 12, 3,-1.4328437159904669E-06, 3.3094328849604699E-07, 1.9790381030890000E-10, 1.9863260618291999E-10 + 12, 4,-9.8989762948794809E-08, 1.2477384606767650E-07, 1.8521062384222000E-10, 1.8506816515755000E-10 + 12, 5, 7.3841719085192487E-07, 9.8313706759767646E-07, 1.6957326958723000E-10, 1.6957690960913000E-10 + 12, 6,-4.4199660810539768E-07,-1.6138324546284839E-06, 1.5136857109862001E-10, 1.5288392827742000E-10 + 12, 7, 3.9950015269219688E-07,-1.2420962147609649E-07, 1.3592081848620999E-10, 1.3511281006034000E-10 + 12, 8,-1.6103036177638721E-06,-3.3390142111388809E-07, 1.1789375872111001E-10, 1.1786734461664001E-10 + 12, 9, 7.2319162327703644E-07, 4.4834612822914618E-07, 9.9879585325839996E-11, 1.0049027768587001E-10 + 12, 10, 5.4275397597080196E-07, 1.3521153280272900E-06, 8.4332707588165004E-11, 8.3844073261327006E-11 + 12, 11, 7.8624501327329339E-07,-1.6864131081620689E-06, 6.2473070064380999E-11, 6.3135152971843006E-11 + 12, 12,-1.0856558755016089E-08,-8.8551160240718884E-08, 3.7956014392819003E-11, 3.7611187752070002E-11 + 13, 0,-4.8952404831803390E-07, 0.0000000000000000E+00, 2.3922705382994002E-10, 0.0000000000000000E+00 + 13, 1,-5.6648398354796893E-07, 2.2788663120968919E-07, 2.3567363500466001E-10, 2.3878585276678001E-10 + 13, 2, 1.9841104168070469E-07, 8.4834357183256708E-07, 2.3277554281839000E-10, 2.3170622185943001E-10 + 13, 3, 2.1767220813221301E-07, 1.7831477048833361E-07, 2.2242693420350000E-10, 2.2314084792430999E-10 + 13, 4, 1.8222961512093351E-07, 6.8437925412020000E-07, 2.1033196052609000E-10, 2.1049042014077001E-10 + 13, 5,-2.1635837716555661E-09,-4.0358226801550021E-07, 1.9467991713149999E-10, 1.9501144632263999E-10 + 13, 6, 2.0889697822006498E-08,-9.2963519094536122E-07, 1.7718448092050001E-10, 1.7896489279944000E-10 + 13, 7,-7.5633516483173801E-07, 4.1816002573524902E-07, 1.6048616227869000E-10, 1.5934076308326999E-10 + 13, 8,-2.3166748437483840E-07, 1.2030332905113710E-09, 1.4249650274048000E-10, 1.4266366612095000E-10 + 13, 9, 1.1327419797014090E-06, 7.6091981962187019E-07, 1.2376915902365999E-10, 1.2469998624707001E-10 + 13, 10,-1.8420690773034191E-07,-6.3834053349287647E-07, 1.0643120864673000E-10, 1.0566216959408000E-10 + 13, 11, 7.2152491396419184E-07,-8.5419916802722339E-07, 8.9076403459725997E-11, 8.9676279954839007E-11 + 13, 12,-1.4713308304428070E-06,-3.5633994441531591E-07, 6.5055201193692995E-11, 6.5129690420097996E-11 + 13, 13, 4.7249117577621590E-07, 8.4684144373715813E-07, 3.6320246678525002E-11, 3.6828348268609003E-11 + 14, 0, 3.0219271864045722E-07, 0.0000000000000000E+00, 2.6556530076336000E-10, 0.0000000000000000E+00 + 14, 1, 7.5309548515849434E-07, 3.0659939197390379E-07, 2.6233129402536999E-10, 2.6556182531491001E-10 + 14, 2, 4.4431057610504009E-07,-5.2539256378857698E-07, 2.5932099596772002E-10, 2.5802064520266001E-10 + 14, 3, 4.2086425064021988E-07,-3.1613141949167798E-07, 2.4997785445991998E-10, 2.5076161843620999E-10 + 14, 4, 2.1086607659015601E-07,-9.6126046924893883E-07, 2.3771365012418000E-10, 2.3784292426187999E-10 + 14, 5,-4.7256009813144700E-07,-9.5209040368264479E-07, 2.2291973969368000E-10, 2.2331186086574999E-10 + 14, 6,-6.8240005934222495E-08,-8.1255730199729201E-08, 2.0456642237235999E-10, 2.0692637694372000E-10 + 14, 7,-4.3833797637763461E-07, 2.5344830630813670E-07, 1.8866572025066999E-10, 1.8730060109863999E-10 + 14, 8, 8.2760862524879536E-07, 3.4542413026249739E-07, 1.6899410807335000E-10, 1.6907860932119999E-10 + 14, 9, 2.5206510270110481E-07, 1.1412035615165890E-06, 1.5033736238848001E-10, 1.5179467349326000E-10 + 14, 10,-4.0006125111822032E-08,-1.5805411521501649E-06, 1.3268506472902000E-10, 1.3209264822394001E-10 + 14, 11,-9.1756400631668835E-07, 2.0181206830551511E-07, 1.1265659621480000E-10, 1.1338872230048000E-10 + 14, 12,-4.6098789395114132E-07,-2.8123541924842100E-07, 9.5933862469390002E-11, 9.6244721746418006E-11 + 14, 13, 8.6159647817875325E-07, 2.0166474791487490E-06, 7.4316359088339006E-11, 7.4854791147524003E-11 + 14, 14, 4.0566294949909653E-09,-7.4397446155337985E-07, 3.8421806471019999E-11, 3.8721148521822999E-11 + 15, 0, 4.9338119977619251E-07, 0.0000000000000000E+00, 2.9517746279730999E-10, 0.0000000000000000E+00 + 15, 1, 2.3504085774446549E-07, 2.5814277194372382E-07, 2.9153422873027998E-10, 2.9510973190729001E-10 + 15, 2,-3.5877664452026991E-07,-7.7775370918244422E-07, 2.8937289735560998E-10, 2.8809597971141002E-10 + 15, 3,-5.6235428831306517E-07,-2.9119276531665789E-07, 2.7991900779889000E-10, 2.8061418388809002E-10 + 15, 4,-7.3107741433901826E-07,-8.2291945418793019E-07, 2.6859288097318999E-10, 2.6869042589351999E-10 + 15, 5,-9.4418999262905104E-07,-4.5309344036893439E-07, 2.5320088176142001E-10, 2.5374084143311002E-10 + 15, 6,-4.6569424928889101E-08, 6.3181043403389908E-07, 2.3538004318833002E-10, 2.3808694212132000E-10 + 15, 7, 9.3252509300585759E-07, 3.3367910077558299E-07, 2.1874533651858999E-10, 2.1704461108567000E-10 + 15, 8, 1.1713557431122270E-06, 4.3397393082264647E-07, 1.9895169713842000E-10, 1.9935016997159999E-10 + 15, 9,-3.1361197239010152E-07,-3.6130776998697968E-07, 1.7843735281586000E-10, 1.8027480683255001E-10 + 15, 10,-1.6617979177073590E-07,-1.4989148710418020E-07, 1.6089851425946000E-10, 1.6020667736964000E-10 + 15, 11,-7.4449581533426630E-07, 6.0499732095853779E-07, 1.4050475077078000E-10, 1.4113581034136000E-10 + 15, 12, 1.0125729414718300E-06, 7.0744643143345193E-07, 1.2077765476258000E-10, 1.2113372705228000E-10 + 15, 13, 2.1632945779220320E-07, 6.9692452639138542E-07, 1.0355725991470000E-10, 1.0471853347515001E-10 + 15, 14, 1.4287311615973200E-07,-1.3675794263071820E-06, 7.4065406404841999E-11, 7.3892787879709996E-11 + 15, 15,-3.2983911020273521E-07, 1.4679301357791700E-07, 3.9262982920572003E-11, 3.8815308015795001E-11 + 16, 0, 6.2492272251973242E-07, 0.0000000000000000E+00, 3.2805690113790010E-10, 0.0000000000000000E+00 + 16, 1,-2.3576455388861381E-07,-5.5301461123008231E-07, 3.2467108947652001E-10, 3.2840534860225002E-10 + 16, 2,-4.8128515175366109E-07,-8.3798338110112221E-08, 3.2223553208065998E-10, 3.2088216250179002E-10 + 16, 3,-5.2884584512722868E-07, 4.3179535117818742E-07, 3.1338928892497998E-10, 3.1432050775531001E-10 + 16, 4,-7.7524880882120419E-07,-4.1236194296459131E-08, 3.0177887171033002E-10, 3.0235677774235001E-10 + 16, 5,-3.3156669814606880E-07, 5.9447709629048816E-07, 2.8723444746464998E-10, 2.8801243692789999E-10 + 16, 6, 1.4873519233098461E-07, 2.1995105650212950E-07, 2.6860427150630999E-10, 2.7201072932818998E-10 + 16, 7, 3.9540191879011490E-07, 2.9924170216304151E-07, 2.5266259649062001E-10, 2.5097762688375000E-10 + 16, 8, 2.4726774880144362E-07, 3.8194759694030728E-07, 2.3126097274353999E-10, 2.3205851951551000E-10 + 16, 9,-3.9083254186985018E-07,-9.2091247731411718E-07, 2.1087090709721001E-10, 2.1309604245963001E-10 + 16, 10,-4.9887892959809643E-07, 3.6222033033373808E-07, 1.9170274792531999E-10, 1.9102897234817000E-10 + 16, 11,-8.0503692776153824E-08,-3.0833185246732417E-07, 1.7102041766089001E-10, 1.7187789196071001E-10 + 16, 12, 4.4116504364058009E-07, 6.4720068684209609E-07, 1.5147887036299000E-10, 1.5226342447604000E-10 + 16, 13,-9.6676684214209105E-08,-5.9086298237427453E-07, 1.2965348593317001E-10, 1.3095008462812001E-10 + 16, 14,-2.2346863154783541E-07,-9.2646534797515104E-07, 1.1023249349617000E-10, 1.1033431106696000E-10 + 16, 15,-8.3320558932387890E-07, 3.7522848734954839E-07, 7.9994374080275996E-11, 7.9815215662268004E-11 + 16, 16, 2.1693483565260650E-07, 1.0572655635748210E-07, 4.4719179009241003E-11, 4.4297873663697003E-11 + 17, 0,-5.4709542986930998E-08, 0.0000000000000000E+00, 3.6486938511132998E-10, 0.0000000000000000E+00 + 17, 1,-3.7399137381517438E-07,-6.3489366161239067E-07, 3.6139366386669997E-10, 3.6555218681949001E-10 + 17, 2,-2.0004915070624970E-07, 1.4769821786575250E-07, 3.5904127896492010E-10, 3.5769757037661011E-10 + 17, 3,-4.4054829757514781E-08, 1.9182219078222199E-07, 3.5064014324423999E-10, 3.5126578586621010E-10 + 17, 4, 4.5617658067346931E-07,-1.7248528799716611E-08, 3.3926128392930001E-10, 3.3982901241850999E-10 + 17, 5, 7.0352057270776358E-07, 2.9716758199903928E-07, 3.2468741422718010E-10, 3.2513551110133001E-10 + 17, 6, 6.2735688086993120E-07,-3.2424361898797991E-07, 3.0601686597374010E-10, 3.0972440072692999E-10 + 17, 7, 3.3067607545801760E-07,-2.4963319407555199E-07, 2.8950999985072002E-10, 2.8759668614058998E-10 + 17, 8,-3.7638667604706180E-07,-1.9171388957514899E-07, 2.6798109429521002E-10, 2.6869443710152001E-10 + 17, 9,-2.0630975795240439E-07,-3.2050677363791618E-07, 2.4565365760960002E-10, 2.4851552404129003E-10 + 17, 10,-5.5645632584498266E-07, 2.8962148266298910E-07, 2.2656892831538001E-10, 2.2570862491793000E-10 + 17, 11, 3.5243509929602669E-07, 3.5354260317607713E-08, 2.0412432326884999E-10, 2.0487607979741001E-10 + 17, 12, 2.0025673973076780E-07,-1.3079093383117499E-08, 1.8339435658969001E-10, 1.8454434218630000E-10 + 17, 13,-4.8612342952491358E-07,-7.7483339436755765E-07, 1.6198903492658001E-10, 1.6326363756298999E-10 + 17, 14,-3.8008064670123599E-07,-4.4794294782534088E-07, 1.3980184549476999E-10, 1.3977109236774000E-10 + 17, 15,-3.3804523377146830E-07, 4.2775478707706719E-07, 1.1880084629867000E-10, 1.1846428801309000E-10 + 17, 16, 1.1049431287614231E-06, 4.9309080047218621E-07, 8.4458348846849004E-11, 8.4040825824180008E-11 + 17, 17, 7.4847480457838662E-08, 2.8783514545524541E-07, 4.6320910087850003E-11, 4.6044632780380997E-11 + 18, 0,-3.1250064729225039E-07, 0.0000000000000000E+00, 4.0647151544338010E-10, 0.0000000000000000E+00 + 18, 1,-1.8296324000412531E-07,-5.7769470567911306E-07, 4.0182053213840002E-10, 4.0613395819612002E-10 + 18, 2,-1.0412207107656000E-07, 4.5344370424782177E-08, 4.0091840702135999E-10, 3.9916212618639010E-10 + 18, 3,-2.9939186240254088E-07,-4.5784264171329838E-07, 3.9111971648737002E-10, 3.9213646782313002E-10 + 18, 4, 8.7501712948002529E-07, 1.1649861097786980E-07, 3.8110333143449998E-10, 3.8149708418869998E-10 + 18, 5, 1.9313905997574150E-07,-6.2249966128910834E-07, 3.6571291034253002E-10, 3.6677764161835998E-10 + 18, 6, 2.0688943917981149E-07,-6.8437607860474299E-07, 3.4697265703707002E-10, 3.5152607038042010E-10 + 18, 7,-2.0217971073446040E-07,-1.6356127281011639E-07, 3.3079412662467999E-10, 3.2849516979069998E-10 + 18, 8, 5.2056773524533092E-08,-2.6360703691128459E-07, 3.0821050140840002E-10, 3.0911912357847998E-10 + 18, 9, 7.7969026722603824E-08, 2.2552919560177530E-07, 2.8548535497369002E-10, 2.8846431362759998E-10 + 18, 10, 6.6329434317302129E-07,-9.4810465055948729E-08, 2.6476790067956001E-10, 2.6398470495955001E-10 + 18, 11, 2.6059961095133892E-07,-3.1345741040837309E-07, 2.4179754271710999E-10, 2.4254330369547999E-10 + 18, 12, 6.8911605863124401E-08,-1.0399482958457250E-07, 2.1927104697961000E-10, 2.2060181574120001E-10 + 18, 13,-3.1271776039499141E-07,-2.5122404024047290E-07, 1.9702549436515000E-10, 1.9801809485136001E-10 + 18, 14,-3.5413778761107888E-07, 2.1245035795232279E-07, 1.7435835357738001E-10, 1.7446770355651000E-10 + 18, 15, 4.0427231994548502E-07, 7.2717527650255237E-07, 1.5073384274190999E-10, 1.5057492391765001E-10 + 18, 16, 4.2540458121371041E-07, 1.9491720533030481E-07, 1.2840222181280000E-10, 1.2749581817833001E-10 + 18, 17,-6.6185211446951153E-09,-1.1055839853856400E-06, 9.2880725482521999E-11, 9.2084105750115004E-11 + 18, 18, 4.9076799412949712E-07, 3.0810258604566902E-07, 5.3443957066628999E-11, 5.3472074408286003E-11 + 19, 0, 5.9593471480895979E-09, 0.0000000000000000E+00, 4.5245741196867011E-10, 0.0000000000000000E+00 + 19, 1, 4.7984786635487607E-08, 5.0606657054152741E-07, 4.4709204533812002E-10, 4.5182610404806003E-10 + 19, 2,-5.2810415618861903E-08, 8.7643122788538723E-08, 4.4666089453549010E-10, 4.4526615944716010E-10 + 19, 3,-5.4861024791885132E-08,-1.9930153462648481E-07, 4.3757055597421999E-10, 4.3792789594428999E-10 + 19, 4, 1.4263562742559671E-07, 2.5044457390705712E-07, 4.2660285644026998E-10, 4.2717024220667010E-10 + 19, 5,-2.9276850316067513E-07,-4.3895607578845078E-07, 4.1214652696741998E-10, 4.1326737715955010E-10 + 19, 6,-2.7765506917946031E-07,-1.4472759335102671E-07, 3.9203776243683999E-10, 3.9750433422089011E-10 + 19, 7,-4.8276286290870036E-07, 2.0107368970881271E-07, 3.7669164889519010E-10, 3.7403156471358002E-10 + 19, 8, 4.2212800997513992E-07,-1.4536614945671280E-07, 3.5255605664897998E-10, 3.5434298597904999E-10 + 19, 9, 2.2422934903485960E-07, 3.8573458502090812E-07, 3.2881766074467010E-10, 3.3270013700692998E-10 + 19, 10, 5.9754731714712053E-07,-1.0349337739528020E-07, 3.0770856996466001E-10, 3.0695011263143010E-10 + 19, 11, 2.6748761625195819E-08,-3.9238448990827910E-07, 2.8331503232646010E-10, 2.8393091220607998E-10 + 19, 12,-1.1353985029612160E-07,-1.5548311289263989E-08, 2.5947446619016998E-10, 2.6083737630754998E-10 + 19, 13,-2.0231048781492040E-07,-2.9076960333033691E-08, 2.3568136690029002E-10, 2.3687764768677002E-10 + 19, 14, 1.8370489788655959E-07, 5.0561428737075958E-07, 2.1267880380456999E-10, 2.1271931494252001E-10 + 19, 15, 8.5469937506493990E-07, 4.5122259786035062E-07, 1.8759663496207000E-10, 1.8754393534591001E-10 + 19, 16,-1.5178564199107839E-08,-1.3538949530565789E-07, 1.6367988101766000E-10, 1.6249570081939000E-10 + 19, 17,-1.4691049019410600E-07,-4.2595464334983788E-07, 1.3905860915571000E-10, 1.3786940077562999E-10 + 19, 18,-6.7605472888485071E-07, 6.4337251024823409E-07, 9.8584004670604003E-11, 9.8376567566550997E-11 + 19, 19,-4.2895397427418769E-07,-8.0085248199827081E-07, 5.4420332957366998E-11, 5.4816334366905998E-11 + 20, 0, 2.9881260537936598E-07, 0.0000000000000000E+00, 5.0184663007148015E-10, 0.0000000000000000E+00 + 20, 1, 1.8580341470164049E-07, 5.5166467757316584E-07, 4.9919952052003007E-10, 5.0467931410559008E-10 + 20, 2, 6.7077484529405142E-08,-2.3854889220809512E-07, 4.9652043698732999E-10, 4.9477800571342004E-10 + 20, 3, 4.2629483620741551E-07, 2.0444588490030209E-07, 4.8921251982749000E-10, 4.9054971284786009E-10 + 20, 4,-3.5522136272433662E-07, 2.1259491309390880E-07, 4.7657854594275000E-10, 4.7779056899115007E-10 + 20, 5, 1.6924862463125410E-07, 1.1045333324553961E-08, 4.6333840475570002E-10, 4.6479758232154010E-10 + 20, 6, 2.0998547236303271E-07, 3.7356644384240269E-07, 4.4289799342539998E-10, 4.4858874167258998E-10 + 20, 7, 7.0244713844181063E-08, 3.7898258389081973E-08, 4.2687270596393011E-10, 4.2425284453157999E-10 + 20, 8, 3.2136235256075510E-07, 3.0592253100467350E-07, 4.0288449827823999E-10, 4.0434545266766999E-10 + 20, 9, 6.5192996480357300E-08, 1.7864588537008961E-07, 3.7724888098863002E-10, 3.8189531242642001E-10 + 20, 10,-4.2083149137409152E-07,-1.0435247651158750E-08, 3.5570227006743001E-10, 3.5466936557795010E-10 + 20, 11,-1.6114641023949900E-07, 2.7220617299528919E-08, 3.2991960684076998E-10, 3.3070174759012002E-10 + 20, 12,-4.7994489225668577E-07,-1.4795540504238811E-07, 3.0414458499437000E-10, 3.0610234070543998E-10 + 20, 13, 1.8109737849233900E-08, 3.4234017008717649E-07, 2.7934920709935001E-10, 2.8050870203489010E-10 + 20, 14, 2.9344108002153201E-07, 2.1671360097310811E-07, 2.5444946864621000E-10, 2.5455916228636002E-10 + 20, 15, 6.0208708189420820E-08,-2.9039462561300382E-07, 2.2927240159561999E-10, 2.2926338492610000E-10 + 20, 16, 2.8414172567249571E-08,-2.7761158323091630E-07, 2.0339971859272001E-10, 2.0208547506292999E-10 + 20, 17,-5.1094548886474557E-07,-3.4292939751667268E-07, 1.7731480238238001E-10, 1.7567791351635001E-10 + 20, 18, 8.9167321908967871E-08, 7.1919662036032088E-07, 1.4970632314231001E-10, 1.5002809557524000E-10 + 20, 19, 1.7333602608012730E-07,-3.2472457615893582E-08, 1.0807864590361000E-10, 1.0850544460582000E-10 + 20, 20,-4.6655014134911748E-07, 1.0747723767163330E-07, 6.1889870171316002E-11, 6.1881103359350995E-11 + 21, 0, 1.2347586822151001E-07, 0.0000000000000000E+00, 5.6092145547486002E-10, 0.0000000000000000E+00 + 21, 1,-1.4966915629923551E-07,-4.2489275995067721E-07, 5.5400146306094002E-10, 5.5948244875631003E-10 + 21, 2,-9.7221100336762922E-08,-5.5509403173538134E-07, 5.5559866749161015E-10, 5.5353506895829009E-10 + 21, 3,-1.5552364970400649E-07, 2.4876536599425881E-07, 5.4445577957016006E-10, 5.4525681021975004E-10 + 21, 4,-4.0097666230990929E-07,-1.3425496908502699E-07, 5.3519179232478005E-10, 5.3652444095995008E-10 + 21, 5, 3.2870712432165809E-07, 1.5936242888488789E-07, 5.1852943652637005E-10, 5.2000039742390003E-10 + 21, 6, 6.9079939553383232E-08, 2.7488485498799867E-07, 4.9981092505811000E-10, 5.0714328273240007E-10 + 21, 7, 2.3320857354459750E-07,-3.0863895600893129E-07, 4.8248339531331006E-10, 4.7975408264332006E-10 + 21, 8, 5.1570907658035643E-08, 3.0886698315441479E-07, 4.5794180553710011E-10, 4.6023982925519998E-10 + 21, 9,-1.8008202114286151E-07,-9.4958964606421974E-08, 4.3188529209723011E-10, 4.3672251035467998E-10 + 21, 10,-2.7938955389263798E-07, 1.1820439495967310E-07, 4.0861710491605002E-10, 4.0802852538335999E-10 + 21, 11,-2.8796500726804299E-07,-1.5823916152800439E-07, 3.8192843799357998E-10, 3.8242625999403001E-10 + 21, 12, 2.5731362205597250E-07, 6.4541115316012470E-08, 3.5479112611681001E-10, 3.5647543697996001E-10 + 21, 13, 8.3708645895450682E-09, 1.0116864535512461E-07, 3.2836010572066999E-10, 3.2931642025641997E-10 + 21, 14, 1.1134161192369960E-07,-3.9795307952092191E-07, 3.0182862717165000E-10, 3.0142690991002002E-10 + 21, 15,-3.6470794043294531E-07,-5.5740534092566497E-07, 2.7431998248750002E-10, 2.7465546207263001E-10 + 21, 16,-2.8556691172751940E-07, 5.5442768395247233E-09, 2.4864100787316002E-10, 2.4739973849435998E-10 + 21, 17,-2.8525352941551792E-07,-3.0837599867540438E-07, 2.2024612655286001E-10, 2.1846283807876001E-10 + 21, 18, 2.8317542188568368E-07, 5.2143158077609181E-07, 1.9151199655527999E-10, 1.9189295501394001E-10 + 21, 19, 1.2069812234460869E-07,-7.8399759298237206E-07, 1.6264619656004000E-10, 1.6424303374405000E-10 + 21, 20,-1.4923345109938631E-07,-3.0137302609478931E-07, 1.1617409847869000E-10, 1.1608810333569000E-10 + 21, 21, 6.9990179109415883E-07, 3.3555798221123139E-07, 6.3777843344463999E-11, 6.3514837029015004E-11 + 22, 0,-1.1264028279608120E-07, 0.0000000000000000E+00, 6.2169060766361004E-10, 0.0000000000000000E+00 + 22, 1,-4.0208656684925192E-07,-1.1826221354554750E-07, 6.1965153364098003E-10, 6.2615300164572006E-10 + 22, 2, 1.0381732434953100E-07, 7.4421453554010241E-08, 6.1619571586371009E-10, 6.1437823674776015E-10 + 22, 3,-3.7677474476696823E-07, 2.0395835626638869E-08, 6.1004989636936010E-10, 6.1121172491412008E-10 + 22, 4, 1.8933663793100171E-07,-3.8145043638466721E-07, 5.9621059310135002E-10, 5.9724675802340004E-10 + 22, 5, 7.3369812805457375E-08, 3.0105913448950862E-08, 5.8380002824166002E-10, 5.8534510562074007E-10 + 22, 6,-2.8170834311131842E-07,-5.2344037555341106E-07, 5.6006184352243010E-10, 5.6811654838524008E-10 + 22, 7, 2.6177231161258758E-07, 5.6295008574609990E-09, 5.4735297948348001E-10, 5.4358506739729000E-10 + 22, 8,-3.7895979428661201E-07,-1.9937087713119211E-07, 5.1845594288649999E-10, 5.2055382967662007E-10 + 22, 9,-1.9429436459151098E-09,-3.0284890893445682E-08, 4.9273472447378003E-10, 4.9842158159693000E-10 + 22, 10,-2.4257445288973869E-07, 2.8902128641733269E-07, 4.6836846575480003E-10, 4.6714925278314001E-10 + 22, 11,-9.3715993038994807E-08,-1.5687894061080690E-07, 4.3998301463812002E-10, 4.4035229977679999E-10 + 22, 12, 3.7559620252873468E-07,-8.9063527407004070E-08, 4.1083535556573011E-10, 4.1329956920568002E-10 + 22, 13,-1.7940313993881491E-07,-1.6316979104796981E-07, 3.8327527467533010E-10, 3.8412764269703002E-10 + 22, 14,-1.7537860033055239E-07,-5.0922917414207737E-07, 3.5463882450144001E-10, 3.5481368759858998E-10 + 22, 15,-4.0193319616745418E-07, 2.8477577528598978E-07, 3.2574983269088001E-10, 3.2589798949287999E-10 + 22, 16,-2.0310502853512480E-07,-2.8750227028928629E-08, 2.9777160193187002E-10, 2.9643383144971998E-10 + 22, 17, 3.4627242633572500E-07, 2.1188159428630770E-07, 2.6924788530568001E-10, 2.6773927227122998E-10 + 22, 18,-1.1210259579021690E-07, 6.0333782848045594E-08, 2.3782341617990002E-10, 2.3795103410093000E-10 + 22, 19, 3.7629022242182029E-07,-4.4970927422294212E-07, 2.0736735553430001E-10, 2.0894773988851999E-10 + 22, 20,-4.3042697908639468E-07, 9.2797413487135024E-08, 1.7661613167423001E-10, 1.7733446902182000E-10 + 22, 21, 2.7695836337858037E-07, 5.6231669757848327E-07, 1.2631250616197999E-10, 1.2595236295029000E-10 + 22, 22,-1.4347583619729300E-07,-3.1326857604805262E-07, 6.9629579226962003E-11, 6.9528756326033998E-11 + 23, 0, 4.7326423023179462E-08, 0.0000000000000000E+00, 6.9514404455145001E-10, 0.0000000000000000E+00 + 23, 1, 2.4374408042259629E-07, 7.3494112778137450E-08, 6.8743829520618005E-10, 6.9375960268094010E-10 + 23, 2, 1.9843225742036479E-07, 4.6791638769017139E-07, 6.9006399300259002E-10, 6.8766546507300006E-10 + 23, 3, 9.9820782624612770E-08,-1.3263849456797489E-07, 6.7785554674036007E-10, 6.7869323133403003E-10 + 23, 4, 4.4503832725168122E-07, 6.6478968884065293E-08, 6.6922174058685010E-10, 6.7067286057720006E-10 + 23, 5,-3.1144276467443448E-07, 1.9519929635273781E-07, 6.5126628872723010E-10, 6.5300927885387002E-10 + 23, 6, 3.6992172443827102E-07,-5.0612553610889074E-07, 6.3188830582267007E-10, 6.4149439834482005E-10 + 23, 7,-2.8487071785549231E-07, 2.9122642661273411E-07, 6.1413147156428008E-10, 6.1044040213774007E-10 + 23, 8,-2.2579336399057979E-07,-2.7709320642165849E-07, 5.8925387976550001E-10, 5.9194463829239007E-10 + 23, 9, 2.7359227298597801E-08, 1.0347913896544291E-07, 5.5785816316293006E-10, 5.6459337667553007E-10 + 23, 10,-9.5790812490446192E-08,-1.4405809987966211E-07, 5.3586962653184008E-10, 5.3508630406274009E-10 + 23, 11, 3.2113423215861948E-07,-3.2797563249356841E-07, 5.0446832601535004E-10, 5.0469166421631008E-10 + 23, 12, 7.3832375707856811E-08,-1.2217476464311631E-08, 4.7417662077922001E-10, 4.7669127302433001E-10 + 23, 13,-2.3140846208712029E-07,-8.4294987746199942E-08, 4.4442532592368999E-10, 4.4544261667375011E-10 + 23, 14,-1.0224728887818180E-07,-3.1770480727318979E-07, 4.1444578033871011E-10, 4.1391850023898002E-10 + 23, 15,-2.3223458029351269E-07, 7.1651363037084832E-07, 3.8305576227990002E-10, 3.8375209257075999E-10 + 23, 16, 4.4498981004519351E-07, 1.0929769880668740E-07, 3.5351146204446999E-10, 3.5173149973274999E-10 + 23, 17, 3.6670408070543000E-07, 7.6096781011156177E-07, 3.2251074277606001E-10, 3.2117057284674000E-10 + 23, 18,-1.7642610439993799E-07,-5.9165257242252639E-07, 2.9126447139445998E-10, 2.9164307450757001E-10 + 23, 19, 4.1926005181402358E-07, 5.6605058404973177E-08, 2.5755571420528000E-10, 2.5985479383052000E-10 + 23, 20,-4.8862524724287533E-07, 4.5045545106160693E-08, 2.2637066371170001E-10, 2.2669893360440999E-10 + 23, 21, 4.9604626390305803E-07, 6.1353338185901423E-07, 1.9428987756624001E-10, 1.9363369423127999E-10 + 23, 22, 1.5353129714946131E-07,-2.0327835563482890E-07, 1.3719233472675999E-10, 1.3690091114198000E-10 + 23, 23,-5.2692420718805256E-07, 2.7878783895039369E-07, 7.5362044269102005E-11, 7.5331515616826995E-11 + 24, 0, 2.6873303650461098E-07, 0.0000000000000000E+00, 7.7182446151493005E-10, 0.0000000000000000E+00 + 24, 1, 1.9057622150479901E-07, 1.6690861715652519E-07, 7.6790671154419009E-10, 7.7503580713019002E-10 + 24, 2, 1.7280977634451560E-08,-1.3850302464473820E-07, 7.6654204116784007E-10, 7.6428108494813003E-10 + 24, 3, 3.2921208881991352E-07,-2.9176290248854039E-07, 7.5830076837382007E-10, 7.5977830689642005E-10 + 24, 4, 5.9218617620265157E-08, 1.1739532630109240E-07, 7.4513892186620003E-10, 7.4698267772589002E-10 + 24, 5,-3.5012008481587548E-07,-1.5733433810897421E-07, 7.3135906527225008E-10, 7.3413088551381010E-10 + 24, 6, 3.1642695265316869E-07, 2.9276330519769568E-08, 7.0655222559914004E-10, 7.1720256827696007E-10 + 24, 7,-3.9264498823450741E-07, 5.1879797911087025E-07, 6.9432846044614006E-10, 6.9002327097593007E-10 + 24, 8, 1.8933598874193869E-07, 1.1078479731924260E-07, 6.6276844743168009E-10, 6.6609220109415005E-10 + 24, 9, 1.2527409470495221E-07, 3.2842814568059190E-07, 6.3575495049220010E-10, 6.4312111341184006E-10 + 24, 10,-1.3450041125559309E-07,-3.0140135882258529E-07, 6.0736843489246001E-10, 6.0679577452778007E-10 + 24, 11, 1.0843022480946701E-08, 3.0915504653432879E-07, 5.7875627748271005E-10, 5.7925345837186004E-10 + 24, 12,-3.1255874794963632E-07,-3.0429281336725132E-07, 5.4378373731387000E-10, 5.4609935260132008E-10 + 24, 13, 7.5599798100225250E-08,-2.7849631860020731E-08, 5.1360702190987009E-10, 5.1495325892458001E-10 + 24, 14, 2.6270393549553840E-08, 3.9848536131460611E-07, 4.8077142508029000E-10, 4.8094993769846003E-10 + 24, 15, 2.1284441845550539E-07,-4.2522142616756123E-08, 4.4779655643690010E-10, 4.4854152251518998E-10 + 24, 16, 6.2126031993091466E-07, 2.8516896440125381E-07, 4.1609027341372002E-10, 4.1444023338576999E-10 + 24, 17,-1.0668238261122960E-07,-1.5743112235114869E-08, 3.8298057311606010E-10, 3.8170659501541001E-10 + 24, 18, 3.1846210628267689E-07,-5.7222896549097009E-07, 3.4922869260153002E-10, 3.4934033965158002E-10 + 24, 19,-2.5065325932672343E-07, 4.2087828662401751E-07, 3.1565513488044000E-10, 3.1754515510757999E-10 + 24, 20,-9.3603719702306670E-08,-1.4150417744732959E-07, 2.8066997368318002E-10, 2.8152979527744002E-10 + 24, 21, 5.0245562931673793E-09, 4.9620199514226188E-07, 2.4723388648750000E-10, 2.4604678129472002E-10 + 24, 22,-1.6939863171226771E-07,-6.9209106756932092E-07, 2.1099215652271000E-10, 2.1020286601959999E-10 + 24, 23,-5.8779889641222291E-07, 4.1208495271270067E-08, 1.4599724461299000E-10, 1.4602346682201001E-10 + 24, 24, 2.8572403319338679E-07, 1.0611983382256000E-07, 7.7481299402831007E-11, 7.7794732165366008E-11 + 25, 0,-1.3915637435630249E-07, 0.0000000000000000E+00, 8.6195340975259007E-10, 0.0000000000000000E+00 + 25, 1,-7.7127738215862221E-08,-1.7543650003502259E-08, 8.5318896866255006E-10, 8.6097616139176004E-10 + 25, 2,-1.7403806696811899E-07,-3.5958046277900300E-07, 8.5662046572052011E-10, 8.5392874905001003E-10 + 25, 3, 1.0709464053995960E-07,-1.4852699810844249E-07, 8.4451062522032008E-10, 8.4495247223287005E-10 + 25, 4,-6.1577275010244243E-07, 1.6531831359390281E-07, 8.3429405274330011E-10, 8.3613264241993008E-10 + 25, 5, 2.7870987142976032E-07,-3.2513243257401430E-07, 8.1713461927319006E-10, 8.1920410443996006E-10 + 25, 6,-1.5069102202669251E-07, 3.5398705137297831E-07, 7.9394057467795008E-10, 8.0646218338521010E-10 + 25, 7, 1.2745235340382241E-07, 2.5722461498610012E-07, 7.7887508506762008E-10, 7.7358362605685015E-10 + 25, 8, 2.5188727311854308E-07, 1.4987704009305059E-07, 7.5013837707236015E-10, 7.5361878626318006E-10 + 25, 9, 8.8548350659178343E-08, 2.0918083387298929E-07, 7.1687764466770011E-10, 7.2533327616075001E-10 + 25, 10, 2.0459340707431701E-07,-2.5755712172913070E-07, 6.9268878214916015E-10, 6.9200954567793005E-10 + 25, 11,-4.0208525901293581E-07, 4.2193640157022459E-07, 6.5725104115827006E-10, 6.5738309288535008E-10 + 25, 12,-3.4108004488017230E-07,-2.2236094344922479E-07, 6.2550862951063007E-10, 6.2815016437989005E-10 + 25, 13,-3.3913589664341351E-09, 2.7928595832259351E-07, 5.8930761667783002E-10, 5.8977086458119003E-10 + 25, 14,-6.0081500302783250E-08, 2.6920677556667728E-07, 5.5677786690828015E-10, 5.5647262099777000E-10 + 25, 15, 4.3084430524138863E-07,-1.7103728129146550E-07, 5.1995466644648003E-10, 5.2113781641361001E-10 + 25, 16,-7.2389572160777240E-08, 2.4780679724823610E-07, 4.8676251842699999E-10, 4.8479282742677008E-10 + 25, 17,-4.0812396698157091E-07,-7.9124914881614433E-07, 4.5072257745514998E-10, 4.4979404989055010E-10 + 25, 18, 1.3039868082469749E-07, 3.7143279380405901E-08, 4.1472252015723002E-10, 4.1528899881160002E-10 + 25, 19,-6.7379262122205607E-07,-3.2745841267601822E-09, 3.7867016962360002E-10, 3.8072410364819998E-10 + 25, 20, 5.7252513267242740E-07, 6.8581143753309572E-08, 3.4448327501239002E-10, 3.4496723848186002E-10 + 25, 21,-6.7333893396917305E-07, 1.8939175435100679E-07, 3.0699475037708001E-10, 3.0691003959778999E-10 + 25, 22, 2.9662492388194308E-07,-4.3844233143149069E-07, 2.6937606789095002E-10, 2.6831876012246002E-10 + 25, 23,-4.7106093096747168E-07, 4.7197829147459941E-07, 2.3104022556810000E-10, 2.3092676676266999E-10 + 25, 24, 3.3585277049000689E-07, 7.1057634707613332E-08, 1.6316345114651999E-10, 1.6303192085638999E-10 + 25, 25, 1.0811211693401540E-07,-2.9679761935162980E-07, 1.1259297247996000E-10, 1.1203314497462000E-10 + 26, 0,-1.7628448725189109E-07, 0.0000000000000000E+00, 9.5646179902204006E-10, 0.0000000000000000E+00 + 26, 1,-1.9213755004800671E-07,-3.8315999458061602E-07, 9.5393576191029013E-10, 9.6246263298386002E-10 + 26, 2,-2.0175674235327581E-07, 1.4205464154316201E-08, 9.5104253749955998E-10, 9.4875807231161001E-10 + 26, 3,-1.4909954219721411E-07, 1.2804480855375901E-07, 9.4432404533318011E-10, 9.4632587013837014E-10 + 26, 4,-3.6897114728500608E-07, 2.1670066344322219E-07, 9.2833128156653011E-10, 9.3137878050170002E-10 + 26, 5, 2.0844964305891050E-07, 2.2188350902353279E-07, 9.1576821157264004E-10, 9.1881776625466012E-10 + 26, 6,-3.2567066782868078E-07, 6.5494767739468042E-08, 8.8770564922389013E-10, 9.0247779114555007E-10 + 26, 7, 1.1641740455968710E-07,-2.3553518937132420E-07, 8.7619089318085003E-10, 8.7064543605862008E-10 + 26, 8, 8.6298428878060334E-08, 2.0973525309858609E-08, 8.4330800855063004E-10, 8.4754326992811008E-10 + 26, 9,-7.2025186638855130E-08,-5.0879344685424231E-07, 8.1148208572013006E-10, 8.2152218295000003E-10 + 26, 10, 1.4853026076295599E-07, 3.8012335713704092E-08, 7.8343084717166002E-10, 7.8257018538250001E-10 + 26, 11,-4.7167856753693068E-07,-1.9898237971716059E-07, 7.5033330375978006E-10, 7.5005160937906007E-10 + 26, 12, 2.2527623574358020E-07,-6.9385296018421442E-08, 7.1113488614674008E-10, 7.1440532141405001E-10 + 26, 13, 5.1145042075150622E-08,-1.0433026848759240E-08, 6.7797537840471008E-10, 6.7969053771201003E-10 + 26, 14, 1.2480011835258409E-07,-8.9757900507323339E-08, 6.3854884342428003E-10, 6.3804149624024002E-10 + 26, 15, 9.0209718719096993E-08,-5.4776152943267738E-08, 6.0310513679445009E-10, 6.0399924463423007E-10 + 26, 16,-5.0820149849512781E-07,-8.4248120896246461E-08, 5.6541225832593001E-10, 5.6317442114490004E-10 + 26, 17,-2.4657849080845568E-07,-3.7013988183926381E-08, 5.2829209190483001E-10, 5.2626042462608999E-10 + 26, 18,-2.3290580440167000E-07, 5.3888816219307701E-07, 4.8910563301772005E-10, 4.8906483677148007E-10 + 26, 19,-2.5970273573784249E-08,-1.6653485548744791E-07, 4.4985121425657999E-10, 4.5273714064118001E-10 + 26, 20, 1.5281466506169339E-07, 3.5290024176266420E-07, 4.1259624893995010E-10, 4.1349844288603998E-10 + 26, 21,-3.1201209295350659E-07,-5.5170201263527412E-07, 3.7539326322068999E-10, 3.7520822414308001E-10 + 26, 22, 4.0844874037237719E-07, 4.5750616882189957E-08, 3.3456391414699010E-10, 3.3354784033926998E-10 + 26, 23,-2.4275552862587361E-07, 2.2709480739465239E-07, 2.9386651172722002E-10, 2.9348739633092002E-10 + 26, 24, 7.3201075677302809E-07,-2.8419292894155149E-09, 2.5267197253394001E-10, 2.5303632791328003E-10 + 26, 25, 2.3151452106338080E-07,-4.4013918971703292E-07, 1.8075273296128000E-10, 1.8214235046023001E-10 + 26, 26,-1.1007952809534780E-07, 4.4794368613723358E-07, 1.0243326452524000E-10, 1.0216275506207000E-10 + 27, 0,-2.2429133670940241E-07, 0.0000000000000000E+00, 1.0688135493884001E-09, 0.0000000000000000E+00 + 27, 1, 5.6344813457961528E-08,-4.2301208758475378E-08, 1.0602470832988000E-09, 1.0683181594241999E-09 + 27, 2,-6.5365457782954078E-10, 2.6901356144063480E-07, 1.0637220835864999E-09, 1.0601972347623999E-09 + 27, 3, 1.2627092149005500E-07, 3.1206623710871861E-07, 1.0509895742518999E-09, 1.0510936256283000E-09 + 27, 4, 3.4424611315322562E-07, 8.5273684327321170E-08, 1.0400982743925000E-09, 1.0427789970397000E-09 + 27, 5, 1.0222231504240270E-07, 2.8166455549002169E-07, 1.0208581479378000E-09, 1.0239737794769001E-09 + 27, 6, 2.6324849166294649E-07,-3.2786198801969991E-07, 9.9729508036308003E-10, 1.0134023842680000E-09 + 27, 7,-9.7211844450005550E-10,-1.1172236487931330E-07, 9.8075960393982001E-10, 9.7517090158288013E-10 + 27, 8,-3.4547432643060329E-07, 6.5907866839411809E-08, 9.5051781070319007E-10, 9.5510104009309010E-10 + 27, 9,-5.6074328514253417E-08,-3.8503746456282642E-07, 9.1430290798289006E-10, 9.2557478196193007E-10 + 27, 10,-3.0969727349976250E-07, 3.6473955062594962E-08, 8.8763013121982011E-10, 8.8667550402064009E-10 + 27, 11,-2.0254335758159719E-07,-2.1423176515356401E-07, 8.5014752295159003E-10, 8.5057628394169010E-10 + 27, 12, 6.0235142746719770E-08,-8.8460963500726847E-09, 8.1196405789325003E-10, 8.1510451808690007E-10 + 27, 13,-7.4459935135365112E-08,-1.0590711498148070E-07, 7.7323646383894011E-10, 7.7401466084491007E-10 + 27, 14, 1.7173555188447300E-07,-4.1929659570348682E-08, 7.3548128924106002E-10, 7.3540610428632011E-10 + 27, 15,-4.5952114701047252E-08, 8.8451767953049423E-08, 6.9161061131437002E-10, 6.9314612367975009E-10 + 27, 16,-1.1942685337678140E-07,-2.2375526433632609E-07, 6.5652105946070015E-10, 6.5394948423620002E-10 + 27, 17, 2.4581814647318210E-07, 3.4807209403586919E-07, 6.1311019500296007E-10, 6.1213461204599009E-10 + 27, 18,-2.0923914984464690E-08, 1.5127172664997909E-07, 5.7337101791642008E-10, 5.7291214942788001E-10 + 27, 19, 7.5026181480599506E-07, 1.2894024808243501E-07, 5.3013677185143005E-10, 5.3336161989875008E-10 + 27, 20,-2.5269557479531271E-07,-5.4762440538989093E-08, 4.9066690405182002E-10, 4.9167385355396000E-10 + 27, 21, 2.5817973151245719E-07,-3.1039157162723509E-07, 4.5106100732553999E-10, 4.4960237268220002E-10 + 27, 22,-1.9717193142012019E-07, 3.9791824063853078E-07, 4.1012278999698001E-10, 4.0910415240501010E-10 + 27, 23, 1.2012853555960479E-07,-2.7057833944663443E-07, 3.6599245562414011E-10, 3.6544114400724002E-10 + 27, 24, 1.6193138480660450E-07, 4.6200158477059414E-09, 3.2142993958500999E-10, 3.2101346973354001E-10 + 27, 25,-1.5582978599656841E-07,-5.7300191246279213E-07, 2.7824565374532001E-10, 2.7818286189845999E-10 + 27, 26,-2.5066768488364009E-07, 3.5159123462776440E-07, 1.9575759027658001E-10, 1.9579089507761000E-10 + 27, 27, 4.9198392164878586E-07,-8.9847065707103033E-08, 1.1208664642179001E-10, 1.1119147652428000E-10 + 28, 0, 3.7688556434609083E-08, 0.0000000000000000E+00, 1.1888554271315999E-09, 0.0000000000000000E+00 + 28, 1, 1.5783700928634909E-07, 1.0124148940442520E-07, 1.1820781772946000E-09, 1.1929555320500000E-09 + 28, 2, 3.1017655862825947E-07, 2.4849703134892952E-07, 1.1829012343091999E-09, 1.1805570026167000E-09 + 28, 3, 1.9313510743590381E-07,-2.7948778667318201E-07, 1.1727549826926000E-09, 1.1749988807797000E-09 + 28, 4, 4.0313724493949629E-07, 1.3956437835763860E-08, 1.1592924430640000E-09, 1.1619190536040999E-09 + 28, 5,-4.8400167777080632E-08,-3.6928288502624842E-08, 1.1427210127532001E-09, 1.1469585357633001E-09 + 28, 6, 4.5375184117556701E-07,-2.7356671662511559E-07, 1.1132589984090000E-09, 1.1324303814092000E-09 + 28, 7,-1.6823684799278519E-07, 2.0162811309386700E-07, 1.1025843788036001E-09, 1.0962536060899999E-09 + 28, 8,-2.4710103415387870E-07, 4.2320818316738424E-09, 1.0656528737516000E-09, 1.0712311667570000E-09 + 28, 9, 1.0710413315087720E-07, 4.2063510832961999E-07, 1.0330440157332001E-09, 1.0443155342090001E-09 + 28, 10,-8.2870261985355222E-08, 1.5609810208320571E-08, 1.0009367938067001E-09, 1.0008198591898999E-09 + 28, 11, 2.2512128014213121E-07, 2.7273955283408457E-07, 9.6472099955680000E-10, 9.6411046443387014E-10 + 28, 12, 8.9524747741376861E-08,-2.0457909203691569E-08, 9.2214004215855010E-10, 9.2600197880948003E-10 + 28, 13,-1.2787142340077051E-07, 4.2619780444182797E-08, 8.8202561545839011E-10, 8.8399409953010002E-10 + 28, 14, 8.9051930235575579E-08, 1.2467634316901960E-07, 8.4011535643316008E-10, 8.3978871109370006E-10 + 28, 15,-7.9032732095102051E-08, 1.3383638181939430E-07, 7.9702276014992007E-10, 7.9856153991781011E-10 + 28, 16, 1.7615849583248601E-07, 5.3584124607366892E-08, 7.5343860356357008E-10, 7.5127731871870009E-10 + 28, 17, 1.8637754507548990E-07, 3.6837259317905662E-07, 7.1261298317508007E-10, 7.1142659302964009E-10 + 28, 18, 2.5127844343473148E-07,-3.5746993951465539E-07, 6.6584527636534005E-10, 6.6562334706439004E-10 + 28, 19, 3.1908355132650678E-07, 2.3285527480023169E-08, 6.2211795888892007E-10, 6.2517618690299008E-10 + 28, 20,-2.9898302714105848E-07,-3.9848909033499608E-07, 5.7856572425524006E-10, 5.7943520893803004E-10 + 28, 21, 2.2935452901297270E-07, 1.3419423339194680E-07, 5.3584146956246004E-10, 5.3465666365097009E-10 + 28, 22,-3.6507274474573752E-07, 1.0399280108543280E-07, 4.9128373725735002E-10, 4.9080511616411003E-10 + 28, 23, 3.9683452222884022E-07, 9.8249634583056008E-08, 4.4683410322354002E-10, 4.4723368747057011E-10 + 28, 24,-2.7695608240874400E-07, 2.0151109007883150E-07, 4.0007687899146002E-10, 3.9842232273392002E-10 + 28, 25,-6.1515040223357603E-08,-1.0472751263353350E-07, 3.5234393073152010E-10, 3.5230593217385010E-10 + 28, 26,-3.2090104339849368E-07, 5.5288152018396721E-07, 3.0367314820848010E-10, 3.0375180079987002E-10 + 28, 27, 5.5931795695475683E-08, 3.5884049270198863E-08, 2.0719243549205999E-10, 2.0685862082665000E-10 + 28, 28,-2.0401164017886030E-07,-5.6962748687958568E-07, 1.1919183251375999E-10, 1.1977316838928000E-10 + 29, 0, 9.3702846930364048E-08, 0.0000000000000000E+00, 1.3269895580359001E-09, 0.0000000000000000E+00 + 29, 1, 7.1348817189583361E-08, 2.0184784435221219E-07, 1.3155461535670000E-09, 1.3251171354550999E-09 + 29, 2, 8.0655382026096740E-08, 1.7215589308587210E-09, 1.3212209487921000E-09, 1.3177394521570001E-09 + 29, 3, 9.6836190539040183E-08,-3.0816637609178689E-07, 1.3065372768155000E-09, 1.3077588404191000E-09 + 29, 4,-1.8680363369921000E-07, 4.9832426947715548E-08, 1.2951001636531000E-09, 1.2987855613128999E-09 + 29, 5,-1.2983945214640070E-07,-1.6917412913966779E-07, 1.2753942240684999E-09, 1.2799248359441001E-09 + 29, 6,-9.0422606023952042E-08, 9.4101032299517958E-09, 1.2467736070723001E-09, 1.2681568018185000E-09 + 29, 7,-2.5192143983259911E-07, 1.7464531562866890E-07, 1.2351661514365999E-09, 1.2269344661147001E-09 + 29, 8,-4.2698625836007993E-08,-8.9228824142963896E-09, 1.1979545264541000E-09, 1.2048748008240000E-09 + 29, 9, 1.0183111147610990E-07, 3.2809139697703049E-07, 1.1597588921124999E-09, 1.1738330374847001E-09 + 29, 10, 1.3883747261810520E-07,-3.1141217839033688E-08, 1.1315980039220000E-09, 1.1312136461913000E-09 + 29, 11, 2.9691958578528659E-07, 4.5742929551007871E-08, 1.0897283264328001E-09, 1.0890766484020999E-09 + 29, 12,-1.3427538075769189E-07, 1.0946619933814260E-07, 1.0477105685632999E-09, 1.0506675691901000E-09 + 29, 13, 1.7506798719067500E-07, 1.7575001570676170E-08, 1.0039468704104000E-09, 1.0046035654782999E-09 + 29, 14, 7.7837043194376414E-08, 1.3395399821071559E-07, 9.5959851019895002E-10, 9.5870365130894005E-10 + 29, 15, 5.2109845392902077E-08, 1.3384672333717210E-07, 9.1214098941280008E-10, 9.1376609155229003E-10 + 29, 16, 1.4794412969211669E-07, 1.4792180554770010E-08, 8.6775123351595004E-10, 8.6517475737255015E-10 + 29, 17,-3.3745554673021112E-08,-7.0045295120893890E-08, 8.1879191105107002E-10, 8.1817758566270015E-10 + 29, 18, 2.1418855086524331E-08,-9.4109030401293733E-08, 7.7463151777979015E-10, 7.7425022762524002E-10 + 29, 19,-1.9709952597879409E-07,-1.3505178388051149E-08, 7.2280809848853001E-10, 7.2639905013149008E-10 + 29, 20,-1.3550431404915160E-07,-8.6822470688634973E-08, 6.7899196912890006E-10, 6.8018441254815006E-10 + 29, 21,-1.5030267724421009E-07, 2.6509414861913419E-07, 6.3106634313102007E-10, 6.3061530257303007E-10 + 29, 22, 7.6578317345497392E-08,-2.0680632146692670E-08, 5.8426835986540006E-10, 5.8384046818594003E-10 + 29, 23, 9.6924765746032127E-08, 3.1356707917145092E-07, 5.3670014366337006E-10, 5.3621904299521004E-10 + 29, 24,-1.4005500648212799E-07,-3.0469382582476341E-07, 4.8959770155743999E-10, 4.8737437835855004E-10 + 29, 25, 1.3633949923828659E-07, 2.7162645618812211E-07, 4.3895921296437011E-10, 4.3882983204924002E-10 + 29, 26, 1.6797261963368699E-08, 2.6538690568921410E-07, 3.8508550765224999E-10, 3.8510496209569001E-10 + 29, 27, 4.7933639268462252E-07, 1.7349828302600341E-08, 3.3003016303705999E-10, 3.2953878250756998E-10 + 29, 28,-6.3045061240640739E-08,-2.1918930385312251E-07, 2.2589465685087999E-10, 2.2557285739724001E-10 + 29, 29,-1.3377254772957059E-07, 1.7717530296083961E-07, 1.3447847912214001E-10, 1.3335017312970001E-10 + 30, 0, 1.1145099036532529E-08, 0.0000000000000000E+00, 1.4728978136750000E-09, 0.0000000000000000E+00 + 30, 1,-1.4654000712290799E-07,-1.1060675702898149E-09, 1.4709044466601000E-09, 1.4823720918823001E-09 + 30, 2,-1.8998886339668401E-07,-4.2452925690369157E-08, 1.4671155028397001E-09, 1.4638162238225000E-09 + 30, 3,-3.5270525533382182E-08, 8.3431426027651058E-09, 1.4608721979792001E-09, 1.4632320295891000E-09 + 30, 4,-2.5787459990413612E-07, 2.1727062750581659E-07, 1.4407045088016000E-09, 1.4453987585056000E-09 + 30, 5,-9.1733739134503762E-08, 1.4967249746880319E-07, 1.4283622679380999E-09, 1.4328344220871001E-09 + 30, 6,-1.3497060122031410E-07, 3.6547714193666440E-07, 1.3921557620139000E-09, 1.4156952846847000E-09 + 30, 7,-8.1965937969819072E-08, 3.0984720440641203E-08, 1.3845794726063999E-09, 1.3763428270698000E-09 + 30, 8, 5.5483344343330702E-08, 9.4218169495485086E-08, 1.3441882432999001E-09, 1.3503448635867001E-09 + 30, 9,-7.5045838407190513E-08,-1.4151056355603200E-08, 1.3046072862255000E-09, 1.3207016246568000E-09 + 30, 10, 5.6994847624024508E-08,-2.4088907113550531E-07, 1.2743909196812000E-09, 1.2735202280074000E-09 + 30, 11, 1.2792396133672280E-07,-1.2025161217794699E-07, 1.2331078806424000E-09, 1.2314816930304000E-09 + 30, 12,-2.9831960575819447E-07, 1.2135602798176551E-07, 1.1846102979878999E-09, 1.1890826119050001E-09 + 30, 13, 1.0057243261066830E-07, 5.6318659207429081E-08, 1.1406458849735000E-09, 1.1421407233890000E-09 + 30, 14,-1.3508582102988071E-07, 4.3938012671177198E-08, 1.0915163435781999E-09, 1.0920306711788001E-09 + 30, 15, 1.2018251166455231E-07,-2.5325824294089910E-08, 1.0420760889225000E-09, 1.0439208944607000E-09 + 30, 16, 7.1709898974243120E-08, 5.8193859688772242E-08, 9.9451736800682008E-10, 9.9171744130412006E-10 + 30, 17,-1.4436063445509550E-07,-1.7785010375395631E-07, 9.4275577413583000E-10, 9.4174197623852006E-10 + 30, 18,-7.2068360221388700E-08, 1.1056105864779130E-08, 8.9190955484790005E-10, 8.9122552472219010E-10 + 30, 19,-2.5431419252392298E-07, 6.2142940580670450E-08, 8.4167431328825006E-10, 8.4437605404986006E-10 + 30, 20, 1.3779199405606101E-07, 1.7379509825482029E-07, 7.8866736332075002E-10, 7.8986435597044009E-10 + 30, 21,-2.6415079885543231E-08, 7.8131348871576513E-08, 7.4174231027921008E-10, 7.4020877325964011E-10 + 30, 22, 2.0178394500258259E-07,-2.0456347280951610E-08, 6.8885380042866001E-10, 6.8750924497837004E-10 + 30, 23,-1.5461409754925761E-07, 9.5584411231037844E-08, 6.3768823637270002E-10, 6.3775682826442010E-10 + 30, 24, 9.0729028915191664E-08,-2.3643420072473279E-07, 5.8713053094652003E-10, 5.8486705790154008E-10 + 30, 25,-1.5661849280927401E-07, 2.7613822597114260E-07, 5.3593035466434009E-10, 5.3679032600515004E-10 + 30, 26, 8.1106286134173940E-08,-3.3710802783892099E-07, 4.7829512025673005E-10, 4.7858912337595001E-10 + 30, 27, 2.8782192752951719E-07,-2.4198363506992220E-07, 4.2217257189487011E-10, 4.2041180763929001E-10 + 30, 28,-2.6001883652509370E-07,-3.0907353476336140E-07, 3.5896293857996998E-10, 3.5924057352975002E-10 + 30, 29, 2.6054562193385649E-08, 7.8234963744760744E-08, 2.4506604590468999E-10, 2.4482135722474002E-10 + 30, 30, 8.1528975676899342E-08,-1.7506214508672120E-08, 1.4465517791511999E-10, 1.4484225410005001E-10 + 31, 0, 1.1287170200725720E-07, 0.0000000000000000E+00, 1.6471924768841000E-09, 0.0000000000000000E+00 + 31, 1,-6.1861543402132000E-08,-8.2210099378316832E-08, 1.6333504227131000E-09, 1.6453070327146000E-09 + 31, 2, 8.5025844206685978E-08, 1.3393507846428070E-08, 1.6417410137102999E-09, 1.6369777839567001E-09 + 31, 3,-3.2918743216093827E-08, 5.3573643937370072E-08, 1.6239900542143999E-09, 1.6249407227327000E-09 + 31, 4,-3.0693415303256343E-08, 7.5885563450514509E-09, 1.6137969354955999E-09, 1.6183516861331000E-09 + 31, 5, 9.5050316393459869E-08, 1.7269202062704191E-07, 1.5893905730041000E-09, 1.5948727330663001E-09 + 31, 6, 6.2956324201030600E-08, 1.1345288369187270E-07, 1.5608907033138001E-09, 1.5885858896495001E-09 + 31, 7, 2.3776715964544131E-07,-1.4083684406008409E-07, 1.5461803301184001E-09, 1.5362354795595999E-09 + 31, 8,-1.9589863816685069E-09, 5.8341997617833577E-08, 1.5096117307078000E-09, 1.5185677410121999E-09 + 31, 9,-1.3343953784693900E-07,-3.7881113773361707E-08, 1.4643730487618001E-09, 1.4815367210514999E-09 + 31, 10,-9.2896662432773253E-08,-2.1821071986276781E-07, 1.4350630809088000E-09, 1.4345169617844000E-09 + 31, 11,-1.8542966166663171E-07, 1.9907916992782680E-07, 1.3895376811342999E-09, 1.3901197588579999E-09 + 31, 12,-8.7291074490033562E-08,-4.1802616371664442E-08, 1.3406392705659000E-09, 1.3447036962871000E-09 + 31, 13, 3.8305025124044411E-08, 1.4128537412595329E-07, 1.2919809789707999E-09, 1.2942278761715999E-09 + 31, 14,-2.8077518866457709E-08,-1.1716735262040720E-07, 1.2430213235248000E-09, 1.2412992477176000E-09 + 31, 15, 8.7898315698478590E-08, 1.0936478913889840E-08, 1.1868873743987001E-09, 1.1896042643973999E-09 + 31, 16,-1.3012232974639761E-07, 1.1169673736089499E-09, 1.1374475459075999E-09, 1.1334609656880999E-09 + 31, 17,-4.4201481315247582E-08, 5.1536615322554183E-08, 1.0817462629711000E-09, 1.0803589497200999E-09 + 31, 18, 4.1257417256225502E-08, 1.5114884345532379E-07, 1.0258649297142999E-09, 1.0256545091810001E-09 + 31, 19, 7.4906796232949318E-08, 9.4773927006365372E-08, 9.6952871392541011E-10, 9.7464869858914000E-10 + 31, 20, 4.1356034912852518E-07, 1.3257915418875509E-07, 9.1817444528070012E-10, 9.1881895474822011E-10 + 31, 21,-1.1142387105448830E-08, 2.7003529896595820E-08, 8.6184780344969005E-10, 8.6017222759691009E-10 + 31, 22, 1.2393827079206459E-07,-1.5723930552253350E-07, 8.0935089222459011E-10, 8.0851273933216005E-10 + 31, 23,-1.3879734508950121E-07,-1.6740283644819311E-07, 7.5096269053905008E-10, 7.5138268561116003E-10 + 31, 24, 2.9885747457820662E-08, 1.1444246279427569E-07, 6.9895322200752001E-10, 6.9581473424738010E-10 + 31, 25, 2.6975995832007031E-09,-8.8502450553843264E-09, 6.4248636797120015E-10, 6.4212503659584010E-10 + 31, 26, 1.7125913226196099E-07,-4.8176107290494673E-08, 5.8456147791565000E-10, 5.8464977496990001E-10 + 31, 27,-1.0794395833441440E-07,-1.2083786428134781E-07, 5.2323267220033007E-10, 5.2279715720796006E-10 + 31, 28,-3.9968578077814322E-07,-1.3145215370927559E-07, 4.6160336598058998E-10, 4.6071039856543010E-10 + 31, 29,-5.1366359009729553E-08, 2.1177654546929961E-07, 3.9436388327854001E-10, 3.9412131292079002E-10 + 31, 30, 4.8898788635706623E-08, 2.0043669526703350E-07, 2.6880779865564999E-10, 2.6960192930992002E-10 + 31, 31,-1.8770411750075860E-07, 1.0366944991723379E-07, 1.6362851055910000E-10, 1.6474116873511000E-10 + 32, 0, 1.4199164415066540E-07, 0.0000000000000000E+00, 1.8298436720472000E-09, 0.0000000000000000E+00 + 32, 1,-6.9008301003050204E-08,-1.8450978973317849E-07, 1.8260817903867000E-09, 1.8382254288500000E-09 + 32, 2, 1.3571452247432001E-07, 1.8175786246893730E-07, 1.8233966952835000E-09, 1.8200173262958000E-09 + 32, 3,-1.5237958885822189E-07, 3.8103547315772572E-08, 1.8160345621225000E-09, 1.8174848646909999E-09 + 32, 4, 2.0243538484911449E-07, 6.5322199228227312E-08, 1.7941587633740000E-09, 1.8008880242029000E-09 + 32, 5, 9.2088695039827188E-08,-6.6707883167499961E-08, 1.7791935573582999E-09, 1.7869448944820001E-09 + 32, 6, 4.2216120461878249E-09,-2.0955403475299489E-07, 1.7390225177629001E-09, 1.7698601693920000E-09 + 32, 7, 1.5998118485636949E-07,-1.7096554939386519E-07, 1.7357410653359000E-09, 1.7253415786337000E-09 + 32, 8,-2.3903048512526019E-07,-3.2727968772223782E-08, 1.6869215294400000E-09, 1.6959189281984001E-09 + 32, 9,-7.2496327817676861E-09,-9.8134727164149334E-08, 1.6486842367227001E-09, 1.6679677449938999E-09 + 32, 10,-1.8918870004901241E-08, 1.1086350047943871E-07, 1.6098861804508000E-09, 1.6103471282871000E-09 + 32, 11,-5.6222017412753783E-08, 8.3747710742040034E-08, 1.5689713960391001E-09, 1.5673754359267000E-09 + 32, 12, 6.8589675912041462E-08, 2.2521676724264161E-08, 1.5144005307817001E-09, 1.5181474176307999E-09 + 32, 13, 4.9231679841695092E-08, 1.6695087709746542E-08, 1.4623672256946999E-09, 1.4649511084137000E-09 + 32, 14, 1.0886511036796810E-08,-1.8631661731997840E-07, 1.4091023929133001E-09, 1.4092468004167000E-09 + 32, 15, 5.1757708843862072E-08, 7.4475120792676999E-08, 1.3508289303736999E-09, 1.3541419312219000E-09 + 32, 16,-1.8517999711912979E-07,-4.9124912903018692E-08, 1.2961065415977001E-09, 1.2927338519270001E-09 + 32, 17, 1.0058940344279920E-08,-2.1331055075556621E-08, 1.2372418011536999E-09, 1.2363661840900000E-09 + 32, 18, 7.7932779460410839E-08, 1.9222315559475072E-09, 1.1781557689048999E-09, 1.1775141435250999E-09 + 32, 19, 2.1076515801856930E-07,-1.1451202158152390E-07, 1.1165212450293999E-09, 1.1198523417108001E-09 + 32, 20, 1.6275293059884601E-07, 3.6307007193329441E-08, 1.0597530451814001E-09, 1.0613237913239999E-09 + 32, 21,-8.7015122326974829E-08,-2.2006564172550320E-07, 1.0023970797554000E-09, 1.0003848408843000E-09 + 32, 22,-1.3141983548181709E-07,-6.5706171114170411E-08, 9.4062246686334006E-10, 9.3931166653792005E-10 + 32, 23,-2.8311552278668531E-08, 1.3422778859965351E-07, 8.8318230012003006E-10, 8.8361103445924005E-10 + 32, 24,-1.4042290961879799E-08, 1.5566011198718880E-07, 8.2369476260654008E-10, 8.1847544970919002E-10 + 32, 25, 9.2227713951455806E-08,-1.2956988000446940E-07, 7.6346105341627009E-10, 7.6423633211170011E-10 + 32, 26, 2.5640567067651571E-08,-6.8537066416396170E-08, 7.0065449110951007E-10, 7.0166199959709010E-10 + 32, 27,-6.2037265088381082E-08,-1.0238839881171219E-07, 6.4008417010413004E-10, 6.3969660546482003E-10 + 32, 28,-2.5102619712448360E-08, 2.7652198344327742E-07, 5.7198148303264006E-10, 5.7151854423029008E-10 + 32, 29, 6.4823341316124624E-08, 1.8604827587785851E-07, 5.0707742720734002E-10, 5.0516076262031009E-10 + 32, 30, 1.5376018182582620E-07,-8.0341209408402635E-08, 4.2881745501903002E-10, 4.3247744620836010E-10 + 32, 31, 1.1600838970022609E-07, 4.0321705631569283E-08, 2.9173442168702998E-10, 2.9412068488415999E-10 + 32, 32, 9.8814690692256247E-08, 7.7107073389659512E-08, 1.7521546474470999E-10, 1.7581011302767999E-10 + 33, 0,-9.3041161995794731E-08, 0.0000000000000000E+00, 2.0420957927936998E-09, 0.0000000000000000E+00 + 33, 1,-3.9155619476503921E-08,-9.5472951755738484E-08, 2.0314257436830001E-09, 2.0458872611762000E-09 + 33, 2, 6.0495095617281389E-08, 2.0388444611356801E-07, 2.0360944782539002E-09, 2.0312744451671000E-09 + 33, 3,-1.2635136701065109E-07,-1.1722570873242300E-07, 2.0204967038969000E-09, 2.0236256121439000E-09 + 33, 4, 9.3039604687992252E-08, 3.0724477493342641E-08, 2.0067193482865000E-09, 2.0114907630335001E-09 + 33, 5,-1.5065698613803810E-07, 5.6365977329365531E-08, 1.9835893857804001E-09, 1.9897633689870002E-09 + 33, 6,-1.4655889148091281E-07,-2.5737195557073528E-07, 1.9474165826865002E-09, 1.9822769220588999E-09 + 33, 7,-8.2933999034545262E-08, 2.9017505639063501E-07, 1.9363848432174000E-09, 1.9258638821784998E-09 + 33, 8,-1.1613016656396209E-07,-1.5174951296085281E-07, 1.8938563716785001E-09, 1.9055326847702998E-09 + 33, 9, 8.9989753058879369E-08, 1.0568950686966560E-07, 1.8443758090054000E-09, 1.8654899358336001E-09 + 33, 10, 5.9820199909105688E-09,-1.0939147720901480E-08, 1.8149230854104000E-09, 1.8152859880700000E-09 + 33, 11, 1.6713370934542750E-07,-1.6807661092338261E-07, 1.7613313380300000E-09, 1.7591409406433999E-09 + 33, 12, 1.7300030910776801E-07,-4.3788526045533312E-08, 1.7116987335214000E-09, 1.7157773756716000E-09 + 33, 13,-2.8840622092309700E-09,-1.9834544529064040E-08, 1.6525657760202000E-09, 1.6543362285702000E-09 + 33, 14, 5.8856684519502652E-08,-1.8654676706815480E-08, 1.5970753777042999E-09, 1.5963209452595001E-09 + 33, 15,-1.2161177303677400E-07, 1.0667577514684720E-07, 1.5345870289086001E-09, 1.5374883019192000E-09 + 33, 16, 2.4639424579856639E-08,-5.9001777810272968E-08, 1.4753247646557000E-09, 1.4724772567582000E-09 + 33, 17, 4.3606539106569017E-08, 2.4765101992831768E-08, 1.4117760489003000E-09, 1.4110893972557999E-09 + 33, 18,-1.2190341059056400E-07, 4.9682588850084671E-08, 1.3488741602557000E-09, 1.3477505629597000E-09 + 33, 19, 2.0067276583451190E-07,-2.1213398184407039E-07, 1.2824597618667999E-09, 1.2861093116400000E-09 + 33, 20,-1.6804639835150579E-07,-3.7630183697907902E-08, 1.2188859681510000E-09, 1.2209298729294999E-09 + 33, 21,-6.7127255683312121E-08,-2.3457283311142829E-07, 1.1590997498802000E-09, 1.1573541133060000E-09 + 33, 22,-5.3737111456736953E-08, 1.1984305878437651E-07, 1.0938325849425000E-09, 1.0923215606811000E-09 + 33, 23,-5.3045917259878153E-09, 1.1866956881209659E-07, 1.0271920741488000E-09, 1.0274781199747000E-09 + 33, 24, 1.5343563704200659E-07, 1.3675531038335189E-07, 9.6849442138469008E-10, 9.6289475703231014E-10 + 33, 25, 4.3550430286284293E-08,-5.8632154795252263E-08, 8.9832960676870006E-10, 8.9844149752293012E-10 + 33, 26,-3.7299578526325522E-08,-1.2038348936563821E-07, 8.3536388643211003E-10, 8.3404207071878011E-10 + 33, 27,-4.8200608425042718E-08, 4.6056492187872341E-08, 7.6750064636291010E-10, 7.6722574220750010E-10 + 33, 28,-5.1790687097832173E-08, 1.9390406026864819E-07, 7.0077122262765006E-10, 6.9927789310119007E-10 + 33, 29, 1.6008323713881899E-07, 1.1772951094374950E-08, 6.2740125651839003E-10, 6.2640646526186003E-10 + 33, 30, 1.0982620523421721E-07,-1.6664622274891639E-07, 5.5346738794537001E-10, 5.5603172828650006E-10 + 33, 31,-1.6505664906862919E-07,-3.0499920741683240E-09, 4.7120305392721007E-10, 4.7462531134952008E-10 + 33, 32,-1.1726664325540211E-08,-4.0000930060880048E-07, 3.2169717829822999E-10, 3.2291334690067001E-10 + 33, 33, 4.5174402586025787E-08,-1.0160378841311640E-07, 1.9751890422947000E-10, 2.0004525952135000E-10 + 34, 0,-1.0632887732392461E-07, 0.0000000000000000E+00, 2.2774455538373000E-09, 0.0000000000000000E+00 + 34, 1, 7.1415401897142285E-08,-4.9939191989393053E-08, 2.2623094640862998E-09, 2.2769233599152000E-09 + 34, 2,-1.0503626578362790E-07, 4.8735766498620773E-08, 2.2715426121178000E-09, 2.2653834431027002E-09 + 34, 3, 8.7426691656420210E-08,-1.6024833581982230E-07, 2.2521775759588000E-09, 2.2539927384797001E-09 + 34, 4,-2.6225259217101110E-08, 1.0118550795607580E-07, 2.2384354292012000E-09, 2.2451768677573002E-09 + 34, 5,-1.4176810909249629E-07,-5.5390063209572447E-08, 2.2133852651424999E-09, 2.2221181830959000E-09 + 34, 6,-1.0099938127383279E-08, 2.8211816113405839E-08, 2.1738036832624999E-09, 2.2139543291405000E-09 + 34, 7,-1.7057769281099299E-07, 2.3369066516504980E-07, 2.1684910445339001E-09, 2.1546410670495000E-09 + 34, 8, 2.2798882443566892E-08, 3.7760690705900123E-08, 2.1182463142377002E-09, 2.1301537206400998E-09 + 34, 9, 4.0136265384011702E-08, 1.2008488518940759E-07, 2.0715916670615001E-09, 2.0953958354617998E-09 + 34, 10,-2.6202192746739029E-08,-1.0944880314150940E-07, 2.0341551100690000E-09, 2.0335611672965002E-09 + 34, 11, 1.1353548626252819E-07,-2.1864018671670321E-07, 1.9858780431187998E-09, 1.9845525213063000E-09 + 34, 12,-7.3683620428852475E-08,-4.6168471565229713E-08, 1.9239764124736999E-09, 1.9274717398329001E-09 + 34, 13,-4.7579850021928781E-08,-1.2081902098228350E-07, 1.8706417695154999E-09, 1.8732171088457001E-09 + 34, 14, 4.1492210791192372E-08, 7.4710644016103333E-08, 1.8037532549055999E-09, 1.8031895442318999E-09 + 34, 15,-1.0997482077328889E-07, 7.6542996763054433E-08, 1.7405561858017999E-09, 1.7451716521645000E-09 + 34, 16, 2.3015340938087661E-07,-2.0105444760175092E-08, 1.6778245628197999E-09, 1.6727586653511000E-09 + 34, 17,-7.8588215654433869E-08, 7.3834738497796672E-08, 1.6078835197818001E-09, 1.6068409021771000E-09 + 34, 18,-3.4445364411745752E-08,-9.4177461860616565E-08, 1.5406561966247999E-09, 1.5390351562932000E-09 + 34, 19, 2.6527544728216930E-08, 1.9789558549789850E-07, 1.4685332769099000E-09, 1.4730881900627000E-09 + 34, 20,-1.7884929132645501E-07,-7.6271520420408341E-08, 1.4004784262042000E-09, 1.4026764956785999E-09 + 34, 21, 4.3553071302506183E-08, 1.3735028836628479E-07, 1.3339258973346000E-09, 1.3305843221517999E-09 + 34, 22,-5.6480634178509722E-08, 1.2819809899443320E-07, 1.2658428942326000E-09, 1.2643718030349000E-09 + 34, 23, 1.2445272972257441E-07,-5.9915928409727871E-08, 1.1931692272158999E-09, 1.1939985102325000E-09 + 34, 24, 1.1122139510232851E-07,-2.8476237200761750E-08, 1.1273280639248001E-09, 1.1203735892750000E-09 + 34, 25,-1.2252918749598119E-07,-8.9601300097711192E-08, 1.0574802636429000E-09, 1.0576717015847999E-09 + 34, 26, 6.1971254618533661E-08,-1.1079395498102480E-07, 9.8196411577357002E-10, 9.8279322299333001E-10 + 34, 27,-9.7315257931023134E-08, 1.2697196806340269E-07, 9.1403619957925004E-10, 9.1259819858949004E-10 + 34, 28, 7.8424659353231952E-08,-2.4000607220929259E-08, 8.4096249691276005E-10, 8.3931922540460011E-10 + 34, 29, 1.1085126465880940E-07, 5.5038737301195307E-08, 7.6813026120130007E-10, 7.6713406072977007E-10 + 34, 30, 8.2084920456087813E-09,-2.3302196924809060E-07, 6.8506332146900009E-10, 6.8883561978433003E-10 + 34, 31,-2.6560926919225028E-07, 1.1935975196949980E-07, 6.0658517134915015E-10, 6.1197667761620008E-10 + 34, 32, 1.3459906830385611E-07, 9.7368420389679973E-08, 5.1827891163423002E-10, 5.1990373600401004E-10 + 34, 33,-3.7864802372635139E-07, 5.5371199259954147E-08, 3.4934093333419999E-10, 3.5232444640827999E-10 + 34, 34, 1.2899929461847120E-08, 5.6821952047653063E-08, 2.1344810624042000E-10, 2.1300130706911001E-10 + 35, 0, 3.4024288438553658E-08, 0.0000000000000000E+00, 2.5324457788890002E-09, 0.0000000000000000E+00 + 35, 1, 7.1144467291398490E-08, 1.5644291900712211E-07, 2.5268609134136001E-09, 2.5422194194264001E-09 + 35, 2,-4.0982271089152203E-08,-5.8861114697072292E-08, 2.5255934604169998E-09, 2.5209660431461000E-09 + 35, 3, 7.1213409593184471E-08,-5.8377933040032508E-08, 2.5155768702520999E-09, 2.5186733474825000E-09 + 35, 4,-1.7929286223617721E-07, 8.1282437672105923E-08, 2.4914216405970002E-09, 2.4999222794892999E-09 + 35, 5, 7.5843741150327531E-08,-2.1312335468172129E-07, 2.4743462846533999E-09, 2.4848714716618001E-09 + 35, 6, 1.6604305607685219E-07, 1.8345312607409969E-07, 2.4247212648326999E-09, 2.4687382815254998E-09 + 35, 7, 3.4327777754990443E-08,-1.0628024552178880E-07, 2.4262558972308001E-09, 2.4123480514337001E-09 + 35, 8, 1.8581591771855159E-07, 8.6471564877470363E-08, 2.3693792621926998E-09, 2.3829510198630002E-09 + 35, 9,-3.9190194801627102E-08,-4.4365185019594807E-08, 2.3201660247167000E-09, 2.3492859516327001E-09 + 35, 10,-5.7246798699595002E-08,-6.8053676100103979E-08, 2.2828472403134001E-09, 2.2844584093076998E-09 + 35, 11,-5.5866176479524723E-08, 3.4259711168244792E-08, 2.2303004116213001E-09, 2.2275031342149998E-09 + 35, 12,-1.5404403505834219E-07, 4.9679959830521061E-08, 2.1680489983270002E-09, 2.1747918519153002E-09 + 35, 13,-6.1235114141357142E-08, 1.5237402884358111E-08, 2.1057361019158998E-09, 2.1064963779645001E-09 + 35, 14,-5.7581877683415301E-08, 1.0354562488116990E-07, 2.0445785040103998E-09, 2.0432624494563002E-09 + 35, 15, 6.8613484166470630E-08,-1.7424767158547829E-07, 1.9670087751257999E-09, 1.9704574436114999E-09 + 35, 16, 1.1821256289905221E-07, 6.9364285159169428E-08, 1.9052301376389001E-09, 1.9017570041860998E-09 + 35, 17,-1.4970719875700659E-07,-7.9225958874616963E-08, 1.8274619407674000E-09, 1.8271857259028001E-09 + 35, 18, 1.1065358799911551E-07,-6.7618308230379339E-08, 1.7553062979314999E-09, 1.7538614559809001E-09 + 35, 19,-1.0512687252784650E-07, 8.1662940248482451E-08, 1.6784920004596000E-09, 1.6840780255742001E-09 + 35, 20, 1.3408159324450509E-07,-1.0208799230801030E-07, 1.6045991231690001E-09, 1.6063458251434001E-09 + 35, 21, 6.8286897314518651E-08, 2.2539568227638071E-07, 1.5314837516506000E-09, 1.5304905928782999E-09 + 35, 22, 6.2016696429319921E-08,-1.0509548000082010E-07, 1.4569450666850999E-09, 1.4551035708741000E-09 + 35, 23, 3.6318691474751922E-08, 5.2156564624231831E-08, 1.3835072832607001E-09, 1.3831107640437000E-09 + 35, 24,-1.4111667891907559E-07,-4.8081363517502137E-10, 1.3085678129336001E-09, 1.3016227843542000E-09 + 35, 25,-5.8129176179972783E-08,-4.2444888159175283E-08, 1.2307270539300001E-09, 1.2304479773704000E-09 + 35, 26, 6.5991573606637773E-09, 3.8245564679828872E-08, 1.1565625973312000E-09, 1.1550229004460000E-09 + 35, 27,-9.5783609026223032E-08, 1.5875449157598520E-09, 1.0754454138995001E-09, 1.0753711751556999E-09 + 35, 28, 1.5153677193595010E-07,-9.4181481113138865E-09, 1.0007695792587000E-09, 1.0000023684061000E-09 + 35, 29,-1.3530417563280879E-07,-5.3988464378971132E-08, 9.2166079123506007E-10, 9.1979251146602006E-10 + 35, 30, 8.8716100802109765E-08,-1.2639213420519021E-07, 8.4030148130901007E-10, 8.4230512978404008E-10 + 35, 31,-5.8321007488984718E-08, 2.8790343528139821E-08, 7.5131557908504002E-10, 7.5621606993206011E-10 + 35, 32, 2.5531808292070540E-07, 2.2268803718401721E-07, 6.6673909172445009E-10, 6.7118012944764003E-10 + 35, 33,-1.1265421756798219E-07,-2.0937342975520031E-07, 5.6720772653031003E-10, 5.7013095607282005E-10 + 35, 34, 1.8927628048535371E-07, 3.3038614834541960E-07, 3.8444751425462010E-10, 3.8252455070643002E-10 + 35, 35, 1.0998534272364330E-07, 2.0758703611699020E-09, 2.3408565153264002E-10, 2.3554523442473000E-10 + 36, 0, 1.7352345802700461E-07, 0.0000000000000000E+00, 2.8212502342747002E-09, 0.0000000000000000E+00 + 36, 1,-9.2007856008371074E-08,-3.0952448936565900E-08, 2.8165760555997999E-09, 2.8341361669071998E-09 + 36, 2, 1.0530738484892290E-07,-1.2561437941773701E-07, 2.8156693630046001E-09, 2.8084066376673000E-09 + 36, 3,-1.1771852403467630E-07, 5.2985466285570571E-08, 2.8040600931308998E-09, 2.8077959235237999E-09 + 36, 4,-1.5313565372215140E-07,-1.9696456884033421E-08, 2.7794738298442998E-09, 2.7902523778882999E-09 + 36, 5, 8.1541689411796423E-08, 8.3350902398881584E-09, 2.7597827763457999E-09, 2.7696544366136001E-09 + 36, 6,-1.1711756774511030E-07, 8.9199564709527033E-09, 2.7106762252863001E-09, 2.7606503382563999E-09 + 36, 7, 1.0130108131381110E-07,-1.2028864718113240E-07, 2.7086044675120001E-09, 2.6913942238664001E-09 + 36, 8, 6.5650983855326063E-08,-4.8429664321144141E-08, 2.6547682023264998E-09, 2.6700785829480000E-09 + 36, 9,-4.6052228595997172E-08,-6.4211098547860040E-08, 2.5977036237053000E-09, 2.6256733035864999E-09 + 36, 10, 7.7793593779078320E-08, 3.1354655672475727E-08, 2.5646202665267000E-09, 2.5631662766898998E-09 + 36, 11,-7.5941886551186470E-08, 4.0196531745606541E-08, 2.5037258880649000E-09, 2.5007888665080001E-09 + 36, 12,-3.2144990107619147E-08, 8.1338662401160021E-09, 2.4413301866691998E-09, 2.4445154670620001E-09 + 36, 13, 1.6000633386291669E-07, 1.7591776430693591E-07, 2.3719714813867001E-09, 2.3770332546191000E-09 + 36, 14,-1.0071815566570820E-07,-4.4889184763151842E-08, 2.3039043299996000E-09, 2.3035417799446002E-09 + 36, 15, 2.4731013684687929E-07,-9.7021649942600052E-08, 2.2291022661302999E-09, 2.2346773320167001E-09 + 36, 16,-1.4505743301733001E-07,-6.3431374593812647E-09, 2.1529048355355999E-09, 2.1490099912252000E-09 + 36, 17,-1.4215542344097820E-08,-1.1551738332249510E-07, 2.0799579513551002E-09, 2.0784931712010999E-09 + 36, 18,-2.1878663531801221E-08, 1.5112603824599780E-07, 1.9955687212862001E-09, 1.9937278599323001E-09 + 36, 19,-1.0375681711073870E-07,-7.0982295220271862E-08, 1.9149031539097000E-09, 1.9189511390907002E-09 + 36, 20, 1.9031547678995510E-07, 7.8676120875159915E-08, 1.8355185718101000E-09, 1.8366357181594000E-09 + 36, 21, 7.8982338009706250E-08,-4.1706629246173578E-08, 1.7556468326195000E-09, 1.7518924448673000E-09 + 36, 22, 4.3102414379775842E-08,-1.0495857453142480E-07, 1.6743456866282000E-09, 1.6724694508701999E-09 + 36, 23, 1.0009537462456780E-08, 9.0614876441402204E-08, 1.5920635995864001E-09, 1.5925734617636001E-09 + 36, 24,-1.3926800128973271E-07,-5.7741876862596088E-08, 1.5172168181862001E-09, 1.5088235512165999E-09 + 36, 25, 3.8683102481227643E-08, 1.1124132178198440E-07, 1.4281235040165001E-09, 1.4290182355881000E-09 + 36, 26, 1.1689001881252919E-08, 7.5888958007031610E-08, 1.3477070755178000E-09, 1.3460045869286001E-09 + 36, 27, 5.8596021799722803E-08,-7.0293939734974430E-08, 1.2654315623703001E-09, 1.2640738704014000E-09 + 36, 28, 1.5076159933213461E-08, 1.9367461081613100E-07, 1.1787580763497000E-09, 1.1770232024983000E-09 + 36, 29,-1.6405335298419641E-07,-1.6850176167800510E-07, 1.0971741557322999E-09, 1.0955640431368001E-09 + 36, 30,-7.4254465455895331E-08, 2.0300350645333650E-07, 1.0085384598780999E-09, 1.0105255204069000E-09 + 36, 31,-1.3240823573794790E-07,-1.5088055722480529E-07, 9.2245194722944007E-10, 9.2485717966927003E-10 + 36, 32, 2.0674945258381269E-07, 1.5594694820138731E-08, 8.2699261936268003E-10, 8.2857322468686009E-10 + 36, 33, 5.2539661504874101E-08,-3.0051816790934441E-07, 7.3102868525657007E-10, 7.3820876170409002E-10 + 36, 34,-6.9262542472925794E-08, 1.5200865678709570E-07, 6.2706320086444002E-10, 6.2632484924636004E-10 + 36, 35, 1.6192670968369651E-07,-2.1443667465336801E-07, 4.1859681991775998E-10, 4.2012936428306001E-10 + 36, 36,-1.0044992897759140E-07,-1.8993104225368830E-07, 2.5956753106882997E-10, 2.6072805184818002E-10 + 37, 0,-6.3741046289057150E-08, 0.0000000000000000E+00, 3.1518095948558001E-09, 0.0000000000000000E+00 + 37, 1,-1.0611777630949591E-07,-1.8632051812706290E-07, 3.1313665100694001E-09, 3.1493456619447998E-09 + 37, 2, 5.6009815733753203E-08,-2.2446207229091391E-08, 3.1456716068961002E-09, 3.1392285172910999E-09 + 37, 3,-1.1842092807006070E-07, 6.2856719350806839E-08, 3.1196750839364001E-09, 3.1211905555981998E-09 + 37, 4, 1.0005914273016900E-07, 2.0014500256915651E-09, 3.1079892460496000E-09, 3.1177072583829998E-09 + 37, 5, 7.2264811073958769E-08, 1.2268383979687680E-07, 3.0732015841466999E-09, 3.0854297562281002E-09 + 37, 6,-1.8449611937997360E-07,-1.1901884817943060E-07, 3.0304744601702001E-09, 3.0869327640330999E-09 + 37, 7, 1.7391050683384140E-08,-2.1457643848063579E-08, 3.0231504961451998E-09, 3.0059997125841998E-09 + 37, 8,-2.5558233195870249E-07,-8.8206213756179182E-08, 2.9698723627135999E-09, 2.9865953479774001E-09 + 37, 9,-1.0620157770467470E-07,-4.8196020181185413E-08, 2.9084951863985999E-09, 2.9425286240098000E-09 + 37, 10,-2.7260453085020112E-09, 5.3806932218296873E-08, 2.8705471149994998E-09, 2.8722537546449999E-09 + 37, 11, 7.4034346997526069E-08,-5.1020913506620543E-08, 2.8128484716105999E-09, 2.8105727604382002E-09 + 37, 12, 7.5141166617708072E-08,-8.7603429403916834E-08, 2.7401206776844001E-09, 2.7458468160325000E-09 + 37, 13, 7.9209639967711802E-08, 3.9366188437952202E-08, 2.6722678019848001E-09, 2.6776668063549000E-09 + 37, 14,-8.1065694344038970E-09,-2.1589652872834841E-07, 2.5966373810077001E-09, 2.5976079093861998E-09 + 37, 15, 4.6533599379558117E-08, 3.9027411416846491E-08, 2.5174525917276999E-09, 2.5221189694953000E-09 + 37, 16,-1.3461496804365910E-07, 7.2253579465040884E-09, 2.4408709701621002E-09, 2.4353147508286999E-09 + 37, 17, 6.9995333268277029E-08, 1.1342404950751620E-07, 2.3510721607072001E-09, 2.3521985170203000E-09 + 37, 18,-1.0608247963127480E-07, 1.5337671225312510E-07, 2.2730067741842998E-09, 2.2700143196357001E-09 + 37, 19, 1.1576736332664570E-07,-7.7702062297068632E-08, 2.1761985016015002E-09, 2.1808174654418998E-09 + 37, 20, 7.5075036055401461E-08, 8.7057509818666982E-08, 2.0951774108844000E-09, 2.0960194485450999E-09 + 37, 21,-1.4994230264514201E-08,-1.1213963263696450E-07, 2.0078114800580001E-09, 2.0057597175989001E-09 + 37, 22, 4.4451651515338033E-08, 4.7873051414468217E-08, 1.9187409988851998E-09, 1.9154298505054001E-09 + 37, 23,-1.2391483089678131E-07, 3.2318312861779412E-08, 1.8305766991071000E-09, 1.8307982256192001E-09 + 37, 24, 4.1417378852924142E-08,-1.9111012269063280E-08, 1.7468341758596000E-09, 1.7377625961983000E-09 + 37, 25, 7.7326621210856984E-08, 5.2528174298109912E-08, 1.6568779891453000E-09, 1.6579351381770999E-09 + 37, 26,-5.2153906481778981E-08,-8.1641095176670622E-08, 1.5632739812481999E-09, 1.5613619366946000E-09 + 37, 27, 1.3504069301523100E-07,-7.9539227844172039E-08, 1.4757992560850001E-09, 1.4748827876534999E-09 + 37, 28,-1.7512993499676440E-07, 1.2705491672337340E-08, 1.3872685679039001E-09, 1.3830462701841000E-09 + 37, 29, 7.1470482599722573E-08,-1.0656217876793470E-07, 1.2921460400488000E-09, 1.2906265564860000E-09 + 37, 30,-5.2602301530057247E-08, 3.1382247811784079E-07, 1.2015701581562999E-09, 1.2028856410909999E-09 + 37, 31, 1.3582137005274120E-07,-1.3247281712944691E-07, 1.1065078362981000E-09, 1.1085860009932001E-09 + 37, 32,-4.4326791130363873E-09, 9.9824161505700014E-08, 1.0115744145062000E-09, 1.0155971288600001E-09 + 37, 33,-2.5639009814894070E-07,-1.6915326937568349E-07, 9.0477544788566002E-10, 9.1192406200485004E-10 + 37, 34,-1.2681810372529520E-07, 4.6625600984098993E-08, 8.0985947613249005E-10, 8.0621423404090010E-10 + 37, 35, 2.2560076674541489E-07, 7.1822320072323871E-08, 6.8792871261185004E-10, 6.8503447028885004E-10 + 37, 36,-3.1953116760542538E-07,-1.4063251761484349E-07, 4.4585230865913999E-10, 4.4635432166606002E-10 + 37, 37,-7.1841285233563025E-08, 9.5724111685223512E-08, 2.8739188630525001E-10, 2.8579275289196999E-10 + 38, 0,-1.5405998574156460E-07, 0.0000000000000000E+00, 3.5101065566660001E-09, 0.0000000000000000E+00 + 38, 1,-1.8725150453835798E-08,-9.1583448966180298E-09, 3.4929417912518999E-09, 3.5121980732936002E-09 + 38, 2,-1.6539765166918122E-08, 1.1561106188445949E-07, 3.5022925680956999E-09, 3.4953427309793000E-09 + 38, 3,-9.1462668167888710E-08,-1.0809767167287560E-08, 3.4808627780116000E-09, 3.4852439813047002E-09 + 38, 4, 1.7191699432976051E-07, 5.9753324011347201E-08, 3.4603743534709998E-09, 3.4715540854127999E-09 + 38, 5,-5.3649166964855931E-08,-3.4281153285803283E-08, 3.4347947240453998E-09, 3.4491925401947000E-09 + 38, 6, 9.9768326570849770E-08,-1.2193059030373480E-07, 3.3744710356326000E-09, 3.4399842470546001E-09 + 38, 7,-5.1102547958078612E-08, 1.5558891893238221E-07, 3.3847062437060001E-09, 3.3654375004525000E-09 + 38, 8,-7.5344401017860512E-08, 7.1912481545413382E-08, 3.3128572905057000E-09, 3.3334488422402000E-09 + 38, 9,-2.8346125209541060E-08, 1.4022018233298871E-07, 3.2610938199923002E-09, 3.2981285973107002E-09 + 38, 10,-5.0959358077642622E-08, 4.1315098657821473E-09, 3.2125213642478002E-09, 3.2150175489171002E-09 + 38, 11, 4.3019144239984183E-08,-9.7559862884989775E-08, 3.1569194688073001E-09, 3.1535092112091000E-09 + 38, 12,-1.7129305870774869E-08,-8.6008156272069162E-08, 3.0792989576717999E-09, 3.0861404139879000E-09 + 38, 13,-5.4530742921943897E-08,-8.6074144792288493E-08, 3.0068513110446998E-09, 3.0072913095337999E-09 + 38, 14, 3.3067505299285671E-08,-2.9537328778986109E-08, 2.9294791709741999E-09, 2.9270130300493999E-09 + 38, 15,-7.4733537259586694E-08,-7.2978104750845752E-08, 2.8391997913145000E-09, 2.8443019352731998E-09 + 38, 16,-5.7136955760144062E-08, 2.2557039578988329E-08, 2.7587533069528001E-09, 2.7541353796084001E-09 + 38, 17, 6.7089470986150363E-08, 7.4411490243961630E-08, 2.6650552753709000E-09, 2.6642121700641001E-09 + 38, 18,-3.4841800828916690E-08,-9.0175956331260211E-08, 2.5723509532211000E-09, 2.5722458776120999E-09 + 38, 19, 1.2084635693765930E-07,-6.5001652205626020E-08, 2.4814144642592002E-09, 2.4831388951254001E-09 + 38, 20, 1.4193912751912891E-08,-3.5504919380974737E-08, 2.3809926330058999E-09, 2.3815803991133001E-09 + 38, 21,-1.9782468811196401E-08,-7.4871070662501524E-08, 2.2934732582701999E-09, 2.2909626971486999E-09 + 38, 22,-9.7071896873553566E-08, 6.8387130079837683E-08, 2.1947603016042999E-09, 2.1920364436354001E-09 + 38, 23,-4.6197364882310861E-08,-1.0098875200363020E-07, 2.0968954790551002E-09, 2.0978787079216000E-09 + 38, 24, 5.9689555035904404E-08, 4.0277524559842673E-08, 2.0093758272000000E-09, 1.9979850216606000E-09 + 38, 25,-3.0551847724494910E-08,-3.8968966243183198E-08, 1.9084477966142001E-09, 1.9088303289307999E-09 + 38, 26, 3.0444961811857902E-08,-1.1656011692344250E-07, 1.8142207375464001E-09, 1.8111685589541001E-09 + 38, 27,-5.6550019523513930E-09, 1.0425749062421010E-07, 1.7105549142549999E-09, 1.7112560393004001E-09 + 38, 28,-1.2783776427157491E-07,-1.2301460525967901E-07, 1.6171299149957000E-09, 1.6172402426568000E-09 + 38, 29, 1.5421865195310449E-07, 2.0412514096121941E-07, 1.5186360970305000E-09, 1.5164788456649000E-09 + 38, 30,-1.3929808499773699E-07,-9.0603406745824274E-08, 1.4153572832958001E-09, 1.4167699683205999E-09 + 38, 31, 3.0879633377199652E-07, 1.6708915299764249E-08, 1.3175180903696000E-09, 1.3208616389976000E-09 + 38, 32,-1.2075088880431910E-07, 2.7990428119870291E-08, 1.2143425758723000E-09, 1.2177273475295001E-09 + 38, 33, 8.2116489399665752E-08, 5.2458046954483246E-09, 1.1112410252017000E-09, 1.1161244972367000E-09 + 38, 34,-4.1154829117260563E-08, 1.8964669066459271E-07, 1.0020742058895000E-09, 9.9616530576910008E-10 + 38, 35, 1.3762911451233519E-07, 4.7481109769087227E-08, 8.8873157584254009E-10, 8.8827714042599009E-10 + 38, 36,-1.3108468771968021E-07,-1.6947905597121030E-07, 7.5614331553341002E-10, 7.5550873771000009E-10 + 38, 37, 7.2116642160146979E-08, 3.1912563596010080E-07, 5.1412734291246003E-10, 5.1375671856824999E-10 + 38, 38, 1.3659382787784400E-07, 6.6003276570958289E-09, 3.4552796305070999E-10, 3.4265571384370999E-10 + 39, 0, 5.3300296518817847E-08, 0.0000000000000000E+00, 3.9035173386167000E-09, 0.0000000000000000E+00 + 39, 1, 1.3292094229078051E-07, 1.1341140325640750E-07, 3.8996077885533002E-09, 3.9205733281837000E-09 + 39, 2, 1.5266323043110379E-09, 8.9367826520247134E-08, 3.8965916997159997E-09, 3.8885742674248004E-09 + 39, 3, 1.1544697443314651E-08,-2.2416832893452210E-08, 3.8874839944472002E-09, 3.8913183880570998E-09 + 39, 4,-1.3943081704577660E-08, 3.7147422269354681E-08, 3.8520163299310000E-09, 3.8663601468312997E-09 + 39, 5,-1.1318742467645000E-07,-1.8968885023875070E-07, 3.8362107326404003E-09, 3.8509071265778998E-09 + 39, 6, 7.5952483998406942E-08,-5.3033608789881273E-09, 3.7658559212065998E-09, 3.8359738669917997E-09 + 39, 7,-7.9199947776868484E-08,-1.4270267969447910E-09, 3.7805803600366997E-09, 3.7579211356360998E-09 + 39, 8, 1.3404990452028629E-07, 2.3597949828772129E-08, 3.7049471690602001E-09, 3.7293811556327002E-09 + 39, 9, 1.0291991059300560E-07, 1.6248764726980340E-08, 3.6439985953520000E-09, 3.6839368214963001E-09 + 39, 10, 8.7014607494870442E-08, 8.8965744828152320E-09, 3.6055242256543002E-09, 3.6049556891345999E-09 + 39, 11, 5.1496874808932923E-08,-2.8020956341249119E-08, 3.5349497695779998E-09, 3.5285404546092001E-09 + 39, 12,-8.1395430631224550E-08,-2.3161127924717901E-08, 3.4623081831655999E-09, 3.4663919881001002E-09 + 39, 13,-2.5737302795979229E-08,-9.0315047807128653E-08, 3.3750667660068000E-09, 3.3808609266695000E-09 + 39, 14,-2.3273029493648630E-08, 5.7524655688520962E-08, 3.2968065431810002E-09, 3.2958792831586000E-09 + 39, 15,-4.4139258526118877E-08, 2.5426920730707720E-08, 3.2003841874628000E-09, 3.2087716818489000E-09 + 39, 16, 6.5978882881334969E-08,-2.1611023131836550E-08, 3.1135077515176999E-09, 3.1082959803441001E-09 + 39, 17, 4.3832121189280372E-08, 1.9101381207184771E-08, 3.0154721637971001E-09, 3.0148821970058999E-09 + 39, 18, 5.0596479080772032E-08,-1.8299313978655771E-07, 2.9155822949350001E-09, 2.9127453986996999E-09 + 39, 19,-7.6265510349001780E-08, 7.1684168850240922E-08, 2.8106916990741000E-09, 2.8184756296819000E-09 + 39, 20,-9.7369681679613324E-09,-1.0481196667219839E-07, 2.7130522307760001E-09, 2.7135824096223001E-09 + 39, 21,-2.6240002446651179E-08, 6.9020629660380632E-08, 2.6065503291009000E-09, 2.6031762556958000E-09 + 39, 22,-6.6994970937120112E-08,-1.1029185616235130E-08, 2.5098415742327000E-09, 2.5068897563658002E-09 + 39, 23, 8.9542132971258069E-08,-1.1968974262989280E-09, 2.4006212631559998E-09, 2.3986153763043000E-09 + 39, 24,-1.7041397913174399E-08,-1.1366060449154550E-08, 2.3016541039886999E-09, 2.2903638687493000E-09 + 39, 25,-5.1927520782573713E-08,-8.6672589942709914E-08, 2.1949088503061001E-09, 2.1966350850529998E-09 + 39, 26,-1.7081179165595821E-08,-3.7483879524688449E-09, 2.0910013985964999E-09, 2.0868739776894002E-09 + 39, 27,-8.1205253651218911E-08, 1.2890360457968621E-07, 1.9846683933008001E-09, 1.9847330640971000E-09 + 39, 28, 1.9062945992975649E-07,-5.5199168562401373E-08, 1.8757307177143999E-09, 1.8731148148633001E-09 + 39, 29,-4.5299166976803431E-08, 2.2098597271240089E-07, 1.7746898737068001E-09, 1.7723317346903001E-09 + 39, 30, 9.1090715649129804E-08,-2.5138211429178671E-07, 1.6630955783419001E-09, 1.6641865173641001E-09 + 39, 31, 6.4382994407110405E-08, 1.5852724149788030E-07, 1.5531692484911999E-09, 1.5553167500657000E-09 + 39, 32,-1.1029173844851870E-07,-2.4387905056924291E-07, 1.4455072701698999E-09, 1.4501550843618000E-09 + 39, 33, 1.7112465675371320E-07, 1.1879831889164030E-07, 1.3318157930413999E-09, 1.3380250605958001E-09 + 39, 34,-4.7767683725477561E-08,-6.6998431683726150E-08, 1.2241834948446000E-09, 1.2224634062290000E-09 + 39, 35, 1.4071716839129849E-07, 9.0275121736172926E-08, 1.0974807815217999E-09, 1.0965931031676001E-09 + 39, 36,-4.6308462864084007E-08,-6.4841144849370293E-08, 9.7802057825835013E-10, 9.8026146250936010E-10 + 39, 37,-4.8600314891556532E-08, 9.0444427821184413E-08, 8.3616341401812012E-10, 8.3512057977080015E-10 + 39, 38, 2.1996393733324770E-07,-1.5314143963627541E-07, 5.6308308094327009E-10, 5.6484166197498009E-10 + 39, 39,-5.1597207682441097E-08,-2.2609837027806909E-07, 3.6651947269690002E-10, 3.6272443115121001E-10 + 40, 0, 1.1431642571856310E-07, 0.0000000000000000E+00, 4.3565370006819999E-09, 0.0000000000000000E+00 + 40, 1, 3.1647660364933687E-08,-1.2315860111016490E-08, 4.3397110308841001E-09, 4.3604376870271002E-09 + 40, 2,-3.3315541741577042E-08,-9.2243360499731132E-08, 4.3488845862202002E-09, 4.3407501716164003E-09 + 40, 3, 1.8701581254687180E-08,-1.0205576486770170E-08, 4.3259698092749996E-09, 4.3282855894414001E-09 + 40, 4,-9.0846175675476103E-08, 5.2977246889948087E-08, 4.3030535433193000E-09, 4.3186024586383997E-09 + 40, 5, 5.0177116883188513E-08,-1.2588819373650590E-07, 4.2698941148760001E-09, 4.2871641305583001E-09 + 40, 6,-1.1217077677067280E-07, 7.8338895154592315E-08, 4.2108068708246004E-09, 4.2901306322554002E-09 + 40, 7, 2.1718041200904621E-08,-4.0624066538334683E-08, 4.2117722322596999E-09, 4.1879109869461004E-09 + 40, 8, 5.0790711991283391E-08,-7.5782032686181212E-08, 4.1491999548176997E-09, 4.1725101234980999E-09 + 40, 9, 5.4145072508608202E-08,-1.1988569027936910E-07, 4.0694632537465999E-09, 4.1157574532163999E-09 + 40, 10, 5.2353949617784162E-09,-1.1620105031857129E-07, 4.0383542100396002E-09, 4.0367892494077997E-09 + 40, 11, 9.2282346940739848E-08,-6.2717455970037551E-08, 3.9626860609048003E-09, 3.9554209508821003E-09 + 40, 12,-1.5666361601992489E-09, 1.3994069734621220E-08, 3.8809162934788000E-09, 3.8854532636673004E-09 + 40, 13, 9.2899519041465608E-09, 3.2590936643575061E-08, 3.7973103734763996E-09, 3.8006691686245001E-09 + 40, 14, 9.2560115363515330E-09, 5.1913782086675288E-08, 3.7033245847340999E-09, 3.7037445643064999E-09 + 40, 15,-9.7810519581017463E-08,-2.6810746744726531E-08, 3.6085468076240998E-09, 3.6160348021295999E-09 + 40, 16, 1.3511476653449830E-07,-1.5392095664508581E-08, 3.5114471030298999E-09, 3.5050676225453000E-09 + 40, 17,-7.4151417465336532E-08,-3.1231163152071722E-08, 3.4056334029001000E-09, 3.4066743913918998E-09 + 40, 18, 6.4803210962662469E-08, 5.7378266470393187E-08, 3.2994032789215998E-09, 3.2977408335367000E-09 + 40, 19,-1.0626163688162100E-07, 7.6350083673886074E-09, 3.1861232408072999E-09, 3.1917200216411999E-09 + 40, 20, 4.4546605643986383E-08,-3.6682748646973609E-09, 3.0791514750688000E-09, 3.0814103421329000E-09 + 40, 21, 1.2842102102267159E-08, 7.4625674210358361E-08, 2.9699040079105998E-09, 2.9652559232456001E-09 + 40, 22, 3.7840203669831671E-08,-3.3115310471431442E-08, 2.8535396864201002E-09, 2.8494261143211001E-09 + 40, 23, 8.6159655778415223E-08, 7.8277455659345510E-08, 2.7447592972053998E-09, 2.7465912469088001E-09 + 40, 24, 4.5210104573282512E-08,-5.9931342692569679E-11, 2.6323757785026999E-09, 2.6193318264278999E-09 + 40, 25,-7.5734740408267544E-08,-4.7314574994362781E-08, 2.5153037673779001E-09, 2.5177476699028998E-09 + 40, 26, 2.7047726198364879E-08, 1.2100929081666580E-07, 2.4052234072232000E-09, 2.4020288985008000E-09 + 40, 27, 4.6492553931192713E-09,-1.7294019656837769E-08, 2.2889893662475000E-09, 2.2870912467736000E-09 + 40, 28, 1.7634915939065211E-07, 1.1870827729518299E-07, 2.1755001129646001E-09, 2.1729526055816002E-09 + 40, 29,-8.9324278875407614E-08,-1.4283544336856740E-07, 2.0556201490151001E-09, 2.0549673373012001E-09 + 40, 30, 1.5463854684447579E-07,-6.1857317940726088E-08, 1.9451147633541998E-09, 1.9457327269791000E-09 + 40, 31,-2.1632223442408729E-07,-1.1714023769748050E-08, 1.8220712123244000E-09, 1.8269589031553999E-09 + 40, 32, 9.9384010028091922E-08,-5.6395574493421521E-08, 1.7050031014447999E-09, 1.7074599268152001E-09 + 40, 33,-7.3948282093979442E-08, 1.2896122476894400E-07, 1.5866996929663999E-09, 1.5931441551130000E-09 + 40, 34, 5.8934031083069974E-09,-2.1542192002439329E-07, 1.4696614302963999E-09, 1.4643536255201001E-09 + 40, 35,-7.5750529510708973E-08, 8.2091206778419993E-08, 1.3451562762663000E-09, 1.3458498668878001E-09 + 40, 36,-1.8911941880542261E-08,-6.1202906971342760E-08, 1.2078348743287000E-09, 1.2086268739590999E-09 + 40, 37,-1.9229968816438468E-08, 1.1666544956875890E-07, 1.0776357825042000E-09, 1.0789382738425000E-09 + 40, 38, 1.0377193592993050E-07,-4.2416003480273973E-08, 9.2587790001143015E-10, 9.2242454195647009E-10 + 40, 39,-1.3635697439148091E-07,-8.8373941074185891E-09, 6.1081355645544000E-10, 6.1268885767370008E-10 + 40, 40,-2.7101100513344118E-07, 8.5026569722694621E-08, 4.0312688286402001E-10, 4.0375879551846002E-10 + 41, 0, 7.9871997105759519E-08, 0.0000000000000000E+00, 4.8536815681199003E-09, 0.0000000000000000E+00 + 41, 1,-4.1043764165845303E-08,-6.6196153339955633E-08, 4.8350442304172997E-09, 4.8576261290102997E-09 + 41, 2, 4.9179249652509597E-08,-1.1013450301150640E-07, 4.8464471250358002E-09, 4.8361660933811004E-09 + 41, 3,-8.2312573526092829E-08, 6.8022061285030301E-08, 4.8193323635516000E-09, 4.8272399868962998E-09 + 41, 4, 3.8617443674673813E-08,-9.1090743411112681E-09, 4.7941319663762997E-09, 4.8143969273559002E-09 + 41, 5, 1.1298131071200550E-07, 8.5119144101046612E-08, 4.7629069930133004E-09, 4.7839494658635999E-09 + 41, 6,-4.4944522954060142E-08, 4.0965479663067913E-08, 4.6917213823435002E-09, 4.7854970542008003E-09 + 41, 7, 7.6006661332476222E-08, 1.1381524181573270E-07, 4.7075929055475000E-09, 4.6794528491369004E-09 + 41, 8, 9.6423737198694305E-09,-6.0828607290153702E-08, 4.6269108707493000E-09, 4.6545254011342998E-09 + 41, 9,-7.0898379878253088E-08,-1.9501451859426580E-08, 4.5583211900231000E-09, 4.6095121746958002E-09 + 41, 10,-7.9036251409471309E-08,-6.9977959671344712E-08, 4.5070592297337997E-09, 4.5100744624033997E-09 + 41, 11,-2.5332751859082841E-08,-7.2000149507426119E-09, 4.4442375164739003E-09, 4.4389303759007002E-09 + 41, 12,-8.3779834167687820E-08, 1.6107004253650731E-08, 4.3452726949190003E-09, 4.3513190941461002E-09 + 41, 13, 4.3832520518013943E-08, 2.3343830710948490E-09, 4.2625274257462999E-09, 4.2696727072243004E-09 + 41, 14,-5.2387436526443118E-08, 5.4724464684394042E-08, 4.1623555086222001E-09, 4.1654643831298999E-09 + 41, 15, 9.2140921844138248E-08,-5.7358728216130062E-08, 4.0582122175180001E-09, 4.0648087523495997E-09 + 41, 16,-3.9377891900435567E-08,-4.5074798336150379E-09, 3.9593746797143000E-09, 3.9549729871267000E-09 + 41, 17, 1.2925635359866230E-08,-9.9882784338163854E-08, 3.8413671255967002E-09, 3.8418836748286002E-09 + 41, 18,-1.0515459741048680E-07, 1.1322243917026700E-07, 3.7324291185028998E-09, 3.7273197797948002E-09 + 41, 19, 9.1774661613064660E-09, 1.6920261273236539E-08, 3.6092848113467000E-09, 3.6106592870390999E-09 + 41, 20, 3.9632674215208358E-08, 5.8044464641165463E-08, 3.4912428471816998E-09, 3.4916299331804000E-09 + 41, 21, 1.0178759866291400E-07, 7.6103953767527410E-09, 3.3733667038349000E-09, 3.3692622207384001E-09 + 41, 22, 6.2787577838306609E-08,-3.0790208864839737E-08, 3.2477543490810001E-09, 3.2465811906238000E-09 + 41, 23,-5.1007543902693572E-08,-3.2075425395985773E-08, 3.1239895203908999E-09, 3.1229016286254000E-09 + 41, 24,-3.3047895443993861E-08,-8.3378097064279612E-08, 3.0133697193626998E-09, 2.9994836997292001E-09 + 41, 25, 1.6129623177286039E-08,-6.0558536238174062E-08, 2.8769574419418000E-09, 2.8778316869239999E-09 + 41, 26,-1.5002838254841810E-08, 9.4476752114376612E-08, 2.7602697723983999E-09, 2.7532163243356001E-09 + 41, 27, 1.2136530532977621E-07,-6.5520567463968040E-08, 2.6342152990620998E-09, 2.6327775530200002E-09 + 41, 28,-8.7029992414525354E-08, 5.3360448749159223E-08, 2.5080191352496999E-09, 2.5064825622519000E-09 + 41, 29,-4.3867782895983341E-08,-9.9875722628776192E-08, 2.3851639274241000E-09, 2.3820578441980002E-09 + 41, 30,-1.4172518633341620E-08, 1.0689722703554319E-07, 2.2524692656559002E-09, 2.2558155685412001E-09 + 41, 31,-7.5750796045780452E-08,-1.0859777542615310E-07, 2.1337986275345002E-09, 2.1367787455794998E-09 + 41, 32, 1.0990067595226921E-07, 1.5469110921301549E-07, 2.0003607840018002E-09, 2.0034216939689999E-09 + 41, 33,-1.4856416031271959E-07,-2.8927228186120179E-08, 1.8714821224508999E-09, 1.8768802546121001E-09 + 41, 34, 1.3058340922204921E-07,-7.6364501320176865E-08, 1.7490305712510000E-09, 1.7434026707973999E-09 + 41, 35,-1.8715593868844011E-07, 4.7505811923597722E-08, 1.6146633741532000E-09, 1.6099521675505000E-09 + 41, 36, 2.2862299407007860E-08,-5.0383898653871342E-08, 1.4804011412513000E-09, 1.4812444875604000E-09 + 41, 37, 8.6528038465078909E-08, 9.5184082798041005E-08, 1.3293936610316000E-09, 1.3279845352863001E-09 + 41, 38, 2.0697875088996049E-08,-1.1672670545989641E-07, 1.1894639017349000E-09, 1.1919128642639000E-09 + 41, 39,-5.5167197888632668E-08,-2.2343651058036722E-08, 1.0074087999986000E-09, 1.0052862319106000E-09 + 41, 40, 9.6016911042050594E-08, 1.4806586193813390E-07, 6.5744510608772002E-10, 6.6164464139373005E-10 + 41, 41, 9.4297429823070367E-08, 1.5006966941898189E-07, 4.4525777036122002E-10, 4.4435036662742010E-10 + 42, 0,-1.7137556396439459E-08, 0.0000000000000000E+00, 5.4017689957910000E-09, 0.0000000000000000E+00 + 42, 1,-3.1085555217210388E-08,-2.0747224762365502E-08, 5.3919944402276000E-09, 5.4180817577411000E-09 + 42, 2, 5.8839602375953784E-09, 8.9574906805196080E-08, 5.3919900463163998E-09, 5.3836798217462004E-09 + 42, 3,-1.1164560247900809E-07, 2.4325083281381339E-08, 5.3767897371886001E-09, 5.3838267222679997E-09 + 42, 4, 1.3329878375203610E-07, 4.4277868919749381E-08, 5.3406497578784996E-09, 5.3581788877020997E-09 + 42, 5, 4.3066671070897467E-08,-4.1939709906210613E-08, 5.3165493991593000E-09, 5.3362312097032999E-09 + 42, 6, 5.1967204239964987E-08,-1.8241878220335150E-08, 5.2324096996497999E-09, 5.3308773213974002E-09 + 42, 7, 7.1622019173744901E-08,-1.9140139720980339E-08, 5.2557894105964003E-09, 5.2262701388934997E-09 + 42, 8, 5.8311147114710313E-08,-1.1124493316667720E-08, 5.1644959036402002E-09, 5.1964986726831999E-09 + 42, 9,-1.9001578573782611E-08, 5.1369916410424927E-08, 5.0951492005025998E-09, 5.1483100277250001E-09 + 42, 10,-1.3435375341139611E-08, 3.9066754006329112E-08, 5.0443578158561998E-09, 5.0471194850878002E-09 + 42, 11,-1.0622266046405760E-07, 9.8714521861853553E-08, 4.9692106940743997E-09, 4.9627628936765996E-09 + 42, 12,-2.9156055296528991E-08, 8.4347934667656576E-09, 4.8769781477038997E-09, 4.8854080920972999E-09 + 42, 13, 3.8083233571807552E-10, 4.2895604594033143E-09, 4.7727788188854000E-09, 4.7812726538014003E-09 + 42, 14,-6.7339003570920022E-08,-9.1892312588592614E-08, 4.6845083362737003E-09, 4.6813027472068001E-09 + 42, 15, 9.7413956855713262E-08, 7.8081651424869584E-09, 4.5586221276299996E-09, 4.5695766565388003E-09 + 42, 16,-1.4850282586143070E-07, 1.5002549465408500E-08, 4.4568519372082000E-09, 4.4530662002780996E-09 + 42, 17, 3.4946457205504107E-08, 3.8312943419232140E-08, 4.3343998306176000E-09, 4.3331500845737000E-09 + 42, 18,-7.2182285726355242E-08, 3.2160647815427140E-08, 4.2106541872980000E-09, 4.2068768426671999E-09 + 42, 19, 7.1998416423079081E-08,-4.8876387429211362E-08, 4.0810072603513002E-09, 4.0879165511319998E-09 + 42, 20, 2.9311588475205901E-08, 1.2032268815139301E-08, 3.9522381333464003E-09, 3.9514996085960003E-09 + 42, 21, 3.2595107644244431E-08,-7.6504405037729211E-08, 3.8251726729373999E-09, 3.8219149401141002E-09 + 42, 22,-2.4316022333933020E-08,-7.7247148133218600E-08, 3.6931637739063002E-09, 3.6886428418410001E-09 + 42, 23,-5.7432782183942077E-08,-9.5532279488757758E-09, 3.5566938045292002E-09, 3.5544464213345000E-09 + 42, 24,-6.0236532348569551E-08,-1.6040599693174530E-08, 3.4302527149196998E-09, 3.4144162123027002E-09 + 42, 25, 2.8551297372567519E-08, 7.0413032253180312E-08, 3.2928332494825000E-09, 3.2952253565486001E-09 + 42, 26, 6.0661485042093622E-09,-9.0879720173483327E-08, 3.1538767760076000E-09, 3.1467205091784000E-09 + 42, 27, 4.1464973076392673E-08,-5.3325049336206543E-08, 3.0242511392858001E-09, 3.0203544105009999E-09 + 42, 28,-1.3537264895830711E-07,-3.6903252119608633E-08, 2.8872191368341000E-09, 2.8845737395957000E-09 + 42, 29, 3.9770580211219934E-09,-2.0070711819112278E-08, 2.7513056856339001E-09, 2.7475759492551000E-09 + 42, 30,-8.9197805112631992E-08, 1.0456360005424260E-07, 2.6128816276740998E-09, 2.6150498290280002E-09 + 42, 31, 9.7777844196337413E-08, 3.3107553848855677E-08, 2.4714268726078998E-09, 2.4765731864935001E-09 + 42, 32, 1.8825796918954612E-09, 1.0264434234318540E-07, 2.3409550811963999E-09, 2.3461074740369999E-09 + 42, 33, 9.9393773922422288E-08,-1.8640078621906360E-07, 2.1928626166884001E-09, 2.2023186871975001E-09 + 42, 34, 6.6586344005882209E-10, 3.7258492673236861E-08, 2.0616812516398999E-09, 2.0567962568017000E-09 + 42, 35,-1.2443040460240540E-07,-2.7676663690450339E-08, 1.9202524458838999E-09, 1.9188358014647998E-09 + 42, 36, 8.2552359862342170E-08, 6.7052447146477134E-08, 1.7731663976384999E-09, 1.7735083532542000E-09 + 42, 37, 1.7102320914112520E-08, 1.8757472258333540E-08, 1.6295031133838001E-09, 1.6281429392958000E-09 + 42, 38, 7.0150903623199802E-09,-1.3743831882202600E-07, 1.4655651166083001E-09, 1.4650036680467001E-09 + 42, 39,-1.2087952580800200E-08, 9.4783993450608066E-10, 1.3074796912351000E-09, 1.3060615085414000E-09 + 42, 40,-1.9957656394902161E-08, 1.4256046084825890E-07, 1.1026687291226999E-09, 1.1046314904318000E-09 + 42, 41, 7.4550314984643260E-08,-1.5665030746893270E-07, 7.2278290660806008E-10, 7.2342710005829003E-10 + 42, 42, 7.5785726183163979E-08,-1.0699873237628981E-07, 4.9554836860219007E-10, 4.9557047908597002E-10 + 43, 0, 3.7471842745203072E-08, 0.0000000000000000E+00, 6.0253203365899999E-09, 0.0000000000000000E+00 + 43, 1, 1.0940925204293170E-07,-4.6518061672389871E-08, 5.9976895823990000E-09, 6.0234860218583997E-09 + 43, 2,-2.7502886464458809E-08, 1.5472351168609571E-07, 6.0179621376051998E-09, 6.0039704322354998E-09 + 43, 3,-5.6634453941805927E-08, 1.6876664552559672E-08, 5.9801392714298998E-09, 5.9889309015686002E-09 + 43, 4, 5.8773884977798221E-08, 7.1322115880910800E-08, 5.9577858996015999E-09, 5.9825408365380001E-09 + 43, 5,-5.7211615963801272E-08,-9.2086521843098551E-08, 5.9165128726402001E-09, 5.9407997363232997E-09 + 43, 6,-2.8183841998379459E-08,-5.2794268166107312E-08, 5.8420115787497001E-09, 5.9533229416897002E-09 + 43, 7,-1.5109491761491501E-08,-5.0671082746500437E-08, 5.8558192559899001E-09, 5.8227415908299003E-09 + 43, 8,-4.7414250585318121E-09,-4.9040619502658312E-08, 5.7715348354562004E-09, 5.8074328828668001E-09 + 43, 9, 5.6927781297715258E-08, 3.6408462864318533E-08, 5.6830874887439000E-09, 5.7462247327744000E-09 + 43, 10, 5.7090865770674262E-08, 2.7299785311151939E-08, 5.6447428163057996E-09, 5.6465016456502997E-09 + 43, 11, 1.1870731193889190E-08, 9.2961007340671258E-09, 5.5577279140890003E-09, 5.5469883506171003E-09 + 43, 12, 9.0647226275979881E-08, 8.0403021621620174E-09, 5.4628005183092003E-09, 5.4687146500335001E-09 + 43, 13,-4.9130097206984682E-08, 1.6668650379244290E-09, 5.3558597587657000E-09, 5.3617160536506003E-09 + 43, 14, 2.6217930541455741E-09,-2.3469552074918950E-08, 5.2478958771784004E-09, 5.2490360718109002E-09 + 43, 15,-6.4975709624341304E-09, 7.5480018508543451E-08, 5.1306220592409996E-09, 5.1408447084330001E-09 + 43, 16,-8.2163284341294182E-08,-8.3266185026719411E-08, 5.0086790890655001E-09, 5.0011388733263998E-09 + 43, 17, 5.7724632602488273E-09, 1.0443482734608840E-07, 4.8846864130076001E-09, 4.8851691959271998E-09 + 43, 18, 1.7307091838774720E-08,-2.4085970491631841E-08, 4.7488667563965998E-09, 4.7442833413172997E-09 + 43, 19,-1.0235890752631530E-08,-8.2465781317579683E-08, 4.6096148408216999E-09, 4.6141102332403997E-09 + 43, 20,-3.4230932223467940E-09,-6.2505704725977740E-08, 4.4749728649339998E-09, 4.4706615183261000E-09 + 43, 21,-3.7149548149485632E-08,-1.0040409706186210E-07, 4.3314022021663000E-09, 4.3258399379140000E-09 + 43, 22,-6.3001129932942259E-08, 3.7076901982693737E-08, 4.1899461044727997E-09, 4.1870402057841003E-09 + 43, 23,-1.0723202701120561E-07, 3.9852254590481537E-08, 4.0415547815925003E-09, 4.0415303353987996E-09 + 43, 24,-2.0456039931410390E-08,-6.2013653216909018E-09, 3.9030030874306999E-09, 3.8877934163141004E-09 + 43, 25, 4.3550529045118801E-08, 5.8145896737270153E-08, 3.7523004527496998E-09, 3.7540894531077999E-09 + 43, 26, 3.5068242081835120E-08,-8.7607428576049401E-08, 3.6111490223843002E-09, 3.6032725038277002E-09 + 43, 27,-3.5308164199753332E-08, 1.0433673431789490E-07, 3.4540331619642999E-09, 3.4514819998214001E-09 + 43, 28,-8.4334891546169632E-08,-3.9205747691987771E-08, 3.3160779997766999E-09, 3.3130222274571000E-09 + 43, 29, 1.8714465002736638E-08, 1.4882653996220990E-07, 3.1644492899173999E-09, 3.1645709535368000E-09 + 43, 30, 2.8212679299797081E-08,-3.7809465777908942E-08, 3.0149745451310000E-09, 3.0156963244294000E-09 + 43, 31, 1.4028441313329869E-07, 6.5448088623924091E-09, 2.8649602485608001E-09, 2.8712861416482001E-09 + 43, 32,-3.6540133060966673E-08,-6.5907204135334644E-08, 2.7150883176357999E-09, 2.7156520007794999E-09 + 43, 33, 1.2489740204139020E-07,-6.6456338416773170E-08, 2.5690642997434999E-09, 2.5780375855160000E-09 + 43, 34,-1.8086846948942510E-07, 1.9156392792913430E-08, 2.4163837359313000E-09, 2.4097968046182998E-09 + 43, 35, 1.1616167578339940E-07, 1.2850018242570830E-08, 2.2638842264652999E-09, 2.2632897512463001E-09 + 43, 36, 2.0183663109732231E-08, 1.1238488442878350E-07, 2.1097672308466000E-09, 2.1102942732254001E-09 + 43, 37, 3.8750508202634828E-08,-5.8657085637391073E-08, 1.9526270099558999E-09, 1.9499845154016002E-09 + 43, 38,-6.3244673688973899E-08, 2.9648542185686360E-08, 1.8014170643809000E-09, 1.8012004698190999E-09 + 43, 39,-7.4635923836723781E-08, 5.6817100944223847E-08, 1.6074015599984000E-09, 1.6071725484010000E-09 + 43, 40, 9.9875709287124122E-09, 6.3582318152205199E-08, 1.4370887165619999E-09, 1.4398700189888999E-09 + 43, 41, 8.7613415840494159E-08,-3.4307045255616830E-08, 1.2093636312690000E-09, 1.2083688335381000E-09 + 43, 42,-1.4901347533796320E-07,-5.1742819938211677E-08, 7.9214314553688009E-10, 7.9125757888886006E-10 + 43, 43,-1.1566099896650800E-07, 3.1341189801872529E-09, 5.4833082654205001E-10, 5.4392920757779999E-10 + 44, 0, 8.5524329039863840E-08, 0.0000000000000000E+00, 6.6967911266635003E-09, 0.0000000000000000E+00 + 44, 1, 8.3850034826364461E-08, 1.5996922284384351E-09, 6.6921769702013998E-09, 6.7198343561653999E-09 + 44, 2,-4.7383539368391223E-08,-1.1429095402858490E-08, 6.6883143523976998E-09, 6.6732824625369002E-09 + 44, 3, 8.6781111274101510E-08,-2.8057002814233372E-08, 6.6740431346801001E-09, 6.6817647116594997E-09 + 44, 4, 4.2635426081388002E-08, 1.4930471391427640E-08, 6.6260675815080003E-09, 6.6501261123059003E-09 + 44, 5, 2.2842110478575401E-08,-5.0239429063942463E-08, 6.6040546069081997E-09, 6.6321629981050997E-09 + 44, 6,-1.0455791021996280E-07, 5.6878315588005877E-08, 6.4969623988667996E-09, 6.6275980438470004E-09 + 44, 7,-1.0353451878592160E-07, 8.9155653753268193E-09, 6.5427071871772004E-09, 6.5037381445638004E-09 + 44, 8,-5.0553512133621493E-08, 5.6768249086620301E-08, 6.4296897676176003E-09, 6.4687020771260002E-09 + 44, 9, 1.2305416456194189E-08, 6.1245464372566503E-09, 6.3536110003647001E-09, 6.4258131447114000E-09 + 44, 10, 2.8702632299081250E-08,-4.5126972323724348E-08, 6.2961207193928998E-09, 6.3015589180575000E-09 + 44, 11, 5.8117779943790087E-08,-1.4470231349996529E-07, 6.2203240012459999E-09, 6.2136338755930998E-09 + 44, 12, 3.8619109743098307E-08, 2.4118475635070741E-09, 6.1072018040489998E-09, 6.1121853427524998E-09 + 44, 13,-2.6946599755023951E-08,-5.4019276646471498E-08, 6.0044769113906999E-09, 6.0135615078900004E-09 + 44, 14, 5.2781117875213477E-08, 5.6331461585802138E-08, 5.8807324455479003E-09, 5.8850082457388002E-09 + 44, 15,-4.1425424068052802E-08, 3.1662857675478171E-08, 5.7593421629291998E-09, 5.7688481156160001E-09 + 44, 16, 1.1381667483408540E-07,-1.2975366283877720E-08, 5.6334362629907003E-09, 5.6276983260458998E-09 + 44, 17, 1.4242528464987590E-08,-1.6798474881229641E-08, 5.4893503352855996E-09, 5.4902030904824004E-09 + 44, 18, 5.4584165288354497E-08,-2.4186520246601400E-08, 5.3561859806542000E-09, 5.3516099812798004E-09 + 44, 19,-5.4594231664254258E-09,-5.6998281404251623E-09, 5.1976103758004998E-09, 5.2023578839582001E-09 + 44, 20,-7.3487034811141202E-08,-4.4555136940010121E-08, 5.0533992178279000E-09, 5.0546728027307998E-09 + 44, 21,-4.1189532928050653E-08, 6.1390113598152640E-08, 4.8998086155248999E-09, 4.8970884757648000E-09 + 44, 22, 9.0384080620170283E-09, 2.5931095822628839E-08, 4.7457262232640001E-09, 4.7394251235608999E-09 + 44, 23,-5.5985903118507084E-09,-3.7199475467357619E-10, 4.5885032375039002E-09, 4.5876877437054997E-09 + 44, 24, 9.9954947413291572E-08, 2.2522892183270111E-08, 4.4358484517249004E-09, 4.4193957154857998E-09 + 44, 25,-3.6462773242078573E-08,-2.8243651922494430E-08, 4.2687147304105004E-09, 4.2727670760407001E-09 + 44, 26,-6.6483391857885573E-09,-5.3173451362795557E-08, 4.1173224604217999E-09, 4.1065085469750002E-09 + 44, 27,-6.1870101571911623E-08, 4.7515448598952557E-08, 3.9562647355410996E-09, 3.9487816005029996E-09 + 44, 28,-8.8922453061693363E-09, 4.3288168671051990E-09, 3.7867868384792999E-09, 3.7855055998343004E-09 + 44, 29,-9.2701585348725802E-09, 7.8290546289377349E-08, 3.6383446165422001E-09, 3.6341138598297999E-09 + 44, 30, 1.0070698114291090E-07,-5.4711399155709687E-08, 3.4669046939539999E-09, 3.4726619431384000E-09 + 44, 31, 6.4874257654950712E-09, 5.6696368420009374E-09, 3.3056866723733998E-09, 3.3138185451482999E-09 + 44, 32,-3.9507976334654172E-08,-1.1480721203835839E-07, 3.1467440936905999E-09, 3.1491574942707001E-09 + 44, 33,-1.0068234422754279E-07, 6.8675085783054870E-08, 2.9779664968550001E-09, 2.9867996820106001E-09 + 44, 34,-5.3958221083960017E-08,-3.1219946448202773E-08, 2.8293649252367000E-09, 2.8230152861647001E-09 + 44, 35, 9.3310092392370870E-08, 8.5294851784949574E-08, 2.6522784888352999E-09, 2.6495105856711999E-09 + 44, 36, 3.2480357689670010E-09,-5.3999135982482587E-08, 2.4880675232694001E-09, 2.4899582891293998E-09 + 44, 37, 6.9878965021720259E-08,-4.3287315369110178E-08, 2.3223134535626002E-09, 2.3190886684744000E-09 + 44, 38,-1.0698171160101250E-07, 4.4964159828214733E-08, 2.1510266392474998E-09, 2.1479233009433999E-09 + 44, 39, 5.2216479475137063E-08, 3.5305334520983051E-08, 1.9704809727963002E-09, 1.9720561064276999E-09 + 44, 40, 1.0020580028866940E-07, 1.3256528091229689E-08, 1.7668689530747999E-09, 1.7705626007434000E-09 + 44, 41,-3.1576137868590563E-08,-5.9021595782638833E-08, 1.5829046390807000E-09, 1.5844586416789000E-09 + 44, 42, 5.8575368170156192E-08,-3.1841242105327440E-08, 1.3317231176191001E-09, 1.3312949749083000E-09 + 44, 43,-6.8395396577632181E-10, 1.0253780798043329E-07, 8.6658888976186010E-10, 8.7255632690104003E-10 + 44, 44, 5.0469092624835337E-08, 1.9157201867454991E-07, 6.0076663153271001E-10, 6.2869135601547004E-10 + 45, 0, 7.0816882022473439E-09, 0.0000000000000000E+00, 7.4580287026931005E-09, 0.0000000000000000E+00 + 45, 1,-1.3296976582754729E-08, 6.6534684936877042E-08, 7.4466498150470999E-09, 7.4720824806187997E-09 + 45, 2,-7.5859988876592400E-08,-5.9366772604899227E-08, 7.4501616340816998E-09, 7.4352565233383004E-09 + 45, 3,-3.1288336452636682E-08, 1.3994387397176570E-09, 7.4245913400217996E-09, 7.4321326792748997E-09 + 45, 4, 4.4801136225444247E-08,-1.3327022127010760E-08, 7.3839487144407003E-09, 7.4158219024254999E-09 + 45, 5, 5.5782576243611607E-08, 1.4549561383987430E-08, 7.3489053927805998E-09, 7.3763819455607999E-09 + 45, 6, 3.0991118828510422E-08, 4.4211678959369152E-08, 7.2523837884696003E-09, 7.3907447173556996E-09 + 45, 7, 3.9204071905402442E-08, 9.0555599872658693E-08, 7.2809829768070000E-09, 7.2401196669200000E-09 + 45, 8, 1.2025282909251610E-09, 2.5768597646997800E-08, 7.1819329447856002E-09, 7.2260509398192002E-09 + 45, 9,-4.1689886151007030E-08, 6.8031942837560232E-08, 7.0831509865671003E-09, 7.1528980839210004E-09 + 45, 10,-9.4904206100350488E-08,-9.3789931341861924E-08, 7.0443395178597996E-09, 7.0452051485728996E-09 + 45, 11,-3.4612560624975093E-08,-2.9884315131893713E-08, 6.9408932581039999E-09, 6.9309721015835996E-09 + 45, 12,-1.0900885608370560E-07,-1.2408142589623951E-08, 6.8387741348567996E-09, 6.8461954018499001E-09 + 45, 13,-6.1089332147779283E-08,-2.0836777079938710E-08, 6.7114240666236999E-09, 6.7215859510143998E-09 + 45, 14,-4.2805435245291702E-10, 6.3067690646383739E-08, 6.6012587136810003E-09, 6.6012059351700001E-09 + 45, 15,-1.0093677530041090E-08,-1.1425541970500419E-07, 6.4506402340688997E-09, 6.4631806765927998E-09 + 45, 16, 7.2495705193027511E-08, 3.5989321277322470E-08, 6.3307528932015998E-09, 6.3226914074112999E-09 + 45, 17, 8.5104685390003286E-09,-6.2755562162705310E-08, 6.1691160315479996E-09, 6.1735979979674001E-09 + 45, 18, 6.0500033515889764E-08,-3.1282446374317503E-08, 6.0242032162173004E-09, 6.0194344580088003E-09 + 45, 19, 2.3247088768756169E-08,-8.9575735474308089E-09, 5.8650547729307998E-09, 5.8678391036225002E-09 + 45, 20,-3.1738876652873501E-09,-2.9464093916811920E-08, 5.6994710766570002E-09, 5.6965666440148003E-09 + 45, 21, 4.7755250994173034E-09, 5.5020158294225963E-08, 5.5397086671470001E-09, 5.5365841056229000E-09 + 45, 22, 5.7477301802602071E-08,-1.0082689121919501E-08, 5.3667964387384999E-09, 5.3653548171760003E-09 + 45, 23, 4.3160171732370847E-08,-4.2464669436463657E-08, 5.1983816440709998E-09, 5.1971506119605000E-09 + 45, 24,-8.6949120978425074E-09, 2.2566936447842442E-08, 5.0369979958771003E-09, 5.0171581758793996E-09 + 45, 25, 4.8261549571777717E-08,-6.3145875372852851E-08, 4.8512516802564998E-09, 4.8564161990824004E-09 + 45, 26,-1.3781083666783460E-08, 2.2072431906074442E-08, 4.6863688586216000E-09, 4.6747605755306999E-09 + 45, 27,-4.6343005056650343E-08,-3.7589942916291363E-09, 4.5106581504901997E-09, 4.5047958927596003E-09 + 45, 28, 7.8148187644440609E-08, 3.7010142461081297E-08, 4.3333489829245001E-09, 4.3314819787330002E-09 + 45, 29,-1.6483929470657571E-08,-1.5000358657275531E-08, 4.1545428797124001E-09, 4.1533458908685004E-09 + 45, 30, 3.6480077514276101E-08,-4.3632243838042597E-08, 3.9866718282299000E-09, 3.9923025173569003E-09 + 45, 31,-9.6047661469233808E-08,-3.0622063205300112E-08, 3.8021254499815003E-09, 3.8112213389371000E-09 + 45, 32,-3.0754307390408008E-08, 4.6331145159682542E-08, 3.6302118576173001E-09, 3.6350899529596999E-09 + 45, 33,-9.7621921347122933E-08, 6.5918633199269810E-08, 3.4494136181665000E-09, 3.4626210934545001E-09 + 45, 34, 5.0261918034275291E-08, 1.8605801408433561E-08, 3.2794599207675002E-09, 3.2719353617325999E-09 + 45, 35,-3.0535133943112557E-08, 2.2722675079391121E-08, 3.1048439504128998E-09, 3.1052041109103002E-09 + 45, 36, 3.0922367051313182E-08,-7.9941687861232612E-08, 2.9129752966724999E-09, 2.9144912771194000E-09 + 45, 37,-1.1239119887232200E-07, 5.1675783240508652E-08, 2.7412540626955001E-09, 2.7361891459877000E-09 + 45, 38,-2.9880709851304671E-08,-3.7096821704852383E-08, 2.5558368756345000E-09, 2.5539872496561999E-09 + 45, 39, 9.0423983289436129E-08,-1.6802746784832989E-08, 2.3602893136554000E-09, 2.3618425830073000E-09 + 45, 40, 2.0771503371137012E-09,-5.0700473636701027E-08, 2.1646963324294999E-09, 2.1727054598575000E-09 + 45, 41,-9.1701384497857444E-08,-1.2205899401810210E-07, 1.9440690819096001E-09, 1.9486359397646999E-09 + 45, 42,-4.2217657939705328E-08, 7.9912712116935674E-08, 1.7440517978280999E-09, 1.7457564627471000E-09 + 45, 43,-1.0311655878126590E-08,-9.3263166085774407E-08, 1.4644645667373000E-09, 1.4607127673898001E-09 + 45, 44, 2.7671029844338330E-08,-9.2947130324559163E-09, 9.6365618254225018E-10, 9.4757371820821998E-10 + 45, 45, 1.2207329320214070E-07,-1.3131956375791811E-07, 6.6941480365647006E-10, 6.7304176718417009E-10 + 46, 0, 7.5987367097924869E-08, 0.0000000000000000E+00, 8.3053668021114993E-09, 0.0000000000000000E+00 + 46, 1,-2.5915102522951452E-08, 2.7471214407565709E-08, 8.2770329627909006E-09, 8.3085952455978004E-09 + 46, 2,-1.9650576990196731E-08, 3.9340574054676623E-08, 8.2935601618923005E-09, 8.2811004224602007E-09 + 46, 3,-4.9506857631354971E-08, 4.3105646467268897E-08, 8.2545022014127999E-09, 8.2663253103632005E-09 + 46, 4,-1.5782825623008170E-09, 2.6671432486938089E-08, 8.2249948762118002E-09, 8.2568385378070002E-09 + 46, 5, 1.7502450665273479E-08, 3.8338098547694833E-09, 8.1764869684272998E-09, 8.2097395500283004E-09 + 46, 6, 1.5814069559538240E-08,-5.2784424123614487E-08, 8.0768459768524001E-09, 8.2321338637521996E-09 + 46, 7, 4.7768073377267318E-09, 2.4809146636188650E-08, 8.1157869331198994E-09, 8.0623846076595004E-09 + 46, 8, 7.6139431128016741E-08,-1.8629588565592900E-08, 8.0005635501464993E-09, 8.0513361440412995E-09 + 46, 9, 5.0854819120122321E-08, 1.4526835821870110E-08, 7.9035811196245999E-09, 7.9836323962919007E-09 + 46, 10, 2.8101249933856340E-08,-1.0860961990170790E-08, 7.8520745044879995E-09, 7.8527797304451999E-09 + 46, 11,-3.8525417399715807E-08, 6.0365548089748078E-08, 7.7639377001531004E-09, 7.7457987582069005E-09 + 46, 12,-1.2884142131903131E-07, 7.0348185116721761E-08, 7.6334810099850997E-09, 7.6375618554772993E-09 + 46, 13, 4.5803597678275481E-08, 7.2435849038390361E-08, 7.5171720607438005E-09, 7.5287487687497998E-09 + 46, 14,-1.0104159353350031E-07, 4.1296463071040362E-08, 7.3794326306473999E-09, 7.3782727633698998E-09 + 46, 15, 1.1226059133059190E-07,-3.4913567682375041E-09, 7.2385174862943001E-09, 7.2555621037341000E-09 + 46, 16,-8.4220283407263993E-08, 2.8839133016602710E-08, 7.0886097450461000E-09, 7.0829212922666001E-09 + 46, 17, 1.3135319549772810E-08,-6.1969901416681331E-08, 6.9404049115801998E-09, 6.9397199259124001E-09 + 46, 18,-4.0650277090905087E-08, 4.7434479194609827E-08, 6.7663352154121997E-09, 6.7622209130460996E-09 + 46, 19,-6.2657249028578554E-08,-4.0167648416253761E-09, 6.6002041007711996E-09, 6.6075417667100003E-09 + 46, 20, 5.0371007082258712E-08, 4.2177565690707537E-08, 6.4299179707206996E-09, 6.4210622424914996E-09 + 46, 21,-1.6105832537482660E-08,-1.7033245141205689E-08, 6.2482293398193003E-09, 6.2410612892534001E-09 + 46, 22, 3.2107379207394822E-08,-5.1936442954534707E-08, 6.0697023633325999E-09, 6.0659292712318000E-09 + 46, 23,-3.2155530442314387E-08,-3.8418176892369747E-08, 5.8807757271295003E-09, 5.8772103219672001E-09 + 46, 24,-6.0076479141730190E-08,-6.9435840605643939E-08, 5.7089132032935996E-09, 5.6842981082902999E-09 + 46, 25,-9.3487152177013471E-10,-4.0034290714103322E-08, 5.5086613020177999E-09, 5.5107917866121003E-09 + 46, 26,-1.2871097130107059E-08, 3.3678749728784060E-08, 5.3257582177330002E-09, 5.3112750558406003E-09 + 46, 27, 1.4742517463960399E-08,-3.2339406308604701E-08, 5.1328301390456000E-09, 5.1295395801021000E-09 + 46, 28, 3.6138205300870661E-09, 7.6558797730435231E-08, 4.9432053643993997E-09, 4.9406249626915998E-09 + 46, 29,-5.0684867347837111E-08,-8.2175164374130581E-08, 4.7522559179426003E-09, 4.7491590473171004E-09 + 46, 30,-2.5515291809644268E-08,-4.1661066427301202E-08, 4.5535319748714999E-09, 4.5625543867611003E-09 + 46, 31,-9.0500065907009915E-09, 5.6963804443523647E-08, 4.3731306340205001E-09, 4.3834219327166004E-09 + 46, 32, 2.9220048537814631E-08, 5.7653353868610242E-08, 4.1731565593020999E-09, 4.1797976021918996E-09 + 46, 33, 2.3061921547064679E-08, 1.2546490744583380E-08, 3.9833142184629997E-09, 3.9967508441705002E-09 + 46, 34, 6.6128195828951323E-08,-2.8573074141846971E-08, 3.7997396486597001E-09, 3.7919849748662001E-09 + 46, 35,-3.3832021781669332E-08,-6.8719335366843424E-08, 3.6004341793315001E-09, 3.5984050251703999E-09 + 46, 36,-8.7462068973516514E-09, 5.9510942291544211E-08, 3.4126128539801999E-09, 3.4106272054872000E-09 + 46, 37,-1.2365684344913969E-07, 3.8299187959994572E-08, 3.2036725152979001E-09, 3.2010442915127002E-09 + 46, 38, 5.1064888612002412E-08, 3.3655627843064681E-08, 3.0146194318339002E-09, 3.0128347459582000E-09 + 46, 39,-2.7508648084207650E-08, 8.6354543160772033E-09, 2.8068162942188001E-09, 2.8049091317473000E-09 + 46, 40,-7.3551417071972269E-08,-3.1395531557854011E-08, 2.5987342664421001E-09, 2.5974583012214001E-09 + 46, 41,-1.7118756362328690E-08,-7.3069559603513099E-08, 2.3863584388643998E-09, 2.3866820021184001E-09 + 46, 42,-6.1193128608632982E-08, 1.6768805906150490E-07, 2.1426151552006999E-09, 2.1433554615214002E-09 + 46, 43, 5.0418991655279243E-08,-3.8740377358284123E-08, 1.9242012357268000E-09, 1.9221222195688999E-09 + 46, 44,-8.9823856959702204E-09, 5.4720559910589031E-08, 1.6119932297693000E-09, 1.6093645421290000E-09 + 46, 45, 1.7942334640991878E-08, 2.0342167500590872E-08, 1.0496166669352001E-09, 1.0463613103364999E-09 + 46, 46,-1.5459355307663359E-07,-6.1676338688784440E-08, 7.5474634607639003E-10, 7.5357071817490003E-10 + 47, 0, 1.5614301951303130E-08, 0.0000000000000000E+00, 9.2308285386458997E-09, 0.0000000000000000E+00 + 47, 1, 2.3609025745226040E-08,-1.0388620820807531E-07, 9.2076274203086001E-09, 9.2396467641682004E-09 + 47, 2, 2.9646393062244212E-09, 5.6639972735709062E-08, 9.2209014635769004E-09, 9.2012985938049996E-09 + 47, 3, 5.2704918819669973E-08,-1.3794245411362680E-09, 9.1850330783676998E-09, 9.1956304559128994E-09 + 47, 4, 8.1851609676822241E-08, 6.6168165171316149E-08, 9.1395365208593003E-09, 9.1812674381112007E-09 + 47, 5,-3.7402201275496719E-09,-3.0625354541469449E-09, 9.1023524941452000E-09, 9.1397019071456001E-09 + 47, 6,-2.0413793975060600E-09,-1.7822421928175191E-08, 8.9786650744105007E-09, 9.1561738623577006E-09 + 47, 7,-2.6096874623112950E-08,-1.1144131567663740E-07, 9.0341807865165008E-09, 8.9893406411892002E-09 + 47, 8, 1.3818880298297440E-08,-3.3102612010965580E-08, 8.9034487705110007E-09, 8.9593703233408001E-09 + 47, 9, 1.9372544607452760E-08,-6.8453890897754640E-08, 8.8090015453294001E-09, 8.9032607234428006E-09 + 47, 10, 3.8836813477129413E-08,-2.5974803105918101E-08, 8.7471499664884008E-09, 8.7553638384027994E-09 + 47, 11, 1.1656363575311950E-08, 4.2575337716427072E-08, 8.6594585695016992E-09, 8.6432890994529999E-09 + 47, 12, 6.5057914860563920E-08,-1.9250824541312060E-08, 8.5209180846839000E-09, 8.5338647648507007E-09 + 47, 13, 1.1995568065751530E-08, 1.1701764231398260E-07, 8.3930540500798007E-09, 8.4038738114510994E-09 + 47, 14, 2.1456691067849820E-08,-4.7139713071279618E-08, 8.2562222470633005E-09, 8.2658063054626996E-09 + 47, 15, 1.7524858871249200E-08, 6.1399267272348140E-08, 8.0945197387208007E-09, 8.1090450669894001E-09 + 47, 16,-1.1249604113748950E-08,-1.7713757486640639E-08, 7.9540008397590994E-09, 7.9482242627827005E-09 + 47, 17,-2.1454349663442841E-09, 3.7108029914748192E-08, 7.7714460942300997E-09, 7.7722428631594994E-09 + 47, 18,-5.6041038707306787E-08, 7.6481633006867533E-08, 7.6146607215393994E-09, 7.6017653321920005E-09 + 47, 19, 5.2653009475560343E-08,-4.9927921104807953E-08, 7.4117373549378004E-09, 7.4148504143118004E-09 + 47, 20,-6.4701501854907848E-09, 5.3749854165845902E-08, 7.2407911223855998E-09, 7.2373446592245002E-09 + 47, 21, 1.7267930904419589E-08,-4.9606225509938342E-08, 7.0399300938140999E-09, 7.0347284193213998E-09 + 47, 22,-6.4399127634675531E-08,-2.0592778131918201E-08, 6.8489628718932999E-09, 6.8399643948823996E-09 + 47, 23,-6.1265781995959141E-08, 5.3791358775832221E-08, 6.6477347069731003E-09, 6.6454594249420003E-09 + 47, 24,-6.0731889860468238E-08,-5.4278119146964663E-08, 6.4527605107044001E-09, 6.4329855759990997E-09 + 47, 25,-2.4833438094629802E-08, 2.7271716452670110E-08, 6.2406126502433996E-09, 6.2496242354982003E-09 + 47, 26, 1.1324794090940210E-09,-1.9916398660419091E-08, 6.0434797129407003E-09, 6.0281453541346998E-09 + 47, 27, 2.5162579317479261E-08,-1.2885048781083690E-08, 5.8355751913820001E-09, 5.8246923313632002E-09 + 47, 28,-6.7332273243277863E-08, 1.1681170810699170E-08, 5.6266282430951002E-09, 5.6253680925565001E-09 + 47, 29,-9.5616818536305902E-09,-1.1275646948486110E-07, 5.4194248607901999E-09, 5.4184690227782999E-09 + 47, 30,-4.8655336502165643E-08, 4.4966992296574312E-08, 5.2059312873363000E-09, 5.2142584359664004E-09 + 47, 31, 3.2232108668396122E-08,-8.7299695372077168E-09, 4.9972272251233997E-09, 5.0105637649557002E-09 + 47, 32, 5.9907918671449982E-08, 1.3308402668684691E-08, 4.8018449526676996E-09, 4.8072219596262003E-09 + 47, 33, 4.1878874831374902E-08,-2.0952992929531140E-08, 4.5765461146223003E-09, 4.5916670420600997E-09 + 47, 34,-8.5950044430708048E-09,-1.0430897798193800E-07, 4.3870289540180000E-09, 4.3767005856342002E-09 + 47, 35, 2.8934072637661000E-08,-3.4453397542848681E-08, 4.1708675369820000E-09, 4.1666793774728996E-09 + 47, 36,-2.6326153823111859E-08, 5.3982163209910893E-08, 3.9541947445926999E-09, 3.9593431640908003E-09 + 47, 37, 5.7671174914856917E-08,-6.3325244439165243E-09, 3.7536429688758004E-09, 3.7477016175814996E-09 + 47, 38, 1.7891910708106471E-08, 6.3276885864226668E-08, 3.5259242235726000E-09, 3.5213813158972001E-09 + 47, 39, 7.0311880456086892E-08,-4.8770149122363111E-08, 3.3102469147118002E-09, 3.3146232634387998E-09 + 47, 40,-1.6027597113483761E-08, 3.3173933296197851E-08, 3.0842952047202999E-09, 3.0856281930724001E-09 + 47, 41,-1.6628981842619978E-08, 5.5436772513276471E-08, 2.8591424278772998E-09, 2.8621177236146000E-09 + 47, 42, 5.7797659469229983E-09, 5.8593961667630912E-08, 2.6255271476004000E-09, 2.6284674566479999E-09 + 47, 43, 1.3859206998216579E-07, 2.9646485641977078E-09, 2.3606925021418998E-09, 2.3599348575995999E-09 + 47, 44,-1.9102331435626339E-08, 9.8336413661396019E-10, 2.1066412056284000E-09, 2.1320734112382002E-09 + 47, 45,-2.0243580815915009E-08, 1.1855552840696251E-08, 1.7772211344349000E-09, 1.7722137056639000E-09 + 47, 46,-5.0494110664638594E-09,-5.9960208767685802E-08, 1.1542744415681999E-09, 1.1463360009904001E-09 + 47, 47, 3.9016634733085787E-08, 1.4013084125490451E-07, 8.2554790793524015E-10, 8.2321638999531012E-10 + 48, 0,-6.3750241134827699E-08, 0.0000000000000000E+00, 1.0259158823852000E-08, 0.0000000000000000E+00 + 48, 1,-1.9762579165320080E-08,-7.1935622896234082E-08, 1.0227434500870000E-08, 1.0260174772260000E-08 + 48, 2,-6.7394142368719490E-08,-3.6465683234284137E-08, 1.0247048485258000E-08, 1.0225478723472000E-08 + 48, 3, 3.1501252776408332E-08,-1.7251413095420339E-08, 1.0199245844961000E-08, 1.0218295733749000E-08 + 48, 4, 8.7113213298670963E-08, 1.4822951110727410E-08, 1.0160260144403001E-08, 1.0203590205489000E-08 + 48, 5,-1.8438600405829319E-08,-2.7758658523185819E-08, 1.0117431386801001E-08, 1.0154562989101001E-08 + 48, 6, 2.2167200974868909E-10, 6.3690413352121522E-08, 9.9862282610952995E-09, 1.0180139382934001E-08 + 48, 7, 2.4290147206032891E-08, 4.1319260120170911E-09, 1.0047557428616999E-08, 9.9915392197516997E-09 + 48, 8,-5.4724685141463572E-08, 1.6965723165524921E-08, 9.9107987845498996E-09, 9.9732403628553006E-09 + 48, 9,-2.6484662888791281E-08, 1.9297319491991659E-08, 9.8033840686030001E-09, 9.9003938883756002E-09 + 48, 10,-2.7412167214822730E-08,-2.7440515466141559E-08, 9.7547149601343005E-09, 9.7531942114681995E-09 + 48, 11,-1.6168441375188821E-08,-8.6346984004787953E-08, 9.6414494346891005E-09, 9.6271409352103004E-09 + 48, 12, 1.0729327872635720E-07,-4.5846866530718681E-08, 9.5150367698897006E-09, 9.5208076526378992E-09 + 48, 13,-4.4650408335363178E-09,-5.8636496229109702E-08, 9.3623656821377002E-09, 9.3751614646196007E-09 + 48, 14, 5.7722413946988117E-08, 3.5568091087407127E-08, 9.2253822589712007E-09, 9.2305020835311005E-09 + 48, 15,-9.7569047018103591E-10, 1.9261911766125070E-08, 9.0542953642801006E-09, 9.0690846300744000E-09 + 48, 16, 7.3595596989960582E-08,-3.3891614405056541E-08, 8.8941124524424000E-09, 8.8878111195082004E-09 + 48, 17,-1.9339901087533729E-08, 5.4579615546871717E-08, 8.7139475490963003E-09, 8.7182790827953004E-09 + 48, 18, 3.1243176920553012E-08,-3.6656109603871737E-08, 8.5227963813165998E-09, 8.5183016863941999E-09 + 48, 19, 7.9703081102276183E-08, 5.6803461722788712E-09, 8.3363891014164998E-09, 8.3354061522043001E-09 + 48, 20,-1.0017600934077501E-09,-6.5581858147522963E-08, 8.1250259546058996E-09, 8.1194737621496994E-09 + 48, 21, 2.5460022420247709E-08, 3.3248858049502239E-09, 7.9329433777085007E-09, 7.9286994805400992E-09 + 48, 22,-3.4140412149492301E-08,-4.9924233757748613E-09, 7.7055574536742999E-09, 7.7061365617312998E-09 + 48, 23,-3.8433009085633043E-08, 6.4918412890670270E-08, 7.4998058409392004E-09, 7.4983001726151007E-09 + 48, 24, 2.3229396021088109E-08, 3.3357193677674663E-08, 7.2920992655258999E-09, 7.2677772249955000E-09 + 48, 25, 6.0057041403143620E-09, 6.0435785326037792E-08, 7.0565639448915996E-09, 7.0667455942011997E-09 + 48, 26, 3.6040279595973920E-08,-5.8091946101487402E-08, 6.8499275139059998E-09, 6.8310556003603997E-09 + 48, 27,-1.2785461164684380E-08, 4.5379759717073297E-08, 6.6187889797227998E-09, 6.6071729314955997E-09 + 48, 28,-4.2663598426513397E-08,-9.8472372395266584E-09, 6.3950221129029000E-09, 6.3883472698864002E-09 + 48, 29, 7.5456155750597337E-09, 4.6154199826600671E-08, 6.1702809230552998E-09, 6.1665175099044000E-09 + 48, 30,-3.8421961280628443E-08, 8.7862016596813804E-09, 5.9379454638974002E-09, 5.9462037251829003E-09 + 48, 31, 9.6022131165956568E-08,-2.6932469663205360E-08, 5.7123621706353999E-09, 5.7202095346676001E-09 + 48, 32,-1.7141580496130462E-08, 1.1830337989638490E-08, 5.4886250401256001E-09, 5.4954383413700004E-09 + 48, 33,-2.9458560564814932E-09,-6.5394256683156383E-08, 5.2636669894201000E-09, 5.2828392681144002E-09 + 48, 34,-3.6433567418369152E-08,-3.9284748074609782E-08, 5.0359505301791002E-09, 5.0280768764745997E-09 + 48, 35,-1.0809840538152090E-08, 1.7315051787370481E-08, 4.8138350268272999E-09, 4.8148664059107002E-09 + 48, 36,-4.4599978135483233E-08, 9.5828716675089686E-09, 4.5817070373405998E-09, 4.5795801176617996E-09 + 48, 37, 8.6617633062467059E-08,-9.6995202952304684E-09, 4.3531094481756998E-09, 4.3429920926809996E-09 + 48, 38, 2.1414062279445689E-10,-6.9569369141697913E-08, 4.1245953616188003E-09, 4.1217433916614997E-09 + 48, 39, 4.0488623371387732E-08, 3.0377216890451748E-08, 3.8712817046300004E-09, 3.8713115094175998E-09 + 48, 40,-2.8874987367371591E-08,-3.5603438761853533E-08, 3.6425082155666002E-09, 3.6433611942538998E-09 + 48, 41, 5.1180716729067438E-11, 4.0577891870018696E-09, 3.3928034203435000E-09, 3.3964307776526002E-09 + 48, 42, 1.2344088160826949E-07, 2.9193576849276589E-08, 3.1491706441176999E-09, 3.1481296425514998E-09 + 48, 43, 1.6155109205826309E-08,-1.1546514332246049E-08, 2.8913096110682001E-09, 2.8932509211199001E-09 + 48, 44,-3.9025840695115792E-08,-1.4568607931726140E-07, 2.5881587356603000E-09, 2.6125641206578002E-09 + 48, 45, 3.7749362680766513E-08, 6.3010509122220051E-08, 2.3417356387534002E-09, 2.3349401924075001E-09 + 48, 46,-4.6298426798492982E-08,-2.6796519091463439E-08, 1.9463723322822000E-09, 1.9483855424161000E-09 + 48, 47,-5.5160436640001762E-08,-2.5423993864680618E-09, 1.2506965804749000E-09, 1.2536668715835000E-09 + 48, 48, 1.2771364094735550E-07,-3.2473999675112073E-08, 9.0062428330639009E-10, 9.0967562717278009E-10 + 49, 0,-2.4331222300097099E-08, 0.0000000000000000E+00, 1.1376179369261000E-08, 0.0000000000000000E+00 + 49, 1,-4.2165875083441413E-08, 6.9326640864386959E-08, 1.1359843304109999E-08, 1.1389886009650000E-08 + 49, 2,-4.0787318991428403E-08,-1.4485001510092071E-08, 1.1363565419930001E-08, 1.1340011617514001E-08 + 49, 3,-3.5424948755476082E-08, 4.5842173279045727E-08, 1.1330082156481000E-08, 1.1346037802739000E-08 + 49, 4,-1.6375765390998910E-08,-1.7169242814551180E-08, 1.1271771427673000E-08, 1.1316080487770000E-08 + 49, 5,-1.4320990741186261E-08,-6.1774980609956594E-09, 1.1240444905649000E-08, 1.1279948552072000E-08 + 49, 6, 4.6681329671375091E-08,-3.7779336068049242E-08, 1.1087003414497000E-08, 1.1295710155314000E-08 + 49, 7, 4.5394108538369321E-08, 7.0940107173856414E-08, 1.1166156033258000E-08, 1.1102773417093000E-08 + 49, 8, 9.6480660913728943E-09, 5.0978431524877972E-08, 1.1010356398125000E-08, 1.1078317364130000E-08 + 49, 9, 1.4046426722771679E-08, 9.1241896197010133E-08, 1.0900801938244999E-08, 1.1010219687807999E-08 + 49, 10,-7.2810281401773550E-09, 5.8187739726673613E-08, 1.0842716413072999E-08, 1.0849207957937001E-08 + 49, 11, 1.3885509441237339E-08,-3.5712294965069268E-09, 1.0736515242617000E-08, 1.0717518155846000E-08 + 49, 12,-3.6000634499406313E-08, 4.3808379732011712E-08, 1.0590753497960999E-08, 1.0595124450052001E-08 + 49, 13,-1.5741494034640448E-08,-8.3960012704327122E-08, 1.0444121921213000E-08, 1.0455996788047000E-08 + 49, 14,-6.6257106865837234E-08, 1.2424856734831949E-07, 1.0283612196586001E-08, 1.0284152884216000E-08 + 49, 15,-1.7857708005444810E-08,-1.3708029051145311E-08, 1.0114064174704001E-08, 1.0132512104993000E-08 + 49, 16, 4.1730099054416340E-08, 3.2514403827523411E-08, 9.9360630176381995E-09, 9.9274827920424995E-09 + 49, 17,-2.6757273808305139E-08,-1.8634423688545310E-08, 9.7471987275226995E-09, 9.7478761893825008E-09 + 49, 18, 4.8511057886281938E-08,-5.7418690964179212E-08, 9.5533125260720992E-09, 9.5373120350762000E-09 + 49, 19,-3.7159318539262001E-09,-9.1865210832788195E-09, 9.3340512023346999E-09, 9.3365833432671004E-09 + 49, 20,-3.6481146158293942E-08,-2.8303741666207851E-08, 9.1325273095475003E-09, 9.1203090201938999E-09 + 49, 21,-3.4098631562893672E-08, 1.2958362047488291E-08, 8.8961204509837992E-09, 8.8945829004208006E-09 + 49, 22,-2.4433338455858041E-08, 2.5669387356113341E-08, 8.6886262763832996E-09, 8.6784422100524004E-09 + 49, 23, 4.1673657485320868E-08,-1.5824983437764751E-08, 8.4379056919616993E-09, 8.4333171924361996E-09 + 49, 24, 2.0141023271968491E-08, 3.4757300406351107E-08, 8.2303972345257007E-09, 8.2012809433585995E-09 + 49, 25, 1.9459015678262761E-08, 2.0867319047319191E-08, 7.9674066046015992E-09, 7.9776138892338007E-09 + 49, 26, 3.8516365473709210E-08,-2.6967572635483520E-08, 7.7447888305233002E-09, 7.7248547457273993E-09 + 49, 27,-3.9670960758801481E-08, 2.5448201469517290E-08, 7.4994672109480000E-09, 7.4845038477497994E-09 + 49, 28, 4.9114401080034791E-08, 2.4976605015527830E-09, 7.2472278623296003E-09, 7.2450107445372999E-09 + 49, 29, 1.8999776588324139E-10, 9.3229817405744326E-08, 7.0047146898463999E-09, 7.0099113831286996E-09 + 49, 30, 3.5213090409624863E-08,-3.1789697680608412E-08, 6.7560970034973003E-09, 6.7679909079842998E-09 + 49, 31, 5.2590207307942507E-08, 1.6147468022602829E-08, 6.5056204434223998E-09, 6.5261445380567996E-09 + 49, 32,-5.6956279664886933E-08,-2.8996340502817620E-08, 6.2647249865473998E-09, 6.2759487891727001E-09 + 49, 33, 2.0967857635557160E-08,-2.1375051060920139E-08, 6.0174728638906997E-09, 6.0408312414718002E-09 + 49, 34,-3.8317462542831381E-08, 4.4978986938763873E-08, 5.7924878830633999E-09, 5.7808457274777002E-09 + 49, 35,-2.4824498682147762E-09, 1.2600487390317820E-08, 5.5254733311894002E-09, 5.5235420152104998E-09 + 49, 36,-7.0874264428959780E-09,-2.5978606803980960E-08, 5.2870107116159002E-09, 5.2923336509363999E-09 + 49, 37, 7.8179489380899363E-09, 6.4085105242359808E-09, 5.0369044414813003E-09, 5.0299460885605996E-09 + 49, 38,-2.5967217013353081E-08,-8.1934023027526323E-08, 4.7830285360344001E-09, 4.7826947662573002E-09 + 49, 39,-4.9024037418913212E-08, 7.2450723366859531E-08, 4.5292568299986003E-09, 4.5284039859686997E-09 + 49, 40,-6.2710985808800299E-09,-3.8441943156038417E-08, 4.2525557913590000E-09, 4.2583760966264000E-09 + 49, 41,-1.7518318450902211E-08, 1.2185790948282380E-08, 4.0047226259989999E-09, 4.0120944980209002E-09 + 49, 42, 2.5884031949442859E-08,-1.8558769655220920E-08, 3.7321887465186000E-09, 3.7354870708992002E-09 + 49, 43,-2.7887373814444430E-08,-7.8158379468500980E-08, 3.4666188389970000E-09, 3.4680804852004001E-09 + 49, 44,-4.5700968787527781E-08,-4.3979653544466968E-08, 3.1754799563285999E-09, 3.1971139297916000E-09 + 49, 45,-7.7171616902294059E-08, 6.6129588102413759E-08, 2.8732081196556999E-09, 2.8597516693852000E-09 + 49, 46, 4.6336356352455108E-08,-8.8822820888147182E-08, 2.5794752660813999E-09, 2.5718741719767002E-09 + 49, 47,-2.9101571862767379E-08, 6.2689944831698471E-08, 2.1508586841130998E-09, 2.1515733579103002E-09 + 49, 48, 1.4530398741670231E-08, 3.8905623266605453E-08, 1.3750623808999000E-09, 1.3684604552421000E-09 + 49, 49,-1.0120468302635470E-07,-9.5048582292414182E-08, 1.0126462086787999E-09, 1.0282894004119000E-09 + 50, 0, 5.5518316183221382E-08, 0.0000000000000000E+00, 1.2612468155256000E-08, 0.0000000000000000E+00 + 50, 1,-8.9146895385472082E-09, 3.5516031480898243E-08, 1.2585260976620999E-08, 1.2615934275477001E-08 + 50, 2, 3.2929035769255971E-08, 5.1659029521868433E-08, 1.2596207503556001E-08, 1.2570950553236001E-08 + 50, 3,-2.5459565398316901E-08, 1.6146472722275330E-08, 1.2549626899989999E-08, 1.2571624935515001E-08 + 50, 4,-1.4219443845740400E-08,-9.5051567762534860E-09, 1.2492683117462001E-08, 1.2554152562496000E-08 + 50, 5,-4.3864879845661912E-08, 1.7470830600592629E-08, 1.2450794948039000E-08, 1.2501744220969000E-08 + 50, 6, 4.9009673284828942E-08,-3.3186764017648152E-08, 1.2300140517216000E-08, 1.2533637339213000E-08 + 50, 7,-1.2112504086960641E-08,-3.2168636180768721E-08, 1.2375917727130999E-08, 1.2306316228101001E-08 + 50, 8, 4.8350478013820593E-08,-3.8834493493167811E-09, 1.2226698492290001E-08, 1.2298928415111000E-08 + 50, 9, 3.9596880587795677E-08,-1.3518411780585161E-08, 1.2097370020938001E-08, 1.2208398335796000E-08 + 50, 10,-2.7647989802063939E-08,-2.4268348504982711E-08, 1.2052247506341000E-08, 1.2050404450118000E-08 + 50, 11, 1.6424064699651550E-08, 7.8566205443877140E-08, 1.1927882651679000E-08, 1.1901405587369000E-08 + 50, 12,-5.4725277778664452E-08,-1.5712145980743479E-08, 1.1777404518012999E-08, 1.1788098098493999E-08 + 50, 13, 3.1639905362049999E-09, 8.4168853825395041E-08, 1.1613046777446000E-08, 1.1633911071162001E-08 + 50, 14,-5.8205650492157793E-08, 7.6394323676880621E-08, 1.1454819144122999E-08, 1.1459061855300000E-08 + 50, 15, 3.3743293316659063E-08, 2.3929876088189739E-08, 1.1261702168767000E-08, 1.1285362518115999E-08 + 50, 16,-2.2172186144013370E-08, 4.1873047424387163E-08, 1.1091395581999000E-08, 1.1085111108443000E-08 + 50, 17,-6.6914992091900333E-09,-8.1120443195919680E-08, 1.0876626054391000E-08, 1.0873927189378000E-08 + 50, 18,-4.2969424993123567E-08, 3.9802656858974972E-08, 1.0676145891736000E-08, 1.0669740888923999E-08 + 50, 19,-4.9307579503998542E-08, 5.2911081209436636E-09, 1.0442592991032000E-08, 1.0445980381447001E-08 + 50, 20,-5.7844168993620931E-08,-1.3151307173354939E-08, 1.0224906853397001E-08, 1.0212035849105999E-08 + 50, 21,-3.9428044790698978E-08, 4.5053087140207853E-08, 9.9859825312458000E-09, 9.9790049262438992E-09 + 50, 22, 2.2083169662579771E-08, 5.5110709458346997E-09, 9.7361887700577006E-09, 9.7352078487261998E-09 + 50, 23,-2.9476540976816099E-09,-2.4761393210521561E-08, 9.4982956872500995E-09, 9.4995110894743008E-09 + 50, 24,-2.3167380785832789E-08,-2.1843917543260049E-08, 9.2440201357062005E-09, 9.2194071051455999E-09 + 50, 25,-1.5149295783642759E-08,-2.9214677020141229E-08, 8.9924127846586001E-09, 8.9980849813800994E-09 + 50, 26, 3.1525034017933167E-08,-4.3600221408865610E-09, 8.7342212064386002E-09, 8.7130134857807994E-09 + 50, 27,-1.9637544899872369E-09,-2.9879265242401342E-08, 8.4760377710076998E-09, 8.4609933894069001E-09 + 50, 28, 3.7760142501198012E-08, 2.2345745664451401E-08, 8.2066091322914004E-09, 8.1988083560779002E-09 + 50, 29,-3.3379962195498513E-08,-1.6147016813120740E-08, 7.9421688710552002E-09, 7.9353685966617998E-09 + 50, 30, 5.5626822040388273E-08,-6.4943913200274539E-08, 7.6734419666758004E-09, 7.6834380684175005E-09 + 50, 31,-1.3183561849283790E-08,-1.6651967391241049E-08, 7.4017027038639999E-09, 7.4216959967212001E-09 + 50, 32, 3.3014021158889700E-08,-1.4714356587998761E-09, 7.1356661886079996E-09, 7.1519633529866001E-09 + 50, 33,-3.0397622393980882E-08, 6.0367040462299743E-08, 6.8696019566927003E-09, 6.8903694740381998E-09 + 50, 34,-1.3876658697445199E-08, 5.4049742497374843E-08, 6.6209958933704002E-09, 6.6116360906557002E-09 + 50, 35,-1.6061381334190741E-08, 1.1444145848826811E-08, 6.3513537602618004E-09, 6.3485801285423001E-09 + 50, 36, 3.8460501215838288E-08, 2.0165185707331649E-08, 6.0665389006140998E-09, 6.0668124779912001E-09 + 50, 37,-2.2900232600051399E-08,-1.7093082517859330E-08, 5.8167434664348001E-09, 5.8072074021301003E-09 + 50, 38,-1.3077862187704181E-08, 3.5588033623319080E-08, 5.5345901821055998E-09, 5.5270463641465003E-09 + 50, 39,-3.0798378590850622E-08, 4.4230857411351352E-08, 5.2543621173034999E-09, 5.2526951397174001E-09 + 50, 40, 5.7326599062175503E-08,-3.0659745719022300E-08, 4.9773256570935999E-09, 4.9775515581820001E-09 + 50, 41,-1.0191812185934289E-08, 9.3692087639537567E-09, 4.6780253086252997E-09, 4.6844040319575999E-09 + 50, 42,-1.3088127341329980E-08,-4.2668539015090678E-08, 4.4067259307626001E-09, 4.4123946598855000E-09 + 50, 43,-6.4205845483577124E-08, 2.8128874072328060E-08, 4.1074892888302001E-09, 4.1096505415733998E-09 + 50, 44, 1.7331881280579619E-08, 4.5832344442830367E-08, 3.8218082246977001E-09, 3.8188993954170004E-09 + 50, 45,-5.4902599234380482E-08, 2.8644337935898591E-08, 3.5096507956676999E-09, 3.5082207062754999E-09 + 50, 46, 7.7656580301768369E-08,-1.3772036351399739E-10, 3.1591518045200002E-09, 3.1541705664860002E-09 + 50, 47,-8.3828906326495605E-08,-3.1106957533502582E-08, 2.8480769154245000E-09, 2.8436174225307999E-09 + 50, 48, 8.5158050861342004E-08, 6.3548965974157246E-09, 2.3573487915759000E-09, 2.3519304944844998E-09 + 50, 49, 2.7467922569274171E-08,-1.3942102845328860E-08, 1.4738128149217000E-09, 1.4571884878495001E-09 + 50, 50,-6.1739297428029809E-08, 7.7790588732529842E-08, 1.1460656731844000E-09, 1.1547962420840001E-09 + 51, 0,-1.4040563887360571E-08, 0.0000000000000000E+00, 1.3945716248327000E-08, 0.0000000000000000E+00 + 51, 1,-2.5807225837876931E-08,-3.9502450870680833E-08, 1.3922458161797000E-08, 1.3958685077531000E-08 + 51, 2,-1.0426422018221650E-08,-2.4233504694116291E-08, 1.3931888682031000E-08, 1.3896683671589999E-08 + 51, 3, 4.1289145072110287E-08,-3.4950981016614383E-08, 1.3887312785153001E-08, 1.3907996037498000E-08 + 51, 4, 3.1731977629578743E-08,-1.1510435654774860E-08, 1.3826095874057000E-08, 1.3875429080391999E-08 + 51, 5,-2.0374747295268731E-08, 1.8336636626905089E-08, 1.3783494558751000E-08, 1.3830731541838000E-08 + 51, 6,-1.1842505205910181E-08, 1.8899193568128690E-08, 1.3612179725785001E-08, 1.3865360316225000E-08 + 51, 7,-4.6910135166158802E-08,-3.9728889660761183E-09, 1.3702321141793999E-08, 1.3626172324043000E-08 + 51, 8,-5.3334369559299073E-08, 4.1863687795779367E-09, 1.3532775415876000E-08, 1.3617044485129000E-08 + 51, 9, 4.1061575767198101E-09,-3.4744177361346277E-08, 1.3407462144696000E-08, 1.3530653388556999E-08 + 51, 10,-2.4682452132809930E-08,-5.0630330784990428E-08, 1.3342909491249000E-08, 1.3352462795288000E-08 + 51, 11,-1.9516148426216509E-08,-2.7020393197178559E-09, 1.3231266120217001E-08, 1.3209250839618001E-08 + 51, 12, 2.8497967307574241E-08,-6.4052101808591779E-08, 1.3062073616355000E-08, 1.3066243377362000E-08 + 51, 13,-5.1042199599653271E-08, 6.8485976934800199E-08, 1.2901219292467000E-08, 1.2923574598862000E-08 + 51, 14, 2.4822824666958461E-08,-3.7680007439771747E-08, 1.2720036429506000E-08, 1.2730543109734999E-08 + 51, 15, 5.6901186025051113E-08, 3.3954835076779923E-08, 1.2529993368020000E-08, 1.2552197013346000E-08 + 51, 16, 4.7821133590500438E-08,-1.0500741483096220E-08, 1.2338485198274999E-08, 1.2333138069272000E-08 + 51, 17, 2.0888245398168282E-08,-2.9670983222235501E-08, 1.2119471783788000E-08, 1.2129945644914001E-08 + 51, 18,-3.3494304644702041E-08, 2.7629980493199201E-08, 1.1895508326657000E-08, 1.1890062846390000E-08 + 51, 19,-2.4963244811027319E-08, 2.2213790963872059E-08, 1.1665092293416000E-08, 1.1670762022595000E-08 + 51, 20,-7.0543879987064577E-09, 1.1757663548559910E-08, 1.1419209107276000E-08, 1.1408285323303999E-08 + 51, 21, 5.5155275750198432E-09, 1.6349508815493969E-08, 1.1172357414194000E-08, 1.1170509238601001E-08 + 51, 22, 4.6871183054180362E-08, 2.4760657365816029E-08, 1.0912166471584001E-08, 1.0905998928139999E-08 + 51, 23,-4.7674186708854723E-08,-1.4813259309489511E-09, 1.0648749528536000E-08, 1.0642256541364000E-08 + 51, 24,-6.4295535384703913E-09,-5.1854019646573983E-08, 1.0399350603098000E-08, 1.0367394097318999E-08 + 51, 25,-4.7085096350693112E-08,-3.7467561377730562E-08, 1.0088974784827999E-08, 1.0104216837876000E-08 + 51, 26, 7.4858689380451180E-09, 5.3314459228113828E-09, 9.8515933754459994E-09, 9.8230601461401997E-09 + 51, 27,-1.3505131178303649E-08,-1.9852236137281241E-08, 9.5487814060388997E-09, 9.5324225138090007E-09 + 51, 28,-3.5192140516645377E-08, 1.6920317449100161E-08, 9.2680049264411994E-09, 9.2685540030330995E-09 + 51, 29,-3.5806469551034971E-08,-2.0193105116909719E-08, 8.9749842700656001E-09, 8.9782932053812005E-09 + 51, 30,-6.8474702779974490E-10,-2.5076180409702710E-08, 8.6836060320119992E-09, 8.7046577680005008E-09 + 51, 31,-2.7433494685854750E-08,-6.7320675345394953E-08, 8.4036354629553002E-09, 8.4189743297253003E-09 + 51, 32, 3.2706290050604787E-08, 4.3010231385512021E-08, 8.1152479897636993E-09, 8.1239423267122993E-09 + 51, 33, 1.5338866864197060E-08, 8.2100122128362807E-09, 7.8157061477371996E-09, 7.8457333589560001E-09 + 51, 34, 4.7999191903997334E-09, 3.6916920243981263E-08, 7.5488699445890007E-09, 7.5385089402637007E-09 + 51, 35, 6.3995124607836612E-08,-4.1549355480916761E-09, 7.2615152692307003E-09, 7.2581348021912001E-09 + 51, 36, 6.3904682931446001E-09,-3.7784328066977954E-09, 6.9659974923164998E-09, 6.9673662595021998E-09 + 51, 37, 1.5992212894221629E-08, 1.1973753049613980E-08, 6.6653620491225004E-09, 6.6562132996134001E-09 + 51, 38,-3.5635931632274437E-08, 5.8339751611619868E-08, 6.3842849686042996E-09, 6.3874017042059001E-09 + 51, 39, 2.4002563695187451E-08,-6.6061487225216589E-08, 6.0746526455409000E-09, 6.0668954088178996E-09 + 51, 40, 7.8430934246473202E-08,-1.1274386844345250E-08, 5.7747862268753000E-09, 5.7717580395886996E-09 + 51, 41,-5.2848798399566121E-08,-3.2427449885975748E-08, 5.4653006214722003E-09, 5.4753314324117000E-09 + 51, 42, 2.0655749951342261E-09, 2.5624789120520449E-08, 5.1456306816257998E-09, 5.1470935699979000E-09 + 51, 43,-3.4224859595931832E-08, 1.3403953523998609E-08, 4.8496545262178000E-09, 4.8523150632151000E-09 + 51, 44, 9.3323835413447984E-08,-1.2249138182468249E-09, 4.5165986241474000E-09, 4.5273585948158001E-09 + 51, 45,-3.5977175181172987E-08, 3.4007573246534718E-09, 4.2069357100146999E-09, 4.2070319493107000E-09 + 51, 46, 4.5427812368221218E-08, 9.9185446152200238E-09, 3.8633560279165000E-09, 3.8656984788233999E-09 + 51, 47,-2.0109373300981450E-08,-6.1566528039729332E-08, 3.4846145725162999E-09, 3.4809278346783999E-09 + 51, 48, 3.9671130193403781E-08, 4.8539180962100123E-08, 3.1360518635293999E-09, 3.1356668144526998E-09 + 51, 49,-9.6295313432649808E-09,-4.8216941875681473E-08, 2.5959235180557999E-09, 2.5945687918626001E-09 + 51, 50,-8.2743608048787692E-08,-9.2491009408480114E-09, 1.7341447906936999E-09, 1.7134538211476000E-09 + 51, 51, 6.5513195149778882E-08,-1.2609497913451269E-08, 1.2981863627650000E-09, 1.2829247811058000E-09 + 52, 0,-4.6998043401862638E-08, 0.0000000000000000E+00, 1.5404544057491000E-08, 0.0000000000000000E+00 + 52, 1,-7.3125989249548971E-08, 1.3193446981940500E-08, 1.5355458908091001E-08, 1.5380442371931001E-08 + 52, 2,-3.9162440267123392E-08,-3.0872193571645832E-08, 1.5377486327465001E-08, 1.5355649373651001E-08 + 52, 3,-1.0380686456439971E-08,-3.7915243521455307E-08, 1.5308886829942001E-08, 1.5337388338010001E-08 + 52, 4, 2.9543026038369640E-08,-1.0176165347588820E-08, 1.5264759325287000E-08, 1.5332139407892001E-08 + 52, 5, 2.1364265555602461E-08,-3.3221184222899608E-08, 1.5204789197676001E-08, 1.5252091581735001E-08 + 52, 6,-5.0212916735820923E-09,-3.8084824156320584E-09, 1.5044532727070000E-08, 1.5312244123198001E-08 + 52, 7, 2.4097352531784110E-09, 1.8867550334226561E-08, 1.5118752720155999E-08, 1.5044261558046001E-08 + 52, 8,-2.4699904721744941E-08, 2.5120589995064679E-08, 1.4955748942836001E-08, 1.5046176846718001E-08 + 52, 9,-6.2582358849204104E-08, 2.6980792694704350E-08, 1.4814258072064000E-08, 1.4945554935076001E-08 + 52, 10, 1.9006405217994720E-08, 2.9767073874791761E-09, 1.4759758841719000E-08, 1.4761826850137000E-08 + 52, 11,-7.6623875539400899E-08,-3.5848238837530522E-08, 1.4634272094994999E-08, 1.4603339772872001E-08 + 52, 12, 3.2480711743471650E-08,-4.8000496597705417E-09, 1.4462695526376000E-08, 1.4470007133795000E-08 + 52, 13,-4.5011134897040232E-08,-2.8345539286740270E-08, 1.4277867055759001E-08, 1.4307202994433001E-08 + 52, 14, 4.5519445887034017E-08,-3.1377455745158911E-08, 1.4109590931620999E-08, 1.4115666996183999E-08 + 52, 15,-1.5734315541709421E-08,-2.7031519649153559E-08, 1.3888253810004000E-08, 1.3915623331237000E-08 + 52, 16, 4.9300208757767551E-08,-4.9808563646618357E-08, 1.3699828824666999E-08, 1.3700043501956000E-08 + 52, 17, 6.4400954141947939E-09,-6.1332286620946051E-09, 1.3468956240437000E-08, 1.3466801618682999E-08 + 52, 18,-1.8891032342919400E-08,-2.8948337399279530E-08, 1.3242661615892000E-08, 1.3225512501545000E-08 + 52, 19, 4.6431841068943923E-08,-5.1366181075607742E-08, 1.2984041640578001E-08, 1.2981668963135001E-08 + 52, 20, 4.2906868844164661E-08,-1.4568512056101220E-08, 1.2741604577147000E-08, 1.2721476335797000E-08 + 52, 21, 4.3849106348132181E-08,-2.7487078212542670E-08, 1.2460761994191000E-08, 1.2452199157794000E-08 + 52, 22, 2.2129641912034591E-08, 6.0857862853891006E-09, 1.2196405554660000E-08, 1.2192243092300000E-08 + 52, 23,-2.7291304341374759E-08,-2.6816692934692270E-08, 1.1906471267927000E-08, 1.1902359551474000E-08 + 52, 24,-5.7325459793046452E-08, 2.0998148353659600E-08, 1.1640675458794000E-08, 1.1611380651188000E-08 + 52, 25,-8.8543516677016567E-09,-1.6492631853621202E-08, 1.1330775831498001E-08, 1.1341739058568000E-08 + 52, 26,-2.8298020291289359E-08, 4.0144029279420067E-08, 1.1041952326965000E-08, 1.1017870001994000E-08 + 52, 27,-4.8630297725006997E-09, 1.6679124128243080E-09, 1.0759961518346000E-08, 1.0731542367285000E-08 + 52, 28,-5.4708722719589192E-08, 1.2937493059482590E-08, 1.0430582232006999E-08, 1.0421081663719000E-08 + 52, 29, 9.2450643784572275E-09, 4.4347180119491128E-08, 1.0135478407770999E-08, 1.0131438194527000E-08 + 52, 30,-2.5366126298359571E-08, 5.1725472523159881E-09, 9.8084404403930001E-09, 9.8249622642011996E-09 + 52, 31, 1.3767739772072860E-08, 1.8768516178763520E-08, 9.5053538318778997E-09, 9.5257153357151994E-09 + 52, 32, 5.4227866572063518E-09, 4.5000864495489661E-08, 9.1965356789198998E-09, 9.2149514276115994E-09 + 52, 33, 3.2968499233536213E-08,-8.6751857754775080E-10, 8.8789481015368003E-09, 8.9070543959285003E-09 + 52, 34, 2.4486825471471770E-08,-2.3811771190500061E-08, 8.5869415131601993E-09, 8.5708923118769006E-09 + 52, 35, 1.0527840310867370E-08,-3.3082861291913313E-08, 8.2705281040128998E-09, 8.2723914962043993E-09 + 52, 36,-1.8757070788135729E-08,-1.2182129829691790E-08, 7.9595412575527000E-09, 7.9625629779519007E-09 + 52, 37, 1.3727245216935850E-08,-1.0521111213145380E-08, 7.6498053575904995E-09, 7.6330385306567994E-09 + 52, 38,-4.4175843590176847E-08,-1.8981307305971281E-08, 7.3140427849498001E-09, 7.3109488864421999E-09 + 52, 39, 3.1903296115492062E-08,-4.7119939331212922E-08, 7.0086401127670004E-09, 7.0059690868939999E-09 + 52, 40,-4.7331716245612263E-08, 3.8104193011362067E-08, 6.6647314412094003E-09, 6.6666998073779999E-09 + 52, 41,-4.5447411640280422E-08,-7.0018698154348914E-08, 6.3399951174455999E-09, 6.3485069467117001E-09 + 52, 42,-5.3922308897598753E-08, 8.0875190816934851E-08, 6.0094873435955997E-09, 6.0118296370318997E-09 + 52, 43, 4.6428064903514002E-08,-6.3339090052245010E-08, 5.6641678216327004E-09, 5.6539673572952004E-09 + 52, 44, 4.4192270666107401E-08, 1.8836035822316309E-08, 5.3399792069535997E-09, 5.3353590793936999E-09 + 52, 45,-4.8120400099369153E-08,-2.7835380437460419E-08, 4.9729589830321999E-09, 4.9776257999947001E-09 + 52, 46,-6.6899199735992966E-09,-6.1489118191177341E-08, 4.6296076136027004E-09, 4.6399820733583999E-09 + 52, 47, 1.7392768758844169E-08,-1.9114839621822230E-08, 4.2593407900264002E-09, 4.2573999643498000E-09 + 52, 48,-6.8888508194770691E-08, 5.2472384269907173E-08, 3.8322444326906999E-09, 3.8377341665740002E-09 + 52, 49, 2.0389298546465718E-08,-4.9792721449842987E-08, 3.4626303645120002E-09, 3.4692936678980002E-09 + 52, 50,-8.1668415940390269E-08, 4.1129950546979502E-08, 2.8991974827547001E-09, 2.8845648423191000E-09 + 52, 51, 2.7881484013889271E-08, 6.4810513354435554E-08, 1.8509552229195999E-09, 1.8541412713019000E-09 + 52, 52,-2.7400623323859001E-08,-6.5761673085644410E-08, 1.4267747922295000E-09, 1.4206157800908001E-09 + 53, 0,-1.6691019592369758E-08, 0.0000000000000000E+00, 1.6927106251376000E-08, 0.0000000000000000E+00 + 53, 1,-1.5666102397789839E-08, 3.5566327968041421E-08, 1.6910938362479999E-08, 1.6941301068388000E-08 + 53, 2,-1.4954443187894230E-09, 2.6854082131425410E-08, 1.6908066954479000E-08, 1.6862938489965001E-08 + 53, 3,-3.2251236580078892E-08, 6.5728992936600433E-09, 1.6863078134422999E-08, 1.6890562337403999E-08 + 53, 4,-9.3863432195234914E-09, 3.4991411194196657E-08, 1.6781329807560000E-08, 1.6845565041945999E-08 + 53, 5,-3.0712799637772112E-08,-1.0582243302834779E-08, 1.6752310920891000E-08, 1.6802167896249999E-08 + 53, 6,-2.3553763041211711E-08,-3.9126633818855677E-08, 1.6544157519902000E-08, 1.6831305948303000E-08 + 53, 7, 4.0104995854707551E-08,-2.4112294204052810E-08, 1.6668794470065000E-08, 1.6572664574909999E-08 + 53, 8, 2.6431132552206969E-08, 1.3608243955304149E-08, 1.6457339225048000E-08, 1.6546754588499000E-08 + 53, 9, 3.7004888408355613E-08, 1.3661920051187479E-09, 1.6335772058480000E-08, 1.6479280263643999E-08 + 53, 10, 5.1230315465902433E-08, 2.7687070539827611E-08, 1.6247981061418000E-08, 1.6262873849342999E-08 + 53, 11, 1.8340782129712009E-09,-1.0336489964999799E-08, 1.6143034972543998E-08, 1.6111193514044999E-08 + 53, 12,-6.2052775075369532E-08, 6.8850250959663399E-08, 1.5946273175166999E-08, 1.5959521297399999E-08 + 53, 13, 2.0460460601186171E-08,-6.1855946586193251E-08, 1.5776240198435999E-08, 1.5790048930125999E-08 + 53, 14,-6.2425954044582042E-09, 1.9837492840787200E-08, 1.5571195897709000E-08, 1.5596068212924000E-08 + 53, 15,-6.9601572151115273E-09, 7.9164254347134379E-09, 1.5368529270133000E-08, 1.5388188739381001E-08 + 53, 16,-9.2892990909516953E-09,-6.9883458666884686E-09, 1.5152139445960001E-08, 1.5148976128229000E-08 + 53, 17,-4.8668807783869368E-09, 1.1532899564587510E-08, 1.4925076052202999E-08, 1.4925648295847000E-08 + 53, 18,-2.0537468213377050E-08,-6.1370744313838980E-08, 1.4677497871454000E-08, 1.4654445390924000E-08 + 53, 19, 9.6975161538131114E-09,-2.9024852370221798E-10, 1.4418681162664000E-08, 1.4410420982192001E-08 + 53, 20, 2.2703555087580229E-08,-1.9281952576269900E-08, 1.4148451313920001E-08, 1.4131204577637000E-08 + 53, 21, 2.7869736922404999E-08,-3.3167635854992492E-08, 1.3863981647431000E-08, 1.3864188251345000E-08 + 53, 22,-1.5409270683423080E-08,-2.9204187712910168E-08, 1.3571785908323001E-08, 1.3568612916676000E-08 + 53, 23, 7.2927962340971284E-09, 2.5346054928181650E-08, 1.3286421038518000E-08, 1.3282409197042000E-08 + 53, 24,-1.1673750352504509E-08,-9.4429903721035365E-09, 1.2993091999689000E-08, 1.2956350133522999E-08 + 53, 25,-1.5844787546178970E-09, 3.7764159239042003E-08, 1.2665994448452001E-08, 1.2686319172582000E-08 + 53, 26, 7.8433956440027791E-09, 2.8567991100722241E-08, 1.2370775676354000E-08, 1.2339647642680999E-08 + 53, 27, 2.8723603650376988E-09, 2.3794926127698481E-08, 1.2042953298298000E-08, 1.2023592688317000E-08 + 53, 28,-1.5065759696539081E-08, 4.3597313937908353E-08, 1.1728051829145999E-08, 1.1718409433701000E-08 + 53, 29, 4.5722543860343827E-08,-1.2570866714267290E-08, 1.1378968518334000E-08, 1.1382787665965000E-08 + 53, 30,-3.2657372209199011E-08, 1.1295809029677430E-08, 1.1056853467901001E-08, 1.1079036185759000E-08 + 53, 31, 6.2565668631466469E-08, 4.2217634282961531E-08, 1.0713107396564000E-08, 1.0737768485607000E-08 + 53, 32, 2.8766150712279080E-08, 2.0837229500576491E-09, 1.0396425000506000E-08, 1.0410230206497000E-08 + 53, 33,-1.8879650637938542E-08, 2.0658673297021081E-08, 1.0051639522024000E-08, 1.0083605061998999E-08 + 53, 34,-4.1190753717460562E-08,-4.8163347805704682E-08, 9.7337769044394996E-09, 9.7238763506490003E-09 + 53, 35,-5.5230420819605743E-08, 1.6631180477438700E-10, 9.3958171746913006E-09, 9.3899321439999999E-09 + 53, 36, 1.2979922422761800E-08,-1.1324425121402990E-08, 9.0617342908561999E-09, 9.0592573081305001E-09 + 53, 37,-3.2126589158522942E-08,-8.0698840294973879E-09, 8.7339429563066002E-09, 8.7166715336931996E-09 + 53, 38, 2.1769774829099241E-08, 3.8218859146666479E-09, 8.3796778835900993E-09, 8.3751673621492005E-09 + 53, 39, 3.4298154117308892E-08,-9.2248762677727159E-09, 8.0214680810334995E-09, 8.0134047177718997E-09 + 53, 40,-8.0785510927542902E-08, 1.1507172316275580E-08, 7.6882464256036006E-09, 7.6893778703007004E-09 + 53, 41, 7.1393641780884312E-08, 3.7640346461293821E-08, 7.3089652919441004E-09, 7.3209813195835999E-09 + 53, 42,-4.9654165937185422E-08,-1.9909290496673799E-09, 6.9660439676264000E-09, 6.9676531280277000E-09 + 53, 43, 1.1101972682468910E-07, 8.8740828524389599E-09, 6.6001721595170999E-09, 6.6018269270909000E-09 + 53, 44,-3.5459649991908157E-08, 1.5132009128550950E-08, 6.2222653172873002E-09, 6.2228974174248997E-09 + 53, 45, 4.9121650219101452E-08,-2.6337973614609719E-08, 5.8668514208298003E-09, 5.8704653074254002E-09 + 53, 46,-2.0202586161871480E-08, 3.0033066282118739E-09, 5.4707664378853003E-09, 5.4798988195103998E-09 + 53, 47, 3.5392682181632917E-08, 1.7741904918560880E-08, 5.1012145933616996E-09, 5.1069694251553003E-09 + 53, 48,-6.7414254293143934E-08,-3.3033971734706463E-08, 4.6953855636049999E-09, 4.6820383315528003E-09 + 53, 49, 8.4262361165395812E-08, 3.5386699332998772E-08, 4.2322433676568998E-09, 4.2302224734801001E-09 + 53, 50,-6.1889922005387833E-08, 7.4290427012850204E-09, 3.8203131176994997E-09, 3.8324320650784004E-09 + 53, 51, 3.6218637647528652E-08, 9.9883624817707802E-09, 3.1677051483488000E-09, 3.1630006035320998E-09 + 53, 52, 9.4627620407400127E-08,-6.7933921620453271E-08, 2.0294236591710000E-09, 2.0052670215972002E-09 + 53, 53,-6.4701085775140611E-08, 3.0103050032314503E-08, 1.5752857413314999E-09, 1.5789111023553999E-09 + 54, 0, 4.3926231289027848E-08, 0.0000000000000000E+00, 1.8569097828866999E-08, 0.0000000000000000E+00 + 54, 1, 1.2008783927256970E-08, 4.2223927127308491E-08, 1.8527204101258001E-08, 1.8553643818324999E-08 + 54, 2, 7.0721778187372190E-08, 1.4556630222110619E-08, 1.8540335114558999E-08, 1.8502233992147999E-08 + 54, 3, 5.0819804558331778E-08, 6.3753171488066512E-08, 1.8468396854807000E-08, 1.8503641607654001E-08 + 54, 4,-4.5444717490999721E-08, 2.4100147675497140E-08, 1.8405381281501001E-08, 1.8484675604941999E-08 + 54, 5,-1.7671380390997081E-08, 3.1896528569287528E-09, 1.8353669811885001E-08, 1.8405221521708000E-08 + 54, 6,-4.6023216069339603E-08, 2.6165267500894481E-08, 1.8159968009433000E-08, 1.8469271654298000E-08 + 54, 7, 4.3196250016040070E-09,-4.9795966203032011E-08, 1.8256723018717999E-08, 1.8170611671806999E-08 + 54, 8,-2.8565227288765071E-09, 9.0807265433742420E-09, 1.8069605217972999E-08, 1.8174360696093000E-08 + 54, 9, 3.5778312217087382E-08,-2.5491524087210481E-08, 1.7915488649706000E-08, 1.8062762202385001E-08 + 54, 10,-9.0250277755164568E-09, 1.3219702757062211E-08, 1.7866604256746999E-08, 1.7868136525361001E-08 + 54, 11, 5.2751760058383094E-09, 6.8078249497292209E-10, 1.7714789304404999E-08, 1.7675607996958000E-08 + 54, 12,-3.8354913258106977E-08,-4.6295397867577959E-09, 1.7548712785658999E-08, 1.7555205782096999E-08 + 54, 13, 3.7734928592909871E-08, 2.7734951045143369E-08, 1.7333201605602999E-08, 1.7354756221362999E-08 + 54, 14,-4.1300802134345663E-08, 1.9162059350642602E-08, 1.7157211518801000E-08, 1.7164068479365001E-08 + 54, 15, 6.3531836160298633E-09, 3.1690672606606553E-08, 1.6911267488018000E-08, 1.6947969076741000E-08 + 54, 16,-2.6533856826950400E-08, 3.2689845798466007E-08, 1.6710326588823999E-08, 1.6705386710552001E-08 + 54, 17, 6.3847421477796457E-09, 3.5476438521018430E-08, 1.6457274807736999E-08, 1.6458703877052000E-08 + 54, 18, 2.9457193534100790E-08,-9.0053222874719414E-09, 1.6216825540185001E-08, 1.6196779236924001E-08 + 54, 19,-6.2958073111271251E-08,-1.0892345287920950E-08, 1.5927565156457999E-08, 1.5929283556617002E-08 + 54, 20, 3.1377467824839969E-09, 1.5773711905280180E-08, 1.5664792427287999E-08, 1.5645921355492999E-08 + 54, 21,-6.2671446988217480E-08, 3.4691725769198473E-08, 1.5360103902737999E-08, 1.5352981583470998E-08 + 54, 22,-4.1647315232253212E-08, 3.0534599352053147E-08, 1.5061505393082001E-08, 1.5060710829965000E-08 + 54, 23, 2.2193135458412811E-08,-2.5389799084360941E-08, 1.4746906757499000E-08, 1.4746863669600999E-08 + 54, 24, 2.5841093437669669E-08,-2.4814116718005610E-08, 1.4457637138008001E-08, 1.4423808349317999E-08 + 54, 25, 2.0868551734792011E-08, 1.7696057564614001E-08, 1.4097723194406999E-08, 1.4123875056221001E-08 + 54, 26, 5.0548419143542632E-08,-2.1998531847733900E-08, 1.3804742772501000E-08, 1.3772005749499999E-08 + 54, 27, 1.5357905368815240E-09, 1.6401679484838179E-08, 1.3456309368959000E-08, 1.3423789838842999E-08 + 54, 28, 1.0042633471445060E-08,-3.6734959695416671E-08, 1.3108471120688001E-08, 1.3104099456414000E-08 + 54, 29,-2.2556636069232699E-08,-1.5692683795595120E-09, 1.2767976135315000E-08, 1.2765804538491001E-08 + 54, 30, 3.7943330926536668E-08,-2.3211084483853620E-08, 1.2392261870229001E-08, 1.2415513552097999E-08 + 54, 31, 1.7965066655535941E-08,-2.1952950141070021E-08, 1.2060428024627001E-08, 1.2079556476534999E-08 + 54, 32, 3.8241477061397297E-08,-8.4353849684386816E-09, 1.1687641752715000E-08, 1.1716439493583001E-08 + 54, 33,-3.0495857681777542E-08, 1.7699225827790901E-08, 1.1343265730240001E-08, 1.1378775162033000E-08 + 54, 34,-4.4975769991237369E-09,-6.9519469040710451E-09, 1.0997165580651000E-08, 1.0990368233738000E-08 + 54, 35,-1.5923034361737749E-08, 4.4077387687169632E-08, 1.0635705907618000E-08, 1.0637549971156000E-08 + 54, 36,-1.0143969383951411E-08, 2.2585998261804580E-08, 1.0267418791527999E-08, 1.0279539616608001E-08 + 54, 37,-1.5926314244131182E-08,-2.6543053317284340E-08, 9.9282286380283992E-09, 9.9113253618795996E-09 + 54, 38, 7.0379785462622013E-09, 1.6539475858318622E-08, 9.5600542349769997E-09, 9.5481115751477007E-09 + 54, 39, 2.2843731988396330E-08, 4.2417810100264172E-08, 9.1708027824298995E-09, 9.1668072760559007E-09 + 54, 40, 1.6737942477516070E-08,-1.0395016022966039E-08, 8.7886432221734008E-09, 8.7875437783335995E-09 + 54, 41, 3.3759743587776683E-08, 5.2252867723500112E-08, 8.4213967693429996E-09, 8.4392032220404001E-09 + 54, 42, 6.5788129576289293E-09,-6.4474262502821200E-08, 8.0223514685597998E-09, 8.0259706560391000E-09 + 54, 43, 4.8989325907204272E-08, 5.0841173313952951E-08, 7.6475294472649999E-09, 7.6476070808533005E-09 + 54, 44,-3.7290913064637728E-08,-4.5201177079201752E-08, 7.2634414894627997E-09, 7.2336291296214003E-09 + 54, 45, 5.4855305925634923E-08, 9.0262906458967249E-10, 6.8356402847410996E-09, 6.8386095805118999E-09 + 54, 46,-4.0292437380047853E-08,-3.9202232531853606E-09, 6.4468827503164003E-09, 6.4592892132743998E-09 + 54, 47, 4.1085250062970322E-08, 3.3799518536846038E-08, 6.0152716273831997E-09, 6.0277930406485003E-09 + 54, 48,-2.0721760942411529E-08, 1.3236234765934920E-08, 5.6321854185535999E-09, 5.6095572808763996E-09 + 54, 49,-8.8057281168314134E-11, 4.0803002647829001E-08, 5.1691979743948004E-09, 5.1651473620730998E-09 + 54, 50,-1.9754983881252422E-08,-8.0973487380146494E-08, 4.6603293034134997E-09, 4.6657131000811999E-09 + 54, 51, 1.8546289446097100E-08, 4.9335150932487057E-08, 4.2219595354321997E-09, 4.2103802030160003E-09 + 54, 52, 5.6815670289527632E-08,-3.4894938614014332E-08, 3.4400218030889001E-09, 3.4528656872436000E-09 + 54, 53,-7.4771215181302731E-08,-3.0461013444809292E-08, 2.2060144548953000E-09, 2.1906629866961999E-09 + 54, 54, 3.7194903778081370E-08, 4.8179833074196091E-08, 1.7488972605114001E-09, 1.7603759371869000E-09 + 55, 0, 1.4848553196557169E-08, 0.0000000000000000E+00, 2.0255852162828001E-08, 0.0000000000000000E+00 + 55, 1,-1.4058680232162451E-08,-3.4121066246251741E-08, 2.0223726645290999E-08, 2.0246889653405001E-08 + 55, 2, 2.6671590937996058E-10,-5.2375271382274752E-08, 2.0213023301817001E-08, 2.0193107413634000E-08 + 55, 3, 1.7479439344560511E-08, 1.7671698679507189E-08, 2.0162159047806999E-08, 2.0195997921686001E-08 + 55, 4,-1.6232198893426281E-08,-3.0768856418781013E-08, 2.0090223154436002E-08, 2.0154461504795999E-08 + 55, 5, 1.2491194101443860E-09, 8.3006511625410902E-09, 2.0041945930315001E-08, 2.0086883670250000E-08 + 55, 6, 2.9546032548126461E-08, 2.7956920139482280E-08, 1.9825853918638999E-08, 2.0141294901611002E-08 + 55, 7,-6.2934723803025273E-09, 5.5289281214079848E-09, 1.9941849102990001E-08, 1.9846067752288999E-08 + 55, 8,-2.8699604371125241E-08, 3.3020725983130329E-09, 1.9728569951087999E-08, 1.9834614494127001E-08 + 55, 9,-2.0114994683042280E-10, 2.8976341698270351E-08, 1.9590335533256001E-08, 1.9731144875840001E-08 + 55, 10,-4.1892554273674942E-08,-2.7527005140683249E-08, 1.9509826615854000E-08, 1.9521090044603000E-08 + 55, 11,-3.1690494713281462E-08, 1.2398804275623759E-08, 1.9374846063292000E-08, 1.9347350665292999E-08 + 55, 12, 2.5812554277739821E-08,-6.1806826407082310E-08, 1.9175910239386000E-08, 1.9189962450077001E-08 + 55, 13,-2.2304345023907789E-08, 2.8416596899984020E-08, 1.8991103575785000E-08, 1.9017353490529999E-08 + 55, 14, 1.6368014467583660E-08,-9.9109789184268809E-09, 1.8770783265897000E-08, 1.8786507551038002E-08 + 55, 15,-7.9280371320809353E-10, 5.7235755682308870E-09, 1.8565520315429000E-08, 1.8587794789385001E-08 + 55, 16, 3.7519218913759983E-08, 2.4135902934264331E-09, 1.8323852990558000E-08, 1.8312473319311000E-08 + 55, 17, 3.0564895086805630E-08,-1.6781151368446430E-08, 1.8082952022139001E-08, 1.8085509187686000E-08 + 55, 18, 6.7344323797808717E-09, 4.1021444845145523E-08, 1.7816456407324001E-08, 1.7799021249295001E-08 + 55, 19,-2.2214234476372539E-08, 2.0680173720233402E-09, 1.7536413466870000E-08, 1.7542332267924001E-08 + 55, 20, 1.6052408128393140E-08, 2.1700891663986469E-08, 1.7244237649420000E-08, 1.7230464691730001E-08 + 55, 21, 1.8403081882094350E-08, 8.2710599665939083E-09, 1.6949797904022999E-08, 1.6945316908438001E-08 + 55, 22, 8.3646738693678966E-09, 6.1478809221210555E-08, 1.6632448060628001E-08, 1.6625203132026999E-08 + 55, 23, 8.2019882925412967E-09, 1.9500420525503821E-09, 1.6318284764157998E-08, 1.6307617685641999E-08 + 55, 24, 2.1085316689551190E-08, 4.7283409137414814E-09, 1.6001896439118000E-08, 1.5966013746327000E-08 + 55, 25,-2.4855216013693911E-08,-4.4899706894485623E-08, 1.5640748640691000E-08, 1.5669137750885999E-08 + 55, 26, 2.3109032210127469E-08, 1.3427998748024830E-08, 1.5323358244500999E-08, 1.5284045224914999E-08 + 55, 27,-3.3069769476958191E-08,-4.6824943394393022E-08, 1.4975034817417000E-08, 1.4942665362973999E-08 + 55, 28,-1.4806325832099880E-08,-1.2152113069977260E-08, 1.4592238128570000E-08, 1.4590902039271999E-08 + 55, 29,-6.8087291176056211E-08,-3.4052741707478109E-09, 1.4241440755162000E-08, 1.4243040173908001E-08 + 55, 30,-1.7703627645540731E-08, 1.2777650262624090E-08, 1.3864585499441000E-08, 1.3881434381983000E-08 + 55, 31,-2.2530898560832640E-08,-5.3537321388998152E-08, 1.3478716267959000E-08, 1.3509100201105000E-08 + 55, 32,-2.4260286387374811E-08,-2.6321663663961160E-08, 1.3128748591137000E-08, 1.3146487826332000E-08 + 55, 33,-2.2815566601572171E-08, 1.7596417593771229E-08, 1.2724287956589000E-08, 1.2767950955395999E-08 + 55, 34, 2.7723247133203460E-08, 5.8653335485771483E-08, 1.2388043059693000E-08, 1.2374273423599000E-08 + 55, 35, 1.3326506650698069E-08, 8.0248811749605771E-08, 1.1990237387740000E-08, 1.1989355532615000E-08 + 55, 36, 3.0284425469817513E-08, 2.5544260213064941E-08, 1.1608954210159001E-08, 1.1607738598316001E-08 + 55, 37,-4.9923156277013099E-09, 1.4389704726578880E-08, 1.1230750840839000E-08, 1.1212949246840999E-08 + 55, 38, 5.6736176497116823E-08,-2.7966911140070791E-08, 1.0847020109660000E-08, 1.0839160377946999E-08 + 55, 39, 1.1734236676501950E-08,-2.2729261446815979E-08, 1.0445571651644000E-08, 1.0435233527582000E-08 + 55, 40, 1.3063950959503171E-08,-6.3495321759772328E-09, 1.0027440587830999E-08, 1.0029416801496000E-08 + 55, 41,-6.0493167978568252E-08,-2.5592644443257859E-08, 9.6140134928287000E-09, 9.6302727751747995E-09 + 55, 42, 6.3287282201381362E-08,-3.6519512963966728E-08, 9.2301820390914993E-09, 9.2375422611580992E-09 + 55, 43,-8.2097749359317121E-08,-1.0044070806770400E-08, 8.7983321262273996E-09, 8.7845399926094998E-09 + 55, 44,-2.8286364381684259E-08,-1.7516756461603471E-08, 8.3955193291789998E-09, 8.3801710744392997E-09 + 55, 45,-3.0409410590102023E-08, 2.3862032000979189E-08, 7.9456051844270002E-09, 7.9545387008854994E-09 + 55, 46,-1.3749244330251250E-08,-8.3976645580965546E-09, 7.5041886879347993E-09, 7.5163053591610003E-09 + 55, 47, 2.7127207716497369E-09, 5.1386942076275433E-08, 7.0926868166237996E-09, 7.0889151663259000E-09 + 55, 48,-2.1853760565014311E-08,-3.2412205573666843E-08, 6.6268981714796001E-09, 6.6152521068145001E-09 + 55, 49,-4.4266785150122721E-10,-8.4213337921105733E-09, 6.1840670473314004E-09, 6.1872596309603002E-09 + 55, 50, 1.2732568684082210E-08, 3.2676249140723782E-08, 5.6920485654712003E-09, 5.6897322203955002E-09 + 55, 51,-1.6354635258231271E-08, 1.1478872642360911E-08, 5.1419461809925997E-09, 5.1298451795178001E-09 + 55, 52, 2.4702648434862821E-08,-3.6279101597158313E-08, 4.6349125317610001E-09, 4.6186407342457004E-09 + 55, 53, 8.7170999765779012E-09,-1.4905022368989959E-08, 3.7874377166505998E-09, 3.7833637523087000E-09 + 55, 54,-2.9323227972810941E-08, 8.3961247778335282E-08, 2.4120683398160999E-09, 2.4041981003412999E-09 + 55, 55,-2.2086028018406869E-08,-5.1715417671369101E-08, 1.9584778322491000E-09, 1.9367436382694999E-09 + 56, 0,-1.1932734881648851E-08, 0.0000000000000000E+00, 2.1985156404556999E-08, 0.0000000000000000E+00 + 56, 1,-7.0871225166908844E-08, 3.8641681074455722E-08, 2.1956792689355001E-08, 2.1976639175697000E-08 + 56, 2,-3.3603114782430217E-08,-4.7810606698333273E-08, 2.1946697327076999E-08, 2.1903165965034001E-08 + 56, 3,-3.3107320973744453E-08,-7.7656091678670951E-08, 2.1880925603969000E-08, 2.1923700060446999E-08 + 56, 4, 5.0032577285218290E-09,-5.6364326409146962E-08, 2.1803996657043001E-08, 2.1877474267659000E-08 + 56, 5,-4.1777382095886552E-08,-2.3067342111362549E-08, 2.1755998480754999E-08, 2.1810388736132001E-08 + 56, 6, 1.4020291048802770E-08,-6.8608717175948577E-09, 2.1539480545025000E-08, 2.1854211341193000E-08 + 56, 7, 7.5007086637424902E-09,-2.2723812623338340E-09, 2.1649705562556999E-08, 2.1554573471426999E-08 + 56, 8,-3.1252495535718451E-08, 2.8481841194410902E-08, 2.1438946072389001E-08, 2.1542472546963999E-08 + 56, 9,-3.7267895693270583E-08, 2.7542771131781479E-08, 2.1285216937377999E-08, 2.1432872316448001E-08 + 56, 10, 2.0871073005265238E-09, 9.7938082753904023E-09, 2.1215728272085001E-08, 2.1223157180087001E-08 + 56, 11,-1.5843361391065881E-08, 3.7504777927324182E-08, 2.1072289333262001E-08, 2.1028783103100999E-08 + 56, 12, 2.2483076910337769E-08, 1.8187539859300521E-08, 2.0879859194790000E-08, 2.0880535052400999E-08 + 56, 13,-4.0230969929810147E-08,-1.1531456784441490E-08, 2.0664578500316998E-08, 2.0702668498395000E-08 + 56, 14, 3.5750146104206479E-09, 8.5054419800780475E-09, 2.0467211265449000E-08, 2.0477747946656001E-08 + 56, 15,-1.9876841997635318E-09,-2.8213920063283041E-09, 2.0220759374663999E-08, 2.0254922731416001E-08 + 56, 16, 4.3726535566161811E-08, 8.7421769261966627E-09, 2.0006900750149001E-08, 2.0010149302870001E-08 + 56, 17,-1.4453560619039120E-09, 1.2478197972131130E-08, 1.9737743708595999E-08, 1.9732324746761000E-08 + 56, 18,-3.9915110550761733E-09, 3.2846884773427170E-09, 1.9494619331896001E-08, 1.9474736222203999E-08 + 56, 19, 9.1004280092814276E-09,-1.1343487087107269E-08, 1.9184262875335000E-08, 1.9188754298184001E-08 + 56, 20, 5.9467812860131087E-09,-4.9051441307418334E-10, 1.8911052585471999E-08, 1.8886014475064999E-08 + 56, 21, 4.0464759336384773E-09,-7.1913020853953971E-08, 1.8587344387629001E-08, 1.8577307493416002E-08 + 56, 22, 2.0179510382623661E-08,-2.0974262127494669E-08, 1.8276812016907001E-08, 1.8273422018760999E-08 + 56, 23, 2.8219825636949951E-08, 2.4809049714962229E-09, 1.7934727636155000E-08, 1.7935539239868998E-08 + 56, 24,-8.8109793002204936E-09, 3.7236044785029432E-08, 1.7627583049245000E-08, 1.7591464769288998E-08 + 56, 25,-1.8687261149443490E-08,-1.2841939650031820E-08, 1.7250173272153001E-08, 1.7276229355774999E-08 + 56, 26,-2.4872429987073328E-08, 8.4491242196578232E-09, 1.6932577524130999E-08, 1.6891691832879001E-08 + 56, 27,-1.0400714598861700E-09, 2.3027757067652129E-09, 1.6561936149474001E-08, 1.6525087429106999E-08 + 56, 28, 5.9634217450002092E-09, 1.1238310074177161E-08, 1.6188613742723999E-08, 1.6175370085601000E-08 + 56, 29,-3.1170168903600581E-08, 1.3229671714463281E-08, 1.5790836494433000E-08, 1.5801098397898000E-08 + 56, 30,-2.2352617740292561E-09, 4.2704496018687501E-08, 1.5413813141221001E-08, 1.5450092561044001E-08 + 56, 31, 8.5790000981193572E-09, 3.1481391012758888E-09, 1.5025516854391999E-08, 1.5046680269698000E-08 + 56, 32,-2.7030611553292609E-08, 1.5205317543233561E-08, 1.4632814561005999E-08, 1.4655282492149001E-08 + 56, 33, 1.4394403067515250E-08, 2.5560662073070188E-08, 1.4249031437071000E-08, 1.4283050777318000E-08 + 56, 34,-9.3467365009604126E-09, 1.2773264421065540E-08, 1.3860555072631000E-08, 1.3841972549442000E-08 + 56, 35, 4.1855439251139450E-09,-2.0392997489899030E-08, 1.3473205004291000E-08, 1.3466093027460000E-08 + 56, 36, 4.9531426593927456E-09, 9.7498403503221794E-09, 1.3042881309044001E-08, 1.3051647478372000E-08 + 56, 37,-2.5591660178739770E-08,-1.8711256485878950E-08, 1.2667093498425000E-08, 1.2639074602813000E-08 + 56, 38, 1.5322101548935169E-08,-2.2656447180228591E-08, 1.2237554426427000E-08, 1.2232316901880000E-08 + 56, 39,-3.5472632100994032E-08,-1.0413181972359199E-08, 1.1827394762823000E-08, 1.1821384072159999E-08 + 56, 40,-1.4287698737672810E-08, 7.8674679510912097E-09, 1.1399125650416999E-08, 1.1392782895444999E-08 + 56, 41,-1.7404832757375981E-08,-3.6160379984470752E-08, 1.0946401488173000E-08, 1.0961986652321999E-08 + 56, 42,-2.4155919176973871E-08, 6.1534629543356289E-08, 1.0516869819515000E-08, 1.0525514553906001E-08 + 56, 43,-5.7955662029208043E-08,-4.0353626301948603E-08, 1.0101174719006000E-08, 1.0096384942736000E-08 + 56, 44, 3.3684164125439371E-08, 6.7358317295381854E-08, 9.6397181785035993E-09, 9.6115996978769997E-09 + 56, 45,-5.8165632480347277E-08, 2.9008239805475479E-09, 9.1864609091498005E-09, 9.1912650018491005E-09 + 56, 46, 8.0045538387696621E-08, 4.6678779795698002E-08, 8.7098037714007004E-09, 8.7192661081898001E-09 + 56, 47,-3.6829432969901267E-08, 7.2071325153694886E-09, 8.2390902242930000E-09, 8.2425294888297004E-09 + 56, 48, 1.9956809546781731E-08,-1.9519964262933199E-08, 7.7926342124519996E-09, 7.7765958734867001E-09 + 56, 49, 5.1503280793249212E-08, 1.4271643703674220E-08, 7.2761024396559001E-09, 7.2721976378025004E-09 + 56, 50,-4.4689067536612654E-09,-3.0807798627173160E-08, 6.8270213503857001E-09, 6.8031809535278000E-09 + 56, 51, 2.5790077262080708E-08,-3.2111733449172441E-08, 6.2660511083078003E-09, 6.2543259247193999E-09 + 56, 52, 1.8554812559999950E-09,-1.9993621847970261E-08, 5.6510965105985996E-09, 5.6333768467284999E-09 + 56, 53,-3.3530514139558771E-08,-2.9119325437705201E-08, 5.0937154010530003E-09, 5.0920173731619000E-09 + 56, 54, 6.5078969513961821E-09, 1.5740387258520900E-09, 4.1448019018495996E-09, 4.1524387261614999E-09 + 56, 55, 1.2738552004149509E-07,-4.5105162324429591E-08, 2.6312346058907002E-09, 2.6419309184124999E-09 + 56, 56,-2.1919178373421420E-08, 6.5491351160329483E-08, 2.1591342723745001E-09, 2.1622117609756001E-09 + 57, 0,-2.1243891495712241E-08, 0.0000000000000000E+00, 2.3723297372618001E-08, 0.0000000000000000E+00 + 57, 1, 4.1531225927031417E-10, 1.9873894089602499E-08, 2.3674945324655001E-08, 2.3697058550305000E-08 + 57, 2, 4.3417614569583733E-09, 5.5426278662686363E-08, 2.3672478352049000E-08, 2.3641963850214002E-08 + 57, 3, 7.2763017113695284E-09, 1.9890550601183790E-08, 2.3598340412454001E-08, 2.3637159315577999E-08 + 57, 4, 2.8352091228297479E-08, 1.0671273735474990E-08, 2.3521892663164001E-08, 2.3611735718708999E-08 + 57, 5,-4.9042245170945766E-10,-8.4411285598917464E-09, 2.3473240193197000E-08, 2.3510375388814001E-08 + 57, 6,-1.0691815038230210E-08,-8.3303307735296533E-09, 2.3248172847385001E-08, 2.3587241918998999E-08 + 57, 7,-2.5327314704575740E-08,-2.5739017197925340E-08, 2.3351382093979001E-08, 2.3259959720911000E-08 + 57, 8,-1.3070032685694571E-08, 1.9360114067206479E-08, 2.3154205639407999E-08, 2.3251685459037999E-08 + 57, 9,-1.1660168794217701E-08,-1.6806979599113609E-08, 2.2982874895444001E-08, 2.3134434973918000E-08 + 57, 10, 3.0903583289763373E-08, 4.2775007179188453E-08, 2.2910988079111999E-08, 2.2932350255206999E-08 + 57, 11, 3.2685671623017221E-08, 2.2829537397021192E-08, 2.2762313506967999E-08, 2.2735649214227999E-08 + 57, 12, 3.1556048497262832E-08, 2.0189978026834209E-08, 2.2568466940405998E-08, 2.2579073990731001E-08 + 57, 13,-8.9845618401068049E-09, 2.7880872789146231E-08, 2.2362806542388999E-08, 2.2392020650459000E-08 + 57, 14, 2.4442288598510530E-08, 1.3110467479758710E-08, 2.2150992037771001E-08, 2.2174612015469998E-08 + 57, 15, 5.4949748479115807E-10, 1.3578456246447679E-08, 2.1920337850445000E-08, 2.1941539376602999E-08 + 57, 16,-2.6828947713793100E-09, 5.9707039337331236E-09, 2.1692641217018999E-08, 2.1687562885678999E-08 + 57, 17, 9.4349779495163113E-10, 1.4042789694091400E-08, 2.1432764048446001E-08, 2.1435737403336999E-08 + 57, 18,-6.6511240894019173E-09, 5.7368744895617897E-09, 2.1167391440240000E-08, 2.1139264029751000E-08 + 57, 19, 1.5910348372004039E-08,-9.3713262858116040E-09, 2.0887494229586999E-08, 2.0888712545183998E-08 + 57, 20,-5.0618387304031450E-09,-1.0019795285925850E-08, 2.0577774187195001E-08, 2.0555168605901998E-08 + 57, 21,-2.6967703819319900E-08,-3.1315575291156651E-08, 2.0271116949638999E-08, 2.0274641989795999E-08 + 57, 22,-3.5344404747113392E-08,-5.5929039885978847E-08, 1.9940026621490001E-08, 1.9939235544622001E-08 + 57, 23,-5.6686179805740703E-08,-1.1844040442389340E-08, 1.9620586371417999E-08, 1.9616127313655998E-08 + 57, 24,-1.7020233517005648E-08,-1.7125111627194060E-09, 1.9285914073699001E-08, 1.9255915406117001E-08 + 57, 25, 3.6562394595858423E-08, 1.5692134192673700E-09, 1.8911498894308999E-08, 1.8949453480397001E-08 + 57, 26,-4.8634076130040093E-08, 9.1834642026123978E-09, 1.8586539695158998E-08, 1.8546672447039001E-08 + 57, 27, 4.6130375797864207E-08, 3.2556492901524107E-08, 1.8218886967526000E-08, 1.8182996100629000E-08 + 57, 28, 5.4689481576062600E-09, 4.6599773260688833E-08, 1.7824076125944001E-08, 1.7815010716331000E-08 + 57, 29, 3.6745930719475888E-08,-1.5119394103802420E-08, 1.7444341521385000E-08, 1.7437643377325000E-08 + 57, 30, 7.7900543131802443E-09,-8.5013810612921877E-09, 1.7027576788149999E-08, 1.7063067177360001E-08 + 57, 31, 1.8504594464247911E-08,-3.8052555878795892E-09, 1.6651207625169000E-08, 1.6678442029561001E-08 + 57, 32, 3.5475679313498653E-08, 3.3085574203563761E-08, 1.6232064783924000E-08, 1.6258424187359001E-08 + 57, 33, 1.4726033412290410E-08,-2.2567664250473270E-08, 1.5822817988690999E-08, 1.5867304571437000E-08 + 57, 34,-4.3056322345365841E-08,-5.9240379288160653E-09, 1.5446828795036001E-08, 1.5440330106886000E-08 + 57, 35, 1.4552897679796521E-08,-7.3424145857240362E-08, 1.5009154952245998E-08, 1.5018115568861001E-08 + 57, 36,-4.9282339815758892E-08,-8.8800324746045985E-09, 1.4603695856140001E-08, 1.4612910040736001E-08 + 57, 37,-1.5213122638216710E-10,-3.0109872919255591E-08, 1.4179450142875000E-08, 1.4154322627231000E-08 + 57, 38,-4.1249824189076837E-08, 1.7627589382168950E-08, 1.3762891028947001E-08, 1.3741743100561000E-08 + 57, 39,-3.9129168006424116E-09, 1.2749240438007280E-09, 1.3308823502511000E-08, 1.3296543003873000E-08 + 57, 40,-1.8941950991629269E-08, 2.8549012200682050E-08, 1.2870180059541000E-08, 1.2869834126193000E-08 + 57, 41,-2.2113855580287060E-09, 7.4195491414365042E-09, 1.2401650584109000E-08, 1.2420263432962000E-08 + 57, 42, 5.1678198013408821E-09, 6.2487645468054121E-08, 1.1937273480416999E-08, 1.1943646432406001E-08 + 57, 43,-1.4575337182998440E-08, 1.5424311548276471E-09, 1.1488146078210000E-08, 1.1474538535383999E-08 + 57, 44, 1.3216504009769249E-09, 3.0557632023929763E-08, 1.1037865301216000E-08, 1.1024537342361000E-08 + 57, 45, 5.8638050373894228E-08,-4.4585190276121127E-08, 1.0515604943803000E-08, 1.0520307222247001E-08 + 57, 46, 5.3921909888683872E-08, 6.1019881645414481E-08, 1.0044849476281000E-08, 1.0060493817069999E-08 + 57, 47,-1.5886993766328458E-08,-6.3739364556579840E-08, 9.5410539885890003E-09, 9.5368363621357004E-09 + 57, 48, 3.2355174011437509E-09, 1.9891566410252481E-08, 9.0391020766838004E-09, 9.0276274365445998E-09 + 57, 49,-1.6635479735933390E-08,-1.4990982908636769E-08, 8.5409750732316004E-09, 8.5333367860444994E-09 + 57, 50,-1.0138335766194510E-08,-1.1696632609939420E-08, 7.9917296316221995E-09, 7.9871772283609007E-09 + 57, 51, 1.3666576278193531E-09,-7.5584952917716056E-09, 7.4776629089633003E-09, 7.4809758882437004E-09 + 57, 52, 2.4540445362470070E-09, 5.0538999901756037E-08, 6.8541846302563997E-09, 6.8528435843003998E-09 + 57, 53,-5.4284527060827491E-08,-6.2108950701615569E-09, 6.2178652950754000E-09, 6.1940934031118997E-09 + 57, 54,-1.3647378052313160E-08, 6.6573613126338164E-08, 5.6010102890217002E-09, 5.6054316172836997E-09 + 57, 55,-1.3048754707484809E-08,-3.0174525200948582E-08, 4.5248170728788996E-09, 4.5464605419020997E-09 + 57, 56,-6.6601689101884101E-08,-9.2417367195420845E-08, 2.8874929321069001E-09, 2.9056229980127001E-09 + 57, 57, 7.6691153671404511E-08, 4.5059601692362497E-08, 2.4192648659456000E-09, 2.4342878064930998E-09 + 58, 0, 1.2047513260901750E-09, 0.0000000000000000E+00, 2.5409261746237998E-08, 0.0000000000000000E+00 + 58, 1, 1.8823657610989730E-08,-1.1569274748816460E-08, 2.5377061354694999E-08, 2.5380480338766999E-08 + 58, 2, 6.6137421776539104E-09,-1.1193037962724350E-08, 2.5342932216723999E-08, 2.5324338895112001E-08 + 58, 3, 3.8047284062618281E-08, 4.1794160984964171E-08, 2.5281831512899999E-08, 2.5331959013757001E-08 + 58, 4, 5.5737543807119143E-08, 2.9800174574788202E-08, 2.5210031315450001E-08, 2.5265972050298001E-08 + 58, 5, 2.5055862915150868E-09, 3.0012350802429448E-10, 2.5148287694420000E-08, 2.5200756809446999E-08 + 58, 6, 1.1726187366599299E-08, 9.3511389743938401E-09, 2.4932861229808000E-08, 2.5234312022540999E-08 + 58, 7,-6.1739502710402631E-09, 3.2225289379238009E-09, 2.5037202896420999E-08, 2.4936245734929001E-08 + 58, 8,-2.2337933506376671E-08, 1.7903454140434040E-08, 2.4800735861965999E-08, 2.4926931901567000E-08 + 58, 9,-2.0217869838145092E-09,-2.1435894067361030E-10, 2.4666603221280999E-08, 2.4800119863225999E-08 + 58, 10, 3.7303900856690607E-08,-1.8138281549426479E-08, 2.4576047667074001E-08, 2.4584718418618999E-08 + 58, 11, 2.5380019538382131E-09,-1.2625375602838040E-09, 2.4443156214503999E-08, 2.4398175117338998E-08 + 58, 12, 1.9190552835155001E-09,-2.5326884839777791E-08, 2.4222183149681001E-08, 2.4248307174367001E-08 + 58, 13, 4.3349935657125041E-08,-5.5039857107314157E-09, 2.4024721454196998E-08, 2.4062054591908001E-08 + 58, 14, 2.6400208518652658E-09,-5.5199832398728522E-09, 2.3815698157629999E-08, 2.3839124279540999E-08 + 58, 15, 2.2896118562855858E-08,-6.7167294601887563E-09, 2.3580258963418999E-08, 2.3608599548070999E-08 + 58, 16,-1.8814575065865669E-08, 1.1681224069162469E-08, 2.3354468235455001E-08, 2.3360845706181999E-08 + 58, 17,-1.2918549972594160E-08,-3.6447320615214430E-09, 2.3111805653418000E-08, 2.3093224009617999E-08 + 58, 18, 1.7610499348107371E-08, 1.9426009155064079E-08, 2.2841010223468000E-08, 2.2811994505252001E-08 + 58, 19, 1.7472656472487431E-09, 1.6190316469249129E-08, 2.2553307433727001E-08, 2.2550086888657001E-08 + 58, 20,-1.8627153910916749E-08, 1.3661725487086760E-08, 2.2263483820704001E-08, 2.2245610550079999E-08 + 58, 21,-2.0974739804714060E-08, 1.6783131812981899E-08, 2.1938602917671999E-08, 2.1929371744731001E-08 + 58, 22,-3.0029943880694499E-09, 2.2584725860829389E-08, 2.1626963054113001E-08, 2.1630591878246001E-08 + 58, 23,-4.5605096042276997E-08, 6.1863124735647972E-08, 2.1285970733616000E-08, 2.1281081725883998E-08 + 58, 24,-1.1172942407539200E-08, 4.3210625972034012E-09, 2.0970597471557000E-08, 2.0947114236464000E-08 + 58, 25, 2.4901249666543930E-08, 3.9213295773485751E-08, 2.0584933848324001E-08, 2.0620356595468000E-08 + 58, 26,-3.5074484593944872E-08,-2.3026039630124899E-08, 2.0273148076289002E-08, 2.0230791843364001E-08 + 58, 27, 2.4936154099689351E-08,-2.6145367340125850E-09, 1.9893773961820999E-08, 1.9855202717656999E-08 + 58, 28,-3.4510547743000031E-08,-3.3592497547361561E-08, 1.9512726534076001E-08, 1.9494154725341999E-08 + 58, 29, 1.9908993362642989E-08,-2.9174410008416009E-08, 1.9113510813227999E-08, 1.9110783818848001E-08 + 58, 30,-1.2495981031819341E-08,-3.1093920172654300E-08, 1.8712553701312000E-08, 1.8733063658064999E-08 + 58, 31,-1.8705067364975680E-08,-5.4283131902508492E-08, 1.8312465602296000E-08, 1.8324285752821000E-08 + 58, 32, 2.5565181696173861E-08,-1.6915591003628391E-08, 1.7908685229686001E-08, 1.7939607917385000E-08 + 58, 33,-2.6439019069162840E-08, 3.0067905518930157E-08, 1.7466011770793000E-08, 1.7515446106870000E-08 + 58, 34,-1.5246597035486791E-08,-1.9786593599663089E-08, 1.7082823988812000E-08, 1.7082370712763001E-08 + 58, 35, 1.2793002485075790E-09, 9.3491926090917502E-10, 1.6661035929050999E-08, 1.6667416675635999E-08 + 58, 36,-4.6691491419155172E-08, 2.3502395496399610E-08, 1.6210830156671999E-08, 1.6224075687680001E-08 + 58, 37, 1.3087725087608799E-08, 2.8977138289796159E-08, 1.5815631184190000E-08, 1.5791498559058000E-08 + 58, 38,-4.1402226929529901E-08, 1.5078509727562809E-08, 1.5339812882953001E-08, 1.5329527115366000E-08 + 58, 39, 4.4214687863619512E-08,-7.6762173132433868E-09, 1.4904978256429001E-08, 1.4890704309938999E-08 + 58, 40,-3.0990227692192272E-09,-1.4164079832113559E-08, 1.4428662602413000E-08, 1.4426732845557000E-08 + 58, 41, 2.0990087804697419E-08, 2.5570376925147250E-09, 1.3963713351700001E-08, 1.3981539721727000E-08 + 58, 42, 3.3703528780648941E-08,-1.4487124682240640E-08, 1.3476132544077001E-08, 1.3493229506713999E-08 + 58, 43, 1.4021584496293430E-08,-1.2582895918467361E-08, 1.2993917406591000E-08, 1.2981216354442000E-08 + 58, 44, 3.2567836769240513E-08,-3.6295402617081532E-08, 1.2507263701137000E-08, 1.2503388732073000E-08 + 58, 45, 4.2916409929425503E-08,-1.8036956983188720E-08, 1.2017784030296001E-08, 1.2015111284432000E-08 + 58, 46,-7.1144538258152400E-08, 3.2872365133388802E-08, 1.1468782545102000E-08, 1.1480444056564999E-08 + 58, 47, 9.5521659354754124E-09,-6.3597445424838331E-08, 1.0980238766690001E-08, 1.0974063725761000E-08 + 58, 48,-4.7810928558461138E-08, 7.4243998925716279E-09, 1.0433666967160001E-08, 1.0412322453846000E-08 + 58, 49,-1.1742980318201830E-08,-1.1000940432308250E-08, 9.8853171003749997E-09, 9.8899645211655995E-09 + 58, 50,-9.3381588076665514E-09, 2.5630718719408970E-08, 9.3589944577075998E-09, 9.3634677968674003E-09 + 58, 51, 1.6319329828924680E-09, 1.4570838989938131E-08, 8.7417316940462007E-09, 8.7643736185063000E-09 + 58, 52, 1.8271757614159821E-09, 5.8032807572724532E-08, 8.1871039188667002E-09, 8.2055978038569993E-09 + 58, 53, 2.5546615691538040E-08, 3.7894814319004807E-08, 7.5218518820381008E-09, 7.5151255663560996E-09 + 58, 54, 1.5635265611709542E-08, 2.6840692699293371E-08, 6.8107433028128002E-09, 6.8286442871962998E-09 + 58, 55, 1.9961030200840940E-08, 2.8679403475494671E-09, 6.1405474838749998E-09, 6.1432889481565001E-09 + 58, 56, 2.6822056952802661E-08,-6.4701730814571170E-09, 5.0027924114675003E-09, 5.0035768672705001E-09 + 58, 57,-4.8004650079194133E-08, 7.5216170477841512E-08, 3.2165849315731002E-09, 3.1940294951165000E-09 + 58, 58, 2.9862851975805060E-09,-7.1622010981229529E-08, 2.6379560770613000E-09, 2.6687755088234001E-09 + 59, 0,-7.5091581591094091E-08, 0.0000000000000000E+00, 2.7000923212774001E-08, 0.0000000000000000E+00 + 59, 1,-1.9038071578597198E-09,-2.3483280674136780E-08, 2.6987277320081000E-08, 2.6990264894905000E-08 + 59, 2, 2.0318680730819010E-08,-4.9375869395951407E-08, 2.6930978249876001E-08, 2.6908173315937000E-08 + 59, 3, 2.3185171040539781E-08,-1.7057122867018701E-08, 2.6882940045801001E-08, 2.6929971560227999E-08 + 59, 4,-2.0501298053190071E-08,-7.1577687868231148E-09, 2.6793465493122001E-08, 2.6849988185269000E-08 + 59, 5, 4.1831845702114471E-09, 9.6073912894566784E-09, 2.6745577311574000E-08, 2.6794166173867000E-08 + 59, 6, 3.5221516357942591E-08, 1.6661748674528059E-09, 2.6520039963649999E-08, 2.6810610090656999E-08 + 59, 7, 3.2000972955992507E-08, 3.8829937775706441E-09, 2.6619140531743002E-08, 2.6525164457805999E-08 + 59, 8, 2.5430958332904771E-08,-6.7540867044082994E-09, 2.6406154409368000E-08, 2.6490132020154001E-08 + 59, 9,-1.4493939798645620E-08, 1.5891521408192990E-08, 2.6245269290411001E-08, 2.6384018383178001E-08 + 59, 10, 1.2874284962696370E-09, 4.9885131653795834E-09, 2.6164567572310001E-08, 2.6170328451440001E-08 + 59, 11,-3.2806495147442693E-08,-9.4803948887888155E-09, 2.6005632718034002E-08, 2.5988580768550002E-08 + 59, 12,-7.8136808023975637E-09,-3.0069108831890848E-08, 2.5819667627380001E-08, 2.5837530956773998E-08 + 59, 13, 4.0228082095788072E-08,-1.2762531030684760E-08, 2.5604951492456000E-08, 2.5639735110728999E-08 + 59, 14,-5.9028641583681418E-09, 1.1634597752272309E-08, 2.5420988372020000E-08, 2.5433203308209000E-08 + 59, 15,-3.2508033424529872E-08,-3.7505592204065928E-08, 2.5181749597636000E-08, 2.5189998330581000E-08 + 59, 16,-5.2234996422991854E-09, 2.8758974462518200E-09, 2.4965112692263999E-08, 2.4952981431446001E-08 + 59, 17,-1.2010233571955460E-08,-2.1938299757070819E-08, 2.4711455963296999E-08, 2.4708853344390000E-08 + 59, 18, 5.1868305551150567E-09, 4.2225453930158641E-09, 2.4456675584854999E-08, 2.4424099947832001E-08 + 59, 19, 5.8747968733477918E-10, 1.4066593209169590E-08, 2.4171145961779000E-08, 2.4170943008490999E-08 + 59, 20,-3.7582430819860744E-09, 7.0336643826736384E-09, 2.3887859577633999E-08, 2.3867265716805000E-08 + 59, 21,-7.8952599520638866E-09, 2.4664626335780549E-08, 2.3574487048164000E-08, 2.3575481548620000E-08 + 59, 22,-4.6400764232985528E-10, 4.1486787053903252E-08, 2.3249649863381001E-08, 2.3260204781574999E-08 + 59, 23, 1.8873261728867758E-08, 1.0337249402667921E-08, 2.2945319208940000E-08, 2.2936792681962001E-08 + 59, 24, 1.7712504152170731E-08, 2.1450436035769050E-08, 2.2619683250668000E-08, 2.2580426627553000E-08 + 59, 25,-2.2263659735897861E-08, 2.6412184918243660E-08, 2.2255972220170000E-08, 2.2284965632852000E-08 + 59, 26, 1.6413954634663330E-08,-2.5609734872547449E-08, 2.1921872262399000E-08, 2.1887537717174000E-08 + 59, 27,-1.9828682667067850E-08,-1.0496627129445150E-08, 2.1571650618858001E-08, 2.1527990550728000E-08 + 59, 28,-2.8285367913793619E-08,-3.2686185070609621E-08, 2.1170029710212999E-08, 2.1169722239048002E-08 + 59, 29,-1.4577962014205940E-08, 2.1734115584931960E-09, 2.0797950041609001E-08, 2.0793353880392999E-08 + 59, 30,-8.5797045139896771E-09,-3.1044197978555687E-08, 2.0375030961257000E-08, 2.0417610858072999E-08 + 59, 31, 2.5368981154946951E-09, 2.5025684864256909E-08, 1.9992556819309000E-08, 2.0015091007162001E-08 + 59, 32,-3.0376217549835529E-10,-1.1263180700285919E-08, 1.9576949972493000E-08, 1.9617077456989000E-08 + 59, 33, 1.9964445388565680E-08,-1.4656666546230770E-08, 1.9174075210812999E-08, 1.9207373410291001E-08 + 59, 34, 3.3952555621562273E-08, 8.8164171379139259E-09, 1.8753864823126002E-08, 1.8741807364301001E-08 + 59, 35,-1.7056905050651480E-08, 2.5554551543429871E-08, 1.8337509794384001E-08, 1.8343284650267000E-08 + 59, 36, 3.5816639882684607E-08, 2.3489390627850799E-08, 1.7898804121317999E-08, 1.7907014301213999E-08 + 59, 37,-2.0323626740639240E-08, 4.6491441134576053E-08, 1.7470465353956000E-08, 1.7436415094340999E-08 + 59, 38, 5.1771774762910912E-08,-3.5850292843105938E-08, 1.7029420978892999E-08, 1.7010435891389999E-08 + 59, 39, 2.8797940652478778E-08, 1.6873684517769471E-08, 1.6535799025125001E-08, 1.6526782316852000E-08 + 59, 40, 6.8679205939191540E-09,-2.5002977481646728E-08, 1.6080619365857000E-08, 1.6083213398295000E-08 + 59, 41,-3.1340550209916322E-08, 4.9162597200635843E-09, 1.5585198208150999E-08, 1.5603850895531000E-08 + 59, 42,-3.4660745619997700E-08,-4.7308379329145953E-08, 1.5111770885507000E-08, 1.5121670119886001E-08 + 59, 43,-8.8810821045887877E-09,-6.3306309091624832E-09, 1.4603008020586000E-08, 1.4603073155339000E-08 + 59, 44, 6.4757674880955480E-10,-5.3427916352357057E-08, 1.4091389098351001E-08, 1.4083162920375000E-08 + 59, 45,-1.4954267628976579E-08, 2.8488296800838760E-08, 1.3579304538970000E-08, 1.3576869769574000E-08 + 59, 46,-1.9036033461663869E-08,-3.1268568793447031E-08, 1.3059727285976000E-08, 1.3068523869076000E-08 + 59, 47, 3.9656198555560764E-09, 4.2271568047756767E-09, 1.2487121723272001E-08, 1.2484310043066000E-08 + 59, 48,-2.0641280280147881E-08, 3.5031907135038968E-08, 1.1965798214329000E-08, 1.1949638558530000E-08 + 59, 49, 2.1222103459619591E-08, 8.5315373284775782E-09, 1.1365625453443000E-08, 1.1368593713595000E-08 + 59, 50, 1.1327130843346560E-08, 3.4548304209638693E-08, 1.0806897340129001E-08, 1.0809534453270001E-08 + 59, 51, 3.7628158169680752E-08,-2.8592771040614770E-08, 1.0215717750091000E-08, 1.0222993229136001E-08 + 59, 52, 9.7025198985422034E-09, 1.0715546914694281E-08, 9.5627499912546006E-09, 9.5844750039766005E-09 + 59, 53, 7.3038998467914648E-09, 1.4802828937142580E-08, 8.9677625706929992E-09, 8.9872272848040000E-09 + 59, 54, 1.9530101503351350E-08,-5.1370145254809817E-08, 8.2323622508636007E-09, 8.2416931669464997E-09 + 59, 55, 3.0875791679519418E-08, 2.5066639639273108E-09, 7.4739674132983006E-09, 7.4823957772288998E-09 + 59, 56, 2.8809792820601490E-08,-1.2008492805136421E-09, 6.7675602980312996E-09, 6.7736552853342996E-09 + 59, 57,-3.2873226969815460E-08,-1.1065316936877790E-08, 5.4883500004618000E-09, 5.4670945877038996E-09 + 59, 58, 5.3696086964138218E-08, 4.7224842380252853E-08, 3.4511797508372999E-09, 3.4660728182546001E-09 + 59, 59,-5.5454066056719202E-08, 3.1334102107489401E-08, 2.9810671321072999E-09, 2.9622354608023002E-09 + 60, 0,-3.3108636764790851E-08, 0.0000000000000000E+00, 2.8484388185223001E-08, 0.0000000000000000E+00 + 60, 1, 6.3245531296261832E-11, 1.3490441539497419E-09, 2.8443329521459999E-08, 2.8447544192618999E-08 + 60, 2, 1.9917628107598070E-08, 1.7868898713223569E-08, 2.8397649380840000E-08, 2.8390852549471998E-08 + 60, 3,-5.3664678607413780E-09,-4.7914590742039280E-09, 2.8338111801216000E-08, 2.8381547978614999E-08 + 60, 4,-1.9702723680909059E-08,-1.4700802313100581E-08, 2.8262961865673999E-08, 2.8322298694695001E-08 + 60, 5,-3.1372550271836148E-08, 5.4229768058367593E-09, 2.8201232313754000E-08, 2.8236015149041999E-08 + 60, 6, 2.7005349245876980E-08,-1.8300971074691819E-08, 2.8000059026256001E-08, 2.8269895107368001E-08 + 60, 7, 6.8339248381042761E-09,-2.9258135055096260E-09, 2.8062680221880001E-08, 2.7979634037254000E-08 + 60, 8, 4.1861779483395059E-09,-2.7012099850646041E-08, 2.7872740519355000E-08, 2.7959475866759999E-08 + 60, 9, 2.4093985319316018E-09, 4.7922791286457374E-09, 2.7710235280044002E-08, 2.7821894041123001E-08 + 60, 10,-1.5008138379742071E-08, 1.2585563615914220E-08, 2.7624203564456001E-08, 2.7651156791209999E-08 + 60, 11, 4.0929390562970649E-09, 1.7236051224280698E-08, 2.7470935511719001E-08, 2.7444890558176001E-08 + 60, 12, 1.1122976597496199E-08, 2.0862382491447250E-08, 2.7291410670863998E-08, 2.7312547039346999E-08 + 60, 13, 1.9375664288297401E-08, 3.1833057034099230E-08, 2.7085083605709999E-08, 2.7108642366823999E-08 + 60, 14,-7.6025086486352066E-09, 5.1858593199798893E-08, 2.6895812549041000E-08, 2.6912087605975001E-08 + 60, 15, 2.0211657793549460E-08,-2.3676682501497068E-08, 2.6668155520655999E-08, 2.6685643349404999E-08 + 60, 16,-1.9360838497187169E-08,-3.3400658435871957E-08, 2.6451153077456000E-08, 2.6443927012372999E-08 + 60, 17, 2.9353412859014520E-08, 2.5009634169745061E-08, 2.6221687958131001E-08, 2.6209751788840002E-08 + 60, 18,-2.7767092525729980E-08,-3.2434893251114942E-08, 2.5961715583733999E-08, 2.5943456436469000E-08 + 60, 19, 1.0414563404275889E-08,-4.5462551687539688E-09, 2.5690908400504000E-08, 2.5698924825759000E-08 + 60, 20,-1.1057579273098140E-08, 1.6959288955175360E-08, 2.5416306094685001E-08, 2.5397756442497001E-08 + 60, 21, 3.3209209507735702E-08,-1.5981218452971819E-08, 2.5124424662745001E-08, 2.5115876269842001E-08 + 60, 22, 1.2478825021207209E-08,-3.6244843463215112E-08, 2.4808876623690000E-08, 2.4811292727183000E-08 + 60, 23, 2.4340366484305301E-08,-3.7137298386868547E-08, 2.4501035754701998E-08, 2.4500843188617001E-08 + 60, 24, 6.9303493912016028E-09,-1.3315350467132830E-08, 2.4196713476991999E-08, 2.4173079353563999E-08 + 60, 25,-6.8193795670069533E-09, 1.1697190004464080E-08, 2.3830137078506001E-08, 2.3874397218769000E-08 + 60, 26,-7.7822578175871859E-09, 5.4713411424861923E-08, 2.3531430850172001E-08, 2.3499716866371001E-08 + 60, 27,-3.0587836825738218E-08,-1.5604018594999539E-08, 2.3173773401254999E-08, 2.3133740980976001E-08 + 60, 28, 3.3586045157243922E-08, 2.8615082870361401E-08, 2.2805295787060000E-08, 2.2802407144463000E-08 + 60, 29, 4.2172744534238094E-09, 3.5869153661210820E-08, 2.2421936374379000E-08, 2.2427676731406002E-08 + 60, 30, 3.0185643205040271E-08, 2.8032185975427322E-08, 2.2035819796305998E-08, 2.2066404666394999E-08 + 60, 31,-8.1002103258777091E-09, 3.2656442856011377E-08, 2.1638667242554001E-08, 2.1669483956988999E-08 + 60, 32,-4.2162348396286048E-08,-2.0099365896839279E-08, 2.1253256444635001E-08, 2.1274800952894999E-08 + 60, 33, 2.5400824161614969E-08, 1.0319427932579880E-08, 2.0839934779352999E-08, 2.0872032863766001E-08 + 60, 34, 1.3035789685759499E-08,-2.5766316510051400E-08, 2.0448753535302999E-08, 2.0442423784029999E-08 + 60, 35, 5.5282965374147284E-09, 1.2923405054370780E-08, 2.0001488166994999E-08, 2.0012716302767001E-08 + 60, 36,-1.2742321344060860E-08, 1.1381796973302179E-09, 1.9588907871742001E-08, 1.9604792979722001E-08 + 60, 37,-1.6661615216549660E-09,-2.1232802026805081E-08, 1.9168606057335000E-08, 1.9146891325671000E-08 + 60, 38, 3.0021625016959630E-08,-2.5850736439286800E-09, 1.8705485839393001E-08, 1.8689295317022999E-08 + 60, 39,-1.4648412475004110E-08,-2.9522087276308460E-08, 1.8244573706194002E-08, 1.8249014411821001E-08 + 60, 40, 1.5473206744825189E-08, 1.5330230901398549E-08, 1.7753275130222999E-08, 1.7751220232629001E-08 + 60, 41,-1.3721112616586730E-08, 1.8260290052167121E-08, 1.7286869375621001E-08, 1.7307275693365999E-08 + 60, 42,-1.9402335532355100E-08,-8.5448126017970379E-09, 1.6778289050945001E-08, 1.6797155619090999E-08 + 60, 43,-7.4084173924363597E-10, 1.0594094195039840E-08, 1.6291618132478999E-08, 1.6297796304134002E-08 + 60, 44,-2.4931723796387840E-08, 2.7318223017861991E-08, 1.5751221484791999E-08, 1.5771171611261000E-08 + 60, 45,-2.1382153577429429E-08, 3.3875496356341523E-08, 1.5232482774720999E-08, 1.5216384248514999E-08 + 60, 46, 3.2993927467381813E-08,-3.8992381867388593E-08, 1.4687771301365001E-08, 1.4702431330182001E-08 + 60, 47,-1.9201224723136971E-08, 3.9756671452646441E-08, 1.4150910802410001E-08, 1.4149023319085000E-08 + 60, 48, 3.9387678175265283E-08, 2.6107738260247449E-08, 1.3564126992324000E-08, 1.3527874163635000E-08 + 60, 49, 2.1849854251892091E-08, 6.2413103119478465E-08, 1.2990460942510999E-08, 1.2996993944737000E-08 + 60, 50,-3.1633782337288500E-09,-3.1661047792882230E-08, 1.2371410712090000E-08, 1.2378080019510000E-08 + 60, 51, 9.2632542761942713E-09, 3.5166982217625498E-09, 1.1771356439167000E-08, 1.1761810530674000E-08 + 60, 52,-2.4168316693313160E-08,-5.2756977043446752E-09, 1.1129002331473000E-08, 1.1143284981364000E-08 + 60, 53,-4.2070791251197377E-09,-3.2588993781587271E-09, 1.0439201168250000E-08, 1.0461511686001999E-08 + 60, 54, 1.1708264116049810E-08,-3.0161967265025688E-09, 9.8131600495227007E-09, 9.8114236742132006E-09 + 60, 55,-5.9913661584365440E-08,-2.1727304165026729E-09, 8.9925217384418008E-09, 9.0028805597320004E-09 + 60, 56,-3.4349111983262941E-08,-1.8121887940579761E-08, 8.2092972623061005E-09, 8.2008904795931002E-09 + 60, 57,-1.1491508456620339E-08, 1.9407185362639842E-09, 7.4154937094794997E-09, 7.4236965081321003E-09 + 60, 58,-2.0323657779749828E-08, 6.8802901325972923E-08, 6.0105320635522002E-09, 5.9646458613258996E-09 + 60, 59,-1.8435731254147809E-08,-6.7674814100362951E-08, 3.8214866103512997E-09, 3.7967938009894003E-09 + 60, 60, 3.5480644283013412E-08, 3.2671657999570567E-08, 3.2209938672415000E-09, 3.2475151691014000E-09 + 61, 0,-2.9130180219896170E-08, 0.0000000000000000E+00, 2.9781164522348000E-08, 0.0000000000000000E+00 + 61, 1, 1.5529868065833130E-08, 3.2834716479107740E-08, 2.9759975821052999E-08, 2.9754601137635002E-08 + 61, 2, 1.4149065639100971E-08,-5.1303556170471017E-09, 2.9678775857431999E-08, 2.9678280567000999E-08 + 61, 3, 1.6457750563703200E-08,-1.7658601641076590E-08, 2.9642005411167001E-08, 2.9685911626282999E-08 + 61, 4,-2.9831405430521712E-09,-1.1442665281632510E-08, 2.9552617868877001E-08, 2.9595328059670000E-08 + 61, 5,-3.3143261645858673E-08, 5.1267789933400538E-09, 2.9504475495845001E-08, 2.9541086969886001E-08 + 61, 6,-3.0168647462827941E-08,-3.7057223003806122E-10, 2.9294441951336001E-08, 2.9537333638019000E-08 + 61, 7,-6.4657091527999573E-09,-1.3880826635212660E-08, 2.9367116239522999E-08, 2.9281259286876999E-08 + 61, 8,-1.0663425428489990E-08,-9.3900374284642398E-09, 2.9152965818455000E-08, 2.9243170665305000E-08 + 61, 9, 3.0074241388433618E-09,-2.1231270239997931E-08, 2.9017654329396000E-08, 2.9127611857311000E-08 + 61, 10, 1.9930166846039130E-08,-2.3660012429757289E-08, 2.8921761563387000E-08, 2.8930112690830001E-08 + 61, 11,-6.9389008014433722E-09, 1.4593403859407450E-08, 2.8782943015170002E-08, 2.8760012979838999E-08 + 61, 12, 3.6514408191221033E-08, 1.4760965337687591E-08, 2.8591000512554001E-08, 2.8616966171759002E-08 + 61, 13, 3.9344717569271672E-08,-8.9846692011105705E-09, 2.8407088712825999E-08, 2.8431340336653001E-08 + 61, 14, 1.0063751701860420E-08,-2.4265074674296419E-08, 2.8217856557009000E-08, 2.8231547242653001E-08 + 61, 15, 1.0756445290579779E-08, 2.6615225288869419E-08, 2.8009728841290001E-08, 2.8018313436471998E-08 + 61, 16, 4.3158058429207126E-09,-1.7219579638812131E-08, 2.7798009123186001E-08, 2.7797129499802999E-08 + 61, 17,-7.0136574273283194E-09, 1.0368676022094290E-08, 2.7579893312517999E-08, 2.7562820875746999E-08 + 61, 18, 1.9780118079406259E-08, 1.5873613629791469E-08, 2.7339215073494001E-08, 2.7321670634627000E-08 + 61, 19, 3.6585976473783750E-09, 3.8486624377278551E-08, 2.7082949929027998E-08, 2.7082454966826001E-08 + 61, 20,-1.2104457686059771E-08, 3.8810671014025192E-08, 2.6816762317364999E-08, 2.6799458625986999E-08 + 61, 21,-2.0169405665127871E-08,-1.4027611613523641E-08, 2.6535272918719999E-08, 2.6538050929126999E-08 + 61, 22,-4.2652611167724140E-08,-2.9095642112579291E-08, 2.6242615552645001E-08, 2.6246638373177002E-08 + 61, 23, 8.9313465924684437E-10, 9.5200986060911763E-09, 2.5949087199543000E-08, 2.5944784252622000E-08 + 61, 24,-3.4078779112168460E-09,-3.7299217412466573E-08, 2.5664062431959001E-08, 2.5636128197291999E-08 + 61, 25, 4.0695776472107601E-08,-1.7135515675758431E-08, 2.5321996843710999E-08, 2.5355975312404999E-08 + 61, 26,-2.6037754530014981E-09, 1.0193130700765800E-08, 2.5027615851228001E-08, 2.4985557454081000E-08 + 61, 27, 9.2865456393948503E-09, 2.6780787089180259E-08, 2.4690301683027002E-08, 2.4661725965614001E-08 + 61, 28, 2.5554012749534961E-08,-1.4537228785195599E-08, 2.4335072132418999E-08, 2.4312955218539000E-08 + 61, 29, 2.5280999273120370E-08,-3.5637875215108240E-08, 2.3981249553552000E-08, 2.3984508619547001E-08 + 61, 30, 1.7582074742540351E-08, 2.9899262696523020E-10, 2.3590875442902999E-08, 2.3624057151017999E-08 + 61, 31,-1.9461396102115349E-08, 4.7193195834991026E-09, 2.3237939660633001E-08, 2.3252598256900002E-08 + 61, 32, 1.9286211346271760E-08, 2.0751588640982350E-08, 2.2837000296677999E-08, 2.2863737882031000E-08 + 61, 33,-1.5850408859360691E-09, 9.2103801168139082E-09, 2.2454528995757001E-08, 2.2488213095407000E-08 + 61, 34,-1.4160166665933209E-08, 1.6323826558519009E-08, 2.2066170956941000E-08, 2.2070417020504000E-08 + 61, 35,-1.9594216975646720E-08,-2.0211202389776729E-08, 2.1658068831209002E-08, 2.1658253551518001E-08 + 61, 36,-3.0488919401607970E-09,-1.7035973644770961E-08, 2.1221749084774001E-08, 2.1239313561716000E-08 + 61, 37,-2.7772349810590451E-08,-6.0257207365959393E-09, 2.0843263810329998E-08, 2.0814544446880999E-08 + 61, 38, 4.2172410190415502E-10, 5.5811510020746159E-09, 2.0398163224175999E-08, 2.0367963674340001E-08 + 61, 39,-1.1375231310601739E-08,-2.9951172321742539E-09, 1.9913796140790002E-08, 1.9920977592014001E-08 + 61, 40,-1.3969940626541909E-08, 2.0834768995022729E-08, 1.9464564924184000E-08, 1.9463704440782998E-08 + 61, 41, 1.6011818928376410E-09,-2.0980672613247059E-08, 1.8966002081239999E-08, 1.8977946592149999E-08 + 61, 42, 3.0729257136870482E-08, 4.2547884423009183E-09, 1.8498067316009999E-08, 1.8513827209368000E-08 + 61, 43,-1.4520699531900530E-08,-5.5374200285790552E-08, 1.7986525028163999E-08, 1.7993779743662000E-08 + 61, 44,-2.0015657687847040E-08, 2.2249604901400101E-08, 1.7461607144120001E-08, 1.7513785622205000E-08 + 61, 45, 2.4953631732441051E-08,-4.8949003577835138E-09, 1.6937135211987000E-08, 1.6935406189246999E-08 + 61, 46, 1.1233947058348081E-08, 1.3245605516922701E-08, 1.6391851544655001E-08, 1.6391430604012001E-08 + 61, 47,-9.6766872945171317E-09, 8.6435694090469070E-09, 1.5840862517631001E-08, 1.5840398739222001E-08 + 61, 48, 1.4072970334352799E-08, 7.2205829690304423E-09, 1.5289600281169002E-08, 1.5261373427984999E-08 + 61, 49,-5.7078183596301883E-08, 8.5056731039286548E-09, 1.4645072353037001E-08, 1.4642456157803000E-08 + 61, 50, 7.1930503799837444E-09,-4.1268630993802851E-08, 1.4071889019193000E-08, 1.4080954015543001E-08 + 61, 51,-4.7711693604012778E-08, 3.8042974530092843E-08, 1.3406531767408000E-08, 1.3405268228532000E-08 + 61, 52,-7.2881536601116533E-09,-3.5168580491916767E-08, 1.2772429218752001E-08, 1.2779785135331999E-08 + 61, 53,-3.2908535821920413E-08,-9.8534605160237535E-11, 1.2096298629322000E-08, 1.2109173691288999E-08 + 61, 54, 1.4528655891089940E-08, 2.1259484567587670E-08, 1.1375768484231000E-08, 1.1392398365321000E-08 + 61, 55,-3.6668880715493373E-08, 2.3869613556866231E-08, 1.0692837388740000E-08, 1.0700973547027000E-08 + 61, 56,-2.4389956927733500E-08, 7.4085640226178401E-08, 9.8366175566569001E-09, 9.8328246147547992E-09 + 61, 57, 1.1679374141286490E-08, 2.4537594708907748E-08, 8.9909480451632004E-09, 8.9619428914987003E-09 + 61, 58,-2.2395473238179290E-08, 2.6334614456163300E-09, 8.0945856244875994E-09, 8.1461175118100001E-09 + 61, 59, 5.1684992799776827E-08, 3.3593287010680960E-09, 6.4916289100676999E-09, 6.5110065464841002E-09 + 61, 60,-6.7508690586049999E-08, 3.3375087509244801E-08, 4.0281571094318998E-09, 4.0409157653684004E-09 + 61, 61, 3.6241042619557372E-08,-4.8316793410786897E-08, 3.5422447267278999E-09, 3.5464042630630999E-09 + 62, 0,-1.5237515803023880E-08, 0.0000000000000000E+00, 3.0899384610665003E-08, 0.0000000000000000E+00 + 62, 1,-1.8531919623790820E-08,-3.8890275894473930E-08, 3.0857463237215000E-08, 3.0849004371518003E-08 + 62, 2, 2.6108324327238651E-08,-1.9842457978630911E-08, 3.0778774811834997E-08, 3.0789573675870001E-08 + 62, 3, 4.2407549595154972E-08,-1.6179649107590579E-08, 3.0732644533909001E-08, 3.0776422310528002E-08 + 62, 4,-7.7063873401823322E-09, 1.2432801385891009E-08, 3.0662884762134998E-08, 3.0695472092522001E-08 + 62, 5, 3.2581293698565353E-08,-5.6766528212745827E-09, 3.0595970727672997E-08, 3.0632027935106002E-08 + 62, 6,-1.4658916811607160E-08, 5.9576260707113587E-08, 3.0420128411111999E-08, 3.0627102329291000E-08 + 62, 7,-3.9890929756252423E-09,-6.3333303134135598E-09, 3.0451327110902997E-08, 3.0382279564940999E-08 + 62, 8, 9.5311643639130883E-09,-1.1989756675781740E-08, 3.0275864932213997E-08, 3.0345055804840998E-08 + 62, 9, 1.2603488219635900E-08, 1.2334063269542271E-09, 3.0122831701590998E-08, 3.0223560113748999E-08 + 62, 10,-3.3586311941422273E-08,-4.1395503370154489E-09, 3.0040185550076000E-08, 3.0056930640781000E-08 + 62, 11, 6.1661044849132983E-09,-2.3408994990373239E-08, 2.9890953250714997E-08, 2.9873583825149000E-08 + 62, 12, 4.8757776369615850E-09,-1.5201455089911658E-08, 2.9729227463772999E-08, 2.9756335277573001E-08 + 62, 13, 1.5883538421322579E-08,-4.2983444724078311E-09, 2.9533057620480002E-08, 2.9561253907258999E-08 + 62, 14, 1.3433005727536640E-08,-1.1422657934616931E-08, 2.9385186996825001E-08, 2.9379089108769999E-08 + 62, 15,-1.2029071301056259E-08,-2.7557318117520870E-08, 2.9164588495436001E-08, 2.9159745329716001E-08 + 62, 16, 7.7882338370000420E-09,-2.5350499849952258E-08, 2.8975974592533000E-08, 2.8984176230942001E-08 + 62, 17, 4.9838941593198823E-09, 2.7034807211267029E-09, 2.8754348089131999E-08, 2.8746828345186001E-08 + 62, 18,-1.7288389745254151E-08, 1.2489286978839300E-08, 2.8541383087340999E-08, 2.8526236242896001E-08 + 62, 19, 1.2281820432965430E-08, 2.4847339811281221E-08, 2.8309658046831999E-08, 2.8301636850533001E-08 + 62, 20,-1.0337069721714379E-09,-1.9970529921677881E-08, 2.8049109418860999E-08, 2.8030746564452998E-08 + 62, 21,-2.3690547845982088E-08, 1.2102811683585121E-08, 2.7792538691541998E-08, 2.7786638629250999E-08 + 62, 22,-1.5988580605384198E-08,-8.4821073918670880E-09, 2.7519644877601999E-08, 2.7511107420759998E-08 + 62, 23,-3.2092207693364531E-08, 1.5436539399695551E-08, 2.7238959567649999E-08, 2.7239027009662001E-08 + 62, 24,-1.1143510954642609E-09,-2.5457776578700371E-08, 2.6961121347761001E-08, 2.6947822161920000E-08 + 62, 25,-9.1111602875161020E-09,-2.4785466123360571E-08, 2.6658076771343999E-08, 2.6701549565392998E-08 + 62, 26, 1.5086398695480401E-08,-1.7569935717630292E-08, 2.6375550939418999E-08, 2.6342858500637001E-08 + 62, 27, 1.1961857516279410E-08, 2.9798041182599981E-08, 2.6067827077262001E-08, 2.6033490635882999E-08 + 62, 28,-8.3799048164308070E-09,-3.0645532958038268E-08, 2.5732466066547999E-08, 2.5725612434688001E-08 + 62, 29,-1.5772310985773290E-09,-4.9885344027723107E-08, 2.5401315152775999E-08, 2.5396678759136000E-08 + 62, 30, 1.4967123397842400E-09, 1.4874585323265660E-08, 2.5047512996626000E-08, 2.5077816077718001E-08 + 62, 31, 5.9851869110575440E-09,-2.3356982632812680E-08, 2.4699854074199999E-08, 2.4711363432541000E-08 + 62, 32,-1.7785868028923150E-08, 4.0816476569953163E-08, 2.4342725900309000E-08, 2.4363780210493000E-08 + 62, 33,-2.5858098267769959E-08, 6.7459020995067190E-09, 2.3963515784113000E-08, 2.3985497687817999E-08 + 62, 34,-7.9590917717540688E-10,-1.4623329643685180E-08, 2.3612072059608001E-08, 2.3608974189382000E-08 + 62, 35, 2.9509128667746738E-08,-2.4503406605707760E-08, 2.3210662682604000E-08, 2.3216582152943000E-08 + 62, 36,-3.6209574607675588E-09,-4.5808145535708322E-09, 2.2810415937261002E-08, 2.2830908141270999E-08 + 62, 37,-2.0162433359424589E-08, 1.1428781780599639E-08, 2.2429910548439001E-08, 2.2395218417138001E-08 + 62, 38,-2.5236457595484909E-08, 5.7275103076928867E-08, 2.2029043752547000E-08, 2.1993991989529001E-08 + 62, 39, 1.1712322362681351E-08, 3.2980073222288307E-08, 2.1556503510032001E-08, 2.1557378158389001E-08 + 62, 40, 1.6212797664427780E-08, 2.2758704808720628E-08, 2.1104574539712999E-08, 2.1112398235205001E-08 + 62, 41, 3.5410149297695009E-09, 1.3804061048909900E-08, 2.0652618973037000E-08, 2.0661906460539999E-08 + 62, 42, 1.3384187922083369E-08,-8.7289276407054339E-09, 2.0160632242992001E-08, 2.0172836066718001E-08 + 62, 43, 8.6505787374064707E-11,-4.3873275702569212E-08, 1.9702914912705999E-08, 1.9705208799417000E-08 + 62, 44,-6.3056408848101623E-09,-3.3079824827468210E-09, 1.9165140202643998E-08, 1.9199752705036000E-08 + 62, 45, 2.2754650441098491E-08, 5.1390233117784097E-09, 1.8672642852935999E-08, 1.8672366525642999E-08 + 62, 46, 3.0819811511665142E-09,-1.4273101957530629E-08, 1.8110297051000999E-08, 1.8118374631736999E-08 + 62, 47,-7.5348848260711023E-09,-1.8025989986679691E-08, 1.7569075345234000E-08, 1.7553686548600002E-08 + 62, 48, 1.3809380157297630E-08,-3.2442066624646798E-08, 1.7017668052383999E-08, 1.6983181361529000E-08 + 62, 49, 3.1318184977251897E-08,-6.2576428901603979E-08, 1.6419489663281001E-08, 1.6420485474881001E-08 + 62, 50, 8.9179426240111851E-10, 3.8099888193077211E-10, 1.5771831598760000E-08, 1.5782294022313002E-08 + 62, 51,-2.7848743062215911E-08, 9.2974945957549845E-09, 1.5170113705013000E-08, 1.5174378372706000E-08 + 62, 52, 9.7184651794905158E-09, 3.1420874280204581E-08, 1.4471893156177000E-08, 1.4471749897231000E-08 + 62, 53,-1.7804636772243061E-08, 5.1381699601684186E-09, 1.3815917374777000E-08, 1.3826510558010001E-08 + 62, 54, 2.1683751028216750E-08, 7.6022286441440682E-10, 1.3107099205432000E-08, 1.3117702719641000E-08 + 62, 55,-7.9774227183647397E-09, 2.7134453637222849E-08, 1.2363941714681000E-08, 1.2344971695433000E-08 + 62, 56, 5.0374589917884743E-08, 2.6010517162053260E-08, 1.1624747797484999E-08, 1.1649282902021001E-08 + 62, 57, 5.6780152753494277E-08,-1.2392053686854310E-08, 1.0714694293530001E-08, 1.0703691238204000E-08 + 62, 58, 1.0037816453422260E-08,-6.5769348915798304E-10, 9.7858082848445002E-09, 9.7997385850099002E-09 + 62, 59,-6.9343878286298350E-09, 2.2798363187571119E-09, 8.8443742798667000E-09, 8.8441752363808005E-09 + 62, 60,-2.7079507981401400E-08,-2.2071484646482510E-08, 7.0724819995894001E-09, 7.0469681831366998E-09 + 62, 61, 1.6646941052809011E-08, 3.8330530122948437E-08, 4.3856613360477998E-09, 4.3659226255275003E-09 + 62, 62,-1.0215911116872690E-08,-1.6276937368969710E-08, 3.9353058949744998E-09, 3.9810735624987004E-09 + 63, 0,-7.5943791443928821E-08, 0.0000000000000000E+00, 3.1788788081735000E-08, 0.0000000000000000E+00 + 63, 1, 2.7691886944838320E-08, 2.0596816742636831E-08, 3.1769147861156999E-08, 3.1753619461662002E-08 + 63, 2, 2.9715992967965741E-08,-1.7168373104857661E-08, 3.1649463056010000E-08, 3.1653987824499997E-08 + 63, 3, 3.6873414498162663E-08,-2.5481699502382870E-08, 3.1641472269514002E-08, 3.1677429317383001E-08 + 63, 4,-8.6167308610847245E-09,-3.2519014195806118E-08, 3.1538272163824002E-08, 3.1564099509351999E-08 + 63, 5, 4.0651639086864612E-08,-1.8003473493273741E-08, 3.1501102349586000E-08, 3.1526921087187999E-08 + 63, 6, 2.1585136644879010E-08, 1.7570149368159169E-09, 3.1314597281618001E-08, 3.1482860321529002E-08 + 63, 7,-1.1290846936463471E-08, 1.7467714563693679E-08, 3.1357124657597000E-08, 3.1291249729190000E-08 + 63, 8, 2.3337178975136330E-08, 3.2342114375696358E-10, 3.1163002352456001E-08, 3.1225726436892001E-08 + 63, 9,-2.2275180302268201E-08, 2.7953733130668779E-08, 3.1043261825079001E-08, 3.1122871550337001E-08 + 63, 10,-1.2816806413405159E-08,-3.0267227144410613E-08, 3.0937953653035003E-08, 3.0951764968447003E-08 + 63, 11, 2.5801605711006790E-08,-8.7939024430990625E-09, 3.0805558925134999E-08, 3.0804259107503002E-08 + 63, 12,-1.8389379773050990E-08, 1.8646342369911689E-09, 3.0649847639131001E-08, 3.0669072099572003E-08 + 63, 13,-8.4856291370786422E-09,-1.5355320220804460E-08, 3.0469816249391998E-08, 3.0505246756316003E-08 + 63, 14,-5.7087279085419957E-10, 1.5452907644911679E-08, 3.0318948805765997E-08, 3.0324007902354998E-08 + 63, 15,-2.1011376973990970E-08,-1.3767017009901371E-08, 3.0128529621199997E-08, 3.0127756213171999E-08 + 63, 16,-2.2581395192308731E-09,-1.0872859509997521E-08, 2.9945159980149001E-08, 2.9952971532521002E-08 + 63, 17, 1.8134489519029020E-08, 1.8866335532876371E-08, 2.9758296026211000E-08, 2.9744052489077001E-08 + 63, 18, 1.4017258991706780E-08, 4.9816702576110223E-09, 2.9545388272229000E-08, 2.9524824703803999E-08 + 63, 19,-1.1532560068250120E-08,-1.0690957080519140E-08, 2.9343677311349000E-08, 2.9349568233097001E-08 + 63, 20,-1.6216888705302001E-08,-1.3947410885202230E-08, 2.9094802913210999E-08, 2.9078167145358002E-08 + 63, 21, 7.4627265733165043E-09,-1.3996209166237691E-08, 2.8852221598228001E-08, 2.8856326439609999E-08 + 63, 22, 4.0529018113074132E-08, 3.5932579945092962E-08, 2.8606627835357001E-08, 2.8611769118975999E-08 + 63, 23,-7.4140290180824763E-09,-7.5820510940248867E-09, 2.8351243201945001E-08, 2.8348410100406000E-08 + 63, 24,-8.2268754280088796E-09,-3.6127920586659491E-08, 2.8099316590913001E-08, 2.8085033998541999E-08 + 63, 25,-2.1304690264748699E-08,-1.4957233990301941E-08, 2.7818781583144000E-08, 2.7857253430978001E-08 + 63, 26, 1.7685548783221130E-08, 1.9841475566080699E-08, 2.7561225141064000E-08, 2.7533904333766000E-08 + 63, 27,-1.1455959894545491E-08,-1.6962190771507431E-08, 2.7263035016433000E-08, 2.7238975300826000E-08 + 63, 28, 1.6142319292065671E-08,-1.9732834012873990E-09, 2.6965648379599002E-08, 2.6952200979715999E-08 + 63, 29,-2.1866482064642398E-09, 2.4083746321721769E-08, 2.6658685034848000E-08, 2.6655820652441001E-08 + 63, 30, 3.1877162289184529E-09,-6.1278281164164450E-09, 2.6327267024295999E-08, 2.6353310828262000E-08 + 63, 31,-1.0248441393665220E-08,-2.0206976321949530E-09, 2.6022900751786999E-08, 2.6030111430457000E-08 + 63, 32,-2.7234181345502660E-08,-2.1901469298433940E-08, 2.5663574980624000E-08, 2.5692830419268000E-08 + 63, 33,-1.3681752825881160E-08,-9.8587973153148841E-09, 2.5337937480627999E-08, 2.5360669600910000E-08 + 63, 34, 2.4508437244074708E-09,-1.1639560393034890E-08, 2.4983011719764000E-08, 2.4992605983647001E-08 + 63, 35, 2.6985356025638891E-08,-1.3436037276956600E-08, 2.4642417107931999E-08, 2.4636312223850999E-08 + 63, 36, 8.5539703263784263E-09,-3.8432582882081003E-08, 2.4256750449971000E-08, 2.4281763669311001E-08 + 63, 37, 4.4998393615860222E-11,-1.6988121256300739E-09, 2.3896296009102000E-08, 2.3874720146918001E-08 + 63, 38, 2.3426928395581240E-09, 1.9971330821829790E-08, 2.3522683351386999E-08, 2.3488141269059000E-08 + 63, 39, 2.1915572602372571E-08, 3.3133479841437401E-08, 2.3094047136546998E-08, 2.3099717186653002E-08 + 63, 40,-1.4030583486448660E-08, 4.1440567158597643E-09, 2.2667659459006001E-08, 2.2659117922604001E-08 + 63, 41, 1.0604175304375370E-08, 3.9141296193608923E-08, 2.2227935811114000E-08, 2.2237735576680001E-08 + 63, 42,-6.2309782507947553E-09,-1.2809717854969799E-08, 2.1774631481078999E-08, 2.1795705376317999E-08 + 63, 43, 1.8283900942308952E-08,-1.1825406610819371E-08, 2.1315640728736000E-08, 2.1314607641428001E-08 + 63, 44,-1.4350276195345560E-08,-1.4573448661338410E-08, 2.0831227182817999E-08, 2.0879333662931999E-08 + 63, 45,-1.0734452180132770E-08,-5.6054527630036539E-09, 2.0339503774517000E-08, 2.0339808806058000E-08 + 63, 46, 1.7038843727042671E-08, 8.4339169316858392E-11, 1.9831564217717999E-08, 1.9837057528383000E-08 + 63, 47,-1.4760539493376309E-08,-3.3815780174300388E-08, 1.9269223588422000E-08, 1.9270992591362999E-08 + 63, 48,-1.6937755172712510E-08, 2.5692447630744990E-09, 1.8743670418483000E-08, 1.8708822726706000E-08 + 63, 49, 1.2180462333865850E-08, 1.2432215099747449E-08, 1.8150753431939001E-08, 1.8147877004548999E-08 + 63, 50,-1.2255077769720080E-08,-2.4908844982229320E-08, 1.7585995615198998E-08, 1.7573175683667001E-08 + 63, 51, 5.6630020038144770E-09,-2.4457413764056681E-08, 1.6892509909201000E-08, 1.6899643662990999E-08 + 63, 52,-2.4661508290094039E-08,-2.2921855890018601E-08, 1.6283753946873001E-08, 1.6278810908301000E-08 + 63, 53, 1.1637836156486369E-08, 1.0189376128409431E-08, 1.5543452408284000E-08, 1.5574108708275999E-08 + 63, 54, 1.2442202938712420E-08,-6.5595671661389934E-09, 1.4899962000260999E-08, 1.4890626887338000E-08 + 63, 55, 7.2766949901697839E-09,-1.1520326893344109E-08, 1.4157789654027000E-08, 1.4132495738149000E-08 + 63, 56, 3.0654557296558080E-08,-6.3128240915831324E-08, 1.3364697751280000E-08, 1.3374943369799000E-08 + 63, 57, 4.4031401037442877E-08,-5.6483314238897011E-08, 1.2631825445683001E-08, 1.2626457665838000E-08 + 63, 58,-5.3451733517514187E-08,-5.9259095870344957E-08, 1.1583037117071000E-08, 1.1619449922422999E-08 + 63, 59,-1.2320226727700480E-08, 2.5111463673349749E-08, 1.0656144427685000E-08, 1.0647130038886000E-08 + 63, 60,-5.4728023000676557E-08,-4.2272795648675733E-08, 9.6325076384051996E-09, 9.6246810610475995E-09 + 63, 61,-4.2589592191794903E-08, 4.3703780610828117E-08, 7.5674953593446006E-09, 7.5357656199930002E-09 + 63, 62,-2.0580522002390040E-08,-1.5820159253095019E-08, 4.6548999034524001E-09, 4.6554384786422000E-09 + 63, 63,-2.5449001938065469E-08,-6.4648636006584637E-09, 4.3945918986273000E-09, 4.3940231470794001E-09 + 64, 0,-7.4449382843840351E-08, 0.0000000000000000E+00, 3.2469647070265998E-08, 0.0000000000000000E+00 + 64, 1,-3.2134535975874391E-08,-1.6485777335694871E-08, 3.2454745257944002E-08, 3.2434977864908997E-08 + 64, 2,-3.7492543129441683E-09, 1.3989217029388180E-08, 3.2309370985183002E-08, 3.2315465877613002E-08 + 64, 3, 1.1949846982847500E-08,-8.4259339503889063E-10, 3.2322092531969003E-08, 3.2348452853436997E-08 + 64, 4,-2.2267209979357219E-08, 1.2605373709994350E-08, 3.2213208295114999E-08, 3.2228604597233001E-08 + 64, 5, 3.3873868285291153E-10,-1.8065489002329500E-08, 3.2179315900339997E-08, 3.2203679019807001E-08 + 64, 6, 8.9096702309749915E-09,-4.9286018744709774E-09, 3.2002661684858001E-08, 3.2153042213845998E-08 + 64, 7,-3.5382643338429680E-09,-3.4717499330702748E-09, 3.2030776901566998E-08, 3.1967560910291000E-08 + 64, 8,-1.1774387069060490E-08, 1.1748360948792639E-08, 3.1867526158036998E-08, 3.1906991329027999E-08 + 64, 9,-1.4973808522952140E-08, 1.1302390381946730E-08, 3.1735480267309999E-08, 3.1803750370269003E-08 + 64, 10, 1.3055379067334780E-08,-4.0030665164994817E-08, 3.1642994183024000E-08, 3.1659068654116000E-08 + 64, 11, 3.2870122447696898E-10, 1.8224206276661419E-08, 3.1505427943118999E-08, 3.1507104462411001E-08 + 64, 12,-5.9965209405000724E-09, 1.3853505417795390E-09, 3.1372222098865000E-08, 3.1398588638998998E-08 + 64, 13, 9.0410021707575163E-09,-7.5510374028616690E-09, 3.1195451212704999E-08, 3.1227732514210999E-08 + 64, 14,-3.7209465964807762E-08,-4.2987766684075811E-10, 3.1071665425003001E-08, 3.1072639410598001E-08 + 64, 15,-4.4379149684779142E-08, 2.3299596621458512E-08, 3.0881130019812000E-08, 3.0867672556243000E-08 + 64, 16,-1.7626855847295689E-08, 6.2157194776472199E-09, 3.0724785778560001E-08, 3.0736764269151002E-08 + 64, 17, 3.9374075132516584E-09, 6.7448865385620587E-09, 3.0538770599093002E-08, 3.0529263149843003E-08 + 64, 18, 2.5398015896857329E-08,-2.0608960605843340E-08, 3.0361043499961998E-08, 3.0338782061530003E-08 + 64, 19, 1.7961973559312631E-09,-1.0707300577863900E-08, 3.0164194599927000E-08, 3.0160125008877001E-08 + 64, 20, 1.1344068421875490E-08,-1.0337467631336110E-08, 2.9953307109461001E-08, 2.9933095266775999E-08 + 64, 21,-1.6329697188216859E-08,-1.1599086132585630E-08, 2.9717169632819001E-08, 2.9711173180534001E-08 + 64, 22,-1.1826052197356130E-08,-6.0245486094387539E-10, 2.9500063566672999E-08, 2.9499005015553001E-08 + 64, 23, 9.7865609250484410E-09,-2.1254358112118091E-09, 2.9264116813437001E-08, 2.9264873545304999E-08 + 64, 24, 1.3326889233735801E-08,-1.8353970354225579E-08, 2.9023518703788000E-08, 2.9023226755794000E-08 + 64, 25, 1.6039271648389301E-09, 1.1577484419109340E-09, 2.8789624809696001E-08, 2.8819499471532000E-08 + 64, 26, 6.7419341769582543E-09, 4.3803129147250249E-09, 2.8544963657049998E-08, 2.8523093276426999E-08 + 64, 27,-4.2572441442897538E-08, 1.6447796974506020E-08, 2.8290720643729001E-08, 2.8264355486304000E-08 + 64, 28, 1.8019192646612490E-08, 3.5679029733359628E-08, 2.8007028532088002E-08, 2.7989249478314999E-08 + 64, 29, 1.2862418179503711E-08, 3.2413425183845530E-09, 2.7739637556347002E-08, 2.7735946195360999E-08 + 64, 30, 2.7932509454073880E-08,-4.1264183964135603E-08, 2.7432543622417000E-08, 2.7453969274017000E-08 + 64, 31, 1.2080390048981080E-08,-8.2059721167175321E-09, 2.7161311684665000E-08, 2.7164010755888001E-08 + 64, 32,-1.6945265954974461E-08, 3.8943986894285802E-09, 2.6839206279085000E-08, 2.6864706129276000E-08 + 64, 33,-1.4229095853173079E-08, 2.9762120309613048E-09, 2.6533169138690999E-08, 2.6545277767147000E-08 + 64, 34, 3.1612763298350502E-08, 3.8557911234177184E-09, 2.6230895148950001E-08, 2.6232214116561000E-08 + 64, 35,-2.2895209443881589E-08,-2.5036190661753769E-08, 2.5888134173403999E-08, 2.5891978370895998E-08 + 64, 36,-2.6383351436709791E-08, 1.3499765909527250E-08, 2.5568506439187000E-08, 2.5591057885523999E-08 + 64, 37, 9.6902701371694182E-09,-7.4978812404463664E-09, 2.5228676667123999E-08, 2.5195888942415002E-08 + 64, 38, 3.2302734095519121E-08, 5.7763257225849601E-09, 2.4883869343010999E-08, 2.4856156158987000E-08 + 64, 39, 9.2014524965311663E-09,-4.3343440936829571E-08, 2.4474637107729001E-08, 2.4486062752519001E-08 + 64, 40, 1.2500707896537740E-08,-2.3071652648993489E-08, 2.4099171573020000E-08, 2.4100888224358999E-08 + 64, 41,-3.4261154811809672E-08, 9.9700017766030358E-09, 2.3680485481563999E-08, 2.3693965793871001E-08 + 64, 42,-6.0097416286279361E-09,-7.7594686952157254E-09, 2.3258614679426001E-08, 2.3290563334430001E-08 + 64, 43, 5.5981717673728683E-09,-7.1833355198882459E-09, 2.2841530991026999E-08, 2.2843955303707000E-08 + 64, 44,-4.5141217304534722E-09,-1.6580659027018880E-08, 2.2351969213548002E-08, 2.2428047600589002E-08 + 64, 45, 2.7892950242233871E-08,-4.8742945589521472E-09, 2.1945910674227001E-08, 2.1938482556991001E-08 + 64, 46, 2.2514602818244649E-08, 8.6996466927462183E-09, 2.1436575485056000E-08, 2.1433600306314001E-08 + 64, 47,-1.2479549898600280E-08, 9.3920250571008670E-10, 2.0936577631763999E-08, 2.0936050781890001E-08 + 64, 48,-2.4785510931200299E-08, 1.2907566793828089E-08, 2.0400519048963002E-08, 2.0371707463557999E-08 + 64, 49, 1.6972193536732529E-08,-3.7873617031007326E-09, 1.9859587691329999E-08, 1.9835745473420999E-08 + 64, 50,-3.0299127083561782E-08,-3.0484224224850741E-08, 1.9282627399332000E-08, 1.9280460707405001E-08 + 64, 51,-1.3252912803313040E-08, 3.9640373769328083E-09, 1.8699299221339000E-08, 1.8676703115534001E-08 + 64, 52,-3.3698528811638562E-08,-5.5677655380967583E-08, 1.8005047130137000E-08, 1.7999458714196001E-08 + 64, 53,-9.2669226579688220E-09, 1.1495155773228341E-08, 1.7370031009717998E-08, 1.7390555619792001E-08 + 64, 54, 4.6573109109871371E-09,-2.2315904896199261E-08, 1.6643101150645999E-08, 1.6653925027734000E-08 + 64, 55,-2.6882956434639729E-08,-2.3961277104260370E-08, 1.5975990249501000E-08, 1.5956654675246001E-08 + 64, 56,-6.5945074783442799E-10,-2.6234657020469080E-08, 1.5201177801869999E-08, 1.5197785454174000E-08 + 64, 57,-3.7246900749063552E-08, 3.7724029179669770E-08, 1.4404426139175000E-08, 1.4421710479838001E-08 + 64, 58,-6.8754005123588049E-08,-5.3971610062573252E-08, 1.3602106798003000E-08, 1.3612084954101001E-08 + 64, 59,-4.8033710371110173E-09, 6.2434348030641530E-08, 1.2514549758802000E-08, 1.2523246322021999E-08 + 64, 60, 2.3488664348531219E-08, 2.1329346999039952E-09, 1.1534245859858999E-08, 1.1561520155938000E-08 + 64, 61,-4.0000339574457222E-08, 1.7497056396858609E-08, 1.0380153150478001E-08, 1.0386882768325001E-08 + 64, 62, 3.9607914432958911E-08,-2.2609578386970621E-08, 8.2293858627941996E-09, 8.1684242860042996E-09 + 64, 63,-2.6201067067444270E-08,-8.3667973718990166E-09, 5.3011083001746000E-09, 5.2136201295475996E-09 + 64, 64, 4.0556285026030066E-09,-9.9324223921493445E-09, 4.8974032083497996E-09, 4.7700597193751002E-09 + 65, 0,-5.0373649385218523E-08, 0.0000000000000000E+00, 3.2979711252249997E-08, 0.0000000000000000E+00 + 65, 1, 6.1440539320017990E-09,-7.3607481231004046E-09, 3.2915689104917000E-08, 3.2887523177812002E-08 + 65, 2,-1.4635090586904640E-08,-5.1984539933336634E-09, 3.2773276880178002E-08, 3.2772701922588001E-08 + 65, 3, 6.2562626963645239E-09,-6.8179442273111503E-09, 3.2781366171998998E-08, 3.2806763529898999E-08 + 65, 4, 3.3705657305245552E-08, 2.1351986563289321E-08, 3.2693583668430999E-08, 3.2691936836181997E-08 + 65, 5,-1.9246751793774729E-08,-2.7083970664319688E-08, 3.2640846277649001E-08, 3.2656796641295997E-08 + 65, 6,-4.5288519985819274E-09, 1.9039640424468279E-08, 3.2498506571673998E-08, 3.2614136814249001E-08 + 65, 7,-2.5659830547821631E-08, 1.1022507125668020E-08, 3.2491301226676998E-08, 3.2441564911273997E-08 + 65, 8, 7.9347509260707208E-09, 3.5511712100806723E-08, 3.2357779931031997E-08, 3.2388709082018997E-08 + 65, 9, 1.5401658170595368E-08, 5.3594821677913671E-09, 3.2224977307155999E-08, 3.2279866585059003E-08 + 65, 10,-2.5809139451387951E-09,-8.3292193719512204E-09, 3.2141810598765000E-08, 3.2150837887446000E-08 + 65, 11, 1.4100032511657780E-09,-2.2518063783763170E-08, 3.2005628284506002E-08, 3.2006608753955002E-08 + 65, 12, 4.3118968323305323E-09,-1.1618847909717671E-08, 3.1892843409960002E-08, 3.1907768738308998E-08 + 65, 13, 6.8293793248579268E-09,-1.0878535838740319E-08, 3.1723134332280002E-08, 3.1751737073412998E-08 + 65, 14,-1.1081929376683840E-08,-1.1895911150363309E-08, 3.1611283303060003E-08, 3.1605936837869002E-08 + 65, 15, 9.7889811481527806E-09, 1.1904730172792501E-08, 3.1435402549105002E-08, 3.1433129893354002E-08 + 65, 16,-3.1444998525805007E-08,-2.8509849941932471E-09, 3.1290894003443002E-08, 3.1297677607640002E-08 + 65, 17, 1.6247572368627081E-08, 4.5265727055545253E-09, 3.1137010448426997E-08, 3.1124521124550999E-08 + 65, 18, 4.1676774145368996E-09, 2.7571868182857240E-09, 3.0963654606114001E-08, 3.0938638148955998E-08 + 65, 19,-3.9612755821232552E-08, 2.5208021030743451E-08, 3.0800560935773999E-08, 3.0794345136491999E-08 + 65, 20, 2.4577179208149300E-09,-1.4391668058398869E-08, 3.0591855000919001E-08, 3.0579658758844003E-08 + 65, 21,-9.6027204302834200E-09,-1.7409428699283549E-08, 3.0398393354832003E-08, 3.0385960229433001E-08 + 65, 22,-1.3545664642899019E-09, 8.3126594875122052E-09, 3.0180801346034999E-08, 3.0192046277791997E-08 + 65, 23, 8.2090969518756101E-09, 5.2765129898844763E-09, 2.9986479087893001E-08, 2.9983628914561001E-08 + 65, 24, 3.0660605977806902E-08,-3.6280898587319020E-08, 2.9770550723870000E-08, 2.9764911345755001E-08 + 65, 25,-2.4574278556145440E-08, 1.1973212969424260E-08, 2.9558480919908998E-08, 2.9583263797398999E-08 + 65, 26,-1.3646774177153830E-08, 1.9519863897160289E-08, 2.9338331335536998E-08, 2.9324100088597000E-08 + 65, 27,-3.1764100657395569E-09, 9.6565869752210594E-09, 2.9099156917231999E-08, 2.9081287193371001E-08 + 65, 28,-2.6050493131592851E-09, 2.8153271783866631E-08, 2.8852536408737001E-08, 2.8842513724126001E-08 + 65, 29, 1.9295938033527311E-08,-1.3381067137353790E-08, 2.8602970268677000E-08, 2.8600560982178001E-08 + 65, 30, 3.3409535004024399E-09,-1.8397873749283459E-08, 2.8338379299195000E-08, 2.8356843307657001E-08 + 65, 31, 1.6279693651416600E-08, 2.7030649219729581E-09, 2.8087474574533999E-08, 2.8098559723041999E-08 + 65, 32, 2.8202449046635931E-09,-1.5206448188486401E-08, 2.7804555284990998E-08, 2.7826173376331001E-08 + 65, 33,-1.5181683809517251E-08,-1.1246193249582661E-08, 2.7538398774413000E-08, 2.7541358921519000E-08 + 65, 34, 1.2475890277340461E-09,-1.0434873744912081E-08, 2.7238177485005000E-08, 2.7250753359791000E-08 + 65, 35, 3.9654905508864327E-09,-7.7723999802645494E-09, 2.6952737718756999E-08, 2.6957420890022999E-08 + 65, 36, 5.2017553859623486E-09, 1.7749107417294802E-08, 2.6652017561657001E-08, 2.6687185762796998E-08 + 65, 37,-1.6850716526981921E-08, 1.6665128322498810E-08, 2.6357085872437000E-08, 2.6343617050841999E-08 + 65, 38,-2.7203615343007070E-08, 1.2988587498351540E-08, 2.6052223606177999E-08, 2.6030785618225000E-08 + 65, 39, 2.3386662955670499E-08,-7.1719720833468350E-09, 2.5674472170111000E-08, 2.5686142255226000E-08 + 65, 40,-3.7129383033347820E-09,-3.0563488672475012E-08, 2.5346308995019999E-08, 2.5341196549413000E-08 + 65, 41,-7.8853995174648540E-09,-1.6984244807583050E-08, 2.4968920088791001E-08, 2.4985023018826001E-08 + 65, 42,-1.4020203210659219E-08, 6.4842609462148248E-09, 2.4588982653832001E-08, 2.4597605870739000E-08 + 65, 43, 1.4769575954496240E-08,-1.2399029796778120E-08, 2.4203564679586001E-08, 2.4215974545331999E-08 + 65, 44, 3.1050160585356162E-08, 1.3375359606085030E-08, 2.3769369219086999E-08, 2.3817917108554001E-08 + 65, 45, 1.0781936561002179E-08, 2.9811617410850281E-09, 2.3384503354282000E-08, 2.3375145743232999E-08 + 65, 46, 5.2390036686861051E-10,-1.8310423713543740E-08, 2.2948406939992999E-08, 2.2936407198830001E-08 + 65, 47,-1.2110801421473970E-09, 2.7016685001210180E-08, 2.2450453123629000E-08, 2.2456081230169999E-08 + 65, 48, 3.1916534239582331E-09, 1.7788682233757571E-08, 2.1988649979848001E-08, 2.1971128745011002E-08 + 65, 49, 1.5238141773081521E-08, 1.6273399730965211E-08, 2.1425192131214999E-08, 2.1426921713955001E-08 + 65, 50, 3.6261009575424311E-08, 2.2138198381060049E-08, 2.0945124995160001E-08, 2.0938721502526999E-08 + 65, 51,-3.9446887953643409E-10, 2.7898937929553481E-08, 2.0342451300917999E-08, 2.0334217672456001E-08 + 65, 52,-3.6958133701971453E-08,-1.6167801717580831E-08, 1.9758559372128001E-08, 1.9740903442491001E-08 + 65, 53,-5.0835342477612115E-10, 3.6715874633191400E-09, 1.9077280683014999E-08, 1.9075835670897001E-08 + 65, 54,-2.5527075284464231E-08, 1.0008427383058449E-08, 1.8461478061087999E-08, 1.8456927917505001E-08 + 65, 55,-1.6940959074174489E-08, 1.1678890022092431E-10, 1.7712458839258998E-08, 1.7697153706285002E-08 + 65, 56, 2.8881330745768340E-08, 1.4622436658367160E-08, 1.7038181773529001E-08, 1.7042579149056001E-08 + 65, 57,-2.4254486119916029E-08, 1.6180550426335621E-08, 1.6253776195238000E-08, 1.6254276494611999E-08 + 65, 58,-1.6510498842722010E-08, 2.7629609662246990E-08, 1.5409911518840002E-08, 1.5452109316826999E-08 + 65, 59,-3.3803670117320393E-08, 2.7052110067149541E-08, 1.4607732409283000E-08, 1.4637694536487999E-08 + 65, 60, 4.9117828285767292E-08,-1.8497742505408230E-08, 1.3457034906313999E-08, 1.3455441972366001E-08 + 65, 61,-4.1991808355709383E-09,-1.8661023655929389E-08, 1.2468022749053000E-08, 1.2481068574772999E-08 + 65, 62, 2.5437974304450860E-08, 4.0496461171912171E-08, 1.1221733875019001E-08, 1.1280588790957000E-08 + 65, 63,-8.7848509700082287E-09,-3.7692957462359342E-08, 8.9091627052292004E-09, 8.9541616399045002E-09 + 65, 64, 1.5262114405323779E-08,-4.1958447869487269E-10, 5.4604902088895997E-09, 5.5929574630289000E-09 + 65, 65,-3.5922044167734411E-08, 2.8889954922094178E-09, 5.3791672584085003E-09, 5.4171355160032002E-09 + 66, 0,-2.2511520008357951E-08, 0.0000000000000000E+00, 3.3273731046016998E-08, 0.0000000000000000E+00 + 66, 1,-1.6650898506102991E-09,-3.9069590310602421E-10, 3.3195301571880998E-08, 3.3172480414124997E-08 + 66, 2,-5.3228575683501458E-09,-1.6755231490103439E-08, 3.3039955998171000E-08, 3.3029147505341003E-08 + 66, 3, 2.5622026366651851E-08, 7.1007004943716289E-09, 3.3063751620458001E-08, 3.3081038693060002E-08 + 66, 4,-3.1719259730059319E-08, 2.7114090583255869E-08, 3.2966017876793001E-08, 3.2957109755877002E-08 + 66, 5, 9.4272380599092478E-09,-2.3271167086174029E-08, 3.2930306186909999E-08, 3.2934076617078001E-08 + 66, 6,-1.3213214638993780E-08,-5.4621151430621639E-09, 3.2786305095313998E-08, 3.2880626856153998E-08 + 66, 7,-1.5758364019074409E-08, 3.6587875409956439E-09, 3.2783046103074001E-08, 3.2733189009153002E-08 + 66, 8, 2.5988498687538911E-08, 3.4245514516507097E-08, 3.2643852400326998E-08, 3.2676277415602003E-08 + 66, 9, 1.1201192054131471E-08, 1.0527868825641421E-08, 3.2538721900150997E-08, 3.2579000285398001E-08 + 66, 10,-1.3505846195285391E-08, 4.7669780607262324E-09, 3.2441535892558997E-08, 3.2447833173654999E-08 + 66, 11, 1.3980382427165259E-09,-8.7763576590849478E-10, 3.2331022291643999E-08, 3.2336426210894001E-08 + 66, 12,-5.8152968995804009E-09,-1.1594443720085560E-08, 3.2208772188250003E-08, 3.2224268080954000E-08 + 66, 13, 1.2113962392989080E-08, 1.7907297220382910E-08, 3.2075376778471998E-08, 3.2098338074496001E-08 + 66, 14, 8.0736809527317541E-09, 1.5532053708287700E-08, 3.1954389911102001E-08, 3.1951340010435997E-08 + 66, 15,-4.6397092381545069E-09, 3.3628148822002881E-09, 3.1804243765363003E-08, 3.1794875915975001E-08 + 66, 16,-8.7048008115685909E-09, 5.0624127547366181E-10, 3.1667847509788000E-08, 3.1676042307069003E-08 + 66, 17,-2.3214176814475130E-08, 3.0592643580949898E-09, 3.1529026687547998E-08, 3.1522041396386001E-08 + 66, 18,-1.3406947591252130E-08, 9.7283019657205285E-09, 3.1384482638225998E-08, 3.1355974194568998E-08 + 66, 19,-2.0000254659363359E-09, 8.2691117767756523E-09, 3.1220311623407998E-08, 3.1221532725506003E-08 + 66, 20,-1.7885627099356461E-08, 3.0497500095823490E-09, 3.1044674341998003E-08, 3.1039699072091999E-08 + 66, 21, 2.2070174374221470E-08,-4.5669178798035738E-09, 3.0857789661096999E-08, 3.0859699358352998E-08 + 66, 22, 3.0942259867676909E-09, 5.6005857272690547E-09, 3.0683067234510001E-08, 3.0686357859409997E-08 + 66, 23, 1.8327731091204601E-08,-1.9035057160541920E-08, 3.0495017737359000E-08, 3.0496778997722002E-08 + 66, 24, 2.2157235068500461E-08,-1.2562941517218090E-08, 3.0315291670028003E-08, 3.0310928751131000E-08 + 66, 25,-1.6101902268019710E-08, 7.5698207618363516E-09, 3.0123724053191998E-08, 3.0140236184933001E-08 + 66, 26,-2.2455108028381420E-08, 1.4441135047716390E-08, 2.9924956058463998E-08, 2.9919744422990999E-08 + 66, 27,-1.5081279638113360E-08, 8.1868002362437970E-09, 2.9727221602679999E-08, 2.9714407333618000E-08 + 66, 28, 1.1023013299868539E-08, 1.4538303036551440E-08, 2.9499270352943000E-08, 2.9488003793567000E-08 + 66, 29,-1.0336228434864701E-08,-1.8292661258702660E-08, 2.9286884874299999E-08, 2.9291194873146000E-08 + 66, 30,-3.6935775432822280E-09, 1.1936359590318340E-08, 2.9043209403379001E-08, 2.9058749533816001E-08 + 66, 31,-1.4738287090717410E-08,-1.6552063053243689E-08, 2.8844598437595000E-08, 2.8837940680626999E-08 + 66, 32,-1.0627479323609780E-08,-2.5383918818776141E-08, 2.8576932484616001E-08, 2.8592839168774999E-08 + 66, 33, 4.3893191378796191E-09,-3.6950371923427688E-09, 2.8354547419022000E-08, 2.8355325399671999E-08 + 66, 34, 7.5619769970966567E-09,-1.6463823287467760E-08, 2.8086151315681000E-08, 2.8091186578258001E-08 + 66, 35,-1.9457408572877029E-09, 6.9126440116157973E-09, 2.7821533986248999E-08, 2.7833162503592001E-08 + 66, 36, 5.8793340758330233E-09, 4.4776671464012873E-09, 2.7585321777701998E-08, 2.7612307315484999E-08 + 66, 37,-2.0732252273474818E-08, 5.1821866305416353E-09, 2.7297501079734001E-08, 2.7282875119288001E-08 + 66, 38, 7.0261452235977683E-09, 2.0143832158110662E-08, 2.7045735851203001E-08, 2.7035258341198999E-08 + 66, 39, 1.7102912650890920E-08,-1.4652716040433541E-08, 2.6690322697840001E-08, 2.6699660423459999E-08 + 66, 40,-3.4308004111736622E-08,-8.0207709984275157E-09, 2.6388962316604002E-08, 2.6400530231806000E-08 + 66, 41, 1.0276085800734230E-09,-8.7967945900204299E-09, 2.6068806186732001E-08, 2.6077084532547001E-08 + 66, 42, 1.0046035536738761E-09,-2.9759587688352611E-08, 2.5727156912785001E-08, 2.5749216109249999E-08 + 66, 43,-8.1556571406130910E-09,-9.8381395749664541E-09, 2.5374793733236001E-08, 2.5395019986192999E-08 + 66, 44, 2.0597780580480098E-09,-1.6079537004986279E-09, 2.4987385606820999E-08, 2.5062594631576998E-08 + 66, 45, 4.8122618414413097E-10, 1.1663773580019380E-08, 2.4642215305004000E-08, 2.4628937945690999E-08 + 66, 46,-3.3876165105028000E-09, 7.8230422616169222E-09, 2.4242450974169000E-08, 2.4243944509366999E-08 + 66, 47, 6.8412649450404349E-10,-6.9230753712942944E-09, 2.3824772102063999E-08, 2.3829566493564000E-08 + 66, 48,-6.4101798339056083E-09, 3.6224466255426157E-08, 2.3385194027353999E-08, 2.3357377574975000E-08 + 66, 49,-3.3831669373333787E-08, 2.0746096782214280E-08, 2.2921851847542001E-08, 2.2904243330710001E-08 + 66, 50,-1.8078863349351720E-08, 1.7879099454058009E-08, 2.2404473503114001E-08, 2.2419985744352000E-08 + 66, 51, 1.4903526135906159E-08,-7.3201833188761900E-08, 2.1910325099385999E-08, 2.1904052684951001E-08 + 66, 52, 7.7525366370604461E-08, 7.1373128539888214E-09, 2.1334854247133001E-08, 2.1316021540771999E-08 + 66, 53, 1.3754236407925550E-08, 6.0449606759056230E-09, 2.0747571957369001E-08, 2.0757073347928000E-08 + 66, 54,-4.1283091106582676E-09,-2.0937030210493141E-08, 2.0089961725015000E-08, 2.0098439849574000E-08 + 66, 55, 5.6967820170768514E-09,-2.3838914174551059E-08, 1.9463991577173999E-08, 1.9472010412401000E-08 + 66, 56,-1.6480363419492661E-09, 2.0735196435222211E-08, 1.8733610634736999E-08, 1.8756245623440001E-08 + 66, 57, 6.5208399314248863E-09, 9.5555516927442503E-09, 1.8105212900815999E-08, 1.8097113729740000E-08 + 66, 58,-5.4735245973153859E-09,-1.2596737529274191E-09, 1.7233612401602999E-08, 1.7289639541572000E-08 + 66, 59, 1.0776500690148420E-08,-2.7508573062808149E-08, 1.6471538944238999E-08, 1.6472322166167000E-08 + 66, 60, 8.1623380947365579E-09,-2.2709291942720719E-08, 1.5630244228200000E-08, 1.5586456889189001E-08 + 66, 61,-6.0863558223073611E-08,-2.3143456865729212E-08, 1.4376453968533001E-08, 1.4361085371578001E-08 + 66, 62, 2.3049304066902740E-08, 1.1278883433419030E-08, 1.3374383900751999E-08, 1.3430295927202999E-08 + 66, 63, 5.2946531095619091E-08,-4.7795605926934243E-08, 1.2098305972187999E-08, 1.2114939185846000E-08 + 66, 64,-2.8958395621563808E-08, 2.1151728593225900E-08, 9.4249162964531006E-09, 9.4790772096174005E-09 + 66, 65, 4.6351473973913762E-08,-2.5555721526976060E-08, 6.0854439494489997E-09, 5.9452043754958996E-09 + 66, 66,-1.3547574785928200E-08, 2.9564983621979921E-08, 6.0383888040812999E-09, 6.0210619948290000E-09 + 67, 0,-8.2042364988053092E-08, 0.0000000000000000E+00, 3.3433566347576000E-08, 0.0000000000000000E+00 + 67, 1, 3.4809886172156630E-09,-2.0242575809330559E-08, 3.3275570709149002E-08, 3.3258298034792998E-08 + 67, 2,-6.1922785732092011E-09,-1.6341600634354181E-08, 3.3126847209147003E-08, 3.3110806614570997E-08 + 67, 3,-1.2175396570356269E-08, 2.4396092798539398E-10, 3.3158685814215001E-08, 3.3172942657999000E-08 + 67, 4, 4.7382759195032523E-09, 3.4426038832712421E-09, 3.3066769181493998E-08, 3.3045353073404000E-08 + 67, 5,-7.0945334894264350E-09,-8.7845956515856489E-09, 3.3022241032643998E-08, 3.3033829248762001E-08 + 67, 6,-1.3880352526323480E-08, 7.3178114560709044E-09, 3.2903678456613000E-08, 3.2972323866611002E-08 + 67, 7,-2.2874512834602740E-08, 1.1392386866221661E-08, 3.2880650367835998E-08, 3.2844853729586003E-08 + 67, 8,-7.0627659232543019E-10, 8.6425591623508795E-09, 3.2762067441995997E-08, 3.2786616875848000E-08 + 67, 9, 1.3497846251707780E-08, 7.6351439056457655E-09, 3.2661966152728002E-08, 3.2695405384711998E-08 + 67, 10, 2.4165076634525370E-08,-1.2413367149951170E-08, 3.2576599782072001E-08, 3.2578707630318998E-08 + 67, 11, 1.0828139472109140E-08,-1.5270751308120601E-08, 3.2461225014802997E-08, 3.2465613022022003E-08 + 67, 12,-4.5729876137577209E-09,-1.4605400507895610E-08, 3.2368714133836002E-08, 3.2380083114071998E-08 + 67, 13, 1.0842741781351791E-08,-9.9649907268058963E-10, 3.2226535242504000E-08, 3.2248388544813997E-08 + 67, 14, 6.7663962114929613E-09, 2.1374906969419409E-08, 3.2144161785173001E-08, 3.2133262385881997E-08 + 67, 15,-2.8234164166358279E-08,-3.8585341394953269E-09, 3.1988730890858997E-08, 3.1976560174646998E-08 + 67, 16,-1.9653679309134410E-08,-1.3859175863098960E-08, 3.1877014085802002E-08, 3.1886413874855003E-08 + 67, 17,-3.7334415072929383E-09, 1.0045754838628440E-08, 3.1747285706535997E-08, 3.1741623063099001E-08 + 67, 18,-1.6494325876283350E-09, 4.8839874008194711E-11, 3.1625970919780000E-08, 3.1597697302998001E-08 + 67, 19, 3.6884764560975749E-09, 2.6563367537715231E-08, 3.1473561733847002E-08, 3.1488960749907000E-08 + 67, 20, 2.5908570138293060E-08, 2.3387833017761730E-09, 3.1316740656646998E-08, 3.1318313115498997E-08 + 67, 21, 9.3071446621351160E-09,-5.4530866671707521E-09, 3.1162500292637997E-08, 3.1155678467185000E-08 + 67, 22,-7.4227114676430093E-09,-2.0066504872185870E-08, 3.1002422888271002E-08, 3.1003130934778002E-08 + 67, 23,-6.4879610717041083E-09, 1.6030125090129539E-08, 3.0833100837497998E-08, 3.0840019251131001E-08 + 67, 24, 1.9970817171213452E-08, 6.6964669050006451E-09, 3.0681037844729002E-08, 3.0678025854933002E-08 + 67, 25,-3.0641949005002991E-08, 2.9629315405706691E-08, 3.0522229943025997E-08, 3.0531549859736003E-08 + 67, 26, 5.6833031108645419E-09,-1.4663264320471030E-08, 3.0334035722600002E-08, 3.0326883706454002E-08 + 67, 27,-1.6262268385028759E-08,-2.8639813051531721E-08, 3.0155844787522999E-08, 3.0146270859329997E-08 + 67, 28, 5.1976766917456631E-09, 2.1174842434001800E-08, 2.9969240410601999E-08, 2.9950162807438001E-08 + 67, 29, 1.3784722957208250E-08, 6.0465133895953267E-09, 2.9767977724981000E-08, 2.9767054694013999E-08 + 67, 30,-3.8579051269073221E-08, 9.7175075812296886E-09, 2.9563757826316002E-08, 2.9570209729226000E-08 + 67, 31, 1.2178729256049900E-08,-2.2915884987181369E-08, 2.9374396665900999E-08, 2.9371743145572999E-08 + 67, 32,-1.7934635512840681E-09,-1.1236163069335729E-08, 2.9153796483741001E-08, 2.9158746319230001E-08 + 67, 33, 1.7571290432680191E-08,-1.4518563433997900E-08, 2.8953147188860000E-08, 2.8950743132173999E-08 + 67, 34, 1.7000159203982740E-08,-1.1989028496605740E-09, 2.8717691511685000E-08, 2.8721351586433999E-08 + 67, 35, 5.5176979192551283E-09,-2.5187862081642861E-08, 2.8473821475605999E-08, 2.8492353745944000E-08 + 67, 36, 1.2268635209375070E-08, 1.6007559263140670E-08, 2.8273402790176999E-08, 2.8301238594593001E-08 + 67, 37,-3.4681867812801901E-09,-7.7906855478179653E-09, 2.8028889160470001E-08, 2.8013796248238999E-08 + 67, 38, 5.2923507173436967E-11, 7.2676030436194689E-09, 2.7809628904658001E-08, 2.7809936655988000E-08 + 67, 39, 1.0363255171171960E-08,-1.4761141556242850E-08, 2.7500561870533001E-08, 2.7509888573489001E-08 + 67, 40, 8.5923714648108362E-09,-9.0468847845579834E-09, 2.7236422504640001E-08, 2.7242715685528999E-08 + 67, 41, 2.4479981442525219E-08,-4.6961593373167618E-09, 2.6952679513628001E-08, 2.6951347115754000E-08 + 67, 42,-2.6643483756935178E-08, 1.9529128967951639E-08, 2.6668705035892999E-08, 2.6678687975895001E-08 + 67, 43, 1.4762164864903990E-08, 5.2292889461143484E-09, 2.6347258140097999E-08, 2.6373274879949000E-08 + 67, 44,-8.0801490422467358E-09,-6.1244874856815208E-09, 2.6000786035738001E-08, 2.6083846852914001E-08 + 67, 45, 9.2782423554687217E-09, 1.6439363181945240E-08, 2.5724435435401000E-08, 2.5722137080558998E-08 + 67, 46, 2.3494960127012481E-08, 1.3895714685920609E-08, 2.5349787562312001E-08, 2.5348754629088000E-08 + 67, 47, 2.6003655400198571E-08,-1.5358438415360071E-08, 2.5002673520702000E-08, 2.5009906609216999E-08 + 67, 48,-1.3483794694802049E-08, 2.5085260180078231E-08, 2.4636388430020999E-08, 2.4597432429021001E-08 + 67, 49, 1.2026609978649870E-08, 7.2249842144973379E-09, 2.4175507490815001E-08, 2.4161760360456999E-08 + 67, 50,-4.1676817350043917E-09, 2.1964521950254719E-09, 2.3789119626680000E-08, 2.3794605736520002E-08 + 67, 51, 1.9836118602524762E-08,-2.0417089758260611E-08, 2.3259268205548998E-08, 2.3237963489673999E-08 + 67, 52,-2.8742622036199501E-08, 3.4696313883168781E-08, 2.2784329877867001E-08, 2.2774444638862999E-08 + 67, 53, 6.6794235737935409E-09,-2.2937580799672769E-08, 2.2224025741716000E-08, 2.2220299751910002E-08 + 67, 54,-2.7453628691544889E-09,-2.1527961420639020E-08, 2.1669496607992001E-08, 2.1682981801452000E-08 + 67, 55, 2.2778803397719551E-08,-1.0944493880214780E-08, 2.1021363788823000E-08, 2.1006071049218000E-08 + 67, 56, 5.4849274963716293E-09, 1.1598478837465610E-08, 2.0427642188317001E-08, 2.0447238491892001E-08 + 67, 57,-9.5188573150943268E-10,-7.2396081226709243E-09, 1.9720069943945000E-08, 1.9719044218080001E-08 + 67, 58,-1.6157726991064779E-08,-1.4291178878468390E-09, 1.9038235506568001E-08, 1.9074856301279998E-08 + 67, 59,-1.2457972772208059E-08, 4.5025747276896881E-08, 1.8223026978353000E-08, 1.8233993652396000E-08 + 67, 60,-2.9093693326444970E-08, 2.8910816244521800E-08, 1.7449254608513999E-08, 1.7416848200942001E-08 + 67, 61,-2.2721673729746590E-08, 2.7612280745647371E-08, 1.6587886315171000E-08, 1.6549516991304000E-08 + 67, 62,-2.4620275319967232E-08, 4.6760687913147191E-08, 1.5271094141913000E-08, 1.5282498577876999E-08 + 67, 63, 3.2026611502940362E-08,-2.8203260181717331E-08, 1.4358619499893000E-08, 1.4341672091596000E-08 + 67, 64, 1.3089454999768500E-08, 2.9928416840258883E-08, 1.2893890423049999E-08, 1.2886361197024001E-08 + 67, 65, 3.9299839801992962E-08, 6.8953295337926541E-08, 1.0154238484613001E-08, 1.0185446086705000E-08 + 67, 66,-3.0235816266102032E-08,-4.5519797221910937E-08, 6.5686372578950999E-09, 6.5817780778550997E-09 + 67, 67, 4.7305692250117892E-08, 1.7927704183023691E-08, 6.5323168795116001E-09, 6.5632751757363002E-09 + 68, 0,-1.4060437580433089E-08, 0.0000000000000000E+00, 3.3410242671013000E-08, 0.0000000000000000E+00 + 68, 1, 1.1856638668738259E-08,-7.1624936313842979E-09, 3.3235480211711002E-08, 3.3224586433359997E-08 + 68, 2,-2.1351118173655570E-08,-1.0771461661547650E-08, 3.3074142666075999E-08, 3.3050683374992998E-08 + 68, 3, 2.5759527114722210E-08,-1.3913162885903369E-08, 3.3119115594318001E-08, 3.3131646969549998E-08 + 68, 4, 9.8343907231008246E-09, 5.6391931488591852E-09, 3.3018854388203001E-08, 3.2988243729244002E-08 + 68, 5, 5.8287856800555752E-11,-7.1190413465649487E-09, 3.2992098482644998E-08, 3.2996330327199999E-08 + 68, 6,-5.1145729003170758E-09, 2.2481835537070411E-08, 3.2872475708754998E-08, 3.2920174599839003E-08 + 68, 7,-9.4662662329646602E-09, 7.0864787179928262E-09, 3.2854513861606003E-08, 3.2823085390979999E-08 + 68, 8, 9.2295708717322816E-09,-8.2235743235046974E-09, 3.2738314720635997E-08, 3.2757578594947001E-08 + 68, 9,-9.8879533757480279E-09,-1.1890069702573460E-08, 3.2651625549525002E-08, 3.2671727395693002E-08 + 68, 10,-1.7329742177763550E-08, 3.3865143344422112E-10, 3.2567138196419000E-08, 3.2566049369245002E-08 + 68, 11,-4.4200911192072299E-09,-1.8279083749978782E-08, 3.2462181530507003E-08, 3.2467779169328003E-08 + 68, 12,-1.1743697238529180E-08,-1.1961577071004141E-08, 3.2375822479880998E-08, 3.2379938118900003E-08 + 68, 13, 6.4824018927447910E-09, 1.9110502531640581E-09, 3.2250097989317001E-08, 3.2272771784505998E-08 + 68, 14, 9.8102118616334437E-09, 4.5571563281183448E-09, 3.2163155679449002E-08, 3.2150564977687999E-08 + 68, 15, 2.3689701830813219E-08, 2.5054875408146831E-08, 3.2028902736620003E-08, 3.2025224113039997E-08 + 68, 16,-2.2459983786357640E-08,-9.6463501292197249E-09, 3.1924583905702002E-08, 3.1925939279823000E-08 + 68, 17,-3.2647560735597141E-09, 8.1585679044529714E-09, 3.1820875844587001E-08, 3.1813623892457999E-08 + 68, 18,-6.2907884183331483E-09, 8.7543591965110668E-09, 3.1700083174898999E-08, 3.1670164935778003E-08 + 68, 19, 1.6952073194400529E-09, 1.4988210977705639E-08, 3.1572026629759998E-08, 3.1593585599270002E-08 + 68, 20,-3.6329819818853048E-09,-1.8728960472695258E-08, 3.1419138243939999E-08, 3.1430655663873002E-08 + 68, 21, 4.8086139156727379E-09,-2.6973926521039381E-08, 3.1295204650505999E-08, 3.1284833630023002E-08 + 68, 22,-5.8364478763023630E-09,-6.0987388424415090E-09, 3.1148726841796002E-08, 3.1149023275689002E-08 + 68, 23,-8.5665897475464837E-09,-2.0659245162033999E-09, 3.0997807114560000E-08, 3.1007351879002999E-08 + 68, 24, 2.6619401571512350E-08,-1.1548026320364069E-09, 3.0868237218892003E-08, 3.0863417239398001E-08 + 68, 25, 5.5202498561049240E-09,-3.7842728813904929E-09, 3.0722661112076003E-08, 3.0738978072252997E-08 + 68, 26,-1.5133208447093580E-08,-8.8898209047251597E-09, 3.0566704870736003E-08, 3.0564564274887003E-08 + 68, 27,-5.9330008366678093E-09,-2.5294616180126899E-08, 3.0413850815306999E-08, 3.0408378498997000E-08 + 68, 28,-1.9752080985181830E-08,-1.3391758342743960E-08, 3.0251528819538000E-08, 3.0239043059648001E-08 + 68, 29,-1.4484064117091589E-08,-1.4737038773033699E-08, 3.0085878951002003E-08, 3.0083242523737997E-08 + 68, 30, 3.2161862546334901E-09, 9.3591906107526821E-09, 2.9900929388471999E-08, 2.9901212358730999E-08 + 68, 31, 1.2009248849353381E-08, 7.3127937489536764E-09, 2.9747571898527000E-08, 2.9742594093068999E-08 + 68, 32,-2.9998357558461322E-08,-2.7356819771891920E-08, 2.9543099766883001E-08, 2.9550077923991000E-08 + 68, 33,-8.8397763095839246E-09,-1.0311359553040620E-08, 2.9391863639476998E-08, 2.9380302034330999E-08 + 68, 34, 6.7226859030169502E-09, 3.9990800946378602E-09, 2.9180021003598001E-08, 2.9174734942864001E-08 + 68, 35,-4.1152756508177641E-09,-1.1110832811384190E-08, 2.8970462622015001E-08, 2.9000164628400999E-08 + 68, 36,-6.7112938842462440E-10, 4.7499419579773067E-09, 2.8806774620441002E-08, 2.8818546648202001E-08 + 68, 37, 2.6949727238917689E-08, 2.4209948350929770E-08, 2.8584261472165001E-08, 2.8571587254184999E-08 + 68, 38, 1.7970956197969700E-09,-7.0075040218608679E-10, 2.8402938759647000E-08, 2.8417676376261001E-08 + 68, 39,-7.4941724921979443E-09,-1.6239072403863311E-08, 2.8121887580127002E-08, 2.8125344387016999E-08 + 68, 40,-2.5290110553892220E-08,-1.9386605063586750E-08, 2.7890021927229001E-08, 2.7894654840073001E-08 + 68, 41,-1.1104586684538780E-08,-3.1088151274537260E-08, 2.7642187135207999E-08, 2.7635268688811999E-08 + 68, 42,-1.7327354894841760E-08, 3.2750772325457660E-08, 2.7389672741132002E-08, 2.7401404841657999E-08 + 68, 43, 1.2779547416731461E-08,-8.4421212017224894E-10, 2.7123648310522999E-08, 2.7140120190977999E-08 + 68, 44,-2.0858458630566221E-08, 7.5317085386708845E-09, 2.6830547597246999E-08, 2.6884256409151000E-08 + 68, 45, 1.2501149175553739E-08,-6.7543118094289182E-09, 2.6567091413083001E-08, 2.6564624317922001E-08 + 68, 46,-3.0730634707344910E-09,-5.4898695548705640E-09, 2.6258788022218001E-08, 2.6261506086092000E-08 + 68, 47, 1.3435546532733470E-08,-2.9540369917196819E-08, 2.5925270897042000E-08, 2.5938153420077000E-08 + 68, 48,-2.4665389481321891E-09, 2.1143807147039820E-08, 2.5632808344766001E-08, 2.5586248402447999E-08 + 68, 49, 5.7214605732993510E-09,-1.1526735307805800E-08, 2.5256401439338000E-08, 2.5241382490464998E-08 + 68, 50,-2.3450888797539340E-08, 1.1733218289063900E-08, 2.4880171124643998E-08, 2.4883812660461000E-08 + 68, 51, 1.1918236047817149E-08,-1.6039784331926739E-08, 2.4485538214993999E-08, 2.4465010583511999E-08 + 68, 52, 2.2139904517736691E-08,-1.7114731407393179E-09, 2.3984642762676001E-08, 2.3974150215736001E-08 + 68, 53,-4.0088202002270763E-09,-1.7210698789309419E-08, 2.3544806516809001E-08, 2.3526998674230999E-08 + 68, 54,-5.2492071034749392E-08,-8.2692556042887146E-09, 2.3020180736477001E-08, 2.3027775785981999E-08 + 68, 55, 2.8048688257383870E-08,-1.3481960618371690E-08, 2.2464307960936999E-08, 2.2471488240150999E-08 + 68, 56, 1.6710417109442980E-09, 2.1293365698639000E-08, 2.1880648092398000E-08, 2.1886446013949000E-08 + 68, 57,-1.5575914489410860E-09, 1.3133246934454410E-08, 2.1345661562048000E-08, 2.1326053350179000E-08 + 68, 58,-4.1063302404071117E-08,-2.2002012233996929E-08, 2.0592991156088001E-08, 2.0591722491324000E-08 + 68, 59,-5.2371022647453807E-08, 7.5891500024720301E-09, 2.0008073526401999E-08, 2.0015528447306999E-08 + 68, 60,-1.8369954220722671E-08, 1.1346223649464280E-08, 1.9154567818499001E-08, 1.9116624857515001E-08 + 68, 61,-8.7610152835638234E-09,-2.2388817620117669E-08, 1.8411916583256001E-08, 1.8357081720373999E-08 + 68, 62, 1.0852957387856650E-08, 3.5199914455287002E-08, 1.7514793075201999E-08, 1.7490901485237001E-08 + 68, 63, 1.7375091341850531E-08,-1.0517123461232960E-08, 1.6187598805324999E-08, 1.6145436265080000E-08 + 68, 64,-2.2117086381405499E-08, 4.4181004491535403E-09, 1.5213841234579001E-08, 1.5284400406959999E-08 + 68, 65,-1.2591807752617281E-08, 3.4444590324765779E-09, 1.3756171616210000E-08, 1.3781488962193000E-08 + 68, 66, 5.6267561134636053E-08,-3.2922367632368193E-08, 1.0813613297599999E-08, 1.0821356569034000E-08 + 68, 67,-5.6281853797891613E-08,-7.4157272687444503E-09, 6.8623851389823000E-09, 6.7772877010763001E-09 + 68, 68, 1.5340438050081701E-08,-2.7600451997187540E-08, 7.2645260838079002E-09, 7.2576128594877999E-09 + 69, 0,-1.1388687392688910E-07, 0.0000000000000000E+00, 3.3289257840933999E-08, 0.0000000000000000E+00 + 69, 1,-1.9474905102782161E-08,-2.2996972065424459E-11, 3.3029549528823002E-08, 3.3027320396641003E-08 + 69, 2, 1.1543579001510020E-08,-5.3598276115665196E-09, 3.2863721694128998E-08, 3.2842605772387998E-08 + 69, 3, 1.9154249287331271E-08, 2.6954410925302940E-08, 3.2934051647911001E-08, 3.2950220812841997E-08 + 69, 4, 3.7522184613455634E-09, 3.4974030177431971E-09, 3.2816336798347997E-08, 3.2777654297025000E-08 + 69, 5, 1.3175646197640680E-08,-1.2139007125138160E-08, 3.2809589432068997E-08, 3.2812436117976003E-08 + 69, 6,-6.8437704138621421E-09,-2.0870379336569551E-09, 3.2688697967237003E-08, 3.2722452037136001E-08 + 69, 7, 1.6312239010579641E-09, 1.0253608127094630E-08, 3.2671385952128999E-08, 3.2645216354873997E-08 + 69, 8, 4.7962853207595322E-09, 8.3196410844159710E-09, 3.2564492525407997E-08, 3.2579793797207003E-08 + 69, 9, 2.5781332171137370E-08,-1.5168288599107709E-09, 3.2482557273989001E-08, 3.2494042209709998E-08 + 69, 10, 2.4474994211827548E-08,-5.0917861729704490E-09, 3.2410977410490001E-08, 3.2410467323776998E-08 + 69, 11,-7.6396714493846004E-09,-2.0840798182327289E-08, 3.2300103908163998E-08, 3.2305677248234001E-08 + 69, 12,-5.8823174872236864E-09,-1.9356360365775790E-08, 3.2238968849753000E-08, 3.2239663260872997E-08 + 69, 13, 2.9344148413498541E-08, 2.6341152970443169E-08, 3.2114803319260998E-08, 3.2125485834028997E-08 + 69, 14, 2.3644664575164061E-09, 5.8863241601789476E-09, 3.2049886084829002E-08, 3.2043723600350999E-08 + 69, 15,-1.9835584985785540E-08,-6.1684767456697723E-09, 3.1917953746683998E-08, 3.1909803343920999E-08 + 69, 16,-5.0449778428580269E-09,-1.0542077678569430E-08, 3.1838435340273000E-08, 3.1836338758660997E-08 + 69, 17,-5.4585381826407514E-10, 2.4055939926443610E-08, 3.1741054198262001E-08, 3.1734421845638999E-08 + 69, 18,-1.1224222778328339E-08, 2.4443187452286050E-08, 3.1640296396747002E-08, 3.1612024210607000E-08 + 69, 19, 2.3207958864514452E-08, 1.7013733876648240E-08, 3.1526843524581998E-08, 3.1552222503101997E-08 + 69, 20,-5.7387304636730300E-10, 1.5042947115555200E-08, 3.1389044021790000E-08, 3.1409214625462001E-08 + 69, 21, 7.1112940863084728E-09,-3.0063898824257552E-08, 3.1279326814258003E-08, 3.1271788914502002E-08 + 69, 22,-1.7411591897696369E-08,-4.8907412244071658E-09, 3.1163884244329999E-08, 3.1156494996500000E-08 + 69, 23,-5.6386842701846970E-09, 6.0625423579077358E-09, 3.1015452620337002E-08, 3.1031224254809997E-08 + 69, 24, 1.2508621050872820E-08, 1.5095176886667571E-08, 3.0915254979078003E-08, 3.0913261761661000E-08 + 69, 25,-5.0985199160502377E-09, 4.7271858830435061E-08, 3.0793775197651997E-08, 3.0800994261343000E-08 + 69, 26,-7.6563323510355326E-09,-3.6692329486713410E-09, 3.0653840388350001E-08, 3.0653632538238999E-08 + 69, 27,-4.3868902441823366E-09, 1.0491562360961860E-08, 3.0508179516176000E-08, 3.0502650357051002E-08 + 69, 28, 1.6624505433732999E-09, 2.0631655801347919E-08, 3.0376595153609003E-08, 3.0369820732310999E-08 + 69, 29,-9.4254817748248356E-09,-1.0542086250563129E-08, 3.0231689435798000E-08, 3.0229190271045997E-08 + 69, 30, 1.3929732891855919E-08, 6.9287680209072622E-09, 3.0074366490395998E-08, 3.0074869196942000E-08 + 69, 31,-3.3434595237403001E-09,-2.6181409737363849E-08, 2.9938236743405000E-08, 2.9929967876783997E-08 + 69, 32, 2.0135387528712528E-08,-1.7704377101805259E-08, 2.9766674629752998E-08, 2.9764613017617999E-08 + 69, 33,-3.2014831035342220E-09,-8.0580154096427952E-09, 2.9628662878871002E-08, 2.9617212229282999E-08 + 69, 34, 7.8902409830861555E-09, 2.4260225944466882E-08, 2.9453992962703000E-08, 2.9441427443960000E-08 + 69, 35,-5.1426332402852748E-09,-5.1699046438865558E-09, 2.9265206274929002E-08, 2.9296608316613000E-08 + 69, 36, 2.7131818784367050E-08,-3.9639983713556960E-08, 2.9132514942538001E-08, 2.9135342561733999E-08 + 69, 37,-1.1692465759273261E-08, 6.8597360384490937E-10, 2.8938156990080001E-08, 2.8919604438280000E-08 + 69, 38,-1.3510230292102179E-08,-6.0247296255564357E-09, 2.8792367045272000E-08, 2.8803025705586000E-08 + 69, 39,-2.2146111663420819E-08, 2.8260255250294889E-08, 2.8550264247447000E-08, 2.8552621899981998E-08 + 69, 40,-4.5998540975413773E-09, 8.9854926177874857E-09, 2.8356909829578002E-08, 2.8366626657880000E-08 + 69, 41,-1.3890971571109691E-08,-1.6366384409786701E-08, 2.8140529649633999E-08, 2.8123020467104001E-08 + 69, 42,-1.7659384429110600E-08,-7.3850916928753303E-09, 2.7926485587398999E-08, 2.7942923172572001E-08 + 69, 43, 4.0434339906986183E-09, 1.3804056684096390E-08, 2.7694307830685001E-08, 2.7713009592981000E-08 + 69, 44,-9.2140592911599718E-09,-1.4418396625911290E-08, 2.7474370529723001E-08, 2.7491893894797001E-08 + 69, 45, 1.9837282237159310E-08, 1.4287508116837360E-08, 2.7237058632017001E-08, 2.7239117504748999E-08 + 69, 46,-9.2405068779750918E-09, 1.2936600213232450E-08, 2.6966614824054001E-08, 2.6962067654474001E-08 + 69, 47,-4.2127091487640952E-10, 8.2993503630751269E-09, 2.6711456242605999E-08, 2.6729807871302999E-08 + 69, 48,-2.4714969808106420E-08,-2.7323205582386541E-08, 2.6426391339557000E-08, 2.6385261282640999E-08 + 69, 49,-1.9233156083936049E-08,-2.2254173703064320E-09, 2.6118851935810999E-08, 2.6118801117064000E-08 + 69, 50,-1.7778738120161168E-08,-2.2482181188083081E-08, 2.5823191189415000E-08, 2.5829596388686999E-08 + 69, 51, 5.1518240697247402E-08,-1.5019422746881260E-08, 2.5416115096797001E-08, 2.5398848512483000E-08 + 69, 52, 2.5901349021544452E-09, 3.0458253153510162E-08, 2.5057481548528999E-08, 2.5044442402097999E-08 + 69, 53, 1.3960566671437180E-08,-9.5326483414303820E-09, 2.4582559863742999E-08, 2.4590100751397001E-08 + 69, 54,-1.1062156850735790E-08,-2.7417551763642001E-08, 2.4188116366427001E-08, 2.4200803878420001E-08 + 69, 55, 3.6608059713997861E-08,-1.3332755802820329E-08, 2.3662083786909001E-08, 2.3664869541931000E-08 + 69, 56,-9.1780995339035209E-09, 1.2330375522769300E-08, 2.3199393686144999E-08, 2.3212487450628998E-08 + 69, 57, 7.1582524351817886E-09,-7.6319582872955547E-09, 2.2640453994083000E-08, 2.2609456491054001E-08 + 69, 58,-1.4203885832727210E-08,-6.1511602741994788E-09, 2.2063180730279999E-08, 2.2054082177058000E-08 + 69, 59,-3.8863419022903910E-09,-1.4651119818567420E-09, 2.1360870531170000E-08, 2.1379113314734999E-08 + 69, 60,-1.5202642869749030E-08, 1.1856108117300580E-08, 2.0805752625187999E-08, 2.0765401091493001E-08 + 69, 61, 1.6550072259894189E-08,-1.5650088870784611E-08, 1.9929351043108999E-08, 1.9907382661004001E-08 + 69, 62, 1.8595671792097341E-09,-1.2019765629813010E-08, 1.9213263641044999E-08, 1.9251542189161001E-08 + 69, 63, 8.2229963314416278E-10,-7.7074677260288156E-09, 1.8390828912351999E-08, 1.8442853549776999E-08 + 69, 64,-2.8295721454597661E-08,-4.8136725907073757E-08, 1.6946739108215999E-08, 1.6977625211579001E-08 + 69, 65, 1.2123244010607710E-08,-8.6058649231401289E-09, 1.6073972063511000E-08, 1.6140827963063001E-08 + 69, 66,-2.6142341365549049E-08, 1.7926726805536629E-08, 1.4501229221739999E-08, 1.4540356333820000E-08 + 69, 67,-5.9965898921057310E-08, 2.4387429854293038E-08, 1.1470004728841000E-08, 1.1519384493185999E-08 + 69, 68,-2.1671136714669381E-08, 1.8850663863821788E-08, 7.3071140319775001E-09, 7.3460650890707001E-09 + 69, 69,-1.0383571238487469E-08,-8.4236539141892482E-09, 7.8673899825607005E-09, 7.8766571263402000E-09 + 70, 0,-2.8603563716425270E-08, 0.0000000000000000E+00, 3.3036358618039999E-08, 0.0000000000000000E+00 + 70, 1, 1.1923057265959440E-08,-2.3132840022126140E-08, 3.2754762122827998E-08, 3.2761682975584997E-08 + 70, 2,-1.6928177120201739E-08,-4.1713719279455724E-09, 3.2592841077081002E-08, 3.2576811072719999E-08 + 70, 3,-6.9010609245023119E-09,-4.7557734681538362E-08, 3.2655945152190999E-08, 3.2677263690263001E-08 + 70, 4,-6.8272368808751896E-09,-5.4607384715999674E-09, 3.2549457687285001E-08, 3.2507068350722002E-08 + 70, 5,-1.9336399949220781E-10,-2.7250150441747082E-09, 3.2539017183759000E-08, 3.2543807043927001E-08 + 70, 6, 4.4975395227936719E-09, 1.1661068420153800E-08, 3.2436185111276997E-08, 3.2456368615608003E-08 + 70, 7,-1.2019353197566890E-08, 1.1150701883847111E-08, 3.2408293910402002E-08, 3.2387304287722000E-08 + 70, 8, 6.8766476637806308E-09,-1.9034075968686030E-09, 3.2321801238236999E-08, 3.2337227822023998E-08 + 70, 9, 1.7277966048657530E-09,-1.8920219271599011E-08, 3.2236337985173999E-08, 3.2237971573286002E-08 + 70, 10,-2.5900083435111949E-10,-7.4913936693846679E-09, 3.2181979874522998E-08, 3.2179253567185998E-08 + 70, 11,-8.5245117521978992E-09,-2.5558035499327660E-08, 3.2064810772616003E-08, 3.2072395239354002E-08 + 70, 12,-1.7536754616242268E-08, 4.4962945424551696E-09, 3.2019128395321002E-08, 3.2019245577000001E-08 + 70, 13, 9.0115833813112283E-09, 1.5913338803171490E-08, 3.1902284087127001E-08, 3.1910857713347998E-08 + 70, 14, 2.0473272160462221E-08,-1.7288587686398790E-09, 3.1831882998986997E-08, 3.1826970692439999E-08 + 70, 15, 1.5880157366049359E-08, 1.6837768769751021E-08, 3.1719870439469003E-08, 3.1712647221808002E-08 + 70, 16,-8.0595499204450670E-09,-1.9446524666630890E-08, 3.1644721308376999E-08, 3.1638698533718000E-08 + 70, 17,-3.9367599857539848E-09, 1.6547853726037169E-09, 3.1567571866572000E-08, 3.1561534488425997E-08 + 70, 18,-7.7662621646509915E-09, 6.1420449570649934E-09, 3.1464625139540003E-08, 3.1441427909358999E-08 + 70, 19,-4.4910420668357208E-09, 2.1216015538631501E-08, 3.1383293778730998E-08, 3.1404902394845001E-08 + 70, 20, 2.6748651956661329E-08, 5.6792159307969828E-11, 3.1238636136058997E-08, 3.1262545351616000E-08 + 70, 21, 1.6742744053191691E-08,-1.0229597981398389E-08, 3.1143320640075997E-08, 3.1143095786917997E-08 + 70, 22, 9.5268440318647903E-09,-1.8512780913010250E-08, 3.1038433789517999E-08, 3.1036045964768997E-08 + 70, 23,-1.8266544198705539E-08, 3.1814321520416013E-08, 3.0908756105865001E-08, 3.0929918115760003E-08 + 70, 24,-8.2728325871809838E-09,-8.6183410004348951E-10, 3.0819107899202002E-08, 3.0818794326930002E-08 + 70, 25, 1.0130032858544400E-08,-5.8551813813940454E-09, 3.0707502423646003E-08, 3.0720631916265998E-08 + 70, 26, 3.3951932591009959E-09,-1.0578194974233620E-08, 3.0594663827963998E-08, 3.0602442145603998E-08 + 70, 27, 6.2214513703575664E-10,-2.8111399308176150E-08, 3.0481372827243001E-08, 3.0476292322883998E-08 + 70, 28,-1.8140539283510899E-08, 6.7992677859043479E-10, 3.0351169774461000E-08, 3.0350608576226997E-08 + 70, 29,-7.9075242241582574E-09, 1.1163992765071691E-08, 3.0245796025812998E-08, 3.0243769590772002E-08 + 70, 30,-1.3703801400170180E-08,-4.7161360203330443E-09, 3.0101290563962999E-08, 3.0103684257123999E-08 + 70, 31, 1.4003221372656730E-08, 1.5608092689935791E-08, 2.9991514958059001E-08, 2.9984440769634002E-08 + 70, 32,-1.6183549948463541E-08,-8.5080454180460340E-09, 2.9847433396170000E-08, 2.9834612963326002E-08 + 70, 33,-4.4786813657509232E-11,-2.9539150290257782E-09, 2.9736470484629001E-08, 2.9718999485217000E-08 + 70, 34, 1.4649739859033490E-08,-6.8507166460511294E-09, 2.9584126151458999E-08, 2.9568697540975000E-08 + 70, 35,-4.8081095299319731E-09,-1.4196644634685930E-08, 2.9436884360330000E-08, 2.9456173833964001E-08 + 70, 36, 6.9734105182903511E-10, 2.3805974252019039E-09, 2.9333068015122000E-08, 2.9323209995958000E-08 + 70, 37, 1.4615749786218570E-08, 1.7996265642523489E-08, 2.9149643124900998E-08, 2.9130915657676000E-08 + 70, 38,-3.1045989942545083E-08,-3.1320380048511530E-09, 2.9039014049285999E-08, 2.9051995280454001E-08 + 70, 39, 1.3148790281025351E-09, 1.6524798249333390E-09, 2.8818213028894000E-08, 2.8815070762879998E-08 + 70, 40,-1.3765722280346450E-08,-1.1534449477680619E-08, 2.8645743722837001E-08, 2.8641239597942000E-08 + 70, 41,-2.6111420022190232E-09, 7.3295059439551202E-09, 2.8468727799599000E-08, 2.8450107093946000E-08 + 70, 42,-1.1233762199402850E-08, 3.7699825858005492E-09, 2.8270280458084002E-08, 2.8276407758293999E-08 + 70, 43, 1.5769352596786721E-08,-1.1733717053254920E-08, 2.8079623536358001E-08, 2.8097440831092000E-08 + 70, 44,-9.3263217941864955E-09,-3.8010309117161091E-08, 2.7906503580290999E-08, 2.7889076797400000E-08 + 70, 45,-2.6401922213000899E-08,-1.0075860468347820E-08, 2.7677426278146000E-08, 2.7685539074631000E-08 + 70, 46, 1.1397124399644710E-08, 7.1278540214887320E-09, 2.7453481065677001E-08, 2.7462038876953000E-08 + 70, 47,-3.2075150947700420E-09,-1.8995829864121581E-08, 2.7229113067225000E-08, 2.7233527673057001E-08 + 70, 48,-2.1069454357050590E-08,-1.2870489658920280E-08, 2.6984331527436999E-08, 2.6957146583148000E-08 + 70, 49, 6.1762788302812462E-10, 2.4216868961622859E-08, 2.6732565609375999E-08, 2.6721918719371000E-08 + 70, 50,-2.2646450355798781E-08,-3.0382098438612532E-09, 2.6501322974567998E-08, 2.6503816954415001E-08 + 70, 51, 3.4426938830612930E-08,-3.3791449016225977E-08, 2.6196476585579001E-08, 2.6173471088411999E-08 + 70, 52,-4.7716242251803453E-08, 2.2562131911969629E-08, 2.5820733659258999E-08, 2.5812212875877001E-08 + 70, 53, 1.9937872600147260E-08,-3.2185599251037740E-09, 2.5490115775333000E-08, 2.5493672821989000E-08 + 70, 54,-1.3612300490370350E-08,-3.6031498495872218E-09, 2.5082406824579000E-08, 2.5100125883696001E-08 + 70, 55, 1.0079611623376240E-08,-1.5288087240950561E-08, 2.4673371032729999E-08, 2.4672787078322000E-08 + 70, 56, 1.0422612037056009E-08,-2.1918919236026019E-08, 2.4278073950657999E-08, 2.4267332600443999E-08 + 70, 57,-3.0158829872559971E-09,-2.1111548350054959E-08, 2.3851679201385998E-08, 2.3814336129879001E-08 + 70, 58, 1.4236432290351169E-08,-3.0007715221803049E-09, 2.3232516668796001E-08, 2.3245248925013001E-08 + 70, 59,-4.7165889902010462E-08,-1.5279460471512019E-11, 2.2762049187365000E-08, 2.2778956480098001E-08 + 70, 60,-1.9180703947653840E-08, 1.3749945641599169E-08, 2.2057378376204001E-08, 2.2039137180895000E-08 + 70, 61,-2.3048864121345989E-08, 3.5243726398225883E-08, 2.1534837234819002E-08, 2.1555460430171001E-08 + 70, 62, 1.6206555395291540E-08, 3.0222864022735292E-08, 2.0661514450857000E-08, 2.0684928569439002E-08 + 70, 63, 3.7354428350443083E-08,-2.0023548038207220E-08, 2.0022251842652999E-08, 2.0047719340333998E-08 + 70, 64,-1.2854662706360769E-10,-2.4650292634864379E-08, 1.9164259238250001E-08, 1.9117188336741001E-08 + 70, 65, 1.7472670958113491E-08, 1.8479864678455521E-08, 1.7746278876955000E-08, 1.7784450774821000E-08 + 70, 66,-1.4809281282693381E-08,-1.8813863583771640E-09, 1.6965468643746999E-08, 1.6954298439692000E-08 + 70, 67,-4.5133348623732204E-09, 2.8233612944585691E-08, 1.5367562652975001E-08, 1.5376706270083001E-08 + 70, 68, 6.8159336960863941E-08, 2.8549064858643569E-08, 1.1971334169210001E-08, 1.1992273570997000E-08 + 70, 69,-1.6589479214403129E-09, 3.7090077381570487E-08, 7.5680077660476996E-09, 7.6075363248372001E-09 + 70, 70,-2.6369438738045919E-09, 1.6323937086419450E-09, 8.6296054671129004E-09, 8.5936833185489998E-09 + 71, 0,-9.9669551926998209E-08, 0.0000000000000000E+00, 3.2722180312853999E-08, 0.0000000000000000E+00 + 71, 1,-1.6972003167125830E-08, 1.9829175912852659E-08, 3.2343398901947001E-08, 3.2355587798100998E-08 + 71, 2, 1.6410402991795750E-09, 1.4659266878294910E-08, 3.2178386520336002E-08, 3.2185749514866003E-08 + 71, 3, 2.4048888325664680E-08, 8.1143011875860034E-09, 3.2265996093558003E-08, 3.2287484987933998E-08 + 71, 4, 3.2714330064309511E-09,-2.6906149250308480E-09, 3.2136787846569998E-08, 3.2095410956967000E-08 + 71, 5, 1.0171848540962650E-08,-2.0776583995093090E-08, 3.2154014635038999E-08, 3.2155707176770002E-08 + 71, 6,-3.8367363613918274E-09, 1.3581771673407720E-08, 3.2043342973699003E-08, 3.2048745536286999E-08 + 71, 7, 3.1573302838679109E-09,-3.4454021587589530E-10, 3.2012588095870999E-08, 3.2001373985132001E-08 + 71, 8, 2.0521284352012259E-09, 9.6729511633649327E-09, 3.1928919288600998E-08, 3.1948397377237002E-08 + 71, 9,-1.3961870462699331E-08,-4.7344132336446522E-09, 3.1852131650347998E-08, 3.1849134607665002E-08 + 71, 10, 5.3782795028767539E-09,-2.1406386686778549E-08, 3.1802501797486999E-08, 3.1801407173451002E-08 + 71, 11,-4.9874586051620124E-09,-7.1600169774440699E-09, 3.1683313031173999E-08, 3.1689428285498997E-08 + 71, 12,-7.6633620504809635E-09, 3.7084112938933061E-09, 3.1649983842422997E-08, 3.1650581552353003E-08 + 71, 13, 2.7826732434214562E-08, 2.3312831331857679E-08, 3.1531728065111998E-08, 3.1539763671516997E-08 + 71, 14,-5.0927725395165751E-09,-3.6585749246244840E-09, 3.1491610897606998E-08, 3.1492798129552000E-08 + 71, 15, 1.5938846748911190E-08,-4.9420806144194544E-09, 3.1368540716649000E-08, 3.1357911026917003E-08 + 71, 16,-9.9075591615217204E-09,-4.6602890813109066E-09, 3.1332557030018002E-08, 3.1319175501138001E-08 + 71, 17,-7.8319674580579210E-09, 1.6491562634654959E-09, 3.1252295463175998E-08, 3.1239586295388997E-08 + 71, 18,-2.1016841004483651E-08,-2.5160294006566361E-09, 3.1160796309237003E-08, 3.1154439744085999E-08 + 71, 19,-1.4004550385970339E-08, 2.2457998927491969E-08, 3.1106851573634003E-08, 3.1125079993379003E-08 + 71, 20,-1.0224141971645081E-08, 7.6484765605507042E-09, 3.0964580497934001E-08, 3.0996359544639002E-08 + 71, 21, 2.9756774379517159E-08,-2.0506890009622570E-08, 3.0883240806770000E-08, 3.0884442686678998E-08 + 71, 22,-6.3419035703899323E-09,-1.5094330683236520E-08, 3.0794116017189999E-08, 3.0786555686601999E-08 + 71, 23, 6.4336129052406133E-09,-1.8356740998683499E-08, 3.0681351822984998E-08, 3.0703352292623003E-08 + 71, 24, 1.7598099628476890E-09,-2.9849119148757509E-10, 3.0602277920157001E-08, 3.0600609766588999E-08 + 71, 25,-3.1065712260445409E-10, 2.3400921025419790E-08, 3.0526743343221000E-08, 3.0541794046417998E-08 + 71, 26,-5.9930109456382523E-09,-1.9963760619881219E-08, 3.0405992883274997E-08, 3.0415278093546002E-08 + 71, 27,-1.7070470307427180E-09, 2.3157791194600560E-08, 3.0304271930509998E-08, 3.0308560917187997E-08 + 71, 28,-5.1361783257442134E-09,-4.0596491729985868E-09, 3.0200990259988998E-08, 3.0203395793211997E-08 + 71, 29,-2.1168834032021471E-08, 6.5723034144021624E-09, 3.0099685618860000E-08, 3.0101396406144000E-08 + 71, 30, 7.3959309096106614E-10, 5.3618943184920433E-09, 2.9993830481220999E-08, 2.9993499155011999E-08 + 71, 31, 2.2587894740109781E-08,-1.9681518909220922E-09, 2.9889234490202000E-08, 2.9883663891722002E-08 + 71, 32, 3.9637875866237476E-09,-1.9127237364091679E-08, 2.9767714085910999E-08, 2.9759439529323002E-08 + 71, 33, 1.8166797460546251E-08, 1.3824708342100070E-08, 2.9678826259123000E-08, 2.9656081114481000E-08 + 71, 34, 5.1837615324224992E-09, 8.9601994733948320E-09, 2.9540546559500000E-08, 2.9529437447896001E-08 + 71, 35,-1.9786395152533160E-08,-6.6575281071534936E-09, 2.9430751431623000E-08, 2.9438180617663999E-08 + 71, 36,-1.4000539013219580E-08,-1.9795667831988011E-08, 2.9336826907829998E-08, 2.9326938224310000E-08 + 71, 37,-2.4101638818299810E-08,-1.4269176741802449E-08, 2.9194320493058000E-08, 2.9181486051643001E-08 + 71, 38, 1.7378614074190311E-08,-3.0612642511839229E-09, 2.9094404854645999E-08, 2.9093088105969999E-08 + 71, 39, 1.3248124472054650E-08, 1.1106773542292081E-08, 2.8917997794760999E-08, 2.8921239608741001E-08 + 71, 40,-1.6798687786015549E-08,-2.2465708140776568E-09, 2.8790395860105000E-08, 2.8779378832863000E-08 + 71, 41,-3.1774220766311701E-08,-2.6914495772114439E-09, 2.8623376466681000E-08, 2.8613466981595998E-08 + 71, 42,-1.7876086568889071E-08, 5.6763295010055950E-09, 2.8481281942429999E-08, 2.8484660560318999E-08 + 71, 43, 1.9785656197862938E-09, 4.5080278025986258E-10, 2.8304157549488001E-08, 2.8312283182580999E-08 + 71, 44,-3.1594992291649627E-08, 2.5122146068178159E-09, 2.8197573226636002E-08, 2.8150978925395001E-08 + 71, 45, 5.1413684254885692E-09,-7.1265583036399699E-09, 2.7991919144613999E-08, 2.8002582607057001E-08 + 71, 46,-5.8581394222992342E-09, 8.7127693993753621E-09, 2.7800930091451000E-08, 2.7810503387818998E-08 + 71, 47, 2.3720012877269321E-08, 1.0684440477625681E-08, 2.7639485622009998E-08, 2.7648188802092000E-08 + 71, 48,-3.1844358069399347E-08,-2.2850767290228900E-08, 2.7402353856235000E-08, 2.7392767555700999E-08 + 71, 49, 9.2027595224473864E-09,-5.5691612414827853E-09, 2.7222121992588001E-08, 2.7229880939779999E-08 + 71, 50,-5.8195281279188491E-08, 4.0330838593115671E-09, 2.6994608583896000E-08, 2.6995813927083998E-08 + 71, 51, 3.3818658089504201E-08,-2.1109563865031301E-09, 2.6737787019683999E-08, 2.6733646300088000E-08 + 71, 52,-1.1807119779700420E-08,-4.3668667649641291E-09, 2.6445948079744999E-08, 2.6451246647251001E-08 + 71, 53, 2.0028043136214541E-08,-2.5319117048019139E-08, 2.6113119429121001E-08, 2.6114337097425001E-08 + 71, 54,-2.6927973889641291E-08, 9.3392357392173318E-09, 2.5834723676456000E-08, 2.5839750837433002E-08 + 71, 55,-9.5190611288506274E-09,-9.8649387834764429E-11, 2.5412091256660001E-08, 2.5416549306797001E-08 + 71, 56, 2.9698432586222858E-09, 1.6396699295273731E-08, 2.5127825150558001E-08, 2.5110824112931001E-08 + 71, 57,-2.2932561074105520E-08,-1.1228975245819830E-09, 2.4718784175073000E-08, 2.4694244095744999E-08 + 71, 58,-9.7654235363317687E-09,-3.0182747552584530E-08, 2.4245545000834001E-08, 2.4249099353825001E-08 + 71, 59,-2.4790257069595590E-08, 1.0220527043897430E-08, 2.3709210331838999E-08, 2.3730126264625999E-08 + 71, 60,-2.4134647301549351E-08, 1.1800138264930850E-08, 2.3243044866268999E-08, 2.3223216425783000E-08 + 71, 61, 2.6037040120129541E-08,-3.7037359850218130E-08, 2.2567866425664001E-08, 2.2577127408746001E-08 + 71, 62,-2.6786780288590279E-08, 1.9658084963050931E-08, 2.2134719438118001E-08, 2.2184300833106000E-08 + 71, 63,-3.0922936149157599E-09,-1.0592030247184380E-08, 2.1327976147104000E-08, 2.1338216532359001E-08 + 71, 64, 2.1775137372322929E-09, 2.0211488351192349E-08, 2.0697653465394001E-08, 2.0603354202599001E-08 + 71, 65,-1.2027501868100529E-08, 4.8052328818708468E-09, 1.9801720536824998E-08, 1.9854726615041999E-08 + 71, 66,-2.2921343393367021E-08,-1.4390435672782921E-08, 1.8409167498283000E-08, 1.8415956294903001E-08 + 71, 67, 1.3798342131329170E-08, 1.0368002686855321E-08, 1.7660340391174999E-08, 1.7642119879012999E-08 + 71, 68, 3.5639111635703982E-08, 4.6533588272512723E-09, 1.5912758513000999E-08, 1.5890336529468000E-08 + 71, 69, 6.1606342042180863E-08,-4.1156264184690961E-08, 1.2713933886716000E-08, 1.2685672586418000E-08 + 71, 70, 5.1293175883268118E-08, 2.9043610365190490E-08, 7.9147192010652001E-09, 7.9197036554027994E-09 + 71, 71,-6.6330531868963483E-09, 1.6537157821882598E-08, 9.3016579416225998E-09, 9.3083667157918994E-09 + 72, 0,-1.2400559997920529E-08, 0.0000000000000000E+00, 3.2304239816966999E-08, 0.0000000000000000E+00 + 72, 1, 8.0861192619732272E-09,-2.5170975454308920E-08, 3.1924985186672001E-08, 3.1949216608337997E-08 + 72, 2,-3.9391354669143189E-09,-5.3083824684429883E-09, 3.1766961869286998E-08, 3.1790273402163997E-08 + 72, 3,-6.2821444142459321E-09,-2.7983538172296119E-08, 3.1849458105724002E-08, 3.1873688486909000E-08 + 72, 4,-4.8904146793067710E-09,-2.5543799442943352E-10, 3.1727310199551000E-08, 3.1687965796262002E-08 + 72, 5,-8.4558626964249657E-09,-4.1843415452525244E-09, 3.1739358005362002E-08, 3.1738503672771000E-08 + 72, 6, 1.0750555071196580E-08, 1.5684858688265550E-08, 3.1642553985997001E-08, 3.1640770804799002E-08 + 72, 7, 2.4797188144752130E-09, 8.6955877876330657E-09, 3.1611211443357997E-08, 3.1603334442440999E-08 + 72, 8, 2.2223212062467300E-10, 6.1619823653171178E-09, 3.1537015407688003E-08, 3.1559228481545998E-08 + 72, 9, 1.3738809088981059E-08,-1.6070868603763529E-08, 3.1459897071127000E-08, 3.1454424057842002E-08 + 72, 10, 1.9970066349137920E-09, 4.4002196043969317E-09, 3.1423021994733000E-08, 3.1424143058249000E-08 + 72, 11,-6.1677571933725112E-09,-2.0265427132374409E-08, 3.1304980120674999E-08, 3.1307616782961001E-08 + 72, 12,-1.4180166323424979E-08,-1.0644402165157641E-08, 3.1285842088277002E-08, 3.1293492279146000E-08 + 72, 13, 2.2302177334952979E-08, 1.0550011525181380E-08, 3.1161032126826998E-08, 3.1165979827772999E-08 + 72, 14, 1.5366841919806931E-08,-1.2005088149135060E-08, 3.1127942780806003E-08, 3.1129978289248002E-08 + 72, 15, 3.1513980674151540E-09, 1.2418446931200630E-08, 3.1008127522071000E-08, 3.0994599982702997E-08 + 72, 16, 5.1300086631206253E-09,-5.1187053086573986E-09, 3.0981325629469000E-08, 3.0967432912810999E-08 + 72, 17,-1.3927324441700581E-08, 1.7826594669930088E-08, 3.0921395395613997E-08, 3.0904855278085001E-08 + 72, 18,-1.6793166246903090E-08, 7.4183914734368198E-09, 3.0817800456626999E-08, 3.0821578102235997E-08 + 72, 19, 1.5981339487919782E-08, 1.4681156451832240E-08, 3.0800247382441001E-08, 3.0813984073528001E-08 + 72, 20, 1.1837174149207389E-08, 3.9933533931539537E-09, 3.0644483874903003E-08, 3.0668375652492003E-08 + 72, 21, 3.1978412055190453E-08,-2.1218228098805071E-08, 3.0569815095000998E-08, 3.0572556942338999E-08 + 72, 22, 4.8208195055958381E-09,-2.0690035692550370E-08, 3.0489324536977999E-08, 3.0480766337005000E-08 + 72, 23,-6.6165346163196112E-09, 2.5198456371908448E-08, 3.0381157796900997E-08, 3.0399078406688998E-08 + 72, 24,-2.3319063280668671E-08, 5.0338620885428616E-09, 3.0318475350628001E-08, 3.0318993216337002E-08 + 72, 25, 4.8940500819204342E-09, 1.6585772746213190E-09, 3.0229421891480999E-08, 3.0242369072943998E-08 + 72, 26, 1.0008997001815649E-08,-7.7318467345780506E-09, 3.0153216071560998E-08, 3.0165958105731000E-08 + 72, 27, 4.7823421503767693E-09, 1.7840796900349469E-09, 3.0059339104376002E-08, 3.0065444810714002E-08 + 72, 28,-6.3562096876581640E-09, 7.5201297610048883E-11, 2.9964403267831999E-08, 2.9969544481763002E-08 + 72, 29,-1.3846442903533530E-08, 1.3063687414270490E-08, 2.9887604657632002E-08, 2.9890352761244998E-08 + 72, 30,-1.5650544312812840E-08, 3.0621322411119319E-09, 2.9785519205768002E-08, 2.9784947913298000E-08 + 72, 31, 2.6074970236616001E-08,-4.2479903649993244E-09, 2.9714618991458000E-08, 2.9713141899179000E-08 + 72, 32,-1.9886452766260940E-08,-1.4973579568647251E-08, 2.9595409343458998E-08, 2.9589489936551000E-08 + 72, 33,-5.2222999469853411E-09, 1.9322927683885319E-08, 2.9536721085513001E-08, 2.9512286851710000E-08 + 72, 34, 8.1622293437080507E-09, 5.4126598841253181E-09, 2.9413910267122999E-08, 2.9411388660240999E-08 + 72, 35,-1.9365710153314760E-08, 2.0945486332794931E-09, 2.9336754474282001E-08, 2.9320732102412999E-08 + 72, 36,-9.3271509527984338E-09, 7.4898289199419246E-09, 2.9282300316578002E-08, 2.9274525433795999E-08 + 72, 37, 1.1340633849627111E-08, 2.0502288699662019E-08, 2.9116794102917000E-08, 2.9109555181106001E-08 + 72, 38,-3.2549823652681770E-08,-1.2080609919362490E-08, 2.9105258443081000E-08, 2.9085088619871000E-08 + 72, 39, 4.3673968816460578E-09,-5.5246725573929090E-09, 2.8875816553183001E-08, 2.8882145151355001E-08 + 72, 40,-1.2914207380117560E-08,-1.8789353730746981E-09, 2.8789117568813000E-08, 2.8768352251284000E-08 + 72, 41, 2.3933716566861770E-10,-1.3060933193229850E-08, 2.8642429328760999E-08, 2.8646561995746000E-08 + 72, 42,-1.2194058585675750E-08, 5.3287840693302753E-09, 2.8511772302484999E-08, 2.8512778451277000E-08 + 72, 43, 2.4260411797904182E-08, 1.9351790994771169E-08, 2.8388727886737001E-08, 2.8388800954703000E-08 + 72, 44,-1.6229856825341321E-08,-6.4610891546807622E-09, 2.8281559128629000E-08, 2.8220768070339999E-08 + 72, 45,-5.6359066767091946E-09, 2.4986308895013620E-09, 2.8105611723965001E-08, 2.8116332614123998E-08 + 72, 46,-1.0687980840826080E-09, 2.5339961588514579E-08, 2.7953548064463001E-08, 2.7956170730341000E-08 + 72, 47,-7.6809531123382634E-09, 5.7748563312877850E-09, 2.7789915627347999E-08, 2.7792544730344001E-08 + 72, 48, 4.5393678115381311E-10,-3.2291470919922321E-08, 2.7599373842615000E-08, 2.7603489913287000E-08 + 72, 49, 2.0535209074818830E-09, 1.0545622624835560E-08, 2.7449648349050000E-08, 2.7447774632485000E-08 + 72, 50,-1.8679363635243709E-08,-1.8243923500786710E-08, 2.7295188933876000E-08, 2.7286088955480999E-08 + 72, 51, 4.4692291710314912E-08, 6.1218812463269743E-09, 2.7074290668637000E-08, 2.7079014028224000E-08 + 72, 52, 1.3605728755701820E-08, 1.1128405282782170E-08, 2.6822848282553002E-08, 2.6833493614060000E-08 + 72, 53,-8.4977076744883624E-09,-1.3326906916445950E-08, 2.6588780604505001E-08, 2.6601745236206000E-08 + 72, 54,-1.7858468017709110E-08, 9.1179610420679758E-09, 2.6307805386175999E-08, 2.6308179441832999E-08 + 72, 55, 7.0308975170499301E-09, 1.1064831997947520E-09, 2.6003229285678000E-08, 2.6008606006124000E-08 + 72, 56, 4.4601868723281169E-10, 2.0182616818735980E-10, 2.5739525091861999E-08, 2.5733415390970000E-08 + 72, 57,-1.2531450217643670E-09,-3.4717037273111938E-08, 2.5431469852234999E-08, 2.5414301146349001E-08 + 72, 58, 2.8779819477405450E-08, 1.4160473820741290E-08, 2.5031629811488999E-08, 2.5044180052901000E-08 + 72, 59,-2.9979959732671387E-08,-1.7603353798346799E-08, 2.4642282720645001E-08, 2.4674016806966001E-08 + 72, 60, 5.1741634477703881E-09, 1.3684359824363790E-08, 2.4103639736884001E-08, 2.4106617264768000E-08 + 72, 61,-9.7817214437971637E-09, 5.2303435451107023E-09, 2.3737987428077001E-08, 2.3708823738826001E-08 + 72, 62, 1.0908869399353859E-08, 1.9859815297657580E-08, 2.3026463818941999E-08, 2.3056461671347001E-08 + 72, 63, 1.6414926909048469E-08,-1.2547451780279930E-08, 2.2767047869682998E-08, 2.2695518737244000E-08 + 72, 64, 3.1876779365586422E-08,-1.2061135625039171E-09, 2.1832078983849999E-08, 2.1733207980593999E-08 + 72, 65, 4.4707014123123871E-08,-2.0621106375592171E-08, 2.1308814381471002E-08, 2.1257994652184002E-08 + 72, 66,-2.9241167935939869E-08,-3.7366723313258573E-08, 2.0537511647816001E-08, 2.0564915642871999E-08 + 72, 67,-2.8994127712788319E-08,-8.5690219739166553E-09, 1.9067485169466001E-08, 1.9072805295757001E-08 + 72, 68, 1.3585103235974300E-08,-7.5335129350855655E-09, 1.8381432835969999E-08, 1.8395246372969001E-08 + 72, 69, 4.5490984189121958E-09, 3.5216462280055030E-09, 1.6674229842645000E-08, 1.6722088897935998E-08 + 72, 70,-3.6951704237315931E-08,-8.4436021284281197E-09, 1.2930460086922000E-08, 1.2938401572436000E-08 + 72, 71, 1.4014721254529831E-09, 7.6701154851347941E-09, 8.2022312571959003E-09, 8.1691909974875993E-09 + 72, 72, 3.0109512089084069E-08,-5.9484322581029043E-09, 9.9302426770602995E-09, 9.9496308124608005E-09 + 73, 0,-5.4741093665270662E-08, 0.0000000000000000E+00, 3.1853110861192998E-08, 0.0000000000000000E+00 + 73, 1,-2.5693317519154569E-08, 1.8926017985384959E-08, 3.1392400512161002E-08, 3.1419047380892998E-08 + 73, 2,-8.0922326403307147E-09, 3.2789664716711121E-09, 3.1214459238027999E-08, 3.1286429980711000E-08 + 73, 3, 2.3331695127011891E-08, 1.5244645000583749E-08, 3.1331848540543998E-08, 3.1353134605871000E-08 + 73, 4, 2.0560672357391488E-09,-5.3440071906492728E-09, 3.1176339803461003E-08, 3.1150317263379002E-08 + 73, 5, 2.1036617688507812E-08,-2.5948452960461820E-08, 3.1230316037523001E-08, 3.1215929847339997E-08 + 73, 6,-3.5450443043776600E-09, 4.7565374325580562E-09, 3.1102715328379999E-08, 3.1100426956771999E-08 + 73, 7,-1.0825912688910680E-08, 8.8992170377227170E-09, 3.1079128807921999E-08, 3.1076388485386001E-08 + 73, 8, 1.2202191756818301E-08, 1.3828337127515269E-10, 3.0995700765555001E-08, 3.1032265912444998E-08 + 73, 9,-2.4721929170621640E-08,-1.4232064399891731E-08, 3.0928898140520998E-08, 3.0918036408212002E-08 + 73, 10, 3.8304752833644743E-09,-1.7037736969687330E-08, 3.0893419809058001E-08, 3.0899319952890999E-08 + 73, 11,-4.1014454401116401E-09,-1.3312221623789280E-09, 3.0756703722018001E-08, 3.0758146578916999E-08 + 73, 12,-9.2048790452939965E-09, 6.9878922614575126E-09, 3.0761810980272997E-08, 3.0771200949956002E-08 + 73, 13, 2.0171674662805600E-08, 1.1970064958384629E-08, 3.0607016967128001E-08, 3.0614833617862998E-08 + 73, 14,-1.1564044861758020E-08,-7.5947855393786930E-09, 3.0625435329757001E-08, 3.0629960461702000E-08 + 73, 15, 5.4762781081225561E-09,-8.2168640102559375E-09, 3.0467754004323997E-08, 3.0447063755049998E-08 + 73, 16, 5.3768934513216186E-09, 2.9149710208360549E-09, 3.0495412943951001E-08, 3.0473157885388999E-08 + 73, 17,-3.3024394527175541E-09,-1.0211270185853780E-08, 3.0419541754220000E-08, 3.0391662738098002E-08 + 73, 18,-2.1073279695128969E-08, 2.6906342069222108E-09, 3.0325933783487998E-08, 3.0354848387682997E-08 + 73, 19,-3.2108539387134749E-09, 2.2396627403926751E-08, 3.0342044262041999E-08, 3.0342879545047001E-08 + 73, 20,-5.3190341515864328E-09, 2.3202686860515431E-08, 3.0181428443743000E-08, 3.0205229514873002E-08 + 73, 21, 1.8542022380512279E-08, 9.0634840293188165E-09, 3.0101129619334997E-08, 3.0109349715164003E-08 + 73, 22, 4.4409278756620576E-09,-3.1566610529249421E-09, 3.0040335917309002E-08, 3.0026370195219997E-08 + 73, 23,-1.0532146277374580E-08, 4.1447345397356967E-09, 2.9946632610083003E-08, 2.9963072222219998E-08 + 73, 24,-1.3253913072054650E-08,-1.2614778301445610E-08, 2.9879209273120000E-08, 2.9880359488098003E-08 + 73, 25, 1.5410644927128019E-08, 8.3743960752836795E-09, 2.9865271732220000E-08, 2.9883926012912998E-08 + 73, 26,-3.1482346640230860E-09,-2.0788514546403189E-08, 2.9717624500683999E-08, 2.9730962858476999E-08 + 73, 27, 1.2198662639287141E-08, 1.7062074299846991E-08, 2.9669154558144001E-08, 2.9679172835920999E-08 + 73, 28, 1.3029895157416510E-08,-1.4326692678052160E-08, 2.9564480438613000E-08, 2.9571075850954999E-08 + 73, 29,-1.2769099818438640E-08, 1.8576959117096880E-08, 2.9507646399117999E-08, 2.9510085360810001E-08 + 73, 30,-2.3426554312624860E-09, 6.9522948784894772E-09, 2.9415366872248999E-08, 2.9414646442784999E-08 + 73, 31, 1.8543160489504450E-08,-8.5016620132344634E-09, 2.9350312188769000E-08, 2.9357633054978999E-08 + 73, 32,-4.8622132824035226E-09,-1.3540584118015830E-09, 2.9254614942096000E-08, 2.9251687447468000E-08 + 73, 33, 8.7505934499858853E-09,-1.7755772274068879E-08, 2.9202958718748001E-08, 2.9175044473426000E-08 + 73, 34, 2.9549936808802122E-09, 1.4320146835805880E-08, 2.9092981194592001E-08, 2.9104014991340999E-08 + 73, 35,-2.7746161904575618E-08, 1.9342711423253679E-08, 2.9053720602177001E-08, 2.9012116900272001E-08 + 73, 36,-5.7314244005564442E-09,-8.7650271232197934E-09, 2.8989865845984999E-08, 2.9000012402945000E-08 + 73, 37, 1.6822268415222970E-09, 4.1129030362602073E-09, 2.8876973110971000E-08, 2.8883860227333000E-08 + 73, 38,-6.1822889155722671E-09,-1.6225511562355602E-08, 2.8858412858609001E-08, 2.8816307667862999E-08 + 73, 39,-7.5209593092276301E-09,-3.9112606686174787E-09, 2.8694942228440999E-08, 2.8711805825626999E-08 + 73, 40,-2.7517460483207380E-08,-1.0214540035513699E-09, 2.8611789914119999E-08, 2.8588246437973999E-08 + 73, 41,-4.3396022448906552E-09,-8.7098777174848236E-09, 2.8497171970786001E-08, 2.8519644023844000E-08 + 73, 42,-9.5261180864394542E-09,-1.3530735779118541E-08, 2.8415864603307999E-08, 2.8406426609902000E-08 + 73, 43,-2.9596129258802169E-09,-1.1558351879537550E-08, 2.8310172495309000E-08, 2.8298208463797000E-08 + 73, 44,-1.9725479802088840E-08,-7.2172154836892608E-09, 2.8249075045048998E-08, 2.8203917850255999E-08 + 73, 45, 1.2109535796542291E-08,-2.0259807776412699E-09, 2.8094231998956999E-08, 2.8103967009766999E-08 + 73, 46,-1.1305474581606660E-08, 2.9162116167722511E-09, 2.8007169396252999E-08, 2.7996800109840002E-08 + 73, 47, 1.7591304519975371E-08, 2.5562850421019249E-08, 2.7892298445972000E-08, 2.7893317322759002E-08 + 73, 48,-1.5241509582782481E-08,-3.6210054636252202E-08, 2.7697226358652000E-08, 2.7714698612800001E-08 + 73, 49, 1.4673587515577570E-08,-4.0942166371660022E-10, 2.7644091541226000E-08, 2.7653354967144998E-08 + 73, 50, 3.3063910496673180E-09, 6.0306834735311729E-09, 2.7426962599196999E-08, 2.7413501880650000E-08 + 73, 51, 3.9793276665634052E-08, 8.5835067168987234E-09, 2.7303183643632998E-08, 2.7320311544823998E-08 + 73, 52,-1.5882375084015339E-08,-9.7961432962044899E-09, 2.7049610866690999E-08, 2.7061694730803000E-08 + 73, 53, 7.9113005303859590E-09,-7.2576117741929739E-09, 2.6863917447584999E-08, 2.6874668587755002E-08 + 73, 54,-2.8303185352910790E-08, 2.9437657299182630E-08, 2.6652697990482999E-08, 2.6646421761030001E-08 + 73, 55, 2.0877114712311380E-08, 1.8084463184397121E-08, 2.6353661826062000E-08, 2.6353267637363001E-08 + 73, 56, 1.0093269685306939E-08,-3.1322259745319729E-09, 2.6170562463766000E-08, 2.6159838362128001E-08 + 73, 57,-1.8199346497611261E-08,-1.2341428361233509E-08, 2.5871886797413001E-08, 2.5869204372138000E-08 + 73, 58,-2.7227314818093759E-08,-8.9894781637201285E-09, 2.5550142874913000E-08, 2.5553228082713999E-08 + 73, 59,-1.9554747623991011E-08,-2.6291038423612571E-08, 2.5191041291585001E-08, 2.5225162112812002E-08 + 73, 60, 4.2308201643079419E-10,-1.0897321138695080E-08, 2.4810677234645999E-08, 2.4815703310126999E-08 + 73, 61,-1.5303138052917889E-08, 7.4345536795173124E-09, 2.4349278354494999E-08, 2.4354206496032999E-08 + 73, 62,-1.6595563708347159E-08, 2.0524931155827349E-08, 2.4033225826939000E-08, 2.4023169937890000E-08 + 73, 63,-5.7194974429583177E-10,-1.3086725437076040E-08, 2.3471215370364999E-08, 2.3455322341739999E-08 + 73, 64,-2.0040041280622340E-09, 6.4320600673165623E-09, 2.3028610019652999E-08, 2.3055597583575001E-08 + 73, 65,-8.5518149983290422E-09, 1.7825477299461139E-08, 2.2233103872789000E-08, 2.2211620033390001E-08 + 73, 66,-1.4407254661105271E-08,-6.1659955876943662E-10, 2.1795780124255999E-08, 2.1857549960167999E-08 + 73, 67,-1.5906940974271650E-09, 2.2667132676949479E-08, 2.0905520432346998E-08, 2.0972755309152001E-08 + 73, 68, 3.7404434051274757E-08,-1.4045722279351511E-09, 1.9584819659375000E-08, 1.9578043735913999E-08 + 73, 69,-1.6133961635387780E-09, 1.1483182599622450E-08, 1.8873305708993001E-08, 1.8910258912401998E-08 + 73, 70,-3.2270022345185597E-08, 4.1734216780398537E-08, 1.6890499482160000E-08, 1.6924658578063999E-08 + 73, 71,-6.2517853081282672E-08, 4.3988808207129403E-08, 1.3431841150515000E-08, 1.3492724824515000E-08 + 73, 72, 5.4059865046867321E-09,-2.9657209661693879E-08, 7.9816395768636996E-09, 7.9589388354222005E-09 + 73, 73, 3.1531773877331532E-08, 1.7744208338832571E-08, 1.0566635178122999E-08, 1.0592024893628001E-08 + 74, 0, 1.0075095302618919E-09, 0.0000000000000000E+00, 3.1332401432589002E-08, 0.0000000000000000E+00 + 74, 1, 9.7280744073455260E-09,-1.9688685572869588E-08, 3.0899355935951000E-08, 3.0935517224389002E-08 + 74, 2, 5.8382050022705834E-09,-1.0376476318903401E-08, 3.0726928896512997E-08, 3.0823567797935003E-08 + 74, 3,-6.2393781927674953E-09,-3.1434745408575618E-08, 3.0838500982598997E-08, 3.0858579736032999E-08 + 74, 4, 6.7706886520845637E-10, 3.8470563461167074E-09, 3.0690669716231999E-08, 3.0673180067371999E-08 + 74, 5,-2.1302668974750321E-08, 3.8760204531147372E-09, 3.0736735289011001E-08, 3.0718292192408998E-08 + 74, 6, 1.8887448284673311E-09, 4.6548054931092217E-09, 3.0621151705821999E-08, 3.0620902030527003E-08 + 74, 7, 8.8910521172329863E-09,-1.1987991401491929E-09, 3.0603063176234002E-08, 3.0594676049820003E-08 + 74, 8, 5.1231791832347462E-09, 6.6857252258708036E-09, 3.0523370781407998E-08, 3.0565353116815001E-08 + 74, 9,-6.8177226329305461E-09,-1.7828236732168929E-08, 3.0460446452600997E-08, 3.0447826638579998E-08 + 74, 10, 1.6799790586647100E-09,-5.0062154013370718E-11, 3.0437571419136000E-08, 3.0443240068849002E-08 + 74, 11,-1.0502091304239840E-08,-2.7282584798622119E-08, 3.0294476344195001E-08, 3.0297420594524999E-08 + 74, 12,-1.1051955307534880E-08,-9.1042587957634570E-09, 3.0324540586312997E-08, 3.0338789144491003E-08 + 74, 13, 1.3706727146467690E-08, 6.8259934514329181E-09, 3.0151231589690998E-08, 3.0151656208619001E-08 + 74, 14,-6.5828114269428559E-10,-1.4102816776234701E-08, 3.0180179957077003E-08, 3.0185577886888002E-08 + 74, 15, 2.0929859994879220E-09, 1.4702473374405839E-08, 3.0025242910728002E-08, 3.0005095205481997E-08 + 74, 16,-7.3396905117352449E-09,-8.0998450215158468E-10, 3.0053250634358001E-08, 3.0032947713774000E-08 + 74, 17,-6.4586224604997291E-09, 1.6780956621800262E-08, 3.0005671853427002E-08, 2.9971872801758001E-08 + 74, 18,-2.5576044026497250E-08,-3.6137738492460249E-09, 2.9889430524349997E-08, 2.9922902402253000E-08 + 74, 19, 4.5026250913407378E-09, 8.8063779908964879E-09, 2.9937538808448001E-08, 2.9942168296745001E-08 + 74, 20, 1.4420627817618549E-08, 3.0187285064099519E-09, 2.9764118302831000E-08, 2.9773448095308000E-08 + 74, 21, 2.8610205089438201E-08,-2.9439727799811380E-08, 2.9681514925355001E-08, 2.9694810899638998E-08 + 74, 22, 3.7771142276217457E-09,-7.3144262868252003E-09, 2.9623851430434000E-08, 2.9611772183968000E-08 + 74, 23,-2.0439583641830281E-09, 5.2570086718028024E-09, 2.9531301101329001E-08, 2.9542757915560001E-08 + 74, 24,-3.4697069030418351E-08, 3.8737020804707541E-10, 2.9479263325078000E-08, 2.9479190797049000E-08 + 74, 25, 1.2710262753414379E-08, 2.1872265728607611E-09, 2.9436416491409999E-08, 2.9438293378712999E-08 + 74, 26, 9.9311429338501156E-09, 3.3528708643665978E-10, 2.9359852837604000E-08, 2.9372544354913999E-08 + 74, 27, 1.4960304177396081E-09, 2.6371128749702250E-08, 2.9279556956110001E-08, 2.9295217073261999E-08 + 74, 28,-1.9166422256374419E-09,-2.0441602759535269E-08, 2.9209492015937999E-08, 2.9214690647255999E-08 + 74, 29,-1.3485586208902279E-08, 1.4290709851165180E-08, 2.9154536003892000E-08, 2.9152438795154002E-08 + 74, 30,-1.5281871399872789E-08,-2.2157672593337750E-09, 2.9068548742639000E-08, 2.9065990081161999E-08 + 74, 31, 2.8490905907943262E-08,-3.5812345661718962E-09, 2.9022697212027002E-08, 2.9036167808250002E-08 + 74, 32,-1.1471024477268380E-08,-5.7786076336430004E-09, 2.8929060361668000E-08, 2.8927585836335999E-08 + 74, 33, 5.9391419943654012E-09, 1.4948147833063799E-08, 2.8911960129134998E-08, 2.8880018548081000E-08 + 74, 34, 1.1808556071112700E-08, 1.7663456463909311E-08, 2.8795147314267998E-08, 2.8816085971700000E-08 + 74, 35,-1.6215338381765901E-08, 2.3461507678399788E-08, 2.8793754987838001E-08, 2.8732883155273999E-08 + 74, 36, 7.3663137674168382E-09, 7.1967417856108246E-09, 2.8760600287301999E-08, 2.8789274769660000E-08 + 74, 37, 1.0555380051232289E-08,-9.4501969568586484E-09, 2.8604811296367001E-08, 2.8630617621010001E-08 + 74, 38,-1.6126661586148269E-08,-1.6874806556435301E-08, 2.8721422837285999E-08, 2.8647793678032999E-08 + 74, 39,-1.0821070428213859E-08, 1.8643884486158760E-08, 2.8434574302589000E-08, 2.8450844015982001E-08 + 74, 40,-2.8903992676261608E-08,-2.4635873452906769E-09, 2.8424655485779001E-08, 2.8400784452689001E-08 + 74, 41,-3.6501829178763288E-09,-1.5494328822211251E-08, 2.8277665981059000E-08, 2.8308219791283000E-08 + 74, 42, 7.4281523917726178E-09,-7.3143502808522306E-09, 2.8240905644524999E-08, 2.8227811130474999E-08 + 74, 43, 4.0090046755752222E-09, 1.3706598682103540E-08, 2.8152996678439999E-08, 2.8139098006765000E-08 + 74, 44,-1.6456097903566950E-08,-5.2517252028764690E-09, 2.8087533877809998E-08, 2.8048085040038001E-08 + 74, 45, 1.1410400145279960E-08,-1.2198872455205670E-08, 2.7972686839029999E-08, 2.7981623081776999E-08 + 74, 46, 5.0215385266106488E-09, 6.8312529776234621E-09, 2.7884717914952999E-08, 2.7871909927184999E-08 + 74, 47, 1.0218386521398239E-08, 1.4956354486089900E-08, 2.7781426246518000E-08, 2.7770431257513999E-08 + 74, 48,-1.9385473035965770E-08,-1.4032042799955039E-08, 2.7643699799397001E-08, 2.7659029835348001E-08 + 74, 49, 6.0798499458002649E-09,-1.1252952225701209E-08, 2.7544467154658000E-08, 2.7543157289043999E-08 + 74, 50,-4.0467447236503293E-08, 1.2866875283719640E-08, 2.7488391798228001E-08, 2.7465451571146001E-08 + 74, 51, 1.3058369595418580E-08, 2.5235260033298021E-08, 2.7295463468578000E-08, 2.7309768442138000E-08 + 74, 52,-9.4925285851321308E-10,-3.0016630154808312E-08, 2.7144144157270001E-08, 2.7149329060281002E-08 + 74, 53,-1.1717606258201639E-09,-1.3144062485990970E-08, 2.6986783052470001E-08, 2.7003106357902000E-08 + 74, 54,-4.2979502454087718E-09, 1.3835523394905591E-08, 2.6816683057475999E-08, 2.6802417190518999E-08 + 74, 55,-1.3964582121447690E-08, 1.1442276484206331E-08, 2.6589819407297001E-08, 2.6583418619510999E-08 + 74, 56,-7.4355996700606504E-09,-2.3892213010518999E-08, 2.6423057985282002E-08, 2.6420826132911999E-08 + 74, 57, 8.5893820495418265E-09, 4.6287818157601543E-09, 2.6196746281063999E-08, 2.6203034626917001E-08 + 74, 58, 1.4223319749053999E-08,-7.2525356592532598E-09, 2.5967234592157000E-08, 2.5972330051369000E-08 + 74, 59,-3.1560945760425668E-08,-1.6233989775165059E-08, 2.5641711714729001E-08, 2.5671006480948001E-08 + 74, 60,-1.0044742383552881E-08, 1.1633309733253759E-08, 2.5342912231703000E-08, 2.5344633813982001E-08 + 74, 61,-1.3719551806473151E-08, 3.6576239767861932E-09, 2.5032642804470999E-08, 2.5036660605096000E-08 + 74, 62,-7.7568104335235130E-09,-9.1145960807675060E-09, 2.4504442441604999E-08, 2.4499502769383000E-08 + 74, 63, 1.4158160495001469E-09,-6.4996803071943812E-09, 2.4342666166959999E-08, 2.4342475490696999E-08 + 74, 64,-9.3909834045483080E-09,-4.8242148396467982E-08, 2.3597821106513000E-08, 2.3636689854764001E-08 + 74, 65, 3.4032045911106451E-08, 7.4997005562898456E-09, 2.3476821068320002E-08, 2.3387712156629000E-08 + 74, 66,-3.8764835942804832E-08,-3.4562456110417973E-08, 2.2637880817535000E-08, 2.2685028716510001E-08 + 74, 67,-3.2394347205966272E-08,-5.0447308844694773E-08, 2.2168936584136000E-08, 2.2158228692076001E-08 + 74, 68,-1.4533345021218889E-08,-1.5283116324058440E-08, 2.1512722190631001E-08, 2.1516337035062000E-08 + 74, 69, 1.9694781066984122E-08, 4.6987237855780159E-09, 1.9969597049261001E-08, 1.9998366688941000E-08 + 74, 70, 8.1001982648263366E-09, 5.0194938793936178E-09, 1.9431571324358000E-08, 1.9443651300339000E-08 + 74, 71,-1.3499424263447450E-08,-3.7384219772171420E-08, 1.7443061840033000E-08, 1.7425027572625001E-08 + 74, 72, 4.3766182611447093E-08,-5.1102401895543206E-09, 1.3268874046908000E-08, 1.3244484317584000E-08 + 74, 73, 1.1643052517374630E-08,-4.8503962606024993E-09, 8.2427942304512004E-09, 8.2052739066543001E-09 + 74, 74, 3.9416412373467242E-08,-7.8497399090510311E-08, 1.1089099053353999E-08, 1.1072578996428999E-08 + 75, 0,-4.6896353147918351E-08, 0.0000000000000000E+00, 3.0790196465616998E-08, 0.0000000000000000E+00 + 75, 1,-2.2592220188599031E-08, 1.4004550230924031E-08, 3.0308720035565001E-08, 3.0342892968273003E-08 + 75, 2,-1.1489675614779261E-08, 1.2488378151695839E-09, 3.0090876019701002E-08, 3.0255677507342999E-08 + 75, 3, 2.3917301221307231E-08, 2.7167552255778501E-08, 3.0261687851638999E-08, 3.0269818855956003E-08 + 75, 4, 2.2431502654085531E-09,-1.2452216208437420E-08, 3.0059938794054000E-08, 3.0068701100105997E-08 + 75, 5, 2.4557419415764809E-08,-1.8021397424405839E-08, 3.0161977311540000E-08, 3.0121060431760997E-08 + 75, 6, 1.4593249956870580E-08,-3.1237499023377250E-09, 2.9991293984441998E-08, 3.0007496030171000E-08 + 75, 7,-5.6494043952309296E-10, 8.3606665288044001E-09, 3.0002355425987001E-08, 2.9986005637715998E-08 + 75, 8, 1.7205734816053740E-08,-8.7007436653910335E-09, 2.9893813224326002E-08, 2.9959779539497999E-08 + 75, 9,-7.1861227215401604E-09,-1.6008310943955761E-08, 2.9846942035006003E-08, 2.9828450422923999E-08 + 75, 10, 8.4184668515083113E-09, 3.3229660845952100E-10, 2.9819558779293998E-08, 2.9832552342006999E-08 + 75, 11,-1.3480374292945201E-08,-1.5826380406286020E-09, 2.9646844908046002E-08, 2.9650162256657001E-08 + 75, 12, 5.5657154819099028E-10, 7.7777717029185979E-09, 2.9704000512095999E-08, 2.9720030310418001E-08 + 75, 13, 1.7238187266363859E-08, 1.9611179251118052E-08, 2.9475578483170000E-08, 2.9494204821185000E-08 + 75, 14,-1.1014627571655960E-08,-5.3122128393950564E-09, 2.9567874138436001E-08, 2.9576271275326001E-08 + 75, 15, 1.8123628394921099E-08,-9.1636833905229820E-09, 2.9351857375967000E-08, 2.9324139033560001E-08 + 75, 16, 8.5615909432337770E-09, 5.7857900040811383E-10, 2.9447240155291000E-08, 2.9420227530740000E-08 + 75, 17,-1.3543572009815250E-08,-1.0930028987987230E-08, 2.9363918241421001E-08, 2.9317144188804001E-08 + 75, 18,-2.1945493454236149E-08,-2.7195403767574349E-10, 2.9268941781789000E-08, 2.9332339306699999E-08 + 75, 19, 1.0041680689885469E-08, 2.2221091019352519E-08, 2.9339729157776999E-08, 2.9318650425820000E-08 + 75, 20, 4.7540706040427858E-10, 9.8099131966942408E-09, 2.9165313154904999E-08, 2.9167760668130000E-08 + 75, 21, 3.3473641591781647E-08, 5.0901818605137374E-09, 2.9039425733817001E-08, 2.9069954150142000E-08 + 75, 22, 3.0905734568768900E-08,-1.1630735277352811E-08, 2.9021776634184001E-08, 2.9002298442249001E-08 + 75, 23,-1.2077910599331690E-08, 1.3782249641757650E-08, 2.8919382712053001E-08, 2.8934201274638000E-08 + 75, 24,-2.3454153833176751E-08, 3.4096962487687702E-09, 2.8872310715345001E-08, 2.8869008859858999E-08 + 75, 25, 7.2552602308290104E-09, 1.0196033044098790E-08, 2.8917346045988001E-08, 2.8947230633014001E-08 + 75, 26, 1.4954735791116050E-09,-8.2357350824665586E-10, 2.8719478009692999E-08, 2.8729571647766999E-08 + 75, 27, 1.0166408650948139E-08, 1.3011408311723590E-09, 2.8698352076590998E-08, 2.8724793638669001E-08 + 75, 28, 5.4859435063085232E-09,-2.5433844057488710E-08, 2.8584993429908001E-08, 2.8588566140959001E-08 + 75, 29,-2.1237084854367151E-08, 2.9148021009514070E-08, 2.8569706825657001E-08, 2.8568015944225001E-08 + 75, 30,-2.6135990199764919E-09, 1.4861830179316009E-08, 2.8459188262275001E-08, 2.8453968903437001E-08 + 75, 31, 1.3028227968488190E-08,-6.4457592785768308E-09, 2.8423052087117000E-08, 2.8440940705947001E-08 + 75, 32,-2.1611515301590511E-08,-3.3878461271873708E-09, 2.8329937779834001E-08, 2.8328073366292000E-08 + 75, 33, 4.4491836586969890E-09,-1.1404533462432310E-09, 2.8313203386331001E-08, 2.8282360195086002E-08 + 75, 34,-1.2966968824587161E-08, 3.8648224673189872E-09, 2.8198380665509999E-08, 2.8239545863871999E-08 + 75, 35,-3.2700622916454551E-08, 8.1795421393296579E-09, 2.8212586931190001E-08, 2.8128125693264001E-08 + 75, 36,-3.1736313896168232E-09,-3.1533093391336070E-09, 2.8188526999179999E-08, 2.8242999191411000E-08 + 75, 37, 2.0433933002914222E-08, 1.7970738355989550E-08, 2.8041349906848000E-08, 2.8100698809307001E-08 + 75, 38,-1.9430804820971561E-08,-8.1717147649672835E-09, 2.8170082651116999E-08, 2.8078218633378000E-08 + 75, 39,-4.8658462139157183E-09,-8.7584994060815594E-09, 2.7956991256435000E-08, 2.7980667876684001E-08 + 75, 40,-4.2498272957354219E-09, 1.9664993544376920E-08, 2.7898861520281001E-08, 2.7886766517764002E-08 + 75, 41, 4.7380510101968991E-09,-1.1881286744552161E-08, 2.7819450303963001E-08, 2.7874914447313000E-08 + 75, 42,-1.2302054753439581E-08,-9.7409571670223080E-09, 2.7788172648337000E-08, 2.7768005241990000E-08 + 75, 43, 1.0568085129256759E-08,-9.1245776790206231E-10, 2.7752728441382000E-08, 2.7732458347114999E-08 + 75, 44,-6.5832878759307027E-09,-1.9399965598599942E-09, 2.7702764258272000E-08, 2.7711945353532999E-08 + 75, 45, 1.1582704133514700E-08,-1.0226266918236140E-08, 2.7631218848474999E-08, 2.7638743366526999E-08 + 75, 46,-3.2627902505987739E-09, 9.2232465562029622E-09, 2.7628471319504999E-08, 2.7604648667289001E-08 + 75, 47, 4.1950998728464543E-08, 2.0187849948557899E-08, 2.7533159531270999E-08, 2.7522010448756000E-08 + 75, 48, 9.7818771333576663E-09,-3.8310433309344022E-08, 2.7441544613863000E-08, 2.7455968755336999E-08 + 75, 49, 8.8023201980156809E-09,-1.0344179675831800E-08, 2.7453661783078998E-08, 2.7449487803124000E-08 + 75, 50, 4.6929200000558797E-10,-9.0915621079922396E-09, 2.7240998321179000E-08, 2.7226039545250002E-08 + 75, 51, 4.7166738224088268E-08, 2.1482290312565371E-08, 2.7287645446410999E-08, 2.7305205509202000E-08 + 75, 52,-1.1299759886721820E-08,-1.8723605672709010E-08, 2.6979537991242999E-08, 2.6979934744044001E-08 + 75, 53,-1.0464448973376690E-08, 8.3269792256715692E-09, 2.6952933078306001E-08, 2.6973710180132999E-08 + 75, 54,-1.9924729532869841E-08, 1.9737346783039450E-08, 2.6756951518010000E-08, 2.6749720810639999E-08 + 75, 55, 3.1779917119005317E-08, 3.2921909620133463E-08, 2.6602210933736000E-08, 2.6580830853626001E-08 + 75, 56, 3.9027133981582889E-09,-2.5435481377500840E-08, 2.6450512067214999E-08, 2.6448432825298001E-08 + 75, 57,-1.2612163164246850E-09, 1.0425085082161961E-08, 2.6265692451217002E-08, 2.6280010776014000E-08 + 75, 58,-1.6384850120440630E-08, 9.5760975006338343E-09, 2.6045493631161999E-08, 2.6039110269743000E-08 + 75, 59, 2.8223034543075930E-08,-5.8152635322268044E-09, 2.5782502127589999E-08, 2.5811925979322999E-08 + 75, 60, 7.0794721625292634E-09,-2.0928410389576309E-08, 2.5527217418160999E-08, 2.5533856050238000E-08 + 75, 61,-1.0646680294221530E-08,-8.7918602535505631E-09, 2.5227481623730001E-08, 2.5261393158642002E-08 + 75, 62,-2.3171923154297691E-08, 1.0343211168529460E-08, 2.5001919141784000E-08, 2.4976898813550999E-08 + 75, 63, 2.7593746524158100E-08,-3.9576713898458146E-09, 2.4645724003286001E-08, 2.4682933309256998E-08 + 75, 64, 3.1148780354186972E-08, 2.7251641718048100E-08, 2.4263285894677001E-08, 2.4417726770759999E-08 + 75, 65,-1.3058503363408430E-08,-7.2923895868523184E-09, 2.3849893822348998E-08, 2.3818480853252002E-08 + 75, 66, 4.9739139505425745E-10,-3.3073378539145731E-08, 2.3740229430299000E-08, 2.3770751648578000E-08 + 75, 67, 3.8476879717568003E-09, 4.0793960126823598E-09, 2.2861140038572001E-08, 2.2838024459891000E-08 + 75, 68,-6.6363768381986002E-09, 4.1510073368538271E-09, 2.2641153814067001E-08, 2.2635679652402000E-08 + 75, 69,-3.1667830476878962E-08,-1.9875903626184519E-08, 2.1754531113504001E-08, 2.1733972794247001E-08 + 75, 70,-1.5655056517275641E-08,-2.4115044540884171E-09, 2.0490838265395999E-08, 2.0486947089326999E-08 + 75, 71, 2.8279821168558261E-08,-1.0590150252496721E-08, 1.9855564205973001E-08, 1.9860801041017998E-08 + 75, 72, 2.1800540481196129E-08,-3.6040807302936321E-09, 1.7330633425846000E-08, 1.7358854313588002E-08 + 75, 73,-1.0906481447157671E-08, 1.4697366851891760E-08, 1.2993485547474000E-08, 1.2985913840682999E-08 + 75, 74, 3.5346406472764548E-09, 9.0478146947524686E-09, 6.6768082652238002E-09, 6.6879501223241000E-09 + 75, 75, 3.3081630082587631E-08,-6.3413289624522398E-09, 1.0411508025864999E-08, 1.0532915055619001E-08 + 76, 0,-1.5275562265008611E-08, 0.0000000000000000E+00, 3.0213832972279998E-08, 0.0000000000000000E+00 + 76, 1, 1.3542100665971810E-08,-8.2180330560394501E-09, 2.9787275279016000E-08, 2.9829917197802998E-08 + 76, 2, 5.0657700631733980E-09,-9.5117019631845240E-09, 2.9572646109747001E-08, 2.9759119175477999E-08 + 76, 3,-1.1936117971844610E-08,-3.5031302786457520E-08, 2.9735483258741999E-08, 2.9737757561228999E-08 + 76, 4,-1.0124385549915980E-08, 1.0792848761168861E-09, 2.9545616487865999E-08, 2.9567263804015999E-08 + 76, 5,-2.1737994856806980E-08, 6.6173065051489138E-09, 2.9636168275081999E-08, 2.9587918993118999E-08 + 76, 6,-5.3327992829587652E-09, 7.2786983083775231E-09, 2.9477948754060001E-08, 2.9503449963428999E-08 + 76, 7, 1.9008737776440359E-09,-7.0126705605203524E-09, 2.9494534317601999E-08, 2.9468908569063000E-08 + 76, 8, 3.2313581266268849E-09, 1.2032812174656250E-08, 2.9390412070953001E-08, 2.9465568095594999E-08 + 76, 9,-1.4764992785650390E-08,-6.9117363195682176E-09, 2.9350734583190999E-08, 2.9329079692246000E-08 + 76, 10, 1.4672751109051880E-10,-6.2719168248767139E-09, 2.9337017057111000E-08, 2.9350291774507000E-08 + 76, 11,-2.7338324615655138E-09,-1.8424891800560789E-08, 2.9156837809429001E-08, 2.9165682081747999E-08 + 76, 12,-1.1149644082276730E-08,-1.9971241189445680E-08, 2.9240254395145000E-08, 2.9262056569741001E-08 + 76, 13, 1.3468297907092810E-08,-9.3353076777423194E-09, 2.8992702726839000E-08, 2.8999632626066999E-08 + 76, 14,-4.8838757883831708E-09,-2.4683766421580301E-08, 2.9099446433667000E-08, 2.9107252157761000E-08 + 76, 15,-9.5458664072040122E-09, 1.5820445586331520E-08, 2.8887345300986001E-08, 2.8862914782523002E-08 + 76, 16,-6.3690280879167703E-09, 2.6425281006155160E-08, 2.8976913791827000E-08, 2.8953180713045000E-08 + 76, 17, 1.4498520996867471E-09, 6.6349195754510533E-09, 2.8921089722593001E-08, 2.8872071641382001E-08 + 76, 18,-2.2379111291654961E-08,-1.9976689360796441E-09, 2.8804032299175000E-08, 2.8863516858389000E-08 + 76, 19, 1.8196016771931870E-09,-2.3222529412256052E-09, 2.8893780186233001E-08, 2.8888615974581999E-08 + 76, 20, 1.2541618121940850E-08, 7.8055666050250177E-09, 2.8704978574429000E-08, 2.8692124032576000E-08 + 76, 21, 2.2051692938196101E-08,-2.0353664992427840E-08, 2.8594445468194001E-08, 2.8624619789605001E-08 + 76, 22, 3.7925707272445038E-09,-1.1713769923490910E-09, 2.8555454642585001E-08, 2.8541364970902001E-08 + 76, 23, 9.9024830706730526E-09, 5.5749438117867420E-09, 2.8467643017073000E-08, 2.8473214981196999E-08 + 76, 24,-5.6185400353265432E-08,-3.4578222543005391E-09, 2.8416281688525000E-08, 2.8413750911178999E-08 + 76, 25, 1.3078069608496070E-08, 1.3853921065570429E-08, 2.8425571308711998E-08, 2.8420469881208000E-08 + 76, 26, 7.0858919667888109E-09,-6.8741098596236528E-09, 2.8331442608140999E-08, 2.8339202093717001E-08 + 76, 27, 6.6077856813822496E-09, 3.1872819814188651E-08, 2.8252137691644999E-08, 2.8277524778382000E-08 + 76, 28,-4.7934972727196860E-09,-2.1554736627160090E-08, 2.8188439215301000E-08, 2.8187154594209000E-08 + 76, 29,-6.9561640400408213E-09, 9.9739093367577158E-09, 2.8154662143283999E-08, 2.8152147527939000E-08 + 76, 30,-1.7298850399053620E-08,-1.0449126853525500E-08, 2.8064297799181999E-08, 2.8052796814519000E-08 + 76, 31, 2.0427945666550649E-08,-5.1559623423232746E-09, 2.8036941325918002E-08, 2.8057472109155001E-08 + 76, 32, 4.6188133452199043E-09, 2.7630723894285118E-09, 2.7937736519744999E-08, 2.7937671569765000E-08 + 76, 33, 1.5473241247945410E-08, 1.8377877833733592E-08, 2.7964452496715000E-08, 2.7932870329807999E-08 + 76, 34, 1.8419218673326231E-08, 2.4203589807593202E-08, 2.7828198594611001E-08, 2.7880115882923999E-08 + 76, 35,-1.1576977790954119E-08, 3.0562695163727928E-08, 2.7899227259010999E-08, 2.7804483780965000E-08 + 76, 36, 1.6842641027547720E-08,-5.2895763194769646E-09, 2.7875632669953999E-08, 2.7949014755679000E-08 + 76, 37, 4.9878334002134014E-09,-1.8508351080782401E-08, 2.7722342349578000E-08, 2.7802775432280001E-08 + 76, 38,-5.8796709181566803E-09,-1.3300991622500540E-08, 2.7925329498394000E-08, 2.7812256481462001E-08 + 76, 39,-2.7255118627242899E-08, 1.7495493331454369E-08, 2.7600031185089000E-08, 2.7621717752444000E-08 + 76, 40,-4.2764383478624343E-08, 1.7328158872592910E-08, 2.7630575172532000E-08, 2.7616694948181000E-08 + 76, 41,-7.5639421613343147E-09,-1.2853264487589751E-08, 2.7462086890615002E-08, 2.7514630589406999E-08 + 76, 42,-4.9785954983282444E-09,-2.0985136562176680E-08, 2.7514874271147000E-08, 2.7494643172457001E-08 + 76, 43, 3.1629980923831900E-09, 4.0956341521482748E-09, 2.7428053391917998E-08, 2.7413107625753001E-08 + 76, 44,-9.7600780864094518E-09, 1.2881658770767079E-08, 2.7408412548273000E-08, 2.7431158045340999E-08 + 76, 45, 1.4562974744161569E-08,-1.2923541118449271E-08, 2.7349188810848000E-08, 2.7348520343827000E-08 + 76, 46, 2.8031007045234328E-09,-4.9700332234777708E-11, 2.7321945105937002E-08, 2.7300615505810000E-08 + 76, 47, 1.0563844804791500E-08, 3.3954909654783391E-08, 2.7244519040886001E-08, 2.7241717254358001E-08 + 76, 48,-5.7270565241558473E-09,-3.5131714347238557E-08, 2.7185385446366999E-08, 2.7189775060950001E-08 + 76, 49, 9.9755836942707195E-09,-2.6604252510247409E-08, 2.7123944250694999E-08, 2.7103728423121000E-08 + 76, 50,-3.2393406975226197E-08, 3.8186460952231392E-08, 2.7146845130828998E-08, 2.7131370276116001E-08 + 76, 51, 8.2363042876565837E-10, 1.8654777173338611E-08, 2.6975910788483999E-08, 2.6979899101007999E-08 + 76, 52,-5.4549961151072283E-09,-4.2051928579463380E-08, 2.6945223394714001E-08, 2.6934631726367999E-08 + 76, 53, 4.3328811794342242E-10,-6.4473774956132926E-09, 2.6780326202199000E-08, 2.6786485643535000E-08 + 76, 54, 7.7791880523864997E-09, 5.5462469156639319E-09, 2.6714178823743999E-08, 2.6711177546643999E-08 + 76, 55,-6.8513337728391641E-09, 2.2994083159371320E-09, 2.6561786397354999E-08, 2.6542042079835000E-08 + 76, 56,-1.6368328545898921E-08,-1.1352469519772731E-08, 2.6474331949048999E-08, 2.6470710832151001E-08 + 76, 57, 1.9502980844022588E-08, 9.8518619349957040E-09, 2.6313065950274001E-08, 2.6321710175306001E-08 + 76, 58,-1.0428224595543189E-09, 2.2733436814127639E-09, 2.6193730194258999E-08, 2.6202553714786000E-08 + 76, 59, 3.3076171807908240E-09,-6.5903650132201812E-09, 2.5890180703211001E-08, 2.5892436416364999E-08 + 76, 60, 1.4122752076930229E-08,-5.9114365282621552E-09, 2.5805991862962001E-08, 2.5803283978064001E-08 + 76, 61,-3.4255838664772141E-09,-5.2510041396373119E-09, 2.5450255956757001E-08, 2.5490475831870000E-08 + 76, 62,-7.4038269195788133E-09,-1.0721046719932759E-09, 2.5206584818110999E-08, 2.5211939806423000E-08 + 76, 63, 5.6962998814675104E-09,-2.2062841195169371E-08, 2.5060593846447999E-08, 2.5086675304051000E-08 + 76, 64, 1.0337872067318880E-08,-1.9947530879960030E-08, 2.4488599691390001E-08, 2.4556140496827999E-08 + 76, 65, 2.4819528792992309E-09, 1.3017093633185060E-08, 2.4504494277821001E-08, 2.4468589136304999E-08 + 76, 66,-3.5220416438795301E-08,-4.2238387100520158E-08, 2.3955662593722001E-08, 2.3972642286038001E-08 + 76, 67,-3.0661429493481528E-08,-3.6404084136808712E-08, 2.3854461612106001E-08, 2.3776014991893999E-08 + 76, 68,-3.9833577415097362E-08, 3.1373686013475949E-08, 2.3096088315052001E-08, 2.3106193023874001E-08 + 76, 69, 2.2162033547726120E-08,-2.2308442943132020E-08, 2.2735238470133998E-08, 2.2725235230212999E-08 + 76, 70,-1.9414412729604591E-08,-1.8240199445376290E-08, 2.2132285443050000E-08, 2.2105760915454001E-08 + 76, 71, 8.6782042079213286E-09,-4.7104721658484278E-09, 2.0521651231786001E-08, 2.0530786170286000E-08 + 76, 72, 7.6652339064003030E-09,-2.8300589690111488E-09, 2.0168134609693001E-08, 2.0164165473630001E-08 + 76, 73,-9.1348519390794130E-08, 7.5511220796638508E-09, 1.7314298573442001E-08, 1.7272712442422999E-08 + 76, 74,-2.0155617172204422E-09, 1.2023358233764180E-08, 1.2164788317635000E-08, 1.2158339209383999E-08 + 76, 75, 1.4435192685666169E-08, 2.5476954128332320E-08, 6.7067335513985998E-09, 6.8163025056430999E-09 + 76, 76,-1.7399521075397470E-08,-8.3679785269692319E-08, 1.1761886531710000E-08, 1.1694608989496999E-08 + 77, 0,-3.3175417267974147E-08, 0.0000000000000000E+00, 2.9620193578158000E-08, 0.0000000000000000E+00 + 77, 1,-2.3866987117812741E-08, 1.1039511430776550E-08, 2.9176390576995000E-08, 2.9214201793273001E-08 + 77, 2,-5.6504285348803869E-09, 2.9688927761926800E-09, 2.8897087456398999E-08, 2.9162464418748001E-08 + 77, 3, 1.9502158111920871E-08, 2.6586603502872751E-08, 2.9137537019366999E-08, 2.9120515790572002E-08 + 77, 4,-6.7737358711472307E-10,-7.6647813847472818E-09, 2.8880553023988002E-08, 2.8936273394312001E-08 + 77, 5, 2.4440475506094458E-08,-2.1718342682824999E-08, 2.9035156898091999E-08, 2.8954511880831001E-08 + 77, 6, 9.6718891732388609E-09,-1.0553399144575810E-09, 2.8808003428403000E-08, 2.8859336307799999E-08 + 77, 7,-2.5042239603513932E-10, 4.1402396035280771E-09, 2.8871645263911000E-08, 2.8823722284535001E-08 + 77, 8, 1.0019704173778510E-08,-1.8574257904051629E-08, 2.8720329085235001E-08, 2.8830531266207002E-08 + 77, 9,-5.3437911644649539E-09,-2.0954378216032671E-08, 2.8705729995842000E-08, 2.8673697008764000E-08 + 77, 10, 5.6073647359257583E-09, 4.5312772316525807E-09, 2.8680599497888000E-08, 2.8704353685767999E-08 + 77, 11,-1.5431932759667848E-08, 9.6923452772768300E-09, 2.8470596450096001E-08, 2.8481195029829001E-08 + 77, 12, 6.2253866165856333E-09, 1.3749722484469730E-08, 2.8577320128683000E-08, 2.8602570403414999E-08 + 77, 13, 1.5801847470957461E-08, 4.0176406856820637E-08, 2.8262707626823001E-08, 2.8295619490897000E-08 + 77, 14,-7.2133863810242424E-09,-1.2001667211665551E-10, 2.8434044854929001E-08, 2.8442703747652001E-08 + 77, 15, 2.8633786159681789E-08,-5.6078084156026958E-09, 2.8146766806993000E-08, 2.8113241410807001E-08 + 77, 16, 9.2223040601533342E-09,-2.1901115013133540E-09, 2.8304986908043000E-08, 2.8280327421007000E-08 + 77, 17,-6.5702753710968109E-09,-3.7504291049909100E-09, 2.8198810975666999E-08, 2.8137898181160999E-08 + 77, 18,-3.3936853691972102E-08,-3.9321007996800506E-09, 2.8108124016739001E-08, 2.8202520781963000E-08 + 77, 19, 9.7217939237696369E-09, 1.5459851261354921E-08, 2.8217637321957999E-08, 2.8176218141439001E-08 + 77, 20,-6.7837325866714813E-09, 1.4812011165582341E-08, 2.8023898566457000E-08, 2.8002438039974002E-08 + 77, 21, 4.4190917660587362E-08, 5.1673639214872533E-09, 2.7836016932860000E-08, 2.7887501919490999E-08 + 77, 22, 3.8384270534673912E-08,-2.4648567299249159E-08, 2.7863000144345002E-08, 2.7841865528972002E-08 + 77, 23,-1.5172212218229539E-08, 7.9174985239677096E-09, 2.7726570202112999E-08, 2.7737918935979001E-08 + 77, 24,-1.5091349711111090E-08, 5.6789638076045476E-09, 2.7712219590799000E-08, 2.7704840980214999E-08 + 77, 25, 1.2483172531881880E-08, 1.7627913133629489E-08, 2.7787358406349999E-08, 2.7839527406585001E-08 + 77, 26, 1.2661709780083051E-08, 4.9849294596781847E-09, 2.7551691054276000E-08, 2.7553539702346000E-08 + 77, 27, 2.5926873419532669E-09, 1.1128680458941290E-08, 2.7524806383454999E-08, 2.7565219159127000E-08 + 77, 28, 9.9625249234237912E-10,-2.7998652220863599E-08, 2.7393450260574000E-08, 2.7390253674555000E-08 + 77, 29,-3.3245345226563961E-08, 3.0347494878684523E-08, 2.7397667612583999E-08, 2.7405050070292001E-08 + 77, 30,-2.3264179601590362E-09, 1.1124559242777140E-08, 2.7263580356506999E-08, 2.7247056829359000E-08 + 77, 31, 9.9654950230004841E-09, 2.8374134453619601E-09, 2.7242173888758001E-08, 2.7263474767456001E-08 + 77, 32,-3.0620307644790332E-08, 4.6602642351817102E-09, 2.7119628045595001E-08, 2.7116614509471001E-08 + 77, 33,-1.7102091694009751E-09, 1.3189180633136450E-08, 2.7121711366707999E-08, 2.7090635162792000E-08 + 77, 34,-3.7013703900785947E-08, 1.3897274299832561E-09, 2.6984183616044999E-08, 2.7054887241173001E-08 + 77, 35,-4.0009848430695690E-08, 1.3288754608315410E-08, 2.7020591214252001E-08, 2.6911644364921999E-08 + 77, 36,-4.6233234567264471E-10,-7.0350701349119657E-09, 2.7035732501414000E-08, 2.7141613956525999E-08 + 77, 37, 1.8054556974615259E-08, 2.2171706132734130E-08, 2.6819830655878999E-08, 2.6947333096885002E-08 + 77, 38,-3.0210532855865199E-08,-7.4505211127095023E-09, 2.7092578081327998E-08, 2.6969316682231001E-08 + 77, 39, 8.9682069096164098E-10,-3.7757932970086457E-08, 2.6793410433097001E-08, 2.6816834724755001E-08 + 77, 40, 9.6648602092708163E-09, 6.6247824680071693E-09, 2.6752720649952999E-08, 2.6772238605457999E-08 + 77, 41, 2.1735393822840308E-08,-5.0720001114834133E-09, 2.6668289671138000E-08, 2.6739491804974998E-08 + 77, 42, 1.3767800851798921E-09, 5.5773107379982709E-09, 2.6671749509795000E-08, 2.6650434126438001E-08 + 77, 43, 1.1148562980873960E-08,-1.2986159400614080E-08, 2.6666317711823001E-08, 2.6663164277087001E-08 + 77, 44, 2.9619569317739052E-10, 4.1047726241601984E-09, 2.6634461988921001E-08, 2.6715675630275001E-08 + 77, 45,-3.9331790474298220E-09,-6.8735071970498514E-09, 2.6642092273277999E-08, 2.6627512976715000E-08 + 77, 46, 2.7172208592959601E-09, 1.0700062690193120E-08, 2.6718362353081999E-08, 2.6677832841400001E-08 + 77, 47, 4.6420626102513601E-08, 1.8709103669224872E-08, 2.6590950428269000E-08, 2.6599986862024999E-08 + 77, 48, 1.3111536260332990E-08,-3.4281223981577357E-08, 2.6666260169894000E-08, 2.6652518875213001E-08 + 77, 49, 7.5377140284312358E-10, 6.6545419309829426E-09, 2.6634023304814001E-08, 2.6604875192968001E-08 + 77, 50,-1.0523138935234350E-09,-2.1221151812756989E-08, 2.6542190632284001E-08, 2.6534647336292001E-08 + 77, 51, 2.7920949126953781E-08, 5.5253300949398177E-09, 2.6669459757218999E-08, 2.6650996492051002E-08 + 77, 52, 2.1865979719953751E-08,-2.0618971497344029E-08, 2.6301373584400001E-08, 2.6284241823912001E-08 + 77, 53,-2.7853088968286620E-08, 2.2145484523373081E-08, 2.6457884780373000E-08, 2.6463802889769000E-08 + 77, 54,-1.3557659475893080E-08, 1.7374493724990901E-08, 2.6150955767417001E-08, 2.6157374508517001E-08 + 77, 55, 1.9470344318422431E-08, 1.6447231786795762E-08, 2.6197948015183000E-08, 2.6158353981445001E-08 + 77, 56, 1.0273807462544781E-09,-2.4024139629688431E-08, 2.6008307265337000E-08, 2.6008539114835999E-08 + 77, 57,-1.8540320002195619E-09, 1.2018832230790190E-08, 2.5939503635904999E-08, 2.5962777208196001E-08 + 77, 58,-1.8772060070030891E-08, 2.0039726655070001E-08, 2.5736040141591999E-08, 2.5744218541357999E-08 + 77, 59, 4.0663977766531342E-08,-4.1600296163430312E-08, 2.5530352639672999E-08, 2.5523354083617001E-08 + 77, 60, 3.9306813456847252E-09,-1.9969167926295411E-08, 2.5344856082812998E-08, 2.5345901313289001E-08 + 77, 61,-4.1622337109332867E-08,-5.5046481987865780E-09, 2.5099685030480999E-08, 2.5141250982497001E-08 + 77, 62,-1.2448489605019870E-08,-6.9067892780718631E-09, 2.4863249162087000E-08, 2.4868988503687999E-08 + 77, 63, 4.5852839944717212E-08,-1.1252617509579430E-08, 2.4821484608242001E-08, 2.4895166263469002E-08 + 77, 64, 5.3578197763863217E-08,-3.7389764862516912E-08, 2.4412043337677001E-08, 2.4550698096371000E-08 + 77, 65, 1.7895694066166530E-08,-9.1403813720807074E-09, 2.4212040694448999E-08, 2.4294290667630001E-08 + 77, 66,-3.0732396298079967E-08,-1.5497918181919450E-09, 2.4217499790107001E-08, 2.4205311012691998E-08 + 77, 67, 4.2652856640314052E-08,-4.3052342995357923E-08, 2.3749531270038002E-08, 2.3718161340608001E-08 + 77, 68,-1.1407124511349361E-08, 2.0760845737049769E-08, 2.3713328790076001E-08, 2.3751707719240000E-08 + 77, 69,-2.1048696726471920E-08,-2.8766692588555371E-08, 2.2925636601356000E-08, 2.2932551932868001E-08 + 77, 70,-2.0901254918761260E-08, 1.3632007112972620E-08, 2.3063686957014001E-08, 2.3038320360697000E-08 + 77, 71, 4.9097185798932417E-08,-4.1448751395953247E-08, 2.1969475356902999E-08, 2.1978833865975000E-08 + 77, 72, 1.0406061832125910E-08,-2.6883803117298302E-08, 2.1007701984061000E-08, 2.0990076335877001E-08 + 77, 73,-9.3703314047004525E-09,-2.3876118906691889E-08, 2.0450987158327000E-08, 2.0450312727740001E-08 + 77, 74, 4.2478267694708333E-08, 5.1333310765161993E-09, 1.6962893498422999E-08, 1.6969142755027001E-08 + 77, 75, 4.2763313137673183E-08,-2.8093520976808132E-08, 9.4171491707272995E-09, 9.4311062016354992E-09 + 77, 76, 3.1027345584730982E-08,-4.5329929440797703E-08, 3.2977544914492999E-09, 3.0714463924657001E-09 + 77, 77, 4.1241019263909587E-08,-1.8342859868567531E-08, 9.6701433469026008E-09, 9.6752409383881003E-09 + 78, 0,-1.3651871576431630E-08, 0.0000000000000000E+00, 2.9022725966607999E-08, 0.0000000000000000E+00 + 78, 1, 2.0007574890190509E-08,-1.1917359750880859E-08, 2.8650773011152999E-08, 2.8692933121429002E-08 + 78, 2, 5.5015226881157541E-09,-1.3307175585156750E-08, 2.8375669371087999E-08, 2.8654852115496999E-08 + 78, 3,-1.5846114998914331E-08,-3.4654647377657671E-08, 2.8604263887904001E-08, 2.8574996280185001E-08 + 78, 4,-2.3430980031731121E-09,-3.0856491033797910E-09, 2.8366786884037001E-08, 2.8434317864359999E-08 + 78, 5,-2.5664356989469272E-08, 1.1227343688026411E-08, 2.8505205054163001E-08, 2.8415304140668999E-08 + 78, 6,-4.2329063954188410E-09, 6.1357015388518117E-09, 2.8292349507894999E-08, 2.8355855306800001E-08 + 78, 7, 3.5175015623792748E-09,-2.2474940557799511E-09, 2.8361007175515001E-08, 2.8296902789315001E-08 + 78, 8, 3.3632024253883828E-10, 1.0006639045488590E-08, 2.8217022016065999E-08, 2.8335591968731999E-08 + 78, 9,-1.4031289397781801E-08,-5.5193672283405140E-09, 2.8212179614964001E-08, 2.8178404344152000E-08 + 78, 10, 4.6561493893151328E-09,-3.5010840421413310E-09, 2.8200131239640000E-08, 2.8223225424240001E-08 + 78, 11, 1.1124683888028340E-09,-2.3469940179413101E-08, 2.7991536966383001E-08, 2.8006762262960000E-08 + 78, 12,-1.4868007306046500E-08,-1.4929135650882432E-08, 2.8117503222743001E-08, 2.8145023889503000E-08 + 78, 13, 1.6474788686008270E-08,-1.4784459967321050E-08, 2.7799112012734001E-08, 2.7810522528038999E-08 + 78, 14,-8.4387914942751674E-09,-2.2806227401399440E-08, 2.7976763396315999E-08, 2.7984629023416000E-08 + 78, 15,-1.9225095190515649E-08, 1.8802105113413839E-08, 2.7702842030478002E-08, 2.7676972362551001E-08 + 78, 16, 1.7127843935318650E-09, 3.2626403374281277E-08, 2.7845636813494998E-08, 2.7828023715371000E-08 + 78, 17,-7.5622432795513458E-09, 1.1509899004014700E-09, 2.7770337032011001E-08, 2.7713000044629999E-08 + 78, 18,-1.9684626514578369E-08,-5.5569923673689877E-09, 2.7659378061241999E-08, 2.7738312861105000E-08 + 78, 19, 8.7851044052141443E-10, 4.4328039045677106E-09, 2.7769446102113000E-08, 2.7759401781888999E-08 + 78, 20, 1.4960110205560601E-08, 2.8467961695772671E-09, 2.7571636843659000E-08, 2.7537996665482000E-08 + 78, 21, 2.2260973857672711E-08,-1.0197133124881559E-08, 2.7420034535393000E-08, 2.7465324812645999E-08 + 78, 22, 1.6918113270808030E-08, 9.2180801400990337E-09, 2.7398984059580001E-08, 2.7389075629542000E-08 + 78, 23, 1.8363442573372010E-08, 5.9259472384364023E-09, 2.7302305808382000E-08, 2.7299076302856001E-08 + 78, 24,-6.5168973335790371E-08,-5.6186570044578471E-09, 2.7254924458824999E-08, 2.7249172486569001E-08 + 78, 25,-8.0020149689858571E-10, 1.7566254567495910E-08, 2.7295617661643000E-08, 2.7291038905248001E-08 + 78, 26,-8.2456842487408573E-10,-1.7370921843393679E-08, 2.7182930720313999E-08, 2.7183913504590999E-08 + 78, 27, 1.3054074184234781E-08, 2.5832528357660160E-08, 2.7097578374793001E-08, 2.7132947110281002E-08 + 78, 28,-2.1954569479845840E-09,-1.7595252584785489E-08, 2.7018945840947000E-08, 2.7009506264643000E-08 + 78, 29,-7.7154740621029854E-09, 1.7392123093596870E-08, 2.7002930004162999E-08, 2.7004558784840000E-08 + 78, 30,-1.3427583783324160E-08,-1.6218206792996189E-08, 2.6891741011854001E-08, 2.6870120249029000E-08 + 78, 31, 1.4463264750810081E-08,-3.7505707209113497E-09, 2.6879151750868000E-08, 2.6904251218031999E-08 + 78, 32,-6.1223042481017193E-09, 9.9449932172918519E-10, 2.6742388387190999E-08, 2.6746287460614001E-08 + 78, 33, 2.0489562195719131E-08, 6.7566468630021109E-09, 2.6807911128987000E-08, 2.6782331857702999E-08 + 78, 34, 2.7571395955110069E-08, 1.9055959971506420E-08, 2.6630852073035000E-08, 2.6702834815196001E-08 + 78, 35,-1.3564025060229139E-08, 2.2978240346277951E-08, 2.6767236534122999E-08, 2.6662547973572999E-08 + 78, 36, 2.9617739040252459E-08,-1.0265219513765370E-09, 2.6720320922733999E-08, 2.6831082668030001E-08 + 78, 37, 1.4328040037043590E-08,-1.1875507753171301E-08, 2.6606710992597999E-08, 2.6740312311114001E-08 + 78, 38, 4.5525139866867448E-09,-9.0210705706669276E-09, 2.6785268182268001E-08, 2.6653404632869001E-08 + 78, 39,-4.4208460736287958E-08, 3.2766453071359660E-08, 2.6475817289000001E-08, 2.6494106890838000E-08 + 78, 40,-5.4775320369152988E-08, 1.9907894410641531E-08, 2.6488068720746000E-08, 2.6491453703792001E-08 + 78, 41,-1.9457995138341308E-08,-3.6877927764556069E-09, 2.6309439970051002E-08, 2.6358210127909000E-08 + 78, 42,-1.9122347312174541E-08,-2.1770803826649540E-08, 2.6408151436312001E-08, 2.6378795380037000E-08 + 78, 43,-3.8034186892750508E-10, 1.6092632991879600E-08, 2.6276071671249999E-08, 2.6286027155563001E-08 + 78, 44, 1.6272779595474851E-08, 2.2786917071128530E-08, 2.6334279548309999E-08, 2.6399877272483000E-08 + 78, 45, 1.7758047251864851E-08,-1.9161221480271919E-08, 2.6277064669265999E-08, 2.6256127706455999E-08 + 78, 46,-8.6432470414735680E-09,-6.3100591625703029E-09, 2.6304824810059999E-08, 2.6277632396778999E-08 + 78, 47, 1.1972538323825741E-08, 4.0590540684757391E-08, 2.6217035242341000E-08, 2.6231422792506999E-08 + 78, 48,-7.2490151276142393E-09,-5.2431032020474942E-08, 2.6240072695935999E-08, 2.6219045747103000E-08 + 78, 49, 9.8093091502996847E-09,-4.3141485986115851E-08, 2.6182608167615000E-08, 2.6140723190875999E-08 + 78, 50,-1.4989609756122451E-08, 5.1926730280767347E-08, 2.6271838034858000E-08, 2.6269793548220998E-08 + 78, 51,-7.9977581382417977E-09, 4.2362178988934302E-08, 2.6146242119058001E-08, 2.6129696649207002E-08 + 78, 52,-3.9298114215323109E-09,-5.7885360953354813E-08, 2.6187464138127000E-08, 2.6175050760403001E-08 + 78, 53, 1.2580059385347289E-08, 6.2664858835279148E-09, 2.5998582355149998E-08, 2.5986094815756000E-08 + 78, 54, 6.8087934100149373E-09, 2.5189629199644882E-08, 2.6035891685100001E-08, 2.6045882430394999E-08 + 78, 55,-2.1999688002496730E-08, 3.0189019107963272E-08, 2.5907400192572001E-08, 2.5875076080633999E-08 + 78, 56,-2.4758465288994892E-08, 8.1324017208831476E-10, 2.5880377881324999E-08, 2.5872704469655001E-08 + 78, 57, 1.4321935089203969E-08, 2.1154986103067481E-08, 2.5800892100797999E-08, 2.5800732794415000E-08 + 78, 58,-1.5044642459842980E-09, 4.1961252310966311E-09, 2.5713873840670999E-08, 2.5736153508446001E-08 + 78, 59, 2.7366093240144619E-08, 1.3547739175822880E-08, 2.5440983614354000E-08, 2.5414914296013000E-08 + 78, 60, 2.9279456595598089E-08,-2.6905744844189551E-08, 2.5503863192235001E-08, 2.5516499087904998E-08 + 78, 61, 1.1466439837640130E-08,-1.4141888252610170E-08, 2.4991111100146999E-08, 2.5029117600117999E-08 + 78, 62,-1.9796134744631280E-08, 2.1843938416468611E-10, 2.5107105989984999E-08, 2.5147989395649001E-08 + 78, 63,-8.7975778397654290E-09, 4.9167262526171497E-09, 2.4712965385141001E-08, 2.4736303794077001E-08 + 78, 64, 2.3532511325432969E-09, 2.9411359430649239E-09, 2.4571651309509999E-08, 2.4527535533070001E-08 + 78, 65,-3.5698843207339763E-08, 1.9204131108677311E-08, 2.4419318339087999E-08, 2.4494257754065999E-08 + 78, 66, 1.7817849411510959E-08,-3.9074902985096453E-09, 2.4252341141734000E-08, 2.4203502614197999E-08 + 78, 67, 5.2506177763955721E-10,-1.7844752125445780E-08, 2.4154972851436001E-08, 2.4125126292405001E-08 + 78, 68,-2.2841657154720451E-08, 1.9268193534068030E-08, 2.3652196465328000E-08, 2.3688446774712000E-08 + 78, 69,-2.3859852421256960E-09,-2.6496448788204721E-09, 2.3687380076543999E-08, 2.3700008761092001E-08 + 78, 70,-1.1958960176482331E-08,-2.8552579453117799E-08, 2.2954447894777000E-08, 2.2927978665686001E-08 + 78, 71, 3.8418648990361743E-08, 4.3855516171037893E-08, 2.2852725861751000E-08, 2.2832843990616999E-08 + 78, 72, 4.3838338903220726E-09,-6.1185921710597652E-09, 2.2033994231110000E-08, 2.2043202436203001E-08 + 78, 73,-8.0089805519322734E-08, 2.0196353740244259E-08, 2.0662450350928001E-08, 2.0664842994675001E-08 + 78, 74, 3.9981913982536377E-08,-1.8126404443645410E-08, 2.0540893705184999E-08, 2.0503913158330001E-08 + 78, 75,-4.4786922638042567E-08, 6.0581028295660345E-08, 1.7101239829210000E-08, 1.7104287001046001E-08 + 78, 76,-3.2203280490699882E-08,-1.2866571278288149E-07, 1.1740315174219000E-08, 1.1586663215419001E-08 + 78, 77,-3.4304873140560701E-08, 1.5206917869573420E-10, 3.4608847496430000E-09, 3.5072885577901998E-09 + 78, 78, 3.1916464057956610E-08, 1.7407733855812471E-08, 1.0603238277694999E-08, 1.0612151701529000E-08 + 79, 0,-2.6580030737536651E-08, 0.0000000000000000E+00, 2.8406033670986000E-08, 0.0000000000000000E+00 + 79, 1,-2.1141447252752409E-08, 1.0080122064768180E-08, 2.8038956558164000E-08, 2.8074335482151001E-08 + 79, 2,-1.3778279671509890E-09, 2.0941879284064319E-09, 2.7690567042505001E-08, 2.8045204385897000E-08 + 79, 3, 1.5752887151062088E-08, 2.7808062591184521E-08, 2.8005627672105000E-08, 2.7952126415059000E-08 + 79, 4,-2.5989916649176110E-09,-1.0014623816836661E-08, 2.7694253516184999E-08, 2.7798980364870999E-08 + 79, 5, 2.3849844931274459E-08,-2.2324311550613210E-08, 2.7898860549656000E-08, 2.7773880629867000E-08 + 79, 6, 1.2693918750351830E-08, 5.8662141671555628E-09, 2.7610978710242001E-08, 2.7706405947611999E-08 + 79, 7, 7.2622025907125383E-10, 6.1289679035864528E-09, 2.7740791201895001E-08, 2.7645389877345999E-08 + 79, 8, 6.2083459977640278E-09,-1.7592246180823199E-08, 2.7538248195141000E-08, 2.7697720471981999E-08 + 79, 9,-7.3729197241414301E-09,-1.3561340768086210E-08, 2.7570194820370001E-08, 2.7520042088585999E-08 + 79, 10, 3.3595386898662121E-09, 1.8483558688248771E-09, 2.7539197431145000E-08, 2.7573801554545001E-08 + 79, 11,-1.0074489446158050E-08, 8.1008957312218816E-09, 2.7307841248052002E-08, 2.7323126010209000E-08 + 79, 12, 8.1007918180071237E-09, 1.4077498800945890E-08, 2.7453343148622999E-08, 2.7483648208023000E-08 + 79, 13, 1.8069577841352691E-08, 5.1489202272910697E-08, 2.7065290294430999E-08, 2.7108884172578001E-08 + 79, 14, 8.1681676241915424E-10, 3.3668686509473910E-09, 2.7301723132253001E-08, 2.7310255626198001E-08 + 79, 15, 3.2017184378222612E-08,-1.1526148860022730E-08, 2.6948164728272999E-08, 2.6913386545771999E-08 + 79, 16, 3.7791328496748159E-09, 1.7097575134615780E-09, 2.7158646039274001E-08, 2.7143137093342999E-08 + 79, 17,-2.2704745380625439E-09,-2.9707056947370810E-09, 2.7027177309864999E-08, 2.6957176212445999E-08 + 79, 18,-4.1347583258882343E-08,-2.5641666062873002E-09, 2.6944842960034001E-08, 2.7059125015556999E-08 + 79, 19, 1.1113670901972480E-08, 8.3861788625615397E-09, 2.7069165784810002E-08, 2.7015936534416001E-08 + 79, 20, 2.0909369680074049E-10, 1.6351909855960989E-08, 2.6868165644142000E-08, 2.6824578831494000E-08 + 79, 21, 4.0304227516878183E-08, 4.0713203039800800E-10, 2.6611386264838000E-08, 2.6682011608180999E-08 + 79, 22, 2.4926830354468559E-08,-3.1187816409330242E-08, 2.6681984838412999E-08, 2.6663814106980999E-08 + 79, 23,-2.0960135677567920E-08, 1.0110876428822050E-08, 2.6497616664130000E-08, 2.6503109331887001E-08 + 79, 24,-2.5687447039431621E-08, 5.4501015736764983E-09, 2.6525681211605000E-08, 2.6514722640449999E-08 + 79, 25, 1.3896953755161469E-08, 1.2662760846587479E-08, 2.6597766123193000E-08, 2.6672381504904999E-08 + 79, 26, 1.0374944404687450E-08, 1.9040072461729770E-08, 2.6348505591400999E-08, 2.6340584320789999E-08 + 79, 27, 6.4902426225915351E-09, 1.0446895561494700E-08, 2.6285353236776000E-08, 2.6342753690812001E-08 + 79, 28,-3.6019150179790861E-09,-2.7443632143430341E-08, 2.6144450094775999E-08, 2.6135247243162001E-08 + 79, 29,-3.8140445458125997E-08, 2.9251802728910129E-08, 2.6145935360195001E-08, 2.6161718395696000E-08 + 79, 30, 8.2678515949939995E-10, 2.9762342108525880E-09, 2.5988044466050000E-08, 2.5958825985356000E-08 + 79, 31, 1.4533424578181500E-08, 4.9407812622841399E-09, 2.5967999749512000E-08, 2.5989766198848000E-08 + 79, 32,-3.3850440008485593E-08, 1.5470106395207130E-08, 2.5798441442357000E-08, 2.5795594954953999E-08 + 79, 33, 5.1482492244217673E-09, 1.7852610682086740E-08, 2.5801743058525000E-08, 2.5773196305063000E-08 + 79, 34,-5.0686057899307728E-08, 3.7483570955424440E-09, 2.5628624204639001E-08, 2.5713655815303001E-08 + 79, 35,-3.3805971674011822E-08, 6.6812094486717069E-09, 2.5657745596432002E-08, 2.5547252433485001E-08 + 79, 36,-1.0435712971752500E-08,-4.2322757653386844E-09, 2.5697343667523000E-08, 2.5840542728893999E-08 + 79, 37, 1.7427425191684930E-08, 1.7389995649766131E-08, 2.5416991345587999E-08, 2.5610403347492999E-08 + 79, 38,-1.9044806554282330E-08,-1.2842381759673790E-09, 2.5778214508578999E-08, 2.5639146831217002E-08 + 79, 39, 8.0549666023742409E-09,-5.3764757974955427E-08, 2.5384889358506000E-08, 2.5397895518933001E-08 + 79, 40, 2.0270332155353359E-08, 1.1271045831613349E-08, 2.5336416758496001E-08, 2.5399561725193000E-08 + 79, 41, 3.7857369162168423E-08,-8.8911425205243866E-09, 2.5235611477268000E-08, 2.5288543715860001E-08 + 79, 42, 1.2389077098640749E-08, 1.6523461188663089E-08, 2.5250439262593001E-08, 2.5228422532365000E-08 + 79, 43, 8.1925126641079528E-09,-3.6181668092325492E-08, 2.5193015143690002E-08, 2.5238994207966001E-08 + 79, 44,-4.0324254736891724E-09, 8.4480013773497379E-09, 2.5215084032786999E-08, 2.5342812682218001E-08 + 79, 45,-3.5876386976691451E-09,-9.6768720508613127E-09, 2.5250716618122999E-08, 2.5200294101511001E-08 + 79, 46, 5.3668257793554629E-09, 4.4538413182462928E-09, 2.5391350216885001E-08, 2.5344426076545000E-08 + 79, 47, 6.7852277040644681E-08, 6.9335781903548050E-09, 2.5172586106689001E-08, 2.5214882580532000E-08 + 79, 48, 1.5956480925978358E-08,-2.7957869552652088E-08, 2.5469307736683000E-08, 2.5411998392650000E-08 + 79, 49,-6.4981800647078387E-09, 2.4928400471582379E-08, 2.5253529642021001E-08, 2.5205430869683000E-08 + 79, 50,-1.1486294039335800E-08,-2.8299550866745701E-08, 2.5446267710243000E-08, 2.5441052560100000E-08 + 79, 51, 1.5985331395678501E-08,-2.3242000248460959E-08, 2.5432180905823999E-08, 2.5369499852719001E-08 + 79, 52, 2.4518970713275470E-08,-3.1043028031664022E-08, 2.5103497486590001E-08, 2.5071275194269000E-08 + 79, 53,-3.9248704786471621E-08, 3.4093821139579131E-08, 2.5370305965605999E-08, 2.5344228906529999E-08 + 79, 54, 5.0033326799261269E-09, 4.2821373026314309E-09, 2.4913997768510002E-08, 2.4932822390549000E-08 + 79, 55, 9.3098384361993808E-09,-2.1436417595878769E-10, 2.5193469068756999E-08, 2.5129465621074000E-08 + 79, 56,-1.7909341351874820E-09,-3.9828410101996712E-08, 2.4819396219342999E-08, 2.4822346468001000E-08 + 79, 57, 1.5628256617125029E-08, 2.5934674432528089E-08, 2.4897227971736001E-08, 2.4932612225959999E-08 + 79, 58,-1.2456951740396420E-08, 2.0287981019799670E-08, 2.4609326924388000E-08, 2.4665653928733999E-08 + 79, 59, 5.0259401777492157E-08,-6.6100280141439520E-08, 2.4416476214743000E-08, 2.4367304540979998E-08 + 79, 60, 6.0923734035883318E-09,-1.2660929361926320E-08, 2.4213544302976999E-08, 2.4233794305003999E-08 + 79, 61,-4.5160894518078632E-08, 1.9672451463032159E-08, 2.3948442609383999E-08, 2.3968781912805999E-08 + 79, 62, 2.4321133263496842E-08,-1.2325140064759700E-08, 2.3530197667136999E-08, 2.3584724123424000E-08 + 79, 63, 6.0517473672512681E-08,-3.1732355109649897E-08, 2.3854644455448001E-08, 2.3936181934036001E-08 + 79, 64, 4.2562104356480792E-08,-5.3090015026228662E-08, 2.3112945127134999E-08, 2.3155291348904999E-08 + 79, 65, 3.8771705558373337E-08,-2.6500876877895741E-08, 2.3135301526009000E-08, 2.3392141334152999E-08 + 79, 66,-6.0744235543600940E-08,-2.6248392167649691E-08, 2.3068607687516000E-08, 2.2904489429946001E-08 + 79, 67,-2.6048881406844131E-08,-5.2226362665936691E-08, 2.2814841884041000E-08, 2.2914609399711001E-08 + 79, 68, 1.0890522127926011E-08, 2.4664285766552631E-08, 2.2451494135044000E-08, 2.2513812431631002E-08 + 79, 69,-2.1936907986687819E-08,-5.9110052787580798E-08, 2.2300693863597001E-08, 2.2372484011166001E-08 + 79, 70, 7.4666285039832773E-09, 9.2233343248650326E-09, 2.2268459269770000E-08, 2.2285827794734999E-08 + 79, 71, 4.1903761185084702E-08,-3.8798729843688747E-08, 2.1173757575620999E-08, 2.1128708171571999E-08 + 79, 72,-1.0966433703587360E-08,-3.7281832449681112E-08, 2.1956656229110999E-08, 2.1890014738789999E-08 + 79, 73,-2.3252182118600310E-08,-1.4921015165695480E-08, 1.9270072124999001E-08, 1.9256931746078001E-08 + 79, 74, 4.5947638667768872E-08, 2.4350363400316529E-08, 1.8794815287880000E-08, 1.8831895642319999E-08 + 79, 75,-3.3686739590678911E-09, 4.5706895926528217E-08, 1.8080066997501999E-08, 1.8153565688750000E-08 + 79, 76,-5.5910246120074843E-08, 9.3301033479905452E-08, 1.1514528615365000E-08, 1.1227534488311999E-08 + 79, 77, 6.1828489376380253E-09,-2.1854205513144799E-08, 1.0985811985810001E-08, 1.1089028933726000E-08 + 79, 78,-1.8979255881771220E-08, 4.7261929386014762E-08, 5.2218607737287998E-09, 5.2306901425724999E-09 + 79, 79, 2.5286185979705590E-09, 1.1264824975435439E-07, 8.8019736257302006E-09, 8.7229168559538994E-09 + 80, 0,-5.5581652244845550E-09, 0.0000000000000000E+00, 2.7809496063867998E-08, 0.0000000000000000E+00 + 80, 1, 1.6105404539531880E-08,-9.4305524346008953E-09, 2.7519346076780001E-08, 2.7555955196302001E-08 + 80, 2, 6.6996778257555003E-09,-1.2000398675073469E-08, 2.7183681699991999E-08, 2.7537900349047001E-08 + 80, 3,-1.4913771135925640E-08,-3.3984412455782507E-08, 2.7479546367451002E-08, 2.7411407030261000E-08 + 80, 4,-1.8616359355659361E-09, 2.2723162227291298E-09, 2.7194259162854001E-08, 2.7307608266155000E-08 + 80, 5,-2.1787117213659941E-08, 1.4311698457650620E-08, 2.7378715730623001E-08, 2.7246596828772001E-08 + 80, 6,-5.0324291407620092E-09, 2.0349773484661131E-10, 2.7109758485564001E-08, 2.7215513479652001E-08 + 80, 7, 5.4258286154019111E-09,-7.1629094284259131E-09, 2.7240288050471001E-08, 2.7128537628625001E-08 + 80, 8,-3.2432500567627532E-09, 5.5941061898149527E-09, 2.7049710146881000E-08, 2.7214908246617001E-08 + 80, 9,-1.3575229751083810E-08,-6.7346379720056380E-09, 2.7096295419965001E-08, 2.7040936621722001E-08 + 80, 10, 6.0338767165288471E-09,-5.5257907517891201E-09, 2.7075113004724001E-08, 2.7107923349240001E-08 + 80, 11,-8.0050257870553730E-10,-2.1938171865205221E-08, 2.6856104922334999E-08, 2.6874854898280999E-08 + 80, 12,-1.1320904524980449E-08,-1.6199449390020519E-08, 2.7009458341087000E-08, 2.7039347909023999E-08 + 80, 13, 1.7856216845155880E-08,-2.4186366001015391E-08, 2.6638795911337001E-08, 2.6656841757622000E-08 + 80, 14,-1.3772590643255320E-08,-2.4486738625583279E-08, 2.6870985988385999E-08, 2.6879132960532002E-08 + 80, 15,-2.5127653175995361E-08, 2.4953602836504131E-08, 2.6547151541928002E-08, 2.6525689347758001E-08 + 80, 16, 7.1008597667007362E-09, 3.4237594353959652E-08, 2.6730563215834999E-08, 2.6722210946258999E-08 + 80, 17,-1.3559531624407820E-08, 1.1962331638381260E-09, 2.6637731031683000E-08, 2.6574938222193999E-08 + 80, 18,-1.3313068257378230E-08,-1.3933135231347740E-08, 2.6535367421886002E-08, 2.6624524118733001E-08 + 80, 19,-6.5665171820662933E-10, 5.9986956470296269E-09, 2.6646175640487998E-08, 2.6635993231359002E-08 + 80, 20, 5.3256747647972054E-09,-1.1645879334670830E-09, 2.6450682188929998E-08, 2.6401796120763001E-08 + 80, 21, 1.7859384685587529E-08,-3.4484342445663418E-10, 2.6259060994827001E-08, 2.6317371629006001E-08 + 80, 22, 2.8517546646613449E-08, 1.4926530250858179E-08, 2.6255313313102000E-08, 2.6250803366181000E-08 + 80, 23, 2.1786189155315910E-08, 6.3492490207413779E-09, 2.6140837202441999E-08, 2.6131784602296001E-08 + 80, 24,-7.3642518997790722E-08,-2.5008483921826949E-09, 2.6107218025356001E-08, 2.6097389639324001E-08 + 80, 25,-4.1976675398835593E-09, 2.5472837659481589E-08, 2.6144164981169000E-08, 2.6144751663473999E-08 + 80, 26,-2.4839953626677899E-09,-1.8557992993411669E-08, 2.6034728347152000E-08, 2.6027232119138000E-08 + 80, 27, 1.3524678562542650E-08, 3.2027234412455680E-08, 2.5928877676997000E-08, 2.5974641403621000E-08 + 80, 28, 1.1788759692830061E-09,-1.9463450344046001E-08, 2.5835049889098001E-08, 2.5819394433451999E-08 + 80, 29,-3.0031703377047017E-11, 2.6392469990709931E-08, 2.5830759358036999E-08, 2.5832789715556999E-08 + 80, 30,-1.0905476474323560E-08,-1.7408655887830911E-08, 2.5686415555287999E-08, 2.5656734556563999E-08 + 80, 31, 4.8453046289875611E-09,-3.1553917918727250E-09, 2.5688273872605999E-08, 2.5715210332136001E-08 + 80, 32,-3.8797294062137374E-09, 1.5105918278718890E-09, 2.5498649206474001E-08, 2.5507118386917000E-08 + 80, 33, 1.4467562566795161E-08, 1.0532264311497570E-08, 2.5593752154250001E-08, 2.5576536590028999E-08 + 80, 34, 2.9957090174492722E-08, 1.7175081443761351E-08, 2.5362397230453002E-08, 2.5436381678179000E-08 + 80, 35,-2.6679638290762940E-08, 3.0050347935913090E-08, 2.5547156670594000E-08, 2.5453393274060001E-08 + 80, 36, 4.7882139135036427E-08,-1.0253981210654091E-08, 2.5452522397769001E-08, 2.5580116288200001E-08 + 80, 37, 2.1335310056201359E-08,-8.6270163459696745E-09, 2.5413192402642001E-08, 2.5591048446484999E-08 + 80, 38, 1.1733655877873139E-08,-1.2837065152313779E-08, 2.5456439543442999E-08, 2.5329192465855002E-08 + 80, 39,-4.2358818553315502E-08, 4.8205354525143243E-08, 2.5232587517348000E-08, 2.5241410488021999E-08 + 80, 40,-6.1925688280558291E-08, 2.1163233395031960E-08, 2.5150162704904001E-08, 2.5178303747426001E-08 + 80, 41,-1.4778028047205430E-08, 6.9861928407038001E-09, 2.4992407410166001E-08, 2.5015343549103001E-08 + 80, 42,-7.4516381551750495E-09,-2.9622241535938709E-08, 2.5086985758967001E-08, 2.5055965193180000E-08 + 80, 43,-4.6417899111812908E-10, 1.5184071707212271E-08, 2.4883531079576002E-08, 2.4926641537090999E-08 + 80, 44, 2.9630914123460569E-08, 3.0829745426810592E-08, 2.5022316721629999E-08, 2.5091762787643001E-08 + 80, 45, 1.3815009729208870E-08,-1.6022633320064719E-08, 2.4903750544597000E-08, 2.4857355119025999E-08 + 80, 46,-3.7622113836815749E-09,-9.5516683889124168E-09, 2.4984648571695999E-08, 2.4966318521517000E-08 + 80, 47,-8.3395782281606974E-09, 3.9326146365600977E-08, 2.4832086979760000E-08, 2.4867299812931999E-08 + 80, 48,-7.4165402895904980E-09,-5.3105370572886257E-08, 2.4929651777814001E-08, 2.4876579760528000E-08 + 80, 49, 9.6883903202945099E-09,-4.5311088372339381E-08, 2.4834830510448000E-08, 2.4774479927988000E-08 + 80, 50,-1.8597550344267569E-10, 6.8631082966286260E-08, 2.5002684299732999E-08, 2.5000273073624001E-08 + 80, 51,-5.2323803606734123E-09, 6.2384243796602041E-08, 2.4894622105521000E-08, 2.4863412956598000E-08 + 80, 52,-9.9280364590895888E-09,-3.9659759888552218E-08, 2.4928036474941000E-08, 2.4919418748146000E-08 + 80, 53, 3.0974627118803122E-08, 2.5145406028728331E-09, 2.4738901736350999E-08, 2.4700843511163000E-08 + 80, 54, 1.4324249654997550E-08, 2.7212568876220099E-08, 2.4830717089246000E-08, 2.4845304072938000E-08 + 80, 55,-3.4611647087559340E-08, 6.0636291133516465E-08, 2.4714583506211999E-08, 2.4664294087304000E-08 + 80, 56,-2.1896688456436420E-08, 2.5306620135170022E-08, 2.4681681759214000E-08, 2.4663796391865999E-08 + 80, 57, 1.6976934247191510E-08, 4.0812804128376697E-09, 2.4680641222375999E-08, 2.4679170243585000E-08 + 80, 58, 7.5350958316826669E-09, 1.6848844518199869E-08, 2.4526954355442001E-08, 2.4572033163790001E-08 + 80, 59, 2.7187534803989398E-08, 1.6049933859608012E-08, 2.4347664997237001E-08, 2.4300460253319001E-08 + 80, 60, 4.1789798980955653E-08,-3.0848877348280398E-08, 2.4410903627827999E-08, 2.4433955891687000E-08 + 80, 61, 1.0290268731821001E-08,-2.6461204206885849E-08, 2.3760307817292000E-08, 2.3775097641050000E-08 + 80, 62,-3.9933564935121653E-08,-7.9262343324545450E-09, 2.4167778438042001E-08, 2.4243240233636001E-08 + 80, 63,-2.6030744450060770E-08, 3.5246868459676597E-08, 2.3274157181616001E-08, 2.3281655738052000E-08 + 80, 64,-5.9943048904649453E-09, 5.3467015227894519E-09, 2.3745340681378000E-08, 2.3574343800997001E-08 + 80, 65,-7.2882879079013380E-08, 3.3028631779662417E-08, 2.3047894729124001E-08, 2.3176469694489999E-08 + 80, 66, 1.8872555283807299E-08, 3.5338553550096107E-08, 2.3444245174544000E-08, 2.3296178594182002E-08 + 80, 67, 5.4838625638220832E-08, 2.8538880314539471E-08, 2.3019120951375000E-08, 2.3023603816566999E-08 + 80, 68,-3.1178521587207417E-08,-3.0311470601815053E-11, 2.2727595990178002E-08, 2.2690790335318000E-08 + 80, 69,-2.6750142767822221E-08, 3.8383509799345972E-08, 2.2751761393620999E-08, 2.2728060015139999E-08 + 80, 70,-2.3386308516903710E-08,-1.9836141583977470E-08, 2.2090263259638001E-08, 2.2100660196533999E-08 + 80, 71, 2.9529758800279319E-08, 7.5624987216249083E-08, 2.2291530115080002E-08, 2.2233574050017000E-08 + 80, 72,-1.0032595911956839E-08,-1.9420146713508260E-08, 2.1022219368817000E-08, 2.1012363609914001E-08 + 80, 73,-3.2735606783809041E-08, 3.4309383774046480E-10, 2.1548877188606001E-08, 2.1527273614317999E-08 + 80, 74, 4.9461442700933757E-08,-1.7576578428478611E-08, 1.8981785612109000E-08, 1.9027076906016000E-08 + 80, 75,-8.4857257257284652E-08, 3.6022191229826377E-08, 1.8957114546149999E-08, 1.8939813872601999E-08 + 80, 76,-1.1068756994004329E-09, 4.2966533959804681E-08, 1.8182909273002999E-08, 1.8095040449487001E-08 + 80, 77,-7.4473875387362241E-08, 4.2482015215949883E-08, 1.4787068274503000E-08, 1.4833713082563000E-08 + 80, 78,-5.5783185324809871E-08,-4.0326401796641487E-08, 1.2148658100906000E-08, 1.2200276523601000E-08 + 80, 79, 3.8147798704151063E-08,-1.9721419218429551E-08, 3.8064906705878004E-09, 3.8718549642204002E-09 + 80, 80, 4.0582099786720437E-08,-5.3860308941804763E-08, 5.8013789517889999E-09, 5.7638703929097002E-09 diff --git a/supportData/LocalGravData/VESTA20H.txt b/supportData/LocalGravData/VESTA20H.txt index 8aeba49c8b..5a274be74a 100755 --- a/supportData/LocalGravData/VESTA20H.txt +++ b/supportData/LocalGravData/VESTA20H.txt @@ -1,232 +1,232 @@ 0.2650000000000000E+06, 0.1728824496930000E+11, 0.4064896055080000E-05, 20, 20, 1, 0.0000000000000000E+00, 0.0000000000000000E+00 0, 0, 1.0000000000000000E+00, 0.0000000000000000E+00, 0.0000000000000000E+00, 0.0000000000000000E+00 - 1, 0, 0.0000000000000000E+00, 0.0000000000000000E+00, 0.0000000000000000E+00, 0.0000000000000000E+00 - 1, 1, 0.0000000000000000E+00, 0.0000000000000000E+00, 0.0000000000000000E+00, 0.0000000000000000E+00 - 2, 0,-0.3177939699038000E-01, 0.0000000000000000E+00, 0.6302192052110000E-08, 0.0000000000000000E+00 - 2, 1,-0.4939139723693000E-09, 0.1596048836604000E-08, 0.1425606048467000E-08, 0.1426922421478000E-08 - 2, 2, 0.4184962374624000E-02, 0.1245378620599000E-02, 0.1151298529854000E-06, 0.2729943583164000E-07 - 3, 0, 0.3310552979215000E-02, 0.0000000000000000E+00, 0.6278614117139000E-08, 0.0000000000000000E+00 - 3, 1,-0.2612671052339000E-02,-0.4338914699986000E-03, 0.2345873359128000E-07, 0.2800174652735000E-07 - 3, 2,-0.7288907471268000E-03,-0.1173043933906000E-02, 0.3325351203588000E-07, 0.1825063066920000E-07 - 3, 3,-0.1546624776698000E-03, 0.2384935863669000E-02, 0.6975468578578000E-08, 0.9683021844873000E-07 - 4, 0, 0.3265599899336000E-02, 0.0000000000000000E+00, 0.1075763269629000E-07, 0.0000000000000000E+00 - 4, 1, 0.4920396871261000E-03, 0.1434888318128000E-03, 0.1090406142407000E-07, 0.1072569551038000E-07 - 4, 2,-0.6300987993766000E-03, 0.2630330871648000E-03, 0.1311457406587000E-07, 0.1578358910614000E-07 - 4, 3,-0.1047527508626000E-03,-0.6699821437764000E-03, 0.5787576847822000E-08, 0.2753437610584000E-07 - 4, 4, 0.8663528462123000E-04,-0.2970368903764000E-03, 0.1304661882953000E-07, 0.1239968008881000E-07 - 5, 0,-0.1202182459510000E-02, 0.0000000000000000E+00, 0.1802716595738000E-07, 0.0000000000000000E+00 - 5, 1, 0.7502015580788000E-03,-0.2508023828499000E-03, 0.1788324831114000E-07, 0.2049493723690000E-07 - 5, 2, 0.2033043303707000E-03, 0.6147958809249000E-04, 0.1470281777411000E-07, 0.1387145727637000E-07 - 5, 3, 0.6626676527554000E-03,-0.4370521656036000E-03, 0.2843998251066000E-07, 0.1948148364007000E-07 - 5, 4,-0.2307769552926000E-04,-0.6047104715904000E-03, 0.1578862863954000E-07, 0.2936925864161000E-07 - 5, 5, 0.6450599419048001E-03, 0.2556270919853000E-03, 0.1409282686729000E-07, 0.4802062536113000E-07 - 6, 0,-0.9971479248165999E-04, 0.0000000000000000E+00, 0.3192348805227000E-07, 0.0000000000000000E+00 - 6, 1,-0.3689637507360000E-03,-0.5367043709220000E-03, 0.3266674605564000E-07, 0.3108051163028000E-07 - 6, 2,-0.2874702927558000E-04, 0.4269129512184000E-04, 0.2888626729058000E-07, 0.2893107119377000E-07 - 6, 3,-0.1930585495620000E-04, 0.1836955099421000E-03, 0.2328896053105000E-07, 0.2429799856217000E-07 - 6, 4,-0.7825617515562001E-04, 0.4575356542208000E-04, 0.1727940775550000E-07, 0.1610483931025000E-07 - 6, 5, 0.2967358835437000E-03,-0.1742565712364000E-03, 0.2086830714082000E-07, 0.1261821207676000E-07 - 6, 6, 0.4938771693044000E-03, 0.4102369776849000E-03, 0.4093432212711000E-07, 0.4622961385480000E-07 - 7, 0, 0.1846938617921000E-03, 0.0000000000000000E+00, 0.5554717870363000E-07, 0.0000000000000000E+00 - 7, 1, 0.1069887372241000E-03, 0.7947893456014000E-03, 0.5668378042444000E-07, 0.5547605969559000E-07 - 7, 2, 0.3986149124117000E-03, 0.1994042422627000E-03, 0.5393656913811000E-07, 0.5277643685540000E-07 - 7, 3,-0.3605183252112000E-03,-0.5651392573033000E-04, 0.4915735164794000E-07, 0.4737103403303000E-07 - 7, 4, 0.3513479329314000E-04, 0.3445571070230000E-03, 0.3633666848066000E-07, 0.3983518758884000E-07 - 7, 5,-0.1874037699812000E-04,-0.1799836022671000E-03, 0.2549356248568000E-07, 0.2360802305471000E-07 - 7, 6, 0.1663646306986000E-05,-0.1220426815628000E-03, 0.1372713585145000E-07, 0.9795292270609001E-08 - 7, 7, 0.4089854666183000E-03, 0.1072749359836000E-03, 0.5130009156280000E-07, 0.5167446259437000E-07 - 8, 0,-0.8717683328542000E-04, 0.0000000000000000E+00, 0.9888907580048000E-07, 0.0000000000000000E+00 - 8, 1, 0.9922336158901000E-04,-0.5133018502066000E-05, 0.9794905351003000E-07, 0.9715537239055999E-07 - 8, 2,-0.6780160184482000E-04,-0.1505492250505000E-03, 0.9689581378651000E-07, 0.9657340203177000E-07 - 8, 3,-0.2665429398226000E-04, 0.2627075165168000E-03, 0.8737656928705000E-07, 0.8818828611603000E-07 - 8, 4,-0.1218869905423000E-03,-0.1661918078467000E-03, 0.7099218985919000E-07, 0.7239278442905000E-07 - 8, 5, 0.1149373505558000E-03,-0.7661327837606000E-04, 0.4809343054921000E-07, 0.4766221315026000E-07 - 8, 6,-0.3681758927829000E-03, 0.1327099040798000E-03, 0.2633585241154000E-07, 0.3854860034084000E-07 - 8, 7, 0.1428100360275000E-03,-0.6924088904599999E-04, 0.3325412794169000E-07, 0.3721396056742000E-07 - 8, 8, 0.8734476308255000E-04, 0.6889241489250000E-04, 0.7907879277156000E-07, 0.7791933392995000E-07 - 9, 0,-0.1919071091121000E-05, 0.0000000000000000E+00, 0.1726021226829000E-06, 0.0000000000000000E+00 - 9, 1,-0.1684323129119000E-03,-0.3380383290444000E-03, 0.1743617506337000E-06, 0.1726925848351000E-06 - 9, 2,-0.2538070436043000E-03,-0.1662129993008000E-03, 0.1697354358211000E-06, 0.1689523215515000E-06 - 9, 3, 0.3192919445457000E-03, 0.5247336331426000E-04, 0.1633291739201000E-06, 0.1628043939440000E-06 - 9, 4,-0.1092751280911000E-03,-0.6250644937545999E-04, 0.1376361483741000E-06, 0.1365255023555000E-06 - 9, 5,-0.4567350098798000E-05, 0.4449563889084000E-04, 0.1001279285625000E-06, 0.9989939226078000E-07 - 9, 6,-0.1927959999225000E-04,-0.6920806781393000E-04, 0.6040134002821000E-07, 0.5975595927013000E-07 - 9, 7,-0.1157531676444000E-03, 0.1846761157069000E-03, 0.4483825713473000E-07, 0.4665266157517000E-07 - 9, 8, 0.8384597764907000E-04, 0.1718410576234000E-03, 0.8090994802024000E-07, 0.7930020726512000E-07 - 9, 9, 0.5514112603942000E-04, 0.9043209949504001E-04, 0.1422279362527000E-06, 0.1428514730291000E-06 - 10, 0, 0.1377592596314000E-03, 0.0000000000000000E+00, 0.3085840497077000E-06, 0.0000000000000000E+00 - 10, 1, 0.9126248158335000E-04, 0.1551484066890000E-03, 0.3053455136806000E-06, 0.3032850221516000E-06 - 10, 2, 0.5022355299758000E-04, 0.6056673571061000E-04, 0.3034967037228000E-06, 0.3019737879639000E-06 - 10, 3, 0.1277546359626000E-04,-0.4062472101134000E-03, 0.2891411493668000E-06, 0.2886923077856000E-06 - 10, 4, 0.3841144635364000E-04, 0.4590948302321000E-04, 0.2631015053606000E-06, 0.2622208074524000E-06 - 10, 5,-0.1052485816696000E-03, 0.8495297065855001E-04, 0.2147893956140000E-06, 0.2150479328492000E-06 - 10, 6, 0.1478466927885000E-03,-0.1141852669825000E-03, 0.1252069338070000E-06, 0.1260209810635000E-06 - 10, 7, 0.8560694501398999E-05, 0.6962138970757000E-04, 0.8809544264141001E-07, 0.8487883896014001E-07 - 10, 8,-0.8534847154915000E-04,-0.1500658841920000E-05, 0.8249942876106000E-07, 0.8153120633312000E-07 - 10, 9, 0.1333765643829000E-03,-0.1700779740113000E-03, 0.1610431260684000E-06, 0.1629232665889000E-06 - 10, 10, 0.1816132626089000E-03,-0.7144286032944000E-04, 0.2558803998708000E-06, 0.2544847432919000E-06 - 11, 0,-0.5795945175749000E-04, 0.0000000000000000E+00, 0.5377556483849000E-06, 0.0000000000000000E+00 - 11, 1, 0.7001648560266000E-04, 0.7784675627781000E-04, 0.5434626022748000E-06, 0.5382166609197000E-06 - 11, 2, 0.1185251090365000E-03, 0.1541644383569000E-03, 0.5321918766358000E-06, 0.5293645199229000E-06 - 11, 3,-0.2297041199361000E-03,-0.4285481675882000E-05, 0.5202141574453000E-06, 0.5194747371489000E-06 - 11, 4, 0.8179980677869000E-04,-0.1137407133655000E-03, 0.4787111665770000E-06, 0.4777996114587000E-06 - 11, 5,-0.2861097125671000E-04,-0.2844235270080000E-04, 0.4248152974238000E-06, 0.4259236882684000E-06 - 11, 6,-0.3328988151364000E-04, 0.9489368589853000E-04, 0.3335066671863000E-06, 0.3347047092450000E-06 - 11, 7, 0.5036476606530000E-04,-0.6179930805018999E-04, 0.2286146172811000E-06, 0.2200937827458000E-06 - 11, 8,-0.2502747990421000E-04,-0.3617888947046000E-04, 0.1365872476952000E-06, 0.1368337002465000E-06 - 11, 9, 0.4338880751807000E-04, 0.8926665393751000E-04, 0.1538828673172000E-06, 0.1547755980588000E-06 - 11, 10,-0.5884646095355000E-05, 0.1303453862551000E-03, 0.3005764813772000E-06, 0.3011744618621000E-06 - 11, 11, 0.1661472599910000E-03, 0.1728729148920000E-04, 0.4500564771040000E-06, 0.4506915969995000E-06 - 12, 0,-0.8421914774262999E-04, 0.0000000000000000E+00, 0.9590113082374000E-06, 0.0000000000000000E+00 - 12, 1,-0.9803753731618000E-04,-0.1421977755211000E-03, 0.9516458379933000E-06, 0.9455269893516000E-06 - 12, 2,-0.5698123826017000E-04,-0.8133115746092999E-05, 0.9487937905151000E-06, 0.9425886899970000E-06 - 12, 3, 0.1968689212900000E-04, 0.3232027118616000E-03, 0.9116566223632000E-06, 0.9115865656016000E-06 - 12, 4, 0.5519906336898000E-05, 0.3168978295415000E-04, 0.8814394098455000E-06, 0.8793077431204000E-06 - 12, 5, 0.1583459799570000E-04,-0.8613105726257000E-04, 0.7872936439127000E-06, 0.7803271173230000E-06 - 12, 6, 0.2796569666407000E-04, 0.3473000020080000E-04, 0.6563795874239000E-06, 0.6557274539630000E-06 - 12, 7,-0.3017024429305000E-04,-0.5888288265104000E-04, 0.5126973904473000E-06, 0.5209144646646000E-06 - 12, 8, 0.2552418024338000E-04,-0.1743567879912000E-05, 0.3710269705078000E-06, 0.3747130362368000E-06 - 12, 9,-0.9379242865317001E-04, 0.1651321065951000E-04, 0.2443392741925000E-06, 0.2467174054561000E-06 - 12, 10, 0.1487383317218000E-06, 0.6673527538838000E-04, 0.2989437458884000E-06, 0.3019876064179000E-06 - 12, 11, 0.5403696279192000E-04, 0.1140975010267000E-04, 0.5234622222010000E-06, 0.5213171069242000E-06 - 12, 12, 0.1475341246490000E-04, 0.1882899338475000E-03, 0.8069360487512000E-06, 0.8107134948502000E-06 - 13, 0, 0.9259459394742000E-04, 0.0000000000000000E+00, 0.1675834463799000E-05, 0.0000000000000000E+00 - 13, 1,-0.3608398602972000E-06, 0.3445045978470000E-05, 0.1685253057109000E-05, 0.1671188244637000E-05 - 13, 2,-0.1573288736973000E-04,-0.1269667133391000E-03, 0.1656108121526000E-05, 0.1651026598585000E-05 - 13, 3, 0.9768852063835000E-04,-0.3060531995128000E-04, 0.1626215935904000E-05, 0.1625317316602000E-05 - 13, 4,-0.1141952901629000E-03, 0.9451620737220001E-04, 0.1549870860119000E-05, 0.1538854553468000E-05 - 13, 5, 0.2230467561032000E-05, 0.6990321437689000E-04, 0.1465529003193000E-05, 0.1465386233332000E-05 - 13, 6, 0.6851295045759000E-04,-0.6928312178615000E-05, 0.1225986500079000E-05, 0.1224221576121000E-05 - 13, 7, 0.2130190568361000E-04,-0.1203746101056000E-04, 0.1020864212755000E-05, 0.1020574763702000E-05 - 13, 8, 0.1190960598695000E-04,-0.1990298917062000E-04, 0.7635066100847000E-06, 0.7740112111670000E-06 - 13, 9,-0.6201692235768000E-04,-0.1139401751714000E-03, 0.5213322915285000E-06, 0.5206755451049000E-06 - 13, 10, 0.5484724909694000E-04,-0.4592082943415000E-04, 0.4342431225637000E-06, 0.4384137581013000E-06 - 13, 11,-0.7696422384537999E-04,-0.2334568581412000E-04, 0.5038942178800000E-06, 0.4922997891523000E-06 - 13, 12, 0.5420089349594000E-04,-0.7024876683824000E-04, 0.9993286717422000E-06, 0.1000233411396000E-05 - 13, 13, 0.4574839949866000E-04,-0.1579213041997000E-04, 0.1448379290343000E-05, 0.1438194327580000E-05 - 14, 0,-0.1334897844190000E-04, 0.0000000000000000E+00, 0.2956554609990000E-05, 0.0000000000000000E+00 - 14, 1, 0.5072493886225000E-04, 0.9540437244880000E-04, 0.2948805512044000E-05, 0.2931416000331000E-05 - 14, 2, 0.2342889378930000E-04,-0.2315079084522000E-05, 0.2927078382678000E-05, 0.2912799858573000E-05 - 14, 3,-0.7235482861171000E-04,-0.1400685046897000E-03, 0.2851303971730000E-05, 0.2850478701508000E-05 - 14, 4, 0.2083606120438000E-05,-0.3545090437847000E-04, 0.2764344904826000E-05, 0.2751550786632000E-05 - 14, 5, 0.1502521743679000E-04, 0.5284464737184000E-04, 0.2593156622994000E-05, 0.2579140145464000E-05 - 14, 6,-0.9313617176051000E-04, 0.1080348342430000E-04, 0.2290354733504000E-05, 0.2279348538251000E-05 - 14, 7, 0.2351129897923000E-04, 0.6682576112325000E-04, 0.1938517848819000E-05, 0.1944730476349000E-05 - 14, 8, 0.1665223348059000E-04,-0.2171693008017000E-04, 0.1506643978827000E-05, 0.1511326688536000E-05 - 14, 9, 0.1191018786524000E-03, 0.5942470254035000E-05, 0.1046575144728000E-05, 0.1032415479722000E-05 - 14, 10,-0.5252789878909000E-04,-0.2899990103804000E-04, 0.7146856291094000E-06, 0.7385361764380000E-06 - 14, 11,-0.2899095001850000E-05,-0.2377952065045000E-04, 0.7427374592681000E-06, 0.7312983128646000E-06 - 14, 12, 0.2172992053561000E-04,-0.4760520574786000E-04, 0.1133658884765000E-05, 0.1127823537370000E-05 - 14, 13,-0.3967707603866000E-05,-0.1267860627795000E-04, 0.1857262701915000E-05, 0.1881300083507000E-05 - 14, 14, 0.6283833854884000E-04, 0.4792499337020000E-04, 0.2558155419738000E-05, 0.2548227558988000E-05 - 15, 0,-0.5260069338292000E-04, 0.0000000000000000E+00, 0.5149281061654000E-05, 0.0000000000000000E+00 - 15, 1,-0.5095143395724000E-05,-0.9267144192046000E-05, 0.5176487394908000E-05, 0.5128218314358000E-05 - 15, 2,-0.7088105506762000E-05, 0.1903112606301000E-04, 0.5088817583093000E-05, 0.5072616470509000E-05 - 15, 3, 0.5118549630622000E-05, 0.2550871106670000E-04, 0.5016381740107000E-05, 0.5010874285110000E-05 - 15, 4, 0.5628011061765000E-04,-0.4565616114654000E-04, 0.4830630475050000E-05, 0.4809636366702000E-05 - 15, 5,-0.1687993871127000E-04,-0.8387188137865001E-04, 0.4566857571604000E-05, 0.4570954994243000E-05 - 15, 6,-0.1941362485319000E-04,-0.3883109179571000E-04, 0.4086225687272000E-05, 0.4076040358346000E-05 - 15, 7,-0.4107194609241000E-04, 0.4270213629677000E-04, 0.3595112808470000E-05, 0.3614211421827000E-05 - 15, 8,-0.4929911947256000E-04, 0.5064113400309000E-04, 0.2927629245482000E-05, 0.2987754388848000E-05 - 15, 9, 0.2894783184979000E-04, 0.1020861091025000E-03, 0.2277597916162000E-05, 0.2264217551775000E-05 - 15, 10, 0.3588547880965000E-04,-0.7380703195300000E-05, 0.1568716636200000E-05, 0.1551260815551000E-05 - 15, 11, 0.2590727603140000E-04, 0.9732089741598001E-05, 0.1025884163119000E-05, 0.9625615229491001E-06 - 15, 12,-0.3302897569891000E-04, 0.8076142002464001E-05, 0.1286227547559000E-05, 0.1320121950725000E-05 - 15, 13,-0.5139258681996000E-04, 0.1098493603363000E-05, 0.2197159473372000E-05, 0.2172669236626000E-05 - 15, 14, 0.4576313922048000E-05,-0.5532553090423000E-05, 0.3365894046703000E-05, 0.3378802661325000E-05 - 15, 15,-0.4254079164137000E-05, 0.1606688012032000E-04, 0.4444933317511000E-05, 0.4471902703240000E-05 - 16, 0, 0.3046956879117000E-04, 0.0000000000000000E+00, 0.8808673728548000E-05, 0.0000000000000000E+00 - 16, 1,-0.1883375125815000E-04,-0.4207246068110000E-04, 0.8822133417483000E-05, 0.8765895112781000E-05 - 16, 2, 0.1605789876777000E-04, 0.3092763770136000E-04, 0.8752211073563000E-05, 0.8703754613291001E-05 - 16, 3, 0.8736317198244000E-04, 0.3066252662352000E-04, 0.8531443154161000E-05, 0.8569763814165000E-05 - 16, 4,-0.2764941799048000E-04, 0.2491397016687000E-04, 0.8191048767561000E-05, 0.8286807254971001E-05 - 16, 5,-0.1035013719932000E-04,-0.6518086116344000E-05, 0.7608239488615000E-05, 0.7549953331386000E-05 - 16, 6, 0.9021746789978000E-04,-0.2384296191283000E-04, 0.7032318629895000E-05, 0.6979181519016000E-05 - 16, 7, 0.1386719585299000E-05,-0.8375695304106000E-04, 0.6277014101456000E-05, 0.6378903527382000E-05 - 16, 8, 0.7519033361517000E-05,-0.5484343110228000E-05, 0.5248710916417000E-05, 0.5346567685261000E-05 - 16, 9,-0.1172687826145000E-03, 0.1077805039274000E-04, 0.4562772906758000E-05, 0.4600347020217000E-05 - 16, 10, 0.5004259911752000E-04,-0.9705622391234001E-05, 0.3401488861093000E-05, 0.3450176706691000E-05 - 16, 11, 0.2521431488435000E-04, 0.3635405773330000E-04, 0.2004346236834000E-05, 0.2015759047803000E-05 - 16, 12, 0.2102067953062000E-04,-0.1095248158506000E-04, 0.1895235045496000E-05, 0.1914740301174000E-05 - 16, 13,-0.1686369430648000E-04,-0.7951986125731000E-05, 0.2456864615099000E-05, 0.2442546031747000E-05 - 16, 14, 0.4603833639406000E-04, 0.4285383392351000E-05, 0.4042506350628000E-05, 0.3870166215432000E-05 - 16, 15,-0.2560236419130000E-04,-0.5287761273671000E-04, 0.5944848967209000E-05, 0.5956568619667000E-05 - 16, 16, 0.6006355659974000E-04, 0.8409456202797000E-05, 0.7780316449162000E-05, 0.7791495546992999E-05 - 17, 0, 0.7228611948272000E-05, 0.0000000000000000E+00, 0.1466325214127000E-04, 0.0000000000000000E+00 - 17, 1,-0.3593076900227000E-05,-0.5149118915071000E-05, 0.1477793698461000E-04, 0.1471121584327000E-04 - 17, 2,-0.1050719791630000E-04, 0.3951958024733000E-04, 0.1442947155154000E-04, 0.1442693996626000E-04 - 17, 3,-0.2124656841927000E-04, 0.8902841927279999E-05, 0.1427887898031000E-04, 0.1432947718700000E-04 - 17, 4, 0.1668065069862000E-05, 0.5816130661953000E-05, 0.1306344632825000E-04, 0.1318349254533000E-04 - 17, 5, 0.5525957697550000E-04, 0.2673649975671000E-04, 0.1223303334329000E-04, 0.1238222615786000E-04 - 17, 6,-0.2951532466566000E-04, 0.3509691162150000E-04, 0.1072305277119000E-04, 0.1039640668171000E-04 - 17, 7,-0.5097818123646000E-05,-0.3937730437467000E-04, 0.1000150363555000E-04, 0.9819512591697001E-05 - 17, 8, 0.4711543821045000E-04,-0.5609779881255000E-04, 0.8787471436200000E-05, 0.8235326243986001E-05 - 17, 9, 0.1662655300064000E-04,-0.5546691396174000E-04, 0.7627301066859000E-05, 0.7754374672671000E-05 - 17, 10,-0.9145153590949001E-04, 0.2708932885136000E-05, 0.6072286029191000E-05, 0.6402585049475000E-05 - 17, 11,-0.6947963895761000E-05,-0.3835740842312000E-04, 0.3952050299883000E-05, 0.3887151223698000E-05 - 17, 12, 0.2160212389868000E-04, 0.3129234877212000E-04, 0.2697808799740000E-05, 0.2691312394710000E-05 - 17, 13,-0.7015498163764000E-05, 0.2342565581658000E-04, 0.3077929914948000E-05, 0.3038348946208000E-05 - 17, 14,-0.1272893996900000E-04, 0.6184136500831000E-05, 0.3702270290722000E-05, 0.3708858336546000E-05 - 17, 15, 0.2090977148901000E-04, 0.1822431945459000E-04, 0.5361594486237000E-05, 0.5294005727669000E-05 - 17, 16,-0.4240232449617000E-04, 0.8629365920897000E-05, 0.9501455203208001E-05, 0.9438136971940001E-05 - 17, 17,-0.3643565384944000E-04, 0.6520296594766000E-05, 0.1334216880090000E-04, 0.1333688666348000E-04 - 18, 0,-0.3160782209377000E-07, 0.0000000000000000E+00, 0.2260977872535000E-04, 0.0000000000000000E+00 - 18, 1, 0.2409651745319000E-04, 0.4121041879383000E-05, 0.2280866664993000E-04, 0.2272723416772000E-04 - 18, 2,-0.3808754272417000E-04,-0.3759900096554000E-04, 0.2259711393939000E-04, 0.2225581732661000E-04 - 18, 3,-0.7488753746930000E-04, 0.4218801235027000E-04, 0.2176524328623000E-04, 0.2205438826418000E-04 - 18, 4,-0.4135160275974000E-05,-0.1769748496366000E-04, 0.1984973691348000E-04, 0.2064302918333000E-04 - 18, 5, 0.7904894860510000E-05,-0.1542899865191000E-04, 0.1663807061359000E-04, 0.1625612752099000E-04 - 18, 6,-0.2422514222424000E-04, 0.2685767019333000E-04, 0.1494154344689000E-04, 0.1558908406040000E-04 - 18, 7,-0.8953889673616999E-05, 0.5854099449034000E-04, 0.1228130995964000E-04, 0.1432567421442000E-04 - 18, 8,-0.3633790787860000E-04, 0.2055611259521000E-05, 0.1315580755139000E-04, 0.1220107580950000E-04 - 18, 9, 0.9398367877424001E-04, 0.2052805277025000E-05, 0.1195509716579000E-04, 0.1160480025077000E-04 - 18, 10, 0.8190744450733001E-05, 0.2611438252972000E-04, 0.1040135529275000E-04, 0.1070471089677000E-04 - 18, 11,-0.4873641218297000E-04,-0.2162546185657000E-04, 0.7302821453813000E-05, 0.7452061890969000E-05 - 18, 12,-0.2047938878228000E-04, 0.2009975587210000E-04, 0.5516901593768000E-05, 0.5266543920135000E-05 - 18, 13, 0.3018793586668000E-04,-0.3470192884471000E-05, 0.4312644277629000E-05, 0.4360829397385000E-05 - 18, 14,-0.3848675701936000E-04, 0.1470831747337000E-04, 0.5344914401045000E-05, 0.5198724329634000E-05 - 18, 15,-0.1979946155327000E-05, 0.1122473143992000E-04, 0.6315967671598000E-05, 0.6450432942650000E-05 - 18, 16,-0.1885315336478000E-04,-0.1293548895567000E-04, 0.8678380592171000E-05, 0.8799575773742000E-05 - 18, 17,-0.7205596374592000E-04, 0.3608474820689000E-05, 0.1597207784077000E-04, 0.1592027674407000E-04 - 18, 18, 0.3982192140283000E-04, 0.2957485517613000E-04, 0.2195166800847000E-04, 0.2212587243447000E-04 - 19, 0, 0.3470430941111000E-05, 0.0000000000000000E+00, 0.3080365365673000E-04, 0.0000000000000000E+00 - 19, 1,-0.5879472131305000E-04, 0.6585008411786000E-05, 0.3189540826669000E-04, 0.3207462114713000E-04 - 19, 2,-0.1729280690223000E-04,-0.2875625667961000E-04, 0.3028711508528000E-04, 0.3069436862850000E-04 - 19, 3, 0.4435491591431000E-04,-0.3914732396766000E-04, 0.3007122264554000E-04, 0.3061609623919000E-04 - 19, 4, 0.8260803766682999E-05, 0.1940210561134000E-04, 0.2378892055173000E-04, 0.2586121857569000E-04 - 19, 5,-0.6925742481018000E-04, 0.1537899824957000E-04, 0.2245843333693000E-04, 0.2313845462821000E-04 - 19, 6, 0.3915424683957000E-04,-0.1600358262850000E-04, 0.1793247790200000E-04, 0.1726609454478000E-04 - 19, 7, 0.5230575221981000E-04,-0.1693116407063000E-04, 0.1817244648354000E-04, 0.1587114334964000E-04 - 19, 8, 0.1844213559037000E-04, 0.7605192912942000E-04, 0.1599046763642000E-04, 0.1327472682617000E-04 - 19, 9,-0.4379012193234000E-04, 0.2496677377452000E-04, 0.1575363444045000E-04, 0.1612865458540000E-04 - 19, 10, 0.7227136121218000E-04,-0.2148860835234000E-04, 0.1381865675192000E-04, 0.1486100537521000E-04 - 19, 11, 0.1231988693355000E-04, 0.3692210176484000E-04, 0.1120842514954000E-04, 0.1051847104171000E-04 - 19, 12,-0.3459643076408000E-04,-0.5223947817594000E-04, 0.8855183783393000E-05, 0.9516470440585000E-05 - 19, 13,-0.1135311672491000E-04,-0.6661868058050000E-05, 0.8182348639738001E-05, 0.7991005892320999E-05 - 19, 14, 0.1155968735254000E-04, 0.1087670675828000E-04, 0.6330671010239000E-05, 0.6321612341589000E-05 - 19, 15,-0.5466708498805000E-04,-0.1390683158823000E-04, 0.5682273259762000E-05, 0.5736511884007000E-05 - 19, 16,-0.1763641171649000E-04,-0.5939128837081000E-04, 0.5351239750733000E-05, 0.5393122419057000E-05 - 19, 17, 0.1966502379529000E-04, 0.1187410520918000E-04, 0.6176469017133000E-05, 0.5897690057932000E-05 - 19, 18, 0.1772902186204000E-04,-0.2608006957300000E-04, 0.1973118833805000E-04, 0.1989391643215000E-04 - 19, 19, 0.1355622725295000E-04,-0.4230550917980000E-04, 0.3347609687528000E-04, 0.3336868665510000E-04 - 20, 0, 0.7386896112898000E-05, 0.0000000000000000E+00, 0.4118025018976000E-04, 0.0000000000000000E+00 - 20, 1, 0.1489488576035000E-04, 0.3116265664847000E-04, 0.4101880017487000E-04, 0.4103037303111000E-04 - 20, 2, 0.6305260866365000E-05,-0.3289478383933000E-04, 0.4045437064402000E-04, 0.3988124515721000E-04 - 20, 3, 0.3566854887036000E-04,-0.1956035123336000E-04, 0.3585817853011000E-04, 0.3843835203853000E-04 - 20, 4, 0.3858918894304000E-04,-0.7071105999335000E-05, 0.3247940299520000E-04, 0.3376435705370000E-04 - 20, 5,-0.2771721970696000E-04, 0.1452638788154000E-04, 0.1940374991189000E-04, 0.2218374652586000E-04 - 20, 6,-0.2813798007217000E-04,-0.5883220909142000E-05, 0.1845153254530000E-04, 0.2107762772012000E-04 - 20, 7,-0.2934150616021000E-04, 0.4509438874294000E-04, 0.1748128417435000E-04, 0.1704929617488000E-04 - 20, 8,-0.1730082329169000E-04, 0.1687440171137000E-04, 0.1895340193191000E-04, 0.1686989283473000E-04 - 20, 9,-0.3348167526029000E-04,-0.2954324249325000E-05, 0.1880941464915000E-04, 0.1589037641301000E-04 - 20, 10,-0.4083866024967000E-04,-0.3003711050255000E-04, 0.1754530015749000E-04, 0.1815941814696000E-04 - 20, 11, 0.7194854690190000E-05, 0.8568282976047000E-05, 0.1657899243127000E-04, 0.1725108360911000E-04 - 20, 12, 0.5762851519291000E-04,-0.3092761620944000E-04, 0.1494299201885000E-04, 0.1490694994232000E-04 - 20, 13,-0.2434771829708000E-04, 0.3671493352070000E-05, 0.1642687699811000E-04, 0.1596799830780000E-04 - 20, 14, 0.4507333871400000E-04,-0.5651367745112000E-05, 0.1239979499244000E-04, 0.1131653017760000E-04 - 20, 15, 0.1201898989702000E-05,-0.5126681779977000E-04, 0.8850735741950999E-05, 0.8985959189873001E-05 - 20, 16, 0.4889374790886000E-04, 0.5857290944036000E-04, 0.7224734655724000E-05, 0.7338451026286000E-05 - 20, 17, 0.1746022628164000E-04,-0.1000122868416000E-04, 0.7191645731357000E-05, 0.7329706561426000E-05 - 20, 18,-0.1181227062961000E-03,-0.9287021873343999E-04, 0.1370605158564000E-04, 0.1350009884750000E-04 - 20, 19,-0.1053877986498000E-04,-0.3477223774870000E-05, 0.3558362257674000E-04, 0.3570240752164000E-04 - 20, 20,-0.2732624476807000E-04, 0.2571662256856000E-04, 0.4558412222904000E-04, 0.4579253893744000E-04 \ No newline at end of file + 1, 0, 0.0000000000000000E+00, 0.0000000000000000E+00, 0.0000000000000000E+00, 0.0000000000000000E+00 + 1, 1, 0.0000000000000000E+00, 0.0000000000000000E+00, 0.0000000000000000E+00, 0.0000000000000000E+00 + 2, 0,-0.3177939699038000E-01, 0.0000000000000000E+00, 0.6302192052110000E-08, 0.0000000000000000E+00 + 2, 1,-0.4939139723693000E-09, 0.1596048836604000E-08, 0.1425606048467000E-08, 0.1426922421478000E-08 + 2, 2, 0.4184962374624000E-02, 0.1245378620599000E-02, 0.1151298529854000E-06, 0.2729943583164000E-07 + 3, 0, 0.3310552979215000E-02, 0.0000000000000000E+00, 0.6278614117139000E-08, 0.0000000000000000E+00 + 3, 1,-0.2612671052339000E-02,-0.4338914699986000E-03, 0.2345873359128000E-07, 0.2800174652735000E-07 + 3, 2,-0.7288907471268000E-03,-0.1173043933906000E-02, 0.3325351203588000E-07, 0.1825063066920000E-07 + 3, 3,-0.1546624776698000E-03, 0.2384935863669000E-02, 0.6975468578578000E-08, 0.9683021844873000E-07 + 4, 0, 0.3265599899336000E-02, 0.0000000000000000E+00, 0.1075763269629000E-07, 0.0000000000000000E+00 + 4, 1, 0.4920396871261000E-03, 0.1434888318128000E-03, 0.1090406142407000E-07, 0.1072569551038000E-07 + 4, 2,-0.6300987993766000E-03, 0.2630330871648000E-03, 0.1311457406587000E-07, 0.1578358910614000E-07 + 4, 3,-0.1047527508626000E-03,-0.6699821437764000E-03, 0.5787576847822000E-08, 0.2753437610584000E-07 + 4, 4, 0.8663528462123000E-04,-0.2970368903764000E-03, 0.1304661882953000E-07, 0.1239968008881000E-07 + 5, 0,-0.1202182459510000E-02, 0.0000000000000000E+00, 0.1802716595738000E-07, 0.0000000000000000E+00 + 5, 1, 0.7502015580788000E-03,-0.2508023828499000E-03, 0.1788324831114000E-07, 0.2049493723690000E-07 + 5, 2, 0.2033043303707000E-03, 0.6147958809249000E-04, 0.1470281777411000E-07, 0.1387145727637000E-07 + 5, 3, 0.6626676527554000E-03,-0.4370521656036000E-03, 0.2843998251066000E-07, 0.1948148364007000E-07 + 5, 4,-0.2307769552926000E-04,-0.6047104715904000E-03, 0.1578862863954000E-07, 0.2936925864161000E-07 + 5, 5, 0.6450599419048001E-03, 0.2556270919853000E-03, 0.1409282686729000E-07, 0.4802062536113000E-07 + 6, 0,-0.9971479248165999E-04, 0.0000000000000000E+00, 0.3192348805227000E-07, 0.0000000000000000E+00 + 6, 1,-0.3689637507360000E-03,-0.5367043709220000E-03, 0.3266674605564000E-07, 0.3108051163028000E-07 + 6, 2,-0.2874702927558000E-04, 0.4269129512184000E-04, 0.2888626729058000E-07, 0.2893107119377000E-07 + 6, 3,-0.1930585495620000E-04, 0.1836955099421000E-03, 0.2328896053105000E-07, 0.2429799856217000E-07 + 6, 4,-0.7825617515562001E-04, 0.4575356542208000E-04, 0.1727940775550000E-07, 0.1610483931025000E-07 + 6, 5, 0.2967358835437000E-03,-0.1742565712364000E-03, 0.2086830714082000E-07, 0.1261821207676000E-07 + 6, 6, 0.4938771693044000E-03, 0.4102369776849000E-03, 0.4093432212711000E-07, 0.4622961385480000E-07 + 7, 0, 0.1846938617921000E-03, 0.0000000000000000E+00, 0.5554717870363000E-07, 0.0000000000000000E+00 + 7, 1, 0.1069887372241000E-03, 0.7947893456014000E-03, 0.5668378042444000E-07, 0.5547605969559000E-07 + 7, 2, 0.3986149124117000E-03, 0.1994042422627000E-03, 0.5393656913811000E-07, 0.5277643685540000E-07 + 7, 3,-0.3605183252112000E-03,-0.5651392573033000E-04, 0.4915735164794000E-07, 0.4737103403303000E-07 + 7, 4, 0.3513479329314000E-04, 0.3445571070230000E-03, 0.3633666848066000E-07, 0.3983518758884000E-07 + 7, 5,-0.1874037699812000E-04,-0.1799836022671000E-03, 0.2549356248568000E-07, 0.2360802305471000E-07 + 7, 6, 0.1663646306986000E-05,-0.1220426815628000E-03, 0.1372713585145000E-07, 0.9795292270609001E-08 + 7, 7, 0.4089854666183000E-03, 0.1072749359836000E-03, 0.5130009156280000E-07, 0.5167446259437000E-07 + 8, 0,-0.8717683328542000E-04, 0.0000000000000000E+00, 0.9888907580048000E-07, 0.0000000000000000E+00 + 8, 1, 0.9922336158901000E-04,-0.5133018502066000E-05, 0.9794905351003000E-07, 0.9715537239055999E-07 + 8, 2,-0.6780160184482000E-04,-0.1505492250505000E-03, 0.9689581378651000E-07, 0.9657340203177000E-07 + 8, 3,-0.2665429398226000E-04, 0.2627075165168000E-03, 0.8737656928705000E-07, 0.8818828611603000E-07 + 8, 4,-0.1218869905423000E-03,-0.1661918078467000E-03, 0.7099218985919000E-07, 0.7239278442905000E-07 + 8, 5, 0.1149373505558000E-03,-0.7661327837606000E-04, 0.4809343054921000E-07, 0.4766221315026000E-07 + 8, 6,-0.3681758927829000E-03, 0.1327099040798000E-03, 0.2633585241154000E-07, 0.3854860034084000E-07 + 8, 7, 0.1428100360275000E-03,-0.6924088904599999E-04, 0.3325412794169000E-07, 0.3721396056742000E-07 + 8, 8, 0.8734476308255000E-04, 0.6889241489250000E-04, 0.7907879277156000E-07, 0.7791933392995000E-07 + 9, 0,-0.1919071091121000E-05, 0.0000000000000000E+00, 0.1726021226829000E-06, 0.0000000000000000E+00 + 9, 1,-0.1684323129119000E-03,-0.3380383290444000E-03, 0.1743617506337000E-06, 0.1726925848351000E-06 + 9, 2,-0.2538070436043000E-03,-0.1662129993008000E-03, 0.1697354358211000E-06, 0.1689523215515000E-06 + 9, 3, 0.3192919445457000E-03, 0.5247336331426000E-04, 0.1633291739201000E-06, 0.1628043939440000E-06 + 9, 4,-0.1092751280911000E-03,-0.6250644937545999E-04, 0.1376361483741000E-06, 0.1365255023555000E-06 + 9, 5,-0.4567350098798000E-05, 0.4449563889084000E-04, 0.1001279285625000E-06, 0.9989939226078000E-07 + 9, 6,-0.1927959999225000E-04,-0.6920806781393000E-04, 0.6040134002821000E-07, 0.5975595927013000E-07 + 9, 7,-0.1157531676444000E-03, 0.1846761157069000E-03, 0.4483825713473000E-07, 0.4665266157517000E-07 + 9, 8, 0.8384597764907000E-04, 0.1718410576234000E-03, 0.8090994802024000E-07, 0.7930020726512000E-07 + 9, 9, 0.5514112603942000E-04, 0.9043209949504001E-04, 0.1422279362527000E-06, 0.1428514730291000E-06 + 10, 0, 0.1377592596314000E-03, 0.0000000000000000E+00, 0.3085840497077000E-06, 0.0000000000000000E+00 + 10, 1, 0.9126248158335000E-04, 0.1551484066890000E-03, 0.3053455136806000E-06, 0.3032850221516000E-06 + 10, 2, 0.5022355299758000E-04, 0.6056673571061000E-04, 0.3034967037228000E-06, 0.3019737879639000E-06 + 10, 3, 0.1277546359626000E-04,-0.4062472101134000E-03, 0.2891411493668000E-06, 0.2886923077856000E-06 + 10, 4, 0.3841144635364000E-04, 0.4590948302321000E-04, 0.2631015053606000E-06, 0.2622208074524000E-06 + 10, 5,-0.1052485816696000E-03, 0.8495297065855001E-04, 0.2147893956140000E-06, 0.2150479328492000E-06 + 10, 6, 0.1478466927885000E-03,-0.1141852669825000E-03, 0.1252069338070000E-06, 0.1260209810635000E-06 + 10, 7, 0.8560694501398999E-05, 0.6962138970757000E-04, 0.8809544264141001E-07, 0.8487883896014001E-07 + 10, 8,-0.8534847154915000E-04,-0.1500658841920000E-05, 0.8249942876106000E-07, 0.8153120633312000E-07 + 10, 9, 0.1333765643829000E-03,-0.1700779740113000E-03, 0.1610431260684000E-06, 0.1629232665889000E-06 + 10, 10, 0.1816132626089000E-03,-0.7144286032944000E-04, 0.2558803998708000E-06, 0.2544847432919000E-06 + 11, 0,-0.5795945175749000E-04, 0.0000000000000000E+00, 0.5377556483849000E-06, 0.0000000000000000E+00 + 11, 1, 0.7001648560266000E-04, 0.7784675627781000E-04, 0.5434626022748000E-06, 0.5382166609197000E-06 + 11, 2, 0.1185251090365000E-03, 0.1541644383569000E-03, 0.5321918766358000E-06, 0.5293645199229000E-06 + 11, 3,-0.2297041199361000E-03,-0.4285481675882000E-05, 0.5202141574453000E-06, 0.5194747371489000E-06 + 11, 4, 0.8179980677869000E-04,-0.1137407133655000E-03, 0.4787111665770000E-06, 0.4777996114587000E-06 + 11, 5,-0.2861097125671000E-04,-0.2844235270080000E-04, 0.4248152974238000E-06, 0.4259236882684000E-06 + 11, 6,-0.3328988151364000E-04, 0.9489368589853000E-04, 0.3335066671863000E-06, 0.3347047092450000E-06 + 11, 7, 0.5036476606530000E-04,-0.6179930805018999E-04, 0.2286146172811000E-06, 0.2200937827458000E-06 + 11, 8,-0.2502747990421000E-04,-0.3617888947046000E-04, 0.1365872476952000E-06, 0.1368337002465000E-06 + 11, 9, 0.4338880751807000E-04, 0.8926665393751000E-04, 0.1538828673172000E-06, 0.1547755980588000E-06 + 11, 10,-0.5884646095355000E-05, 0.1303453862551000E-03, 0.3005764813772000E-06, 0.3011744618621000E-06 + 11, 11, 0.1661472599910000E-03, 0.1728729148920000E-04, 0.4500564771040000E-06, 0.4506915969995000E-06 + 12, 0,-0.8421914774262999E-04, 0.0000000000000000E+00, 0.9590113082374000E-06, 0.0000000000000000E+00 + 12, 1,-0.9803753731618000E-04,-0.1421977755211000E-03, 0.9516458379933000E-06, 0.9455269893516000E-06 + 12, 2,-0.5698123826017000E-04,-0.8133115746092999E-05, 0.9487937905151000E-06, 0.9425886899970000E-06 + 12, 3, 0.1968689212900000E-04, 0.3232027118616000E-03, 0.9116566223632000E-06, 0.9115865656016000E-06 + 12, 4, 0.5519906336898000E-05, 0.3168978295415000E-04, 0.8814394098455000E-06, 0.8793077431204000E-06 + 12, 5, 0.1583459799570000E-04,-0.8613105726257000E-04, 0.7872936439127000E-06, 0.7803271173230000E-06 + 12, 6, 0.2796569666407000E-04, 0.3473000020080000E-04, 0.6563795874239000E-06, 0.6557274539630000E-06 + 12, 7,-0.3017024429305000E-04,-0.5888288265104000E-04, 0.5126973904473000E-06, 0.5209144646646000E-06 + 12, 8, 0.2552418024338000E-04,-0.1743567879912000E-05, 0.3710269705078000E-06, 0.3747130362368000E-06 + 12, 9,-0.9379242865317001E-04, 0.1651321065951000E-04, 0.2443392741925000E-06, 0.2467174054561000E-06 + 12, 10, 0.1487383317218000E-06, 0.6673527538838000E-04, 0.2989437458884000E-06, 0.3019876064179000E-06 + 12, 11, 0.5403696279192000E-04, 0.1140975010267000E-04, 0.5234622222010000E-06, 0.5213171069242000E-06 + 12, 12, 0.1475341246490000E-04, 0.1882899338475000E-03, 0.8069360487512000E-06, 0.8107134948502000E-06 + 13, 0, 0.9259459394742000E-04, 0.0000000000000000E+00, 0.1675834463799000E-05, 0.0000000000000000E+00 + 13, 1,-0.3608398602972000E-06, 0.3445045978470000E-05, 0.1685253057109000E-05, 0.1671188244637000E-05 + 13, 2,-0.1573288736973000E-04,-0.1269667133391000E-03, 0.1656108121526000E-05, 0.1651026598585000E-05 + 13, 3, 0.9768852063835000E-04,-0.3060531995128000E-04, 0.1626215935904000E-05, 0.1625317316602000E-05 + 13, 4,-0.1141952901629000E-03, 0.9451620737220001E-04, 0.1549870860119000E-05, 0.1538854553468000E-05 + 13, 5, 0.2230467561032000E-05, 0.6990321437689000E-04, 0.1465529003193000E-05, 0.1465386233332000E-05 + 13, 6, 0.6851295045759000E-04,-0.6928312178615000E-05, 0.1225986500079000E-05, 0.1224221576121000E-05 + 13, 7, 0.2130190568361000E-04,-0.1203746101056000E-04, 0.1020864212755000E-05, 0.1020574763702000E-05 + 13, 8, 0.1190960598695000E-04,-0.1990298917062000E-04, 0.7635066100847000E-06, 0.7740112111670000E-06 + 13, 9,-0.6201692235768000E-04,-0.1139401751714000E-03, 0.5213322915285000E-06, 0.5206755451049000E-06 + 13, 10, 0.5484724909694000E-04,-0.4592082943415000E-04, 0.4342431225637000E-06, 0.4384137581013000E-06 + 13, 11,-0.7696422384537999E-04,-0.2334568581412000E-04, 0.5038942178800000E-06, 0.4922997891523000E-06 + 13, 12, 0.5420089349594000E-04,-0.7024876683824000E-04, 0.9993286717422000E-06, 0.1000233411396000E-05 + 13, 13, 0.4574839949866000E-04,-0.1579213041997000E-04, 0.1448379290343000E-05, 0.1438194327580000E-05 + 14, 0,-0.1334897844190000E-04, 0.0000000000000000E+00, 0.2956554609990000E-05, 0.0000000000000000E+00 + 14, 1, 0.5072493886225000E-04, 0.9540437244880000E-04, 0.2948805512044000E-05, 0.2931416000331000E-05 + 14, 2, 0.2342889378930000E-04,-0.2315079084522000E-05, 0.2927078382678000E-05, 0.2912799858573000E-05 + 14, 3,-0.7235482861171000E-04,-0.1400685046897000E-03, 0.2851303971730000E-05, 0.2850478701508000E-05 + 14, 4, 0.2083606120438000E-05,-0.3545090437847000E-04, 0.2764344904826000E-05, 0.2751550786632000E-05 + 14, 5, 0.1502521743679000E-04, 0.5284464737184000E-04, 0.2593156622994000E-05, 0.2579140145464000E-05 + 14, 6,-0.9313617176051000E-04, 0.1080348342430000E-04, 0.2290354733504000E-05, 0.2279348538251000E-05 + 14, 7, 0.2351129897923000E-04, 0.6682576112325000E-04, 0.1938517848819000E-05, 0.1944730476349000E-05 + 14, 8, 0.1665223348059000E-04,-0.2171693008017000E-04, 0.1506643978827000E-05, 0.1511326688536000E-05 + 14, 9, 0.1191018786524000E-03, 0.5942470254035000E-05, 0.1046575144728000E-05, 0.1032415479722000E-05 + 14, 10,-0.5252789878909000E-04,-0.2899990103804000E-04, 0.7146856291094000E-06, 0.7385361764380000E-06 + 14, 11,-0.2899095001850000E-05,-0.2377952065045000E-04, 0.7427374592681000E-06, 0.7312983128646000E-06 + 14, 12, 0.2172992053561000E-04,-0.4760520574786000E-04, 0.1133658884765000E-05, 0.1127823537370000E-05 + 14, 13,-0.3967707603866000E-05,-0.1267860627795000E-04, 0.1857262701915000E-05, 0.1881300083507000E-05 + 14, 14, 0.6283833854884000E-04, 0.4792499337020000E-04, 0.2558155419738000E-05, 0.2548227558988000E-05 + 15, 0,-0.5260069338292000E-04, 0.0000000000000000E+00, 0.5149281061654000E-05, 0.0000000000000000E+00 + 15, 1,-0.5095143395724000E-05,-0.9267144192046000E-05, 0.5176487394908000E-05, 0.5128218314358000E-05 + 15, 2,-0.7088105506762000E-05, 0.1903112606301000E-04, 0.5088817583093000E-05, 0.5072616470509000E-05 + 15, 3, 0.5118549630622000E-05, 0.2550871106670000E-04, 0.5016381740107000E-05, 0.5010874285110000E-05 + 15, 4, 0.5628011061765000E-04,-0.4565616114654000E-04, 0.4830630475050000E-05, 0.4809636366702000E-05 + 15, 5,-0.1687993871127000E-04,-0.8387188137865001E-04, 0.4566857571604000E-05, 0.4570954994243000E-05 + 15, 6,-0.1941362485319000E-04,-0.3883109179571000E-04, 0.4086225687272000E-05, 0.4076040358346000E-05 + 15, 7,-0.4107194609241000E-04, 0.4270213629677000E-04, 0.3595112808470000E-05, 0.3614211421827000E-05 + 15, 8,-0.4929911947256000E-04, 0.5064113400309000E-04, 0.2927629245482000E-05, 0.2987754388848000E-05 + 15, 9, 0.2894783184979000E-04, 0.1020861091025000E-03, 0.2277597916162000E-05, 0.2264217551775000E-05 + 15, 10, 0.3588547880965000E-04,-0.7380703195300000E-05, 0.1568716636200000E-05, 0.1551260815551000E-05 + 15, 11, 0.2590727603140000E-04, 0.9732089741598001E-05, 0.1025884163119000E-05, 0.9625615229491001E-06 + 15, 12,-0.3302897569891000E-04, 0.8076142002464001E-05, 0.1286227547559000E-05, 0.1320121950725000E-05 + 15, 13,-0.5139258681996000E-04, 0.1098493603363000E-05, 0.2197159473372000E-05, 0.2172669236626000E-05 + 15, 14, 0.4576313922048000E-05,-0.5532553090423000E-05, 0.3365894046703000E-05, 0.3378802661325000E-05 + 15, 15,-0.4254079164137000E-05, 0.1606688012032000E-04, 0.4444933317511000E-05, 0.4471902703240000E-05 + 16, 0, 0.3046956879117000E-04, 0.0000000000000000E+00, 0.8808673728548000E-05, 0.0000000000000000E+00 + 16, 1,-0.1883375125815000E-04,-0.4207246068110000E-04, 0.8822133417483000E-05, 0.8765895112781000E-05 + 16, 2, 0.1605789876777000E-04, 0.3092763770136000E-04, 0.8752211073563000E-05, 0.8703754613291001E-05 + 16, 3, 0.8736317198244000E-04, 0.3066252662352000E-04, 0.8531443154161000E-05, 0.8569763814165000E-05 + 16, 4,-0.2764941799048000E-04, 0.2491397016687000E-04, 0.8191048767561000E-05, 0.8286807254971001E-05 + 16, 5,-0.1035013719932000E-04,-0.6518086116344000E-05, 0.7608239488615000E-05, 0.7549953331386000E-05 + 16, 6, 0.9021746789978000E-04,-0.2384296191283000E-04, 0.7032318629895000E-05, 0.6979181519016000E-05 + 16, 7, 0.1386719585299000E-05,-0.8375695304106000E-04, 0.6277014101456000E-05, 0.6378903527382000E-05 + 16, 8, 0.7519033361517000E-05,-0.5484343110228000E-05, 0.5248710916417000E-05, 0.5346567685261000E-05 + 16, 9,-0.1172687826145000E-03, 0.1077805039274000E-04, 0.4562772906758000E-05, 0.4600347020217000E-05 + 16, 10, 0.5004259911752000E-04,-0.9705622391234001E-05, 0.3401488861093000E-05, 0.3450176706691000E-05 + 16, 11, 0.2521431488435000E-04, 0.3635405773330000E-04, 0.2004346236834000E-05, 0.2015759047803000E-05 + 16, 12, 0.2102067953062000E-04,-0.1095248158506000E-04, 0.1895235045496000E-05, 0.1914740301174000E-05 + 16, 13,-0.1686369430648000E-04,-0.7951986125731000E-05, 0.2456864615099000E-05, 0.2442546031747000E-05 + 16, 14, 0.4603833639406000E-04, 0.4285383392351000E-05, 0.4042506350628000E-05, 0.3870166215432000E-05 + 16, 15,-0.2560236419130000E-04,-0.5287761273671000E-04, 0.5944848967209000E-05, 0.5956568619667000E-05 + 16, 16, 0.6006355659974000E-04, 0.8409456202797000E-05, 0.7780316449162000E-05, 0.7791495546992999E-05 + 17, 0, 0.7228611948272000E-05, 0.0000000000000000E+00, 0.1466325214127000E-04, 0.0000000000000000E+00 + 17, 1,-0.3593076900227000E-05,-0.5149118915071000E-05, 0.1477793698461000E-04, 0.1471121584327000E-04 + 17, 2,-0.1050719791630000E-04, 0.3951958024733000E-04, 0.1442947155154000E-04, 0.1442693996626000E-04 + 17, 3,-0.2124656841927000E-04, 0.8902841927279999E-05, 0.1427887898031000E-04, 0.1432947718700000E-04 + 17, 4, 0.1668065069862000E-05, 0.5816130661953000E-05, 0.1306344632825000E-04, 0.1318349254533000E-04 + 17, 5, 0.5525957697550000E-04, 0.2673649975671000E-04, 0.1223303334329000E-04, 0.1238222615786000E-04 + 17, 6,-0.2951532466566000E-04, 0.3509691162150000E-04, 0.1072305277119000E-04, 0.1039640668171000E-04 + 17, 7,-0.5097818123646000E-05,-0.3937730437467000E-04, 0.1000150363555000E-04, 0.9819512591697001E-05 + 17, 8, 0.4711543821045000E-04,-0.5609779881255000E-04, 0.8787471436200000E-05, 0.8235326243986001E-05 + 17, 9, 0.1662655300064000E-04,-0.5546691396174000E-04, 0.7627301066859000E-05, 0.7754374672671000E-05 + 17, 10,-0.9145153590949001E-04, 0.2708932885136000E-05, 0.6072286029191000E-05, 0.6402585049475000E-05 + 17, 11,-0.6947963895761000E-05,-0.3835740842312000E-04, 0.3952050299883000E-05, 0.3887151223698000E-05 + 17, 12, 0.2160212389868000E-04, 0.3129234877212000E-04, 0.2697808799740000E-05, 0.2691312394710000E-05 + 17, 13,-0.7015498163764000E-05, 0.2342565581658000E-04, 0.3077929914948000E-05, 0.3038348946208000E-05 + 17, 14,-0.1272893996900000E-04, 0.6184136500831000E-05, 0.3702270290722000E-05, 0.3708858336546000E-05 + 17, 15, 0.2090977148901000E-04, 0.1822431945459000E-04, 0.5361594486237000E-05, 0.5294005727669000E-05 + 17, 16,-0.4240232449617000E-04, 0.8629365920897000E-05, 0.9501455203208001E-05, 0.9438136971940001E-05 + 17, 17,-0.3643565384944000E-04, 0.6520296594766000E-05, 0.1334216880090000E-04, 0.1333688666348000E-04 + 18, 0,-0.3160782209377000E-07, 0.0000000000000000E+00, 0.2260977872535000E-04, 0.0000000000000000E+00 + 18, 1, 0.2409651745319000E-04, 0.4121041879383000E-05, 0.2280866664993000E-04, 0.2272723416772000E-04 + 18, 2,-0.3808754272417000E-04,-0.3759900096554000E-04, 0.2259711393939000E-04, 0.2225581732661000E-04 + 18, 3,-0.7488753746930000E-04, 0.4218801235027000E-04, 0.2176524328623000E-04, 0.2205438826418000E-04 + 18, 4,-0.4135160275974000E-05,-0.1769748496366000E-04, 0.1984973691348000E-04, 0.2064302918333000E-04 + 18, 5, 0.7904894860510000E-05,-0.1542899865191000E-04, 0.1663807061359000E-04, 0.1625612752099000E-04 + 18, 6,-0.2422514222424000E-04, 0.2685767019333000E-04, 0.1494154344689000E-04, 0.1558908406040000E-04 + 18, 7,-0.8953889673616999E-05, 0.5854099449034000E-04, 0.1228130995964000E-04, 0.1432567421442000E-04 + 18, 8,-0.3633790787860000E-04, 0.2055611259521000E-05, 0.1315580755139000E-04, 0.1220107580950000E-04 + 18, 9, 0.9398367877424001E-04, 0.2052805277025000E-05, 0.1195509716579000E-04, 0.1160480025077000E-04 + 18, 10, 0.8190744450733001E-05, 0.2611438252972000E-04, 0.1040135529275000E-04, 0.1070471089677000E-04 + 18, 11,-0.4873641218297000E-04,-0.2162546185657000E-04, 0.7302821453813000E-05, 0.7452061890969000E-05 + 18, 12,-0.2047938878228000E-04, 0.2009975587210000E-04, 0.5516901593768000E-05, 0.5266543920135000E-05 + 18, 13, 0.3018793586668000E-04,-0.3470192884471000E-05, 0.4312644277629000E-05, 0.4360829397385000E-05 + 18, 14,-0.3848675701936000E-04, 0.1470831747337000E-04, 0.5344914401045000E-05, 0.5198724329634000E-05 + 18, 15,-0.1979946155327000E-05, 0.1122473143992000E-04, 0.6315967671598000E-05, 0.6450432942650000E-05 + 18, 16,-0.1885315336478000E-04,-0.1293548895567000E-04, 0.8678380592171000E-05, 0.8799575773742000E-05 + 18, 17,-0.7205596374592000E-04, 0.3608474820689000E-05, 0.1597207784077000E-04, 0.1592027674407000E-04 + 18, 18, 0.3982192140283000E-04, 0.2957485517613000E-04, 0.2195166800847000E-04, 0.2212587243447000E-04 + 19, 0, 0.3470430941111000E-05, 0.0000000000000000E+00, 0.3080365365673000E-04, 0.0000000000000000E+00 + 19, 1,-0.5879472131305000E-04, 0.6585008411786000E-05, 0.3189540826669000E-04, 0.3207462114713000E-04 + 19, 2,-0.1729280690223000E-04,-0.2875625667961000E-04, 0.3028711508528000E-04, 0.3069436862850000E-04 + 19, 3, 0.4435491591431000E-04,-0.3914732396766000E-04, 0.3007122264554000E-04, 0.3061609623919000E-04 + 19, 4, 0.8260803766682999E-05, 0.1940210561134000E-04, 0.2378892055173000E-04, 0.2586121857569000E-04 + 19, 5,-0.6925742481018000E-04, 0.1537899824957000E-04, 0.2245843333693000E-04, 0.2313845462821000E-04 + 19, 6, 0.3915424683957000E-04,-0.1600358262850000E-04, 0.1793247790200000E-04, 0.1726609454478000E-04 + 19, 7, 0.5230575221981000E-04,-0.1693116407063000E-04, 0.1817244648354000E-04, 0.1587114334964000E-04 + 19, 8, 0.1844213559037000E-04, 0.7605192912942000E-04, 0.1599046763642000E-04, 0.1327472682617000E-04 + 19, 9,-0.4379012193234000E-04, 0.2496677377452000E-04, 0.1575363444045000E-04, 0.1612865458540000E-04 + 19, 10, 0.7227136121218000E-04,-0.2148860835234000E-04, 0.1381865675192000E-04, 0.1486100537521000E-04 + 19, 11, 0.1231988693355000E-04, 0.3692210176484000E-04, 0.1120842514954000E-04, 0.1051847104171000E-04 + 19, 12,-0.3459643076408000E-04,-0.5223947817594000E-04, 0.8855183783393000E-05, 0.9516470440585000E-05 + 19, 13,-0.1135311672491000E-04,-0.6661868058050000E-05, 0.8182348639738001E-05, 0.7991005892320999E-05 + 19, 14, 0.1155968735254000E-04, 0.1087670675828000E-04, 0.6330671010239000E-05, 0.6321612341589000E-05 + 19, 15,-0.5466708498805000E-04,-0.1390683158823000E-04, 0.5682273259762000E-05, 0.5736511884007000E-05 + 19, 16,-0.1763641171649000E-04,-0.5939128837081000E-04, 0.5351239750733000E-05, 0.5393122419057000E-05 + 19, 17, 0.1966502379529000E-04, 0.1187410520918000E-04, 0.6176469017133000E-05, 0.5897690057932000E-05 + 19, 18, 0.1772902186204000E-04,-0.2608006957300000E-04, 0.1973118833805000E-04, 0.1989391643215000E-04 + 19, 19, 0.1355622725295000E-04,-0.4230550917980000E-04, 0.3347609687528000E-04, 0.3336868665510000E-04 + 20, 0, 0.7386896112898000E-05, 0.0000000000000000E+00, 0.4118025018976000E-04, 0.0000000000000000E+00 + 20, 1, 0.1489488576035000E-04, 0.3116265664847000E-04, 0.4101880017487000E-04, 0.4103037303111000E-04 + 20, 2, 0.6305260866365000E-05,-0.3289478383933000E-04, 0.4045437064402000E-04, 0.3988124515721000E-04 + 20, 3, 0.3566854887036000E-04,-0.1956035123336000E-04, 0.3585817853011000E-04, 0.3843835203853000E-04 + 20, 4, 0.3858918894304000E-04,-0.7071105999335000E-05, 0.3247940299520000E-04, 0.3376435705370000E-04 + 20, 5,-0.2771721970696000E-04, 0.1452638788154000E-04, 0.1940374991189000E-04, 0.2218374652586000E-04 + 20, 6,-0.2813798007217000E-04,-0.5883220909142000E-05, 0.1845153254530000E-04, 0.2107762772012000E-04 + 20, 7,-0.2934150616021000E-04, 0.4509438874294000E-04, 0.1748128417435000E-04, 0.1704929617488000E-04 + 20, 8,-0.1730082329169000E-04, 0.1687440171137000E-04, 0.1895340193191000E-04, 0.1686989283473000E-04 + 20, 9,-0.3348167526029000E-04,-0.2954324249325000E-05, 0.1880941464915000E-04, 0.1589037641301000E-04 + 20, 10,-0.4083866024967000E-04,-0.3003711050255000E-04, 0.1754530015749000E-04, 0.1815941814696000E-04 + 20, 11, 0.7194854690190000E-05, 0.8568282976047000E-05, 0.1657899243127000E-04, 0.1725108360911000E-04 + 20, 12, 0.5762851519291000E-04,-0.3092761620944000E-04, 0.1494299201885000E-04, 0.1490694994232000E-04 + 20, 13,-0.2434771829708000E-04, 0.3671493352070000E-05, 0.1642687699811000E-04, 0.1596799830780000E-04 + 20, 14, 0.4507333871400000E-04,-0.5651367745112000E-05, 0.1239979499244000E-04, 0.1131653017760000E-04 + 20, 15, 0.1201898989702000E-05,-0.5126681779977000E-04, 0.8850735741950999E-05, 0.8985959189873001E-05 + 20, 16, 0.4889374790886000E-04, 0.5857290944036000E-04, 0.7224734655724000E-05, 0.7338451026286000E-05 + 20, 17, 0.1746022628164000E-04,-0.1000122868416000E-04, 0.7191645731357000E-05, 0.7329706561426000E-05 + 20, 18,-0.1181227062961000E-03,-0.9287021873343999E-04, 0.1370605158564000E-04, 0.1350009884750000E-04 + 20, 19,-0.1053877986498000E-04,-0.3477223774870000E-05, 0.3558362257674000E-04, 0.3570240752164000E-04 + 20, 20,-0.2732624476807000E-04, 0.2571662256856000E-04, 0.4558412222904000E-04, 0.4579253893744000E-04 diff --git a/supportData/LocalGravData/eros007790.tab b/supportData/LocalGravData/eros007790.tab index dad0e396d5..562ca35fba 100755 --- a/supportData/LocalGravData/eros007790.tab +++ b/supportData/LocalGravData/eros007790.tab @@ -11684,4 +11684,4 @@ f 3891 3890 3896 f 3891 3896 3892 f 3892 3896 3895 f 3892 3895 3893 -f 3894 3895 3896 \ No newline at end of file +f 3894 3895 3896 From a008adb320b1467d80772dd4f60b3e2b59288412 Mon Sep 17 00:00:00 2001 From: Reece Humphreys Date: Mon, 22 Sep 2025 10:50:22 -0600 Subject: [PATCH 2/6] Add ruff format to pre-commit --- .pre-commit-config.yaml | 22 +++++++++++++--------- requirements_dev.txt | 1 + 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 8443f3b395..e1225ddf08 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,12 +1,16 @@ repos: -- repo: https://github.com/pre-commit/pre-commit-hooks + - repo: https://github.com/pre-commit/pre-commit-hooks rev: v2.3.0 hooks: - - id: trailing-whitespace - - id: end-of-file-fixer - - id: check-yaml - - id: check-added-large-files - args: ['--maxkb=10000'] - - id: check-case-conflict - - id: check-merge-conflict - - id: debug-statements + - id: trailing-whitespace + - id: end-of-file-fixer + - id: check-yaml + - id: check-added-large-files + args: ["--maxkb=10000"] + - id: check-case-conflict + - id: check-merge-conflict + - id: debug-statements + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.13.1 + hooks: + - id: ruff-format diff --git a/requirements_dev.txt b/requirements_dev.txt index 52552f009b..0d1d9c393a 100644 --- a/requirements_dev.txt +++ b/requirements_dev.txt @@ -13,5 +13,6 @@ pytest-rerunfailures==13.0 pre-commit>=3.5.0,<=4.2.0 clang-format==20.1.0 +ruff>=0.13.1 psutil==7.0.0 From 8794a84a2bd25b75cc6d38eb86b5da142042623e Mon Sep 17 00:00:00 2001 From: Reece Humphreys Date: Mon, 22 Sep 2025 10:50:33 -0600 Subject: [PATCH 3/6] Run ruff format on the repo --- conanfile.py | 324 ++- docs/source/codeSamples/bsk-1.py | 6 +- docs/source/codeSamples/bsk-2.py | 2 +- docs/source/codeSamples/bsk-2a.py | 2 +- docs/source/codeSamples/bsk-2b.py | 12 +- docs/source/codeSamples/bsk-3.py | 2 +- docs/source/codeSamples/bsk-4.py | 30 +- docs/source/codeSamples/bsk-5.py | 21 +- docs/source/codeSamples/bsk-6.py | 9 +- docs/source/codeSamples/bsk-7.py | 2 +- docs/source/codeSamples/bsk-8.py | 4 +- docs/source/codeSamples/making-pyModules.py | 2 +- docs/source/conf.py | 306 +-- examples/BskSim/BSK_masters.py | 24 +- examples/BskSim/models/BSK_Dynamics.py | 168 +- .../BskSim/models/BSK_FormationDynamics.py | 80 +- examples/BskSim/models/BSK_FormationFsw.py | 132 +- examples/BskSim/models/BSK_Fsw.py | 228 +- examples/BskSim/plotting/BSK_Plotting.py | 326 +-- .../BskSim/scenarios/scenario_AddRWFault.py | 39 +- .../BskSim/scenarios/scenario_AttEclipse.py | 64 +- .../BskSim/scenarios/scenario_AttFeedback.py | 42 +- .../scenarios/scenario_AttGuidHyperbolic.py | 67 +- .../BskSim/scenarios/scenario_AttGuidance.py | 50 +- .../BskSim/scenarios/scenario_AttModes.py | 36 +- .../BskSim/scenarios/scenario_AttSteering.py | 37 +- .../BskSim/scenarios/scenario_BasicOrbit.py | 33 +- .../scenarios/scenario_BasicOrbitFormation.py | 69 +- .../BskSim/scenarios/scenario_FeedbackRW.py | 46 +- .../scenarios/scenario_LambertGuidance.py | 159 +- .../scenario_RelativePointingFormation.py | 76 +- .../scenarioBskSimAttFeedbackMC.py | 81 +- .../scenarioRerunMonteCarlo.py | 9 +- .../scenarioVisualizeMonteCarlo.py | 18 +- .../MultiSatBskSim/BSK_MultiSatMasters.py | 67 +- .../modelsMultiSat/BSK_EnvironmentEarth.py | 65 +- .../modelsMultiSat/BSK_EnvironmentMercury.py | 64 +- .../modelsMultiSat/BSK_MultiSatDynamics.py | 124 +- .../modelsMultiSat/BSK_MultiSatFsw.py | 179 +- .../plottingMultiSat/BSK_MultiSatPlotting.py | 315 +-- .../scenario_AttGuidMultiSat.py | 136 +- .../scenario_BasicOrbitMultiSat.py | 109 +- .../scenario_BasicOrbitMultiSat_MT.py | 98 +- .../scenario_StationKeepingMultiSat.py | 338 ++- examples/OpNavScenarios/BSK_OpNav.py | 49 +- .../modelsOpNav/BSK_OpNavDynamics.py | 270 ++- .../modelsOpNav/BSK_OpNavFsw.py | 385 +++- .../plottingOpNav/OpNav_Plotting.py | 1658 ++++++++------ .../CNN_ImageGen/OpNavMonteCarlo.py | 144 +- .../CNN_ImageGen/scenario_CNNImages.py | 125 +- .../scenariosOpNav/OpNavMC/MonteCarlo.py | 145 +- .../OpNavMC/scenario_LimbAttOD.py | 189 +- .../OpNavMC/scenario_OpNavAttODMC.py | 96 +- .../scenariosOpNav/scenario_CNNAttOD.py | 251 ++- .../scenariosOpNav/scenario_OpNavAttOD.py | 243 ++- .../scenariosOpNav/scenario_OpNavAttODLimb.py | 196 +- .../scenariosOpNav/scenario_OpNavHeading.py | 287 ++- .../scenariosOpNav/scenario_OpNavOD.py | 226 +- .../scenariosOpNav/scenario_OpNavODLimb.py | 177 +- .../scenariosOpNav/scenario_OpNavPoint.py | 169 +- .../scenariosOpNav/scenario_OpNavPointLimb.py | 85 +- .../scenariosOpNav/scenario_faultDetOpNav.py | 165 +- examples/SunLineKF_test_utilities.py | 893 ++++---- examples/mujoco/scenarioArmWithThrusters.py | 27 +- examples/mujoco/scenarioAsteroidLanding.py | 12 +- examples/mujoco/scenarioBranchingPanels.py | 68 +- examples/mujoco/scenarioDeployPanels.py | 21 +- examples/mujoco/scenarioReactionWheel.py | 6 +- examples/mujoco/scenarioSRPInPanels.py | 16 +- examples/mujoco/scenarioSimpleDocking.py | 4 +- .../mujoco/scenarioUnbalancedThrusters.py | 16 +- examples/scenarioAerocapture.py | 129 +- examples/scenarioAlbedo.py | 173 +- examples/scenarioAsteroidArrival.py | 218 +- examples/scenarioAttGuideHyperbolic.py | 118 +- examples/scenarioAttLocPoint.py | 104 +- .../scenarioAttitudeConstrainedManeuver.py | 414 ++-- .../scenarioAttitudeConstraintViolation.py | 420 ++-- examples/scenarioAttitudeFeedback.py | 114 +- examples/scenarioAttitudeFeedback2T.py | 129 +- examples/scenarioAttitudeFeedback2T_TH.py | 293 +-- .../scenarioAttitudeFeedback2T_stateEffTH.py | 296 +-- examples/scenarioAttitudeFeedbackNoEarth.py | 107 +- examples/scenarioAttitudeFeedbackRW.py | 215 +- examples/scenarioAttitudeFeedbackRWPower.py | 156 +- examples/scenarioAttitudeGG.py | 83 +- examples/scenarioAttitudeGuidance.py | 119 +- examples/scenarioAttitudePointing.py | 87 +- examples/scenarioAttitudePointingPy.py | 108 +- examples/scenarioAttitudePrescribed.py | 54 +- examples/scenarioAttitudeSteering.py | 188 +- examples/scenarioBasicOrbit.py | 181 +- examples/scenarioBasicOrbitStream.py | 176 +- examples/scenarioBskLog.py | 37 +- examples/scenarioCSS.py | 150 +- examples/scenarioCSSFilters.py | 362 +++- examples/scenarioCentralBody.py | 101 +- examples/scenarioConstrainedDynamics.py | 110 +- examples/scenarioCustomGravBody.py | 93 +- examples/scenarioDataDemo.py | 59 +- examples/scenarioDataToViz.py | 108 +- examples/scenarioDebrisReorbitET.py | 171 +- examples/scenarioDeployingPanel.py | 195 +- examples/scenarioDeployingSolarArrays.py | 643 ++++-- examples/scenarioDragDeorbit.py | 83 +- examples/scenarioDragRendezvous.py | 400 ++-- examples/scenarioDragSensitivity.py | 84 +- examples/scenarioExtendingBoom.py | 358 ++- examples/scenarioFlexiblePanel.py | 326 ++- examples/scenarioFlybySpice.py | 109 +- examples/scenarioFormationBasic.py | 231 +- examples/scenarioFormationMeanOEFeedback.py | 145 +- examples/scenarioFormationReconfig.py | 64 +- examples/scenarioFuelSlosh.py | 69 +- examples/scenarioGaussMarkovRandomWalk.py | 58 +- examples/scenarioGroundDownlink.py | 153 +- examples/scenarioGroundLocationImaging.py | 3 +- examples/scenarioGroundMapping.py | 9 +- examples/scenarioHaloOrbit.py | 161 +- examples/scenarioHelioTransSpice.py | 36 +- examples/scenarioHingedRigidBody.py | 98 +- examples/scenarioHohmann.py | 137 +- examples/scenarioInertialSpiral.py | 101 +- examples/scenarioIntegrators.py | 10 +- examples/scenarioJupiterArrival.py | 138 +- examples/scenarioLagrangePointOrbit.py | 157 +- examples/scenarioLambertSolver.py | 275 ++- .../scenarioMagneticFieldCenteredDipole.py | 141 +- examples/scenarioMagneticFieldWMM.py | 123 +- examples/scenarioMomentumDumping.py | 322 +-- examples/scenarioMonteCarloAttRW.py | 441 ++-- examples/scenarioMonteCarloSpice.py | 31 +- examples/scenarioMtbMomentumManagement.py | 363 ++-- .../scenarioMtbMomentumManagementSimple.py | 407 ++-- examples/scenarioOrbitManeuver.py | 95 +- examples/scenarioOrbitManeuverTH.py | 126 +- examples/scenarioOrbitMultiBody.py | 190 +- examples/scenarioPatchedConics.py | 262 ++- examples/scenarioPowerDemo.py | 49 +- examples/scenarioQuadMaps.py | 206 +- examples/scenarioRendezVous.py | 209 +- examples/scenarioRoboticArm.py | 274 ++- examples/scenarioRotatingPanel.py | 121 +- examples/scenarioSatelliteConstellation.py | 147 +- examples/scenarioSepMomentumManagement.py | 799 ++++--- examples/scenarioSmallBodyFeedbackControl.py | 647 +++++- examples/scenarioSmallBodyLandmarks.py | 262 ++- examples/scenarioSmallBodyNav.py | 873 ++++++-- examples/scenarioSmallBodyNavUKF.py | 760 +++++-- examples/scenarioSpacecraftLocation.py | 69 +- examples/scenarioSpiceSpacecraft.py | 100 +- examples/scenarioSpinningBodiesTwoDOF.py | 154 +- examples/scenarioSweepingSpacecraft.py | 184 +- examples/scenarioTAM.py | 140 +- examples/scenarioTAMcomparison.py | 237 +- examples/scenarioTempMeasurementAttitude.py | 150 +- examples/scenarioTwoChargedSC.py | 189 +- .../scenarioVariableTimeStepIntegrators.py | 65 +- examples/scenarioVizPoint.py | 163 +- libs/mujoco/conanfile.py | 38 +- run_all_test.py | 4 +- setup.py | 100 +- .../_UnitTest/test_CMsgTimeWritten.py | 16 +- .../_UnitTest/test_MessagingInclude.py | 2 + .../_UnitTest/test_NonSwigMessaging.py | 112 +- .../_UnitTest/test_RecordInputMessages.py | 80 +- .../test_deprecatedRecorderFormat.py | 8 +- .../messaging/_UnitTest/test_unsubscribe.py | 7 +- .../messaging/cMsgCInterfacePy/__init__.py | 7 +- .../msgAutoSource/generatePackageInit.py | 16 +- .../msgAutoSource/generateSWIGModules.py | 110 +- .../system_model/_UnitTest/test_PySysModel.py | 30 +- .../_UnitTest/test_BSpline.py | 72 +- .../_UnitTest/test_avsLibrarySelfCheck.py | 50 +- .../_UnitTest/test_keplerianOrbit.py | 35 +- src/conftest.py | 27 +- .../test_lowPassFilterTorqueCommand.py | 72 +- .../mrpFeedback/_UnitTest/test_mrpFeedback.py | 247 ++- .../mrpPD/_UnitTest/Support/results_MRP_PD.py | 9 +- .../attControl/mrpPD/_UnitTest/test_mrpPD.py | 38 +- .../_UnitTest/test_MRP_steeringInt.py | 150 +- .../_UnitTest/test_MRP_steeringUnit.py | 66 +- .../_UnitTest/test_mtbFeedforward.py | 161 +- .../_UnitTest/test_mtbMomentumManagement.py | 145 +- .../test_mtbMomentumManagementSimple.py | 132 +- .../_UnitTest/test_PRV_Steering.py | 114 +- .../_UnitTest/test_rateServoFullNonlinear.py | 216 +- .../_UnitTest/test_thrMomentumManagement.py | 83 +- .../_UnitTest/test_cssWlsEstUnitTest.py | 259 ++- .../InertialUKF/_UnitTest/test_inertialKF.py | 697 ++++-- .../_UnitTest/headingSuKF_test_utilities.py | 136 +- .../headingSuKF/_UnitTest/test_headingSuKF.py | 316 ++- .../_UnitTest/SunLineOEKF_test_utilities.py | 348 +-- .../okeefeEKF/_UnitTest/test_okeefeEKF.py | 690 ++++-- .../_UnitTest/SunLineEKF_test_utilities.py | 424 ++-- .../sunlineEKF/_UnitTest/test_SunLineEKF.py | 873 ++++++-- .../_UnitTest/test_sunlineEphem.py | 107 +- .../_UnitTest/SunLineSEKF_test_utilities.py | 411 ++-- .../sunlineSEKF/_UnitTest/test_SunLineSEKF.py | 865 +++++--- .../_UnitTest/SunLineSuKF_test_utilities.py | 199 +- .../sunlineSuKF/_UnitTest/test_SunLineSuKF.py | 507 +++-- .../_UnitTest/SunLineuKF_test_utilities.py | 190 +- .../sunlineUKF/_UnitTest/test_SunLineUKF.py | 319 +-- .../_UnitTest/test_attRefCorrection.py | 21 +- .../_UnitTest/test_attTrackingError.py | 60 +- .../_UnitTest/test_celestialTwoBodyPoint.py | 191 +- .../test_ConstrainedAttitudeManeuver.py | 511 +++-- .../Support/results_eulerRotation.py | 51 +- .../_UnitTest/test_eulerRotation.py | 152 +- .../_UnitTest/Support/results_hillPoint.py | 25 +- .../hillPoint/_UnitTest/test_hillPoint.py | 105 +- .../inertial3D/_UnitTest/test_inertial3D.py | 101 +- .../Support/results_inertial3DSpin.py | 20 +- .../_UnitTest/test_inertial3DSpin.py | 165 +- .../_UnitTest/test_locationPointing.py | 109 +- .../_UnitTest/Support/truth_mrpRotation.py | 13 +- .../mrpRotation/_UnitTest/test_mrpRotation.py | 108 +- .../_UnitTest/test_oneAxisSolarArrayPoint.py | 121 +- .../opNavPoint/_UnitTest/test_opNavPoint.py | 196 +- .../_UnitTest/test_sunSafePoint.py | 243 ++- .../Support/results_velocityPoint.py | 20 +- .../_UnitTest/test_velocityPoint.py | 80 +- .../_UnitTest/test_WaypointReference.py | 188 +- .../_UnitTest/test_dvGuidance.py | 102 +- .../_UnitTest/test_dipoleMapping.py | 180 +- .../test_forceTorqueThrForceMapping.py | 272 ++- .../_UnitTest/test_hingedRigidBodyPIDMotor.py | 73 +- .../_UnitTest/test_prescribedRot2DOF.py | 198 +- .../Support/results_rwMotorTorque.py | 79 +- .../_UnitTest/test_rwMotorTorque.py | 84 +- .../test_rwMotorTorqueParametrized.py | 299 ++- .../_UnitTest/test_rwMotorVoltage.py | 275 +-- .../rwNullSpace/_UnitTest/test_rwNullSpace.py | 93 +- .../_UnitTest/test_solarArrayReference.py | 152 +- .../_UnitTest/test_thrFiringRemainder.py | 184 +- .../_UnitTest/test_thrFiringSchmitt.py | 202 +- .../Support/Results_thrForceMapping.py | 81 +- .../_UnitTest/test_thrForceMap_configErr.py | 262 ++- .../_UnitTest/test_thrForceMapping.py | 349 +-- .../_UnitTest/test_thrMomentumDumping.py | 133 +- .../_UnitTest/test_thrustRWDesat.py | 129 +- .../test_thrusterPlatformReference.py | 59 +- .../_UnitTest/test_thrusterPlatformState.py | 115 +- .../_UnitTest/test_torque2Dipole.py | 115 +- .../_UnitTest/test_torqueScheduler.py | 214 +- .../_UnitTest/test_vscmgGimbalRateServo.py | 176 +- .../_UnitTest/test_vscmgVelocitySteering.py | 197 +- .../_UnitTest/test_etSphericalControl.py | 144 +- .../_UnitTest/test_formationBarycenter.py | 66 +- .../_UnitTest/test_hillStateConverter.py | 29 +- .../_UnitTest/test_hillToAttRef.py | 54 +- .../_UnitTest/test_meanOEFeedback.py | 115 +- .../_UnitTest/test_spacecraftPointing.py | 185 +- .../_UnitTest/test_spacecraftReconfig.py | 69 +- .../_UnitTest/test_centerRadiusCNN.py | 56 +- .../_UnitTest/test_horizonOpNav.py | 1034 +++++++-- .../_UnitTest/test_houghCirlces.py | 135 +- .../limbFinding/_UnitTest/test_limbFinding.py | 70 +- .../_UnitTest/test_pixelLineConverter.py | 93 +- .../_UnitTest/test_faultDetection.py | 140 +- .../_UnitTest/pixelLineBias_test_utilities.py | 257 ++- .../_UnitTest/test_pixelLineBiasUKF.py | 263 ++- .../_UnitTest/relativeODuKF_test_utilities.py | 257 ++- .../_UnitTest/test_relativeODuKF.py | 246 ++- .../_UnitTest/test_lambertPlanner.py | 129 +- .../_UnitTest/test_lambertSecondDV.py | 68 +- .../_UnitTest/Support/IzzoLambert.py | 161 +- .../_UnitTest/test_lambertSolver.py | 257 ++- .../test_lambertSurfaceRelativeVelocity.py | 118 +- .../_UnitTest/test_lambertValidator.py | 324 +-- .../test_smallBodyWaypointFeedback.py | 80 +- .../_UnitTest/test_rwConfigData.py | 58 +- .../CSSSensorData/_UnitTest/test_cssComm.py | 95 +- .../TAMSensorData/_UnitTest/test_tamComm.py | 39 +- .../_UnitTest/test_rateMsgConverter.py | 63 +- .../test_scanningInstrumentController.py | 106 +- .../test_simpleInstrumentController.py | 150 +- .../_UnitTest/test_smallBodyNavEKF.py | 134 +- .../_UnitTest/test_smallBodyNavUKF.py | 140 +- .../_UnitTest/test_thrustCMEstimation.py | 67 +- .../_UnitTest/test_chebyPosEphem.py | 152 +- .../_UnitTest/test_dvAccumulation.py | 73 +- .../_UnitTest/test_ephemDifference.py | 81 +- .../_UnitTest/test_ephemNavConverter.py | 65 +- .../_UnitTest/test_navAggregate.py | 306 +-- .../_UnitTest/test_oeStateEphem.py | 287 +-- .../_UnitTest/test_vehicleConfigData.py | 57 +- .../_UnitTest/test_cModuleTemplate.py | 122 +- .../test_cModuleTemplateParametrized.py | 182 +- .../test_cppModuleTemplateParametrized.py | 184 +- src/reportconf.py | 61 +- .../encoder/_UnitTest/test_encoder.py | 36 +- .../test_hingedBodyLinearProfiler.py | 39 +- .../_UnitTest/test_motorVoltageInterface.py | 105 +- .../test_prescribedLinearTranslation.py | 180 +- .../_UnitTest/test_prescribedRotation1DOF.py | 167 +- .../_UnitTest/test_tempMeasurement.py | 189 +- .../_UnitTest/test_bore_ang_calc.py | 152 +- .../test_bore_ang_calc_inertial_heading.py | 51 +- .../Support/orb_elem_convert_support.py | 56 +- .../_UnitTest/test_orb_elem_convert.py | 828 +++++-- .../_UnitTest/test_extPulsedTorque.py | 99 +- .../FuelTank/_UnitTest/test_mass_depletion.py | 111 +- .../FuelTank/_UnitTest/test_tank_models.py | 395 ++-- .../_UnitTest/test_gravityGradient.py | 122 +- .../test_hingedRigidBodyStateEffector.py | 683 ++++-- .../Integrators/_UnitTest/test_Integrators.py | 166 +- .../_UnitTest/test_linearSpringMassDamper.py | 338 ++- .../MtbEffector/_UnitTest/test_MtbEffector.py | 158 +- .../test_nHingedRigidBodyStateEffector.py | 220 +- .../_UnitTest/Support/makeSphereXML.py | 92 +- .../_UnitTest/test_radiationPressure.py | 290 ++- .../test_radiation_pressure_integrated.py | 87 +- .../RadiationPressure/parseSRPLookup.py | 31 +- .../_UnitTest/test_ThrusterDynamicsUnit.py | 1914 +++++++++++++---- .../test_thruster_dynamics_attached_body.py | 80 +- .../_UnitTest/test_thruster_integrated_sim.py | 71 +- .../test_ThrusterStateEffectorUnit.py | 341 ++- ...CMGStateEffector_ConfigureVSCMGRequests.py | 178 +- .../test_VSCMGStateEffector_integrated.py | 534 +++-- .../test_ConstraintDynamicEffectorUnit.py | 340 ++- .../_UnitTest/test_InOutMessageFiltering.py | 357 +-- .../dragEffector/_UnitTest/test_atmoDrag.py | 202 +- .../test_dualhingedRigidBodyStateEffector.py | 425 ++-- .../_UnitTest/test_extForceTorque.py | 147 +- .../test_extForceTorqueIntegrated.py | 114 +- .../_UnitTest/test_unitFacetDrag.py | 173 +- .../test_unitFacetSRPDynamicEffector.py | 300 ++- .../_UnitTest/test_gravityDynEffector.py | 530 +++-- .../_UnitTest/test_gravitySpacecraft.py | 235 +- .../dynamics/gravityEffector/gravCoeffOps.py | 129 +- .../_UnitTest/test_hingedRigidBodyMotor.py | 44 +- ...test_linearTranslationNDOFStateEffector.py | 750 ++++--- ...st_linearTranslationOneDOFStateEffector.py | 451 ++-- .../_UnitTest/test_msmForceTorque.py | 98 +- .../test_PrescribedMotionStateEffector.py | 617 ++++-- .../_UnitTest/test_reactionWheelMemoryLeak.py | 47 +- ...nWheelStateEffector_ConfigureRWRequests.py | 113 +- ...est_reactionWheelStateEffector_RWUpdate.py | 92 +- ...t_reactionWheelStateEffector_integrated.py | 662 ++++-- .../spacecraft/_UnitTest/test_spacecraft.py | 986 ++++++--- .../_UnitTest/test_sphericalPendulum.py | 388 ++-- .../test_spinningBodyNDOFStateEffector.py | 676 ++++-- .../test_spinningBodyOneDOFStateEffector.py | 157 +- .../test_spinningBodyTwoDOFStateEffector.py | 190 +- .../_UnitTest/test_stateArchitecture.py | 77 +- .../test_integratedExponentialAtmosphere.py | 77 +- .../test_unitTestExponentialAtmosphere.py | 120 +- .../_UnitTest/test_msiseAtmosphere.py | 122 +- .../test_unitTestTabularAtmosphere.py | 81 +- .../albedo/_UnitTest/test_albedo.py | 71 +- .../_UnitTest/test_dentonFluxModel.py | 144 +- .../eclipse/_UnitTest/test_eclipse.py | 173 +- .../_UnitTest/test_ephemconvert.py | 59 +- .../_UnitTest/test_unitGroundLocation.py | 138 +- .../_UnitTest/test_groundMapping.py | 18 +- .../test_magneticFieldCenteredDipole.py | 140 +- .../_UnitTest/test_magneticFieldWMM.py | 211 +- .../_UnitTest/test_planetEphemeris.py | 159 +- .../solarFlux/_UnitTest/test_solarFlux.py | 15 +- .../_UnitTest/test_unitSpacecraftLocation.py | 78 +- .../_UnitTest/test_unitSpice.py | 454 +++- .../_UnitTest/test_unitSpiceSpacecraft.py | 164 +- .../MJSystemCoM/_UnitTest/test_MJSystemCoM.py | 83 +- .../NBodyGravity/_UnitTest/test_gravity.py | 45 +- .../_UnitTest/test_PIDControllers.py | 60 +- .../_UnitTest/test_integration.py | 14 +- .../_UnitTest/test_mass_update.py | 8 +- .../_UnitTest/test_stateful_sys_model.py | 20 +- .../_GeneralModuleFiles/_UnitTest/test_vis.py | 8 +- .../_GeneralModuleFiles/pythonUtils.py | 8 +- .../_UnitTest/test_unitPinholeCamera.py | 114 +- .../_UnitTest/test_planetHeading.py | 8 +- .../planetNav/_UnitTest/test_planetNav.py | 260 ++- .../simpleNav/_UnitTest/test_simpleNav.py | 1046 +++++++-- .../_UnitTest/test_mappingInstrument.py | 14 +- .../_UnitTest/test_simpleInstrument.py | 68 +- .../test_spaceToGroundTransmitter.py | 176 +- .../_UnitTest/test_partitionedStorageUnit.py | 272 ++- .../_UnitTest/test_simpleStorageUnit.py | 208 +- .../_UnitTest/test_simpleTransmitter.py | 84 +- .../_UnitTest/test_unitReactionWheelPower.py | 90 +- .../_UnitTest/test_simpleBattery.py | 60 +- .../_UnitTest/test_simpleBatteryFault.py | 67 +- .../_UnitTest/test_unitSimplePowerMonitor.py | 42 +- .../_UnitTest/test_unitSimplePowerSink.py | 65 +- .../_UnitTest/test_unitSimpleSolarPanel.py | 61 +- .../sensors/camera/_UnitTest/test_camera.py | 122 +- .../camera/_UnitTest/test_colorAdjust.py | 91 +- .../_UnitTest/test_CSSConfig.py | 179 +- .../_UnitTest/test_coarseSunSensor.py | 511 ++++- .../_UnitTest/test_coarseSunSensorFaults.py | 19 +- .../test_hingedRigidBodyMotorSensor.py | 115 +- .../imuSensor/_UnitTest/test_imu_sensor.py | 783 +++++-- .../_UnitTest/test_magnetometer.py | 103 +- .../_UnitTest/test_magnetometerFaults.py | 220 +- .../_UnitTest/test_simpleMassProps.py | 56 +- .../_UnitTest/test_simpleVoltEstimator.py | 85 +- .../_UnitTest/test_star_tracker.py | 131 +- .../_UnitTest/test_motorThermal.py | 90 +- .../_UnitTest/test_sensorThermal.py | 37 +- .../_UnitTest/test_dataFileToViz.py | 204 +- .../_UnitTest/test_vizInterface.py | 67 +- src/tests/test_bskMcTestScript.py | 31 +- src/tests/test_bskPrinciples.py | 15 +- src/tests/test_bskTestOpNav.py | 36 +- src/tests/test_bskTestScript.py | 38 +- src/tests/test_deprecated.py | 7 +- src/tests/test_messaging.py | 153 +- src/tests/test_orbitalMotion.py | 218 +- src/tests/test_protectAllClasses.py | 3 +- src/tests/test_rbkRoutine.py | 1311 ++++++----- src/tests/test_scenarioAerocapture.py | 6 +- src/tests/test_scenarioAlbedo.py | 28 +- src/tests/test_scenarioAttGuidMultiSat.py | 5 +- src/tests/test_scenarioAttGuideHyperbolic.py | 5 +- src/tests/test_scenarioAttLocPoint.py | 4 +- ...est_scenarioAttitudeConstrainedManeuver.py | 19 +- ...est_scenarioAttitudeConstraintViolation.py | 27 +- src/tests/test_scenarioAttitudeFeedback.py | 29 +- src/tests/test_scenarioAttitudeFeedback2T.py | 14 +- .../test_scenarioAttitudeFeedback2T_TH.py | 3 +- ...t_scenarioAttitudeFeedback2T_stateEffTH.py | 7 +- .../test_scenarioAttitudeFeedbackNoEarth.py | 26 +- src/tests/test_scenarioAttitudeFeedbackRW.py | 13 +- .../test_scenarioAttitudeFeedbackRWPower.py | 10 +- src/tests/test_scenarioAttitudeGG.py | 5 +- src/tests/test_scenarioAttitudeGuidance.py | 4 +- src/tests/test_scenarioAttitudePointing.py | 3 +- src/tests/test_scenarioAttitudePointingPy.py | 3 +- src/tests/test_scenarioAttitudePrescribed.py | 4 +- src/tests/test_scenarioAttitudeSteering.py | 5 +- src/tests/test_scenarioBasicOrbit.py | 40 +- src/tests/test_scenarioBasicOrbitMultiSat.py | 13 +- .../test_scenarioBasicOrbitMultiSat_MT.py | 11 +- src/tests/test_scenarioBasicOrbitStream.py | 34 +- src/tests/test_scenarioBskLog.py | 30 +- src/tests/test_scenarioCSS.py | 32 +- src/tests/test_scenarioCSSFilters.py | 19 +- src/tests/test_scenarioCentralBody.py | 23 +- src/tests/test_scenarioConstrainedDynamics.py | 8 +- src/tests/test_scenarioCustomGravBody.py | 9 +- src/tests/test_scenarioDataDemo.py | 15 +- src/tests/test_scenarioDataToViz.py | 11 +- src/tests/test_scenarioDebrisReorbitET.py | 9 +- src/tests/test_scenarioDragDeorbit.py | 11 +- src/tests/test_scenarioDragRendezvous.py | 17 +- src/tests/test_scenarioDragSensitivity.py | 2 +- src/tests/test_scenarioFlexiblePanel.py | 3 +- src/tests/test_scenarioFlybySpice.py | 19 +- src/tests/test_scenarioFormationBasic.py | 3 +- .../test_scenarioFormationMeanOEFeedback.py | 84 +- src/tests/test_scenarioFormationReconfig.py | 137 +- src/tests/test_scenarioFuelSlosh.py | 25 +- .../test_scenarioGaussMarkovRandomWalk.py | 29 +- src/tests/test_scenarioGroundMapping.py | 11 +- src/tests/test_scenarioHelioTransSpice.py | 13 +- src/tests/test_scenarioHingedRigidBody.py | 13 +- src/tests/test_scenarioHohmann.py | 4 +- src/tests/test_scenarioIntegrators.py | 71 +- src/tests/test_scenarioLagrangePointOrbit.py | 32 +- ...est_scenarioMagneticFieldCenteredDipole.py | 20 +- src/tests/test_scenarioMagneticFieldWMM.py | 10 +- src/tests/test_scenarioMomentumDumping.py | 2 +- src/tests/test_scenarioMonteCarloAttRW.py | 2 +- src/tests/test_scenarioMonteCarloSpice.py | 13 +- src/tests/test_scenarioOrbitManeuver.py | 2 +- src/tests/test_scenarioOrbitMultiBody.py | 17 +- src/tests/test_scenarioPatchedConics.py | 15 +- src/tests/test_scenarioPowerDemo.py | 10 +- src/tests/test_scenarioRotatingPanel.py | 9 +- .../test_scenarioSatelliteConstellation.py | 18 +- .../test_scenarioSepMomentumManagement.py | 11 +- src/tests/test_scenarioSmallBodyLandmarks.py | 24 +- src/tests/test_scenarioSpacecraftLocation.py | 19 +- .../test_scenarioSpinningBodiesTwoDOF.py | 5 +- .../test_scenarioStationKeepingMultiSat.py | 17 +- src/tests/test_scenarioSweepingSpacecraft.py | 44 +- src/tests/test_scenarioTAM.py | 75 +- src/tests/test_scenarioTAMcomparison.py | 46 +- src/tests/test_scenarioTest.py | 60 +- ...est_scenarioVariableTimeStepIntegrators.py | 20 +- src/tests/test_scenarioVizPoint.py | 22 +- src/tests/test_utilitiesUKF.py | 212 +- src/utilities/MonteCarlo/AnalysisBaseClass.py | 304 ++- src/utilities/MonteCarlo/Controller.py | 238 +- src/utilities/MonteCarlo/DataWriter.py | 82 +- src/utilities/MonteCarlo/Dispersions.py | 303 ++- src/utilities/MonteCarlo/RetentionPolicy.py | 36 +- .../_UnitTests/test_scenarioBasicOrbitMC.py | 110 +- src/utilities/RigidBodyKinematics.py | 1355 ++++++------ src/utilities/SimulationBaseClass.py | 338 ++- src/utilities/SpherePlot.py | 69 +- src/utilities/astroFunctions.py | 507 +++-- src/utilities/bskExamples.py | 8 +- src/utilities/bskLargeData.py | 11 +- src/utilities/datasheet_RW.py | 60 +- src/utilities/fswSetupRW.py | 8 +- src/utilities/fswSetupThrusters.py | 8 +- src/utilities/fswSetupVSCMGs.py | 26 +- src/utilities/macros.py | 22 +- src/utilities/makeDraftModule.py | 949 ++++---- src/utilities/mujocoUtils/conanfile.py | 5 +- src/utilities/orbitalMotion.py | 743 +++++-- src/utilities/planetStates.py | 43 +- src/utilities/pyswice_ck_utilities.py | 77 +- src/utilities/pyswice_spk_utilities.py | 5 +- src/utilities/pythonVariableLogger.py | 26 +- src/utilities/quadMapSupport.py | 59 +- src/utilities/readAtmTable.py | 105 +- src/utilities/simIncludeGravBody.py | 88 +- src/utilities/simIncludeRW.py | 309 +-- src/utilities/simIncludeThruster.py | 117 +- src/utilities/simSetPlanetEnvironment.py | 92 +- src/utilities/simulationArchTypes.py | 2 +- src/utilities/simulationProgessBar.py | 10 +- src/utilities/tabulate.py | 517 +++-- .../tests/test_RigidBodyKinematics.py | 17 +- src/utilities/tests/test_ck_utilities.py | 50 +- src/utilities/tests/test_event_utilities.py | 9 +- src/utilities/unitTestSupport.py | 185 +- 521 files changed, 53975 insertions(+), 29384 deletions(-) diff --git a/conanfile.py b/conanfile.py index ec34cb6113..5f390afc11 100644 --- a/conanfile.py +++ b/conanfile.py @@ -18,7 +18,7 @@ from conan.tools.files import copy from pathlib import Path -sys.path.insert(1, './src/utilities/') +sys.path.insert(1, "./src/utilities/") import makeDraftModule # XXX: this statement is needed to enable Windows to print ANSI codes in the Terminal @@ -26,10 +26,10 @@ os.system("") # define the print color codes -statusColor = '\033[92m' -failColor = '\033[91m' -warningColor = '\033[93m' -endColor = '\033[0m' +statusColor = "\033[92m" +failColor = "\033[91m" +warningColor = "\033[93m" +endColor = "\033[0m" # # define BSK module option list (option name and default value) @@ -40,7 +40,6 @@ "buildProject": [[True, False], True], "pyPkgCanary": [[True, False], False], "recorderPropertyRollback": [[True, False], False], - # XXX: Set managePipEnvironment to True to keep the old behaviour of # managing the `pip` environment directly (upgrading, installing Python # packages, etc.). This behaviour is deprecated using the new pip-based @@ -50,25 +49,33 @@ "managePipEnvironment": [[True, False], True], } bskModuleOptionsString = { - "autoKey": [["", "u", "s","c"], ""], # TODO: Remove, used only for managePipEnvironment. + "autoKey": [ + ["", "u", "s", "c"], + "", + ], # TODO: Remove, used only for managePipEnvironment. "pathToExternalModules": [["ANY"], ""], "pyLimitedAPI": [["ANY"], ""], } bskModuleOptionsFlag = { "clean": [[True, False], False], - "allOptPkg": [[True, False], False] # TODO: Remove, used only for managePipEnvironment. + "allOptPkg": [ + [True, False], + False, + ], # TODO: Remove, used only for managePipEnvironment. } def is_running_virtual_env(): return sys.prefix != sys.base_prefix + required_conan_version = ">=2.0.5" + class BasiliskConan(ConanFile): name = "Basilisk" homepage = "https://avslab.github.io/basilisk/" - f = open('docs/source/bskVersion.txt', 'r') + f = open("docs/source/bskVersion.txt", "r") version = f.read() f.close() # generators = "CMakeDeps" @@ -115,7 +122,7 @@ def system_requirements(self): # managePipEnvironment (i.e. conanfile.py-based build). # ensure latest pip is installed - cmakeCmdString = f'{sys.executable} -m pip install --upgrade pip' + cmakeCmdString = f"{sys.executable} -m pip install --upgrade pip" print(statusColor + "Updating pip:" + endColor) print(cmakeCmdString) os.system(cmakeCmdString) @@ -123,22 +130,22 @@ def system_requirements(self): # TODO: Remove this: requirements and optional requirements are # installed automatically by add_basilisk_to_sys_path(). Only build # system requirements need to be installed here. - reqPath = '.github/workflows/' if self.options.get_safe("pyPkgCanary") else '' + reqPath = ".github/workflows/" if self.options.get_safe("pyPkgCanary") else "" - reqFile = open(f'{reqPath}requirements.txt', 'r') - required = reqFile.read().replace("`", "").split('\n') + reqFile = open(f"{reqPath}requirements.txt", "r") + required = reqFile.read().replace("`", "").split("\n") reqFile.close() pkgList = [x.lower() for x in required] - reqFile = open(f'{reqPath}requirements_dev.txt', 'r') - required = reqFile.read().replace("`", "").split('\n') + reqFile = open(f"{reqPath}requirements_dev.txt", "r") + required = reqFile.read().replace("`", "").split("\n") reqFile.close() pkgList += [x.lower() for x in required] checkStr = "Required" if self.options.get_safe("allOptPkg"): - optFile = open(f'{reqPath}requirements_doc.txt', 'r') - optionalPkgs = optFile.read().replace("`", "").split('\n') + optFile = open(f"{reqPath}requirements_doc.txt", "r") + optionalPkgs = optFile.read().replace("`", "").split("\n") optFile.close() optionalPkgs = [x.lower() for x in optionalPkgs] pkgList += optionalPkgs @@ -161,7 +168,8 @@ def system_requirements(self): print("Found: " + statusColor + elem + endColor) else: raise Exception( - f"Version conflict for {req.name}: {installed_version} does not satisfy {req.specifier}") + f"Version conflict for {req.name}: {installed_version} does not satisfy {req.specifier}" + ) except importlib.metadata.PackageNotFoundError: missing_packages.append(elem) except Exception as e: @@ -175,12 +183,18 @@ def system_requirements(self): if self.options.get_safe("autoKey"): choice = self.options.get_safe("autoKey") else: - choice = input(warningColor + f"Required python package " + elem + " is missing" + endColor + - "\nInstall for user (u), system (s) or cancel(c)? ") - if choice == 'c': + choice = input( + warningColor + + f"Required python package " + + elem + + " is missing" + + endColor + + "\nInstall for user (u), system (s) or cancel(c)? " + ) + if choice == "c": print(warningColor + "Skipping installing " + elem + endColor) continue - elif choice == 'u': + elif choice == "u": installCmd.append("--user") installCmd.append(elem) try: @@ -202,7 +216,7 @@ def requirements(self): self.requires("opencv/4.5.5") if self.options.get_safe("vizInterface") or self.options.get_safe("opNav"): - self.requires("protobuf/3.21.12") # For compatibility with openCV + self.requires("protobuf/3.21.12") # For compatibility with openCV self.requires("cppzmq/4.5.0") if self.options.get_safe("mujoco"): @@ -216,22 +230,29 @@ def configure(self): if os.path.exists(distPath): shutil.rmtree(distPath, ignore_errors=True) if self.settings.get_safe("build_type") == "Debug": - print(warningColor + "Build type is set to Debug. Performance will be significantly lower." + endColor) + print( + warningColor + + "Build type is set to Debug. Performance will be significantly lower." + + endColor + ) # Install additional opencv methods if self.options.get_safe("opNav"): - self.options['opencv'].contrib = True - self.options['opencv'].with_ffmpeg = False # video frame encoding lib - self.options['opencv'].gapi = False # graph manipulations framework - self.options['opencv'].with_tiff = False # generate image in TIFF format - self.options['opencv'].with_openexr = False # generate image in EXR format - self.options['opencv'].with_quirc = False # QR code lib - self.options['opencv'].with_webp = False # raster graphics file format for web + self.options["opencv"].contrib = True + self.options["opencv"].with_ffmpeg = False # video frame encoding lib + self.options["opencv"].gapi = False # graph manipulations framework + self.options["opencv"].with_tiff = False # generate image in TIFF format + self.options["opencv"].with_openexr = False # generate image in EXR format + self.options["opencv"].with_quirc = False # QR code lib + self.options[ + "opencv" + ].with_webp = False # raster graphics file format for web # Other dependency options if self.options.get_safe("vizInterface") or self.options.get_safe("opNav"): - self.options['zeromq'].encryption = False # Basilisk does not use data streaming encryption. - + self.options[ + "zeromq" + ].encryption = False # Basilisk does not use data streaming encryption. def package_id(self): if self.info.settings.compiler == "Visual Studio": @@ -241,10 +262,11 @@ def package_id(self): self.info.settings.compiler.runtime = "MT/MTd" def layout(self): - cmake_layout(self, - src_folder=str(self.options.get_safe("sourceFolder")), - build_folder=str(self.options.get_safe("buildFolder")) - ) + cmake_layout( + self, + src_folder=str(self.options.get_safe("sourceFolder")), + build_folder=str(self.options.get_safe("buildFolder")), + ) # XXX: Override the build folder again to keep it consistent between # multi- (e.g. Visual Studio) and single-config (e.g. Make) generators. @@ -267,24 +289,37 @@ def generate(self): copy(self, "*.dll", libdir, "../Basilisk") if self.options.get_safe("pathToExternalModules"): - print(statusColor + "Including External Folder: " + endColor + str(self.options.pathToExternalModules)) + print( + statusColor + + "Including External Folder: " + + endColor + + str(self.options.pathToExternalModules) + ) if self.settings.build_type == "Debug": - self.output.warning("Build type is set to Debug. Performance will be significantly slower.") + self.output.warning( + "Build type is set to Debug. Performance will be significantly slower." + ) # ------------------------------------------------------------- # Run the CMake configuration generators. # ------------------------------------------------------------- deps = CMakeDeps(self) - deps.set_property("eigen", "cmake_target_name", "Eigen3::Eigen3") # XXX: Override, original is "Eigen3::Eigen" - deps.set_property("cppzmq", "cmake_target_name", "cppzmq::cppzmq") # XXX: Override, original is "cppzmq" + deps.set_property( + "eigen", "cmake_target_name", "Eigen3::Eigen3" + ) # XXX: Override, original is "Eigen3::Eigen" + deps.set_property( + "cppzmq", "cmake_target_name", "cppzmq::cppzmq" + ) # XXX: Override, original is "cppzmq" deps.generate() tc = CMakeToolchain(self) generatorString = str(self.options.get_safe("generator")) if generatorString == "": # Select default generator supplied to cmake based on os - if self.settings.os == "Macos" and not self.options.get_safe("buildProject"): + if self.settings.os == "Macos" and not self.options.get_safe( + "buildProject" + ): generatorString = "Xcode" tc.generator = generatorString elif self.settings.os == "Windows": @@ -293,7 +328,9 @@ def generate(self): self.options["*"].shared = True else: print("Creating a make file for project. ") - print("Specify your own using the -o generator='Name' flag during conan install") + print( + "Specify your own using the -o generator='Name' flag during conan install" + ) else: tc.generator = generatorString if self.settings.os == "Windows": @@ -301,15 +338,24 @@ def generate(self): print("cmake generator set to: " + statusColor + generatorString + endColor) tc.cache_variables["BUILD_OPNAV"] = bool(self.options.get_safe("opNav")) - tc.cache_variables["BUILD_VIZINTERFACE"] = bool(self.options.get_safe("vizInterface")) + tc.cache_variables["BUILD_VIZINTERFACE"] = bool( + self.options.get_safe("vizInterface") + ) tc.cache_variables["BUILD_MUJOCO"] = bool(self.options.get_safe("mujoco")) if self.options.get_safe("pathToExternalModules"): - tc.cache_variables["EXTERNAL_MODULES_PATH"] = Path(str(self.options.pathToExternalModules)).resolve().as_posix() - tc.cache_variables["PYTHON_VERSION"] = f"{sys.version_info.major}.{sys.version_info.minor}.{sys.version_info.micro}" - tc.cache_variables["RECORDER_PROPERTY_ROLLBACK"] = "1" if self.options.get_safe("recorderPropertyRollback") else "0" + tc.cache_variables["EXTERNAL_MODULES_PATH"] = ( + Path(str(self.options.pathToExternalModules)).resolve().as_posix() + ) + tc.cache_variables["PYTHON_VERSION"] = ( + f"{sys.version_info.major}.{sys.version_info.minor}.{sys.version_info.micro}" + ) + tc.cache_variables["RECORDER_PROPERTY_ROLLBACK"] = ( + "1" if self.options.get_safe("recorderPropertyRollback") else "0" + ) # get the header directory for numpy import numpy + tc.cache_variables["NUMPY_INCLUDE_DIR"] = numpy.get_include() # Set the build rpath, since we don't install the targets, so that the @@ -325,7 +371,6 @@ def generate(self): tc.generate() def build(self): - cmake = CMake(self) print(statusColor + "Configuring cmake..." + endColor) cmake.configure() @@ -339,7 +384,9 @@ def build(self): start = datetime.now() cmake.build() print("Total Build Time: " + str(datetime.now() - start)) - print(f"{statusColor}The Basilisk build is successful and the scripts are ready to run{endColor}") + print( + f"{statusColor}The Basilisk build is successful and the scripts are ready to run{endColor}" + ) # On Windows, copy project-built DLLs next to the Python extension modules # so they are bundled in the wheel and resolvable at runtime without PATH tweaks. if self.settings.os == "Windows": @@ -359,7 +406,10 @@ def build(self): # As a fallback, scan the build tree for any remaining DLLs. for root, _dirs, files in os.walk(self.build_folder): # Skip the destination to avoid self-copy - if os.path.commonpath([root, basilisk_dst_root]) == basilisk_dst_root: + if ( + os.path.commonpath([root, basilisk_dst_root]) + == basilisk_dst_root + ): continue if any(f.lower().endswith(".dll") for f in files): try: @@ -378,11 +428,15 @@ def build(self): else: print(f"{statusColor}Finished configuring the Basilisk project.{endColor}") if self.settings.os != "Linux": - print(f"{statusColor}Please open project file inside dist3 with IDE " - f"and build the project for {self.settings.build_type}{endColor}") + print( + f"{statusColor}Please open project file inside dist3 with IDE " + f"and build the project for {self.settings.build_type}{endColor}" + ) else: - print(f"{statusColor}Please go to dist3 folder and run command " - f"`make -j `{endColor}") + print( + f"{statusColor}Please go to dist3 folder and run command " + f"`make -j `{endColor}" + ) return def add_basilisk_to_sys_path(self): @@ -390,44 +444,70 @@ def add_basilisk_to_sys_path(self): # NOTE: "--no-build-isolation" is used here only to force pip to use the # packages installed directly by this Conanfile (using the # "managePipEnvironment" option). Otherwise, it is not necessary. - add_basilisk_module_command = [sys.executable, "-m", "pip", "install", "--no-build-isolation", "-e", "../"] + add_basilisk_module_command = [ + sys.executable, + "-m", + "pip", + "install", + "--no-build-isolation", + "-e", + "../", + ] if self.options.get_safe("allOptPkg"): # Install the optional requirements as well add_basilisk_module_command[-1] = ".[optional]" - if not is_running_virtual_env() and self.options.get_safe("autoKey") != 's': + if not is_running_virtual_env() and self.options.get_safe("autoKey") != "s": add_basilisk_module_command.append("--user") - process = subprocess.Popen(add_basilisk_module_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + process = subprocess.Popen( + add_basilisk_module_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE + ) output, err = process.communicate() if process.returncode: - print("Error %s while running %s" % (err.decode(), add_basilisk_module_command)) + print( + "Error %s while running %s" + % (err.decode(), add_basilisk_module_command) + ) sys.exit(1) else: print("This resulted in the stdout: \n%s" % output.decode()) if err.decode() != "": print("This resulted in the stderr: \n%s" % err.decode()) + def get_mujoco_version(): with open("./libs/mujoco/version.txt") as f: return f.read().strip() + def is_conan_package_available(ref: str): """ Run 'conan list' and return True if package exists in local or remote caches. """ try: output = subprocess.check_output( - [sys.executable, "-m", "conans.conan", "list", ref, "-c", "-f", "json", "-verror"], + [ + sys.executable, + "-m", + "conans.conan", + "list", + ref, + "-c", + "-f", + "json", + "-verror", + ], stderr=subprocess.STDOUT, - universal_newlines=True + universal_newlines=True, ) parsed = json.loads(output) - return any( "error" not in v for v in parsed.values() ) + return any("error" not in v for v in parsed.values()) except subprocess.CalledProcessError: return False + def conan_create_mujoco(print_fn: Optional[Callable[[str], None]] = print): """ If the 'mujoco/VERSION' package is not found in any remote or the local cache, @@ -439,18 +519,32 @@ def conan_create_mujoco(print_fn: Optional[Callable[[str], None]] = print): if print_fn is not None: print_fn(f"Package {ref} not found locally, creating it...") # Run 'conan create' in the external recipe directory - subprocess.run([sys.executable, "-m", "conans.conan", "create", ".", "-s" ,"compiler.cppstd=17"], cwd="./libs/mujoco" ) + subprocess.run( + [ + sys.executable, + "-m", + "conans.conan", + "create", + ".", + "-s", + "compiler.cppstd=17", + ], + cwd="./libs/mujoco", + ) else: if print_fn is not None: print_fn(f"Package {ref} already available, skipping creation.") + if __name__ == "__main__": # make sure conan is configured to use the libstdc++11 by default # XXX: This needs to be run before dispatching to Conan (i.e. outside of the # ConanFile object), because it affects the configuration of the first run. # (Running it here fixes https://github.com/AVSLab/basilisk/issues/525) try: - subprocess.check_output([sys.executable, "-m", "conans.conan", "profile", "detect", "--exist-ok"]) + subprocess.check_output( + [sys.executable, "-m", "conans.conan", "profile", "detect", "--exist-ok"] + ) except: # if profile already exists the above command returns an error. Just ignore in this # case. We don't want to overwrite an existing profile file @@ -460,24 +554,46 @@ def conan_create_mujoco(print_fn: Optional[Callable[[str], None]] = print): parser = argparse.ArgumentParser(description="Configure the Basilisk framework.") # define the optional arguments parser.add_argument("--generator", help="cmake generator") - parser.add_argument("--buildType", help="build type", default="Release", choices=["Release", "Debug"]) - parser.add_argument("--mujocoReplay", - help="Whether to build the 'replay' utility for visualizing MuJoCo results", - default=False, - type=lambda x: (str(x).lower() == 'true'), - choices=[True, False]) + parser.add_argument( + "--buildType", + help="build type", + default="Release", + choices=["Release", "Debug"], + ) + parser.add_argument( + "--mujocoReplay", + help="Whether to build the 'replay' utility for visualizing MuJoCo results", + default=False, + type=lambda x: (str(x).lower() == "true"), + choices=[True, False], + ) # parser.add_argument("--clean", help="make a clean distribution folder", action="store_true") for opt, value in bskModuleOptionsBool.items(): - parser.add_argument("--" + opt, help="build modules for " + opt + " behavior", default=value[1], - type=lambda x: (str(x).lower() == 'true')) + parser.add_argument( + "--" + opt, + help="build modules for " + opt + " behavior", + default=value[1], + type=lambda x: (str(x).lower() == "true"), + ) for opt, value in bskModuleOptionsString.items(): - parser.add_argument("--" + opt, help="using string option for " + opt, default=value[1]) + parser.add_argument( + "--" + opt, help="using string option for " + opt, default=value[1] + ) for opt, value in bskModuleOptionsFlag.items(): if sys.version_info < (3, 9, 0): - parser.add_argument("--" + opt, help="using flag option for " + opt, default=value[1], action='store_true') + parser.add_argument( + "--" + opt, + help="using flag option for " + opt, + default=value[1], + action="store_true", + ) else: - parser.add_argument("--" + opt, help="using flag option for " + opt, default=value[1], - action=argparse.BooleanOptionalAction) + parser.add_argument( + "--" + opt, + help="using flag option for " + opt, + default=value[1], + action=argparse.BooleanOptionalAction, + ) args = parser.parse_args() # set the build destination folder @@ -501,24 +617,42 @@ def conan_create_mujoco(print_fn: Optional[Callable[[str], None]] = print): conan_create_mujoco() if args.mujocoReplay: - print(f"{statusColor}Building 'replay' tool, since '--mujocoReplay true' was used") + print( + f"{statusColor}Building 'replay' tool, since '--mujocoReplay true' was used" + ) try: - subprocess.check_output([sys.executable, "-m", "conans.conan", "build", ".", "-s" ,"compiler.cppstd=17", "--build=missing"], cwd="./src/utilities/mujocoUtils" ) + subprocess.check_output( + [ + sys.executable, + "-m", + "conans.conan", + "build", + ".", + "-s", + "compiler.cppstd=17", + "--build=missing", + ], + cwd="./src/utilities/mujocoUtils", + ) except: raise RuntimeError("Failed to install MuJoCo replay! See error above.") # setup conan install command arguments conanInstallList = list() - conanInstallList.append(f'{sys.executable} -m conans.conan install . --build=missing') - conanInstallList.append(' -s build_type=' + str(args.buildType)) - conanInstallList.append(' -s compiler.cppstd=17') + conanInstallList.append( + f"{sys.executable} -m conans.conan install . --build=missing" + ) + conanInstallList.append(" -s build_type=" + str(args.buildType)) + conanInstallList.append(" -s compiler.cppstd=17") conanBuildOptionsList = list() # setup list of conan build arguments - conanBuildOptionsList.append(' -s compiler.cppstd=17') + conanBuildOptionsList.append(" -s compiler.cppstd=17") if args.generator: conanBuildOptionsList.append(' -o "&:generator=' + str(args.generator) + '"') for opt, value in bskModuleOptionsBool.items(): - conanBuildOptionsList.append(' -o "&:' + opt + '=' + str(vars(args)[opt]) + '"') - conanInstallList.append(''.join(conanBuildOptionsList)) # argument get used in both install and build + conanBuildOptionsList.append(' -o "&:' + opt + "=" + str(vars(args)[opt]) + '"') + conanInstallList.append( + "".join(conanBuildOptionsList) + ) # argument get used in both install and build # Most of these options go to both conan install and build commands for opt, value in bskModuleOptionsString.items(): @@ -526,28 +660,38 @@ def conan_create_mujoco(print_fn: Optional[Callable[[str], None]] = print): if opt == "pathToExternalModules": externalPath = os.path.abspath(str(vars(args)[opt]).rstrip(os.path.sep)) if os.path.exists(externalPath): - conanInstallList.append(' -o "&:' + opt + '=' + externalPath + '"') - conanBuildOptionsList.append(' -o "&:' + opt + '=' + externalPath + '"') + conanInstallList.append(' -o "&:' + opt + "=" + externalPath + '"') + conanBuildOptionsList.append( + ' -o "&:' + opt + "=" + externalPath + '"' + ) else: - print(f"{failColor}Error: path {str(vars(args)[opt])} does not exist{endColor}") + print( + f"{failColor}Error: path {str(vars(args)[opt])} does not exist{endColor}" + ) sys.exit(1) else: if opt != "autoKey": - conanInstallList.append(' -o "&:' + opt + '=' + str(vars(args)[opt]) + '"') - conanBuildOptionsList.append(' -o "&:' + opt + '=' + str(vars(args)[opt]) + '"') + conanInstallList.append( + ' -o "&:' + opt + "=" + str(vars(args)[opt]) + '"' + ) + conanBuildOptionsList.append( + ' -o "&:' + opt + "=" + str(vars(args)[opt]) + '"' + ) # only used for conan install arguments, not build for opt, value in bskModuleOptionsFlag.items(): if vars(args)[opt]: conanInstallList.append(' -o "&:' + opt + '=True"') - conanInstallString = ''.join(conanInstallList) + conanInstallString = "".join(conanInstallList) print(statusColor + "Running conan install:" + endColor) print(conanInstallString) completedProcess = subprocess.run(conanInstallString, shell=True, check=True) # run conan build - buildCmdString = f'{sys.executable} -m conans.conan build . ' + ''.join(conanBuildOptionsList) + buildCmdString = f"{sys.executable} -m conans.conan build . " + "".join( + conanBuildOptionsList + ) print(statusColor + "Running conan build:" + endColor) print(buildCmdString) completedProcess = subprocess.run(buildCmdString, shell=True, check=True) diff --git a/docs/source/codeSamples/bsk-1.py b/docs/source/codeSamples/bsk-1.py index ad7a26a66f..25d97534c3 100644 --- a/docs/source/codeSamples/bsk-1.py +++ b/docs/source/codeSamples/bsk-1.py @@ -33,9 +33,9 @@ def run(): fswProcess = scSim.CreateNewProcess("fswProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(5.))) - dynProcess.addTask(scSim.CreateNewTask("sensorTask", macros.sec2nano(10.))) - fswProcess.addTask(scSim.CreateNewTask("fswTask", macros.sec2nano(10.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(5.0))) + dynProcess.addTask(scSim.CreateNewTask("sensorTask", macros.sec2nano(10.0))) + fswProcess.addTask(scSim.CreateNewTask("fswTask", macros.sec2nano(10.0))) # initialize Simulation: scSim.InitializeSimulation() diff --git a/docs/source/codeSamples/bsk-2.py b/docs/source/codeSamples/bsk-2.py index 163323be97..bb9c64d887 100644 --- a/docs/source/codeSamples/bsk-2.py +++ b/docs/source/codeSamples/bsk-2.py @@ -34,7 +34,7 @@ def run(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(5.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(5.0))) # create copies of the Basilisk modules mod1 = cModuleTemplate.cModuleTemplate() diff --git a/docs/source/codeSamples/bsk-2a.py b/docs/source/codeSamples/bsk-2a.py index e684730ddb..d82701bde9 100644 --- a/docs/source/codeSamples/bsk-2a.py +++ b/docs/source/codeSamples/bsk-2a.py @@ -33,7 +33,7 @@ def run(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.0))) # create modules mod1 = cModuleTemplate.cModuleTemplate() diff --git a/docs/source/codeSamples/bsk-2b.py b/docs/source/codeSamples/bsk-2b.py index 72612e6649..0d5a3c9e28 100644 --- a/docs/source/codeSamples/bsk-2b.py +++ b/docs/source/codeSamples/bsk-2b.py @@ -34,12 +34,12 @@ def run(): fswProcess = scSim.CreateNewProcess("fswProcess", 10) # create the dynamics task and specify the integration update time - fswProcess.addTask(scSim.CreateNewTask("fswTask1", macros.sec2nano(1.))) - fswProcess.addTask(scSim.CreateNewTask("fswTask2", macros.sec2nano(2.))) - fswProcess.addTask(scSim.CreateNewTask("fswTask3", macros.sec2nano(3.)), 10) - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask1", macros.sec2nano(1.))) - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask2", macros.sec2nano(5.)), 10) - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask3", macros.sec2nano(10.))) + fswProcess.addTask(scSim.CreateNewTask("fswTask1", macros.sec2nano(1.0))) + fswProcess.addTask(scSim.CreateNewTask("fswTask2", macros.sec2nano(2.0))) + fswProcess.addTask(scSim.CreateNewTask("fswTask3", macros.sec2nano(3.0)), 10) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask1", macros.sec2nano(1.0))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask2", macros.sec2nano(5.0)), 10) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask3", macros.sec2nano(10.0))) # create modules mod1 = cModuleTemplate.cModuleTemplate() diff --git a/docs/source/codeSamples/bsk-3.py b/docs/source/codeSamples/bsk-3.py index 3f6e270089..2b1f1143e4 100644 --- a/docs/source/codeSamples/bsk-3.py +++ b/docs/source/codeSamples/bsk-3.py @@ -35,7 +35,7 @@ def run(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(5.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(5.0))) # create modules mod1 = cModuleTemplate.cModuleTemplate() diff --git a/docs/source/codeSamples/bsk-4.py b/docs/source/codeSamples/bsk-4.py index d2fdf02345..bbc305f4c8 100644 --- a/docs/source/codeSamples/bsk-4.py +++ b/docs/source/codeSamples/bsk-4.py @@ -37,7 +37,7 @@ def run(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.0))) # create modules mod1 = cModuleTemplate.cModuleTemplate() @@ -48,7 +48,7 @@ def run(): # setup message recording msgRec = mod1.dataOutMsg.recorder() scSim.AddModelToTask("dynamicsTask", msgRec) - msgRec2 = mod1.dataOutMsg.recorder(macros.sec2nano(20.)) + msgRec2 = mod1.dataOutMsg.recorder(macros.sec2nano(20.0)) scSim.AddModelToTask("dynamicsTask", msgRec2) # initialize Simulation: @@ -63,16 +63,22 @@ def run(): plt.figure(1) figureList = {} for idx in range(3): - plt.plot(msgRec.times() * macros.NANO2SEC, msgRec.dataVector[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$x_{' + str(idx) + '}$') - plt.plot(msgRec2.times() * macros.NANO2SEC, msgRec2.dataVector[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\hat x_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [sec]') - plt.ylabel('Module Data [units]') + plt.plot( + msgRec.times() * macros.NANO2SEC, + msgRec.dataVector[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$x_{" + str(idx) + "}$", + ) + plt.plot( + msgRec2.times() * macros.NANO2SEC, + msgRec2.dataVector[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\hat x_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [sec]") + plt.ylabel("Module Data [units]") if "pytest" not in sys.modules: plt.show() figureList["bsk-4"] = plt.figure(1) diff --git a/docs/source/codeSamples/bsk-5.py b/docs/source/codeSamples/bsk-5.py index cf4adca0c9..b93f651d68 100644 --- a/docs/source/codeSamples/bsk-5.py +++ b/docs/source/codeSamples/bsk-5.py @@ -38,7 +38,7 @@ def run(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.0))) # create modules mod1 = cppModuleTemplate.CppModuleTemplate() @@ -46,7 +46,7 @@ def run(): scSim.AddModelToTask("dynamicsTask", mod1) # create stand-alone input message - msgData = messaging.CModuleTemplateMsgPayload(dataVector = [1., 2., 3.]) + msgData = messaging.CModuleTemplateMsgPayload(dataVector=[1.0, 2.0, 3.0]) msg = messaging.CModuleTemplateMsg().write(msgData) # connect to stand-alone msg @@ -64,7 +64,7 @@ def run(): scSim.ExecuteSimulation() # change input message and continue simulation - msgData.dataVector = [-1., -2., -3.] + msgData.dataVector = [-1.0, -2.0, -3.0] msg.write(msgData) scSim.ConfigureStopTime(macros.sec2nano(20.0)) scSim.ExecuteSimulation() @@ -74,12 +74,15 @@ def run(): figureList = {} plt.figure(1) for idx in range(3): - plt.plot(msgRec.times() * macros.NANO2SEC, msgRec.dataVector[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [sec]') - plt.ylabel('Module Data [units]') + plt.plot( + msgRec.times() * macros.NANO2SEC, + msgRec.dataVector[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [sec]") + plt.ylabel("Module Data [units]") figureList["bsk-5"] = plt.figure(1) if "pytest" not in sys.modules: plt.show() diff --git a/docs/source/codeSamples/bsk-6.py b/docs/source/codeSamples/bsk-6.py index 233934cb43..bef48f945d 100644 --- a/docs/source/codeSamples/bsk-6.py +++ b/docs/source/codeSamples/bsk-6.py @@ -34,7 +34,7 @@ def run(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.0))) # create modules mod1 = cModuleTemplate.cModuleTemplate() @@ -47,12 +47,12 @@ def run(): # set module variables mod1.dummy = 1 - mod1.dumVector = [1., 2., 3.] + mod1.dumVector = [1.0, 2.0, 3.0] mod2.setDummy(1) - mod2.setDumVector([1., 2., 3.]) + mod2.setDumVector([1.0, 2.0, 3.0]) # request these module variables to be recorded - mod1Logger = mod1.logger("dummy", macros.sec2nano(1.)) + mod1Logger = mod1.logger("dummy", macros.sec2nano(1.0)) scSim.AddModelToTask("dynamicsTask", mod1Logger) mod2Logger = mod2.logger(["dummy", "dumVector"]) scSim.AddModelToTask("dynamicsTask", mod2Logger) @@ -75,5 +75,6 @@ def run(): print("mod2.getDumVector():") print(mod2Logger.dumVector) + if __name__ == "__main__": run() diff --git a/docs/source/codeSamples/bsk-7.py b/docs/source/codeSamples/bsk-7.py index 3ac72f4cb1..18952feecc 100644 --- a/docs/source/codeSamples/bsk-7.py +++ b/docs/source/codeSamples/bsk-7.py @@ -35,7 +35,7 @@ def run(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.0))) # create modules mod1 = cModuleTemplate.cModuleTemplate() diff --git a/docs/source/codeSamples/bsk-8.py b/docs/source/codeSamples/bsk-8.py index cf8e7f0d3a..f1d342ad6a 100644 --- a/docs/source/codeSamples/bsk-8.py +++ b/docs/source/codeSamples/bsk-8.py @@ -34,8 +34,8 @@ def run(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("cTask", macros.sec2nano(1.))) - dynProcess.addTask(scSim.CreateNewTask("cppTask", macros.sec2nano(1.))) + dynProcess.addTask(scSim.CreateNewTask("cTask", macros.sec2nano(1.0))) + dynProcess.addTask(scSim.CreateNewTask("cppTask", macros.sec2nano(1.0))) # create modules mod1 = cModuleTemplate.cModuleTemplate() diff --git a/docs/source/codeSamples/making-pyModules.py b/docs/source/codeSamples/making-pyModules.py index 2d746ba0d4..25c672b380 100644 --- a/docs/source/codeSamples/making-pyModules.py +++ b/docs/source/codeSamples/making-pyModules.py @@ -116,7 +116,7 @@ def UpdateState(self, CurrentSimNanos): self.bskLogger.bskLog( bskLogging.BSK_INFORMATION, - f"Python Module ID {self.moduleID} ran Update at {CurrentSimNanos*1e-9}s", + f"Python Module ID {self.moduleID} ran Update at {CurrentSimNanos * 1e-9}s", ) diff --git a/docs/source/conf.py b/docs/source/conf.py index 8faf86ac5c..6e889d244b 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -9,6 +9,7 @@ # -- Path setup -------------------------------------------------------------- import datetime + # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. @@ -21,11 +22,13 @@ from docutils import nodes from docutils.parsers.rst import roles + def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): - node = nodes.inline(rawtext, f"[BETA] {text}", classes=['beta-label']) + node = nodes.inline(rawtext, f"[BETA] {text}", classes=["beta-label"]) return [node], [] -roles.register_local_role('beta', beta_role) + +roles.register_local_role("beta", beta_role) # # create RST showing supportData folder information @@ -55,8 +58,10 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): f.write("Support Data Files\n") f.write("==================\n\n") f.write(".. note::\n\n") - f.write(" This folder contains a listing of all the data files in the folder ``basilisk/supportData`` " - "that are packaged into Basilisk.\n\n") + f.write( + " This folder contains a listing of all the data files in the folder ``basilisk/supportData`` " + "that are packaged into Basilisk.\n\n" + ) # Sort folders alphabetically and write each section for folder in sorted(folder_files.keys()): @@ -97,38 +102,39 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): # -- Project information ----------------------------------------------------- now = datetime.datetime.now() -f = open('bskVersion.txt', 'r') +f = open("bskVersion.txt", "r") bskVersion = f.read() f.close() -project = u'Basilisk' -copyright = str(now.year) + u', Autonomous Vehicle Systems (AVS) Laboratory' -author = u'AVS Lab' +project = "Basilisk" +copyright = str(now.year) + ", Autonomous Vehicle Systems (AVS) Laboratory" +author = "AVS Lab" release = bskVersion -version = u'version ' + release +version = "version " + release from Basilisk.utilities.deprecated import BSKDeprecationWarning import warnings + warnings.filterwarnings("ignore", category=BSKDeprecationWarning) # -- General configuration --------------------------------------------------- # If your documentation needs a minimal Sphinx version, state it here. # -needs_sphinx = '7.0' +needs_sphinx = "7.0" # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.mathjax', - 'sphinx.ext.viewcode', - 'sphinx.ext.napoleon', + "sphinx.ext.autodoc", + "sphinx.ext.mathjax", + "sphinx.ext.viewcode", + "sphinx.ext.napoleon", "sphinx_rtd_theme", - 'recommonmark', - 'breathe', - 'sphinx_copybutton' + "recommonmark", + "breathe", + "sphinx_copybutton", ] # filter out terminal prompt text from the code blocks @@ -136,21 +142,21 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): copybutton_prompt_is_regexp = True breathe_doxygen_config_options = { - 'WARN_AS_ERROR': 'YES' - , 'WARN_IF_UNDOCUMENTED': 'YES' # Ensure undocumented variables, functions, etc., raise warnings + "WARN_AS_ERROR": "YES", + "WARN_IF_UNDOCUMENTED": "YES", # Ensure undocumented variables, functions, etc., raise warnings } # Add any paths that contain templates here, relative to this directory. -#templates_path = ['_templates'] +# templates_path = ['_templates'] # The suffix(es) of source filenames. # You can specify multiple suffix as a list of string: # -#source_suffix = ['.rst', '.md', '.svg'] -source_suffix = {'.rst': 'restructuredtext'} +# source_suffix = ['.rst', '.md', '.svg'] +source_suffix = {".rst": "restructuredtext"} # The master toctree document. -master_doc = 'index' +master_doc = "index" # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -163,15 +169,15 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. exclude_patterns = [ - 'examples/BskSim/scenarios/index.rst', - 'examples/BskSim/index.rst', - 'examples/MultiSatBskSim/scenariosMultiSat/index.rst', - 'examples/MultiSatBskSim/index.rst', - 'examples/OpNavScenarios/scenariosOpNav/index.rst', - 'examples/OpNavScenarios/scenariosOpNav/CNN_ImageGen/index.rst', - 'examples/OpNavScenarios/scenariosOpNav/OpNavMC/index.rst', - 'examples/OpNavScenarios/index.rst', - 'examples/mujoco/index.rst', + "examples/BskSim/scenarios/index.rst", + "examples/BskSim/index.rst", + "examples/MultiSatBskSim/scenariosMultiSat/index.rst", + "examples/MultiSatBskSim/index.rst", + "examples/OpNavScenarios/scenariosOpNav/index.rst", + "examples/OpNavScenarios/scenariosOpNav/CNN_ImageGen/index.rst", + "examples/OpNavScenarios/scenariosOpNav/OpNavMC/index.rst", + "examples/OpNavScenarios/index.rst", + "examples/mujoco/index.rst", ] # The name of the Pygments (syntax highlighting) style to use. @@ -189,21 +195,21 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): # further. For a list of options available for each theme, see the # documentation. html_theme_options = { - 'logo_only': False, - 'prev_next_buttons_location': 'bottom', - 'style_external_links': True, - 'vcs_pageview_mode': '', - 'style_nav_header_background': '#CFB87C', + "logo_only": False, + "prev_next_buttons_location": "bottom", + "style_external_links": True, + "vcs_pageview_mode": "", + "style_nav_header_background": "#CFB87C", # Toc options - 'collapse_navigation': True, - 'sticky_navigation': True, - 'navigation_depth': 4, - 'includehidden': True, - 'titles_only': False + "collapse_navigation": True, + "sticky_navigation": True, + "navigation_depth": 4, + "includehidden": True, + "titles_only": False, } html_css_files = [ - 'css/custom.css', + "css/custom.css", ] html_logo = "./_images/static/Basilisk-Logo.png" @@ -211,13 +217,12 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] # Custom sidebar templates, must be a dictionary that maps document names # to template names. - # # The default sidebars (for documents that don't match any pattern) are # defined by theme itself. Builtin themes are using these templates by @@ -230,7 +235,7 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): # -- Options for HTMLHelp output --------------------------------------------- # Output file base name for HTML help builder. -htmlhelp_basename = 'Basiliskdoc' +htmlhelp_basename = "Basiliskdoc" # -- Options for LaTeX output ------------------------------------------------ @@ -239,15 +244,12 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): # The paper size ('letterpaper' or 'a4paper'). # # 'papersize': 'letterpaper', - # The font size ('10pt', '11pt' or '12pt'). # # 'pointsize': '10pt', - # Additional stuff for the LaTeX preamble. # # 'preamble': '', - # Latex figure (float) alignment # # 'figure_align': 'htbp', @@ -257,8 +259,7 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): # (source start file, target name, title, # author, documentclass [howto, manual, or own class]). latex_documents = [ - (master_doc, 'Basilisk.tex', u'Basilisk Documentation', - u'AVS Lab', 'manual'), + (master_doc, "Basilisk.tex", "Basilisk Documentation", "AVS Lab", "manual"), ] @@ -266,10 +267,7 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): # One entry per manual page. List of tuples # (source start file, name, description, authors, manual section). -man_pages = [ - (master_doc, 'basilisk', u'Basilisk Documentation', - [author], 1) -] +man_pages = [(master_doc, "basilisk", "Basilisk Documentation", [author], 1)] # -- Options for Texinfo output ---------------------------------------------- @@ -278,9 +276,15 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): # (source start file, target name, title, author, # dir menu entry, description, category) texinfo_documents = [ - (master_doc, 'Basilisk', u'Basilisk Documentation', - author, 'Basilisk', 'One line description of project.', - 'Miscellaneous'), + ( + master_doc, + "Basilisk", + "Basilisk Documentation", + author, + "Basilisk", + "One line description of project.", + "Miscellaneous", + ), ] @@ -299,7 +303,7 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): # epub_uid = '' # A list of files that should not be packed into the epub file. -epub_exclude_files = ['search.html'] +epub_exclude_files = ["search.html"] # -- Extension configuration ------------------------------------------------- @@ -310,36 +314,38 @@ def beta_role(name, rawtext, text, lineno, inliner, options={}, content=[]): from glob import glob import shutil -class fileCrawler(): + +class fileCrawler: def __init__(self, newFiles=False): self.newFiles = newFiles self.breathe_projects_source = {} self.counter = 0 - def grabRelevantFiles(self,dir_path): - dirs_in_dir = glob(dir_path + '*/') + def grabRelevantFiles(self, dir_path): + dirs_in_dir = glob(dir_path + "*/") files_in_dir = glob(dir_path + "*.h") files_in_dir.extend(glob(dir_path + "*.hpp")) files_in_dir.extend(glob(dir_path + "*.c")) files_in_dir.extend(glob(dir_path + "*.cpp")) files_in_dir.extend(glob(dir_path + "*.py")) - # Remove any directories that shouldn't be added directly to the website removeList = [] for i in range(len(dirs_in_dir)): - if "_Documentation" in dirs_in_dir[i] or \ - "__pycache__" in dirs_in_dir[i] or \ - "_VizFiles" in dirs_in_dir[i] or \ - "Support" in dirs_in_dir[i] or \ - "cmake" in dirs_in_dir[i] or \ - "topLevelModules" in dirs_in_dir[i] or \ - "outputFiles" in dirs_in_dir[i] or \ - "msgAutoSource" in dirs_in_dir[i] or \ - "alg_contain" in dirs_in_dir[i] or \ - "dataForExamples" in dirs_in_dir[i] or \ - "tests" in dirs_in_dir[i] or \ - "mujocoUtils" in dirs_in_dir[i]: + if ( + "_Documentation" in dirs_in_dir[i] + or "__pycache__" in dirs_in_dir[i] + or "_VizFiles" in dirs_in_dir[i] + or "Support" in dirs_in_dir[i] + or "cmake" in dirs_in_dir[i] + or "topLevelModules" in dirs_in_dir[i] + or "outputFiles" in dirs_in_dir[i] + or "msgAutoSource" in dirs_in_dir[i] + or "alg_contain" in dirs_in_dir[i] + or "dataForExamples" in dirs_in_dir[i] + or "tests" in dirs_in_dir[i] + or "mujocoUtils" in dirs_in_dir[i] + ): removeList.extend([i]) for i in sorted(removeList, reverse=True): del dirs_in_dir[i] @@ -347,16 +353,18 @@ def grabRelevantFiles(self,dir_path): # Remove unnecessary source files (all files except .py, .c, .cpp, .h) removeList = [] for i in range(len(files_in_dir)): - if "__init__" in files_in_dir[i] or \ - "conftest.py" in files_in_dir[i] or \ - "*.xml" in files_in_dir[i] or \ - "vizMessage.pb.cc" in files_in_dir[i] or \ - "vizMessage.pb.h" in files_in_dir[i] or \ - "vizMessage.proto" in files_in_dir[i] or \ - "EGM9615.h" in files_in_dir[i] or \ - "SunLineKF_test_utilities.py" in files_in_dir[i] or \ - "datashader_utilities.py" in files_in_dir[i] or \ - "reportconf.py" in files_in_dir[i]: + if ( + "__init__" in files_in_dir[i] + or "conftest.py" in files_in_dir[i] + or "*.xml" in files_in_dir[i] + or "vizMessage.pb.cc" in files_in_dir[i] + or "vizMessage.pb.h" in files_in_dir[i] + or "vizMessage.proto" in files_in_dir[i] + or "EGM9615.h" in files_in_dir[i] + or "SunLineKF_test_utilities.py" in files_in_dir[i] + or "datashader_utilities.py" in files_in_dir[i] + or "reportconf.py" in files_in_dir[i] + ): removeList.extend([i]) for i in sorted(removeList, reverse=True): del files_in_dir[i] @@ -367,7 +375,7 @@ def grabRelevantFiles(self,dir_path): return paths_in_dir - def seperateFilesAndDirs(self,paths): + def seperateFilesAndDirs(self, paths): files = [] dirs = [] for path in paths: @@ -378,7 +386,6 @@ def seperateFilesAndDirs(self,paths): return sorted(files), sorted(dirs) def populateDocIndex(self, index_path, file_paths, dir_paths): - # get the folder name name = os.path.basename(os.path.normpath(index_path)) lines = "" @@ -386,17 +393,17 @@ def populateDocIndex(self, index_path, file_paths, dir_paths): # if a _default.rst file exists in a folder, then use it to generate the index.rst file try: pathToFolder, folderName = dir_paths[0].split(name) - docFileName = os.path.join(os.path.join(pathToFolder, name), '_default.rst') - with open(docFileName, 'r') as docFile: + docFileName = os.path.join(os.path.join(pathToFolder, name), "_default.rst") + with open(docFileName, "r") as docFile: docContents = docFile.read() lines += docContents + "\n\n" - except: # Auto-generate the index.rst file + except: # Auto-generate the index.rst file # add page tag if name.startswith("_"): - pathToFolder = index_path.split("/"+name)[0] + pathToFolder = index_path.split("/" + name)[0] lines += ".. " + name + pathToFolder.split("/")[-1] + ":\n\n" - elif name == 'utilities': + elif name == "utilities": pathToFolder = index_path.split("/" + name)[0] lines += ".. _Folder_" + name + pathToFolder.split("/")[-1] + ":\n\n" else: @@ -408,26 +415,30 @@ def populateDocIndex(self, index_path, file_paths, dir_paths): # pull in folder _doc.rst file if it exists try: pathToFolder, folderName = dir_paths[0].split(name) - docFileName = os.path.join(os.path.join(pathToFolder, name), '_doc.rst') + docFileName = os.path.join(os.path.join(pathToFolder, name), "_doc.rst") if os.path.isfile(docFileName): - with open(docFileName, 'r') as docFile: + with open(docFileName, "r") as docFile: docContents = docFile.read() lines += docContents + "\n\n" except: pass # Add a linking point to all local files - lines += """\n\n.. toctree::\n :maxdepth: 1\n :caption: """ + "Files:\n\n" + lines += ( + """\n\n.. toctree::\n :maxdepth: 1\n :caption: """ + "Files:\n\n" + ) calledNames = [] for file_path in sorted(file_paths): fileName = os.path.basename(os.path.normpath(file_path)) - fileName = fileName[:fileName.rfind('.')] + fileName = fileName[: fileName.rfind(".")] if not fileName in calledNames: lines += " " + fileName + "\n" calledNames.append(fileName) # Add a linking point to all local directories - lines += """.. toctree::\n :maxdepth: 1\n :caption: """ + "Directories:\n\n" + lines += ( + """.. toctree::\n :maxdepth: 1\n :caption: """ + "Directories:\n\n" + ) for dir_path in sorted(dir_paths): dirName = os.path.basename(os.path.normpath(dir_path)) lines += " " + dirName + "/index\n" @@ -443,7 +454,13 @@ def generateAutoDoc(self, path, files_paths): # Sort the files by language py_file_paths = sorted([s for s in files_paths if ".py" in s]) - c_file_paths = sorted([s for s in files_paths if ".c" in s or ".cpp" in s or ".h" in s or ".hpp" in s]) + c_file_paths = sorted( + [ + s + for s in files_paths + if ".c" in s or ".cpp" in s or ".h" in s or ".hpp" in s + ] + ) # Create the .rst file for C-Modules @@ -453,7 +470,7 @@ def generateAutoDoc(self, path, files_paths): for c_file_path in c_file_paths: c_file_name = os.path.basename(c_file_path) c_file_local_paths.append(c_file_name) - c_file_name = c_file_name[:c_file_name.rfind('.')] + c_file_name = c_file_name[: c_file_name.rfind(".")] c_file_basenames.append(c_file_name) c_file_basenames = np.unique(c_file_basenames) @@ -469,23 +486,38 @@ def generateAutoDoc(self, path, files_paths): module_files = [] sources = {} for c_file_basename in c_file_basenames: - module_files_temp = [] lines = "" - if c_file_basename == 'orbitalMotion' or c_file_basename == 'rigidBodyKinematics': + if ( + c_file_basename == "orbitalMotion" + or c_file_basename == "rigidBodyKinematics" + ): pathToFolder = src_path.split("/" + c_file_basename)[0] - lines += ".. _" + c_file_basename + pathToFolder.split("/")[-1] + ":\n\n" + lines += ( + ".. _" + c_file_basename + pathToFolder.split("/")[-1] + ":\n\n" + ) else: lines += ".. _" + c_file_basename + ":\n\n" if "architecture" in src_path or "utilities" in src_path: - lines += c_file_basename + "\n" + "=" * (len(c_file_basename) + 8) + "\n\n" + lines += ( + c_file_basename + + "\n" + + "=" * (len(c_file_basename) + 8) + + "\n\n" + ) else: - lines += "Module: " + c_file_basename + "\n" + "=" * (len(c_file_basename) + 8) + "\n\n" + lines += ( + "Module: " + + c_file_basename + + "\n" + + "=" * (len(c_file_basename) + 8) + + "\n\n" + ) # pull in the module documentation file if it exists - docFileName = os.path.join(src_path, c_file_basename + '.rst') + docFileName = os.path.join(src_path, c_file_basename + ".rst") if os.path.isfile(docFileName): - with open(docFileName, 'r', encoding="utf8") as docFile: + with open(docFileName, "r", encoding="utf8") as docFile: docContents = docFile.read() lines += docContents + "\n\n" lines += "----\n\n" @@ -494,8 +526,14 @@ def generateAutoDoc(self, path, files_paths): # make sure the list of files match the base name perfectly # this avoids issues where one file name is contained in another # file name - c_file_list_coarse = [s for s in c_file_local_paths if c_file_basename in s] - c_file_list = [file_name for file_name in c_file_list_coarse if file_name.rsplit(".", 1)[0] == c_file_basename] + c_file_list_coarse = [ + s for s in c_file_local_paths if c_file_basename in s + ] + c_file_list = [ + file_name + for file_name in c_file_list_coarse + if file_name.rsplit(".", 1)[0] == c_file_basename + ] module_files.extend(c_file_list) module_files_temp.extend(c_file_list) @@ -505,11 +543,17 @@ def generateAutoDoc(self, path, files_paths): if name == "_GeneralModuleFiles": name += str(self.counter) self.counter += 1 - lines += """.. autodoxygenfile:: """ + module_file + """\n :project: """ + name + """\n\n""" + lines += ( + """.. autodoxygenfile:: """ + + module_file + + """\n :project: """ + + name + + """\n\n""" + ) # lines += """.. inheritance-diagram:: """ + module_file + """\n\n""" if self.newFiles: - with open(os.path.join(path, c_file_basename + ".rst"), "w") as f: + with open(os.path.join(path, c_file_basename + ".rst"), "w") as f: f.write(lines) sources.update({name: (src_path, module_files)}) @@ -517,27 +561,32 @@ def generateAutoDoc(self, path, files_paths): # Create the .rst file for the python module if not py_file_paths == []: # Add the module path to sys.path so sphinx can produce docs - src_dir = path[path.find("/")+1:] - src_dir = src_dir[src_dir.find("/")+1:] - sys.path.append(os.path.abspath(officialSrc+"/"+src_dir)) + src_dir = path[path.find("/") + 1 :] + src_dir = src_dir[src_dir.find("/") + 1 :] + sys.path.append(os.path.abspath(officialSrc + "/" + src_dir)) for py_file in sorted(py_file_paths): fileName = os.path.basename(py_file) if fileName not in ["__init__.py"]: - fileName = fileName[:fileName.rfind('.')] - lines = ".. _"+ fileName + ":\n\n" + fileName = fileName[: fileName.rfind(".")] + lines = ".. _" + fileName + ":\n\n" lines += fileName + "\n" + "=" * len(fileName) + "\n\n" - lines += """.. toctree::\n :maxdepth: 1\n :caption: """ + "Files" + ":\n\n" - lines += """.. automodule:: """ + fileName + """\n :members:\n :show-inheritance:\n\n""" + lines += ( + """.. toctree::\n :maxdepth: 1\n :caption: """ + + "Files" + + ":\n\n" + ) + lines += ( + """.. automodule:: """ + + fileName + + """\n :members:\n :show-inheritance:\n\n""" + ) if self.newFiles: - with open(path+"/"+fileName+".rst", "w") as f: + with open(path + "/" + fileName + ".rst", "w") as f: f.write(lines) return sources - - - def run(self, srcDir): # Find all files and directories in the src directory paths_in_dir = self.grabRelevantFiles(srcDir) @@ -555,10 +604,10 @@ def run(self, srcDir): pass # Populate the index.rst file of the local directory - self.populateDocIndex(officialDoc+"/"+index_path, file_paths, dir_paths) + self.populateDocIndex(officialDoc + "/" + index_path, file_paths, dir_paths) # Generate the correct auto-doc function for C, C++, and python modules - sources = self.generateAutoDoc(officialDoc+"/"+index_path, file_paths) + sources = self.generateAutoDoc(officialDoc + "/" + index_path, file_paths) # Need to update the translation layer from doxygen to sphinx (breathe) self.breathe_projects_source.update(sources) @@ -572,6 +621,7 @@ def run(self, srcDir): else: return + rebuild = True officialSrc = "../../src" officialDoc = "./Documentation/" @@ -592,15 +642,15 @@ def run(self, srcDir): breathe_projects_source = fileCrawler.run("../../examples") # breathe_projects_source = fileCrawler.run("../../supportData") # breathe_projects_source = fileCrawler.run("../../externalTools") - with open("breathe.data", 'wb') as f: + with open("breathe.data", "wb") as f: pickle.dump(breathe_projects_source, f) else: - with open('breathe.data', 'rb') as f: + with open("breathe.data", "rb") as f: breathe_projects_source = pickle.load(f) - #breathe_projects_source = pickle.load('breathe.data') - #fileCrawler.run("../../../Basilisk/src/") + # breathe_projects_source = pickle.load('breathe.data') + # fileCrawler.run("../../../Basilisk/src/") -#TODO: Pickle the breathe_project_source and load that back in +# TODO: Pickle the breathe_project_source and load that back in # Example of how to link C with Breathe # breathe_projects_source = {"BasiliskFSW": ("../../src/fswAlgorithms/attControl/mrpFeedback", ['mrpFeedback.c', 'mrpFeedback.h'])} diff --git a/examples/BskSim/BSK_masters.py b/examples/BskSim/BSK_masters.py index 530c6c72e7..125668bb11 100644 --- a/examples/BskSim/BSK_masters.py +++ b/examples/BskSim/BSK_masters.py @@ -28,7 +28,7 @@ path = os.path.dirname(os.path.abspath(filename)) # Import Dynamics and FSW models -sys.path.append(path + '/models') +sys.path.append(path + "/models") class BSKSim(SimulationBaseClass.SimBaseClass): @@ -55,24 +55,30 @@ def __init__(self, fswRate=0.1, dynRate=0.1): self.fsw_added = False def get_DynModel(self): - assert (self.dynamics_added is True), "It is mandatory to use a dynamics model as an argument" + assert self.dynamics_added is True, ( + "It is mandatory to use a dynamics model as an argument" + ) return self.DynModels def set_DynModel(self, dynModel): self.dynamics_added = True - self.DynamicsProcessName = 'DynamicsProcess' # Create simulation process name + self.DynamicsProcessName = "DynamicsProcess" # Create simulation process name self.dynProc = self.CreateNewProcess(self.DynamicsProcessName) # Create process - self.DynModels = dynModel.BSKDynamicModels(self, self.dynRate) # Create Dynamics and FSW classes + self.DynModels = dynModel.BSKDynamicModels( + self, self.dynRate + ) # Create Dynamics and FSW classes def get_FswModel(self): - assert (self.fsw_added is True), "A flight software model has not been added yet" + assert self.fsw_added is True, "A flight software model has not been added yet" return self.FSWModels def set_FswModel(self, fswModel): self.fsw_added = True self.FSWProcessName = "FSWProcess" # Create simulation process name self.fswProc = self.CreateNewProcess(self.FSWProcessName) # Create process - self.FSWModels = fswModel.BSKFswModels(self, self.fswRate) # Create Dynamics and FSW classes + self.FSWModels = fswModel.BSKFswModels( + self, self.fswRate + ) # Create Dynamics and FSW classes class BSKScenario(object): @@ -81,18 +87,18 @@ def __init__(self): def configure_initial_conditions(self): """ - Developer must override this method in their BSK_Scenario derived subclass. + Developer must override this method in their BSK_Scenario derived subclass. """ pass def log_outputs(self): """ - Developer must override this method in their BSK_Scenario derived subclass. + Developer must override this method in their BSK_Scenario derived subclass. """ pass def pull_outputs(self): """ - Developer must override this method in their BSK_Scenario derived subclass. + Developer must override this method in their BSK_Scenario derived subclass. """ pass diff --git a/examples/BskSim/models/BSK_Dynamics.py b/examples/BskSim/models/BSK_Dynamics.py index 6cc48daab4..f52133d7a9 100644 --- a/examples/BskSim/models/BSK_Dynamics.py +++ b/examples/BskSim/models/BSK_Dynamics.py @@ -36,11 +36,12 @@ bskPath = __path__[0] -class BSKDynamicModels(): +class BSKDynamicModels: """ General bskSim simulation class that sets up the spacecraft simulation configuration. """ + def __init__(self, SimBase, dynRate): # define empty class variables self.sun = None @@ -58,7 +59,9 @@ def __init__(self, SimBase, dynRate): self.processTasksTimeStep = mc.sec2nano(dynRate) # Create task - SimBase.dynProc.addTask(SimBase.CreateNewTask(self.taskName, self.processTasksTimeStep)) + SimBase.dynProc.addTask( + SimBase.CreateNewTask(self.taskName, self.processTasksTimeStep) + ) # Instantiate Dyn modules as objects self.scObject = spacecraft.Spacecraft() @@ -69,7 +72,9 @@ def __init__(self, SimBase, dynRate): self.eclipseObject = eclipse.Eclipse() self.CSSConstellationObject = coarseSunSensor.CSSConstellation() self.rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector() - self.thrustersDynamicEffector = thrusterDynamicEffector.ThrusterDynamicEffector() + self.thrustersDynamicEffector = ( + thrusterDynamicEffector.ThrusterDynamicEffector() + ) self.EarthEphemObject = ephemerisConverter.EphemerisConverter() # Initialize all modules and write init one-time messages @@ -123,11 +128,13 @@ def SetSpacecraftHub(self): """ self.scObject.ModelTag = "bskSat" # -- Crate a new variable for the sim sc inertia I_sc. Note: this is currently accessed from FSWClass - self.I_sc = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + self.I_sc = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] self.scObject.hub.mHub = 750.0 # kg - spacecraft mass - self.scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + self.scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM self.scObject.hub.IHubPntBc_B = sp.np2EigenMatrix3d(self.I_sc) def SetGravityBodies(self): @@ -135,31 +142,37 @@ def SetGravityBodies(self): Specify what gravitational bodies to include in the simulation """ timeInitString = "2012 MAY 1 00:28:30.0" - gravBodies = self.gravFactory.createBodies(['sun', 'earth', 'moon']) - gravBodies['earth'].isCentralBody = True + gravBodies = self.gravFactory.createBodies(["sun", "earth", "moon"]) + gravBodies["earth"].isCentralBody = True self.sun = 0 self.earth = 1 self.moon = 2 self.gravFactory.addBodiesTo(self.scObject) - self.gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/', - timeInitString, - epochInMsg=True) + self.gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", timeInitString, epochInMsg=True + ) self.epochMsg = self.gravFactory.epochMsg - self.gravFactory.spiceObject.zeroBase = 'Earth' + self.gravFactory.spiceObject.zeroBase = "Earth" - self.EarthEphemObject.addSpiceInputMsg(self.gravFactory.spiceObject.planetStateOutMsgs[self.earth]) + self.EarthEphemObject.addSpiceInputMsg( + self.gravFactory.spiceObject.planetStateOutMsgs[self.earth] + ) def SetEclipseObject(self): """ Specify what celestial object is causing an eclipse message. """ self.eclipseObject.ModelTag = "eclipseObject" - self.eclipseObject.sunInMsg.subscribeTo(self.gravFactory.spiceObject.planetStateOutMsgs[self.sun]) + self.eclipseObject.sunInMsg.subscribeTo( + self.gravFactory.spiceObject.planetStateOutMsgs[self.sun] + ) # add all celestial objects in spiceObjects except for the sun (0th object) for c in range(1, len(self.gravFactory.spiceObject.planetStateOutMsgs)): - self.eclipseObject.addPlanetToModel(self.gravFactory.spiceObject.planetStateOutMsgs[c]) + self.eclipseObject.addPlanetToModel( + self.gravFactory.spiceObject.planetStateOutMsgs[c] + ) self.eclipseObject.addSpacecraftToModel(self.scObject.scStateOutMsg) def SetExternalForceTorqueObject(self): @@ -175,41 +188,46 @@ def SetSimpleNavObject(self): def SetReactionWheelDynEffector(self): """Set the 4 reaction wheel devices.""" # specify RW momentum capacity - maxRWMomentum = 50. # Nms + maxRWMomentum = 50.0 # Nms # Define orthogonal RW pyramid # -- Pointing directions - rwElAngle = np.array([40.0, 40.0, 40.0, 40.0])*mc.D2R - rwAzimuthAngle = np.array([45.0, 135.0, 225.0, 315.0])*mc.D2R - rwPosVector = [[0.8, 0.8, 1.79070], - [0.8, -0.8, 1.79070], - [-0.8, -0.8, 1.79070], - [-0.8, 0.8, 1.79070] - ] - - gsHat = (rbk.Mi(-rwAzimuthAngle[0], 3).dot(rbk.Mi(rwElAngle[0], 2))).dot(np.array([1, 0, 0])) - self.RW1 = self.rwFactory.create('Honeywell_HR16', - gsHat, - maxMomentum=maxRWMomentum, - rWB_B=rwPosVector[0]) - - gsHat = (rbk.Mi(-rwAzimuthAngle[1], 3).dot(rbk.Mi(rwElAngle[1], 2))).dot(np.array([1, 0, 0])) - self.RW2 = self.rwFactory.create('Honeywell_HR16', - gsHat, - maxMomentum=maxRWMomentum, - rWB_B=rwPosVector[1]) - - gsHat = (rbk.Mi(-rwAzimuthAngle[2], 3).dot(rbk.Mi(rwElAngle[2], 2))).dot(np.array([1, 0, 0])) - self.RW3 = self.rwFactory.create('Honeywell_HR16', - gsHat, - maxMomentum=maxRWMomentum, - rWB_B=rwPosVector[2]) - - gsHat = (rbk.Mi(-rwAzimuthAngle[3], 3).dot(rbk.Mi(rwElAngle[3], 2))).dot(np.array([1, 0, 0])) - self.RW4 = self.rwFactory.create('Honeywell_HR16', - gsHat, - maxMomentum=maxRWMomentum, - rWB_B=rwPosVector[3]) + rwElAngle = np.array([40.0, 40.0, 40.0, 40.0]) * mc.D2R + rwAzimuthAngle = np.array([45.0, 135.0, 225.0, 315.0]) * mc.D2R + rwPosVector = [ + [0.8, 0.8, 1.79070], + [0.8, -0.8, 1.79070], + [-0.8, -0.8, 1.79070], + [-0.8, 0.8, 1.79070], + ] + + gsHat = (rbk.Mi(-rwAzimuthAngle[0], 3).dot(rbk.Mi(rwElAngle[0], 2))).dot( + np.array([1, 0, 0]) + ) + self.RW1 = self.rwFactory.create( + "Honeywell_HR16", gsHat, maxMomentum=maxRWMomentum, rWB_B=rwPosVector[0] + ) + + gsHat = (rbk.Mi(-rwAzimuthAngle[1], 3).dot(rbk.Mi(rwElAngle[1], 2))).dot( + np.array([1, 0, 0]) + ) + self.RW2 = self.rwFactory.create( + "Honeywell_HR16", gsHat, maxMomentum=maxRWMomentum, rWB_B=rwPosVector[1] + ) + + gsHat = (rbk.Mi(-rwAzimuthAngle[2], 3).dot(rbk.Mi(rwElAngle[2], 2))).dot( + np.array([1, 0, 0]) + ) + self.RW3 = self.rwFactory.create( + "Honeywell_HR16", gsHat, maxMomentum=maxRWMomentum, rWB_B=rwPosVector[2] + ) + + gsHat = (rbk.Mi(-rwAzimuthAngle[3], 3).dot(rbk.Mi(rwElAngle[3], 2))).dot( + np.array([1, 0, 0]) + ) + self.RW4 = self.rwFactory.create( + "Honeywell_HR16", gsHat, maxMomentum=maxRWMomentum, rWB_B=rwPosVector[3] + ) self.rwFactory.addToSpacecraft("RWA", self.rwStateEffector, self.scObject) @@ -221,15 +239,15 @@ def SetThrusterStateEffector(self): # 8 thrusters are modeled that act in pairs to provide the desired torque thPos = [ - [825.5/1000.0, 880.3/1000.0, 1765.3/1000.0], - [825.5/1000.0, 880.3/1000.0, 260.4/1000.0], - [880.3/1000.0, 825.5/1000.0, 1765.3/1000.0], - [880.3/1000.0, 825.5/1000.0, 260.4/1000.0], - [-825.5/1000.0, -880.3/1000.0, 1765.3/1000.0], - [-825.5/1000.0, -880.3/1000.0, 260.4/1000.0], - [-880.3/1000.0, -825.5/1000.0, 1765.3/1000.0], - [-880.3/1000.0, -825.5/1000.0, 260.4/1000.0] - ] + [825.5 / 1000.0, 880.3 / 1000.0, 1765.3 / 1000.0], + [825.5 / 1000.0, 880.3 / 1000.0, 260.4 / 1000.0], + [880.3 / 1000.0, 825.5 / 1000.0, 1765.3 / 1000.0], + [880.3 / 1000.0, 825.5 / 1000.0, 260.4 / 1000.0], + [-825.5 / 1000.0, -880.3 / 1000.0, 1765.3 / 1000.0], + [-825.5 / 1000.0, -880.3 / 1000.0, 260.4 / 1000.0], + [-880.3 / 1000.0, -825.5 / 1000.0, 1765.3 / 1000.0], + [-880.3 / 1000.0, -825.5 / 1000.0, 260.4 / 1000.0], + ] thDir = [ [0.0, -1.0, 0.0], [0.0, -1.0, 0.0], @@ -238,31 +256,29 @@ def SetThrusterStateEffector(self): [0.0, 1.0, 0.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0], - [1.0, 0.0, 0.0] + [1.0, 0.0, 0.0], ] for pos_B, dir_B in zip(thPos, thDir): - thFactory.create( - 'MOOG_Monarc_1' - , pos_B - , dir_B - ) + thFactory.create("MOOG_Monarc_1", pos_B, dir_B) # create thruster object container and tie to spacecraft object - self.thFactory.addToSpacecraft("ACS Thrusters", - self.thrustersDynamicEffector, - self.scObject) + self.thFactory.addToSpacecraft( + "ACS Thrusters", self.thrustersDynamicEffector, self.scObject + ) def SetCSSConstellation(self): """Set the 8 CSS sensors""" self.CSSConstellationObject.ModelTag = "cssConstellation" # Create class-level registry if it doesn't exist - if not hasattr(self, '_css_registry'): + if not hasattr(self, "_css_registry"): self._css_registry = [] def setupCSS(cssDevice): - cssDevice.fov = 80. * mc.D2R # half-angle field of view value + cssDevice.fov = 80.0 * mc.D2R # half-angle field of view value cssDevice.scaleFactor = 2.0 - cssDevice.sunInMsg.subscribeTo(self.gravFactory.spiceObject.planetStateOutMsgs[self.sun]) + cssDevice.sunInMsg.subscribeTo( + self.gravFactory.spiceObject.planetStateOutMsgs[self.sun] + ) cssDevice.stateInMsg.subscribeTo(self.scObject.scStateOutMsg) cssDevice.sunEclipseInMsg.subscribeTo(self.eclipseObject.eclipseOutMsgs[0]) # Store CSS in class-level registry @@ -271,19 +287,19 @@ def setupCSS(cssDevice): # setup CSS sensor normal vectors in body frame components nHat_B_List = [ [0.0, 0.707107, 0.707107], - [0.707107, 0., 0.707107], + [0.707107, 0.0, 0.707107], [0.0, -0.707107, 0.707107], - [-0.707107, 0., 0.707107], + [-0.707107, 0.0, 0.707107], [0.0, -0.965926, -0.258819], [-0.707107, -0.353553, -0.612372], - [0., 0.258819, -0.965926], - [0.707107, -0.353553, -0.612372] + [0.0, 0.258819, -0.965926], + [0.707107, -0.353553, -0.612372], ] numCSS = len(nHat_B_List) # store all cssList = [] - for nHat_B, i in zip(nHat_B_List, list(range(1,numCSS+1))): + for nHat_B, i in zip(nHat_B_List, list(range(1, numCSS + 1))): CSS = coarseSunSensor.CoarseSunSensor() setupCSS(CSS) CSS.ModelTag = "CSS" + str(i) @@ -301,13 +317,11 @@ def PeriodicRWFault(self, probability, faultType, fault, faultRW, currentTime): if np.random.uniform() < probability: self.AddRWFault(faultType, fault, faultRW, currentTime) - - def AddRWFault(self, faultType, fault, faultRW, currentTime): """ Adds a static friction fault to the reaction wheel. """ - self.RWFaultLog.append([faultType, fault, faultRW, currentTime*mc.NANO2MIN]) + self.RWFaultLog.append([faultType, fault, faultRW, currentTime * mc.NANO2MIN]) if faultType == "friction": if faultRW == 1: self.RW1.fCoulomb += fault diff --git a/examples/BskSim/models/BSK_FormationDynamics.py b/examples/BskSim/models/BSK_FormationDynamics.py index 5c2e4bc65e..2a8623602d 100644 --- a/examples/BskSim/models/BSK_FormationDynamics.py +++ b/examples/BskSim/models/BSK_FormationDynamics.py @@ -18,8 +18,12 @@ import numpy as np from Basilisk import __path__ -from Basilisk.simulation import (spacecraft, extForceTorque, simpleNav, - reactionWheelStateEffector) +from Basilisk.simulation import ( + spacecraft, + extForceTorque, + simpleNav, + reactionWheelStateEffector, +) from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.utilities import macros as mc from Basilisk.utilities import simIncludeRW, simIncludeGravBody @@ -28,8 +32,7 @@ bskPath = __path__[0] - -class BSKDynamicModels(): +class BSKDynamicModels: def __init__(self, SimBase, dynRate): # Define process name, task name and task time-step self.processName = SimBase.DynamicsProcessName @@ -38,8 +41,12 @@ def __init__(self, SimBase, dynRate): self.processTasksTimeStep = mc.sec2nano(dynRate) # Create task - SimBase.dynProc.addTask(SimBase.CreateNewTask(self.taskName, self.processTasksTimeStep)) - SimBase.dynProc.addTask(SimBase.CreateNewTask(self.taskName2, self.processTasksTimeStep)) + SimBase.dynProc.addTask( + SimBase.CreateNewTask(self.taskName, self.processTasksTimeStep) + ) + SimBase.dynProc.addTask( + SimBase.CreateNewTask(self.taskName2, self.processTasksTimeStep) + ) # Instantiate Dyn modules as objects self.scObject = spacecraft.Spacecraft() @@ -56,7 +63,7 @@ def __init__(self, SimBase, dynRate): # Create gravity body self.gravFactory = simIncludeGravBody.gravBodyFactory() planet = self.gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body self.gravFactory.addBodiesTo(self.scObject) self.gravFactory.addBodiesTo(self.scObject2) @@ -78,19 +85,23 @@ def __init__(self, SimBase, dynRate): def SetSpacecraftHub(self): self.scObject.ModelTag = "chief" - self.I_sc = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + self.I_sc = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] self.scObject.hub.mHub = 750.0 # kg - spacecraft mass - self.scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + self.scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM self.scObject.hub.IHubPntBc_B = sp.np2EigenMatrix3d(self.I_sc) self.scObject2.ModelTag = "deputy" - self.I_sc2 = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + self.I_sc2 = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] self.scObject2.hub.mHub = 750.0 # kg - spacecraft mass - self.scObject2.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + self.scObject2.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM self.scObject2.hub.IHubPntBc_B = sp.np2EigenMatrix3d(self.I_sc2) def SetSimpleNavObject(self): @@ -104,31 +115,34 @@ def SetReactionWheelDynEffector(self): # Make a fresh RW factory instance, this is critical to run multiple times # specify RW momentum capacity - maxRWMomentum = 50. # Nms + maxRWMomentum = 50.0 # Nms # Define orthogonal RW pyramid # -- Pointing directions - rwElAngle = np.array([40.0, 40.0, 40.0, 40.0])*mc.D2R - rwAzimuthAngle = np.array([45.0, 135.0, 225.0, 315.0])*mc.D2R - rwPosVector = [[0.8, 0.8, 1.79070], - [0.8, -0.8, 1.79070], - [-0.8, -0.8, 1.79070], - [-0.8, 0.8, 1.79070] - ] + rwElAngle = np.array([40.0, 40.0, 40.0, 40.0]) * mc.D2R + rwAzimuthAngle = np.array([45.0, 135.0, 225.0, 315.0]) * mc.D2R + rwPosVector = [ + [0.8, 0.8, 1.79070], + [0.8, -0.8, 1.79070], + [-0.8, -0.8, 1.79070], + [-0.8, 0.8, 1.79070], + ] for elAngle, azAngle, posVector in zip(rwElAngle, rwAzimuthAngle, rwPosVector): - gsHat = (rbk.Mi(-azAngle,3).dot(rbk.Mi(elAngle,2))).dot(np.array([1,0,0])) - self.rwFactory.create('Honeywell_HR16', - gsHat, - maxMomentum=maxRWMomentum, - rWB_B=posVector) - self.rwFactory2.create('Honeywell_HR16', - gsHat, - maxMomentum=maxRWMomentum, - rWB_B =posVector) + gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot( + np.array([1, 0, 0]) + ) + self.rwFactory.create( + "Honeywell_HR16", gsHat, maxMomentum=maxRWMomentum, rWB_B=posVector + ) + self.rwFactory2.create( + "Honeywell_HR16", gsHat, maxMomentum=maxRWMomentum, rWB_B=posVector + ) self.rwFactory.addToSpacecraft("RW_chief", self.rwStateEffector, self.scObject) - self.rwFactory2.addToSpacecraft("RW_deputy", self.rwStateEffector2, self.scObject2) + self.rwFactory2.addToSpacecraft( + "RW_deputy", self.rwStateEffector2, self.scObject2 + ) def SetExternalForceTorqueObject(self): self.extForceTorqueObject2.ModelTag = "externalDisturbance" diff --git a/examples/BskSim/models/BSK_FormationFsw.py b/examples/BskSim/models/BSK_FormationFsw.py index b54108b5a3..c22249b933 100644 --- a/examples/BskSim/models/BSK_FormationFsw.py +++ b/examples/BskSim/models/BSK_FormationFsw.py @@ -32,7 +32,7 @@ from Basilisk.utilities import macros as mc -class BSKFswModels(): +class BSKFswModels: def __init__(self, SimBase, fswRate): # define empty class variables self.vcMsg = None @@ -53,14 +53,26 @@ def __init__(self, SimBase, fswRate): self.processTasksTimeStep = mc.sec2nano(fswRate) # Create tasks - SimBase.fswProc.addTask(SimBase.CreateNewTask("inertial3DPointTask", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("mrpFeedbackRWsTask", self.processTasksTimeStep), 10) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("inertial3DPointTask", self.processTasksTimeStep), 20 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("mrpFeedbackRWsTask", self.processTasksTimeStep), 10 + ) - SimBase.fswProc.addTask(SimBase.CreateNewTask("inertial3DPointTask2", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("mrpFeedbackRWsTask2", self.processTasksTimeStep), 10) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("inertial3DPointTask2", self.processTasksTimeStep), 20 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("mrpFeedbackRWsTask2", self.processTasksTimeStep), 10 + ) - SimBase.fswProc.addTask(SimBase.CreateNewTask("mrpFeedbackTask", self.processTasksTimeStep), 10) - SimBase.fswProc.addTask(SimBase.CreateNewTask("spacecraftPointingTask", self.processTasksTimeStep)) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("mrpFeedbackTask", self.processTasksTimeStep), 10 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("spacecraftPointingTask", self.processTasksTimeStep) + ) # Create modules self.inertial3D = inertial3D.inertial3D() @@ -106,12 +118,11 @@ def __init__(self, SimBase, fswRate): SimBase.AddModelToTask("mrpFeedbackRWsTask", self.mrpFeedbackRWs, 9) SimBase.AddModelToTask("mrpFeedbackRWsTask", self.rwMotorTorque, 8) - SimBase.AddModelToTask("inertial3DPointTask2", self.inertial3D2, 10) SimBase.AddModelToTask("inertial3DPointTask2", self.trackingError2, 9) SimBase.AddModelToTask("mrpFeedbackRWsTask2", self.mrpFeedbackRWs2, 9) - SimBase.AddModelToTask("mrpFeedbackRWsTask2", self.rwMotorTorque2,8) + SimBase.AddModelToTask("mrpFeedbackRWsTask2", self.rwMotorTorque2, 8) SimBase.AddModelToTask("mrpFeedbackTask", self.mrpFeedbackControl, 10) @@ -170,34 +181,50 @@ def SetInertial3DPointGuidance(self): messaging.AttRefMsg_C_addAuthor(self.inertial3D2.attRefOutMsg, self.attRef2Msg) def SetAttitudeTrackingError(self, SimBase): - self.trackingError.attNavInMsg.subscribeTo(SimBase.DynModels.simpleNavObject.attOutMsg) + self.trackingError.attNavInMsg.subscribeTo( + SimBase.DynModels.simpleNavObject.attOutMsg + ) self.trackingError.attRefInMsg.subscribeTo(self.attRefMsg) - messaging.AttGuidMsg_C_addAuthor(self.trackingError.attGuidOutMsg, self.attGuidMsg) + messaging.AttGuidMsg_C_addAuthor( + self.trackingError.attGuidOutMsg, self.attGuidMsg + ) - self.trackingError2.attNavInMsg.subscribeTo(SimBase.DynModels.simpleNavObject2.attOutMsg) + self.trackingError2.attNavInMsg.subscribeTo( + SimBase.DynModels.simpleNavObject2.attOutMsg + ) self.trackingError2.attRefInMsg.subscribeTo(self.attRef2Msg) - messaging.AttGuidMsg_C_addAuthor(self.trackingError2.attGuidOutMsg, self.attGuid2Msg) + messaging.AttGuidMsg_C_addAuthor( + self.trackingError2.attGuidOutMsg, self.attGuid2Msg + ) def SetMRPFeedbackRWA(self, SimBase): self.mrpFeedbackRWs.K = 3.5 self.mrpFeedbackRWs.Ki = -1 self.mrpFeedbackRWs.P = 30.0 - self.mrpFeedbackRWs.integralLimit = 2. / self.mrpFeedbackRWs.Ki * 0.1 + self.mrpFeedbackRWs.integralLimit = 2.0 / self.mrpFeedbackRWs.Ki * 0.1 self.mrpFeedbackRWs.vehConfigInMsg.subscribeTo(self.vcMsg) - self.mrpFeedbackRWs.rwSpeedsInMsg.subscribeTo(SimBase.DynModels.rwStateEffector.rwSpeedOutMsg) + self.mrpFeedbackRWs.rwSpeedsInMsg.subscribeTo( + SimBase.DynModels.rwStateEffector.rwSpeedOutMsg + ) self.mrpFeedbackRWs.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) self.mrpFeedbackRWs.guidInMsg.subscribeTo(self.attGuidMsg) - messaging.CmdTorqueBodyMsg_C_addAuthor(self.mrpFeedbackRWs.cmdTorqueOutMsg, self.cmdTorqueMsg) + messaging.CmdTorqueBodyMsg_C_addAuthor( + self.mrpFeedbackRWs.cmdTorqueOutMsg, self.cmdTorqueMsg + ) self.mrpFeedbackRWs2.K = 3.5 self.mrpFeedbackRWs2.Ki = -1 # TURN OFF IN CASE OF RUNNING Inertial3D!!! self.mrpFeedbackRWs2.P = 30.0 - self.mrpFeedbackRWs2.integralLimit = 2. / self.mrpFeedbackRWs2.Ki * 0.1 + self.mrpFeedbackRWs2.integralLimit = 2.0 / self.mrpFeedbackRWs2.Ki * 0.1 self.mrpFeedbackRWs2.vehConfigInMsg.subscribeTo(self.vcMsg) - self.mrpFeedbackRWs2.rwSpeedsInMsg.subscribeTo(SimBase.DynModels.rwStateEffector2.rwSpeedOutMsg) + self.mrpFeedbackRWs2.rwSpeedsInMsg.subscribeTo( + SimBase.DynModels.rwStateEffector2.rwSpeedOutMsg + ) self.mrpFeedbackRWs2.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) self.mrpFeedbackRWs2.guidInMsg.subscribeTo(self.attGuid2Msg) - messaging.CmdTorqueBodyMsg_C_addAuthor(self.mrpFeedbackRWs2.cmdTorqueOutMsg, self.cmdTorque2Msg) + messaging.CmdTorqueBodyMsg_C_addAuthor( + self.mrpFeedbackRWs2.cmdTorqueOutMsg, self.cmdTorque2Msg + ) def SetRWConfigMsg(self): # Configure RW pyramid exactly as it is in the Dynamics (i.e. FSW with perfect knowledge) @@ -208,52 +235,65 @@ def SetRWConfigMsg(self): fswSetupRW.clearSetup() for elAngle, azAngle in zip(rwElAngle, rwAzimuthAngle): - gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot(np.array([1, 0, 0])) - fswSetupRW.create(gsHat, # spin axis - wheelJs, # kg*m^2 - 0.2) # Nm uMax + gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot( + np.array([1, 0, 0]) + ) + fswSetupRW.create( + gsHat, # spin axis + wheelJs, # kg*m^2 + 0.2, + ) # Nm uMax self.fswRwConfigMsg = fswSetupRW.writeConfigMessage() def SetRWMotorTorque(self): - controlAxes_B = [ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0] + controlAxes_B = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] self.rwMotorTorque.controlAxes_B = controlAxes_B self.rwMotorTorque.vehControlInMsg.subscribeTo(self.cmdTorqueMsg) - messaging.ArrayMotorTorqueMsg_C_addAuthor(self.rwMotorTorque.rwMotorTorqueOutMsg, self.cmdRwMotorMsg) + messaging.ArrayMotorTorqueMsg_C_addAuthor( + self.rwMotorTorque.rwMotorTorqueOutMsg, self.cmdRwMotorMsg + ) self.rwMotorTorque.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) self.rwMotorTorque2.controlAxes_B = controlAxes_B self.rwMotorTorque2.vehControlInMsg.subscribeTo(self.cmdTorque2Msg) - messaging.ArrayMotorTorqueMsg_C_addAuthor(self.rwMotorTorque2.rwMotorTorqueOutMsg, self.cmdRwMotor2Msg) + messaging.ArrayMotorTorqueMsg_C_addAuthor( + self.rwMotorTorque2.rwMotorTorqueOutMsg, self.cmdRwMotor2Msg + ) self.rwMotorTorque2.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) def SetSpacecraftPointing(self, SimBase): - self.spacecraftPointing.chiefPositionInMsg.subscribeTo(SimBase.DynModels.simpleNavObject.transOutMsg) - self.spacecraftPointing.deputyPositionInMsg.subscribeTo(SimBase.DynModels.simpleNavObject2.transOutMsg) - messaging.AttRefMsg_C_addAuthor(self.spacecraftPointing.attReferenceOutMsg, self.attRef2Msg) + self.spacecraftPointing.chiefPositionInMsg.subscribeTo( + SimBase.DynModels.simpleNavObject.transOutMsg + ) + self.spacecraftPointing.deputyPositionInMsg.subscribeTo( + SimBase.DynModels.simpleNavObject2.transOutMsg + ) + messaging.AttRefMsg_C_addAuthor( + self.spacecraftPointing.attReferenceOutMsg, self.attRef2Msg + ) self.spacecraftPointing.alignmentVector_B = [1.0, 2.0, 3.0] def SetVehicleConfiguration(self): # use the same inertia in the FSW algorithm as in the simulation - vcData = messaging.VehicleConfigMsgPayload(ISCPntB_B=[ - 900.0, 0.0, 0.0, - 0.0, 800.0, 0.0, - 0.0, 0.0, 600.0 - ]) + vcData = messaging.VehicleConfigMsgPayload( + ISCPntB_B=[900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] + ) self.vcMsg = messaging.VehicleConfigMsg().write(vcData) def SetMRPFeedbackControl(self): self.mrpFeedbackControl.guidInMsg.subscribeTo(self.attGuid2Msg) self.mrpFeedbackControl.vehConfigInMsg.subscribeTo(self.vcMsg) - messaging.CmdTorqueBodyMsg_C_addAuthor(self.mrpFeedbackControl.cmdTorqueOutMsg, self.cmdTorqueDirectMsg) + messaging.CmdTorqueBodyMsg_C_addAuthor( + self.mrpFeedbackControl.cmdTorqueOutMsg, self.cmdTorqueDirectMsg + ) self.mrpFeedbackControl.K = 10.0 - self.mrpFeedbackControl.Ki = 0.0001 # Note: make value negative to turn off integral feedback + self.mrpFeedbackControl.Ki = ( + 0.0001 # Note: make value negative to turn off integral feedback + ) self.mrpFeedbackControl.P = 30.0 - self.mrpFeedbackControl.integralLimit = 2. / self.mrpFeedbackControl.Ki * 0.1 + self.mrpFeedbackControl.integralLimit = 2.0 / self.mrpFeedbackControl.Ki * 0.1 # Global call to initialize every module def InitAllFSWObjects(self, SimBase): @@ -282,9 +322,15 @@ def setupGatewayMsgs(self, SimBase): self.zeroGateWayMsgs() # connect gateway FSW effector command msgs with the dynamics - SimBase.DynModels.extForceTorqueObject2.cmdTorqueInMsg.subscribeTo(self.cmdTorqueDirectMsg) - SimBase.DynModels.rwStateEffector.rwMotorCmdInMsg.subscribeTo(self.cmdRwMotorMsg) - SimBase.DynModels.rwStateEffector2.rwMotorCmdInMsg.subscribeTo(self.cmdRwMotor2Msg) + SimBase.DynModels.extForceTorqueObject2.cmdTorqueInMsg.subscribeTo( + self.cmdTorqueDirectMsg + ) + SimBase.DynModels.rwStateEffector.rwMotorCmdInMsg.subscribeTo( + self.cmdRwMotorMsg + ) + SimBase.DynModels.rwStateEffector2.rwMotorCmdInMsg.subscribeTo( + self.cmdRwMotor2Msg + ) def zeroGateWayMsgs(self): """Zero all the FSW gateway message payloads""" diff --git a/examples/BskSim/models/BSK_Fsw.py b/examples/BskSim/models/BSK_Fsw.py index 34636c7bf7..601cb63474 100644 --- a/examples/BskSim/models/BSK_Fsw.py +++ b/examples/BskSim/models/BSK_Fsw.py @@ -44,6 +44,7 @@ class BSKFswModels: """Defines the bskSim FSW class""" + def __init__(self, SimBase, fswRate): # define empty class variables self.vcMsg = None @@ -70,7 +71,7 @@ def __init__(self, SimBase, fswRate): self.sunSafePoint.ModelTag = "sunSafePoint" self.velocityPoint = velocityPoint.velocityPoint() - self.velocityPoint.ModelTag = "velocityPoint" + self.velocityPoint.ModelTag = "velocityPoint" self.cssWlsEst = cssWlsEst.cssWlsEst() self.cssWlsEst.ModelTag = "cssWlsEst" @@ -102,8 +103,12 @@ def __init__(self, SimBase, fswRate): self.lambertValidatorObject = lambertValidator.LambertValidator() self.lambertValidatorObject.ModelTag = "LambertValidator" - self.lambertSurfaceRelativeVelocityObject = lambertSurfaceRelativeVelocity.LambertSurfaceRelativeVelocity() - self.lambertSurfaceRelativeVelocityObject.ModelTag = "LambertSurfaceRelativeVelocity" + self.lambertSurfaceRelativeVelocityObject = ( + lambertSurfaceRelativeVelocity.LambertSurfaceRelativeVelocity() + ) + self.lambertSurfaceRelativeVelocityObject.ModelTag = ( + "LambertSurfaceRelativeVelocity" + ) self.lambertSecondDvObject = lambertSecondDV.LambertSecondDV() self.lambertSecondDvObject.ModelTag = "LambertSecondDV" @@ -115,15 +120,35 @@ def __init__(self, SimBase, fswRate): self.InitAllFSWObjects(SimBase) # Create tasks - SimBase.fswProc.addTask(SimBase.CreateNewTask("inertial3DPointTask", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("hillPointTask", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("sunSafePointTask", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("velocityPointTask", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("mrpFeedbackTask", self.processTasksTimeStep), 10) - SimBase.fswProc.addTask(SimBase.CreateNewTask("mrpSteeringRWsTask", self.processTasksTimeStep), 10) - SimBase.fswProc.addTask(SimBase.CreateNewTask("mrpFeedbackRWsTask", self.processTasksTimeStep), 10) - SimBase.fswProc.addTask(SimBase.CreateNewTask("lambertGuidanceFirstDV", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("lambertGuidanceSecondDV", self.processTasksTimeStep), 20) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("inertial3DPointTask", self.processTasksTimeStep), 20 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("hillPointTask", self.processTasksTimeStep), 20 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("sunSafePointTask", self.processTasksTimeStep), 20 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("velocityPointTask", self.processTasksTimeStep), 20 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("mrpFeedbackTask", self.processTasksTimeStep), 10 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("mrpSteeringRWsTask", self.processTasksTimeStep), 10 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("mrpFeedbackRWsTask", self.processTasksTimeStep), 10 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("lambertGuidanceFirstDV", self.processTasksTimeStep), + 20, + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("lambertGuidanceSecondDV", self.processTasksTimeStep), + 20, + ) # Assign initialized modules to tasks SimBase.AddModelToTask("inertial3DPointTask", self.inertial3D, 10) @@ -147,12 +172,25 @@ def __init__(self, SimBase, fswRate): SimBase.AddModelToTask("mrpFeedbackRWsTask", self.mrpFeedbackRWs, 9) SimBase.AddModelToTask("mrpFeedbackRWsTask", self.rwMotorTorque, 8) - SimBase.AddModelToTask("lambertGuidanceFirstDV", self.lambertPlannerObject, None, 10) - SimBase.AddModelToTask("lambertGuidanceFirstDV", self.lambertSolverObject, None, 9) - SimBase.AddModelToTask("lambertGuidanceFirstDV", self.lambertValidatorObject, None, 8) + SimBase.AddModelToTask( + "lambertGuidanceFirstDV", self.lambertPlannerObject, None, 10 + ) + SimBase.AddModelToTask( + "lambertGuidanceFirstDV", self.lambertSolverObject, None, 9 + ) + SimBase.AddModelToTask( + "lambertGuidanceFirstDV", self.lambertValidatorObject, None, 8 + ) - SimBase.AddModelToTask("lambertGuidanceSecondDV", self.lambertSurfaceRelativeVelocityObject, None, 10) - SimBase.AddModelToTask("lambertGuidanceSecondDV", self.lambertSecondDvObject, None, 9) + SimBase.AddModelToTask( + "lambertGuidanceSecondDV", + self.lambertSurfaceRelativeVelocityObject, + None, + 10, + ) + SimBase.AddModelToTask( + "lambertGuidanceSecondDV", self.lambertSecondDvObject, None, 9 + ) # Create events to be called for triggering GN&C maneuvers SimBase.fswProc.disableAllTasks() @@ -298,80 +336,108 @@ def SetInertial3DPointGuidance(self): def SetHillPointGuidance(self, SimBase): """Define the Hill pointing guidance module""" - self.hillPoint.transNavInMsg.subscribeTo(SimBase.DynModels.simpleNavObject.transOutMsg) - self.hillPoint.celBodyInMsg.subscribeTo(SimBase.DynModels.EarthEphemObject.ephemOutMsgs[0]) # earth + self.hillPoint.transNavInMsg.subscribeTo( + SimBase.DynModels.simpleNavObject.transOutMsg + ) + self.hillPoint.celBodyInMsg.subscribeTo( + SimBase.DynModels.EarthEphemObject.ephemOutMsgs[0] + ) # earth messaging.AttRefMsg_C_addAuthor(self.hillPoint.attRefOutMsg, self.attRefMsg) def SetSunSafePointGuidance(self, SimBase): """Define the sun safe pointing guidance module""" - self.sunSafePoint.imuInMsg.subscribeTo(SimBase.DynModels.simpleNavObject.attOutMsg) + self.sunSafePoint.imuInMsg.subscribeTo( + SimBase.DynModels.simpleNavObject.attOutMsg + ) self.sunSafePoint.sunDirectionInMsg.subscribeTo(self.cssWlsEst.navStateOutMsg) self.sunSafePoint.sHatBdyCmd = [0.0, 0.0, 1.0] - messaging.AttGuidMsg_C_addAuthor(self.sunSafePoint.attGuidanceOutMsg, self.attGuidMsg) + messaging.AttGuidMsg_C_addAuthor( + self.sunSafePoint.attGuidanceOutMsg, self.attGuidMsg + ) def SetVelocityPointGuidance(self, SimBase): """Define the velocity pointing guidance module""" - self.velocityPoint.transNavInMsg.subscribeTo(SimBase.DynModels.simpleNavObject.transOutMsg) - self.velocityPoint.celBodyInMsg.subscribeTo(SimBase.DynModels.EarthEphemObject.ephemOutMsgs[0]) - self.velocityPoint.mu = SimBase.DynModels.gravFactory.gravBodies['earth'].mu + self.velocityPoint.transNavInMsg.subscribeTo( + SimBase.DynModels.simpleNavObject.transOutMsg + ) + self.velocityPoint.celBodyInMsg.subscribeTo( + SimBase.DynModels.EarthEphemObject.ephemOutMsgs[0] + ) + self.velocityPoint.mu = SimBase.DynModels.gravFactory.gravBodies["earth"].mu messaging.AttRefMsg_C_addAuthor(self.velocityPoint.attRefOutMsg, self.attRefMsg) def SetAttitudeTrackingError(self, SimBase): """Define the attitude tracking error module""" - self.trackingError.attNavInMsg.subscribeTo(SimBase.DynModels.simpleNavObject.attOutMsg) + self.trackingError.attNavInMsg.subscribeTo( + SimBase.DynModels.simpleNavObject.attOutMsg + ) self.trackingError.attRefInMsg.subscribeTo(self.attRefMsg) - messaging.AttGuidMsg_C_addAuthor(self.trackingError.attGuidOutMsg, self.attGuidMsg) + messaging.AttGuidMsg_C_addAuthor( + self.trackingError.attGuidOutMsg, self.attGuidMsg + ) def SetCSSWlsEst(self, SimBase): - """Set the FSW CSS configuration information """ + """Set the FSW CSS configuration information""" nHat_B_vec = [ [0.0, 0.707107, 0.707107], - [0.707107, 0., 0.707107], + [0.707107, 0.0, 0.707107], [0.0, -0.707107, 0.707107], - [-0.707107, 0., 0.707107], + [-0.707107, 0.0, 0.707107], [0.0, -0.965926, -0.258819], [-0.707107, -0.353553, -0.612372], - [0., 0.258819, -0.965926], - [0.707107, -0.353553, -0.612372] + [0.0, 0.258819, -0.965926], + [0.707107, -0.353553, -0.612372], ] cssConfig = messaging.CSSConfigMsgPayload( - nCSS = len(nHat_B_vec), - cssVals = [ + nCSS=len(nHat_B_vec), + cssVals=[ messaging.CSSUnitConfigMsgPayload( CBias=1.0, nHat_B=CSSHat, ) for CSSHat in nHat_B_vec - ] + ], ) self.cssConfigMsg = messaging.CSSConfigMsg().write(cssConfig) - self.cssWlsEst.cssDataInMsg.subscribeTo(SimBase.DynModels.CSSConstellationObject.constellationOutMsg) + self.cssWlsEst.cssDataInMsg.subscribeTo( + SimBase.DynModels.CSSConstellationObject.constellationOutMsg + ) self.cssWlsEst.cssConfigInMsg.subscribeTo(self.cssConfigMsg) def SetMRPFeedbackControl(self, SimBase): """Set the MRP feedback module configuration""" self.mrpFeedbackControl.guidInMsg.subscribeTo(self.attGuidMsg) self.mrpFeedbackControl.vehConfigInMsg.subscribeTo(self.vcMsg) - messaging.CmdTorqueBodyMsg_C_addAuthor(self.mrpFeedbackControl.cmdTorqueOutMsg, self.cmdTorqueDirectMsg) + messaging.CmdTorqueBodyMsg_C_addAuthor( + self.mrpFeedbackControl.cmdTorqueOutMsg, self.cmdTorqueDirectMsg + ) self.mrpFeedbackControl.K = 3.5 - self.mrpFeedbackControl.Ki = -1.0 # Note: make value negative to turn off integral feedback + self.mrpFeedbackControl.Ki = ( + -1.0 + ) # Note: make value negative to turn off integral feedback self.mrpFeedbackControl.P = 30.0 - self.mrpFeedbackControl.integralLimit = 2. / self.mrpFeedbackControl.Ki * 0.1 + self.mrpFeedbackControl.integralLimit = 2.0 / self.mrpFeedbackControl.Ki * 0.1 def SetMRPFeedbackRWA(self, SimBase): """Set the MRP feedback information if RWs are considered""" self.mrpFeedbackRWs.K = 3.5 - self.mrpFeedbackRWs.Ki = -1 # Note: make value negative to turn off integral feedback + self.mrpFeedbackRWs.Ki = ( + -1 + ) # Note: make value negative to turn off integral feedback self.mrpFeedbackRWs.P = 30.0 - self.mrpFeedbackRWs.integralLimit = 2. / self.mrpFeedbackRWs.Ki * 0.1 + self.mrpFeedbackRWs.integralLimit = 2.0 / self.mrpFeedbackRWs.Ki * 0.1 self.mrpFeedbackRWs.vehConfigInMsg.subscribeTo(self.vcMsg) - self.mrpFeedbackRWs.rwSpeedsInMsg.subscribeTo(SimBase.DynModels.rwStateEffector.rwSpeedOutMsg) + self.mrpFeedbackRWs.rwSpeedsInMsg.subscribeTo( + SimBase.DynModels.rwStateEffector.rwSpeedOutMsg + ) self.mrpFeedbackRWs.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) self.mrpFeedbackRWs.guidInMsg.subscribeTo(self.attGuidMsg) - messaging.CmdTorqueBodyMsg_C_addAuthor(self.mrpFeedbackRWs.cmdTorqueOutMsg, self.cmdTorqueMsg) + messaging.CmdTorqueBodyMsg_C_addAuthor( + self.mrpFeedbackRWs.cmdTorqueOutMsg, self.cmdTorqueMsg + ) def SetMRPSteering(self): """Set the MRP Steering module""" @@ -386,14 +452,18 @@ def SetRateServo(self, SimBase): self.rateServo.guidInMsg.subscribeTo(self.attGuidMsg) self.rateServo.vehConfigInMsg.subscribeTo(self.vcMsg) self.rateServo.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) - self.rateServo.rwSpeedsInMsg.subscribeTo(SimBase.DynModels.rwStateEffector.rwSpeedOutMsg) + self.rateServo.rwSpeedsInMsg.subscribeTo( + SimBase.DynModels.rwStateEffector.rwSpeedOutMsg + ) self.rateServo.rateSteeringInMsg.subscribeTo(self.mrpSteering.rateCmdOutMsg) - messaging.CmdTorqueBodyMsg_C_addAuthor(self.rateServo.cmdTorqueOutMsg, self.cmdTorqueMsg) + messaging.CmdTorqueBodyMsg_C_addAuthor( + self.rateServo.cmdTorqueOutMsg, self.cmdTorqueMsg + ) self.rateServo.Ki = 5.0 self.rateServo.P = 150.0 - self.rateServo.integralLimit = 2. / self.rateServo.Ki * 0.1 - self.rateServo.knownTorquePntB_B = [0., 0., 0.] + self.rateServo.integralLimit = 2.0 / self.rateServo.Ki * 0.1 + self.rateServo.knownTorquePntB_B = [0.0, 0.0, 0.0] def SetVehicleConfiguration(self): """Set the spacecraft configuration information""" @@ -411,54 +481,72 @@ def SetRWConfigMsg(self): fswSetupRW.clearSetup() for elAngle, azAngle in zip(rwElAngle, rwAzimuthAngle): - gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot(np.array([1, 0, 0])) - fswSetupRW.create(gsHat, # spin axis - wheelJs, # kg*m^2 - 0.2) # Nm uMax + gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot( + np.array([1, 0, 0]) + ) + fswSetupRW.create( + gsHat, # spin axis + wheelJs, # kg*m^2 + 0.2, + ) # Nm uMax self.fswRwConfigMsg = fswSetupRW.writeConfigMessage() def SetRWMotorTorque(self): """Set the RW motor torque information""" - controlAxes_B = [ - 1.0, 0.0, 0.0 - , 0.0, 1.0, 0.0 - , 0.0, 0.0, 1.0 - ] + controlAxes_B = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] self.rwMotorTorque.controlAxes_B = controlAxes_B self.rwMotorTorque.vehControlInMsg.subscribeTo(self.cmdTorqueMsg) - messaging.ArrayMotorTorqueMsg_C_addAuthor(self.rwMotorTorque.rwMotorTorqueOutMsg, self.cmdRwMotorMsg) + messaging.ArrayMotorTorqueMsg_C_addAuthor( + self.rwMotorTorque.rwMotorTorqueOutMsg, self.cmdRwMotorMsg + ) self.rwMotorTorque.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) def SetLambertPlannerObject(self, SimBase): """Set the lambert planner object.""" - self.lambertPlannerObject.navTransInMsg.subscribeTo(SimBase.DynModels.simpleNavObject.transOutMsg) + self.lambertPlannerObject.navTransInMsg.subscribeTo( + SimBase.DynModels.simpleNavObject.transOutMsg + ) def SetLambertSolverObject(self): """Set the lambert solver object.""" - self.lambertSolverObject.lambertProblemInMsg.subscribeTo(self.lambertPlannerObject.lambertProblemOutMsg) + self.lambertSolverObject.lambertProblemInMsg.subscribeTo( + self.lambertPlannerObject.lambertProblemOutMsg + ) def SetLambertValidatorObject(self, SimBase): """Set the lambert validator object.""" - self.lambertValidatorObject.navTransInMsg.subscribeTo(SimBase.DynModels.simpleNavObject.transOutMsg) - self.lambertValidatorObject.lambertProblemInMsg.subscribeTo(self.lambertPlannerObject.lambertProblemOutMsg) + self.lambertValidatorObject.navTransInMsg.subscribeTo( + SimBase.DynModels.simpleNavObject.transOutMsg + ) + self.lambertValidatorObject.lambertProblemInMsg.subscribeTo( + self.lambertPlannerObject.lambertProblemOutMsg + ) self.lambertValidatorObject.lambertPerformanceInMsg.subscribeTo( - self.lambertSolverObject.lambertPerformanceOutMsg) - self.lambertValidatorObject.lambertSolutionInMsg.subscribeTo(self.lambertSolverObject.lambertSolutionOutMsg) + self.lambertSolverObject.lambertPerformanceOutMsg + ) + self.lambertValidatorObject.lambertSolutionInMsg.subscribeTo( + self.lambertSolverObject.lambertSolutionOutMsg + ) self.lambertValidatorObject.dvBurnCmdOutMsg = self.dvBurnCmdMsg def SetLambertSurfaceRelativeVelocityObject(self, SimBase): """Set the lambert surface relative velocity object.""" self.lambertSurfaceRelativeVelocityObject.lambertProblemInMsg.subscribeTo( - self.lambertPlannerObject.lambertProblemOutMsg) + self.lambertPlannerObject.lambertProblemOutMsg + ) self.lambertSurfaceRelativeVelocityObject.ephemerisInMsg.subscribeTo( - SimBase.DynModels.EarthEphemObject.ephemOutMsgs[0]) + SimBase.DynModels.EarthEphemObject.ephemOutMsgs[0] + ) def SetLambertSecondDvObject(self): """Set the lambert second DV object.""" - self.lambertSecondDvObject.lambertSolutionInMsg.subscribeTo(self.lambertSolverObject.lambertSolutionOutMsg) + self.lambertSecondDvObject.lambertSolutionInMsg.subscribeTo( + self.lambertSolverObject.lambertSolutionOutMsg + ) self.lambertSecondDvObject.desiredVelocityInMsg.subscribeTo( - self.lambertSurfaceRelativeVelocityObject.desiredVelocityOutMsg) + self.lambertSurfaceRelativeVelocityObject.desiredVelocityOutMsg + ) self.lambertSecondDvObject.dvBurnCmdOutMsg = self.dvBurnCmdMsg # Global call to initialize every module @@ -501,8 +589,12 @@ def setupGatewayMsgs(self, SimBase): self.zeroGateWayMsgs() # connect gateway FSW effector command msgs with the dynamics - SimBase.DynModels.extForceTorqueObject.cmdTorqueInMsg.subscribeTo(self.cmdTorqueDirectMsg) - SimBase.DynModels.rwStateEffector.rwMotorCmdInMsg.subscribeTo(self.cmdRwMotorMsg) + SimBase.DynModels.extForceTorqueObject.cmdTorqueInMsg.subscribeTo( + self.cmdTorqueDirectMsg + ) + SimBase.DynModels.rwStateEffector.rwMotorCmdInMsg.subscribeTo( + self.cmdRwMotorMsg + ) def zeroGateWayMsgs(self): """Zero all the FSW gateway message payloads""" diff --git a/examples/BskSim/plotting/BSK_Plotting.py b/examples/BskSim/plotting/BSK_Plotting.py index d66f9f70ae..01f5cf8bab 100644 --- a/examples/BskSim/plotting/BSK_Plotting.py +++ b/examples/BskSim/plotting/BSK_Plotting.py @@ -23,30 +23,33 @@ from Basilisk.utilities import unitTestSupport # --------------------------------- COMPONENTS & SUBPLOT HANDLING ----------------------------------------------- # -color_x = 'dodgerblue' -color_y = 'salmon' -color_z = 'lightgreen' +color_x = "dodgerblue" +color_y = "salmon" +color_z = "lightgreen" m2km = 1.0 / 1000.0 + def show_all_plots(): plt.show() + def clear_all_plots(): plt.close("all") + def save_all_plots(fileName, figureNames): figureList = {} numFigures = len(figureNames) for i in range(0, numFigures): pltName = fileName + "_" + figureNames[i] - figureList[pltName] = plt.figure(i+1) + figureList[pltName] = plt.figure(i + 1) return figureList def plot3components(timeAxis, vec, id=None): plt.figure(id) time = timeAxis * mc.NANO2MIN - plt.xlabel('Time, min') + plt.xlabel("Time, min") plt.plot(time, vec[:, 0], color_x) plt.plot(time, vec[:, 1], color_y) plt.plot(time, vec[:, 2], color_z) @@ -54,25 +57,26 @@ def plot3components(timeAxis, vec, id=None): def plot_sigma(timeAxis, sigma, id=None): plot3components(timeAxis, sigma, id) - plt.legend([r'$\sigma_1$', r'$\sigma_2$', r'$\sigma_3$']) - plt.ylabel('MRP') + plt.legend([r"$\sigma_1$", r"$\sigma_2$", r"$\sigma_3$"]) + plt.ylabel("MRP") def plot_omega(timeAxis, omega, id=None): plot3components(timeAxis, omega, id) - plt.ylabel('Angular Rate, rad/s') - plt.legend([r'$\omega_1$', r'$\omega_2$', r'$\omega_3$']) + plt.ylabel("Angular Rate, rad/s") + plt.legend([r"$\omega_1$", r"$\omega_2$", r"$\omega_3$"]) + def subplot_sigma(subplot, timeAxis, sigma, id=None): plot3components(timeAxis, sigma, id) - plt.legend([r'$\sigma_1$', r'$\sigma_2$', r'$\sigma_3$']) - plt.ylabel('MRP') + plt.legend([r"$\sigma_1$", r"$\sigma_2$", r"$\sigma_3$"]) + plt.ylabel("MRP") def subplot_omega(subplot, timeAxis, omega, id=None): plot3components(timeAxis, omega, id) - plt.ylabel('Angular Rate, rad/s') - plt.legend([r'$\omega_1$', r'$\omega_2$', r'$\omega_3$']) + plt.ylabel("Angular Rate, rad/s") + plt.legend([r"$\omega_1$", r"$\omega_2$", r"$\omega_3$"]) # ------------------------------------- MAIN PLOT HANDLING ------------------------------------------------------ # @@ -87,9 +91,9 @@ def subplot_omega(subplot, timeAxis, omega, id=None): def plot_controlTorque(timeAxis, Lr, id=None): plot3components(timeAxis, Lr, id) - plt.ylabel(r'Control Torque, $N \cdot m$') - plt.legend(['$L_{r,1}$', '$L_{r,2}$', '$L_{r,3}$']) - plt.title('Control Torque $L_r$') + plt.ylabel(r"Control Torque, $N \cdot m$") + plt.legend(["$L_{r,1}$", "$L_{r,2}$", "$L_{r,3}$"]) + plt.title("Control Torque $L_r$") return @@ -97,57 +101,57 @@ def plot_trackingError(timeAxis, sigma_BR, omega_BR_B, id=None): # plt.figure(id) plt.subplot(211) plot_sigma(timeAxis, sigma_BR, id) - plt.title(r'Att Error: $\sigma_{BR}$') + plt.title(r"Att Error: $\sigma_{BR}$") plt.subplot(212) - #plt.figure(id) + # plt.figure(id) plot_omega(timeAxis, omega_BR_B, id) - plt.title(r'Rate Error: $^B{\omega_{BR}}$') + plt.title(r"Rate Error: $^B{\omega_{BR}}$") return def plot_attitudeGuidance(timeAxis, sigma_RN, omega_RN_N, id=None): plot_sigma(timeAxis, sigma_RN, id) plt.ylim([-1.0, 1.0]) - plt.title(r'Ref Att: $\sigma_{RN}$') + plt.title(r"Ref Att: $\sigma_{RN}$") plot_omega(timeAxis, omega_RN_N, id) - plt.title(r'Ref Rate: $^N{\omega_{RN}}$') + plt.title(r"Ref Rate: $^N{\omega_{RN}}$") return def plot_rotationalNav(timeAxis, sigma_BN, omega_BN_B, id=None): plt.figure() plot_sigma(timeAxis, sigma_BN, id) - plt.title(r'Sc Att: $\sigma_{BN}$') + plt.title(r"Sc Att: $\sigma_{BN}$") plot_omega(timeAxis, omega_BN_B, id) - plt.title(r'Sc Rate: $^B{\omega_{BN}}$') + plt.title(r"Sc Rate: $^B{\omega_{BN}}$") return def plot_shadow_fraction(timeAxis, shadow_factor, id=None): plt.figure(id) plt.plot(timeAxis, shadow_factor) - plt.xlabel('Time min') - plt.ylabel('Shadow Fraction') + plt.xlabel("Time min") + plt.ylabel("Shadow Fraction") return def plot_sun_point(timeAxis, sunPoint, id=None): plot3components(timeAxis, sunPoint, id) - plt.xlabel('Time') - plt.ylabel('Sun Point Vec') + plt.xlabel("Time") + plt.ylabel("Sun Point Vec") return def plot_orbit(r_BN, id=None): plt.figure(id) - plt.xlabel('$R_x$, km') - plt.ylabel('$R_y$, km') + plt.xlabel("$R_x$, km") + plt.ylabel("$R_y$, km") plt.plot(r_BN[:, 0] * m2km, r_BN[:, 1] * m2km, color_x) plt.scatter(0, 0, c=color_x) - plt.title('Spacecraft Orbit') + plt.title("Spacecraft Orbit") return @@ -157,40 +161,50 @@ def plot_attitude_error(timeLineSet, dataSigmaBR, id=None): ax = fig.gca() vectorData = unitTestSupport.pullVectorSetFromData(dataSigmaBR) sNorm = np.array([np.linalg.norm(v) for v in vectorData]) - plt.plot(timeLineSet, sNorm, - color=unitTestSupport.getLineColor(1, 3), - ) - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error Norm $|\sigma_{B/R}|$') - ax.set_yscale('log') + plt.plot( + timeLineSet, + sNorm, + color=unitTestSupport.getLineColor(1, 3), + ) + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error Norm $|\sigma_{B/R}|$") + ax.set_yscale("log") def plot_control_torque(timeLineSet, dataLr, id=None, livePlot=False): plt.figure(id) for idx in range(3): - plt.plot(timeLineSet, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') + plt.plot( + timeLineSet, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) if not livePlot: - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Control Torque $L_r$ [Nm]') + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Control Torque $L_r$ [Nm]") def plot_rate_error(timeLineSet, dataOmegaBR, id=None, livePlot=False): plt.figure(id) for idx in range(3): - plt.plot(timeLineSet, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') + plt.plot( + timeLineSet, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) if not livePlot: - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") return -def plot_orientation(timeLineSet, vectorPosData, vectorVelData, vectorMRPData, id=None, livePlot=False): +def plot_orientation( + timeLineSet, vectorPosData, vectorVelData, vectorMRPData, id=None, livePlot=False +): data = np.empty([len(vectorPosData), 3]) for idx in range(0, len(vectorPosData)): ir = vectorPosData[idx] / np.linalg.norm(vectorPosData[idx]) @@ -198,75 +212,109 @@ def plot_orientation(timeLineSet, vectorPosData, vectorVelData, vectorMRPData, i ih = hv / np.linalg.norm(hv) itheta = np.cross(ih, ir) dcmBN = RigidBodyKinematics.MRP2C(vectorMRPData[idx]) - data[idx] = [np.dot(ir, dcmBN[0]), np.dot(itheta, dcmBN[1]), np.dot(ih, dcmBN[2])] + data[idx] = [ + np.dot(ir, dcmBN[0]), + np.dot(itheta, dcmBN[1]), + np.dot(ih, dcmBN[2]), + ] plt.figure(id) - labelStrings = (r'$\hat\imath_r\cdot \hat b_1$' - , r'${\hat\imath}_{\theta}\cdot \hat b_2$' - , r'$\hat\imath_h\cdot \hat b_3$') + labelStrings = ( + r"$\hat\imath_r\cdot \hat b_1$", + r"${\hat\imath}_{\theta}\cdot \hat b_2$", + r"$\hat\imath_h\cdot \hat b_3$", + ) for idx in range(0, 3): - plt.plot(timeLineSet, data[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=labelStrings[idx]) + plt.plot( + timeLineSet, + data[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=labelStrings[idx], + ) if not livePlot: - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Orientation Illustration') + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Orientation Illustration") def plot_rw_cmd_torque(timeData, dataUsReq, numRW, id=None, livePlot=False): plt.figure(id) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) if not livePlot: - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") -def plot_rw_cmd_actual_torque(timeData, dataUsReq, dataRW, numRW, id=None, livePlot=False): +def plot_rw_cmd_actual_torque( + timeData, dataUsReq, dataRW, numRW, id=None, livePlot=False +): """compare commanded and actual RW motor torques""" plt.figure(id) for idx in range(numRW): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.plot(timeData, dataRW[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.plot( + timeData, + dataRW[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") def plot_rw_speeds(timeData, dataOmegaRW, numRW, id=None, livePlot=False): plt.figure(id) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / mc.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx) + '}$') + plt.plot( + timeData, + dataOmegaRW[:, idx] / mc.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx) + "}$", + ) if not livePlot: - plt.legend(loc='upper right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed (RPM) ') + plt.legend(loc="upper right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed (RPM) ") + -def plot_rw_friction(timeData, dataFrictionRW, numRW, dataFaultLog=[], id=None, livePlot=False): +def plot_rw_friction( + timeData, dataFrictionRW, numRW, dataFaultLog=[], id=None, livePlot=False +): plt.figure(id) for idx in range(numRW): - plt.plot(timeData, dataFrictionRW[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$RW_{' + str(idx+1) + '} Friction$') + plt.plot( + timeData, + dataFrictionRW[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$RW_{" + str(idx + 1) + "} Friction$", + ) if dataFaultLog: # fourth column of dataFaultLog is the fault times - plt.scatter([row[3] for row in dataFaultLog], np.zeros(len(dataFaultLog)), marker="x", color=(1,0,0), - label='Faults') + plt.scatter( + [row[3] for row in dataFaultLog], + np.zeros(len(dataFaultLog)), + marker="x", + color=(1, 0, 0), + label="Faults", + ) if not livePlot: - plt.legend(loc='upper right') - plt.xlabel('Time [min]') - plt.ylabel('RW Static Friction ') + plt.legend(loc="upper right") + plt.xlabel("Time [min]") + plt.ylabel("RW Static Friction ") def plot_planet(oe, planet): @@ -276,11 +324,11 @@ def plot_planet(oe, planet): # draw the planet fig = plt.gcf() ax = fig.gca() - planetColor = '#008800' + planetColor = "#008800" planetRadius = planet.radEquator / 1000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) - plt.xlabel('$i_e$ Cord. [km]') - plt.ylabel('$i_p$ Cord. [km]') + plt.xlabel("$i_e$ Cord. [km]") + plt.ylabel("$i_p$ Cord. [km]") plt.grid() @@ -294,13 +342,23 @@ def plot_peri_and_orbit(oe, mu, r_BN_N, v_BN_N, id=None): rData.append(oeData.rmag) fData.append(oeData.f + oeData.omega - oe.omega) plt.figure(id) - plt.plot(rData * np.cos(fData) / 1000, rData * np.sin(fData) / 1000, color='#aa0000', linewidth=3.0) + 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.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + "--", + color="#555555", + ) def plot_rel_orbit(timeData, r_chief, r_deputy, id=None, livePlot=False): @@ -320,50 +378,50 @@ def plot_position(time, r_BN_N_truth, r_BN_N_meas, tTN, r_TN_N, id=None): """Plot the position result.""" fig, ax = plt.subplots(3, sharex=True, num=id) fig.add_subplot(111, frameon=False) - plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time, r_BN_N_meas[:, 0], 'k*', label='measurement', markersize=2) - ax[1].plot(time, r_BN_N_meas[:, 1], 'k*', markersize=2) - ax[2].plot(time, r_BN_N_meas[:, 2], 'k*', markersize=2) + ax[0].plot(time, r_BN_N_meas[:, 0], "k*", label="measurement", markersize=2) + ax[1].plot(time, r_BN_N_meas[:, 1], "k*", markersize=2) + ax[2].plot(time, r_BN_N_meas[:, 2], "k*", markersize=2) - ax[0].plot(time, r_BN_N_truth[:, 0], label='truth') + ax[0].plot(time, r_BN_N_truth[:, 0], label="truth") ax[1].plot(time, r_BN_N_truth[:, 1]) ax[2].plot(time, r_BN_N_truth[:, 2]) - ax[0].plot(tTN, r_TN_N[0], 'rx', label='target') - ax[1].plot(tTN, r_TN_N[1], 'rx') - ax[2].plot(tTN, r_TN_N[2], 'rx') + ax[0].plot(tTN, r_TN_N[0], "rx", label="target") + ax[1].plot(tTN, r_TN_N[1], "rx") + ax[2].plot(tTN, r_TN_N[2], "rx") - plt.xlabel('Time [min]') - plt.title('Spacecraft Position') + plt.xlabel("Time [min]") + plt.title("Spacecraft Position") - ax[0].set_ylabel('${}^Nr_{BN_1}$ [m]') - ax[1].set_ylabel('${}^Nr_{BN_2}$ [m]') - ax[2].set_ylabel('${}^Nr_{BN_3}$ [m]') + ax[0].set_ylabel("${}^Nr_{BN_1}$ [m]") + ax[1].set_ylabel("${}^Nr_{BN_2}$ [m]") + ax[2].set_ylabel("${}^Nr_{BN_3}$ [m]") - ax[0].legend(loc='upper right') + ax[0].legend(loc="upper right") def plot_velocity(time, v_BN_N_truth, v_BN_N_meas, id=None): """Plot the velocity result.""" fig, ax = plt.subplots(3, sharex=True, num=id) fig.add_subplot(111, frameon=False) - plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time, v_BN_N_meas[:, 0], 'k*', label='measurement', markersize=2) - ax[1].plot(time, v_BN_N_meas[:, 1], 'k*', markersize=2) - ax[2].plot(time, v_BN_N_meas[:, 2], 'k*', markersize=2) + ax[0].plot(time, v_BN_N_meas[:, 0], "k*", label="measurement", markersize=2) + ax[1].plot(time, v_BN_N_meas[:, 1], "k*", markersize=2) + ax[2].plot(time, v_BN_N_meas[:, 2], "k*", markersize=2) - ax[0].plot(time, v_BN_N_truth[:, 0], label='truth') + ax[0].plot(time, v_BN_N_truth[:, 0], label="truth") ax[1].plot(time, v_BN_N_truth[:, 1]) ax[2].plot(time, v_BN_N_truth[:, 2]) - plt.xlabel('Time [min]') - plt.title('Spacecraft Velocity') + plt.xlabel("Time [min]") + plt.title("Spacecraft Velocity") - ax[0].set_ylabel('${}^Nv_{BN_1}$ [m/s]') - ax[1].set_ylabel('${}^Nv_{BN_2}$ [m/s]') - ax[2].set_ylabel('${}^Nv_{BN_3}$ [m/s]') + ax[0].set_ylabel("${}^Nv_{BN_1}$ [m/s]") + ax[1].set_ylabel("${}^Nv_{BN_2}$ [m/s]") + ax[2].set_ylabel("${}^Nv_{BN_3}$ [m/s]") ax[0].legend() @@ -373,23 +431,29 @@ def plot_surface_rel_velocity(timeData, r_BN_N, v_BN_N, sigma_PN, omega_PN_P, id for idx in range(0, len(r_BN_N)): dcmPN = RigidBodyKinematics.MRP2C(sigma_PN[idx]) omega_PN_N = np.dot(dcmPN.transpose(), omega_PN_P[idx]) - s1Hat_N = np.cross(omega_PN_N, r_BN_N[idx])/np.linalg.norm(np.cross(omega_PN_N, r_BN_N[idx])) - s3Hat_N = r_BN_N[idx]/np.linalg.norm(r_BN_N[idx]) - s2Hat_N = np.cross(s3Hat_N, s1Hat_N)/np.linalg.norm(np.cross(s3Hat_N, s1Hat_N)) - dcmSN = np.array([s1Hat_N.transpose(), s2Hat_N.transpose(), s3Hat_N.transpose()]) + s1Hat_N = np.cross(omega_PN_N, r_BN_N[idx]) / np.linalg.norm( + np.cross(omega_PN_N, r_BN_N[idx]) + ) + s3Hat_N = r_BN_N[idx] / np.linalg.norm(r_BN_N[idx]) + s2Hat_N = np.cross(s3Hat_N, s1Hat_N) / np.linalg.norm( + np.cross(s3Hat_N, s1Hat_N) + ) + dcmSN = np.array( + [s1Hat_N.transpose(), s2Hat_N.transpose(), s3Hat_N.transpose()] + ) v_BS_S[idx] = np.dot(dcmSN, v_BN_N[idx] - np.cross(omega_PN_N, r_BN_N[idx])) fig, ax = plt.subplots(3, sharex=True, num=id) fig.add_subplot(111, frameon=False) - plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) ax[0].plot(timeData, v_BS_S[:, 0]) ax[1].plot(timeData, v_BS_S[:, 1]) ax[2].plot(timeData, v_BS_S[:, 2]) - plt.xlabel('Time [min]') - plt.title('Surface Relative Velocity') + plt.xlabel("Time [min]") + plt.title("Surface Relative Velocity") - ax[0].set_ylabel('${}^Sv_{BS_1}$ [m/s]') - ax[1].set_ylabel('${}^Sv_{BS_2}$ [m/s]') - ax[2].set_ylabel('${}^Sv_{BS_3}$ [m/s]') + ax[0].set_ylabel("${}^Sv_{BS_1}$ [m/s]") + ax[1].set_ylabel("${}^Sv_{BS_2}$ [m/s]") + ax[2].set_ylabel("${}^Sv_{BS_3}$ [m/s]") diff --git a/examples/BskSim/scenarios/scenario_AddRWFault.py b/examples/BskSim/scenarios/scenario_AddRWFault.py index cab860f04c..a9d210ad5a 100644 --- a/examples/BskSim/scenarios/scenario_AddRWFault.py +++ b/examples/BskSim/scenarios/scenario_AddRWFault.py @@ -111,9 +111,9 @@ def action_repeatedRWFault(self): path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../') -sys.path.append(path + '/../models') -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../") +sys.path.append(path + "/../models") +sys.path.append(path + "/../plotting") import BSK_Dynamics import BSK_Fsw import BSK_Plotting as BSK_plt @@ -124,7 +124,7 @@ def action_repeatedRWFault(self): class scenario_AddRWFault(BSKSim, BSKScenario): def __init__(self): super(scenario_AddRWFault, self).__init__() - self.name = 'scenario_AddRWFault' + self.name = "scenario_AddRWFault" # declare additional class variables self.msgRecList = {} @@ -139,7 +139,7 @@ def __init__(self): self.oneTimeRWFaultFlag = 1 self.repeatRWFaultFlag = 1 - self.oneTimeFaultTime = macros.min2nano(10.) + self.oneTimeFaultTime = macros.min2nano(10.0) DynModels = self.get_DynModel() self.DynModels.RWFaultLog = [] @@ -155,13 +155,17 @@ def configure_initial_conditions(self): oe.f = 85.3 * macros.D2R DynModels = self.get_DynModel() - mu = DynModels.gravFactory.gravBodies['earth'].mu + mu = DynModels.gravFactory.gravBodies["earth"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) DynModels.scObject.hub.r_CN_NInit = rN # m - r_CN_N DynModels.scObject.hub.v_CN_NInit = vN # m/s - v_CN_N DynModels.scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels.scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B + DynModels.scObject.hub.omega_BN_BInit = [ + [0.001], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B def log_outputs(self): FswModel = self.get_FswModel() @@ -174,12 +178,16 @@ def log_outputs(self): self.msgRecList[self.attGuidName] = FswModel.attGuidMsg.recorder(samplingTime) self.AddModelToTask(DynModel.taskName, self.msgRecList[self.attGuidName]) - self.msgRecList[self.sNavTransName] = DynModel.simpleNavObject.transOutMsg.recorder(samplingTime) + self.msgRecList[self.sNavTransName] = ( + DynModel.simpleNavObject.transOutMsg.recorder(samplingTime) + ) self.AddModelToTask(DynModel.taskName, self.msgRecList[self.sNavTransName]) self.rwLogs = [] for item in range(4): - self.rwLogs.append(DynModel.rwStateEffector.rwOutMsgs[item].recorder(samplingTime)) + self.rwLogs.append( + DynModel.rwStateEffector.rwOutMsgs[item].recorder(samplingTime) + ) self.AddModelToTask(DynModel.taskName, self.rwLogs[item]) return @@ -203,7 +211,9 @@ def pull_outputs(self, showPlots): BSK_plt.plot_attitude_error(timeData, sigma_BR) BSK_plt.plot_rate_error(timeData, omega_BR_B) BSK_plt.plot_rw_speeds(timeData, RW_speeds, num_RW) - BSK_plt.plot_rw_friction(timeData, RW_friction, num_RW, self.DynModels.RWFaultLog) + BSK_plt.plot_rw_friction( + timeData, RW_friction, num_RW, self.DynModels.RWFaultLog + ) figureList = {} if showPlots: @@ -219,7 +229,7 @@ def pull_outputs(self, showPlots): def runScenario(scenario): """method to initialize and execute the scenario""" - simulationTime = macros.min2nano(30.) + simulationTime = macros.min2nano(30.0) scenario.modeRequest = "hillPoint" # Run the simulation @@ -232,10 +242,10 @@ def runScenario(scenario): def run(showPlots): """ - The scenarios can be run with the following parameters: + The scenarios can be run with the following parameters: - Args: - showPlots (bool): Determines if the script should display plots + Args: + showPlots (bool): Determines if the script should display plots """ scenario = scenario_AddRWFault() @@ -243,5 +253,6 @@ def run(showPlots): figureList = scenario.pull_outputs(showPlots) return figureList + if __name__ == "__main__": run(True) diff --git a/examples/BskSim/scenarios/scenario_AttEclipse.py b/examples/BskSim/scenarios/scenario_AttEclipse.py index 9f24947d24..4822edc498 100755 --- a/examples/BskSim/scenarios/scenario_AttEclipse.py +++ b/examples/BskSim/scenarios/scenario_AttEclipse.py @@ -130,6 +130,7 @@ import sys import numpy as np + # Import utilities from Basilisk.utilities import orbitalMotion, macros, vizSupport @@ -137,15 +138,16 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_masters import BSKSim, BSKScenario import BSK_Dynamics, BSK_Fsw # Import plotting file for your scenario -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../plotting") import BSK_Plotting as BSK_plt -sys.path.append(path + '/../../scenarios') +sys.path.append(path + "/../../scenarios") + # To begin, one must first create a class that will # inherent from the masterSim class and provide a name to the sim. @@ -154,7 +156,7 @@ class scenario_AttitudeEclipse(BSKSim, BSKScenario): def __init__(self): super(scenario_AttitudeEclipse, self).__init__(fswRate=1.0, dynRate=1.0) - self.name = 'scenario_AttitudeEclipse' + self.name = "scenario_AttitudeEclipse" self.shadowRec = None self.rwSpeedRec = None @@ -170,10 +172,13 @@ def __init__(self): # if this scenario is to interface with the BSK Viz, uncomment the following line DynModels = self.get_DynModel() - vizSupport.enableUnityVisualization(self, DynModels.taskName, DynModels.scObject - # , saveFile=__file__ - , rwEffectorList=DynModels.rwStateEffector - ) + vizSupport.enableUnityVisualization( + self, + DynModels.taskName, + DynModels.scObject, + # , saveFile=__file__ + rwEffectorList=DynModels.rwStateEffector, + ) def configure_initial_conditions(self): # Configure Dynamics initial conditions @@ -185,25 +190,39 @@ def configure_initial_conditions(self): oe.Omega = 48.2 * macros.D2R oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R - mu = self.get_DynModel().gravFactory.gravBodies['earth'].mu + mu = self.get_DynModel().gravFactory.gravBodies["earth"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) self.get_DynModel().scObject.hub.r_CN_NInit = rN # m - r_CN_N self.get_DynModel().scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - self.get_DynModel().scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - self.get_DynModel().scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B + self.get_DynModel().scObject.hub.sigma_BNInit = [ + [0.1], + [0.2], + [-0.3], + ] # sigma_BN_B + self.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.001], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B def log_outputs(self): samplingTime = self.get_FswModel().processTasksTimeStep # Dynamics process outputs: log messages below if desired. - self.shadowRec = self.get_DynModel().eclipseObject.eclipseOutMsgs[0].recorder(samplingTime) - self.rwSpeedRec = self.get_DynModel().rwStateEffector.rwSpeedOutMsg.recorder(samplingTime) + self.shadowRec = ( + self.get_DynModel().eclipseObject.eclipseOutMsgs[0].recorder(samplingTime) + ) + self.rwSpeedRec = self.get_DynModel().rwStateEffector.rwSpeedOutMsg.recorder( + samplingTime + ) # FSW process outputs self.rwMotorRec = self.get_FswModel().cmdRwMotorMsg.recorder(samplingTime) self.sunSafeRec = self.get_FswModel().attGuidMsg.recorder(samplingTime) - self.cssEstRec = self.get_FswModel().cssWlsEst.navStateOutMsg.recorder(samplingTime) + self.cssEstRec = self.get_FswModel().cssWlsEst.navStateOutMsg.recorder( + samplingTime + ) self.AddModelToTask(self.get_DynModel().taskName, self.shadowRec) self.AddModelToTask(self.get_DynModel().taskName, self.rwSpeedRec) @@ -243,7 +262,14 @@ def pull_outputs(self, showPlots): BSK_plt.show_all_plots() else: fileName = os.path.basename(os.path.splitext(__file__)[0]) - figureNames = ["attitudeErrorNorm", "rwMotorTorque", "rateError", "rwSpeed", "shadowFraction", "sunDirectionVector"] + figureNames = [ + "attitudeErrorNorm", + "rwMotorTorque", + "rateError", + "rwSpeed", + "shadowFraction", + "sunDirectionVector", + ] figureList = BSK_plt.save_all_plots(fileName, figureNames) return figureList @@ -253,7 +279,7 @@ def runScenario(TheScenario): # Initialize simulation TheScenario.InitializeSimulation() # Configure FSW mode - TheScenario.modeRequest = 'sunSafePoint' + TheScenario.modeRequest = "sunSafePoint" # Configure run time and execute simulation simulationTime = macros.min2nano(60.0) @@ -263,10 +289,10 @@ def runScenario(TheScenario): def run(showPlots): """ - The scenarios can be run with the followings setups parameters: + The scenarios can be run with the followings setups parameters: - Args: - showPlots (bool): Determines if the script should display plots + Args: + showPlots (bool): Determines if the script should display plots """ diff --git a/examples/BskSim/scenarios/scenario_AttFeedback.py b/examples/BskSim/scenarios/scenario_AttFeedback.py index a93b80b672..0aa479dc63 100644 --- a/examples/BskSim/scenarios/scenario_AttFeedback.py +++ b/examples/BskSim/scenarios/scenario_AttFeedback.py @@ -35,9 +35,9 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../') -sys.path.append(path + '/../models') -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../") +sys.path.append(path + "/../models") +sys.path.append(path + "/../plotting") from BSK_masters import BSKSim, BSKScenario import BSK_Dynamics, BSK_Fsw import BSK_Plotting as BSK_plt @@ -47,7 +47,7 @@ class scenario_AttFeedback(BSKSim, BSKScenario): def __init__(self): super(scenario_AttFeedback, self).__init__() - self.name = 'scenarioBskSimAttFeedbackMC' + self.name = "scenarioBskSimAttFeedbackMC" # declare additional class variables self.msgRecList = {} @@ -62,10 +62,13 @@ def __init__(self): # if this scenario is to interface with the BSK Viz, uncomment the following line DynModels = self.get_DynModel() - vizSupport.enableUnityVisualization(self, DynModels.taskName, DynModels.scObject - # , saveFile=__file__ - , rwEffectorList=DynModels.rwStateEffector - ) + vizSupport.enableUnityVisualization( + self, + DynModels.taskName, + DynModels.scObject, + # , saveFile=__file__ + rwEffectorList=DynModels.rwStateEffector, + ) def configure_initial_conditions(self): # Configure Dynamics initial conditions @@ -78,13 +81,17 @@ def configure_initial_conditions(self): oe.f = 85.3 * macros.D2R DynModels = self.get_DynModel() - mu = DynModels.gravFactory.gravBodies['earth'].mu + mu = DynModels.gravFactory.gravBodies["earth"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) DynModels.scObject.hub.r_CN_NInit = rN # m - r_CN_N DynModels.scObject.hub.v_CN_NInit = vN # m/s - v_CN_N DynModels.scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels.scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B + DynModels.scObject.hub.omega_BN_BInit = [ + [0.001], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B def log_outputs(self): FswModel = self.get_FswModel() @@ -94,7 +101,9 @@ def log_outputs(self): self.msgRecList[self.attGuidName] = FswModel.attGuidMsg.recorder(samplingTime) self.AddModelToTask(DynModel.taskName, self.msgRecList[self.attGuidName]) - self.msgRecList[self.sNavTransName] = DynModel.simpleNavObject.transOutMsg.recorder(samplingTime) + self.msgRecList[self.sNavTransName] = ( + DynModel.simpleNavObject.transOutMsg.recorder(samplingTime) + ) self.AddModelToTask(DynModel.taskName, self.msgRecList[self.sNavTransName]) return @@ -124,10 +133,10 @@ def pull_outputs(self, showPlots): def runScenario(scenario): """method to initialize and execute the scenario""" - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) scenario.InitializeSimulation() - scenario.modeRequest = 'inertial3D' + scenario.modeRequest = "inertial3D" scenario.ConfigureStopTime(simulationTime) scenario.ExecuteSimulation() return @@ -135,10 +144,10 @@ def runScenario(scenario): def run(showPlots): """ - The scenarios can be run with the followings setups parameters: + The scenarios can be run with the followings setups parameters: - Args: - showPlots (bool): Determines if the script should display plots + Args: + showPlots (bool): Determines if the script should display plots """ scenario = scenario_AttFeedback() @@ -147,5 +156,6 @@ def run(showPlots): return + if __name__ == "__main__": run(True) diff --git a/examples/BskSim/scenarios/scenario_AttGuidHyperbolic.py b/examples/BskSim/scenarios/scenario_AttGuidHyperbolic.py index 67b0c87eae..e8a00d577c 100644 --- a/examples/BskSim/scenarios/scenario_AttGuidHyperbolic.py +++ b/examples/BskSim/scenarios/scenario_AttGuidHyperbolic.py @@ -73,13 +73,13 @@ """ - # Get current file path import inspect import os import sys import numpy as np + # Import utilities from Basilisk.utilities import orbitalMotion, macros, vizSupport @@ -87,15 +87,15 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_masters import BSKSim, BSKScenario # Import plotting files for your scenario -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../plotting") import BSK_Plotting as BSK_plt import BSK_Dynamics, BSK_Fsw -sys.path.append(path + '/../../') +sys.path.append(path + "/../../") import scenarioAttGuideHyperbolic as scene_plt @@ -103,7 +103,7 @@ class scenario_VelocityPointing(BSKSim, BSKScenario): def __init__(self): super(scenario_VelocityPointing, self).__init__() - self.name = 'scenario_VelocityPointing' + self.name = "scenario_VelocityPointing" self.attNavRec = None self.transNavRec = None @@ -118,10 +118,13 @@ def __init__(self): # if this scenario is to interface with the BSK Viz, uncomment the following line DynModels = self.get_DynModel() - vizSupport.enableUnityVisualization(self, DynModels.taskName, DynModels.scObject - # , saveFile=__file__ - , rwEffectorList=DynModels.rwStateEffector - ) + vizSupport.enableUnityVisualization( + self, + DynModels.taskName, + DynModels.scObject, + # , saveFile=__file__ + rwEffectorList=DynModels.rwStateEffector, + ) def configure_initial_conditions(self): # Configure Dynamics initial conditions @@ -132,22 +135,33 @@ def configure_initial_conditions(self): oe.Omega = 48.2 * macros.D2R oe.omega = 347.8 * macros.D2R oe.f = 30 * macros.D2R - mu = self.get_DynModel().gravFactory.gravBodies['earth'].mu + mu = self.get_DynModel().gravFactory.gravBodies["earth"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) self.get_DynModel().scObject.hub.r_CN_NInit = rN # m - r_CN_N self.get_DynModel().scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - self.get_DynModel().scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - self.get_DynModel().scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B + self.get_DynModel().scObject.hub.sigma_BNInit = [ + [0.1], + [0.2], + [-0.3], + ] # sigma_BN_B + self.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.001], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B # Safe orbit elements for postprocessing self.oe = oe - def log_outputs(self): # Dynamics process outputs samplingTime = self.get_FswModel().processTasksTimeStep - self.attNavRec = self.get_DynModel().simpleNavObject.attOutMsg.recorder(samplingTime) - self.transNavRec = self.get_DynModel().simpleNavObject.transOutMsg.recorder(samplingTime) + self.attNavRec = self.get_DynModel().simpleNavObject.attOutMsg.recorder( + samplingTime + ) + self.transNavRec = self.get_DynModel().simpleNavObject.transOutMsg.recorder( + samplingTime + ) # FSW process outputs self.attErrRec = self.get_FswModel().attGuidMsg.recorder(samplingTime) @@ -174,10 +188,13 @@ def pull_outputs(self, showPlots): scene_plt.plot_track_error_norm(timeLineSet, sigma_BR) scene_plt.plot_control_torque(timeLineSet, Lr) scene_plt.plot_rate_error(timeLineSet, omega_BR_B) - scene_plt.plot_orbit(self.oe, - self.get_DynModel().gravFactory.gravBodies['earth'].mu, - self.get_DynModel().gravFactory.gravBodies['earth'].radEquator, - r_BN_N, v_BN_N) + scene_plt.plot_orbit( + self.oe, + self.get_DynModel().gravFactory.gravBodies["earth"].mu, + self.get_DynModel().gravFactory.gravBodies["earth"].radEquator, + r_BN_N, + v_BN_N, + ) figureList = {} if showPlots: BSK_plt.show_all_plots() @@ -193,19 +210,20 @@ def runScenario(TheScenario): # Initialize simulation TheScenario.InitializeSimulation() # Configure FSW mode - TheScenario.modeRequest = 'velocityPoint' + TheScenario.modeRequest = "velocityPoint" # Configure run time and execute simulation - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) TheScenario.ConfigureStopTime(simulationTime) TheScenario.ExecuteSimulation() + def run(showPlots): """ - The scenarios can be run with the followings setups parameters: + The scenarios can be run with the followings setups parameters: - Args: - showPlots (bool): Determines if the script should display plots + Args: + showPlots (bool): Determines if the script should display plots """ # Instantiate base simulation @@ -217,5 +235,6 @@ def run(showPlots): return figureList + if __name__ == "__main__": run(True) diff --git a/examples/BskSim/scenarios/scenario_AttGuidance.py b/examples/BskSim/scenarios/scenario_AttGuidance.py index 8f28feb6b1..3e1e7a56b5 100644 --- a/examples/BskSim/scenarios/scenario_AttGuidance.py +++ b/examples/BskSim/scenarios/scenario_AttGuidance.py @@ -84,14 +84,13 @@ """ - - # Get current file path import inspect import os import sys import numpy as np + # Import utilities from Basilisk.utilities import orbitalMotion, macros, vizSupport @@ -99,19 +98,20 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_masters import BSKSim, BSKScenario import BSK_Dynamics, BSK_Fsw # Import plotting files for your scenario -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../plotting") import BSK_Plotting as BSK_plt + # Create your own scenario child class class scenario_HillPointing(BSKSim, BSKScenario): def __init__(self): super(scenario_HillPointing, self).__init__() - self.name = 'scenario_AttGuidance' + self.name = "scenario_AttGuidance" # declare additional class variables self.attNavRec = None @@ -128,10 +128,13 @@ def __init__(self): # if this scenario is to interface with the BSK Viz, uncomment the following line DynModels = self.get_DynModel() - vizSupport.enableUnityVisualization(self, DynModels.taskName, DynModels.scObject - # , saveFile=__file__ - , rwEffectorList=DynModels.rwStateEffector - ) + vizSupport.enableUnityVisualization( + self, + DynModels.taskName, + DynModels.scObject, + # , saveFile=__file__ + rwEffectorList=DynModels.rwStateEffector, + ) def configure_initial_conditions(self): # Configure Dynamics initial conditions @@ -144,13 +147,17 @@ def configure_initial_conditions(self): oe.f = 85.3 * macros.D2R DynModels = self.get_DynModel() - mu = DynModels.gravFactory.gravBodies['earth'].mu + mu = DynModels.gravFactory.gravBodies["earth"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) DynModels.scObject.hub.r_CN_NInit = rN # m - r_CN_N DynModels.scObject.hub.v_CN_NInit = vN # m/s - v_CN_N DynModels.scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels.scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B + DynModels.scObject.hub.omega_BN_BInit = [ + [0.001], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B def log_outputs(self): FswModel = self.get_FswModel() @@ -198,21 +205,26 @@ def pull_outputs(self, showPlots): BSK_plt.show_all_plots() else: fileName = os.path.basename(os.path.splitext(__file__)[0]) - figureNames = ["attitudeErrorNorm", "rwMotorTorque", "rateError", "orientation", "attitudeGuidance"] + figureNames = [ + "attitudeErrorNorm", + "rwMotorTorque", + "rateError", + "orientation", + "attitudeGuidance", + ] figureList = BSK_plt.save_all_plots(fileName, figureNames) return figureList -def runScenario(TheScenario): - +def runScenario(TheScenario): # Initialize simulation TheScenario.InitializeSimulation() # Configure FSW mode - TheScenario.modeRequest = 'hillPoint' + TheScenario.modeRequest = "hillPoint" # Configure run time and execute simulation - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) TheScenario.ConfigureStopTime(simulationTime) TheScenario.ExecuteSimulation() @@ -220,10 +232,10 @@ def runScenario(TheScenario): def run(showPlots): """ - The scenarios can be run with the followings setups parameters: + The scenarios can be run with the followings setups parameters: - Args: - showPlots (bool): Determines if the script should display plots + Args: + showPlots (bool): Determines if the script should display plots """ # Instantiate base simulation diff --git a/examples/BskSim/scenarios/scenario_AttModes.py b/examples/BskSim/scenarios/scenario_AttModes.py index eeed45b224..efc05b255a 100644 --- a/examples/BskSim/scenarios/scenario_AttModes.py +++ b/examples/BskSim/scenarios/scenario_AttModes.py @@ -49,9 +49,9 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../') -sys.path.append(path + '/../models') -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../") +sys.path.append(path + "/../models") +sys.path.append(path + "/../plotting") from BSK_masters import BSKSim, BSKScenario import BSK_Dynamics, BSK_Fsw import BSK_Plotting as BSK_plt @@ -61,7 +61,7 @@ class scenario_AttModes(BSKSim, BSKScenario): def __init__(self): super(scenario_AttModes, self).__init__() - self.name = 'scenario_AttModes' + self.name = "scenario_AttModes" # declare additional class variables self.msgRecList = {} @@ -92,13 +92,17 @@ def configure_initial_conditions(self): oe.f = 85.3 * macros.D2R DynModels = self.get_DynModel() - mu = DynModels.gravFactory.gravBodies['earth'].mu + mu = DynModels.gravFactory.gravBodies["earth"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) DynModels.scObject.hub.r_CN_NInit = rN # m - r_CN_N DynModels.scObject.hub.v_CN_NInit = vN # m/s - v_CN_N DynModels.scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels.scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B + DynModels.scObject.hub.omega_BN_BInit = [ + [0.001], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B def log_outputs(self): FswModel = self.get_FswModel() @@ -108,7 +112,9 @@ def log_outputs(self): self.msgRecList[self.attGuidName] = FswModel.attGuidMsg.recorder(samplingTime) self.AddModelToTask(DynModel.taskName, self.msgRecList[self.attGuidName]) - self.msgRecList[self.sNavTransName] = DynModel.simpleNavObject.transOutMsg.recorder(samplingTime) + self.msgRecList[self.sNavTransName] = ( + DynModel.simpleNavObject.transOutMsg.recorder(samplingTime) + ) self.AddModelToTask(DynModel.taskName, self.msgRecList[self.sNavTransName]) return @@ -138,18 +144,19 @@ def pull_outputs(self, showPlots): def runScenario(scenario): """method to initialize and execute the scenario""" - simulationTime = macros.min2nano(30.) + simulationTime = macros.min2nano(30.0) scenario.InitializeSimulation() - attitudeModeTime = macros.min2nano(10.) + attitudeModeTime = macros.min2nano(10.0) attitudeMode = ["hillPoint", "inertial3D"] currentSimulationTime = 0 while currentSimulationTime < simulationTime: - # Configure alternating FSW mode - scenario.modeRequest = attitudeMode[int((currentSimulationTime / attitudeModeTime) % len(attitudeMode))] + scenario.modeRequest = attitudeMode[ + int((currentSimulationTime / attitudeModeTime) % len(attitudeMode)) + ] # Add the attitude mode time to the current simulation time currentSimulationTime += attitudeModeTime @@ -163,10 +170,10 @@ def runScenario(scenario): def run(showPlots): """ - The scenarios can be run with the followings setups parameters: + The scenarios can be run with the followings setups parameters: - Args: - showPlots (bool): Determines if the script should display plots + Args: + showPlots (bool): Determines if the script should display plots """ scenario = scenario_AttModes() @@ -174,5 +181,6 @@ def run(showPlots): figureList = scenario.pull_outputs(showPlots) return figureList + if __name__ == "__main__": run(True) diff --git a/examples/BskSim/scenarios/scenario_AttSteering.py b/examples/BskSim/scenarios/scenario_AttSteering.py index 654fad48f3..a0914cf2e7 100644 --- a/examples/BskSim/scenarios/scenario_AttSteering.py +++ b/examples/BskSim/scenarios/scenario_AttSteering.py @@ -71,13 +71,13 @@ """ - # Get current file path import inspect import os import sys import numpy as np + # Import utilities from Basilisk.utilities import orbitalMotion, macros, vizSupport @@ -85,12 +85,12 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_masters import BSKSim, BSKScenario import BSK_Dynamics, BSK_Fsw # Import plotting file for your scenario -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../plotting") import BSK_Plotting as BSK_plt @@ -98,7 +98,7 @@ class scenario_AttitudeSteeringRW(BSKSim, BSKScenario): def __init__(self): super(scenario_AttitudeSteeringRW, self).__init__() - self.name = 'scenario_AttitudeSteeringRW' + self.name = "scenario_AttitudeSteeringRW" self.rwSpeedRec = None self.attErrRec = None @@ -113,10 +113,13 @@ def __init__(self): # if this scenario is to interface with the BSK Viz, uncomment the following line DynModels = self.get_DynModel() - vizSupport.enableUnityVisualization(self, DynModels.taskName, DynModels.scObject - # , saveFile=__file__ - , rwEffectorList=DynModels.rwStateEffector - ) + vizSupport.enableUnityVisualization( + self, + DynModels.taskName, + DynModels.scObject, + # , saveFile=__file__ + rwEffectorList=DynModels.rwStateEffector, + ) def configure_initial_conditions(self): # Configure Dynamics initial conditions @@ -127,7 +130,7 @@ def configure_initial_conditions(self): oe.Omega = 48.2 * macros.D2R oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R - mu = self.get_DynModel().gravFactory.gravBodies['earth'].mu + mu = self.get_DynModel().gravFactory.gravBodies["earth"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) self.get_DynModel().scObject.hub.r_CN_NInit = rN # [m] @@ -138,10 +141,14 @@ def configure_initial_conditions(self): def log_outputs(self): samplingTime = self.get_FswModel().processTasksTimeStep # Dynamics process outputs: - self.rwSpeedRec = self.get_DynModel().rwStateEffector.rwSpeedOutMsg.recorder(samplingTime) + self.rwSpeedRec = self.get_DynModel().rwStateEffector.rwSpeedOutMsg.recorder( + samplingTime + ) # FSW process outputs self.attErrRec = self.get_FswModel().attGuidMsg.recorder(samplingTime) - self.rateCmdRec = self.get_FswModel().mrpSteering.rateCmdOutMsg.recorder(samplingTime) + self.rateCmdRec = self.get_FswModel().mrpSteering.rateCmdOutMsg.recorder( + samplingTime + ) self.rwMotorRec = self.get_FswModel().cmdRwMotorMsg.recorder(samplingTime) self.AddModelToTask(self.get_DynModel().taskName, self.rwSpeedRec) @@ -180,18 +187,19 @@ def pull_outputs(self, showPlots): return figureList -def runScenario(scenario): +def runScenario(scenario): # Initialize simulation scenario.InitializeSimulation() # Configure FSW mode - scenario.modeRequest = 'steeringRW' + scenario.modeRequest = "steeringRW" # Configure run time and execute simulation - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) scenario.ConfigureStopTime(simulationTime) scenario.ExecuteSimulation() + def run(showPlots): """ The scenarios can be run with the followings setups parameters: @@ -208,5 +216,6 @@ def run(showPlots): return figureList + if __name__ == "__main__": run(True) diff --git a/examples/BskSim/scenarios/scenario_BasicOrbit.py b/examples/BskSim/scenarios/scenario_BasicOrbit.py index b9b30278e6..f1d64eaf65 100644 --- a/examples/BskSim/scenarios/scenario_BasicOrbit.py +++ b/examples/BskSim/scenarios/scenario_BasicOrbit.py @@ -162,9 +162,9 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../') -sys.path.append(path + '/../models') -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../") +sys.path.append(path + "/../models") +sys.path.append(path + "/../plotting") from BSK_masters import BSKSim, BSKScenario import BSK_Dynamics import BSK_Fsw @@ -177,7 +177,7 @@ class scenario_BasicOrbit(BSKSim, BSKScenario): def __init__(self): super(scenario_BasicOrbit, self).__init__() - self.name = 'scenario_BasicOrbit' + self.name = "scenario_BasicOrbit" # declare empty class variables self.sNavAttRec = None @@ -190,10 +190,13 @@ def __init__(self): self.log_outputs() # if this scenario is to interface with the BSK Viz, uncomment the following line - vizSupport.enableUnityVisualization(self, self.DynModels.taskName, self.DynModels.scObject - # , saveFile=__file__ - , rwEffectorList=self.DynModels.rwStateEffector - ) + vizSupport.enableUnityVisualization( + self, + self.DynModels.taskName, + self.DynModels.scObject, + # , saveFile=__file__ + rwEffectorList=self.DynModels.rwStateEffector, + ) def configure_initial_conditions(self): DynModels = self.get_DynModel() @@ -206,13 +209,17 @@ def configure_initial_conditions(self): oe.Omega = 48.2 * macros.D2R oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R - mu = DynModels.gravFactory.gravBodies['earth'].mu + mu = DynModels.gravFactory.gravBodies["earth"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) DynModels.scObject.hub.r_CN_NInit = rN # m - r_CN_N DynModels.scObject.hub.v_CN_NInit = vN # m/s - v_CN_N DynModels.scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels.scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B + DynModels.scObject.hub.omega_BN_BInit = [ + [0.001], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B def log_outputs(self): # Dynamics process outputs @@ -246,15 +253,14 @@ def pull_outputs(self, showPlots): def runScenario(scenario): - # Initialize simulation scenario.InitializeSimulation() # Configure FSW mode - scenario.modeRequest = 'standby' + scenario.modeRequest = "standby" # Configure run time and execute simulation - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) scenario.ConfigureStopTime(simulationTime) scenario.ExecuteSimulation() @@ -276,5 +282,6 @@ def run(showPlots): return figureList + if __name__ == "__main__": run(True) diff --git a/examples/BskSim/scenarios/scenario_BasicOrbitFormation.py b/examples/BskSim/scenarios/scenario_BasicOrbitFormation.py index 306b69d737..70296fd218 100644 --- a/examples/BskSim/scenarios/scenario_BasicOrbitFormation.py +++ b/examples/BskSim/scenarios/scenario_BasicOrbitFormation.py @@ -132,27 +132,27 @@ class inherits from the BSKSim class, # Get current file path import sys, os, inspect + filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_masters import BSKSim, BSKScenario import BSK_FormationDynamics, BSK_FormationFsw # Import plotting files for your scenario -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../plotting") import BSK_Plotting as BSK_plt -sys.path.append(path + '/../../scenarios') - +sys.path.append(path + "/../../scenarios") # Create your own scenario child class class scenario_BasicOrbitFormation(BSKSim, BSKScenario): def __init__(self): super(scenario_BasicOrbitFormation, self).__init__() - self.name = 'scenario_BasicOrbitFormation' + self.name = "scenario_BasicOrbitFormation" # declare empty class variables self.sNavTransRec = None @@ -170,14 +170,19 @@ def __init__(self): # if this scenario is to interface with the BSK Viz, uncomment the following line if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(self, self.DynModels.taskName - , [self.get_DynModel().scObject, self.get_DynModel().scObject2] - , rwEffectorList=[self.DynModels.rwStateEffector, self.DynModels.rwStateEffector2] - # , saveFile=__file__ - ) + viz = vizSupport.enableUnityVisualization( + self, + self.DynModels.taskName, + [self.get_DynModel().scObject, self.get_DynModel().scObject2], + rwEffectorList=[ + self.DynModels.rwStateEffector, + self.DynModels.rwStateEffector2, + ], + # , saveFile=__file__ + ) def configure_initial_conditions(self): - self.mu = self.get_DynModel().gravFactory.gravBodies['earth'].mu + self.mu = self.get_DynModel().gravFactory.gravBodies["earth"].mu # Configure Dynamics initial conditions self.oe = orbitalMotion.ClassicElements() @@ -191,8 +196,16 @@ def configure_initial_conditions(self): orbitalMotion.rv2elem(self.mu, rN, vN) self.get_DynModel().scObject.hub.r_CN_NInit = rN # m - r_CN_N self.get_DynModel().scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - self.get_DynModel().scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - self.get_DynModel().scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B + self.get_DynModel().scObject.hub.sigma_BNInit = [ + [0.1], + [0.2], + [-0.3], + ] # sigma_BN_B + self.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.001], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B # Configure Dynamics initial conditions self.oe2 = orbitalMotion.ClassicElements() @@ -206,8 +219,16 @@ def configure_initial_conditions(self): orbitalMotion.rv2elem(self.mu, rN2, vN2) self.get_DynModel().scObject2.hub.r_CN_NInit = rN2 # m - r_CN_N self.get_DynModel().scObject2.hub.v_CN_NInit = vN2 # m/s - v_CN_N - self.get_DynModel().scObject2.hub.sigma_BNInit = [[-0.3], [0.0], [0.5]] # sigma_BN_B - self.get_DynModel().scObject2.hub.omega_BN_BInit = [[0.003], [-0.02], [0.01]] # rad/s - omega_BN_B + self.get_DynModel().scObject2.hub.sigma_BNInit = [ + [-0.3], + [0.0], + [0.5], + ] # sigma_BN_B + self.get_DynModel().scObject2.hub.omega_BN_BInit = [ + [0.003], + [-0.02], + [0.01], + ] # rad/s - omega_BN_B def log_outputs(self): samplingTime = self.get_DynModel().processTasksTimeStep @@ -215,7 +236,9 @@ def log_outputs(self): FswModel = self.get_FswModel() self.sNavTransRec = DynModels.simpleNavObject.transOutMsg.recorder(samplingTime) - self.sNavTrans2Rec = DynModels.simpleNavObject2.transOutMsg.recorder(samplingTime) + self.sNavTrans2Rec = DynModels.simpleNavObject2.transOutMsg.recorder( + samplingTime + ) self.attErrRec = FswModel.attGuidMsg.recorder(samplingTime) self.attErr2Rec = FswModel.attGuid2Msg.recorder(samplingTime) self.scStateRec = DynModels.scObject.scStateOutMsg.recorder(samplingTime) @@ -258,20 +281,26 @@ def pull_outputs(self, showPlots): BSK_plt.show_all_plots() else: fileName = os.path.basename(os.path.splitext(__file__)[0]) - figureNames = ["attitude_error_chief", "rate_error_chief", "attitude_error_deputy", - "rate_error_deputy", "orbits"] + figureNames = [ + "attitude_error_chief", + "rate_error_chief", + "attitude_error_deputy", + "rate_error_deputy", + "orbits", + ] figureList = BSK_plt.save_all_plots(fileName, figureNames) return figureList + def runScenario(scenario): scenario.InitializeSimulation() # Configure FSW mode - scenario.modeRequest = 'inertial3D' + scenario.modeRequest = "inertial3D" # Configure run time and execute simulation - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) scenario.ConfigureStopTime(simulationTime) scenario.ExecuteSimulation() diff --git a/examples/BskSim/scenarios/scenario_FeedbackRW.py b/examples/BskSim/scenarios/scenario_FeedbackRW.py index 1760b1dc12..f13c7555f1 100644 --- a/examples/BskSim/scenarios/scenario_FeedbackRW.py +++ b/examples/BskSim/scenarios/scenario_FeedbackRW.py @@ -132,7 +132,6 @@ """ - # Get current file path import inspect import os @@ -147,9 +146,9 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../') -sys.path.append(path + '/../models') -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../") +sys.path.append(path + "/../models") +sys.path.append(path + "/../plotting") import BSK_Dynamics import BSK_Fsw import BSK_Plotting as BSK_plt @@ -160,7 +159,7 @@ class scenario_AttitudeFeedbackRW(BSKSim, BSKScenario): def __init__(self): super(scenario_AttitudeFeedbackRW, self).__init__() - self.name = 'scenario_AttitudeFeedbackRW' + self.name = "scenario_AttitudeFeedbackRW" # declare additional class variables self.rwSpeedRec = None @@ -176,10 +175,13 @@ def __init__(self): # if this scenario is to interface with the BSK Viz, uncomment the following line DynModels = self.get_DynModel() - vizSupport.enableUnityVisualization(self, DynModels.taskName, DynModels.scObject - # , saveFile=__file__ - , rwEffectorList=DynModels.rwStateEffector - ) + vizSupport.enableUnityVisualization( + self, + DynModels.taskName, + DynModels.scObject, + # , saveFile=__file__ + rwEffectorList=DynModels.rwStateEffector, + ) def configure_initial_conditions(self): # Configure Dynamics initial conditions @@ -192,13 +194,17 @@ def configure_initial_conditions(self): oe.f = 85.3 * macros.D2R DynModels = self.get_DynModel() - mu = DynModels.gravFactory.gravBodies['earth'].mu + mu = DynModels.gravFactory.gravBodies["earth"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) DynModels.scObject.hub.r_CN_NInit = rN # m - r_CN_N DynModels.scObject.hub.v_CN_NInit = vN # m/s - v_CN_N DynModels.scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels.scObject.hub.omega_BN_BInit = [[0.1], [-0.01], [0.03]] # rad/s - omega_BN_B + DynModels.scObject.hub.omega_BN_BInit = [ + [0.1], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B def log_outputs(self): FswModel = self.get_FswModel() @@ -214,7 +220,9 @@ def log_outputs(self): self.rwLogs = [] for item in range(4): - self.rwLogs.append(DynModel.rwStateEffector.rwOutMsgs[item].recorder(samplingTime)) + self.rwLogs.append( + DynModel.rwStateEffector.rwOutMsgs[item].recorder(samplingTime) + ) self.AddModelToTask(DynModel.taskName, self.rwLogs[item]) return @@ -254,23 +262,23 @@ def runScenario(scenario): scenario.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.min2nano(10.) - scenario.modeRequest = 'inertial3D' + simulationTime = macros.min2nano(10.0) + scenario.modeRequest = "inertial3D" scenario.ConfigureStopTime(simulationTime) scenario.ExecuteSimulation() - simulationTime = macros.min2nano(30.) - scenario.modeRequest = 'directInertial3D' + simulationTime = macros.min2nano(30.0) + scenario.modeRequest = "directInertial3D" scenario.ConfigureStopTime(simulationTime) scenario.ExecuteSimulation() def run(showPlots): """ - The scenarios can be run with the followings setups parameters: + The scenarios can be run with the followings setups parameters: - Args: - showPlots (bool): Determines if the script should display plots + Args: + showPlots (bool): Determines if the script should display plots """ TheScenario = scenario_AttitudeFeedbackRW() diff --git a/examples/BskSim/scenarios/scenario_LambertGuidance.py b/examples/BskSim/scenarios/scenario_LambertGuidance.py index 2aef14393c..1af3678b87 100644 --- a/examples/BskSim/scenarios/scenario_LambertGuidance.py +++ b/examples/BskSim/scenarios/scenario_LambertGuidance.py @@ -88,6 +88,7 @@ import sys import numpy as np + # Import utilities from Basilisk.utilities import orbitalMotion, macros, vizSupport, unitTestSupport @@ -95,9 +96,9 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../') -sys.path.append(path + '/../models') -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../") +sys.path.append(path + "/../models") +sys.path.append(path + "/../plotting") from BSK_masters import BSKSim, BSKScenario import BSK_Dynamics import BSK_Fsw @@ -109,8 +110,8 @@ # Create your own scenario child class class scenario_LambertGuidance(BSKSim, BSKScenario): def __init__(self): - super(scenario_LambertGuidance, self).__init__(fswRate=60., dynRate=10.) - self.name = 'scenario_LambertGuidance' + super(scenario_LambertGuidance, self).__init__(fswRate=60.0, dynRate=10.0) + self.name = "scenario_LambertGuidance" # declare empty class variables self.sNavAttRec = None @@ -123,24 +124,54 @@ def __init__(self): DynModels = self.get_DynModel() FswModels = self.get_FswModel() rEarth = 6378 * 1e3 - self.r_TN_N = np.array([(42164 * 1e3), 0., 0.]) # targeted position - pos_sigma_sc = 1. + self.r_TN_N = np.array([(42164 * 1e3), 0.0, 0.0]) # targeted position + pos_sigma_sc = 1.0 vel_sigma_sc = 0.1 - self.tm1 = 2460. - self.tm2 = 4980. - vRelativeDesired_S = np.array([0., 0., 0.]) - p_matrix_sc = np.diag([pos_sigma_sc, pos_sigma_sc, pos_sigma_sc, - vel_sigma_sc, vel_sigma_sc, vel_sigma_sc, - 0., 0., 0., - 0., 0., 0., - 0., 0., 0., - 0., 0., 0.]) - walk_bounds_sc = [[10.], [10.], [10.], - [1], [1], [1], - [0.], [0.], [0.], - [0.], [0.], [0.], - [0.], [0.], [0.], - [0.], [0.], [0.]] + self.tm1 = 2460.0 + self.tm2 = 4980.0 + vRelativeDesired_S = np.array([0.0, 0.0, 0.0]) + p_matrix_sc = np.diag( + [ + pos_sigma_sc, + pos_sigma_sc, + pos_sigma_sc, + vel_sigma_sc, + vel_sigma_sc, + vel_sigma_sc, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + ) + walk_bounds_sc = [ + [10.0], + [10.0], + [10.0], + [1], + [1], + [1], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + ] DynModels.simpleNavObject.PMatrix = p_matrix_sc DynModels.simpleNavObject.walkBounds = walk_bounds_sc @@ -148,56 +179,81 @@ def __init__(self): FswModels.lambertPlannerObject.setR_TN_N(self.r_TN_N) FswModels.lambertPlannerObject.setFinalTime(self.tm2) FswModels.lambertPlannerObject.setManeuverTime(self.tm1) - FswModels.lambertPlannerObject.setMu(DynModels.gravFactory.gravBodies['earth'].mu) + FswModels.lambertPlannerObject.setMu( + DynModels.gravFactory.gravBodies["earth"].mu + ) FswModels.lambertPlannerObject.setNumRevolutions(0) FswModels.lambertValidatorObject.setFinalTime(self.tm2) FswModels.lambertValidatorObject.setManeuverTime(self.tm1) - FswModels.lambertValidatorObject.setMaxDistanceTarget(500.) + FswModels.lambertValidatorObject.setMaxDistanceTarget(500.0) FswModels.lambertValidatorObject.setMinOrbitRadius(rEarth) - FswModels.lambertValidatorObject.setUncertaintyStates(np.diag([pos_sigma_sc, pos_sigma_sc, pos_sigma_sc, - vel_sigma_sc, vel_sigma_sc, vel_sigma_sc])) + FswModels.lambertValidatorObject.setUncertaintyStates( + np.diag( + [ + pos_sigma_sc, + pos_sigma_sc, + pos_sigma_sc, + vel_sigma_sc, + vel_sigma_sc, + vel_sigma_sc, + ] + ) + ) FswModels.lambertValidatorObject.setUncertaintyDV(0.1) - FswModels.lambertValidatorObject.setDvConvergenceTolerance(1.) + FswModels.lambertValidatorObject.setDvConvergenceTolerance(1.0) - FswModels.lambertSurfaceRelativeVelocityObject.setVRelativeDesired_S(vRelativeDesired_S) + FswModels.lambertSurfaceRelativeVelocityObject.setVRelativeDesired_S( + vRelativeDesired_S + ) FswModels.lambertSurfaceRelativeVelocityObject.setTime(self.tm2) self.configure_initial_conditions() self.log_outputs() # if this scenario is to interface with the BSK Viz, uncomment the following line - vizSupport.enableUnityVisualization(self, self.DynModels.taskName, self.DynModels.scObject - # , saveFile=__file__ - , rwEffectorList=self.DynModels.rwStateEffector - ) + vizSupport.enableUnityVisualization( + self, + self.DynModels.taskName, + self.DynModels.scObject, + # , saveFile=__file__ + rwEffectorList=self.DynModels.rwStateEffector, + ) def configure_initial_conditions(self): DynModels = self.get_DynModel() # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 40000. * 1e3 # meters + oe.a = 40000.0 * 1e3 # meters oe.e = 0.001 - oe.i = 1. * macros.D2R - oe.Omega = 1. * macros.D2R - oe.omega = 1. * macros.D2R - oe.f = -30. * macros.D2R - mu = DynModels.gravFactory.gravBodies['earth'].mu + oe.i = 1.0 * macros.D2R + oe.Omega = 1.0 * macros.D2R + oe.omega = 1.0 * macros.D2R + oe.f = -30.0 * macros.D2R + mu = DynModels.gravFactory.gravBodies["earth"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) DynModels.scObject.hub.r_CN_NInit = rN # m - r_CN_N DynModels.scObject.hub.v_CN_NInit = vN # m/s - v_CN_N DynModels.scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels.scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B + DynModels.scObject.hub.omega_BN_BInit = [ + [0.001], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B def log_outputs(self): DynModels = self.get_DynModel() FswModels = self.get_FswModel() self.sc_truth_recorder = DynModels.scObject.scStateOutMsg.recorder() self.sc_meas_recorder = DynModels.simpleNavObject.transOutMsg.recorder() - self.dv1Cmd_recorder = FswModels.lambertValidatorObject.dvBurnCmdOutMsg.recorder() - self.dv2Cmd_recorder = FswModels.lambertSecondDvObject.dvBurnCmdOutMsg.recorder() + self.dv1Cmd_recorder = ( + FswModels.lambertValidatorObject.dvBurnCmdOutMsg.recorder() + ) + self.dv2Cmd_recorder = ( + FswModels.lambertSecondDvObject.dvBurnCmdOutMsg.recorder() + ) self.ephem_recorder = DynModels.EarthEphemObject.ephemOutMsgs[0].recorder() self.AddModelToTask(DynModels.taskName, self.sc_truth_recorder) self.AddModelToTask(DynModels.taskName, self.sc_meas_recorder) @@ -218,9 +274,17 @@ def pull_outputs(self, showPlots): # Plot results BSK_plt.clear_all_plots() BSK_plt.plot_orbit(r_BN_N_truth) - BSK_plt.plot_position(time, np.array(r_BN_N_truth), np.array(r_BN_N_meas), self.tm2 / 60., self.r_TN_N) + BSK_plt.plot_position( + time, + np.array(r_BN_N_truth), + np.array(r_BN_N_meas), + self.tm2 / 60.0, + self.r_TN_N, + ) BSK_plt.plot_velocity(time, np.array(v_BN_N_truth), np.array(v_BN_N_meas)) - BSK_plt.plot_surface_rel_velocity(time, r_BN_N_truth, v_BN_N_truth, sigma_PN, omega_PN_N) + BSK_plt.plot_surface_rel_velocity( + time, r_BN_N_truth, v_BN_N_truth, sigma_PN, omega_PN_N + ) figureList = {} if showPlots: @@ -234,13 +298,12 @@ def pull_outputs(self, showPlots): def runScenario(scenario): - # Initialize simulation scenario.InitializeSimulation() velRef = scenario.DynModels.scObject.dynManager.getStateObject("hubVelocity") # Configure FSW mode for first Lambert maneuver - scenario.modeRequest = 'lambertFirstDV' + scenario.modeRequest = "lambertFirstDV" # Configure run time and execute simulation simulationTime = macros.sec2nano(scenario.tm1) @@ -256,7 +319,7 @@ def runScenario(scenario): # After reading the Delta-V command, the state managers velocity is updated through velRef.setState(unitTestSupport.np2EigenVectorXd(vm_N + dv_N)) # Configure FSW mode for second Lambert maneuver - scenario.modeRequest = 'lambertSecondDV' + scenario.modeRequest = "lambertSecondDV" # To start up the simulation again, note that the total simulation time must be provided, # not just the next incremental simulation time. @@ -273,11 +336,11 @@ def runScenario(scenario): # After reading the Delta-V command, the state managers velocity is updated through velRef.setState(unitTestSupport.np2EigenVectorXd(vm_N + dv_N)) # disable flight software after maneuver - scenario.modeRequest = 'standby' + scenario.modeRequest = "standby" # To start up the simulation again, note that the total simulation time must be provided, # not just the next incremental simulation time. - simulationTime = macros.sec2nano(6000.) + simulationTime = macros.sec2nano(6000.0) scenario.ConfigureStopTime(simulationTime) scenario.ExecuteSimulation() diff --git a/examples/BskSim/scenarios/scenario_RelativePointingFormation.py b/examples/BskSim/scenarios/scenario_RelativePointingFormation.py index e35c4eaa4c..56cdf7cac5 100644 --- a/examples/BskSim/scenarios/scenario_RelativePointingFormation.py +++ b/examples/BskSim/scenarios/scenario_RelativePointingFormation.py @@ -124,32 +124,32 @@ class inherits from the BSKSim class, """ - - # Import utilities from Basilisk.utilities import orbitalMotion, macros, vizSupport # Get current file path import sys, os, inspect + filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_masters import BSKSim, BSKScenario import BSK_FormationDynamics, BSK_FormationFsw # Import plotting files for your scenario -sys.path.append(path + '/../plotting') +sys.path.append(path + "/../plotting") import BSK_Plotting as BSK_plt -sys.path.append(path + '/../../scenarios') +sys.path.append(path + "/../../scenarios") + # Create your own scenario child class class scenario_RelativePointingFormation(BSKSim, BSKScenario): def __init__(self): super(scenario_RelativePointingFormation, self).__init__() - self.name = 'scenario_RelativePointingFormation' + self.name = "scenario_RelativePointingFormation" # declare empty class variables self.sNavTransRec = None @@ -171,14 +171,19 @@ def __init__(self): # if this scenario is to interface with the BSK Viz, uncomment the following line if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(self, self.DynModels.taskName - , [self.get_DynModel().scObject, self.get_DynModel().scObject2] - , rwEffectorList=[self.DynModels.rwStateEffector, self.DynModels.rwStateEffector2] - , saveFile=__file__ - ) + viz = vizSupport.enableUnityVisualization( + self, + self.DynModels.taskName, + [self.get_DynModel().scObject, self.get_DynModel().scObject2], + rwEffectorList=[ + self.DynModels.rwStateEffector, + self.DynModels.rwStateEffector2, + ], + saveFile=__file__, + ) def configure_initial_conditions(self): - mu = self.get_DynModel().gravFactory.gravBodies['earth'].mu + mu = self.get_DynModel().gravFactory.gravBodies["earth"].mu # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() @@ -192,8 +197,16 @@ def configure_initial_conditions(self): orbitalMotion.rv2elem(mu, rN, vN) self.get_DynModel().scObject.hub.r_CN_NInit = rN # m - r_CN_N self.get_DynModel().scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - self.get_DynModel().scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - self.get_DynModel().scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B + self.get_DynModel().scObject.hub.sigma_BNInit = [ + [0.1], + [0.2], + [-0.3], + ] # sigma_BN_B + self.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.001], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B # Configure Dynamics initial conditions oe2 = oe @@ -202,8 +215,16 @@ def configure_initial_conditions(self): orbitalMotion.rv2elem(mu, rN2, vN2) self.get_DynModel().scObject2.hub.r_CN_NInit = rN2 # m - r_CN_N self.get_DynModel().scObject2.hub.v_CN_NInit = vN2 # m/s - v_CN_N - self.get_DynModel().scObject2.hub.sigma_BNInit = [[-0.3], [0.0], [0.5]] # sigma_BN_B - self.get_DynModel().scObject2.hub.omega_BN_BInit = [[0.003], [-0.02], [0.01]] # rad/s - omega_BN_B + self.get_DynModel().scObject2.hub.sigma_BNInit = [ + [-0.3], + [0.0], + [0.5], + ] # sigma_BN_B + self.get_DynModel().scObject2.hub.omega_BN_BInit = [ + [0.003], + [-0.02], + [0.01], + ] # rad/s - omega_BN_B def log_outputs(self): samplingTime = self.get_DynModel().processTasksTimeStep @@ -211,14 +232,18 @@ def log_outputs(self): FswModel = self.get_FswModel() self.sNavTransRec = DynModels.simpleNavObject.transOutMsg.recorder(samplingTime) - self.sNavTrans2Rec = DynModels.simpleNavObject2.transOutMsg.recorder(samplingTime) + self.sNavTrans2Rec = DynModels.simpleNavObject2.transOutMsg.recorder( + samplingTime + ) self.sNavAttRec = DynModels.simpleNavObject.attOutMsg.recorder(samplingTime) self.sNavAtt2Rec = DynModels.simpleNavObject2.attOutMsg.recorder(samplingTime) self.attErrRec = FswModel.attGuidMsg.recorder(samplingTime) self.attErr2Rec = FswModel.attGuid2Msg.recorder(samplingTime) self.scStateRec = DynModels.scObject.scStateOutMsg.recorder(samplingTime) self.scState2Rec = DynModels.scObject2.scStateOutMsg.recorder(samplingTime) - self.attRef2Msg = FswModel.spacecraftPointing.attReferenceOutMsg.recorder(samplingTime) + self.attRef2Msg = FswModel.spacecraftPointing.attReferenceOutMsg.recorder( + samplingTime + ) self.cmdTor2Msg = FswModel.cmdTorqueDirectMsg.recorder(samplingTime) self.AddModelToTask(DynModels.taskName, self.sNavTransRec) @@ -266,18 +291,24 @@ def pull_outputs(self, showPlots): BSK_plt.show_all_plots() else: fileName = os.path.basename(os.path.splitext(__file__)[0]) - figureNames = ["attitude_error", "relative_orbit", "sigma_RN", - "sigma_BN_deputy", "sigma_BR_deputy"] + figureNames = [ + "attitude_error", + "relative_orbit", + "sigma_RN", + "sigma_BN_deputy", + "sigma_BR_deputy", + ] figureList = BSK_plt.save_all_plots(fileName, figureNames) return figureList + def runScenario(scenario): # Initialize simulation scenario.InitializeSimulation() # Configure FSW mode - scenario.modeRequest = 'spacecraftPointing' + scenario.modeRequest = "spacecraftPointing" # Configure run time and execute simulation simulationTime = macros.min2nano(10.0) @@ -285,8 +316,8 @@ def runScenario(scenario): scenario.ExecuteSimulation() -def run(showPlots): +def run(showPlots): TheScenario = scenario_RelativePointingFormation() runScenario(TheScenario) @@ -294,5 +325,6 @@ def run(showPlots): return figureList + if __name__ == "__main__": run(True) diff --git a/examples/MonteCarloExamples/scenarioBskSimAttFeedbackMC.py b/examples/MonteCarloExamples/scenarioBskSimAttFeedbackMC.py index b46215ee77..798f6e103c 100644 --- a/examples/MonteCarloExamples/scenarioBskSimAttFeedbackMC.py +++ b/examples/MonteCarloExamples/scenarioBskSimAttFeedbackMC.py @@ -17,7 +17,6 @@ # - r""" Monte Carlo Simulation for Attitude Feedback Scenario ===================================================== @@ -88,21 +87,26 @@ path = os.path.dirname(os.path.abspath(filename)) from Basilisk import __path__ + bskPath = __path__[0] # import general simulation support files import sys from Basilisk.utilities.MonteCarlo.Controller import Controller from Basilisk.utilities.MonteCarlo.RetentionPolicy import RetentionPolicy -from Basilisk.utilities.MonteCarlo.Dispersions import (UniformEulerAngleMRPDispersion, UniformDispersion, - NormalVectorCartDispersion) +from Basilisk.utilities.MonteCarlo.Dispersions import ( + UniformEulerAngleMRPDispersion, + UniformDispersion, + NormalVectorCartDispersion, +) -sys.path.append(path+"/../BskSim/scenarios/") +sys.path.append(path + "/../BskSim/scenarios/") import scenario_AttFeedback sNavTransName = "sNavTransMsg" attGuidName = "attGuidMsg" + def run(show_plots): """This function is called by the py.test environment.""" @@ -110,36 +114,67 @@ def run(show_plots): # This module is used to execute monte carlo simulations, and access # retained data from previously executed MonteCarlo runs. monteCarlo = Controller() - monteCarlo.setSimulationFunction(scenario_AttFeedback.scenario_AttFeedback) # Required: function that configures the base scenario - monteCarlo.setExecutionFunction(scenario_AttFeedback.runScenario) # Required: function that runs the scenario + monteCarlo.setSimulationFunction( + scenario_AttFeedback.scenario_AttFeedback + ) # Required: function that configures the base scenario + monteCarlo.setExecutionFunction( + scenario_AttFeedback.runScenario + ) # Required: function that runs the scenario monteCarlo.setExecutionCount(4) # Required: Number of MCs to run - monteCarlo.setArchiveDir(path + "/scenarioBskSimAttFeedbackMC") # Optional: If/where to save retained data. - monteCarlo.setShouldDisperseSeeds(True) # Optional: Randomize the seed for each module + monteCarlo.setArchiveDir( + path + "/scenarioBskSimAttFeedbackMC" + ) # Optional: If/where to save retained data. + monteCarlo.setShouldDisperseSeeds( + True + ) # Optional: Randomize the seed for each module # monteCarlo.setThreadCount(2) # Optional: Number of processes to spawn MCs on, automatically sizes for personal computer. # monteCarlo.setVerbose(True) # Optional: Produce supplemental text output in console describing status - monteCarlo.setVarCast('float') # Optional: Downcast the retained numbers to float32 to save on storage space - monteCarlo.setDispMagnitudeFile(True) # Optional: Produce a .txt file that shows dispersion in std dev units + monteCarlo.setVarCast( + "float" + ) # Optional: Downcast the retained numbers to float32 to save on storage space + monteCarlo.setDispMagnitudeFile( + True + ) # Optional: Produce a .txt file that shows dispersion in std dev units # Statistical dispersions can be applied to initial parameters using the MonteCarlo module - dispMRPInit = 'TaskList[0].TaskModels[0].hub.sigma_BNInit' - dispOmegaInit = 'TaskList[0].TaskModels[0].hub.omega_BN_BInit' - dispMass = 'TaskList[0].TaskModels[0].hub.mHub' - dispCoMOff = 'TaskList[0].TaskModels[0].hub.r_BcB_B' - dispInertia = 'hubref.IHubPntBc_B' + dispMRPInit = "TaskList[0].TaskModels[0].hub.sigma_BNInit" + dispOmegaInit = "TaskList[0].TaskModels[0].hub.omega_BN_BInit" + dispMass = "TaskList[0].TaskModels[0].hub.mHub" + dispCoMOff = "TaskList[0].TaskModels[0].hub.r_BcB_B" + dispInertia = "hubref.IHubPntBc_B" dispList = [dispMRPInit, dispOmegaInit, dispMass, dispCoMOff, dispInertia] # Add dispersions with their dispersion type - monteCarlo.addDispersion(UniformEulerAngleMRPDispersion('TaskList[0].TaskModels[0].hub.sigma_BNInit')) - monteCarlo.addDispersion(NormalVectorCartDispersion('TaskList[0].TaskModels[0].hub.omega_BN_BInit', 0.0, 0.75 / 3.0 * np.pi / 180)) - monteCarlo.addDispersion(UniformDispersion('TaskList[0].TaskModels[0].hub.mHub', ([750.0 - 0.05*750, 750.0 + 0.05*750]))) - monteCarlo.addDispersion(NormalVectorCartDispersion('TaskList[0].TaskModels[0].hub.r_BcB_B', [0.0, 0.0, 1.0], [0.05 / 3.0, 0.05 / 3.0, 0.1 / 3.0])) + monteCarlo.addDispersion( + UniformEulerAngleMRPDispersion("TaskList[0].TaskModels[0].hub.sigma_BNInit") + ) + monteCarlo.addDispersion( + NormalVectorCartDispersion( + "TaskList[0].TaskModels[0].hub.omega_BN_BInit", + 0.0, + 0.75 / 3.0 * np.pi / 180, + ) + ) + monteCarlo.addDispersion( + UniformDispersion( + "TaskList[0].TaskModels[0].hub.mHub", + ([750.0 - 0.05 * 750, 750.0 + 0.05 * 750]), + ) + ) + monteCarlo.addDispersion( + NormalVectorCartDispersion( + "TaskList[0].TaskModels[0].hub.r_BcB_B", + [0.0, 0.0, 1.0], + [0.05 / 3.0, 0.05 / 3.0, 0.1 / 3.0], + ) + ) # A `RetentionPolicy` is used to define what data from the simulation should be retained. A `RetentionPolicy` # is a list of messages and variables to log from each simulation run. It also can have a callback, # used for plotting/processing the retained data. retentionPolicy = RetentionPolicy() - samplingTime = int(2E9) + samplingTime = int(2e9) retentionPolicy.addMessageLog(sNavTransName, ["r_BN_N"]) retentionPolicy.addMessageLog(attGuidName, ["sigma_BR", "omega_BR_B"]) retentionPolicy.setDataCallback(displayPlots) @@ -153,14 +188,12 @@ def run(show_plots): return + def displayPlots(data, retentionPolicy): states = data["messages"][attGuidName + ".sigma_BR"] time = states[:, 0] plt.figure(1) - plt.plot(time, states[:,1], - time, states[:,2], - time, states[:,3]) - + plt.plot(time, states[:, 1], time, states[:, 2], time, states[:, 3]) if __name__ == "__main__": diff --git a/examples/MonteCarloExamples/scenarioRerunMonteCarlo.py b/examples/MonteCarloExamples/scenarioRerunMonteCarlo.py index 8ee014bf94..e0204901d6 100644 --- a/examples/MonteCarloExamples/scenarioRerunMonteCarlo.py +++ b/examples/MonteCarloExamples/scenarioRerunMonteCarlo.py @@ -25,6 +25,7 @@ """ + import inspect import os import sys @@ -38,9 +39,11 @@ path = os.path.dirname(os.path.abspath(filename)) from Basilisk import __path__ + bskPath = __path__[0] -sys.path.append(path+"/../BskSim/scenarios/") +sys.path.append(path + "/../BskSim/scenarios/") + def run(time=None): """ @@ -57,7 +60,7 @@ def run(time=None): # Step 1-3: Change to the relevant scenario scenarioName = "scenario_AttFeedback" # This is the actual scenario module name - mcName = "scenarioBskSimAttFeedbackMC" # This is the MC script name + mcName = "scenarioBskSimAttFeedbackMC" # This is the MC script name monteCarlo = Controller() monteCarlo.numProcess = 3 @@ -88,7 +91,7 @@ def run(time=None): # Step 4: Add any additional retention policies desired retentionPolicy = RetentionPolicy() - retentionPolicy.logRate = int(2E9) + retentionPolicy.logRate = int(2e9) retentionPolicy.addMessageLog("attGuidMsg", ["sigma_BR"]) monteCarlo.addRetentionPolicy(retentionPolicy) diff --git a/examples/MonteCarloExamples/scenarioVisualizeMonteCarlo.py b/examples/MonteCarloExamples/scenarioVisualizeMonteCarlo.py index c778f96762..8a3afa16b7 100644 --- a/examples/MonteCarloExamples/scenarioVisualizeMonteCarlo.py +++ b/examples/MonteCarloExamples/scenarioVisualizeMonteCarlo.py @@ -130,14 +130,17 @@ from bokeh.application.handlers.function import FunctionHandler from tornado.ioloop import IOLoop + def create_document(doc): - data_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), "scenarioBskSimAttFeedbackMC") + data_dir = os.path.join( + os.path.dirname(os.path.abspath(__file__)), "scenarioBskSimAttFeedbackMC" + ) doc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), "docs") plot_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), "saved_plots") try: plotter = MonteCarloPlotter(data_dir, save_plots=True, doc_dir=doc_dir) - plotter.load_data(['attGuidMsg.sigma_BR', 'attGuidMsg.omega_BR_B']) + plotter.load_data(["attGuidMsg.sigma_BR", "attGuidMsg.omega_BR_B"]) layout = plotter.show_plots() doc.add_root(layout) @@ -148,23 +151,28 @@ def create_document(doc): logger.error(error_message) doc.add_root(Div(text=error_message)) + def open_browser(): webbrowser.open("http://localhost:5006") + def run(useBokeh_server=False): if not bokeh_available: logger.error("Bokeh is not available. This script requires Bokeh to run.") return if useBokeh_server: + def bk_worker(): app = Application(FunctionHandler(create_document)) - server = Server({'/': app}, io_loop=IOLoop(), allow_websocket_origin=["*"]) + server = Server({"/": app}, io_loop=IOLoop(), allow_websocket_origin=["*"]) server.start() Timer(1, open_browser).start() # Open browser after 1 second server.io_loop.start() - logger.info("Starting Bokeh server. A browser window should open automatically.") + logger.info( + "Starting Bokeh server. A browser window should open automatically." + ) bk_worker() else: # Create a document for the static case @@ -174,7 +182,9 @@ def bk_worker(): # Set up the output file output_file("monte_carlo_plots.html", title="BSK Monte Carlo Visualization") + if __name__ == "__main__": import sys + useBokeh_server = "--bokeh-server" in sys.argv run(useBokeh_server) diff --git a/examples/MultiSatBskSim/BSK_MultiSatMasters.py b/examples/MultiSatBskSim/BSK_MultiSatMasters.py index 82a94cc682..c34359c76b 100644 --- a/examples/MultiSatBskSim/BSK_MultiSatMasters.py +++ b/examples/MultiSatBskSim/BSK_MultiSatMasters.py @@ -23,6 +23,7 @@ from Basilisk import __path__ from Basilisk.fswAlgorithms import formationBarycenter + # Import architectural modules from Basilisk.utilities import SimulationBaseClass, macros as mc @@ -31,7 +32,7 @@ bskPath = __path__[0] # Import Dynamics and FSW models -sys.path.append(path + '/models') +sys.path.append(path + "/models") class BSKSim(SimulationBaseClass.SimBaseClass): @@ -48,7 +49,15 @@ class BSKSim(SimulationBaseClass.SimBaseClass): """ - def __init__(self, numberSpacecraft, relativeNavigation=False, fswRate=0.1, dynRate=0.1, envRate=0.1, relNavRate=0.1): + def __init__( + self, + numberSpacecraft, + relativeNavigation=False, + fswRate=0.1, + dynRate=0.1, + envRate=0.1, + relNavRate=0.1, + ): self.dynRate = dynRate self.fswRate = fswRate self.envRate = envRate @@ -81,7 +90,9 @@ def __init__(self, numberSpacecraft, relativeNavigation=False, fswRate=0.1, dynR self.add_relativeNavigation() def get_EnvModel(self): - assert (self.environment_added is True), "It is mandatory to use an environment model as an argument" + assert self.environment_added is True, ( + "It is mandatory to use an environment model as an argument" + ) return self.EnvModel def set_EnvModel(self, envModel): @@ -93,7 +104,9 @@ def set_EnvModel(self, envModel): self.EnvModel = envModel.BSKEnvironmentModel(self, self.envRate) def get_DynModel(self): - assert (self.dynamics_added is True), "It is mandatory to use a dynamics model as an argument" + assert self.dynamics_added is True, ( + "It is mandatory to use a dynamics model as an argument" + ) return self.DynModels def set_DynModel(self, dynModel): @@ -101,12 +114,20 @@ def set_DynModel(self, dynModel): # Add the dynamics classes for spacecraftIndex in range(self.numberSpacecraft): - self.DynamicsProcessName.append("DynamicsProcess" + str(spacecraftIndex)) # Create simulation process name - self.dynProc.append(self.CreateNewProcess(self.DynamicsProcessName[spacecraftIndex], 200)) # Create process - self.DynModels.append(dynModel[spacecraftIndex].BSKDynamicModels(self, self.dynRate, spacecraftIndex)) + self.DynamicsProcessName.append( + "DynamicsProcess" + str(spacecraftIndex) + ) # Create simulation process name + self.dynProc.append( + self.CreateNewProcess(self.DynamicsProcessName[spacecraftIndex], 200) + ) # Create process + self.DynModels.append( + dynModel[spacecraftIndex].BSKDynamicModels( + self, self.dynRate, spacecraftIndex + ) + ) def get_FswModel(self): - assert (self.fsw_added is True), "A flight software model has not been added yet" + assert self.fsw_added is True, "A flight software model has not been added yet" return self.FSWModels def set_FswModel(self, fswModel): @@ -114,9 +135,17 @@ def set_FswModel(self, fswModel): # Add the FSW classes for spacecraftIndex in range(self.numberSpacecraft): - self.FSWProcessName.append("FSWProcess" + str(spacecraftIndex)) # Create simulation process name - self.fswProc.append(self.CreateNewProcess(self.FSWProcessName[spacecraftIndex], 100)) # Create process - self.FSWModels.append(fswModel[spacecraftIndex].BSKFswModels(self, self.fswRate, spacecraftIndex)) + self.FSWProcessName.append( + "FSWProcess" + str(spacecraftIndex) + ) # Create simulation process name + self.fswProc.append( + self.CreateNewProcess(self.FSWProcessName[spacecraftIndex], 100) + ) # Create process + self.FSWModels.append( + fswModel[spacecraftIndex].BSKFswModels( + self, self.fswRate, spacecraftIndex + ) + ) def add_relativeNavigation(self): processName = "RelativeNavigation" @@ -125,12 +154,18 @@ def add_relativeNavigation(self): # Create an independt process for barycenter calculation self.relNavProc = self.CreateNewProcess(processName, 150) - self.relNavProc.addTask(self.CreateNewTask(self.relativeNavigationTaskName, processTasksTimeStep), 20) + self.relNavProc.addTask( + self.CreateNewTask(self.relativeNavigationTaskName, processTasksTimeStep), + 20, + ) # Add the formationBarycenter module self.relativeNavigationModule = formationBarycenter.FormationBarycenter() self.relativeNavigationModule.ModelTag = "RelativeNavigation" - self.AddModelToTask(self.relativeNavigationTaskName, self.relativeNavigationModule, 0) + self.AddModelToTask( + self.relativeNavigationTaskName, self.relativeNavigationModule, 0 + ) + class BSKScenario(object): def __init__(self): @@ -138,18 +173,18 @@ def __init__(self): def configure_initial_conditions(self): """ - Developer must override this method in their BSK_Scenario derived subclass. + Developer must override this method in their BSK_Scenario derived subclass. """ pass def log_outputs(self): """ - Developer must override this method in their BSK_Scenario derived subclass. + Developer must override this method in their BSK_Scenario derived subclass. """ pass def pull_outputs(self, showPlots, spacecraftIndex): """ - Developer must override this method in their BSK_Scenario derived subclass. + Developer must override this method in their BSK_Scenario derived subclass. """ pass diff --git a/examples/MultiSatBskSim/modelsMultiSat/BSK_EnvironmentEarth.py b/examples/MultiSatBskSim/modelsMultiSat/BSK_EnvironmentEarth.py index d74a8499f6..9fb8954ee9 100644 --- a/examples/MultiSatBskSim/modelsMultiSat/BSK_EnvironmentEarth.py +++ b/examples/MultiSatBskSim/modelsMultiSat/BSK_EnvironmentEarth.py @@ -27,6 +27,7 @@ class BSKEnvironmentModel: """Defines the Earth Environment.""" + def __init__(self, SimBase, envRate): # Define empty class variables self.mu = None @@ -40,7 +41,9 @@ def __init__(self, SimBase, envRate): processTasksTimeStep = mc.sec2nano(envRate) # Create task - SimBase.envProc.addTask(SimBase.CreateNewTask(self.envTaskName, processTasksTimeStep)) + SimBase.envProc.addTask( + SimBase.CreateNewTask(self.envTaskName, processTasksTimeStep) + ) # Instantiate Env modules as objects self.gravFactory = simIncludeGravBody.gravBodyFactory() @@ -65,26 +68,34 @@ def SetGravityBodies(self): Specify what gravitational bodies to include in the simulation. """ # Create gravity bodies - gravBodies = self.gravFactory.createBodies(['sun', 'earth', 'moon']) - gravBodies['earth'].isCentralBody = True - self.mu = self.gravFactory.gravBodies['earth'].mu - self.planetRadius = self.gravFactory.gravBodies['earth'].radEquator + gravBodies = self.gravFactory.createBodies(["sun", "earth", "moon"]) + gravBodies["earth"].isCentralBody = True + self.mu = self.gravFactory.gravBodies["earth"].mu + self.planetRadius = self.gravFactory.gravBodies["earth"].radEquator self.sun = 0 self.earth = 1 self.moon = 2 # Override information with SPICE timeInitString = "2021 MAY 04 07:47:48.965 (UTC)" - self.gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/', - timeInitString, - epochInMsg=True) - self.gravFactory.spiceObject.zeroBase = 'Earth' + self.gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", timeInitString, epochInMsg=True + ) + self.gravFactory.spiceObject.zeroBase = "Earth" # Add pyswice instances - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'de430.bsp') # solar system bodies - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'naif0012.tls') # leap second file - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'de-403-masses.tpc') # solar system masses - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'pck00010.tpc') # generic Planetary Constants + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "de430.bsp" + ) # solar system bodies + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "naif0012.tls" + ) # leap second file + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "de-403-masses.tpc" + ) # solar system masses + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "pck00010.tpc" + ) # generic Planetary Constants def SetEpochObject(self): """ @@ -92,20 +103,30 @@ def SetEpochObject(self): """ # self.epochMsg = self.gravFactory.epochMsg - self.ephemObject.ModelTag = 'EphemData' - self.ephemObject.addSpiceInputMsg(self.gravFactory.spiceObject.planetStateOutMsgs[self.sun]) - self.ephemObject.addSpiceInputMsg(self.gravFactory.spiceObject.planetStateOutMsgs[self.earth]) - self.ephemObject.addSpiceInputMsg(self.gravFactory.spiceObject.planetStateOutMsgs[self.moon]) + self.ephemObject.ModelTag = "EphemData" + self.ephemObject.addSpiceInputMsg( + self.gravFactory.spiceObject.planetStateOutMsgs[self.sun] + ) + self.ephemObject.addSpiceInputMsg( + self.gravFactory.spiceObject.planetStateOutMsgs[self.earth] + ) + self.ephemObject.addSpiceInputMsg( + self.gravFactory.spiceObject.planetStateOutMsgs[self.moon] + ) def SetEclipseObject(self): """ Specify what celestial object is causing an eclipse message. """ self.eclipseObject.ModelTag = "eclipseObject" - self.eclipseObject.sunInMsg.subscribeTo(self.gravFactory.spiceObject.planetStateOutMsgs[self.sun]) + self.eclipseObject.sunInMsg.subscribeTo( + self.gravFactory.spiceObject.planetStateOutMsgs[self.sun] + ) # add all celestial objects in spiceObjects except for the sun (0th object) for item in range(1, len(self.gravFactory.spiceObject.planetStateOutMsgs)): - self.eclipseObject.addPlanetToModel(self.gravFactory.spiceObject.planetStateOutMsgs[item]) + self.eclipseObject.addPlanetToModel( + self.gravFactory.spiceObject.planetStateOutMsgs[item] + ) def SetGroundLocations(self): """ @@ -113,8 +134,10 @@ def SetGroundLocations(self): """ self.groundStation.ModelTag = "BoulderGroundStation" self.groundStation.planetRadius = self.planetRadius - self.groundStation.specifyLocation(np.radians(40.009971), np.radians(-105.243895), 1624) - self.groundStation.minimumElevation = np.radians(10.) + self.groundStation.specifyLocation( + np.radians(40.009971), np.radians(-105.243895), 1624 + ) + self.groundStation.minimumElevation = np.radians(10.0) self.groundStation.maximumRange = 1e9 # Global call to initialize every module diff --git a/examples/MultiSatBskSim/modelsMultiSat/BSK_EnvironmentMercury.py b/examples/MultiSatBskSim/modelsMultiSat/BSK_EnvironmentMercury.py index f51d6cd44b..3a9bfb496f 100644 --- a/examples/MultiSatBskSim/modelsMultiSat/BSK_EnvironmentMercury.py +++ b/examples/MultiSatBskSim/modelsMultiSat/BSK_EnvironmentMercury.py @@ -27,19 +27,22 @@ class BSKEnvironmentModel: """Defines the Mercury Environment.""" + def __init__(self, SimBase, envRate): # Define empty class variables self.mu = None self.planetRadius = None self.sun = None - self.mercury= None + self.mercury = None # Define process name, task name and task time-step self.envTaskName = "EnvironmentTask" processTasksTimeStep = mc.sec2nano(envRate) # Create task - SimBase.envProc.addTask(SimBase.CreateNewTask(self.envTaskName, processTasksTimeStep)) + SimBase.envProc.addTask( + SimBase.CreateNewTask(self.envTaskName, processTasksTimeStep) + ) # Instantiate Env modules as objects self.gravFactory = simIncludeGravBody.gravBodyFactory() @@ -64,26 +67,33 @@ def SetGravityBodies(self): Specify what gravitational bodies to include in the simulation. """ # Create gravity bodies - gravBodies = self.gravFactory.createBodies(['sun', 'mercury']) - gravBodies['mercury'].isCentralBody = True - self.mu = self.gravFactory.gravBodies['mercury'].mu - self.planetRadius = self.gravFactory.gravBodies['mercury'].radEquator + gravBodies = self.gravFactory.createBodies(["sun", "mercury"]) + gravBodies["mercury"].isCentralBody = True + self.mu = self.gravFactory.gravBodies["mercury"].mu + self.planetRadius = self.gravFactory.gravBodies["mercury"].radEquator self.sun = 0 self.mercury = 1 # Override information with SPICE timeInitString = "2012 MAY 1 00:28:30.0" - self.gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/', - timeInitString, - epochInMsg=True - ) - self.gravFactory.spiceObject.zeroBase = 'mercury' + self.gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", timeInitString, epochInMsg=True + ) + self.gravFactory.spiceObject.zeroBase = "mercury" # Add pyswice instances - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'de430.bsp') # solar system bodies - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'naif0012.tls') # leap second file - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'de-403-masses.tpc') # solar system masses - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'pck00010.tpc') # generic Planetary Constants + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "de430.bsp" + ) # solar system bodies + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "naif0012.tls" + ) # leap second file + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "de-403-masses.tpc" + ) # solar system masses + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "pck00010.tpc" + ) # generic Planetary Constants def SetEpochObject(self): """ @@ -91,19 +101,27 @@ def SetEpochObject(self): """ # self.epochMsg = self.gravFactory.epochMsg - self.ephemObject.ModelTag = 'EphemData' - self.ephemObject.addSpiceInputMsg(self.gravFactory.spiceObject.planetStateOutMsgs[self.sun]) - self.ephemObject.addSpiceInputMsg(self.gravFactory.spiceObject.planetStateOutMsgs[self.mercury]) + self.ephemObject.ModelTag = "EphemData" + self.ephemObject.addSpiceInputMsg( + self.gravFactory.spiceObject.planetStateOutMsgs[self.sun] + ) + self.ephemObject.addSpiceInputMsg( + self.gravFactory.spiceObject.planetStateOutMsgs[self.mercury] + ) def SetEclipseObject(self): """ Specify what celestial object is causing an eclipse message. """ self.eclipseObject.ModelTag = "eclipseObject" - self.eclipseObject.sunInMsg.subscribeTo(self.gravFactory.spiceObject.planetStateOutMsgs[self.sun]) + self.eclipseObject.sunInMsg.subscribeTo( + self.gravFactory.spiceObject.planetStateOutMsgs[self.sun] + ) # add all celestial objects in spiceObjects except for the sun (0th object) for item in range(1, len(self.gravFactory.spiceObject.planetStateOutMsgs)): - self.eclipseObject.addPlanetToModel(self.gravFactory.spiceObject.planetStateOutMsgs[item]) + self.eclipseObject.addPlanetToModel( + self.gravFactory.spiceObject.planetStateOutMsgs[item] + ) def SetGroundLocations(self): """ @@ -111,8 +129,10 @@ def SetGroundLocations(self): """ self.groundStation.ModelTag = "GroundStation" self.groundStation.planetRadius = self.planetRadius - self.groundStation.specifyLocation(np.radians(40.009971), np.radians(-105.243895), 1624) - self.groundStation.minimumElevation = np.radians(10.) + self.groundStation.specifyLocation( + np.radians(40.009971), np.radians(-105.243895), 1624 + ) + self.groundStation.minimumElevation = np.radians(10.0) self.groundStation.maximumRange = 1e9 # Global call to initialize every module diff --git a/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatDynamics.py b/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatDynamics.py index 61462d85d4..23232df093 100644 --- a/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatDynamics.py +++ b/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatDynamics.py @@ -18,11 +18,25 @@ import numpy as np from Basilisk import __path__ -from Basilisk.simulation import (spacecraft, simpleNav, simpleMassProps, reactionWheelStateEffector, - thrusterDynamicEffector, simpleSolarPanel, simplePowerSink, simpleBattery, fuelTank, - ReactionWheelPower) -from Basilisk.utilities import (macros as mc, unitTestSupport as sp, RigidBodyKinematics as rbk, - simIncludeRW, simIncludeThruster) +from Basilisk.simulation import ( + spacecraft, + simpleNav, + simpleMassProps, + reactionWheelStateEffector, + thrusterDynamicEffector, + simpleSolarPanel, + simplePowerSink, + simpleBattery, + fuelTank, + ReactionWheelPower, +) +from Basilisk.utilities import ( + macros as mc, + unitTestSupport as sp, + RigidBodyKinematics as rbk, + simIncludeRW, + simIncludeThruster, +) bskPath = __path__[0] @@ -31,6 +45,7 @@ class BSKDynamicModels: """ Defines the Dynamics class. """ + def __init__(self, SimBase, dynRate, spacecraftIndex): self.I_sc = None self.solarPanelAxis = None @@ -44,7 +59,9 @@ def __init__(self, SimBase, dynRate, spacecraftIndex): self.processTasksTimeStep = mc.sec2nano(dynRate) # Create task - SimBase.dynProc[spacecraftIndex].addTask(SimBase.CreateNewTask(self.taskName, self.processTasksTimeStep)) + SimBase.dynProc[spacecraftIndex].addTask( + SimBase.CreateNewTask(self.taskName, self.processTasksTimeStep) + ) # Instantiate Dyn modules as objects self.scObject = spacecraft.Spacecraft() @@ -88,11 +105,13 @@ def SetSpacecraftHub(self): Defines the spacecraft object properties. """ self.scObject.ModelTag = "sat-" + str(self.spacecraftIndex) - self.I_sc = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + self.I_sc = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] self.scObject.hub.mHub = 750.0 # kg - spacecraft mass - self.scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + self.scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM self.scObject.hub.IHubPntBc_B = sp.np2EigenMatrix3d(self.I_sc) def SetGravityBodies(self, SimBase): @@ -125,34 +144,43 @@ def SetSimpleMassPropsObject(self): """ Defines the navigation module. """ - self.simpleMassPropsObject.ModelTag = "SimpleMassProperties" + str(self.spacecraftIndex) - self.simpleMassPropsObject.scMassPropsInMsg.subscribeTo(self.scObject.scMassOutMsg) + self.simpleMassPropsObject.ModelTag = "SimpleMassProperties" + str( + self.spacecraftIndex + ) + self.simpleMassPropsObject.scMassPropsInMsg.subscribeTo( + self.scObject.scMassOutMsg + ) def SetReactionWheelDynEffector(self): """ Defines the RW state effector. """ # specify RW momentum capacity - maxRWMomentum = 50. # Nms + maxRWMomentum = 50.0 # Nms # Define orthogonal RW pyramid # -- Pointing directions rwElAngle = np.array([40.0, 40.0, 40.0, 40.0]) * mc.D2R rwAzimuthAngle = np.array([45.0, 135.0, 225.0, 315.0]) * mc.D2R - rwPosVector = [[0.8, 0.8, 1.79070], - [0.8, -0.8, 1.79070], - [-0.8, -0.8, 1.79070], - [-0.8, 0.8, 1.79070]] + rwPosVector = [ + [0.8, 0.8, 1.79070], + [0.8, -0.8, 1.79070], + [-0.8, -0.8, 1.79070], + [-0.8, 0.8, 1.79070], + ] for elAngle, azAngle, posVector in zip(rwElAngle, rwAzimuthAngle, rwPosVector): - gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot(np.array([1, 0, 0])) - self.rwFactory.create('Honeywell_HR16', - gsHat, - maxMomentum=maxRWMomentum, - rWB_B=posVector) + gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot( + np.array([1, 0, 0]) + ) + self.rwFactory.create( + "Honeywell_HR16", gsHat, maxMomentum=maxRWMomentum, rWB_B=posVector + ) self.numRW = self.rwFactory.getNumOfDevices() - self.rwFactory.addToSpacecraft("RWArray" + str(self.spacecraftIndex), self.rwStateEffector, self.scObject) + self.rwFactory.addToSpacecraft( + "RWArray" + str(self.spacecraftIndex), self.rwStateEffector, self.scObject + ) def SetThrusterDynEffector(self): """ @@ -163,12 +191,16 @@ def SetThrusterDynEffector(self): # create the thruster devices by specifying the thruster type and its location and direction for pos_B, dir_B in zip(location, direction): - self.thrusterFactory.create('MOOG_Monarc_90HT', pos_B, dir_B, useMinPulseTime=False) + self.thrusterFactory.create( + "MOOG_Monarc_90HT", pos_B, dir_B, useMinPulseTime=False + ) self.numThr = self.thrusterFactory.getNumOfDevices() # create thruster object container and tie to spacecraft object - self.thrusterFactory.addToSpacecraft("thrusterFactory", self.thrusterDynamicEffector, self.scObject) + self.thrusterFactory.addToSpacecraft( + "thrusterFactory", self.thrusterDynamicEffector, self.scObject + ) def SetFuelTank(self): """ @@ -191,38 +223,56 @@ def SetFuelTank(self): def SetReactionWheelPower(self): """Sets the reaction wheel power parameters""" for item in range(self.numRW): - self.rwPowerList[item].ModelTag = self.scObject.ModelTag + "RWPower" + str(item) - self.rwPowerList[item].basePowerNeed = 5. # baseline power draw, Watt - self.rwPowerList[item].rwStateInMsg.subscribeTo(self.rwStateEffector.rwOutMsgs[item]) + self.rwPowerList[item].ModelTag = ( + self.scObject.ModelTag + "RWPower" + str(item) + ) + self.rwPowerList[item].basePowerNeed = 5.0 # baseline power draw, Watt + self.rwPowerList[item].rwStateInMsg.subscribeTo( + self.rwStateEffector.rwOutMsgs[item] + ) self.rwPowerList[item].mechToElecEfficiency = 0.5 def SetSolarPanel(self, SimBase): """Sets the solar panel""" self.solarPanel.ModelTag = "solarPanel" self.solarPanel.stateInMsg.subscribeTo(self.scObject.scStateOutMsg) - self.solarPanel.sunEclipseInMsg.subscribeTo(SimBase.EnvModel.eclipseObject.eclipseOutMsgs[0]) # choose the earth message - self.solarPanel.sunInMsg.subscribeTo(SimBase.EnvModel.gravFactory.spiceObject.planetStateOutMsgs[SimBase.EnvModel.sun]) + self.solarPanel.sunEclipseInMsg.subscribeTo( + SimBase.EnvModel.eclipseObject.eclipseOutMsgs[0] + ) # choose the earth message + self.solarPanel.sunInMsg.subscribeTo( + SimBase.EnvModel.gravFactory.spiceObject.planetStateOutMsgs[ + SimBase.EnvModel.sun + ] + ) self.solarPanelAxis = [0, 0, 1] - self.solarPanel.setPanelParameters(self.solarPanelAxis, # panel normal vector in the body frame - 0.4 * 0.4 * 2 + 0.2 * 0.4 * 2, # area, m^2 - 0.35) # efficiency + self.solarPanel.setPanelParameters( + self.solarPanelAxis, # panel normal vector in the body frame + 0.4 * 0.4 * 2 + 0.2 * 0.4 * 2, # area, m^2 + 0.35, + ) # efficiency def SetPowerSink(self): """Defines the energy sink parameters""" self.powerSink.ModelTag = "powerSink" - self.powerSink.nodePowerOut = -2. # Watt + self.powerSink.nodePowerOut = -2.0 # Watt def SetBattery(self): """Sets up the battery with all the power components""" self.powerMonitor.ModelTag = "powerMonitor" - self.powerMonitor.storageCapacity = 2 * 60.0 * 3600.0 # Convert from W-hr to Joule - self.powerMonitor.storedCharge_Init = self.powerMonitor.storageCapacity * 0.6 # 40% depletion + self.powerMonitor.storageCapacity = ( + 2 * 60.0 * 3600.0 + ) # Convert from W-hr to Joule + self.powerMonitor.storedCharge_Init = ( + self.powerMonitor.storageCapacity * 0.6 + ) # 40% depletion # attach the sources/sinks to the battery self.powerMonitor.addPowerNodeToModel(self.solarPanel.nodePowerOutMsg) self.powerMonitor.addPowerNodeToModel(self.powerSink.nodePowerOutMsg) for item in range(self.numRW): - self.powerMonitor.addPowerNodeToModel(self.rwPowerList[item].nodePowerOutMsg) + self.powerMonitor.addPowerNodeToModel( + self.rwPowerList[item].nodePowerOutMsg + ) # Global call to initialize every module def InitAllDynObjects(self, SimBase): diff --git a/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatFsw.py b/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatFsw.py index 0c8e977191..22fb41a469 100644 --- a/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatFsw.py +++ b/examples/MultiSatBskSim/modelsMultiSat/BSK_MultiSatFsw.py @@ -34,6 +34,7 @@ class BSKFswModels: """Defines the FSW class""" + def __init__(self, SimBase, fswRate, spacecraftIndex): # define empty class variables self.spacecraftIndex = spacecraftIndex @@ -56,18 +57,43 @@ def __init__(self, SimBase, fswRate, spacecraftIndex): self.processTasksTimeStep = mc.sec2nano(fswRate) # Create tasks - SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("inertialPointTask" + str(spacecraftIndex), - self.processTasksTimeStep), 20) - SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("sunPointTask" + str(spacecraftIndex), - self.processTasksTimeStep), 20) - SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("locPointTask" + str(spacecraftIndex), - self.processTasksTimeStep), 20) - SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("spacecraftReconfigTask" + str(spacecraftIndex), - self.processTasksTimeStep), 15) - SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("trackingErrorTask" + str(spacecraftIndex), - self.processTasksTimeStep), 10) - SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("mrpFeedbackRWsTask" + str(spacecraftIndex), - self.processTasksTimeStep), 5) + SimBase.fswProc[spacecraftIndex].addTask( + SimBase.CreateNewTask( + "inertialPointTask" + str(spacecraftIndex), self.processTasksTimeStep + ), + 20, + ) + SimBase.fswProc[spacecraftIndex].addTask( + SimBase.CreateNewTask( + "sunPointTask" + str(spacecraftIndex), self.processTasksTimeStep + ), + 20, + ) + SimBase.fswProc[spacecraftIndex].addTask( + SimBase.CreateNewTask( + "locPointTask" + str(spacecraftIndex), self.processTasksTimeStep + ), + 20, + ) + SimBase.fswProc[spacecraftIndex].addTask( + SimBase.CreateNewTask( + "spacecraftReconfigTask" + str(spacecraftIndex), + self.processTasksTimeStep, + ), + 15, + ) + SimBase.fswProc[spacecraftIndex].addTask( + SimBase.CreateNewTask( + "trackingErrorTask" + str(spacecraftIndex), self.processTasksTimeStep + ), + 10, + ) + SimBase.fswProc[spacecraftIndex].addTask( + SimBase.CreateNewTask( + "mrpFeedbackRWsTask" + str(spacecraftIndex), self.processTasksTimeStep + ), + 5, + ) # Create module data and module wraps self.inertial3DPoint = inertial3D.inertial3D() @@ -98,18 +124,28 @@ def __init__(self, SimBase, fswRate, spacecraftIndex): self.InitAllFSWObjects(SimBase) # Assign initialized modules to tasks - SimBase.AddModelToTask("inertialPointTask" + str(spacecraftIndex), self.inertial3DPoint, 10) + SimBase.AddModelToTask( + "inertialPointTask" + str(spacecraftIndex), self.inertial3DPoint, 10 + ) SimBase.AddModelToTask("sunPointTask" + str(spacecraftIndex), self.sunPoint, 10) SimBase.AddModelToTask("locPointTask" + str(spacecraftIndex), self.locPoint, 10) - SimBase.AddModelToTask("spacecraftReconfigTask" + str(spacecraftIndex), self.spacecraftReconfig, 10) + SimBase.AddModelToTask( + "spacecraftReconfigTask" + str(spacecraftIndex), self.spacecraftReconfig, 10 + ) - SimBase.AddModelToTask("trackingErrorTask" + str(spacecraftIndex), self.trackingError, 9) + SimBase.AddModelToTask( + "trackingErrorTask" + str(spacecraftIndex), self.trackingError, 9 + ) - SimBase.AddModelToTask("mrpFeedbackRWsTask" + str(spacecraftIndex), self.mrpFeedbackRWs, 7) - SimBase.AddModelToTask("mrpFeedbackRWsTask" + str(spacecraftIndex), self.rwMotorTorque, 6) + SimBase.AddModelToTask( + "mrpFeedbackRWsTask" + str(spacecraftIndex), self.mrpFeedbackRWs, 7 + ) + SimBase.AddModelToTask( + "mrpFeedbackRWsTask" + str(spacecraftIndex), self.rwMotorTorque, 6 + ) # Create events to be called for triggering GN&C maneuvers SimBase.fswProc[spacecraftIndex].disableAllTasks() @@ -223,16 +259,24 @@ def SetInertial3DPointGuidance(self): Defines the inertial pointing guidance module. """ self.inertial3DPoint.sigma_R0N = [0.1, 0.2, -0.3] - messaging.AttRefMsg_C_addAuthor(self.inertial3DPoint.attRefOutMsg, self.attRefMsg) + messaging.AttRefMsg_C_addAuthor( + self.inertial3DPoint.attRefOutMsg, self.attRefMsg + ) def SetSunPointGuidance(self, SimBase): """ Defines the Sun pointing guidance module. """ self.sunPoint.pHat_B = SimBase.DynModels[self.spacecraftIndex].solarPanelAxis - self.sunPoint.scAttInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleNavObject.attOutMsg) - self.sunPoint.scTransInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleNavObject.transOutMsg) - self.sunPoint.celBodyInMsg.subscribeTo(SimBase.EnvModel.ephemObject.ephemOutMsgs[SimBase.EnvModel.sun]) + self.sunPoint.scAttInMsg.subscribeTo( + SimBase.DynModels[self.spacecraftIndex].simpleNavObject.attOutMsg + ) + self.sunPoint.scTransInMsg.subscribeTo( + SimBase.DynModels[self.spacecraftIndex].simpleNavObject.transOutMsg + ) + self.sunPoint.celBodyInMsg.subscribeTo( + SimBase.EnvModel.ephemObject.ephemOutMsgs[SimBase.EnvModel.sun] + ) messaging.AttRefMsg_C_addAuthor(self.sunPoint.attRefOutMsg, self.attRefMsg) def SetLocationPointGuidance(self, SimBase): @@ -240,9 +284,15 @@ def SetLocationPointGuidance(self, SimBase): Defines the Earth location pointing guidance module. """ self.locPoint.pHat_B = [1, 0, 0] - self.locPoint.scAttInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleNavObject.attOutMsg) - self.locPoint.scTransInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleNavObject.transOutMsg) - self.locPoint.locationInMsg.subscribeTo(SimBase.EnvModel.groundStation.currentGroundStateOutMsg) + self.locPoint.scAttInMsg.subscribeTo( + SimBase.DynModels[self.spacecraftIndex].simpleNavObject.attOutMsg + ) + self.locPoint.scTransInMsg.subscribeTo( + SimBase.DynModels[self.spacecraftIndex].simpleNavObject.transOutMsg + ) + self.locPoint.locationInMsg.subscribeTo( + SimBase.EnvModel.groundStation.currentGroundStateOutMsg + ) messaging.AttRefMsg_C_addAuthor(self.locPoint.attRefOutMsg, self.attRefMsg) def SetSpacecraftOrbitReconfig(self, SimBase): @@ -250,13 +300,20 @@ def SetSpacecraftOrbitReconfig(self, SimBase): Defines the station keeping module. """ self.spacecraftReconfig.deputyTransInMsg.subscribeTo( - SimBase.DynModels[self.spacecraftIndex].simpleNavObject.transOutMsg) + SimBase.DynModels[self.spacecraftIndex].simpleNavObject.transOutMsg + ) self.spacecraftReconfig.attRefInMsg.subscribeTo(self.attRefMsg) self.spacecraftReconfig.thrustConfigInMsg.subscribeTo(self.fswThrusterConfigMsg) - self.spacecraftReconfig.vehicleConfigInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleMassPropsObject.vehicleConfigOutMsg) + self.spacecraftReconfig.vehicleConfigInMsg.subscribeTo( + SimBase.DynModels[ + self.spacecraftIndex + ].simpleMassPropsObject.vehicleConfigOutMsg + ) self.spacecraftReconfig.mu = SimBase.EnvModel.mu # [m^3/s^2] self.spacecraftReconfig.attControlTime = 400 # [s] - messaging.AttRefMsg_C_addAuthor(self.spacecraftReconfig.attRefOutMsg, self.attRefMsg) + messaging.AttRefMsg_C_addAuthor( + self.spacecraftReconfig.attRefOutMsg, self.attRefMsg + ) # connect a blank chief message chiefData = messaging.NavTransMsgPayload() @@ -268,9 +325,12 @@ def SetAttitudeTrackingError(self, SimBase): Defines the module that converts a reference message into a guidance message. """ self.trackingError.attNavInMsg.subscribeTo( - SimBase.DynModels[self.spacecraftIndex].simpleNavObject.attOutMsg) + SimBase.DynModels[self.spacecraftIndex].simpleNavObject.attOutMsg + ) self.trackingError.attRefInMsg.subscribeTo(self.attRefMsg) - messaging.AttGuidMsg_C_addAuthor(self.trackingError.attGuidOutMsg, self.attGuidMsg) + messaging.AttGuidMsg_C_addAuthor( + self.trackingError.attGuidOutMsg, self.attGuidMsg + ) def SetMRPFeedbackRWA(self, SimBase): """ @@ -279,15 +339,24 @@ def SetMRPFeedbackRWA(self, SimBase): self.decayTime = 50 self.xi = 0.9 self.mrpFeedbackRWs.Ki = -1 # make value negative to turn off integral feedback - self.mrpFeedbackRWs.P = 2 * np.max(SimBase.DynModels[self.spacecraftIndex].I_sc) / self.decayTime - self.mrpFeedbackRWs.K = (self.mrpFeedbackRWs.P / self.xi) * \ - (self.mrpFeedbackRWs.P / self.xi) / np.max( - SimBase.DynModels[self.spacecraftIndex].I_sc) - self.mrpFeedbackRWs.integralLimit = 2. / self.mrpFeedbackRWs.Ki * 0.1 + self.mrpFeedbackRWs.P = ( + 2 * np.max(SimBase.DynModels[self.spacecraftIndex].I_sc) / self.decayTime + ) + self.mrpFeedbackRWs.K = ( + (self.mrpFeedbackRWs.P / self.xi) + * (self.mrpFeedbackRWs.P / self.xi) + / np.max(SimBase.DynModels[self.spacecraftIndex].I_sc) + ) + self.mrpFeedbackRWs.integralLimit = 2.0 / self.mrpFeedbackRWs.Ki * 0.1 - self.mrpFeedbackRWs.vehConfigInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleMassPropsObject.vehicleConfigOutMsg) + self.mrpFeedbackRWs.vehConfigInMsg.subscribeTo( + SimBase.DynModels[ + self.spacecraftIndex + ].simpleMassPropsObject.vehicleConfigOutMsg + ) self.mrpFeedbackRWs.rwSpeedsInMsg.subscribeTo( - SimBase.DynModels[self.spacecraftIndex].rwStateEffector.rwSpeedOutMsg) + SimBase.DynModels[self.spacecraftIndex].rwStateEffector.rwSpeedOutMsg + ) self.mrpFeedbackRWs.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) self.mrpFeedbackRWs.guidInMsg.subscribeTo(self.attGuidMsg) @@ -297,14 +366,18 @@ def SetRWConfigMsg(self, SimBase): """ # Configure RW pyramid exactly as it is in the Dynamics (i.e. FSW with perfect knowledge) # the same msg is used here for both spacecraft - self.fswRwConfigMsg = SimBase.DynModels[self.spacecraftIndex].rwFactory.getConfigMessage() + self.fswRwConfigMsg = SimBase.DynModels[ + self.spacecraftIndex + ].rwFactory.getConfigMessage() def SetThrustersConfigMsg(self, SimBase): """ Imports the thrusters configuration information. """ fswSetupThrusters.clearSetup() - for key, th in SimBase.DynModels[self.spacecraftIndex].thrusterFactory.thrusterList.items(): + for key, th in SimBase.DynModels[ + self.spacecraftIndex + ].thrusterFactory.thrusterList.items(): loc_B_tmp = list(itertools.chain.from_iterable(th.thrLoc_B)) dir_B_tmp = list(itertools.chain.from_iterable(th.thrDir_B)) fswSetupThrusters.create(loc_B_tmp, dir_B_tmp, th.MaxThrust) @@ -314,12 +387,12 @@ def SetRWMotorTorque(self): """ Defines the motor torque from the control law. """ - controlAxes_B = [1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0] + controlAxes_B = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] self.rwMotorTorque.controlAxes_B = controlAxes_B - self.rwMotorTorque.vehControlInMsg.subscribeTo(self.mrpFeedbackRWs.cmdTorqueOutMsg) + self.rwMotorTorque.vehControlInMsg.subscribeTo( + self.mrpFeedbackRWs.cmdTorqueOutMsg + ) self.rwMotorTorque.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) # Global call to initialize every module @@ -346,10 +419,16 @@ def setupGatewayMsgs(self, SimBase): self.zeroGateWayMsgs() # connect gateway FSW effector command msgs with the dynamics - SimBase.DynModels[self.spacecraftIndex].rwStateEffector.rwMotorCmdInMsg.subscribeTo( - self.rwMotorTorque.rwMotorTorqueOutMsg) - SimBase.DynModels[self.spacecraftIndex].thrusterDynamicEffector.cmdsInMsg.subscribeTo( - self.spacecraftReconfig.onTimeOutMsg) + SimBase.DynModels[ + self.spacecraftIndex + ].rwStateEffector.rwMotorCmdInMsg.subscribeTo( + self.rwMotorTorque.rwMotorTorqueOutMsg + ) + SimBase.DynModels[ + self.spacecraftIndex + ].thrusterDynamicEffector.cmdsInMsg.subscribeTo( + self.spacecraftReconfig.onTimeOutMsg + ) def zeroGateWayMsgs(self): """Zero all FSW gateway message payloads""" @@ -357,8 +436,12 @@ def zeroGateWayMsgs(self): self.attGuidMsg.write(messaging.AttGuidMsgPayload()) # Zero all actuator commands - self.rwMotorTorque.rwMotorTorqueOutMsg.write(messaging.ArrayMotorTorqueMsgPayload()) - self.spacecraftReconfig.onTimeOutMsg.write(messaging.THRArrayOnTimeCmdMsgPayload()) + self.rwMotorTorque.rwMotorTorqueOutMsg.write( + messaging.ArrayMotorTorqueMsgPayload() + ) + self.spacecraftReconfig.onTimeOutMsg.write( + messaging.THRArrayOnTimeCmdMsgPayload() + ) # If CMGs are present in the configuration: # self.cmgCmdMsg.write(messaging.CMGCmdMsgPayload()) diff --git a/examples/MultiSatBskSim/plottingMultiSat/BSK_MultiSatPlotting.py b/examples/MultiSatBskSim/plottingMultiSat/BSK_MultiSatPlotting.py index fda13654f9..783d75faee 100644 --- a/examples/MultiSatBskSim/plottingMultiSat/BSK_MultiSatPlotting.py +++ b/examples/MultiSatBskSim/plottingMultiSat/BSK_MultiSatPlotting.py @@ -22,268 +22,343 @@ # --------------------------------- COMPONENTS & SUBPLOT HANDLING ----------------------------------------------- # + def show_all_plots(): plt.show() + def clear_all_plots(): plt.close("all") + def save_all_plots(fileName, figureNames): figureList = {} numFigures = len(figureNames) for i in range(0, numFigures): pltName = fileName + "_" + figureNames[i] - figureList[pltName] = plt.figure(i+1) + figureList[pltName] = plt.figure(i + 1) return figureList + # ------------------------------------- MAIN PLOT HANDLING ------------------------------------------------------ # -color_x = 'dodgerblue' -color_y = 'salmon' -color_z = 'lightgreen' +color_x = "dodgerblue" +color_y = "salmon" +color_z = "lightgreen" m2km = 1.0 / 1000.0 + def plot_attitude(timeData, dataSigmaBN, id=None): """Plot the spacecraft attitude.""" plt.figure(id) for idx in range(3): - plt.plot(timeData, dataSigmaBN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx+1) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude $\sigma_{B/N}$') + plt.plot( + timeData, + dataSigmaBN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx + 1) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude $\sigma_{B/N}$") return + def plot_attitude_error(timeData, dataSigmaBR, id=None): """Plot the spacecraft attitude error.""" plt.figure(id) for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx+1) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Tracking Error $\sigma_{B/R}$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx + 1) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Tracking Error $\sigma_{B/R}$") return + def plot_attitude_reference(timeData, dataSigmaRN, id=None): """Plot the spacecraft attitude reference.""" plt.figure(id) for idx in range(3): - plt.plot(timeData, dataSigmaRN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx+1) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Reference $\sigma_{R/N}$') + plt.plot( + timeData, + dataSigmaRN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx + 1) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Reference $\sigma_{R/N}$") return + def plot_rate(timeData, dataOmegaBN, id=None): """Plot the body angular velocity rate tracking errors.""" plt.figure(id) for idx in range(3): - plt.plot(timeData, dataOmegaBN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BN,' + str(idx+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Angular Rate (rad/s)') + plt.plot( + timeData, + dataOmegaBN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BN," + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Angular Rate (rad/s)") return + def plot_rate_error(timeData, dataOmegaBR, id=None): """Plot the body angular velocity rate tracking errors.""" plt.figure(id) for idx in range(3): - plt.plot(timeData, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error (rad/s)') + plt.plot( + timeData, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error (rad/s)") return + def plot_rate_reference(timeData, dataOmegaRN, id=None): """Plot the body angular velocity rate reference.""" plt.figure(id) for idx in range(3): - plt.plot(timeData, dataOmegaRN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{RN,' + str(idx+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Reference Rate (rad/s)') + plt.plot( + timeData, + dataOmegaRN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{RN," + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Reference Rate (rad/s)") return + def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW, id=None): """Plot the RW motor torques.""" plt.figure(id) for idx in range(numRW): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx+1) + '}$') - plt.plot(timeData, dataRW[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx + 1) + "}$", + ) + plt.plot( + timeData, + dataRW[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") return + def plot_rw_speeds(timeData, dataOmegaRW, numRW, id=None): """Plot the RW spin rates.""" plt.figure(id) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed (RPM) ') + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed (RPM) ") return + def plot_rw_voltages(timeData, dataVolt, numRW, id=None): """Plot the RW voltage inputs.""" plt.figure(id) for idx in range(numRW): - plt.plot(timeData, dataVolt[:, idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$V_{' + str(idx+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Voltage (V)') + plt.plot( + timeData, + dataVolt[:, idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$V_{" + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Voltage (V)") return + def plot_rw_power(timeData, dataRwPower, numRW, id=None): """Plot the RW power drain.""" plt.figure(id) for idx in range(numRW): - plt.plot(timeData, dataRwPower[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$p_{rw,' + str(idx+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Power (W)') + plt.plot( + timeData, + dataRwPower[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$p_{rw," + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Power (W)") return + def plot_power(timeData, netData, supplyData, sinkData, id=None): """Plot the power inputs and outputs""" plt.figure(id) - plt.plot(timeData, netData, label='Net Power') - plt.plot(timeData, supplyData, label='Panel Power') - plt.plot(timeData, sinkData, label='Power Draw') - plt.xlabel('Time [min]') - plt.ylabel('Power [W]') + plt.plot(timeData, netData, label="Net Power") + plt.plot(timeData, supplyData, label="Panel Power") + plt.plot(timeData, sinkData, label="Power Draw") + plt.xlabel("Time [min]") + plt.ylabel("Power [W]") plt.grid(True) - plt.legend(loc='lower right') + plt.legend(loc="lower right") return + def plot_battery(timeData, storageData, id=None): """Plot the energy inside the onboard battery""" plt.figure(id) plt.plot(timeData, storageData) - plt.xlabel('Time [min]') - plt.ylabel('Stored Energy [W-s]') + plt.xlabel("Time [min]") + plt.ylabel("Stored Energy [W-s]") return + def plot_rw_temperature(timeData, tempData, numRW, id=None): """Plot the reaction wheel temperatures""" plt.figure(id) for idx in range(numRW): - plt.plot(timeData, tempData[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$T_{rw,' + str(idx+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Temperatures [ºC]') + plt.plot( + timeData, + tempData[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$T_{rw," + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Temperatures [ºC]") return + def plot_thrust(timeData, thrustData, numThr, id=None): """Plot the thrusters net force output""" plt.figure(id) for idx in range(numThr): - plt.plot(timeData, thrustData[idx], - color=unitTestSupport.getLineColor(idx, numThr), - label='$F_{thr,' + str(idx + 1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Thrust [N]') + plt.plot( + timeData, + thrustData[idx], + color=unitTestSupport.getLineColor(idx, numThr), + label="$F_{thr," + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Thrust [N]") return + def plot_thrust_percentage(timeData, thrustData, numThr, id=None): """Plot the thrust as a percentage of maximum""" plt.figure(id) for idx in range(numThr): - plt.plot(timeData, thrustData[idx], - color=unitTestSupport.getLineColor(idx, numThr), - label='$F_{thr,' + str(idx + 1) + '}$') - plt.legend(loc='lower right') + plt.plot( + timeData, + thrustData[idx], + color=unitTestSupport.getLineColor(idx, numThr), + label="$F_{thr," + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") plt.ylim([0, 1.1]) - plt.xlabel('Time [min]') - plt.ylabel('Thrust Percentage') + plt.xlabel("Time [min]") + plt.ylabel("Thrust Percentage") return + def plot_fuel(timeData, fuelData, id=None): """Plot the fuel mass information""" plt.figure(id) plt.plot(timeData, fuelData) - plt.xlabel('Time [min]') - plt.ylabel('Stored Fuel Mass [kg]') + plt.xlabel("Time [min]") + plt.ylabel("Stored Fuel Mass [kg]") return + def plot_orbit(r_BN, id=None): """Plot the spacecraft inertial orbitS.""" plt.figure(id, figsize=(6, 5)) - ax = plt.axes(projection='3d') - ax.plot(r_BN[:, 0] * m2km, r_BN[:, 1] * m2km, r_BN[:, 2] * m2km, - label="Spacecraft") + ax = plt.axes(projection="3d") + ax.plot(r_BN[:, 0] * m2km, r_BN[:, 1] * m2km, r_BN[:, 2] * m2km, label="Spacecraft") ax.set_xlim3d(-8000, 8000) ax.set_ylim3d(-8000, 8000) ax.set_zlim3d(-8000, 8000) ax.scatter(0, 0, 0, c=color_x) - ax.set_title('Spacecraft Inertial Orbit') - ax.set_xlabel('x [km]') - ax.set_ylabel('y [km]') - ax.set_zlabel('z [km]') + ax.set_title("Spacecraft Inertial Orbit") + ax.set_xlabel("x [km]") + ax.set_ylabel("y [km]") + ax.set_zlabel("z [km]") ax.legend(loc=2) return + def plot_orbits(r_BN, numberSpacecraft, id=None): """Plot the spacecraft inertial orbits.""" plt.figure(id, figsize=(6, 5)) - ax = plt.axes(projection='3d') + ax = plt.axes(projection="3d") for i in range(numberSpacecraft): - ax.plot(r_BN[i][:, 0] * m2km, r_BN[i][:, 1] * m2km, r_BN[i][:, 2] * m2km, - label="Spacecraft " + str(i), - c=unitTestSupport.getLineColor(i, numberSpacecraft)) + ax.plot( + r_BN[i][:, 0] * m2km, + r_BN[i][:, 1] * m2km, + r_BN[i][:, 2] * m2km, + label="Spacecraft " + str(i), + c=unitTestSupport.getLineColor(i, numberSpacecraft), + ) ax.set_xlim3d(-8000, 8000) ax.set_ylim3d(-8000, 8000) ax.set_zlim3d(-8000, 8000) ax.scatter(0, 0, 0, c=color_x) - ax.set_title('Spacecraft Inertial Orbits') - ax.set_xlabel('x [km]') - ax.set_ylabel('y [km]') - ax.set_zlabel('z [km]') + ax.set_title("Spacecraft Inertial Orbits") + ax.set_xlabel("x [km]") + ax.set_ylabel("y [km]") + ax.set_zlabel("z [km]") ax.legend(loc=2) return + def plot_relative_orbits(r_BN, numberSpacecraft, id=None): """Plot the spacecraft inertial orbits.""" plt.figure(id, figsize=(6, 5)) - ax = plt.axes(projection='3d') + ax = plt.axes(projection="3d") for i in range(numberSpacecraft): - ax.plot(r_BN[i][:, 0] * m2km, r_BN[i][:, 1] * m2km, r_BN[i][:, 2] * m2km, - label="Spacecraft " + str(i), - c=unitTestSupport.getLineColor(i, numberSpacecraft)) - ax.set_box_aspect((np.ptp(r_BN[i][:, 0]), np.ptp(r_BN[i][:, 1]), np.ptp(r_BN[i][:, 2]))) + ax.plot( + r_BN[i][:, 0] * m2km, + r_BN[i][:, 1] * m2km, + r_BN[i][:, 2] * m2km, + label="Spacecraft " + str(i), + c=unitTestSupport.getLineColor(i, numberSpacecraft), + ) + ax.set_box_aspect( + (np.ptp(r_BN[i][:, 0]), np.ptp(r_BN[i][:, 1]), np.ptp(r_BN[i][:, 2])) + ) ax.scatter(0, 0, 0, c=color_x) - ax.set_title('Spacecraft Relative Orbits in Hill Frame') - ax.set_xlabel('$i_r$ [km]') - ax.set_ylabel(r'$i_{\theta}$ [km]') - ax.set_zlabel('$i_h$ [km]') + ax.set_title("Spacecraft Relative Orbits in Hill Frame") + ax.set_xlabel("$i_r$ [km]") + ax.set_ylabel(r"$i_{\theta}$ [km]") + ax.set_zlabel("$i_h$ [km]") ax.legend(loc=2) return + def plot_orbital_element_differences(timeData, oed, id=None): """Plot the orbital element difference between the chief and another spacecraft.""" plt.figure(id) diff --git a/examples/MultiSatBskSim/scenariosMultiSat/scenario_AttGuidMultiSat.py b/examples/MultiSatBskSim/scenariosMultiSat/scenario_AttGuidMultiSat.py index f9aaed5e47..7ef6ce1def 100644 --- a/examples/MultiSatBskSim/scenariosMultiSat/scenario_AttGuidMultiSat.py +++ b/examples/MultiSatBskSim/scenariosMultiSat/scenario_AttGuidMultiSat.py @@ -98,6 +98,7 @@ """ import copy + # Get current file path import inspect import os @@ -110,24 +111,27 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../') -sys.path.append(path + '/../modelsMultiSat') -sys.path.append(path + '/../plottingMultiSat') +sys.path.append(path + "/../") +sys.path.append(path + "/../modelsMultiSat") +sys.path.append(path + "/../plottingMultiSat") from BSK_MultiSatMasters import BSKSim, BSKScenario import BSK_EnvironmentEarth, BSK_MultiSatDynamics, BSK_MultiSatFsw # Import plotting files for your scenario import BSK_MultiSatPlotting as plt + # Create your own scenario child class class scenario_AttGuidFormationFlying(BSKSim, BSKScenario): def __init__(self, numberSpacecraft): - super(scenario_AttGuidFormationFlying, self).__init__(numberSpacecraft, fswRate=10, dynRate=10, envRate=10) - self.name = 'scenario_AttGuidFormationFlying' + super(scenario_AttGuidFormationFlying, self).__init__( + numberSpacecraft, fswRate=10, dynRate=10, envRate=10 + ) + self.name = "scenario_AttGuidFormationFlying" self.set_EnvModel(BSK_EnvironmentEarth) - self.set_DynModel([BSK_MultiSatDynamics]*self.numberSpacecraft) - self.set_FswModel([BSK_MultiSatFsw]*self.numberSpacecraft) + self.set_DynModel([BSK_MultiSatDynamics] * self.numberSpacecraft) + self.set_FswModel([BSK_MultiSatFsw] * self.numberSpacecraft) # declare empty class variables self.samplingTime = [] @@ -151,10 +155,13 @@ def __init__(self, numberSpacecraft): DynModelsList.append(self.DynModels[i].scObject) rwStateEffectorList.append(self.DynModels[i].rwStateEffector) - vizSupport.enableUnityVisualization(self, self.DynModels[0].taskName, DynModelsList - # , saveFile=__file__ - , rwEffectorList=rwStateEffectorList - ) + vizSupport.enableUnityVisualization( + self, + self.DynModels[0].taskName, + DynModelsList, + # , saveFile=__file__ + rwEffectorList=rwStateEffectorList, + ) def configure_initial_conditions(self): EnvModel = self.get_EnvModel() @@ -162,7 +169,7 @@ def configure_initial_conditions(self): # Configure Dynamics initial conditions self.oe.append(orbitalMotion.ClassicElements()) - self.oe[0].a = 1.1*EnvModel.planetRadius # meters + self.oe[0].a = 1.1 * EnvModel.planetRadius # meters self.oe[0].e = 0.01 self.oe[0].i = 45.0 * macros.D2R self.oe[0].Omega = 48.2 * macros.D2R @@ -173,11 +180,15 @@ def configure_initial_conditions(self): DynModels[0].scObject.hub.r_CN_NInit = rN # m - r_CN_N DynModels[0].scObject.hub.v_CN_NInit = vN # m/s - v_CN_N DynModels[0].scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels[0].scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + DynModels[0].scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B # Configure Dynamics initial conditions self.oe.append(copy.deepcopy(self.oe[0])) - self.oe[1].a = 1.2*EnvModel.planetRadius + self.oe[1].a = 1.2 * EnvModel.planetRadius self.oe[1].e = 0.1 self.oe[1].i = 30.0 * macros.D2R rN2, vN2 = orbitalMotion.elem2rv(EnvModel.mu, self.oe[1]) @@ -185,7 +196,11 @@ def configure_initial_conditions(self): DynModels[1].scObject.hub.r_CN_NInit = rN2 # m - r_CN_N DynModels[1].scObject.hub.v_CN_NInit = vN2 # m/s - v_CN_N DynModels[1].scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels[1].scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + DynModels[1].scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B # Configure Dynamics initial conditions self.oe.append(copy.deepcopy(self.oe[0])) @@ -197,7 +212,11 @@ def configure_initial_conditions(self): DynModels[2].scObject.hub.r_CN_NInit = rN3 # m - r_CN_N DynModels[2].scObject.hub.v_CN_NInit = vN3 # m/s - v_CN_N DynModels[2].scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels[2].scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + DynModels[2].scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B def log_outputs(self): # Process outputs @@ -209,31 +228,62 @@ def log_outputs(self): # Loop through every spacecraft for spacecraft in range(self.numberSpacecraft): - # log the navigation messages - self.snTransLog.append(DynModels[spacecraft].simpleNavObject.transOutMsg.recorder(self.samplingTime)) - self.snAttLog.append(DynModels[spacecraft].simpleNavObject.attOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.snTransLog[spacecraft]) - self.AddModelToTask(DynModels[spacecraft].taskName, self.snAttLog[spacecraft]) + self.snTransLog.append( + DynModels[spacecraft].simpleNavObject.transOutMsg.recorder( + self.samplingTime + ) + ) + self.snAttLog.append( + DynModels[spacecraft].simpleNavObject.attOutMsg.recorder( + self.samplingTime + ) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.snTransLog[spacecraft] + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.snAttLog[spacecraft] + ) # log the attitude error messages - self.attErrorLog.append(FswModels[spacecraft].attGuidMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.attErrorLog[spacecraft]) + self.attErrorLog.append( + FswModels[spacecraft].attGuidMsg.recorder(self.samplingTime) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.attErrorLog[spacecraft] + ) # log the RW torque messages self.rwMotorLog.append( - FswModels[spacecraft].rwMotorTorque.rwMotorTorqueOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.rwMotorLog[spacecraft]) + FswModels[spacecraft].rwMotorTorque.rwMotorTorqueOutMsg.recorder( + self.samplingTime + ) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.rwMotorLog[spacecraft] + ) # log the RW wheel speed information - self.rwSpeedLog.append(DynModels[spacecraft].rwStateEffector.rwSpeedOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.rwSpeedLog[spacecraft]) + self.rwSpeedLog.append( + DynModels[spacecraft].rwStateEffector.rwSpeedOutMsg.recorder( + self.samplingTime + ) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.rwSpeedLog[spacecraft] + ) # log addition RW information (power, etc) for item in range(DynModels[spacecraft].numRW): self.rwLogs[spacecraft].append( - DynModels[spacecraft].rwStateEffector.rwOutMsgs[item].recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.rwLogs[spacecraft][item]) + DynModels[spacecraft] + .rwStateEffector.rwOutMsgs[item] + .recorder(self.samplingTime) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.rwLogs[spacecraft][item] + ) def pull_outputs(self, show_plots, spacecraftIndex): # Dynamics process outputs @@ -266,16 +316,26 @@ def pull_outputs(self, show_plots, spacecraftIndex): plt.plot_rate(timeLineSetMin, dataOmegaBN_B, 2) plt.plot_attitude_error(timeLineSetMin, dataSigmaBR, 3) plt.plot_rate_error(timeLineSetMin, dataOmegaBR, 4) - plt.plot_rw_motor_torque(timeLineSetMin, dataUsReq, dataRW, DynModels[spacecraftIndex].numRW, 5) - plt.plot_rw_speeds(timeLineSetMin, dataOmegaRW, DynModels[spacecraftIndex].numRW, 6) + plt.plot_rw_motor_torque( + timeLineSetMin, dataUsReq, dataRW, DynModels[spacecraftIndex].numRW, 5 + ) + plt.plot_rw_speeds( + timeLineSetMin, dataOmegaRW, DynModels[spacecraftIndex].numRW, 6 + ) figureList = {} if show_plots: plt.show_all_plots() else: fileName = os.path.basename(os.path.splitext(__file__)[0]) - figureNames = ["attitude", "rate", "attitude_tracking_error", "tracking_error_rate", - "rw_motor_torque", "rw_speeds"] + figureNames = [ + "attitude", + "rate", + "attitude_tracking_error", + "tracking_error_rate", + "rw_motor_torque", + "rw_speeds", + ] figureList = plt.save_all_plots(fileName, figureNames) # close the plots being saved off to avoid over-writing old and new figures @@ -283,6 +343,7 @@ def pull_outputs(self, show_plots, spacecraftIndex): return figureList + def runScenario(scenario): # Configure FSW mode scenario.FSWModels[0].modeRequest = "sunPointing" @@ -293,16 +354,17 @@ def runScenario(scenario): scenario.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.hour2nano(1.) + simulationTime = macros.hour2nano(1.0) scenario.ConfigureStopTime(simulationTime) scenario.ExecuteSimulation() # change flight mode on selected spacecraft scenario.FSWModels[1].modeRequest = "sunPointing" scenario.FSWModels[2].modeRequest = "inertialPointing" - scenario.ConfigureStopTime(2*simulationTime) + scenario.ConfigureStopTime(2 * simulationTime) scenario.ExecuteSimulation() + def run(show_plots, numberSpacecraft): """ The scenarios can be run with the followings setups parameters: @@ -322,6 +384,4 @@ def run(show_plots, numberSpacecraft): if __name__ == "__main__": - run(show_plots=True, - numberSpacecraft=3 - ) + run(show_plots=True, numberSpacecraft=3) diff --git a/examples/MultiSatBskSim/scenariosMultiSat/scenario_BasicOrbitMultiSat.py b/examples/MultiSatBskSim/scenariosMultiSat/scenario_BasicOrbitMultiSat.py index 0bbbe86c90..9e8bf5a206 100644 --- a/examples/MultiSatBskSim/scenariosMultiSat/scenario_BasicOrbitMultiSat.py +++ b/examples/MultiSatBskSim/scenariosMultiSat/scenario_BasicOrbitMultiSat.py @@ -126,12 +126,14 @@ class will be used in other scenarios that make use of those control surfaces (s """ import copy + # Get current file path import inspect import os import sys from Basilisk.architecture import messaging + # Import utilities from Basilisk.utilities import orbitalMotion, macros, vizSupport @@ -139,20 +141,23 @@ class will be used in other scenarios that make use of those control surfaces (s path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../') -sys.path.append(path + '/../modelsMultiSat') -sys.path.append(path + '/../plottingMultiSat') +sys.path.append(path + "/../") +sys.path.append(path + "/../modelsMultiSat") +sys.path.append(path + "/../plottingMultiSat") from BSK_MultiSatMasters import BSKSim, BSKScenario import BSK_EnvironmentEarth, BSK_EnvironmentMercury, BSK_MultiSatDynamics # Import plotting files for your scenario import BSK_MultiSatPlotting as plt + # Create your own scenario child class class scenario_BasicOrbitFormationFlying(BSKSim, BSKScenario): def __init__(self, numberSpacecraft, environment): - super(scenario_BasicOrbitFormationFlying, self).__init__(numberSpacecraft, fswRate=10, dynRate=10, envRate=10) - self.name = 'scenario_BasicOrbitFormationFlying' + super(scenario_BasicOrbitFormationFlying, self).__init__( + numberSpacecraft, fswRate=10, dynRate=10, envRate=10 + ) + self.name = "scenario_BasicOrbitFormationFlying" if environment == "Earth": self.set_EnvModel(BSK_EnvironmentEarth) @@ -162,7 +167,7 @@ def __init__(self, numberSpacecraft, environment): # Here we are setting the list of spacecraft dynamics to be a homogenous formation. # To use a heterogeneous spacecraft formation, this list can contain different classes # of type BSKDynamicModels - self.set_DynModel([BSK_MultiSatDynamics]*numberSpacecraft) + self.set_DynModel([BSK_MultiSatDynamics] * numberSpacecraft) # declare empty class variables self.samplingTime = [] @@ -188,7 +193,9 @@ def __init__(self, numberSpacecraft, environment): batteryPanel = vizSupport.vizInterface.GenericStorage() batteryPanel.label = "Battery" batteryPanel.units = "Ws" - batteryPanel.color = vizSupport.vizInterface.IntVector(vizSupport.toRGBA255("red") + vizSupport.toRGBA255("green")) + batteryPanel.color = vizSupport.vizInterface.IntVector( + vizSupport.toRGBA255("red") + vizSupport.toRGBA255("green") + ) batteryPanel.thresholds = vizSupport.vizInterface.IntVector([20]) batteryInMsg = messaging.PowerStorageStatusMsgReader() batteryInMsg.subscribeTo(self.DynModels[0].powerMonitor.batPowerOutMsg) @@ -197,22 +204,29 @@ def __init__(self, numberSpacecraft, environment): tankPanel = vizSupport.vizInterface.GenericStorage() tankPanel.label = "Tank" tankPanel.units = "kg" - tankPanel.color = vizSupport.vizInterface.IntVector(vizSupport.toRGBA255("cyan")) + tankPanel.color = vizSupport.vizInterface.IntVector( + vizSupport.toRGBA255("cyan") + ) tankInMsg = messaging.FuelTankMsgReader() - tankInMsg.subscribeTo(self.DynModels[0].fuelTankStateEffector.fuelTankOutMsg) + tankInMsg.subscribeTo( + self.DynModels[0].fuelTankStateEffector.fuelTankOutMsg + ) tankPanel.fuelTankStateInMsg = tankInMsg # Add this line to maintain Python references self.vizPanels = [batteryPanel, tankPanel] - storageList = [None]*self.numberSpacecraft + storageList = [None] * self.numberSpacecraft storageList[0] = [batteryPanel, tankPanel] - viz = vizSupport.enableUnityVisualization(self, self.DynModels[0].taskName, DynModelsList - # , saveFile=__file__ - , rwEffectorList=rwStateEffectorList - , genericStorageList=storageList - ) + viz = vizSupport.enableUnityVisualization( + self, + self.DynModels[0].taskName, + DynModelsList, + # , saveFile=__file__ + rwEffectorList=rwStateEffectorList, + genericStorageList=storageList, + ) vizSupport.setInstrumentGuiSetting(viz, showGenericStoragePanel=True) def configure_initial_conditions(self): @@ -232,7 +246,11 @@ def configure_initial_conditions(self): DynModels[0].scObject.hub.r_CN_NInit = rN # m - r_CN_N DynModels[0].scObject.hub.v_CN_NInit = vN # m/s - v_CN_N DynModels[0].scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels[0].scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + DynModels[0].scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B # Configure Dynamics initial conditions self.oe.append(copy.deepcopy(self.oe[0])) @@ -244,7 +262,11 @@ def configure_initial_conditions(self): DynModels[1].scObject.hub.r_CN_NInit = rN2 # m - r_CN_N DynModels[1].scObject.hub.v_CN_NInit = vN2 # m/s - v_CN_N DynModels[1].scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels[1].scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + DynModels[1].scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B # Configure Dynamics initial conditions self.oe.append(copy.deepcopy(self.oe[0])) @@ -256,7 +278,11 @@ def configure_initial_conditions(self): DynModels[2].scObject.hub.r_CN_NInit = rN3 # m - r_CN_N DynModels[2].scObject.hub.v_CN_NInit = vN3 # m/s - v_CN_N DynModels[2].scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels[2].scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + DynModels[2].scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B def log_outputs(self): # Process outputs @@ -267,22 +293,44 @@ def log_outputs(self): # Loop through every spacecraft for spacecraft in range(self.numberSpacecraft): - # log the navigation messages - self.snTransLog.append(DynModels[spacecraft].simpleNavObject.transOutMsg.recorder(self.samplingTime)) - self.snAttLog.append(DynModels[spacecraft].simpleNavObject.attOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.snTransLog[spacecraft]) - self.AddModelToTask(DynModels[spacecraft].taskName, self.snAttLog[spacecraft]) + self.snTransLog.append( + DynModels[spacecraft].simpleNavObject.transOutMsg.recorder( + self.samplingTime + ) + ) + self.snAttLog.append( + DynModels[spacecraft].simpleNavObject.attOutMsg.recorder( + self.samplingTime + ) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.snTransLog[spacecraft] + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.snAttLog[spacecraft] + ) # log the RW wheel speed information - self.rwSpeedLog.append(DynModels[spacecraft].rwStateEffector.rwSpeedOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.rwSpeedLog[spacecraft]) + self.rwSpeedLog.append( + DynModels[spacecraft].rwStateEffector.rwSpeedOutMsg.recorder( + self.samplingTime + ) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.rwSpeedLog[spacecraft] + ) # log addition RW information (power, etc) for item in range(DynModels[spacecraft].numRW): self.rwLogs[spacecraft].append( - DynModels[spacecraft].rwStateEffector.rwOutMsgs[item].recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.rwLogs[spacecraft][item]) + DynModels[spacecraft] + .rwStateEffector.rwOutMsgs[item] + .recorder(self.samplingTime) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.rwLogs[spacecraft][item] + ) def pull_outputs(self, show_plots): # @@ -346,7 +394,8 @@ def run(show_plots, numberSpacecraft, environment): if __name__ == "__main__": - run(show_plots=True, + run( + show_plots=True, numberSpacecraft=3, - environment="Earth" # Earth or Mercury - ) + environment="Earth", # Earth or Mercury + ) diff --git a/examples/MultiSatBskSim/scenariosMultiSat/scenario_BasicOrbitMultiSat_MT.py b/examples/MultiSatBskSim/scenariosMultiSat/scenario_BasicOrbitMultiSat_MT.py index 826a81fca5..b47e859c0e 100644 --- a/examples/MultiSatBskSim/scenariosMultiSat/scenario_BasicOrbitMultiSat_MT.py +++ b/examples/MultiSatBskSim/scenariosMultiSat/scenario_BasicOrbitMultiSat_MT.py @@ -61,6 +61,7 @@ import sys from Basilisk.architecture import messaging + # Import utilities from Basilisk.utilities import orbitalMotion, macros, vizSupport @@ -68,9 +69,9 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../') -sys.path.append(path + '/../modelsMultiSat') -sys.path.append(path + '/../plottingMultiSat') +sys.path.append(path + "/../") +sys.path.append(path + "/../modelsMultiSat") +sys.path.append(path + "/../plottingMultiSat") from BSK_MultiSatMasters import BSKSim, BSKScenario import BSK_EnvironmentEarth, BSK_EnvironmentMercury, BSK_MultiSatDynamics @@ -81,8 +82,10 @@ # Create your own scenario child class class scenario_BasicOrbitFormationFlying(BSKSim, BSKScenario): def __init__(self, numberSpacecraft, environment): - super(scenario_BasicOrbitFormationFlying, self).__init__(numberSpacecraft, fswRate=10, dynRate=10, envRate=10) - self.name = 'scenario_BasicOrbitFormationFlying' + super(scenario_BasicOrbitFormationFlying, self).__init__( + numberSpacecraft, fswRate=10, dynRate=10, envRate=10 + ) + self.name = "scenario_BasicOrbitFormationFlying" if environment == "Earth": self.set_EnvModel(BSK_EnvironmentEarth) @@ -119,7 +122,8 @@ def __init__(self, numberSpacecraft, environment): batteryPanel.label = "Battery" batteryPanel.units = "Ws" batteryPanel.color = vizSupport.vizInterface.IntVector( - vizSupport.toRGBA255("red") + vizSupport.toRGBA255("green")) + vizSupport.toRGBA255("red") + vizSupport.toRGBA255("green") + ) batteryPanel.thresholds = vizSupport.vizInterface.IntVector([20]) batteryInMsg = messaging.PowerStorageStatusMsgReader() batteryInMsg.subscribeTo(self.DynModels[0].powerMonitor.batPowerOutMsg) @@ -128,9 +132,13 @@ def __init__(self, numberSpacecraft, environment): tankPanel = vizSupport.vizInterface.GenericStorage() tankPanel.label = "Tank" tankPanel.units = "kg" - tankPanel.color = vizSupport.vizInterface.IntVector(vizSupport.toRGBA255("cyan")) + tankPanel.color = vizSupport.vizInterface.IntVector( + vizSupport.toRGBA255("cyan") + ) tankInMsg = messaging.FuelTankMsgReader() - tankInMsg.subscribeTo(self.DynModels[0].fuelTankStateEffector.fuelTankOutMsg) + tankInMsg.subscribeTo( + self.DynModels[0].fuelTankStateEffector.fuelTankOutMsg + ) tankPanel.fuelTankStateInMsg = tankInMsg # Add this line to maintain Python references @@ -139,11 +147,14 @@ def __init__(self, numberSpacecraft, environment): storageList = [None] * self.numberSpacecraft storageList[0] = [batteryPanel, tankPanel] - viz = vizSupport.enableUnityVisualization(self, self.DynModels[0].taskName, DynModelsList - # , saveFile=__file__ - , rwEffectorList=rwStateEffectorList - , genericStorageList=storageList - ) + viz = vizSupport.enableUnityVisualization( + self, + self.DynModels[0].taskName, + DynModelsList, + # , saveFile=__file__ + rwEffectorList=rwStateEffectorList, + genericStorageList=storageList, + ) vizSupport.setInstrumentGuiSetting(viz, showGenericStoragePanel=True) def configure_initial_conditions(self): @@ -153,7 +164,7 @@ def configure_initial_conditions(self): # Configure Dynamics initial conditions for i in range(self.numberSpacecraft): self.oe.append(orbitalMotion.ClassicElements()) - self.oe[i].a = 1.1 * EnvModel.planetRadius + 1E5 * (i + 1) # meters + self.oe[i].a = 1.1 * EnvModel.planetRadius + 1e5 * (i + 1) # meters self.oe[i].e = 0.01 + 0.001 * (i) self.oe[i].i = 45.0 * macros.D2R self.oe[i].Omega = (48.2 + 5.0 * i) * macros.D2R @@ -163,8 +174,16 @@ def configure_initial_conditions(self): orbitalMotion.rv2elem(EnvModel.mu, rN, vN) DynModels[i].scObject.hub.r_CN_NInit = rN # m - r_CN_N DynModels[i].scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - DynModels[i].scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B - DynModels[0].scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + DynModels[i].scObject.hub.sigma_BNInit = [ + [0.1], + [0.2], + [-0.3], + ] # sigma_BN_B + DynModels[0].scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B def log_outputs(self): # Process outputs @@ -175,22 +194,44 @@ def log_outputs(self): # Loop through every spacecraft for spacecraft in range(self.numberSpacecraft): - # log the navigation messages - self.snTransLog.append(DynModels[spacecraft].simpleNavObject.transOutMsg.recorder(self.samplingTime)) - self.snAttLog.append(DynModels[spacecraft].simpleNavObject.attOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.snTransLog[spacecraft]) - self.AddModelToTask(DynModels[spacecraft].taskName, self.snAttLog[spacecraft]) + self.snTransLog.append( + DynModels[spacecraft].simpleNavObject.transOutMsg.recorder( + self.samplingTime + ) + ) + self.snAttLog.append( + DynModels[spacecraft].simpleNavObject.attOutMsg.recorder( + self.samplingTime + ) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.snTransLog[spacecraft] + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.snAttLog[spacecraft] + ) # log the RW wheel speed information - self.rwSpeedLog.append(DynModels[spacecraft].rwStateEffector.rwSpeedOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.rwSpeedLog[spacecraft]) + self.rwSpeedLog.append( + DynModels[spacecraft].rwStateEffector.rwSpeedOutMsg.recorder( + self.samplingTime + ) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.rwSpeedLog[spacecraft] + ) # log addition RW information (power, etc) for item in range(DynModels[spacecraft].numRW): self.rwLogs[spacecraft].append( - DynModels[spacecraft].rwStateEffector.rwOutMsgs[item].recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.rwLogs[spacecraft][item]) + DynModels[spacecraft] + .rwStateEffector.rwOutMsgs[item] + .recorder(self.samplingTime) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.rwLogs[spacecraft][item] + ) def pull_outputs(self, show_plots): # @@ -260,8 +301,9 @@ def run(show_plots, numberSpacecraft, environment, numThreads): if __name__ == "__main__": - run(show_plots=True, + run( + show_plots=True, numberSpacecraft=32, environment="Earth", # Earth or Mercury - numThreads=4 - ) + numThreads=4, + ) diff --git a/examples/MultiSatBskSim/scenariosMultiSat/scenario_StationKeepingMultiSat.py b/examples/MultiSatBskSim/scenariosMultiSat/scenario_StationKeepingMultiSat.py index cbfc9f0092..f4b6a00dae 100644 --- a/examples/MultiSatBskSim/scenariosMultiSat/scenario_StationKeepingMultiSat.py +++ b/examples/MultiSatBskSim/scenariosMultiSat/scenario_StationKeepingMultiSat.py @@ -136,6 +136,7 @@ """ import copy + # Get current file path import inspect import math @@ -144,16 +145,22 @@ import numpy as np from Basilisk.architecture import messaging + # Import utilities -from Basilisk.utilities import orbitalMotion, macros, vizSupport, RigidBodyKinematics as rbk +from Basilisk.utilities import ( + orbitalMotion, + macros, + vizSupport, + RigidBodyKinematics as rbk, +) filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../') -sys.path.append(path + '/../modelsMultiSat') -sys.path.append(path + '/../plottingMultiSat') +sys.path.append(path + "/../") +sys.path.append(path + "/../modelsMultiSat") +sys.path.append(path + "/../plottingMultiSat") from BSK_MultiSatMasters import BSKSim, BSKScenario import BSK_EnvironmentEarth import BSK_MultiSatDynamics @@ -162,12 +169,19 @@ # Import plotting files for your scenario import BSK_MultiSatPlotting as plt + # Create your own scenario child class class scenario_StationKeepingFormationFlying(BSKSim, BSKScenario): def __init__(self, numberSpacecraft, relativeNavigation): super(scenario_StationKeepingFormationFlying, self).__init__( - numberSpacecraft, relativeNavigation=relativeNavigation, fswRate=1, dynRate=1, envRate=1, relNavRate=1) - self.name = 'scenario_StationKeepingFormationFlying' + numberSpacecraft, + relativeNavigation=relativeNavigation, + fswRate=1, + dynRate=1, + envRate=1, + relNavRate=1, + ) + self.name = "scenario_StationKeepingFormationFlying" # Connect the environment, dynamics and FSW classes. It is crucial that these are set in the order specified, as # some connects made imply that some modules already exist @@ -207,7 +221,9 @@ def __init__(self, numberSpacecraft, relativeNavigation): for i in range(self.numberSpacecraft): DynModelsList.append(self.DynModels[i].scObject) rwStateEffectorList.append(self.DynModels[i].rwStateEffector) - thDynamicEffectorList.append([self.DynModels[i].thrusterDynamicEffector]) + thDynamicEffectorList.append( + [self.DynModels[i].thrusterDynamicEffector] + ) gsList = [] # Initialize the vizPanels list before the loop @@ -216,7 +232,9 @@ def __init__(self, numberSpacecraft, relativeNavigation): batteryPanel = vizSupport.vizInterface.GenericStorage() batteryPanel.label = "Battery" batteryPanel.units = "Ws" - batteryPanel.color = vizSupport.vizInterface.IntVector(vizSupport.toRGBA255("red") + vizSupport.toRGBA255("lightgreen")) + batteryPanel.color = vizSupport.vizInterface.IntVector( + vizSupport.toRGBA255("red") + vizSupport.toRGBA255("lightgreen") + ) batteryPanel.thresholds = vizSupport.vizInterface.IntVector([20]) batteryInMsg = messaging.PowerStorageStatusMsgReader() batteryInMsg.subscribeTo(self.DynModels[i].powerMonitor.batPowerOutMsg) @@ -225,9 +243,13 @@ def __init__(self, numberSpacecraft, relativeNavigation): tankPanel = vizSupport.vizInterface.GenericStorage() tankPanel.label = "Tank" tankPanel.units = "kg" - tankPanel.color = vizSupport.vizInterface.IntVector(vizSupport.toRGBA255("cyan")) + tankPanel.color = vizSupport.vizInterface.IntVector( + vizSupport.toRGBA255("cyan") + ) tankInMsg = messaging.FuelTankMsgReader() - tankInMsg.subscribeTo(self.DynModels[i].fuelTankStateEffector.fuelTankOutMsg) + tankInMsg.subscribeTo( + self.DynModels[i].fuelTankStateEffector.fuelTankOutMsg + ) tankPanel.fuelTankStateInMsg = tankInMsg # Append panels to the class-level list @@ -235,19 +257,27 @@ def __init__(self, numberSpacecraft, relativeNavigation): self.vizPanels.append(tankPanel) gsList.append([batteryPanel, tankPanel]) - viz = vizSupport.enableUnityVisualization(self, self.DynModels[0].taskName, DynModelsList - # , saveFile=__file__ - , rwEffectorList=rwStateEffectorList - , thrEffectorList=thDynamicEffectorList - , genericStorageList=gsList - ) + viz = vizSupport.enableUnityVisualization( + self, + self.DynModels[0].taskName, + DynModelsList, + # , saveFile=__file__ + rwEffectorList=rwStateEffectorList, + thrEffectorList=thDynamicEffectorList, + genericStorageList=gsList, + ) viz.settings.showSpacecraftLabels = True viz.settings.orbitLinesOn = 2 # show osculating relative orbit trajectories viz.settings.mainCameraTarget = "sat-1" - viz.liveSettings.relativeOrbitChief = "sat-0" # set the chief for relative orbit trajectory + viz.liveSettings.relativeOrbitChief = ( + "sat-0" # set the chief for relative orbit trajectory + ) for i in range(self.numberSpacecraft): - vizSupport.setInstrumentGuiSetting(viz, spacecraftName=self.DynModels[i].scObject.ModelTag, - showGenericStoragePanel=True) + vizSupport.setInstrumentGuiSetting( + viz, + spacecraftName=self.DynModels[i].scObject.ModelTag, + showGenericStoragePanel=True, + ) def configure_initial_conditions(self): EnvModel = self.get_EnvModel() @@ -255,7 +285,7 @@ def configure_initial_conditions(self): # Configure initial conditions for spacecraft 0 self.oe.append(orbitalMotion.ClassicElements()) - self.oe[0].a = 1.4*EnvModel.planetRadius # meters + self.oe[0].a = 1.4 * EnvModel.planetRadius # meters self.oe[0].e = 0.2 self.oe[0].i = 45.0 * macros.D2R self.oe[0].Omega = 48.2 * macros.D2R @@ -266,7 +296,11 @@ def configure_initial_conditions(self): DynModels[0].scObject.hub.r_CN_NInit = rN # m - r_CN_N DynModels[0].scObject.hub.v_CN_NInit = vN # m/s - v_CN_N DynModels[0].scObject.hub.sigma_BNInit = [[0.1], [0.6], [-0.8]] # sigma_BN_B - DynModels[0].scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + DynModels[0].scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B # Configure initial conditions for spacecraft 1 self.oe.append(copy.deepcopy(self.oe[0])) @@ -276,7 +310,11 @@ def configure_initial_conditions(self): DynModels[1].scObject.hub.r_CN_NInit = rN2 # m - r_CN_N DynModels[1].scObject.hub.v_CN_NInit = vN2 # m/s - v_CN_N DynModels[1].scObject.hub.sigma_BNInit = [[0.1], [0.6], [-0.8]] # sigma_BN_B - DynModels[1].scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + DynModels[1].scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B # Configure initial conditions for spacecraft 2 self.oe.append(copy.deepcopy(self.oe[0])) @@ -286,7 +324,11 @@ def configure_initial_conditions(self): DynModels[2].scObject.hub.r_CN_NInit = rN3 # m - r_CN_N DynModels[2].scObject.hub.v_CN_NInit = vN3 # m/s - v_CN_N DynModels[2].scObject.hub.sigma_BNInit = [[0.1], [0.6], [-0.8]] # sigma_BN_B - DynModels[2].scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + DynModels[2].scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B def log_outputs(self, relativeNavigation): # Process outputs @@ -298,58 +340,126 @@ def log_outputs(self, relativeNavigation): # Log the barycentre's position and velocity if relativeNavigation: - self.chiefTransLog = self.relativeNavigationModule.transOutMsg.recorder(self.samplingTime) + self.chiefTransLog = self.relativeNavigationModule.transOutMsg.recorder( + self.samplingTime + ) self.AddModelToTask(self.relativeNavigationTaskName, self.chiefTransLog) # Loop through every spacecraft for spacecraft in range(self.numberSpacecraft): - # log the navigation messages - self.snTransLog.append(DynModels[spacecraft].simpleNavObject.transOutMsg.recorder(self.samplingTime)) - self.snAttLog.append(DynModels[spacecraft].simpleNavObject.attOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.snTransLog[spacecraft]) - self.AddModelToTask(DynModels[spacecraft].taskName, self.snAttLog[spacecraft]) + self.snTransLog.append( + DynModels[spacecraft].simpleNavObject.transOutMsg.recorder( + self.samplingTime + ) + ) + self.snAttLog.append( + DynModels[spacecraft].simpleNavObject.attOutMsg.recorder( + self.samplingTime + ) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.snTransLog[spacecraft] + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.snAttLog[spacecraft] + ) # log the reference messages - self.attRefLog.append(FswModels[spacecraft].attRefMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.attRefLog[spacecraft]) + self.attRefLog.append( + FswModels[spacecraft].attRefMsg.recorder(self.samplingTime) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.attRefLog[spacecraft] + ) # log the attitude error messages - self.attErrorLog.append(FswModels[spacecraft].attGuidMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.attErrorLog[spacecraft]) + self.attErrorLog.append( + FswModels[spacecraft].attGuidMsg.recorder(self.samplingTime) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.attErrorLog[spacecraft] + ) # log the RW torque messages self.rwMotorLog.append( - FswModels[spacecraft].rwMotorTorque.rwMotorTorqueOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.rwMotorLog[spacecraft]) + FswModels[spacecraft].rwMotorTorque.rwMotorTorqueOutMsg.recorder( + self.samplingTime + ) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.rwMotorLog[spacecraft] + ) # log the RW wheel speed information - self.rwSpeedLog.append(DynModels[spacecraft].rwStateEffector.rwSpeedOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.rwSpeedLog[spacecraft]) + self.rwSpeedLog.append( + DynModels[spacecraft].rwStateEffector.rwSpeedOutMsg.recorder( + self.samplingTime + ) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.rwSpeedLog[spacecraft] + ) # log addition RW information (power, etc) for item in range(DynModels[spacecraft].numRW): - self.rwLogs[spacecraft].append(DynModels[spacecraft].rwStateEffector.rwOutMsgs[item].recorder(self.samplingTime)) - self.rwPowerLogs[spacecraft].append(DynModels[spacecraft].rwPowerList[item].nodePowerOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.rwLogs[spacecraft][item]) - self.AddModelToTask(DynModels[spacecraft].taskName, self.rwPowerLogs[spacecraft][item]) + self.rwLogs[spacecraft].append( + DynModels[spacecraft] + .rwStateEffector.rwOutMsgs[item] + .recorder(self.samplingTime) + ) + self.rwPowerLogs[spacecraft].append( + DynModels[spacecraft] + .rwPowerList[item] + .nodePowerOutMsg.recorder(self.samplingTime) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.rwLogs[spacecraft][item] + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.rwPowerLogs[spacecraft][item] + ) # log the remaining power modules - self.spLog.append(DynModels[spacecraft].solarPanel.nodePowerOutMsg.recorder(self.samplingTime)) - self.psLog.append(DynModels[spacecraft].powerSink.nodePowerOutMsg.recorder(self.samplingTime)) - self.pmLog.append(DynModels[spacecraft].powerMonitor.batPowerOutMsg.recorder(self.samplingTime)) + self.spLog.append( + DynModels[spacecraft].solarPanel.nodePowerOutMsg.recorder( + self.samplingTime + ) + ) + self.psLog.append( + DynModels[spacecraft].powerSink.nodePowerOutMsg.recorder( + self.samplingTime + ) + ) + self.pmLog.append( + DynModels[spacecraft].powerMonitor.batPowerOutMsg.recorder( + self.samplingTime + ) + ) self.AddModelToTask(DynModels[spacecraft].taskName, self.spLog[spacecraft]) self.AddModelToTask(DynModels[spacecraft].taskName, self.psLog[spacecraft]) self.AddModelToTask(DynModels[spacecraft].taskName, self.pmLog[spacecraft]) # log fuel information - self.fuelLog.append(DynModels[spacecraft].fuelTankStateEffector.fuelTankOutMsg.recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.fuelLog[spacecraft]) + self.fuelLog.append( + DynModels[spacecraft].fuelTankStateEffector.fuelTankOutMsg.recorder( + self.samplingTime + ) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.fuelLog[spacecraft] + ) # log thruster information for item in range(DynModels[spacecraft].numThr): - self.thrLogs[spacecraft].append(DynModels[spacecraft].thrusterDynamicEffector.thrusterOutMsgs[item].recorder(self.samplingTime)) - self.AddModelToTask(DynModels[spacecraft].taskName, self.thrLogs[spacecraft][item]) + self.thrLogs[spacecraft].append( + DynModels[spacecraft] + .thrusterDynamicEffector.thrusterOutMsgs[item] + .recorder(self.samplingTime) + ) + self.AddModelToTask( + DynModels[spacecraft].taskName, self.thrLogs[spacecraft][item] + ) def pull_outputs(self, showPlots, relativeNavigation, spacecraftIndex): # Process outputs @@ -381,7 +491,9 @@ def pull_outputs(self, showPlots, relativeNavigation, spacecraftIndex): dataThrustPercentage = [] for item in range(DynModels[spacecraftIndex].numThr): dataThrust.append(self.thrLogs[spacecraftIndex][item].thrustForce_B) - dataThrustPercentage.append(self.thrLogs[spacecraftIndex][item].thrustFactor) + dataThrustPercentage.append( + self.thrLogs[spacecraftIndex][item].thrustFactor + ) # Save power info supplyData = self.spLog[spacecraftIndex].netPower @@ -422,20 +534,42 @@ def pull_outputs(self, showPlots, relativeNavigation, spacecraftIndex): dr = [] if relativeNavigation: for i in range(self.numberSpacecraft): - rd = np.array([orbitalMotion.rv2hill(dataChiefPosition[item], dataChiefVelocity[item], r_BN_N[i][item], - v_BN_N[i][item])[0] for item in range(simLength)]) + rd = np.array( + [ + orbitalMotion.rv2hill( + dataChiefPosition[item], + dataChiefVelocity[item], + r_BN_N[i][item], + v_BN_N[i][item], + )[0] + for item in range(simLength) + ] + ) dr.append(rd) else: for i in range(1, self.numberSpacecraft): - rd = np.array([orbitalMotion.rv2hill(dataChiefPosition[item], dataChiefVelocity[item], r_BN_N[i][item], - v_BN_N[i][item])[0] for item in range(simLength)]) + rd = np.array( + [ + orbitalMotion.rv2hill( + dataChiefPosition[item], + dataChiefVelocity[item], + r_BN_N[i][item], + v_BN_N[i][item], + )[0] + for item in range(simLength) + ] + ) dr.append(rd) # Compute the orbital element differences between the spacecraft and the chief oed = np.empty((simLength, 6)) for i in range(simLength): - oe_tmp = orbitalMotion.rv2elem(EnvModel.mu, dataChiefPosition[i], dataChiefVelocity[i]) - oe2_tmp = orbitalMotion.rv2elem(EnvModel.mu, r_BN_N[spacecraftIndex][i], v_BN_N[spacecraftIndex][i]) + oe_tmp = orbitalMotion.rv2elem( + EnvModel.mu, dataChiefPosition[i], dataChiefVelocity[i] + ) + oe2_tmp = orbitalMotion.rv2elem( + EnvModel.mu, r_BN_N[spacecraftIndex][i], v_BN_N[spacecraftIndex][i] + ) oed[i, 0] = (oe2_tmp.a - oe_tmp.a) / oe_tmp.a oed[i, 1] = oe2_tmp.e - oe_tmp.e oed[i, 2] = oe2_tmp.i - oe_tmp.i @@ -443,7 +577,9 @@ def pull_outputs(self, showPlots, relativeNavigation, spacecraftIndex): oed[i, 4] = oe2_tmp.omega - oe_tmp.omega E_tmp = orbitalMotion.f2E(oe_tmp.f, oe_tmp.e) E2_tmp = orbitalMotion.f2E(oe2_tmp.f, oe2_tmp.e) - oed[i, 5] = orbitalMotion.E2M(E2_tmp, oe2_tmp.e) - orbitalMotion.E2M(E_tmp, oe_tmp.e) + oed[i, 5] = orbitalMotion.E2M(E2_tmp, oe2_tmp.e) - orbitalMotion.E2M( + E_tmp, oe_tmp.e + ) for j in range(3, 6): if oed[i, j] > math.pi: oed[i, j] = oed[i, j] - 2 * math.pi @@ -451,7 +587,7 @@ def pull_outputs(self, showPlots, relativeNavigation, spacecraftIndex): oed[i, j] = oed[i, j] + 2 * math.pi # Compute the orbit period - T = 2*math.pi*math.sqrt(self.oe[spacecraftIndex].a ** 3 / EnvModel.mu) + T = 2 * math.pi * math.sqrt(self.oe[spacecraftIndex].a ** 3 / EnvModel.mu) # # Plot results @@ -464,23 +600,42 @@ def pull_outputs(self, showPlots, relativeNavigation, spacecraftIndex): plt.plot_rate_error(timeLineSetMin, dataOmegaBR, 4) plt.plot_attitude_reference(timeLineSetMin, dataSigmaRN, 5) plt.plot_rate_reference(timeLineSetMin, dataOmegaRN_B, 6) - plt.plot_rw_motor_torque(timeLineSetMin, dataUsReq, dataRW, DynModels[spacecraftIndex].numRW, 7) - plt.plot_rw_speeds(timeLineSetMin, dataOmegaRW, DynModels[spacecraftIndex].numRW, 8) + plt.plot_rw_motor_torque( + timeLineSetMin, dataUsReq, dataRW, DynModels[spacecraftIndex].numRW, 7 + ) + plt.plot_rw_speeds( + timeLineSetMin, dataOmegaRW, DynModels[spacecraftIndex].numRW, 8 + ) plt.plot_orbits(r_BN_N, self.numberSpacecraft, 9) plt.plot_relative_orbits(dr, len(dr), 10) plt.plot_orbital_element_differences(timeLineSetSec / T, oed, 11) plt.plot_power(timeLineSetMin, netData, supplyData, sinkData, 12) plt.plot_fuel(timeLineSetMin, dataFuelMass, 13) - plt.plot_thrust_percentage(timeLineSetMin, dataThrustPercentage, DynModels[spacecraftIndex].numThr, 14) + plt.plot_thrust_percentage( + timeLineSetMin, dataThrustPercentage, DynModels[spacecraftIndex].numThr, 14 + ) figureList = {} if showPlots: plt.show_all_plots() else: fileName = os.path.basename(os.path.splitext(__file__)[0]) - figureNames = ["attitude", "rate", "attitudeTrackingError", "trackingErrorRate", "attitudeReference", - "rateReference", "rwMotorTorque", "rwSpeeds", "orbits", "relativeOrbits", "oeDifferences", - "power", "fuel", "thrustPercentage"] + figureNames = [ + "attitude", + "rate", + "attitudeTrackingError", + "trackingErrorRate", + "attitudeReference", + "rateReference", + "rwMotorTorque", + "rwSpeeds", + "orbits", + "relativeOrbits", + "oeDifferences", + "power", + "fuel", + "thrustPercentage", + ] figureList = plt.save_all_plots(fileName, figureNames) # close the plots being saved off to avoid over-writing old and new figures @@ -503,12 +658,21 @@ def runScenario(scenario, relativeNavigation): if relativeNavigation: scenario.relativeNavigationModule.addSpacecraftToModel( scenario.DynModels[spacecraft].simpleNavObject.transOutMsg, - scenario.DynModels[spacecraft].simpleMassPropsObject.vehicleConfigOutMsg) - scenario.FSWModels[spacecraft].spacecraftReconfig.chiefTransInMsg.subscribeTo( - scenario.relativeNavigationModule.transOutMsg) + scenario.DynModels[ + spacecraft + ].simpleMassPropsObject.vehicleConfigOutMsg, + ) + scenario.FSWModels[ + spacecraft + ].spacecraftReconfig.chiefTransInMsg.subscribeTo( + scenario.relativeNavigationModule.transOutMsg + ) else: - scenario.FSWModels[spacecraft].spacecraftReconfig.chiefTransInMsg.subscribeTo( - scenario.DynModels[0].simpleNavObject.transOutMsg) + scenario.FSWModels[ + spacecraft + ].spacecraftReconfig.chiefTransInMsg.subscribeTo( + scenario.DynModels[0].simpleNavObject.transOutMsg + ) # Configure the relative navigation module if relativeNavigation: @@ -518,17 +682,38 @@ def runScenario(scenario, relativeNavigation): # Set up the station keeping requirements if relativeNavigation: scenario.FSWModels[0].stationKeeping = "ON" - scenario.FSWModels[0].spacecraftReconfig.targetClassicOED = [0.0000, -0.005, -0.001, 0.0000, 0.0000, 0.000] + scenario.FSWModels[0].spacecraftReconfig.targetClassicOED = [ + 0.0000, + -0.005, + -0.001, + 0.0000, + 0.0000, + 0.000, + ] scenario.FSWModels[1].stationKeeping = "ON" - scenario.FSWModels[1].spacecraftReconfig.targetClassicOED = [0.0000, 0.005, 0.0000, 0.0000, 0.0000, -0.003] + scenario.FSWModels[1].spacecraftReconfig.targetClassicOED = [ + 0.0000, + 0.005, + 0.0000, + 0.0000, + 0.0000, + -0.003, + ] scenario.FSWModels[2].stationKeeping = "ON" - scenario.FSWModels[2].spacecraftReconfig.targetClassicOED = [0.0000, 0.000, 0.001, 0.0000, 0.0000, 0.003] + scenario.FSWModels[2].spacecraftReconfig.targetClassicOED = [ + 0.0000, + 0.000, + 0.001, + 0.0000, + 0.0000, + 0.003, + ] # Initialize simulation scenario.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.hour2nano(2.) + simulationTime = macros.hour2nano(2.0) scenario.ConfigureStopTime(simulationTime) scenario.ExecuteSimulation() @@ -554,7 +739,9 @@ def run(showPlots, numberSpacecraft, relativeNavigation): """ # Configure a scenario in the base simulation - TheScenario = scenario_StationKeepingFormationFlying(numberSpacecraft, relativeNavigation) + TheScenario = scenario_StationKeepingFormationFlying( + numberSpacecraft, relativeNavigation + ) runScenario(TheScenario, relativeNavigation) figureList = TheScenario.pull_outputs(showPlots, relativeNavigation, 1) @@ -562,7 +749,4 @@ def run(showPlots, numberSpacecraft, relativeNavigation): if __name__ == "__main__": - run(showPlots=True, - numberSpacecraft=3, - relativeNavigation=False - ) + run(showPlots=True, numberSpacecraft=3, relativeNavigation=False) diff --git a/examples/OpNavScenarios/BSK_OpNav.py b/examples/OpNavScenarios/BSK_OpNav.py index 297aebc577..6084e7b257 100644 --- a/examples/OpNavScenarios/BSK_OpNav.py +++ b/examples/OpNavScenarios/BSK_OpNav.py @@ -102,7 +102,6 @@ """ - # Get current file path import inspect import os @@ -116,10 +115,10 @@ path = os.path.dirname(os.path.abspath(filename)) # Import Dynamics and FSW models -sys.path.append(path + '/modelsOpNav') +sys.path.append(path + "/modelsOpNav") # TODO : Modify the path to the viz here -appPath = '/Applications/Vizard.app/Contents/MacOS/Vizard' #If on Mac +appPath = "/Applications/Vizard.app/Contents/MacOS/Vizard" # If on Mac # appPath = './../../Applications/Vizard.app' #If on Linux @@ -127,6 +126,7 @@ class BSKSim(SimulationBaseClass.SimBaseClass): """ BSK Simulation base class for opNav scenarios """ + def __init__(self, fswRate=0.1, dynRate=0.1): self.dynRate = dynRate self.fswRate = fswRate @@ -146,24 +146,32 @@ def __init__(self, fswRate=0.1, dynRate=0.1): self.fsw_added = False def get_DynModel(self): - assert (self.dynamics_added is True), "It is mandatory to use a dynamics model as an argument" + assert self.dynamics_added is True, ( + "It is mandatory to use a dynamics model as an argument" + ) return self.DynModels def set_DynModel(self, dynModel): self.dynamics_added = True - self.DynamicsProcessName = 'DynamicsProcess' # Create simulation process name - self.dynProc = self.CreateNewProcess(self.DynamicsProcessName, 100) # Create process - self.DynModels = dynModel.BSKDynamicModels(self, self.dynRate) # Create Dynamics and FSW classes + self.DynamicsProcessName = "DynamicsProcess" # Create simulation process name + self.dynProc = self.CreateNewProcess( + self.DynamicsProcessName, 100 + ) # Create process + self.DynModels = dynModel.BSKDynamicModels( + self, self.dynRate + ) # Create Dynamics and FSW classes def get_FswModel(self): - assert (self.fsw_added is True), "A flight software model has not been added yet" + assert self.fsw_added is True, "A flight software model has not been added yet" return self.FSWModels def set_FswModel(self, fswModel): self.fsw_added = True self.FSWProcessName = "FSWProcess" # Create simulation process name self.fswProc = self.CreateNewProcess(self.FSWProcessName, 10) # Create process - self.FSWModels = fswModel.BSKFswModels(self, self.fswRate) # Create Dynamics and FSW classes + self.FSWModels = fswModel.BSKFswModels( + self, self.fswRate + ) # Create Dynamics and FSW classes class BSKScenario(object): @@ -176,38 +184,45 @@ def __init__(self, masterSim, showPlots): def configure_initial_conditions(self): """ - Developer must override this method in their BSK_Scenario derived subclass. + Developer must override this method in their BSK_Scenario derived subclass. """ pass def log_outputs(self): """ - Developer must override this method in their BSK_Scenario derived subclass. + Developer must override this method in their BSK_Scenario derived subclass. """ pass def pull_outputs(self): """ - Developer must override this method in their BSK_Scenario derived subclass. + Developer must override this method in their BSK_Scenario derived subclass. """ pass + def run_vizard(self, mode): try: self.vizard = subprocess.Popen( - [self.masterSim.vizPath, "--args", mode, "tcp://localhost:5556"], stdout=subprocess.DEVNULL) + [self.masterSim.vizPath, "--args", mode, "tcp://localhost:5556"], + stdout=subprocess.DEVNULL, + ) print("Vizard spawned with PID = " + str(self.vizard.pid)) except FileNotFoundError: print("Vizard application not found") if sys.platform != "darwin": print( - "Either download Vizard at this path %s or change appPath in BSK_OpNav.py file" % self.masterSim.vizPath) + "Either download Vizard at this path %s or change appPath in BSK_OpNav.py file" + % self.masterSim.vizPath + ) else: - print("1. Download Vizard interface \n2. Move it to Applications \n" - "3. Change only application name while initializing appPath variable in BSK_OpNav") + print( + "1. Download Vizard interface \n2. Move it to Applications \n" + "3. Change only application name while initializing appPath variable in BSK_OpNav" + ) exit(1) - def end_scenario(self): + def end_scenario(self): if self.vizard is None: print("vizard application is not launched") exit(1) diff --git a/examples/OpNavScenarios/modelsOpNav/BSK_OpNavDynamics.py b/examples/OpNavScenarios/modelsOpNav/BSK_OpNavDynamics.py index 3f4bbad0c2..4ede6a1ecc 100644 --- a/examples/OpNavScenarios/modelsOpNav/BSK_OpNavDynamics.py +++ b/examples/OpNavScenarios/modelsOpNav/BSK_OpNavDynamics.py @@ -26,17 +26,24 @@ """ - import inspect import math import os import numpy as np from Basilisk import __path__ -from Basilisk.simulation import (spacecraft, extForceTorque, simpleNav, - reactionWheelStateEffector, coarseSunSensor, eclipse, - thrusterDynamicEffector, ephemerisConverter, vizInterface, - camera) +from Basilisk.simulation import ( + spacecraft, + extForceTorque, + simpleNav, + reactionWheelStateEffector, + coarseSunSensor, + eclipse, + thrusterDynamicEffector, + ephemerisConverter, + vizInterface, + camera, +) from Basilisk.topLevelModules import pyswice from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.utilities import macros as mc @@ -48,10 +55,12 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -class BSKDynamicModels(): + +class BSKDynamicModels: """ BSK Dynamics model for the op nav simulations """ + def __init__(self, SimBase, dynRate): # define empty class variables self.cameraRate = None @@ -69,12 +78,16 @@ def __init__(self, SimBase, dynRate): self.processTasksTimeStep = mc.sec2nano(dynRate) # Create task - SimBase.dynProc.addTask(SimBase.CreateNewTask(self.taskName, self.processTasksTimeStep), 1000) - SimBase.dynProc.addTask(SimBase.CreateNewTask(self.taskCamera, mc.sec2nano(60)), 999) + SimBase.dynProc.addTask( + SimBase.CreateNewTask(self.taskName, self.processTasksTimeStep), 1000 + ) + SimBase.dynProc.addTask( + SimBase.CreateNewTask(self.taskCamera, mc.sec2nano(60)), 999 + ) # Instantiate Dyn modules as objects self.simBasePath = bskPath - self.cameraMRP_CB =[] + self.cameraMRP_CB = [] self.cameraRez = [] self.rwFactory = simIncludeRW.rwFactory() @@ -86,7 +99,9 @@ def __init__(self, SimBase, dynRate): self.eclipseObject = eclipse.Eclipse() self.CSSConstellationObject = coarseSunSensor.CSSConstellation() self.rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector() - self.thrustersDynamicEffector = thrusterDynamicEffector.ThrusterDynamicEffector() + self.thrustersDynamicEffector = ( + thrusterDynamicEffector.ThrusterDynamicEffector() + ) self.cameraMod = camera.Camera() self.cameraMod2 = camera.Camera() self.ephemObject = ephemerisConverter.EphemerisConverter() @@ -124,7 +139,12 @@ def SetCamera(self): if self.cameraMod.saveImages: if not os.path.exists(imgFolder): os.makedirs(imgFolder) - print("Saving camera ID:" + str(self.cameraMod.cameraID) + " Images to: " + self.cameraMod.saveDir) + print( + "Saving camera ID:" + + str(self.cameraMod.cameraID) + + " Images to: " + + self.cameraMod.saveDir + ) # Noise parameters # self.cameraMod.gaussian = 2 @@ -136,16 +156,21 @@ def SetCamera(self): # Camera config self.cameraRate = 60 self.cameraMod.renderRate = int(mc.sec2nano(self.cameraRate)) # in - self.cameraMRP_CB = [0., 0., 0.] # Arbitrary camera orientation + self.cameraMRP_CB = [0.0, 0.0, 0.0] # Arbitrary camera orientation self.cameraMod.sigma_CB = self.cameraMRP_CB - self.cameraMod.cameraPos_B = [0., 0.2, 2.2] # in meters + self.cameraMod.cameraPos_B = [0.0, 0.2, 2.2] # in meters self.cameraRez = [512, 512] # [1024,1024] # in pixels - self.cameraSize = [10.*1E-3, self.cameraRez[1]/self.cameraRez[0]*10.*1E-3] # in m + self.cameraSize = [ + 10.0 * 1e-3, + self.cameraRez[1] / self.cameraRez[0] * 10.0 * 1e-3, + ] # in m self.cameraMod.resolution = self.cameraRez self.cameraMod.fieldOfView = np.deg2rad(55) self.cameraMod.parentName = self.scObject.ModelTag - self.cameraMod.skyBox = 'black' - self.cameraFocal = self.cameraSize[1]/2./np.tan(self.cameraMod.fieldOfView/2.) # in m + self.cameraMod.skyBox = "black" + self.cameraFocal = ( + self.cameraSize[1] / 2.0 / np.tan(self.cameraMod.fieldOfView / 2.0) + ) # in m def SetCamera2(self): # this 2nd camera is setup, but not used in the FSW image processing @@ -163,25 +188,32 @@ def SetCamera2(self): if self.cameraMod2.saveImages: if not os.path.exists(imgFolder): os.makedirs(imgFolder) - print("Saving camera ID:" + str(self.cameraMod2.cameraID) + " Images to: " + self.cameraMod2.saveDir) + print( + "Saving camera ID:" + + str(self.cameraMod2.cameraID) + + " Images to: " + + self.cameraMod2.saveDir + ) self.cameraMod2.blurParam = 3 # Camera config self.cameraMod2.renderRate = int(mc.sec2nano(self.cameraRate)) # in - self.cameraMod2.sigma_CB = [0., 0.5, 0.] - self.cameraMod2.cameraPos_B = [0., 0.2, 2.2] # in meters + self.cameraMod2.sigma_CB = [0.0, 0.5, 0.0] + self.cameraMod2.cameraPos_B = [0.0, 0.2, 2.2] # in meters self.cameraMod2.resolution = self.cameraRez self.cameraMod2.fieldOfView = np.deg2rad(55) self.cameraMod2.parentName = self.scObject.ModelTag - self.cameraMod2.skyBox = 'black' + self.cameraMod2.skyBox = "black" def SetVizInterface(self, SimBase): self.vizInterface = vizSupport.enableUnityVisualization( - SimBase, self.taskName, [self.scObject] + SimBase, + self.taskName, + [self.scObject], # , saveFile=__file__ - , rwEffectorList=[self.rwStateEffector] - ) + rwEffectorList=[self.rwStateEffector], + ) # setup OpNav behavior by connecting camera module config message self.vizInterface.addCamMsgToModule(self.cameraMod.cameraConfigOutMsg) self.vizInterface.addCamMsgToModule(self.cameraMod2.cameraConfigOutMsg) @@ -192,11 +224,13 @@ def SetVizInterface(self, SimBase): def SetSpacecraftHub(self): self.scObject.ModelTag = "bskSat" # -- Crate a new variable for the sim sc inertia I_sc. Note: this is currently accessed from FSWClass - self.I_sc = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + self.I_sc = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] self.scObject.hub.mHub = 750.0 # kg - spacecraft mass - self.scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + self.scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM self.scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(self.I_sc) def SetGravityEffector(self): @@ -205,33 +239,48 @@ def SetGravityEffector(self): """ timeInitString = "2019 DECEMBER 12 18:00:00.0" - gravBodies = self.gravFactory.createBodies(['sun', 'earth', 'mars barycenter', 'jupiter barycenter']) - gravBodies['mars barycenter'].isCentralBody = True + gravBodies = self.gravFactory.createBodies( + ["sun", "earth", "mars barycenter", "jupiter barycenter"] + ) + gravBodies["mars barycenter"].isCentralBody = True self.sun = 0 self.earth = 1 self.mars = 2 self.jupiter = 3 - gravBodies['mars barycenter'].useSphericalHarmonicsGravityModel( - bskPath + '/supportData/LocalGravData/GGM2BData.txt', 2) + gravBodies["mars barycenter"].useSphericalHarmonicsGravityModel( + bskPath + "/supportData/LocalGravData/GGM2BData.txt", 2 + ) self.gravFactory.addBodiesTo(self.scObject) - self.gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/', - timeInitString, - epochInMsg=True) + self.gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", timeInitString, epochInMsg=True + ) self.gravFactory.spiceObject.referenceBase = "J2000" - self.gravFactory.spiceObject.zeroBase = 'mars barycenter' - - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'de430.bsp') # solar system bodies - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'naif0012.tls') # leap second file - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'de-403-masses.tpc') # solar system masses - pyswice.furnsh_c(self.gravFactory.spiceObject.SPICEDataPath + 'pck00010.tpc') # generic Planetary Constants + self.gravFactory.spiceObject.zeroBase = "mars barycenter" + + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "de430.bsp" + ) # solar system bodies + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "naif0012.tls" + ) # leap second file + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "de-403-masses.tpc" + ) # solar system masses + pyswice.furnsh_c( + self.gravFactory.spiceObject.SPICEDataPath + "pck00010.tpc" + ) # generic Planetary Constants def SetEclipseObject(self): - self.eclipseObject.sunInMsg.subscribeTo(self.gravFactory.spiceObject.planetStateOutMsgs[self.sun]) + self.eclipseObject.sunInMsg.subscribeTo( + self.gravFactory.spiceObject.planetStateOutMsgs[self.sun] + ) for c in range(1, len(self.gravFactory.spiceObject.planetStateOutMsgs)): - self.eclipseObject.addPlanetToModel(self.gravFactory.spiceObject.planetStateOutMsgs[c]) + self.eclipseObject.addPlanetToModel( + self.gravFactory.spiceObject.planetStateOutMsgs[c] + ) self.eclipseObject.addSpacecraftToModel(self.scObject.scStateOutMsg) def SetExternalForceTorqueObject(self): @@ -247,14 +296,47 @@ def SetSimpleNavObject(self): sunSig = 0.1 * math.pi / 180.0 DVsig = 0.005 PMatrix = np.diag( - [posSigma, posSigma, posSigma, velSigma, velSigma, velSigma, attSigma, attSigma, attSigma, attRateSig, - attRateSig, attRateSig, sunSig, sunSig, sunSig, DVsig, DVsig, DVsig]) - errorBounds = [100000.0, 100000.0, 100000.0, # Position - 0.1, 0.1, 0.1, # Velocity - 1E-18 * math.pi / 180.0, 1E-18 * math.pi / 180.0, 1E-18 * math.pi / 180.0, # Attitude - 1E-18 * math.pi / 180.0, 1E-18 * math.pi / 180.0, 1E-18 * math.pi / 180.0, # Attitude Rate - 5.0 * math.pi / 180.0, 5.0 * math.pi / 180.0, 5.0 * math.pi / 180.0, # Sun vector - 0.5, 0.5, 0.5] # Accumulated DV + [ + posSigma, + posSigma, + posSigma, + velSigma, + velSigma, + velSigma, + attSigma, + attSigma, + attSigma, + attRateSig, + attRateSig, + attRateSig, + sunSig, + sunSig, + sunSig, + DVsig, + DVsig, + DVsig, + ] + ) + errorBounds = [ + 100000.0, + 100000.0, + 100000.0, # Position + 0.1, + 0.1, + 0.1, # Velocity + 1e-18 * math.pi / 180.0, + 1e-18 * math.pi / 180.0, + 1e-18 * math.pi / 180.0, # Attitude + 1e-18 * math.pi / 180.0, + 1e-18 * math.pi / 180.0, + 1e-18 * math.pi / 180.0, # Attitude Rate + 5.0 * math.pi / 180.0, + 5.0 * math.pi / 180.0, + 5.0 * math.pi / 180.0, # Sun vector + 0.5, + 0.5, + 0.5, + ] # Accumulated DV # PMatrix = np.zeros_like(np.eye(18)) # errorBounds = [0.0] * 18 # Accumulated DV self.SimpleNavObject.walkBounds = np.array(errorBounds) @@ -267,26 +349,30 @@ def SetSimpleNavObject(self): def SetReactionWheelDynEffector(self): """Set the 4 reaction wheel devices.""" # specify RW momentum capacity - maxRWMomentum = 50. # Nms + maxRWMomentum = 50.0 # Nms # Define orthogonal RW pyramid # -- Pointing directions - rwElAngle = np.array([40.0, 40.0, 40.0, 40.0])*mc.D2R - rwAzimuthAngle = np.array([45.0, 135.0, 225.0, 315.0])*mc.D2R - rwPosVector = [[0.8, 0.8, 1.79070], - [0.8, -0.8, 1.79070], - [-0.8, -0.8, 1.79070], - [-0.8, 0.8, 1.79070] - ] + rwElAngle = np.array([40.0, 40.0, 40.0, 40.0]) * mc.D2R + rwAzimuthAngle = np.array([45.0, 135.0, 225.0, 315.0]) * mc.D2R + rwPosVector = [ + [0.8, 0.8, 1.79070], + [0.8, -0.8, 1.79070], + [-0.8, -0.8, 1.79070], + [-0.8, 0.8, 1.79070], + ] for elAngle, azAngle, posVector in zip(rwElAngle, rwAzimuthAngle, rwPosVector): - gsHat = (rbk.Mi(-azAngle,3).dot(rbk.Mi(elAngle,2))).dot(np.array([1,0,0])) - self.rwFactory.create('Honeywell_HR16', - gsHat, - maxMomentum=maxRWMomentum, - rWB_B=posVector) + gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot( + np.array([1, 0, 0]) + ) + self.rwFactory.create( + "Honeywell_HR16", gsHat, maxMomentum=maxRWMomentum, rWB_B=posVector + ) - self.rwFactory.addToSpacecraft("RWStateEffector", self.rwStateEffector, self.scObject) + self.rwFactory.addToSpacecraft( + "RWStateEffector", self.rwStateEffector, self.scObject + ) def SetACSThrusterStateEffector(self): # Make a fresh TH factory instance, this is critical to run multiple times @@ -294,15 +380,15 @@ def SetACSThrusterStateEffector(self): # 8 thrusters are modeled that act in pairs to provide the desired torque thPos = [ - [825.5/1000.0, 880.3/1000.0, 1765.3/1000.0], - [825.5/1000.0, 880.3/1000.0, 260.4/1000.0], - [880.3/1000.0, 825.5/1000.0, 1765.3/1000.0], - [880.3/1000.0, 825.5/1000.0, 260.4/1000.0], - [-825.5/1000.0, -880.3/1000.0, 1765.3/1000.0], - [-825.5/1000.0, -880.3/1000.0, 260.4/1000.0], - [-880.3/1000.0, -825.5/1000.0, 1765.3/1000.0], - [-880.3/1000.0, -825.5/1000.0, 260.4/1000.0] - ] + [825.5 / 1000.0, 880.3 / 1000.0, 1765.3 / 1000.0], + [825.5 / 1000.0, 880.3 / 1000.0, 260.4 / 1000.0], + [880.3 / 1000.0, 825.5 / 1000.0, 1765.3 / 1000.0], + [880.3 / 1000.0, 825.5 / 1000.0, 260.4 / 1000.0], + [-825.5 / 1000.0, -880.3 / 1000.0, 1765.3 / 1000.0], + [-825.5 / 1000.0, -880.3 / 1000.0, 260.4 / 1000.0], + [-880.3 / 1000.0, -825.5 / 1000.0, 1765.3 / 1000.0], + [-880.3 / 1000.0, -825.5 / 1000.0, 260.4 / 1000.0], + ] thDir = [ [0.0, -1.0, 0.0], [0.0, -1.0, 0.0], @@ -311,31 +397,29 @@ def SetACSThrusterStateEffector(self): [0.0, 1.0, 0.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0], - [1.0, 0.0, 0.0] + [1.0, 0.0, 0.0], ] for pos_B, dir_B in zip(thPos, thDir): - thFactory.create( - 'MOOG_Monarc_1' - , pos_B - , dir_B - ) + thFactory.create("MOOG_Monarc_1", pos_B, dir_B) # create thruster object container and tie to spacecraft object - thFactory.addToSpacecraft("Thrusters", - self.thrustersDynamicEffector, - self.scObject) + thFactory.addToSpacecraft( + "Thrusters", self.thrustersDynamicEffector, self.scObject + ) def SetCSSConstellation(self): """Set the 8 CSS sensors""" self.CSSConstellationObject.ModelTag = "cssConstellation" # Create class-level registry if it doesn't exist - if not hasattr(self, '_css_registry'): + if not hasattr(self, "_css_registry"): self._css_registry = [] def setupCSS(cssDevice): - cssDevice.fov = 80. * mc.D2R # half-angle field of view value + cssDevice.fov = 80.0 * mc.D2R # half-angle field of view value cssDevice.scaleFactor = 2.0 - cssDevice.sunInMsg.subscribeTo(self.gravFactory.spiceObject.planetStateOutMsgs[self.sun]) + cssDevice.sunInMsg.subscribeTo( + self.gravFactory.spiceObject.planetStateOutMsgs[self.sun] + ) cssDevice.stateInMsg.subscribeTo(self.scObject.scStateOutMsg) cssDevice.sunEclipseInMsg.subscribeTo(self.eclipseObject.eclipseOutMsgs[0]) # Store CSS in class-level registry @@ -344,19 +428,19 @@ def setupCSS(cssDevice): # setup CSS sensor normal vectors in body frame components nHat_B_List = [ [0.0, 0.707107, 0.707107], - [0.707107, 0., 0.707107], + [0.707107, 0.0, 0.707107], [0.0, -0.707107, 0.707107], - [-0.707107, 0., 0.707107], + [-0.707107, 0.0, 0.707107], [0.0, -0.965926, -0.258819], [-0.707107, -0.353553, -0.612372], - [0., 0.258819, -0.965926], - [0.707107, -0.353553, -0.612372] + [0.0, 0.258819, -0.965926], + [0.707107, -0.353553, -0.612372], ] numCSS = len(nHat_B_List) # store all cssList = [] - for nHat_B, i in zip(nHat_B_List, list(range(1, numCSS+1))): + for nHat_B, i in zip(nHat_B_List, list(range(1, numCSS + 1))): CSS = coarseSunSensor.CoarseSunSensor() setupCSS(CSS) CSS.ModelTag = "CSS" + str(i) @@ -368,8 +452,10 @@ def setupCSS(cssDevice): def SetEphemConvert(self): # Initialize the ephemeris module - self.ephemObject.ModelTag = 'EphemData' - self.ephemObject.addSpiceInputMsg(self.gravFactory.spiceObject.planetStateOutMsgs[self.mars]) + self.ephemObject.ModelTag = "EphemData" + self.ephemObject.addSpiceInputMsg( + self.gravFactory.spiceObject.planetStateOutMsgs[self.mars] + ) def SetSimpleGrav(self): planet = self.gravFactory.createMarsBarycenter() diff --git a/examples/OpNavScenarios/modelsOpNav/BSK_OpNavFsw.py b/examples/OpNavScenarios/modelsOpNav/BSK_OpNavFsw.py index cf51ffcf70..02a5bc7d49 100644 --- a/examples/OpNavScenarios/modelsOpNav/BSK_OpNavFsw.py +++ b/examples/OpNavScenarios/modelsOpNav/BSK_OpNavFsw.py @@ -28,7 +28,6 @@ """ - import math from pathlib import Path @@ -60,6 +59,7 @@ try: from Basilisk.fswAlgorithms import centerRadiusCNN # FSW for OpNav + centerRadiusCNNIncluded = True except ImportError: centerRadiusCNNIncluded = False @@ -74,10 +74,11 @@ def get_repo_root(start: Path = Path(__file__).resolve()) -> Path: raise RuntimeError("Repo root not found") -class BSKFswModels(): +class BSKFswModels: """ OpNav BSK FSW Models """ + def __init__(self, SimBase, fswRate): # define empty class variables self.vcMsg = None @@ -143,21 +144,51 @@ def __init__(self, SimBase, fswRate): self.InitAllFSWObjects(SimBase) # Create tasks - SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavPointTask", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("headingPointTask", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavPointLimbTask", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavAttODLimbTask", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavPointTaskCheat", self.processTasksTimeStep), 20) - SimBase.fswProc.addTask(SimBase.CreateNewTask("mrpFeedbackRWsTask", self.processTasksTimeStep), 15) - SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavODTask", self.processTasksTimeStep), 5) - SimBase.fswProc.addTask(SimBase.CreateNewTask("imageProcTask", self.processTasksTimeStep), 9) - SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavODTaskLimb", self.processTasksTimeStep), 15) - SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavODTaskB", self.processTasksTimeStep), 9) - SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavAttODTask", self.processTasksTimeStep), 9) - SimBase.fswProc.addTask(SimBase.CreateNewTask("cnnAttODTask", self.processTasksTimeStep), 9) - SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavFaultDet", self.processTasksTimeStep), 9) - SimBase.fswProc.addTask(SimBase.CreateNewTask("attODFaultDet", self.processTasksTimeStep), 9) - SimBase.fswProc.addTask(SimBase.CreateNewTask("cnnFaultDet", self.processTasksTimeStep), 9) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("opNavPointTask", self.processTasksTimeStep), 20 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("headingPointTask", self.processTasksTimeStep), 20 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("opNavPointLimbTask", self.processTasksTimeStep), 20 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("opNavAttODLimbTask", self.processTasksTimeStep), 20 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("opNavPointTaskCheat", self.processTasksTimeStep), 20 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("mrpFeedbackRWsTask", self.processTasksTimeStep), 15 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("opNavODTask", self.processTasksTimeStep), 5 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("imageProcTask", self.processTasksTimeStep), 9 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("opNavODTaskLimb", self.processTasksTimeStep), 15 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("opNavODTaskB", self.processTasksTimeStep), 9 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("opNavAttODTask", self.processTasksTimeStep), 9 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("cnnAttODTask", self.processTasksTimeStep), 9 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("opNavFaultDet", self.processTasksTimeStep), 9 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("attODFaultDet", self.processTasksTimeStep), 9 + ) + SimBase.fswProc.addTask( + SimBase.CreateNewTask("cnnFaultDet", self.processTasksTimeStep), 9 + ) SimBase.AddModelToTask("opNavPointTask", self.imageProcessing, 15) SimBase.AddModelToTask("opNavPointTask", self.pixelLine, 12) @@ -435,63 +466,109 @@ def __init__(self, SimBase, fswRate): # ------------------------------------------------------------------------------------------- # # These are module-initialization methods def SetHillPointGuidance(self, SimBase): - self.hillPoint.transNavInMsg.subscribeTo(SimBase.DynModels.SimpleNavObject.transOutMsg) - self.hillPoint.celBodyInMsg.subscribeTo(SimBase.DynModels.ephemObject.ephemOutMsgs[0]) + self.hillPoint.transNavInMsg.subscribeTo( + SimBase.DynModels.SimpleNavObject.transOutMsg + ) + self.hillPoint.celBodyInMsg.subscribeTo( + SimBase.DynModels.ephemObject.ephemOutMsgs[0] + ) def SetOpNavPointGuidance(self, SimBase): - messaging.AttGuidMsg_C_addAuthor(self.opNavPoint.attGuidanceOutMsg, self.attGuidMsg) - self.opNavPoint.imuInMsg.subscribeTo(SimBase.DynModels.SimpleNavObject.attOutMsg) - self.opNavPoint.cameraConfigInMsg.subscribeTo(SimBase.DynModels.cameraMod.cameraConfigOutMsg) + messaging.AttGuidMsg_C_addAuthor( + self.opNavPoint.attGuidanceOutMsg, self.attGuidMsg + ) + self.opNavPoint.imuInMsg.subscribeTo( + SimBase.DynModels.SimpleNavObject.attOutMsg + ) + self.opNavPoint.cameraConfigInMsg.subscribeTo( + SimBase.DynModels.cameraMod.cameraConfigOutMsg + ) self.opNavPoint.opnavDataInMsg.subscribeTo(self.opnavMsg) - self.opNavPoint.smallAngle = 0.001*np.pi/180. - self.opNavPoint.timeOut = 1000 # Max time in sec between images before engaging search + self.opNavPoint.smallAngle = 0.001 * np.pi / 180.0 + self.opNavPoint.timeOut = ( + 1000 # Max time in sec between images before engaging search + ) # self.opNavPointData.opNavAxisSpinRate = 0.1*np.pi/180. self.opNavPoint.omega_RN_B = [0.001, 0.0, -0.001] - self.opNavPoint.alignAxis_C = [0., 0., 1] + self.opNavPoint.alignAxis_C = [0.0, 0.0, 1] def SetHeadingUKF(self, SimBase): self.headingUKF.opnavDataInMsg.subscribeTo(self.opnavMsg) - self.headingUKF.cameraConfigInMsg.subscribeTo(SimBase.DynModels.cameraMod.cameraConfigOutMsg) + self.headingUKF.cameraConfigInMsg.subscribeTo( + SimBase.DynModels.cameraMod.cameraConfigOutMsg + ) self.headingUKF.alpha = 0.02 self.headingUKF.beta = 2.0 self.headingUKF.kappa = 0.0 - self.headingUKF.state = [0.0, 0., 0., 0., 0.] + self.headingUKF.state = [0.0, 0.0, 0.0, 0.0, 0.0] self.headingUKF.stateInit = [0.0, 0.0, 1.0, 0.0, 0.0] - self.headingUKF.covarInit = [0.2, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.2, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.2, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.005, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.005] + self.headingUKF.covarInit = [ + 0.2, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.2, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.2, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.005, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.005, + ] qNoiseIn = np.identity(5) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-6 * 1E-6 - qNoiseIn[3:5, 3:5] = qNoiseIn[3:5, 3:5] * 1E-6 * 1E-6 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1e-6 * 1e-6 + qNoiseIn[3:5, 3:5] = qNoiseIn[3:5, 3:5] * 1e-6 * 1e-6 self.headingUKF.qNoise = qNoiseIn.reshape(25).tolist() self.headingUKF.qObsVal = 0.001 def SetAttTrackingErrorCam(self, SimBase): self.trackingErrorCam.attRefInMsg.subscribeTo(self.hillPoint.attRefOutMsg) - self.trackingErrorCam.attNavInMsg.subscribeTo(SimBase.DynModels.SimpleNavObject.attOutMsg) - messaging.AttGuidMsg_C_addAuthor(self.trackingErrorCam.attGuidOutMsg, self.attGuidMsg) + self.trackingErrorCam.attNavInMsg.subscribeTo( + SimBase.DynModels.SimpleNavObject.attOutMsg + ) + messaging.AttGuidMsg_C_addAuthor( + self.trackingErrorCam.attGuidOutMsg, self.attGuidMsg + ) - M2 = rbk.euler2(90 * macros.D2R) #rbk.euler2(-90 * macros.D2R) # - M3 = rbk.euler1(90 * macros.D2R) #rbk.euler3(90 * macros.D2R) # + M2 = rbk.euler2(90 * macros.D2R) # rbk.euler2(-90 * macros.D2R) # + M3 = rbk.euler1(90 * macros.D2R) # rbk.euler3(90 * macros.D2R) # M_cam = rbk.MRP2C(SimBase.DynModels.cameraMRP_CB) - MRP = rbk.C2MRP(np.dot(np.dot(M3, M2), M_cam)) # This assures that the s/c does not control to the hill frame, but to a rotated frame such that the camera is pointing to the planet + MRP = rbk.C2MRP( + np.dot(np.dot(M3, M2), M_cam) + ) # This assures that the s/c does not control to the hill frame, but to a rotated frame such that the camera is pointing to the planet self.trackingErrorCam.sigma_R0R = MRP # self.trackingErrorCamData.sigma_R0R = [1./3+0.1, 1./3-0.1, 0.1-1/3] def SetMRPFeedbackRWA(self, SimBase): self.mrpFeedbackRWs.K = 3.5 - self.mrpFeedbackRWs.Ki = -1 # Note: make value negative to turn off integral feedback + self.mrpFeedbackRWs.Ki = ( + -1 + ) # Note: make value negative to turn off integral feedback self.mrpFeedbackRWs.P = 30.0 - self.mrpFeedbackRWs.integralLimit = 2. / self.mrpFeedbackRWs.Ki * 0.1 + self.mrpFeedbackRWs.integralLimit = 2.0 / self.mrpFeedbackRWs.Ki * 0.1 self.mrpFeedbackRWs.vehConfigInMsg.subscribeTo(self.vcMsg) - self.mrpFeedbackRWs.rwSpeedsInMsg.subscribeTo(SimBase.DynModels.rwStateEffector.rwSpeedOutMsg) + self.mrpFeedbackRWs.rwSpeedsInMsg.subscribeTo( + SimBase.DynModels.rwStateEffector.rwSpeedOutMsg + ) self.mrpFeedbackRWs.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) self.mrpFeedbackRWs.guidInMsg.subscribeTo(self.attGuidMsg) @@ -509,28 +586,32 @@ def SetRWConfigMsg(self): fswSetupRW.clearSetup() for elAngle, azAngle in zip(rwElAngle, rwAzimuthAngle): - gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot(np.array([1, 0, 0])) - fswSetupRW.create(gsHat, # spin axis - wheelJs, # kg*m^2 - 0.2) # Nm uMax + gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot( + np.array([1, 0, 0]) + ) + fswSetupRW.create( + gsHat, # spin axis + wheelJs, # kg*m^2 + 0.2, + ) # Nm uMax self.fswRwConfigMsg = fswSetupRW.writeConfigMessage() def SetRWMotorTorque(self, SimBase): - controlAxes_B = [ - 1.0, 0.0, 0.0 - , 0.0, 1.0, 0.0 - , 0.0, 0.0, 1.0 - ] + controlAxes_B = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] self.rwMotorTorque.controlAxes_B = controlAxes_B - self.rwMotorTorque.vehControlInMsg.subscribeTo(self.mrpFeedbackRWs.cmdTorqueOutMsg) - SimBase.DynModels.rwStateEffector.rwMotorCmdInMsg.subscribeTo(self.rwMotorTorque.rwMotorTorqueOutMsg) + self.rwMotorTorque.vehControlInMsg.subscribeTo( + self.mrpFeedbackRWs.cmdTorqueOutMsg + ) + SimBase.DynModels.rwStateEffector.rwMotorCmdInMsg.subscribeTo( + self.rwMotorTorque.rwMotorTorqueOutMsg + ) self.rwMotorTorque.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) def SetCNNOpNav(self, SimBase): self.opNavCNN.imageInMsg.subscribeTo(SimBase.DynModels.cameraMod.imageOutMsg) self.opNavCNN.opnavCirclesOutMsg = self.opnavCirclesMsg - self.opNavCNN.pixelNoise = [5,5,5] + self.opNavCNN.pixelNoise = [5, 5, 5] self.opNavCNN.pathToNetwork = str( get_repo_root() / "src" @@ -541,7 +622,9 @@ def SetCNNOpNav(self, SimBase): ) def SetImageProcessing(self, SimBase): - self.imageProcessing.imageInMsg.subscribeTo(SimBase.DynModels.cameraMod.imageOutMsg) + self.imageProcessing.imageInMsg.subscribeTo( + SimBase.DynModels.cameraMod.imageOutMsg + ) self.imageProcessing.opnavCirclesOutMsg = self.opnavCirclesMsg self.imageProcessing.saveImages = 0 @@ -553,12 +636,14 @@ def SetImageProcessing(self, SimBase): self.imageProcessing.blurrSize = 9 self.imageProcessing.noiseSF = 1 self.imageProcessing.dpValue = 1 - self.imageProcessing.saveDir = 'Test' + self.imageProcessing.saveDir = "Test" self.imageProcessing.houghMaxRadius = 0 # int(512 / 1.25) def SetPixelLineConversion(self, SimBase): self.pixelLine.circlesInMsg.subscribeTo(self.opnavCirclesMsg) - self.pixelLine.cameraConfigInMsg.subscribeTo(SimBase.DynModels.cameraMod.cameraConfigOutMsg) + self.pixelLine.cameraConfigInMsg.subscribeTo( + SimBase.DynModels.cameraMod.cameraConfigOutMsg + ) self.pixelLine.attInMsg.subscribeTo(SimBase.DynModels.SimpleNavObject.attOutMsg) self.pixelLine.planetTarget = 2 messaging.OpNavMsg_C_addAuthor(self.pixelLine.opNavOutMsg, self.opnavMsg) @@ -574,8 +659,12 @@ def SetLimbFinding(self, SimBase): def SetHorizonNav(self, SimBase): self.horizonNav.limbInMsg.subscribeTo(self.limbFinding.opnavLimbOutMsg) - self.horizonNav.cameraConfigInMsg.subscribeTo(SimBase.DynModels.cameraMod.cameraConfigOutMsg) - self.horizonNav.attInMsg.subscribeTo(SimBase.DynModels.SimpleNavObject.attOutMsg) + self.horizonNav.cameraConfigInMsg.subscribeTo( + SimBase.DynModels.cameraMod.cameraConfigOutMsg + ) + self.horizonNav.attInMsg.subscribeTo( + SimBase.DynModels.SimpleNavObject.attOutMsg + ) self.horizonNav.planetTarget = 2 self.horizonNav.noiseSF = 1 # 2 should work though messaging.OpNavMsg_C_addAuthor(self.horizonNav.opNavOutMsg, self.opnavMsg) @@ -589,42 +678,82 @@ def SetRelativeODFilter(self): self.relativeOD.kappa = 0.0 self.relativeOD.noiseSF = 7.5 - mu = 42828.314 * 1E9 # m^3/s^2 + mu = 42828.314 * 1e9 # m^3/s^2 elementsInit = orbitalMotion.ClassicElements() - elementsInit.a = 10000 * 1E3 # m + elementsInit.a = 10000 * 1e3 # m elementsInit.e = 0.2 elementsInit.i = 10 * macros.D2R - elementsInit.Omega = 25. * macros.D2R - elementsInit.omega = 10. * macros.D2R + elementsInit.Omega = 25.0 * macros.D2R + elementsInit.omega = 10.0 * macros.D2R elementsInit.f = 40 * macros.D2R r, v = orbitalMotion.elem2rv(mu, elementsInit) self.relativeOD.stateInit = r.tolist() + v.tolist() - self.relativeOD.covarInit = [1. * 1E6, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1. * 1E6, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1. * 1E6, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.02 * 1E6, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.02 * 1E6, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.02 * 1E6] + self.relativeOD.covarInit = [ + 1.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.02 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.02 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.02 * 1e6, + ] qNoiseIn = np.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1e-3 * 1e-3 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1e-4 * 1e-4 self.relativeOD.qNoise = qNoiseIn.reshape(36).tolist() def SetFaultDetection(self, SimBase): self.opNavFault.navMeasPrimaryInMsg.subscribeTo(self.opnavPrimaryMsg) self.opNavFault.navMeasSecondaryInMsg.subscribeTo(self.opnavSecondaryMsg) - self.opNavFault.cameraConfigInMsg.subscribeTo(SimBase.DynModels.cameraMod.cameraConfigOutMsg) - self.opNavFault.attInMsg.subscribeTo(SimBase.DynModels.SimpleNavObject.attOutMsg) + self.opNavFault.cameraConfigInMsg.subscribeTo( + SimBase.DynModels.cameraMod.cameraConfigOutMsg + ) + self.opNavFault.attInMsg.subscribeTo( + SimBase.DynModels.SimpleNavObject.attOutMsg + ) messaging.OpNavMsg_C_addAuthor(self.opNavFault.opNavOutMsg, self.opnavMsg) self.opNavFault.sigmaFault = 0.3 self.opNavFault.faultMode = 0 def SetPixelLineFilter(self, SimBase): self.pixelLineFilter.circlesInMsg.subscribeTo(self.opnavCirclesMsg) - self.pixelLineFilter.cameraConfigInMsg.subscribeTo(SimBase.DynModels.cameraMod.cameraConfigOutMsg) - self.pixelLineFilter.attInMsg.subscribeTo(SimBase.DynModels.SimpleNavObject.attOutMsg) + self.pixelLineFilter.cameraConfigInMsg.subscribeTo( + SimBase.DynModels.cameraMod.cameraConfigOutMsg + ) + self.pixelLineFilter.attInMsg.subscribeTo( + SimBase.DynModels.SimpleNavObject.attOutMsg + ) self.pixelLineFilter.planetIdInit = 2 self.pixelLineFilter.alpha = 0.02 @@ -632,32 +761,106 @@ def SetPixelLineFilter(self, SimBase): self.pixelLineFilter.kappa = 0.0 self.pixelLineFilter.gamma = 0.9 - mu = 42828.314 * 1E9 # m^3/s^2 + mu = 42828.314 * 1e9 # m^3/s^2 elementsInit = orbitalMotion.ClassicElements() - elementsInit.a = 10000 * 1E3 # m + elementsInit.a = 10000 * 1e3 # m elementsInit.e = 0.2 elementsInit.i = 10 * macros.D2R - elementsInit.Omega = 25. * macros.D2R - elementsInit.omega = 10. * macros.D2R + elementsInit.Omega = 25.0 * macros.D2R + elementsInit.omega = 10.0 * macros.D2R elementsInit.f = 40 * macros.D2R r, v = orbitalMotion.elem2rv(mu, elementsInit) bias = [1, 1, 2] self.pixelLineFilter.stateInit = r.tolist() + v.tolist() + bias - self.pixelLineFilter.covarInit = [10. * 1E6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 10. * 1E6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 10. * 1E6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.01 * 1E6, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.01 * 1E6, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.01 * 1E6, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1] + self.pixelLineFilter.covarInit = [ + 10.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 10.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 10.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1, + ] qNoiseIn = np.identity(9) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 - qNoiseIn[6:9, 6:9] = qNoiseIn[6:9, 6:9] * 1E-8 * 1E-8 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1e-3 * 1e-3 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1e-4 * 1e-4 + qNoiseIn[6:9, 6:9] = qNoiseIn[6:9, 6:9] * 1e-8 * 1e-8 self.pixelLineFilter.qNoise = qNoiseIn.reshape(9 * 9).tolist() # Global call to initialize every module diff --git a/examples/OpNavScenarios/plottingOpNav/OpNav_Plotting.py b/examples/OpNavScenarios/plottingOpNav/OpNav_Plotting.py index bd6882c33a..ad94d0a8d9 100644 --- a/examples/OpNavScenarios/plottingOpNav/OpNav_Plotting.py +++ b/examples/OpNavScenarios/plottingOpNav/OpNav_Plotting.py @@ -27,23 +27,30 @@ import matplotlib as mpl import matplotlib.pyplot as plt import numpy as np + # import scipy.optimize from Basilisk.utilities import macros as mc from Basilisk.utilities import unitTestSupport from matplotlib.patches import Ellipse -color_x = 'dodgerblue' -color_y = 'salmon' -color_z = 'lightgreen' +color_x = "dodgerblue" +color_y = "salmon" +color_z = "lightgreen" m2km = 1.0 / 1000.0 -ns2min = 1/60.*1E-9 +ns2min = 1 / 60.0 * 1e-9 -mpl.rcParams.update({'font.size' : 8 }) +mpl.rcParams.update({"font.size": 8}) # If a specific style for plotting wants to be used try: plt.style.use("myStyle") - params = {'axes.labelsize': 8, 'axes.titlesize': 8, 'legend.fontsize': 8, 'xtick.labelsize': 7, - 'ytick.labelsize': 7, 'text.usetex': True} + params = { + "axes.labelsize": 8, + "axes.titlesize": 8, + "legend.fontsize": 8, + "xtick.labelsize": 7, + "ytick.labelsize": 7, + "text.usetex": True, + } mpl.rcParams.update(params) except: pass @@ -68,136 +75,185 @@ # fitfunc = lambda t: A * np.sin(w*t + p) + c # return {"amp": A, "omega": w, "phase": p, "offset": c, "freq": f, "period": 1./f, "fitfunc": fitfunc, "maxcov": np.max(pcov), "rawres": (guess,popt,pcov)} + def show_all_plots(): plt.show() + def clear_all_plots(): plt.close("all") + def omegaTrack(rError, covar): - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) t = rError[:, 0] * ns2min - plt.figure(num=2210101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , rError[:, 1]*180./np.pi, color = colorList[2] , label= 'Error') - plt.plot(t, 3*np.sqrt(covar[:, 0,0])*180./np.pi, color=colorList[8], linestyle = '--', label=r'Covar (3-$\sigma$)') - plt.plot(t, -3*np.sqrt(covar[:, 0,0])*180./np.pi, color=colorList[8], linestyle = '--') - plt.legend(loc='best') - plt.ylabel(r'$\mathbf{\omega}_{' + str(2) +r'}$ Error in $\mathcal{C}$ ($^\circ$/s)') - plt.ylim([-0.07,0.07]) - plt.xlabel('Time (min)') + plt.figure(num=2210101, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, rError[:, 1] * 180.0 / np.pi, color=colorList[2], label="Error") + plt.plot( + t, + 3 * np.sqrt(covar[:, 0, 0]) * 180.0 / np.pi, + color=colorList[8], + linestyle="--", + label=r"Covar (3-$\sigma$)", + ) + plt.plot( + t, + -3 * np.sqrt(covar[:, 0, 0]) * 180.0 / np.pi, + color=colorList[8], + linestyle="--", + ) + plt.legend(loc="best") + plt.ylabel( + r"$\mathbf{\omega}_{" + str(2) + r"}$ Error in $\mathcal{C}$ ($^\circ$/s)" + ) + plt.ylim([-0.07, 0.07]) + plt.xlabel("Time (min)") # plt.savefig('RateCam1.pdf') - plt.figure(num=2210201, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , rError[:, 2]*180./np.pi, color = colorList[2]) - plt.plot(t, 3*np.sqrt(covar[:, 1,1])*180./np.pi, color=colorList[8], linestyle = '--') - plt.plot(t, -3*np.sqrt(covar[:, 1,1])*180./np.pi, color=colorList[8], linestyle = '--') - plt.ylabel(r'$\mathbf{\omega}_{' + str(3) +r'}$ Error in $\mathcal{C}$ ($^\circ$/s)') - plt.ylim([-0.07,0.07]) - plt.xlabel('Time (min)') + plt.figure(num=2210201, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, rError[:, 2] * 180.0 / np.pi, color=colorList[2]) + plt.plot( + t, + 3 * np.sqrt(covar[:, 1, 1]) * 180.0 / np.pi, + color=colorList[8], + linestyle="--", + ) + plt.plot( + t, + -3 * np.sqrt(covar[:, 1, 1]) * 180.0 / np.pi, + color=colorList[8], + linestyle="--", + ) + plt.ylabel( + r"$\mathbf{\omega}_{" + str(3) + r"}$ Error in $\mathcal{C}$ ($^\circ$/s)" + ) + plt.ylim([-0.07, 0.07]) + plt.xlabel("Time (min)") # plt.savefig('RateCam2.pdf') - def vecTrack(ref, track, covar): - - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) rError = np.copy(ref) - rError[:,1:] -= track[:,1:] + rError[:, 1:] -= track[:, 1:] errorDeg = np.zeros([len(rError[:, 0]), 2]) covDeg = np.zeros([len(rError[:, 0]), 2]) for i in range(len(errorDeg[:, 0])): errorDeg[i, 0] = rError[i, 0] covDeg[i, 0] = rError[i, 0] - errorDeg[i, 1] = np.arccos(np.dot(ref[i, 1:4], track[i, 1:4]))*180./np.pi + errorDeg[i, 1] = np.arccos(np.dot(ref[i, 1:4], track[i, 1:4])) * 180.0 / np.pi covDeg[i, 1] = np.arccos(np.dot(ref[i, 1:4], track[i, 1:4])) covarVec = np.array( - [track[i, 1] + np.sqrt(covar[i, 0,0]), track[i, 2] + np.sqrt(covar[i, 1,1]), - track[i, 3] + np.sqrt(covar[i, 2,2])]) + [ + track[i, 1] + np.sqrt(covar[i, 0, 0]), + track[i, 2] + np.sqrt(covar[i, 1, 1]), + track[i, 3] + np.sqrt(covar[i, 2, 2]), + ] + ) covarVec = covarVec / np.linalg.norm(covarVec) - covDeg[i, 1] = 3 * np.arccos(np.dot(covarVec, track[i, 1:4]))*180./np.pi + covDeg[i, 1] = 3 * np.arccos(np.dot(covarVec, track[i, 1:4])) * 180.0 / np.pi t = ref[:, 0] * ns2min - plt.figure(num=101011, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , errorDeg[:, 1], color = colorList[1] , label= "Off-point") - plt.plot(t, covDeg[:,1], color=colorList[5], linestyle = '--', label=r'Covar (3-$\sigma$)') - plt.legend(loc='upper right') - plt.ylabel(r'Mean $\hat{\mathbf{h}}$ Error in $\mathcal{C}$ ($^\circ$)') - plt.xlabel('Time (min)') - plt.ylim([0,2]) + plt.figure(num=101011, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, errorDeg[:, 1], color=colorList[1], label="Off-point") + plt.plot( + t, covDeg[:, 1], color=colorList[5], linestyle="--", label=r"Covar (3-$\sigma$)" + ) + plt.legend(loc="upper right") + plt.ylabel(r"Mean $\hat{\mathbf{h}}$ Error in $\mathcal{C}$ ($^\circ$)") + plt.xlabel("Time (min)") + plt.ylim([0, 2]) # plt.savefig('HeadingDeg.pdf') - plt.figure(num=10101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , rError[:, 1], color = colorList[2] , label= 'Error') - plt.plot(t, 3*np.sqrt(covar[:, 0,0]), color=colorList[8], linestyle = '--', label=r'Covar (3-$\sigma$)') - plt.plot(t, -3*np.sqrt(covar[:, 0,0]), color=colorList[8], linestyle = '--') + plt.figure(num=10101, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, rError[:, 1], color=colorList[2], label="Error") + plt.plot( + t, + 3 * np.sqrt(covar[:, 0, 0]), + color=colorList[8], + linestyle="--", + label=r"Covar (3-$\sigma$)", + ) + plt.plot(t, -3 * np.sqrt(covar[:, 0, 0]), color=colorList[8], linestyle="--") plt.legend() - plt.ylabel(r'$\hat{\mathbf{h}}_{' + str(1) +r'}$ Error in $\mathcal{C}$ (-)') - plt.ylim([-0.04,0.04]) - plt.xlabel('Time (min)') + plt.ylabel(r"$\hat{\mathbf{h}}_{" + str(1) + r"}$ Error in $\mathcal{C}$ (-)") + plt.ylim([-0.04, 0.04]) + plt.xlabel("Time (min)") # plt.savefig('HeadingCam1.pdf') - plt.figure(num=10201, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , rError[:, 2], color = colorList[2] ) - plt.plot(t, 3*np.sqrt(covar[:, 1,1]), color=colorList[8], linestyle = '--') - plt.plot(t, -3*np.sqrt(covar[:, 1,1]), color=colorList[8], linestyle = '--') - plt.ylabel(r'$\hat{\mathbf{h}}_{' + str(2) +r'}$ Error in $\mathcal{C}$ (-)') - plt.ylim([-0.04,0.04]) - plt.xlabel('Time (min)') + plt.figure(num=10201, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, rError[:, 2], color=colorList[2]) + plt.plot(t, 3 * np.sqrt(covar[:, 1, 1]), color=colorList[8], linestyle="--") + plt.plot(t, -3 * np.sqrt(covar[:, 1, 1]), color=colorList[8], linestyle="--") + plt.ylabel(r"$\hat{\mathbf{h}}_{" + str(2) + r"}$ Error in $\mathcal{C}$ (-)") + plt.ylim([-0.04, 0.04]) + plt.xlabel("Time (min)") # plt.savefig('HeadingCam2.pdf') - plt.figure(num=10301, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , rError[:, 3], color = colorList[2]) - plt.plot(t, 3*np.sqrt(covar[:, 2,2]), color=colorList[8], linestyle = '--') - plt.plot(t, -3*np.sqrt(covar[:, 2,2]), color=colorList[8], linestyle = '--') - plt.ylabel(r'$\hat{\mathbf{h}}_{' + str(3) +r'}$ Error in $\mathcal{C}$ (-)') - plt.ylim([-0.04,0.04]) - plt.xlabel('Time (min)') + plt.figure(num=10301, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, rError[:, 3], color=colorList[2]) + plt.plot(t, 3 * np.sqrt(covar[:, 2, 2]), color=colorList[8], linestyle="--") + plt.plot(t, -3 * np.sqrt(covar[:, 2, 2]), color=colorList[8], linestyle="--") + plt.ylabel(r"$\hat{\mathbf{h}}_{" + str(3) + r"}$ Error in $\mathcal{C}$ (-)") + plt.ylim([-0.04, 0.04]) + plt.xlabel("Time (min)") # plt.savefig('HeadingCam3.pdf') def plot_faults(dataFaults, valid1, valid2): - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) # for i in range(len(dataFaults[:,0])): # if (dataFaults[i,0]*ns2min%1 != 0): # valid1[i, 1] = np.nan # valid2[i, 1] = np.nan - plt.figure(10101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.xlabel('Time (min)') - plt.ylabel('Detected Fault') - plt.scatter(valid1[:,0]* ns2min, valid1[:,1], color = colorList[5], alpha = 0.1, label = "Limb") - plt.scatter(valid2[:,0]* ns2min, valid2[:,1], color = colorList[8], alpha = 0.1, label = "Circ") - plt.scatter(dataFaults[:,0]* ns2min, dataFaults[:,1], marker = '.', color = colorList[2], label = "Faults") + plt.figure(10101, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.xlabel("Time (min)") + plt.ylabel("Detected Fault") + plt.scatter( + valid1[:, 0] * ns2min, valid1[:, 1], color=colorList[5], alpha=0.1, label="Limb" + ) + plt.scatter( + valid2[:, 0] * ns2min, valid2[:, 1], color=colorList[8], alpha=0.1, label="Circ" + ) + plt.scatter( + dataFaults[:, 0] * ns2min, + dataFaults[:, 1], + marker=".", + color=colorList[2], + label="Faults", + ) plt.legend() # plt.savefig('FaultsDetected.pdf') return + def diff_methods(vec1, meth1, meth2, val1, val2): - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) validIdx1 = [] - for i in range(len(val1[:,0])): - if np.abs(val1[i,1] - 1) < 0.01: + for i in range(len(val1[:, 0])): + if np.abs(val1[i, 1] - 1) < 0.01: validIdx1.append(i) validIdx2 = [] - for i in range(len(val2[:,0])): - if np.abs(val2[i,1] - 1) < 0.01: + for i in range(len(val2[:, 0])): + if np.abs(val2[i, 1] - 1) < 0.01: validIdx2.append(i) diff1 = np.full([len(validIdx1), 4], np.nan) @@ -205,90 +261,163 @@ def diff_methods(vec1, meth1, meth2, val1, val2): diff2 = np.full([len(validIdx2), 4], np.nan) diffNorms2 = np.full([len(validIdx2), 2], np.nan) for i in range(len(validIdx1)): - diff1[i,0] = vec1[validIdx1[i],0] - diff1[i,1:] = vec1[validIdx1[i],1:] - meth1[validIdx1[i],1:] - diffNorms1[i,0] = vec1[validIdx1[i],0] - diffNorms1[i,1] = np.linalg.norm(vec1[validIdx1[i],1:]) - np.linalg.norm(meth1[validIdx1[i],1:]) + diff1[i, 0] = vec1[validIdx1[i], 0] + diff1[i, 1:] = vec1[validIdx1[i], 1:] - meth1[validIdx1[i], 1:] + diffNorms1[i, 0] = vec1[validIdx1[i], 0] + diffNorms1[i, 1] = np.linalg.norm(vec1[validIdx1[i], 1:]) - np.linalg.norm( + meth1[validIdx1[i], 1:] + ) for i in range(len(validIdx2)): - diff2[i,0] = vec1[validIdx2[i],0] - diff2[i,1:] = vec1[validIdx2[i],1:] - meth2[validIdx2[i],1:] - diffNorms2[i,0] = vec1[validIdx2[i],0] - diffNorms2[i,1] = np.linalg.norm(vec1[validIdx2[i],1:]) - np.linalg.norm(meth2[validIdx2[i],1:]) - plt.figure(1, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.xlabel('Time') - plt.plot(diff1[:, 0] * ns2min, diff1[:, 1] * m2km, color = colorList[1], label=r"$\mathbf{r}_\mathrm{Limb}$") - plt.plot(diff1[:, 0] * ns2min, diff1[:, 2] * m2km, color = colorList[5]) - plt.plot(diff1[:, 0] * ns2min, diff1[:, 3] * m2km, color = colorList[8]) - plt.plot(diff2[:, 0] * ns2min, diff2[:, 1] * m2km, color=colorList[1], label=r"$\mathbf{r}_\mathrm{Circ}$", linestyle ='--', linewidth=2) - plt.plot(diff2[:, 0] * ns2min, diff2[:, 2] * m2km, color=colorList[5], linestyle ='--', linewidth=2) - plt.plot(diff2[:, 0] * ns2min, diff2[:, 3] * m2km, color=colorList[8], linestyle ='--', linewidth=2) + diff2[i, 0] = vec1[validIdx2[i], 0] + diff2[i, 1:] = vec1[validIdx2[i], 1:] - meth2[validIdx2[i], 1:] + diffNorms2[i, 0] = vec1[validIdx2[i], 0] + diffNorms2[i, 1] = np.linalg.norm(vec1[validIdx2[i], 1:]) - np.linalg.norm( + meth2[validIdx2[i], 1:] + ) + plt.figure(1, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.xlabel("Time") + plt.plot( + diff1[:, 0] * ns2min, + diff1[:, 1] * m2km, + color=colorList[1], + label=r"$\mathbf{r}_\mathrm{Limb}$", + ) + plt.plot(diff1[:, 0] * ns2min, diff1[:, 2] * m2km, color=colorList[5]) + plt.plot(diff1[:, 0] * ns2min, diff1[:, 3] * m2km, color=colorList[8]) + plt.plot( + diff2[:, 0] * ns2min, + diff2[:, 1] * m2km, + color=colorList[1], + label=r"$\mathbf{r}_\mathrm{Circ}$", + linestyle="--", + linewidth=2, + ) + plt.plot( + diff2[:, 0] * ns2min, + diff2[:, 2] * m2km, + color=colorList[5], + linestyle="--", + linewidth=2, + ) + plt.plot( + diff2[:, 0] * ns2min, + diff2[:, 3] * m2km, + color=colorList[8], + linestyle="--", + linewidth=2, + ) plt.legend() plt.ylabel(r"$\mathbf{r}_{\mathrm{true}} - \mathbf{r}_{\mathrm{opnav}}$ (km)") plt.xlabel("Time (min)") # plt.savefig('MeasErrorComponents.pdf') - plt.figure(2, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.xlabel('Time') - plt.plot(diff1[:, 0] * ns2min, diffNorms1[:,1] * m2km, color = colorList[1]) - plt.plot(diff2[:, 0] * ns2min, diffNorms2[:,1] * m2km, color = colorList[1], linestyle="--", linewidth=2) + plt.figure(2, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.xlabel("Time") + plt.plot(diff1[:, 0] * ns2min, diffNorms1[:, 1] * m2km, color=colorList[1]) + plt.plot( + diff2[:, 0] * ns2min, + diffNorms2[:, 1] * m2km, + color=colorList[1], + linestyle="--", + linewidth=2, + ) plt.ylabel(r"$|\mathbf{r}_{\mathrm{true}}|$ - $|\mathbf{r}_{\mathrm{opnav}}|$ (km)") plt.xlabel("Time (min)") # plt.savefig('MeasErrorNorm.pdf') + def diff_vectors(vec1, vec2, valid, string): - assert len(vec1[0,:]) == len(vec2[0,:]), print("Vectors need to be the same size") + assert len(vec1[0, :]) == len(vec2[0, :]), print("Vectors need to be the same size") - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) validIdx = [] - for i in range(len(valid[:,0])): - if np.abs(valid[i,1] - 1) < 0.01: + for i in range(len(valid[:, 0])): + if np.abs(valid[i, 1] - 1) < 0.01: validIdx.append(i) diff = np.full([len(validIdx), 4], np.nan) diffNorms = np.full([len(validIdx), 2], np.nan) - m2km2 = m2km*m2km + m2km2 = m2km * m2km for i in range(len(validIdx)): - diff[i,0] = vec1[validIdx[i],0] - diff[i,1:] = vec1[validIdx[i],1:] - vec2[validIdx[i],1:] - diffNorms[i,0] = vec1[validIdx[i],0] - diffNorms[i,1] = np.linalg.norm(vec1[validIdx[i],1:]) - np.linalg.norm(vec2[validIdx[i],1:]) + diff[i, 0] = vec1[validIdx[i], 0] + diff[i, 1:] = vec1[validIdx[i], 1:] - vec2[validIdx[i], 1:] + diffNorms[i, 0] = vec1[validIdx[i], 0] + diffNorms[i, 1] = np.linalg.norm(vec1[validIdx[i], 1:]) - np.linalg.norm( + vec2[validIdx[i], 1:] + ) # plt.figure(1, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.figure(1, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.xlabel('Time') - plt.plot(diff[:, 0] * ns2min, diff[:, 1] * m2km2, color = colorList[1], label=r"$x_\mathrm{"+string+"}$") - plt.plot(diff[:, 0] * ns2min, np.mean(diff[:, 1]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[1], linestyle = '--') - plt.plot(diff[:, 0] * ns2min, diff[:, 2] * m2km2, color = colorList[5], label=r"$y_\mathrm{"+string+"}$") - plt.plot(diff[:, 0] * ns2min, np.mean(diff[:, 2]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[5], linestyle = '--') - plt.plot(diff[:, 0] * ns2min, diff[:, 3] * m2km2, color = colorList[8], label=r"$z_\mathrm{"+string+"}$") - plt.plot(diff[:, 0] * ns2min, np.mean(diff[:, 3]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[8], linestyle = '--') + plt.figure(1, figsize=(3.5, 2.0), facecolor="w", edgecolor="k") + plt.xlabel("Time") + plt.plot( + diff[:, 0] * ns2min, + diff[:, 1] * m2km2, + color=colorList[1], + label=r"$x_\mathrm{" + string + "}$", + ) + plt.plot( + diff[:, 0] * ns2min, + np.mean(diff[:, 1]) * m2km2 * np.ones(len(diff[:, 0])), + color=colorList[1], + linestyle="--", + ) + plt.plot( + diff[:, 0] * ns2min, + diff[:, 2] * m2km2, + color=colorList[5], + label=r"$y_\mathrm{" + string + "}$", + ) + plt.plot( + diff[:, 0] * ns2min, + np.mean(diff[:, 2]) * m2km2 * np.ones(len(diff[:, 0])), + color=colorList[5], + linestyle="--", + ) + plt.plot( + diff[:, 0] * ns2min, + diff[:, 3] * m2km2, + color=colorList[8], + label=r"$z_\mathrm{" + string + "}$", + ) + plt.plot( + diff[:, 0] * ns2min, + np.mean(diff[:, 3]) * m2km2 * np.ones(len(diff[:, 0])), + color=colorList[8], + linestyle="--", + ) plt.legend() plt.ylabel(r"$\mathbf{r}_{\mathrm{true}} - \mathbf{r}_{\mathrm{opnav}}$ (km)") plt.xlabel("Time (min)") ##plt.savefig('MeasErrorComponents.pdf') # plt.figure(2, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.figure(2, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.xlabel('Time') - plt.plot(diff[:, 0] * ns2min, diffNorms[:,1] * m2km2, color = colorList[1]) - plt.plot(diff[:, 0] * ns2min, np.mean(diffNorms[:,1]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[1], linestyle="--") + plt.figure(2, figsize=(3.5, 2.0), facecolor="w", edgecolor="k") + plt.xlabel("Time") + plt.plot(diff[:, 0] * ns2min, diffNorms[:, 1] * m2km2, color=colorList[1]) + plt.plot( + diff[:, 0] * ns2min, + np.mean(diffNorms[:, 1]) * m2km2 * np.ones(len(diff[:, 0])), + color=colorList[1], + linestyle="--", + ) plt.ylabel(r"$|\mathbf{r}_{\mathrm{true}}|$ - $|\mathbf{r}_{\mathrm{opnav}}|$ (km)") plt.xlabel("Time (min)") - #plt.savefig('MeasErrorNorm.pdf') + # plt.savefig('MeasErrorNorm.pdf') return + def nav_percentages(truth, states, covar, valid, string): - numStates = len(states[0:,1:]) - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + numStates = len(states[0:, 1:]) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) validIdx = [] - for i in range(len(valid[:,0])): - if np.abs(valid[i,1] - 1) < 0.01: + for i in range(len(valid[:, 0])): + if np.abs(valid[i, 1] - 1) < 0.01: validIdx.append(i) diffPos = np.full([len(validIdx), 2], np.nan) diffVel = np.full([len(validIdx), 2], np.nan) @@ -297,49 +426,84 @@ def nav_percentages(truth, states, covar, valid, string): m2km2 = m2km for i in range(len(validIdx)): - diffPos[i,0] = states[validIdx[i],0] - diffPos[i,1] = np.linalg.norm(states[validIdx[i],1:4] - truth[validIdx[i],1:4])/np.linalg.norm(truth[validIdx[i],1:4])*100 - diffVel[i,0] = states[validIdx[i],0] - diffVel[i,1] = np.linalg.norm(states[validIdx[i],4:7] - truth[validIdx[i],4:7])/np.linalg.norm(truth[validIdx[i],4:7])*100 - covarPos[i,0] = states[validIdx[i],0] - posVec = np.sqrt(np.array([covar[validIdx[i],1], covar[validIdx[i],1 + 6+1], covar[validIdx[i],1 + 2*(6+1)]])) - covarPos[i,1] = 3*np.linalg.norm(posVec)/np.linalg.norm(truth[validIdx[i],1:4])*100 - covarVel[i,0] = states[validIdx[i],0] - velVec = np.sqrt(np.array([covar[validIdx[i],1 + 3*(6+1)], covar[validIdx[i],1 + 4*(6+1)], covar[validIdx[i],1 + 5*(6+1)]])) - covarVel[i,1] = 3*np.linalg.norm(velVec)/np.linalg.norm(truth[validIdx[i],4:7])*100 - plt.figure(101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') + diffPos[i, 0] = states[validIdx[i], 0] + diffPos[i, 1] = ( + np.linalg.norm(states[validIdx[i], 1:4] - truth[validIdx[i], 1:4]) + / np.linalg.norm(truth[validIdx[i], 1:4]) + * 100 + ) + diffVel[i, 0] = states[validIdx[i], 0] + diffVel[i, 1] = ( + np.linalg.norm(states[validIdx[i], 4:7] - truth[validIdx[i], 4:7]) + / np.linalg.norm(truth[validIdx[i], 4:7]) + * 100 + ) + covarPos[i, 0] = states[validIdx[i], 0] + posVec = np.sqrt( + np.array( + [ + covar[validIdx[i], 1], + covar[validIdx[i], 1 + 6 + 1], + covar[validIdx[i], 1 + 2 * (6 + 1)], + ] + ) + ) + covarPos[i, 1] = ( + 3 * np.linalg.norm(posVec) / np.linalg.norm(truth[validIdx[i], 1:4]) * 100 + ) + covarVel[i, 0] = states[validIdx[i], 0] + velVec = np.sqrt( + np.array( + [ + covar[validIdx[i], 1 + 3 * (6 + 1)], + covar[validIdx[i], 1 + 4 * (6 + 1)], + covar[validIdx[i], 1 + 5 * (6 + 1)], + ] + ) + ) + covarVel[i, 1] = ( + 3 * np.linalg.norm(velVec) / np.linalg.norm(truth[validIdx[i], 4:7]) * 100 + ) + plt.figure(101, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") # plt.figure(101, figsize=(3.5, 2), facecolor='w', edgecolor='k') - plt.plot(diffPos[:, 0] * ns2min, diffPos[:, 1] , color = colorList[1], label = "Error") - plt.plot(covarPos[:, 0] * ns2min, covarPos[:,1], color = colorList[8], linestyle = '--', label=r'Covar ($3\sigma$)') - plt.legend(loc='upper right') - plt.ylabel(r"$\mathbf{r}_\mathrm{"+string+r"}$ errors ($\%$)") + plt.plot(diffPos[:, 0] * ns2min, diffPos[:, 1], color=colorList[1], label="Error") + plt.plot( + covarPos[:, 0] * ns2min, + covarPos[:, 1], + color=colorList[8], + linestyle="--", + label=r"Covar ($3\sigma$)", + ) + plt.legend(loc="upper right") + plt.ylabel(r"$\mathbf{r}_\mathrm{" + string + r"}$ errors ($\%$)") plt.xlabel("Time (min)") # plt.ylim([0,3.5]) - #plt.savefig('PercentErrorPos.pdf') + # plt.savefig('PercentErrorPos.pdf') - plt.figure(102, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') + plt.figure(102, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") # plt.figure(102, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(diffVel[:, 0] * ns2min, diffVel[:, 1], color = colorList[1]) - plt.plot(covarVel[:, 0] * ns2min, covarVel[:,1], color = colorList[8], linestyle = '--') - plt.ylabel(r"$\dot{\mathbf{r}}_\mathrm{"+string+ r"}$ errors ($\%$)") + plt.plot(diffVel[:, 0] * ns2min, diffVel[:, 1], color=colorList[1]) + plt.plot( + covarVel[:, 0] * ns2min, covarVel[:, 1], color=colorList[8], linestyle="--" + ) + plt.ylabel(r"$\dot{\mathbf{r}}_\mathrm{" + string + r"}$ errors ($\%$)") plt.xlabel("Time (min)") # plt.ylim([0,15]) - #plt.savefig('PercentErrorVel.pdf') - + # plt.savefig('PercentErrorVel.pdf') - RMSPos = np.sqrt(sum(diffPos[:, 1] ** 2)/len(diffPos[:, 1])) - RMSPosCov = np.sqrt(sum((covarPos[:, 1]) ** 2)/len(covarPos[:, 1])) - RMSVel = np.sqrt(sum(diffVel[:, 1] ** 2)/len(diffVel[:, 1])) - RMSVelCov = np.sqrt(sum((covarVel[:, 1]) ** 2)/len(covarVel[:, 1])) + RMSPos = np.sqrt(sum(diffPos[:, 1] ** 2) / len(diffPos[:, 1])) + RMSPosCov = np.sqrt(sum((covarPos[:, 1]) ** 2) / len(covarPos[:, 1])) + RMSVel = np.sqrt(sum(diffVel[:, 1] ** 2) / len(diffVel[:, 1])) + RMSVelCov = np.sqrt(sum((covarVel[:, 1]) ** 2) / len(covarVel[:, 1])) - print('-------- Pos RMS '+ string + '--------') + print("-------- Pos RMS " + string + "--------") print(RMSPos) print(RMSPosCov) - print('------------------------') - print('-------- Vel RMS '+ string + '--------') + print("------------------------") + print("-------- Vel RMS " + string + "--------") print(RMSVel) print(RMSVelCov) - print('------------------------') + print("------------------------") # RMS Errors Computed for heading and rate # unitTestSupport.writeTeXSnippet('RMSPos_'+ string, str(round(RMSPos,3)), os.path.dirname(__file__)) # unitTestSupport.writeTeXSnippet('RMSPosCov_'+ string, str(round(RMSPosCov,3)),os.path.dirname(__file__)) @@ -347,280 +511,323 @@ def nav_percentages(truth, states, covar, valid, string): # unitTestSupport.writeTeXSnippet('RMSVelCov_'+ string, str(round(RMSVelCov,3)),os.path.dirname(__file__)) return + def plot_orbit(r_BN): plt.figure() - plt.xlabel('$R_x$, km') - plt.ylabel('$R_y$, km') + plt.xlabel("$R_x$, km") + plt.ylabel("$R_y$, km") plt.plot(r_BN[:, 1] * m2km, r_BN[:, 2] * m2km, color_x) plt.scatter(0, 0) - plt.title('Spacecraft Orbit') + plt.title("Spacecraft Orbit") return + def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW): - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) - plt.figure(22, figsize=(3, 1.8), facecolor='w', edgecolor='k') + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) + plt.figure(22, figsize=(3, 1.8), facecolor="w", edgecolor="k") for idx in range(1, numRW): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=colorList[idx*2], - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.plot(timeData, dataRW[idx - 1][:, 1], - color=colorList[idx*2], - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - #plt.savefig('RWMotorTorque.pdf') - plt.xlabel('Time (min)') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=colorList[idx * 2], + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.plot( + timeData, + dataRW[idx - 1][:, 1], + color=colorList[idx * 2], + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + # plt.savefig('RWMotorTorque.pdf') + plt.xlabel("Time (min)") + plt.ylabel("RW Motor Torque (Nm)") def plot_TwoOrbits(r_BN, r_BN2): - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) # fig = plt.figure(5, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - fig = plt.figure(5, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - ax = fig.add_subplot(projection='3d') - ax.set_xlabel(r'$R_x$, km') - ax.set_ylabel(r'$R_y$, km') - ax.set_zlabel(r'$R_z$, km') - ax.plot(r_BN[:, 1] * m2km, r_BN[:, 2] * m2km, r_BN[:, 3] * m2km, color=colorList[1], label="True") - for i in range(len(r_BN2[:,0])): - if np.abs(r_BN2[i, 1])>0 or np.abs(r_BN2[i, 2])>0: - ax.scatter(r_BN2[i, 1] * m2km, r_BN2[i, 2] * m2km, r_BN2[i, 3] * m2km, color=colorList[8], label="Measured") - ax.scatter(0, 0, color='r') + fig = plt.figure(5, figsize=(3.5, 2.0), facecolor="w", edgecolor="k") + ax = fig.add_subplot(projection="3d") + ax.set_xlabel(r"$R_x$, km") + ax.set_ylabel(r"$R_y$, km") + ax.set_zlabel(r"$R_z$, km") + ax.plot( + r_BN[:, 1] * m2km, + r_BN[:, 2] * m2km, + r_BN[:, 3] * m2km, + color=colorList[1], + label="True", + ) + for i in range(len(r_BN2[:, 0])): + if np.abs(r_BN2[i, 1]) > 0 or np.abs(r_BN2[i, 2]) > 0: + ax.scatter( + r_BN2[i, 1] * m2km, + r_BN2[i, 2] * m2km, + r_BN2[i, 3] * m2km, + color=colorList[8], + label="Measured", + ) + ax.scatter(0, 0, color="r") # ax.set_title('Orbit and Measurements') return + def plot_attitude_error(timeLineSet, dataSigmaBR): - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) - plt.figure(5555, figsize=(3, 1.8), facecolor='w', edgecolor='k') + plt.figure(5555, figsize=(3, 1.8), facecolor="w", edgecolor="k") plt.rcParams["font.size"] = "8" fig = plt.gcf() ax = fig.gca() vectorData = unitTestSupport.pullVectorSetFromData(dataSigmaBR) sNorm = np.array([np.linalg.norm(v) for v in vectorData]) - plt.plot(timeLineSet, sNorm, - color=colorList[0], - ) - plt.xlabel('Time (min)') + plt.plot( + timeLineSet, + sNorm, + color=colorList[0], + ) + plt.xlabel("Time (min)") plt.xlim([40, 100]) - plt.ylabel(r'Attitude Error Norm $|\sigma_{C/R}|$') - #plt.savefig('AttErrorNorm.pdf') - ax.set_yscale('log') + plt.ylabel(r"Attitude Error Norm $|\sigma_{C/R}|$") + # plt.savefig('AttErrorNorm.pdf') + ax.set_yscale("log") def plot_rate_error(timeLineSet, dataOmegaBR): - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) - plt.figure(666666, figsize=(3, 1.8), facecolor='w', edgecolor='k') + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) + plt.figure(666666, figsize=(3, 1.8), facecolor="w", edgecolor="k") plt.rcParams["font.size"] = "8" - styleList = ['-', '--', ':'] + styleList = ["-", "--", ":"] for idx in range(1, 4): - plt.plot(timeLineSet, dataOmegaBR[:, idx], - color=colorList[idx*2], - label=r'$\omega_{BR,' + str(idx) + '}$', - linestyle= styleList[idx-1] ) - plt.legend(loc='lower right') + plt.plot( + timeLineSet, + dataOmegaBR[:, idx], + color=colorList[idx * 2], + label=r"$\omega_{BR," + str(idx) + "}$", + linestyle=styleList[idx - 1], + ) + plt.legend(loc="lower right") plt.xlim([40, 100]) - plt.xlabel('Time (min)') - #plt.savefig('RateErrorControl.pdf') - plt.ylabel('Rate Tracking Error (rad/s) ') + plt.xlabel("Time (min)") + # plt.savefig('RateErrorControl.pdf') + plt.ylabel("Rate Tracking Error (rad/s) ") return def plot_rw_cmd_torque(timeData, dataUsReq, numRW): plt.figure() for idx in range(1, 4): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time (min)') - #plt.savefig('RWMotorTorque.pdf') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time (min)") + # plt.savefig('RWMotorTorque.pdf') + plt.ylabel("RW Motor Torque (Nm)") + def plot_rw_speeds(timeData, dataOmegaRW, numRW): plt.figure() for idx in range(1, numRW + 1): - plt.plot(timeData, dataOmegaRW[:, idx] / mc.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx) + '}$') - plt.legend(loc='upper right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed (RPM) ') + plt.plot( + timeData, + dataOmegaRW[:, idx] / mc.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx) + "}$", + ) + plt.legend(loc="upper right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed (RPM) ") -def plotStateCovarPlot(x, Pflat): - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) +def plotStateCovarPlot(x, Pflat): + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) - numStates = len(x[0,:])-1 + numStates = len(x[0, :]) - 1 - P = np.zeros([len(Pflat[:,0]),numStates,numStates]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*ns2min - if not np.isnan(Pflat[i,1]): - Plast = Pflat[i,1:].reshape([numStates,numStates]) + P = np.zeros([len(Pflat[:, 0]), numStates, numStates]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * ns2min + if not np.isnan(Pflat[i, 1]): + Plast = Pflat[i, 1:].reshape([numStates, numStates]) P[i, :, :] = Plast - x0 = x[i,7:10] + x0 = x[i, 7:10] if numStates == 6 or numStates == 3: - P[i,:,:] = Pflat[i,1:].reshape([numStates,numStates]) - - - if numStates == 9 : - plt.figure(10, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 1]*m2km, label='$r_1$', color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 0, 0])*m2km, '--', label=r'Covar (3-$\sigma$)', color = colorList[8]) - plt.plot(t ,- 3 * np.sqrt(P[:, 0, 0])*m2km, '--', color = colorList[8]) - plt.legend(loc='best') - plt.ylabel('Position Error (km)') + P[i, :, :] = Pflat[i, 1:].reshape([numStates, numStates]) + + if numStates == 9: + plt.figure(10, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 1] * m2km, label="$r_1$", color=colorList[1]) + plt.plot( + t, + 3 * np.sqrt(P[:, 0, 0]) * m2km, + "--", + label=r"Covar (3-$\sigma$)", + color=colorList[8], + ) + plt.plot(t, -3 * np.sqrt(P[:, 0, 0]) * m2km, "--", color=colorList[8]) + plt.legend(loc="best") + plt.ylabel("Position Error (km)") plt.grid() - plt.figure(11, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 4]*m2km, color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8]) - plt.plot(t ,- 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8]) - plt.ylabel('Rate Error (km/s)') + plt.figure(11, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 4] * m2km, color=colorList[1]) + plt.plot(t, 3 * np.sqrt(P[:, 3, 3]) * m2km, "--", color=colorList[8]) + plt.plot(t, -3 * np.sqrt(P[:, 3, 3]) * m2km, "--", color=colorList[8]) + plt.ylabel("Rate Error (km/s)") plt.grid() - plt.figure(12, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 7], color = colorList[1]) - plt.plot(t , x0[0] + 3 * np.sqrt(P[:, 6, 6]), '--', color = colorList[8]) - plt.plot(t , x0[0] - 3 * np.sqrt(P[:, 6, 6]), '--', color = colorList[8]) - plt.title('First bias component (km/s)') + plt.figure(12, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 7], color=colorList[1]) + plt.plot(t, x0[0] + 3 * np.sqrt(P[:, 6, 6]), "--", color=colorList[8]) + plt.plot(t, x0[0] - 3 * np.sqrt(P[:, 6, 6]), "--", color=colorList[8]) + plt.title("First bias component (km/s)") plt.grid() - plt.figure(13, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 2]*m2km, color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8]) - plt.plot(t ,- 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8]) - plt.title('Second pos component (km)') + plt.figure(13, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 2] * m2km, color=colorList[1]) + plt.plot(t, 3 * np.sqrt(P[:, 1, 1]) * m2km, "--", color=colorList[8]) + plt.plot(t, -3 * np.sqrt(P[:, 1, 1]) * m2km, "--", color=colorList[8]) + plt.title("Second pos component (km)") plt.grid() - plt.figure(14, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 5]*m2km, color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8]) - plt.plot(t ,- 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8]) - plt.xlabel('Time (min)') - plt.title('Second rate component (km/s)') + plt.figure(14, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 5] * m2km, color=colorList[1]) + plt.plot(t, 3 * np.sqrt(P[:, 4, 4]) * m2km, "--", color=colorList[8]) + plt.plot(t, -3 * np.sqrt(P[:, 4, 4]) * m2km, "--", color=colorList[8]) + plt.xlabel("Time (min)") + plt.title("Second rate component (km/s)") plt.grid() - plt.figure(15, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 8], color = colorList[1]) - plt.plot(t , x0[1] + 3 * np.sqrt(P[:, 7, 7]), '--', color = colorList[8]) - plt.plot(t , x0[1] - 3 * np.sqrt(P[:, 7, 7]), '--', color = colorList[8]) - plt.title('Second bias component (km/s)') + plt.figure(15, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 8], color=colorList[1]) + plt.plot(t, x0[1] + 3 * np.sqrt(P[:, 7, 7]), "--", color=colorList[8]) + plt.plot(t, x0[1] - 3 * np.sqrt(P[:, 7, 7]), "--", color=colorList[8]) + plt.title("Second bias component (km/s)") plt.grid() - plt.figure(16, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 3]*m2km, color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8]) - plt.plot(t ,-3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8]) - plt.xlabel('Time (min)') - plt.title('Third pos component (km)') + plt.figure(16, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 3] * m2km, color=colorList[1]) + plt.plot(t, 3 * np.sqrt(P[:, 2, 2]) * m2km, "--", color=colorList[8]) + plt.plot(t, -3 * np.sqrt(P[:, 2, 2]) * m2km, "--", color=colorList[8]) + plt.xlabel("Time (min)") + plt.title("Third pos component (km)") plt.grid() - plt.figure(17, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 6]*m2km, color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8]) - plt.plot(t , -3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8]) - plt.xlabel('Time (min)') - plt.title('Third rate component (km/s)') + plt.figure(17, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 6] * m2km, color=colorList[1]) + plt.plot(t, 3 * np.sqrt(P[:, 5, 5]) * m2km, "--", color=colorList[8]) + plt.plot(t, -3 * np.sqrt(P[:, 5, 5]) * m2km, "--", color=colorList[8]) + plt.xlabel("Time (min)") + plt.title("Third rate component (km/s)") plt.grid() - plt.figure(18, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 9], color = colorList[1]) - plt.plot(t , x0[2] + 3 * np.sqrt(P[:, 8, 8]), '--', color = colorList[8]) - plt.plot(t , x0[2] - 3 * np.sqrt(P[:, 8, 8]), '--', color = colorList[8]) - plt.title('Third bias component (km/s)') + plt.figure(18, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 9], color=colorList[1]) + plt.plot(t, x0[2] + 3 * np.sqrt(P[:, 8, 8]), "--", color=colorList[8]) + plt.plot(t, x0[2] - 3 * np.sqrt(P[:, 8, 8]), "--", color=colorList[8]) + plt.title("Third bias component (km/s)") plt.grid() - if numStates == 6 or numStates ==3: - plt.figure(20, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 1]*m2km, label='State Error', color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 0, 0])*m2km, '--', label=r'Covar (3-$\sigma$)', color = colorList[8]) - plt.plot(t ,- 3 * np.sqrt(P[:, 0, 0])*m2km, '--', color = colorList[8]) - plt.legend(loc='best') - plt.ylabel(r'$r_1$ Error (km)') - #plt.savefig('Filterpos1.pdf') + if numStates == 6 or numStates == 3: + plt.figure(20, figsize=(2.4, 1.4), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 1] * m2km, label="State Error", color=colorList[1]) + plt.plot( + t, + 3 * np.sqrt(P[:, 0, 0]) * m2km, + "--", + label=r"Covar (3-$\sigma$)", + color=colorList[8], + ) + plt.plot(t, -3 * np.sqrt(P[:, 0, 0]) * m2km, "--", color=colorList[8]) + plt.legend(loc="best") + plt.ylabel(r"$r_1$ Error (km)") + # plt.savefig('Filterpos1.pdf') if numStates == 6: - plt.figure(21, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 4]*m2km, color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8]) - plt.plot(t ,- 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8]) - plt.ylabel(r'$v_1$ Error (km/s)') - #plt.savefig('Filtervel1.pdf') - - if numStates == 6 or numStates ==3: - plt.figure(22, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 2]*m2km, color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8]) - plt.plot(t ,- 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8]) - plt.ylabel(r'$r_2$ Error (km)') - #plt.savefig('Filterpos2.pdf') + plt.figure(21, figsize=(2.4, 1.4), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 4] * m2km, color=colorList[1]) + plt.plot(t, 3 * np.sqrt(P[:, 3, 3]) * m2km, "--", color=colorList[8]) + plt.plot(t, -3 * np.sqrt(P[:, 3, 3]) * m2km, "--", color=colorList[8]) + plt.ylabel(r"$v_1$ Error (km/s)") + # plt.savefig('Filtervel1.pdf') + + if numStates == 6 or numStates == 3: + plt.figure(22, figsize=(2.4, 1.4), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 2] * m2km, color=colorList[1]) + plt.plot(t, 3 * np.sqrt(P[:, 1, 1]) * m2km, "--", color=colorList[8]) + plt.plot(t, -3 * np.sqrt(P[:, 1, 1]) * m2km, "--", color=colorList[8]) + plt.ylabel(r"$r_2$ Error (km)") + # plt.savefig('Filterpos2.pdf') if numStates == 6: - plt.figure(23, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 5]*m2km, color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8]) - plt.plot(t ,- 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8]) - plt.ylabel(r'$v_2$ Error (km/s)') - #plt.savefig('Filtervel2.pdf') - - if numStates == 6 or numStates ==3: - plt.figure(24, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 3]*m2km, color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8]) - plt.plot(t ,-3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8]) - plt.xlabel('Time (min)') - plt.ylabel(r'$r_3$ Error (km)') - #plt.savefig('Filterpos3.pdf') + plt.figure(23, figsize=(2.4, 1.4), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 5] * m2km, color=colorList[1]) + plt.plot(t, 3 * np.sqrt(P[:, 4, 4]) * m2km, "--", color=colorList[8]) + plt.plot(t, -3 * np.sqrt(P[:, 4, 4]) * m2km, "--", color=colorList[8]) + plt.ylabel(r"$v_2$ Error (km/s)") + # plt.savefig('Filtervel2.pdf') + + if numStates == 6 or numStates == 3: + plt.figure(24, figsize=(2.4, 1.4), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 3] * m2km, color=colorList[1]) + plt.plot(t, 3 * np.sqrt(P[:, 2, 2]) * m2km, "--", color=colorList[8]) + plt.plot(t, -3 * np.sqrt(P[:, 2, 2]) * m2km, "--", color=colorList[8]) + plt.xlabel("Time (min)") + plt.ylabel(r"$r_3$ Error (km)") + # plt.savefig('Filterpos3.pdf') if numStates == 6: - plt.figure(25, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') - plt.plot(t , x[:, 6]*m2km, color = colorList[1]) - plt.plot(t , 3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8]) - plt.plot(t , -3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8]) - plt.xlabel('Time (min)') - plt.ylabel(r'$v_3$ Error (km/s)') - #plt.savefig('Filtervel3.pdf') + plt.figure(25, figsize=(2.4, 1.4), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 6] * m2km, color=colorList[1]) + plt.plot(t, 3 * np.sqrt(P[:, 5, 5]) * m2km, "--", color=colorList[8]) + plt.plot(t, -3 * np.sqrt(P[:, 5, 5]) * m2km, "--", color=colorList[8]) + plt.xlabel("Time (min)") + plt.ylabel(r"$v_3$ Error (km/s)") + # plt.savefig('Filtervel3.pdf') def centerXY(centerPoints, size): - - t= centerPoints[:, 0]*ns2min + t = centerPoints[:, 0] * ns2min subtime = [] subX = [] subY = [] center = np.full([len(t), 3], np.nan) - for i in range(len(centerPoints[:,0])): - if centerPoints[i, 1:3].any() > 1E-10: - subtime.append(centerPoints[i, 0]*ns2min) - center[i,1:] = centerPoints[i,1:3] - size/2 + 0.5 - subX.append(center[i,1]) + for i in range(len(centerPoints[:, 0])): + if centerPoints[i, 1:3].any() > 1e-10: + subtime.append(centerPoints[i, 0] * ns2min) + center[i, 1:] = centerPoints[i, 1:3] - size / 2 + 0.5 + subX.append(center[i, 1]) subY.append(center[i, 2]) - colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.cm.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.cm.get_cmap("inferno").colors[int(i * colorsInt)]) # xCurve = fit_sin(subtime, subX) # yCurve = fit_sin(subtime, subY) @@ -628,38 +835,37 @@ def centerXY(centerPoints, size): # print "Periods in Errors are , " + str(xCurve["period"]) + " and " + str(yCurve["period"]) + " s" # plt.figure(100, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.figure(100, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(subtime, subX, ".", label = "X-center", color = colorList[8]) + plt.figure(100, figsize=(3.5, 2.0), facecolor="w", edgecolor="k") + plt.plot(subtime, subX, ".", label="X-center", color=colorList[8]) # plt.plot(t, xCurve["fitfunc"](t), "r", label="X-curve") - plt.plot(subtime, subY, ".", label = "Y-center", color = colorList[1]) + plt.plot(subtime, subY, ".", label="Y-center", color=colorList[1]) # plt.plot(t, yCurve["fitfunc"](t), "b", label="Y-curve") - plt.legend(loc='best') + plt.legend(loc="best") plt.ylabel("Pixels") - plt.title('First unit Vec component in C frame (-)') - #plt.savefig('CentersXY.pdf') + plt.title("First unit Vec component in C frame (-)") + # plt.savefig('CentersXY.pdf') def pixelAndPos(x, r, centers, size): - - t= x[:, 0]*ns2min - r_norm = np.zeros([len(r[:,0]), 5]) - r_norm[:,0] = r[:,0] + t = x[:, 0] * ns2min + r_norm = np.zeros([len(r[:, 0]), 5]) + r_norm[:, 0] = r[:, 0] centerPoints = np.full([len(t), 3], np.nan) maxDiff = np.zeros(3) for i in range(3): values = [] for j in range(len(x[:, 0])): - if not np.isnan(x[j,i+1]): - values.append(x[j,i+1]) + if not np.isnan(x[j, i + 1]): + values.append(x[j, i + 1]) try: maxDiff[i] = np.max(np.abs(values)) except ValueError: - maxDiff[i] =1 - for i in range(len(x[:,0])): - r_norm[i,1:4] = r[i,1:]/np.linalg.norm(r[i,1:]) * maxDiff - r_norm[i,4] = np.linalg.norm(r[i,1:]) - if centers[i, 1:3].any() > 1E-10: - centerPoints[i,1:] = centers[i,1:3] - size/2 + maxDiff[i] = 1 + for i in range(len(x[:, 0])): + r_norm[i, 1:4] = r[i, 1:] / np.linalg.norm(r[i, 1:]) * maxDiff + r_norm[i, 4] = np.linalg.norm(r[i, 1:]) + if centers[i, 1:3].any() > 1e-10: + centerPoints[i, 1:] = centers[i, 1:3] - size / 2 for i in range(2): values = [] for j in range(len(centerPoints[:, 0])): @@ -669,361 +875,442 @@ def pixelAndPos(x, r, centers, size): maxCenter = np.max(np.abs(values)) except ValueError: maxCenter = 1 - centerPoints[:, i+1]/=maxCenter/maxDiff[i] + centerPoints[:, i + 1] /= maxCenter / maxDiff[i] - - colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.cm.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.cm.get_cmap("inferno").colors[int(i * colorsInt)]) # plt.figure(200, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.figure(200, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(t, x[:, 1], ".", label='Truth - Meas', color = colorList[1]) - plt.plot(t, r_norm[:, 1], label = "True Pos", color = colorList[5]) - plt.plot(t, centerPoints[:,1], ".", label = "X-center", color = colorList[8]) + plt.figure(200, figsize=(3.5, 2.0), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 1], ".", label="Truth - Meas", color=colorList[1]) + plt.plot(t, r_norm[:, 1], label="True Pos", color=colorList[5]) + plt.plot(t, centerPoints[:, 1], ".", label="X-center", color=colorList[8]) # plt.plot(t, x1["fitfunc"](t), "r--", label = "Fit") - plt.legend(loc='best') - plt.title('First unit Vec component in C frame (-)') + plt.legend(loc="best") + plt.title("First unit Vec component in C frame (-)") # plt.figure(201, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.figure(201, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(t, x[:, 2], ".", color = colorList[1]) - plt.plot(t, r_norm[:, 2], color = colorList[5]) - plt.plot(t, centerPoints[:,2], ".", label = "X-center", color = colorList[8]) + plt.figure(201, figsize=(3.5, 2.0), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 2], ".", color=colorList[1]) + plt.plot(t, r_norm[:, 2], color=colorList[5]) + plt.plot(t, centerPoints[:, 2], ".", label="X-center", color=colorList[8]) # plt.plot(t, x2["fitfunc"](t), "r--") - plt.title('Second unit Vec component in C frame (-)') + plt.title("Second unit Vec component in C frame (-)") # plt.figure(202, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.figure(202, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(t, x[:, 3], ".", color = colorList[1]) - plt.plot(t, r_norm[:, 3], color = colorList[5]) + plt.figure(202, figsize=(3.5, 2.0), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 3], ".", color=colorList[1]) + plt.plot(t, r_norm[:, 3], color=colorList[5]) # plt.plot(t, x3["fitfunc"](t), "r--") - plt.xlabel('Time (min)') - plt.title('Third unit Vec component in C frame (-)') + plt.xlabel("Time (min)") + plt.title("Third unit Vec component in C frame (-)") # plt.figure(203, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.figure(203, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(t, x[:, 4]*m2km, ".", color = colorList[1]) - plt.xlabel('Time (min)') - plt.title('Norm in C frame (km)') - + plt.figure(203, figsize=(3.5, 2.0), facecolor="w", edgecolor="k") + plt.plot(t, x[:, 4] * m2km, ".", color=colorList[1]) + plt.xlabel("Time (min)") + plt.title("Norm in C frame (km)") def imgProcVsExp(true, centers, radii, size): - - - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) - t= true[:, 0]*ns2min - centerline = np.ones([len(t),2])*(size/2+0.5) + t = true[:, 0] * ns2min + centerline = np.ones([len(t), 2]) * (size / 2 + 0.5) found = 0 for i in range(len(t)): - if np.abs(centers[i,1]) <1E-10 and np.abs(centers[i,2]) < 1E-10: - centers[i,1:3] = np.array([np.nan,np.nan]) - radii[i,1] = np.nan + if np.abs(centers[i, 1]) < 1e-10 and np.abs(centers[i, 2]) < 1e-10: + centers[i, 1:3] = np.array([np.nan, np.nan]) + radii[i, 1] = np.nan else: found = 1 if found == 0: - centerline[i,:] = np.array([np.nan,np.nan]) + centerline[i, :] = np.array([np.nan, np.nan]) - plt.figure(301, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') + plt.figure(301, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") # plt.figure(301, figsize=(3.5, 2.), facecolor='w', edgecolor='k') plt.rcParams["font.size"] = "8" - plt.plot(t, true[:, 1], "+", label='Truth Xpix', color = colorList[1]) - plt.plot(t, centerline[:,0], "--", label='X-center', color = colorList[9]) - plt.plot(t, centers[:, 1], '.', label = "ImagProc Xpix", color = colorList[5], alpha=0.7) - plt.legend(loc='best') + plt.plot(t, true[:, 1], "+", label="Truth Xpix", color=colorList[1]) + plt.plot(t, centerline[:, 0], "--", label="X-center", color=colorList[9]) + plt.plot( + t, centers[:, 1], ".", label="ImagProc Xpix", color=colorList[5], alpha=0.7 + ) + plt.legend(loc="best") try: - plt.ylim([centerline[-1,1]-15, centerline[-1,1]+15]) + plt.ylim([centerline[-1, 1] - 15, centerline[-1, 1] + 15]) except ValueError: pass - plt.ylabel('X (px)') - plt.xlabel('Time (min)') - #plt.savefig('Xpix.pdf') + plt.ylabel("X (px)") + plt.xlabel("Time (min)") + # plt.savefig('Xpix.pdf') - plt.figure(302, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') + plt.figure(302, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") # plt.figure(302, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(t, true[:, 2], "+", label='Truth Ypix', color = colorList[1]) - plt.plot(t, centerline[:,1], "--", label='Y-center', color = colorList[9]) - plt.plot(t, centers[:, 2], '.', label = "ImagProc Ypix", color = colorList[5], alpha=0.7) - plt.legend(loc='best') + plt.plot(t, true[:, 2], "+", label="Truth Ypix", color=colorList[1]) + plt.plot(t, centerline[:, 1], "--", label="Y-center", color=colorList[9]) + plt.plot( + t, centers[:, 2], ".", label="ImagProc Ypix", color=colorList[5], alpha=0.7 + ) + plt.legend(loc="best") try: - plt.ylim([centerline[-1,1]-15, centerline[-1,1]+15]) + plt.ylim([centerline[-1, 1] - 15, centerline[-1, 1] + 15]) except ValueError: pass - plt.ylabel('Y (px)') - plt.xlabel('Time (min)') - #plt.savefig('Ypix.pdf') + plt.ylabel("Y (px)") + plt.xlabel("Time (min)") + # plt.savefig('Ypix.pdf') - plt.figure(312, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') + plt.figure(312, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") # plt.figure(312, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(t, true[:, 3], "+", label=r'Truth $\rho$', color = colorList[1]) - plt.plot(t, radii[:, 1],'.', label = r"ImagProc $\rho$", color = colorList[5], alpha=0.7) - plt.legend(loc='best') - plt.ylabel(r'$\rho$ (px)') - plt.xlabel('Time (min)') - #plt.savefig('Rhopix.pdf') - + plt.plot(t, true[:, 3], "+", label=r"Truth $\rho$", color=colorList[1]) + plt.plot( + t, radii[:, 1], ".", label=r"ImagProc $\rho$", color=colorList[5], alpha=0.7 + ) + plt.legend(loc="best") + plt.ylabel(r"$\rho$ (px)") + plt.xlabel("Time (min)") + # plt.savefig('Rhopix.pdf') - plt.figure(303, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') + plt.figure(303, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") # plt.figure(303, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(t, true[:, 1] - centers[:, 1], ".", label=r'$\mathrm{X}_\mathrm{true} - \mathrm{X}_\mathrm{hough}$', color = colorList[1]) - plt.legend(loc='best') - plt.ylabel('X error (px)') + plt.plot( + t, + true[:, 1] - centers[:, 1], + ".", + label=r"$\mathrm{X}_\mathrm{true} - \mathrm{X}_\mathrm{hough}$", + color=colorList[1], + ) + plt.legend(loc="best") + plt.ylabel("X error (px)") plt.grid() - #plt.savefig('Xerror.pdf') + # plt.savefig('Xerror.pdf') - plt.figure(304, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') + plt.figure(304, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") # plt.figure(304, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(t, true[:, 2] - centers[:, 2], ".", label=r'$\mathrm{Y}_\mathrm{true} - \mathrm{Y}_\mathrm{hough}$', color = colorList[1]) - plt.legend(loc='best') - plt.ylabel('Y error (px)') + plt.plot( + t, + true[:, 2] - centers[:, 2], + ".", + label=r"$\mathrm{Y}_\mathrm{true} - \mathrm{Y}_\mathrm{hough}$", + color=colorList[1], + ) + plt.legend(loc="best") + plt.ylabel("Y error (px)") plt.grid() - #plt.savefig('Yerror.pdf') + # plt.savefig('Yerror.pdf') - plt.figure(305, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') + plt.figure(305, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") # plt.figure(305, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(t, true[:, 3] - radii[:, 1], ".", label=r'$\mathrm{\rho}_\mathrm{true} - \mathrm{\rho}_\mathrm{hough}$', color = colorList[1]) - plt.legend(loc='best') - plt.ylabel('Radius error (px)') + plt.plot( + t, + true[:, 3] - radii[:, 1], + ".", + label=r"$\mathrm{\rho}_\mathrm{true} - \mathrm{\rho}_\mathrm{hough}$", + color=colorList[1], + ) + plt.legend(loc="best") + plt.ylabel("Radius error (px)") plt.xlabel("Time (min)") plt.grid() - #plt.savefig('Rhoerror.pdf') + # plt.savefig('Rhoerror.pdf') def plotPostFitResiduals(Res, noise): + MeasNoise = np.zeros([len(Res[:, 0]), 3]) + Noiselast = np.zeros([3, 3]) - MeasNoise = np.zeros([len(Res[:,0]), 3]) - Noiselast = np.zeros([3,3]) - - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(10.) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (10.0) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) - t= np.zeros(len(Res[:,0])) - for i in range(len(Res[:,0])): - t[i] = Res[i, 0]*ns2min + t = np.zeros(len(Res[:, 0])) + for i in range(len(Res[:, 0])): + t[i] = Res[i, 0] * ns2min if isinstance(noise, float): - MeasNoise[i, :] = np.array([3*np.sqrt(noise),3*np.sqrt(noise),3*np.sqrt(noise)]) + MeasNoise[i, :] = np.array( + [3 * np.sqrt(noise), 3 * np.sqrt(noise), 3 * np.sqrt(noise)] + ) else: if not np.isnan(noise[i, 1]): Noiselast = noise[i, 1:].reshape([3, 3]) - MeasNoise[i, :] = np.array([3*np.sqrt(Noiselast[0,0]), 3*np.sqrt(Noiselast[1,1]), 3*np.sqrt(Noiselast[2,2])])/5 + MeasNoise[i, :] = ( + np.array( + [ + 3 * np.sqrt(Noiselast[0, 0]), + 3 * np.sqrt(Noiselast[1, 1]), + 3 * np.sqrt(Noiselast[2, 2]), + ] + ) + / 5 + ) if np.isnan(noise[i, 1]): - MeasNoise[i, :] = np.array([3*np.sqrt(Noiselast[0,0]), 3*np.sqrt(Noiselast[1,1]), 3*np.sqrt(Noiselast[2,2])])/5 + MeasNoise[i, :] = ( + np.array( + [ + 3 * np.sqrt(Noiselast[0, 0]), + 3 * np.sqrt(Noiselast[1, 1]), + 3 * np.sqrt(Noiselast[2, 2]), + ] + ) + / 5 + ) # Don't plot zero values, since they mean that no measurement is taken - for j in range(len(Res[0,:])-1): - if -1E-10 < Res[i,j+1] < 1E-10: - Res[i, j+1] = np.nan - if len(Res[0,:])-1 == 3: - plt.figure(401, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , Res[:, 1]*m2km, ".", label='Residual', color = colorList[1]) - plt.plot(t , MeasNoise[:,0]*m2km, '--', label=r'Noise ($3\sigma$)', color = colorList[8]) - plt.plot(t , -MeasNoise[:,0]*m2km, '--', color = colorList[8]) + for j in range(len(Res[0, :]) - 1): + if -1e-10 < Res[i, j + 1] < 1e-10: + Res[i, j + 1] = np.nan + if len(Res[0, :]) - 1 == 3: + plt.figure(401, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, Res[:, 1] * m2km, ".", label="Residual", color=colorList[1]) + plt.plot( + t, + MeasNoise[:, 0] * m2km, + "--", + label=r"Noise ($3\sigma$)", + color=colorList[8], + ) + plt.plot(t, -MeasNoise[:, 0] * m2km, "--", color=colorList[8]) plt.legend(loc=2) - max = np.amax(MeasNoise[:,0]) - if max >1E-15: - plt.ylim([-2*max*m2km, 2*max*m2km]) - plt.ylabel(r'$r_1$ Measured (km)') + max = np.amax(MeasNoise[:, 0]) + if max > 1e-15: + plt.ylim([-2 * max * m2km, 2 * max * m2km]) + plt.ylabel(r"$r_1$ Measured (km)") plt.xlabel("Time (min)") plt.grid() - #plt.savefig('Res1.pdf') - - plt.figure(402, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , Res[:, 2]*m2km, ".", color = colorList[1]) - plt.plot(t , MeasNoise[:,1]*m2km, '--', color = colorList[8]) - plt.plot(t , -MeasNoise[:,1]*m2km, '--', color = colorList[8]) - max = np.amax(MeasNoise[:,1]) - if max >1E-15: - plt.ylim([-2*max*m2km, 2*max*m2km]) - plt.ylabel(r'$r_2$ Measured (km)') + # plt.savefig('Res1.pdf') + + plt.figure(402, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, Res[:, 2] * m2km, ".", color=colorList[1]) + plt.plot(t, MeasNoise[:, 1] * m2km, "--", color=colorList[8]) + plt.plot(t, -MeasNoise[:, 1] * m2km, "--", color=colorList[8]) + max = np.amax(MeasNoise[:, 1]) + if max > 1e-15: + plt.ylim([-2 * max * m2km, 2 * max * m2km]) + plt.ylabel(r"$r_2$ Measured (km)") plt.xlabel("Time (min)") plt.grid() - #plt.savefig('Res2.pdf') - - - plt.figure(403, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , Res[:, 3]*m2km, ".", color = colorList[1]) - plt.plot(t , MeasNoise[:,2]*m2km, '--', color = colorList[8]) - plt.plot(t , -MeasNoise[:,2]*m2km, '--', color = colorList[8]) - max = np.amax(MeasNoise[:,2]) - if max >1E-15: - plt.ylim([-2*max*m2km, 2*max*m2km]) - plt.ylabel(r'$r_3$ Measured (km)') + # plt.savefig('Res2.pdf') + + plt.figure(403, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, Res[:, 3] * m2km, ".", color=colorList[1]) + plt.plot(t, MeasNoise[:, 2] * m2km, "--", color=colorList[8]) + plt.plot(t, -MeasNoise[:, 2] * m2km, "--", color=colorList[8]) + max = np.amax(MeasNoise[:, 2]) + if max > 1e-15: + plt.ylim([-2 * max * m2km, 2 * max * m2km]) + plt.ylabel(r"$r_3$ Measured (km)") plt.xlabel("Time (min)") plt.grid() - #plt.savefig('Res3.pdf') - - - if len(Res[0,:])-1 == 6: - plt.figure(405, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , Res[:, 1], "b.") - plt.plot(t , MeasNoise[:,0], 'r--') - plt.plot(t , -MeasNoise[:,0], 'r--') - plt.legend(loc='best') - plt.title('X-pixel component') + # plt.savefig('Res3.pdf') + + if len(Res[0, :]) - 1 == 6: + plt.figure(405, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, Res[:, 1], "b.") + plt.plot(t, MeasNoise[:, 0], "r--") + plt.plot(t, -MeasNoise[:, 0], "r--") + plt.legend(loc="best") + plt.title("X-pixel component") plt.grid() - plt.figure(406, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , Res[:, 4], "b.") - plt.plot(t , MeasNoise[:,0], 'r--') - plt.plot(t , -MeasNoise[:,0], 'r--') - plt.title('X-bias component') + plt.figure(406, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, Res[:, 4], "b.") + plt.plot(t, MeasNoise[:, 0], "r--") + plt.plot(t, -MeasNoise[:, 0], "r--") + plt.title("X-bias component") plt.grid() - plt.figure(407, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , Res[:, 2], "b.") - plt.plot(t , MeasNoise[:,1], 'r--') - plt.plot(t , -MeasNoise[:,1], 'r--') - plt.title('Y-pixel component') + plt.figure(407, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, Res[:, 2], "b.") + plt.plot(t, MeasNoise[:, 1], "r--") + plt.plot(t, -MeasNoise[:, 1], "r--") + plt.title("Y-pixel component") plt.grid() - plt.figure(408, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , Res[:, 5], "b.") - plt.plot(t , MeasNoise[:,1], 'r--') - plt.plot(t , -MeasNoise[:,1], 'r--') - plt.xlabel('Time (min)') - plt.title('Y-bias component') + plt.figure(408, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, Res[:, 5], "b.") + plt.plot(t, MeasNoise[:, 1], "r--") + plt.plot(t, -MeasNoise[:, 1], "r--") + plt.xlabel("Time (min)") + plt.title("Y-bias component") plt.grid() - plt.figure(409, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , Res[:, 3], "b.") - plt.plot(t , MeasNoise[:,2], 'r--') - plt.plot(t , -MeasNoise[:,2], 'r--') - plt.xlabel('Time (min)') - plt.ylabel('Rho-component') + plt.figure(409, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, Res[:, 3], "b.") + plt.plot(t, MeasNoise[:, 2], "r--") + plt.plot(t, -MeasNoise[:, 2], "r--") + plt.xlabel("Time (min)") + plt.ylabel("Rho-component") plt.grid() - plt.figure(410, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.plot(t , Res[:, 6], "b.") - plt.plot(t , MeasNoise[:,2], 'r--') - plt.plot(t , -MeasNoise[:,2], 'r--') - plt.xlabel('Time (min)') - plt.ylabel('Rho-bias component') + plt.figure(410, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") + plt.plot(t, Res[:, 6], "b.") + plt.plot(t, MeasNoise[:, 2], "r--") + plt.plot(t, -MeasNoise[:, 2], "r--") + plt.xlabel("Time (min)") + plt.ylabel("Rho-bias component") plt.grid() + def plot_cirlces(centers, radii, validity, resolution): circleIndx = [] - for i in range(len(centers[:,0])): + for i in range(len(centers[:, 0])): if validity[i, 1] == 1: circleIndx.append(i) - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(len(circleIndx)+1) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (len(circleIndx) + 1) colorList = [] for i in range(len(circleIndx)): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) - plt.figure(500, figsize=(3, 3), facecolor='w', edgecolor='k') + plt.figure(500, figsize=(3, 3), facecolor="w", edgecolor="k") ax = plt.gca() for i in range(len(circleIndx)): - if i%30 ==0: - ell_xy = Ellipse(xy=(centers[circleIndx[i],1], centers[circleIndx[i],2]), - width=radii[circleIndx[i],1], height=radii[circleIndx[i],1], - angle=0, linestyle='-', linewidth=3, color=colorList[i], alpha=0.7) - ell_xy.set(facecolor='none') + if i % 30 == 0: + ell_xy = Ellipse( + xy=(centers[circleIndx[i], 1], centers[circleIndx[i], 2]), + width=radii[circleIndx[i], 1], + height=radii[circleIndx[i], 1], + angle=0, + linestyle="-", + linewidth=3, + color=colorList[i], + alpha=0.7, + ) + ell_xy.set(facecolor="none") ax.add_patch(ell_xy) ax.invert_yaxis() ax.set_xlim(0, resolution[0]) - ax.set_ylim(resolution[1],0) - plt.xlabel('X-axis (px)') - plt.ylabel('Y-axis (px)') + ax.set_ylim(resolution[1], 0) + plt.xlabel("X-axis (px)") + plt.ylabel("Y-axis (px)") plt.axis("equal") - #plt.savefig('Circles.pdf') + # plt.savefig('Circles.pdf') -def plot_limb(limbPoints, numLimb, validity, resolution): +def plot_limb(limbPoints, numLimb, validity, resolution): indx = [] time = [] - for i in range(len(limbPoints[:,0])): + for i in range(len(limbPoints[:, 0])): if validity[i, 1] == 1: indx.append(i) numBerPoints = [] - colorsInt = len(mpl.pyplot.get_cmap("inferno").colors)/(len(indx)+1) + colorsInt = len(mpl.pyplot.get_cmap("inferno").colors) / (len(indx) + 1) colorList = [] for i in range(len(indx)): - colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i*colorsInt)]) + colorList.append(mpl.pyplot.get_cmap("inferno").colors[int(i * colorsInt)]) numBerPoints.append(numLimb[indx[i], 1]) - time.append(numLimb[indx[i], 0]*1E-9/60) + time.append(numLimb[indx[i], 0] * 1e-9 / 60) - limbX = np.zeros([len(indx), 2*2000]) - limbY = np.zeros([len(indx), 2*2000]) + limbX = np.zeros([len(indx), 2 * 2000]) + limbY = np.zeros([len(indx), 2 * 2000]) for i in range(len(indx)): for j in range(1, int(numLimb[indx[i], 1])): - if np.abs(limbPoints[indx[i], 2*j] + limbPoints[indx[i], 2*j-1])>1E-1: - limbX[i,j] = limbPoints[indx[i], 2*j-1] - limbY[i,j] = limbPoints[indx[i], 2*j] - plt.figure(600, figsize=(3, 3), facecolor='w', edgecolor='k') + if ( + np.abs(limbPoints[indx[i], 2 * j] + limbPoints[indx[i], 2 * j - 1]) + > 1e-1 + ): + limbX[i, j] = limbPoints[indx[i], 2 * j - 1] + limbY[i, j] = limbPoints[indx[i], 2 * j] + plt.figure(600, figsize=(3, 3), facecolor="w", edgecolor="k") ax = plt.gca() for i in range(len(indx)): - if i%30 ==0: - ax.scatter(limbX[i,:int(numLimb[indx[i],1])-1], limbY[i,:int(numLimb[indx[i],1])-1], color=colorList[i], marker='.', alpha=0.2) + if i % 30 == 0: + ax.scatter( + limbX[i, : int(numLimb[indx[i], 1]) - 1], + limbY[i, : int(numLimb[indx[i], 1]) - 1], + color=colorList[i], + marker=".", + alpha=0.2, + ) ax.invert_yaxis() ax.set_xlim(0, resolution[0]) - ax.set_ylim(resolution[1],0) - plt.xlabel('X-axis (px)') - plt.ylabel('Y-axis (px)') + ax.set_ylim(resolution[1], 0) + plt.xlabel("X-axis (px)") + plt.ylabel("Y-axis (px)") plt.axis("equal") - #plt.savefig('Limbs.pdf') + # plt.savefig('Limbs.pdf') - plt.figure(601, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') + plt.figure(601, figsize=(2.7, 1.6), facecolor="w", edgecolor="k") plt.plot(time, numBerPoints, color=colorList[1]) - plt.xlabel('Time (min)') - plt.ylabel('Limb Size (px)') - #plt.savefig('LimbPoints.pdf') + plt.xlabel("Time (min)") + plt.ylabel("Limb Size (px)") + # plt.savefig('LimbPoints.pdf') + class AnimatedCircles(object): """An animated scatter plot using matplotlib.animations.FuncAnimation.""" + def __init__(self, size, centers, radii, validity): self.sensorSize = size self.idx = 0 self.i = 0 # Setup the figure and axes... - self.fig, self.ax = plt.subplots(num='Circles Animation')#, figsize=(10, 10), dpi=80, facecolor='w') + self.fig, self.ax = plt.subplots( + num="Circles Animation" + ) # , figsize=(10, 10), dpi=80, facecolor='w') self.ax.invert_yaxis() self.ax.set_xlim(0, self.sensorSize[0]) - self.ax.set_ylim(self.sensorSize[1],0) + self.ax.set_ylim(self.sensorSize[1], 0) self.ax.set_aspect("equal") # Then setup FuncAnimation. self.setData(centers, radii, validity) - self.ani = mpl.animation.FuncAnimation(self.fig, self.update, interval=100, - init_func=self.setup_plot, frames=len(self.circleIndx), blit=True) - self.ani.save('../../TestImages/circlesAnimated.gif', writer='imagemagick', fps=60) + self.ani = mpl.animation.FuncAnimation( + self.fig, + self.update, + interval=100, + init_func=self.setup_plot, + frames=len(self.circleIndx), + blit=True, + ) + self.ani.save( + "../../TestImages/circlesAnimated.gif", writer="imagemagick", fps=60 + ) def setData(self, centers, radii, validity): self.circleIndx = [] - for i in range(len(centers[:,0])): + for i in range(len(centers[:, 0])): if validity[i, 1] == 1: self.circleIndx.append(i) self.centers = np.zeros([len(self.circleIndx), 3]) self.radii = np.zeros([len(self.circleIndx), 2]) for i in range(len(self.circleIndx)): - self.centers[i,0] = centers[self.circleIndx[i],0] + self.centers[i, 0] = centers[self.circleIndx[i], 0] self.centers[i, 1:] = centers[self.circleIndx[i], 1:3] - self.radii[i,0] = centers[self.circleIndx[i],0] + self.radii[i, 0] = centers[self.circleIndx[i], 0] self.radii[i, 1] = radii[self.circleIndx[i], 1] - self.colorList = mpl.pyplot.get_cmap("inferno").colors - + self.colorList = mpl.pyplot.get_cmap("inferno").colors def setup_plot(self): """Initial drawing of the scatter plot.""" - self.scat = self.ax.scatter(self.centers[0,1], self.centers[0,2], c=self.colorList[0], s=self.radii[0,1]) + self.scat = self.ax.scatter( + self.centers[0, 1], + self.centers[0, 2], + c=self.colorList[0], + s=self.radii[0, 1], + ) # For FuncAnimation's sake, we need to return the artist we'll be using # Note that it expects a sequence of artists, thus the trailing comma. - return self.scat, + return (self.scat,) def data_stream(self, i): while self.idx < len(self.circleIndx): - xy = np.array([[self.sensorSize[0]/2+0.5, self.sensorSize[0]/2+0.5],[self.centers[i,1], self.centers[i,2]],[self.centers[i,1], self.centers[i,2]]]) - s = np.array([1, 1, self.radii[i,1]]) - c = [self.colorList[-1], self.colorList[i],self.colorList[i]] - yield np.c_[xy[:,0], xy[:,1], s[:]], c + xy = np.array( + [ + [self.sensorSize[0] / 2 + 0.5, self.sensorSize[0] / 2 + 0.5], + [self.centers[i, 1], self.centers[i, 2]], + [self.centers[i, 1], self.centers[i, 2]], + ] + ) + s = np.array([1, 1, self.radii[i, 1]]) + c = [self.colorList[-1], self.colorList[i], self.colorList[i]] + yield np.c_[xy[:, 0], xy[:, 1], s[:]], c def update(self, i): """Update the scatter plot.""" @@ -1032,97 +1319,117 @@ def update(self, i): # Set x and y data... self.scat.set_offsets(data[:, :2]) # Set sizes... - self.scat.set_sizes((data[:, 2]/2.)**2.) + self.scat.set_sizes((data[:, 2] / 2.0) ** 2.0) # Set colors.. # self.scat.set_array(data[:, 3]) self.scat.set_edgecolor(color) self.scat.set_facecolor("none") # We need to return the updated artist for FuncAnimation to draw.. # Note that it expects a sequence of artists, thus the trailing comma. - return self.scat, + return (self.scat,) -def StateErrorCovarPlot(x, Pflat, FilterType, show_plots): - nstates = int(np.sqrt(len(Pflat[0,:])-1)) +def StateErrorCovarPlot(x, Pflat, FilterType, show_plots): + nstates = int(np.sqrt(len(Pflat[0, :]) - 1)) mpl.rc("figure", figsize=(3, 1.8)) - colorsInt = int(len(mpl.pyplot.get_cmap().colors)/10) + colorsInt = int(len(mpl.pyplot.get_cmap().colors) / 10) colorList = [] for i in range(10): - colorList.append(mpl.pyplot.get_cmap().colors[i*colorsInt]) + colorList.append(mpl.pyplot.get_cmap().colors[i * colorsInt]) - P = np.zeros([len(Pflat[:,0]),nstates,nstates]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*ns2min - P[i,:,:] = Pflat[i,1:37].reshape([nstates,nstates]) - for j in range(len(P[0,0,:])): - P[i,j,j] = np.sqrt(P[i,j,j]) + P = np.zeros([len(Pflat[:, 0]), nstates, nstates]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * ns2min + P[i, :, :] = Pflat[i, 1:37].reshape([nstates, nstates]) + for j in range(len(P[0, 0, :])): + P[i, j, j] = np.sqrt(P[i, j, j]) for i in range(3): - if i ==0: - plt.figure(num=None, facecolor='w', edgecolor='k') - plt.plot(t , x[:, i+1], color = colorList[0], label='Error') - plt.plot(t , 3 * P[:, i, i],color = colorList[-1], linestyle = '--', label=r'Covar (3-$\sigma$)') - plt.plot(t , -3 * P[:, i, i], color = colorList[-1], linestyle = '--') - plt.legend(loc='best') - plt.ylabel('$d_' + str(i+1) + '$ Error (-)') - plt.ylim([-0.1,0.1]) - plt.xlabel('Time (min)') - unitTestSupport.saveFigurePDF('StateCovarHeading'+ FilterType + str(i) , plt, './') + if i == 0: + plt.figure(num=None, facecolor="w", edgecolor="k") + plt.plot(t, x[:, i + 1], color=colorList[0], label="Error") + plt.plot( + t, + 3 * P[:, i, i], + color=colorList[-1], + linestyle="--", + label=r"Covar (3-$\sigma$)", + ) + plt.plot(t, -3 * P[:, i, i], color=colorList[-1], linestyle="--") + plt.legend(loc="best") + plt.ylabel("$d_" + str(i + 1) + "$ Error (-)") + plt.ylim([-0.1, 0.1]) + plt.xlabel("Time (min)") + unitTestSupport.saveFigurePDF( + "StateCovarHeading" + FilterType + str(i), plt, "./" + ) if show_plots: plt.show() plt.close("all") else: - plt.figure(num=None, facecolor='w', edgecolor='k') - plt.plot(t , x[:, i+1], color = colorList[0]) - plt.plot(t , 3 * P[:, i, i],color = colorList[-1], linestyle = '--') - plt.plot(t , -3 * P[:, i, i], color = colorList[-1], linestyle = '--') - plt.ylabel('$d_' + str(i+1) + '$ Error (-)') - plt.xlabel('Time (min)') - plt.ylim([-0.1,0.1]) - unitTestSupport.saveFigurePDF('StateCovarHeading'+ FilterType + str(i) , plt, './') + plt.figure(num=None, facecolor="w", edgecolor="k") + plt.plot(t, x[:, i + 1], color=colorList[0]) + plt.plot(t, 3 * P[:, i, i], color=colorList[-1], linestyle="--") + plt.plot(t, -3 * P[:, i, i], color=colorList[-1], linestyle="--") + plt.ylabel("$d_" + str(i + 1) + "$ Error (-)") + plt.xlabel("Time (min)") + plt.ylim([-0.1, 0.1]) + unitTestSupport.saveFigurePDF( + "StateCovarHeading" + FilterType + str(i), plt, "./" + ) if show_plots: plt.show() plt.close("all") - if nstates>3: - for i in range(3,nstates): + if nstates > 3: + for i in range(3, nstates): if i == 3: - plt.figure(num=None, facecolor='w', edgecolor='k') - plt.plot(t, x[:, i + 1], color = colorList[0], label='Error') - plt.plot(t, 3 * P[:, i, i], color = colorList[-1],linestyle = '--', label=r'Covar (3-$\sigma$)') - plt.plot(t, -3 * P[:, i, i], color = colorList[-1],linestyle = '--') - plt.ylim([-0.0007,0.0007]) + plt.figure(num=None, facecolor="w", edgecolor="k") + plt.plot(t, x[:, i + 1], color=colorList[0], label="Error") + plt.plot( + t, + 3 * P[:, i, i], + color=colorList[-1], + linestyle="--", + label=r"Covar (3-$\sigma$)", + ) + plt.plot(t, -3 * P[:, i, i], color=colorList[-1], linestyle="--") + plt.ylim([-0.0007, 0.0007]) # plt.legend(loc='best') if nstates == 5: - plt.ylabel(r'$\omega_' + str(i - 1) + r'$ Error (-)') + plt.ylabel(r"$\omega_" + str(i - 1) + r"$ Error (-)") else: - plt.ylabel(r'$d\'_' + str(i-2) + r'$ Error (-)') - plt.xlabel('Time (min)') - unitTestSupport.saveFigurePDF('StateCovarRate' + FilterType + str(i), plt, './') + plt.ylabel(r"$d\'_" + str(i - 2) + r"$ Error (-)") + plt.xlabel("Time (min)") + unitTestSupport.saveFigurePDF( + "StateCovarRate" + FilterType + str(i), plt, "./" + ) if show_plots: plt.show() plt.close("all") else: - plt.figure(num=None, facecolor='w', edgecolor='k') - plt.plot(t, x[:, i + 1], color = colorList[0]) - plt.plot(t, 3 * P[:, i, i], color = colorList[-1],linestyle = '--') - plt.plot(t, -3 * P[:, i, i], color = colorList[-1],linestyle = '--') - plt.ylim([-0.0007,0.0007]) + plt.figure(num=None, facecolor="w", edgecolor="k") + plt.plot(t, x[:, i + 1], color=colorList[0]) + plt.plot(t, 3 * P[:, i, i], color=colorList[-1], linestyle="--") + plt.plot(t, -3 * P[:, i, i], color=colorList[-1], linestyle="--") + plt.ylim([-0.0007, 0.0007]) if nstates == 5: - plt.ylabel(r'$\omega_' + str(i - 1) + r'$ Error (-)') + plt.ylabel(r"$\omega_" + str(i - 1) + r"$ Error (-)") else: - plt.ylabel(r'$d\'_' + str(i-2) + r'$ Error (-)') - plt.xlabel('Time (min)') - unitTestSupport.saveFigurePDF('StateCovarRate' + FilterType + str(i), plt, './') + plt.ylabel(r"$d\'_" + str(i - 2) + r"$ Error (-)") + plt.xlabel("Time (min)") + unitTestSupport.saveFigurePDF( + "StateCovarRate" + FilterType + str(i), plt, "./" + ) if show_plots: plt.show() plt.close("all") plt.close("all") -def PostFitResiduals(Res, covar_B, FilterType, show_plots): +def PostFitResiduals(Res, covar_B, FilterType, show_plots): mpl.rc("figure", figsize=(3, 1.8)) colorsInt = int(len(mpl.pyplot.get_cmap().colors) / 10) @@ -1136,62 +1443,92 @@ def PostFitResiduals(Res, covar_B, FilterType, show_plots): constantVal = np.array([np.nan] * 4) for i in range(len(Res[:, 0])): t[i] = Res[i, 0] * ns2min - if np.abs(covar_B[i, 1]) < 1E-15 and prevNoise[0] == 0: - MeasNoise[i,:] = np.full(3, np.nan) - elif np.abs(covar_B[i, 1]) < 1E-15 and prevNoise[0] != 0: - MeasNoise[i,:] = prevNoise + if np.abs(covar_B[i, 1]) < 1e-15 and prevNoise[0] == 0: + MeasNoise[i, :] = np.full(3, np.nan) + elif np.abs(covar_B[i, 1]) < 1e-15 and prevNoise[0] != 0: + MeasNoise[i, :] = prevNoise else: - MeasNoise[i,0] = 3 * np.sqrt(covar_B[i, 1]) - MeasNoise[i,1] = 3 * np.sqrt(covar_B[i, 1 + 4]) - MeasNoise[i,2] = 3 * np.sqrt(covar_B[i, 1 + 8]) - prevNoise = MeasNoise[i,:] + MeasNoise[i, 0] = 3 * np.sqrt(covar_B[i, 1]) + MeasNoise[i, 1] = 3 * np.sqrt(covar_B[i, 1 + 4]) + MeasNoise[i, 2] = 3 * np.sqrt(covar_B[i, 1 + 8]) + prevNoise = MeasNoise[i, :] - # Don't plot constant values, they mean no measurement is taken + # Don't plot constant values, they mean no measurement is taken if i > 0: for j in range(1, 4): constantRes = np.abs(Res[i, j] - Res[i - 1, j]) - if constantRes < 1E-10 or np.abs(constantVal[j - 1] - Res[i, j]) < 1E-10: + if ( + constantRes < 1e-10 + or np.abs(constantVal[j - 1] - Res[i, j]) < 1e-10 + ): constantVal[j - 1] = Res[i, j] Res[i, j] = np.nan for i in range(3): if i == 0: - plt.figure(num=None, dpi=80, facecolor='w', edgecolor='k') - plt.plot(t, Res[:, i + 1], color=colorList[0], linestyle='', marker='.', label='Residual') - plt.plot(t, MeasNoise[:,i], color=colorList[-1], linestyle="--", label=r'Noise (3-$\sigma$)') - plt.plot(t, -MeasNoise[:,i], color=colorList[-1], linestyle="--", ) - plt.legend(loc='best') - plt.ylabel('$r_' + str(i + 1) + '$ (-)') - plt.xlabel('Time (min)') - plt.ylim([-2*MeasNoise[-1,i], 2*MeasNoise[-1,i]]) - unitTestSupport.saveFigurePDF('PostFit' + FilterType + str(i), plt, './') + plt.figure(num=None, dpi=80, facecolor="w", edgecolor="k") + plt.plot( + t, + Res[:, i + 1], + color=colorList[0], + linestyle="", + marker=".", + label="Residual", + ) + plt.plot( + t, + MeasNoise[:, i], + color=colorList[-1], + linestyle="--", + label=r"Noise (3-$\sigma$)", + ) + plt.plot( + t, + -MeasNoise[:, i], + color=colorList[-1], + linestyle="--", + ) + plt.legend(loc="best") + plt.ylabel("$r_" + str(i + 1) + "$ (-)") + plt.xlabel("Time (min)") + plt.ylim([-2 * MeasNoise[-1, i], 2 * MeasNoise[-1, i]]) + unitTestSupport.saveFigurePDF("PostFit" + FilterType + str(i), plt, "./") if show_plots: plt.show() plt.close("all") else: - plt.figure(num=None, dpi=80, facecolor='w', edgecolor='k') - plt.plot(t, Res[:, i + 1], color=colorList[0], linestyle='', marker='.') - plt.plot(t, MeasNoise[:,i], color=colorList[-1], linestyle="--") - plt.plot(t, -MeasNoise[:,i], color=colorList[-1], linestyle="--", ) - plt.ylabel('$r_' + str(i + 1) + '$ (-)') - plt.xlabel('Time min') - plt.ylim([-2*MeasNoise[-1,i], 2*MeasNoise[-1,i]]) - unitTestSupport.saveFigurePDF('PostFit' + FilterType + str(i), plt, './') + plt.figure(num=None, dpi=80, facecolor="w", edgecolor="k") + plt.plot(t, Res[:, i + 1], color=colorList[0], linestyle="", marker=".") + plt.plot(t, MeasNoise[:, i], color=colorList[-1], linestyle="--") + plt.plot( + t, + -MeasNoise[:, i], + color=colorList[-1], + linestyle="--", + ) + plt.ylabel("$r_" + str(i + 1) + "$ (-)") + plt.xlabel("Time min") + plt.ylim([-2 * MeasNoise[-1, i], 2 * MeasNoise[-1, i]]) + unitTestSupport.saveFigurePDF("PostFit" + FilterType + str(i), plt, "./") if show_plots: plt.show() plt.close("all") plt.close("all") + class AnimatedLimb(object): """An animated scatter plot using matplotlib.animations.FuncAnimation.""" + def __init__(self, size, limb, centers, validity): self.stream = self.data_stream() self.sensorSize = size self.idx = 0 self.i = 0 # Setup the figure and axes... - self.fig, self.ax = plt.subplots(num='Limb Animation') # , figsize=(10, 10), dpi=80, facecolor='w') + self.fig, self.ax = plt.subplots( + num="Limb Animation" + ) # , figsize=(10, 10), dpi=80, facecolor='w') self.ax.invert_yaxis() self.ax.set_xlim(0, self.sensorSize[0]) self.ax.set_ylim(self.sensorSize[1], 0) @@ -1201,41 +1538,50 @@ def __init__(self, size, limb, centers, validity): # Setup the figure and axes... self.fig, self.ax = plt.subplots() # Then setup FuncAnimation. - self.ani = mpl.animation.FuncAnimation(self.fig, self.update, interval=100, - init_func=self.setup_plot, blit=True) + self.ani = mpl.animation.FuncAnimation( + self.fig, self.update, interval=100, init_func=self.setup_plot, blit=True + ) def setup_plot(self): """Initial drawing of the scatter plot.""" - self.scat = self.ax.scatter(self.centers[0,1], self.centers[0,2], c=self.colorList[0], s=self.radii[0,1]) + self.scat = self.ax.scatter( + self.centers[0, 1], + self.centers[0, 2], + c=self.colorList[0], + s=self.radii[0, 1], + ) # For FuncAnimation's sake, we need to return the artist we'll be using # Note that it expects a sequence of artists, thus the trailing comma. - return self.scat, - + return (self.scat,) def setData(self, centers, limbPoints, validity): self.pointIndx = [] - for i in range(len(limbPoints[:,0])): + for i in range(len(limbPoints[:, 0])): if validity[i, 1] == 1: self.pointIndx.append(i) self.centers = np.zeros([len(self.pointIndx), 2]) - self.points = np.zeros([len(self.pointIndx), 2*1000]) + self.points = np.zeros([len(self.pointIndx), 2 * 1000]) for i in range(len(self.pointIndx)): self.centers[i, 1:] = centers[self.pointIndx[i], 1:2] - self.points[i,0] = limbPoints[self.pointIndx[i],1:] - self.colorList = mpl.pyplot.get_cmap("inferno").colors - + self.points[i, 0] = limbPoints[self.pointIndx[i], 1:] + self.colorList = mpl.pyplot.get_cmap("inferno").colors def data_stream(self, i): """Generate a random walk (brownian motion). Data is scaled to produce a soft "flickering" effect.""" limb = [] while self.idx < len(self.circleIndx): - xy = np.array([[self.sensorSize[0]/2+0.5, self.sensorSize[0]/2+0.5],[self.centers[i,1], self.centers[i,2]]]) - for j in range(len(self.points[i,:])/2): - if self.points[i,j] >1E-1 or self.points[i,j+1] >1E-1: - limb.append([self.points[i,j], self.points[i,j+1]]) + xy = np.array( + [ + [self.sensorSize[0] / 2 + 0.5, self.sensorSize[0] / 2 + 0.5], + [self.centers[i, 1], self.centers[i, 2]], + ] + ) + for j in range(len(self.points[i, :]) / 2): + if self.points[i, j] > 1e-1 or self.points[i, j + 1] > 1e-1: + limb.append([self.points[i, j], self.points[i, j + 1]]) c = [self.colorList[-1], self.colorList[i], self.colorList[i]] - yield np.c_[xy[:,0], xy[:,1], limb[:,0], limb[:,1]], c + yield np.c_[xy[:, 0], xy[:, 1], limb[:, 0], limb[:, 1]], c def update(self, i): """Update the scatter plot.""" diff --git a/examples/OpNavScenarios/scenariosOpNav/CNN_ImageGen/OpNavMonteCarlo.py b/examples/OpNavScenarios/scenariosOpNav/CNN_ImageGen/OpNavMonteCarlo.py index d9f84e1558..60665d3055 100644 --- a/examples/OpNavScenarios/scenariosOpNav/CNN_ImageGen/OpNavMonteCarlo.py +++ b/examples/OpNavScenarios/scenariosOpNav/CNN_ImageGen/OpNavMonteCarlo.py @@ -29,8 +29,6 @@ """ - - import csv import inspect import os @@ -41,10 +39,15 @@ path = os.path.dirname(os.path.abspath(filename)) from Basilisk import __path__ + bskPath = __path__[0] from Basilisk.utilities.MonteCarlo.Controller import Controller, RetentionPolicy -from Basilisk.utilities.MonteCarlo.Dispersions import OrbitalElementDispersion, MRPDispersionPerAxis, UniformDispersion +from Basilisk.utilities.MonteCarlo.Dispersions import ( + OrbitalElementDispersion, + MRPDispersionPerAxis, + UniformDispersion, +) # import simulation related support from Basilisk.utilities import RigidBodyKinematics as rbk @@ -61,6 +64,7 @@ var2 = "sigma_BN" var3 = "valid" + def run(show_plots): """Main Simulation Method""" @@ -86,29 +90,40 @@ def run(show_plots): # Add some dispersions dispDict = {} - dispDict["mu"] = 4.2828371901284001E+13 - dispDict["a"] = ["normal", 14000*1E3, 2500*1E3] # 12000 - dispDict["e"] = ["uniform", 0.2, 0.5] # 0.4, 0.7 + dispDict["mu"] = 4.2828371901284001e13 + dispDict["a"] = ["normal", 14000 * 1e3, 2500 * 1e3] # 12000 + dispDict["e"] = ["uniform", 0.2, 0.5] # 0.4, 0.7 dispDict["i"] = ["uniform", np.deg2rad(40), np.deg2rad(90)] dispDict["Omega"] = None dispDict["omega"] = None dispDict["f"] = ["uniform", np.deg2rad(0), np.deg2rad(359)] - disp1Name = 'get_DynModel().scObject.hub.r_CN_NInit' - disp2Name = 'get_DynModel().scObject.hub.v_CN_NInit' - disp3Name = 'get_FswModel().trackingErrorCam.sigma_R0R' - dispGauss = 'get_DynModel().cameraMod.gaussian' - dispDC = 'get_DynModel().cameraMod.darkCurrent' - dispSP = 'get_DynModel().cameraMod.saltPepper' - dispCR = 'get_DynModel().cameraMod.cosmicRays' - dispBlur = 'get_DynModel().cameraMod.blurParam' - - monteCarlo.addDispersion(OrbitalElementDispersion(disp1Name,disp2Name, dispDict)) - monteCarlo.addDispersion(MRPDispersionPerAxis(disp3Name, bounds=[[1./3-0.051, 1./3+0.051], [1./3-0.051, 1./3+0.051], [-1./3-0.051, -1./3+0.051]])) - monteCarlo.addDispersion(UniformDispersion(dispGauss, [0,5])) - monteCarlo.addDispersion(UniformDispersion(dispSP, [0,2.5])) - monteCarlo.addDispersion(UniformDispersion(dispCR, [0.5,4])) - monteCarlo.addDispersion(UniformDispersion(dispBlur, [1,6])) + disp1Name = "get_DynModel().scObject.hub.r_CN_NInit" + disp2Name = "get_DynModel().scObject.hub.v_CN_NInit" + disp3Name = "get_FswModel().trackingErrorCam.sigma_R0R" + dispGauss = "get_DynModel().cameraMod.gaussian" + dispDC = "get_DynModel().cameraMod.darkCurrent" + dispSP = "get_DynModel().cameraMod.saltPepper" + dispCR = "get_DynModel().cameraMod.cosmicRays" + dispBlur = "get_DynModel().cameraMod.blurParam" + + monteCarlo.addDispersion( + OrbitalElementDispersion(disp1Name, disp2Name, dispDict) + ) + monteCarlo.addDispersion( + MRPDispersionPerAxis( + disp3Name, + bounds=[ + [1.0 / 3 - 0.051, 1.0 / 3 + 0.051], + [1.0 / 3 - 0.051, 1.0 / 3 + 0.051], + [-1.0 / 3 - 0.051, -1.0 / 3 + 0.051], + ], + ) + ) + monteCarlo.addDispersion(UniformDispersion(dispGauss, [0, 5])) + monteCarlo.addDispersion(UniformDispersion(dispSP, [0, 2.5])) + monteCarlo.addDispersion(UniformDispersion(dispCR, [0.5, 4])) + monteCarlo.addDispersion(UniformDispersion(dispBlur, [1, 6])) # Add retention policy retentionPolicy = RetentionPolicy() @@ -121,30 +136,44 @@ def run(show_plots): if POST: monteCarlo = Controller.load(dirName) - for i in range(0,NUMBER_OF_RUNS): + for i in range(0, NUMBER_OF_RUNS): try: monteCarloData = monteCarlo.getRetainedData(i) except FileNotFoundError: - print("File not found, ", i) + print("File not found, ", i) continue - csvfile = open(dirName + "/run" + str(i) + "/data.csv", 'w') + csvfile = open(dirName + "/run" + str(i) + "/data.csv", "w") writer = csv.writer(csvfile) - writer.writerow(['Filename', 'Valid', 'X_p', 'Y_p', 'rho_p', 'r_BN_N_1', 'r_BN_N_2', 'r_BN_N_3']) + writer.writerow( + [ + "Filename", + "Valid", + "X_p", + "Y_p", + "rho_p", + "r_BN_N_1", + "r_BN_N_2", + "r_BN_N_3", + ] + ) timeAxis = monteCarloData["messages"][retainedMessageName1 + ".times"] - position_N = unitTestSupport.addTimeColumn(timeAxis, - monteCarloData["messages"][retainedMessageName1 + "." + var1]) - sigma_BN = unitTestSupport.addTimeColumn(timeAxis, - monteCarloData["messages"][retainedMessageName1 + "." + var2]) - validCircle = unitTestSupport.addTimeColumn(timeAxis, - monteCarloData["messages"][retainedMessageName2 + "." + var3]) - - renderRate = 60*1E9 - sigma_CB = [0., 0., 0.] # Arbitrary camera orientation + position_N = unitTestSupport.addTimeColumn( + timeAxis, monteCarloData["messages"][retainedMessageName1 + "." + var1] + ) + sigma_BN = unitTestSupport.addTimeColumn( + timeAxis, monteCarloData["messages"][retainedMessageName1 + "." + var2] + ) + validCircle = unitTestSupport.addTimeColumn( + timeAxis, monteCarloData["messages"][retainedMessageName2 + "." + var3] + ) + + renderRate = 60 * 1e9 + sigma_CB = [0.0, 0.0, 0.0] # Arbitrary camera orientation sizeOfCam = [512, 512] - sizeMM = [10. * 1E-3, 10. * 1E-3] # in m + sizeMM = [10.0 * 1e-3, 10.0 * 1e-3] # in m fieldOfView = np.deg2rad(55) # in degrees - focal = sizeMM[0] / 2. / np.tan(fieldOfView / 2.) # in m + focal = sizeMM[0] / 2.0 / np.tan(fieldOfView / 2.0) # in m pixelSize = [] pixelSize.append(sizeMM[0] / sizeOfCam[0]) @@ -159,21 +188,46 @@ def run(show_plots): trueRhat_C[:, 0] = validCircle[:, 0] ModeIdx = 0 - Rmars = 3396.19 * 1E3 + Rmars = 3396.19 * 1e3 for j in range(len(position_N[:, 0])): if position_N[j, 0] in validCircle[:, 0]: ModeIdx = j break for i in range(len(validCircle[:, 0])): - if validCircle[i, 1] > 1E-5 or (validCircle[i, 0]%renderRate ==0 and validCircle[i, 0] > 1): - trueRhat_C[i, 1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[ModeIdx + i, 1:4])), - position_N[ModeIdx + i, 1:4]) / np.linalg.norm(position_N[ModeIdx + i, 1:4]) - trueCircles[i, 3] = focal * np.tan(np.arcsin(Rmars / np.linalg.norm(position_N[ModeIdx + i, 1:4]))) / pixelSize[0] + if validCircle[i, 1] > 1e-5 or ( + validCircle[i, 0] % renderRate == 0 and validCircle[i, 0] > 1 + ): + trueRhat_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[ModeIdx + i, 1:4])), + position_N[ModeIdx + i, 1:4], + ) / np.linalg.norm(position_N[ModeIdx + i, 1:4]) + trueCircles[i, 3] = ( + focal + * np.tan( + np.arcsin(Rmars / np.linalg.norm(position_N[ModeIdx + i, 1:4])) + ) + / pixelSize[0] + ) trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] - trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 - trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 - - writer.writerow([str("{0:.6f}".format(position_N[i,0]*1E-9))+".jpg", validCircle[i, 1], trueCircles[i, 1], trueCircles[i, 2], trueCircles[i, 3], position_N[i,1], position_N[i,2], position_N[i,3]]) + trueCircles[i, 1] = ( + trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 + ) + trueCircles[i, 2] = ( + trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 + ) + + writer.writerow( + [ + str("{0:.6f}".format(position_N[i, 0] * 1e-9)) + ".jpg", + validCircle[i, 1], + trueCircles[i, 1], + trueCircles[i, 2], + trueCircles[i, 3], + position_N[i, 1], + position_N[i, 2], + position_N[i, 3], + ] + ) csvfile.close() if show_plots: diff --git a/examples/OpNavScenarios/scenariosOpNav/CNN_ImageGen/scenario_CNNImages.py b/examples/OpNavScenarios/scenariosOpNav/CNN_ImageGen/scenario_CNNImages.py index f78eea8834..75db25a819 100644 --- a/examples/OpNavScenarios/scenariosOpNav/CNN_ImageGen/scenario_CNNImages.py +++ b/examples/OpNavScenarios/scenariosOpNav/CNN_ImageGen/scenario_CNNImages.py @@ -22,6 +22,7 @@ This script is called by OpNavScenarios/CNN_ImageGen/OpNavMonteCarlo.py in order to generate images. """ + # Get current file path import inspect import os @@ -29,6 +30,7 @@ import sys from Basilisk.utilities import RigidBodyKinematics as rbk + # Import utilities from Basilisk.utilities import orbitalMotion, macros, unitTestSupport @@ -36,26 +38,28 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../..') +sys.path.append(path + "/../..") from BSK_OpNav import BSKSim, BSKScenario import BSK_OpNavDynamics, BSK_OpNavFsw import numpy as np # Import plotting file for your scenario -sys.path.append(path + '/../../plottingOpNav') +sys.path.append(path + "/../../plottingOpNav") import OpNav_Plotting as BSK_plt + # Create your own scenario child class class scenario_OpNav(BSKSim): """Main Simulation Class""" + def __init__(self): super(scenario_OpNav, self).__init__(BSKSim) self.fswRate = 0.5 self.dynRate = 0.5 self.set_DynModel(BSK_OpNavDynamics) self.set_FswModel(BSK_OpNavFsw) - self.name = 'scenario_opnav' - self.filterUse = "bias" #"relOD" + self.name = "scenario_opnav" + self.filterUse = "bias" # "relOD" self.configure_initial_conditions() # set recorded message information @@ -69,27 +73,37 @@ def __init__(self): def configure_initial_conditions(self): # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 18000*1E3 # meters - oe.e = 0. + oe.a = 18000 * 1e3 # meters + oe.e = 0.0 oe.i = 20 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 100. * macros.D2R #90 good - mu = self.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 100.0 * macros.D2R # 90 good + mu = self.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - MRP= [0,0,0] - if self.filterUse =="relOD": + MRP = [0, 0, 0] + if self.filterUse == "relOD": self.get_FswModel().relativeOD.stateInit = rN.tolist() + vN.tolist() if self.filterUse == "bias": - self.get_FswModel().pixelLineFilter.stateInit = rN.tolist() + vN.tolist() + bias + self.get_FswModel().pixelLineFilter.stateInit = ( + rN.tolist() + vN.tolist() + bias + ) # self.get_DynModel().scObject.hub.r_CN_NInit = rN # m - r_CN_N # self.get_DynModel().scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - self.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B self.get_DynModel().cameraMod.fieldOfView = np.deg2rad(55) def log_outputs(self): @@ -99,11 +113,19 @@ def log_outputs(self): # FSW process outputs samplingTime = self.get_FswModel().processTasksTimeStep - self.msgRecList[self.retainedMessageName1] = DynModel.scObject.scStateOutMsg.recorder(samplingTime) - self.AddModelToTask(DynModel.taskName, self.msgRecList[self.retainedMessageName1]) + self.msgRecList[self.retainedMessageName1] = ( + DynModel.scObject.scStateOutMsg.recorder(samplingTime) + ) + self.AddModelToTask( + DynModel.taskName, self.msgRecList[self.retainedMessageName1] + ) - self.msgRecList[self.retainedMessageName2] = FswModel.opnavCirclesMsg.recorder(samplingTime) - self.AddModelToTask(DynModel.taskName, self.msgRecList[self.retainedMessageName2]) + self.msgRecList[self.retainedMessageName2] = FswModel.opnavCirclesMsg.recorder( + samplingTime + ) + self.AddModelToTask( + DynModel.taskName, self.msgRecList[self.retainedMessageName2] + ) return @@ -115,12 +137,14 @@ def pull_outputs(self, showPlots): ## Image processing circleStates = self.scRecmsgRecList[self.retainedMessageName2] - validCircle = unitTestSupport.addTimeColumn(circleStates.times(), circleStates.valid) + validCircle = unitTestSupport.addTimeColumn( + circleStates.times(), circleStates.valid + ) sigma_CB = self.get_DynModel().cameraMRP_CB sizeMM = self.get_DynModel().cameraSize sizeOfCam = self.get_DynModel().cameraRez - focal = self.get_DynModel().cameraFocal #in m + focal = self.get_DynModel().cameraFocal # in m pixelSize = [] pixelSize.append(sizeMM[0] / sizeOfCam[0]) @@ -130,24 +154,37 @@ def pull_outputs(self, showPlots): # Plot results BSK_plt.clear_all_plots() - trueRhat_C = np.full([len(validCircle[:,0]), 4], np.nan) - trueCircles = np.full([len(validCircle[:,0]), 4], np.nan) - trueCircles[:,0] = validCircle[:,0] - trueRhat_C[:,0] = validCircle[:,0] + trueRhat_C = np.full([len(validCircle[:, 0]), 4], np.nan) + trueCircles = np.full([len(validCircle[:, 0]), 4], np.nan) + trueCircles[:, 0] = validCircle[:, 0] + trueRhat_C[:, 0] = validCircle[:, 0] ModeIdx = 0 - Rmars = 3396.19*1E3 + Rmars = 3396.19 * 1e3 for j in range(len(position_N[:, 0])): if position_N[j, 0] in validCircle[:, 0]: ModeIdx = j break - for i in range(len(validCircle[:,0])): - if validCircle[i,1] > 1E-5: - trueRhat_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[ModeIdx+i , 1:4])) ,position_N[ModeIdx+i, 1:4])/np.linalg.norm(position_N[ModeIdx+i, 1:4]) - trueCircles[i,3] = focal*np.tan(np.arcsin(Rmars/np.linalg.norm(position_N[ModeIdx+i,1:4])))/pixelSize[0] - trueRhat_C[i,1:] *= focal/trueRhat_C[i,3] - trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0]/2 - 0.5 - trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1]/2 - 0.5 + for i in range(len(validCircle[:, 0])): + if validCircle[i, 1] > 1e-5: + trueRhat_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[ModeIdx + i, 1:4])), + position_N[ModeIdx + i, 1:4], + ) / np.linalg.norm(position_N[ModeIdx + i, 1:4]) + trueCircles[i, 3] = ( + focal + * np.tan( + np.arcsin(Rmars / np.linalg.norm(position_N[ModeIdx + i, 1:4])) + ) + / pixelSize[0] + ) + trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] + trueCircles[i, 1] = ( + trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 + ) + trueCircles[i, 2] = ( + trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 + ) return @@ -163,32 +200,36 @@ def run(TheScenario, runLog): TheScenario.get_DynModel().cameraMod.fieldOfView = np.deg2rad(55) # in degrees TheScenario.get_DynModel().cameraMod.cameraIsOn = 1 TheScenario.get_DynModel().cameraMod.saveImages = 1 - TheScenario.get_DynModel().cameraMod.saveDir = runLog.split('/')[-2] +'/' +runLog.split('/')[-1] + '/' + TheScenario.get_DynModel().cameraMod.saveDir = ( + runLog.split("/")[-2] + "/" + runLog.split("/")[-1] + "/" + ) TheScenario.get_DynModel().vizInterface.noDisplay = True # Modes: "None", "-directComm", "-noDisplay" # The following code spawns the Vizard application from python as a function of the mode selected above, and the platform. TheScenario.vizard = subprocess.Popen( - [TheScenario.vizPath, "--args", "-noDisplay", "tcp://localhost:5556"], stdout=subprocess.DEVNULL) + [TheScenario.vizPath, "--args", "-noDisplay", "tcp://localhost:5556"], + stdout=subprocess.DEVNULL, + ) print("Vizard spawned with PID = " + str(TheScenario.vizard.pid)) # Configure FSW mode - TheScenario.modeRequest = 'imageGen' + TheScenario.modeRequest = "imageGen" # Initialize simulation TheScenario.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.min2nano(100.) + simulationTime = macros.min2nano(100.0) TheScenario.ConfigureStopTime(simulationTime) - print('Starting Execution') + print("Starting Execution") TheScenario.ExecuteSimulation() TheScenario.vizard.kill() spice = TheScenario.get_DynModel().spiceObject - spice.unloadSpiceKernel(spice.SPICEDataPath, 'de430.bsp') - spice.unloadSpiceKernel(spice.SPICEDataPath, 'naif0012.tls') - spice.unloadSpiceKernel(spice.SPICEDataPath, 'de-403-masses.tpc') - spice.unloadSpiceKernel(spice.SPICEDataPath, 'pck00010.tpc') + spice.unloadSpiceKernel(spice.SPICEDataPath, "de430.bsp") + spice.unloadSpiceKernel(spice.SPICEDataPath, "naif0012.tls") + spice.unloadSpiceKernel(spice.SPICEDataPath, "de-403-masses.tpc") + spice.unloadSpiceKernel(spice.SPICEDataPath, "pck00010.tpc") return diff --git a/examples/OpNavScenarios/scenariosOpNav/OpNavMC/MonteCarlo.py b/examples/OpNavScenarios/scenariosOpNav/OpNavMC/MonteCarlo.py index 8f355d1d36..d80485f6e0 100644 --- a/examples/OpNavScenarios/scenariosOpNav/OpNavMC/MonteCarlo.py +++ b/examples/OpNavScenarios/scenariosOpNav/OpNavMC/MonteCarlo.py @@ -29,9 +29,9 @@ """ - import os import inspect + # import scenario_LimbAttOD as scenario import scenario_OpNavAttODMC as scenario from BSK_OpNav import BSKSim @@ -43,10 +43,15 @@ path = os.path.dirname(os.path.abspath(filename)) from Basilisk import __path__ + bskPath = __path__[0] from Basilisk.utilities.MonteCarlo.Controller import Controller, RetentionPolicy -from Basilisk.utilities.MonteCarlo.Dispersions import OrbitalElementDispersion, UniformDispersion +from Basilisk.utilities.MonteCarlo.Dispersions import ( + OrbitalElementDispersion, + UniformDispersion, +) + # import simulation related support from Basilisk.utilities import macros import matplotlib.pyplot as plt @@ -54,13 +59,20 @@ import numpy as np m2km = 1.0 / 1000.0 -ns2min = 1/60.*1E-9 +ns2min = 1 / 60.0 * 1e-9 -mpl.rcParams.update({'font.size' : 8 }) -#seaborn-colorblind, 'seaborn-paper', 'bmh', 'tableau-colorblind10', 'seaborn-deep', 'myStyle', 'aiaa' +mpl.rcParams.update({"font.size": 8}) +# seaborn-colorblind, 'seaborn-paper', 'bmh', 'tableau-colorblind10', 'seaborn-deep', 'myStyle', 'aiaa' # plt.style.use("myStyle") -params = {'axes.labelsize': 8,'axes.titlesize':8, 'legend.fontsize': 8, 'xtick.labelsize': 7, 'ytick.labelsize': 7, 'text.usetex': True} +params = { + "axes.labelsize": 8, + "axes.titlesize": 8, + "legend.fontsize": 8, + "xtick.labelsize": 7, + "ytick.labelsize": 7, + "text.usetex": True, +} mpl.rcParams.update(params) retainedMessageNameSc = "scMsg" @@ -68,8 +80,9 @@ retainedMessageNameOpNav = "opnavMsg" retainedRate = macros.sec2nano(10) + def displayPlots(data, retentionPolicy): - mpl.rcParams['image.cmap'] = 'inferno' + mpl.rcParams["image.cmap"] = "inferno" position_N = data["messages"][retainedMessageNameSc + ".r_BN_N"] vel_N = data["messages"][retainedMessageNameSc + ".v_BN_N"] @@ -82,8 +95,8 @@ def displayPlots(data, retentionPolicy): truth[:, 4:7] = np.copy(vel_N[:, 1:]) validIdx = [] - for i in range(len(valid[:,0])): - if np.abs(valid[i,1] - 1) < 0.01: + for i in range(len(valid[:, 0])): + if np.abs(valid[i, 1] - 1) < 0.01: validIdx.append(i) diffPos = np.full([len(validIdx), 2], np.nan) diffVel = np.full([len(validIdx), 2], np.nan) @@ -92,46 +105,73 @@ def displayPlots(data, retentionPolicy): m2km2 = m2km for i in range(len(validIdx)): - diffPos[i,0] = states[validIdx[i],0] - diffPos[i,1] = np.linalg.norm(states[validIdx[i],1:4] - truth[validIdx[i],1:4])/np.linalg.norm(truth[validIdx[i],1:4])*100 - diffVel[i,0] = states[validIdx[i],0] - diffVel[i,1] = np.linalg.norm(states[validIdx[i],4:7] - truth[validIdx[i],4:7])/np.linalg.norm(truth[validIdx[i],4:7])*100 - covarPos[i,0] = states[validIdx[i],0] - posVec = np.sqrt(np.array([covar[validIdx[i],1], covar[validIdx[i],1 + 6+1], covar[validIdx[i],1 + 2*(6+1)]])) - covarPos[i,1] = 3*np.linalg.norm(posVec)/np.linalg.norm(truth[validIdx[i],1:4])*100 - covarVel[i,0] = states[validIdx[i],0] - velVec = np.sqrt(np.array([covar[validIdx[i],1 + 3*(6+1)], covar[validIdx[i],1 + 4*(6+1)], covar[validIdx[i],1 + 5*(6+1)]])) - covarVel[i,1] = 3*np.linalg.norm(velVec)/np.linalg.norm(truth[validIdx[i],4:7])*100 + diffPos[i, 0] = states[validIdx[i], 0] + diffPos[i, 1] = ( + np.linalg.norm(states[validIdx[i], 1:4] - truth[validIdx[i], 1:4]) + / np.linalg.norm(truth[validIdx[i], 1:4]) + * 100 + ) + diffVel[i, 0] = states[validIdx[i], 0] + diffVel[i, 1] = ( + np.linalg.norm(states[validIdx[i], 4:7] - truth[validIdx[i], 4:7]) + / np.linalg.norm(truth[validIdx[i], 4:7]) + * 100 + ) + covarPos[i, 0] = states[validIdx[i], 0] + posVec = np.sqrt( + np.array( + [ + covar[validIdx[i], 1], + covar[validIdx[i], 1 + 6 + 1], + covar[validIdx[i], 1 + 2 * (6 + 1)], + ] + ) + ) + covarPos[i, 1] = ( + 3 * np.linalg.norm(posVec) / np.linalg.norm(truth[validIdx[i], 1:4]) * 100 + ) + covarVel[i, 0] = states[validIdx[i], 0] + velVec = np.sqrt( + np.array( + [ + covar[validIdx[i], 1 + 3 * (6 + 1)], + covar[validIdx[i], 1 + 4 * (6 + 1)], + covar[validIdx[i], 1 + 5 * (6 + 1)], + ] + ) + ) + covarVel[i, 1] = ( + 3 * np.linalg.norm(velVec) / np.linalg.norm(truth[validIdx[i], 4:7]) * 100 + ) # plt.figure(101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') - plt.figure(101, figsize=(3.5, 2), facecolor='w', edgecolor='k') + plt.figure(101, figsize=(3.5, 2), facecolor="w", edgecolor="k") plt.plot(diffPos[:, 0] * ns2min, diffPos[:, 1]) - plt.ylabel("$\mathbf{r}_\mathrm{"+"Circ"+"}$ errors ($\%$)") + plt.ylabel("$\mathbf{r}_\mathrm{" + "Circ" + "}$ errors ($\%$)") plt.xlabel("Time (min)") # plt.ylim([0,3.5]) - plt.savefig('MCErrorPos.pdf') + plt.savefig("MCErrorPos.pdf") - plt.figure(103, figsize=(3.5, 2), facecolor='w', edgecolor='k') - plt.plot(covarPos[:, 0] * ns2min, covarPos[:,1], linestyle = '--') - plt.ylabel("$\mathbf{r}_\mathrm{"+"Circ"+"}$ covar ($\%$)") + plt.figure(103, figsize=(3.5, 2), facecolor="w", edgecolor="k") + plt.plot(covarPos[:, 0] * ns2min, covarPos[:, 1], linestyle="--") + plt.ylabel("$\mathbf{r}_\mathrm{" + "Circ" + "}$ covar ($\%$)") plt.xlabel("Time (min)") # plt.ylim([0,3.5]) - plt.savefig('MCCovarPos.pdf') + plt.savefig("MCCovarPos.pdf") - plt.figure(102, figsize=(3.5, 2.), facecolor='w', edgecolor='k') + plt.figure(102, figsize=(3.5, 2.0), facecolor="w", edgecolor="k") plt.plot(diffVel[:, 0] * ns2min, diffVel[:, 1]) - plt.ylabel("$\dot{\mathbf{r}}_\mathrm{"+"Circ"+ "}$ errors ($\%$)") + plt.ylabel("$\dot{\mathbf{r}}_\mathrm{" + "Circ" + "}$ errors ($\%$)") plt.xlabel("Time (min)") - plt.savefig('MCErrorVel.pdf') + plt.savefig("MCErrorVel.pdf") - plt.figure(104, figsize=(3.5, 2.), facecolor='w', edgecolor='k') - plt.plot(covarVel[:, 0] * ns2min, covarVel[:,1], linestyle = '--') - plt.ylabel("$\dot{\mathbf{r}}_\mathrm{"+"Circ"+ "}$ covar ($\%$)") + plt.figure(104, figsize=(3.5, 2.0), facecolor="w", edgecolor="k") + plt.plot(covarVel[:, 0] * ns2min, covarVel[:, 1], linestyle="--") + plt.ylabel("$\dot{\mathbf{r}}_\mathrm{" + "Circ" + "}$ covar ($\%$)") plt.xlabel("Time (min)") - plt.savefig('MCCovarVel.pdf') + plt.savefig("MCCovarVel.pdf") def run(show_plots): - NUMBER_OF_RUNS = 2 VERBOSE = True PROCESSES = 1 @@ -154,26 +194,41 @@ def run(show_plots): # Add some dispersions dispDict = {} - dispDict["mu"] = 4.2828371901284001E+13 - dispDict["a"] = ["normal", 22000*1E3, 3000*1E3] + dispDict["mu"] = 4.2828371901284001e13 + dispDict["a"] = ["normal", 22000 * 1e3, 3000 * 1e3] dispDict["e"] = ["uniform", 0.2, 0.4] dispDict["i"] = ["uniform", -np.deg2rad(20), np.deg2rad(20)] dispDict["Omega"] = None dispDict["omega"] = None - dispDict["f"] = ["uniform", 0., np.deg2rad(180)] + dispDict["f"] = ["uniform", 0.0, np.deg2rad(180)] - disp1Name = 'get_DynModel().scObject.hub.r_CN_NInit' - disp2Name = 'get_DynModel().scObject.hub.v_CN_NInit' - dispFOV = 'get_DynModel().cameraMod.fieldOfView' - dispNoise = 'get_FswModel().relativeOD.noiseSF' + disp1Name = "get_DynModel().scObject.hub.r_CN_NInit" + disp2Name = "get_DynModel().scObject.hub.v_CN_NInit" + dispFOV = "get_DynModel().cameraMod.fieldOfView" + dispNoise = "get_FswModel().relativeOD.noiseSF" monteCarlo.addDispersion(UniformDispersion(dispNoise, [1, 10])) - monteCarlo.addDispersion(UniformDispersion(dispFOV, [np.deg2rad(40) - np.deg2rad(0.001), np.deg2rad(40) + np.deg2rad(0.001)])) - monteCarlo.addDispersion(OrbitalElementDispersion(disp1Name,disp2Name, dispDict)) + monteCarlo.addDispersion( + UniformDispersion( + dispFOV, + [ + np.deg2rad(40) - np.deg2rad(0.001), + np.deg2rad(40) + np.deg2rad(0.001), + ], + ) + ) + monteCarlo.addDispersion( + OrbitalElementDispersion(disp1Name, disp2Name, dispDict) + ) # Add retention policy retentionPolicy = RetentionPolicy() - retentionPolicy.addMessageLog(retainedMessageNameSc, ["r_BN_N", "v_BN_N", "sigma_BN"]) - retentionPolicy.addMessageLog(retainedMessageNameOpNav, ["r_BN_N", "covar_N", "r_BN_C", "covar_C", "valid"]) + retentionPolicy.addMessageLog( + retainedMessageNameSc, ["r_BN_N", "v_BN_N", "sigma_BN"] + ) + retentionPolicy.addMessageLog( + retainedMessageNameOpNav, + ["r_BN_N", "covar_N", "r_BN_C", "covar_C", "valid"], + ) retentionPolicy.addMessageLog(retainedMessageNameFilt, ["state", "covar"]) retentionPolicy.setDataCallback(displayPlots) monteCarlo.addRetentionPolicy(retentionPolicy) diff --git a/examples/OpNavScenarios/scenariosOpNav/OpNavMC/scenario_LimbAttOD.py b/examples/OpNavScenarios/scenariosOpNav/OpNavMC/scenario_LimbAttOD.py index 3c1e6a9f98..d2baebfb04 100644 --- a/examples/OpNavScenarios/scenariosOpNav/OpNavMC/scenario_LimbAttOD.py +++ b/examples/OpNavScenarios/scenariosOpNav/OpNavMC/scenario_LimbAttOD.py @@ -22,6 +22,7 @@ This script is called by OpNavScenarios/OpNavMC/MonteCarlo.py in order to make MC data. """ + # Get current file path import inspect import os @@ -29,6 +30,7 @@ import sys from Basilisk.utilities import RigidBodyKinematics as rbk + # Import utilities from Basilisk.utilities import orbitalMotion, macros, unitTestSupport @@ -36,25 +38,27 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../..') +sys.path.append(path + "/../..") from BSK_OpNav import BSKSim import BSK_OpNavDynamics, BSK_OpNavFsw import numpy as np # Import plotting file for your scenario -sys.path.append(path + '/../../plottingOpNav') +sys.path.append(path + "/../../plottingOpNav") import OpNav_Plotting as BSK_plt + # Create your own scenario child class class scenario_OpNav(BSKSim): """Main Simulation Class""" + def __init__(self): super(scenario_OpNav, self).__init__(BSKSim) self.fswRate = 0.5 self.dynRate = 0.5 self.set_DynModel(BSK_OpNavDynamics) self.set_FswModel(BSK_OpNavFsw) - self.name = 'scenario_opnav' + self.name = "scenario_opnav" self.configure_initial_conditions() self.msgRecList = {} @@ -66,30 +70,40 @@ def __init__(self): def configure_initial_conditions(self): # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 18000 * 1E3 # meters + oe.a = 18000 * 1e3 # meters oe.e = 0.6 oe.i = 10 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 80. * macros.D2R # 90 good - mu = self.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 80.0 * macros.D2R # 90 good + mu = self.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - rError= np.array([10000.,10000., -10000]) - vError= np.array([100, -10, 10]) + rError = np.array([10000.0, 10000.0, -10000]) + vError = np.array([100, -10, 10]) - MRP= [0,-0.3,0] - self.get_FswModel().relativeOD.stateInit = (rN+rError).tolist() + (vN+vError).tolist() + MRP = [0, -0.3, 0] + self.get_FswModel().relativeOD.stateInit = (rN + rError).tolist() + ( + vN + vError + ).tolist() self.get_DynModel().scObject.hub.r_CN_NInit = rN # m - r_CN_N self.get_DynModel().scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - self.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B qNoiseIn = np.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1e-3 * 1e-3 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1e-4 * 1e-4 self.get_FswModel().relativeOD.qNoise = qNoiseIn.reshape(36).tolist() self.get_FswModel().horizonNav.noiseSF = 20 @@ -101,17 +115,33 @@ def log_outputs(self): # FSW process outputs samplingTime = self.get_FswModel().processTasksTimeStep - self.msgRecList[self.retainedMessageNameSc] = DynModel.scObject.scStateOutMsg.recorder(samplingTime) - self.AddModelToTask(DynModel.taskName, self.msgRecList[self.retainedMessageNameSc]) - - self.msgRecList[self.retainedMessageNameFilt] = FswModel.relativeOD.filtDataOutMsg.recorder(samplingTime) - self.AddModelToTask(DynModel.taskName, self.msgRecList[self.retainedMessageNameFilt]) - - self.msgRecList[self.retainedMessageNameOpNav] = FswModel.opnavMsg.recorder(samplingTime) - self.AddModelToTask(DynModel.taskName, self.msgRecList[self.retainedMessageNameOpNav]) - - self.msgRecList[self.retainedMessageNameLimb] = FswModel.limbFinding.opnavLimbOutMsg.recorder(samplingTime) - self.AddModelToTask(DynModel.taskName, self.msgRecList[self.retainedMessageNameLimb]) + self.msgRecList[self.retainedMessageNameSc] = ( + DynModel.scObject.scStateOutMsg.recorder(samplingTime) + ) + self.AddModelToTask( + DynModel.taskName, self.msgRecList[self.retainedMessageNameSc] + ) + + self.msgRecList[self.retainedMessageNameFilt] = ( + FswModel.relativeOD.filtDataOutMsg.recorder(samplingTime) + ) + self.AddModelToTask( + DynModel.taskName, self.msgRecList[self.retainedMessageNameFilt] + ) + + self.msgRecList[self.retainedMessageNameOpNav] = FswModel.opnavMsg.recorder( + samplingTime + ) + self.AddModelToTask( + DynModel.taskName, self.msgRecList[self.retainedMessageNameOpNav] + ) + + self.msgRecList[self.retainedMessageNameLimb] = ( + FswModel.limbFinding.opnavLimbOutMsg.recorder(samplingTime) + ) + self.AddModelToTask( + DynModel.taskName, self.msgRecList[self.retainedMessageNameLimb] + ) return @@ -128,7 +158,9 @@ def pull_outputs(self, showPlots): ## Image processing limbRec = self.msgRecList[self.retainedMessageNameLimb] limb = unitTestSupport.addTimeColumn(limbRec.times(), limbRec.limbPoints) - numLimbPoints = unitTestSupport.addTimeColumn(limbRec.times(), limbRec.numLimbPoints) + numLimbPoints = unitTestSupport.addTimeColumn( + limbRec.times(), limbRec.numLimbPoints + ) validLimb = unitTestSupport.addTimeColumn(limbRec.times(), limbRec.valid) ## OpNav Out @@ -148,7 +180,7 @@ def pull_outputs(self, showPlots): sigma_CB = self.get_DynModel().cameraMRP_CB sizeMM = self.get_DynModel().cameraSize sizeOfCam = self.get_DynModel().cameraRez - focal = self.get_DynModel().cameraFocal #in m + focal = self.get_DynModel().cameraFocal # in m pixelSize = [] pixelSize.append(sizeMM[0] / sizeOfCam[0]) @@ -157,26 +189,27 @@ def pull_outputs(self, showPlots): dcm_CB = rbk.MRP2C(sigma_CB) # Plot results BSK_plt.clear_all_plots() - stateError = np.zeros([len(position_N[:,0]), NUM_STATES+1]) - navCovarLong = np.full([len(position_N[:,0]), 1+NUM_STATES*NUM_STATES], np.nan) - navCovarLong[:,0] = np.copy(position_N[:,0]) + stateError = np.zeros([len(position_N[:, 0]), NUM_STATES + 1]) + navCovarLong = np.full( + [len(position_N[:, 0]), 1 + NUM_STATES * NUM_STATES], np.nan + ) + navCovarLong[:, 0] = np.copy(position_N[:, 0]) stateError[:, 0:4] = np.copy(position_N) - stateError[:,4:7] = np.copy(velocity_N[:,1:]) - measError = np.full([len(measPos[:,0]), 4], np.nan) - measError[:,0] = measPos[:,0] - measError_C = np.full([len(measPos[:,0]), 5], np.nan) - measError_C[:,0] = measPos[:,0] - trueRhat_C = np.full([len(numLimbPoints[:,0]), 4], np.nan) - trueR_C = np.full([len(numLimbPoints[:,0]), 4], np.nan) - trueCircles = np.full([len(numLimbPoints[:,0]), 4], np.nan) - trueCircles[:,0] = numLimbPoints[:,0] - trueRhat_C[:,0] = numLimbPoints[:,0] - trueR_C[:,0] = numLimbPoints[:,0] - + stateError[:, 4:7] = np.copy(velocity_N[:, 1:]) + measError = np.full([len(measPos[:, 0]), 4], np.nan) + measError[:, 0] = measPos[:, 0] + measError_C = np.full([len(measPos[:, 0]), 5], np.nan) + measError_C[:, 0] = measPos[:, 0] + trueRhat_C = np.full([len(numLimbPoints[:, 0]), 4], np.nan) + trueR_C = np.full([len(numLimbPoints[:, 0]), 4], np.nan) + trueCircles = np.full([len(numLimbPoints[:, 0]), 4], np.nan) + trueCircles[:, 0] = numLimbPoints[:, 0] + trueRhat_C[:, 0] = numLimbPoints[:, 0] + trueR_C[:, 0] = numLimbPoints[:, 0] switchIdx = 0 - Rmars = 3396.19*1E3 + Rmars = 3396.19 * 1e3 for j in range(len(stateError[:, 0])): if stateError[j, 0] in navState[:, 0]: stateError[j, 1:4] -= navState[j - switchIdx, 1:4] @@ -184,21 +217,39 @@ def pull_outputs(self, showPlots): else: stateError[j, 1:] = np.full(NUM_STATES, np.nan) switchIdx += 1 - for i in range(len(numLimbPoints[:,0])): - if numLimbPoints[i,1] > 1E-8: - measError[i, 1:4] = position_N[i +switchIdx, 1:4] - measPos[i, 1:4] - measError_C[i, 4] = np.linalg.norm(position_N[i +switchIdx, 1:4]) - np.linalg.norm(r_C[i, 1:4]) - trueR_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i +switchIdx, 1:4])) , position_N[i +switchIdx, 1:4]) - trueRhat_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i +switchIdx, 1:4])) ,position_N[i +switchIdx, 1:4])/np.linalg.norm(position_N[i +switchIdx, 1:4]) - trueCircles[i,3] = focal*np.tan(np.arcsin(Rmars/np.linalg.norm(position_N[i,1:4])))/pixelSize[0] - trueRhat_C[i,1:] *= focal/trueRhat_C[i,3] - measError_C[i, 1:4] = trueRhat_C[i,1:] - r_C[i, 1:4]/np.linalg.norm(r_C[i, 1:4]) - trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0]/2 - 0.5 - trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1]/2 - 0.5 + for i in range(len(numLimbPoints[:, 0])): + if numLimbPoints[i, 1] > 1e-8: + measError[i, 1:4] = position_N[i + switchIdx, 1:4] - measPos[i, 1:4] + measError_C[i, 4] = np.linalg.norm( + position_N[i + switchIdx, 1:4] + ) - np.linalg.norm(r_C[i, 1:4]) + trueR_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) + trueRhat_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) / np.linalg.norm(position_N[i + switchIdx, 1:4]) + trueCircles[i, 3] = ( + focal + * np.tan(np.arcsin(Rmars / np.linalg.norm(position_N[i, 1:4]))) + / pixelSize[0] + ) + trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] + measError_C[i, 1:4] = trueRhat_C[i, 1:] - r_C[i, 1:4] / np.linalg.norm( + r_C[i, 1:4] + ) + trueCircles[i, 1] = ( + trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 + ) + trueCircles[i, 2] = ( + trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 + ) else: - measCovar[i,1:] = np.full(3*3, np.nan) + measCovar[i, 1:] = np.full(3 * 3, np.nan) covar_C[i, 1:] = np.full(3 * 3, np.nan) - navCovarLong[switchIdx:,:] = np.copy(navCovar) + navCovarLong[switchIdx:, :] = np.copy(navCovar) timeData = position_N[:, 0] * macros.NANO2MIN @@ -222,7 +273,6 @@ def pull_outputs(self, showPlots): def run(TheScenario): - TheScenario.log_outputs() TheScenario.configure_initial_conditions() @@ -230,31 +280,32 @@ def run(TheScenario): TheScenario.get_DynModel().vizInterface.liveStream = True vizard = subprocess.Popen( - [TheScenario.vizPath, "--args", "-directComm", - "tcp://localhost:5556"], stdout=subprocess.DEVNULL) + [TheScenario.vizPath, "--args", "-directComm", "tcp://localhost:5556"], + stdout=subprocess.DEVNULL, + ) print("Vizard spawned with PID = " + str(vizard.pid)) # Configure FSW mode - TheScenario.modeRequest = 'prepOpNav' + TheScenario.modeRequest = "prepOpNav" # Initialize simulation TheScenario.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.min2nano(3.) + simulationTime = macros.min2nano(3.0) TheScenario.ConfigureStopTime(simulationTime) TheScenario.ExecuteSimulation() - TheScenario.modeRequest = 'OpNavAttODLimb' + TheScenario.modeRequest = "OpNavAttODLimb" # TheBSKSim.get_DynModel().SetLocalConfigData(TheBSKSim, 60, True) - simulationTime = macros.min2nano(100.) + simulationTime = macros.min2nano(100.0) TheScenario.ConfigureStopTime(simulationTime) TheScenario.ExecuteSimulation() vizard.kill() spice = TheScenario.get_DynModel().gravFactory.spiceObject - spice.unloadSpiceKernel(spice.SPICEDataPath, 'de430.bsp') - spice.unloadSpiceKernel(spice.SPICEDataPath, 'naif0012.tls') - spice.unloadSpiceKernel(spice.SPICEDataPath, 'de-403-masses.tpc') - spice.unloadSpiceKernel(spice.SPICEDataPath, 'pck00010.tpc') + spice.unloadSpiceKernel(spice.SPICEDataPath, "de430.bsp") + spice.unloadSpiceKernel(spice.SPICEDataPath, "naif0012.tls") + spice.unloadSpiceKernel(spice.SPICEDataPath, "de-403-masses.tpc") + spice.unloadSpiceKernel(spice.SPICEDataPath, "pck00010.tpc") return diff --git a/examples/OpNavScenarios/scenariosOpNav/OpNavMC/scenario_OpNavAttODMC.py b/examples/OpNavScenarios/scenariosOpNav/OpNavMC/scenario_OpNavAttODMC.py index 2d9ab46eaa..fd19b289a7 100644 --- a/examples/OpNavScenarios/scenariosOpNav/OpNavMC/scenario_OpNavAttODMC.py +++ b/examples/OpNavScenarios/scenariosOpNav/OpNavMC/scenario_OpNavAttODMC.py @@ -22,6 +22,7 @@ This script is called by OpNavScenarios/OpNavMC/MonteCarlo.py in order to make MC data. """ + # Get current file path import inspect import os @@ -35,25 +36,26 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/../..') +sys.path.append(path + "/../..") from BSK_OpNav import BSKSim import BSK_OpNavDynamics, BSK_OpNavFsw import numpy as np # Import plotting file for your scenario -sys.path.append(path + '/../../plottingOpNav') +sys.path.append(path + "/../../plottingOpNav") # Create your own scenario child class class scenario_OpNav(BSKSim): """Main Simulation Class""" + def __init__(self): super(scenario_OpNav, self).__init__(BSKSim) self.fswRate = 0.5 self.dynRate = 0.5 self.set_DynModel(BSK_OpNavDynamics) self.set_FswModel(BSK_OpNavFsw) - self.name = 'scenario_opnav' + self.name = "scenario_opnav" self.configure_initial_conditions() self.msgRecList = {} @@ -64,33 +66,43 @@ def __init__(self): def configure_initial_conditions(self): # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 18000 * 1E3 # meters + oe.a = 18000 * 1e3 # meters oe.e = 0.6 oe.i = 10 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 80. * macros.D2R # 90 good - mu = self.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 80.0 * macros.D2R # 90 good + mu = self.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - rError= np.array([10000.,10000., -10000]) - vError= np.array([100, -10, 10]) - MRP= [0,0,0] - self.get_FswModel().relativeOD.stateInit = (rN + rError).tolist() + (vN + vError).tolist() + rError = np.array([10000.0, 10000.0, -10000]) + vError = np.array([100, -10, 10]) + MRP = [0, 0, 0] + self.get_FswModel().relativeOD.stateInit = (rN + rError).tolist() + ( + vN + vError + ).tolist() self.get_DynModel().scObject.hub.r_CN_NInit = rN # m - r_CN_N self.get_DynModel().scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - self.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B qNoiseIn = np.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1e-3 * 1e-3 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1e-4 * 1e-4 self.get_FswModel().relativeOD.qNoise = qNoiseIn.reshape(36).tolist() self.get_FswModel().imageProcessing.noiseSF = 1 - self.get_FswModel().relativeOD.noiseSF = 5#7.5 + self.get_FswModel().relativeOD.noiseSF = 5 # 7.5 def log_outputs(self): # Dynamics process outputs: log messages below if desired. @@ -100,20 +112,31 @@ def log_outputs(self): # FSW process outputs samplingTime = self.get_FswModel().processTasksTimeStep - self.msgRecList[self.retainedMessageNameSc] = DynModel.scObject.scStateOutMsg.recorder(samplingTime) - self.AddModelToTask(DynModel.taskName, self.msgRecList[self.retainedMessageNameSc]) - - self.msgRecList[self.retainedMessageNameFilt] = FswModel.relativeOD.filtDataOutMsg.recorder(samplingTime) - self.AddModelToTask(DynModel.taskName, self.msgRecList[self.retainedMessageNameFilt]) - - self.msgRecList[self.retainedMessageNameOpNav] = FswModel.opnavMsg.recorder(samplingTime) - self.AddModelToTask(DynModel.taskName, self.msgRecList[self.retainedMessageNameOpNav]) + self.msgRecList[self.retainedMessageNameSc] = ( + DynModel.scObject.scStateOutMsg.recorder(samplingTime) + ) + self.AddModelToTask( + DynModel.taskName, self.msgRecList[self.retainedMessageNameSc] + ) + + self.msgRecList[self.retainedMessageNameFilt] = ( + FswModel.relativeOD.filtDataOutMsg.recorder(samplingTime) + ) + self.AddModelToTask( + DynModel.taskName, self.msgRecList[self.retainedMessageNameFilt] + ) + + self.msgRecList[self.retainedMessageNameOpNav] = FswModel.opnavMsg.recorder( + samplingTime + ) + self.AddModelToTask( + DynModel.taskName, self.msgRecList[self.retainedMessageNameOpNav] + ) return def run(TheScenario): - TheScenario.log_outputs() TheScenario.configure_initial_conditions() @@ -121,33 +144,36 @@ def run(TheScenario): TheScenario.get_DynModel().vizInterface.liveStream = True vizard = subprocess.Popen( - [TheScenario.vizPath, "--args", "-directComm", "tcp://localhost:5556"], stdout=subprocess.DEVNULL) + [TheScenario.vizPath, "--args", "-directComm", "tcp://localhost:5556"], + stdout=subprocess.DEVNULL, + ) print("Vizard spawned with PID = " + str(vizard.pid)) # Configure FSW mode - TheScenario.modeRequest = 'prepOpNav' + TheScenario.modeRequest = "prepOpNav" # Initialize simulation TheScenario.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.min2nano(3.) + simulationTime = macros.min2nano(3.0) TheScenario.ConfigureStopTime(simulationTime) TheScenario.ExecuteSimulation() - TheScenario.modeRequest = 'OpNavAttOD' + TheScenario.modeRequest = "OpNavAttOD" # TheBSKSim.get_DynModel().SetLocalConfigData(TheBSKSim, 60, True) - simulationTime = macros.min2nano(100.) + simulationTime = macros.min2nano(100.0) TheScenario.ConfigureStopTime(simulationTime) TheScenario.ExecuteSimulation() vizard.kill() spice = TheScenario.get_DynModel().spiceObject - spice.unloadSpiceKernel(spice.SPICEDataPath, 'de430.bsp') - spice.unloadSpiceKernel(spice.SPICEDataPath, 'naif0012.tls') - spice.unloadSpiceKernel(spice.SPICEDataPath, 'de-403-masses.tpc') - spice.unloadSpiceKernel(spice.SPICEDataPath, 'pck00010.tpc') + spice.unloadSpiceKernel(spice.SPICEDataPath, "de430.bsp") + spice.unloadSpiceKernel(spice.SPICEDataPath, "naif0012.tls") + spice.unloadSpiceKernel(spice.SPICEDataPath, "de-403-masses.tpc") + spice.unloadSpiceKernel(spice.SPICEDataPath, "pck00010.tpc") return + if __name__ == "__main__": # Instantiate base simulation diff --git a/examples/OpNavScenarios/scenariosOpNav/scenario_CNNAttOD.py b/examples/OpNavScenarios/scenariosOpNav/scenario_CNNAttOD.py index f672accac8..8fd889a173 100644 --- a/examples/OpNavScenarios/scenariosOpNav/scenario_CNNAttOD.py +++ b/examples/OpNavScenarios/scenariosOpNav/scenario_CNNAttOD.py @@ -28,7 +28,6 @@ """ - # Get current file path import inspect import os @@ -36,6 +35,7 @@ import time from Basilisk.utilities import RigidBodyKinematics as rbk + # Import utilities from Basilisk.utilities import orbitalMotion, macros, unitTestSupport @@ -43,23 +43,25 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_OpNav import BSKSim, BSKScenario import BSK_OpNavDynamics, BSK_OpNavFsw import numpy as np # Import plotting file for your scenario -sys.path.append(path + '/../plottingOpNav') +sys.path.append(path + "/../plottingOpNav") import OpNav_Plotting as BSK_plt + # Create your own scenario child class class scenario_OpNav(BSKScenario): """Main Simulation Class""" + def __init__(self, masterSim, showPlots=False): super(scenario_OpNav, self).__init__(masterSim, showPlots) - self.name = 'scenario_opnav' + self.name = "scenario_opnav" self.masterSim = masterSim - self.filterUse ="relOD" #"bias" # + self.filterUse = "relOD" # "bias" # # declare additional class variables self.opNavRec = None @@ -68,39 +70,51 @@ def __init__(self, masterSim, showPlots=False): self.filtRec = None def configure_initial_conditions(self): - print('%s: configure_initial_conditions' % self.name) + print("%s: configure_initial_conditions" % self.name) # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 18000 * 1E3 # meters + oe.a = 18000 * 1e3 # meters oe.e = 0.6 oe.i = 10 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 80. * macros.D2R # 90 good - mu = self.masterSim.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 80.0 * macros.D2R # 90 good + mu = self.masterSim.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - rError= np.array([10000.,10000., -10000]) - vError= np.array([100, -10, 10]) - MRP= [0,0,0] - if self.filterUse =="relOD": - self.masterSim.get_FswModel().relativeOD.stateInit = (rN + rError).tolist() + (vN + vError).tolist() + rError = np.array([10000.0, 10000.0, -10000]) + vError = np.array([100, -10, 10]) + MRP = [0, 0, 0] + if self.filterUse == "relOD": + self.masterSim.get_FswModel().relativeOD.stateInit = ( + rN + rError + ).tolist() + (vN + vError).tolist() if self.filterUse == "bias": - self.masterSim.get_FswModel().relativeOD.stateInit = (rN + rError).tolist() + (vN + vError).tolist() + bias + self.masterSim.get_FswModel().relativeOD.stateInit = ( + (rN + rError).tolist() + (vN + vError).tolist() + bias + ) self.masterSim.get_DynModel().scObject.hub.r_CN_NInit = rN # m - r_CN_N self.masterSim.get_DynModel().scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B qNoiseIn = np.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1e-3 * 1e-3 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1e-4 * 1e-4 self.masterSim.get_FswModel().relativeOD.qNoise = qNoiseIn.reshape(36).tolist() self.masterSim.get_FswModel().imageProcessing.noiseSF = 1 - self.masterSim.get_FswModel().relativeOD.noiseSF = 5#7.5 + self.masterSim.get_FswModel().relativeOD.noiseSF = 5 # 7.5 def log_outputs(self): # Dynamics process outputs: log messages below if desired. @@ -116,7 +130,9 @@ def log_outputs(self): self.opNavRec = FswModel.opnavMsg.recorder(samplingTime) self.masterSim.AddModelToTask(DynModel.taskName, self.opNavRec) if self.filterUse == "bias": - self.filtRec = FswModel.pixelLineFilter.filtDataOutMsg.recorder(samplingTime) + self.filtRec = FswModel.pixelLineFilter.filtDataOutMsg.recorder( + samplingTime + ) self.masterSim.AddModelToTask(DynModel.taskName, self.filtRec) self.scRec = DynModel.scObject.scStateOutMsg.recorder(samplingTime) @@ -130,37 +146,69 @@ def pull_outputs(self, showPlots): # Dynamics process outputs: pull log messages below if any ## Spacecraft true states - position_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.r_BN_N) - velocity_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.v_BN_N) + position_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.r_BN_N + ) + velocity_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.v_BN_N + ) ## Attitude - sigma_BN = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.sigma_BN) + sigma_BN = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.sigma_BN + ) ## Image processing - circleCenters = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.circlesCenters) - circleRadii = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.circlesRadii) - validCircle = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.valid) + circleCenters = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.circlesCenters + ) + circleRadii = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.circlesRadii + ) + validCircle = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.valid + ) if self.filterUse == "bias": NUM_STATES = 9 ## Navigation results - navState = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.state) - navCovar = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.covar) - navPostFits = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.postFitRes) + navState = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.state + ) + navCovar = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.covar + ) + navPostFits = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.postFitRes + ) if self.filterUse == "relOD": NUM_STATES = 6 ## Navigation results - navState = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.state) - navCovar = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.covar) - navPostFits = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.postFitRes) - measPos = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_N) - r_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_C) - measCovar = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_N) - covar_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_C) + navState = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.state + ) + navCovar = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.covar + ) + navPostFits = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.postFitRes + ) + measPos = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.r_BN_N + ) + r_C = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.r_BN_C + ) + measCovar = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_N + ) + covar_C = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_C + ) sigma_CB = self.masterSim.get_DynModel().cameraMRP_CB sizeMM = self.masterSim.get_DynModel().cameraSize sizeOfCam = self.masterSim.get_DynModel().cameraRez - focal = self.masterSim.get_DynModel().cameraFocal #in m + focal = self.masterSim.get_DynModel().cameraFocal # in m pixelSize = [] pixelSize.append(sizeMM[0] / sizeOfCam[0]) @@ -169,35 +217,37 @@ def pull_outputs(self, showPlots): dcm_CB = rbk.MRP2C(sigma_CB) # Plot results BSK_plt.clear_all_plots() - stateError = np.zeros([len(position_N[:,0]), NUM_STATES+1]) - navCovarLong = np.full([len(position_N[:,0]), 1+NUM_STATES*NUM_STATES], np.nan) - navCovarLong[:,0] = np.copy(position_N[:,0]) + stateError = np.zeros([len(position_N[:, 0]), NUM_STATES + 1]) + navCovarLong = np.full( + [len(position_N[:, 0]), 1 + NUM_STATES * NUM_STATES], np.nan + ) + navCovarLong[:, 0] = np.copy(position_N[:, 0]) stateError[:, 0:4] = np.copy(position_N) - stateError[:,4:7] = np.copy(velocity_N[:,1:]) - pixCovar = np.ones([len(circleCenters[:,0]), 3*3+1]) - pixCovar[:,0] = circleCenters[:,0] - pixCovar[:,1:]*=np.array([1,0,0,0,1,0,0,0,2]) + stateError[:, 4:7] = np.copy(velocity_N[:, 1:]) + pixCovar = np.ones([len(circleCenters[:, 0]), 3 * 3 + 1]) + pixCovar[:, 0] = circleCenters[:, 0] + pixCovar[:, 1:] *= np.array([1, 0, 0, 0, 1, 0, 0, 0, 2]) if self.filterUse == "relOD": - measError = np.full([len(measPos[:,0]), 4], np.nan) - measError[:,0] = measPos[:,0] - measError_C = np.full([len(measPos[:,0]), 5], np.nan) - measError_C[:,0] = measPos[:,0] - trueRhat_C = np.full([len(circleCenters[:,0]), 4], np.nan) - trueR_C = np.full([len(circleCenters[:,0]), 4], np.nan) - trueCircles = np.full([len(circleCenters[:,0]), 4], np.nan) - trueCircles[:,0] = circleCenters[:,0] - trueRhat_C[:,0] = circleCenters[:,0] - trueR_C[:,0] = circleCenters[:,0] - truth = np.zeros([len(position_N[:,0]), 7]) - truth[:,0:4] = np.copy(position_N) - truth[:,4:7] = np.copy(velocity_N[:,1:]) + measError = np.full([len(measPos[:, 0]), 4], np.nan) + measError[:, 0] = measPos[:, 0] + measError_C = np.full([len(measPos[:, 0]), 5], np.nan) + measError_C[:, 0] = measPos[:, 0] + trueRhat_C = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueR_C = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueCircles = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueCircles[:, 0] = circleCenters[:, 0] + trueRhat_C[:, 0] = circleCenters[:, 0] + trueR_C[:, 0] = circleCenters[:, 0] + truth = np.zeros([len(position_N[:, 0]), 7]) + truth[:, 0:4] = np.copy(position_N) + truth[:, 4:7] = np.copy(velocity_N[:, 1:]) centerBias = np.copy(circleCenters) radBias = np.copy(circleRadii) switchIdx = 0 - Rmars = 3396.19*1E3 + Rmars = 3396.19 * 1e3 for j in range(len(stateError[:, 0])): if stateError[j, 0] in navState[:, 0]: stateError[j, 1:4] -= navState[j - switchIdx, 1:4] @@ -205,34 +255,53 @@ def pull_outputs(self, showPlots): else: stateError[j, 1:] = np.full(NUM_STATES, np.nan) switchIdx += 1 - for i in range(len(circleCenters[:,0])): - if circleCenters[i,1:].any() > 1E-8 or circleCenters[i,1:].any() < -1E-8: - trueR_C[i, 1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), - position_N[i + switchIdx, 1:4]) - trueRhat_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i +switchIdx, 1:4])) ,position_N[i +switchIdx, 1:4])/np.linalg.norm(position_N[i +switchIdx, 1:4]) - trueCircles[i,3] = focal*np.tan(np.arcsin(Rmars/np.linalg.norm(position_N[i,1:4])))/pixelSize[0] - trueRhat_C[i,1:] *= focal/trueRhat_C[i,3] - trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0]/2 - 0.5 - trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1]/2 - 0.5 + for i in range(len(circleCenters[:, 0])): + if circleCenters[i, 1:].any() > 1e-8 or circleCenters[i, 1:].any() < -1e-8: + trueR_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) + trueRhat_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) / np.linalg.norm(position_N[i + switchIdx, 1:4]) + trueCircles[i, 3] = ( + focal + * np.tan(np.arcsin(Rmars / np.linalg.norm(position_N[i, 1:4]))) + / pixelSize[0] + ) + trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] + trueCircles[i, 1] = ( + trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 + ) + trueCircles[i, 2] = ( + trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 + ) if self.filterUse == "bias": - centerBias[i,1:3] = np.round(navState[i, 7:9]) - radBias[i,1] = np.round(navState[i, -1]) + centerBias[i, 1:3] = np.round(navState[i, 7:9]) + radBias[i, 1] = np.round(navState[i, -1]) if self.filterUse == "relOD": - measError[i, 1:4] = position_N[i +switchIdx, 1:4] - measPos[i, 1:4] - measError_C[i, 4] = np.linalg.norm(position_N[i +switchIdx, 1:4]) - np.linalg.norm(r_C[i, 1:4]) - measError_C[i, 1:4] = trueRhat_C[i,1:] - r_C[i, 1:4]/np.linalg.norm(r_C[i, 1:4]) + measError[i, 1:4] = position_N[i + switchIdx, 1:4] - measPos[i, 1:4] + measError_C[i, 4] = np.linalg.norm( + position_N[i + switchIdx, 1:4] + ) - np.linalg.norm(r_C[i, 1:4]) + measError_C[i, 1:4] = trueRhat_C[i, 1:] - r_C[ + i, 1:4 + ] / np.linalg.norm(r_C[i, 1:4]) else: if self.filterUse == "relOD": - measCovar[i,1:] = np.full(3*3, np.nan) + measCovar[i, 1:] = np.full(3 * 3, np.nan) covar_C[i, 1:] = np.full(3 * 3, np.nan) - navCovarLong[switchIdx:,:] = np.copy(navCovar) + navCovarLong[switchIdx:, :] = np.copy(navCovar) timeData = position_N[:, 0] * macros.NANO2MIN # BSK_plt.AnimatedCircles(sizeOfCam, circleCenters, circleRadii, validCircle) - BSK_plt.plot_TwoOrbits(position_N[switchIdx:,:], measPos) + BSK_plt.plot_TwoOrbits(position_N[switchIdx:, :], measPos) BSK_plt.diff_vectors(trueR_C, r_C, validCircle, "Circ") - BSK_plt.nav_percentages(truth[switchIdx:,:], navState, navCovar, validCircle, "CNN") + BSK_plt.nav_percentages( + truth[switchIdx:, :], navState, navCovar, validCircle, "CNN" + ) BSK_plt.plot_cirlces(circleCenters, circleRadii, validCircle, sizeOfCam) # # BSK_plt.plot_rate_error(timeData, sigma_BR) @@ -241,10 +310,12 @@ def pull_outputs(self, showPlots): # # BSK_plt.plotStateCovarPlot(measError, measCovar) # BSK_plt.pixelAndPos(measError_C, position_N[switchIdx:,:], circleCenters, np.array(sizeOfCam)) if self.filterUse == "bias": - circleCenters[i,1:] += centerBias[i,1:] - circleRadii[i,1:] += radBias[i,1:] + circleCenters[i, 1:] += centerBias[i, 1:] + circleRadii[i, 1:] += radBias[i, 1:] BSK_plt.plotPostFitResiduals(navPostFits, pixCovar) - BSK_plt.imgProcVsExp(trueCircles, circleCenters, circleRadii, np.array(sizeOfCam)) + BSK_plt.imgProcVsExp( + trueCircles, circleCenters, circleRadii, np.array(sizeOfCam) + ) # BSK_plt.centerXY(circleCenters, np.array(sizeOfCam)) if self.filterUse == "relOD": BSK_plt.plotPostFitResiduals(navPostFits, measCovar) @@ -260,7 +331,6 @@ def pull_outputs(self, showPlots): def run(showPlots, simTime=None): - # Instantiate base simulation TheBSKSim = BSKSim(fswRate=0.5, dynRate=0.5) TheBSKSim.set_DynModel(BSK_OpNavDynamics) @@ -280,19 +350,19 @@ def run(showPlots, simTime=None): # Modes: "None", "-directComm", "-noDisplay" TheScenario.run_vizard("-noDisplay") # Configure FSW mode - TheScenario.masterSim.modeRequest = 'prepOpNav' + TheScenario.masterSim.modeRequest = "prepOpNav" # Initialize simulation TheBSKSim.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.min2nano(5.) + simulationTime = macros.min2nano(5.0) TheBSKSim.ConfigureStopTime(simulationTime) - print('Starting Execution') + print("Starting Execution") t1 = time.time() TheBSKSim.ExecuteSimulation() if TheScenario.filterUse == "bias": - TheScenario.masterSim.modeRequest = 'OpNavAttODB' + TheScenario.masterSim.modeRequest = "OpNavAttODB" if TheScenario.filterUse == "relOD": - TheScenario.masterSim.modeRequest = 'CNNAttOD' + TheScenario.masterSim.modeRequest = "CNNAttOD" if simTime != None: simulationTime = macros.min2nano(simTime) else: @@ -300,11 +370,12 @@ def run(showPlots, simTime=None): TheBSKSim.ConfigureStopTime(simulationTime) TheBSKSim.ExecuteSimulation() t2 = time.time() - print('Finished Execution in ', t2-t1, ' seconds. Post-processing results') - #Terminate vizard and show plots + print("Finished Execution in ", t2 - t1, " seconds. Post-processing results") + # Terminate vizard and show plots figureList = TheScenario.end_scenario() return figureList + if __name__ == "__main__": if not BSK_OpNavFsw.centerRadiusCNNIncluded: print("centerRadiusCNN module is not built, so this scenario can't run.") diff --git a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavAttOD.py b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavAttOD.py index 2a6f406f78..2461613ad2 100644 --- a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavAttOD.py +++ b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavAttOD.py @@ -50,7 +50,6 @@ """ - # Get current file path import inspect import os @@ -58,6 +57,7 @@ import time from Basilisk.utilities import RigidBodyKinematics as rbk + # Import utilities from Basilisk.utilities import orbitalMotion, macros, unitTestSupport @@ -65,21 +65,23 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_OpNav import BSKSim, BSKScenario import BSK_OpNavDynamics, BSK_OpNavFsw import numpy as np # Import plotting file for your scenario -sys.path.append(path + '/../plottingOpNav') +sys.path.append(path + "/../plottingOpNav") import OpNav_Plotting as BSK_plt + # Create your own scenario child class class scenario_OpNav(BSKScenario): """Main Simulation Class""" + def __init__(self, masterSim, showPlots=False): super(scenario_OpNav, self).__init__(masterSim, showPlots) - self.name = 'scenario_opnav' + self.name = "scenario_opnav" self.masterSim = masterSim self.filterUse = "relOD" @@ -89,36 +91,47 @@ def __init__(self, masterSim, showPlots=False): self.scRec = None self.filtRec = None - def configure_initial_conditions(self): # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 18000 * 1E3 # meters + oe.a = 18000 * 1e3 # meters oe.e = 0.6 oe.i = 10 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 80. * macros.D2R # 90 good - mu = self.masterSim.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 80.0 * macros.D2R # 90 good + mu = self.masterSim.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - rError= np.array([10000.,10000., -10000]) - vError= np.array([100, -10, 10]) - MRP= [0,0,0] - if self.filterUse =="relOD": - self.masterSim.get_FswModel().relativeOD.stateInit = (rN + rError).tolist() + (vN + vError).tolist() + rError = np.array([10000.0, 10000.0, -10000]) + vError = np.array([100, -10, 10]) + MRP = [0, 0, 0] + if self.filterUse == "relOD": + self.masterSim.get_FswModel().relativeOD.stateInit = ( + rN + rError + ).tolist() + (vN + vError).tolist() if self.filterUse == "bias": - self.masterSim.get_FswModel().relativeOD.stateInit = (rN + rError).tolist() + (vN + vError).tolist() + bias + self.masterSim.get_FswModel().relativeOD.stateInit = ( + (rN + rError).tolist() + (vN + vError).tolist() + bias + ) self.masterSim.get_DynModel().scObject.hub.r_CN_NInit = rN self.masterSim.get_DynModel().scObject.hub.v_CN_NInit = vN - self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B qNoiseIn = np.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1e-3 * 1e-3 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1e-4 * 1e-4 self.masterSim.get_FswModel().relativeOD.qNoise = qNoiseIn.reshape(36).tolist() self.masterSim.get_FswModel().imageProcessing.noiseSF = 1 self.masterSim.get_FswModel().relativeOD.noiseSF = 5 @@ -145,7 +158,9 @@ def log_outputs(self): self.opNavRec = FswModel.opnavMsg.recorder(samplingTime) self.masterSim.AddModelToTask(DynModel.taskName, self.opNavRec) if self.filterUse == "bias": - self.filtRec = FswModel.pixelLineFilter.filtDataOutMsg.recorder(samplingTime) + self.filtRec = FswModel.pixelLineFilter.filtDataOutMsg.recorder( + samplingTime + ) self.masterSim.AddModelToTask(DynModel.taskName, self.filtRec) self.scRec = DynModel.scObject.scStateOutMsg.recorder(samplingTime) @@ -157,37 +172,69 @@ def log_outputs(self): def pull_outputs(self, showPlots): ## Spacecraft true states - position_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.r_BN_N) - velocity_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.v_BN_N) + position_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.r_BN_N + ) + velocity_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.v_BN_N + ) ## Attitude - sigma_BN = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.sigma_BN) + sigma_BN = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.sigma_BN + ) ## Image processing - circleCenters = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.circlesCenters) - circleRadii = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.circlesRadii) - validCircle = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.valid) + circleCenters = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.circlesCenters + ) + circleRadii = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.circlesRadii + ) + validCircle = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.valid + ) if self.filterUse == "bias": NUM_STATES = 9 ## Navigation results - navState = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.state) - navCovar = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.covar) - navPostFits = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.postFitRes) + navState = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.state + ) + navCovar = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.covar + ) + navPostFits = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.postFitRes + ) if self.filterUse == "relOD": NUM_STATES = 6 ## Navigation results - navState = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.state) - navCovar = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.covar) - navPostFits = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.postFitRes) - measPos = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_N) - r_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_C) - measCovar = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_N) - covar_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_C) + navState = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.state + ) + navCovar = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.covar + ) + navPostFits = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.postFitRes + ) + measPos = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.r_BN_N + ) + r_C = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.r_BN_C + ) + measCovar = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_N + ) + covar_C = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_C + ) sigma_CB = self.masterSim.get_DynModel().cameraMRP_CB sizeMM = self.masterSim.get_DynModel().cameraSize sizeOfCam = self.masterSim.get_DynModel().cameraRez - focal = self.masterSim.get_DynModel().cameraFocal #in m + focal = self.masterSim.get_DynModel().cameraFocal # in m pixelSize = [] pixelSize.append(sizeMM[0] / sizeOfCam[0]) @@ -196,35 +243,37 @@ def pull_outputs(self, showPlots): dcm_CB = rbk.MRP2C(sigma_CB) # Plot results BSK_plt.clear_all_plots() - stateError = np.zeros([len(position_N[:,0]), NUM_STATES+1]) - navCovarLong = np.full([len(position_N[:,0]), 1+NUM_STATES*NUM_STATES], np.nan) - navCovarLong[:,0] = np.copy(position_N[:,0]) + stateError = np.zeros([len(position_N[:, 0]), NUM_STATES + 1]) + navCovarLong = np.full( + [len(position_N[:, 0]), 1 + NUM_STATES * NUM_STATES], np.nan + ) + navCovarLong[:, 0] = np.copy(position_N[:, 0]) stateError[:, 0:4] = np.copy(position_N) - stateError[:,4:7] = np.copy(velocity_N[:,1:]) - pixCovar = np.ones([len(circleCenters[:,0]), 3*3+1]) - pixCovar[:,0] = circleCenters[:,0] - pixCovar[:,1:]*=np.array([1,0,0,0,1,0,0,0,2]) + stateError[:, 4:7] = np.copy(velocity_N[:, 1:]) + pixCovar = np.ones([len(circleCenters[:, 0]), 3 * 3 + 1]) + pixCovar[:, 0] = circleCenters[:, 0] + pixCovar[:, 1:] *= np.array([1, 0, 0, 0, 1, 0, 0, 0, 2]) if self.filterUse == "relOD": - measError = np.full([len(measPos[:,0]), 4], np.nan) - measError[:,0] = measPos[:,0] - measError_C = np.full([len(measPos[:,0]), 5], np.nan) - measError_C[:,0] = measPos[:,0] - trueRhat_C = np.full([len(circleCenters[:,0]), 4], np.nan) - trueR_C = np.full([len(circleCenters[:,0]), 4], np.nan) - trueCircles = np.full([len(circleCenters[:,0]), 4], np.nan) - trueCircles[:,0] = circleCenters[:,0] - trueRhat_C[:,0] = circleCenters[:,0] - trueR_C[:,0] = circleCenters[:,0] - truth = np.zeros([len(position_N[:,0]), 7]) - truth[:,0:4] = np.copy(position_N) - truth[:,4:7] = np.copy(velocity_N[:,1:]) + measError = np.full([len(measPos[:, 0]), 4], np.nan) + measError[:, 0] = measPos[:, 0] + measError_C = np.full([len(measPos[:, 0]), 5], np.nan) + measError_C[:, 0] = measPos[:, 0] + trueRhat_C = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueR_C = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueCircles = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueCircles[:, 0] = circleCenters[:, 0] + trueRhat_C[:, 0] = circleCenters[:, 0] + trueR_C[:, 0] = circleCenters[:, 0] + truth = np.zeros([len(position_N[:, 0]), 7]) + truth[:, 0:4] = np.copy(position_N) + truth[:, 4:7] = np.copy(velocity_N[:, 1:]) centerBias = np.copy(circleCenters) radBias = np.copy(circleRadii) switchIdx = 0 - Rmars = 3396.19*1E3 + Rmars = 3396.19 * 1e3 for j in range(len(stateError[:, 0])): if stateError[j, 0] in navState[:, 0]: stateError[j, 1:4] -= navState[j - switchIdx, 1:4] @@ -232,39 +281,60 @@ def pull_outputs(self, showPlots): else: stateError[j, 1:] = np.full(NUM_STATES, np.nan) switchIdx += 1 - for i in range(len(circleCenters[:,0])): - if circleCenters[i,1:].any() > 1E-8 or circleCenters[i,1:].any() < -1E-8: - trueR_C[i, 1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), - position_N[i + switchIdx, 1:4]) - trueRhat_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i +switchIdx, 1:4])) ,position_N[i +switchIdx, 1:4])/np.linalg.norm(position_N[i +switchIdx, 1:4]) - trueCircles[i,3] = focal*np.tan(np.arcsin(Rmars/np.linalg.norm(position_N[i,1:4])))/pixelSize[0] - trueRhat_C[i,1:] *= focal/trueRhat_C[i,3] - trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0]/2 - 0.5 - trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1]/2 - 0.5 + for i in range(len(circleCenters[:, 0])): + if circleCenters[i, 1:].any() > 1e-8 or circleCenters[i, 1:].any() < -1e-8: + trueR_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) + trueRhat_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) / np.linalg.norm(position_N[i + switchIdx, 1:4]) + trueCircles[i, 3] = ( + focal + * np.tan(np.arcsin(Rmars / np.linalg.norm(position_N[i, 1:4]))) + / pixelSize[0] + ) + trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] + trueCircles[i, 1] = ( + trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 + ) + trueCircles[i, 2] = ( + trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 + ) if self.filterUse == "bias": - centerBias[i,1:3] = np.round(navState[i, 7:9]) - radBias[i,1] = np.round(navState[i, -1]) + centerBias[i, 1:3] = np.round(navState[i, 7:9]) + radBias[i, 1] = np.round(navState[i, -1]) if self.filterUse == "relOD": - measError[i, 1:4] = position_N[i +switchIdx, 1:4] - measPos[i, 1:4] - measError_C[i, 4] = np.linalg.norm(position_N[i +switchIdx, 1:4]) - np.linalg.norm(r_C[i, 1:4]) - measError_C[i, 1:4] = trueRhat_C[i,1:] - r_C[i, 1:4]/np.linalg.norm(r_C[i, 1:4]) + measError[i, 1:4] = position_N[i + switchIdx, 1:4] - measPos[i, 1:4] + measError_C[i, 4] = np.linalg.norm( + position_N[i + switchIdx, 1:4] + ) - np.linalg.norm(r_C[i, 1:4]) + measError_C[i, 1:4] = trueRhat_C[i, 1:] - r_C[ + i, 1:4 + ] / np.linalg.norm(r_C[i, 1:4]) else: if self.filterUse == "relOD": - measCovar[i,1:] = np.full(3*3, np.nan) + measCovar[i, 1:] = np.full(3 * 3, np.nan) covar_C[i, 1:] = np.full(3 * 3, np.nan) - navCovarLong[switchIdx:,:] = np.copy(navCovar) + navCovarLong[switchIdx:, :] = np.copy(navCovar) - BSK_plt.plot_TwoOrbits(position_N[switchIdx:,:], measPos) + BSK_plt.plot_TwoOrbits(position_N[switchIdx:, :], measPos) BSK_plt.diff_vectors(trueR_C, r_C, validCircle, "Circ") - BSK_plt.nav_percentages(truth[switchIdx:,:], navState, navCovar, validCircle, "Circ") + BSK_plt.nav_percentages( + truth[switchIdx:, :], navState, navCovar, validCircle, "Circ" + ) BSK_plt.plot_cirlces(circleCenters, circleRadii, validCircle, sizeOfCam) BSK_plt.plotStateCovarPlot(stateError, navCovarLong) if self.filterUse == "bias": - circleCenters[i,1:] += centerBias[i,1:] - circleRadii[i,1:] += radBias[i,1:] + circleCenters[i, 1:] += centerBias[i, 1:] + circleRadii[i, 1:] += radBias[i, 1:] BSK_plt.plotPostFitResiduals(navPostFits, pixCovar) - BSK_plt.imgProcVsExp(trueCircles, circleCenters, circleRadii, np.array(sizeOfCam)) + BSK_plt.imgProcVsExp( + trueCircles, circleCenters, circleRadii, np.array(sizeOfCam) + ) if self.filterUse == "relOD": BSK_plt.plotPostFitResiduals(navPostFits, measCovar) figureList = {} @@ -279,7 +349,6 @@ def pull_outputs(self, showPlots): def run(showPlots, simTime=None): - # Instantiate base simulation TheBSKSim = BSKSim(fswRate=0.5, dynRate=0.5) TheBSKSim.set_DynModel(BSK_OpNavDynamics) @@ -300,19 +369,19 @@ def run(showPlots, simTime=None): TheScenario.run_vizard("-noDisplay") # Configure FSW mode - TheScenario.masterSim.modeRequest = 'prepOpNav' + TheScenario.masterSim.modeRequest = "prepOpNav" # Initialize simulation TheBSKSim.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.min2nano(3.) + simulationTime = macros.min2nano(3.0) TheBSKSim.ConfigureStopTime(simulationTime) - print('Starting Execution') + print("Starting Execution") t1 = time.time() TheBSKSim.ExecuteSimulation() if TheScenario.filterUse == "bias": - TheScenario.masterSim.modeRequest = 'OpNavAttODB' + TheScenario.masterSim.modeRequest = "OpNavAttODB" if TheScenario.filterUse == "relOD": - TheScenario.masterSim.modeRequest = 'OpNavAttOD' + TheScenario.masterSim.modeRequest = "OpNavAttOD" if simTime != None: simulationTime = macros.min2nano(simTime) else: @@ -320,7 +389,7 @@ def run(showPlots, simTime=None): TheBSKSim.ConfigureStopTime(simulationTime) TheBSKSim.ExecuteSimulation() t2 = time.time() - print('Finished Execution in ', t2-t1, ' seconds. Post-processing results') + print("Finished Execution in ", t2 - t1, " seconds. Post-processing results") # Terminate vizard and show plots figureList = TheScenario.end_scenario() return figureList diff --git a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavAttODLimb.py b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavAttODLimb.py index e314d0787c..3d72dde5f4 100644 --- a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavAttODLimb.py +++ b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavAttODLimb.py @@ -32,8 +32,6 @@ """ - - # Get current file path import inspect import os @@ -41,6 +39,7 @@ import time from Basilisk.utilities import RigidBodyKinematics as rbk + # Import utilities from Basilisk.utilities import orbitalMotion, macros, unitTestSupport @@ -48,22 +47,23 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_OpNav import BSKSim, BSKScenario import BSK_OpNavDynamics, BSK_OpNavFsw import numpy as np # Import plotting file for your scenario -sys.path.append(path + '/../plottingOpNav') +sys.path.append(path + "/../plottingOpNav") import OpNav_Plotting as BSK_plt + # Create your own scenario child class class scenario_OpNav(BSKScenario): """Main Simulation Class""" def __init__(self, masterSim, showPlots=False): super(scenario_OpNav, self).__init__(masterSim, showPlots) - self.name = 'scenario_opnav' + self.name = "scenario_opnav" self.masterSim = masterSim # declare additional class variables @@ -75,30 +75,40 @@ def __init__(self, masterSim, showPlots=False): def configure_initial_conditions(self): # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 18000 * 1E3 # meters + oe.a = 18000 * 1e3 # meters oe.e = 0.6 oe.i = 10 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 80. * macros.D2R # 90 good - mu = self.masterSim.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 80.0 * macros.D2R # 90 good + mu = self.masterSim.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - rError= np.array([10000.,10000., -10000]) - vError= np.array([100, -10, 10]) + rError = np.array([10000.0, 10000.0, -10000]) + vError = np.array([100, -10, 10]) - MRP= [0,-0.3,0] - self.masterSim.get_FswModel().relativeOD.stateInit = (rN+rError).tolist() + (vN+vError).tolist() + MRP = [0, -0.3, 0] + self.masterSim.get_FswModel().relativeOD.stateInit = (rN + rError).tolist() + ( + vN + vError + ).tolist() self.masterSim.get_DynModel().scObject.hub.r_CN_NInit = rN self.masterSim.get_DynModel().scObject.hub.v_CN_NInit = vN - self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B qNoiseIn = np.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1e-3 * 1e-3 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1e-4 * 1e-4 self.masterSim.get_FswModel().relativeOD.qNoise = qNoiseIn.reshape(36).tolist() self.masterSim.get_FswModel().horizonNav.noiseSF = 70 self.masterSim.get_FswModel().relativeOD.noiseSF = 5 @@ -108,10 +118,9 @@ def configure_initial_conditions(self): # self.masterSim.get_DynModel().cameraMod.gaussian = 5 #2 # # self.masterSim.get_DynModel().cameraMod.darkCurrent = 0.5 #0 # # self.masterSim.get_DynModel().cameraMod.saltPepper = 1 # 0.5 # - self.masterSim.get_DynModel().cameraMod.cosmicRays = 1 #1 # + self.masterSim.get_DynModel().cameraMod.cosmicRays = 1 # 1 # # self.masterSim.get_DynModel().cameraMod.blurParam = 5 #3 # - def log_outputs(self): # Dynamics process outputs: log messages below if desired. FswModel = self.masterSim.get_FswModel() @@ -135,33 +144,57 @@ def log_outputs(self): def pull_outputs(self, showPlots): # Dynamics process outputs: pull log messages below if any ## Spacecraft true states - position_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.r_BN_N) - velocity_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.v_BN_N) + position_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.r_BN_N + ) + velocity_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.v_BN_N + ) ## Attitude - sigma_BN = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.sigma_BN) + sigma_BN = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.sigma_BN + ) ## Image processing - limb = unitTestSupport.addTimeColumn(self.limbRec.times(), self.limbRec.limbPoints) - numLimbPoints = unitTestSupport.addTimeColumn(self.limbRec.times(), self.limbRec.numLimbPoints) - validLimb = unitTestSupport.addTimeColumn(self.limbRec.times(), self.limbRec.valid) + limb = unitTestSupport.addTimeColumn( + self.limbRec.times(), self.limbRec.limbPoints + ) + numLimbPoints = unitTestSupport.addTimeColumn( + self.limbRec.times(), self.limbRec.numLimbPoints + ) + validLimb = unitTestSupport.addTimeColumn( + self.limbRec.times(), self.limbRec.valid + ) ## OpNav Out - measPos = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_N) + measPos = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.r_BN_N + ) r_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_C) - measCovar = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_N) - covar_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_C) + measCovar = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_N + ) + covar_C = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_C + ) NUM_STATES = 6 ## Navigation results - navState = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.state) - navCovar = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.covar) - navPostFits = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.postFitRes) + navState = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.state + ) + navCovar = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.covar + ) + navPostFits = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.postFitRes + ) sigma_CB = self.masterSim.get_DynModel().cameraMRP_CB sizeMM = self.masterSim.get_DynModel().cameraSize sizeOfCam = self.masterSim.get_DynModel().cameraRez - focal = self.masterSim.get_DynModel().cameraFocal #in m + focal = self.masterSim.get_DynModel().cameraFocal # in m pixelSize = [] pixelSize.append(sizeMM[0] / sizeOfCam[0]) @@ -170,28 +203,30 @@ def pull_outputs(self, showPlots): dcm_CB = rbk.MRP2C(sigma_CB) # Plot results BSK_plt.clear_all_plots() - stateError = np.zeros([len(position_N[:,0]), NUM_STATES+1]) - navCovarLong = np.full([len(position_N[:,0]), 1+NUM_STATES*NUM_STATES], np.nan) - navCovarLong[:,0] = np.copy(position_N[:,0]) + stateError = np.zeros([len(position_N[:, 0]), NUM_STATES + 1]) + navCovarLong = np.full( + [len(position_N[:, 0]), 1 + NUM_STATES * NUM_STATES], np.nan + ) + navCovarLong[:, 0] = np.copy(position_N[:, 0]) stateError[:, 0:4] = np.copy(position_N) - stateError[:,4:7] = np.copy(velocity_N[:,1:]) - measError = np.full([len(measPos[:,0]), 4], np.nan) - measError[:,0] = measPos[:,0] - measError_C = np.full([len(measPos[:,0]), 5], np.nan) - measError_C[:,0] = measPos[:,0] - trueRhat_C = np.full([len(numLimbPoints[:,0]), 4], np.nan) - trueR_C = np.full([len(numLimbPoints[:,0]), 4], np.nan) - trueCircles = np.full([len(numLimbPoints[:,0]), 4], np.nan) - trueCircles[:,0] = numLimbPoints[:,0] - trueRhat_C[:,0] = numLimbPoints[:,0] - trueR_C[:,0] = numLimbPoints[:,0] - truth = np.zeros([len(position_N[:,0]), 7]) - truth[:,0:4] = np.copy(position_N) - truth[:,4:7] = np.copy(velocity_N[:,1:]) + stateError[:, 4:7] = np.copy(velocity_N[:, 1:]) + measError = np.full([len(measPos[:, 0]), 4], np.nan) + measError[:, 0] = measPos[:, 0] + measError_C = np.full([len(measPos[:, 0]), 5], np.nan) + measError_C[:, 0] = measPos[:, 0] + trueRhat_C = np.full([len(numLimbPoints[:, 0]), 4], np.nan) + trueR_C = np.full([len(numLimbPoints[:, 0]), 4], np.nan) + trueCircles = np.full([len(numLimbPoints[:, 0]), 4], np.nan) + trueCircles[:, 0] = numLimbPoints[:, 0] + trueRhat_C[:, 0] = numLimbPoints[:, 0] + trueR_C[:, 0] = numLimbPoints[:, 0] + truth = np.zeros([len(position_N[:, 0]), 7]) + truth[:, 0:4] = np.copy(position_N) + truth[:, 4:7] = np.copy(velocity_N[:, 1:]) switchIdx = 0 - Rmars = 3396.19*1E3 + Rmars = 3396.19 * 1e3 for j in range(len(stateError[:, 0])): if stateError[j, 0] in navState[:, 0]: stateError[j, 1:4] -= navState[j - switchIdx, 1:4] @@ -199,27 +234,47 @@ def pull_outputs(self, showPlots): else: stateError[j, 1:] = np.full(NUM_STATES, np.nan) switchIdx += 1 - for i in range(len(numLimbPoints[:,0])): - if numLimbPoints[i,1] > 1E-8: - measError[i, 1:4] = position_N[i +switchIdx, 1:4] - measPos[i, 1:4] - measError_C[i, 4] = np.linalg.norm(position_N[i +switchIdx, 1:4]) - np.linalg.norm(r_C[i, 1:4]) - trueR_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i +switchIdx, 1:4])) , position_N[i +switchIdx, 1:4]) - trueRhat_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i +switchIdx, 1:4])) ,position_N[i +switchIdx, 1:4])/np.linalg.norm(position_N[i +switchIdx, 1:4]) - trueCircles[i,3] = focal*np.tan(np.arcsin(Rmars/np.linalg.norm(position_N[i,1:4])))/pixelSize[0] - trueRhat_C[i,1:] *= focal/trueRhat_C[i,3] - measError_C[i, 1:4] = trueRhat_C[i,1:] - r_C[i, 1:4]/np.linalg.norm(r_C[i, 1:4]) - trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0]/2 - 0.5 - trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1]/2 - 0.5 + for i in range(len(numLimbPoints[:, 0])): + if numLimbPoints[i, 1] > 1e-8: + measError[i, 1:4] = position_N[i + switchIdx, 1:4] - measPos[i, 1:4] + measError_C[i, 4] = np.linalg.norm( + position_N[i + switchIdx, 1:4] + ) - np.linalg.norm(r_C[i, 1:4]) + trueR_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) + trueRhat_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) / np.linalg.norm(position_N[i + switchIdx, 1:4]) + trueCircles[i, 3] = ( + focal + * np.tan(np.arcsin(Rmars / np.linalg.norm(position_N[i, 1:4]))) + / pixelSize[0] + ) + trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] + measError_C[i, 1:4] = trueRhat_C[i, 1:] - r_C[i, 1:4] / np.linalg.norm( + r_C[i, 1:4] + ) + trueCircles[i, 1] = ( + trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 + ) + trueCircles[i, 2] = ( + trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 + ) else: - measCovar[i,1:] = np.full(3*3, np.nan) + measCovar[i, 1:] = np.full(3 * 3, np.nan) covar_C[i, 1:] = np.full(3 * 3, np.nan) - navCovarLong[switchIdx:,:] = np.copy(navCovar) + navCovarLong[switchIdx:, :] = np.copy(navCovar) timeData = position_N[:, 0] * macros.NANO2MIN BSK_plt.plot_TwoOrbits(position_N, measPos) BSK_plt.diff_vectors(trueR_C, r_C, validLimb, "Limb") - BSK_plt.nav_percentages(truth[switchIdx:,:], navState, navCovar, validLimb, "Limb") + BSK_plt.nav_percentages( + truth[switchIdx:, :], navState, navCovar, validLimb, "Limb" + ) BSK_plt.plot_limb(limb, numLimbPoints, validLimb, sizeOfCam) # BSK_plt.AnimatedScatter(sizeOfCam, circleCenters, circleRadii, validCircle) BSK_plt.plotStateCovarPlot(stateError, navCovarLong) @@ -238,7 +293,6 @@ def pull_outputs(self, showPlots): def run(showPlots, simTime=None): - # Instantiate base simulation TheBSKSim = BSKSim(fswRate=0.5, dynRate=0.5) TheBSKSim.set_DynModel(BSK_OpNavDynamics) @@ -258,16 +312,16 @@ def run(showPlots, simTime=None): TheScenario.run_vizard("-noDisplay") # Configure FSW mode - TheScenario.masterSim.modeRequest = 'prepOpNav' + TheScenario.masterSim.modeRequest = "prepOpNav" # Initialize simulation TheBSKSim.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.min2nano(3.) + simulationTime = macros.min2nano(3.0) TheBSKSim.ConfigureStopTime(simulationTime) - print('Starting Execution') + print("Starting Execution") t1 = time.time() TheBSKSim.ExecuteSimulation() - TheScenario.masterSim.modeRequest = 'OpNavAttODLimb' + TheScenario.masterSim.modeRequest = "OpNavAttODLimb" if simTime != None: simulationTime = macros.min2nano(simTime) else: @@ -275,7 +329,7 @@ def run(showPlots, simTime=None): TheBSKSim.ConfigureStopTime(simulationTime) TheBSKSim.ExecuteSimulation() t2 = time.time() - print('Finished Execution in ', t2-t1, ' seconds. Post-processing results') + print("Finished Execution in ", t2 - t1, " seconds. Post-processing results") # Terminate vizard and show plots figureList = TheScenario.end_scenario() return figureList diff --git a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavHeading.py b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavHeading.py index ac280a8755..a18534569c 100644 --- a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavHeading.py +++ b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavHeading.py @@ -31,6 +31,7 @@ python3 scenario_OpNavHeading.py """ + # Get current file path import inspect import os @@ -38,6 +39,7 @@ import time from Basilisk.utilities import RigidBodyKinematics as rbk + # Import utilities from Basilisk.utilities import orbitalMotion, macros, unitTestSupport @@ -45,34 +47,39 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_OpNav import BSKSim, BSKScenario import BSK_OpNavDynamics, BSK_OpNavFsw import numpy as np # Import plotting file for your scenario -sys.path.append(path + '/../plottingOpNav') +sys.path.append(path + "/../plottingOpNav") import OpNav_Plotting as BSK_plt + def DCM(bVec, d): - DCM_exp = np.zeros([3,3]) + DCM_exp = np.zeros([3, 3]) - if np.linalg.norm(np.cross(bVec,d)) <1E-5: + if np.linalg.norm(np.cross(bVec, d)) < 1e-5: return np.eye(3) else: DCM_exp[:, 0] = np.array(d) / np.linalg.norm(d) - DCM_exp[:, 1] = np.cross(DCM_exp[:, 0], bVec) / np.linalg.norm(np.array(np.cross(DCM_exp[:, 0], bVec))) + DCM_exp[:, 1] = np.cross(DCM_exp[:, 0], bVec) / np.linalg.norm( + np.array(np.cross(DCM_exp[:, 0], bVec)) + ) DCM_exp[:, 2] = np.cross(DCM_exp[:, 0], DCM_exp[:, 1]) / np.linalg.norm( - np.cross(DCM_exp[:, 0], DCM_exp[:, 1])) + np.cross(DCM_exp[:, 0], DCM_exp[:, 1]) + ) return DCM_exp + # Create your own scenario child class class scenario_OpNav(BSKScenario): """Main Simulation Class""" def __init__(self, masterSim, showPlots=False): super(scenario_OpNav, self).__init__(masterSim, showPlots) - self.name = 'scenario_opnav' + self.name = "scenario_opnav" self.masterSim = masterSim self.filterUse = "bias" # "relOD" @@ -88,35 +95,48 @@ def __init__(self, masterSim, showPlots=False): def configure_initial_conditions(self): # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 18000*1E3 # meters + oe.a = 18000 * 1e3 # meters self.semiMajAxis = oe.a - oe.e = 0. + oe.e = 0.0 oe.i = 20 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 100. * macros.D2R #90 good - mu = self.masterSim.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 100.0 * macros.D2R # 90 good + mu = self.masterSim.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - MRP= [0,0,0] - if self.filterUse =="relOD": - self.masterSim.get_FswModel().relativeOD.stateInit = rN.tolist() + vN.tolist() + MRP = [0, 0, 0] + if self.filterUse == "relOD": + self.masterSim.get_FswModel().relativeOD.stateInit = ( + rN.tolist() + vN.tolist() + ) if self.filterUse == "bias": - self.masterSim.get_FswModel().pixelLineFilter.stateInit = rN.tolist() + vN.tolist() + bias + self.masterSim.get_FswModel().pixelLineFilter.stateInit = ( + rN.tolist() + vN.tolist() + bias + ) self.masterSim.get_DynModel().scObject.hub.r_CN_NInit = rN # m - r_CN_N self.masterSim.get_DynModel().scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B # Search self.masterSim.get_FswModel().opNavPoint.omega_RN_B = [0.001, 0.0, -0.001] # self.masterSim.get_FswModel().opNavPoint.opnavDataInMsgName = "heading_filtered" self.masterSim.get_FswModel().imageProcessing.noiseSF = 0.5 self.masterSim.get_FswModel().headingUKF.noiseSF = 1.001 self.masterSim.get_FswModel().opNavPoint.opnavDataInMsg.subscribeTo( - self.masterSim.get_FswModel().headingUKF.opnavDataOutMsg) + self.masterSim.get_FswModel().headingUKF.opnavDataOutMsg + ) def log_outputs(self): # Dynamics process outputs: log messages below if desired. @@ -128,7 +148,9 @@ def log_outputs(self): self.opNavRec = FswModel.opnavMsg.recorder(samplingTime) self.attGuidRec = FswModel.attGuidMsg.recorder(samplingTime) - self.rwMotorRec = FswModel.rwMotorTorque.rwMotorTorqueOutMsg.recorder(samplingTime) + self.rwMotorRec = FswModel.rwMotorTorque.rwMotorTorqueOutMsg.recorder( + samplingTime + ) self.circlesRec = FswModel.opnavCirclesMsg.recorder(samplingTime) self.scRec = DynModel.scObject.scStateOutMsg.recorder(samplingTime) @@ -140,7 +162,9 @@ def log_outputs(self): self.rwLogs = [] for item in range(4): - self.rwLogs.append(DynModel.rwStateEffector.rwOutMsgs[item].recorder(samplingTime)) + self.rwLogs.append( + DynModel.rwStateEffector.rwOutMsgs[item].recorder(samplingTime) + ) self.masterSim.AddModelToTask(DynModel.taskName, self.rwLogs[item]) self.headingBVecLog = FswModel.headingUKF.logger("bVec_B") @@ -156,37 +180,71 @@ def pull_outputs(self, showPlots): # Dynamics process outputs: pull log messages below if any ## Spacecraft true states - position_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.r_BN_N) + position_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.r_BN_N + ) ## Attitude - sigma_BN = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.sigma_BN) - Outomega_BN = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.omega_BN_B) + sigma_BN = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.sigma_BN + ) + Outomega_BN = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.omega_BN_B + ) ## Image processing - circleCenters = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.circlesCenters) - circleRadii = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.circlesRadii) - validCircle = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.valid) - - frame = unitTestSupport.addTimeColumn(self.headingBVecLog.times(), self.headingBVecLog.bVec_B) + circleCenters = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.circlesCenters + ) + circleRadii = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.circlesRadii + ) + validCircle = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.valid + ) + + frame = unitTestSupport.addTimeColumn( + self.headingBVecLog.times(), self.headingBVecLog.bVec_B + ) numRW = 4 dataRW = [] for i in range(numRW): - dataRW.append(unitTestSupport.addTimeColumn(self.rwMotorRec.times(), self.rwLogs[i].u_current)) - - measPos = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_N) + dataRW.append( + unitTestSupport.addTimeColumn( + self.rwMotorRec.times(), self.rwLogs[i].u_current + ) + ) + + measPos = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.r_BN_N + ) r_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_C) - measCovar = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_N) - covar_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_C) - covar_B = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_B) + measCovar = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_N + ) + covar_C = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_C + ) + covar_B = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_B + ) FilterType = "Switch-SRuKF" numStates = 5 # Get the filter outputs through the messages - stateLog = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.state) - r_BN_C = unitTestSupport.addTimeColumn(self.opNavFiltRec.times(), self.opNavFiltRec.r_BN_C) - postFitLog = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.postFitRes) - covarLog = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.covar) + stateLog = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.state + ) + r_BN_C = unitTestSupport.addTimeColumn( + self.opNavFiltRec.times(), self.opNavFiltRec.r_BN_C + ) + postFitLog = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.postFitRes + ) + covarLog = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.covar + ) stateLog[0, 3] = 1.0 # adjust first measurement to be non-zero for i in range(len(stateLog[:, 0])): stateLog[i, 1:4] = stateLog[i, 1:4] / np.linalg.norm(stateLog[i, 1:4]) @@ -194,11 +252,11 @@ def pull_outputs(self, showPlots): sHat_B = np.zeros(np.shape(position_N)) sHatDot_B = np.zeros(np.shape(position_N)) for i in range(len(position_N[:, 0])): - sHat_N = - position_N[i,1:4]/np.linalg.norm(position_N[i,1:4]) + sHat_N = -position_N[i, 1:4] / np.linalg.norm(position_N[i, 1:4]) dcm_BN = rbk.MRP2C(sigma_BN[i, 1:]) sHat_B[i, 0] = sHatDot_B[i, 0] = position_N[i, 0] sHat_B[i, 1:] = np.dot(dcm_BN, sHat_N) - sHatDot_B[i, 1:] = - np.cross(Outomega_BN[i, 1:], sHat_B[i, 1:]) + sHatDot_B[i, 1:] = -np.cross(Outomega_BN[i, 1:], sHat_B[i, 1:]) stateLogSEKF = np.zeros([len(stateLog[:, 0]), 7]) stateLogSEKF[:, 0:4] = stateLog[:, 0:4] @@ -216,17 +274,21 @@ def pull_outputs(self, showPlots): covarLog_B = np.copy(covarLog) filterOmega_BN_S = np.zeros(np.shape(trueOmega_BN_S)) filterOmega_BN_S[:, 2:] = stateLog[:, 4:6] - dcmBS = np.zeros([position_N.shape[0],3,3]) + dcmBS = np.zeros([position_N.shape[0], 3, 3]) for i in range(len(stateLog[:, 0])): DCM_BS = DCM(frame[i, 1:], sHat_B[i, 1:]) - dcmBS[i,:,:] = DCM_BS + dcmBS[i, :, :] = DCM_BS expected[i, 4:6] = np.dot(DCM_BS, Outomega_BN[i, 1:])[1:3] trueOmega_BN_S[i, 1:] = np.dot(np.transpose(DCM_BS), Outomega_BN[i, 1:]) - filterOmega_BN[i, 1:] = np.dot(DCM_BS, np.array([0., stateLog[i, 4], stateLog[i, 5]])) - stateLogSEKF[i, 4:] = - np.cross(filterOmega_BN[i, 1:], stateLog[i, 1:4]) + filterOmega_BN[i, 1:] = np.dot( + DCM_BS, np.array([0.0, stateLog[i, 4], stateLog[i, 5]]) + ) + stateLogSEKF[i, 4:] = -np.cross(filterOmega_BN[i, 1:], stateLog[i, 1:4]) tempCovar = np.zeros([3, 3]) - tempCovar[1:, 1:] = np.reshape(covarLog[i, 1:5 * 5 + 1], [5, 5])[3:, 3:] - covarLog_B[i, -4:] = np.dot(np.dot(DCM_BS, tempCovar), np.transpose(DCM_BS))[1:, 1:].flatten() + tempCovar[1:, 1:] = np.reshape(covarLog[i, 1 : 5 * 5 + 1], [5, 5])[3:, 3:] + covarLog_B[i, -4:] = np.dot( + np.dot(DCM_BS, tempCovar), np.transpose(DCM_BS) + )[1:, 1:].flatten() # # plot the results @@ -243,11 +305,18 @@ def pull_outputs(self, showPlots): errorDeg[i, 0] = stateLog[i, 0] rateError[i, 0] = stateLog[i, 0] covarDeg[i, 0] = stateLog[i, 0] - errorDeg[i, 1] = np.arccos(np.dot(stateLogSEKF[i, 1:4], expectedSEKF[i, 1:4])) + errorDeg[i, 1] = np.arccos( + np.dot(stateLogSEKF[i, 1:4], expectedSEKF[i, 1:4]) + ) rateError[i, 1] = np.linalg.norm(errorVsTruthSEKF[i, 4:]) - covarVec = np.array([stateLog[i, 1] + np.sqrt(covarLog[i, 1]), stateLog[i, 2] + np.sqrt(covarLog[i, 2 + numStates]), - stateLog[i, 3] + np.sqrt(covarLog[i, 3 + 2 * numStates])]) + covarVec = np.array( + [ + stateLog[i, 1] + np.sqrt(covarLog[i, 1]), + stateLog[i, 2] + np.sqrt(covarLog[i, 2 + numStates]), + stateLog[i, 3] + np.sqrt(covarLog[i, 3 + 2 * numStates]), + ] + ) covarVec = covarVec / np.linalg.norm(covarVec) covarDeg[i, 1] = 3 * np.arccos(np.dot(covarVec, stateLog[i, 1:4])) # covarDeg[i, 1] = np.linalg.norm(np.array([np.sqrt(covarLog[i,1]),np.sqrt(covarLog[i,1]),np.sqrt(covarLog[i,1])])) @@ -266,7 +335,7 @@ def pull_outputs(self, showPlots): sigma_CB = self.masterSim.get_DynModel().cameraMRP_CB sizeMM = self.masterSim.get_DynModel().cameraSize sizeOfCam = self.masterSim.get_DynModel().cameraRez - focal = self.masterSim.get_DynModel().cameraFocal #in m + focal = self.masterSim.get_DynModel().cameraFocal # in m pixelSize = [] pixelSize.append(sizeMM[0] / sizeOfCam[0]) @@ -275,53 +344,71 @@ def pull_outputs(self, showPlots): dcm_CB = rbk.MRP2C(sigma_CB) # Plot results BSK_plt.clear_all_plots() - pixCovar = np.ones([len(circleCenters[:,0]), 3*3+1]) - pixCovar[:,0] = circleCenters[:,0] - pixCovar[:,1:]*=np.array([1,0,0,0,1,0,0,0,2]) + pixCovar = np.ones([len(circleCenters[:, 0]), 3 * 3 + 1]) + pixCovar[:, 0] = circleCenters[:, 0] + pixCovar[:, 1:] *= np.array([1, 0, 0, 0, 1, 0, 0, 0, 2]) - measError = np.full([len(measPos[:,0]), 4], np.nan) - measError[:,0] = measPos[:,0] - measError_C = np.full([len(measPos[:,0]), 5], np.nan) - measError_C[:,0] = measPos[:,0] + measError = np.full([len(measPos[:, 0]), 4], np.nan) + measError[:, 0] = measPos[:, 0] + measError_C = np.full([len(measPos[:, 0]), 5], np.nan) + measError_C[:, 0] = measPos[:, 0] - trueRhat_C = np.full([len(circleCenters[:,0]), 4], np.nan) - trueCircles = np.full([len(circleCenters[:,0]), 4], np.nan) - trueCircles[:,0] = circleCenters[:,0] - trueRhat_C[:,0] = circleCenters[:,0] + trueRhat_C = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueCircles = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueCircles[:, 0] = circleCenters[:, 0] + trueRhat_C[:, 0] = circleCenters[:, 0] centerBias = np.copy(circleCenters) radBias = np.copy(circleRadii) ModeIdx = 0 modeSwitch = 0 - Rmars = 3396.19*1E3 + Rmars = 3396.19 * 1e3 for j in range(len(position_N[:, 0])): if circleCenters[j, 1] > 0: modeSwitch = j break covarC = np.zeros([covarLog.shape[0], 3, 3]) covarOmega = np.zeros([covarLog.shape[0], 2, 2]) - for i in range(len(circleCenters[:,0])): - trueRhat_C[i, 1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[ModeIdx + i, 1:4])), - position_N[ModeIdx + i, 1:4]) / np.linalg.norm(position_N[ModeIdx + i, 1:4]) + for i in range(len(circleCenters[:, 0])): + trueRhat_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[ModeIdx + i, 1:4])), + position_N[ModeIdx + i, 1:4], + ) / np.linalg.norm(position_N[ModeIdx + i, 1:4]) trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] - covarC[i, :, :] = np.array(covarLog[i, 1:]).reshape([5, 5])[:3,:3] - covarOmega[i, :, :] = np.array(covarLog[i, 1:]).reshape([5, 5])[4:,4:] - covarC[i, :,:] = np.dot(np.dot(dcm_CB, covarC[i, :, :]), dcm_CB.T) - temp = np.zeros([3,3]) - temp[0,0] = covarOmega[i, 1, 1] - temp[1:,1:] = covarOmega[i, :, :] - covarOmega[i, :,:] = np.dot(np.dot(dcmBS[i,:,:], temp), dcmBS[i,:,:].T)[1:,1:] - if circleCenters[i,1:].any() > 1E-8 or circleCenters[i,1:].any() < -1E-8: - trueCircles[i,3] = focal*np.tan(np.arcsin(Rmars/np.linalg.norm(position_N[ModeIdx+i,1:4])))/pixelSize[0] - trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0]/2 - 0.5 - trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1]/2 - 0.5 - - measError[i, 1:4] = position_N[ModeIdx+i, 1:4] - measPos[i, 1:4] - measError_C[i, 4] = np.linalg.norm(position_N[ModeIdx+i, 1:4]) - np.linalg.norm(r_C[i, 1:4]) - measError_C[i, 1:4] = trueRhat_C[i,1:] - r_C[i, 1:4]/np.linalg.norm(r_C[i, 1:4]) + covarC[i, :, :] = np.array(covarLog[i, 1:]).reshape([5, 5])[:3, :3] + covarOmega[i, :, :] = np.array(covarLog[i, 1:]).reshape([5, 5])[4:, 4:] + covarC[i, :, :] = np.dot(np.dot(dcm_CB, covarC[i, :, :]), dcm_CB.T) + temp = np.zeros([3, 3]) + temp[0, 0] = covarOmega[i, 1, 1] + temp[1:, 1:] = covarOmega[i, :, :] + covarOmega[i, :, :] = np.dot( + np.dot(dcmBS[i, :, :], temp), dcmBS[i, :, :].T + )[1:, 1:] + if circleCenters[i, 1:].any() > 1e-8 or circleCenters[i, 1:].any() < -1e-8: + trueCircles[i, 3] = ( + focal + * np.tan( + np.arcsin(Rmars / np.linalg.norm(position_N[ModeIdx + i, 1:4])) + ) + / pixelSize[0] + ) + trueCircles[i, 1] = ( + trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 + ) + trueCircles[i, 2] = ( + trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 + ) + + measError[i, 1:4] = position_N[ModeIdx + i, 1:4] - measPos[i, 1:4] + measError_C[i, 4] = np.linalg.norm( + position_N[ModeIdx + i, 1:4] + ) - np.linalg.norm(r_C[i, 1:4]) + measError_C[i, 1:4] = trueRhat_C[i, 1:] - r_C[i, 1:4] / np.linalg.norm( + r_C[i, 1:4] + ) else: - measCovar[i,1:] = np.full(3*3, np.nan) + measCovar[i, 1:] = np.full(3 * 3, np.nan) timeData = position_N[:, 0] * macros.NANO2MIN @@ -329,20 +416,24 @@ def pull_outputs(self, showPlots): # BSK_plt.plot_cirlces(timeData[switchIdx:], circleCenters, circleRadii, validCircle, sizeOfCam) # plt.close('all') show_plots = True - m2km = 1E-3 - covar_B[:,1:] *=1./(self.semiMajAxis**2) - covarOmega[:,1,1] *=3 + m2km = 1e-3 + covar_B[:, 1:] *= 1.0 / (self.semiMajAxis**2) + covarOmega[:, 1, 1] *= 3 r_NB_hat_C = np.copy(r_BN_C) r_NB_hat_C[0, 3] = 1.0 # adjust first state to have a non-zero norm for i in range(r_NB_hat_C.shape[0]): - r_NB_hat_C[i,1:]*= -1./np.linalg.norm(r_NB_hat_C[i,1:]) - trueRhat_C[i,1:]*=1./np.linalg.norm(trueRhat_C[i,1:]) + r_NB_hat_C[i, 1:] *= -1.0 / np.linalg.norm(r_NB_hat_C[i, 1:]) + trueRhat_C[i, 1:] *= 1.0 / np.linalg.norm(trueRhat_C[i, 1:]) rError = np.copy(r_NB_hat_C) - rError[:,1:] -= trueRhat_C[:,1:] - omegaError = np.zeros([position_N.shape[0],3]) - omegaError[:,0] = position_N[:,0] - omegaError[:,1:] = errorVsTruth[:,4:] - BSK_plt.vecTrack(trueRhat_C[modeSwitch:,:], r_NB_hat_C[modeSwitch:,:], covarC[modeSwitch:,:]) + rError[:, 1:] -= trueRhat_C[:, 1:] + omegaError = np.zeros([position_N.shape[0], 3]) + omegaError[:, 0] = position_N[:, 0] + omegaError[:, 1:] = errorVsTruth[:, 4:] + BSK_plt.vecTrack( + trueRhat_C[modeSwitch:, :], + r_NB_hat_C[modeSwitch:, :], + covarC[modeSwitch:, :], + ) # BSK_plt.omegaTrack(omegaError[modeSwitch:], covarOmega[modeSwitch:,:,:]) # BSK_plt.PostFitResiduals(postFitLog[modeSwitch:,:], covar_B[modeSwitch:,:], FilterType, show_plots) # BSK_plt.plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW) @@ -363,8 +454,7 @@ def pull_outputs(self, showPlots): return figureList -def run(showPlots, simTime = None): - +def run(showPlots, simTime=None): # Instantiate base simulation TheBSKSim = BSKSim(fswRate=0.5, dynRate=0.5) TheBSKSim.set_DynModel(BSK_OpNavDynamics) @@ -384,7 +474,7 @@ def run(showPlots, simTime = None): TheScenario.run_vizard("-noDisplay") # Configure FSW mode - TheScenario.masterSim.modeRequest = 'pointHead' + TheScenario.masterSim.modeRequest = "pointHead" # Initialize simulation TheBSKSim.InitializeSimulation() # Configure run time and execute simulation @@ -393,14 +483,15 @@ def run(showPlots, simTime = None): else: simulationTime = macros.min2nano(200) TheBSKSim.ConfigureStopTime(simulationTime) - print('Starting Execution') + print("Starting Execution") t1 = time.time() TheBSKSim.ExecuteSimulation() t2 = time.time() - print('Finished Execution in ', t2-t1, ' seconds. Post-processing results') + print("Finished Execution in ", t2 - t1, " seconds. Post-processing results") # Terminate vizard and show plots figureList = TheScenario.end_scenario() return figureList + if __name__ == "__main__": run(True) diff --git a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavOD.py b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavOD.py index 2e2f1bc13a..9a14a2db71 100644 --- a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavOD.py +++ b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavOD.py @@ -32,6 +32,7 @@ import time from Basilisk.utilities import RigidBodyKinematics as rbk + # Import utilities from Basilisk.utilities import orbitalMotion, macros, unitTestSupport @@ -39,24 +40,25 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_OpNav import BSKSim, BSKScenario import BSK_OpNavDynamics, BSK_OpNavFsw import numpy as np # Import plotting file for your scenario -sys.path.append(path + '/../plottingOpNav') +sys.path.append(path + "/../plottingOpNav") import OpNav_Plotting as BSK_plt + # Create your own scenario child class class scenario_OpNav(BSKScenario): """Main Simulation Class""" def __init__(self, masterSim, showPlots=False): super(scenario_OpNav, self).__init__(masterSim, showPlots) - self.name = 'scenario_opnav' + self.name = "scenario_opnav" self.masterSim = masterSim - self.filterUse = "bias" #"relOD" + self.filterUse = "bias" # "relOD" # declare additional class variables self.opNavRec = None @@ -67,31 +69,43 @@ def __init__(self, masterSim, showPlots=False): def configure_initial_conditions(self): # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 10000 * 1E3 # meters + oe.a = 10000 * 1e3 # meters oe.e = 0.7 oe.i = 40 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 1. * macros.D2R # 90 good - mu = self.masterSim.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 1.0 * macros.D2R # 90 good + mu = self.masterSim.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - MRP= [0,0,0] - if self.filterUse =="relOD": - self.masterSim.get_FswModel().relativeOD.stateInit = rN.tolist() + vN.tolist() + MRP = [0, 0, 0] + if self.filterUse == "relOD": + self.masterSim.get_FswModel().relativeOD.stateInit = ( + rN.tolist() + vN.tolist() + ) if self.filterUse == "bias": - self.masterSim.get_FswModel().pixelLineFilter.stateInit = rN.tolist() + vN.tolist() + bias + self.masterSim.get_FswModel().pixelLineFilter.stateInit = ( + rN.tolist() + vN.tolist() + bias + ) self.masterSim.get_DynModel().scObject.hub.r_CN_NInit = rN self.masterSim.get_DynModel().scObject.hub.v_CN_NInit = vN - self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B qNoiseIn = np.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1e-3 * 1e-3 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1e-4 * 1e-4 self.masterSim.get_FswModel().relativeOD.qNoise = qNoiseIn.reshape(36).tolist() self.masterSim.get_FswModel().imageProcessing.noiseSF = 0.5 @@ -109,7 +123,9 @@ def log_outputs(self): self.opNavRec = FswModel.opnavMsg.recorder(samplingTime) self.masterSim.AddModelToTask(DynModel.taskName, self.opNavRec) if self.filterUse == "bias": - self.filtRec = FswModel.pixelLineFilter.filtDataOutMsg.recorder(samplingTime) + self.filtRec = FswModel.pixelLineFilter.filtDataOutMsg.recorder( + samplingTime + ) self.masterSim.AddModelToTask(DynModel.taskName, self.filtRec) self.scRec = DynModel.scObject.scStateOutMsg.recorder(samplingTime) @@ -122,37 +138,69 @@ def log_outputs(self): def pull_outputs(self, showPlots): # Dynamics process outputs: pull log messages below if any ## Spacecraft true states - position_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.r_BN_N) - velocity_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.v_BN_N) + position_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.r_BN_N + ) + velocity_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.v_BN_N + ) ## Attitude - sigma_BN = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.sigma_BN) + sigma_BN = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.sigma_BN + ) ## Image processing - circleCenters = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.circlesCenters) - circleRadii = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.circlesRadii) - validCircle = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.valid) + circleCenters = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.circlesCenters + ) + circleRadii = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.circlesRadii + ) + validCircle = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.valid + ) if self.filterUse == "bias": NUM_STATES = 9 ## Navigation results - navState = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.state) - navCovar = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.covar) - navPostFits = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.postFitRes) + navState = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.state + ) + navCovar = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.covar + ) + navPostFits = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.postFitRes + ) if self.filterUse == "relOD": NUM_STATES = 6 ## Navigation results - navState = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.state) - navCovar = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.covar) - navPostFits = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.postFitRes) - measPos = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_N) - r_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_C) - measCovar = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_N) - covar_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_C) + navState = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.state + ) + navCovar = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.covar + ) + navPostFits = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.postFitRes + ) + measPos = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.r_BN_N + ) + r_C = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.r_BN_C + ) + measCovar = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_N + ) + covar_C = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_C + ) sigma_CB = self.masterSim.get_DynModel().cameraMRP_CB sizeMM = self.masterSim.get_DynModel().cameraSize sizeOfCam = self.masterSim.get_DynModel().cameraRez - focal = self.masterSim.get_DynModel().cameraFocal #in m + focal = self.masterSim.get_DynModel().cameraFocal # in m pixelSize = [] pixelSize.append(sizeMM[0] / sizeOfCam[0]) @@ -161,32 +209,34 @@ def pull_outputs(self, showPlots): dcm_CB = rbk.MRP2C(sigma_CB) # Plot results BSK_plt.clear_all_plots() - stateError = np.zeros([len(position_N[:,0]), NUM_STATES+1]) - navCovarLong = np.full([len(position_N[:,0]), 1+NUM_STATES*NUM_STATES], np.nan) - navCovarLong[:,0] = np.copy(position_N[:,0]) + stateError = np.zeros([len(position_N[:, 0]), NUM_STATES + 1]) + navCovarLong = np.full( + [len(position_N[:, 0]), 1 + NUM_STATES * NUM_STATES], np.nan + ) + navCovarLong[:, 0] = np.copy(position_N[:, 0]) stateError[:, 0:4] = np.copy(position_N) - stateError[:,4:7] = np.copy(velocity_N[:,1:]) - pixCovar = np.ones([len(circleCenters[:,0]), 3*3+1]) - pixCovar[:,0] = circleCenters[:,0] - pixCovar[:,1:]*=np.array([1,0,0,0,1,0,0,0,2]) + stateError[:, 4:7] = np.copy(velocity_N[:, 1:]) + pixCovar = np.ones([len(circleCenters[:, 0]), 3 * 3 + 1]) + pixCovar[:, 0] = circleCenters[:, 0] + pixCovar[:, 1:] *= np.array([1, 0, 0, 0, 1, 0, 0, 0, 2]) if self.filterUse == "relOD": - measError = np.full([len(measPos[:,0]), 4], np.nan) - measError[:,0] = measPos[:,0] - measError_C = np.full([len(measPos[:,0]), 5], np.nan) - measError_C[:,0] = measPos[:,0] - trueRhat_C = np.full([len(circleCenters[:,0]), 4], np.nan) - trueR_C = np.full([len(circleCenters[:,0]), 4], np.nan) - trueCircles = np.full([len(circleCenters[:,0]), 4], np.nan) - trueCircles[:,0] = circleCenters[:,0] - trueRhat_C[:,0] = circleCenters[:,0] - trueR_C[:,0] = circleCenters[:,0] + measError = np.full([len(measPos[:, 0]), 4], np.nan) + measError[:, 0] = measPos[:, 0] + measError_C = np.full([len(measPos[:, 0]), 5], np.nan) + measError_C[:, 0] = measPos[:, 0] + trueRhat_C = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueR_C = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueCircles = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueCircles[:, 0] = circleCenters[:, 0] + trueRhat_C[:, 0] = circleCenters[:, 0] + trueR_C[:, 0] = circleCenters[:, 0] centerBias = np.copy(circleCenters) radBias = np.copy(circleRadii) switchIdx = 0 - Rmars = 3396.19*1E3 + Rmars = 3396.19 * 1e3 for j in range(len(stateError[:, 0])): if stateError[j, 0] in navState[:, 0]: stateError[j, 1:4] -= navState[j - switchIdx, 1:4] @@ -194,27 +244,44 @@ def pull_outputs(self, showPlots): else: stateError[j, 1:] = np.full(NUM_STATES, np.nan) switchIdx += 1 - for i in range(len(circleCenters[:,0])): - if circleCenters[i,1:].any() > 1E-8 or circleCenters[i,1:].any() < -1E-8: + for i in range(len(circleCenters[:, 0])): + if circleCenters[i, 1:].any() > 1e-8 or circleCenters[i, 1:].any() < -1e-8: if self.filterUse == "bias": - centerBias[i,1:3] = np.round(navState[i, 7:9]) - radBias[i,1] = np.round(navState[i, -1]) + centerBias[i, 1:3] = np.round(navState[i, 7:9]) + radBias[i, 1] = np.round(navState[i, -1]) if self.filterUse == "relOD": - measError[i, 1:4] = position_N[i +switchIdx, 1:4] - measPos[i, 1:4] - measError_C[i, 4] = np.linalg.norm(position_N[i +switchIdx, 1:4]) - np.linalg.norm(r_C[i, 1:4]) - measError_C[i, 1:4] = trueRhat_C[i,1:] - r_C[i, 1:4]/np.linalg.norm(r_C[i, 1:4]) - trueR_C[i, 1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), - position_N[i + switchIdx, 1:4]) - trueRhat_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i +switchIdx, 1:4])) ,position_N[i +switchIdx, 1:4])/np.linalg.norm(position_N[i +switchIdx, 1:4]) - trueCircles[i,3] = focal*np.tan(np.arcsin(Rmars/np.linalg.norm(position_N[i,1:4])))/pixelSize[0] - trueRhat_C[i,1:] *= focal/trueRhat_C[i,3] - trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0]/2 - 0.5 - trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1]/2 - 0.5 + measError[i, 1:4] = position_N[i + switchIdx, 1:4] - measPos[i, 1:4] + measError_C[i, 4] = np.linalg.norm( + position_N[i + switchIdx, 1:4] + ) - np.linalg.norm(r_C[i, 1:4]) + measError_C[i, 1:4] = trueRhat_C[i, 1:] - r_C[ + i, 1:4 + ] / np.linalg.norm(r_C[i, 1:4]) + trueR_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) + trueRhat_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) / np.linalg.norm(position_N[i + switchIdx, 1:4]) + trueCircles[i, 3] = ( + focal + * np.tan(np.arcsin(Rmars / np.linalg.norm(position_N[i, 1:4]))) + / pixelSize[0] + ) + trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] + trueCircles[i, 1] = ( + trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 + ) + trueCircles[i, 2] = ( + trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 + ) else: if self.filterUse == "relOD": - measCovar[i,1:] = np.full(3*3, np.nan) + measCovar[i, 1:] = np.full(3 * 3, np.nan) covar_C[i, 1:] = np.full(3 * 3, np.nan) - navCovarLong[switchIdx:,:] = np.copy(navCovar) + navCovarLong[switchIdx:, :] = np.copy(navCovar) timeData = position_N[:, 0] * macros.NANO2MIN @@ -226,10 +293,12 @@ def pull_outputs(self, showPlots): BSK_plt.plot_cirlces(circleCenters, circleRadii, validCircle, sizeOfCam) BSK_plt.plotStateCovarPlot(stateError, navCovarLong) if self.filterUse == "bias": - circleCenters[i,1:] += centerBias[i,1:] - circleRadii[i,1:] += radBias[i,1:] + circleCenters[i, 1:] += centerBias[i, 1:] + circleRadii[i, 1:] += radBias[i, 1:] BSK_plt.plotPostFitResiduals(navPostFits, pixCovar) - BSK_plt.imgProcVsExp(trueCircles, circleCenters, circleRadii, np.array(sizeOfCam)) + BSK_plt.imgProcVsExp( + trueCircles, circleCenters, circleRadii, np.array(sizeOfCam) + ) if self.filterUse == "relOD": BSK_plt.plotPostFitResiduals(navPostFits, measCovar) figureList = {} @@ -244,7 +313,6 @@ def pull_outputs(self, showPlots): def run(showPlots, simTime=None): - # Instantiate base simulation TheBSKSim = BSKSim(fswRate=0.5, dynRate=0.5) TheBSKSim.set_DynModel(BSK_OpNavDynamics) @@ -266,19 +334,19 @@ def run(showPlots, simTime=None): TheScenario.run_vizard("-noDisplay") # Configure FSW mode - TheScenario.masterSim.modeRequest = 'prepOpNav' + TheScenario.masterSim.modeRequest = "prepOpNav" # Initialize simulation TheBSKSim.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) TheBSKSim.ConfigureStopTime(simulationTime) - print('Starting Execution') + print("Starting Execution") t1 = time.time() TheBSKSim.ExecuteSimulation() if TheScenario.filterUse == "relOD": - TheScenario.masterSim.modeRequest = 'OpNavOD' + TheScenario.masterSim.modeRequest = "OpNavOD" else: - TheScenario.masterSim.modeRequest = 'OpNavODB' + TheScenario.masterSim.modeRequest = "OpNavODB" if simTime != None: simulationTime = macros.min2nano(simTime) else: @@ -286,7 +354,7 @@ def run(showPlots, simTime=None): TheBSKSim.ConfigureStopTime(simulationTime) TheBSKSim.ExecuteSimulation() t2 = time.time() - print('Finished Execution in ', t2-t1, ' seconds. Post-processing results') + print("Finished Execution in ", t2 - t1, " seconds. Post-processing results") # Terminate vizard and show plots figureList = TheScenario.end_scenario() return figureList diff --git a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavODLimb.py b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavODLimb.py index 66a1c314f2..43ecda706b 100644 --- a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavODLimb.py +++ b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavODLimb.py @@ -32,6 +32,7 @@ import time from Basilisk.utilities import RigidBodyKinematics as rbk + # Import utilities from Basilisk.utilities import orbitalMotion, macros, unitTestSupport @@ -39,22 +40,23 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_OpNav import BSKSim, BSKScenario import BSK_OpNavDynamics, BSK_OpNavFsw import numpy as np # Import plotting file for your scenario -sys.path.append(path + '/../plottingOpNav') +sys.path.append(path + "/../plottingOpNav") import OpNav_Plotting as BSK_plt + # Create your own scenario child class class scenario_OpNav(BSKScenario): """Main Simulation Class""" def __init__(self, masterSim, showPlots=False): super(scenario_OpNav, self).__init__(masterSim, showPlots) - self.name = 'scenario_opnav' + self.name = "scenario_opnav" self.masterSim = masterSim # declare additional class variables @@ -66,32 +68,39 @@ def __init__(self, masterSim, showPlots=False): def configure_initial_conditions(self): # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 18000 * 1E3 # meters + oe.a = 18000 * 1e3 # meters oe.e = 0.2 oe.i = 10 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 0. * macros.D2R # 90 good - mu = self.masterSim.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 0.0 * macros.D2R # 90 good + mu = self.masterSim.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - MRP= [0,-0.3,0] + MRP = [0, -0.3, 0] self.masterSim.get_FswModel().relativeOD.stateInit = rN.tolist() + vN.tolist() self.masterSim.get_DynModel().scObject.hub.r_CN_NInit = rN self.masterSim.get_DynModel().scObject.hub.v_CN_NInit = vN - self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B qNoiseIn = np.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1e-3 * 1e-3 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1e-4 * 1e-4 self.masterSim.get_FswModel().relativeOD.qNoise = qNoiseIn.reshape(36).tolist() self.masterSim.get_FswModel().horizonNav.noiseSF = 20 - def log_outputs(self): # Dynamics process outputs: log messages below if desired. FswModel = self.masterSim.get_FswModel() @@ -117,33 +126,57 @@ def pull_outputs(self, showPlots): # Dynamics process outputs: pull log messages below if any ## Spacecraft true states - position_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.r_BN_N) - velocity_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.v_BN_N) + position_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.r_BN_N + ) + velocity_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.v_BN_N + ) ## Attitude - sigma_BN = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.sigma_BN) + sigma_BN = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.sigma_BN + ) ## Image processing - limb = unitTestSupport.addTimeColumn(self.limbRec.times(), self.limbRec.limbPoints) - numLimbPoints = unitTestSupport.addTimeColumn(self.limbRec.times(), self.limbRec.numLimbPoints) - validLimb = unitTestSupport.addTimeColumn(self.limbRec.times(), self.limbRec.valid) + limb = unitTestSupport.addTimeColumn( + self.limbRec.times(), self.limbRec.limbPoints + ) + numLimbPoints = unitTestSupport.addTimeColumn( + self.limbRec.times(), self.limbRec.numLimbPoints + ) + validLimb = unitTestSupport.addTimeColumn( + self.limbRec.times(), self.limbRec.valid + ) ## OpNav Out - measPos = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_N) + measPos = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.r_BN_N + ) r_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_C) - measCovar = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_N) - covar_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_C) + measCovar = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_N + ) + covar_C = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_C + ) NUM_STATES = 6 ## Navigation results - navState = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.state) - navCovar = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.covar) - navPostFits = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.postFitRes) + navState = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.state + ) + navCovar = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.covar + ) + navPostFits = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.postFitRes + ) sigma_CB = self.masterSim.get_DynModel().cameraMRP_CB sizeMM = self.masterSim.get_DynModel().cameraSize sizeOfCam = self.masterSim.get_DynModel().cameraRez - focal = self.masterSim.get_DynModel().cameraFocal #in m + focal = self.masterSim.get_DynModel().cameraFocal # in m pixelSize = [] pixelSize.append(sizeMM[0] / sizeOfCam[0]) @@ -152,26 +185,27 @@ def pull_outputs(self, showPlots): dcm_CB = rbk.MRP2C(sigma_CB) # Plot results BSK_plt.clear_all_plots() - stateError = np.zeros([len(position_N[:,0]), NUM_STATES+1]) - navCovarLong = np.full([len(position_N[:,0]), 1+NUM_STATES*NUM_STATES], np.nan) - navCovarLong[:,0] = np.copy(position_N[:,0]) + stateError = np.zeros([len(position_N[:, 0]), NUM_STATES + 1]) + navCovarLong = np.full( + [len(position_N[:, 0]), 1 + NUM_STATES * NUM_STATES], np.nan + ) + navCovarLong[:, 0] = np.copy(position_N[:, 0]) stateError[:, 0:4] = np.copy(position_N) - stateError[:,4:7] = np.copy(velocity_N[:,1:]) - measError = np.full([len(measPos[:,0]), 4], np.nan) - measError[:,0] = measPos[:,0] - measError_C = np.full([len(measPos[:,0]), 5], np.nan) - measError_C[:,0] = measPos[:,0] - trueRhat_C = np.full([len(numLimbPoints[:,0]), 4], np.nan) - trueR_C = np.full([len(numLimbPoints[:,0]), 4], np.nan) - trueCircles = np.full([len(numLimbPoints[:,0]), 4], np.nan) - trueCircles[:,0] = numLimbPoints[:,0] - trueRhat_C[:,0] = numLimbPoints[:,0] - trueR_C[:,0] = numLimbPoints[:,0] - + stateError[:, 4:7] = np.copy(velocity_N[:, 1:]) + measError = np.full([len(measPos[:, 0]), 4], np.nan) + measError[:, 0] = measPos[:, 0] + measError_C = np.full([len(measPos[:, 0]), 5], np.nan) + measError_C[:, 0] = measPos[:, 0] + trueRhat_C = np.full([len(numLimbPoints[:, 0]), 4], np.nan) + trueR_C = np.full([len(numLimbPoints[:, 0]), 4], np.nan) + trueCircles = np.full([len(numLimbPoints[:, 0]), 4], np.nan) + trueCircles[:, 0] = numLimbPoints[:, 0] + trueRhat_C[:, 0] = numLimbPoints[:, 0] + trueR_C[:, 0] = numLimbPoints[:, 0] switchIdx = 0 - Rmars = 3396.19*1E3 + Rmars = 3396.19 * 1e3 for j in range(len(stateError[:, 0])): if stateError[j, 0] in navState[:, 0]: stateError[j, 1:4] -= navState[j - switchIdx, 1:4] @@ -179,21 +213,39 @@ def pull_outputs(self, showPlots): else: stateError[j, 1:] = np.full(NUM_STATES, np.nan) switchIdx += 1 - for i in range(len(numLimbPoints[:,0])): - if numLimbPoints[i,1] > 1E-8: - measError[i, 1:4] = position_N[i +switchIdx, 1:4] - measPos[i, 1:4] - measError_C[i, 4] = np.linalg.norm(position_N[i +switchIdx, 1:4]) - np.linalg.norm(r_C[i, 1:4]) - measError_C[i, 1:4] = trueRhat_C[i,1:] - r_C[i, 1:4]/np.linalg.norm(r_C[i, 1:4]) - trueR_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i +switchIdx, 1:4])) , position_N[i +switchIdx, 1:4]) - trueRhat_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i +switchIdx, 1:4])) ,position_N[i +switchIdx, 1:4])/np.linalg.norm(position_N[i +switchIdx, 1:4]) - trueCircles[i,3] = focal*np.tan(np.arcsin(Rmars/np.linalg.norm(position_N[i,1:4])))/pixelSize[0] - trueRhat_C[i,1:] *= focal/trueRhat_C[i,3] - trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0]/2 - 0.5 - trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1]/2 - 0.5 + for i in range(len(numLimbPoints[:, 0])): + if numLimbPoints[i, 1] > 1e-8: + measError[i, 1:4] = position_N[i + switchIdx, 1:4] - measPos[i, 1:4] + measError_C[i, 4] = np.linalg.norm( + position_N[i + switchIdx, 1:4] + ) - np.linalg.norm(r_C[i, 1:4]) + measError_C[i, 1:4] = trueRhat_C[i, 1:] - r_C[i, 1:4] / np.linalg.norm( + r_C[i, 1:4] + ) + trueR_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) + trueRhat_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) / np.linalg.norm(position_N[i + switchIdx, 1:4]) + trueCircles[i, 3] = ( + focal + * np.tan(np.arcsin(Rmars / np.linalg.norm(position_N[i, 1:4]))) + / pixelSize[0] + ) + trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] + trueCircles[i, 1] = ( + trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 + ) + trueCircles[i, 2] = ( + trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 + ) else: - measCovar[i,1:] = np.full(3*3, np.nan) + measCovar[i, 1:] = np.full(3 * 3, np.nan) covar_C[i, 1:] = np.full(3 * 3, np.nan) - navCovarLong[switchIdx:,:] = np.copy(navCovar) + navCovarLong[switchIdx:, :] = np.copy(navCovar) timeData = position_N[:, 0] * macros.NANO2MIN @@ -217,8 +269,7 @@ def pull_outputs(self, showPlots): return figureList -def run(showPlots, simTime = None): - +def run(showPlots, simTime=None): # Instantiate base simulation TheBSKSim = BSKSim(fswRate=0.5, dynRate=0.5) TheBSKSim.set_DynModel(BSK_OpNavDynamics) @@ -238,16 +289,16 @@ def run(showPlots, simTime = None): TheScenario.run_vizard("-noDisplay") # Configure FSW mode - TheScenario.masterSim.modeRequest = 'prepOpNav' + TheScenario.masterSim.modeRequest = "prepOpNav" # Initialize simulation TheBSKSim.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.min2nano(3.) + simulationTime = macros.min2nano(3.0) TheBSKSim.ConfigureStopTime(simulationTime) - print('Starting Execution') + print("Starting Execution") t1 = time.time() TheBSKSim.ExecuteSimulation() - TheScenario.masterSim.modeRequest = 'OpNavODLimb' + TheScenario.masterSim.modeRequest = "OpNavODLimb" if simTime != None: simulationTime = macros.min2nano(simTime) else: @@ -255,7 +306,7 @@ def run(showPlots, simTime = None): TheBSKSim.ConfigureStopTime(simulationTime) TheBSKSim.ExecuteSimulation() t2 = time.time() - print('Finished Execution in ', t2-t1, ' seconds. Post-processing results') + print("Finished Execution in ", t2 - t1, " seconds. Post-processing results") # Terminate vizard and show plots figureList = TheScenario.end_scenario() return figureList diff --git a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavPoint.py b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavPoint.py index c3207ea5bb..e08ad176a7 100644 --- a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavPoint.py +++ b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavPoint.py @@ -32,6 +32,7 @@ import time from Basilisk.utilities import RigidBodyKinematics as rbk + # Import utilities from Basilisk.utilities import orbitalMotion, macros, unitTestSupport @@ -39,24 +40,25 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_OpNav import BSKSim, BSKScenario import BSK_OpNavDynamics, BSK_OpNavFsw import numpy as np # Import plotting file for your scenario -sys.path.append(path + '/../plottingOpNav') +sys.path.append(path + "/../plottingOpNav") import OpNav_Plotting as BSK_plt + # Create your own scenario child class class scenario_OpNav(BSKScenario): """Main Simulation Class""" def __init__(self, masterSim, showPlots=False): super(scenario_OpNav, self).__init__(masterSim, showPlots) - self.name = 'scenario_opnav' + self.name = "scenario_opnav" self.masterSim = masterSim - self.filterUse = "bias" #"relOD" + self.filterUse = "bias" # "relOD" # declare additional class variables self.rwMotorRec = None @@ -69,27 +71,39 @@ def __init__(self, masterSim, showPlots=False): def configure_initial_conditions(self): # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 18000*1E3 # meters - oe.e = 0. + oe.a = 18000 * 1e3 # meters + oe.e = 0.0 oe.i = 20 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 100. * macros.D2R # 90 good - mu = self.masterSim.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 100.0 * macros.D2R # 90 good + mu = self.masterSim.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - MRP= [0,0,0] + MRP = [0, 0, 0] if self.filterUse == "relOD": - self.masterSim.get_FswModel().relativeOD.stateInit = rN.tolist() + vN.tolist() + self.masterSim.get_FswModel().relativeOD.stateInit = ( + rN.tolist() + vN.tolist() + ) if self.filterUse == "bias": - self.masterSim.get_FswModel().pixelLineFilter.stateInit = rN.tolist() + vN.tolist() + bias + self.masterSim.get_FswModel().pixelLineFilter.stateInit = ( + rN.tolist() + vN.tolist() + bias + ) self.masterSim.get_DynModel().scObject.hub.r_CN_NInit = rN self.masterSim.get_DynModel().scObject.hub.v_CN_NInit = vN - self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B # Search self.masterSim.get_FswModel().opNavPoint.omega_RN_B = [0.001, 0.0, -0.001] @@ -102,7 +116,9 @@ def log_outputs(self): self.opNavRec = FswModel.opnavMsg.recorder(samplingTime) self.attGuidRec = FswModel.attGuidMsg.recorder(samplingTime) - self.rwMotorRec = FswModel.rwMotorTorque.rwMotorTorqueOutMsg.recorder(samplingTime) + self.rwMotorRec = FswModel.rwMotorTorque.rwMotorTorqueOutMsg.recorder( + samplingTime + ) self.circlesRec = FswModel.opnavCirclesMsg.recorder(samplingTime) self.scRec = DynModel.scObject.scStateOutMsg.recorder(samplingTime) self.masterSim.AddModelToTask(DynModel.taskName, self.opNavRec) @@ -113,33 +129,54 @@ def log_outputs(self): self.rwLogs = [] for item in range(4): - self.rwLogs.append(DynModel.rwStateEffector.rwOutMsgs[item].recorder(samplingTime)) + self.rwLogs.append( + DynModel.rwStateEffector.rwOutMsgs[item].recorder(samplingTime) + ) self.masterSim.AddModelToTask(DynModel.taskName, self.rwLogs[item]) return def pull_outputs(self, showPlots): - ## Spacecraft true states - position_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.r_BN_N) + position_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.r_BN_N + ) ## Attitude - sigma_BN = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.sigma_BN) + sigma_BN = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.sigma_BN + ) ## Image processing - circleCenters = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.circlesCenters) - circleRadii = unitTestSupport.addTimeColumn(self.circlesRec.times(), self.circlesRec.circlesRadii) + circleCenters = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.circlesCenters + ) + circleRadii = unitTestSupport.addTimeColumn( + self.circlesRec.times(), self.circlesRec.circlesRadii + ) numRW = 4 - dataUsReq = unitTestSupport.addTimeColumn(self.rwMotorRec.times(), self.rwMotorRec.motorTorque) + dataUsReq = unitTestSupport.addTimeColumn( + self.rwMotorRec.times(), self.rwMotorRec.motorTorque + ) dataRW = [] for i in range(numRW): - dataRW.append(unitTestSupport.addTimeColumn(self.rwMotorRec.times(), self.rwLogs[i].u_current)) - - measPos = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_N) + dataRW.append( + unitTestSupport.addTimeColumn( + self.rwMotorRec.times(), self.rwLogs[i].u_current + ) + ) + + measPos = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.r_BN_N + ) r_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_C) - measCovar = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_N) - covar_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_C) + measCovar = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_N + ) + covar_C = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_C + ) sigma_CB = self.masterSim.get_DynModel().cameraMRP_CB sizeMM = self.masterSim.get_DynModel().cameraSize @@ -153,49 +190,68 @@ def pull_outputs(self, showPlots): dcm_CB = rbk.MRP2C(sigma_CB) # Plot results BSK_plt.clear_all_plots() - pixCovar = np.ones([len(circleCenters[:,0]), 3*3+1]) - pixCovar[:,0] = circleCenters[:,0] - pixCovar[:,1:]*=np.array([1,0,0,0,1,0,0,0,2]) + pixCovar = np.ones([len(circleCenters[:, 0]), 3 * 3 + 1]) + pixCovar[:, 0] = circleCenters[:, 0] + pixCovar[:, 1:] *= np.array([1, 0, 0, 0, 1, 0, 0, 0, 2]) - measError = np.full([len(measPos[:,0]), 4], np.nan) - measError[:,0] = measPos[:,0] - measError_C = np.full([len(measPos[:,0]), 5], np.nan) - measError_C[:,0] = measPos[:,0] + measError = np.full([len(measPos[:, 0]), 4], np.nan) + measError[:, 0] = measPos[:, 0] + measError_C = np.full([len(measPos[:, 0]), 5], np.nan) + measError_C[:, 0] = measPos[:, 0] - trueRhat_C = np.full([len(circleCenters[:,0]), 4], np.nan) - trueCircles = np.full([len(circleCenters[:,0]), 4], np.nan) - trueCircles[:,0] = circleCenters[:,0] - trueRhat_C[:,0] = circleCenters[:,0] + trueRhat_C = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueCircles = np.full([len(circleCenters[:, 0]), 4], np.nan) + trueCircles[:, 0] = circleCenters[:, 0] + trueRhat_C[:, 0] = circleCenters[:, 0] centerBias = np.copy(circleCenters) radBias = np.copy(circleRadii) ModeIdx = 0 - Rmars = 3396.19*1E3 + Rmars = 3396.19 * 1e3 for j in range(len(position_N[:, 0])): if position_N[j, 0] in circleCenters[:, 0]: ModeIdx = j break - for i in range(len(circleCenters[:,0])): - if circleCenters[i,1:].any() > 1E-8 or circleCenters[i,1:].any() < -1E-8: - trueRhat_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[ModeIdx+i , 1:4])) ,position_N[ModeIdx+i, 1:4])/np.linalg.norm(position_N[ModeIdx+i, 1:4]) - trueCircles[i,3] = focal*np.tan(np.arcsin(Rmars/np.linalg.norm(position_N[ModeIdx+i,1:4])))/pixelSize[0] - trueRhat_C[i,1:] *= focal/trueRhat_C[i,3] - trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0]/2 - 0.5 - trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1]/2 - 0.5 - - measError[i, 1:4] = position_N[ModeIdx+i, 1:4] - measPos[i, 1:4] - measError_C[i, 4] = np.linalg.norm(position_N[ModeIdx+i, 1:4]) - np.linalg.norm(r_C[i, 1:4]) - measError_C[i, 1:4] = trueRhat_C[i,1:] - r_C[i, 1:4]/np.linalg.norm(r_C[i, 1:4]) + for i in range(len(circleCenters[:, 0])): + if circleCenters[i, 1:].any() > 1e-8 or circleCenters[i, 1:].any() < -1e-8: + trueRhat_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[ModeIdx + i, 1:4])), + position_N[ModeIdx + i, 1:4], + ) / np.linalg.norm(position_N[ModeIdx + i, 1:4]) + trueCircles[i, 3] = ( + focal + * np.tan( + np.arcsin(Rmars / np.linalg.norm(position_N[ModeIdx + i, 1:4])) + ) + / pixelSize[0] + ) + trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] + trueCircles[i, 1] = ( + trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 + ) + trueCircles[i, 2] = ( + trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 + ) + + measError[i, 1:4] = position_N[ModeIdx + i, 1:4] - measPos[i, 1:4] + measError_C[i, 4] = np.linalg.norm( + position_N[ModeIdx + i, 1:4] + ) - np.linalg.norm(r_C[i, 1:4]) + measError_C[i, 1:4] = trueRhat_C[i, 1:] - r_C[i, 1:4] / np.linalg.norm( + r_C[i, 1:4] + ) else: - measCovar[i,1:] = np.full(3*3, np.nan) + measCovar[i, 1:] = np.full(3 * 3, np.nan) covar_C[i, 1:] = np.full(3 * 3, np.nan) timeData = position_N[:, 0] * macros.NANO2MIN BSK_plt.plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW) - BSK_plt.imgProcVsExp(trueCircles, circleCenters, circleRadii, np.array(sizeOfCam)) + BSK_plt.imgProcVsExp( + trueCircles, circleCenters, circleRadii, np.array(sizeOfCam) + ) figureList = {} if showPlots: @@ -209,7 +265,6 @@ def pull_outputs(self, showPlots): def run(showPlots, simTime=None): - # Instantiate base simulation TheBSKSim = BSKSim(fswRate=0.5, dynRate=0.5) TheBSKSim.set_DynModel(BSK_OpNavDynamics) @@ -229,7 +284,7 @@ def run(showPlots, simTime=None): # Modes: "None", "-directComm", "-noDisplay" TheScenario.run_vizard("-noDisplay") # Configure FSW mode - TheScenario.masterSim.modeRequest = 'pointOpNav' + TheScenario.masterSim.modeRequest = "pointOpNav" # Initialize simulation TheBSKSim.InitializeSimulation() # Configure run time and execute simulation @@ -238,11 +293,11 @@ def run(showPlots, simTime=None): else: simulationTime = macros.min2nano(200) TheBSKSim.ConfigureStopTime(simulationTime) - print('Starting Execution') + print("Starting Execution") t1 = time.time() TheBSKSim.ExecuteSimulation() t2 = time.time() - print('Finished Execution in ', t2-t1, ' seconds. Post-processing results') + print("Finished Execution in ", t2 - t1, " seconds. Post-processing results") # Terminate vizard and show plots figureList = TheScenario.end_scenario() return figureList diff --git a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavPointLimb.py b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavPointLimb.py index 0a65c418d6..b484fd0261 100644 --- a/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavPointLimb.py +++ b/examples/OpNavScenarios/scenariosOpNav/scenario_OpNavPointLimb.py @@ -39,23 +39,24 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_OpNav import BSKSim, BSKScenario import BSK_OpNavDynamics, BSK_OpNavFsw # Import plotting file for your scenario -sys.path.append(path + '/../plottingOpNav') +sys.path.append(path + "/../plottingOpNav") import OpNav_Plotting as BSK_plt + # Create your own scenario child class class scenario_OpNav(BSKScenario): """Main Simulation Class""" def __init__(self, masterSim, showPlots=False): super(scenario_OpNav, self).__init__(masterSim, showPlots) - self.name = 'scenario_opnav' + self.name = "scenario_opnav" self.masterSim = masterSim - self.filterUse = "bias" #"relOD" + self.filterUse = "bias" # "relOD" # declare additional class variables self.rwMotorRec = None @@ -65,27 +66,39 @@ def __init__(self, masterSim, showPlots=False): def configure_initial_conditions(self): # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 18000*1E3 # meters - oe.e = 0. + oe.a = 18000 * 1e3 # meters + oe.e = 0.0 oe.i = 20 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 100. * macros.D2R # 90 good - mu = self.masterSim.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 100.0 * macros.D2R # 90 good + mu = self.masterSim.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - MRP= [0,0,0] - if self.filterUse =="relOD": - self.masterSim.get_FswModel().relativeOD.stateInit = rN.tolist() + vN.tolist() + MRP = [0, 0, 0] + if self.filterUse == "relOD": + self.masterSim.get_FswModel().relativeOD.stateInit = ( + rN.tolist() + vN.tolist() + ) if self.filterUse == "bias": - self.masterSim.get_FswModel().pixelLineFilter.stateInit = rN.tolist() + vN.tolist() + bias + self.masterSim.get_FswModel().pixelLineFilter.stateInit = ( + rN.tolist() + vN.tolist() + bias + ) self.masterSim.get_DynModel().scObject.hub.r_CN_NInit = rN self.masterSim.get_DynModel().scObject.hub.v_CN_NInit = vN - self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B # Search self.masterSim.get_FswModel().opNavPoint.omega_RN_B = [0.001, 0.0, -0.001] @@ -98,27 +111,40 @@ def log_outputs(self): samplingTime = self.masterSim.get_FswModel().processTasksTimeStep self.attGuidRec = FswModel.attGuidMsg.recorder(samplingTime) - self.rwMotorRec = FswModel.rwMotorTorque.rwMotorTorqueOutMsg.recorder(samplingTime) + self.rwMotorRec = FswModel.rwMotorTorque.rwMotorTorqueOutMsg.recorder( + samplingTime + ) self.masterSim.AddModelToTask(DynModel.taskName, self.attGuidRec) self.masterSim.AddModelToTask(DynModel.taskName, self.rwMotorRec) self.rwLogs = [] for item in range(4): - self.rwLogs.append(DynModel.rwStateEffector.rwOutMsgs[item].recorder(samplingTime)) + self.rwLogs.append( + DynModel.rwStateEffector.rwOutMsgs[item].recorder(samplingTime) + ) self.masterSim.AddModelToTask(DynModel.taskName, self.rwLogs[item]) return def pull_outputs(self, showPlots): - - sigma_BR = unitTestSupport.addTimeColumn(self.attGuidRec.times(), self.attGuidRec.sigma_BR) - omega_BR_B = unitTestSupport.addTimeColumn(self.attGuidRec.times(), self.attGuidRec.omega_BR_B) + sigma_BR = unitTestSupport.addTimeColumn( + self.attGuidRec.times(), self.attGuidRec.sigma_BR + ) + omega_BR_B = unitTestSupport.addTimeColumn( + self.attGuidRec.times(), self.attGuidRec.omega_BR_B + ) numRW = 4 - dataUsReq = unitTestSupport.addTimeColumn(self.rwMotorRec.times(), self.rwMotorRec.motorTorque) + dataUsReq = unitTestSupport.addTimeColumn( + self.rwMotorRec.times(), self.rwMotorRec.motorTorque + ) dataRW = [] for i in range(numRW): - dataRW.append(unitTestSupport.addTimeColumn(self.rwMotorRec.times(), self.rwLogs[i].u_current)) + dataRW.append( + unitTestSupport.addTimeColumn( + self.rwMotorRec.times(), self.rwLogs[i].u_current + ) + ) # Plot results BSK_plt.clear_all_plots() @@ -141,7 +167,6 @@ def pull_outputs(self, showPlots): def run(showPlots, simTime=None): - # Instantiate base simulation TheBSKSim = BSKSim(fswRate=0.5, dynRate=0.5) TheBSKSim.set_DynModel(BSK_OpNavDynamics) @@ -162,24 +187,24 @@ def run(showPlots, simTime=None): TheScenario.run_vizard("-noDisplay") # Configure FSW mode - TheScenario.masterSim.modeRequest = 'prepOpNav' + TheScenario.masterSim.modeRequest = "prepOpNav" # Initialize simulation TheBSKSim.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.min2nano(5.) + simulationTime = macros.min2nano(5.0) TheBSKSim.ConfigureStopTime(simulationTime) - print('Starting Execution') + print("Starting Execution") t1 = time.time() TheBSKSim.ExecuteSimulation() - TheScenario.masterSim.modeRequest = 'pointLimb' + TheScenario.masterSim.modeRequest = "pointLimb" if simTime != None: simulationTime = macros.min2nano(simTime) else: - simulationTime = macros.min2nano(200.) + simulationTime = macros.min2nano(200.0) TheBSKSim.ConfigureStopTime(simulationTime) TheBSKSim.ExecuteSimulation() t2 = time.time() - print('Finished Execution in ', t2-t1, ' seconds. Post-processing results') + print("Finished Execution in ", t2 - t1, " seconds. Post-processing results") # Terminate vizard and show plots figureList = TheScenario.end_scenario() return figureList diff --git a/examples/OpNavScenarios/scenariosOpNav/scenario_faultDetOpNav.py b/examples/OpNavScenarios/scenariosOpNav/scenario_faultDetOpNav.py index 107639b217..b780266298 100644 --- a/examples/OpNavScenarios/scenariosOpNav/scenario_faultDetOpNav.py +++ b/examples/OpNavScenarios/scenariosOpNav/scenario_faultDetOpNav.py @@ -30,7 +30,6 @@ """ - # Get current file path import inspect import os @@ -38,6 +37,7 @@ import time from Basilisk.utilities import RigidBodyKinematics as rbk + # Import utilities from Basilisk.utilities import orbitalMotion, macros, unitTestSupport @@ -45,14 +45,14 @@ path = os.path.dirname(os.path.abspath(filename)) # Import master classes: simulation base class and scenario base class -sys.path.append(path + '/..') +sys.path.append(path + "/..") from BSK_OpNav import BSKSim, BSKScenario import BSK_OpNavDynamics import BSK_OpNavFsw import numpy as np # Import plotting file for your scenario -sys.path.append(path + '/../plottingOpNav') +sys.path.append(path + "/../plottingOpNav") import OpNav_Plotting as BSK_plt from Basilisk.architecture import messaging @@ -64,7 +64,7 @@ class scenario_OpNav(BSKScenario): def __init__(self, masterSim, showPlots=False): super(scenario_OpNav, self).__init__(masterSim, showPlots) - self.name = 'scenario_opnav' + self.name = "scenario_opnav" self.masterSim = masterSim # declare additional class variables @@ -77,29 +77,41 @@ def __init__(self, masterSim, showPlots=False): def configure_initial_conditions(self): # Configure Dynamics initial conditions oe = orbitalMotion.ClassicElements() - oe.a = 18000 * 1E3 # meters + oe.a = 18000 * 1e3 # meters oe.e = 0.6 oe.i = 10 * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 190. * macros.D2R - oe.f = 80. * macros.D2R # 90 good - mu = self.masterSim.get_DynModel().gravFactory.gravBodies['mars barycenter'].mu + oe.Omega = 25.0 * macros.D2R + oe.omega = 190.0 * macros.D2R + oe.f = 80.0 * macros.D2R # 90 good + mu = self.masterSim.get_DynModel().gravFactory.gravBodies["mars barycenter"].mu rN, vN = orbitalMotion.elem2rv(mu, oe) orbitalMotion.rv2elem(mu, rN, vN) bias = [0, 0, -2] - MRP= [0,-0.3,0] + MRP = [0, -0.3, 0] self.masterSim.get_FswModel().relativeOD.stateInit = rN.tolist() + vN.tolist() self.masterSim.get_DynModel().scObject.hub.r_CN_NInit = rN # m - r_CN_N self.masterSim.get_DynModel().scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B - self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B + self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [ + [MRP[0]], + [MRP[1]], + [MRP[2]], + ] # sigma_BN_B + self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.0], + ] # rad/s - omega_BN_B # primary_opnav, secondary_opnav FswModel = self.masterSim.get_FswModel() - messaging.OpNavMsg_C_addAuthor(FswModel.horizonNav.opNavOutMsg, FswModel.opnavPrimaryMsg) - messaging.OpNavMsg_C_addAuthor(FswModel.pixelLine.opNavOutMsg, FswModel.opnavSecondaryMsg) + messaging.OpNavMsg_C_addAuthor( + FswModel.horizonNav.opNavOutMsg, FswModel.opnavPrimaryMsg + ) + messaging.OpNavMsg_C_addAuthor( + FswModel.pixelLine.opNavOutMsg, FswModel.opnavSecondaryMsg + ) # Filter noise param self.masterSim.get_FswModel().relativeOD.noiseSF = 5 @@ -108,7 +120,7 @@ def configure_initial_conditions(self): # self.masterSim.get_DynModel().cameraMod.gaussian = 2 #3 # # self.masterSim.get_DynModel().cameraMod.darkCurrent = 0 #1 # # self.masterSim.get_DynModel().cameraMod.saltPepper = 0.5 #1 # - self.masterSim.get_DynModel().cameraMod.cosmicRays = 2 # 2 # + self.masterSim.get_DynModel().cameraMod.cosmicRays = 2 # 2 # # self.masterSim.get_DynModel().cameraMod.blurParam = 3 #4 # # Fault params @@ -142,26 +154,50 @@ def pull_outputs(self, showPlots): NUM_STATES = 6 ## Spacecraft true states - position_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.r_BN_N) - velocity_N = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.v_BN_N) + position_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.r_BN_N + ) + velocity_N = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.v_BN_N + ) ## Attitude - sigma_BN = unitTestSupport.addTimeColumn(self.scRec.times(), self.scRec.sigma_BN) + sigma_BN = unitTestSupport.addTimeColumn( + self.scRec.times(), self.scRec.sigma_BN + ) ## Navigation results - navState = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.state) - navCovar = unitTestSupport.addTimeColumn(self.filtRec.times(), self.filtRec.covar) - - validLimb = unitTestSupport.addTimeColumn(self.opNavPrimRec.times(), self.opNavPrimRec.valid) - validHough = unitTestSupport.addTimeColumn(self.opNavSecRec.times(), self.opNavSecRec.valid) + navState = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.state + ) + navCovar = unitTestSupport.addTimeColumn( + self.filtRec.times(), self.filtRec.covar + ) + + validLimb = unitTestSupport.addTimeColumn( + self.opNavPrimRec.times(), self.opNavPrimRec.valid + ) + validHough = unitTestSupport.addTimeColumn( + self.opNavSecRec.times(), self.opNavSecRec.valid + ) ## Fault Detection - measPos = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_N) - valid = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.valid) - faults = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.faultDetected) + measPos = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.r_BN_N + ) + valid = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.valid + ) + faults = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.faultDetected + ) r_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.r_BN_C) - measCovar = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_N) - covar_C = unitTestSupport.addTimeColumn(self.opNavRec.times(), self.opNavRec.covar_C) + measCovar = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_N + ) + covar_C = unitTestSupport.addTimeColumn( + self.opNavRec.times(), self.opNavRec.covar_C + ) sigma_CB = self.masterSim.get_DynModel().cameraMRP_CB sizeMM = self.masterSim.get_DynModel().cameraSize @@ -175,24 +211,26 @@ def pull_outputs(self, showPlots): dcm_CB = rbk.MRP2C(sigma_CB) # Plot results BSK_plt.clear_all_plots() - stateError = np.zeros([len(position_N[:,0]), NUM_STATES+1]) - navCovarLong = np.full([len(position_N[:,0]), 1+NUM_STATES*NUM_STATES], np.nan) - navCovarLong[:,0] = np.copy(position_N[:,0]) + stateError = np.zeros([len(position_N[:, 0]), NUM_STATES + 1]) + navCovarLong = np.full( + [len(position_N[:, 0]), 1 + NUM_STATES * NUM_STATES], np.nan + ) + navCovarLong[:, 0] = np.copy(position_N[:, 0]) stateError[:, 0:4] = np.copy(position_N) - stateError[:,4:7] = np.copy(velocity_N[:,1:]) - measError = np.full([len(measPos[:,0]), 4], np.nan) - measError[:,0] = measPos[:,0] - measError_C = np.full([len(measPos[:,0]), 5], np.nan) - measError_C[:,0] = measPos[:,0] - trueRhat_C = np.full([len(measPos[:,0]), 4], np.nan) - trueRhat_C[:,0] = measPos[:,0] - truth = np.zeros([len(position_N[:,0]), 7]) - truth[:,0:4] = np.copy(position_N) - truth[:,4:7] = np.copy(velocity_N[:,1:]) + stateError[:, 4:7] = np.copy(velocity_N[:, 1:]) + measError = np.full([len(measPos[:, 0]), 4], np.nan) + measError[:, 0] = measPos[:, 0] + measError_C = np.full([len(measPos[:, 0]), 5], np.nan) + measError_C[:, 0] = measPos[:, 0] + trueRhat_C = np.full([len(measPos[:, 0]), 4], np.nan) + trueRhat_C[:, 0] = measPos[:, 0] + truth = np.zeros([len(position_N[:, 0]), 7]) + truth[:, 0:4] = np.copy(position_N) + truth[:, 4:7] = np.copy(velocity_N[:, 1:]) switchIdx = 0 - Rmars = 3396.19*1E3 + Rmars = 3396.19 * 1e3 for j in range(len(stateError[:, 0])): if stateError[j, 0] in navState[:, 0]: stateError[j, 1:4] -= navState[j - switchIdx, 1:4] @@ -200,17 +238,24 @@ def pull_outputs(self, showPlots): else: stateError[j, 1:] = np.full(NUM_STATES, np.nan) switchIdx += 1 - for i in range(len(measPos[:,0])): - if measPos[i,1] > 1E-8: - measError[i, 1:4] = position_N[i +switchIdx, 1:4] - measPos[i, 1:4] - measError_C[i, 4] = np.linalg.norm(position_N[i +switchIdx, 1:4]) - np.linalg.norm(r_C[i, 1:4]) - measError_C[i, 1:4] = trueRhat_C[i,1:] - r_C[i, 1:4]/np.linalg.norm(r_C[i, 1:4]) - trueRhat_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i +switchIdx, 1:4])) ,position_N[i +switchIdx, 1:4])/np.linalg.norm(position_N[i +switchIdx, 1:4]) - trueRhat_C[i,1:] *= focal/trueRhat_C[i,3] + for i in range(len(measPos[:, 0])): + if measPos[i, 1] > 1e-8: + measError[i, 1:4] = position_N[i + switchIdx, 1:4] - measPos[i, 1:4] + measError_C[i, 4] = np.linalg.norm( + position_N[i + switchIdx, 1:4] + ) - np.linalg.norm(r_C[i, 1:4]) + measError_C[i, 1:4] = trueRhat_C[i, 1:] - r_C[i, 1:4] / np.linalg.norm( + r_C[i, 1:4] + ) + trueRhat_C[i, 1:] = np.dot( + np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), + position_N[i + switchIdx, 1:4], + ) / np.linalg.norm(position_N[i + switchIdx, 1:4]) + trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] else: - measCovar[i,1:] = np.full(3*3, np.nan) + measCovar[i, 1:] = np.full(3 * 3, np.nan) covar_C[i, 1:] = np.full(3 * 3, np.nan) - navCovarLong[switchIdx:,:] = np.copy(navCovar) + navCovarLong[switchIdx:, :] = np.copy(navCovar) timeData = position_N[:, 0] * macros.NANO2MIN @@ -220,7 +265,9 @@ def pull_outputs(self, showPlots): # BSK_plt.AnimatedScatter(sizeOfCam, circleCenters, circleRadii, validCircle) # BSK_plt.plot_cirlces(timeData[switchIdx:], circleCenters, circleRadii, validCircle, sizeOfCam) BSK_plt.plot_faults(faults, validLimb, validHough) - BSK_plt.nav_percentages(truth[switchIdx:,:], navState, navCovar, valid, "Fault") + BSK_plt.nav_percentages( + truth[switchIdx:, :], navState, navCovar, valid, "Fault" + ) BSK_plt.plotStateCovarPlot(stateError, navCovarLong) # BSK_plt.imgProcVsExp(trueCircles, circleCenters, circleRadii, np.array(sizeOfCam)) @@ -235,9 +282,9 @@ def pull_outputs(self, showPlots): return figureList -# Time in min -def run(showPlots, simTime = None): +# Time in min +def run(showPlots, simTime=None): # Instantiate base simulation TheBSKSim = BSKSim(fswRate=0.5, dynRate=0.5) TheBSKSim.set_DynModel(BSK_OpNavDynamics) @@ -257,16 +304,16 @@ def run(showPlots, simTime = None): TheScenario.run_vizard("-noDisplay") # Configure FSW mode - TheScenario.masterSim.modeRequest = 'prepOpNav' + TheScenario.masterSim.modeRequest = "prepOpNav" # Initialize simulation TheBSKSim.InitializeSimulation() # Configure run time and execute simulation - simulationTime = macros.min2nano(3.) + simulationTime = macros.min2nano(3.0) TheBSKSim.ConfigureStopTime(simulationTime) - print('Starting Execution') + print("Starting Execution") t1 = time.time() TheBSKSim.ExecuteSimulation() - TheScenario.masterSim.modeRequest = 'FaultDet' + TheScenario.masterSim.modeRequest = "FaultDet" if simTime != None: simulationTime = macros.min2nano(simTime) else: @@ -274,7 +321,7 @@ def run(showPlots, simTime = None): TheBSKSim.ConfigureStopTime(simulationTime) TheBSKSim.ExecuteSimulation() t2 = time.time() - print('Finished Execution in ', t2-t1, ' seconds. Post-processing results') + print("Finished Execution in ", t2 - t1, " seconds. Post-processing results") # Terminate vizard and show plots figureList = TheScenario.end_scenario() return figureList diff --git a/examples/SunLineKF_test_utilities.py b/examples/SunLineKF_test_utilities.py index 838b239ac3..5e656822b9 100644 --- a/examples/SunLineKF_test_utilities.py +++ b/examples/SunLineKF_test_utilities.py @@ -39,587 +39,631 @@ def StateErrorCovarPlot(x, Pflat, FilterType, show_plots, saveFigures): :param saveFigures: flag :return: """ - nstates = int(np.sqrt(len(Pflat[0,:])-1)) + nstates = int(np.sqrt(len(Pflat[0, :]) - 1)) - P = np.zeros([len(Pflat[:,0]),nstates,nstates]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*1E-9 - P[i,:,:] = Pflat[i,1:37].reshape([nstates,nstates]) - for j in range(len(P[0,0,:])): - P[i,j,j] = np.sqrt(P[i,j,j]) + P = np.zeros([len(Pflat[:, 0]), nstates, nstates]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * 1e-9 + P[i, :, :] = Pflat[i, 1:37].reshape([nstates, nstates]) + for j in range(len(P[0, 0, :])): + P[i, j, j] = np.sqrt(P[i, j, j]) if nstates == 6: - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t , x[:, 1], "b", label='Error Filter') - plt.plot(t , 3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t , -3 * np.sqrt(P[:, 0, 0]), 'r--') - plt.legend(loc='upper right') - plt.ylabel('$d_x$(m)') - plt.title('First LOS component') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.plot(t, 3 * np.sqrt(P[:, 0, 0]), "r--", label="Covar Filter") + plt.plot(t, -3 * np.sqrt(P[:, 0, 0]), "r--") + plt.legend(loc="upper right") + plt.ylabel("$d_x$(m)") + plt.title("First LOS component") plt.grid() plt.subplot(322) - plt.plot(t , x[:, 4], "b") - plt.plot(t , 3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.ylabel(r'$\dot{d}_x$(m)') - plt.title('First rate component') + plt.plot(t, x[:, 4], "b") + plt.plot(t, 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 3, 3]), "r--") + plt.ylabel(r"$\dot{d}_x$(m)") + plt.title("First rate component") plt.grid() plt.subplot(323) - plt.plot(t , x[:, 2], "b") - plt.plot(t , 3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.ylabel(r'$d_y$(m)') - plt.title('Second LOS component') + plt.plot(t, x[:, 2], "b") + plt.plot(t, 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 1, 1]), "r--") + plt.ylabel(r"$d_y$(m)") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(t , x[:, 5], "b") - plt.plot(t , 3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.ylabel(r'$\dot{d}_y$(m)') - plt.title('Second rate component') + plt.plot(t, x[:, 5], "b") + plt.plot(t, 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 4, 4]), "r--") + plt.ylabel(r"$\dot{d}_y$(m)") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(t , x[:, 3], "b") - plt.plot(t , 3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.ylabel('$d_z$(m)') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(t, x[:, 3], "b") + plt.plot(t, 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 2, 2]), "r--") + plt.ylabel("$d_z$(m)") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() - if FilterType == 'SuKF': + if FilterType == "SuKF": plt.subplot(326) plt.plot(t, x[:, 6], "b") - plt.plot(t, x[:, 6] + 3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.plot(t, x[:, 6] -3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.ylabel(r'$\dot{d}_z$(m)') - plt.xlabel('t(s)') - plt.title('Sun Intensity') + plt.plot(t, x[:, 6] + 3 * np.sqrt(P[:, 5, 5]), "r--") + plt.plot(t, x[:, 6] - 3 * np.sqrt(P[:, 5, 5]), "r--") + plt.ylabel(r"$\dot{d}_z$(m)") + plt.xlabel("t(s)") + plt.title("Sun Intensity") plt.grid() else: plt.subplot(326) - plt.plot(t , x[:, 6], "b") - plt.plot(t , 3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.ylabel(r'$\dot{d}_z$(m)') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(t, x[:, 6], "b") + plt.plot(t, 3 * np.sqrt(P[:, 5, 5]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 5, 5]), "r--") + plt.ylabel(r"$\dot{d}_z$(m)") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() if nstates == 3: - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(t, x[:, 1], "b", label='Error Filter') - plt.plot(t, 3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t, -3 * np.sqrt(P[:, 0, 0]), 'r--') - plt.legend(loc='lower right') - plt.ylabel('$d_x$(m)') - plt.title('First LOS component') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.plot(t, 3 * np.sqrt(P[:, 0, 0]), "r--", label="Covar Filter") + plt.plot(t, -3 * np.sqrt(P[:, 0, 0]), "r--") + plt.legend(loc="lower right") + plt.ylabel("$d_x$(m)") + plt.title("First LOS component") plt.grid() plt.subplot(312) plt.plot(t, x[:, 2], "b") - plt.plot(t, 3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.plot(t, -3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.ylabel('$d_y$(m)') - plt.title('Second LOST component') + plt.plot(t, 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 1, 1]), "r--") + plt.ylabel("$d_y$(m)") + plt.title("Second LOST component") plt.grid() plt.subplot(313) plt.plot(t, x[:, 3], "b") - plt.plot(t, 3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.plot(t, -3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.ylabel('$d_z$(m)') - plt.title('Third LOS component') + plt.plot(t, 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 2, 2]), "r--") + plt.ylabel("$d_z$(m)") + plt.title("Third LOS component") plt.grid() if nstates == 5: - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t , x[:, 1], "b", label='Error Filter') - plt.plot(t , 3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t , -3 * np.sqrt(P[:, 0, 0]), 'r--') - plt.legend(loc='lower right') - plt.ylabel('$d_x$(m)') - plt.title('First LOS component') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.plot(t, 3 * np.sqrt(P[:, 0, 0]), "r--", label="Covar Filter") + plt.plot(t, -3 * np.sqrt(P[:, 0, 0]), "r--") + plt.legend(loc="lower right") + plt.ylabel("$d_x$(m)") + plt.title("First LOS component") plt.grid() plt.subplot(323) - plt.plot(t , x[:, 2], "b") - plt.plot(t , 3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.ylabel('$d_y$(m)') - plt.title('Second LOS component') + plt.plot(t, x[:, 2], "b") + plt.plot(t, 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 1, 1]), "r--") + plt.ylabel("$d_y$(m)") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(t , x[:, 3], "b") - plt.plot(t , 3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.ylabel(r'$\omega_y$(m)') - plt.title('Second rate component') + plt.plot(t, x[:, 3], "b") + plt.plot(t, 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 3, 3]), "r--") + plt.ylabel(r"$\omega_y$(m)") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(t , x[:, 3], "b") - plt.plot(t , 3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.ylabel('$d_z$(m)') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(t, x[:, 3], "b") + plt.plot(t, 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 2, 2]), "r--") + plt.ylabel("$d_z$(m)") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(t , x[:, 5], "b") - plt.plot(t , 3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.ylabel(r'$\omega_z$(m)') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(t, x[:, 5], "b") + plt.plot(t, 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 4, 4]), "r--") + plt.ylabel(r"$\omega_z$(m)") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() if saveFigures: - unitTestSupport.saveScenarioFigure('scenario_Filters_StatesPlot'+FilterType, plt, path) + unitTestSupport.saveScenarioFigure( + "scenario_Filters_StatesPlot" + FilterType, plt, path + ) if show_plots: plt.show() - plt.close('all') + plt.close("all") def StatesPlotCompare(x, x2, Pflat, Pflat2, FilterType, show_plots, saveFigures): + nstates = int(np.sqrt(len(Pflat[0, :]) - 1)) - nstates = int(np.sqrt(len(Pflat[0,:])-1)) - - P = np.zeros([len(Pflat[:,0]),nstates,nstates]) - P2 = np.zeros([len(Pflat[:,0]),nstates,nstates]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*1E-9 - P[i,:,:] = Pflat[i,1:(nstates*nstates +1)].reshape([nstates,nstates]) - P2[i, :, :] = Pflat2[i, 1:(nstates*nstates +1)].reshape([nstates, nstates]) + P = np.zeros([len(Pflat[:, 0]), nstates, nstates]) + P2 = np.zeros([len(Pflat[:, 0]), nstates, nstates]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * 1e-9 + P[i, :, :] = Pflat[i, 1 : (nstates * nstates + 1)].reshape([nstates, nstates]) + P2[i, :, :] = Pflat2[i, 1 : (nstates * nstates + 1)].reshape([nstates, nstates]) if nstates == 6: - - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t[0:30] , x[0:30, 1], "b", label='Error Filter') - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 0, 0]), 'r--') - plt.plot(t[0:30] , x2[0:30, 1], "g", label='Error Expected') - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 0, 0]), 'c--', label='Covar Expected') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 0, 0]), 'c--') - plt.legend(loc='lower right') - plt.ylabel('$d_x$(m)') - plt.title('First LOS component') + plt.plot(t[0:30], x[0:30, 1], "b", label="Error Filter") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 0, 0]), "r--", label="Covar Filter") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 0, 0]), "r--") + plt.plot(t[0:30], x2[0:30, 1], "g", label="Error Expected") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 0, 0]), "c--", label="Covar Expected") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 0, 0]), "c--") + plt.legend(loc="lower right") + plt.ylabel("$d_x$(m)") + plt.title("First LOS component") plt.grid() plt.subplot(322) - plt.plot(t[0:30] , x[0:30, 4], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 3, 3]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 3, 3]), 'r--') - plt.plot(t[0:30] , x2[0:30, 4], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 3, 3]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 3, 3]), 'c--') - plt.ylabel(r'$\dot{d}_x$(m)') - plt.title('First rate component') + plt.plot(t[0:30], x[0:30, 4], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 3, 3]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 3, 3]), "r--") + plt.plot(t[0:30], x2[0:30, 4], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 3, 3]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 3, 3]), "c--") + plt.ylabel(r"$\dot{d}_x$(m)") + plt.title("First rate component") plt.grid() plt.subplot(323) - plt.plot(t[0:30] , x[0:30, 2], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 1, 1]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 1, 1]), 'r--') - plt.plot(t[0:30] , x2[0:30, 2], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.ylabel('$d_y$(m)') - plt.title('Second LOS component') + plt.plot(t[0:30], x[0:30, 2], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 1, 1]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 1, 1]), "r--") + plt.plot(t[0:30], x2[0:30, 2], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.ylabel("$d_y$(m)") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(t[0:30] , x[0:30, 5], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 4, 4]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 4, 4]), 'r--') - plt.plot(t[0:30] , x2[0:30, 5], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 4, 4]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 4, 4]), 'c--') - plt.ylabel(r'$\dot{d}_y$(m)') - plt.title('Second rate component') + plt.plot(t[0:30], x[0:30, 5], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 4, 4]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 4, 4]), "r--") + plt.plot(t[0:30], x2[0:30, 5], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 4, 4]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 4, 4]), "c--") + plt.ylabel(r"$\dot{d}_y$(m)") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(t[0:30] , x[0:30, 3], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 2, 2]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 2, 2]), 'r--') - plt.plot(t[0:30] , x2[0:30, 3], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.ylabel('$d_z$(m)') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(t[0:30], x[0:30, 3], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 2, 2]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 2, 2]), "r--") + plt.plot(t[0:30], x2[0:30, 3], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.ylabel("$d_z$(m)") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() - - if FilterType == 'SuKF': + if FilterType == "SuKF": plt.subplot(326) plt.plot(t[0:30], x[0:30, 6], "b") - plt.plot(t[0:30], x[0:30, 6] + 3 * np.sqrt(P[0:30, 5, 5]), 'r--') - plt.plot(t[0:30], x[0:30, 6] -3 * np.sqrt(P[0:30, 5, 5]), 'r--') + plt.plot(t[0:30], x[0:30, 6] + 3 * np.sqrt(P[0:30, 5, 5]), "r--") + plt.plot(t[0:30], x[0:30, 6] - 3 * np.sqrt(P[0:30, 5, 5]), "r--") plt.plot(t[0:30], x2[0:30, 6], "g") - plt.plot(t[0:30], x2[0:30, 6]+ 3 * np.sqrt(P2[0:30, 5, 5]), 'c--') - plt.plot(t[0:30], x2[0:30, 6]-3 * np.sqrt(P2[0:30, 5, 5]), 'c--') - plt.ylabel('S') - plt.xlabel('t(s)') - plt.title('Solar Intensity') + plt.plot(t[0:30], x2[0:30, 6] + 3 * np.sqrt(P2[0:30, 5, 5]), "c--") + plt.plot(t[0:30], x2[0:30, 6] - 3 * np.sqrt(P2[0:30, 5, 5]), "c--") + plt.ylabel("S") + plt.xlabel("t(s)") + plt.title("Solar Intensity") plt.grid() else: plt.subplot(326) - plt.plot(t[0:30] , x[0:30, 6], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 5, 5]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 5, 5]), 'r--') - plt.plot(t[0:30] , x2[0:30, 6], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 5, 5]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 5, 5]), 'c--') - plt.ylabel(r'$\dot{d}_z$(m)') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(t[0:30], x[0:30, 6], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 5, 5]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 5, 5]), "r--") + plt.plot(t[0:30], x2[0:30, 6], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 5, 5]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 5, 5]), "c--") + plt.ylabel(r"$\dot{d}_z$(m)") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() if nstates == 3: - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(t[0:30], x[0:30, 1], "b", label='Error Filter') - plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 0, 0]), 'r--') - plt.plot(t[0:30], x2[0:30, 1], "g", label='Error Expected') - plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 0, 0]), 'c--', label='Covar Expected') - plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 0, 0]), 'c--') - plt.ylabel('$d_x$(m)') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(t[0:30], x[0:30, 1], "b", label="Error Filter") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 0, 0]), "r--", label="Covar Filter") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 0, 0]), "r--") + plt.plot(t[0:30], x2[0:30, 1], "g", label="Error Expected") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 0, 0]), "c--", label="Covar Expected") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 0, 0]), "c--") + plt.ylabel("$d_x$(m)") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(312) plt.plot(t[0:30], x[0:30, 2], "b") - plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 1, 1]), 'r--') - plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 1, 1]), 'r--') + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 1, 1]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 1, 1]), "r--") plt.plot(t[0:30], x2[0:30, 2], "g") - plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.ylabel('$d_y$(m)') - plt.title('Second LOS component') + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.ylabel("$d_y$(m)") + plt.title("Second LOS component") plt.grid() plt.subplot(313) plt.plot(t[0:30], x[0:30, 3], "b") - plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 2, 2]), 'r--') - plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 2, 2]), 'r--') + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 2, 2]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 2, 2]), "r--") plt.plot(t[0:30], x2[0:30, 3], "g") - plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.ylabel('$d_z$(m)') - plt.title('Third LOS component') + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.ylabel("$d_z$(m)") + plt.title("Third LOS component") plt.grid() if nstates == 5: - - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t[0:30] , x[0:30, 1], "b", label='Error Filter') - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 0, 0]), 'r--') - plt.plot(t[0:30] , x2[0:30, 1], "g", label='Error Expected') - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 0, 0]), 'c--', label='Covar Expected') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 0, 0]), 'c--') - plt.legend(loc='lower right') - plt.ylabel('$d_x$(m)') - plt.title('First LOS component') + plt.plot(t[0:30], x[0:30, 1], "b", label="Error Filter") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 0, 0]), "r--", label="Covar Filter") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 0, 0]), "r--") + plt.plot(t[0:30], x2[0:30, 1], "g", label="Error Expected") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 0, 0]), "c--", label="Covar Expected") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 0, 0]), "c--") + plt.legend(loc="lower right") + plt.ylabel("$d_x$(m)") + plt.title("First LOS component") plt.grid() plt.subplot(323) - plt.plot(t[0:30] , x[0:30, 2], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 1, 1]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 1, 1]), 'r--') - plt.plot(t[0:30] , x2[0:30, 2], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.ylabel('$d_y$(m)') - plt.title('Second LOS component') + plt.plot(t[0:30], x[0:30, 2], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 1, 1]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 1, 1]), "r--") + plt.plot(t[0:30], x2[0:30, 2], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.ylabel("$d_y$(m)") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(t[0:30] , x[0:30, 4], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 3, 3]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 3, 3]), 'r--') - plt.plot(t[0:30] , x2[0:30, 4], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 3, 3]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 3, 3]), 'c--') - plt.ylabel(r'$\omega_y$(m)') - plt.title('Second rate component') + plt.plot(t[0:30], x[0:30, 4], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 3, 3]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 3, 3]), "r--") + plt.plot(t[0:30], x2[0:30, 4], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 3, 3]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 3, 3]), "c--") + plt.ylabel(r"$\omega_y$(m)") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(t[0:30] , x[0:30, 3], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 2, 2]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 2, 2]), 'r--') - plt.plot(t[0:30] , x2[0:30, 3], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.ylabel('$d_z$(m)') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(t[0:30], x[0:30, 3], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 2, 2]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 2, 2]), "r--") + plt.plot(t[0:30], x2[0:30, 3], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.ylabel("$d_z$(m)") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(t[0:30] , x[0:30, 5], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 4, 4]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 4, 4]), 'r--') - plt.plot(t[0:30] , x2[0:30, 5], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 4, 4]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 4, 4]), 'c--') - plt.ylabel(r'$\omega_z$(m)') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(t[0:30], x[0:30, 5], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 4, 4]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 4, 4]), "r--") + plt.plot(t[0:30], x2[0:30, 5], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 4, 4]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 4, 4]), "c--") + plt.ylabel(r"$\omega_z$(m)") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() if saveFigures: - unitTestSupport.saveScenarioFigure('scenario_Filters_StatesCompare'+FilterType, plt, path) + unitTestSupport.saveScenarioFigure( + "scenario_Filters_StatesCompare" + FilterType, plt, path + ) if show_plots: plt.show() plt.close() + def numMeasurements(numObs, FilterType, show_plots, saveFigures): plt.plot(111) - plt.plot(numObs[:,0]*(1E-9) , numObs[:, 1], "b") - plt.ylim([0,5]) - plt.xlabel('t(s)') - plt.title('Number of Activated CSS') + plt.plot(numObs[:, 0] * (1e-9), numObs[:, 1], "b") + plt.ylim([0, 5]) + plt.xlabel("t(s)") + plt.title("Number of Activated CSS") if saveFigures: - unitTestSupport.saveScenarioFigure('scenario_Filters_Obs'+ FilterType, plt, path) + unitTestSupport.saveScenarioFigure( + "scenario_Filters_Obs" + FilterType, plt, path + ) if show_plots: plt.show() plt.close() -def PostFitResiduals(Res, noise, FilterType, show_plots, saveFigures): - MeasNoise = np.zeros(len(Res[:,0])) - t= np.zeros(len(Res[:,0])) - constantVal = np.array([np.nan]*4) - for i in range(len(Res[:,0])): - t[i] = Res[i, 0]*1E-9 - MeasNoise[i] = 3*noise +def PostFitResiduals(Res, noise, FilterType, show_plots, saveFigures): + MeasNoise = np.zeros(len(Res[:, 0])) + t = np.zeros(len(Res[:, 0])) + constantVal = np.array([np.nan] * 4) + for i in range(len(Res[:, 0])): + t[i] = Res[i, 0] * 1e-9 + MeasNoise[i] = 3 * noise # Don't plot constant values, they mean no measurement is taken - if i>0: - for j in range(1,5): - with np.errstate(invalid='ignore'): - constantRes = np.abs(Res[i,j]-Res[i-1,j]) - if constantRes < 1E-10 or np.abs(constantVal[j-1] - Res[i,j])<1E-10: - constantVal[j-1] = Res[i, j] + if i > 0: + for j in range(1, 5): + with np.errstate(invalid="ignore"): + constantRes = np.abs(Res[i, j] - Res[i - 1, j]) + if ( + constantRes < 1e-10 + or np.abs(constantVal[j - 1] - Res[i, j]) < 1e-10 + ): + constantVal[j - 1] = Res[i, j] Res[i, j] = np.nan - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(411) - plt.plot(t , Res[:, 1], "b.", label='Residual') - plt.plot(t , MeasNoise, 'r--', label='Covar') - plt.plot(t , -MeasNoise, 'r--') - plt.legend(loc='lower right') - plt.ylabel('$r_1$(m)') - plt.ylim([-5*noise, 5*noise]) - plt.title('First CSS') - + plt.plot(t, Res[:, 1], "b.", label="Residual") + plt.plot(t, MeasNoise, "r--", label="Covar") + plt.plot(t, -MeasNoise, "r--") + plt.legend(loc="lower right") + plt.ylabel("$r_1$(m)") + plt.ylim([-5 * noise, 5 * noise]) + plt.title("First CSS") plt.subplot(412) - plt.plot(t , Res[:, 2], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylabel('$r_2$(m)') - plt.ylim([-5*noise, 5*noise]) - plt.title('Second CSS') + plt.plot(t, Res[:, 2], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylabel("$r_2$(m)") + plt.ylim([-5 * noise, 5 * noise]) + plt.title("Second CSS") plt.subplot(413) - plt.plot(t , Res[:, 3], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylabel('$r_3$(m)') - plt.ylim([-5*noise, 5*noise]) - plt.title('Third CSS') + plt.plot(t, Res[:, 3], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylabel("$r_3$(m)") + plt.ylim([-5 * noise, 5 * noise]) + plt.title("Third CSS") plt.subplot(414) - plt.plot(t , Res[:, 4], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-5*noise, 5*noise]) - plt.ylabel('$r_4$(m)') - plt.xlabel('t(s)') - plt.title('Fourth CSS') + plt.plot(t, Res[:, 4], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-5 * noise, 5 * noise]) + plt.ylabel("$r_4$(m)") + plt.xlabel("t(s)") + plt.title("Fourth CSS") if saveFigures: - unitTestSupport.saveScenarioFigure('scenario_Filters_PostFit'+ FilterType, plt, path) + unitTestSupport.saveScenarioFigure( + "scenario_Filters_PostFit" + FilterType, plt, path + ) if show_plots: plt.show() plt.close() -def StatesVsExpected(stateLog, Pflat, expectedStateArray, FilterType, show_plots, saveFigures): - nstates = int(np.sqrt(len(Pflat[0,:])-1)) +def StatesVsExpected( + stateLog, Pflat, expectedStateArray, FilterType, show_plots, saveFigures +): + nstates = int(np.sqrt(len(Pflat[0, :]) - 1)) P = np.zeros([len(Pflat[:, 0]), nstates, nstates]) for i in range(len(Pflat[:, 0])): - P[i, :, :] = Pflat[i, 1:(nstates*nstates +1)].reshape([nstates, nstates]) - for j in range(len(P[0,0,:])): - P[i,j,j] = np.sqrt(P[i,j,j]) + P[i, :, :] = Pflat[i, 1 : (nstates * nstates + 1)].reshape([nstates, nstates]) + for j in range(len(P[0, 0, :])): + P[i, j, j] = np.sqrt(P[i, j, j]) - if nstates ==6: - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + if nstates == 6: + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 1], 'k--', label='Expected') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1], 'b', label='Filter') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1] + P[:,0,0], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1] - P[:,0,0], 'r--', label='Covar') - plt.legend(loc='lower right') - plt.ylabel('$d_x$(m)') - plt.title('First LOS component') + plt.plot( + stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 1], "k--", label="Expected" + ) + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1], "b", label="Filter") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1] + P[:, 0, 0], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 1] - P[:, 0, 0], "r--", label="Covar" + ) + plt.legend(loc="lower right") + plt.ylabel("$d_x$(m)") + plt.title("First LOS component") plt.grid() plt.subplot(322) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 4], 'k--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 4], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 4] + P[:,3,3], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 4] - P[:,3,3], 'r--', label='Covar') - plt.ylabel(r'$\dot{d}_x$(m)') - plt.title('First rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 4], "k--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 4], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 4] + P[:, 3, 3], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 4] - P[:, 3, 3], "r--", label="Covar" + ) + plt.ylabel(r"$\dot{d}_x$(m)") + plt.title("First rate component") plt.grid() plt.subplot(323) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 2], 'k--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2] + P[:,1,1], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2] - P[:,1,1], 'r--', label='Covar') - plt.ylabel('$d_y$(m)') - plt.title('Second LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 2], "k--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2] + P[:, 1, 1], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 2] - P[:, 1, 1], "r--", label="Covar" + ) + plt.ylabel("$d_y$(m)") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 5], 'k--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 5], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 5] + P[:,4,4], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 5] - P[:,4,4], 'r--', label='Covar') - plt.ylabel(r'$\dot{d}_y$(m)') - plt.title('Second rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 5], "k--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 5], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 5] + P[:, 4, 4], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 5] - P[:, 4, 4], "r--", label="Covar" + ) + plt.ylabel(r"$\dot{d}_y$(m)") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 3], 'k--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3] + P[:,2,2], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3] - P[:,2,2], 'r--', label='Covar') - plt.ylabel('$d_z$(m)') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 3], "k--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3] + P[:, 2, 2], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 3] - P[:, 2, 2], "r--", label="Covar" + ) + plt.ylabel("$d_z$(m)") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 6], 'k--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 6], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 6] + P[:,5,5], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 6] - P[:,5,5], 'r--', label='Covar') - plt.ylabel(r'$\dot{d}_z$(m)') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 6], "k--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 6], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 6] + P[:, 5, 5], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 6] - P[:, 5, 5], "r--", label="Covar" + ) + plt.ylabel(r"$\dot{d}_z$(m)") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - if nstates ==3: - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + if nstates == 3: + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 1], 'k--', label='Expected') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1], 'b', label='Filter') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1] + P[:, 0, 0], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1] - P[:, 0, 0], 'r--', label='Covar') - plt.ylabel('$d_x$(m)') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot( + stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 1], "k--", label="Expected" + ) + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1], "b", label="Filter") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1] + P[:, 0, 0], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 1] - P[:, 0, 0], "r--", label="Covar" + ) + plt.ylabel("$d_x$(m)") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(312) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 2], 'k--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2] + P[:, 1, 1], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2] - P[:, 1, 1], 'r--', label='Covar') - plt.ylabel('$d_y$(m)') - plt.title('Second LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 2], "k--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2] + P[:, 1, 1], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 2] - P[:, 1, 1], "r--", label="Covar" + ) + plt.ylabel("$d_y$(m)") + plt.title("Second LOS component") plt.grid() plt.subplot(313) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 3], 'k--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3] + P[:, 2, 2], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3] - P[:, 2, 2], 'r--', label='Covar') - plt.ylabel('$d_z$(m)') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 3], "k--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3] + P[:, 2, 2], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 3] - P[:, 2, 2], "r--", label="Covar" + ) + plt.ylabel("$d_z$(m)") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() - if nstates ==5: - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + if nstates == 5: + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 1], 'k--', label='Expected') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1], 'b', label='Filter') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1] + P[:,0,0], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1] - P[:,0,0], 'r--', label='Covar') - plt.legend(loc='lower right') - plt.ylabel('$d_x$(m)') - plt.title('First LOS component') + plt.plot( + stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 1], "k--", label="Expected" + ) + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1], "b", label="Filter") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1] + P[:, 0, 0], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 1] - P[:, 0, 0], "r--", label="Covar" + ) + plt.legend(loc="lower right") + plt.ylabel("$d_x$(m)") + plt.title("First LOS component") plt.grid() plt.subplot(323) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 2], 'k--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2] + P[:,1,1], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2] - P[:,1,1], 'r--', label='Covar') - plt.ylabel('$d_y$(m)') - plt.title('Second LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 2], "k--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2] + P[:, 1, 1], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 2] - P[:, 1, 1], "r--", label="Covar" + ) + plt.ylabel("$d_y$(m)") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 4], 'k--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 4], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 4] + P[:,3,3], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 4] - P[:,3,3], 'r--', label='Covar') - plt.ylabel(r'$\omega_y$(m)') - plt.title('Second rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 4], "k--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 4], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 4] + P[:, 3, 3], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 4] - P[:, 3, 3], "r--", label="Covar" + ) + plt.ylabel(r"$\omega_y$(m)") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 3], 'k--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3] + P[:,2,2], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3] - P[:,2,2], 'r--', label='Covar') - plt.ylabel('$d_z$(m)') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 3], "k--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3] + P[:, 2, 2], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 3] - P[:, 2, 2], "r--", label="Covar" + ) + plt.ylabel("$d_z$(m)") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 5], 'k--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 5], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 5] + P[:,4,4], 'r--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 5] - P[:,4,4], 'r--', label='Covar') - plt.ylabel(r'$\omega_z$(m)') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 5], "k--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 5], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 5] + P[:, 4, 4], "r--") + plt.plot( + stateLog[:, 0] * 1.0e-9, stateLog[:, 5] - P[:, 4, 4], "r--", label="Covar" + ) + plt.ylabel(r"$\omega_z$(m)") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - if saveFigures: - unitTestSupport.saveScenarioFigure('scenario_Filters_StatesExpected' + FilterType , plt, path) + unitTestSupport.saveScenarioFigure( + "scenario_Filters_StatesExpected" + FilterType, plt, path + ) if show_plots: plt.show() @@ -627,78 +671,79 @@ def StatesVsExpected(stateLog, Pflat, expectedStateArray, FilterType, show_plots def StatesVsTargets(target1, target2, stateLog, FilterType, show_plots, saveFigures): + nstates = int(stateLog[0, :]) - nstates = int(stateLog[0,:]) - - target = np.ones([len(stateLog[:, 0]),nstates]) - for i in range((len(stateLog[:, 0])-1)/2): + target = np.ones([len(stateLog[:, 0]), nstates]) + for i in range((len(stateLog[:, 0]) - 1) / 2): target[i, :] = target1 - target[i+(len(stateLog[:, 0]) - 1) / 2,:] = target2 + target[i + (len(stateLog[:, 0]) - 1) / 2, :] = target2 if nstates == 6: - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1], 'b', label='Filter') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 0], 'r--', label='Expected') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1], "b", label="Filter") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 0], "r--", label="Expected") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(322) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 4], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 3], 'r--') - plt.title('First rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 4], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 3], "r--") + plt.title("First rate component") plt.grid() plt.subplot(323) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 1], 'r--') - plt.title('Second LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 1], "r--") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 5], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 4], 'r--') - plt.title('Second rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 5], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 4], "r--") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 2], 'r--') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 2], "r--") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 6], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 5], 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 6], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 5], "r--") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() if nstates == 3: - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1], 'b', label='Filter') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 0], 'r--', label='Expected') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1], "b", label="Filter") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 0], "r--", label="Expected") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(312) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 1], 'r--') - plt.title('Second rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 1], "r--") + plt.title("Second rate component") plt.grid() plt.subplot(313) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 2], 'r--') - plt.title('Third LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 2], "r--") + plt.title("Third LOS component") plt.grid() if saveFigures: - unitTestSupport.saveScenarioFigure('scenario_Filters_StatesTarget' + FilterType, plt, path) + unitTestSupport.saveScenarioFigure( + "scenario_Filters_StatesTarget" + FilterType, plt, path + ) if show_plots: plt.show() diff --git a/examples/mujoco/scenarioArmWithThrusters.py b/examples/mujoco/scenarioArmWithThrusters.py index d71ac09a2c..76804d45e0 100644 --- a/examples/mujoco/scenarioArmWithThrusters.py +++ b/examples/mujoco/scenarioArmWithThrusters.py @@ -101,17 +101,17 @@ # These are the instant at which each action ends. For example, # ARM_3_DEPLOY ends at t=20s. FIRST_THRUST ends at t=52.5 s, etc. -START = 0 # s -ARM_1_DEPLOY = 5 # s -ARM_2_DEPLOY = ARM_1_DEPLOY + 10 # s -ARM_3_DEPLOY = ARM_2_DEPLOY + 5 # s -FIRST_THRUST = ARM_3_DEPLOY + 32.5 # s -FIRST_ROLL = FIRST_THRUST + 10 # s -SECOND_THRUST = FIRST_ROLL + 30 # s -SECOND_ROLL = SECOND_THRUST + 5 # s -ARM_4_DEPLOY = SECOND_ROLL + 5 # s -THIRD_THRUST = ARM_4_DEPLOY + 30 # s -END = THIRD_THRUST # s +START = 0 # s +ARM_1_DEPLOY = 5 # s +ARM_2_DEPLOY = ARM_1_DEPLOY + 10 # s +ARM_3_DEPLOY = ARM_2_DEPLOY + 5 # s +FIRST_THRUST = ARM_3_DEPLOY + 32.5 # s +FIRST_ROLL = FIRST_THRUST + 10 # s +SECOND_THRUST = FIRST_ROLL + 30 # s +SECOND_ROLL = SECOND_THRUST + 5 # s +ARM_4_DEPLOY = SECOND_ROLL + 5 # s +THIRD_THRUST = ARM_4_DEPLOY + 30 # s +END = THIRD_THRUST # s # This dictionary maps the joint name to the body it is attached to. BODY_OF_JOINT = { @@ -174,7 +174,7 @@ def run(showPlots: bool = False, visualize: bool = False): visualize (bool, optional): If True, the ``MJScene`` visualization tool is run on the simulation results. Defaults to False. """ - dt = 1 # s + dt = 1 # s # Create a simulation, process, and task as usual scSim = SimulationBaseClass.SimBaseClass() @@ -192,7 +192,6 @@ def run(showPlots: bool = False, visualize: bool = False): # Creat the thrust interpolators to define the piecewise thrust profile modelsActuator = [] for thrusterName, interpPoint in ACTUATOR_INTERPOLATION_POINTS.items(): - act: mujoco.MJSingleActuator = scene.getSingleActuator(thrusterName) # mujoco.SingleActuatorInterpolator has a single output message, @@ -218,7 +217,6 @@ def run(showPlots: bool = False, visualize: bool = False): # Create the joint interpolators to define the motion of the joints modelsJoint = [] for jointName, interpPoint in JOINT_INTERPOLATION_POINTS.items(): - # We can access the joint object by getting the body that # the joint is attached to, and then getting the joint object joint: mujoco.MJScalarJoint = scene.getBody( @@ -268,7 +266,6 @@ def run(showPlots: bool = False, visualize: bool = False): scSim.ExecuteSimulation() if showPlots: - # Plot the attitude of the 'hub' body plt.figure() att = np.squeeze(bodyStateRecorder.sigma_BN) diff --git a/examples/mujoco/scenarioAsteroidLanding.py b/examples/mujoco/scenarioAsteroidLanding.py index 77c160f094..dfa4d60cd4 100644 --- a/examples/mujoco/scenarioAsteroidLanding.py +++ b/examples/mujoco/scenarioAsteroidLanding.py @@ -113,10 +113,10 @@ def run(showPlots: bool = False, visualize: bool = False): visualize (bool, optional): If True, the ``MJScene`` visualization tool is run on the simulation results. Defaults to False. """ - dt = 1 # s + dt = 1 # s - timeThrustTurnOff = 47.5 # s - tf = 70 # s + timeThrustTurnOff = 47.5 # s + tf = 70 # s # Create a simulation, process, and task as usual scSim = SimulationBaseClass.SimBaseClass() @@ -159,7 +159,7 @@ def run(showPlots: bool = False, visualize: bool = False): gravity.frameInMsg.subscribeTo(gravityApplicationSite.stateOutMsg) # Set a thruster force of 275 N trying to slowdown our descent - thrust = 275 # N + thrust = 275 # N thrustMsg = messaging.SingleActuatorMsg() thrustMsg.write(messaging.SingleActuatorMsgPayload(input=thrust)) @@ -178,14 +178,14 @@ def run(showPlots: bool = False, visualize: bool = False): scSim.InitializeSimulation() # Initial velocity of 1 m/s towards asteroid - scene.getBody("hub").setVelocity([0, 0, -1]) # m/s + scene.getBody("hub").setVelocity([0, 0, -1]) # m/s # Run the simulation for some time with the thruster on scSim.ConfigureStopTime(macros.sec2nano(timeThrustTurnOff)) scSim.ExecuteSimulation() # Near surface, turn off thrusters and let gravity land us - thrustMsg.write(messaging.SingleActuatorMsgPayload(input=0)) # N + thrustMsg.write(messaging.SingleActuatorMsgPayload(input=0)) # N # Run until simulation completion scSim.ConfigureStopTime(macros.sec2nano(tf)) diff --git a/examples/mujoco/scenarioBranchingPanels.py b/examples/mujoco/scenarioBranchingPanels.py index eb20b45f16..38ff1cb376 100644 --- a/examples/mujoco/scenarioBranchingPanels.py +++ b/examples/mujoco/scenarioBranchingPanels.py @@ -86,18 +86,25 @@ CURRENT_FOLDER = os.path.dirname(__file__) XML_PATH = f"{CURRENT_FOLDER}/sat_w_branching_panels.xml" + @contextmanager def catchtime(): tic = toc = perf_counter() yield lambda: toc - tic toc = perf_counter() + BLUE = "#004488" YELLOW = "#DDAA33" RED = "#BB5566" + def generateProfiles( - initialPoint: float, finalPoint: float, vMax: float, aMax: float, timeOffset: int = 0 + initialPoint: float, + finalPoint: float, + vMax: float, + aMax: float, + timeOffset: int = 0, ) -> List[mujoco.ScalarJointStateInterpolator]: """ Generate a position and velocity profile for a point-to-point movement. @@ -180,6 +187,7 @@ def generateProfiles( return interpolators + def run(showPlots: bool = False, visualize: bool = False): """Main function, see scenario description. @@ -191,7 +199,7 @@ def run(showPlots: bool = False, visualize: bool = False): """ dt = macros.sec2nano(10) - operationTime = macros.sec2nano(90*60) + operationTime = macros.sec2nano(90 * 60) # Create a simulation, process, and task as usual scSim = SimulationBaseClass.SimBaseClass() @@ -246,11 +254,11 @@ def addJointController(panelID: str, initialAngle: float, timeOffset: int): # Generate the position and velocity profiles for the joint positionInterpolator, velocityInterpolator = generateProfiles( - initialPoint=initialAngle, # rad - finalPoint=0, # rad + initialPoint=initialAngle, # rad + finalPoint=0, # rad vMax=np.deg2rad(0.05), aMax=np.deg2rad(0.0001), - timeOffset=timeOffset + timeOffset=timeOffset, ) # Use the C++ JointPIDController @@ -262,8 +270,12 @@ def addJointController(panelID: str, initialAngle: float, timeOffset: int): # Connect the interpolators to the PID controller for the desired # position and velocity - pidController.desiredPosInMsg.subscribeTo(positionInterpolator.interpolatedOutMsg) - pidController.desiredVelInMsg.subscribeTo(velocityInterpolator.interpolatedOutMsg) + pidController.desiredPosInMsg.subscribeTo( + positionInterpolator.interpolatedOutMsg + ) + pidController.desiredVelInMsg.subscribeTo( + velocityInterpolator.interpolatedOutMsg + ) # Connect the PID controller to the joint for the measured position and velocity pidController.measuredPosInMsg.subscribeTo(joint.stateOutMsg) @@ -307,7 +319,7 @@ def lockJoint(panelID: str, angle: float): # create a stand-alone message with the given joint angle jointConstraintMsg = messaging.ScalarJointStateMsg() jointConstraintMsgPayload = messaging.ScalarJointStateMsgPayload() - jointConstraintMsgPayload.state = angle # rad + jointConstraintMsgPayload.state = angle # rad jointConstraintMsg.write(jointConstraintMsgPayload, 0, -1) # connect the message to the `constrainedStateInMsg` input of @@ -325,12 +337,12 @@ def unlockJoint(panelID: str): joint = joints[panelID] joint.constrainedStateInMsg.unsubscribe() - addJointController("10", initialAngle=np.pi/2, timeOffset=0) - addJointController("20", initialAngle=np.pi, timeOffset=0) - addJointController("1p", initialAngle=np.pi, timeOffset=operationTime) - addJointController("2p", initialAngle=np.pi, timeOffset=operationTime) - addJointController("1n", initialAngle=np.pi, timeOffset=2*operationTime) - addJointController("2n", initialAngle=np.pi, timeOffset=2*operationTime) + addJointController("10", initialAngle=np.pi / 2, timeOffset=0) + addJointController("20", initialAngle=np.pi, timeOffset=0) + addJointController("1p", initialAngle=np.pi, timeOffset=operationTime) + addJointController("2p", initialAngle=np.pi, timeOffset=operationTime) + addJointController("1n", initialAngle=np.pi, timeOffset=2 * operationTime) + addJointController("2n", initialAngle=np.pi, timeOffset=2 * operationTime) for panelID in ["1p", "1n", "2p", "2n"]: lockJoint(panelID, angle=np.pi) @@ -354,7 +366,7 @@ def unlockJoint(panelID: str): # Set the initial angles of the joints (stowed configuration) for panelID in ["10", "1p", "1n", "20", "2p", "2n"]: - initialAngle = np.pi/2 if panelID == "10" else np.pi # rad + initialAngle = np.pi / 2 if panelID == "10" else np.pi # rad joints[panelID].setPosition(initialAngle) # Configure the stop time of the simulation @@ -371,7 +383,7 @@ def unlockJoint(panelID: str): unlockJoint("2p") # Configure the stop time of the simulation - scSim.ConfigureStopTime(2*operationTime) + scSim.ConfigureStopTime(2 * operationTime) # Run the simulation with catchtime() as executeTime: @@ -384,7 +396,7 @@ def unlockJoint(panelID: str): unlockJoint("2n") # Configure the stop time of the simulation - scSim.ConfigureStopTime(3*operationTime) + scSim.ConfigureStopTime(3 * operationTime) # Run the simulation with catchtime() as executeTime: @@ -396,9 +408,10 @@ def unlockJoint(panelID: str): import matplotlib.pyplot as plt # Plot the desired and achieved joint angle for each panel - fig, axs = plt.subplots(ncols=2, nrows=3, sharex="all", sharey="all", squeeze=False) + fig, axs = plt.subplots( + ncols=2, nrows=3, sharex="all", sharey="all", squeeze=False + ) for ax, panelID in zip(axs.flat, ["10", "20", "1p", "2p", "1n", "2n"]): - desiredPosRecorder = desiredPosRecorderModels[panelID] measuredPosRecorder = measuredPosRecorderModels[panelID] @@ -407,7 +420,7 @@ def unlockJoint(panelID: str): np.rad2deg(desiredPosRecorder.state), "-", color=BLUE, - lw = 2, + lw=2, label="Desired Profile" if ax is axs.flat[0] else None, ) ax.plot( @@ -415,16 +428,18 @@ def unlockJoint(panelID: str): np.rad2deg(measuredPosRecorder.state), "--", color=RED, - lw = 2, + lw=2, label="Achieved" if ax is axs.flat[0] else None, ) for line in [1, 2]: - ax.axvline(line * operationTime * macros.NANO2MIN, color="k", linestyle=":") + ax.axvline( + line * operationTime * macros.NANO2MIN, color="k", linestyle=":" + ) ax.set_title(f"Panel {panelID}") ax.set_yticks([0, 45, 90, 135, 180]) - if ax in axs[:,0]: + if ax in axs[:, 0]: ax.set_ylabel("Angle [deg]") - if ax in axs[-1,:]: + if ax in axs[-1, :]: ax.set_xlabel("Time [min]") fig.suptitle("Panel angle") @@ -437,9 +452,8 @@ def unlockJoint(panelID: str): if visualize: speedUp = 120 qpos = np.squeeze(stateRecorder.qpos) - mujoco.visualize( - stateRecorder.times(), qpos, scene, speedUp - ) + mujoco.visualize(stateRecorder.times(), qpos, scene, speedUp) + if __name__ == "__main__": run(True, True) diff --git a/examples/mujoco/scenarioDeployPanels.py b/examples/mujoco/scenarioDeployPanels.py index a491aba4fe..43fe88941f 100644 --- a/examples/mujoco/scenarioDeployPanels.py +++ b/examples/mujoco/scenarioDeployPanels.py @@ -77,9 +77,9 @@ CURRENT_FOLDER = os.path.dirname(__file__) XML_PATH = f"{CURRENT_FOLDER}/sat_w_deployable_panels.xml" -JOINT_START_END = [(np.pi / 2, 0), (np.pi, 0), (np.pi, 0)] # rad -MAX_PROFILE_VEL = np.deg2rad(0.05) # rad -MAX_PROFILE_ACCEL = np.deg2rad(0.0001) # rad +JOINT_START_END = [(np.pi / 2, 0), (np.pi, 0), (np.pi, 0)] # rad +MAX_PROFILE_VEL = np.deg2rad(0.05) # rad +MAX_PROFILE_ACCEL = np.deg2rad(0.0001) # rad @contextmanager @@ -185,8 +185,8 @@ def run(initialSpin: bool = False, showPlots: bool = False, visualize: bool = Fa run on the simulation results. Defaults to False. """ - dt = 1 # s - tf = 80 * 60 # s + dt = 1 # s + tf = 80 * 60 # s # Create a simulation, process, and task as usual scSim = SimulationBaseClass.SimBaseClass() @@ -325,7 +325,7 @@ def run(initialSpin: bool = False, showPlots: bool = False, visualize: bool = Fa # and attitude of any free-floating bodies. Moreover, you can do # this at any time during the simulation. if initialSpin: - scene.getBody("hub").setAttitudeRate([0, 0.8, 0]) # rad/s + scene.getBody("hub").setAttitudeRate([0, 0.8, 0]) # rad/s # Configure the stop time of the simulation scSim.ConfigureStopTime(macros.sec2nano(tf)) @@ -342,7 +342,6 @@ def run(initialSpin: bool = False, showPlots: bool = False, visualize: bool = Fa # Plot the desired and achieved joint angle for each panel fig, axs = plt.subplots(2, 3) for ax, (side, i) in zip(axs.flat, panelIds): - desiredPosRecorder = desiredPosRecorderModels[(side, i)] measuredPosRecorder = measuredPosRecorderModels[(side, i)] @@ -425,7 +424,7 @@ def __init__(self, *args: Any): def registerStates(self, registerer: StatefulSysModel.DynParamRegisterer): self.integralErrorState = registerer.registerState(1, 1, "integralError") - self.integralErrorState.setState([[0]]) # explicitely zero initialize + self.integralErrorState.setState([[0]]) # explicitely zero initialize def UpdateState(self, CurrentSimNanos: int): """Computes the control command from the measured and desired @@ -436,7 +435,11 @@ def UpdateState(self, CurrentSimNanos: int): stateIntegralError = self.integralErrorState.getState()[0][0] # Compute the control output - control_output = self.K_p * stateError + self.K_d * stateDotError + self.K_i * stateIntegralError + control_output = ( + self.K_p * stateError + + self.K_d * stateDotError + + self.K_i * stateIntegralError + ) # Set the derivative of the integral error inner state self.integralErrorState.setDerivative([[stateError]]) diff --git a/examples/mujoco/scenarioReactionWheel.py b/examples/mujoco/scenarioReactionWheel.py index a9216e3561..2f2fc183eb 100644 --- a/examples/mujoco/scenarioReactionWheel.py +++ b/examples/mujoco/scenarioReactionWheel.py @@ -144,8 +144,8 @@ def run(showPlots: bool = False, visualize: bool = False): visualize (bool, optional): If True, the ``MJScene`` visualization tool is run on the simulation results. Defaults to False. """ - dt = 1 # s - tf = 60 # s + dt = 1 # s + tf = 60 # s # Create a simulation, process, and task as usual scSim = SimulationBaseClass.SimBaseClass() @@ -168,7 +168,7 @@ def run(showPlots: bool = False, visualize: bool = False): # actuators' are those actuators that apply a single scalar # input to the system. In this case, the actuator is a motor # that applies a torque to the 'wheel_1' joint. - torque = 2 # N*m + torque = 2 # N*m torqueMsg = messaging.SingleActuatorMsg() torqueMsg.write(messaging.SingleActuatorMsgPayload(input=torque)) diff --git a/examples/mujoco/scenarioSRPInPanels.py b/examples/mujoco/scenarioSRPInPanels.py index e2d2ae3a92..9b90dd0bd5 100644 --- a/examples/mujoco/scenarioSRPInPanels.py +++ b/examples/mujoco/scenarioSRPInPanels.py @@ -107,16 +107,16 @@ JOINT_PROFILES = ( [ - [macros.sec2nano(0), np.pi / 2], # nanoseconds, rad - [macros.sec2nano(60), 0], # nanoseconds, rad + [macros.sec2nano(0), np.pi / 2], # nanoseconds, rad + [macros.sec2nano(60), 0], # nanoseconds, rad ], [ - [macros.sec2nano(0), np.pi], # nanoseconds, rad - [macros.sec2nano(60), 0], # nanoseconds, rad + [macros.sec2nano(0), np.pi], # nanoseconds, rad + [macros.sec2nano(60), 0], # nanoseconds, rad ], [ - [macros.sec2nano(0), np.pi], # nanoseconds, rad - [macros.sec2nano(60), 0], # nanoseconds, rad + [macros.sec2nano(0), np.pi], # nanoseconds, rad + [macros.sec2nano(60), 0], # nanoseconds, rad ], ) @@ -196,7 +196,6 @@ def run(initialSpin: bool = False, showPlots: bool = False, visualize: bool = Fa srpModels = {} srpForceRecorders = {} for side, i in panelIds: - srpModel = SRPPanel(srpFactor=1) srpModel.ModelTag = f"SRP_{side}{i}" @@ -269,7 +268,7 @@ def run(initialSpin: bool = False, showPlots: bool = False, visualize: bool = Fa # We can only set the attitude rate of the hub because it's # a free-floating body. if initialSpin: - scene.getBody("hub").setAttitudeRate([0, 0.8, 0]) # rad/s + scene.getBody("hub").setAttitudeRate([0, 0.8, 0]) # rad/s # Run the simulation with catchtime() as executeTime: @@ -297,7 +296,6 @@ def run(initialSpin: bool = False, showPlots: bool = False, visualize: bool = Fa # Plot the SRP force on each panel fig, axs = plt.subplots(2, 3) for ax, (side, i) in zip(axs.flat, panelIds): - rec = srpForceRecorders[(side, i)] ax.plot(rec.times() * macros.NANO2SEC, rec.input) diff --git a/examples/mujoco/scenarioSimpleDocking.py b/examples/mujoco/scenarioSimpleDocking.py index 1c5a04086c..729986146f 100644 --- a/examples/mujoco/scenarioSimpleDocking.py +++ b/examples/mujoco/scenarioSimpleDocking.py @@ -118,8 +118,8 @@ def run(showPlots: bool = False, visualize: bool = False): scSim.InitializeSimulation() # Set the initial position of both CubeSats - scene.getBody("hub_1").setPosition([-1, 0, 0]) # m - scene.getBody("hub_2").setPosition([0, 0, -2]) # m + scene.getBody("hub_1").setPosition([-1, 0, 0]) # m + scene.getBody("hub_2").setPosition([0, 0, -2]) # m # Thrust so that the spacecraft get close to each other force = 1.0 diff --git a/examples/mujoco/scenarioUnbalancedThrusters.py b/examples/mujoco/scenarioUnbalancedThrusters.py index c88b290373..b8a3081586 100644 --- a/examples/mujoco/scenarioUnbalancedThrusters.py +++ b/examples/mujoco/scenarioUnbalancedThrusters.py @@ -91,8 +91,8 @@ def run(showPlots: bool = False, visualize: bool = False): visualize (bool, optional): If True, the ``MJScene`` visualization tool is run on the simulation results. Defaults to False. """ - dt = 1 # s - tf = 2.5 * 60 # s + dt = 1 # s + tf = 2.5 * 60 # s # Create a simulation, process, and task as usual scSim = SimulationBaseClass.SimBaseClass() @@ -108,14 +108,13 @@ def run(showPlots: bool = False, visualize: bool = False): integ = svIntegrators.svIntegratorRKF45(scene) scene.setIntegrator(integ) - thrust = 5 # N - mDot = -1 # kg/s + thrust = 5 # N + mDot = -1 # kg/s messages = [] massPropRecorders = [] for i in range(1, 5): - # Define a standalone ``SingleActuatorMsg`` used to # define the force that each thruster applies. # The thrusters defined in the XML ('tank_1_thrust', etc.) @@ -140,7 +139,9 @@ def run(showPlots: bool = False, visualize: bool = False): # is decreasing linearly with time, with the last tank # decreasing at twice the rate of the others. mDotMsg = messaging.SCMassPropsMsg() - mDotMsg.write(messaging.SCMassPropsMsgPayload(massSC=mDot if i < 4 else mDot * 2)) + mDotMsg.write( + messaging.SCMassPropsMsgPayload(massSC=mDot if i < 4 else mDot * 2) + ) bodyName = f"tank_{i}" body = scene.getBody(bodyName) @@ -173,11 +174,10 @@ def run(showPlots: bool = False, visualize: bool = False): scSim.ExecuteSimulation() if showPlots: - # Plot the mass of the tanks for i, (style, rec) in enumerate(zip(("-", "--", ":", "-"), massPropRecorders)): plt.plot( - rec.times(), np.squeeze(rec.massSC), style, label=f"Thruster {i+1}" + rec.times(), np.squeeze(rec.massSC), style, label=f"Thruster {i + 1}" ) plt.xlabel("Time [s]") plt.ylabel("Mass [kg]") diff --git a/examples/scenarioAerocapture.py b/examples/scenarioAerocapture.py index 2c27bac9fc..97d6faac31 100644 --- a/examples/scenarioAerocapture.py +++ b/examples/scenarioAerocapture.py @@ -81,13 +81,16 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ from Basilisk.simulation import dragDynamicEffector + # import simulation related support from Basilisk.simulation import spacecraft from Basilisk.simulation import tabularAtmosphere, simpleNav + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros @@ -124,18 +127,26 @@ def sph2rv(xxsph): hda = xxsph[5] NI = np.eye(3) - IE = np.array([[np.cos(lat) * np.cos(lon), -np.sin(lon), -np.sin(lat) * np.cos(lon)], - [np.cos(lat) * np.sin(lon), np.cos(lon), -np.sin(lat) * np.sin(lon)], - [np.sin(lat), 0, np.cos(lat)]]) - ES = np.array([[np.cos(gam), 0, np.sin(gam)], - [-np.sin(gam) * np.sin(hda), np.cos(hda), np.cos(gam) * np.sin(hda)], - [-np.sin(gam) * np.cos(hda), -np.sin(hda), np.cos(gam) * np.cos(hda)]]) - - e1_E = np.array([1,0,0]) + IE = np.array( + [ + [np.cos(lat) * np.cos(lon), -np.sin(lon), -np.sin(lat) * np.cos(lon)], + [np.cos(lat) * np.sin(lon), np.cos(lon), -np.sin(lat) * np.sin(lon)], + [np.sin(lat), 0, np.cos(lat)], + ] + ) + ES = np.array( + [ + [np.cos(gam), 0, np.sin(gam)], + [-np.sin(gam) * np.sin(hda), np.cos(hda), np.cos(gam) * np.sin(hda)], + [-np.sin(gam) * np.cos(hda), -np.sin(hda), np.cos(gam) * np.cos(hda)], + ] + ) + + e1_E = np.array([1, 0, 0]) rvec_N = (r * NI @ IE) @ e1_E - s3_S = np.array([0,0,1]) - uvec_N = u * ( NI @ IE @ ES) @ s3_S + s3_S = np.array([0, 0, 1]) + uvec_N = u * (NI @ IE @ ES) @ s3_S return rvec_N, uvec_N @@ -163,24 +174,24 @@ def run(show_plots, planetCase): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # Construct algorithm and associated C++ container # change module to tabAtmo - tabAtmo = tabularAtmosphere.TabularAtmosphere() # update with current values - tabAtmo.ModelTag = "tabularAtmosphere" # update python name of test module + tabAtmo = tabularAtmosphere.TabularAtmosphere() # update with current values + tabAtmo.ModelTag = "tabularAtmosphere" # update python name of test module atmoTaskName = "atmosphere" # define constants & load data - if planetCase == 'Earth': + if planetCase == "Earth": r_eq = 6378136.6 - dataFileName = bskPath + '/supportData/AtmosphereData/EarthGRAMNominal.txt' - altList, rhoList, tempList = readAtmTable(dataFileName, 'EarthGRAM') + dataFileName = bskPath + "/supportData/AtmosphereData/EarthGRAMNominal.txt" + altList, rhoList, tempList = readAtmTable(dataFileName, "EarthGRAM") else: r_eq = 3397.2 * 1000 - dataFileName = bskPath + '/supportData/AtmosphereData/MarsGRAMNominal.txt' - altList, rhoList, tempList = readAtmTable(dataFileName, 'MarsGRAM') + dataFileName = bskPath + "/supportData/AtmosphereData/MarsGRAMNominal.txt" + altList, rhoList, tempList = readAtmTable(dataFileName, "MarsGRAM") # assign constants & ref. data to module tabAtmo.planetRadius = r_eq @@ -191,7 +202,7 @@ def run(show_plots, planetCase): # Drag Effector projArea = 10.0 # Set drag area in m^2 dragCoeff = 2.2 # Set drag ceofficient - m_sc = 2530.0 # kg + m_sc = 2530.0 # kg dragEffector = dragDynamicEffector.DragDynamicEffector() dragEffector.ModelTag = "DragEff" @@ -199,7 +210,7 @@ def run(show_plots, planetCase): dragEffectorTaskName = "drag" dragEffector.coreParams.projectedArea = projArea dragEffector.coreParams.dragCoeff = dragCoeff - dragEffector.coreParams.comOffset = [1., 0., 0.] + dragEffector.coreParams.comOffset = [1.0, 0.0, 0.0] dynProcess.addTask(scSim.CreateNewTask(atmoTaskName, simulationTimeStep)) dynProcess.addTask(scSim.CreateNewTask(dragEffectorTaskName, simulationTimeStep)) @@ -240,25 +251,25 @@ def run(show_plots, planetCase): # attach gravity model to spacecraft gravFactory.addBodiesTo(scObject) - if planetCase == 'Earth': + if planetCase == "Earth": r = 6503 * 1000 u = 11.2 * 1000 gam = -5.15 * macros.D2R else: - r = (3397.2 + 125.) * 1000 + r = (3397.2 + 125.0) * 1000 u = 6 * 1000 gam = -10 * macros.D2R lon = 0 lat = 0 - hda = np.pi/2 - xxsph = [r,lon,lat,u,gam,hda] + hda = np.pi / 2 + xxsph = [r, lon, lat, u, gam, hda] rN, vN = sph2rv(xxsph) scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m - v_CN_N # set the simulation time - if planetCase == 'Earth': + if planetCase == "Earth": simulationTime = macros.sec2nano(300) else: simulationTime = macros.sec2nano(400) @@ -279,9 +290,12 @@ def run(show_plots, planetCase): scObject.hub.v_CN_NInit = vN # m - v_CN_N # if this scenario is to interface with the BSK Viz, uncomment the following line - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation # @@ -308,30 +322,31 @@ def run(show_plots, planetCase): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') - for idx in range(0,3): - plt.plot(dataLog.times()*macros.NANO2MIN, posData[:, idx]/1000., - color=unitTestSupport.getLineColor(idx,3), - label='$r_{BN,'+str(idx)+'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Inertial Position [km]') + ax.ticklabel_format(useOffset=False, style="plain") + for idx in range(0, 3): + plt.plot( + dataLog.times() * macros.NANO2MIN, + posData[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Inertial Position [km]") plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") smaData = [] engData = [] for idx in range(0, len(posData)): oeData = orbitalMotion.rv2elem(mu, posData[idx, 0:3], velData[idx, 0:3]) - smaData.append(oeData.a/1000.) - engData.append(-mu/(2*oeData.a)/1e6) # km^2/s^2 - plt.plot(dataLog.times()*macros.NANO2MIN, engData - , color='#aa0000' - ) - plt.xlabel('Time [min]') - plt.ylabel('Energy [km^2/s^2]') + smaData.append(oeData.a / 1000.0) + engData.append(-mu / (2 * oeData.a) / 1e6) # km^2/s^2 + plt.plot(dataLog.times() * macros.NANO2MIN, engData, color="#aa0000") + plt.xlabel("Time [min]") + plt.ylabel("Energy [km^2/s^2]") plt.grid() pltName = fileName + "2" + planetCase figureList[pltName] = plt.figure(2) @@ -342,19 +357,19 @@ def run(show_plots, planetCase): plt.figure(3) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - plt.plot(dataNewAtmoLog.times()*macros.NANO2MIN, densData) - plt.xlabel('Time [min]') - plt.ylabel('Density in kg/m^3') + ax.ticklabel_format(useOffset=False, style="sci") + plt.plot(dataNewAtmoLog.times() * macros.NANO2MIN, densData) + plt.xlabel("Time [min]") + plt.ylabel("Density in kg/m^3") pltName = fileName + "3" + planetCase figureList[pltName] = plt.figure(3) plt.figure(4) fig = plt.gcf() ax = fig.gca() - plt.plot(v/1e3, (r-r_eq)/1e3) - plt.xlabel('velocity [km/s]') - plt.ylabel('altitude [km]') + plt.plot(v / 1e3, (r - r_eq) / 1e3) + plt.xlabel("velocity [km/s]") + plt.ylabel("altitude [km]") plt.grid() pltName = fileName + "4" + planetCase figureList[pltName] = plt.figure(4) @@ -362,9 +377,9 @@ def run(show_plots, planetCase): plt.figure(5) fig = plt.gcf() ax = fig.gca() - plt.plot(dataLog.times()*macros.NANO2MIN, (r-r_eq)/1e3) - plt.xlabel('time [min]') - plt.ylabel('altitude [km]') + plt.plot(dataLog.times() * macros.NANO2MIN, (r - r_eq) / 1e3) + plt.xlabel("time [min]") + plt.ylabel("altitude [km]") plt.grid() pltName = fileName + "5" + planetCase figureList[pltName] = plt.figure(5) @@ -376,5 +391,7 @@ def run(show_plots, planetCase): return figureList # close the plots being saved off to avoid over-writing old and new figures -if __name__ == '__main__': - run(True, 'Mars') # planet arrival case, can be Earth or Mars + + +if __name__ == "__main__": + run(True, "Mars") # planet arrival case, can be Earth or Mars diff --git a/examples/scenarioAlbedo.py b/examples/scenarioAlbedo.py index 45c5df3a3e..7ca71b7d5f 100644 --- a/examples/scenarioAlbedo.py +++ b/examples/scenarioAlbedo.py @@ -137,27 +137,35 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.simulation import albedo from Basilisk.simulation import coarseSunSensor from Basilisk.simulation import eclipse + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros, simIncludeGravBody from Basilisk.utilities import orbitalMotion as om -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions bskPath = __path__[0] fileNameString = os.path.basename(os.path.splitext(__file__)[0]) -def run(show_plots, albedoData, multipleInstrument, multiplePlanet, useEclipse, simTimeStep): +def run( + show_plots, albedoData, multipleInstrument, multiplePlanet, useEclipse, simTimeStep +): """ At the end of the python script you can specify the following example parameters. @@ -180,25 +188,27 @@ def run(show_plots, albedoData, multipleInstrument, multiplePlanet, useEclipse, dynProcess = scSim.CreateNewProcess(simProcessName) # Create the dynamics task if simTimeStep is None: - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) else: simulationTimeStep = macros.sec2nano(simTimeStep) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # Create sun message - sunPositionMsg = messaging.SpicePlanetStateMsgPayload(PositionVector=[-om.AU * 1000., 0.0, 0.0]) + sunPositionMsg = messaging.SpicePlanetStateMsgPayload( + PositionVector=[-om.AU * 1000.0, 0.0, 0.0] + ) sunMsg = messaging.SpicePlanetStateMsg().write(sunPositionMsg) # Create planet message (earth) gravFactory = simIncludeGravBody.gravBodyFactory() # Create planet message (earth) - planetCase1 = 'earth' + planetCase1 = "earth" planet1 = gravFactory.createEarth() planet1.isCentralBody = True # ensure this is the central gravitational body req1 = planet1.radEquator planetPositionMsg1 = messaging.SpicePlanetStateMsgPayload( - PositionVector=[0., 0., 0.], + PositionVector=[0.0, 0.0, 0.0], PlanetName=planetCase1, J20002Pfix=np.identity(3), ) @@ -206,9 +216,9 @@ def run(show_plots, albedoData, multipleInstrument, multiplePlanet, useEclipse, pl1Msg = messaging.SpicePlanetStateMsg().write(planetPositionMsg1) if multiplePlanet: # Create planet message (moon) - planetCase2 = 'moon' + planetCase2 = "moon" planetPositionMsg2 = messaging.SpicePlanetStateMsgPayload( - PositionVector=[0., 384400. * 1000, 0.], + PositionVector=[0.0, 384400.0 * 1000, 0.0], PlanetName=planetCase2, J20002Pfix=np.identity(3), ) @@ -223,18 +233,24 @@ def run(show_plots, albedoData, multipleInstrument, multiplePlanet, useEclipse, scObject.ModelTag = "bsk-Sat" rLEO = req1 + 500 * 1000 # m # Define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) if multiplePlanet: # Set initial spacecraft states scObject.hub.r_CN_NInit = [[0.0], [rLEO], [0.0]] # m - r_CN_N scObject.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] # m - v_CN_N scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] # sigma_BN_B - scObject.hub.omega_BN_BInit = [[0.0], [0.0], [1. * macros.D2R]] # rad/s - omega_BN_B + scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [1.0 * macros.D2R], + ] # rad/s - omega_BN_B else: # Single planet case (earth) @@ -247,13 +263,17 @@ def run(show_plots, albedoData, multipleInstrument, multiplePlanet, useEclipse, rN, vN = om.elem2rv(planet1.mu, oe) # set the simulation time n = np.sqrt(planet1.mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n simulationTime = macros.sec2nano(0.5 * P) # Set initial spacecraft states scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m - v_CN_N scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] # sigma_BN_B - scObject.hub.omega_BN_BInit = [[0.0], [0.0], [.5 * macros.D2R]] # rad/s - omega_BN_B + scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [0.5 * macros.D2R], + ] # rad/s - omega_BN_B gravFactory.addBodiesTo(scObject) # Add spacecraft object to the simulation process @@ -278,9 +298,9 @@ def run(show_plots, albedoData, multipleInstrument, multiplePlanet, useEclipse, def setupCSS(CSS): CSS.stateInMsg.subscribeTo(scObject.scStateOutMsg) CSS.sunInMsg.subscribeTo(sunMsg) - CSS.fov = 80. * macros.D2R + CSS.fov = 80.0 * macros.D2R CSS.maxOutput = 1.0 - CSS.nHat_B = np.array([1., 0., 0.]) + CSS.nHat_B = np.array([1.0, 0.0, 0.0]) if useEclipse: CSS.sunEclipseInMsg.subscribeTo(eclipseObject.eclipseOutMsgs[0]) @@ -318,14 +338,14 @@ def setupCSS(CSS): CSS2 = coarseSunSensor.CoarseSunSensor() CSS2.ModelTag = "CSS2" setupCSS(CSS2) - CSS2.nHat_B = np.array([-1., 0., 0.]) + CSS2.nHat_B = np.array([-1.0, 0.0, 0.0]) albModule.addInstrumentConfig(CSS2.fov, CSS2.nHat_B, CSS2.r_PB_B) CSS2.albedoInMsg.subscribeTo(albModule.albOutMsgs[1]) # CSS-3 CSS3 = coarseSunSensor.CoarseSunSensor() CSS3.ModelTag = "CSS3" setupCSS(CSS3) - CSS3.nHat_B = np.array([0., -1., 0.]) + CSS3.nHat_B = np.array([0.0, -1.0, 0.0]) albModule.addInstrumentConfig(CSS3.fov, CSS3.nHat_B, CSS3.r_PB_B) CSS3.albedoInMsg.subscribeTo(albModule.albOutMsgs[2]) # @@ -363,19 +383,19 @@ def setupCSS(CSS): if multiplePlanet: velRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubVelocity) # Configure a simulation stop time and execute the simulation run - T1 = macros.sec2nano(500.) + T1 = macros.sec2nano(500.0) scSim.ConfigureStopTime(T1) scSim.ExecuteSimulation() # get the current spacecraft states vVt = unitTestSupport.EigenVector3d2np(velRef.getState()) - T2 = macros.sec2nano(1000.) + T2 = macros.sec2nano(1000.0) # Set second spacecraft states for decrease in altitude vVt = vVt + [0.0, 375300, 0.0] # m - v_CN_N velRef.setState(vVt) scSim.ConfigureStopTime(T1 + T2) scSim.ExecuteSimulation() # get the current spacecraft states - T3 = macros.sec2nano(500.) + T3 = macros.sec2nano(500.0) # Set second spacecraft states for decrease in altitude vVt = [0.0, 0.0, 0.0] # m - v_CN_N velRef.setState(vVt) @@ -414,61 +434,96 @@ def setupCSS(CSS): timeAxis = dataLog.times() if multipleInstrument: for idx in range(3): - plt.plot(timeAxis * macros.NANO2SEC, dataAlb[:, idx], - linewidth=2, alpha=0.7, color=unitTestSupport.getLineColor(idx, 3), - label='Albedo$_{' + str(idx) + '}$') + plt.plot( + timeAxis * macros.NANO2SEC, + dataAlb[:, idx], + linewidth=2, + alpha=0.7, + color=unitTestSupport.getLineColor(idx, 3), + label="Albedo$_{" + str(idx) + "}$", + ) if not multiplePlanet: - plt.plot(timeAxis * macros.NANO2SEC, dataCSS[:, idx], - '--', linewidth=1.5, color=unitTestSupport.getLineColor(idx, 3), - label='CSS$_{' + str(idx) + '}$') + plt.plot( + timeAxis * macros.NANO2SEC, + dataCSS[:, idx], + "--", + linewidth=1.5, + color=unitTestSupport.getLineColor(idx, 3), + label="CSS$_{" + str(idx) + "}$", + ) else: - plt.plot(timeAxis * macros.NANO2SEC, dataAlb, - linewidth=2, alpha=0.7, color=unitTestSupport.getLineColor(0, 2), - label='Alb$_{1}$') + plt.plot( + timeAxis * macros.NANO2SEC, + dataAlb, + linewidth=2, + alpha=0.7, + color=unitTestSupport.getLineColor(0, 2), + label="Alb$_{1}$", + ) if not multiplePlanet: - plt.plot(timeAxis * macros.NANO2SEC, dataCSS, - '--', linewidth=1.5, color=unitTestSupport.getLineColor(1, 2), - label='CSS$_{1}$') + plt.plot( + timeAxis * macros.NANO2SEC, + dataCSS, + "--", + linewidth=1.5, + color=unitTestSupport.getLineColor(1, 2), + label="CSS$_{1}$", + ) if multiplePlanet: - plt.legend(loc='upper center') + plt.legend(loc="upper center") else: - plt.legend(loc='upper right') - plt.xlabel('Time [s]') - plt.ylabel('Instrument\'s signal') + plt.legend(loc="upper right") + plt.xlabel("Time [s]") + plt.ylabel("Instrument's signal") figureList = {} - pltName = fileNameString + str(1) + str(int(albedoData)) + str(int(multipleInstrument)) + str( - int(multiplePlanet)) + str( - int(useEclipse)) + pltName = ( + fileNameString + + str(1) + + str(int(albedoData)) + + str(int(multipleInstrument)) + + str(int(multiplePlanet)) + + str(int(useEclipse)) + ) figureList[pltName] = plt.figure(1) if multiplePlanet: # Show radius of SC plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') - rData = np.linalg.norm(posData, axis=1) / 1000. - plt.plot(timeAxis * macros.NANO2SEC, rData, color='#aa0000') - plt.xlabel('Time [s]') - plt.ylabel('Radius [km]') - pltName = fileNameString + str(2) + str(int(albedoData)) + str(int(multipleInstrument)) + str( - int(multiplePlanet)) + str( - int(useEclipse)) + ax.ticklabel_format(useOffset=False, style="plain") + rData = np.linalg.norm(posData, axis=1) / 1000.0 + plt.plot(timeAxis * macros.NANO2SEC, rData, color="#aa0000") + plt.xlabel("Time [s]") + plt.ylabel("Radius [km]") + pltName = ( + fileNameString + + str(2) + + str(int(albedoData)) + + str(int(multipleInstrument)) + + str(int(multiplePlanet)) + + str(int(useEclipse)) + ) figureList[pltName] = plt.figure(2) if albedoData: - filePath = os.path.abspath(dataPath + '/' + fileName) - ALB1 = np.genfromtxt(filePath, delimiter=',') + filePath = os.path.abspath(dataPath + "/" + fileName) + ALB1 = np.genfromtxt(filePath, delimiter=",") # ALB coefficient figures fig = plt.figure(2) ax = fig.add_subplot(111) - ax.set_title('Earth Albedo Coefficients (All Sky)') - ax.set(xlabel='Longitude (deg)', ylabel='Latitude (deg)') - plt.imshow(ALB1, cmap='Reds', interpolation='none', extent=[-180, 180, 90, -90]) - plt.colorbar(orientation='vertical') + ax.set_title("Earth Albedo Coefficients (All Sky)") + ax.set(xlabel="Longitude (deg)", ylabel="Latitude (deg)") + plt.imshow(ALB1, cmap="Reds", interpolation="none", extent=[-180, 180, 90, -90]) + plt.colorbar(orientation="vertical") ax.set_ylim(ax.get_ylim()[::-1]) - pltName = fileNameString + str(2) + str(int(albedoData)) + str(int(multipleInstrument)) + str( - int(multiplePlanet)) + str( - int(useEclipse)) + pltName = ( + fileNameString + + str(2) + + str(int(albedoData)) + + str(int(multipleInstrument)) + + str(int(multiplePlanet)) + + str(int(useEclipse)) + ) figureList[pltName] = plt.figure(2) if show_plots: @@ -489,5 +544,5 @@ def setupCSS(CSS): True, # multipleInstrument False, # multiplePlanet True, # useEclipse - None # simTimeStep + None, # simTimeStep ) diff --git a/examples/scenarioAsteroidArrival.py b/examples/scenarioAsteroidArrival.py index fff5a0e350..5fe3621167 100755 --- a/examples/scenarioAsteroidArrival.py +++ b/examples/scenarioAsteroidArrival.py @@ -202,9 +202,27 @@ fileName = os.path.basename(os.path.splitext(__file__)[0]) -from Basilisk.utilities import (SimulationBaseClass, macros, simIncludeGravBody, vizSupport, unitTestSupport, orbitalMotion) -from Basilisk.simulation import spacecraft, extForceTorque, simpleNav, ephemerisConverter, planetEphemeris -from Basilisk.fswAlgorithms import mrpFeedback, attTrackingError, velocityPoint, locationPointing +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + simIncludeGravBody, + vizSupport, + unitTestSupport, + orbitalMotion, +) +from Basilisk.simulation import ( + spacecraft, + extForceTorque, + simpleNav, + ephemerisConverter, + planetEphemeris, +) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + velocityPoint, + locationPointing, +) from Basilisk.architecture import messaging, astroConstants try: @@ -247,15 +265,15 @@ def run(show_plots): # Setup celestial object ephemeris module for the asteroid gravBodyEphem = planetEphemeris.PlanetEphemeris() - gravBodyEphem.ModelTag = 'planetEphemeris' + gravBodyEphem.ModelTag = "planetEphemeris" scSim.AddModelToTask(simTaskName, gravBodyEphem) gravBodyEphem.setPlanetNames(planetEphemeris.StringVector(["bennu"])) # Specify orbital parameters of the asteroid timeInitString = "2011 January 1 0:00:00.0" diam = 2 * 245.03 # m - G = 6.67408 * (10 ** -11) # m^3 / kg*s^2 - massBennu = 7.329 * (10 ** 10) # kg + G = 6.67408 * (10**-11) # m^3 / kg*s^2 + massBennu = 7.329 * (10**10) # kg mu = G * massBennu # Bennu grav. parameter, m^3/s^2 oeAsteroid = planetEphemeris.ClassicElements() oeAsteroid.a = 1.1264 * astroConstants.AU * 1000 # m @@ -270,23 +288,25 @@ def run(show_plots): gravBodyEphem.rightAscension = planetEphemeris.DoubleVector([85.65 * macros.D2R]) gravBodyEphem.declination = planetEphemeris.DoubleVector([-60.17 * macros.D2R]) gravBodyEphem.lst0 = planetEphemeris.DoubleVector([0.0 * macros.D2R]) - gravBodyEphem.rotRate = planetEphemeris.DoubleVector([360 * macros.D2R / (4.296057 * 3600.)]) # rad/sec + gravBodyEphem.rotRate = planetEphemeris.DoubleVector( + [360 * macros.D2R / (4.296057 * 3600.0)] + ) # rad/sec # Set orbital radii about asteroid - r0 = diam/2.0 + 800 # capture orbit, meters - r1 = diam/2.0 + 600 # intermediate orbit, meters - r2 = diam/2.0 + 400 # final science orbit, meters - r3 = diam/2.0 + 200 # meters, very close fly-by, elliptic orbit + r0 = diam / 2.0 + 800 # capture orbit, meters + r1 = diam / 2.0 + 600 # intermediate orbit, meters + r2 = diam / 2.0 + 400 # final science orbit, meters + r3 = diam / 2.0 + 200 # meters, very close fly-by, elliptic orbit rP = r0 - rA = 3*rP + rA = 3 * rP # Set orbital periods - P0 = np.pi*2/np.sqrt(mu/(r0**3)) - P01 = np.pi*2/np.sqrt(mu/(((r0+r1)/2)**3)) - P1 = np.pi*2/np.sqrt(mu/(r1**3)) - P12 = np.pi*2/np.sqrt(mu/(((r1+r2)/2)**3)) - P2 = np.pi*2/np.sqrt(mu/(r2**3)) - P23 = np.pi*2/np.sqrt(mu/(((r2+r3)/2)**3)) + P0 = np.pi * 2 / np.sqrt(mu / (r0**3)) + P01 = np.pi * 2 / np.sqrt(mu / (((r0 + r1) / 2) ** 3)) + P1 = np.pi * 2 / np.sqrt(mu / (r1**3)) + P12 = np.pi * 2 / np.sqrt(mu / (((r1 + r2) / 2) ** 3)) + P2 = np.pi * 2 / np.sqrt(mu / (r2**3)) + P23 = np.pi * 2 / np.sqrt(mu / (((r2 + r3) / 2) ** 3)) # Create additional gravitational bodies gravFactory = simIncludeGravBody.gravBodyFactory() @@ -305,12 +325,15 @@ def run(show_plots): scSim.AddModelToTask(simTaskName, spiceObject) # Create the asteroid custom gravitational body - asteroid = gravFactory.createCustomGravObject("bennu", mu - , modelDictionaryKey="Bennu" - , radEquator=565. / 2.0 - ) - asteroid.isCentralBody = True # ensures the asteroid is the central gravitational body - asteroid.planetBodyInMsg.subscribeTo(gravBodyEphem.planetOutMsgs[0]) # connect asteroid ephem. to custom grav body + asteroid = gravFactory.createCustomGravObject( + "bennu", mu, modelDictionaryKey="Bennu", radEquator=565.0 / 2.0 + ) + asteroid.isCentralBody = ( + True # ensures the asteroid is the central gravitational body + ) + asteroid.planetBodyInMsg.subscribeTo( + gravBodyEphem.planetOutMsgs[0] + ) # connect asteroid ephem. to custom grav body # Create the spacecraft object scObject = spacecraft.Spacecraft() @@ -323,7 +346,7 @@ def run(show_plots): # Create an ephemeris converter to convert messages of type # 'SpicePlanetStateMsgPayload' to 'EphemerisMsgPayload' ephemObject = ephemerisConverter.EphemerisConverter() - ephemObject.ModelTag = 'EphemData' + ephemObject.ModelTag = "EphemData" ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[earthIdx]) ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[sunIdx]) # Recall the asteroid was not created with Spice. @@ -331,32 +354,38 @@ def run(show_plots): scSim.AddModelToTask(simTaskName, ephemObject) # Define the spacecraft inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # Define the initial spacecraft orbit about the asteroid oe = orbitalMotion.ClassicElements() - oe.a = (rP + rA)/2.0 + oe.a = (rP + rA) / 2.0 oe.e = 1 - (rP / oe.a) oe.i = 90.0 * macros.D2R oe.Omega = 180.0 * macros.D2R oe.omega = 347.8 * macros.D2R oe.f = -45.0 * macros.D2R - Ecc = np.arctan(np.tan(-oe.f/2)*np.sqrt((1-oe.e)/(1+oe.e)))*2 # eccentric anomaly - M = Ecc - oe.e*np.sin(Ecc) # mean anomaly - n = np.sqrt(mu/(oe.a**3)) - h = np.sqrt(mu*oe.a*(1-oe.e**2)) # specific angular momentum - vP = h/rP - V_SC_C_B = np.sqrt(mu / rP) # [m/s] (2) spacecraft circular parking speed relative to bennu. + Ecc = ( + np.arctan(np.tan(-oe.f / 2) * np.sqrt((1 - oe.e) / (1 + oe.e))) * 2 + ) # eccentric anomaly + M = Ecc - oe.e * np.sin(Ecc) # mean anomaly + n = np.sqrt(mu / (oe.a**3)) + h = np.sqrt(mu * oe.a * (1 - oe.e**2)) # specific angular momentum + vP = h / rP + V_SC_C_B = np.sqrt( + mu / rP + ) # [m/s] (2) spacecraft circular parking speed relative to bennu. Delta_V_Parking_Orbit = V_SC_C_B - vP # Setting initial position and velocity vectors using orbital elements r_N, v_N = orbitalMotion.elem2rv(mu, oe) - T1 = M/n # time until spacecraft reaches periapsis of arrival trajectory + T1 = M / n # time until spacecraft reaches periapsis of arrival trajectory # Initialize spacecraft states with the initialization variables scObject.hub.r_CN_NInit = r_N # [m] = r_BN_N @@ -405,7 +434,9 @@ def run(show_plots): sciencePointGuidance.celBodyInMsg.subscribeTo(ephemObject.ephemOutMsgs[asteroidIdx]) sciencePointGuidance.scTransInMsg.subscribeTo(sNavObject.transOutMsg) sciencePointGuidance.scAttInMsg.subscribeTo(sNavObject.attOutMsg) - sciencePointGuidance.pHat_B = cameraLocation # y-axis set for science-pointing sensor + sciencePointGuidance.pHat_B = ( + cameraLocation # y-axis set for science-pointing sensor + ) sciencePointGuidance.useBoresightRateDamping = 1 scSim.AddModelToTask(simTaskName, sciencePointGuidance) @@ -423,7 +454,9 @@ def run(show_plots): attError = attTrackingError.attTrackingError() attError.ModelTag = "attErrorInertial3D" scSim.AddModelToTask(simTaskName, attError) - attError.attRefInMsg.subscribeTo(sunPointGuidance.attRefOutMsg) # initial flight mode + attError.attRefInMsg.subscribeTo( + sunPointGuidance.attRefOutMsg + ) # initial flight mode attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg) # Create the FSW vehicle configuration message @@ -438,10 +471,10 @@ def run(show_plots): mrpControl.guidInMsg.subscribeTo(attError.attGuidOutMsg) mrpControl.vehConfigInMsg.subscribeTo(vcMsg) mrpControl.Ki = -1.0 # make value negative to turn off integral feedback - II = 900. - mrpControl.P = 2*II/(20*60) - mrpControl.K = mrpControl.P*mrpControl.P/II - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + II = 900.0 + mrpControl.P = 2 * II / (20 * 60) + mrpControl.K = mrpControl.P * mrpControl.P / II + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # Connect the torque command to external torque effector extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) @@ -461,15 +494,17 @@ def run(show_plots): genericSensor.fieldOfView.push_back(10.0 * macros.D2R) genericSensor.normalVector = cameraLocation genericSensor.size = 10 - genericSensor.color = vizInterface.IntVector(vizSupport.toRGBA255("white", alpha=0.1)) + genericSensor.color = vizInterface.IntVector( + vizSupport.toRGBA255("white", alpha=0.1) + ) genericSensor.label = "scienceCamera" genericSensor.genericSensorCmd = 1 # Set up the antenna visualization for transmission to Earth transceiverHUD = vizInterface.Transceiver() - transceiverHUD.r_SB_B = [0., 0., 1.38] + transceiverHUD.r_SB_B = [0.0, 0.0, 1.38] transceiverHUD.fieldOfView = 40.0 * macros.D2R - transceiverHUD.normalVector = [0., 0., 1.] + transceiverHUD.normalVector = [0.0, 0.0, 1.0] transceiverHUD.color = vizInterface.IntVector(vizSupport.toRGBA255("cyan")) transceiverHUD.label = "antenna" transceiverHUD.animationSpeed = 1 @@ -499,9 +534,12 @@ def run(show_plots): scData.thrInfo = vizInterface.ThrClusterVector([thrInfo]) # Create the Vizard visualization file and set parameters - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) viz.epochInMsg.subscribeTo(gravFactory.epochMsg) viz.settings.showCelestialBodyLabels = 1 @@ -515,10 +553,15 @@ def run(show_plots): viz.settings.keyboardAngularRate = np.deg2rad(0.5) # Create the science mode camera - vizSupport.createStandardCamera(viz, setMode=1, spacecraftName=scObject.ModelTag, - fieldOfView=10 * macros.D2R, - displayName="10˚ FOV Camera", - pointingVector_B=[0, 1, 0], position_B=cameraLocation) + vizSupport.createStandardCamera( + viz, + setMode=1, + spacecraftName=scObject.ModelTag, + fieldOfView=10 * macros.D2R, + displayName="10˚ FOV Camera", + pointingVector_B=[0, 1, 0], + position_B=cameraLocation, + ) # Note: After running the enableUnityVisualization() method, we need to clear the # vizInterface spacecraft data container, scData, and push our custom copy to it. @@ -575,12 +618,16 @@ def runDvBurn(simTime, burnSign, planetMsg): transceiverHUD.transceiverState = 0 # antenna off genericSensor.isHidden = 1 if burnSign > 0: - attError.sigma_R0R = [np.tan((np.pi/2)/4), 0, 0] + attError.sigma_R0R = [np.tan((np.pi / 2) / 4), 0, 0] else: attError.sigma_R0R = [-np.tan((np.pi / 2) / 4), 0, 0] minTime = 40 * 60 if simTime < minTime: - print("ERROR: runPosDvBurn must have simTime larger than " + str(minTime) + " min") + print( + "ERROR: runPosDvBurn must have simTime larger than " + + str(minTime) + + " min" + ) exit(1) else: simulationTime += macros.sec2nano(minTime) @@ -595,7 +642,7 @@ def runDvBurn(simTime, burnSign, planetMsg): simulationTime = 0 np.set_printoptions(precision=16) - burnTime = 200*60 + burnTime = 200 * 60 # Run thruster burn for arrival to the capture orbit with thrusters on runDvBurn(T1, -1, velAsteroidGuidance.attRefOutMsg) @@ -611,9 +658,9 @@ def runDvBurn(simTime, burnSign, planetMsg): # Travel in a circular orbit at r0, incorporating several attitude pointing modes runDvBurn(burnTime, -1, velAsteroidGuidance.attRefOutMsg) - runSensorSciencePointing(P0/3.-burnTime) - runPanelSunPointing(P0/3.) - runAntennaEarthPointing(P0/3. - burnTime) + runSensorSciencePointing(P0 / 3.0 - burnTime) + runPanelSunPointing(P0 / 3.0) + runAntennaEarthPointing(P0 / 3.0 - burnTime) runDvBurn(burnTime, -1, velAsteroidGuidance.attRefOutMsg) # Get access to dynManager translational states for future access to the states @@ -626,7 +673,7 @@ def runDvBurn(simTime, burnSign, planetMsg): # Conduct the first burn of a Hohmann transfer from r0 to r1 rData = np.linalg.norm(rN) vData = np.linalg.norm(vN) - at = (rData + r1) * .5 + at = (rData + r1) * 0.5 v0p = np.sqrt((2 * mu / rData) - (mu / at)) vHat = vN / vData vVt = vN + vHat * (v0p - vData) @@ -635,7 +682,7 @@ def runDvBurn(simTime, burnSign, planetMsg): # Run thruster burn mode along with sun-pointing during the transfer orbit runDvBurn(burnTime, -1, velAsteroidGuidance.attRefOutMsg) - runPanelSunPointing(P01/2. - burnTime*2) + runPanelSunPointing(P01 / 2.0 - burnTime * 2) runDvBurn(burnTime, -1, velAsteroidGuidance.attRefOutMsg) # Retrieve the latest relative position and velocity components @@ -653,8 +700,8 @@ def runDvBurn(simTime, burnSign, planetMsg): # Run thruster burn visualization along with attitude pointing modes runDvBurn(burnTime, -1, velAsteroidGuidance.attRefOutMsg) - runSensorSciencePointing(P1/4-burnTime) - runAntennaEarthPointing(P1/4-burnTime) + runSensorSciencePointing(P1 / 4 - burnTime) + runAntennaEarthPointing(P1 / 4 - burnTime) runDvBurn(burnTime, -1, velAsteroidGuidance.attRefOutMsg) # Retrieve the latest relative position and velocity components @@ -664,7 +711,7 @@ def runDvBurn(simTime, burnSign, planetMsg): # Conduct a second Hohmann transfer from r1 to r2, initial burn rData = np.linalg.norm(rN) vData = np.linalg.norm(vN) - at = (rData + r2) * .5 + at = (rData + r2) * 0.5 v2p = np.sqrt((2 * mu / rData) - (mu / at)) vHat = vN / vData vVt = vN + vHat * (v2p - vData) @@ -673,7 +720,7 @@ def runDvBurn(simTime, burnSign, planetMsg): # Run thruster burn section with science pointing mode runDvBurn(burnTime, -1, velAsteroidGuidance.attRefOutMsg) - runSensorSciencePointing(P12/2-burnTime*2) + runSensorSciencePointing(P12 / 2 - burnTime * 2) runDvBurn(burnTime, -1, velAsteroidGuidance.attRefOutMsg) # Retrieve the latest relative position and velocity components @@ -691,7 +738,7 @@ def runDvBurn(simTime, burnSign, planetMsg): # Run thruster visualization with science pointing mode runDvBurn(burnTime, -1, velAsteroidGuidance.attRefOutMsg) - runSensorSciencePointing(P2-burnTime) + runSensorSciencePointing(P2 - burnTime) # Retrieve the latest relative position and velocity components rN = scRec.r_BN_N[-1] - astRec.PositionVector[-1] @@ -700,7 +747,7 @@ def runDvBurn(simTime, burnSign, planetMsg): # Conduct a third Hohmann transfer from r2 to r3, initial burn rData = np.linalg.norm(rN) vData = np.linalg.norm(vN) - at = (rData + r3) * .5 + at = (rData + r3) * 0.5 v3p = np.sqrt((2 * mu / rData) - (mu / at)) vHat = vN / vData vVt = vN + vHat * (v3p - vData) @@ -709,7 +756,7 @@ def runDvBurn(simTime, burnSign, planetMsg): # Run thruster visualization with science-pointing mode runDvBurn(burnTime, -1, velAsteroidGuidance.attRefOutMsg) - runSensorSciencePointing(3*P23-burnTime) + runSensorSciencePointing(3 * P23 - burnTime) # Retrieve logged spacecraft position relative to asteroid posData1 = scRec.r_BN_N # inertial pos. wrt. Sun @@ -741,27 +788,38 @@ def plotOrbits(timeAxis, posData1, posData2, rP, diam): # Draw the planet fig = plt.gcf() ax = fig.gca() - ax.set_aspect('equal') - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - ax.get_xaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - planetColor = '#008800' - planetRadius = .5*(diam) # m + ax.set_aspect("equal") + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + ax.get_xaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + planetColor = "#008800" + planetRadius = 0.5 * (diam) # m ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # Draw the actual orbit from pulled data (DataRec) - plt.plot(posData2[:, 0], posData2[:, 2], color='orangered', - label='Simulated Flight') - plt.xlabel('X Distance, Inertial [m]') - plt.ylabel('Z Distance, Inertial [m]') + plt.plot( + posData2[:, 0], posData2[:, 2], color="orangered", label="Simulated Flight" + ) + plt.xlabel("X Distance, Inertial [m]") + plt.ylabel("Z Distance, Inertial [m]") # Draw desired parking orbit fData = np.linspace(0, 2 * np.pi, 100) rData = [] for indx in range(0, len(fData)): rData.append(rP) - plt.plot(rData* np.cos(fData), rData * np.sin(fData), '--', color='#555555', label='Desired Circ.Capture Orbit') - plt.legend(loc='upper right') + plt.plot( + rData * np.cos(fData), + rData * np.sin(fData), + "--", + color="#555555", + label="Desired Circ.Capture Orbit", + ) + plt.legend(loc="upper right") plt.grid() pltName = fileName + "1" figureList[pltName] = plt.figure(1) diff --git a/examples/scenarioAttGuideHyperbolic.py b/examples/scenarioAttGuideHyperbolic.py index 274d1eac61..9b88ea9b43 100755 --- a/examples/scenarioAttGuideHyperbolic.py +++ b/examples/scenarioAttGuideHyperbolic.py @@ -120,18 +120,26 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import mrpFeedback, attTrackingError, velocityPoint from Basilisk.simulation import extForceTorque, simpleNav, spacecraft -from Basilisk.utilities import SimulationBaseClass, macros, orbitalMotion, simIncludeGravBody, unitTestSupport +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, +) from Basilisk.utilities import vizSupport bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) + def plot_track_error_norm(timeLineSet, dataSigmaBR): """Plot the attitude tracking error norm value.""" plt.figure(1) @@ -139,45 +147,55 @@ def plot_track_error_norm(timeLineSet, dataSigmaBR): ax = fig.gca() vectorData = dataSigmaBR sNorm = np.array([np.linalg.norm(v) for v in vectorData]) - plt.plot(timeLineSet, sNorm, - color=unitTestSupport.getLineColor(1, 3), - ) - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error Norm $|\sigma_{B/R}|$') - ax.set_yscale('log') + plt.plot( + timeLineSet, + sNorm, + color=unitTestSupport.getLineColor(1, 3), + ) + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error Norm $|\sigma_{B/R}|$") + ax.set_yscale("log") + def plot_control_torque(timeLineSet, dataLr): """Plot the attiude control torque effort.""" plt.figure(2) for idx in range(3): - plt.plot(timeLineSet, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Control Torque $L_r$ [Nm]') + plt.plot( + timeLineSet, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Control Torque $L_r$ [Nm]") + def plot_rate_error(timeLineSet, dataOmegaBR): """Plot the body angular velocity tracking errors.""" plt.figure(3) for idx in range(3): - plt.plot(timeLineSet, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeLineSet, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") def plot_orbit(oe, mu, planet_radius, dataPos, dataVel): """Plot the spacecraft orbit trajectory.""" # draw orbit in perifocal frame p = oe.a * (1 - oe.e * oe.e) - plt.figure(4, figsize=tuple(np.array((1.0, 1.)) * 4.75), dpi=100) + plt.figure(4, figsize=tuple(np.array((1.0, 1.0)) * 4.75), dpi=100) # draw the planet fig = plt.gcf() ax = fig.gca() - planetColor = '#008800' + planetColor = "#008800" # planet = gravFactory.createEarth() planetRadius = planet_radius / 1000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) @@ -188,21 +206,32 @@ def plot_orbit(oe, mu, planet_radius, dataPos, dataVel): oeData = orbitalMotion.rv2elem(mu, dataPos[idx], dataVel[idx]) 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, label='Simulated Flight') + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + color="#aa0000", + linewidth=3.0, + label="Simulated Flight", + ) plt.axis(np.array([-1, 1, -1, 1]) * 1.25 * np.amax(rData) / 1000) # draw the full osculating orbit from the initial conditions - tempAngle = (1. / 2.) * (np.pi - 2 * np.arcsin(1 / oe.e)) * 1.01 + tempAngle = (1.0 / 2.0) * (np.pi - 2 * np.arcsin(1 / oe.e)) * 1.01 fData = np.linspace(np.pi - tempAngle, -np.pi + tempAngle, 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', label='Orbit Track') - plt.xlabel('$i_e$ Cord. [km]') - plt.ylabel('$i_p$ Cord. [km]') - plt.legend(loc='lower left') + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + "--", + color="#555555", + label="Orbit Track", + ) + plt.xlabel("$i_e$ Cord. [km]") + plt.ylabel("$i_p$ Cord. [km]") + plt.legend(loc="lower left") plt.grid() @@ -224,7 +253,7 @@ def run(show_plots, useAltBodyFrame): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.sec2nano(750.) + simulationTime = macros.sec2nano(750.0) # # create the simulation process @@ -232,7 +261,7 @@ def run(show_plots, useAltBodyFrame): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(1.) + simulationTimeStep = macros.sec2nano(1.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -243,11 +272,13 @@ def run(show_plots, useAltBodyFrame): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -335,7 +366,7 @@ def run(show_plots, useAltBodyFrame): mrpControl.K = 3.5 mrpControl.Ki = -1.0 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # connect torque command to external torque effector extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) @@ -344,7 +375,9 @@ def run(show_plots, useAltBodyFrame): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) mrpLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) attErrLog = attError.attGuidOutMsg.recorder(samplingTime) snAttLog = sNavObject.attOutMsg.recorder(samplingTime) @@ -358,9 +391,12 @@ def run(show_plots, useAltBodyFrame): # # if this scenario is to interface with the BSK Viz, uncomment the following line - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation @@ -423,5 +459,5 @@ def run(show_plots, useAltBodyFrame): if __name__ == "__main__": run( True, # show_plots - False # useAltBodyFrame - ) + False, # useAltBodyFrame + ) diff --git a/examples/scenarioAttLocPoint.py b/examples/scenarioAttLocPoint.py index 1a89ebf300..2e6976184e 100755 --- a/examples/scenarioAttLocPoint.py +++ b/examples/scenarioAttLocPoint.py @@ -67,26 +67,34 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import locationPointing + # import FSW Algorithm related support from Basilisk.fswAlgorithms import mrpFeedback from Basilisk.simulation import extForceTorque from Basilisk.simulation import groundLocation from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.architecture import astroConstants + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -102,35 +110,44 @@ def plot_attitude_error(timeLineSet, dataSigmaBR): ax = fig.gca() vectorData = dataSigmaBR sNorm = np.array([np.linalg.norm(v) for v in vectorData]) - plt.plot(timeLineSet, sNorm, - color=unitTestSupport.getLineColor(1, 3), - ) - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error Norm $|\sigma_{B/R}|$') - ax.set_yscale('log') + plt.plot( + timeLineSet, + sNorm, + color=unitTestSupport.getLineColor(1, 3), + ) + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error Norm $|\sigma_{B/R}|$") + ax.set_yscale("log") + def plot_control_torque(timeLineSet, dataLr): """Plot the control torque response.""" plt.figure(2) for idx in range(3): - plt.plot(timeLineSet, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Control Torque $L_r$ [Nm]') + plt.plot( + timeLineSet, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Control Torque $L_r$ [Nm]") def plot_rate_error(timeLineSet, dataOmegaBR): """Plot the body angular velocity tracking error.""" plt.figure(3) for idx in range(3): - plt.plot(timeLineSet, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeLineSet, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") return @@ -151,7 +168,7 @@ def run(show_plots): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(20.) + simulationTime = macros.min2nano(20.0) # # create the simulation process @@ -170,11 +187,13 @@ def run(show_plots): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -196,7 +215,7 @@ def run(show_plots): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - oe.a = (6378 + 600)*1000. # meters + oe.a = (6378 + 600) * 1000.0 # meters oe.e = 0.1 oe.i = 63.3 * macros.D2R oe.Omega = 88.2 * macros.D2R @@ -228,9 +247,9 @@ def run(show_plots): # Create the ground location groundStation = groundLocation.GroundLocation() groundStation.ModelTag = "BoulderGroundStation" - groundStation.planetRadius = astroConstants.REQ_EARTH*1e3 # meters + groundStation.planetRadius = astroConstants.REQ_EARTH * 1e3 # meters groundStation.specifyLocation(np.radians(40.009971), np.radians(-105.243895), 1624) - groundStation.minimumElevation = np.radians(10.) + groundStation.minimumElevation = np.radians(10.0) groundStation.maximumRange = 1e9 # meters groundStation.addSpacecraftToModel(scObject.scStateOutMsg) scSim.AddModelToTask(simTaskName, groundStation) @@ -260,7 +279,7 @@ def run(show_plots): mrpControl.K = 5.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # connect torque command to external torque effector extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) @@ -269,7 +288,9 @@ def run(show_plots): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) mrpLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) attErrLog = locPoint.attGuidOutMsg.recorder(samplingTime) snAttLog = sNavObject.attOutMsg.recorder(samplingTime) @@ -291,16 +312,21 @@ def run(show_plots): # if this scenario is to interface with the BSK Viz, uncomment the following lines if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) - vizSupport.addLocation(viz, stationName="Boulder Station" - , parentBodyName=earth.displayName - , r_GP_P=unitTestSupport.EigenVector3d2list(groundStation.r_LP_P_Init) - , fieldOfView=np.radians(160.) - , color='pink' - , range=2000.0*1000 # meters - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) + vizSupport.addLocation( + viz, + stationName="Boulder Station", + parentBodyName=earth.displayName, + r_GP_P=unitTestSupport.EigenVector3d2list(groundStation.r_LP_P_Init), + fieldOfView=np.radians(160.0), + color="pink", + range=2000.0 * 1000, # meters + ) viz.settings.spacecraftSizeMultiplier = 1.5 viz.settings.showLocationCommLines = 1 viz.settings.showLocationCones = 1 diff --git a/examples/scenarioAttitudeConstrainedManeuver.py b/examples/scenarioAttitudeConstrainedManeuver.py index d1884a5b39..44e3852f26 100644 --- a/examples/scenarioAttitudeConstrainedManeuver.py +++ b/examples/scenarioAttitudeConstrainedManeuver.py @@ -98,19 +98,33 @@ import numpy as np from Basilisk import __path__ from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, constrainedAttitudeManeuver, rwMotorTorque) -from Basilisk.simulation import (reactionWheelStateEffector, simpleNav, spacecraft, boreAngCalc) -from Basilisk.utilities import (SimulationBaseClass, macros, - orbitalMotion, simIncludeGravBody, - simIncludeRW, unitTestSupport, vizSupport) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + constrainedAttitudeManeuver, + rwMotorTorque, +) +from Basilisk.simulation import ( + reactionWheelStateEffector, + simpleNav, + spacecraft, + boreAngCalc, +) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + simIncludeRW, + unitTestSupport, + vizSupport, +) bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) - def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCase): - # Create simulation variable names simTaskName = "simTask" simProcessName = "simProcess" @@ -143,8 +157,8 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas gravFactory = simIncludeGravBody.gravBodyFactory() # Next a series of gravitational bodies are included - gravBodies = gravFactory.createBodies('earth', 'sun') - planet = gravBodies['earth'] + gravBodies = gravFactory.createBodies("earth", "sun") + planet = gravBodies["earth"] planet.isCentralBody = True mu = planet.mu @@ -158,7 +172,7 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas spiceObject = gravFactory.createSpiceInterface(time=timeInitString, epochInMsg=True) # Earth is gravity center - spiceObject.zeroBase = 'Earth' + spiceObject.zeroBase = "Earth" # The SPICE object is added to the simulation task list. scSim.AddModelToTask(simTaskName, gravFactory.spiceObject, 2) @@ -168,7 +182,7 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - oe.a = 7000. * 1000 # meters + oe.a = 7000.0 * 1000 # meters oe.e = 0.0001 oe.i = 33.3 * macros.D2R oe.Omega = 148.2 * macros.D2R @@ -177,23 +191,29 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas rN, vN = orbitalMotion.elem2rv(mu, oe) # sets of initial attitudes that yield the desired constraint violations (attitudeSetCase) - sigma_BN_start = [ [0.522, -0.065, 0.539], # to violate one keepIn only - [0.314, -0.251, 0.228], # to violate two keepIn and not keepOut - [-0.378, 0.119, -0.176], # to violate keepOut and both keepIn - [-0.412, 0.044, -0.264] ] # to violate keepOut only + sigma_BN_start = [ + [0.522, -0.065, 0.539], # to violate one keepIn only + [0.314, -0.251, 0.228], # to violate two keepIn and not keepOut + [-0.378, 0.119, -0.176], # to violate keepOut and both keepIn + [-0.412, 0.044, -0.264], + ] # to violate keepOut only # To set the spacecraft initial conditions, the following initial position and velocity variables are set: scObject.hub.r_CN_NInit = rN # m - r_BN_N scObject.hub.v_CN_NInit = vN # m/s - v_BN_N - scObject.hub.sigma_BNInit = sigma_BN_start[attitudeSetCase] # MRP set to customize initial inertial attitude - scObject.hub.omega_BN_BInit = [[0.], [0.], [0.]] # rad/s - omega_CN_B + scObject.hub.sigma_BNInit = sigma_BN_start[ + attitudeSetCase + ] # MRP set to customize initial inertial attitude + scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_CN_B # define the simulation inertia - I = [0.02 / 3, 0., 0., - 0., 0.1256 / 3, 0., - 0., 0., 0.1256 / 3] + I = [0.02 / 3, 0.0, 0.0, 0.0, 0.1256 / 3, 0.0, 0.0, 0.0, 0.1256 / 3] scObject.hub.mHub = 4.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # @@ -208,21 +228,33 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments maxMomentum = 0.01 maxSpeed = 6000 * macros.RPM - RW1 = rwFactory.create('custom', [1, 0, 0], Omega=0. # RPM - , Omega_max=maxSpeed - , maxMomentum=maxMomentum - , u_max=0.001 - , RWModel=varRWModel) - RW2 = rwFactory.create('custom', [0, 1, 0], Omega=0. # RPM - , Omega_max=maxSpeed - , maxMomentum=maxMomentum - , u_max=0.001 - , RWModel=varRWModel) - RW3 = rwFactory.create('custom', [0, 0, 1], Omega=0. # RPM - , Omega_max=maxSpeed - , maxMomentum=maxMomentum - , u_max=0.001 - , RWModel=varRWModel) + RW1 = rwFactory.create( + "custom", + [1, 0, 0], + Omega=0.0, # RPM + Omega_max=maxSpeed, + maxMomentum=maxMomentum, + u_max=0.001, + RWModel=varRWModel, + ) + RW2 = rwFactory.create( + "custom", + [0, 1, 0], + Omega=0.0, # RPM + Omega_max=maxSpeed, + maxMomentum=maxMomentum, + u_max=0.001, + RWModel=varRWModel, + ) + RW3 = rwFactory.create( + "custom", + [0, 0, 1], + Omega=0.0, # RPM + Omega_max=maxSpeed, + maxMomentum=maxMomentum, + u_max=0.001, + RWModel=varRWModel, + ) numRW = rwFactory.getNumOfDevices() @@ -244,10 +276,12 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas # # sets of initial attitudes that yield the desired constraint violations (attitudeSetCase) - sigma_BN_target = [ [0.342, 0.223, -0.432], # to violate one keepIn only - [0.326, -0.206, -0.823], # to violate two keepIn and not keepOut - [0.350, 0.220, -0.440], # to violate keepOut and both keepIn - [0.350, 0.220, -0.440] ] # to violate keepOut only + sigma_BN_target = [ + [0.342, 0.223, -0.432], # to violate one keepIn only + [0.326, -0.206, -0.823], # to violate two keepIn and not keepOut + [0.350, 0.220, -0.440], # to violate keepOut and both keepIn + [0.350, 0.220, -0.440], + ] # to violate keepOut only # setup readManeuver guidance module CAM = constrainedAttitudeManeuver.ConstrainedAttitudeManeuver(8) @@ -257,8 +291,8 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas CAM.avgOmega = 0.04 CAM.BSplineType = 0 CAM.costFcnType = 1 - CAM.appendKeepOutDirection([1,0,0], starTrackerFov*macros.D2R) - CAM.appendKeepInDirection([0,1,0], sunSensorFov*macros.D2R) + CAM.appendKeepOutDirection([1, 0, 0], starTrackerFov * macros.D2R) + CAM.appendKeepInDirection([0, 1, 0], sunSensorFov * macros.D2R) scSim.AddModelToTask(simTaskName, CAM) # setup the attitude tracking error evaluation module @@ -273,9 +307,9 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas decayTime = 10.0 xi = 1.0 mrpControl.Ki = -1 # make value negative to turn off integral feedback - mrpControl.P = 3*np.max(I)/decayTime - mrpControl.K = (mrpControl.P/xi)*(mrpControl.P/xi)/np.max(I) - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.P = 3 * np.max(I) / decayTime + mrpControl.K = (mrpControl.P / xi) * (mrpControl.P / xi) / np.max(I) + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # add module that maps the Lr control torque into the RW motor torques rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() @@ -289,26 +323,28 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas # Boresight vector modules. stBACObject = boreAngCalc.BoreAngCalc() stBACObject.ModelTag = "starTrackerBoresight" - stBACObject.boreVec_B = [1., 0., 0.] # boresight in body frame + stBACObject.boreVec_B = [1.0, 0.0, 0.0] # boresight in body frame scSim.AddModelToTask(simTaskName, stBACObject) ssyBACObject = boreAngCalc.BoreAngCalc() ssyBACObject.ModelTag = "SunSensorBoresight" - ssyBACObject.boreVec_B = [0., 1., 0.] # boresight in body frame + ssyBACObject.boreVec_B = [0.0, 1.0, 0.0] # boresight in body frame scSim.AddModelToTask(simTaskName, ssyBACObject) if use2SunSensors: - CAM.appendKeepInDirection([0,0,1], sunSensorFov*macros.D2R) + CAM.appendKeepInDirection([0, 0, 1], sunSensorFov * macros.D2R) sszBACObject = boreAngCalc.BoreAngCalc() sszBACObject.ModelTag = "SunSensorBoresight" - sszBACObject.boreVec_B = [0., 0., 1.] # boresight in body frame + sszBACObject.boreVec_B = [0.0, 0.0, 1.0] # boresight in body frame scSim.AddModelToTask(simTaskName, sszBACObject) # # Setup data logging before the simulation is initialized # numDataPoints = 500 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) sNavRec = sNavObject.attOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, sNavRec) CAMRec = CAM.attRefOutMsg.recorder(samplingTime) @@ -378,19 +414,43 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas # Vizard Visualization Option # --------------------------- - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - # saveFile=__file__ - ) - vizSupport.createConeInOut(viz, toBodyName='sun_planet_data', coneColor = 'r', - normalVector_B=[1, 0, 0], incidenceAngle=starTrackerFov*macros.D2R, isKeepIn=False, - coneHeight=10.0, coneName='sunKeepOut') - vizSupport.createConeInOut(viz, toBodyName='sun_planet_data', coneColor = 'g', - normalVector_B=[0, 1, 0], incidenceAngle=sunSensorFov*macros.D2R, isKeepIn=True, - coneHeight=10.0, coneName='sunKeepIn') + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # saveFile=__file__ + ) + vizSupport.createConeInOut( + viz, + toBodyName="sun_planet_data", + coneColor="r", + normalVector_B=[1, 0, 0], + incidenceAngle=starTrackerFov * macros.D2R, + isKeepIn=False, + coneHeight=10.0, + coneName="sunKeepOut", + ) + vizSupport.createConeInOut( + viz, + toBodyName="sun_planet_data", + coneColor="g", + normalVector_B=[0, 1, 0], + incidenceAngle=sunSensorFov * macros.D2R, + isKeepIn=True, + coneHeight=10.0, + coneName="sunKeepIn", + ) if use2SunSensors: - vizSupport.createConeInOut(viz, toBodyName='sun_planet_data', coneColor = 'b', - normalVector_B=[0, 0, 1], incidenceAngle=sunSensorFov*macros.D2R, isKeepIn=True, - coneHeight=10.0, coneName='sunKeepIn') + vizSupport.createConeInOut( + viz, + toBodyName="sun_planet_data", + coneColor="b", + normalVector_B=[0, 0, 1], + incidenceAngle=sunSensorFov * macros.D2R, + isKeepIn=True, + coneHeight=10.0, + coneName="sunKeepIn", + ) # initialize Simulation: This function runs the self_init() # cross_init() and reset() routines on each module. @@ -418,37 +478,78 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas np.set_printoptions(precision=16) - # Displays the plots relative to the S/C attitude, maneuver, RW speeds and torques and boresight angles timeData = rwMotorLog.times() * macros.NANO2MIN plot_attitude_error(timeData, dataSigmaBR) figureList = {} - pltName = fileName + "1" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "1" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(1) plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW) - pltName = fileName + "2" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "2" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(2) plot_rate_error(timeData, dataOmegaBR) - pltName = fileName + "3" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "3" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(3) plot_rw_speeds(timeData, dataOmegaRW, numRW) - pltName = fileName + "4" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "4" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(4) plot_st_miss_angle(timeData, dataSTMissAngle, starTrackerFov) - pltName = fileName + "5" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "5" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(5) dataSS = [dataSSyMissAngle] if use2SunSensors: dataSS.append(dataSSzMissAngle) plot_ss_miss_angle(timeData, dataSS, sunSensorFov) - pltName = fileName + "6" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "6" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(6) if show_plots: @@ -465,112 +566,155 @@ def plot_attitude_error(timeData, dataSigmaBR): """Plot the attitude errors.""" plt.figure(1) for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") + def plot_rw_cmd_torque(timeData, dataUsReq, numRW): """Plot the RW command torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") + def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW): """Plot the RW actual motor torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.plot(timeData, dataRW[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.plot( + timeData, + dataRW[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") + def plot_rate_error(timeData, dataOmegaBR): """Plot the body angular velocity rate tracking errors.""" plt.figure(3) for idx in range(3): - plt.plot(timeData, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error (rad/s) ') + plt.plot( + timeData, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error (rad/s) ") + def plot_rw_speeds(timeData, dataOmegaRW, numRW): """Plot the RW spin rates.""" plt.figure(4) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed (RPM) ') + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed (RPM) ") + def plot_st_miss_angle(timeData, dataMissAngle, Fov): """Plot the miss angle between star tacker boresight and Sun.""" fig, ax = plt.subplots() trans = mtransforms.blended_transform_factory(ax.transData, ax.transAxes) - dataFov = np.ones(len(timeData))*Fov - plt.plot(timeData, dataFov, '--', - color = 'r', label = r'f.o.v.') - data = dataMissAngle*macros.R2D + dataFov = np.ones(len(timeData)) * Fov + plt.plot(timeData, dataFov, "--", color="r", label=r"f.o.v.") + data = dataMissAngle * macros.R2D for idx in range(1): - plt.plot(timeData, data, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\alpha $') - plt.fill_between(timeData, 0, 1, where=dataFov >= data, facecolor='red', - alpha = 0.4, interpolate=True, transform = trans) - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('StarTracker/Sun angle \u03B1 (deg)') + plt.plot( + timeData, + data, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\alpha $", + ) + plt.fill_between( + timeData, + 0, + 1, + where=dataFov >= data, + facecolor="red", + alpha=0.4, + interpolate=True, + transform=trans, + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("StarTracker/Sun angle \u03b1 (deg)") + def plot_ss_miss_angle(timeData, dataMissAngle, Fov): """Plot the miss angle between sun sensor(s) boresight and Sun.""" fig, ax = plt.subplots() trans = mtransforms.blended_transform_factory(ax.transData, ax.transAxes) - dataFov = np.ones(len(timeData))*Fov - plt.plot(timeData, dataFov, '--', - color = 'r', label = r'f.o.v.') + dataFov = np.ones(len(timeData)) * Fov + plt.plot(timeData, dataFov, "--", color="r", label=r"f.o.v.") data = [] for d in dataMissAngle: - data.append(d*macros.R2D) + data.append(d * macros.R2D) for idx in range(len(data)): - plt.plot(timeData, data[idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\beta_{' + str(idx+1) + '}$') - dataMinAngle = 180*np.ones(len(timeData)) + plt.plot( + timeData, + data[idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\beta_{" + str(idx + 1) + "}$", + ) + dataMinAngle = 180 * np.ones(len(timeData)) for i in range(len(timeData)): for j in range(len(data)): if data[j][i] < dataMinAngle[i]: dataMinAngle[i] = data[j][i] - plt.fill_between(timeData, 0, 1, where = dataFov < dataMinAngle, facecolor='red', - alpha = 0.4, interpolate=True, transform = trans) - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('SunSensor/Sun angle \u03B2 (deg)') - + plt.fill_between( + timeData, + 0, + 1, + where=dataFov < dataMinAngle, + facecolor="red", + alpha=0.4, + interpolate=True, + transform=trans, + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("SunSensor/Sun angle \u03b2 (deg)") if __name__ == "__main__": run( - True, # show_plots - True, # use2SunSensors - 30, # starTrackerFov - 70, # sunSensorFov - 3 # attitudeSetCase + True, # show_plots + True, # use2SunSensors + 30, # starTrackerFov + 70, # sunSensorFov + 3, # attitudeSetCase ) diff --git a/examples/scenarioAttitudeConstraintViolation.py b/examples/scenarioAttitudeConstraintViolation.py index 881f3cec75..a0337cdd7a 100644 --- a/examples/scenarioAttitudeConstraintViolation.py +++ b/examples/scenarioAttitudeConstraintViolation.py @@ -142,19 +142,33 @@ import numpy as np from Basilisk import __path__ from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, inertial3D, rwMotorTorque) -from Basilisk.simulation import (reactionWheelStateEffector, simpleNav, spacecraft, boreAngCalc) -from Basilisk.utilities import (SimulationBaseClass, macros, - orbitalMotion, simIncludeGravBody, - simIncludeRW, unitTestSupport, vizSupport) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + inertial3D, + rwMotorTorque, +) +from Basilisk.simulation import ( + reactionWheelStateEffector, + simpleNav, + spacecraft, + boreAngCalc, +) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + simIncludeRW, + unitTestSupport, + vizSupport, +) bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) - def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCase): - # Create simulation variable names simTaskName = "simTask" simProcessName = "simProcess" @@ -189,8 +203,8 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas gravFactory = simIncludeGravBody.gravBodyFactory() # Next a series of gravitational bodies are included - gravBodies = gravFactory.createBodies('earth', 'sun') - planet = gravBodies['earth'] + gravBodies = gravFactory.createBodies("earth", "sun") + planet = gravBodies["earth"] planet.isCentralBody = True mu = planet.mu @@ -199,14 +213,14 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas # Next, the default SPICE support module is created and configured. timeInitString = "2021 JANUARY 15 00:28:30.0" - spiceTimeStringFormat = '%Y %B %d %H:%M:%S.%f' + spiceTimeStringFormat = "%Y %B %d %H:%M:%S.%f" timeInit = datetime.strptime(timeInitString, spiceTimeStringFormat) # The following is a support macro that creates a `spiceObject` instance spiceObject = gravFactory.createSpiceInterface(time=timeInitString, epochInMsg=True) # Earth is gravity center - spiceObject.zeroBase = 'Earth' + spiceObject.zeroBase = "Earth" # The SPICE object is added to the simulation task list. scSim.AddModelToTask(simTaskName, spiceObject, 2) @@ -216,7 +230,7 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - oe.a = 7000. * 1000 # meters + oe.a = 7000.0 * 1000 # meters oe.e = 0.0001 oe.i = 33.3 * macros.D2R oe.Omega = 148.2 * macros.D2R @@ -225,24 +239,30 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas rN, vN = orbitalMotion.elem2rv(mu, oe) oe = orbitalMotion.rv2elem(mu, rN, vN) - # sets of initial attitudes that yield the desired constraint violations (attitudeSetCase) - sigma_BN_start = [ [0.522, -0.065, 0.539], # to violate one keepIn only - [0.314, -0.251, 0.228], # to violate two keepIn and not keepOut - [-0.378, 0.119, -0.176], # to violate keepOut and both keepIn - [-0.412, 0.044, -0.264] ] # to violate keepOut only + # sets of initial attitudes that yield the desired constraint violations (attitudeSetCase) + sigma_BN_start = [ + [0.522, -0.065, 0.539], # to violate one keepIn only + [0.314, -0.251, 0.228], # to violate two keepIn and not keepOut + [-0.378, 0.119, -0.176], # to violate keepOut and both keepIn + [-0.412, 0.044, -0.264], + ] # to violate keepOut only # To set the spacecraft initial conditions, the following initial position and velocity variables are set: scObject.hub.r_CN_NInit = rN # m - r_BN_N scObject.hub.v_CN_NInit = vN # m/s - v_BN_N - scObject.hub.sigma_BNInit = sigma_BN_start[attitudeSetCase] # change this MRP set to customize initial inertial attitude - scObject.hub.omega_BN_BInit = [[0.], [0.], [0.]] # rad/s - omega_CN_B + scObject.hub.sigma_BNInit = sigma_BN_start[ + attitudeSetCase + ] # change this MRP set to customize initial inertial attitude + scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_CN_B # define the simulation inertia - I = [0.02 / 3, 0., 0., - 0., 0.1256 / 3, 0., - 0., 0., 0.1256 / 3] + I = [0.02 / 3, 0.0, 0.0, 0.0, 0.1256 / 3, 0.0, 0.0, 0.0, 0.1256 / 3] scObject.hub.mHub = 4.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # @@ -257,21 +277,33 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments maxMomentum = 0.01 maxSpeed = 6000 * macros.RPM - RW1 = rwFactory.create('custom', [1, 0, 0], Omega=0. # RPM - , Omega_max=maxSpeed - , maxMomentum=maxMomentum - , u_max=0.001 - , RWModel=varRWModel) - RW2 = rwFactory.create('custom', [0, 1, 0], Omega=0. # RPM - , Omega_max=maxSpeed - , maxMomentum=maxMomentum - , u_max=0.001 - , RWModel=varRWModel) - RW3 = rwFactory.create('custom', [0, 0, 1], Omega=0. # RPM - , Omega_max=maxSpeed - , maxMomentum=maxMomentum - , u_max=0.001 - , RWModel=varRWModel) + RW1 = rwFactory.create( + "custom", + [1, 0, 0], + Omega=0.0, # RPM + Omega_max=maxSpeed, + maxMomentum=maxMomentum, + u_max=0.001, + RWModel=varRWModel, + ) + RW2 = rwFactory.create( + "custom", + [0, 1, 0], + Omega=0.0, # RPM + Omega_max=maxSpeed, + maxMomentum=maxMomentum, + u_max=0.001, + RWModel=varRWModel, + ) + RW3 = rwFactory.create( + "custom", + [0, 0, 1], + Omega=0.0, # RPM + Omega_max=maxSpeed, + maxMomentum=maxMomentum, + u_max=0.001, + RWModel=varRWModel, + ) numRW = rwFactory.getNumOfDevices() @@ -292,17 +324,21 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas # setup the FSW algorithm tasks # - # sets of initial attitudes that yield the desired constraint violations (attitudeSetCase) - sigma_BN_target = [ [0.342, 0.223, -0.432], # to violate one keepIn only - [0.326, -0.206, -0.823], # to violate two keepIn and not keepOut - [0.350, 0.220, -0.440], # to violate keepOut and both keepIn - [0.350, 0.220, -0.440] ] # to violate keepOut only + # sets of initial attitudes that yield the desired constraint violations (attitudeSetCase) + sigma_BN_target = [ + [0.342, 0.223, -0.432], # to violate one keepIn only + [0.326, -0.206, -0.823], # to violate two keepIn and not keepOut + [0.350, 0.220, -0.440], # to violate keepOut and both keepIn + [0.350, 0.220, -0.440], + ] # to violate keepOut only # setup inertial3D guidance module inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = sigma_BN_target[attitudeSetCase] # change this MRP set to customize final inertial attitude + inertial3DObj.sigma_R0N = sigma_BN_target[ + attitudeSetCase + ] # change this MRP set to customize final inertial attitude # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -316,9 +352,9 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas decayTime = 10.0 xi = 1.0 mrpControl.Ki = -1 # make value negative to turn off integral feedback - mrpControl.P = 3*np.max(I)/decayTime - mrpControl.K = (mrpControl.P/xi)*(mrpControl.P/xi)/np.max(I) - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.P = 3 * np.max(I) / decayTime + mrpControl.K = (mrpControl.P / xi) * (mrpControl.P / xi) / np.max(I) + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # add module that maps the Lr control torque into the RW motor torques rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() @@ -332,25 +368,27 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas # Boresight vector modules. stBACObject = boreAngCalc.BoreAngCalc() stBACObject.ModelTag = "starTrackerBoresight" - stBACObject.boreVec_B = [1., 0., 0.] # boresight in body frame + stBACObject.boreVec_B = [1.0, 0.0, 0.0] # boresight in body frame scSim.AddModelToTask(simTaskName, stBACObject) ssyBACObject = boreAngCalc.BoreAngCalc() ssyBACObject.ModelTag = "SunSensorBoresight" - ssyBACObject.boreVec_B = [0., 1., 0.] # boresight in body frame + ssyBACObject.boreVec_B = [0.0, 1.0, 0.0] # boresight in body frame scSim.AddModelToTask(simTaskName, ssyBACObject) if use2SunSensors: sszBACObject = boreAngCalc.BoreAngCalc() sszBACObject.ModelTag = "SunSensorBoresight" - sszBACObject.boreVec_B = [0., 0., 1.] # boresight in body frame + sszBACObject.boreVec_B = [0.0, 0.0, 1.0] # boresight in body frame scSim.AddModelToTask(simTaskName, sszBACObject) # # Setup data logging before the simulation is initialized # numDataPoints = 200 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataRec = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataRec) rwMotorLog = rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime) @@ -391,7 +429,7 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg) sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg) attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg) - attError.attRefInMsg.subscribeTo(inertial3DObj.attRefOutMsg) # for inertial3D + attError.attRefInMsg.subscribeTo(inertial3DObj.attRefOutMsg) # for inertial3D mrpControl.guidInMsg.subscribeTo(attError.attGuidOutMsg) mrpControl.vehConfigInMsg.subscribeTo(vcMsg) mrpControl.rwParamsInMsg.subscribeTo(fswRwParamMsg) @@ -412,19 +450,43 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas # Vizard Visualization Option # --------------------------- - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - # saveFile=__file__ - ) - vizSupport.createConeInOut(viz, toBodyName='sun_planet_data', coneColor = 'r', - normalVector_B=[1, 0, 0], incidenceAngle=starTrackerFov*macros.D2R, isKeepIn=False, - coneHeight=10.0, coneName='sunKeepOut') - vizSupport.createConeInOut(viz, toBodyName='sun_planet_data', coneColor = 'g', - normalVector_B=[0, 1, 0], incidenceAngle=sunSensorFov*macros.D2R, isKeepIn=True, - coneHeight=10.0, coneName='sunKeepIn') + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # saveFile=__file__ + ) + vizSupport.createConeInOut( + viz, + toBodyName="sun_planet_data", + coneColor="r", + normalVector_B=[1, 0, 0], + incidenceAngle=starTrackerFov * macros.D2R, + isKeepIn=False, + coneHeight=10.0, + coneName="sunKeepOut", + ) + vizSupport.createConeInOut( + viz, + toBodyName="sun_planet_data", + coneColor="g", + normalVector_B=[0, 1, 0], + incidenceAngle=sunSensorFov * macros.D2R, + isKeepIn=True, + coneHeight=10.0, + coneName="sunKeepIn", + ) if use2SunSensors: - vizSupport.createConeInOut(viz, toBodyName='sun_planet_data', coneColor = 'b', - normalVector_B=[0, 0, 1], incidenceAngle=sunSensorFov*macros.D2R, isKeepIn=True, - coneHeight=10.0, coneName='sunKeepIn') + vizSupport.createConeInOut( + viz, + toBodyName="sun_planet_data", + coneColor="b", + normalVector_B=[0, 0, 1], + incidenceAngle=sunSensorFov * macros.D2R, + isKeepIn=True, + coneHeight=10.0, + coneName="sunKeepIn", + ) # initialize Simulation: This function runs the self_init() # cross_init() and reset() routines on each module. @@ -456,37 +518,78 @@ def run(show_plots, use2SunSensors, starTrackerFov, sunSensorFov, attitudeSetCas np.set_printoptions(precision=16) - # Displays the plots relative to the S/C attitude, maneuver, RW speeds and torques and boresight angles timeData = rwMotorLog.times() * macros.NANO2MIN plot_attitude_error(timeData, dataSigmaBR) figureList = {} - pltName = fileName + "1" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "1" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(1) plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW) - pltName = fileName + "2" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "2" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(2) plot_rate_error(timeData, dataOmegaBR) - pltName = fileName + "3" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "3" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(3) plot_rw_speeds(timeData, dataOmegaRW, numRW) - pltName = fileName + "4" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "4" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(4) plot_st_miss_angle(timeData, dataSTMissAngle, starTrackerFov) - pltName = fileName + "5" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "5" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(5) dataSS = [dataSSyMissAngle] if use2SunSensors: dataSS.append(dataSSzMissAngle) plot_ss_miss_angle(timeData, dataSS, sunSensorFov) - pltName = fileName + "6" + str(int(use2SunSensors)) + str(starTrackerFov) + str(sunSensorFov) + str(attitudeSetCase) + pltName = ( + fileName + + "6" + + str(int(use2SunSensors)) + + str(starTrackerFov) + + str(sunSensorFov) + + str(attitudeSetCase) + ) figureList[pltName] = plt.figure(6) if show_plots: @@ -503,112 +606,155 @@ def plot_attitude_error(timeData, dataSigmaBR): """Plot the attitude errors.""" plt.figure(1) for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") + def plot_rw_cmd_torque(timeData, dataUsReq, numRW): """Plot the RW command torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") + def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW): """Plot the RW actual motor torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.plot(timeData, dataRW[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.plot( + timeData, + dataRW[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") + def plot_rate_error(timeData, dataOmegaBR): """Plot the body angular velocity rate tracking errors.""" plt.figure(3) for idx in range(3): - plt.plot(timeData, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error (rad/s) ') + plt.plot( + timeData, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error (rad/s) ") + def plot_rw_speeds(timeData, dataOmegaRW, numRW): """Plot the RW spin rates.""" plt.figure(4) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed (RPM) ') + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed (RPM) ") + def plot_st_miss_angle(timeData, dataMissAngle, Fov): """Plot the miss angle between star tacker boresight and Sun.""" fig, ax = plt.subplots() trans = mtransforms.blended_transform_factory(ax.transData, ax.transAxes) - dataFov = np.ones(len(timeData))*Fov - plt.plot(timeData, dataFov, '--', - color = 'r', label = r'f.o.v.') - data = dataMissAngle*macros.R2D + dataFov = np.ones(len(timeData)) * Fov + plt.plot(timeData, dataFov, "--", color="r", label=r"f.o.v.") + data = dataMissAngle * macros.R2D for idx in range(1): - plt.plot(timeData, data, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\alpha $') - plt.fill_between(timeData, 0, 1, where=dataFov >= data, facecolor='red', - alpha = 0.4, interpolate=True, transform = trans) - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('StarTracker/Sun angle \u03B1 (deg)') + plt.plot( + timeData, + data, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\alpha $", + ) + plt.fill_between( + timeData, + 0, + 1, + where=dataFov >= data, + facecolor="red", + alpha=0.4, + interpolate=True, + transform=trans, + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("StarTracker/Sun angle \u03b1 (deg)") + def plot_ss_miss_angle(timeData, dataMissAngle, Fov): """Plot the miss angle between sun sensor(s) boresight and Sun.""" fig, ax = plt.subplots() trans = mtransforms.blended_transform_factory(ax.transData, ax.transAxes) - dataFov = np.ones(len(timeData))*Fov - plt.plot(timeData, dataFov, '--', - color = 'r', label = r'f.o.v.') + dataFov = np.ones(len(timeData)) * Fov + plt.plot(timeData, dataFov, "--", color="r", label=r"f.o.v.") data = [] for d in dataMissAngle: - data.append(d*macros.R2D) + data.append(d * macros.R2D) for idx in range(len(data)): - plt.plot(timeData, data[idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\beta_{' + str(idx+1) + '}$') - dataMinAngle = 180*np.ones(len(timeData)) + plt.plot( + timeData, + data[idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\beta_{" + str(idx + 1) + "}$", + ) + dataMinAngle = 180 * np.ones(len(timeData)) for i in range(len(timeData)): for j in range(len(data)): if data[j][i] < dataMinAngle[i]: dataMinAngle[i] = data[j][i] - plt.fill_between(timeData, 0, 1, where = dataFov < dataMinAngle, facecolor='red', - alpha = 0.4, interpolate=True, transform = trans) - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('SunSensor/Sun angle \u03B2 (deg)') - + plt.fill_between( + timeData, + 0, + 1, + where=dataFov < dataMinAngle, + facecolor="red", + alpha=0.4, + interpolate=True, + transform=trans, + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("SunSensor/Sun angle \u03b2 (deg)") if __name__ == "__main__": run( - True, # show_plots - True, # use2SunSensors - 20, # starTrackerFov - 70, # sunSensorFov - 2 # attitudeSetCase + True, # show_plots + True, # use2SunSensors + 20, # starTrackerFov + 70, # sunSensorFov + 2, # attitudeSetCase ) diff --git a/examples/scenarioAttitudeFeedback.py b/examples/scenarioAttitudeFeedback.py index 6738da6794..2f4cbc5dc6 100755 --- a/examples/scenarioAttitudeFeedback.py +++ b/examples/scenarioAttitudeFeedback.py @@ -158,7 +158,9 @@ # import general simulation support files from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import matplotlib.pyplot as plt from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion @@ -184,6 +186,7 @@ # 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]) @@ -209,7 +212,7 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque, useCMsg): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -217,7 +220,7 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque, useCMsg): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -228,11 +231,13 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque, useCMsg): scObject = spacecraft.Spacecraft() scObject.ModelTag = "spacecraftBody" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -275,7 +280,7 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque, useCMsg): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -292,7 +297,7 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque, useCMsg): else: mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 if useKnownTorque: mrpControl.knownTorquePntB_B = [0.25, -0.25, 0.1] @@ -337,7 +342,9 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque, useCMsg): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) snLog = scObject.scStateOutMsg.recorder(samplingTime) # instead of recording the contents of a C++ output message, you can also recording # the incoming contents of a C++ input message. However, note that you must setup the @@ -372,9 +379,12 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque, useCMsg): scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B # if this scenario is to interface with the BSK Viz, uncomment the following line - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation @@ -394,44 +404,68 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque, useCMsg): plt.close("all") # clears out plots from earlier test runs plt.figure(1) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, attErrorLog.sigma_BR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeAxis * macros.NANO2MIN, + attErrorLog.sigma_BR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") figureList = {} - pltName = fileName + "1" + str(int(useUnmodeledTorque)) + str(int(useIntGain)) + str(int(useKnownTorque)) + pltName = ( + fileName + + "1" + + str(int(useUnmodeledTorque)) + + str(int(useIntGain)) + + str(int(useKnownTorque)) + ) figureList[pltName] = plt.figure(1) plt.figure(2) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, mrpLog.torqueRequestBody[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Control Torque $L_r$ [Nm]') - pltName = fileName + "2" + str(int(useUnmodeledTorque)) + str(int(useIntGain)) + str(int(useKnownTorque)) + plt.plot( + timeAxis * macros.NANO2MIN, + mrpLog.torqueRequestBody[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Control Torque $L_r$ [Nm]") + pltName = ( + fileName + + "2" + + str(int(useUnmodeledTorque)) + + str(int(useIntGain)) + + str(int(useKnownTorque)) + ) figureList[pltName] = plt.figure(2) plt.figure(3) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, attErrorLog.omega_BR_B[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeAxis * macros.NANO2MIN, + attErrorLog.omega_BR_B[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") plt.figure(4) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, snLog.r_BN_N[:, idx] / 1000., - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Inertial Position [km]') + plt.plot( + timeAxis * macros.NANO2MIN, + snLog.r_BN_N[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Inertial Position [km]") if show_plots: plt.show() @@ -452,5 +486,5 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque, useCMsg): False, # useUnmodeledTorque False, # useIntGain False, # useKnownTorque - False + False, ) diff --git a/examples/scenarioAttitudeFeedback2T.py b/examples/scenarioAttitudeFeedback2T.py index b87a7deb0b..a9d5fd9f4b 100755 --- a/examples/scenarioAttitudeFeedback2T.py +++ b/examples/scenarioAttitudeFeedback2T.py @@ -118,30 +118,37 @@ # Creation Date: Nov. 25, 2016 # - import os import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import attTrackingError from Basilisk.fswAlgorithms import inertial3D + # import FSW Algorithm related support from Basilisk.fswAlgorithms import mrpFeedback from Basilisk.simulation import extForceTorque from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -170,7 +177,7 @@ def run(show_plots, useUnmodeledTorque, useIntGain): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -192,11 +199,13 @@ def run(show_plots, useUnmodeledTorque, useIntGain): scObject = spacecraft.Spacecraft() scObject.ModelTag = "spacecraftBody" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -239,7 +248,7 @@ def run(show_plots, useUnmodeledTorque, useIntGain): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(fswTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -256,18 +265,22 @@ def run(show_plots, useUnmodeledTorque, useIntGain): else: mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # # Setup data logging before the simulation is initialized # # Add logging object to a task group, this controls the logging rate numDataPoints = 100 - dataLog = scObject.scStateOutMsg.recorder(unitTestSupport.samplingTime(simulationTime, simTimeStep, numDataPoints)) - attErrorLog = attError.attGuidOutMsg.recorder(unitTestSupport.samplingTime(simulationTime, - fswTimeStep, numDataPoints)) - mrpLog = mrpControl.cmdTorqueOutMsg.recorder(unitTestSupport.samplingTime(simulationTime, - fswTimeStep, numDataPoints)) + dataLog = scObject.scStateOutMsg.recorder( + unitTestSupport.samplingTime(simulationTime, simTimeStep, numDataPoints) + ) + attErrorLog = attError.attGuidOutMsg.recorder( + unitTestSupport.samplingTime(simulationTime, fswTimeStep, numDataPoints) + ) + mrpLog = mrpControl.cmdTorqueOutMsg.recorder( + unitTestSupport.samplingTime(simulationTime, fswTimeStep, numDataPoints) + ) scSim.AddModelToTask(dynTaskName, dataLog) scSim.AddModelToTask(fswTaskName, attErrorLog) @@ -310,9 +323,12 @@ def run(show_plots, useUnmodeledTorque, useIntGain): mrpControl.vehConfigInMsg.subscribeTo(configDataMsg) # if this scenario is to interface with the BSK Viz, uncomment the following lines - vizSupport.enableUnityVisualization(scSim, dynTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + dynTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation @@ -345,53 +361,68 @@ def run(show_plots, useUnmodeledTorque, useIntGain): plt.close("all") # clears out plots from earlier test runs plt.figure(1) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeAxis * macros.NANO2MIN, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") figureList = {} pltName = fileName + "1" + str(int(useUnmodeledTorque)) + str(int(useIntGain)) figureList[pltName] = plt.figure(1) plt.figure(2) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Control Torque $L_r$ [Nm]') + plt.plot( + timeAxis * macros.NANO2MIN, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Control Torque $L_r$ [Nm]") pltName = fileName + "2" + str(int(useUnmodeledTorque)) + str(int(useIntGain)) figureList[pltName] = plt.figure(2) plt.figure(3) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeAxis * macros.NANO2MIN, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") plt.figure(4) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataPos[:, idx] / 1000, - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Inertial Position [km] ') + plt.plot( + timeAxis * macros.NANO2MIN, + dataPos[:, idx] / 1000, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Inertial Position [km] ") plt.figure(5) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataSigmaBN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Inertial MRP Attitude ') + plt.plot( + timeAxis * macros.NANO2MIN, + dataSigmaBN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Inertial MRP Attitude ") if show_plots: plt.show() @@ -410,5 +441,5 @@ def run(show_plots, useUnmodeledTorque, useIntGain): run( True, # show_plots False, # useUnmodeledTorque - False # useIntGain - ) + False, # useIntGain + ) diff --git a/examples/scenarioAttitudeFeedback2T_TH.py b/examples/scenarioAttitudeFeedback2T_TH.py index 9c144c8453..23a58463ef 100755 --- a/examples/scenarioAttitudeFeedback2T_TH.py +++ b/examples/scenarioAttitudeFeedback2T_TH.py @@ -208,22 +208,27 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import attTrackingError from Basilisk.fswAlgorithms import inertial3D + # import FSW Algorithm related support from Basilisk.fswAlgorithms import mrpFeedback from Basilisk.fswAlgorithms import thrFiringSchmitt from Basilisk.fswAlgorithms import thrForceMapping from Basilisk.simulation import extForceTorque from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft from Basilisk.simulation import thrusterDynamicEffector + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import fswSetupThrusters @@ -231,69 +236,91 @@ from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody from Basilisk.utilities import simIncludeThruster -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) + # Plotting functions def plot_attitude_error(timeDataFSW, dataSigmaBR): """Plot the attitude errors.""" plt.figure(1) for idx in range(3): - plt.plot(timeDataFSW, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeDataFSW, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") + def plot_rate_error(timeDataFSW, dataOmegaBR): """Plot the body angular velocity tracking errors.""" plt.figure(2) for idx in range(3): - plt.plot(timeDataFSW, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeDataFSW, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") + def plot_requested_torque(timeDataFSW, dataLr): """Plot the commanded attitude control torque.""" plt.figure(3) for idx in range(3): - plt.plot(timeDataFSW, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$L_{r,' + str(idx) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Control Torque $L_r$ [Nm]') + plt.plot( + timeDataFSW, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$L_{r," + str(idx) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Control Torque $L_r$ [Nm]") + def plot_thrForce(timeDataFSW, dataMap, numTh): """Plot the Thruster force values.""" plt.figure(4) for idx in range(numTh): - plt.plot(timeDataFSW, dataMap[:, idx], - color=unitTestSupport.getLineColor(idx, numTh), - label=r'$thrForce_{' + str(idx) + r'}$' - ) - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Force requested [N]') + plt.plot( + timeDataFSW, + dataMap[:, idx], + color=unitTestSupport.getLineColor(idx, numTh), + label=r"$thrForce_{" + str(idx) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Force requested [N]") + def plot_OnTimeRequest(timeDataFSW, dataSchm, numTh): """Plot the thruster on time requests.""" plt.figure(5) for idx in range(numTh): - plt.plot(timeDataFSW, dataSchm[:, idx], - color=unitTestSupport.getLineColor(idx, numTh), - label=r'$OnTimeRequest_{' + str(idx) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('OnTimeRequest [sec]') + plt.plot( + timeDataFSW, + dataSchm[:, idx], + color=unitTestSupport.getLineColor(idx, numTh), + label=r"$OnTimeRequest_{" + str(idx) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("OnTimeRequest [sec]") def run(show_plots, useDVThrusters): @@ -317,7 +344,7 @@ def run(show_plots, useDVThrusters): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -339,11 +366,13 @@ def run(show_plots, useDVThrusters): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -376,132 +405,44 @@ def run(show_plots, useDVThrusters): # create arrays for thrusters' locations and directions if useDVThrusters: - location = [ - [ - 0, - 0.95, - -1.1 - ], - [ - 0.8227241335952166, - 0.4750000000000003, - -1.1 - ], - [ - 0.8227241335952168, - -0.47499999999999976, - -1.1 - ], - [ - 0, - -0.95, - -1.1 - ], - [ - -0.8227241335952165, - -0.4750000000000004, - -1.1 - ], - [ - -0.822724133595217, - 0.4749999999999993, - -1.1 - ] + [0, 0.95, -1.1], + [0.8227241335952166, 0.4750000000000003, -1.1], + [0.8227241335952168, -0.47499999999999976, -1.1], + [0, -0.95, -1.1], + [-0.8227241335952165, -0.4750000000000004, -1.1], + [-0.822724133595217, 0.4749999999999993, -1.1], ] - direction = [[0.0, 0.0, 1.0], - [0.0, 0.0, 1.0], - [0.0, 0.0, 1.0], - [0.0, 0.0, 1.0], - [0.0, 0.0, 1.0], - [0.0, 0.0, 1.0]] + direction = [ + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0], + ] else: - location = [ - [ - 3.874945160902288e-2, - -1.206182747348013, - 0.85245 - ], - [ - 3.874945160902288e-2, - -1.206182747348013, - -0.85245 - ], - [ - -3.8749451609022656e-2, - -1.206182747348013, - 0.85245 - ], - [ - -3.8749451609022656e-2, - -1.206182747348013, - -0.85245 - ], - [ - -3.874945160902288e-2, - 1.206182747348013, - 0.85245 - ], - [ - -3.874945160902288e-2, - 1.206182747348013, - -0.85245 - ], - [ - 3.8749451609022656e-2, - 1.206182747348013, - 0.85245 - ], - [ - 3.8749451609022656e-2, - 1.206182747348013, - -0.85245 - ] + [3.874945160902288e-2, -1.206182747348013, 0.85245], + [3.874945160902288e-2, -1.206182747348013, -0.85245], + [-3.8749451609022656e-2, -1.206182747348013, 0.85245], + [-3.8749451609022656e-2, -1.206182747348013, -0.85245], + [-3.874945160902288e-2, 1.206182747348013, 0.85245], + [-3.874945160902288e-2, 1.206182747348013, -0.85245], + [3.8749451609022656e-2, 1.206182747348013, 0.85245], + [3.8749451609022656e-2, 1.206182747348013, -0.85245], ] direction = [ - [ - -0.7071067811865476, - 0.7071067811865475, - 0.0 - ], - [ - -0.7071067811865476, - 0.7071067811865475, - 0.0 - ], - [ - 0.7071067811865475, - 0.7071067811865476, - 0.0 - ], - [ - 0.7071067811865475, - 0.7071067811865476, - 0.0 - ], - [ - 0.7071067811865476, - -0.7071067811865475, - 0.0 - ], - [ - 0.7071067811865476, - -0.7071067811865475, - 0.0 - ], - [ - -0.7071067811865475, - -0.7071067811865476, - 0.0 - ], - [ - -0.7071067811865475, - -0.7071067811865476, - 0.0 - ] + [-0.7071067811865476, 0.7071067811865475, 0.0], + [-0.7071067811865476, 0.7071067811865475, 0.0], + [0.7071067811865475, 0.7071067811865476, 0.0], + [0.7071067811865475, 0.7071067811865476, 0.0], + [0.7071067811865476, -0.7071067811865475, 0.0], + [0.7071067811865476, -0.7071067811865475, 0.0], + [-0.7071067811865475, -0.7071067811865476, 0.0], + [-0.7071067811865475, -0.7071067811865476, 0.0], ] # create the set of thruster in the dynamics task @@ -513,11 +454,10 @@ def run(show_plots, useDVThrusters): # create the thruster devices by specifying the thruster type and its location and direction for pos_B, dir_B in zip(location, direction): - if useDVThrusters: - thFactory.create('MOOG_Monarc_22_6', pos_B, dir_B) + thFactory.create("MOOG_Monarc_22_6", pos_B, dir_B) else: - thFactory.create('MOOG_Monarc_1', pos_B, dir_B) + thFactory.create("MOOG_Monarc_1", pos_B, dir_B) # get number of thruster devices numTh = thFactory.getNumOfDevices() @@ -533,7 +473,7 @@ def run(show_plots, useDVThrusters): # setup inertial3D guidance module inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation scSim.AddModelToTask(fswTaskName, inertial3DObj) # setup the attitude tracking error evaluation module @@ -545,10 +485,10 @@ def run(show_plots, useDVThrusters): mrpControl = mrpFeedback.mrpFeedback() mrpControl.ModelTag = "mrpFeedback" scSim.AddModelToTask(fswTaskName, mrpControl) - mrpControl.K = 3.5*10.0 + mrpControl.K = 3.5 * 10.0 mrpControl.Ki = 0.0002 # make value negative to turn off integral feedback - mrpControl.P = 30.0*10.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.P = 30.0 * 10.0 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # setup the thruster force mapping module thrForceMappingObj = thrForceMapping.thrForceMapping() @@ -556,13 +496,10 @@ def run(show_plots, useDVThrusters): scSim.AddModelToTask(fswTaskName, thrForceMappingObj) if useDVThrusters: - controlAxes_B = [1, 0, 0, - 0, 1, 0] + controlAxes_B = [1, 0, 0, 0, 1, 0] thrForceMappingObj.thrForceSign = -1 else: - controlAxes_B = [1, 0, 0, - 0, 1, 0, - 0, 0, 1] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] thrForceMappingObj.thrForceSign = +1 thrForceMappingObj.controlAxes_B = controlAxes_B @@ -571,8 +508,8 @@ def run(show_plots, useDVThrusters): thrFiringSchmittObj.ModelTag = "thrFiringSchmitt" scSim.AddModelToTask(fswTaskName, thrFiringSchmittObj) thrFiringSchmittObj.thrMinFireTime = 0.002 - thrFiringSchmittObj.level_on = .75 - thrFiringSchmittObj.level_off = .25 + thrFiringSchmittObj.level_on = 0.75 + thrFiringSchmittObj.level_off = 0.25 if useDVThrusters: thrFiringSchmittObj.baseThrustState = 1 @@ -581,7 +518,9 @@ def run(show_plots, useDVThrusters): # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, fswTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, fswTimeStep, numDataPoints + ) mrpTorqueLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) snTransLog = sNavObject.transOutMsg.recorder(samplingTime) @@ -653,11 +592,14 @@ def run(show_plots, useDVThrusters): thrusterSet.cmdsInMsg.subscribeTo(thrFiringSchmittObj.onTimeOutMsg) # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, scObject - # , saveFile=fileName - , thrEffectorList=thrusterSet - , thrColors=vizSupport.toRGBA255("red") - ) + viz = vizSupport.enableUnityVisualization( + scSim, + dynTaskName, + scObject, + # , saveFile=fileName + thrEffectorList=thrusterSet, + thrColors=vizSupport.toRGBA255("red"), + ) vizSupport.setActuatorGuiSetting(viz, showThrusterLabels=True) # @@ -716,6 +658,7 @@ def run(show_plots, useDVThrusters): return figureList + # # This statement below ensures that the unit test scrip can be run as a # stand-along python script diff --git a/examples/scenarioAttitudeFeedback2T_stateEffTH.py b/examples/scenarioAttitudeFeedback2T_stateEffTH.py index 51e6b29afc..17202325d8 100644 --- a/examples/scenarioAttitudeFeedback2T_stateEffTH.py +++ b/examples/scenarioAttitudeFeedback2T_stateEffTH.py @@ -93,23 +93,28 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import attTrackingError from Basilisk.fswAlgorithms import inertial3D + # import FSW Algorithm related support from Basilisk.fswAlgorithms import mrpFeedback from Basilisk.fswAlgorithms import thrFiringSchmitt from Basilisk.fswAlgorithms import thrForceMapping from Basilisk.simulation import extForceTorque from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft from Basilisk.simulation import svIntegrators from Basilisk.simulation import thrusterStateEffector + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import fswSetupThrusters @@ -117,7 +122,10 @@ from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody from Basilisk.utilities import simIncludeThruster -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -130,72 +138,90 @@ def plot_attitude_error(timeDataFSW, dataSigmaBR): """Plot the attitude errors.""" plt.figure(1) for idx in range(3): - plt.plot(timeDataFSW, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + r'$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeDataFSW, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + r"$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") def plot_rate_error(timeDataFSW, dataOmegaBR): """Plot the body angular velocity tracking errors.""" plt.figure(2) for idx in range(3): - plt.plot(timeDataFSW, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeDataFSW, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") def plot_requested_torque(timeDataFSW, dataLr): """Plot the commanded attitude control torque.""" plt.figure(3) for idx in range(3): - plt.plot(timeDataFSW, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Control Torque $L_r$ [Nm]') + plt.plot( + timeDataFSW, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Control Torque $L_r$ [Nm]") def plot_thrForce(timeDataFSW, dataMap, numTh): """Plot the Thruster force values.""" plt.figure(4) for idx in range(numTh): - plt.plot(timeDataFSW, dataMap[:, idx], - color=unitTestSupport.getLineColor(idx, numTh), - label=r'$thrForce_{' + str(idx) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Force requested [N]') + plt.plot( + timeDataFSW, + dataMap[:, idx], + color=unitTestSupport.getLineColor(idx, numTh), + label=r"$thrForce_{" + str(idx) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Force requested [N]") def plot_OnTimeRequest(timeDataFSW, dataSchm, numTh): """Plot the thruster on time requests.""" plt.figure(5) for idx in range(numTh): - plt.plot(timeDataFSW, dataSchm[:, idx], - color=unitTestSupport.getLineColor(idx, numTh), - label=r'$OnTimeRequest_{' + str(idx) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('OnTimeRequest [sec]') + plt.plot( + timeDataFSW, + dataSchm[:, idx], + color=unitTestSupport.getLineColor(idx, numTh), + label=r"$OnTimeRequest_{" + str(idx) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("OnTimeRequest [sec]") def plot_trueThrForce(timeDataFSW, dataMap, numTh): """Plot the Thruster force values.""" plt.figure(6) for idx in range(numTh): - plt.plot(timeDataFSW, dataMap[:, idx], - color=unitTestSupport.getLineColor(idx, numTh), - label=r'$thrForce_{' + str(idx) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Force implemented[N]') + plt.plot( + timeDataFSW, + dataMap[:, idx], + color=unitTestSupport.getLineColor(idx, numTh), + label=r"$thrForce_{" + str(idx) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Force implemented[N]") def run(show_plots, useDVThrusters): @@ -219,7 +245,7 @@ def run(show_plots, useDVThrusters): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -241,11 +267,13 @@ def run(show_plots, useDVThrusters): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -278,132 +306,44 @@ def run(show_plots, useDVThrusters): # create arrays for thrusters' locations and directions if useDVThrusters: - location = [ - [ - 0, - 0.95, - -1.1 - ], - [ - 0.8227241335952166, - 0.4750000000000003, - -1.1 - ], - [ - 0.8227241335952168, - -0.47499999999999976, - -1.1 - ], - [ - 0, - -0.95, - -1.1 - ], - [ - -0.8227241335952165, - -0.4750000000000004, - -1.1 - ], - [ - -0.822724133595217, - 0.4749999999999993, - -1.1 - ] + [0, 0.95, -1.1], + [0.8227241335952166, 0.4750000000000003, -1.1], + [0.8227241335952168, -0.47499999999999976, -1.1], + [0, -0.95, -1.1], + [-0.8227241335952165, -0.4750000000000004, -1.1], + [-0.822724133595217, 0.4749999999999993, -1.1], ] - direction = [[0.0, 0.0, 1.0], - [0.0, 0.0, 1.0], - [0.0, 0.0, 1.0], - [0.0, 0.0, 1.0], - [0.0, 0.0, 1.0], - [0.0, 0.0, 1.0]] + direction = [ + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0], + ] else: - location = [ - [ - 3.874945160902288e-2, - -1.206182747348013, - 0.85245 - ], - [ - 3.874945160902288e-2, - -1.206182747348013, - -0.85245 - ], - [ - -3.8749451609022656e-2, - -1.206182747348013, - 0.85245 - ], - [ - -3.8749451609022656e-2, - -1.206182747348013, - -0.85245 - ], - [ - -3.874945160902288e-2, - 1.206182747348013, - 0.85245 - ], - [ - -3.874945160902288e-2, - 1.206182747348013, - -0.85245 - ], - [ - 3.8749451609022656e-2, - 1.206182747348013, - 0.85245 - ], - [ - 3.8749451609022656e-2, - 1.206182747348013, - -0.85245 - ] + [3.874945160902288e-2, -1.206182747348013, 0.85245], + [3.874945160902288e-2, -1.206182747348013, -0.85245], + [-3.8749451609022656e-2, -1.206182747348013, 0.85245], + [-3.8749451609022656e-2, -1.206182747348013, -0.85245], + [-3.874945160902288e-2, 1.206182747348013, 0.85245], + [-3.874945160902288e-2, 1.206182747348013, -0.85245], + [3.8749451609022656e-2, 1.206182747348013, 0.85245], + [3.8749451609022656e-2, 1.206182747348013, -0.85245], ] direction = [ - [ - -0.7071067811865476, - 0.7071067811865475, - 0.0 - ], - [ - -0.7071067811865476, - 0.7071067811865475, - 0.0 - ], - [ - 0.7071067811865475, - 0.7071067811865476, - 0.0 - ], - [ - 0.7071067811865475, - 0.7071067811865476, - 0.0 - ], - [ - 0.7071067811865476, - -0.7071067811865475, - 0.0 - ], - [ - 0.7071067811865476, - -0.7071067811865475, - 0.0 - ], - [ - -0.7071067811865475, - -0.7071067811865476, - 0.0 - ], - [ - -0.7071067811865475, - -0.7071067811865476, - 0.0 - ] + [-0.7071067811865476, 0.7071067811865475, 0.0], + [-0.7071067811865476, 0.7071067811865475, 0.0], + [0.7071067811865475, 0.7071067811865476, 0.0], + [0.7071067811865475, 0.7071067811865476, 0.0], + [0.7071067811865476, -0.7071067811865475, 0.0], + [0.7071067811865476, -0.7071067811865475, 0.0], + [-0.7071067811865475, -0.7071067811865476, 0.0], + [-0.7071067811865475, -0.7071067811865476, 0.0], ] # create the set of thruster in the dynamics task @@ -420,9 +360,9 @@ def run(show_plots, useDVThrusters): # create the thruster devices by specifying the thruster type and its location and direction for pos_B, dir_B in zip(location, direction): if useDVThrusters: - thFactory.create('MOOG_Monarc_22_6', pos_B, dir_B, cutoffFrequency=1.) + thFactory.create("MOOG_Monarc_22_6", pos_B, dir_B, cutoffFrequency=1.0) else: - thFactory.create('MOOG_Monarc_1', pos_B, dir_B, cutoffFrequency=1.) + thFactory.create("MOOG_Monarc_1", pos_B, dir_B, cutoffFrequency=1.0) # get number of thruster devices numTh = thFactory.getNumOfDevices() @@ -438,7 +378,7 @@ def run(show_plots, useDVThrusters): # setup inertial3D guidance module inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation scSim.AddModelToTask(fswTaskName, inertial3DObj) # setup the attitude tracking error evaluation module @@ -453,7 +393,7 @@ def run(show_plots, useDVThrusters): mrpControl.K = 3.5 * 10.0 mrpControl.Ki = 0.0002 # make value negative to turn off integral feedback mrpControl.P = 30.0 * 10.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # setup the thruster force mapping module thrForceMappingObj = thrForceMapping.thrForceMapping() @@ -461,13 +401,10 @@ def run(show_plots, useDVThrusters): scSim.AddModelToTask(fswTaskName, thrForceMappingObj) if useDVThrusters: - controlAxes_B = [1, 0, 0, - 0, 1, 0] + controlAxes_B = [1, 0, 0, 0, 1, 0] thrForceMappingObj.thrForceSign = -1 else: - controlAxes_B = [1, 0, 0, - 0, 1, 0, - 0, 0, 1] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] thrForceMappingObj.thrForceSign = +1 thrForceMappingObj.controlAxes_B = controlAxes_B @@ -476,8 +413,8 @@ def run(show_plots, useDVThrusters): thrFiringSchmittObj.ModelTag = "thrFiringSchmitt" scSim.AddModelToTask(fswTaskName, thrFiringSchmittObj) thrFiringSchmittObj.thrMinFireTime = 0.002 - thrFiringSchmittObj.level_on = .75 - thrFiringSchmittObj.level_off = .25 + thrFiringSchmittObj.level_on = 0.75 + thrFiringSchmittObj.level_off = 0.25 if useDVThrusters: thrFiringSchmittObj.baseThrustState = 1 @@ -486,7 +423,9 @@ def run(show_plots, useDVThrusters): # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, fswTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, fswTimeStep, numDataPoints + ) mrpTorqueLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) snTransLog = sNavObject.transOutMsg.recorder(samplingTime) @@ -563,11 +502,14 @@ def run(show_plots, useDVThrusters): thrusterSet.cmdsInMsg.subscribeTo(thrFiringSchmittObj.onTimeOutMsg) # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, scObject - # , saveFile=fileName - , thrEffectorList=thrusterSet - , thrColors=vizSupport.toRGBA255("red") - ) + viz = vizSupport.enableUnityVisualization( + scSim, + dynTaskName, + scObject, + # , saveFile=fileName + thrEffectorList=thrusterSet, + thrColors=vizSupport.toRGBA255("red"), + ) vizSupport.setActuatorGuiSetting(viz, showThrusterLabels=True) # diff --git a/examples/scenarioAttitudeFeedbackNoEarth.py b/examples/scenarioAttitudeFeedbackNoEarth.py index b169f4a234..07302eae9c 100755 --- a/examples/scenarioAttitudeFeedbackNoEarth.py +++ b/examples/scenarioAttitudeFeedbackNoEarth.py @@ -108,23 +108,31 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import attTrackingError from Basilisk.fswAlgorithms import inertial3D + # import FSW Algorithm related support from Basilisk.fswAlgorithms import mrpFeedback from Basilisk.simulation import extForceTorque from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -132,7 +140,6 @@ fileName = os.path.basename(os.path.splitext(__file__)[0]) - def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque): """ The scenarios can be run with the followings setups parameters: @@ -153,7 +160,7 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -161,7 +168,7 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -172,11 +179,13 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -208,7 +217,7 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -225,7 +234,7 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque): else: mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 if useKnownTorque: mrpControl.knownTorquePntB_B = [0.25, -0.25, 0.1] @@ -233,7 +242,9 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) mrpLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, attErrorLog) @@ -255,10 +266,13 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque): scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - , modelDictionaryKeyList="3USat" - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + modelDictionaryKeyList="3USat", + # , saveFile=fileName + ) # # connect the messages to the modules # @@ -295,35 +309,56 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque): plt.close("all") # clears out plots from earlier test runs plt.figure(1) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeAxis * macros.NANO2MIN, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") figureList = {} - pltName = fileName + "1" + str(int(useUnmodeledTorque)) + str(int(useIntGain))+ str(int(useKnownTorque)) + pltName = ( + fileName + + "1" + + str(int(useUnmodeledTorque)) + + str(int(useIntGain)) + + str(int(useKnownTorque)) + ) figureList[pltName] = plt.figure(1) plt.figure(2) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Control Torque $L_r$ [Nm]') - pltName = fileName + "2" + str(int(useUnmodeledTorque)) + str(int(useIntGain)) + str(int(useKnownTorque)) + plt.plot( + timeAxis * macros.NANO2MIN, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Control Torque $L_r$ [Nm]") + pltName = ( + fileName + + "2" + + str(int(useUnmodeledTorque)) + + str(int(useIntGain)) + + str(int(useKnownTorque)) + ) figureList[pltName] = plt.figure(2) plt.figure(3) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeAxis * macros.NANO2MIN, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") if show_plots: plt.show() @@ -343,5 +378,5 @@ def run(show_plots, useUnmodeledTorque, useIntGain, useKnownTorque): True, # show_plots False, # useUnmodeledTorque False, # useIntGain - False # useKnownTorque + False, # useKnownTorque ) diff --git a/examples/scenarioAttitudeFeedbackRW.py b/examples/scenarioAttitudeFeedbackRW.py index 5abcb8089b..e62c196a3d 100755 --- a/examples/scenarioAttitudeFeedbackRW.py +++ b/examples/scenarioAttitudeFeedbackRW.py @@ -279,16 +279,34 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, - inertial3D, rwMotorTorque, rwMotorVoltage) -from Basilisk.simulation import reactionWheelStateEffector, motorVoltageInterface, simpleNav, spacecraft -from Basilisk.utilities import (SimulationBaseClass, fswSetupRW, macros, - orbitalMotion, simIncludeGravBody, - simIncludeRW, unitTestSupport, vizSupport) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + inertial3D, + rwMotorTorque, + rwMotorVoltage, +) +from Basilisk.simulation import ( + reactionWheelStateEffector, + motorVoltageInterface, + simpleNav, + spacecraft, +) +from Basilisk.utilities import ( + SimulationBaseClass, + fswSetupRW, + macros, + orbitalMotion, + simIncludeGravBody, + simIncludeRW, + unitTestSupport, + vizSupport, +) bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) @@ -299,73 +317,99 @@ def plot_attitude_error(timeData, dataSigmaBR): """Plot the attitude errors.""" plt.figure(1) for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") + def plot_rw_cmd_torque(timeData, dataUsReq, numRW): """Plot the RW command torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") + def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW): """Plot the RW actual motor torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.plot(timeData, dataRW[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.plot( + timeData, + dataRW[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") + def plot_rate_error(timeData, dataOmegaBR): """Plot the body angular velocity rate tracking errors.""" plt.figure(3) for idx in range(3): - plt.plot(timeData, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error (rad/s) ') + plt.plot( + timeData, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error (rad/s) ") + def plot_rw_speeds(timeData, dataOmegaRW, numRW): """Plot the RW spin rates.""" plt.figure(4) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed (RPM) ') + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed (RPM) ") def plot_rw_voltages(timeData, dataVolt, numRW): """Plot the RW voltage inputs.""" plt.figure(5) for idx in range(numRW): - plt.plot(timeData, dataVolt[:, idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$V_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Voltage (V)') + plt.plot( + timeData, + dataVolt[:, idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$V_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Voltage (V)") + def run(show_plots, useJitterSimple, useRWVoltageIO): """ @@ -386,7 +430,7 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -394,7 +438,7 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -405,11 +449,13 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -438,16 +484,28 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): varRWModel = messaging.JitterSimple # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments - RW1 = rwFactory.create('Honeywell_HR16', [1, 0, 0], maxMomentum=50., Omega=100. # RPM - , RWModel=varRWModel - ) - RW2 = rwFactory.create('Honeywell_HR16', [0, 1, 0], maxMomentum=50., Omega=200. # RPM - , RWModel=varRWModel - ) - RW3 = rwFactory.create('Honeywell_HR16', [0, 0, 1], maxMomentum=50., Omega=300. # RPM - , rWB_B=[0.5, 0.5, 0.5] # meters - , RWModel=varRWModel - ) + RW1 = rwFactory.create( + "Honeywell_HR16", + [1, 0, 0], + maxMomentum=50.0, + Omega=100.0, # RPM + RWModel=varRWModel, + ) + RW2 = rwFactory.create( + "Honeywell_HR16", + [0, 1, 0], + maxMomentum=50.0, + Omega=200.0, # RPM + RWModel=varRWModel, + ) + RW3 = rwFactory.create( + "Honeywell_HR16", + [0, 0, 1], + maxMomentum=50.0, + Omega=300.0, # RPM + rWB_B=[0.5, 0.5, 0.5], # meters + RWModel=varRWModel, + ) # In this simulation the RW objects RW1, RW2 or RW3 are not modified further. However, you can over-ride # any values generate in the `.create()` process using for example RW1.Omega_max = 100. to change the # maximum wheel speed. @@ -468,7 +526,7 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): rwVoltageIO.ModelTag = "rwVoltageInterface" # set module parameters(s) - rwVoltageIO.setGains(np.array([0.2 / 10.] * 3)) # [Nm/V] conversion gain + rwVoltageIO.setGains(np.array([0.2 / 10.0] * 3)) # [Nm/V] conversion gain # Add test module to runtime call list scSim.AddModelToTask(simTaskName, rwVoltageIO) @@ -487,7 +545,7 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -501,7 +559,7 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): mrpControl.K = 3.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # add module that maps the Lr control torque into the RW motor torques rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() @@ -509,9 +567,7 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): scSim.AddModelToTask(simTaskName, rwMotorTorqueObj) # Make the RW control all three body axes - controlAxes_B = [ - 1, 0, 0, 0, 1, 0, 0, 0, 1 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] rwMotorTorqueObj.controlAxes_B = controlAxes_B if useRWVoltageIO: @@ -529,7 +585,9 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) rwMotorLog = rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) snTransLog = sNavObject.transOutMsg.recorder(samplingTime) @@ -598,10 +656,13 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_CN_B # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - , rwEffectorList=rwStateEffector - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + rwEffectorList=rwStateEffector, + ) # link messages sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg) attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg) @@ -618,7 +679,9 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): rwVoltageIO.motorVoltageInMsg.subscribeTo(fswRWVoltage.voltageOutMsg) rwStateEffector.rwMotorCmdInMsg.subscribeTo(rwVoltageIO.motorTorqueOutMsg) else: - rwStateEffector.rwMotorCmdInMsg.subscribeTo(rwMotorTorqueObj.rwMotorTorqueOutMsg) + rwStateEffector.rwMotorCmdInMsg.subscribeTo( + rwMotorTorqueObj.rwMotorTorqueOutMsg + ) # # initialize Simulation @@ -689,5 +752,5 @@ def run(show_plots, useJitterSimple, useRWVoltageIO): run( True, # show_plots False, # useJitterSimple - True # useRWVoltageIO + True, # useRWVoltageIO ) diff --git a/examples/scenarioAttitudeFeedbackRWPower.py b/examples/scenarioAttitudeFeedbackRWPower.py index f87d735909..477ae8e46f 100755 --- a/examples/scenarioAttitudeFeedbackRWPower.py +++ b/examples/scenarioAttitudeFeedbackRWPower.py @@ -84,18 +84,29 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, - inertial3D, rwMotorTorque) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + inertial3D, + rwMotorTorque, +) from Basilisk.simulation import ReactionWheelPower from Basilisk.simulation import reactionWheelStateEffector, simpleNav, spacecraft from Basilisk.simulation import simpleBattery -from Basilisk.utilities import (SimulationBaseClass, macros, - orbitalMotion, simIncludeGravBody, - simIncludeRW, unitTestSupport, vizSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + simIncludeRW, + unitTestSupport, + vizSupport, +) bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) @@ -106,38 +117,52 @@ def plot_attitude_error(timeData, dataSigmaBR): """Plot the attitude errors.""" plt.figure(1) for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") + def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW): """Plot the RW actual motor torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.plot(timeData, dataRW[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.plot( + timeData, + dataRW[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") + def plot_rw_power(timeData, dataRwPower, numRW): """Plot the RW actual motor torques.""" plt.figure(3) for idx in range(3): - plt.plot(timeData, dataRwPower[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$p_{rw,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Power (W)') + plt.plot( + timeData, + dataRwPower[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$p_{rw," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Power (W)") def run(show_plots, useRwPowerGeneration): @@ -158,7 +183,7 @@ def run(show_plots, useRwPowerGeneration): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -166,7 +191,7 @@ def run(show_plots, useRwPowerGeneration): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -177,11 +202,13 @@ def run(show_plots, useRwPowerGeneration): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -208,16 +235,28 @@ def run(show_plots, useRwPowerGeneration): varRWModel = messaging.BalancedWheels # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments - RW1 = rwFactory.create('Honeywell_HR16', [1, 0, 0], maxMomentum=50., Omega=100. # RPM - , RWModel=varRWModel - ) - RW2 = rwFactory.create('Honeywell_HR16', [0, 1, 0], maxMomentum=50., Omega=200. # RPM - , RWModel=varRWModel - ) - RW3 = rwFactory.create('Honeywell_HR16', [0, 0, 1], maxMomentum=50., Omega=300. # RPM - , rWB_B=[0.5, 0.5, 0.5] # meters - , RWModel=varRWModel - ) + RW1 = rwFactory.create( + "Honeywell_HR16", + [1, 0, 0], + maxMomentum=50.0, + Omega=100.0, # RPM + RWModel=varRWModel, + ) + RW2 = rwFactory.create( + "Honeywell_HR16", + [0, 1, 0], + maxMomentum=50.0, + Omega=200.0, # RPM + RWModel=varRWModel, + ) + RW3 = rwFactory.create( + "Honeywell_HR16", + [0, 0, 1], + maxMomentum=50.0, + Omega=300.0, # RPM + rWB_B=[0.5, 0.5, 0.5], # meters + RWModel=varRWModel, + ) rwList = [RW1, RW2, RW3] numRW = rwFactory.getNumOfDevices() @@ -241,7 +280,7 @@ def run(show_plots, useRwPowerGeneration): for c in range(numRW): powerRW = ReactionWheelPower.ReactionWheelPower() powerRW.ModelTag = scObject.ModelTag + "RWPower" + str(c) - powerRW.basePowerNeed = 5. # baseline power draw, Watts + powerRW.basePowerNeed = 5.0 # baseline power draw, Watts powerRW.rwStateInMsg.subscribeTo(rwStateEffector.rwOutMsgs[c]) if useRwPowerGeneration: powerRW.mechToElecEfficiency = 0.5 @@ -274,7 +313,7 @@ def run(show_plots, useRwPowerGeneration): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -294,7 +333,7 @@ def run(show_plots, useRwPowerGeneration): mrpControl.K = 3.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # add module that maps the Lr control torque into the RW motor torques rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() @@ -306,16 +345,16 @@ def run(show_plots, useRwPowerGeneration): rwStateEffector.rwMotorCmdInMsg.subscribeTo(rwMotorTorqueObj.rwMotorTorqueOutMsg) # Make the RW control all three body axes - controlAxes_B = [ - 1, 0, 0, 0, 1, 0, 0, 0, 1 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] rwMotorTorqueObj.controlAxes_B = controlAxes_B # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) rwCmdLog = rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime) attErrLog = attError.attGuidOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, rwCmdLog) @@ -353,10 +392,13 @@ def run(show_plots, useRwPowerGeneration): scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_CN_B # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - , rwEffectorList=rwStateEffector - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + rwEffectorList=rwStateEffector, + ) # # initialize Simulation @@ -401,8 +443,8 @@ def run(show_plots, useRwPowerGeneration): plt.figure(4) plt.plot(timeData, batteryStorageLog) - plt.xlabel('Time [min]') - plt.ylabel('Battery Storage (Ws)') + plt.xlabel("Time [min]") + plt.ylabel("Battery Storage (Ws)") pltName = fileName + "4" + str(useRwPowerGeneration) figureList[pltName] = plt.figure(4) @@ -422,5 +464,5 @@ def run(show_plots, useRwPowerGeneration): if __name__ == "__main__": run( True, # show_plots - True # useRwPowerGeneration + True, # useRwPowerGeneration ) diff --git a/examples/scenarioAttitudeGG.py b/examples/scenarioAttitudeGG.py index 79cb4dbd36..16111a05b7 100755 --- a/examples/scenarioAttitudeGG.py +++ b/examples/scenarioAttitudeGG.py @@ -70,26 +70,34 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import attTrackingError from Basilisk.fswAlgorithms import hillPoint + # import FSW Algorithm related support from Basilisk.fswAlgorithms import mrpFeedback from Basilisk.simulation import GravityGradientEffector from Basilisk.simulation import extForceTorque from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -114,7 +122,7 @@ def run(show_plots): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -133,11 +141,13 @@ def run(show_plots): scObject = spacecraft.Spacecraft() scObject.ModelTag = "spacecraftBody" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -217,13 +227,15 @@ def run(show_plots): mrpControl.K = 3.5 mrpControl.Ki = -1.0 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) mrpLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) attErrLog = attError.attGuidOutMsg.recorder(samplingTime) ggLog = ggEff.gravityGradientOutMsg.recorder(samplingTime) @@ -251,10 +263,13 @@ def run(show_plots): extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) # if this scenario is to interface with the BSK Viz, uncomment the following lines - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - , modelDictionaryKeyList="6USat" - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + modelDictionaryKeyList="6USat", + # , saveFile=fileName + ) # # initialize Simulation @@ -287,39 +302,45 @@ def run(show_plots): ax = fig.gca() vectorData = dataSigmaBR sNorm = np.array([np.linalg.norm(v) for v in vectorData]) - plt.plot(timeLineSet, sNorm, - color=unitTestSupport.getLineColor(1, 3), - ) - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error Norm $|\sigma_{B/R}|$') - ax.set_yscale('log') + plt.plot( + timeLineSet, + sNorm, + color=unitTestSupport.getLineColor(1, 3), + ) + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error Norm $|\sigma_{B/R}|$") + ax.set_yscale("log") figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) - plt.figure(2) fig = plt.gcf() ax = fig.gca() vectorData = dataLr sNorm = np.array([np.linalg.norm(v) for v in vectorData]) - plt.plot(timeLineSet, sNorm, - color=unitTestSupport.getLineColor(1, 3), - ) - plt.xlabel('Time [min]') - plt.ylabel(r'Control Torque $L_r$ [Nm]') - ax.set_yscale('log') + plt.plot( + timeLineSet, + sNorm, + color=unitTestSupport.getLineColor(1, 3), + ) + plt.xlabel("Time [min]") + plt.ylabel(r"Control Torque $L_r$ [Nm]") + ax.set_yscale("log") pltName = fileName + "2" figureList[pltName] = plt.figure(2) plt.figure(3) for idx in range(3): - plt.plot(timeLineSet, ggData[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$r_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'GG Torque [Nm]') + plt.plot( + timeLineSet, + ggData[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$r_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"GG Torque [Nm]") pltName = fileName + "3" figureList[pltName] = plt.figure(3) diff --git a/examples/scenarioAttitudeGuidance.py b/examples/scenarioAttitudeGuidance.py index 174ffa42b8..aaca7424f3 100755 --- a/examples/scenarioAttitudeGuidance.py +++ b/examples/scenarioAttitudeGuidance.py @@ -133,26 +133,34 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import attTrackingError from Basilisk.fswAlgorithms import hillPoint + # import FSW Algorithm related support from Basilisk.fswAlgorithms import mrpFeedback from Basilisk.simulation import extForceTorque from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft from Basilisk.utilities import RigidBodyKinematics + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -168,36 +176,47 @@ def plot_attitude_error(timeLineSet, dataSigmaBR): ax = fig.gca() vectorData = dataSigmaBR sNorm = np.array([np.linalg.norm(v) for v in vectorData]) - plt.plot(timeLineSet, sNorm, - color=unitTestSupport.getLineColor(1, 3), - ) - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error Norm $|\sigma_{B/R}|$') - ax.set_yscale('log') + plt.plot( + timeLineSet, + sNorm, + color=unitTestSupport.getLineColor(1, 3), + ) + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error Norm $|\sigma_{B/R}|$") + ax.set_yscale("log") + def plot_control_torque(timeLineSet, dataLr): """Plot the control torque response.""" plt.figure(2) for idx in range(3): - plt.plot(timeLineSet, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Control Torque $L_r$ [Nm]') + plt.plot( + timeLineSet, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Control Torque $L_r$ [Nm]") + def plot_rate_error(timeLineSet, dataOmegaBR): """Plot the body angular velocity tracking error.""" plt.figure(3) for idx in range(3): - plt.plot(timeLineSet, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeLineSet, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") return + def plot_orientation(timeLineSet, dataPos, dataVel, dataSigmaBN): """Plot the spacecraft orientation.""" vectorPosData = dataPos @@ -210,18 +229,27 @@ def plot_orientation(timeLineSet, dataPos, dataVel, dataSigmaBN): ih = hv / np.linalg.norm(hv) itheta = np.cross(ih, ir) dcmBN = RigidBodyKinematics.MRP2C(vectorMRPData[idx]) - data[idx] = [np.dot(ir, dcmBN[0]), np.dot(itheta, dcmBN[1]), np.dot(ih, dcmBN[2])] + data[idx] = [ + np.dot(ir, dcmBN[0]), + np.dot(itheta, dcmBN[1]), + np.dot(ih, dcmBN[2]), + ] plt.figure(4) - labelStrings = (r'$\hat\imath_r\cdot \hat b_1$' - , r'${\hat\imath}_{\theta}\cdot \hat b_2$' - , r'$\hat\imath_h\cdot \hat b_3$') + labelStrings = ( + r"$\hat\imath_r\cdot \hat b_1$", + r"${\hat\imath}_{\theta}\cdot \hat b_2$", + r"$\hat\imath_h\cdot \hat b_3$", + ) for idx in range(3): - plt.plot(timeLineSet, data[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=labelStrings[idx]) - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Orientation Illustration') + plt.plot( + timeLineSet, + data[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=labelStrings[idx], + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Orientation Illustration") def run(show_plots, useAltBodyFrame): @@ -242,7 +270,7 @@ def run(show_plots, useAltBodyFrame): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -261,11 +289,13 @@ def run(show_plots, useAltBodyFrame): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -327,7 +357,9 @@ def run(show_plots, useAltBodyFrame): # if you want to connect attGuidance.celBodyInMsg, then you need a planet ephemeris message of # type EphemerisMsgPayload. In this simulation the input message is not connected to create an empty planet # ephemeris message which puts the earth at (0,0,0) origin with zero speed. - CelBodyData = messaging.EphemerisMsgPayload() # make zero'd planet ephemeris message + CelBodyData = ( + messaging.EphemerisMsgPayload() + ) # make zero'd planet ephemeris message celBodyInMsg = messaging.EphemerisMsg().write(CelBodyData) attGuidance.celBodyInMsg.subscribeTo(celBodyInMsg) scSim.AddModelToTask(simTaskName, attGuidance) @@ -349,7 +381,7 @@ def run(show_plots, useAltBodyFrame): mrpControl.K = 3.5 mrpControl.Ki = -1.0 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # connect torque command to external torque effector extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) @@ -358,7 +390,9 @@ def run(show_plots, useAltBodyFrame): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) mrpLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) attErrLog = attError.attGuidOutMsg.recorder(samplingTime) snAttLog = sNavObject.attOutMsg.recorder(samplingTime) @@ -379,9 +413,12 @@ def run(show_plots, useAltBodyFrame): mrpControl.vehConfigInMsg.subscribeTo(configDataMsg) # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation @@ -443,5 +480,5 @@ def run(show_plots, useAltBodyFrame): if __name__ == "__main__": run( True, # show_plots - True # useAltBodyFrame + True, # useAltBodyFrame ) diff --git a/examples/scenarioAttitudePointing.py b/examples/scenarioAttitudePointing.py index 3146873154..c979321257 100755 --- a/examples/scenarioAttitudePointing.py +++ b/examples/scenarioAttitudePointing.py @@ -86,23 +86,31 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import attTrackingError from Basilisk.fswAlgorithms import inertial3D + # import FSW Algorithm related support from Basilisk.fswAlgorithms import mrpFeedback from Basilisk.simulation import extForceTorque from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -133,7 +141,7 @@ def run(show_plots, useLargeTumble): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -141,7 +149,7 @@ def run(show_plots, useLargeTumble): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -152,11 +160,13 @@ def run(show_plots, useLargeTumble): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B if useLargeTumble: @@ -188,7 +198,7 @@ def run(show_plots, useLargeTumble): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -202,13 +212,15 @@ def run(show_plots, useLargeTumble): mrpControl.K = 3.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # # Setup data logging before the simulation is initialized # numDataPoints = 50 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) mrpLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, attErrorLog) @@ -234,9 +246,12 @@ def run(show_plots, useLargeTumble): mrpControl.vehConfigInMsg.subscribeTo(configDataMsg) # if this scenario is to interface with the BSK Viz, uncomment the following lines - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation @@ -264,35 +279,44 @@ def run(show_plots, useLargeTumble): plt.close("all") # clears out plots from earlier test runs plt.figure(1) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeAxis * macros.NANO2MIN, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") figureList = {} pltName = fileName + "1" + str(int(useLargeTumble)) figureList[pltName] = plt.figure(1) plt.figure(2) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Control Torque $L_r$ [Nm]') + plt.plot( + timeAxis * macros.NANO2MIN, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Control Torque $L_r$ [Nm]") pltName = fileName + "2" + str(int(useLargeTumble)) figureList[pltName] = plt.figure(2) plt.figure(3) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeAxis * macros.NANO2MIN, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") if show_plots: plt.show() @@ -300,7 +324,6 @@ def run(show_plots, useLargeTumble): # close the plots being saved off to avoid over-writing old and new figures plt.close("all") - return figureList diff --git a/examples/scenarioAttitudePointingPy.py b/examples/scenarioAttitudePointingPy.py index 2c1c25f275..fa0adc1080 100755 --- a/examples/scenarioAttitudePointingPy.py +++ b/examples/scenarioAttitudePointingPy.py @@ -80,23 +80,31 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import attTrackingError + # import FSW Algorithm related support # from Basilisk.fswAlgorithms import mrpFeedback from Basilisk.fswAlgorithms import inertial3D from Basilisk.simulation import extForceTorque from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport from Basilisk.architecture import sysModel @@ -127,7 +135,7 @@ def run(show_plots): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -135,7 +143,7 @@ def run(show_plots): dynProcess = scSim.CreateNewProcess(simProcessName, 10) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -146,11 +154,13 @@ def run(show_plots): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B @@ -179,7 +189,7 @@ def run(show_plots): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -197,7 +207,9 @@ def run(show_plots): # Setup data logging before the simulation is initialized # numDataPoints = 50 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) mrpLog = pyMRPPD.cmdTorqueOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, attErrorLog) @@ -213,9 +225,12 @@ def run(show_plots): extFTObject.cmdTorqueInMsg.subscribeTo(pyMRPPD.cmdTorqueOutMsg) # if this scenario is to interface with the BSK Viz, uncomment the following lines - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation @@ -243,35 +258,44 @@ def run(show_plots): plt.close("all") # clears out plots from earlier test runs plt.figure(1) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeAxis * macros.NANO2MIN, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) plt.figure(2) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Control Torque $L_r$ [Nm]') + plt.plot( + timeAxis * macros.NANO2MIN, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Control Torque $L_r$ [Nm]") pltName = fileName + "2" figureList[pltName] = plt.figure(2) plt.figure(3) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeAxis * macros.NANO2MIN, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") if show_plots: plt.show() @@ -305,6 +329,7 @@ class PythonMRPPD(sysModel.SysModel): can complete any other computations you need (``Numpy``, ``matplotlib``, vision processing AI, whatever). """ + def __init__(self): super(PythonMRPPD, self).__init__() @@ -345,7 +370,10 @@ def UpdateState(self, CurrentSimNanos): torqueOutMsgBuffer = messaging.CmdTorqueBodyMsgPayload() # compute control solution - lrCmd = np.array(guidMsgBuffer.sigma_BR) * self.K + np.array(guidMsgBuffer.omega_BR_B) * self.P + lrCmd = ( + np.array(guidMsgBuffer.sigma_BR) * self.K + + np.array(guidMsgBuffer.omega_BR_B) * self.P + ) torqueOutMsgBuffer.torqueRequestBody = (-lrCmd).tolist() self.cmdTorqueOutMsg.write(torqueOutMsgBuffer, CurrentSimNanos, self.moduleID) @@ -355,13 +383,23 @@ def UpdateState(self, CurrentSimNanos): # accessed from sysModel if False: """Sample Python module method""" - self.bskLogger.bskLog(sysModel.BSK_INFORMATION, f"Time: {CurrentSimNanos * 1.0E-9} s") - self.bskLogger.bskLog(sysModel.BSK_INFORMATION, f"TorqueRequestBody: {torqueOutMsgBuffer.torqueRequestBody}") - self.bskLogger.bskLog(sysModel.BSK_INFORMATION, f"sigma_BR: {guidMsgBuffer.sigma_BR}") - self.bskLogger.bskLog(sysModel.BSK_INFORMATION, f"omega_BR_B: {guidMsgBuffer.omega_BR_B}") + self.bskLogger.bskLog( + sysModel.BSK_INFORMATION, f"Time: {CurrentSimNanos * 1.0e-9} s" + ) + self.bskLogger.bskLog( + sysModel.BSK_INFORMATION, + f"TorqueRequestBody: {torqueOutMsgBuffer.torqueRequestBody}", + ) + self.bskLogger.bskLog( + sysModel.BSK_INFORMATION, f"sigma_BR: {guidMsgBuffer.sigma_BR}" + ) + self.bskLogger.bskLog( + sysModel.BSK_INFORMATION, f"omega_BR_B: {guidMsgBuffer.omega_BR_B}" + ) return + # # This statement below ensures that the unit test scrip can be run as a # stand-along python script diff --git a/examples/scenarioAttitudePrescribed.py b/examples/scenarioAttitudePrescribed.py index 77a772567e..ba59db0bb6 100755 --- a/examples/scenarioAttitudePrescribed.py +++ b/examples/scenarioAttitudePrescribed.py @@ -85,21 +85,28 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ from Basilisk.fswAlgorithms import attRefCorrection + # import FSW Algorithm related support from Basilisk.fswAlgorithms import hillPoint from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -126,7 +133,7 @@ def run(show_plots, useAltBodyFrame): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(3*60.) + simulationTime = macros.min2nano(3 * 60.0) # # create the simulation process @@ -145,11 +152,13 @@ def run(show_plots, useAltBodyFrame): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -181,7 +190,6 @@ def run(show_plots, useAltBodyFrame): scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - # add the simple Navigation sensor module. This sets the SC attitude, rate, position # velocity navigation message sNavObject = simpleNav.SimpleNav() @@ -207,7 +215,7 @@ def run(show_plots, useAltBodyFrame): attRefCor = attRefCorrection.attRefCorrection() attRefCor.ModelTag = "attRefCor" scSim.AddModelToTask(simTaskName, attRefCor) - attRefCor.sigma_BcB = [0.0, 0.0, math.tan(math.pi/8)] + attRefCor.sigma_BcB = [0.0, 0.0, math.tan(math.pi / 8)] attRefCor.attRefInMsg.subscribeTo(attGuidance.attRefOutMsg) scObject.attRefInMsg.subscribeTo(attRefCor.attRefOutMsg) else: @@ -217,14 +225,19 @@ def run(show_plots, useAltBodyFrame): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) snAttLog = sNavObject.attOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, snAttLog) # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation @@ -252,12 +265,15 @@ def run(show_plots, useAltBodyFrame): plt.figure(1) for idx in range(3): - plt.plot(timeLineSet, dataSigmaBN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Inertial Attitude $\sigma_{B/N}$') + plt.plot( + timeLineSet, + dataSigmaBN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Inertial Attitude $\sigma_{B/N}$") figureList = {} pltName = fileName + "1" + str(int(useAltBodyFrame)) figureList[pltName] = plt.figure(1) @@ -278,5 +294,5 @@ def run(show_plots, useAltBodyFrame): if __name__ == "__main__": run( True, # show_plots - False # useAltBodyFrame + False, # useAltBodyFrame ) diff --git a/examples/scenarioAttitudeSteering.py b/examples/scenarioAttitudeSteering.py index f2f3ea9aa0..593cfdea44 100755 --- a/examples/scenarioAttitudeSteering.py +++ b/examples/scenarioAttitudeSteering.py @@ -138,13 +138,16 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import attTrackingError from Basilisk.fswAlgorithms import hillPoint + # import FSW Algorithm related support from Basilisk.fswAlgorithms import mrpSteering from Basilisk.fswAlgorithms import rateServoFullNonlinear @@ -152,9 +155,11 @@ from Basilisk.simulation import extForceTorque from Basilisk.simulation import reactionWheelStateEffector from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft from Basilisk.utilities import RigidBodyKinematics as rb + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import fswSetupRW @@ -162,7 +167,10 @@ from Basilisk.utilities import orbitalMotion as om from Basilisk.utilities import simIncludeGravBody from Basilisk.utilities import simIncludeRW -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -174,65 +182,83 @@ def plot_attitude_error(timeData, dataSigmaBR): """Plot the attitude error.""" plt.figure(1) for idx in range(3): - plt.semilogy(timeData, np.abs(dataSigmaBR[:, idx]), - color=unitTestSupport.getLineColor(idx, 3), - label=r'$|\sigma_' + str(idx) + '|$') - plt.legend(loc='upper right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.semilogy( + timeData, + np.abs(dataSigmaBR[:, idx]), + color=unitTestSupport.getLineColor(idx, 3), + label=r"$|\sigma_" + str(idx) + "|$", + ) + plt.legend(loc="upper right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") + def plot_rw_cmd_torque(timeData, dataUsReq, numRW): """plot the commanded RW torque.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") + def plot_rw_motor_torque(timeData, dataRW, numRW): """Plot the actual RW motor torque.""" plt.figure(2) for idx in range(3): - plt.semilogy(timeData, np.abs(dataRW[idx]), - color=unitTestSupport.getLineColor(idx, numRW), - label='$|u_{s,' + str(idx) + '}|$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.semilogy( + timeData, + np.abs(dataRW[idx]), + color=unitTestSupport.getLineColor(idx, numRW), + label="$|u_{s," + str(idx) + "}|$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") + def plot_rate_error(timeData, dataOmegaBR, dataOmegaBRAst): """Plot the body angular velocity tracking errors""" plt.figure(3) for idx in range(3): - plt.semilogy(timeData, np.abs(dataOmegaBR[:, idx]) / macros.D2R, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$|\omega_{BR,' + str(idx) + '}|$') + plt.semilogy( + timeData, + np.abs(dataOmegaBR[:, idx]) / macros.D2R, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$|\omega_{BR," + str(idx) + "}|$", + ) for idx in range(3): - plt.semilogy(timeData, np.abs(dataOmegaBRAst[:, idx]) / macros.D2R, - '--', - color=unitTestSupport.getLineColor(idx, 3) - ) - plt.legend(loc='upper right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error (deg/s) ') + plt.semilogy( + timeData, + np.abs(dataOmegaBRAst[:, idx]) / macros.D2R, + "--", + color=unitTestSupport.getLineColor(idx, 3), + ) + plt.legend(loc="upper right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error (deg/s) ") def plot_rw_speeds(timeData, dataOmegaRW, numRW): """Plot the RW speeds.""" plt.figure(4) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx) + '}$') - plt.legend(loc='upper right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed (RPM) ') - - + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx) + "}$", + ) + plt.legend(loc="upper right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed (RPM) ") def run(show_plots, simCase): @@ -274,7 +300,7 @@ def run(show_plots, simCase): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -282,7 +308,7 @@ def run(show_plots, simCase): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -293,11 +319,13 @@ def run(show_plots, simCase): scObject = spacecraft.Spacecraft() scObject.ModelTag = "spacecraftBody" # define the simulation inertia - I = [500., 0., 0., - 0., 300., 0., - 0., 0., 200.] - scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + I = [500.0, 0.0, 0.0, 0.0, 300.0, 0.0, 0.0, 0.0, 200.0] + scObject.hub.mHub = 750.0 # kg - spacecraft mass + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # clear prior gravitational body and SPICE setup definitions @@ -316,21 +344,24 @@ def run(show_plots, simCase): # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments initOmega = [100.0, 200.0, 300.0] - RW1 = rwFactory.create('Honeywell_HR16' - , [1, 0, 0] - , maxMomentum=50. - , Omega=initOmega[0] # RPM - ) - RW2 = rwFactory.create('Honeywell_HR16' - , [0, 1, 0] - , maxMomentum=50. - , Omega=initOmega[1] # RPM - ) - RW3 = rwFactory.create('Honeywell_HR16' - , [0, 0, 1] - , maxMomentum=50. - , Omega=initOmega[2] # RPM - ) + RW1 = rwFactory.create( + "Honeywell_HR16", + [1, 0, 0], + maxMomentum=50.0, + Omega=initOmega[0], # RPM + ) + RW2 = rwFactory.create( + "Honeywell_HR16", + [0, 1, 0], + maxMomentum=50.0, + Omega=initOmega[1], # RPM + ) + RW3 = rwFactory.create( + "Honeywell_HR16", + [0, 0, 1], + maxMomentum=50.0, + Omega=initOmega[2], # RPM + ) numRW = rwFactory.getNumOfDevices() @@ -386,7 +417,7 @@ def run(show_plots, simCase): else: mrpControl.ignoreOuterLoopFeedforward = True mrpControl.K3 = 0.75 - mrpControl.omega_max = 1. * macros.D2R + mrpControl.omega_max = 1.0 * macros.D2R # setup Rate servo module servo = rateServoFullNonlinear.rateServoFullNonlinear() @@ -395,10 +426,10 @@ def run(show_plots, simCase): if simCase == 1: servo.Ki = -1 else: - servo.Ki = 5. + servo.Ki = 5.0 servo.P = 150.0 - servo.integralLimit = 2. / servo.Ki * 0.1 - servo.knownTorquePntB_B = [0., 0., 0.] + servo.integralLimit = 2.0 / servo.Ki * 0.1 + servo.knownTorquePntB_B = [0.0, 0.0, 0.0] scSim.AddModelToTask(simTaskName, servo) @@ -408,11 +439,7 @@ def run(show_plots, simCase): scSim.AddModelToTask(simTaskName, rwMotorTorqueObj) # Make the RW control all three body axes - controlAxes_B = [ - 1, 0, 0, - 0, 1, 0, - 0, 0, 1 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] rwMotorTorqueObj.controlAxes_B = controlAxes_B # create the FSW vehicle configuration message @@ -446,7 +473,9 @@ def run(show_plots, simCase): # Setup data logging before the simulation is initialized # numDataPoints = 200 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) rwMotorLog = rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) snTransLog = sNavObject.transOutMsg.recorder(samplingTime) @@ -486,13 +515,20 @@ def run(show_plots, simCase): sBN = rb.C2MRP(BH) scObject.hub.sigma_BNInit = [[sBN[0]], [sBN[1]], [sBN[2]]] # sigma_CN_B n = np.sqrt(mu / (oe.a * oe.a * oe.a)) - scObject.hub.omega_BN_BInit = [[n * HN[2, 0]], [n * HN[2, 1]], [n * HN[2, 2]]] # rad/s - omega_CN_B + scObject.hub.omega_BN_BInit = [ + [n * HN[2, 0]], + [n * HN[2, 1]], + [n * HN[2, 2]], + ] # rad/s - omega_CN_B # if this scenario is to interface with the BSK Viz, uncomment the following lines - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - , rwEffectorList=rwStateEffector - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + rwEffectorList=rwStateEffector, + ) # # initialize Simulation @@ -560,5 +596,5 @@ def run(show_plots, simCase): if __name__ == "__main__": run( True, # show_plots - 0 # simCase + 0, # simCase ) diff --git a/examples/scenarioBasicOrbit.py b/examples/scenarioBasicOrbit.py index c90540e66a..3c5ccb684b 100644 --- a/examples/scenarioBasicOrbit.py +++ b/examples/scenarioBasicOrbit.py @@ -185,7 +185,6 @@ """ - # # Basilisk Scenario Script and Integrated Test # @@ -200,6 +199,7 @@ import matplotlib.pyplot as plt import numpy as np + # To play with any scenario scripts as tutorials, you should make a copy of them into a custom folder # outside of the Basilisk directory. # @@ -217,13 +217,21 @@ # import simulation related support from Basilisk.simulation import spacecraft + # general support file with common unit test functions # import general simulation support files -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport, vizSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, + vizSupport, +) # always import the Basilisk messaging support + def run(show_plots, orbitCase, useSphericalHarmonics, planetCase): """ At the end of the python script you can specify the following example parameters. @@ -260,7 +268,7 @@ def run(show_plots, orbitCase, useSphericalHarmonics, planetCase): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # setup the simulation tasks/objects @@ -285,20 +293,24 @@ def run(show_plots, orbitCase, useSphericalHarmonics, planetCase): # overridden. If multiple bodies are simulated, then their positions can be # dynamically updated. See scenarioOrbitMultiBody.py to learn how this is # done via a SPICE object. - if planetCase == 'Mars': + if planetCase == "Mars": planet = gravFactory.createMarsBarycenter() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body if useSphericalHarmonics: - planet.useSphericalHarmonicsGravityModel(bskPath + '/supportData/LocalGravData/GGM2BData.txt', 100) + planet.useSphericalHarmonicsGravityModel( + bskPath + "/supportData/LocalGravData/GGM2BData.txt", 100 + ) else: # Earth planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body if useSphericalHarmonics: # If extra customization is required, see the createEarth() macro to change additional values. # For example, the spherical harmonics are turned off by default. To engage them, the following code # is used - planet.useSphericalHarmonicsGravityModel(bskPath + '/supportData/LocalGravData/GGM03S-J2-only.txt', 2) + planet.useSphericalHarmonicsGravityModel( + bskPath + "/supportData/LocalGravData/GGM03S-J2-only.txt", 2 + ) # The value 2 indicates that the first two harmonics, excluding the 0th order harmonic, # are included. This harmonics data file only includes a zeroth order and J2 term. @@ -320,17 +332,17 @@ def run(show_plots, orbitCase, useSphericalHarmonics, planetCase): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rLEO = 7000. * 1000 # meters - rGEO = 42000. * 1000 # meters - if orbitCase == 'GEO': + rLEO = 7000.0 * 1000 # meters + rGEO = 42000.0 * 1000 # meters + if orbitCase == "GEO": oe.a = rGEO oe.e = 0.00001 oe.i = 0.0 * macros.D2R - elif orbitCase == 'GTO': + elif orbitCase == "GTO": oe.a = (rLEO + rGEO) / 2.0 oe.e = 1.0 - rLEO / oe.a oe.i = 0.0 * macros.D2R - else: # LEO case, default case 0 + else: # LEO case, default case 0 oe.a = rLEO oe.e = 0.0001 oe.i = 33.3 * macros.D2R @@ -338,10 +350,11 @@ def run(show_plots, orbitCase, useSphericalHarmonics, planetCase): oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements # with circular or equatorial orbit, some angles are arbitrary - # To set the spacecraft initial conditions, the following initial position and velocity variables are set: scObject.hub.r_CN_NInit = rN # m - r_BN_N scObject.hub.v_CN_NInit = vN # m/s - v_BN_N @@ -364,9 +377,9 @@ def run(show_plots, orbitCase, useSphericalHarmonics, planetCase): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n if useSphericalHarmonics: - simulationTime = macros.sec2nano(3. * P) + simulationTime = macros.sec2nano(3.0 * P) else: simulationTime = macros.sec2nano(0.75 * P) @@ -381,7 +394,9 @@ def run(show_plots, orbitCase, useSphericalHarmonics, planetCase): # The recorder can be put onto a separate task with its own update rate. However, this can be # trickier to do as the recording timing must be carefully balanced with the module msg writing # to avoid recording an older message. - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) # create a logging task object of the spacecraft output message at the desired down sampling ratio dataRec = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataRec) @@ -395,10 +410,13 @@ def run(show_plots, orbitCase, useSphericalHarmonics, planetCase): # Vizard and played back after running the BSK simulation. # To enable this, uncomment this line: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - # saveFile=__file__ - # liveStream=True - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # saveFile=__file__ + # liveStream=True + ) # The vizInterface module must be built into BSK. This is done if the correct CMake options are selected. # The default CMake will include this vizInterface module in the BSK build. See the BSK HTML documentation on @@ -451,8 +469,18 @@ def run(show_plots, orbitCase, useSphericalHarmonics, planetCase): # the inertial position vector components, while the second plot either shows a planar # orbit view relative to the peri-focal frame (no spherical harmonics), or the # semi-major axis time history plot (with spherical harmonics turned on). - figureList, finalDiff = plotOrbits(dataRec.times(), posData, velData, oe, mu, P, - orbitCase, useSphericalHarmonics, planetCase, planet) + figureList, finalDiff = plotOrbits( + dataRec.times(), + posData, + velData, + oe, + mu, + P, + orbitCase, + useSphericalHarmonics, + planetCase, + planet, + ) if show_plots: plt.show() @@ -463,22 +491,36 @@ def run(show_plots, orbitCase, useSphericalHarmonics, planetCase): return finalDiff, figureList -def plotOrbits(timeAxis, posData, velData, oe, mu, P, orbitCase, useSphericalHarmonics, planetCase, planet): +def plotOrbits( + timeAxis, + posData, + velData, + oe, + mu, + P, + orbitCase, + useSphericalHarmonics, + planetCase, + 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') + ax.ticklabel_format(useOffset=False, style="plain") finalDiff = 0.0 for idx in range(3): - plt.plot(timeAxis * macros.NANO2SEC / P, posData[:, idx] / 1000., - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Inertial Position [km]') + plt.plot( + timeAxis * macros.NANO2SEC / P, + posData[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Inertial Position [km]") figureList = {} pltName = fileName + "1" + orbitCase + str(int(useSphericalHarmonics)) + planetCase figureList[pltName] = plt.figure(1) @@ -492,10 +534,10 @@ def plotOrbits(timeAxis, posData, velData, oe, mu, P, orbitCase, useSphericalHar # draw the planet fig = plt.gcf() ax = fig.gca() - if planetCase == 'Mars': - planetColor = '#884400' + if planetCase == "Mars": + planetColor = "#884400" else: - planetColor = '#008800' + planetColor = "#008800" planetRadius = planet.radEquator / 1000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # draw the actual orbit @@ -505,27 +547,35 @@ def plotOrbits(timeAxis, posData, velData, oe, mu, P, orbitCase, useSphericalHar oeData = orbitalMotion.rv2elem(mu, posData[idx], velData[idx]) 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 - ) + 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('$i_e$ Cord. [km]') - plt.ylabel('$i_p$ Cord. [km]') + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + "--", + color="#555555", + ) + plt.xlabel("$i_e$ Cord. [km]") + plt.ylabel("$i_p$ Cord. [km]") plt.grid() plt.figure(3) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") Deltar = np.empty((0, 3)) E0 = orbitalMotion.f2E(oe.f, oe.e) M0 = orbitalMotion.E2M(E0, oe.e) - n = np.sqrt(mu/(oe.a*oe.a*oe.a)) + n = np.sqrt(mu / (oe.a * oe.a * oe.a)) oe2 = copy(oe) for idx in range(0, len(posData)): M = M0 + n * timeAxis[idx] * macros.NANO2SEC @@ -534,13 +584,18 @@ def plotOrbits(timeAxis, posData, velData, oe, mu, P, orbitCase, useSphericalHar rv, vv = orbitalMotion.elem2rv(mu, oe2) Deltar = np.append(Deltar, [posData[idx] - rv], axis=0) for idx in range(3): - plt.plot(timeAxis * macros.NANO2SEC / P, Deltar[:, idx] , - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\Delta r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Trajectory Differences [m]') - pltName = fileName + "3" + orbitCase + str(int(useSphericalHarmonics)) + planetCase + plt.plot( + timeAxis * macros.NANO2SEC / P, + Deltar[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\Delta r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Trajectory Differences [m]") + pltName = ( + fileName + "3" + orbitCase + str(int(useSphericalHarmonics)) + planetCase + ) figureList[pltName] = plt.figure(3) finalDiff = np.linalg.norm(Deltar[-1]) @@ -549,24 +604,28 @@ def plotOrbits(timeAxis, posData, velData, oe, mu, P, orbitCase, useSphericalHar plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") smaData = [] for idx in range(0, len(posData)): oeData = orbitalMotion.rv2elem(mu, posData[idx], velData[idx]) - smaData.append(oeData.a / 1000.) - plt.plot(timeAxis * macros.NANO2SEC / P, smaData, color='#aa0000', - ) - plt.xlabel('Time [orbits]') - plt.ylabel('SMA [km]') + smaData.append(oeData.a / 1000.0) + plt.plot( + timeAxis * macros.NANO2SEC / P, + smaData, + color="#aa0000", + ) + plt.xlabel("Time [orbits]") + plt.ylabel("SMA [km]") pltName = fileName + "2" + orbitCase + str(int(useSphericalHarmonics)) + planetCase figureList[pltName] = plt.figure(2) return figureList, finalDiff + if __name__ == "__main__": run( - True, # show_plots - 'LEO', # orbit Case (LEO, GTO, GEO) - False, # useSphericalHarmonics - 'Earth' # planetCase (Earth, Mars) + True, # show_plots + "LEO", # orbit Case (LEO, GTO, GEO) + False, # useSphericalHarmonics + "Earth", # planetCase (Earth, Mars) ) diff --git a/examples/scenarioBasicOrbitStream.py b/examples/scenarioBasicOrbitStream.py index c27f005393..45da25eb67 100644 --- a/examples/scenarioBasicOrbitStream.py +++ b/examples/scenarioBasicOrbitStream.py @@ -88,19 +88,37 @@ # import simulation related support from Basilisk.simulation import spacecraft + # general support file with common unit test functions # import general simulation support files -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport, vizSupport, simIncludeThruster) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, + vizSupport, + simIncludeThruster, +) from Basilisk.simulation import simSynch from Basilisk.architecture import messaging from Basilisk.simulation import thrusterDynamicEffector + try: from Basilisk.simulation import vizInterface except ImportError: pass -def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSphericalHarmonics, planetCase): + +def run( + show_plots, + liveStream, + broadcastStream, + timeStep, + orbitCase, + useSphericalHarmonics, + planetCase, +): """ At the end of the python script you can specify the following example parameters. @@ -147,9 +165,7 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric # initialize spacecraft object and set properties scObject = spacecraft.Spacecraft() scObject.ModelTag = "bskSat" - I = [60., 0., 0., - 0., 30., 0., - 0., 0., 40.] + I = [60.0, 0.0, 0.0, 0.0, 30.0, 0.0, 0.0, 0.0, 40.0] scObject.hub.mHub = 50.0 # kg - spacecraft mass scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) @@ -158,17 +174,21 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric # setup Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() - if planetCase == 'Mars': + if planetCase == "Mars": planet = gravFactory.createMarsBarycenter() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body if useSphericalHarmonics: - planet.useSphericalHarmonicsGravityModel(bskPath + '/supportData/LocalGravData/GGM2BData.txt', 100) + planet.useSphericalHarmonicsGravityModel( + bskPath + "/supportData/LocalGravData/GGM2BData.txt", 100 + ) else: # Earth planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body if useSphericalHarmonics: - planet.useSphericalHarmonicsGravityModel(bskPath + '/supportData/LocalGravData/GGM03S-J2-only.txt', 2) + planet.useSphericalHarmonicsGravityModel( + bskPath + "/supportData/LocalGravData/GGM03S-J2-only.txt", 2 + ) mu = planet.mu # attach gravity model to spacecraft @@ -179,25 +199,27 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rLEO = 7000. * 1000 # meters - rGEO = 42000. * 1000 # meters - if orbitCase == 'GEO': + rLEO = 7000.0 * 1000 # meters + rGEO = 42000.0 * 1000 # meters + if orbitCase == "GEO": oe.a = rGEO oe.e = 0.00001 oe.i = 0.0 * macros.D2R - elif orbitCase == 'GTO': + elif orbitCase == "GTO": oe.a = (rLEO + rGEO) / 2.0 oe.e = 1.0 - rLEO / oe.a oe.i = 0.0 * macros.D2R - else: # LEO case, default case 0 - oe.a = 2.5*rLEO + else: # LEO case, default case 0 + oe.a = 2.5 * rLEO oe.e = 0.10 oe.i = 33.3 * macros.D2R oe.Omega = 48.2 * macros.D2R oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements # with circular or equatorial orbit, some angles are arbitrary # @@ -212,7 +234,7 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric # Make a fresh thruster factory instance, this is critical to run multiple times thFactory = simIncludeThruster.thrusterFactory() - thFactory.create('MOOG_Monarc_22_6', [0, 0, 0], [0, -1.5, 0]) + thFactory.create("MOOG_Monarc_22_6", [0, 0, 0], [0, -1.5, 0]) thrModelTag = "ACSThrusterDynamics" thFactory.addToSpacecraft(thrModelTag, thrusterSet, scObject) @@ -224,9 +246,9 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n if useSphericalHarmonics: - simulationTime = macros.sec2nano(3. * P) + simulationTime = macros.sec2nano(3.0 * P) else: simulationTime = macros.sec2nano(1 * P) @@ -237,7 +259,9 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric numDataPoints = 400 else: numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) @@ -253,12 +277,15 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric scSim.AddModelToTask(simTaskName, clockSync) # Configure Vizard, using liveStream and broadcastStream options - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - , thrEffectorList=thrusterSet - , thrColors=vizSupport.toRGBA255("white") - , liveStream=liveStream - , broadcastStream=broadcastStream - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + thrEffectorList=thrusterSet, + thrColors=vizSupport.toRGBA255("white"), + liveStream=liveStream, + broadcastStream=broadcastStream, + ) # Set key listeners viz.settings.keyboardLiveInput = "bxpqz" @@ -318,13 +345,12 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric priorKeyPressTime = dt.datetime.now() while incrementalStopTime < simulationTime: - currState = scObject.scStateOutMsg.read() alt = np.linalg.norm(currState.r_BN_N) - planet.radEquator velNorm = np.linalg.norm(currState.v_BN_N) if vizSupport.vizFound: - hudpanel.displayString = f"HUD\nAltitude: {alt/1000:.2f} km\nInertial Velocity: {velNorm/1000:.2f} km/s" + hudpanel.displayString = f"HUD\nAltitude: {alt / 1000:.2f} km\nInertial Velocity: {velNorm / 1000:.2f} km/s" viz.vizEventDialogs.append(hudpanel) # Here, I only want to run a single BSK timestep before checking for user responses. @@ -348,20 +374,22 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric # Parse keyboard inputs, perform actions now = dt.datetime.now() - if (now - priorKeyPressTime).total_seconds() > 1.0: # check that 1s has passed since last key press - if 'b' in keyInputs: + if ( + now - priorKeyPressTime + ).total_seconds() > 1.0: # check that 1s has passed since last key press + if "b" in keyInputs: print("key - b") priorKeyPressTime = now if not continueBurn: print("burn panel") viz.vizEventDialogs.append(burnpanel) - if 'q' in keyInputs: + if "q" in keyInputs: print("key - q") # Set terminateVizard flag, send to Vizard to cleanly close Vizard and end scenario viz.liveSettings.terminateVizard = True viz.UpdateState(incrementalStopTime) exit(0) - if 'x' in keyInputs: + if "x" in keyInputs: print("key - x") priorKeyPressTime = now if continueBurn: @@ -369,11 +397,11 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric continueBurn = False thrMsgData.OnTimeRequest = [0, 0, 0] thrMsg.write(thrMsgData, incrementalStopTime) - if 'z' in keyInputs: + if "z" in keyInputs: print("key - z") priorKeyPressTime = now vizSupport.endFlag = True # End scenario - if 'p' in keyInputs: + if "p" in keyInputs: print("key - p") priorKeyPressTime = now vizSupport.pauseFlag = not vizSupport.pauseFlag # Toggle @@ -392,7 +420,7 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric print("Cancelling burn.") # Append info panel - if incrementalStopTime == 100*simulationTimeStep: + if incrementalStopTime == 100 * simulationTimeStep: viz.vizEventDialogs.append(infopanel) # Turn on thrusters @@ -416,16 +444,19 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") for idx in range(3): - plt.plot(dataLog.times() * macros.NANO2SEC / P, posData[:, idx] / 1000., - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Inertial Position [km]') + plt.plot( + dataLog.times() * macros.NANO2SEC / P, + posData[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Inertial Position [km]") figureList = {} - pltName = fileName + "1" + orbitCase + str(int(useSphericalHarmonics))+ planetCase + pltName = fileName + "1" + orbitCase + str(int(useSphericalHarmonics)) + planetCase figureList[pltName] = plt.figure(1) if useSphericalHarmonics is False: @@ -437,10 +468,10 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric # draw the planet fig = plt.gcf() ax = fig.gca() - if planetCase == 'Mars': - planetColor = '#884400' + if planetCase == "Mars": + planetColor = "#884400" else: - planetColor = '#008800' + planetColor = "#008800" planetRadius = planet.radEquator / 1000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # draw the actual orbit @@ -450,32 +481,43 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric oeData = orbitalMotion.rv2elem(mu, posData[idx], velData[idx]) 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 - ) + 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('$i_e$ Cord. [km]') - plt.ylabel('$i_p$ Cord. [km]') + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + "--", + color="#555555", + ) + plt.xlabel("$i_e$ Cord. [km]") + plt.ylabel("$i_p$ Cord. [km]") plt.grid() else: plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") smaData = [] for idx in range(0, len(posData)): oeData = orbitalMotion.rv2elem(mu, posData[idx], velData[idx]) - smaData.append(oeData.a / 1000.) - plt.plot(posData[:, 0] * macros.NANO2SEC / P, smaData, color='#aa0000', - ) - plt.xlabel('Time [orbits]') - plt.ylabel('SMA [km]') + smaData.append(oeData.a / 1000.0) + plt.plot( + posData[:, 0] * macros.NANO2SEC / P, + smaData, + color="#aa0000", + ) + plt.xlabel("Time [orbits]") + plt.ylabel("SMA [km]") pltName = fileName + "2" + orbitCase + str(int(useSphericalHarmonics)) + planetCase figureList[pltName] = plt.figure(2) @@ -495,11 +537,11 @@ def run(show_plots, liveStream, broadcastStream, timeStep, orbitCase, useSpheric # if __name__ == "__main__": run( - False, # show_plots - True, # liveStream - True, # broadcastStream - 1.0, # time step (s) - 'LEO', # orbit Case (LEO, GTO, GEO) - False, # useSphericalHarmonics - 'Earth' # planetCase (Earth, Mars) + False, # show_plots + True, # liveStream + True, # broadcastStream + 1.0, # time step (s) + "LEO", # orbit Case (LEO, GTO, GEO) + False, # useSphericalHarmonics + "Earth", # planetCase (Earth, Mars) ) diff --git a/examples/scenarioBskLog.py b/examples/scenarioBskLog.py index f6e5850640..5ebbf5ce69 100755 --- a/examples/scenarioBskLog.py +++ b/examples/scenarioBskLog.py @@ -39,7 +39,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation @@ -49,36 +49,37 @@ from Basilisk.architecture import bskLogging from Basilisk.architecture import messaging + def run(case): """ - At the end of the python script you can specify the following example parameters. + At the end of the python script you can specify the following example parameters. - Args: - case (int): + Args: + case (int): - ====== ======================================== - Int Definition - ====== ======================================== - 0 Uses the BSK default verbosity of DEBUG - 1 Sets the verbosity globally to WARNING - 2 Sets the verbosity the a module to ERROR - ====== ======================================== + ====== ======================================== + Int Definition + ====== ======================================== + 0 Uses the BSK default verbosity of DEBUG + 1 Sets the verbosity globally to WARNING + 2 Sets the verbosity the a module to ERROR + ====== ======================================== - """ + """ if case == 1: # here the verbosity is set globally to WARNING or higher. # This call must be made at the beginning of the script, certainly before # SimulationBaseClass.SimBaseClass() is called. bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -123,7 +124,7 @@ def run(case): unitTestSim.InitializeSimulation() # Set the simulation time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -137,5 +138,5 @@ def run(case): # if __name__ == "__main__": run( - 2, # case 1 uses global default, case 2 use module specific default, case 0 uses compiler default - ) + 2, # case 1 uses global default, case 2 use module specific default, case 0 uses compiler default + ) diff --git a/examples/scenarioCSS.py b/examples/scenarioCSS.py index acbb64cec7..c66b604d5e 100755 --- a/examples/scenarioCSS.py +++ b/examples/scenarioCSS.py @@ -150,7 +150,6 @@ """ - # # Basilisk Scenario Script and Integrated Test # @@ -163,19 +162,25 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.simulation import coarseSunSensor + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion as om -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.utilities import vizSupport bskPath = __path__[0] @@ -202,7 +207,7 @@ def run(show_plots, useCSSConstellation, usePlatform, useEclipse, useKelly): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.sec2nano(300.) + simulationTime = macros.sec2nano(300.0) # # create the simulation process @@ -210,7 +215,7 @@ def run(show_plots, useCSSConstellation, usePlatform, useEclipse, useKelly): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(1.) + simulationTimeStep = macros.sec2nano(1.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -221,20 +226,26 @@ def run(show_plots, useCSSConstellation, usePlatform, useEclipse, useKelly): scObject = spacecraft.Spacecraft() scObject.ModelTag = "spacecraftBody" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] - scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] + scObject.hub.mHub = 750.0 # kg - spacecraft mass + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # # set initial spacecraft states # - scObject.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] # m - r_CN_N - scObject.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] # m/s - v_CN_N - scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] # sigma_BN_B - scObject.hub.omega_BN_BInit = [[0.0], [0.0], [1.*macros.D2R]] # rad/s - omega_BN_B + scObject.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] # m - r_CN_N + scObject.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] # m/s - v_CN_N + scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] # sigma_BN_B + scObject.hub.omega_BN_BInit = [ + [0.0], + [0.0], + [1.0 * macros.D2R], + ] # rad/s - omega_BN_B # add spacecraft object to the simulation process scSim.AddModelToTask(simTaskName, scObject) @@ -242,7 +253,9 @@ def run(show_plots, useCSSConstellation, usePlatform, useEclipse, useKelly): # # create simulation messages # - sunPositionMsgData = messaging.SpicePlanetStateMsgPayload(PositionVector=[0.0, om.AU*1000.0, 0.0]) + sunPositionMsgData = messaging.SpicePlanetStateMsgPayload( + PositionVector=[0.0, om.AU * 1000.0, 0.0] + ) sunPositionMsg = messaging.SpicePlanetStateMsg().write(sunPositionMsgData) if useEclipse: @@ -250,11 +263,11 @@ def run(show_plots, useCSSConstellation, usePlatform, useEclipse, useKelly): eclipseMsg = messaging.EclipseMsg().write(eclipseMsgData) def setupCSS(CSS): - CSS.fov = 80. * macros.D2R + CSS.fov = 80.0 * macros.D2R CSS.scaleFactor = 2.0 CSS.maxOutput = 2.0 CSS.minOutput = 0.5 - CSS.r_B = [2.00131, 2.36638, 1.] + CSS.r_B = [2.00131, 2.36638, 1.0] CSS.sunInMsg.subscribeTo(sunPositionMsg) CSS.stateInMsg.subscribeTo(scObject.scStateOutMsg) if useKelly: @@ -262,10 +275,10 @@ def setupCSS(CSS): if useEclipse: CSS.sunEclipseInMsg.subscribeTo(eclipseMsg) if usePlatform: - CSS.setBodyToPlatformDCM(90. * macros.D2R, 0., 0.) - CSS.theta = -90. * macros.D2R + CSS.setBodyToPlatformDCM(90.0 * macros.D2R, 0.0, 0.0) + CSS.theta = -90.0 * macros.D2R CSS.phi = 0 * macros.D2R - CSS.setUnitDirectionVectorWithPerturbation(0., 0.) + CSS.setUnitDirectionVectorWithPerturbation(0.0, 0.0) else: CSS.nHat_B = np.array([1.0, 0.0, 0.0]) @@ -283,8 +296,8 @@ def setupCSS(CSS): CSS2.CSSGroupID = 0 CSS2.r_B = [-3.05, 0.55, 1.0] if usePlatform: - CSS2.theta = 0.*macros.D2R - CSS2.setUnitDirectionVectorWithPerturbation(0., 0.) + CSS2.theta = 0.0 * macros.D2R + CSS2.setUnitDirectionVectorWithPerturbation(0.0, 0.0) else: CSS2.nHat_B = np.array([0.0, 1.0, 0.0]) @@ -292,11 +305,11 @@ def setupCSS(CSS): CSS3.ModelTag = "CSS3_sensor" setupCSS(CSS3) CSS3.CSSGroupID = 1 - CSS3.fov = 45.0*macros.D2R + CSS3.fov = 45.0 * macros.D2R CSS3.r_B = [-3.05, 0.55, 1.0] if usePlatform: - CSS3.theta = 90. * macros.D2R - CSS3.setUnitDirectionVectorWithPerturbation(0., 0.) + CSS3.theta = 90.0 * macros.D2R + CSS3.setUnitDirectionVectorWithPerturbation(0.0, 0.0) else: CSS3.nHat_B = np.array([-1.0, 0.0, 0.0]) @@ -335,13 +348,21 @@ def setupCSS(CSS): scSim.AddModelToTask(simTaskName, css3Log) # optional saving off to Vizard compatible file - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - # saveFile=__file__, - # liveStream=True, - cssList=[cssList] - ) - vizSupport.setInstrumentGuiSetting(viz, viewCSSPanel=True, viewCSSCoverage=True, - viewCSSBoresight=True, showCSSLabels=True) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # saveFile=__file__, + # liveStream=True, + cssList=[cssList], + ) + vizSupport.setInstrumentGuiSetting( + viz, + viewCSSPanel=True, + viewCSSCoverage=True, + viewCSSBoresight=True, + showCSSLabels=True, + ) # # initialize Simulation @@ -362,7 +383,7 @@ def setupCSS(CSS): dataCSS2 = [] dataCSS3 = [] if useCSSConstellation: - dataCSSArray = cssConstLog.CosValue[:, :len(cssList)] + dataCSSArray = cssConstLog.CosValue[:, : len(cssList)] else: dataCSS1 = css1Log.OutputData dataCSS2 = css2Log.OutputData @@ -373,32 +394,49 @@ def setupCSS(CSS): # plot the results # fileNameString = os.path.basename(os.path.splitext(__file__)[0]) - plt.close("all") # clears out plots from earlier test runs + plt.close("all") # clears out plots from earlier test runs plt.figure(1) if useCSSConstellation: for idx in range(len(cssList)): - plt.plot(cssConstLog.times()*macros.NANO2SEC, dataCSSArray[:, idx], - color=unitTestSupport.getLineColor(idx,3), - label='CSS$_{'+str(idx)+'}$') + plt.plot( + cssConstLog.times() * macros.NANO2SEC, + dataCSSArray[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="CSS$_{" + str(idx) + "}$", + ) else: timeAxis = css1Log.times() - plt.plot(timeAxis * macros.NANO2SEC, dataCSS1, - color=unitTestSupport.getLineColor(0, 3), - label='CSS$_{1}$') - plt.plot(timeAxis * macros.NANO2SEC, dataCSS2, - color=unitTestSupport.getLineColor(1, 3), - label='CSS$_{2}$') - plt.plot(timeAxis * macros.NANO2SEC, dataCSS3, - color=unitTestSupport.getLineColor(2, 3), - label='CSS$_{3}$') - plt.legend(loc='lower right') - plt.xlabel('Time [sec]') - plt.ylabel('CSS Signals ') + plt.plot( + timeAxis * macros.NANO2SEC, + dataCSS1, + color=unitTestSupport.getLineColor(0, 3), + label="CSS$_{1}$", + ) + plt.plot( + timeAxis * macros.NANO2SEC, + dataCSS2, + color=unitTestSupport.getLineColor(1, 3), + label="CSS$_{2}$", + ) + plt.plot( + timeAxis * macros.NANO2SEC, + dataCSS3, + color=unitTestSupport.getLineColor(2, 3), + label="CSS$_{3}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [sec]") + plt.ylabel("CSS Signals ") figureList = {} - pltName = fileNameString+str(int(useCSSConstellation))+str(int(usePlatform))+str(int(useEclipse))+str(int(useKelly)) + pltName = ( + fileNameString + + str(int(useCSSConstellation)) + + str(int(usePlatform)) + + str(int(useEclipse)) + + str(int(useKelly)) + ) figureList[pltName] = plt.figure(1) - if show_plots: plt.show() @@ -414,9 +452,9 @@ def setupCSS(CSS): # if __name__ == "__main__": run( - True, # show_plots - False, # useCSSConstellation - False, # usePlatform - False, # useEclipse - False # useKelly - ) + True, # show_plots + False, # useCSSConstellation + False, # usePlatform + False, # useEclipse + False, # useKelly + ) diff --git a/examples/scenarioCSSFilters.py b/examples/scenarioCSSFilters.py index 591a17b0ed..f4fa4a4807 100755 --- a/examples/scenarioCSSFilters.py +++ b/examples/scenarioCSSFilters.py @@ -258,11 +258,10 @@ # Creation Date: November 20, 2017 # - - import numpy as np from Basilisk import __path__ + bskPath = __path__[0] from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros @@ -271,7 +270,13 @@ from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.simulation import spacecraft, coarseSunSensor -from Basilisk.fswAlgorithms import sunlineUKF, sunlineEKF, okeefeEKF, sunlineSEKF, sunlineSuKF +from Basilisk.fswAlgorithms import ( + sunlineUKF, + sunlineEKF, + okeefeEKF, + sunlineSEKF, + sunlineSuKF, +) from Basilisk.architecture import messaging import SunLineKF_test_utilities as Fplot @@ -284,67 +289,159 @@ def setupUKFData(filterObject): filterObject.kappa = 0.0 filterObject.state = [1.0, 0.1, 0.0, 0.0, 0.01, 0.0] - filterObject.covar = [1., 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1., 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1., 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.02] + filterObject.covar = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.02, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.02, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.02, + ] qNoiseIn = np.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3]*0.017*0.017 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6]*0.0017*0.0017 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 0.017 * 0.017 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 0.0017 * 0.0017 filterObject.qNoise = qNoiseIn.reshape(36).tolist() filterObject.qObsVal = 0.017**2 - filterObject.sensorUseThresh = np.sqrt(filterObject.qObsVal)*5 + filterObject.sensorUseThresh = np.sqrt(filterObject.qObsVal) * 5 def setupEKFData(filterObject): """Setup EKF Filter Data""" filterObject.state = [1.0, 0.1, 0.0, 0.0, 0.01, 0.0] filterObject.x = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - filterObject.covar = [1., 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1., 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1., 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.02] + filterObject.covar = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.02, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.02, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.02, + ] filterObject.qProcVal = 0.001**2 filterObject.qObsVal = 0.017**2 - filterObject.sensorUseThresh = np.sqrt(filterObject.qObsVal)*5 + filterObject.sensorUseThresh = np.sqrt(filterObject.qObsVal) * 5 + + filterObject.eKFSwitch = ( + 5.0 # If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF + ) - filterObject.eKFSwitch = 5. #If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF def setupOEKFData(filterObject): """Setup OEKF Filter Data""" - filterObject.omega = [0., 0., 0.] + filterObject.omega = [0.0, 0.0, 0.0] filterObject.state = [1.0, 0.1, 0.0] filterObject.x = [0.0, 0.0, 0.0] - filterObject.covar = [1., 0.0, 0.0, - 0.0, 1., 0.0, - 0.0, 0.0, 1.] + filterObject.covar = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] filterObject.qProcVal = 0.1**2 - filterObject.qObsVal = 0.017 ** 2 - filterObject.sensorUseThresh = np.sqrt(filterObject.qObsVal)*5 + filterObject.qObsVal = 0.017**2 + filterObject.sensorUseThresh = np.sqrt(filterObject.qObsVal) * 5 + + filterObject.eKFSwitch = ( + 5.0 # If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF + ) - filterObject.eKFSwitch = 5. #If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF def setupSEKFData(filterObject): """Setup SEKF Filter Data""" - filterObject.state = [1.0, 0.1, 0., 0., 0.] + filterObject.state = [1.0, 0.1, 0.0, 0.0, 0.0] filterObject.x = [0.0, 0.0, 0.0, 0.0, 0.0] - filterObject.covar = [1., 0.0, 0.0, 0.0, 0.0, - 0.0, 1., 0.0, 0.0, 0.0, - 0.0, 0.0, 1., 0.0, 0.0, - 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.01] + filterObject.covar = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + ] filterObject.qProcVal = 0.001**2 - filterObject.qObsVal = 0.017 ** 2 - filterObject.sensorUseThresh = np.sqrt(filterObject.qObsVal)*5 + filterObject.qObsVal = 0.017**2 + filterObject.sensorUseThresh = np.sqrt(filterObject.qObsVal) * 5 - filterObject.eKFSwitch = 5. #If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF + filterObject.eKFSwitch = ( + 5.0 # If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF + ) def setupSuKFData(filterObject): @@ -353,21 +450,53 @@ def setupSuKFData(filterObject): filterObject.beta = 2.0 filterObject.kappa = 0.0 - filterObject.stateInit = [1.0, 0.1, 0., 0., 0., 1.] - filterObject.covarInit = [1., 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1., 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1., 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001] + filterObject.stateInit = [1.0, 0.1, 0.0, 0.0, 0.0, 1.0] + filterObject.covarInit = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0001, + ] qNoiseIn = np.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3]*0.001**2 - qNoiseIn[3:5, 3:5] = qNoiseIn[3:5, 3:5]*0.0001**2 - qNoiseIn[5, 5] = qNoiseIn[5, 5]*0.000001**2 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 0.001**2 + qNoiseIn[3:5, 3:5] = qNoiseIn[3:5, 3:5] * 0.0001**2 + qNoiseIn[5, 5] = qNoiseIn[5, 5] * 0.000001**2 filterObject.qNoise = qNoiseIn.reshape(36).tolist() filterObject.qObsVal = 0.017**2 - filterObject.sensorUseThresh = np.sqrt(filterObject.qObsVal)*5 + filterObject.sensorUseThresh = np.sqrt(filterObject.qObsVal) * 5 def run(saveFigures, show_plots, FilterType, simTime): @@ -414,27 +543,32 @@ def run(saveFigures, show_plots, FilterType, simTime): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] - scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] + scObject.hub.mHub = 750.0 # kg - spacecraft mass + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # # set initial spacecraft states # - scObject.hub.r_CN_NInit = [[-om.AU*1000.], [0.0], [0.0]] # m - r_CN_N - scObject.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] # m/s - v_CN_N - scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.]] # sigma_BN_B - scObject.hub.omega_BN_BInit = [[-0.1*macros.D2R], [0.5*macros.D2R], [0.5*macros.D2R]] # rad/s - omega_BN_B + scObject.hub.r_CN_NInit = [[-om.AU * 1000.0], [0.0], [0.0]] # m - r_CN_N + scObject.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] # m/s - v_CN_N + scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] # sigma_BN_B + scObject.hub.omega_BN_BInit = [ + [-0.1 * macros.D2R], + [0.5 * macros.D2R], + [0.5 * macros.D2R], + ] # rad/s - omega_BN_B # add spacecraft object to the simulation process scSim.AddModelToTask(simTaskName, scObject) dataLog = scObject.scStateOutMsg.recorder() scSim.AddModelToTask(simTaskName, dataLog) - # Make a CSS constelation cssConstelation = coarseSunSensor.CSSConstellation() CSSOrientationList = [ @@ -445,18 +579,20 @@ def run(saveFigures, show_plots, FilterType, simTime): [-0.70710678118654746, 0, 0.70710678118654757], [-0.70710678118654746, 0.70710678118654757, 0.0], [-0.70710678118654746, 0, -0.70710678118654757], - [-0.70710678118654746, -0.70710678118654757, 0.0] + [-0.70710678118654746, -0.70710678118654757, 0.0], ] counter = 0 + def setupCSS(CSS): - CSS.minOutput = 0. + CSS.minOutput = 0.0 CSS.senNoiseStd = 0.017 CSS.sunInMsg.subscribeTo(sunMsg) CSS.stateInMsg.subscribeTo(scObject.scStateOutMsg) # Store CSS in registry to prevent garbage collection - if not hasattr(setupCSS, '_css_registry'): + if not hasattr(setupCSS, "_css_registry"): setupCSS._css_registry = [] setupCSS._css_registry.append(CSS) + for CSSHat in CSSOrientationList: newCSS = coarseSunSensor.CoarseSunSensor() newCSS.ModelTag = "CSS" + str(counter) @@ -470,14 +606,14 @@ def setupCSS(CSS): # add the FSW CSS information # cssConstVehicle = messaging.CSSConfigMsgPayload( - nCSS = len(CSSOrientationList), - cssVals = [ + nCSS=len(CSSOrientationList), + cssVals=[ messaging.CSSUnitConfigMsgPayload( nHat_B=CSSHat, CBias=1.0, ) for CSSHat in CSSOrientationList - ] + ], ) cssConstMsg = messaging.CSSConfigMsg().write(cssConstVehicle) @@ -486,7 +622,7 @@ def setupCSS(CSS): # numStates = 6 bVecLogger = None - if FilterType == 'EKF': + if FilterType == "EKF": module = sunlineEKF.sunlineEKF() module.ModelTag = "SunlineEKF" setupEKFData(module) @@ -494,7 +630,7 @@ def setupCSS(CSS): # Add test module to runtime call list scSim.AddModelToTask(simTaskName, module) - if FilterType == 'OEKF': + if FilterType == "OEKF": numStates = 3 module = okeefeEKF.okeefeEKF() @@ -504,7 +640,7 @@ def setupCSS(CSS): # Add test module to runtime call list scSim.AddModelToTask(simTaskName, module) - if FilterType == 'uKF': + if FilterType == "uKF": module = sunlineUKF.sunlineUKF() module.ModelTag = "SunlineUKF" setupUKFData(module) @@ -512,7 +648,7 @@ def setupCSS(CSS): # Add test module to runtime call list scSim.AddModelToTask(simTaskName, module) - if FilterType == 'SEKF': + if FilterType == "SEKF": numStates = 5 module = sunlineSEKF.sunlineSEKF() @@ -521,10 +657,10 @@ def setupCSS(CSS): # Add test module to runtime call list scSim.AddModelToTask(simTaskName, module) - bVecLogger = module.logger('bVec_B', simulationTimeStep) + bVecLogger = module.logger("bVec_B", simulationTimeStep) scSim.AddModelToTask(simTaskName, bVecLogger) - if FilterType == 'SuKF': + if FilterType == "SuKF": numStates = 6 module = sunlineSuKF.sunlineSuKF() module.ModelTag = "SunlineSuKF" @@ -532,7 +668,7 @@ def setupCSS(CSS): # Add test module to runtime call list scSim.AddModelToTask(simTaskName, module) - bVecLogger = module.logger('bVec_B', simulationTimeStep) + bVecLogger = module.logger("bVec_B", simulationTimeStep) scSim.AddModelToTask(simTaskName, bVecLogger) module.cssDataInMsg.subscribeTo(cssConstelation.constellationOutMsg) @@ -572,65 +708,78 @@ def addTimeColumn(time, data): # Get the filter outputs through the messages stateLog = addTimeColumn(timeAxis, filtLog.state[:, range(numStates)]) postFitLog = addTimeColumn(timeAxis, filtLog.postFitRes[:, :8]) - covarLog = addTimeColumn(timeAxis, filtLog.covar[:, range(numStates*numStates)]) + covarLog = addTimeColumn(timeAxis, filtLog.covar[:, range(numStates * numStates)]) obsLog = addTimeColumn(timeAxis, filtLog.numObs) # Get bVec_B through the variable logger bVecLog = None if bVecLogger is None else addTimeColumn(timeAxis, bVecLogger.bVec_B) - dcmLog = np.zeros([len(stateLog[:,0]),3,3]) - omegaExp = np.zeros([len(stateLog[:,0]),3]) - if FilterType == 'SEKF': + dcmLog = np.zeros([len(stateLog[:, 0]), 3, 3]) + omegaExp = np.zeros([len(stateLog[:, 0]), 3]) + if FilterType == "SEKF": dcm = sunlineSEKF.new_doubleArray(3 * 3) for j in range(9): sunlineSEKF.doubleArray_setitem(dcm, j, 0) - for i in range(len(stateLog[:,0])): - sunlineSEKF.sunlineSEKFComputeDCM_BS(stateLog[i,1:4].tolist(), bVecLog[i, 1:4].tolist(), dcm) + for i in range(len(stateLog[:, 0])): + sunlineSEKF.sunlineSEKFComputeDCM_BS( + stateLog[i, 1:4].tolist(), bVecLog[i, 1:4].tolist(), dcm + ) dcmOut = [] for j in range(9): dcmOut.append(sunlineSEKF.doubleArray_getitem(dcm, j)) - dcmLog[i,:,:] = np.array(dcmOut).reshape([3,3]) - omegaExp[i,:] = -np.dot(dcmLog[i,:,:], np.array([0, stateLog[i,4], stateLog[i,5]])) - if FilterType == 'SuKF': + dcmLog[i, :, :] = np.array(dcmOut).reshape([3, 3]) + omegaExp[i, :] = -np.dot( + dcmLog[i, :, :], np.array([0, stateLog[i, 4], stateLog[i, 5]]) + ) + if FilterType == "SuKF": dcm = sunlineSuKF.new_doubleArray(3 * 3) for j in range(9): sunlineSuKF.doubleArray_setitem(dcm, j, 0) - for i in range(len(stateLog[:,0])): - sunlineSuKF.sunlineSuKFComputeDCM_BS(stateLog[i,1:4].tolist(), bVecLog[i, 1:4].tolist(), dcm) + for i in range(len(stateLog[:, 0])): + sunlineSuKF.sunlineSuKFComputeDCM_BS( + stateLog[i, 1:4].tolist(), bVecLog[i, 1:4].tolist(), dcm + ) dcmOut = [] for j in range(9): dcmOut.append(sunlineSuKF.doubleArray_getitem(dcm, j)) - dcmLog[i,:,:] = np.array(dcmOut).reshape([3,3]) - omegaExp[i,:] = np.dot(dcmLog[i,:,:].T,Outomega_BN[i,1:]) - + dcmLog[i, :, :] = np.array(dcmOut).reshape([3, 3]) + omegaExp[i, :] = np.dot(dcmLog[i, :, :].T, Outomega_BN[i, 1:]) sHat_B = np.zeros(np.shape(OutSunPos)) sHatDot_B = np.zeros(np.shape(OutSunPos)) - for i in range(len(OutSunPos[:,0])): - sHat_N = (OutSunPos[i,1:] - Outr_BN_N[i,1:])/np.linalg.norm(OutSunPos[i,1:] - Outr_BN_N[i,1:]) - dcm_BN = rbk.MRP2C(OutSigma_BN[i,1:]) - sHat_B[i,0] = sHatDot_B[i,0]= OutSunPos[i,0] - sHat_B[i,1:] = np.dot(dcm_BN, sHat_N) - sHatDot_B[i,1:] = - np.cross(Outomega_BN[i,1:], sHat_B[i,1:] ) + for i in range(len(OutSunPos[:, 0])): + sHat_N = (OutSunPos[i, 1:] - Outr_BN_N[i, 1:]) / np.linalg.norm( + OutSunPos[i, 1:] - Outr_BN_N[i, 1:] + ) + dcm_BN = rbk.MRP2C(OutSigma_BN[i, 1:]) + sHat_B[i, 0] = sHatDot_B[i, 0] = OutSunPos[i, 0] + sHat_B[i, 1:] = np.dot(dcm_BN, sHat_N) + sHatDot_B[i, 1:] = -np.cross(Outomega_BN[i, 1:], sHat_B[i, 1:]) expected = np.zeros(np.shape(stateLog)) - expected[:,0:4] = sHat_B + expected[:, 0:4] = sHat_B # The OEKF has fewer states - if FilterType != 'OEKF' and FilterType != 'SEKF' and FilterType != 'SuKF': - expected[:, 4:] = sHatDot_B[:,1:] - if FilterType == 'SEKF' or FilterType == 'SuKF': + if FilterType != "OEKF" and FilterType != "SEKF" and FilterType != "SuKF": + expected[:, 4:] = sHatDot_B[:, 1:] + if FilterType == "SEKF" or FilterType == "SuKF": for i in range(len(stateLog[:, 0])): - expected[i, 4] = omegaExp[i,1] - expected[i, 5] = omegaExp[i,2] + expected[i, 4] = omegaExp[i, 1] + expected[i, 5] = omegaExp[i, 2] # plot the results # errorVsTruth = np.copy(stateLog) - errorVsTruth[:,1:] -= expected[:,1:] + errorVsTruth[:, 1:] -= expected[:, 1:] - Fplot.StateErrorCovarPlot(errorVsTruth, covarLog, FilterType, show_plots, saveFigures) - Fplot.StatesVsExpected(stateLog, covarLog, expected, FilterType, show_plots, saveFigures) - Fplot.PostFitResiduals(postFitLog, np.sqrt(module.qObsVal), FilterType, show_plots, saveFigures) + Fplot.StateErrorCovarPlot( + errorVsTruth, covarLog, FilterType, show_plots, saveFigures + ) + Fplot.StatesVsExpected( + stateLog, covarLog, expected, FilterType, show_plots, saveFigures + ) + Fplot.PostFitResiduals( + postFitLog, np.sqrt(module.qObsVal), FilterType, show_plots, saveFigures + ) Fplot.numMeasurements(obsLog, FilterType, show_plots, saveFigures) if show_plots: @@ -639,18 +788,19 @@ def addTimeColumn(time, data): # close the plots being saved off to avoid over-writing old and new figures plt.close("all") - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found return + # # This statement below ensures that the unit test scrip can be run as a # stand-along python script # if __name__ == "__main__": - run(False, # save figures to file - True, # show_plots - 'SuKF', - 400 - ) + run( + False, # save figures to file + True, # show_plots + "SuKF", + 400, + ) diff --git a/examples/scenarioCentralBody.py b/examples/scenarioCentralBody.py index 23346b44c8..04c8a78706 100644 --- a/examples/scenarioCentralBody.py +++ b/examples/scenarioCentralBody.py @@ -65,7 +65,6 @@ """ - # # Basilisk Scenario Script and Integrated Test # @@ -78,6 +77,7 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ @@ -85,10 +85,16 @@ bskPath = __path__[0] # import simulation related support from Basilisk.simulation import spacecraft + # general support file with common unit test functions # import general simulation support files -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, +) from Basilisk.utilities import planetStates from numpy import array from numpy.linalg import norm @@ -103,10 +109,10 @@ def run(show_plots, useCentral): """ - Args: - show_plots (bool): Determines if the script should display plots - useCentral (bool): Specifies if the planet is the center of the coordinate system - """ + Args: + show_plots (bool): Determines if the script should display plots + useCentral (bool): Specifies if the planet is the center of the coordinate system + """ # Create simulation variable names simTaskName = "simTask" @@ -121,7 +127,7 @@ def run(show_plots, useCentral): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -138,13 +144,13 @@ def run(show_plots, useCentral): # setup Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() planet = gravFactory.createEarth() - planet.isCentralBody = useCentral # ensure this is the central gravitational body + planet.isCentralBody = useCentral # ensure this is the central gravitational body mu = planet.mu # set up sun gravFactory.createSun() - #Set up spice with spice time + # Set up spice with spice time UTCInit = "2012 MAY 1 00:28:30.0" spiceObject = gravFactory.createSpiceInterface(time=UTCInit, epochInMsg=True) scSim.AddModelToTask(simTaskName, spiceObject) @@ -157,7 +163,7 @@ def run(show_plots, useCentral): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rLEO = 7000. * 1000 # meters + rLEO = 7000.0 * 1000 # meters oe.a = rLEO oe.e = 0.000001 oe.i = 0.0 * macros.D2R @@ -165,9 +171,11 @@ def run(show_plots, useCentral): oe.omega = 0.0 * macros.D2R oe.f = 0.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - truth_r = norm(rN) #for test results - truth_v = norm(vN) #for test results - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements wrt ECI frame + truth_r = norm(rN) # for test results + truth_v = norm(vN) # for test results + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements wrt ECI frame # # initialize Spacecraft States with the initialization variables @@ -188,10 +196,12 @@ def run(show_plots, useCentral): scObject.hub.r_CN_NInit = rN # m - r_BN_N scObject.hub.v_CN_NInit = vN # m/s - v_BN_N else: - #by default planetstates.planetPositionVelocity returns SSB central ICRS coordinates for the planet at the time + # by default planetstates.planetPositionVelocity returns SSB central ICRS coordinates for the planet at the time # requested. also pck0010.tpc ephemeris file - #look in the function for how to use other ephemeris files, reference frames, and observers - planetPosition, planetVelocity = planetStates.planetPositionVelocity('EARTH', UTCInit) + # look in the function for how to use other ephemeris files, reference frames, and observers + planetPosition, planetVelocity = planetStates.planetPositionVelocity( + "EARTH", UTCInit + ) scObject.hub.r_CN_NInit = rN + array(planetPosition) scObject.hub.v_CN_NInit = vN + array(planetVelocity) # In the above call, the first input is the planet to get the states of and the second is the UTC time @@ -202,26 +212,30 @@ def run(show_plots, useCentral): # zero base. Additionally, it is assumed for this script that the Keplerian orbital elements were given relative # to the Earth Centered Inertial Frame which is aligned with the spice inertial frame. - # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n - simulationTime = macros.sec2nano(0.75*P) + P = 2.0 * np.pi / n + simulationTime = macros.sec2nano(0.75 * P) # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) plLog = spiceObject.planetStateOutMsgs[0].recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) scSim.AddModelToTask(simTaskName, plLog) # if this scenario is to interface with the BSK Viz, uncomment the following line - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation: This function clears the simulation log, and runs the self_init() @@ -253,7 +267,7 @@ def run(show_plots, useCentral): # Plots found when running this scenario are the same as the basic orbit scenario and are included for visual inspection that the results are # roughly the same regardless of the use of a central body. - #bring the s/c pos, vel back to earth relative coordinates to plot + # bring the s/c pos, vel back to earth relative coordinates to plot posData[:] -= earthPositionHistory[:] velData[:] -= earthVelocityHistory[:] @@ -270,14 +284,17 @@ def run(show_plots, useCentral): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") for idx in range(3): - plt.plot(dataLog.times() * macros.NANO2SEC / P, posData[:, idx] / 1000., - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Inertial Position [km]') + plt.plot( + dataLog.times() * macros.NANO2SEC / P, + posData[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Inertial Position [km]") figureList = {} pltName = fileName + "1" + str(int(useCentral)) figureList[pltName] = plt.figure(1) @@ -290,7 +307,7 @@ def run(show_plots, useCentral): # draw the planet fig = plt.gcf() ax = fig.gca() - planetColor = '#008800' + planetColor = "#008800" planetRadius = planet.radEquator / 1000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # draw the actual orbit @@ -300,15 +317,20 @@ def run(show_plots, useCentral): oeData = orbitalMotion.rv2elem(mu, posData[idx], velData[idx]) rData.append(oeData.rmag) fData.append(oeData.f + oeData.omega - oe.omega) - plt.plot(posData[:,0] / 1000, posData[:,1] / 1000, color='#aa0000', linewidth=3.0) + plt.plot(posData[:, 0] / 1000, posData[:, 1] / 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('$i_e$ Cord. [km]') - plt.ylabel('$i_p$ Cord. [km]') + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + "--", + color="#555555", + ) + plt.xlabel("$i_e$ Cord. [km]") + plt.ylabel("$i_p$ Cord. [km]") plt.grid() pltName = fileName + "2" + str(int(useCentral)) figureList[pltName] = plt.figure(2) @@ -321,11 +343,12 @@ def run(show_plots, useCentral): return out_r, out_v, truth_r, truth_v, 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 - False # useCentral + True, # show_plots + False, # useCentral ) diff --git a/examples/scenarioConstrainedDynamics.py b/examples/scenarioConstrainedDynamics.py index 97027e5ae5..bd627ce80a 100644 --- a/examples/scenarioConstrainedDynamics.py +++ b/examples/scenarioConstrainedDynamics.py @@ -67,17 +67,29 @@ # Basilisk imports from Basilisk.architecture import messaging -from Basilisk.utilities import (SimulationBaseClass, orbitalMotion, macros, RigidBodyKinematics) -from Basilisk.simulation import (spacecraft, constraintDynamicEffector, gravityEffector, svIntegrators) +from Basilisk.utilities import ( + SimulationBaseClass, + orbitalMotion, + macros, + RigidBodyKinematics, +) +from Basilisk.simulation import ( + spacecraft, + constraintDynamicEffector, + gravityEffector, + svIntegrators, +) import matplotlib.pyplot as plt # Utility imports import numpy as np from Basilisk import __path__ + bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) + def run(show_plots, env): """ The scenario can be run with the following setup parameters: @@ -115,16 +127,24 @@ def run(show_plots, env): # Define mass properties of the rigid hub of both spacecraft scObject1.hub.mHub = 750.0 scObject1.hub.r_BcB_B = [[0.0], [0.0], [1.0]] - scObject1.hub.IHubPntBc_B = [[600.0, 0.0, 0.0], [0.0, 600.0, 0.0], [0.0, 0.0, 600.0]] + scObject1.hub.IHubPntBc_B = [ + [600.0, 0.0, 0.0], + [0.0, 600.0, 0.0], + [0.0, 0.0, 600.0], + ] scObject2.hub.mHub = 750.0 scObject2.hub.r_BcB_B = [[0.0], [0.0], [1.0]] - scObject2.hub.IHubPntBc_B = [[600.0, 0.0, 0.0], [0.0, 600.0, 0.0], [0.0, 0.0, 600.0]] + scObject2.hub.IHubPntBc_B = [ + [600.0, 0.0, 0.0], + [0.0, 600.0, 0.0], + [0.0, 0.0, 600.0], + ] # Add Earth gravity to the simulation if requested - if env == 'Gravity': + if env == "Gravity": earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # [meters^3/s^2] + earthGravBody.mu = 0.3986004415e15 # [meters^3/s^2] earthGravBody.isCentralBody = True scObject1.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) scObject2.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -137,32 +157,34 @@ def run(show_plots, env): oe.omega = 15.0 * macros.D2R oe.f = 90.0 * macros.D2R r_B2N_N_0, rDot_B2N_N = orbitalMotion.elem2rv(earthGravBody.mu, oe) - else: # If no gravity requested, place in free-floating space - r_B2N_N_0 = np.array([1,1,1]) - rDot_B2N_N = np.array([1,1,1]) + else: # If no gravity requested, place in free-floating space + r_B2N_N_0 = np.array([1, 1, 1]) + rDot_B2N_N = np.array([1, 1, 1]) # With initial attitudes at zero (B1, B2, and N frames all initially aligned) - dir = r_B2N_N_0/np.linalg.norm(r_B2N_N_0) + dir = r_B2N_N_0 / np.linalg.norm(r_B2N_N_0) l = 0.1 - COMoffset = 0.1 # distance from COM to where the arm connects to the spacecraft hub, same for both spacecraft [meters] - r_P1B1_B1 = np.dot(dir,COMoffset) - r_P2B2_B2 = np.dot(-dir,COMoffset) - r_P2P1_B1Init = np.dot(dir,l) + COMoffset = 0.1 # distance from COM to where the arm connects to the spacecraft hub, same for both spacecraft [meters] + r_P1B1_B1 = np.dot(dir, COMoffset) + r_P2B2_B2 = np.dot(-dir, COMoffset) + r_P2P1_B1Init = np.dot(dir, l) r_B1N_N_0 = r_B2N_N_0 + r_P2B2_B2 - r_P2P1_B1Init - r_P1B1_B1 rDot_B1N_N = rDot_B2N_N # Compute rotational states # let C be the frame at the combined COM of the two vehicles - r_CN_N = (r_B1N_N_0 * scObject1.hub.mHub + r_B2N_N_0 * scObject2.hub.mHub) / (scObject1.hub.mHub + scObject2.hub.mHub) + r_CN_N = (r_B1N_N_0 * scObject1.hub.mHub + r_B2N_N_0 * scObject2.hub.mHub) / ( + scObject1.hub.mHub + scObject2.hub.mHub + ) r_B1C_N = r_B1N_N_0 - r_CN_N r_B2C_N = r_B2N_N_0 - r_CN_N # compute relative velocity due to spin and COM offset - target_spin = [0.01,0.01,0.01] + target_spin = [0.01, 0.01, 0.01] omega_CN_N = np.array(target_spin) omega_B1N_B1_0 = omega_CN_N omega_B2N_B2_0 = omega_CN_N - dv_B1C_N = np.cross(omega_CN_N,r_B1C_N) - dv_B2C_N = np.cross(omega_CN_N,r_B2C_N) + dv_B1C_N = np.cross(omega_CN_N, r_B1C_N) + dv_B2C_N = np.cross(omega_CN_N, r_B2C_N) rDot_B1N_N_0 = rDot_B1N_N + dv_B1C_N rDot_B2N_N_0 = rDot_B2N_N + dv_B2C_N @@ -180,7 +202,7 @@ def run(show_plots, env): constraintEffector.setR_P1B1_B1(r_P1B1_B1) constraintEffector.setR_P2B2_B2(r_P2B2_B2) constraintEffector.setR_P2P1_B1Init(r_P2P1_B1Init) - constraintEffector.setAlpha(1E2) + constraintEffector.setAlpha(1e2) constraintEffector.setBeta(1e3) constraintEffector.ModelTag = "constraintEffector" @@ -221,12 +243,14 @@ def run(show_plots, env): r_P2B2_B1 = np.empty(r_B1N_N_hist.shape) sigma_B2B1 = np.empty(sigma_B1N_hist.shape) for i in range(r_B1N_N_hist.shape[0]): - dcm_B1N = RigidBodyKinematics.MRP2C(sigma_B1N_hist[i,:]) - r_B1N_B1[i,:] = dcm_B1N@r_B1N_N_hist[i,:] - r_B2N_B1[i,:] = dcm_B1N@r_B2N_N_hist[i,:] - dcm_NB2 = np.transpose(RigidBodyKinematics.MRP2C(sigma_B2N_hist[i,:])) - r_P2B2_B1[i,:] = dcm_B1N@dcm_NB2@r_P2B2_B2 - sigma_B2B1[i,:] = RigidBodyKinematics.subMRP(sigma_B2N_hist[i,:],sigma_B1N_hist[i,:]) + dcm_B1N = RigidBodyKinematics.MRP2C(sigma_B1N_hist[i, :]) + r_B1N_B1[i, :] = dcm_B1N @ r_B1N_N_hist[i, :] + r_B2N_B1[i, :] = dcm_B1N @ r_B2N_N_hist[i, :] + dcm_NB2 = np.transpose(RigidBodyKinematics.MRP2C(sigma_B2N_hist[i, :])) + r_P2B2_B1[i, :] = dcm_B1N @ dcm_NB2 @ r_P2B2_B2 + sigma_B2B1[i, :] = RigidBodyKinematics.subMRP( + sigma_B2N_hist[i, :], sigma_B1N_hist[i, :] + ) psi_B1 = r_B1N_B1 + r_P1B1_B1 + r_P2P1_B1Init - (r_B2N_B1 + r_P2B2_B1) # @@ -239,34 +263,42 @@ def run(show_plots, env): plt.clf() for i in range(3): plt.semilogy(constraintTimeData, np.abs(psi_B1[:, i])) - plt.semilogy(constraintTimeData, np.linalg.norm(psi_B1,axis=1)) - plt.legend([r'$\psi_1$',r'$\psi_2$',r'$\psi_3$',r'$\psi$ magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'relative connection position: $\psi$ (meters)') - plt.title('Direction Constraint Violation Components') + plt.semilogy(constraintTimeData, np.linalg.norm(psi_B1, axis=1)) + plt.legend([r"$\psi_1$", r"$\psi_2$", r"$\psi_3$", r"$\psi$ magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"relative connection position: $\psi$ (meters)") + plt.title("Direction Constraint Violation Components") pltName = fileName + "directionConstraint" figureList[pltName] = plt.figure(1) plt.figure(2) plt.clf() for i in range(3): - plt.semilogy(constraintTimeData, np.abs(4*np.arctan(sigma_B2B1[:, i]) * macros.R2D)) - plt.semilogy(constraintTimeData, np.linalg.norm(4*np.arctan(sigma_B2B1) * macros.R2D,axis=1)) - plt.legend([r'$\phi_1$',r'$\phi_2$',r'$\phi_3$',r'$\phi$ magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'relative attitude angle: $\phi$ (deg)') - plt.title('Attitude Constraint Violation Components') + plt.semilogy( + constraintTimeData, np.abs(4 * np.arctan(sigma_B2B1[:, i]) * macros.R2D) + ) + plt.semilogy( + constraintTimeData, + np.linalg.norm(4 * np.arctan(sigma_B2B1) * macros.R2D, axis=1), + ) + plt.legend([r"$\phi_1$", r"$\phi_2$", r"$\phi_3$", r"$\phi$ magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"relative attitude angle: $\phi$ (deg)") + plt.title("Attitude Constraint Violation Components") pltName = fileName + "attitudeConstraint" figureList[pltName] = plt.figure(2) if show_plots: plt.show() - plt.close("all") # close the plots being saved off to avoid over-writing old and new figures + plt.close( + "all" + ) # close the plots being saved off to avoid over-writing old and new figures return figureList + if __name__ == "__main__": run( - True, # show_plots: True or False - 'Gravity' # env: either 'Gravity' or 'NoGravity' + True, # show_plots: True or False + "Gravity", # env: either 'Gravity' or 'NoGravity' ) diff --git a/examples/scenarioCustomGravBody.py b/examples/scenarioCustomGravBody.py index 4fc4f0285e..6e110b54d9 100755 --- a/examples/scenarioCustomGravBody.py +++ b/examples/scenarioCustomGravBody.py @@ -84,7 +84,12 @@ import matplotlib.pyplot as plt from Basilisk.simulation import planetEphemeris from Basilisk.simulation import spacecraft -from Basilisk.utilities import (SimulationBaseClass, macros, simIncludeGravBody, vizSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + simIncludeGravBody, + vizSupport, +) from Basilisk.utilities import orbitalMotion from Basilisk.utilities import unitTestSupport @@ -94,7 +99,6 @@ fileName = os.path.basename(os.path.splitext(__file__)[0]) - def run(show_plots): """ The scenarios can be run with the followings setups parameters: @@ -125,7 +129,7 @@ def run(show_plots): # setup celestial object ephemeris module gravBodyEphem = planetEphemeris.PlanetEphemeris() - gravBodyEphem.ModelTag = 'planetEphemeris' + gravBodyEphem.ModelTag = "planetEphemeris" scSim.AddModelToTask(simTaskName, gravBodyEphem) gravBodyEphem.setPlanetNames(planetEphemeris.StringVector(["Itokawa", "earth"])) @@ -133,10 +137,10 @@ def run(show_plots): oeAsteroid = planetEphemeris.ClassicElements() oeAsteroid.a = 1.3241 * orbitalMotion.AU * 1000 # meters oeAsteroid.e = 0.2801 - oeAsteroid.i = 1.6214*macros.D2R - oeAsteroid.Omega = 69.081*macros.D2R - oeAsteroid.omega = 162.82*macros.D2R - oeAsteroid.f = 90.0*macros.D2R + oeAsteroid.i = 1.6214 * macros.D2R + oeAsteroid.Omega = 69.081 * macros.D2R + oeAsteroid.omega = 162.82 * macros.D2R + oeAsteroid.f = 90.0 * macros.D2R oeEarth = planetEphemeris.ClassicElements() oeEarth.a = orbitalMotion.AU * 1000 # meters @@ -147,27 +151,38 @@ def run(show_plots): oeEarth.f = 270.0 * macros.D2R # specify celestial object orbit - gravBodyEphem.planetElements = planetEphemeris.classicElementVector([oeAsteroid, oeEarth]) + gravBodyEphem.planetElements = planetEphemeris.classicElementVector( + [oeAsteroid, oeEarth] + ) # specify celestial object orientation - gravBodyEphem.rightAscension = planetEphemeris.DoubleVector([0.0 * macros.D2R, 0.0 * macros.D2R]) - gravBodyEphem.declination = planetEphemeris.DoubleVector([0.0 * macros.D2R, 0.0 * macros.D2R]) - gravBodyEphem.lst0 = planetEphemeris.DoubleVector([0.0 * macros.D2R, 0.0 * macros.D2R]) + gravBodyEphem.rightAscension = planetEphemeris.DoubleVector( + [0.0 * macros.D2R, 0.0 * macros.D2R] + ) + gravBodyEphem.declination = planetEphemeris.DoubleVector( + [0.0 * macros.D2R, 0.0 * macros.D2R] + ) + gravBodyEphem.lst0 = planetEphemeris.DoubleVector( + [0.0 * macros.D2R, 0.0 * macros.D2R] + ) gravBodyEphem.rotRate = planetEphemeris.DoubleVector( - [360 * macros.D2R / (12.132 * 3600.), 360 * macros.D2R / (24. * 3600.)]) + [360 * macros.D2R / (12.132 * 3600.0), 360 * macros.D2R / (24.0 * 3600.0)] + ) # setup Sun gravity body gravFactory = simIncludeGravBody.gravBodyFactory() gravFactory.createSun() # setup asteroid gravity body - mu = 2.34268 # meters^3/s^2 + mu = 2.34268 # meters^3/s^2 asteroid = gravFactory.createCustomGravObject("Itokawa", mu, radEquator=200) asteroid.isCentralBody = True # ensure this is the central gravitational body asteroid.planetBodyInMsg.subscribeTo(gravBodyEphem.planetOutMsgs[0]) # setup Earth gravity Body - earth = gravFactory.createCustomGravObject("earth", 0.3986004415E+15, radEquator=6378136.6) + earth = gravFactory.createCustomGravObject( + "earth", 0.3986004415e15, radEquator=6378136.6 + ) earth.planetBodyInMsg.subscribeTo(gravBodyEphem.planetOutMsgs[1]) # create SC object @@ -194,7 +209,9 @@ def run(show_plots): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) scRec = scObject.scStateOutMsg.recorder(samplingTime) astRec = gravBodyEphem.planetOutMsgs[0].recorder(samplingTime) scSim.AddModelToTask(simTaskName, scRec) @@ -205,19 +222,26 @@ def run(show_plots): # Note that the gravitational body information is pulled automatically from the spacecraft object(s) # Even if custom gravitational bodies are added, this information is pulled by the method below if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) viz.settings.showSpacecraftLabels = 1 # load CAD for custom gravity model - vizSupport.createCustomModel(viz, - # Specifying relative model path is useful for sharing scenarios and resources: - modelPath=os.path.join("..", "dataForExamples", "Itokawa", "ItokawaHayabusa.obj"), - # Specifying absolute model path is preferable for live-streaming: - # modelPath=os.path.join(path, "dataForExamples", "Itokawa", "ItokawaHayabusa.obj"), - shader=1, - simBodiesToModify=['Itokawa'], - scale=[962, 962, 962]) + vizSupport.createCustomModel( + viz, + # Specifying relative model path is useful for sharing scenarios and resources: + modelPath=os.path.join( + "..", "dataForExamples", "Itokawa", "ItokawaHayabusa.obj" + ), + # Specifying absolute model path is preferable for live-streaming: + # modelPath=os.path.join(path, "dataForExamples", "Itokawa", "ItokawaHayabusa.obj"), + shader=1, + simBodiesToModify=["Itokawa"], + scale=[962, 962, 962], + ) # initialize Simulation scSim.InitializeSimulation() @@ -237,14 +261,17 @@ def run(show_plots): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") for idx in range(3): - plt.plot(timeAxis, posData[:, idx] , - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BI,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [h]') - plt.ylabel('Itokawa Relative Position [m]') + plt.plot( + timeAxis, + posData[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BI," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [h]") + plt.ylabel("Itokawa Relative Position [m]") figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) diff --git a/examples/scenarioDataDemo.py b/examples/scenarioDataDemo.py index cb3b410336..5b410591aa 100644 --- a/examples/scenarioDataDemo.py +++ b/examples/scenarioDataDemo.py @@ -67,6 +67,7 @@ """ + import os import numpy as np @@ -76,6 +77,7 @@ from Basilisk.simulation import simpleStorageUnit from Basilisk.simulation import simpleTransmitter from Basilisk.simulation import spacecraft + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros @@ -89,14 +91,14 @@ def run(show_plots): - taskName = "unitTask" # arbitrary name (don't change) - processname = "TestProcess" # arbitrary name (don't change) + taskName = "unitTask" # arbitrary name (don't change) + processname = "TestProcess" # arbitrary name (don't change) # Create a sim module as an empty container scenarioSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(1.0) # update process rate update time + testProcessRate = macros.sec2nano(1.0) # update process rate update time testProc = scenarioSim.CreateNewProcess(processname) testProc.addTask(scenarioSim.CreateNewTask(taskName, testProcessRate)) @@ -108,30 +110,30 @@ def run(show_plots): gravFactory = simIncludeGravBody.gravBodyFactory() planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu sun = gravFactory.createSun() # attach gravity model to spacecraft gravFactory.addBodiesTo(scObject) # setup Spice interface for some solar system bodies - timeInitString = '2021 MAY 04 07:47:48.965 (UTC)' + timeInitString = "2021 MAY 04 07:47:48.965 (UTC)" spiceObject = gravFactory.createSpiceInterface(time=timeInitString) scenarioSim.AddModelToTask(taskName, spiceObject, -1) # setup orbit using orbitalMotion library oe = orbitalMotion.ClassicElements() - oe.a = astroConstants.REQ_EARTH*1e3 + 400e3 + oe.a = astroConstants.REQ_EARTH * 1e3 + 400e3 oe.e = 0.0 - oe.i = 0.0*macros.D2R + oe.i = 0.0 * macros.D2R - oe.Omega = 0.0*macros.D2R - oe.omega = 0.0*macros.D2R - oe.f = 75.0*macros.D2R + oe.Omega = 0.0 * macros.D2R + oe.omega = 0.0 * macros.D2R + oe.f = 75.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - n = np.sqrt(mu/oe.a/oe.a/oe.a) - P = 2.*np.pi/n + n = np.sqrt(mu / oe.a / oe.a / oe.a) + P = 2.0 * np.pi / n scObject.hub.r_CN_NInit = rN scObject.hub.v_CN_NInit = vN @@ -143,29 +145,29 @@ def run(show_plots): # Create an instrument instrument = simpleInstrument.SimpleInstrument() instrument.ModelTag = "instrument1" - instrument.nodeBaudRate = 1200. # baud + instrument.nodeBaudRate = 1200.0 # baud instrument.nodeDataName = "Instrument 1" # baud scenarioSim.AddModelToTask(taskName, instrument) # Create another instrument instrument2 = simpleInstrument.SimpleInstrument() instrument2.ModelTag = "instrument2" - instrument2.nodeBaudRate = 1200. # baud - instrument2.nodeDataName = "Instrument 2" # baud + instrument2.nodeBaudRate = 1200.0 # baud + instrument2.nodeDataName = "Instrument 2" # baud scenarioSim.AddModelToTask(taskName, instrument2) # Create a "transmitter" transmitter = simpleTransmitter.SimpleTransmitter() transmitter.ModelTag = "transmitter" - transmitter.nodeBaudRate = -16000. # baud - transmitter.packetSize = -1E6 # bits + transmitter.nodeBaudRate = -16000.0 # baud + transmitter.packetSize = -1e6 # bits transmitter.numBuffers = 2 scenarioSim.AddModelToTask(taskName, transmitter) # Create a partitionedStorageUnit and attach the instrument to it dataMonitor = partitionedStorageUnit.PartitionedStorageUnit() dataMonitor.ModelTag = "dataMonitor" - dataMonitor.storageCapacity = 8E9 # bits (1 GB) + dataMonitor.storageCapacity = 8e9 # bits (1 GB) dataMonitor.addDataNodeToModel(instrument.nodeDataOutMsg) dataMonitor.addDataNodeToModel(instrument2.nodeDataOutMsg) dataMonitor.addDataNodeToModel(transmitter.nodeDataOutMsg) @@ -178,7 +180,7 @@ def run(show_plots): # Create a simpleStorageUnit and attach the instrument to it dataMonitor2 = simpleStorageUnit.SimpleStorageUnit() dataMonitor2.ModelTag = "dataMonitor2" - dataMonitor2.storageCapacity = 1E5 # bits + dataMonitor2.storageCapacity = 1e5 # bits dataMonitor2.addDataNodeToModel(instrument.nodeDataOutMsg) dataMonitor2.addDataNodeToModel(instrument2.nodeDataOutMsg) dataMonitor2.addDataNodeToModel(transmitter.nodeDataOutMsg) @@ -195,7 +197,7 @@ def run(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - scenarioSim.ConfigureStopTime(macros.sec2nano(P)) # seconds to stop simulation + scenarioSim.ConfigureStopTime(macros.sec2nano(P)) # seconds to stop simulation # Begin the simulation time run set above scenarioSim.ExecuteSimulation() @@ -214,11 +216,11 @@ def run(show_plots): figureList = {} plt.close("all") # clears out plots from earlier test runs plt.figure(1) - plt.plot(tvec, storageLevel / 8E3, label='Data Unit Total Storage Level (KB)') - plt.plot(tvec, storedData[:, 0] / 8E3, label='Instrument 1 Partition Level (KB)') - plt.plot(tvec, storedData[:, 1] / 8E3, label='Instrument 2 Partition Level (KB)') - plt.xlabel('Time (Hr)') - plt.ylabel('Data Stored (KB)') + plt.plot(tvec, storageLevel / 8e3, label="Data Unit Total Storage Level (KB)") + plt.plot(tvec, storedData[:, 0] / 8e3, label="Instrument 1 Partition Level (KB)") + plt.plot(tvec, storedData[:, 1] / 8e3, label="Instrument 2 Partition Level (KB)") + plt.xlabel("Time (Hr)") + plt.ylabel("Data Stored (KB)") plt.grid(True) plt.legend() @@ -227,9 +229,9 @@ def run(show_plots): plt.figure(2) - plt.plot(tvec, storageNetBaud / 8E3, label='Net Baud Rate (KB/s)') - plt.xlabel('Time (Hr)') - plt.ylabel('Data Rate (KB/s)') + plt.plot(tvec, storageNetBaud / 8e3, label="Net Baud Rate (KB/s)") + plt.xlabel("Time (Hr)") + plt.ylabel("Data Rate (KB/s)") plt.grid(True) plt.legend() @@ -242,6 +244,7 @@ def run(show_plots): return figureList + # This statement below ensures that the unitTestScript can be run as a # stand-alone python script diff --git a/examples/scenarioDataToViz.py b/examples/scenarioDataToViz.py index 860fc40ad5..76353dcfe0 100755 --- a/examples/scenarioDataToViz.py +++ b/examples/scenarioDataToViz.py @@ -74,7 +74,12 @@ import numpy as np from Basilisk.simulation import dataFileToViz from Basilisk.simulation import spacecraft -from Basilisk.utilities import (SimulationBaseClass, macros, simIncludeGravBody, vizSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + simIncludeGravBody, + vizSupport, +) from Basilisk.utilities import unitTestSupport try: @@ -87,7 +92,6 @@ fileName = os.path.basename(os.path.splitext(__file__)[0]) - def run(show_plots, attType): """ The scenarios can be run with the followings setups parameters: @@ -99,13 +103,17 @@ def run(show_plots, attType): path = os.path.dirname(os.path.abspath(__file__)) if attType == 0: - dataFileName = os.path.join(path, "dataForExamples", "scHoldTraj_rotating_MRP.csv") + dataFileName = os.path.join( + path, "dataForExamples", "scHoldTraj_rotating_MRP.csv" + ) elif attType == 1: - dataFileName = os.path.join(path, "dataForExamples", "scHoldTraj_rotating_EP.csv") + dataFileName = os.path.join( + path, "dataForExamples", "scHoldTraj_rotating_EP.csv" + ) else: print("unknown attType variable") exit() - file1 = open(dataFileName, 'r') + file1 = open(dataFileName, "r") Lines = file1.readlines() delimiter = "," t0 = float(Lines[1].split(delimiter)[0]) @@ -160,7 +168,9 @@ def run(show_plots, attType): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = [] for scCounter in range(2): dataLog.append(dataModule.scStateOutMsgs[scCounter].recorder(samplingTime)) @@ -169,32 +179,39 @@ def run(show_plots, attType): # 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: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scList - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scList, + # , saveFile=fileName + ) viz.settings.trueTrajectoryLinesOn = 2 # relative to chief spacecraft viz.settings.showSpacecraftLabels = 1 viz.settings.spacecraftShadowBrightness = 0.2 # load CAD for target spacecraft - vizSupport.createCustomModel(viz, - # Specifying relative model path is useful for sharing scenarios and resources: - modelPath=os.path.join("..", "dataForExamples", "Aura_27.obj"), - # Specifying absolute model path is preferable for live-streaming: - # modelPath=os.path.join(path, "dataForExamples", "Aura_27.obj"), - shader=1, - simBodiesToModify=[scList[1].ModelTag], - rotation=[180. * macros.D2R, 0.0 * macros.D2R, -90. * macros.D2R], - scale=[1, 1, 1]) + vizSupport.createCustomModel( + viz, + # Specifying relative model path is useful for sharing scenarios and resources: + modelPath=os.path.join("..", "dataForExamples", "Aura_27.obj"), + # Specifying absolute model path is preferable for live-streaming: + # modelPath=os.path.join(path, "dataForExamples", "Aura_27.obj"), + shader=1, + simBodiesToModify=[scList[1].ModelTag], + rotation=[180.0 * macros.D2R, 0.0 * macros.D2R, -90.0 * macros.D2R], + scale=[1, 1, 1], + ) # load CAD for servicer spacecraft - vizSupport.createCustomModel(viz, - # Specifying relative model path is useful for sharing scenarios and resources: - modelPath=os.path.join("..", "dataForExamples", "Loral-1300Com-main.obj"), - # Specifying absolute model path is preferable for live-streaming: - # modelPath=os.path.join(path, "dataForExamples", "Loral-1300Com-main.obj"), - simBodiesToModify=[scList[0].ModelTag], - rotation=[0. * macros.D2R, -90.0 * macros.D2R, 0. * macros.D2R], - scale=[0.09, 0.09, 0.09]) + vizSupport.createCustomModel( + viz, + # Specifying relative model path is useful for sharing scenarios and resources: + modelPath=os.path.join("..", "dataForExamples", "Loral-1300Com-main.obj"), + # Specifying absolute model path is preferable for live-streaming: + # modelPath=os.path.join(path, "dataForExamples", "Loral-1300Com-main.obj"), + simBodiesToModify=[scList[0].ModelTag], + rotation=[0.0 * macros.D2R, -90.0 * macros.D2R, 0.0 * macros.D2R], + scale=[0.09, 0.09, 0.09], + ) # over-ride the default to not read the SC states from scObjects, but set them directly # to read from the dataFileToFiz output message @@ -231,15 +248,25 @@ def run(show_plots, attType): for idx in sigmaB1N: sNorm = np.linalg.norm(idx) s1Data.append(sNorm) - plt.plot(timeData, s1Data, color=unitTestSupport.getLineColor(1, 3), label=r'$|\sigma_{B1/N}|$') + plt.plot( + timeData, + s1Data, + color=unitTestSupport.getLineColor(1, 3), + label=r"$|\sigma_{B1/N}|$", + ) s2Data = [] for idx in sigmaB2N: sNorm = np.linalg.norm(idx) s2Data.append(sNorm) - plt.plot(timeData, s2Data, color=unitTestSupport.getLineColor(2, 3), label=r'$|\sigma_{B2/N}|$') - plt.xlabel('Time [h]') - plt.ylabel(r'MRP Norm') - plt.legend(loc='lower right') + plt.plot( + timeData, + s2Data, + color=unitTestSupport.getLineColor(2, 3), + label=r"$|\sigma_{B2/N}|$", + ) + plt.xlabel("Time [h]") + plt.ylabel(r"MRP Norm") + plt.legend(loc="lower right") pltName = fileName + "1" figureList[pltName] = plt.figure(1) @@ -249,13 +276,16 @@ def run(show_plots, attType): rhoData.append(r2 - r1) rhoData = np.array(rhoData) for idx in range(3): - plt.plot(timeData, rhoData[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\rho_{' + str(idx+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [h]') - plt.ylabel(r'$\rho_{S/T}$ (Inertial) [m]') - plt.legend(loc='lower right') + plt.plot( + timeData, + rhoData[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\rho_{" + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [h]") + plt.ylabel(r"$\rho_{S/T}$ (Inertial) [m]") + plt.legend(loc="lower right") pltName = fileName + "2" figureList[pltName] = plt.figure(2) @@ -275,5 +305,5 @@ def run(show_plots, attType): if __name__ == "__main__": run( True, # show_plots - 0 # attitude coordinate type, 0 - MRP, 1 - quaternions + 0, # attitude coordinate type, 0 - MRP, 1 - quaternions ) diff --git a/examples/scenarioDebrisReorbitET.py b/examples/scenarioDebrisReorbitET.py index d390585a30..1781a23367 100644 --- a/examples/scenarioDebrisReorbitET.py +++ b/examples/scenarioDebrisReorbitET.py @@ -74,9 +74,15 @@ 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, deprecated) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, + vizSupport, + deprecated, +) try: from Basilisk.simulation import vizInterface @@ -136,10 +142,14 @@ def run(show_plots): scSim.AddModelToTask(dynTaskName, scObjectDebris) # Create VehicleConfig messages including the S/C mass (for etSphericalControl) - servicerConfigOutData = messaging.VehicleConfigMsgPayload(massSC=scObjectServicer.hub.mHub) + servicerConfigOutData = messaging.VehicleConfigMsgPayload( + massSC=scObjectServicer.hub.mHub + ) servicerVehicleConfigMsg = messaging.VehicleConfigMsg().write(servicerConfigOutData) - debrisConfigOutData = messaging.VehicleConfigMsgPayload(massSC=scObjectDebris.hub.mHub) + debrisConfigOutData = messaging.VehicleConfigMsgPayload( + massSC=scObjectDebris.hub.mHub + ) debrisVehicleConfigMsg = messaging.VehicleConfigMsg().write(debrisConfigOutData) # clear prior gravitational body and SPICE setup definitions @@ -160,25 +170,31 @@ def run(show_plots): scSim.AddModelToTask(dynTaskName, MSMmodule) # define electric potentials - voltServicerInMsgData = messaging.VoltMsgPayload(voltage=25000.) - # [V] servicer potential + voltServicerInMsgData = messaging.VoltMsgPayload(voltage=25000.0) + # [V] servicer potential voltServicerInMsg = messaging.VoltMsg().write(voltServicerInMsgData) - voltDebrisInMsgData = messaging.VoltMsgPayload(voltage=-25000.) - # [V] debris potential + voltDebrisInMsgData = messaging.VoltMsgPayload(voltage=-25000.0) + # [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 = [5.] # radius of sphere is 5m - spPosListDebris = [[0., 0., 0.]] # one sphere located at origin of body frame - rListDebris = [4.] # radius of sphere is 4m + spPosListServicer = [[0.0, 0.0, 0.0]] # one sphere located at origin of body frame + rListServicer = [5.0] # radius of sphere is 5m + spPosListDebris = [[0.0, 0.0, 0.0]] # one sphere located at origin of body frame + rListDebris = [4.0] # radius of sphere is 4m # add spacecraft to state - MSMmodule.addSpacecraftToModel(scObjectServicer.scStateOutMsg, messaging.DoubleVector(rListServicer), - unitTestSupport.npList2EigenXdVector(spPosListServicer)) - MSMmodule.addSpacecraftToModel(scObjectDebris.scStateOutMsg, messaging.DoubleVector(rListDebris), - unitTestSupport.npList2EigenXdVector(spPosListDebris)) + 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) @@ -231,65 +247,77 @@ def run(show_plots): 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) + 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] + 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.theta_r = 0.0 # in-plane rotation angle + etSphericalControlObj.phi_r = 0.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) + 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.a = 42164.0 * 1e3 # [m] geostationary orbit + oe.e = 0.0 + oe.i = 0.0 + oe.Omega = 0.0 oe.omega = 0 - oe.f = 0. + oe.f = 0.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_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 + P = 2.0 * np.pi / n # orbit period # # Setup data logging before the simulation is initialized # numDataPoints = 1000 - simulationTime = macros.sec2nano(1. * P) + simulationTime = macros.sec2nano(1.0 * P) samplingTime = simulationTime // (numDataPoints - 1) dataRecS = scObjectServicer.scStateOutMsg.recorder(samplingTime) dataRecD = scObjectDebris.scStateOutMsg.recorder(samplingTime) @@ -303,7 +331,7 @@ def run(show_plots): msmInfoServicer = vizInterface.MultiShapeInfo() msmInfoServicer.msmChargeInMsg.subscribeTo(MSMmodule.chargeMsmOutMsgs[0]) msmServicerList = [] - for (pos, rad) in zip(spPosListServicer, rListServicer): + for pos, rad in zip(spPosListServicer, rListServicer): msmServicer = vizInterface.MultiShape() msmServicer.position = pos msmServicer.dimensions = [rad, rad, rad] @@ -316,7 +344,7 @@ def run(show_plots): msmInfoDebris = vizInterface.MultiShapeInfo() msmInfoDebris.msmChargeInMsg.subscribeTo(MSMmodule.chargeMsmOutMsgs[1]) msmDebrisList = [] - for (pos, rad) in zip(spPosListDebris, rListDebris): + for pos, rad in zip(spPosListDebris, rListDebris): msmDebris = vizInterface.MultiShape() msmDebris.position = pos msmDebris.dimensions = [rad, rad, rad] @@ -333,10 +361,13 @@ def run(show_plots): vizInterface.MultiSphereInfo() vizInterface.MultiSphereVector() - viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, [scObjectServicer, scObjectDebris] - # , saveFile=fileName - , msmInfoList=[msmInfoServicer, msmInfoDebris] - ) + viz = vizSupport.enableUnityVisualization( + scSim, + dynTaskName, + [scObjectServicer, scObjectDebris], + # , saveFile=fileName + msmInfoList=[msmInfoServicer, msmInfoDebris], + ) # # initialize Simulation @@ -359,7 +390,9 @@ def run(show_plots): np.set_printoptions(precision=16) - figureList = plotOrbits(timeData, posDataS, velDataS, posDataD, velDataD, oe, mu, P, earth) + figureList = plotOrbits( + timeData, posDataS, velDataS, posDataD, velDataD, oe, mu, P, earth + ) if show_plots: plt.show() @@ -376,12 +409,12 @@ def plotOrbits(timeData, posDataS, velDataS, posDataD, velDataD, oe, mu, P, plan plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + 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]') + plt.xlabel("Time [orbits]") + plt.ylabel("Separation [m]") figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) @@ -389,11 +422,11 @@ def plotOrbits(timeData, posDataS, velDataS, posDataD, velDataD, oe, mu, P, plan # draw orbit in perifocal frame p = oe.a * (1 - oe.e * oe.e) plt.figure(2) - plt.axis('equal') + plt.axis("equal") # draw the planet fig = plt.gcf() ax = fig.gca() - planetColor = '#008800' + planetColor = "#008800" planetRadius = planet.radEquator / 1000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # draw the actual orbit @@ -403,16 +436,25 @@ def plotOrbits(timeData, posDataS, velDataS, posDataD, velDataD, oe, mu, P, plan 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) + 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.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" @@ -421,14 +463,19 @@ def plotOrbits(timeData, posDataS, velDataS, posDataD, velDataD, oe, mu, P, plan plt.figure(3) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + 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]') + plt.plot( + timeData * macros.NANO2SEC / P, + (aData - oe.a) / 1000.0, + 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) diff --git a/examples/scenarioDeployingPanel.py b/examples/scenarioDeployingPanel.py index 8e985ec216..e9b8863d59 100644 --- a/examples/scenarioDeployingPanel.py +++ b/examples/scenarioDeployingPanel.py @@ -90,8 +90,14 @@ fileName = os.path.basename(os.path.splitext(__file__)[0]) from Basilisk.simulation import spacecraft -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport, vizSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, + vizSupport, +) from Basilisk.simulation import hingedRigidBodyStateEffector, simpleSolarPanel from Basilisk.simulation import hingedBodyLinearProfiler, hingedRigidBodyMotor import math @@ -115,7 +121,7 @@ def run(show_plots): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # create the spacecraft hub @@ -127,21 +133,21 @@ def run(show_plots): # setup Earth Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() - gravBodies = gravFactory.createBodies('earth', 'sun') - gravBodies['earth'].isCentralBody = True - mu = gravBodies['earth'].mu + gravBodies = gravFactory.createBodies("earth", "sun") + gravBodies["earth"].isCentralBody = True + mu = gravBodies["earth"].mu sun = 1 gravFactory.addBodiesTo(scObject) timeInitString = "2012 MAY 1 00:28:30.0" spiceObject = gravFactory.createSpiceInterface(time=timeInitString, epochInMsg=True) - spiceObject.zeroBase = 'earth' + spiceObject.zeroBase = "earth" scSim.AddModelToTask(simTaskName, spiceObject) # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rLEO = 7000. * 1000 # meters - rGEO = 42000. * 1000 # meters + rLEO = 7000.0 * 1000 # meters + rGEO = 42000.0 * 1000 # meters oe.a = (rLEO + rGEO) / 2.0 oe.e = 1.0 - rLEO / oe.a oe.i = 0.0 * macros.D2R @@ -149,13 +155,19 @@ def run(show_plots): oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements # To set the spacecraft initial conditions, the following initial position and velocity variables are set: scObject.hub.r_CN_NInit = rN # m - r_BN_N scObject.hub.v_CN_NInit = vN # m/s - v_BN_N # point the body 3 axis towards the sun in the inertial n2 direction - scObject.hub.sigma_BNInit = [[math.tan(-90. / 4. * macros.D2R)], [0.0], [0.0]] # sigma_BN_B + scObject.hub.sigma_BNInit = [ + [math.tan(-90.0 / 4.0 * macros.D2R)], + [0.0], + [0.0], + ] # sigma_BN_B scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B # configure panels @@ -164,9 +176,9 @@ def run(show_plots): panel1.mass = 100.0 panel1.IPntS_S = [[100.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] panel1.d = 1.5 - panel1.k = 200. - panel1.c = 20. - panel1.r_HB_B = [[-.5], [0.0], [-1.0]] + panel1.k = 200.0 + panel1.c = 20.0 + panel1.r_HB_B = [[-0.5], [0.0], [-1.0]] panel1.dcm_HB = [[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 1.0]] panel1.thetaInit = -np.pi panel1.thetaDotInit = 0 @@ -176,9 +188,9 @@ def run(show_plots): panel2.mass = 100.0 panel2.IPntS_S = [[100.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] panel2.d = 1.5 - panel2.k = 200. - panel2.c = 20. - panel2.r_HB_B = [[.5], [0.0], [-1.0]] + panel2.k = 200.0 + panel2.c = 20.0 + panel2.r_HB_B = [[0.5], [0.0], [-1.0]] panel2.dcm_HB = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] panel2.thetaInit = -np.pi panel2.thetaDotInit = 0 @@ -213,11 +225,15 @@ def run(show_plots): motor2.P = 10 # derivative gain constant motor1.hingedBodyStateSensedInMsg.subscribeTo(panel1.hingedRigidBodyOutMsg) - motor1.hingedBodyStateReferenceInMsg.subscribeTo(profiler1.hingedRigidBodyReferenceOutMsg) + motor1.hingedBodyStateReferenceInMsg.subscribeTo( + profiler1.hingedRigidBodyReferenceOutMsg + ) panel1.motorTorqueInMsg.subscribeTo(motor1.motorTorqueOutMsg) motor2.hingedBodyStateSensedInMsg.subscribeTo(panel2.hingedRigidBodyOutMsg) - motor2.hingedBodyStateReferenceInMsg.subscribeTo(profiler2.hingedRigidBodyReferenceOutMsg) + motor2.hingedBodyStateReferenceInMsg.subscribeTo( + profiler2.hingedRigidBodyReferenceOutMsg + ) panel2.motorTorqueInMsg.subscribeTo(motor2.motorTorqueOutMsg) # add panel to spacecraft hub @@ -259,7 +275,9 @@ def run(show_plots): # Setup data logging before the simulation is initialized numDataPoints = 1000 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) p1Log = panel1.hingedRigidBodyOutMsg.recorder(samplingTime) prof1Log = profiler1.hingedRigidBodyReferenceOutMsg.recorder(samplingTime) @@ -280,24 +298,31 @@ def run(show_plots): scSim.AddModelToTask(simTaskName, pwr2Log) if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, - [scObject - , [panel1.ModelTag, panel1.hingedRigidBodyConfigLogOutMsg] - , [panel2.ModelTag, panel2.hingedRigidBodyConfigLogOutMsg] - ] - # , saveFile=__file__ - ) - - vizSupport.createCustomModel(viz - , simBodiesToModify=[panel1.ModelTag] - , modelPath="CUBE" - , scale=[3, 1, 0.1] - , color=vizSupport.toRGBA255("blue")) - vizSupport.createCustomModel(viz - , simBodiesToModify=[panel2.ModelTag] - , modelPath="CUBE" - , scale=[3, 1, 0.1] - , color=vizSupport.toRGBA255("blue")) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + [ + scObject, + [panel1.ModelTag, panel1.hingedRigidBodyConfigLogOutMsg], + [panel2.ModelTag, panel2.hingedRigidBodyConfigLogOutMsg], + ], + # , saveFile=__file__ + ) + + vizSupport.createCustomModel( + viz, + simBodiesToModify=[panel1.ModelTag], + modelPath="CUBE", + scale=[3, 1, 0.1], + color=vizSupport.toRGBA255("blue"), + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=[panel2.ModelTag], + modelPath="CUBE", + scale=[3, 1, 0.1], + color=vizSupport.toRGBA255("blue"), + ) viz.settings.orbitLinesOn = -1 scSim.InitializeSimulation() @@ -316,10 +341,17 @@ def run(show_plots): np.set_printoptions(precision=16) - figureList = plotOrbits(dataLog.times(), dataSigmaBN, dataOmegaBN_B, - panel1thetaLog, panel1thetaDotLog, - panel2thetaLog, panel2thetaDotLog, - pwrLog1, pwrLog2) + figureList = plotOrbits( + dataLog.times(), + dataSigmaBN, + dataOmegaBN_B, + panel1thetaLog, + panel1thetaDotLog, + panel2thetaLog, + panel2thetaDotLog, + pwrLog1, + pwrLog2, + ) if show_plots: plt.show() @@ -330,10 +362,17 @@ def run(show_plots): return figureList -def plotOrbits(timeAxis, dataSigmaBN, dataOmegaBN, - panel1thetaLog, panel1thetaDotLog, - panel2thetaLog, panel2thetaDotLog, - pwrLog1, pwrLog2): +def plotOrbits( + timeAxis, + dataSigmaBN, + dataOmegaBN, + panel1thetaLog, + panel1thetaDotLog, + panel2thetaLog, + panel2thetaDotLog, + pwrLog1, + pwrLog2, +): plt.close("all") # clears out plots from earlier test runs # sigma B/N @@ -341,15 +380,18 @@ def plotOrbits(timeAxis, dataSigmaBN, dataOmegaBN, plt.figure(figCounter) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") timeData = timeAxis * macros.NANO2SEC for idx in range(3): - plt.plot(timeData, dataSigmaBN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [s]') - plt.ylabel(r'MRP Attitude $\sigma_{B/N}$') + plt.plot( + timeData, + dataSigmaBN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [s]") + plt.ylabel(r"MRP Attitude $\sigma_{B/N}$") figureList = {} pltName = fileName + str(figCounter) figureList[pltName] = plt.figure(figCounter) @@ -359,14 +401,17 @@ def plotOrbits(timeAxis, dataSigmaBN, dataOmegaBN, plt.figure(figCounter) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") for idx in range(3): - plt.plot(timeData, dataOmegaBN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [s]') - plt.ylabel(r'Rate $\omega_{B/N}$') + plt.plot( + timeData, + dataOmegaBN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [s]") + plt.ylabel(r"Rate $\omega_{B/N}$") pltName = fileName + str(figCounter) figureList[pltName] = plt.figure(figCounter) @@ -374,14 +419,14 @@ def plotOrbits(timeAxis, dataSigmaBN, dataOmegaBN, figCounter += 1 plt.figure(figCounter) ax1 = plt.figure(figCounter).add_subplot(111) - ax1.plot(timeData, panel1thetaLog, color='royalblue') - plt.xlabel('Time [s]') - plt.ylabel('Panel 1 Angle [rad]', color='royalblue') + ax1.plot(timeData, panel1thetaLog, color="royalblue") + plt.xlabel("Time [s]") + plt.ylabel("Panel 1 Angle [rad]", color="royalblue") ax2 = plt.figure(figCounter).add_subplot(111, sharex=ax1, frameon=False) - ax2.plot(timeData, panel1thetaDotLog, color='indianred') + ax2.plot(timeData, panel1thetaDotLog, color="indianred") ax2.yaxis.tick_right() ax2.yaxis.set_label_position("right") - plt.ylabel('Panel 1 Angle Rate [rad/s]', color='indianred') + plt.ylabel("Panel 1 Angle Rate [rad/s]", color="indianred") pltName = fileName + str(figCounter) figureList[pltName] = plt.figure(figCounter) @@ -389,14 +434,14 @@ def plotOrbits(timeAxis, dataSigmaBN, dataOmegaBN, figCounter += 1 plt.figure(figCounter) ax1 = plt.figure(figCounter).add_subplot(111) - ax1.plot(timeData, panel2thetaLog, color='royalblue') - plt.xlabel('Time [s]') - plt.ylabel('Panel 2 Angle [rad]', color='royalblue') + ax1.plot(timeData, panel2thetaLog, color="royalblue") + plt.xlabel("Time [s]") + plt.ylabel("Panel 2 Angle [rad]", color="royalblue") ax2 = plt.figure(figCounter).add_subplot(111, sharex=ax1, frameon=False) - ax2.plot(timeData, panel2thetaDotLog, color='indianred') + ax2.plot(timeData, panel2thetaDotLog, color="indianred") ax2.yaxis.tick_right() ax2.yaxis.set_label_position("right") - plt.ylabel('Panel 2 Angle Rate [rad/s]', color='indianred') + plt.ylabel("Panel 2 Angle Rate [rad/s]", color="indianred") pltName = fileName + str(figCounter) figureList[pltName] = plt.figure(figCounter) @@ -404,11 +449,11 @@ def plotOrbits(timeAxis, dataSigmaBN, dataOmegaBN, figCounter += 1 plt.figure(figCounter) ax1 = plt.figure(figCounter).add_subplot(111) - ax1.plot(timeData, pwrLog1, color='goldenrod', label="Panel 1") - ax1.plot(timeData, pwrLog2, '--', color='goldenrod', label="Panel 2") - plt.xlabel('Time [s]') - plt.ylabel('Panel Power [W]') - plt.legend(loc='lower right') + ax1.plot(timeData, pwrLog1, color="goldenrod", label="Panel 1") + ax1.plot(timeData, pwrLog2, "--", color="goldenrod", label="Panel 2") + plt.xlabel("Time [s]") + plt.ylabel("Panel Power [W]") + plt.legend(loc="lower right") pltName = fileName + str(figCounter) figureList[pltName] = plt.figure(figCounter) diff --git a/examples/scenarioDeployingSolarArrays.py b/examples/scenarioDeployingSolarArrays.py index 9428077eb9..647e23c024 100644 --- a/examples/scenarioDeployingSolarArrays.py +++ b/examples/scenarioDeployingSolarArrays.py @@ -129,6 +129,7 @@ filename = os.path.basename(os.path.splitext(__file__)[0]) path = os.path.dirname(os.path.abspath(filename)) + def run(show_plots): """ The scenario can be run with the followings set up parameter: @@ -166,14 +167,22 @@ def run(show_plots): lengthHub = 4.0 # [m] widthHub = 2.0 # [m] depthHub = 2.0 # [m] - IHub_11 = (1/12) * massHub * (lengthHub * lengthHub + depthHub * depthHub) # [kg m^2] - IHub_22 = (1/12) * massHub * (lengthHub * lengthHub + widthHub * widthHub) # [kg m^2] - IHub_33 = (1/12) * massHub * (widthHub * widthHub + depthHub * depthHub) # [kg m^2] + IHub_11 = ( + (1 / 12) * massHub * (lengthHub * lengthHub + depthHub * depthHub) + ) # [kg m^2] + IHub_22 = ( + (1 / 12) * massHub * (lengthHub * lengthHub + widthHub * widthHub) + ) # [kg m^2] + IHub_33 = ( + (1 / 12) * massHub * (widthHub * widthHub + depthHub * depthHub) + ) # [kg m^2] scObject.hub.mHub = massHub # kg scObject.hub.r_BcB_B = [0.0, 0.0, 0.0] # [m] - scObject.hub.IHubPntBc_B = [[IHub_11, 0.0, 0.0], - [0.0, IHub_22, 0.0], - [0.0, 0.0, IHub_33]] # [kg m^2] (Hub approximated as rectangular prism) + scObject.hub.IHubPntBc_B = [ + [IHub_11, 0.0, 0.0], + [0.0, IHub_22, 0.0], + [0.0, 0.0, IHub_33], + ] # [kg m^2] (Hub approximated as rectangular prism) # Set the initial inertial hub states scObject.hub.r_CN_NInit = [0.0, 0.0, 0.0] @@ -192,8 +201,8 @@ def run(show_plots): # Position vector of solar array frame origin points with respect to hub frame origin point B # expressed in B frame components - rArray1SB_B = np.array([2.0, 0.0, 0.0]) # [m] - rArray2SB_B = np.array([-2.0, 0.0, 0.0]) # [m] + rArray1SB_B = np.array([2.0, 0.0, 0.0]) # [m] + rArray2SB_B = np.array([-2.0, 0.0, 0.0]) # [m] # Position vector of mount frame origin points with respect to hub frame origin point B r_M1B_B = r_M1S1_B + rArray1SB_B # [m] @@ -202,20 +211,33 @@ def run(show_plots): # Array element geometric parameters num_elements = 10 mass_element = 5.0 # [kg] - rot_hat_M = np.array([0.0, 1.0, 0.0]) # Array articulation axis in mount frame components + rot_hat_M = np.array( + [0.0, 1.0, 0.0] + ) # Array articulation axis in mount frame components radius_array = 4.0 # [m] length_element = radius_array # [m] width_element = 2 * radius_array * np.cos(72 * macros.D2R) # [m] thickness_element = 0.01 # [m] - I_element_11 = (1/12) * mass_element * (length_element * length_element - + thickness_element * thickness_element) # [kg m^2] - I_element_22 = (1/12) * mass_element * (length_element * length_element - + width_element * width_element) # [kg m^2] - I_element_33 = (1/12) * mass_element * (width_element * width_element - + thickness_element * thickness_element) # [kg m^2] - IElement_PntPc_P = [[I_element_11, 0.0, 0.0], - [0.0, I_element_22, 0.0], - [0.0, 0.0, I_element_33]] # [kg m^2] (Elements approximated as rectangular prisms) + I_element_11 = ( + (1 / 12) + * mass_element + * (length_element * length_element + thickness_element * thickness_element) + ) # [kg m^2] + I_element_22 = ( + (1 / 12) + * mass_element + * (length_element * length_element + width_element * width_element) + ) # [kg m^2] + I_element_33 = ( + (1 / 12) + * mass_element + * (width_element * width_element + thickness_element * thickness_element) + ) # [kg m^2] + IElement_PntPc_P = [ + [I_element_11, 0.0, 0.0], + [0.0, I_element_22, 0.0], + [0.0, 0.0, I_element_33], + ] # [kg m^2] (Elements approximated as rectangular prisms) # Deployment temporal information ramp_duration = 2.0 # [s] @@ -248,8 +270,12 @@ def run(show_plots): array1ElementList = list() array2ElementList = list() for i in range(num_elements): - array1ElementList.append(prescribedMotionStateEffector.PrescribedMotionStateEffector()) - array2ElementList.append(prescribedMotionStateEffector.PrescribedMotionStateEffector()) + array1ElementList.append( + prescribedMotionStateEffector.PrescribedMotionStateEffector() + ) + array2ElementList.append( + prescribedMotionStateEffector.PrescribedMotionStateEffector() + ) array1ElementList[i].ModelTag = "array1Element" + str(i + 1) array2ElementList[i].ModelTag = "array2Element" + str(i + 1) array1ElementList[i].mass = mass_element # [kg] @@ -258,12 +284,16 @@ def run(show_plots): array2ElementList[i].IPntPc_P = IElement_PntPc_P # [kg m^2] array1ElementList[i].r_MB_B = r_M1B_B # [m] array2ElementList[i].r_MB_B = r_M2B_B # [m] - array1ElementList[i].r_PcP_P = [- radius_array * np.cos(72 * macros.D2R), - 0.0, - (1/3) * radius_array * np.sin(72 * macros.D2R)] # [m] For triangular wedge - array2ElementList[i].r_PcP_P = [radius_array * np.cos(72 * macros.D2R), - 0.0, - (1/3) * radius_array * np.sin(72 * macros.D2R)] # [m] For triangular wedge + array1ElementList[i].r_PcP_P = [ + -radius_array * np.cos(72 * macros.D2R), + 0.0, + (1 / 3) * radius_array * np.sin(72 * macros.D2R), + ] # [m] For triangular wedge + array2ElementList[i].r_PcP_P = [ + radius_array * np.cos(72 * macros.D2R), + 0.0, + (1 / 3) * radius_array * np.sin(72 * macros.D2R), + ] # [m] For triangular wedge array1ElementList[i].r_PM_M = r_PM1_M1Init1 # [m] array2ElementList[i].r_PM_M = r_PM2_M2Init1 # [m] array1ElementList[i].rPrime_PM_M = np.array([0.0, 0.0, 0.0]) # [m/s] @@ -298,8 +328,12 @@ def run(show_plots): array2ElementMessageData.theta = array2ThetaInit1 # [rad] array1ElementMessageData.thetaDot = 0.0 # [rad/s] array2ElementMessageData.thetaDot = 0.0 # [rad/s] - array1ElementRefMsgList1.append(messaging.HingedRigidBodyMsg().write(array1ElementMessageData)) - array2ElementRefMsgList1.append(messaging.HingedRigidBodyMsg().write(array2ElementMessageData)) + array1ElementRefMsgList1.append( + messaging.HingedRigidBodyMsg().write(array1ElementMessageData) + ) + array2ElementRefMsgList1.append( + messaging.HingedRigidBodyMsg().write(array2ElementMessageData) + ) # Create stand-alone element translational state messages array1ElementTranslationMessageData = messaging.PrescribedTranslationMsgPayload() @@ -308,10 +342,18 @@ def run(show_plots): array2ElementTranslationMessageData.r_PM_M = r_PM2_M2Init1 # [m] array1ElementTranslationMessageData.rPrime_PM_M = np.array([0.0, 0.0, 0.0]) # [m/s] array2ElementTranslationMessageData.rPrime_PM_M = np.array([0.0, 0.0, 0.0]) # [m/s] - array1ElementTranslationMessageData.rPrimePrime_PM_M = np.array([0.0, 0.0, 0.0]) # [m/s^2] - array2ElementTranslationMessageData.rPrimePrime_PM_M = np.array([0.0, 0.0, 0.0]) # [m/s^2] - array1ElementTranslationMessage = messaging.PrescribedTranslationMsg().write(array1ElementTranslationMessageData) - array2ElementTranslationMessage = messaging.PrescribedTranslationMsg().write(array2ElementTranslationMessageData) + array1ElementTranslationMessageData.rPrimePrime_PM_M = np.array( + [0.0, 0.0, 0.0] + ) # [m/s^2] + array2ElementTranslationMessageData.rPrimePrime_PM_M = np.array( + [0.0, 0.0, 0.0] + ) # [m/s^2] + array1ElementTranslationMessage = messaging.PrescribedTranslationMsg().write( + array1ElementTranslationMessageData + ) + array2ElementTranslationMessage = messaging.PrescribedTranslationMsg().write( + array2ElementTranslationMessageData + ) # Initialize the prescribedRotation1DOF module configuration data array1MaxRotAccelList1 = [] @@ -321,14 +363,18 @@ def run(show_plots): if j == 0: thetaInit = array1ThetaInit1 # [rad] thetaRef = array1ThetaInit2 # [rad] - thetaDDotMax = np.abs(thetaRef - thetaInit) / ((init_coast_duration * ramp_duration) - + (ramp_duration * ramp_duration)) + thetaDDotMax = np.abs(thetaRef - thetaInit) / ( + (init_coast_duration * ramp_duration) + + (ramp_duration * ramp_duration) + ) array1MaxRotAccelList1.append(thetaDDotMax) else: thetaInit = array2ThetaInit1 # [rad] thetaRef = array2ThetaInit1 # [rad] - thetaDDotMax = np.abs(thetaRef - thetaInit) / ((init_coast_duration * ramp_duration) - + (ramp_duration * ramp_duration)) + thetaDDotMax = np.abs(thetaRef - thetaInit) / ( + (init_coast_duration * ramp_duration) + + (ramp_duration * ramp_duration) + ) array2MaxRotAccelList1.append(thetaDDotMax) array1RotProfilerList = list() @@ -336,8 +382,12 @@ def run(show_plots): for i in range(num_elements): array1RotProfilerList.append(prescribedRotation1DOF.PrescribedRotation1DOF()) array2RotProfilerList.append(prescribedRotation1DOF.PrescribedRotation1DOF()) - array1RotProfilerList[i].ModelTag = "prescribedRotation1DOFArray1Element" + str(i + 1) - array2RotProfilerList[i].ModelTag = "prescribedRotation1DOFArray2Element" + str(i + 1) + array1RotProfilerList[i].ModelTag = "prescribedRotation1DOFArray1Element" + str( + i + 1 + ) + array2RotProfilerList[i].ModelTag = "prescribedRotation1DOFArray2Element" + str( + i + 1 + ) array1RotProfilerList[i].setCoastOptionBangDuration(ramp_duration) # [s] array2RotProfilerList[i].setCoastOptionBangDuration(ramp_duration) # [s] array1RotProfilerList[i].setRotHat_M(rot_hat_M) @@ -349,35 +399,87 @@ def run(show_plots): scSim.AddModelToTask(fswTaskName, array1RotProfilerList[i]) scSim.AddModelToTask(fswTaskName, array2RotProfilerList[i]) - array1RotProfilerList[i].spinningBodyInMsg.subscribeTo(array1ElementRefMsgList1[i]) - array2RotProfilerList[i].spinningBodyInMsg.subscribeTo(array2ElementRefMsgList1[i]) - array1ElementList[i].prescribedRotationInMsg.subscribeTo(array1RotProfilerList[i].prescribedRotationOutMsg) - array2ElementList[i].prescribedRotationInMsg.subscribeTo(array2RotProfilerList[i].prescribedRotationOutMsg) - array1ElementList[i].prescribedTranslationInMsg.subscribeTo(array1ElementTranslationMessage) - array2ElementList[i].prescribedTranslationInMsg.subscribeTo(array2ElementTranslationMessage) + array1RotProfilerList[i].spinningBodyInMsg.subscribeTo( + array1ElementRefMsgList1[i] + ) + array2RotProfilerList[i].spinningBodyInMsg.subscribeTo( + array2ElementRefMsgList1[i] + ) + array1ElementList[i].prescribedRotationInMsg.subscribeTo( + array1RotProfilerList[i].prescribedRotationOutMsg + ) + array2ElementList[i].prescribedRotationInMsg.subscribeTo( + array2RotProfilerList[i].prescribedRotationOutMsg + ) + array1ElementList[i].prescribedTranslationInMsg.subscribeTo( + array1ElementTranslationMessage + ) + array2ElementList[i].prescribedTranslationInMsg.subscribeTo( + array2ElementTranslationMessage + ) # Set up data logging scStateData = scObject.scStateOutMsg.recorder(dataRecRate) - array1Element1PrescribedDataLog = array1RotProfilerList[0].spinningBodyOutMsg.recorder(dataRecRate) - array1Element2PrescribedDataLog = array1RotProfilerList[1].spinningBodyOutMsg.recorder(dataRecRate) - array1Element3PrescribedDataLog = array1RotProfilerList[2].spinningBodyOutMsg.recorder(dataRecRate) - array1Element4PrescribedDataLog = array1RotProfilerList[3].spinningBodyOutMsg.recorder(dataRecRate) - array1Element5PrescribedDataLog = array1RotProfilerList[4].spinningBodyOutMsg.recorder(dataRecRate) - array1Element6PrescribedDataLog = array1RotProfilerList[5].spinningBodyOutMsg.recorder(dataRecRate) - array1Element7PrescribedDataLog = array1RotProfilerList[6].spinningBodyOutMsg.recorder(dataRecRate) - array1Element8PrescribedDataLog = array1RotProfilerList[7].spinningBodyOutMsg.recorder(dataRecRate) - array1Element9PrescribedDataLog = array1RotProfilerList[8].spinningBodyOutMsg.recorder(dataRecRate) - array1Element10PrescribedDataLog = array1RotProfilerList[9].spinningBodyOutMsg.recorder(dataRecRate) - array2Element1PrescribedDataLog = array2RotProfilerList[0].spinningBodyOutMsg.recorder(dataRecRate) - array2Element2PrescribedDataLog = array2RotProfilerList[1].spinningBodyOutMsg.recorder(dataRecRate) - array2Element3PrescribedDataLog = array2RotProfilerList[2].spinningBodyOutMsg.recorder(dataRecRate) - array2Element4PrescribedDataLog = array2RotProfilerList[3].spinningBodyOutMsg.recorder(dataRecRate) - array2Element5PrescribedDataLog = array2RotProfilerList[4].spinningBodyOutMsg.recorder(dataRecRate) - array2Element6PrescribedDataLog = array2RotProfilerList[5].spinningBodyOutMsg.recorder(dataRecRate) - array2Element7PrescribedDataLog = array2RotProfilerList[6].spinningBodyOutMsg.recorder(dataRecRate) - array2Element8PrescribedDataLog = array2RotProfilerList[7].spinningBodyOutMsg.recorder(dataRecRate) - array2Element9PrescribedDataLog = array2RotProfilerList[8].spinningBodyOutMsg.recorder(dataRecRate) - array2Element10PrescribedDataLog = array2RotProfilerList[9].spinningBodyOutMsg.recorder(dataRecRate) + array1Element1PrescribedDataLog = array1RotProfilerList[ + 0 + ].spinningBodyOutMsg.recorder(dataRecRate) + array1Element2PrescribedDataLog = array1RotProfilerList[ + 1 + ].spinningBodyOutMsg.recorder(dataRecRate) + array1Element3PrescribedDataLog = array1RotProfilerList[ + 2 + ].spinningBodyOutMsg.recorder(dataRecRate) + array1Element4PrescribedDataLog = array1RotProfilerList[ + 3 + ].spinningBodyOutMsg.recorder(dataRecRate) + array1Element5PrescribedDataLog = array1RotProfilerList[ + 4 + ].spinningBodyOutMsg.recorder(dataRecRate) + array1Element6PrescribedDataLog = array1RotProfilerList[ + 5 + ].spinningBodyOutMsg.recorder(dataRecRate) + array1Element7PrescribedDataLog = array1RotProfilerList[ + 6 + ].spinningBodyOutMsg.recorder(dataRecRate) + array1Element8PrescribedDataLog = array1RotProfilerList[ + 7 + ].spinningBodyOutMsg.recorder(dataRecRate) + array1Element9PrescribedDataLog = array1RotProfilerList[ + 8 + ].spinningBodyOutMsg.recorder(dataRecRate) + array1Element10PrescribedDataLog = array1RotProfilerList[ + 9 + ].spinningBodyOutMsg.recorder(dataRecRate) + array2Element1PrescribedDataLog = array2RotProfilerList[ + 0 + ].spinningBodyOutMsg.recorder(dataRecRate) + array2Element2PrescribedDataLog = array2RotProfilerList[ + 1 + ].spinningBodyOutMsg.recorder(dataRecRate) + array2Element3PrescribedDataLog = array2RotProfilerList[ + 2 + ].spinningBodyOutMsg.recorder(dataRecRate) + array2Element4PrescribedDataLog = array2RotProfilerList[ + 3 + ].spinningBodyOutMsg.recorder(dataRecRate) + array2Element5PrescribedDataLog = array2RotProfilerList[ + 4 + ].spinningBodyOutMsg.recorder(dataRecRate) + array2Element6PrescribedDataLog = array2RotProfilerList[ + 5 + ].spinningBodyOutMsg.recorder(dataRecRate) + array2Element7PrescribedDataLog = array2RotProfilerList[ + 6 + ].spinningBodyOutMsg.recorder(dataRecRate) + array2Element8PrescribedDataLog = array2RotProfilerList[ + 7 + ].spinningBodyOutMsg.recorder(dataRecRate) + array2Element9PrescribedDataLog = array2RotProfilerList[ + 8 + ].spinningBodyOutMsg.recorder(dataRecRate) + array2Element10PrescribedDataLog = array2RotProfilerList[ + 9 + ].spinningBodyOutMsg.recorder(dataRecRate) scSim.AddModelToTask(fswTaskName, scStateData) scSim.AddModelToTask(fswTaskName, array1Element1PrescribedDataLog) @@ -405,38 +507,57 @@ def run(show_plots): if vizSupport.vizFound: scBodyList = [scObject] for i in range(num_elements): - scBodyList.append(["Array1Element" + str(i+1), array1ElementList[i].prescribedMotionConfigLogOutMsg]) - scBodyList.append(["Array2Element" + str(i+1), array2ElementList[i].prescribedMotionConfigLogOutMsg]) - - viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, scBodyList, - # saveFile=filename - ) + scBodyList.append( + [ + "Array1Element" + str(i + 1), + array1ElementList[i].prescribedMotionConfigLogOutMsg, + ] + ) + scBodyList.append( + [ + "Array2Element" + str(i + 1), + array2ElementList[i].prescribedMotionConfigLogOutMsg, + ] + ) + + viz = vizSupport.enableUnityVisualization( + scSim, + dynTaskName, + scBodyList, + # saveFile=filename + ) viz.settings.showSpacecraftAsSprites = -1 - vizSupport.createCustomModel(viz - , simBodiesToModify=[scObject.ModelTag] - , modelPath="CYLINDER" - , scale=[widthHub, depthHub, lengthHub] - , color=vizSupport.toRGBA255("gray")) + vizSupport.createCustomModel( + viz, + simBodiesToModify=[scObject.ModelTag], + modelPath="CYLINDER", + scale=[widthHub, depthHub, lengthHub], + color=vizSupport.toRGBA255("gray"), + ) for i in range(num_elements): - vizSupport.createCustomModel(viz, - simBodiesToModify=["Array1Element" + str(i+1)], - # Specifying relative model path is useful for sharing scenarios and resources: - modelPath=os.path.join("..", "dataForExamples", "triangularPanel.obj"), - # Specifying absolute model path is preferable for live-streaming: - # modelPath=os.path.join(path, "dataForExamples", "triangularPanel.obj"), - rotation=[-np.pi/2, 0, np.pi/2], - scale=[1.3, 1.3, 1.3], - color=vizSupport.toRGBA255("green")) - vizSupport.createCustomModel(viz, - simBodiesToModify=["Array2Element" + str(i+1)], - # Specifying relative model path is useful for sharing scenarios and resources: - modelPath=os.path.join("..", "dataForExamples", "triangularPanel.obj"), - # Specifying absolute model path is preferable for live-streaming: - # modelPath=os.path.join(path, "dataForExamples", "triangularPanel.obj"), - rotation=[-np.pi/2, 0, np.pi/2], - scale=[1.3, 1.3, 1.3], - color=vizSupport.toRGBA255("blue")) + vizSupport.createCustomModel( + viz, + simBodiesToModify=["Array1Element" + str(i + 1)], + # Specifying relative model path is useful for sharing scenarios and resources: + modelPath=os.path.join("..", "dataForExamples", "triangularPanel.obj"), + # Specifying absolute model path is preferable for live-streaming: + # modelPath=os.path.join(path, "dataForExamples", "triangularPanel.obj"), + rotation=[-np.pi / 2, 0, np.pi / 2], + scale=[1.3, 1.3, 1.3], + color=vizSupport.toRGBA255("green"), + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=["Array2Element" + str(i + 1)], + # Specifying relative model path is useful for sharing scenarios and resources: + modelPath=os.path.join("..", "dataForExamples", "triangularPanel.obj"), + # Specifying absolute model path is preferable for live-streaming: + # modelPath=os.path.join(path, "dataForExamples", "triangularPanel.obj"), + rotation=[-np.pi / 2, 0, np.pi / 2], + scale=[1.3, 1.3, 1.3], + color=vizSupport.toRGBA255("blue"), + ) scSim.InitializeSimulation() simTime1 = init_deploy_duration + 10 # [s] @@ -448,22 +569,31 @@ def run(show_plots): for i in range(num_elements): thetaInit = array1ThetaInit2 # [rad] thetaRef = (36 * i * macros.D2R) + array1ThetaInit2 # [rad] - thetaDDotMax = np.abs(thetaRef - thetaInit) / ((main_coast_duration * ramp_duration) - + (ramp_duration * ramp_duration)) # [rad/s^2] + thetaDDotMax = np.abs(thetaRef - thetaInit) / ( + (main_coast_duration * ramp_duration) + (ramp_duration * ramp_duration) + ) # [rad/s^2] array1MaxRotAccelList2.append(thetaDDotMax) # Update the array 1 stand-alone element translational state messages array1ElementTranslationMessageData = messaging.PrescribedTranslationMsgPayload( - r_PM_M=r_PM1_M1Init2, # [m] + r_PM_M=r_PM1_M1Init2, # [m] rPrime_PM_M=np.array([0.0, 0.0, 0.0]), # [m/s] rPrimePrime_PM_M=np.array([0.0, 0.0, 0.0]), # [m/s^2] ) - array1ElementTranslationMessage = messaging.PrescribedTranslationMsg().write(array1ElementTranslationMessageData) + array1ElementTranslationMessage = messaging.PrescribedTranslationMsg().write( + array1ElementTranslationMessageData + ) array1ElementRefMsgList2 = list() for i in range(num_elements): - array1ElementList[i].prescribedTranslationInMsg.subscribeTo(array1ElementTranslationMessage) - array1ElementList[i].r_PcP_P = [0.0, 0.0, - (2/3) * radius_array * np.sin(72 * macros.D2R)] + array1ElementList[i].prescribedTranslationInMsg.subscribeTo( + array1ElementTranslationMessage + ) + array1ElementList[i].r_PcP_P = [ + 0.0, + 0.0, + -(2 / 3) * radius_array * np.sin(72 * macros.D2R), + ] array1ElementList[i].r_PM_M = r_PM1_M1Init2 # [m] array1ElementList[i].sigma_PM = sigma_PM1Init2 @@ -474,9 +604,13 @@ def run(show_plots): theta=(36 * i * macros.D2R) + array1ThetaInit2, # [rad] thetaDot=0.0, # [rad/s] ) - array1ElementRefMsgList2.append(messaging.HingedRigidBodyMsg().write(array1ElementMessageData)) + array1ElementRefMsgList2.append( + messaging.HingedRigidBodyMsg().write(array1ElementMessageData) + ) - array1RotProfilerList[i].spinningBodyInMsg.subscribeTo(array1ElementRefMsgList2[i]) + array1RotProfilerList[i].spinningBodyInMsg.subscribeTo( + array1ElementRefMsgList2[i] + ) simTime2 = main_deploy_duration + 10 # [s] scSim.ConfigureStopTime(macros.sec2nano(simTime1 + simTime2)) @@ -487,8 +621,9 @@ def run(show_plots): for i in range(num_elements): thetaInit = array2ThetaInit1 # [rad] thetaRef = array2ThetaInit2 # [rad] - thetaDDotMax = np.abs(thetaRef - thetaInit) / ((init_coast_duration * ramp_duration) - + (ramp_duration * ramp_duration)) # [rad/s^2] + thetaDDotMax = np.abs(thetaRef - thetaInit) / ( + (init_coast_duration * ramp_duration) + (ramp_duration * ramp_duration) + ) # [rad/s^2] array2MaxRotAccelList2.append(thetaDDotMax) array2ElementRefMsgList2 = list() @@ -497,11 +632,15 @@ def run(show_plots): array2ElementMessageData = messaging.HingedRigidBodyMsgPayload( theta=array2ThetaInit2, # [rad] - thetaDot=0.0, # [rad/s] + thetaDot=0.0, # [rad/s] + ) + array2ElementRefMsgList2.append( + messaging.HingedRigidBodyMsg().write(array2ElementMessageData) ) - array2ElementRefMsgList2.append(messaging.HingedRigidBodyMsg().write(array2ElementMessageData)) - array2RotProfilerList[i].spinningBodyInMsg.subscribeTo(array2ElementRefMsgList2[i]) + array2RotProfilerList[i].spinningBodyInMsg.subscribeTo( + array2ElementRefMsgList2[i] + ) simTime3 = init_deploy_duration + 10 # [s] scSim.ConfigureStopTime(macros.sec2nano(simTime1 + simTime2 + simTime3)) @@ -512,22 +651,31 @@ def run(show_plots): for i in range(num_elements): thetaInit = array2ThetaInit2 # [rad] thetaRef = (36 * i * macros.D2R) + array2ThetaInit2 # [rad] - thetaDDotMax = np.abs(thetaRef - thetaInit) / ((main_coast_duration * ramp_duration) - + (ramp_duration * ramp_duration)) # [rad/s^2] + thetaDDotMax = np.abs(thetaRef - thetaInit) / ( + (main_coast_duration * ramp_duration) + (ramp_duration * ramp_duration) + ) # [rad/s^2] array2MaxRotAccelList3.append(thetaDDotMax) # Update the array 2 stand-alone element translational state messages array2ElementTranslationMessageData = messaging.PrescribedTranslationMsgPayload( - r_PM_M=r_PM2_M2Init2, # [m] - rPrime_PM_M=np.array([0.0, 0.0, 0.0]), # [m/s] - rPrimePrime_PM_M=np.array([0.0, 0.0, 0.0]), # [m/s^2] + r_PM_M=r_PM2_M2Init2, # [m] + rPrime_PM_M=np.array([0.0, 0.0, 0.0]), # [m/s] + rPrimePrime_PM_M=np.array([0.0, 0.0, 0.0]), # [m/s^2] + ) + array2ElementTranslationMessage = messaging.PrescribedTranslationMsg().write( + array2ElementTranslationMessageData ) - array2ElementTranslationMessage = messaging.PrescribedTranslationMsg().write(array2ElementTranslationMessageData) array2ElementRefMsgList3 = list() for i in range(num_elements): - array2ElementList[i].prescribedTranslationInMsg.subscribeTo(array2ElementTranslationMessage) - array2ElementList[i].r_PcP_P = [0.0, 0.0, - (2/3) * radius_array * np.sin(72 * macros.D2R)] + array2ElementList[i].prescribedTranslationInMsg.subscribeTo( + array2ElementTranslationMessage + ) + array2ElementList[i].r_PcP_P = [ + 0.0, + 0.0, + -(2 / 3) * radius_array * np.sin(72 * macros.D2R), + ] array2ElementList[i].r_PM_M = r_PM2_M2Init2 # [m] array2ElementList[i].sigma_PM = sigma_PM2Init2 @@ -535,12 +683,16 @@ def run(show_plots): array2RotProfilerList[i].setThetaDDotMax(array2MaxRotAccelList3[i]) # [rad/s^2] array2ElementMessageData = messaging.HingedRigidBodyMsgPayload( - theta=(36 * i * macros.D2R) + array2ThetaInit2, # [rad] - thetaDot=0.0, # [rad/s] + theta=(36 * i * macros.D2R) + array2ThetaInit2, # [rad] + thetaDot=0.0, # [rad/s] + ) + array2ElementRefMsgList3.append( + messaging.HingedRigidBodyMsg().write(array2ElementMessageData) ) - array2ElementRefMsgList3.append(messaging.HingedRigidBodyMsg().write(array2ElementMessageData)) - array2RotProfilerList[i].spinningBodyInMsg.subscribeTo(array2ElementRefMsgList3[i]) + array2RotProfilerList[i].spinningBodyInMsg.subscribeTo( + array2ElementRefMsgList3[i] + ) simTime4 = main_deploy_duration + 10 # [s] scSim.ConfigureStopTime(macros.sec2nano(simTime1 + simTime2 + simTime3 + simTime4)) @@ -571,26 +723,66 @@ def run(show_plots): theta_array2Element8 = array2Element8PrescribedDataLog.theta * macros.R2D # [deg] theta_array2Element9 = array2Element9PrescribedDataLog.theta * macros.R2D # [deg] theta_array2Element10 = array2Element10PrescribedDataLog.theta * macros.R2D # [deg] - thetaDot_array1Element1 = array1Element1PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array1Element2 = array1Element2PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array1Element3 = array1Element3PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array1Element4 = array1Element4PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array1Element5 = array1Element5PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array1Element6 = array1Element6PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array1Element7 = array1Element7PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array1Element8 = array1Element8PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array1Element9 = array1Element9PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array1Element10 = array1Element10PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array2Element1 = array2Element1PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array2Element2 = array2Element2PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array2Element3 = array2Element3PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array2Element4 = array2Element4PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array2Element5 = array2Element5PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array2Element6 = array2Element6PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array2Element7 = array2Element7PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array2Element8 = array2Element8PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array2Element9 = array2Element9PrescribedDataLog.thetaDot * macros.R2D # [deg/s] - thetaDot_array2Element10 = array2Element10PrescribedDataLog.thetaDot * macros.R2D # [deg/s] + thetaDot_array1Element1 = ( + array1Element1PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array1Element2 = ( + array1Element2PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array1Element3 = ( + array1Element3PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array1Element4 = ( + array1Element4PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array1Element5 = ( + array1Element5PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array1Element6 = ( + array1Element6PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array1Element7 = ( + array1Element7PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array1Element8 = ( + array1Element8PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array1Element9 = ( + array1Element9PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array1Element10 = ( + array1Element10PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array2Element1 = ( + array2Element1PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array2Element2 = ( + array2Element2PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array2Element3 = ( + array2Element3PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array2Element4 = ( + array2Element4PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array2Element5 = ( + array2Element5PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array2Element6 = ( + array2Element6PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array2Element7 = ( + array2Element7PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array2Element8 = ( + array2Element8PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array2Element9 = ( + array2Element9PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] + thetaDot_array2Element10 = ( + array2Element10PrescribedDataLog.thetaDot * macros.R2D + ) # [deg/s] # Plot the results figureList = {} @@ -599,20 +791,20 @@ def run(show_plots): # Plot array 1 element angles plt.figure(1) plt.clf() - plt.plot(timespan, theta_array1Element1, label=r'$\theta_1$') - plt.plot(timespan, theta_array1Element2, label=r'$\theta_2$') - plt.plot(timespan, theta_array1Element3, label=r'$\theta_3$') - plt.plot(timespan, theta_array1Element4, label=r'$\theta_4$') - plt.plot(timespan, theta_array1Element5, label=r'$\theta_5$') - plt.plot(timespan, theta_array1Element6, label=r'$\theta_6$') - plt.plot(timespan, theta_array1Element7, label=r'$\theta_7$') - plt.plot(timespan, theta_array1Element8, label=r'$\theta_8$') - plt.plot(timespan, theta_array1Element9, label=r'$\theta_9$') - plt.plot(timespan, theta_array1Element10, label=r'$\theta_{10}$') - plt.title(r'Array 1 Element Angles', fontsize=16) - plt.ylabel('(deg)', fontsize=14) - plt.xlabel('Time (min)', fontsize=14) - plt.legend(bbox_to_anchor=(1.25, 0.5), loc='center right', prop={'size': 8}) + plt.plot(timespan, theta_array1Element1, label=r"$\theta_1$") + plt.plot(timespan, theta_array1Element2, label=r"$\theta_2$") + plt.plot(timespan, theta_array1Element3, label=r"$\theta_3$") + plt.plot(timespan, theta_array1Element4, label=r"$\theta_4$") + plt.plot(timespan, theta_array1Element5, label=r"$\theta_5$") + plt.plot(timespan, theta_array1Element6, label=r"$\theta_6$") + plt.plot(timespan, theta_array1Element7, label=r"$\theta_7$") + plt.plot(timespan, theta_array1Element8, label=r"$\theta_8$") + plt.plot(timespan, theta_array1Element9, label=r"$\theta_9$") + plt.plot(timespan, theta_array1Element10, label=r"$\theta_{10}$") + plt.title(r"Array 1 Element Angles", fontsize=16) + plt.ylabel("(deg)", fontsize=14) + plt.xlabel("Time (min)", fontsize=14) + plt.legend(bbox_to_anchor=(1.25, 0.5), loc="center right", prop={"size": 8}) plt.grid(True) pltName = filename + "_Array1ElementTheta" figureList[pltName] = plt.figure(1) @@ -620,20 +812,20 @@ def run(show_plots): # Plot array 2 element angles plt.figure(2) plt.clf() - plt.plot(timespan, theta_array2Element1, label=r'$\theta_1$') - plt.plot(timespan, theta_array2Element2, label=r'$\theta_2$') - plt.plot(timespan, theta_array2Element3, label=r'$\theta_3$') - plt.plot(timespan, theta_array2Element4, label=r'$\theta_4$') - plt.plot(timespan, theta_array2Element5, label=r'$\theta_5$') - plt.plot(timespan, theta_array2Element6, label=r'$\theta_6$') - plt.plot(timespan, theta_array2Element7, label=r'$\theta_7$') - plt.plot(timespan, theta_array2Element8, label=r'$\theta_8$') - plt.plot(timespan, theta_array2Element9, label=r'$\theta_9$') - plt.plot(timespan, theta_array2Element10, label=r'$\theta_{10}$') - plt.title(r'Array 2 Element Angles', fontsize=16) - plt.ylabel('(deg)', fontsize=14) - plt.xlabel('Time (min)', fontsize=14) - plt.legend(bbox_to_anchor=(1.25, 0.5), loc='center right', prop={'size': 8}) + plt.plot(timespan, theta_array2Element1, label=r"$\theta_1$") + plt.plot(timespan, theta_array2Element2, label=r"$\theta_2$") + plt.plot(timespan, theta_array2Element3, label=r"$\theta_3$") + plt.plot(timespan, theta_array2Element4, label=r"$\theta_4$") + plt.plot(timespan, theta_array2Element5, label=r"$\theta_5$") + plt.plot(timespan, theta_array2Element6, label=r"$\theta_6$") + plt.plot(timespan, theta_array2Element7, label=r"$\theta_7$") + plt.plot(timespan, theta_array2Element8, label=r"$\theta_8$") + plt.plot(timespan, theta_array2Element9, label=r"$\theta_9$") + plt.plot(timespan, theta_array2Element10, label=r"$\theta_{10}$") + plt.title(r"Array 2 Element Angles", fontsize=16) + plt.ylabel("(deg)", fontsize=14) + plt.xlabel("Time (min)", fontsize=14) + plt.legend(bbox_to_anchor=(1.25, 0.5), loc="center right", prop={"size": 8}) plt.grid(True) pltName = filename + "_Array2ElementTheta" figureList[pltName] = plt.figure(2) @@ -641,20 +833,20 @@ def run(show_plots): # Plot array 1 element angle rates plt.figure(3) plt.clf() - plt.plot(timespan, thetaDot_array1Element1, label=r'$\dot{\theta}_1$') - plt.plot(timespan, thetaDot_array1Element2, label=r'$\dot{\theta}_2$') - plt.plot(timespan, thetaDot_array1Element3, label=r'$\dot{\theta}_3$') - plt.plot(timespan, thetaDot_array1Element4, label=r'$\dot{\theta}_4$') - plt.plot(timespan, thetaDot_array1Element5, label=r'$\dot{\theta}_5$') - plt.plot(timespan, thetaDot_array1Element6, label=r'$\dot{\theta}_6$') - plt.plot(timespan, thetaDot_array1Element7, label=r'$\dot{\theta}_7$') - plt.plot(timespan, thetaDot_array1Element8, label=r'$\dot{\theta}_8$') - plt.plot(timespan, thetaDot_array1Element9, label=r'$\dot{\theta}_9$') - plt.plot(timespan, thetaDot_array1Element10, label=r'$\dot{\theta}_{10}$') - plt.title(r'Array 1 Element Angle Rates', fontsize=16) - plt.ylabel('(deg/s)', fontsize=14) - plt.xlabel('Time (min)', fontsize=14) - plt.legend(bbox_to_anchor=(1.25, 0.5), loc='center right', prop={'size': 8}) + plt.plot(timespan, thetaDot_array1Element1, label=r"$\dot{\theta}_1$") + plt.plot(timespan, thetaDot_array1Element2, label=r"$\dot{\theta}_2$") + plt.plot(timespan, thetaDot_array1Element3, label=r"$\dot{\theta}_3$") + plt.plot(timespan, thetaDot_array1Element4, label=r"$\dot{\theta}_4$") + plt.plot(timespan, thetaDot_array1Element5, label=r"$\dot{\theta}_5$") + plt.plot(timespan, thetaDot_array1Element6, label=r"$\dot{\theta}_6$") + plt.plot(timespan, thetaDot_array1Element7, label=r"$\dot{\theta}_7$") + plt.plot(timespan, thetaDot_array1Element8, label=r"$\dot{\theta}_8$") + plt.plot(timespan, thetaDot_array1Element9, label=r"$\dot{\theta}_9$") + plt.plot(timespan, thetaDot_array1Element10, label=r"$\dot{\theta}_{10}$") + plt.title(r"Array 1 Element Angle Rates", fontsize=16) + plt.ylabel("(deg/s)", fontsize=14) + plt.xlabel("Time (min)", fontsize=14) + plt.legend(bbox_to_anchor=(1.25, 0.5), loc="center right", prop={"size": 8}) plt.grid(True) pltName = filename + "_Array1ElementThetaDot" figureList[pltName] = plt.figure(3) @@ -662,20 +854,20 @@ def run(show_plots): # Plot array 2 element angle rates plt.figure(4) plt.clf() - plt.plot(timespan, thetaDot_array2Element1, label=r'$\dot{\theta}_1$') - plt.plot(timespan, thetaDot_array2Element2, label=r'$\dot{\theta}_2$') - plt.plot(timespan, thetaDot_array2Element3, label=r'$\dot{\theta}_3$') - plt.plot(timespan, thetaDot_array2Element4, label=r'$\dot{\theta}_4$') - plt.plot(timespan, thetaDot_array2Element5, label=r'$\dot{\theta}_5$') - plt.plot(timespan, thetaDot_array2Element6, label=r'$\dot{\theta}_6$') - plt.plot(timespan, thetaDot_array2Element7, label=r'$\dot{\theta}_7$') - plt.plot(timespan, thetaDot_array2Element8, label=r'$\dot{\theta}_8$') - plt.plot(timespan, thetaDot_array2Element9, label=r'$\dot{\theta}_9$') - plt.plot(timespan, thetaDot_array2Element10, label=r'$\dot{\theta}_{10}$') - plt.title(r'Array 2 Element Angle Rates', fontsize=16) - plt.ylabel('(deg/s)', fontsize=14) - plt.xlabel('Time (min)', fontsize=14) - plt.legend(bbox_to_anchor=(1.25, 0.5), loc='center right', prop={'size': 8}) + plt.plot(timespan, thetaDot_array2Element1, label=r"$\dot{\theta}_1$") + plt.plot(timespan, thetaDot_array2Element2, label=r"$\dot{\theta}_2$") + plt.plot(timespan, thetaDot_array2Element3, label=r"$\dot{\theta}_3$") + plt.plot(timespan, thetaDot_array2Element4, label=r"$\dot{\theta}_4$") + plt.plot(timespan, thetaDot_array2Element5, label=r"$\dot{\theta}_5$") + plt.plot(timespan, thetaDot_array2Element6, label=r"$\dot{\theta}_6$") + plt.plot(timespan, thetaDot_array2Element7, label=r"$\dot{\theta}_7$") + plt.plot(timespan, thetaDot_array2Element8, label=r"$\dot{\theta}_8$") + plt.plot(timespan, thetaDot_array2Element9, label=r"$\dot{\theta}_9$") + plt.plot(timespan, thetaDot_array2Element10, label=r"$\dot{\theta}_{10}$") + plt.title(r"Array 2 Element Angle Rates", fontsize=16) + plt.ylabel("(deg/s)", fontsize=14) + plt.xlabel("Time (min)", fontsize=14) + plt.legend(bbox_to_anchor=(1.25, 0.5), loc="center right", prop={"size": 8}) plt.grid(True) pltName = filename + "_Array2ElementThetaDot" figureList[pltName] = plt.figure(4) @@ -683,13 +875,16 @@ def run(show_plots): # Plot r_BN_N plt.figure(5) plt.clf() - plt.plot(timespan, r_BN_N[:, 0], label=r'$r_{1}$') - plt.plot(timespan, r_BN_N[:, 1], label=r'$r_{2}$') - plt.plot(timespan, r_BN_N[:, 2], label=r'$r_{3}$') - plt.title(r'${}^\mathcal{N} r_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial Trajectory', fontsize=16) - plt.ylabel('(m)', fontsize=14) - plt.xlabel('Time (min)', fontsize=14) - plt.legend(bbox_to_anchor=(1.25, 0.5), loc='center right', prop={'size': 12}) + plt.plot(timespan, r_BN_N[:, 0], label=r"$r_{1}$") + plt.plot(timespan, r_BN_N[:, 1], label=r"$r_{2}$") + plt.plot(timespan, r_BN_N[:, 2], label=r"$r_{3}$") + plt.title( + r"${}^\mathcal{N} r_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial Trajectory", + fontsize=16, + ) + plt.ylabel("(m)", fontsize=14) + plt.xlabel("Time (min)", fontsize=14) + plt.legend(bbox_to_anchor=(1.25, 0.5), loc="center right", prop={"size": 12}) plt.grid(True) pltName = filename + "_HubInertialPosition" figureList[pltName] = plt.figure(5) @@ -697,13 +892,16 @@ def run(show_plots): # Plot sigma_BN plt.figure(6) plt.clf() - plt.plot(timespan, sigma_BN[:, 0], label=r'$\sigma_{1}$') - plt.plot(timespan, sigma_BN[:, 1], label=r'$\sigma_{2}$') - plt.plot(timespan, sigma_BN[:, 2], label=r'$\sigma_{3}$') - plt.title(r'$\sigma_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial MRP Attitude', fontsize=16) - plt.ylabel('', fontsize=14) - plt.xlabel('Time (min)', fontsize=14) - plt.legend(bbox_to_anchor=(1.25, 0.5), loc='center right', prop={'size': 12}) + plt.plot(timespan, sigma_BN[:, 0], label=r"$\sigma_{1}$") + plt.plot(timespan, sigma_BN[:, 1], label=r"$\sigma_{2}$") + plt.plot(timespan, sigma_BN[:, 2], label=r"$\sigma_{3}$") + plt.title( + r"$\sigma_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial MRP Attitude", + fontsize=16, + ) + plt.ylabel("", fontsize=14) + plt.xlabel("Time (min)", fontsize=14) + plt.legend(bbox_to_anchor=(1.25, 0.5), loc="center right", prop={"size": 12}) plt.grid(True) pltName = filename + "_HubInertialMRPAttitude" figureList[pltName] = plt.figure(6) @@ -711,13 +909,16 @@ def run(show_plots): # Plot omega_BN_B plt.figure(7) plt.clf() - plt.plot(timespan, omega_BN_B[:, 0], label=r'$\omega_{1}$') - plt.plot(timespan, omega_BN_B[:, 1], label=r'$\omega_{2}$') - plt.plot(timespan, omega_BN_B[:, 2], label=r'$\omega_{3}$') - plt.title(r'Spacecraft Hub Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$', fontsize=16) - plt.xlabel('Time (min)', fontsize=14) - plt.ylabel('(deg/s)', fontsize=14) - plt.legend(bbox_to_anchor=(1.25, 0.5), loc='center right', prop={'size': 12}) + plt.plot(timespan, omega_BN_B[:, 0], label=r"$\omega_{1}$") + plt.plot(timespan, omega_BN_B[:, 1], label=r"$\omega_{2}$") + plt.plot(timespan, omega_BN_B[:, 2], label=r"$\omega_{3}$") + plt.title( + r"Spacecraft Hub Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$", + fontsize=16, + ) + plt.xlabel("Time (min)", fontsize=14) + plt.ylabel("(deg/s)", fontsize=14) + plt.legend(bbox_to_anchor=(1.25, 0.5), loc="center right", prop={"size": 12}) plt.grid(True) pltName = filename + "_HubInertialAngularVelocity" figureList[pltName] = plt.figure(7) @@ -727,9 +928,12 @@ def run(show_plots): plt.figure(8) plt.clf() plt.plot(timespan, omega_BN_BNorm) - plt.title(r'Hub Angular Velocity Norm $|{}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}|$', fontsize=16) - plt.ylabel(r'(deg/s)', fontsize=14) - plt.xlabel(r'(min)', fontsize=14) + plt.title( + r"Hub Angular Velocity Norm $|{}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}|$", + fontsize=16, + ) + plt.ylabel(r"(deg/s)", fontsize=14) + plt.xlabel(r"(min)", fontsize=14) plt.grid(True) pltName = filename + "_HubInertialAngularVelocityNorm" figureList[pltName] = plt.figure(8) @@ -740,7 +944,8 @@ def run(show_plots): return figureList + if __name__ == "__main__": run( - True, # show_plots + True, # show_plots ) diff --git a/examples/scenarioDragDeorbit.py b/examples/scenarioDragDeorbit.py index 3fdb0595e1..55424ca8bf 100644 --- a/examples/scenarioDragDeorbit.py +++ b/examples/scenarioDragDeorbit.py @@ -177,7 +177,7 @@ def run(show_plots, initialAlt=250, deorbitAlt=100, model="exponential"): simProcessName = "simProcess" scSim = SimulationBaseClass.SimBaseClass() dynProcess = scSim.CreateNewProcess(simProcessName) - simulationTimeStep = macros.sec2nano(15.) + simulationTimeStep = macros.sec2nano(15.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # Initialize atmosphere model and add to sim @@ -192,11 +192,29 @@ def run(show_plots, initialAlt=250, deorbitAlt=100, model="exponential"): ap = 8 f107 = 110 sw_msg = { - "ap_24_0": ap, "ap_3_0": ap, "ap_3_-3": ap, "ap_3_-6": ap, "ap_3_-9": ap, - "ap_3_-12": ap, "ap_3_-15": ap, "ap_3_-18": ap, "ap_3_-21": ap, "ap_3_-24": ap, - "ap_3_-27": ap, "ap_3_-30": ap, "ap_3_-33": ap, "ap_3_-36": ap, "ap_3_-39": ap, - "ap_3_-42": ap, "ap_3_-45": ap, "ap_3_-48": ap, "ap_3_-51": ap, "ap_3_-54": ap, - "ap_3_-57": ap, "f107_1944_0": f107, "f107_24_-24": f107 + "ap_24_0": ap, + "ap_3_0": ap, + "ap_3_-3": ap, + "ap_3_-6": ap, + "ap_3_-9": ap, + "ap_3_-12": ap, + "ap_3_-15": ap, + "ap_3_-18": ap, + "ap_3_-21": ap, + "ap_3_-24": ap, + "ap_3_-27": ap, + "ap_3_-30": ap, + "ap_3_-33": ap, + "ap_3_-36": ap, + "ap_3_-39": ap, + "ap_3_-42": ap, + "ap_3_-45": ap, + "ap_3_-48": ap, + "ap_3_-51": ap, + "ap_3_-54": ap, + "ap_3_-57": ap, + "f107_1944_0": f107, + "f107_24_-24": f107, } swMsgList = [] @@ -256,10 +274,12 @@ def run(show_plots, initialAlt=250, deorbitAlt=100, model="exponential"): # set the simulation time increments n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n simulationTime = macros.sec2nano(100 * P) numDataPoints = 10000 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) # Setup data logging before the simulation is initialized dataRec = scObject.scStateOutMsg.recorder(samplingTime) @@ -290,10 +310,13 @@ def run(show_plots, initialAlt=250, deorbitAlt=100, model="exponential"): # be stored in a binary file inside the _VizFiles sub-folder with the scenario folder. This file can be read in by # Vizard and played back after running the BSK simulation. # To enable this, uncomment this line:] - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - # saveFile=__file__ - # liveStream=True - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # saveFile=__file__ + # liveStream=True + ) # initialize Simulation scSim.InitializeSimulation() @@ -308,7 +331,9 @@ def run(show_plots, initialAlt=250, deorbitAlt=100, model="exponential"): dragForce = forceLog.forceExternal_B denseData = dataAtmoLog.neutralDensity - figureList = plotOrbits(dataRec.times(), posData, velData, dragForce, denseData, oe, mu, planet, model) + figureList = plotOrbits( + dataRec.times(), posData, velData, dragForce, denseData, oe, mu, planet, model + ) if show_plots: plt.show() @@ -337,8 +362,8 @@ def register_fig(i): plt.axis(np.array([-oe.rApoap, oe.rPeriap, -b, b]) / 1000 * 1.25) # draw the planet fig, ax = register_fig(1) - ax.axis('equal') - planetColor = '#008800' + ax.axis("equal") + planetColor = "#008800" planetRadius = planet.radEquator / 1000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # draw the actual orbit @@ -348,32 +373,36 @@ def register_fig(i): oeData = orbitalMotion.rv2elem(mu, posData[idx], velData[idx]) 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=1.0 - ) - plt.xlabel('$i_e$ Cord. [km]') - plt.ylabel('$i_p$ Cord. [km]') + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + color="#aa0000", + linewidth=1.0, + ) + plt.xlabel("$i_e$ Cord. [km]") + plt.ylabel("$i_p$ Cord. [km]") # draw altitude as a function of time fig, ax = register_fig(2) - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") alt = np.array(rData) / 1000 - planetRadius plt.plot(timeAxis * macros.NANO2HOUR, alt) - plt.xlabel('$t$ [h]') - plt.ylabel('Alt. [km]') + plt.xlabel("$t$ [h]") + plt.ylabel("Alt. [km]") pltName = fileName + "2" figureList[pltName] = plt.figure(2) # draw density as a function of altitude fig, ax = register_fig(3) plt.semilogy(alt, denseData) - plt.xlabel('Alt. [km]') - plt.ylabel('$\\rho$ [kg/m$^2$]') + plt.xlabel("Alt. [km]") + plt.ylabel("$\\rho$ [kg/m$^2$]") # draw drag as a function of time fig, ax = register_fig(4) plt.semilogy(timeAxis * macros.NANO2HOUR, np.linalg.norm(dragForce, 2, 1)) - plt.xlabel('$t$ [hr]') - plt.ylabel('$|F_drag|$ [N]') + plt.xlabel("$t$ [hr]") + plt.ylabel("$|F_drag|$ [N]") return figureList @@ -383,5 +412,5 @@ def register_fig(i): show_plots=True, initialAlt=250, deorbitAlt=100, - model="msis" # "msis", "exponential" + model="msis", # "msis", "exponential" ) diff --git a/examples/scenarioDragRendezvous.py b/examples/scenarioDragRendezvous.py index 3986ba2240..b39fda6aed 100644 --- a/examples/scenarioDragRendezvous.py +++ b/examples/scenarioDragRendezvous.py @@ -80,7 +80,12 @@ import numpy as np from Basilisk import __path__ from Basilisk.fswAlgorithms import hillStateConverter, hillToAttRef, hillPoint -from Basilisk.simulation import spacecraft, facetDragDynamicEffector, simpleNav, exponentialAtmosphere +from Basilisk.simulation import ( + spacecraft, + facetDragDynamicEffector, + simpleNav, + exponentialAtmosphere, +) from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros @@ -95,35 +100,123 @@ # Declare some linearized drag HCW dynamics -drag_state_dynamics = [[0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00], - [4.134628279603025589e-06,0.000000000000000000e+00,0.000000000000000000e+00,-7.178791202675993545e-10,2.347943292785702706e-03,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,-2.347943292785702706e-03,-1.435758240535198709e-09,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00]] -drag_ctrl_effects = [[0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,4.1681080996120926e-05], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00] - ] -drag_sens_effects = [[0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,-7.178791202675151888e-10,0.000000000000000000e+00,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,-1.435758240535030378e-09,0.000000000000000000e+00], - [0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00]] -drag_R_inv = [[1e-8,0,0], - [0,1e-8,0], - [0,0,1e-8]] +drag_state_dynamics = [ + [ + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 1.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + ], + [ + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 1.000000000000000000e00, + 0.000000000000000000e00, + ], + [ + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + ], + [ + 4.134628279603025589e-06, + 0.000000000000000000e00, + 0.000000000000000000e00, + -7.178791202675993545e-10, + 2.347943292785702706e-03, + 0.000000000000000000e00, + ], + [ + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + -2.347943292785702706e-03, + -1.435758240535198709e-09, + 0.000000000000000000e00, + ], + [ + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + ], +] +drag_ctrl_effects = [ + [0.000000000000000000e00, 0.000000000000000000e00, 0.000000000000000000e00], + [0.000000000000000000e00, 0.000000000000000000e00, 0.000000000000000000e00], + [0.000000000000000000e00, 0.000000000000000000e00, 0.000000000000000000e00], + [0.000000000000000000e00, 0.000000000000000000e00, 0.000000000000000000e00], + [0.000000000000000000e00, 0.000000000000000000e00, 4.1681080996120926e-05], + [0.000000000000000000e00, 0.000000000000000000e00, 0.000000000000000000e00], +] +drag_sens_effects = [ + [ + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + ], + [ + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + ], + [ + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + ], + [ + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + -7.178791202675151888e-10, + 0.000000000000000000e00, + 0.000000000000000000e00, + ], + [ + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + -1.435758240535030378e-09, + 0.000000000000000000e00, + ], + [ + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + 0.000000000000000000e00, + ], +] +drag_R_inv = [[1e-8, 0, 0], [0, 1e-8, 0], [0, 0, 1e-8]] # Static LQR gain path = os.path.dirname(os.path.abspath(__file__)) dataFileName = os.path.join(path, "dataForExamples", "static_lqr_controlGain.npz") -lqr_gain_set = np.load(dataFileName)['arr_0.npy'] +lqr_gain_set = np.load(dataFileName)["arr_0.npy"] fileName = os.path.basename(os.path.splitext(__file__)[0]) + def setup_spacecraft_plant(rN, vN, modelName): """ Convenience function to set up a spacecraft and its associated dragEffector and simpleNav module. @@ -139,30 +232,30 @@ def setup_spacecraft_plant(rN, vN, modelName): scObject.ModelTag = modelName scObject.hub.mHub = 6.0 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] - I = [10., 0., 0., - 0., 9., 0., - 0., 0., 8.] + I = [10.0, 0.0, 0.0, 0.0, 9.0, 0.0, 0.0, 0.0, 8.0] scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) scObject.hub.r_CN_NInit = rN scObject.hub.v_CN_NInit = vN scNav = simpleNav.SimpleNav() - scNav.ModelTag = modelName+'_navigator' + scNav.ModelTag = modelName + "_navigator" scNav.scStateInMsg.subscribeTo(scObject.scStateOutMsg) - dragArea = 0.3*0.2 + dragArea = 0.3 * 0.2 dragCoeff = 2.2 - normalVector = -rbk.euler3(np.radians(45.)).dot(np.array([0,-1,0])) - panelLocation = [0,0,0] + normalVector = -rbk.euler3(np.radians(45.0)).dot(np.array([0, -1, 0])) + panelLocation = [0, 0, 0] dragEffector = facetDragDynamicEffector.FacetDragDynamicEffector() - dragEffector.ModelTag = modelName+'_dragEffector' + dragEffector.ModelTag = modelName + "_dragEffector" dragEffector.addFacet(dragArea, dragCoeff, normalVector, panelLocation) scObject.addDynamicEffector(dragEffector) return scObject, dragEffector, scNav -def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', useJ2=False): +def drag_simulator( + altOffset, trueAnomOffset, densMultiplier, ctrlType="lqr", useJ2=False +): """ Basilisk simulation of a two-spacecraft rendezvous using relative-attitude driven differential drag. Includes both static gain and desensitized time-varying gain options and the option to use simulated attitude control or @@ -174,7 +267,9 @@ def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', us """ startTime = time.time() - print(f"Starting process execution for altOffset = {altOffset}, trueAnomOffset={trueAnomOffset}, densMultiplier={densMultiplier} with {ctrlType} controls...") + print( + f"Starting process execution for altOffset = {altOffset}, trueAnomOffset={trueAnomOffset}, densMultiplier={densMultiplier} with {ctrlType} controls..." + ) scSim = SimulationBaseClass.SimBaseClass() @@ -195,7 +290,9 @@ def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', us earth.isCentralBody = True mu = earth.mu if useJ2: - earth.useSphericalHarmonicsGravityModel(bskPath + '/supportData/LocalGravData/GGM03S.txt', 2) + earth.useSphericalHarmonicsGravityModel( + bskPath + "/supportData/LocalGravData/GGM03S.txt", 2 + ) # timeInitString = '2021 MAY 04 07:47:48.965 (UTC)' # spiceObject = gravFactory.createSpiceInterface(time=timeInitString) @@ -203,37 +300,37 @@ def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', us # Density atmosphere = exponentialAtmosphere.ExponentialAtmosphere() - atmosphere.ModelTag = 'atmosphere' + atmosphere.ModelTag = "atmosphere" # atmosphere.planetPosInMsg.subscribeTo(spiceObject.planetStateOutMsgs[0]) - atmosphere.planetRadius = astroConstants.REQ_EARTH*1e3 + 300e3 # m + atmosphere.planetRadius = astroConstants.REQ_EARTH * 1e3 + 300e3 # m atmosphere.envMinReach = -300e3 atmosphere.envMaxReach = +300e3 - atmosphere.scaleHeight = 8.0e3 # m - atmosphere.baseDensity = 2.022E-14 * 1000 * densMultiplier # kg/m^3 + atmosphere.scaleHeight = 8.0e3 # m + atmosphere.baseDensity = 2.022e-14 * 1000 * densMultiplier # kg/m^3 ## Set up chief, deputy orbits: chief_oe = orbitalMotion.ClassicElements() - chief_oe.a = astroConstants.REQ_EARTH * 1e3 + 300e3 # meters - chief_oe.e = 0. - chief_oe.i = np.radians(45.) + chief_oe.a = astroConstants.REQ_EARTH * 1e3 + 300e3 # meters + chief_oe.e = 0.0 + chief_oe.i = np.radians(45.0) chief_oe.Omega = np.radians(20.0) - chief_oe.omega = np.radians(30.) - chief_oe.f = np.radians(20.) + chief_oe.omega = np.radians(30.0) + chief_oe.f = np.radians(20.0) chief_rN, chief_vN = orbitalMotion.elem2rv(mu, chief_oe) dep_oe = orbitalMotion.ClassicElements() dep_oe.a = orbitalMotion.REQ_EARTH * 1e3 + 300e3 + (altOffset) # meters - dep_oe.e = 0. - dep_oe.i = np.radians(45.) + dep_oe.e = 0.0 + dep_oe.i = np.radians(45.0) dep_oe.Omega = np.radians(20.0) - dep_oe.omega = np.radians(30.) - dep_oe.f = np.radians(20. - trueAnomOffset) + dep_oe.omega = np.radians(30.0) + dep_oe.f = np.radians(20.0 - trueAnomOffset) dep_rN, dep_vN = orbitalMotion.elem2rv(mu, dep_oe) # Initialize s/c dynamics, drag, navigation solutions - chiefSc, chiefDrag, chiefNav = setup_spacecraft_plant(chief_rN, chief_vN, 'wiggum') - depSc, depDrag, depNav = setup_spacecraft_plant(dep_rN,dep_vN, 'lou') + chiefSc, chiefDrag, chiefNav = setup_spacecraft_plant(chief_rN, chief_vN, "wiggum") + depSc, depDrag, depNav = setup_spacecraft_plant(dep_rN, dep_vN, "lou") # Connect s/c to environment (gravity, density) gravFactory.addBodiesTo(chiefSc) @@ -245,7 +342,7 @@ def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', us chiefDrag.atmoDensInMsg.subscribeTo(atmosphere.envOutMsgs[-1]) # Add all dynamics stuff to dynamics task - scSim.AddModelToTask(dynTaskName, atmosphere,920) + scSim.AddModelToTask(dynTaskName, atmosphere, 920) # scSim.AddModelToTask(dynTaskName, ephemConverter, 921) scSim.AddModelToTask(dynTaskName, chiefDrag, 890) scSim.AddModelToTask(dynTaskName, depDrag, 891) @@ -259,16 +356,18 @@ def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', us # Chief S/C # hillPoint - set such that the chief attitude follows its hill frame. chiefAttRef = hillPoint.hillPoint() - chiefAttRef.ModelTag = 'chief_att_ref' + chiefAttRef.ModelTag = "chief_att_ref" chiefAttRef.transNavInMsg.subscribeTo(chiefNav.transOutMsg) # chiefAttRefData.celBodyInMsg.subscribeTo(ephemConverter.ephemOutMsgs[-1]) # We shouldn't need this because the planet is the origin - chiefSc.attRefInMsg.subscribeTo(chiefAttRef.attRefOutMsg) # Force the chief spacecraft to follow the hill direction + chiefSc.attRefInMsg.subscribeTo( + chiefAttRef.attRefOutMsg + ) # Force the chief spacecraft to follow the hill direction depHillRef = hillPoint.hillPoint() - depHillRef.ModelTag = 'dep_hill_ref' + depHillRef.ModelTag = "dep_hill_ref" depHillRef.transNavInMsg.subscribeTo(depNav.transOutMsg) # chiefAttRefData.celBodyInMsg.subscribeTo(ephemConverter.ephemOutMsgs[-1]) # We shouldn't need this because the planet is the origin - #chiefSc.attRefInMsg.subscribeTo(chiefAttRefData.attRefOutMsg) # Force the chief spacecraft to follow the hill direction + # chiefSc.attRefInMsg.subscribeTo(chiefAttRefData.attRefOutMsg) # Force the chief spacecraft to follow the hill direction # hillStateConverter hillStateNavObj = hillStateConverter.hillStateConverter() @@ -278,7 +377,7 @@ def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', us # hillToAtt guidance law w/ static gain depAttRef = hillToAttRef.hillToAttRef() - depAttRef.ModelTag = 'dep_att_ref' + depAttRef.ModelTag = "dep_att_ref" depAttRef.gainMatrix = hillToAttRef.MultiArray(lqr_gain_set) # Configure parameters common to relative attitude guidance modules depAttRef.hillStateInMsg.subscribeTo(hillStateNavObj.hillStateOutMsg) @@ -289,14 +388,13 @@ def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', us # Set the deputy spacecraft to directly follow the attRefMessage depSc.attRefInMsg.subscribeTo(depAttRef.attRefOutMsg) - scSim.AddModelToTask(dynTaskName, chiefAttRef, 710) - scSim.AddModelToTask(dynTaskName, hillStateNavObj,790) - scSim.AddModelToTask(dynTaskName, depHillRef,789) + scSim.AddModelToTask(dynTaskName, hillStateNavObj, 790) + scSim.AddModelToTask(dynTaskName, depHillRef, 789) scSim.AddModelToTask(dynTaskName, depAttRef, 700) # ----- log ----- # - orbit_period = 2*np.pi/np.sqrt(mu/chief_oe.a**3) - simulationTime = 40*orbit_period#106920.14366466808 + orbit_period = 2 * np.pi / np.sqrt(mu / chief_oe.a**3) + simulationTime = 40 * orbit_period # 106920.14366466808 simulationTime = macros.sec2nano(simulationTime) numDataPoints = 21384 samplingTime = simulationTime // (numDataPoints - 1) @@ -322,22 +420,25 @@ def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', us scSim.AddModelToTask(dynTaskName, chiefDragForceLog, 705) scSim.AddModelToTask(dynTaskName, depDragForceLog, 706) - for ind,rec in enumerate(atmoRecs): - scSim.AddModelToTask(dynTaskName, rec, 707+ind) + for ind, rec in enumerate(atmoRecs): + scSim.AddModelToTask(dynTaskName, rec, 707 + ind) # 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 - viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, [chiefSc, depSc], - # saveFile=fileName, - ) + viz = vizSupport.enableUnityVisualization( + scSim, + dynTaskName, + [chiefSc, depSc], + # saveFile=fileName, + ) # ----- execute sim ----- # scSim.InitializeSimulation() scSim.ConfigureStopTime(simulationTime) setupTimeStamp = time.time() - setupTime = setupTimeStamp-startTime + setupTime = setupTimeStamp - startTime print(f"Sim setup complete in {setupTime} seconds, executing...") - #scSim.ShowExecutionFigure(True) + # scSim.ShowExecutionFigure(True) scSim.ExecuteSimulation() execTimeStamp = time.time() @@ -345,22 +446,22 @@ def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', us print(f"Sim complete in {execTime} seconds, pulling data...") # ----- pull ----- # results_dict = {} - results_dict['chiefDrag_B'] = chiefDragForceLog.forceExternal_B - results_dict['depDrag_B'] = depDragForceLog.forceExternal_B - results_dict['dynTimeData'] = chiefStateRec.times() - results_dict['fswTimeData'] = depAttRec.times() - results_dict['wiggum.r_BN_N'] = chiefStateRec.r_BN_N - results_dict['wiggum.v_BN_N'] = chiefStateRec.v_BN_N - results_dict['lou.r_BN_N'] = depStateRec.r_BN_N - results_dict['lou.v_BN_N'] = depStateRec.v_BN_N - results_dict['relState.r_DC_H'] = hillStateRec.r_DC_H - results_dict['relState.v_DC_H'] = hillStateRec.v_DC_H - results_dict['wiggum.sigma_BN'] = chiefStateRec.sigma_BN - results_dict['lou.sigma_BN'] = depStateRec.sigma_BN - results_dict['depDensity'] = atmoRecs[0].neutralDensity - results_dict['chiefDensity'] = atmoRecs[1].neutralDensity - results_dict['mu'] = mu - results_dict['dens_mult'] = densMultiplier + results_dict["chiefDrag_B"] = chiefDragForceLog.forceExternal_B + results_dict["depDrag_B"] = depDragForceLog.forceExternal_B + results_dict["dynTimeData"] = chiefStateRec.times() + results_dict["fswTimeData"] = depAttRec.times() + results_dict["wiggum.r_BN_N"] = chiefStateRec.r_BN_N + results_dict["wiggum.v_BN_N"] = chiefStateRec.v_BN_N + results_dict["lou.r_BN_N"] = depStateRec.r_BN_N + results_dict["lou.v_BN_N"] = depStateRec.v_BN_N + results_dict["relState.r_DC_H"] = hillStateRec.r_DC_H + results_dict["relState.v_DC_H"] = hillStateRec.v_DC_H + results_dict["wiggum.sigma_BN"] = chiefStateRec.sigma_BN + results_dict["lou.sigma_BN"] = depStateRec.sigma_BN + results_dict["depDensity"] = atmoRecs[0].neutralDensity + results_dict["chiefDensity"] = atmoRecs[1].neutralDensity + results_dict["mu"] = mu + results_dict["dens_mult"] = densMultiplier pullTimeStamp = time.time() pullTime = pullTimeStamp - execTimeStamp overallTime = pullTimeStamp - startTime @@ -368,130 +469,133 @@ def drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', us return results_dict -def run(show_plots, altOffset, trueAnomOffset, densMultiplier, ctrlType='lqr', useJ2=False): - results = drag_simulator(altOffset, trueAnomOffset, densMultiplier, ctrlType=ctrlType, useJ2=useJ2) - - timeData = results['dynTimeData'] - fswTimeData = results['fswTimeData'] - pos = results['wiggum.r_BN_N'] - vel = results['wiggum.v_BN_N'] - chiefAtt = results['wiggum.sigma_BN'] - pos2 = results['lou.r_BN_N'] - vel2= results['lou.v_BN_N'] - depAtt = results['lou.sigma_BN'] - hillPos = results['relState.r_DC_H'] - hillVel = results['relState.v_DC_H'] - depDensity = results['depDensity'] - chiefDensity = results['chiefDensity'] - depDrag = results['depDrag_B'] - chiefDrag = results['chiefDrag_B'] +def run( + show_plots, altOffset, trueAnomOffset, densMultiplier, ctrlType="lqr", useJ2=False +): + results = drag_simulator( + altOffset, trueAnomOffset, densMultiplier, ctrlType=ctrlType, useJ2=useJ2 + ) + + timeData = results["dynTimeData"] + fswTimeData = results["fswTimeData"] + pos = results["wiggum.r_BN_N"] + vel = results["wiggum.v_BN_N"] + chiefAtt = results["wiggum.sigma_BN"] + pos2 = results["lou.r_BN_N"] + vel2 = results["lou.v_BN_N"] + depAtt = results["lou.sigma_BN"] + hillPos = results["relState.r_DC_H"] + hillVel = results["relState.v_DC_H"] + depDensity = results["depDensity"] + chiefDensity = results["chiefDensity"] + depDrag = results["depDrag_B"] + chiefDrag = results["chiefDrag_B"] densData = results numDataPoints = len(timeData) - mu = results['mu'] - rel_mrp_hist = np.empty([numDataPoints,3]) + mu = results["mu"] + rel_mrp_hist = np.empty([numDataPoints, 3]) - for ind in range(0,numDataPoints): - rel_mrp_hist[ind,:] = rbk.subMRP(depAtt[ind,:], chiefAtt[ind,:]) + for ind in range(0, numDataPoints): + rel_mrp_hist[ind, :] = rbk.subMRP(depAtt[ind, :], chiefAtt[ind, :]) figureList = {} # Plots for general consumption plt.figure() - plt.plot(timeData[1:], hillPos[1:,0],label="r_1") + plt.plot(timeData[1:], hillPos[1:, 0], label="r_1") plt.grid() - plt.xlabel('Time') - plt.ylabel('Hill X Position (m)') + plt.xlabel("Time") + plt.ylabel("Hill X Position (m)") pltName = fileName + "_hillX" figureList[pltName] = plt.figure(1) plt.figure() - plt.plot(timeData[1:], hillPos[1:,1],label="r_2") + plt.plot(timeData[1:], hillPos[1:, 1], label="r_2") plt.grid() - plt.xlabel('Time') - plt.ylabel('Hill Y Position (m)') + plt.xlabel("Time") + plt.ylabel("Hill Y Position (m)") pltName = fileName + "_hillY" figureList[pltName] = plt.figure(2) - plt.figure() - plt.plot(timeData[1:], hillVel[1:,0],label="v_1") + plt.plot(timeData[1:], hillVel[1:, 0], label="v_1") plt.grid() - plt.xlabel('Time') - plt.ylabel('Hill X Velocity (m/s)') + plt.xlabel("Time") + plt.ylabel("Hill X Velocity (m/s)") pltName = fileName + "_hilldX" figureList[pltName] = plt.figure(3) plt.figure() - plt.plot(timeData[1:], hillVel[1:,1],label="v_2") - plt.ylabel('Hill Y Velocity (m/s)') + plt.plot(timeData[1:], hillVel[1:, 1], label="v_2") + plt.ylabel("Hill Y Velocity (m/s)") pltName = fileName + "_hilldy" figureList[pltName] = plt.figure(4) plt.figure() - plt.semilogy(timeData[1:], chiefDensity[1:],label=r'Chief $\rho$') - plt.semilogy(timeData[1:], depDensity[1:],label=r'Deputy $\rho$') + plt.semilogy(timeData[1:], chiefDensity[1:], label=r"Chief $\rho$") + plt.semilogy(timeData[1:], depDensity[1:], label=r"Deputy $\rho$") plt.grid() plt.legend() - plt.xlabel('Time') - plt.ylabel('Density (kg/m3)') + plt.xlabel("Time") + plt.ylabel("Density (kg/m3)") pltName = fileName + "_densities" figureList[pltName] = plt.figure(5) plt.figure() - plt.plot(hillPos[1:,0],hillPos[1:,1]) + plt.plot(hillPos[1:, 0], hillPos[1:, 1]) plt.grid() - plt.xlabel('Hill X (m)') - plt.ylabel('Hill Y (m)') + plt.xlabel("Hill X (m)") + plt.ylabel("Hill Y (m)") pltName = fileName + "_hillTraj" figureList[pltName] = plt.figure(6) plt.figure() plt.plot(timeData, rel_mrp_hist) plt.grid() - plt.xlabel('Time') - plt.ylabel('Relative MRP Value') + plt.xlabel("Time") + plt.ylabel("Relative MRP Value") pltName = fileName + "_relativeAtt" figureList[pltName] = plt.figure(7) # Debug plots plt.figure() - plt.plot(timeData[1:], depDrag[1:,0]-chiefDrag[1:,0],label="delta a_1") - plt.plot(timeData[1:], depDrag[1:,1]-chiefDrag[1:,1],label="delta a_2") - plt.plot(timeData[1:], depDrag[1:,2]-chiefDrag[1:,2],label="delta a_3") + plt.plot(timeData[1:], depDrag[1:, 0] - chiefDrag[1:, 0], label="delta a_1") + plt.plot(timeData[1:], depDrag[1:, 1] - chiefDrag[1:, 1], label="delta a_2") + plt.plot(timeData[1:], depDrag[1:, 2] - chiefDrag[1:, 2], label="delta a_3") plt.grid() plt.legend() - plt.xlabel('Time') - plt.ylabel('Relative acceleration due to drag, body frame (m/s)') + plt.xlabel("Time") + plt.ylabel("Relative acceleration due to drag, body frame (m/s)") plt.figure() - plt.plot(timeData[1:], chiefDrag[1:,0],label="chief a_1") - plt.plot(timeData[1:], chiefDrag[1:,1],label="chief a_2") - plt.plot(timeData[1:], chiefDrag[1:,2],label="chief a_3") + plt.plot(timeData[1:], chiefDrag[1:, 0], label="chief a_1") + plt.plot(timeData[1:], chiefDrag[1:, 1], label="chief a_2") + plt.plot(timeData[1:], chiefDrag[1:, 2], label="chief a_3") plt.grid() plt.legend() - plt.xlabel('Time') - plt.ylabel('Relative acceleration due to drag, body frame (m/s)') + plt.xlabel("Time") + plt.ylabel("Relative acceleration due to drag, body frame (m/s)") plt.figure() - plt.plot(timeData[1:], depDrag[1:,0],label="dep a_1") - plt.plot(timeData[1:], depDrag[1:,1],label="dep a_2") - plt.plot(timeData[1:], depDrag[1:,2],label="dep a_3") + plt.plot(timeData[1:], depDrag[1:, 0], label="dep a_1") + plt.plot(timeData[1:], depDrag[1:, 1], label="dep a_2") + plt.plot(timeData[1:], depDrag[1:, 2], label="dep a_3") plt.grid() plt.legend() - plt.xlabel('Time') - plt.ylabel('Relative acceleration due to drag, body frame (m/s)') + plt.xlabel("Time") + plt.ylabel("Relative acceleration due to drag, body frame (m/s)") - - if(show_plots): + if show_plots: plt.show() plt.close("all") return figureList + if __name__ == "__main__": run( True, # show_plots - 0.0, # altitude offset (m) - 0.1, # True anomaly offset (deg) - 1, # Density multiplier (nondimensional) - ctrlType='lqr', - useJ2=False + 0.0, # altitude offset (m) + 0.1, # True anomaly offset (deg) + 1, # Density multiplier (nondimensional) + ctrlType="lqr", + useJ2=False, ) diff --git a/examples/scenarioDragSensitivity.py b/examples/scenarioDragSensitivity.py index 8201190d95..7c739f0662 100644 --- a/examples/scenarioDragSensitivity.py +++ b/examples/scenarioDragSensitivity.py @@ -67,12 +67,13 @@ fileName = os.path.basename(os.path.splitext(__file__)[0]) + def sim_wrapper(arg): """ - Because multiprocessing.map only considers single-input functions as targets for maps, - this function maps args and kwargs from the dicts provided by multiprocessing to the inputs - demanded by drag_simulator. - """ + Because multiprocessing.map only considers single-input functions as targets for maps, + this function maps args and kwargs from the dicts provided by multiprocessing to the inputs + demanded by drag_simulator. + """ args, kwargs = arg result = drag_simulator(*args, **kwargs) @@ -81,24 +82,23 @@ def sim_wrapper(arg): def drag_sensitivity_analysis(ctrlType, nuOffsetNum, densityNum, rerunSims=False): - - alt_offsets= [0] # np.arange(-100,100,10) - nu_offsets = np.arange(0.001, 1, (1-0.001)/nuOffsetNum) + alt_offsets = [0] # np.arange(-100,100,10) + nu_offsets = np.arange(0.001, 1, (1 - 0.001) / nuOffsetNum) density_multipliers = np.logspace(-1, 0.4, num=densityNum) pool = mp.Pool(mp.cpu_count()) - X,Y,Z = np.meshgrid(alt_offsets, nu_offsets, density_multipliers) + X, Y, Z = np.meshgrid(alt_offsets, nu_offsets, density_multipliers) positions = np.vstack([X.ravel(), Y.ravel(), Z.ravel()]).T - kwargs = {'ctrlType': ctrlType} + kwargs = {"ctrlType": ctrlType} arg = [(position, kwargs) for position in positions] if rerunSims: sim_results = pool.map(sim_wrapper, arg) - with open(ctrlType+"_sweep_results.pickle", "wb") as output_file: - pickle.dump(sim_results, output_file,-1) + with open(ctrlType + "_sweep_results.pickle", "wb") as output_file: + pickle.dump(sim_results, output_file, -1) else: - with open(ctrlType+"_sweep_results.pickle", "rb") as fp: + with open(ctrlType + "_sweep_results.pickle", "rb") as fp: sim_results = pickle.load(fp) pool.close() @@ -116,37 +116,38 @@ def results_to_ranges_and_plot(results_list): init_relvel = np.empty(len(results_list)) dens_list = np.empty(len(results_list)) - density_colorvals = np.logspace(-1,0.4,num=20) + density_colorvals = np.logspace(-1, 0.4, num=20) # normalizer = cmap.norma for ind, result in enumerate(results_list): - hill_pos = result['relState.r_DC_H'] - hill_vel = result['relState.v_DC_H'] - init_relpos[ind] = np.linalg.norm(hill_pos[1,:]) - init_relvel[ind] = np.linalg.norm(hill_vel[1,:]) - pos_errs[ind] = np.linalg.norm(hill_pos[-1,:]) - vel_errs[ind] = np.linalg.norm(hill_vel[-1,:]) - dens_list[ind] = result['dens_mult'] - - - if ind%5==0: - plt.plot(hill_pos[:,0],hill_pos[:,1] ) + hill_pos = result["relState.r_DC_H"] + hill_vel = result["relState.v_DC_H"] + init_relpos[ind] = np.linalg.norm(hill_pos[1, :]) + init_relvel[ind] = np.linalg.norm(hill_vel[1, :]) + pos_errs[ind] = np.linalg.norm(hill_pos[-1, :]) + vel_errs[ind] = np.linalg.norm(hill_vel[-1, :]) + dens_list[ind] = result["dens_mult"] + + if ind % 5 == 0: + plt.plot(hill_pos[:, 0], hill_pos[:, 1]) plt.grid() - plt.xlabel('Hill X (m)') - plt.ylabel('Hill Y (m)') + plt.xlabel("Hill X (m)") + plt.ylabel("Hill Y (m)") return init_relpos, init_relvel, pos_errs, vel_errs, dens_list, fig -def comparison_sweep(savePickle): +def comparison_sweep(savePickle): with open("lqr_sweep_results.pickle", "rb") as fp: lqr_sim_results = pickle.load(fp) if not savePickle: os.remove("lqr_sweep_results.pickle") figlist = {} - lqr_init_range, lqr_init_vel, lqr_err_range, lqr_err_vel, lqr_dens, lqr_fig = results_to_ranges_and_plot(lqr_sim_results) - figlist[fileName+'_hillTrajectories'] = lqr_fig + lqr_init_range, lqr_init_vel, lqr_err_range, lqr_err_vel, lqr_dens, lqr_fig = ( + results_to_ranges_and_plot(lqr_sim_results) + ) + figlist[fileName + "_hillTrajectories"] = lqr_fig unique_ranges = np.unique(lqr_init_range.round(decimals=2)) x_shape = len(unique_ranges) @@ -154,38 +155,41 @@ def comparison_sweep(savePickle): y_shape = len(unique_dens) import matplotlib.colors as colors + fig = plt.figure() - X,Y = np.meshgrid(unique_ranges, unique_dens) + X, Y = np.meshgrid(unique_ranges, unique_dens) Z = lqr_err_range.reshape([x_shape, y_shape]) Z_vel = lqr_err_vel.reshape([x_shape, y_shape]) # Position error contours - contour = plt.contourf(X.T,Y.T,Z, 30,norm=colors.LogNorm(Z.min(), Z.max())) - plt.ylabel('Density Multiplier') - plt.xlabel('Initial Displacement (m)') + contour = plt.contourf(X.T, Y.T, Z, 30, norm=colors.LogNorm(Z.min(), Z.max())) + plt.ylabel("Density Multiplier") + plt.xlabel("Initial Displacement (m)") cbar = fig.colorbar(contour) cbar.set_label("Terminal Positioning Error (m)") - figlist[fileName+'_positionSensitivity'] = fig + figlist[fileName + "_positionSensitivity"] = fig # Velocity error contours fig2 = plt.figure() - contour2 = plt.contourf(X.T,Y.T,Z_vel, 30,norm=colors.LogNorm(Z.min(), Z.max())) - plt.ylabel('Density Multiplier') - plt.xlabel('Initial Displacement (m)') + contour2 = plt.contourf(X.T, Y.T, Z_vel, 30, norm=colors.LogNorm(Z.min(), Z.max())) + plt.ylabel("Density Multiplier") + plt.xlabel("Initial Displacement (m)") cbar2 = fig2.colorbar(contour2) cbar2.set_label("Terminal Velocity Error (m/s)") - figlist[fileName+'_velocitySensitivity'] = fig2 + figlist[fileName + "_velocitySensitivity"] = fig2 return figlist + def run(show_plots, nuOffsetNum, densityNum, rerunSims=False, savePickle=False): - drag_sensitivity_analysis('lqr', nuOffsetNum, densityNum, rerunSims=rerunSims) + drag_sensitivity_analysis("lqr", nuOffsetNum, densityNum, rerunSims=rerunSims) plots = comparison_sweep(savePickle) if show_plots: plt.show() return plots -if __name__=='__main__': + +if __name__ == "__main__": nuOffsetNum = 15 densityNum = 20 run(True, nuOffsetNum, densityNum, rerunSims=True, savePickle=False) diff --git a/examples/scenarioExtendingBoom.py b/examples/scenarioExtendingBoom.py index 6193ad02c5..bafcb75456 100644 --- a/examples/scenarioExtendingBoom.py +++ b/examples/scenarioExtendingBoom.py @@ -65,10 +65,15 @@ from Basilisk.architecture import messaging from Basilisk.utilities import SimulationBaseClass, vizSupport, simIncludeGravBody -from Basilisk.simulation import spacecraft, linearTranslationNDOFStateEffector, prescribedLinearTranslation +from Basilisk.simulation import ( + spacecraft, + linearTranslationNDOFStateEffector, + prescribedLinearTranslation, +) from Basilisk.utilities import macros, orbitalMotion, unitTestSupport from Basilisk import __path__ + bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) @@ -85,7 +90,7 @@ def run(show_plots): dynProcess = scSim.CreateNewProcess(dynProcessName) # Create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(dynTaskName, simulationTimeStep)) # Define the spacecraft's properties @@ -96,9 +101,29 @@ def run(show_plots): scObject.ModelTag = "scObject" scObject.hub.mHub = scGeometry.massHub scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] - scObject.hub.IHubPntBc_B = [[scGeometry.massHub / 12 * (scGeometry.lengthHub ** 2 + scGeometry.heightHub ** 2), 0.0, 0.0], - [0.0, scGeometry.massHub / 12 * (scGeometry.widthHub ** 2 + scGeometry.heightHub ** 2), 0.0], - [0.0, 0.0, scGeometry.massHub / 12 * (scGeometry.lengthHub ** 2 + scGeometry.widthHub ** 2)]] + scObject.hub.IHubPntBc_B = [ + [ + scGeometry.massHub + / 12 + * (scGeometry.lengthHub**2 + scGeometry.heightHub**2), + 0.0, + 0.0, + ], + [ + 0.0, + scGeometry.massHub + / 12 + * (scGeometry.widthHub**2 + scGeometry.heightHub**2), + 0.0, + ], + [ + 0.0, + 0.0, + scGeometry.massHub + / 12 + * (scGeometry.lengthHub**2 + scGeometry.widthHub**2), + ], + ] scSim.AddModelToTask(dynTaskName, scObject) # Set the spacecraft's initial conditions @@ -108,14 +133,16 @@ def run(show_plots): scObject.hub.omega_BN_BInit = [[0.05], [-0.05], [0.05]] gravFactory = simIncludeGravBody.gravBodyFactory() - gravBodies = gravFactory.createBodies(['earth', 'sun']) - gravBodies['earth'].isCentralBody = True - mu = gravBodies['earth'].mu + gravBodies = gravFactory.createBodies(["earth", "sun"]) + gravBodies["earth"].isCentralBody = True + mu = gravBodies["earth"].mu gravFactory.addBodiesTo(scObject) timeInitString = "2016 JUNE 3 01:34:30.0" - gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/', timeInitString, epochInMsg=True) - gravFactory.spiceObject.zeroBase = 'earth' + gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", timeInitString, epochInMsg=True + ) + gravFactory.spiceObject.zeroBase = "earth" scSim.AddModelToTask(dynTaskName, gravFactory.spiceObject) oe = orbitalMotion.ClassicElements() @@ -131,16 +158,38 @@ def run(show_plots): scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] scObject.hub.omega_BN_BInit = [[0.1], [0.1], [0.1]] - translatingBodyEffector = linearTranslationNDOFStateEffector.linearTranslationNDOFStateEffector() + translatingBodyEffector = ( + linearTranslationNDOFStateEffector.linearTranslationNDOFStateEffector() + ) translatingBodyEffector.ModelTag = "translatingBodyEffector" scObject.addStateEffector(translatingBodyEffector) scSim.AddModelToTask(dynTaskName, translatingBodyEffector) translatingBody1 = linearTranslationNDOFStateEffector.translatingBody() translatingBody1.setMass(100) - translatingBody1.setIPntFc_F([[translatingBody1.getMass() / 12 * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2), 0.0, 0.0], - [0.0, translatingBody1.getMass() / 12 * (scGeometry.diameterArm / 2) ** 2, 0.0], - [0.0, 0.0, translatingBody1.getMass() / 12 * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2)]]) + translatingBody1.setIPntFc_F( + [ + [ + translatingBody1.getMass() + / 12 + * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm**2), + 0.0, + 0.0, + ], + [ + 0.0, + translatingBody1.getMass() / 12 * (scGeometry.diameterArm / 2) ** 2, + 0.0, + ], + [ + 0.0, + 0.0, + translatingBody1.getMass() + / 12 + * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm**2), + ], + ] + ) translatingBody1.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) translatingBody1.setR_FcF_F([[0.0], [scGeometry.heightArm / 2], [0.0]]) translatingBody1.setR_F0P_P([[0], [scGeometry.lengthHub / 2], [0]]) @@ -153,10 +202,29 @@ def run(show_plots): translatingBody2 = linearTranslationNDOFStateEffector.translatingBody() translatingBody2.setMass(100) - translatingBody2.setIPntFc_F([[translatingBody2.getMass() / 12 * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2), 0.0, 0.0], - [0.0, translatingBody2.getMass() / 12 * (scGeometry.diameterArm / 2) ** 2, 0.0], - [0.0, 0.0, translatingBody2.getMass() / 12 * ( - 3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2)]]) + translatingBody2.setIPntFc_F( + [ + [ + translatingBody2.getMass() + / 12 + * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm**2), + 0.0, + 0.0, + ], + [ + 0.0, + translatingBody2.getMass() / 12 * (scGeometry.diameterArm / 2) ** 2, + 0.0, + ], + [ + 0.0, + 0.0, + translatingBody2.getMass() + / 12 + * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm**2), + ], + ] + ) translatingBody2.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) translatingBody2.setR_FcF_F([[0.0], [scGeometry.heightArm / 2], [0.0]]) translatingBody2.setR_F0P_P([[0], [0], [0]]) @@ -173,22 +241,44 @@ def run(show_plots): profiler2.setTransPosInit(translatingBody2.getRhoInit()) profiler2.setSmoothingDuration(10) scSim.AddModelToTask(dynTaskName, profiler2) - translatingBodyEffector.translatingBodyRefInMsgs[1].subscribeTo(profiler2.linearTranslationRigidBodyOutMsg) + translatingBodyEffector.translatingBodyRefInMsgs[1].subscribeTo( + profiler2.linearTranslationRigidBodyOutMsg + ) translatingRigidBodyMsgData = messaging.LinearTranslationRigidBodyMsgPayload( - rho=scGeometry.heightArm, # [m] - rhoDot=0, # [m/s] + rho=scGeometry.heightArm, # [m] + rhoDot=0, # [m/s] + ) + translatingRigidBodyMsg2 = messaging.LinearTranslationRigidBodyMsg().write( + translatingRigidBodyMsgData ) - translatingRigidBodyMsg2 = messaging.LinearTranslationRigidBodyMsg().write(translatingRigidBodyMsgData) profiler2.linearTranslationRigidBodyInMsg.subscribeTo(translatingRigidBodyMsg2) translatingBody3 = linearTranslationNDOFStateEffector.translatingBody() translatingBody3.setMass(100) - translatingBody3.setIPntFc_F([[translatingBody3.getMass() / 12 * ( - 3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2), 0.0, 0.0], - [0.0, translatingBody3.getMass() / 12 * (scGeometry.diameterArm / 2) ** 2, 0.0], - [0.0, 0.0, translatingBody3.getMass() / 12 * ( - 3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2)]]) + translatingBody3.setIPntFc_F( + [ + [ + translatingBody3.getMass() + / 12 + * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm**2), + 0.0, + 0.0, + ], + [ + 0.0, + translatingBody3.getMass() / 12 * (scGeometry.diameterArm / 2) ** 2, + 0.0, + ], + [ + 0.0, + 0.0, + translatingBody3.getMass() + / 12 + * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm**2), + ], + ] + ) translatingBody3.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) translatingBody3.setR_FcF_F([[0.0], [scGeometry.heightArm / 2], [0.0]]) translatingBody3.setR_F0P_P([[0], [0], [0]]) @@ -205,22 +295,44 @@ def run(show_plots): profiler3.setTransPosInit(translatingBody3.getRhoInit()) profiler3.setSmoothingDuration(10) scSim.AddModelToTask(dynTaskName, profiler3) - translatingBodyEffector.translatingBodyRefInMsgs[2].subscribeTo(profiler3.linearTranslationRigidBodyOutMsg) + translatingBodyEffector.translatingBodyRefInMsgs[2].subscribeTo( + profiler3.linearTranslationRigidBodyOutMsg + ) translatingRigidBodyMsgData = messaging.LinearTranslationRigidBodyMsgPayload( rho=scGeometry.heightArm, # [m] rhoDot=0, # [m/s] ) - translatingRigidBodyMsg3 = messaging.LinearTranslationRigidBodyMsg().write(translatingRigidBodyMsgData) + translatingRigidBodyMsg3 = messaging.LinearTranslationRigidBodyMsg().write( + translatingRigidBodyMsgData + ) profiler3.linearTranslationRigidBodyInMsg.subscribeTo(translatingRigidBodyMsg3) translatingBody4 = linearTranslationNDOFStateEffector.translatingBody() translatingBody4.setMass(100) - translatingBody4.setIPntFc_F([[translatingBody4.getMass() / 12 * ( - 3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2), 0.0, 0.0], - [0.0, translatingBody4.getMass() / 12 * (scGeometry.diameterArm / 2) ** 2, 0.0], - [0.0, 0.0, translatingBody4.getMass() / 12 * ( - 3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm ** 2)]]) + translatingBody4.setIPntFc_F( + [ + [ + translatingBody4.getMass() + / 12 + * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm**2), + 0.0, + 0.0, + ], + [ + 0.0, + translatingBody4.getMass() / 12 * (scGeometry.diameterArm / 2) ** 2, + 0.0, + ], + [ + 0.0, + 0.0, + translatingBody4.getMass() + / 12 + * (3 * (scGeometry.diameterArm / 2) ** 2 + scGeometry.heightArm**2), + ], + ] + ) translatingBody4.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) translatingBody4.setR_FcF_F([[0.0], [scGeometry.heightArm / 2], [0.0]]) translatingBody4.setR_F0P_P([[0], [0], [0]]) @@ -237,13 +349,17 @@ def run(show_plots): profiler4.setTransPosInit(translatingBody4.getRhoInit()) profiler4.setSmoothingDuration(10) scSim.AddModelToTask(dynTaskName, profiler4) - translatingBodyEffector.translatingBodyRefInMsgs[3].subscribeTo(profiler4.linearTranslationRigidBodyOutMsg) + translatingBodyEffector.translatingBodyRefInMsgs[3].subscribeTo( + profiler4.linearTranslationRigidBodyOutMsg + ) translatingRigidBodyMsgData = messaging.LinearTranslationRigidBodyMsgPayload( - rho=scGeometry.heightArm, # [m] - rhoDot=0, # [m/s] + rho=scGeometry.heightArm, # [m] + rhoDot=0, # [m/s] + ) + translatingRigidBodyMsg4 = messaging.LinearTranslationRigidBodyMsg().write( + translatingRigidBodyMsgData ) - translatingRigidBodyMsg4 = messaging.LinearTranslationRigidBodyMsg().write(translatingRigidBodyMsgData) profiler4.linearTranslationRigidBodyInMsg.subscribeTo(translatingRigidBodyMsg4) scLog = scObject.scStateOutMsg.recorder() @@ -254,47 +370,74 @@ def run(show_plots): scSim.AddModelToTask(dynTaskName, rhoLog[-1]) if vizSupport.vizFound: - scBodyList = [scObject, - ["arm1", translatingBodyEffector.translatingBodyConfigLogOutMsgs[0]], - ["arm2", translatingBodyEffector.translatingBodyConfigLogOutMsgs[1]], - ["arm3", translatingBodyEffector.translatingBodyConfigLogOutMsgs[2]], - ["arm4", translatingBodyEffector.translatingBodyConfigLogOutMsgs[3]]] - viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, scBodyList - # , saveFile=fileName - ) - vizSupport.createCustomModel(viz - , simBodiesToModify=[scObject.ModelTag] - , modelPath="CUBE" - , color=vizSupport.toRGBA255("gold") - , scale=[scGeometry.widthHub, scGeometry.lengthHub, scGeometry.heightHub]) - vizSupport.createCustomModel(viz - , simBodiesToModify=["arm1"] - , modelPath="CYLINDER" - , color=vizSupport.toRGBA255("gray") - , scale=[scGeometry.diameterArm, scGeometry.diameterArm, scGeometry.heightArm / 2] - , rotation=[np.pi / 2, 0, 0] - ) - vizSupport.createCustomModel(viz - , simBodiesToModify=["arm2"] - , modelPath="CYLINDER" - , color=vizSupport.toRGBA255("darkgray") - , scale=[.95 * scGeometry.diameterArm, .95 * scGeometry.diameterArm, scGeometry.heightArm / 2] - , rotation=[np.pi / 2, 0, 0] - ) - vizSupport.createCustomModel(viz - , simBodiesToModify=["arm3"] - , modelPath="CYLINDER" - , color=vizSupport.toRGBA255("silver") - , scale=[.90 * scGeometry.diameterArm, .90 * scGeometry.diameterArm, scGeometry.heightArm / 2] - , rotation=[np.pi / 2, 0, 0] - ) - vizSupport.createCustomModel(viz - , simBodiesToModify=["arm4"] - , modelPath="CYLINDER" - , color=vizSupport.toRGBA255("lightgray") - , scale=[.85 * scGeometry.diameterArm, .85 * scGeometry.diameterArm, scGeometry.heightArm / 2] - , rotation=[np.pi / 2, 0, 0] - ) + scBodyList = [ + scObject, + ["arm1", translatingBodyEffector.translatingBodyConfigLogOutMsgs[0]], + ["arm2", translatingBodyEffector.translatingBodyConfigLogOutMsgs[1]], + ["arm3", translatingBodyEffector.translatingBodyConfigLogOutMsgs[2]], + ["arm4", translatingBodyEffector.translatingBodyConfigLogOutMsgs[3]], + ] + viz = vizSupport.enableUnityVisualization( + scSim, + dynTaskName, + scBodyList, + # , saveFile=fileName + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=[scObject.ModelTag], + modelPath="CUBE", + color=vizSupport.toRGBA255("gold"), + scale=[scGeometry.widthHub, scGeometry.lengthHub, scGeometry.heightHub], + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=["arm1"], + modelPath="CYLINDER", + color=vizSupport.toRGBA255("gray"), + scale=[ + scGeometry.diameterArm, + scGeometry.diameterArm, + scGeometry.heightArm / 2, + ], + rotation=[np.pi / 2, 0, 0], + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=["arm2"], + modelPath="CYLINDER", + color=vizSupport.toRGBA255("darkgray"), + scale=[ + 0.95 * scGeometry.diameterArm, + 0.95 * scGeometry.diameterArm, + scGeometry.heightArm / 2, + ], + rotation=[np.pi / 2, 0, 0], + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=["arm3"], + modelPath="CYLINDER", + color=vizSupport.toRGBA255("silver"), + scale=[ + 0.90 * scGeometry.diameterArm, + 0.90 * scGeometry.diameterArm, + scGeometry.heightArm / 2, + ], + rotation=[np.pi / 2, 0, 0], + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=["arm4"], + modelPath="CYLINDER", + color=vizSupport.toRGBA255("lightgray"), + scale=[ + 0.85 * scGeometry.diameterArm, + 0.85 * scGeometry.diameterArm, + scGeometry.heightArm / 2, + ], + rotation=[np.pi / 2, 0, 0], + ) viz.settings.orbitLinesOn = -1 simulationTime = macros.min2nano(3.0) @@ -337,11 +480,11 @@ def plotArmTimeHistory(timeData, rho, rhoDot, figureList): plt.figure(1) ax = plt.axes() for idx, disp in enumerate(rho): - plt.plot(timeData, disp, label=r'$\rho_' + str(idx+1) + '$') - plt.legend(fontsize='14') + plt.plot(timeData, disp, label=r"$\rho_" + str(idx + 1) + "$") + plt.legend(fontsize="14") # plt.title('Displacements', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\rho$ [m]', fontsize='18') + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\rho$ [m]", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -351,11 +494,11 @@ def plotArmTimeHistory(timeData, rho, rhoDot, figureList): plt.figure(2) ax = plt.axes() for idx, velo in enumerate(rhoDot): - plt.plot(timeData, velo, label=r'$\dot{\rho}_' + str(idx + 1) + '$') - plt.legend(fontsize='14') + plt.plot(timeData, velo, label=r"$\dot{\rho}_" + str(idx + 1) + "$") + plt.legend(fontsize="14") # plt.title('Displacement Rates', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\dot{\rho}$ [m/s]', fontsize='18') + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\dot{\rho}$ [m/s]", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -367,13 +510,16 @@ def plotSCStates(timeData, attLog, omegaLog, figureList): plt.figure(3) ax = plt.axes() for idx in range(3): - plt.plot(timeData, attLog[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(fontsize='14') + plt.plot( + timeData, + attLog[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(fontsize="14") # plt.title('Attitude', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\sigma_{B/N}$', fontsize='18') + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\sigma_{B/N}$", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -383,17 +529,23 @@ def plotSCStates(timeData, attLog, omegaLog, figureList): plt.figure(4) ax = plt.axes() for idx in range(3): - plt.plot(timeData, omegaLog[:, idx], - color=unitTestSupport.getLineColor(idx, 4), - label=r'$\omega_' + str(idx) + '$') - plt.plot(timeData, np.linalg.norm(omegaLog, axis=1), - color=unitTestSupport.getLineColor(3, 4), - label=r'$|\mathbf{\omega}|$', - linestyle='dashed') - plt.legend(fontsize='14') + plt.plot( + timeData, + omegaLog[:, idx], + color=unitTestSupport.getLineColor(idx, 4), + label=r"$\omega_" + str(idx) + "$", + ) + plt.plot( + timeData, + np.linalg.norm(omegaLog, axis=1), + color=unitTestSupport.getLineColor(3, 4), + label=r"$|\mathbf{\omega}|$", + linestyle="dashed", + ) + plt.legend(fontsize="14") # plt.title('Attitude Rate', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\omega_{B/N}$ [rad/s]', fontsize='18') + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\omega_{B/N}$ [rad/s]", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) diff --git a/examples/scenarioFlexiblePanel.py b/examples/scenarioFlexiblePanel.py index 1181ac9094..9d79d8def4 100644 --- a/examples/scenarioFlexiblePanel.py +++ b/examples/scenarioFlexiblePanel.py @@ -83,9 +83,22 @@ import matplotlib.pyplot as plt import numpy as np -from Basilisk.utilities import (SimulationBaseClass, vizSupport, simIncludeGravBody, macros, orbitalMotion, - unitTestSupport, RigidBodyKinematics as rbk) -from Basilisk.simulation import spacecraft, spinningBodyNDOFStateEffector, simpleNav, extForceTorque, svIntegrators +from Basilisk.utilities import ( + SimulationBaseClass, + vizSupport, + simIncludeGravBody, + macros, + orbitalMotion, + unitTestSupport, + RigidBodyKinematics as rbk, +) +from Basilisk.simulation import ( + spacecraft, + spinningBodyNDOFStateEffector, + simpleNav, + extForceTorque, + svIntegrators, +) from Basilisk.fswAlgorithms import mrpFeedback, inertial3D, attTrackingError from Basilisk.architecture import messaging @@ -100,7 +113,9 @@ def run(show_plots, numberOfSegments): scGeometry = geometryClass(numberOfSegments) scObject, panel, extForceTorque = setUpDynModules(scSim, scGeometry) - thetaData, attErrorLog = setUpFSWModules(scSim, scObject, panel, scGeometry, extForceTorque) + thetaData, attErrorLog = setUpFSWModules( + scSim, scObject, panel, scGeometry, extForceTorque + ) if vizSupport.vizFound: setUpVizard(scSim, scObject, panel, scGeometry) @@ -113,7 +128,7 @@ def run(show_plots, numberOfSegments): def createSimBaseClass(): scSim = SimulationBaseClass.SimBaseClass() - scSim.simulationTime = macros.min2nano(10.) + scSim.simulationTime = macros.min2nano(10.0) scSim.dynTaskName = "dynTask" scSim.dynProcess = scSim.CreateNewProcess("dynProcess") simTimeStep = macros.sec2nano(0.1) @@ -159,9 +174,29 @@ def setUpSpacecraft(scSim, scGeometry): scObject = spacecraft.Spacecraft() scObject.ModelTag = "scObject" scObject.hub.mHub = scGeometry.massHub - scObject.hub.IHubPntBc_B = [[scGeometry.massHub / 12 * (scGeometry.lengthHub ** 2 + scGeometry.heightHub ** 2), 0.0, 0.0], - [0.0, scGeometry.massHub / 12 * (scGeometry.widthHub ** 2 + scGeometry.heightHub ** 2), 0.0], - [0.0, 0.0, scGeometry.massHub / 12 * (scGeometry.lengthHub ** 2 + scGeometry.widthHub ** 2)]] + scObject.hub.IHubPntBc_B = [ + [ + scGeometry.massHub + / 12 + * (scGeometry.lengthHub**2 + scGeometry.heightHub**2), + 0.0, + 0.0, + ], + [ + 0.0, + scGeometry.massHub + / 12 + * (scGeometry.widthHub**2 + scGeometry.heightHub**2), + 0.0, + ], + [ + 0.0, + 0.0, + scGeometry.massHub + / 12 + * (scGeometry.lengthHub**2 + scGeometry.widthHub**2), + ], + ] scSim.AddModelToTask(scSim.dynTaskName, scObject) return scObject @@ -173,15 +208,17 @@ def setUpFlexiblePanel(scSim, scObject, scGeometry): for idx in range(scGeometry.numberOfSegments): spinningBody = spinningBodyNDOFStateEffector.SpinningBody() spinningBody.setMass(0.0) - spinningBody.setISPntSc_S([[0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0]]) - spinningBody.setDCM_S0P([[1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, 1.0]]) + spinningBody.setISPntSc_S([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]) + spinningBody.setDCM_S0P([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) spinningBody.setR_ScS_S([[0.0], [scGeometry.lengthSubPanel / 2], [0.0]]) if idx == 0: - spinningBody.setR_SP_P([[0.0], [scGeometry.lengthHub / 2], [scGeometry.heightHub / 2 - scGeometry.thicknessSubPanel / 2]]) + spinningBody.setR_SP_P( + [ + [0.0], + [scGeometry.lengthHub / 2], + [scGeometry.heightHub / 2 - scGeometry.thicknessSubPanel / 2], + ] + ) else: spinningBody.setR_SP_P([[0.0], [scGeometry.lengthSubPanel], 0.0]) spinningBody.setSHat_S([[1], [0], [0]]) @@ -193,12 +230,32 @@ def setUpFlexiblePanel(scSim, scObject, scGeometry): spinningBody = spinningBodyNDOFStateEffector.SpinningBody() spinningBody.setMass(scGeometry.massSubPanel) - spinningBody.setISPntSc_S([[scGeometry.massSubPanel / 12 * (scGeometry.lengthSubPanel ** 2 + scGeometry.thicknessSubPanel ** 2), 0.0, 0.0], - [0.0, scGeometry.massSubPanel / 12 * (scGeometry.widthSubPanel ** 2 + scGeometry.thicknessSubPanel ** 2), 0.0], - [0.0, 0.0, scGeometry.massSubPanel / 12 * (scGeometry.widthSubPanel ** 2 + scGeometry.lengthSubPanel ** 2)]]) - spinningBody.setDCM_S0P([[1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, 1.0]]) + spinningBody.setISPntSc_S( + [ + [ + scGeometry.massSubPanel + / 12 + * (scGeometry.lengthSubPanel**2 + scGeometry.thicknessSubPanel**2), + 0.0, + 0.0, + ], + [ + 0.0, + scGeometry.massSubPanel + / 12 + * (scGeometry.widthSubPanel**2 + scGeometry.thicknessSubPanel**2), + 0.0, + ], + [ + 0.0, + 0.0, + scGeometry.massSubPanel + / 12 + * (scGeometry.widthSubPanel**2 + scGeometry.lengthSubPanel**2), + ], + ] + ) + spinningBody.setDCM_S0P([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) spinningBody.setR_ScS_S([[0.0], [scGeometry.lengthSubPanel / 2], [0.0]]) spinningBody.setR_SP_P([[0.0], [0.0], [0.0]]) spinningBody.setSHat_S([[0], [1], [0]]) @@ -224,16 +281,16 @@ def setUpExtForceTorque(scSim, scObject): def setUpGravity(scSim, scObject): gravFactory = simIncludeGravBody.gravBodyFactory() - gravBodies = gravFactory.createBodies(['earth', 'sun']) - gravBodies['earth'].isCentralBody = True - mu = gravBodies['earth'].mu + gravBodies = gravFactory.createBodies(["earth", "sun"]) + gravBodies["earth"].isCentralBody = True + mu = gravBodies["earth"].mu gravFactory.addBodiesTo(scObject) timeInitString = "2012 MAY 1 00:28:30.0" - gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/', - timeInitString, - epochInMsg=True) - gravFactory.spiceObject.zeroBase = 'earth' + gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", timeInitString, epochInMsg=True + ) + gravFactory.spiceObject.zeroBase = "earth" scSim.AddModelToTask(scSim.dynTaskName, gravFactory.spiceObject) return mu @@ -260,7 +317,9 @@ def setUpFSWModules(scSim, scObject, panel, scGeometry, extFTObject): attError = setUpGuidance(scSim, sNavObject, inertial3DObj) setUpControl(scSim, extFTObject, attError, scGeometry) - thetaData, attErrorLog = setUpRecorders(scSim, scObject, panel, attError, scGeometry) + thetaData, attErrorLog = setUpRecorders( + scSim, scObject, panel, attError, scGeometry + ) return thetaData, attErrorLog @@ -294,17 +353,68 @@ def setUpGuidance(scSim, sNavObject, inertial3DObj): def setUpControl(scSim, extFTObject, attError, scGeometry): - IHubPntBc_B = np.array([[scGeometry.massHub / 12 * (scGeometry.lengthHub ** 2 + scGeometry.heightHub ** 2), 0.0, 0.0], - [0.0, scGeometry.massHub / 12 * (scGeometry.widthHub ** 2 + scGeometry.heightHub ** 2), 0.0], - [0.0, 0.0, scGeometry.massHub / 12 * (scGeometry.lengthHub ** 2 + scGeometry.widthHub ** 2)]]) + IHubPntBc_B = np.array( + [ + [ + scGeometry.massHub + / 12 + * (scGeometry.lengthHub**2 + scGeometry.heightHub**2), + 0.0, + 0.0, + ], + [ + 0.0, + scGeometry.massHub + / 12 + * (scGeometry.widthHub**2 + scGeometry.heightHub**2), + 0.0, + ], + [ + 0.0, + 0.0, + scGeometry.massHub + / 12 + * (scGeometry.lengthHub**2 + scGeometry.widthHub**2), + ], + ] + ) IPanelPntSc_B = np.array( - [[scGeometry.massPanel / 12 * (scGeometry.lengthPanel ** 2 + scGeometry.thicknessPanel ** 2), 0.0, 0.0], - [0.0, scGeometry.massPanel / 12 * (scGeometry.widthPanel ** 2 + scGeometry.thicknessPanel ** 2), 0.0], - [0.0, 0.0, scGeometry.massPanel / 12 * (scGeometry.widthPanel ** 2 + scGeometry.lengthPanel ** 2)]]) - r_ScB_B = [0.0, scGeometry.lengthHub / 2 + scGeometry.lengthPanel / 2, - scGeometry.heightHub / 2 - scGeometry.thicknessSubPanel / 2] - IHubPntB_B = IHubPntBc_B + IPanelPntSc_B - scGeometry.massPanel * np.array(rbk.v3Tilde(r_ScB_B)) @ np.array( - rbk.v3Tilde(r_ScB_B)) + [ + [ + scGeometry.massPanel + / 12 + * (scGeometry.lengthPanel**2 + scGeometry.thicknessPanel**2), + 0.0, + 0.0, + ], + [ + 0.0, + scGeometry.massPanel + / 12 + * (scGeometry.widthPanel**2 + scGeometry.thicknessPanel**2), + 0.0, + ], + [ + 0.0, + 0.0, + scGeometry.massPanel + / 12 + * (scGeometry.widthPanel**2 + scGeometry.lengthPanel**2), + ], + ] + ) + r_ScB_B = [ + 0.0, + scGeometry.lengthHub / 2 + scGeometry.lengthPanel / 2, + scGeometry.heightHub / 2 - scGeometry.thicknessSubPanel / 2, + ] + IHubPntB_B = ( + IHubPntBc_B + + IPanelPntSc_B + - scGeometry.massPanel + * np.array(rbk.v3Tilde(r_ScB_B)) + @ np.array(rbk.v3Tilde(r_ScB_B)) + ) mrpControl = mrpFeedback.mrpFeedback() mrpControl.ModelTag = "mrpFeedback" @@ -314,7 +424,9 @@ def setUpControl(scSim, extFTObject, attError, scGeometry): mrpControl.P = 2 * np.max(IHubPntB_B) / decayTime mrpControl.K = (mrpControl.P / xi) ** 2 / np.max(IHubPntB_B) - configData = messaging.VehicleConfigMsgPayload(IHubPntB_B=list(IHubPntB_B.flatten())) + configData = messaging.VehicleConfigMsgPayload( + IHubPntB_B=list(IHubPntB_B.flatten()) + ) configDataMsg = messaging.VehicleConfigMsg() configDataMsg.write(configData) @@ -342,20 +454,33 @@ def setUpRecorders(scSim, scObject, panel, attError, scGeometry): def setUpVizard(scSim, scObject, panel, scGeometry): scBodyList = [scObject] for idx in range(scGeometry.numberOfSegments): - scBodyList.append([f"subPanel{idx + 1}", panel.spinningBodyConfigLogOutMsgs[2 * idx + 1]]) - viz = vizSupport.enableUnityVisualization(scSim, scSim.dynTaskName, scBodyList - # , saveFile=fileName - ) - vizSupport.createCustomModel(viz - , simBodiesToModify=[scObject.ModelTag] - , modelPath="CUBE" - , color=vizSupport.toRGBA255("gold") - , scale=[scGeometry.widthHub, scGeometry.lengthHub, scGeometry.heightHub]) + scBodyList.append( + [f"subPanel{idx + 1}", panel.spinningBodyConfigLogOutMsgs[2 * idx + 1]] + ) + viz = vizSupport.enableUnityVisualization( + scSim, + scSim.dynTaskName, + scBodyList, + # , saveFile=fileName + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=[scObject.ModelTag], + modelPath="CUBE", + color=vizSupport.toRGBA255("gold"), + scale=[scGeometry.widthHub, scGeometry.lengthHub, scGeometry.heightHub], + ) for idx in range(scGeometry.numberOfSegments): - vizSupport.createCustomModel(viz - , simBodiesToModify=[f"subPanel{idx + 1}"] - , modelPath="CUBE" - , scale=[scGeometry.widthSubPanel, scGeometry.lengthSubPanel, scGeometry.thicknessSubPanel]) + vizSupport.createCustomModel( + viz, + simBodiesToModify=[f"subPanel{idx + 1}"], + modelPath="CUBE", + scale=[ + scGeometry.widthSubPanel, + scGeometry.lengthSubPanel, + scGeometry.thicknessSubPanel, + ], + ) viz.settings.orbitLinesOn = -1 @@ -383,11 +508,15 @@ def plotting(show_plots, thetaData, attErrorLog, scGeometry): plt.clf() ax = plt.axes() for idx in range(scGeometry.numberOfSegments): - plt.plot(timeSecDyn, macros.R2D * theta[2 * idx], label=r'$\theta_' + str(idx + 1) + '$') - plt.legend(fontsize='14') - plt.title('Bending Angles', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\theta$ [deg]', fontsize='18') + plt.plot( + timeSecDyn, + macros.R2D * theta[2 * idx], + label=r"$\theta_" + str(idx + 1) + "$", + ) + plt.legend(fontsize="14") + plt.title("Bending Angles", fontsize="22") + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\theta$ [deg]", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -398,11 +527,15 @@ def plotting(show_plots, thetaData, attErrorLog, scGeometry): plt.clf() ax = plt.axes() for idx in range(scGeometry.numberOfSegments): - plt.plot(timeSecDyn, macros.R2D * theta[2 * idx + 1], label=r'$\beta_' + str(idx + 1) + '$') - plt.legend(fontsize='14') - plt.title('Torsional Angles', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\beta$ [deg]', fontsize='18') + plt.plot( + timeSecDyn, + macros.R2D * theta[2 * idx + 1], + label=r"$\beta_" + str(idx + 1) + "$", + ) + plt.legend(fontsize="14") + plt.title("Torsional Angles", fontsize="22") + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\beta$ [deg]", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -413,11 +546,15 @@ def plotting(show_plots, thetaData, attErrorLog, scGeometry): plt.clf() ax = plt.axes() for idx in range(scGeometry.numberOfSegments): - plt.plot(timeSecDyn, macros.R2D * thetaDot[2 * idx], label=r'$\dot{\theta}_' + str(idx + 1) + '$') - plt.legend(fontsize='14') - plt.title('Bending Angle Rates', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\dot{\theta}$ [deg/s]', fontsize='18') + plt.plot( + timeSecDyn, + macros.R2D * thetaDot[2 * idx], + label=r"$\dot{\theta}_" + str(idx + 1) + "$", + ) + plt.legend(fontsize="14") + plt.title("Bending Angle Rates", fontsize="22") + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\dot{\theta}$ [deg/s]", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -428,11 +565,15 @@ def plotting(show_plots, thetaData, attErrorLog, scGeometry): plt.clf() ax = plt.axes() for idx in range(scGeometry.numberOfSegments): - plt.plot(timeSecDyn, macros.R2D * thetaDot[2 * idx + 1], label=r'$\dot{\beta}_' + str(idx + 1) + '$') - plt.legend(fontsize='14') - plt.title('Torsional Angle Rates', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\dot{\beta}$ [deg/s]', fontsize='18') + plt.plot( + timeSecDyn, + macros.R2D * thetaDot[2 * idx + 1], + label=r"$\dot{\beta}_" + str(idx + 1) + "$", + ) + plt.legend(fontsize="14") + plt.title("Torsional Angle Rates", fontsize="22") + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\dot{\beta}$ [deg/s]", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -443,13 +584,16 @@ def plotting(show_plots, thetaData, attErrorLog, scGeometry): plt.clf() ax = plt.axes() for idx in range(3): - plt.plot(timeSecFSW, attErrorLog.sigma_BR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(fontsize='14') - plt.title('Attitude Error', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\sigma_{B/R}$', fontsize='18') + plt.plot( + timeSecFSW, + attErrorLog.sigma_BR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(fontsize="14") + plt.title("Attitude Error", fontsize="22") + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\sigma_{B/R}$", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -460,13 +604,16 @@ def plotting(show_plots, thetaData, attErrorLog, scGeometry): plt.clf() ax = plt.axes() for idx in range(3): - plt.plot(timeSecFSW, attErrorLog.omega_BR_B[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_' + str(idx) + '$') - plt.legend(fontsize='14') - plt.title('Attitude Error Rate', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\omega_{B/R}$', fontsize='18') + plt.plot( + timeSecFSW, + attErrorLog.omega_BR_B[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_" + str(idx) + "$", + ) + plt.legend(fontsize="14") + plt.title("Attitude Error Rate", fontsize="22") + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\omega_{B/R}$", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -482,7 +629,4 @@ def plotting(show_plots, thetaData, attErrorLog, scGeometry): if __name__ == "__main__": - run( - True, - 5 - ) + run(True, 5) diff --git a/examples/scenarioFlybySpice.py b/examples/scenarioFlybySpice.py index 484d7aa991..b56cd396fe 100644 --- a/examples/scenarioFlybySpice.py +++ b/examples/scenarioFlybySpice.py @@ -247,8 +247,19 @@ def runVelocityPointing(simTime, planetMsg): bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) -from Basilisk.simulation import spacecraft, gravityEffector, extForceTorque, simpleNav, ephemerisConverter -from Basilisk.utilities import SimulationBaseClass, macros, simIncludeGravBody, unitTestSupport +from Basilisk.simulation import ( + spacecraft, + gravityEffector, + extForceTorque, + simpleNav, + ephemerisConverter, +) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + simIncludeGravBody, + unitTestSupport, +) from Basilisk.architecture import messaging from Basilisk.utilities import vizSupport @@ -260,15 +271,20 @@ def runVelocityPointing(simTime, planetMsg): # import FSW Algorithm related support from Basilisk.fswAlgorithms import hillPoint -from Basilisk.fswAlgorithms import mrpFeedback, attTrackingError, velocityPoint, locationPointing +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + velocityPoint, + locationPointing, +) def run(planetCase): """ - At the end of the python script you can specify the following example parameters. + At the end of the python script you can specify the following example parameters. - Args: - planetCase (str): {'venus', 'earth', 'mars'} + Args: + planetCase (str): {'venus', 'earth', 'mars'} """ # Create simulation variable names simTaskName = "simTask" @@ -284,7 +300,7 @@ def run(planetCase): dynProcess = scSim.CreateNewProcess(simProcessName) # Create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) # Add the dynamics task to the dynamics process dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) @@ -326,14 +342,16 @@ def run(planetCase): # Create the Spice interface and add the correct path to the ephemeris data spiceObject = gravFactory.createSpiceInterface(time=timeInitString, epochInMsg=True) - spiceObject.zeroBase = 'Earth' + spiceObject.zeroBase = "Earth" # Specify Spice spacecraft name scNames = ["-60000"] spiceObject.addSpacecraftNames(messaging.StringVector(scNames)) # Load the custom spacecraft trajectory Spice file using the SpiceInterface class loadSpiceKernel() method - spiceObject.loadSpiceKernel("spacecraft_21T01.bsp", os.path.join(path, "dataForExamples", "Spice/")) + spiceObject.loadSpiceKernel( + "spacecraft_21T01.bsp", os.path.join(path, "dataForExamples", "Spice/") + ) # Connect the configured Spice translational output message to spacecraft object's transRefInMsg input message scObject.transRefInMsg.subscribeTo(spiceObject.transRefStateOutMsgs[0]) @@ -345,7 +363,7 @@ def run(planetCase): # Create an ephemeris converter to convert Spice messages of type plantetStateOutMsgs to ephemeris messages # of type ephemOutMsgs. This converter is required for the velocityPoint and locationPointing modules. ephemObject = ephemerisConverter.EphemerisConverter() - ephemObject.ModelTag = 'EphemData' + ephemObject.ModelTag = "EphemData" ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[earthIdx]) ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[sunIdx]) ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[moonIdx]) @@ -354,11 +372,13 @@ def run(planetCase): scSim.AddModelToTask(simTaskName, ephemObject) # Define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # To set the spacecraft initial conditions, the following initial position and velocity variables are set: @@ -452,7 +472,9 @@ def run(planetCase): attError = attTrackingError.attTrackingError() attError.ModelTag = "attErrorInertial3D" scSim.AddModelToTask(simTaskName, attError) - attError.attRefInMsg.subscribeTo(velEarthGuidance.attRefOutMsg) # initial flight mode + attError.attRefInMsg.subscribeTo( + velEarthGuidance.attRefOutMsg + ) # initial flight mode attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg) # Create the FSW vehicle configuration message @@ -467,10 +489,10 @@ def run(planetCase): mrpControl.guidInMsg.subscribeTo(attError.attGuidOutMsg) mrpControl.vehConfigInMsg.subscribeTo(vcMsg) mrpControl.Ki = -1.0 # make value negative to turn off integral feedback - II = 900. - mrpControl.P = 2*II/(3*60) - mrpControl.K = mrpControl.P*mrpControl.P/II - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + II = 900.0 + mrpControl.P = 2 * II / (3 * 60) + mrpControl.K = mrpControl.P * mrpControl.P / II + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # Connect torque command to external torque effector extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) @@ -481,27 +503,36 @@ def run(planetCase): if vizSupport.vizFound: # Set up antenna transmission to Earth visualization transceiverHUD = vizInterface.Transceiver() - transceiverHUD.r_SB_B = [0.23, 0., 1.38] + transceiverHUD.r_SB_B = [0.23, 0.0, 1.38] transceiverHUD.fieldOfView = 40.0 * macros.D2R - transceiverHUD.normalVector = [0.0, 0., 1.0] + transceiverHUD.normalVector = [0.0, 0.0, 1.0] transceiverHUD.color = vizInterface.IntVector(vizSupport.toRGBA255("cyan")) transceiverHUD.label = "antenna" transceiverHUD.animationSpeed = 1 # Configure vizard settings vizFile = os.path.realpath(__file__).strip(".py") + "_" + planetCase + ".py" - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=vizFile - , transceiverList=transceiverHUD) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=vizFile + transceiverList=transceiverHUD, + ) viz.epochInMsg.subscribeTo(gravFactory.epochMsg) viz.settings.orbitLinesOn = -1 viz.settings.keyboardAngularRate = np.deg2rad(0.5) viz.settings.showMissionTime = 1 - vizSupport.createStandardCamera(viz, setMode=1, spacecraftName=scObject.ModelTag, - fieldOfView=10 * macros.D2R, - displayName="10˚ FOV Camera", - pointingVector_B=[0,1,0], position_B=cameraLocation) + vizSupport.createStandardCamera( + viz, + setMode=1, + spacecraftName=scObject.ModelTag, + fieldOfView=10 * macros.D2R, + displayName="10˚ FOV Camera", + pointingVector_B=[0, 1, 0], + position_B=cameraLocation, + ) # Initialize and execute simulation for the first section (stops at periapsis of hyperbola before delta V) scSim.InitializeSimulation() @@ -512,7 +543,7 @@ def runVelocityPointing(simTime, planetMsg): attError.attRefInMsg.subscribeTo(planetMsg) if vizSupport.vizFound: transceiverHUD.transceiverState = 0 # antenna off - attError.sigma_R0R = [np.tan(90.*macros.D2R/4), 0, 0] + attError.sigma_R0R = [np.tan(90.0 * macros.D2R / 4), 0, 0] simulationTime += macros.sec2nano(simTime) scSim.ConfigureStopTime(simulationTime) scSim.ExecuteSimulation() @@ -542,32 +573,34 @@ def runSensorSciencePointing(simTime): attError.attRefInMsg.subscribeTo(sciencePointGuidance.attRefOutMsg) if vizSupport.vizFound: transceiverHUD.transceiverState = 0 # antenna off - attError.sigma_R0R = [-1./3., 1./3., -1./3.] + attError.sigma_R0R = [-1.0 / 3.0, 1.0 / 3.0, -1.0 / 3.0] simulationTime += macros.sec2nano(simTime) scSim.ConfigureStopTime(simulationTime) scSim.ExecuteSimulation() - hour = 60*60 + hour = 60 * 60 # Execute desired attitude flight modes - runVelocityPointing(4*hour, velPlant.attRefOutMsg) + runVelocityPointing(4 * hour, velPlant.attRefOutMsg) - runAntennaEarthPointing(4*hour) + runAntennaEarthPointing(4 * hour) - runSensorSciencePointing(12*hour) + runSensorSciencePointing(12 * hour) - runAntennaEarthPointing(4*hour) + runAntennaEarthPointing(4 * hour) - runPanelSunPointing(4*hour) + runPanelSunPointing(4 * hour) # Unload custom Spice kernel at the end of each simulation gravFactory.unloadSpiceKernels() - spiceObject.unloadSpiceKernel("spacecraft_21T01.bsp", os.path.join(path, "Data", "Spice/")) + spiceObject.unloadSpiceKernel( + "spacecraft_21T01.bsp", os.path.join(path, "Data", "Spice/") + ) return if __name__ == "__main__": run( - "mars" # venus, earth, mars + "mars" # venus, earth, mars ) diff --git a/examples/scenarioFormationBasic.py b/examples/scenarioFormationBasic.py index 44fd40f060..18aa205354 100755 --- a/examples/scenarioFormationBasic.py +++ b/examples/scenarioFormationBasic.py @@ -107,12 +107,27 @@ import matplotlib.pyplot as plt import numpy as np from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, - rwMotorTorque, hillPoint) -from Basilisk.simulation import reactionWheelStateEffector, simpleNav, spacecraft, svIntegrators -from Basilisk.utilities import (SimulationBaseClass, macros, - orbitalMotion, simIncludeGravBody, - simIncludeRW, unitTestSupport, vizSupport) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + rwMotorTorque, + hillPoint, +) +from Basilisk.simulation import ( + reactionWheelStateEffector, + simpleNav, + spacecraft, + svIntegrators, +) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + simIncludeRW, + unitTestSupport, + vizSupport, +) try: from Basilisk.simulation import vizInterface @@ -122,6 +137,7 @@ # 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]) @@ -131,65 +147,83 @@ def plot_attitude_error(timeData, dataSigmaBR): """Plot the attitude errors.""" plt.figure(1) for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") def plot_rw_cmd_torque(timeData, dataUsReq, numRW): """Plot the RW command torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW): """Plot the RW actual motor torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.plot(timeData, dataRW[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.plot( + timeData, + dataRW[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") def plot_rate_error(timeData, dataOmegaBR): """Plot the body angular velocity rate tracking errors.""" plt.figure(3) for idx in range(3): - plt.plot(timeData, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error (rad/s) ') + plt.plot( + timeData, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error (rad/s) ") def plot_rw_speeds(timeData, dataOmegaRW, numRW): """Plot the RW spin rates.""" plt.figure(4) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed (RPM) ') + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed (RPM) ") def run(show_plots): @@ -210,7 +244,7 @@ def run(show_plots): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(40.) + simulationTime = macros.min2nano(40.0) # # create the simulation process @@ -218,7 +252,7 @@ def run(show_plots): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.5) + simulationTimeStep = macros.sec2nano(0.5) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -229,18 +263,14 @@ def run(show_plots): scObject = spacecraft.Spacecraft() scObject.ModelTag = "Servicer" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # create the debris object states scObject2 = spacecraft.Spacecraft() scObject2.ModelTag = "Debris" - I2 = [600., 0., 0., - 0., 650., 0., - 0., 0, 450.] + I2 = [600.0, 0.0, 0.0, 0.0, 650.0, 0.0, 0.0, 0, 450.0] scObject2.hub.mHub = 350.0 # kg scObject2.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I2) # this next step is not required, just a demonstration how we can ensure that @@ -257,9 +287,7 @@ def run(show_plots): # make another debris object */ scObject3 = spacecraft.Spacecraft() scObject3.ModelTag = "DebrisSat" - I3 = [600., 0., 0., - 0., 650., 0., - 0., 0, 450.] + I3 = [600.0, 0.0, 0.0, 0.0, 650.0, 0.0, 0.0, 0, 450.0] scObject3.hub.mHub = 350.0 # kg scObject3.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I3) @@ -291,16 +319,28 @@ def run(show_plots): varRWModel = messaging.BalancedWheels # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments - RW1 = rwFactory.create('Honeywell_HR16', [1, 0, 0], maxMomentum=50., Omega=100. # RPM - , RWModel=varRWModel - ) - RW2 = rwFactory.create('Honeywell_HR16', [0, 1, 0], maxMomentum=50., Omega=200. # RPM - , RWModel=varRWModel - ) - RW3 = rwFactory.create('Honeywell_HR16', [0, 0, 1], maxMomentum=50., Omega=300. # RPM - , rWB_B=[0.5, 0.5, 0.5] # meters - , RWModel=varRWModel - ) + RW1 = rwFactory.create( + "Honeywell_HR16", + [1, 0, 0], + maxMomentum=50.0, + Omega=100.0, # RPM + RWModel=varRWModel, + ) + RW2 = rwFactory.create( + "Honeywell_HR16", + [0, 1, 0], + maxMomentum=50.0, + Omega=200.0, # RPM + RWModel=varRWModel, + ) + RW3 = rwFactory.create( + "Honeywell_HR16", + [0, 0, 1], + maxMomentum=50.0, + Omega=300.0, # RPM + rWB_B=[0.5, 0.5, 0.5], # meters + RWModel=varRWModel, + ) numRW = rwFactory.getNumOfDevices() @@ -314,8 +354,8 @@ def run(show_plots): # add free-spinning RWs to the debris object rwFactory2 = simIncludeRW.rwFactory() - rwFactory2.create('Honeywell_HR16', [1, 0, 0], maxMomentum=50., Omega=1000.0) - rwFactory2.create('Honeywell_HR16', [0, 1, 0], maxMomentum=50., Omega=-1000.0) + rwFactory2.create("Honeywell_HR16", [1, 0, 0], maxMomentum=50.0, Omega=1000.0) + rwFactory2.create("Honeywell_HR16", [0, 1, 0], maxMomentum=50.0, Omega=-1000.0) numRW2 = rwFactory2.getNumOfDevices() rwStateEffector2 = reactionWheelStateEffector.ReactionWheelStateEffector() rwFactory2.addToSpacecraft("debrisRW", rwStateEffector2, scObject2) @@ -341,7 +381,11 @@ def run(show_plots): # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() attError.ModelTag = "attErrorInertial3D" - attError.sigma_R0R = [0.414214, 0.0, 0.0] # point the 3rd body axis in the along-track direction + attError.sigma_R0R = [ + 0.414214, + 0.0, + 0.0, + ] # point the 3rd body axis in the along-track direction scSim.AddModelToTask(simTaskName, attError) attError.attRefInMsg.subscribeTo(attGuidance.attRefOutMsg) attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg) @@ -365,7 +409,7 @@ def run(show_plots): mrpControl.K = 3.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # add module that maps the Lr control torque into the RW motor torques rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() @@ -376,16 +420,16 @@ def run(show_plots): rwMotorTorqueObj.rwParamsInMsg.subscribeTo(fswRwMsg) rwStateEffector.rwMotorCmdInMsg.subscribeTo(rwMotorTorqueObj.rwMotorTorqueOutMsg) # Make the RW control all three body axes - controlAxes_B = [ - 1, 0, 0, 0, 1, 0, 0, 0, 1 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] rwMotorTorqueObj.controlAxes_B = controlAxes_B # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) rwCmdLog = rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime) attErrLog = attError.attGuidOutMsg.recorder(samplingTime) sNavLog = sNavObject.transOutMsg.recorder(samplingTime) @@ -432,7 +476,7 @@ def run(show_plots): # setup 2nd debris object states oe3 = copy.deepcopy(oe) - oe3.f += 40./oe3.a + oe3.f += 40.0 / oe3.a r3N, v3N = orbitalMotion.elem2rv(mu, oe3) scObject3.hub.r_CN_NInit = r3N # m - r_CN_N scObject3.hub.v_CN_NInit = v3N # m/s - v_CN_N @@ -451,20 +495,28 @@ def run(show_plots): servicerLight.markerDiameter = 0.1 servicerLight.color = vizInterface.IntVector(vizSupport.toRGBA255("red")) - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, [scObject, scObject2, scObject3] - , rwEffectorList=[rwStateEffector, rwStateEffector2, None] - , lightList=[[servicerLight], None, None] - # , saveFile=fileName, - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + [scObject, scObject2, scObject3], + rwEffectorList=[rwStateEffector, rwStateEffector2, None], + lightList=[[servicerLight], None, None], + # , saveFile=fileName, + ) # setup one-way instrument camera by having frameRate be 0 - vizSupport.createCameraConfigMsg(viz, parentName=scObject.ModelTag, - cameraID=1, fieldOfView=40 * macros.D2R, - resolution=[1024, 1024], renderRate=0., - cameraPos_B=[0., 0., 2.0], sigma_CB=[0., 0., 0.] - ) + vizSupport.createCameraConfigMsg( + viz, + parentName=scObject.ModelTag, + cameraID=1, + fieldOfView=40 * macros.D2R, + resolution=[1024, 1024], + renderRate=0.0, + cameraPos_B=[0.0, 0.0, 2.0], + sigma_CB=[0.0, 0.0, 0.0], + ) viz.settings.trueTrajectoryLinesOn = 1 viz.settings.orbitLinesOn = 2 - viz.settings.messageBufferSize = -1 # force the full file to be read in at once + viz.settings.messageBufferSize = -1 # force the full file to be read in at once # # initialize Simulation @@ -514,11 +566,14 @@ def run(show_plots): plt.figure(5) for idx in range(numRW2): - plt.plot(timeData, omegaRW2[idx]*60/(2*3.14159), - color=unitTestSupport.getLineColor(idx, numRW2), - label=r'$\Omega_{s,' + str(idx) + '}$') - plt.xlabel('Time [min]') - plt.ylabel('RW2 Omega (rpm)') + plt.plot( + timeData, + omegaRW2[idx] * 60 / (2 * 3.14159), + color=unitTestSupport.getLineColor(idx, numRW2), + label=r"$\Omega_{s," + str(idx) + "}$", + ) + plt.xlabel("Time [min]") + plt.ylabel("RW2 Omega (rpm)") if show_plots: plt.show() @@ -535,5 +590,5 @@ def run(show_plots): # if __name__ == "__main__": run( - True # show_plots + True # show_plots ) diff --git a/examples/scenarioFormationMeanOEFeedback.py b/examples/scenarioFormationMeanOEFeedback.py index 3ce6c74ba1..b6ce872acc 100644 --- a/examples/scenarioFormationMeanOEFeedback.py +++ b/examples/scenarioFormationMeanOEFeedback.py @@ -65,7 +65,6 @@ """ - import math import os @@ -113,9 +112,7 @@ def run(show_plots, useClassicElem, numOrbits): scObject.ModelTag = "scObject" scObject2.ModelTag = "scObject2" - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 500.0 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) @@ -131,7 +128,9 @@ def run(show_plots, useClassicElem, numOrbits): earth = gravFactory.createEarth() earth.isCentralBody = True mu = earth.mu - earth.useSphericalHarmonicsGravityModel(bskPath + '/supportData/LocalGravData/GGM03S.txt', 2) + earth.useSphericalHarmonicsGravityModel( + bskPath + "/supportData/LocalGravData/GGM03S.txt", 2 + ) gravFactory.addBodiesTo(scObject) gravFactory.addBodiesTo(scObject2) @@ -162,20 +161,52 @@ def run(show_plots, useClassicElem, numOrbits): meanOEFeedbackObj.chiefTransInMsg.subscribeTo(simpleNavObject.transOutMsg) meanOEFeedbackObj.deputyTransInMsg.subscribeTo(simpleNavObject2.transOutMsg) extFTObject2.cmdForceInertialInMsg.subscribeTo(meanOEFeedbackObj.forceOutMsg) - meanOEFeedbackObj.K = [1e7, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1e7, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1e7, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1e7, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1e7, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1e7] + meanOEFeedbackObj.K = [ + 1e7, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1e7, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1e7, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1e7, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1e7, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1e7, + ] meanOEFeedbackObj.targetDiffOeMean = [0.000, 0.000, 0.000, 0.0003, 0.0002, 0.0001] if useClassicElem: meanOEFeedbackObj.oeType = 0 # 0: classic else: meanOEFeedbackObj.oeType = 1 # 1: equinoctial - meanOEFeedbackObj.mu = astroConstants.MU_EARTH*1e9 # [m^3/s^2] - meanOEFeedbackObj.req = astroConstants.REQ_EARTH*1e3 # [m] - meanOEFeedbackObj.J2 = astroConstants.J2_EARTH # [] + meanOEFeedbackObj.mu = astroConstants.MU_EARTH * 1e9 # [m^3/s^2] + meanOEFeedbackObj.req = astroConstants.REQ_EARTH * 1e3 # [m] + meanOEFeedbackObj.J2 = astroConstants.J2_EARTH # [] scSim.AddModelToTask(fswTaskName, meanOEFeedbackObj, 1) # ----- Setup spacecraft initial states ----- # @@ -194,7 +225,7 @@ def run(show_plots, useClassicElem, numOrbits): scObject.hub.v_CN_NInit = vN # m/s oe2 = orbitalMotion.ClassicElements() - oe2.a = oe.a*(1 + 0.0001) + oe2.a = oe.a * (1 + 0.0001) oe2.e = oe.e + 0.0002 oe2.i = oe.i - 0.0003 oe2.Omega = oe.Omega + 0.0004 @@ -207,11 +238,13 @@ def run(show_plots, useClassicElem, numOrbits): scObject2.hub.v_CN_NInit = vN2 # m/s # ----- log ----- # - orbit_period = 2*math.pi/math.sqrt(mu/oe.a**3) - simulationTime = orbit_period*numOrbits + orbit_period = 2 * math.pi / math.sqrt(mu / oe.a**3) + simulationTime = orbit_period * numOrbits simulationTime = macros.sec2nano(simulationTime) numDataPoints = 1000 - samplingTime = unitTestSupport.samplingTime(simulationTime, dynTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, dynTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) dataLog2 = scObject2.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTaskName, dataLog) @@ -219,9 +252,12 @@ def run(show_plots, useClassicElem, numOrbits): # 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 - viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, [scObject, scObject2] - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + dynTaskName, + [scObject, scObject2], + # , saveFile=fileName + ) # ----- execute sim ----- # scSim.InitializeSimulation() @@ -233,7 +269,7 @@ def run(show_plots, useClassicElem, numOrbits): vel = dataLog.v_BN_N pos2 = dataLog2.r_BN_N vel2 = dataLog2.v_BN_N - timeData = dataLog.times()*macros.NANO2SEC/orbit_period + timeData = dataLog.times() * macros.NANO2SEC / orbit_period # ----- plot ----- # # classical oe (figure1) @@ -243,26 +279,41 @@ def run(show_plots, useClassicElem, numOrbits): # spacecraft 1 (chief) oe_cl_osc = orbitalMotion.rv2elem(mu, pos[i], vel[i]) oe_cl_mean = orbitalMotion.ClassicElements() - orbitalMotion.clMeanOscMap(orbitalMotion.REQ_EARTH*1e3, orbitalMotion.J2_EARTH, oe_cl_osc, oe_cl_mean, -1) + orbitalMotion.clMeanOscMap( + orbitalMotion.REQ_EARTH * 1e3, + orbitalMotion.J2_EARTH, + oe_cl_osc, + oe_cl_mean, + -1, + ) # spacecraft 2 (deputy) oe2_cl_osc = orbitalMotion.rv2elem(mu, pos2[i], vel2[i]) oe2_cl_mean = orbitalMotion.ClassicElements() - orbitalMotion.clMeanOscMap(orbitalMotion.REQ_EARTH*1e3, orbitalMotion.J2_EARTH, oe2_cl_osc, oe2_cl_mean, -1) + orbitalMotion.clMeanOscMap( + orbitalMotion.REQ_EARTH * 1e3, + orbitalMotion.J2_EARTH, + oe2_cl_osc, + oe2_cl_mean, + -1, + ) # calculate oed - oed_cl[i, 0] = (oe2_cl_mean.a - oe_cl_mean.a)/oe_cl_mean.a # delta a (normalized) + oed_cl[i, 0] = ( + oe2_cl_mean.a - oe_cl_mean.a + ) / oe_cl_mean.a # delta a (normalized) oed_cl[i, 1] = oe2_cl_mean.e - oe_cl_mean.e # delta e oed_cl[i, 2] = oe2_cl_mean.i - oe_cl_mean.i # delta i oed_cl[i, 3] = oe2_cl_mean.Omega - oe_cl_mean.Omega # delta Omega oed_cl[i, 4] = oe2_cl_mean.omega - oe_cl_mean.omega # delta omega E_tmp = orbitalMotion.f2E(oe_cl_mean.f, oe_cl_mean.e) E2_tmp = orbitalMotion.f2E(oe2_cl_mean.f, oe2_cl_mean.e) - oed_cl[i, 5] = orbitalMotion.E2M( - E2_tmp, oe2_cl_mean.e) - orbitalMotion.E2M(E_tmp, oe_cl_mean.e) # delta M + oed_cl[i, 5] = orbitalMotion.E2M(E2_tmp, oe2_cl_mean.e) - orbitalMotion.E2M( + E_tmp, oe_cl_mean.e + ) # delta M for j in range(3, 6): - while(oed_cl[i, j] > math.pi): - oed_cl[i, j] = oed_cl[i, j] - 2*math.pi - while(oed_cl[i, j] < -math.pi): - oed_cl[i, j] = oed_cl[i, j] + 2*math.pi + while oed_cl[i, j] > math.pi: + oed_cl[i, j] = oed_cl[i, j] - 2 * math.pi + while oed_cl[i, j] < -math.pi: + oed_cl[i, j] = oed_cl[i, j] + 2 * math.pi plt.plot(timeData, oed_cl[:, 0], label="da") plt.plot(timeData, oed_cl[:, 1], label="de") plt.plot(timeData, oed_cl[:, 2], label="di") @@ -282,26 +333,40 @@ def run(show_plots, useClassicElem, numOrbits): # spacecraft 1 (chief) oe_cl_osc = orbitalMotion.rv2elem(mu, pos[i], vel[i]) oe_cl_mean = orbitalMotion.ClassicElements() - orbitalMotion.clMeanOscMap(orbitalMotion.REQ_EARTH*1e3, orbitalMotion.J2_EARTH, oe_cl_osc, oe_cl_mean, -1) + orbitalMotion.clMeanOscMap( + orbitalMotion.REQ_EARTH * 1e3, + orbitalMotion.J2_EARTH, + oe_cl_osc, + oe_cl_mean, + -1, + ) oe_eq_mean = orbitalMotion.EquinoctialElements() orbitalMotion.clElem2eqElem(oe_cl_mean, oe_eq_mean) # spacecraft 2 (deputy) oe2_cl_osc = orbitalMotion.rv2elem(mu, pos2[i], vel2[i]) oe2_cl_mean = orbitalMotion.ClassicElements() - orbitalMotion.clMeanOscMap(orbitalMotion.REQ_EARTH*1e3, orbitalMotion.J2_EARTH, oe2_cl_osc, oe2_cl_mean, -1) + orbitalMotion.clMeanOscMap( + orbitalMotion.REQ_EARTH * 1e3, + orbitalMotion.J2_EARTH, + oe2_cl_osc, + oe2_cl_mean, + -1, + ) oe2_eq_mean = orbitalMotion.EquinoctialElements() orbitalMotion.clElem2eqElem(oe2_cl_mean, oe2_eq_mean) # calculate oed - oed_eq[i, 0] = (oe2_eq_mean.a - oe_eq_mean.a)/oe_eq_mean.a # delta a (normalized) + oed_eq[i, 0] = ( + oe2_eq_mean.a - oe_eq_mean.a + ) / oe_eq_mean.a # delta a (normalized) oed_eq[i, 1] = oe2_eq_mean.P1 - oe_eq_mean.P1 # delta P1 oed_eq[i, 2] = oe2_eq_mean.P2 - oe_eq_mean.P2 # delta P2 oed_eq[i, 3] = oe2_eq_mean.Q1 - oe_eq_mean.Q1 # delta Q1 oed_eq[i, 4] = oe2_eq_mean.Q2 - oe_eq_mean.Q2 # delta Q2 oed_eq[i, 5] = oe2_eq_mean.l - oe_eq_mean.l # delta l - while(oed_eq[i, 5] > math.pi): - oed_eq[i, 5] = oed_eq[i, 5] - 2*math.pi - while(oed_eq[i, 5] < -math.pi): - oed_eq[i, 5] = oed_eq[i, 5] + 2*math.pi + while oed_eq[i, 5] > math.pi: + oed_eq[i, 5] = oed_eq[i, 5] - 2 * math.pi + while oed_eq[i, 5] < -math.pi: + oed_eq[i, 5] = oed_eq[i, 5] + 2 * math.pi plt.plot(timeData, oed_eq[:, 0], label="da") plt.plot(timeData, oed_eq[:, 1], label="dP1") plt.plot(timeData, oed_eq[:, 2], label="dP2") @@ -314,7 +379,7 @@ def run(show_plots, useClassicElem, numOrbits): pltName = fileName + "2" + str(int(useClassicElem)) figureList[pltName] = plt.figure(2) - if(show_plots): + if show_plots: plt.show() plt.close("all") @@ -325,5 +390,5 @@ def run(show_plots, useClassicElem, numOrbits): run( True, # show_plots True, # useClassicElem - 40 # number of orbits + 40, # number of orbits ) diff --git a/examples/scenarioFormationReconfig.py b/examples/scenarioFormationReconfig.py index a4de7eef19..64b668dc8f 100644 --- a/examples/scenarioFormationReconfig.py +++ b/examples/scenarioFormationReconfig.py @@ -67,7 +67,6 @@ :align: center """ - import itertools import math import os @@ -122,9 +121,7 @@ def run(show_plots, useRefAttitude): scObject.ModelTag = "scObject" scObject2.ModelTag = "scObject2" - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 500.0 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) @@ -150,7 +147,7 @@ def run(show_plots, useRefAttitude): location = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] direction = [[1.0, 1.0, 1.0], [1.0, 1.0, 1.0]] # get thrust in +z direction for pos_B, dir_B in zip(location, direction): - thFactory2.create('MOOG_Monarc_22_6', pos_B, dir_B, useMinPulseTime=False) + thFactory2.create("MOOG_Monarc_22_6", pos_B, dir_B, useMinPulseTime=False) thFactory2.addToSpacecraft(scObject2.ModelTag, thrusterEffector2, scObject2) # extObj @@ -205,9 +202,16 @@ def run(show_plots, useRefAttitude): spacecraftReconfigModule.thrustConfigInMsg.subscribeTo(fswThrConfMsg) spacecraftReconfigModule.vehicleConfigInMsg.subscribeTo(vcMsg) thrusterEffector2.cmdsInMsg.subscribeTo(spacecraftReconfigModule.onTimeOutMsg) - spacecraftReconfigModule.mu = astroConstants.MU_EARTH*1e9 # [m^3/s^2] + spacecraftReconfigModule.mu = astroConstants.MU_EARTH * 1e9 # [m^3/s^2] spacecraftReconfigModule.attControlTime = 400 # [s] - spacecraftReconfigModule.targetClassicOED = [0.0000, 0.0001, 0.0002, -0.0001, -0.0002, -0.0003] + spacecraftReconfigModule.targetClassicOED = [ + 0.0000, + 0.0001, + 0.0002, + -0.0001, + -0.0002, + -0.0003, + ] scSim.AddModelToTask(fswTaskName, spacecraftReconfigModule, 10) # att_Error @@ -227,11 +231,11 @@ def run(show_plots, useRefAttitude): mrpControl.K = 10 mrpControl.Ki = 0.0002 mrpControl.P = 50.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # ----- Setup spacecraft initial states ----- # oe = orbitalMotion.ClassicElements() - oe.a = 11000*1e3 # meters + oe.a = 11000 * 1e3 # meters oe.e = 0.4 oe.i = 60.0 * macros.D2R oe.Omega = 90 * macros.D2R @@ -247,7 +251,7 @@ def run(show_plots, useRefAttitude): scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B oe2 = oe - oe2.a = (1 + 0.0003)*oe2.a + oe2.a = (1 + 0.0003) * oe2.a oe2.e = oe2.e - 0.0002 oe2.i = oe2.i + 0.0001 oe2.Omega = oe2.Omega + 0.0004 @@ -262,11 +266,13 @@ def run(show_plots, useRefAttitude): scObject2.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B # ----- log ----- # - orbit_period = 2*math.pi/math.sqrt(mu/oe.a**3) - simulationTime = orbit_period*1.1 + orbit_period = 2 * math.pi / math.sqrt(mu / oe.a**3) + simulationTime = orbit_period * 1.1 simulationTime = macros.sec2nano(simulationTime) numDataPoints = 1000 - samplingTime = unitTestSupport.samplingTime(simulationTime, dynTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, dynTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) dataLog2 = scObject2.scStateOutMsg.recorder(samplingTime) attRefLog = spacecraftReconfigModule.attRefOutMsg.recorder(samplingTime) @@ -280,9 +286,12 @@ def run(show_plots, useRefAttitude): # 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 - viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, [scObject, scObject2] - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + dynTaskName, + [scObject, scObject2], + # , saveFile=fileName + ) # ----- execute sim ----- # scSim.InitializeSimulation() @@ -295,7 +304,7 @@ def run(show_plots, useRefAttitude): pos2 = dataLog2.r_BN_N vel2 = dataLog2.v_BN_N attErr = attErrLog.sigma_BR - timeData = dataLog.times()*macros.NANO2SEC/orbit_period + timeData = dataLog.times() * macros.NANO2SEC / orbit_period # ----- plot ----- # # classic orbital element difference (figure1) @@ -304,19 +313,21 @@ def run(show_plots, useRefAttitude): for i in range(0, len(pos[:, 0])): oe_tmp = orbitalMotion.rv2elem(mu, pos[i], vel[i]) oe2_tmp = orbitalMotion.rv2elem(mu, pos2[i], vel2[i]) - oed[i, 0] = (oe2_tmp.a - oe_tmp.a)/oe_tmp.a + oed[i, 0] = (oe2_tmp.a - oe_tmp.a) / oe_tmp.a oed[i, 1] = oe2_tmp.e - oe_tmp.e oed[i, 2] = oe2_tmp.i - oe_tmp.i oed[i, 3] = oe2_tmp.Omega - oe_tmp.Omega oed[i, 4] = oe2_tmp.omega - oe_tmp.omega E_tmp = orbitalMotion.f2E(oe_tmp.f, oe_tmp.e) E2_tmp = orbitalMotion.f2E(oe2_tmp.f, oe2_tmp.e) - oed[i, 5] = orbitalMotion.E2M(E2_tmp, oe2_tmp.e) - orbitalMotion.E2M(E_tmp, oe_tmp.e) + oed[i, 5] = orbitalMotion.E2M(E2_tmp, oe2_tmp.e) - orbitalMotion.E2M( + E_tmp, oe_tmp.e + ) for j in range(3, 6): - if(oed[i, j] > math.pi): - oed[i, j] = oed[i, j] - 2*math.pi - if(oed[i, j] < -math.pi): - oed[i, j] = oed[i, j] + 2*math.pi + if oed[i, j] > math.pi: + oed[i, j] = oed[i, j] - 2 * math.pi + if oed[i, j] < -math.pi: + oed[i, j] = oed[i, j] + 2 * math.pi plt.plot(timeData, oed[:, 0], label="da") plt.plot(timeData, oed[:, 1], label="de") plt.plot(timeData, oed[:, 2], label="di") @@ -338,14 +349,15 @@ def run(show_plots, useRefAttitude): plt.ylabel("MRP Error") pltName = fileName + "2" + str(int(useRefAttitude)) figureList[pltName] = plt.figure(2) - if(show_plots): + if show_plots: plt.show() plt.close("all") return pos, vel, pos2, vel2, attErr, numDataPoints, figureList + if __name__ == "__main__": run( - show_plots = True, # show_plots - useRefAttitude = False # useRefAttitude + show_plots=True, # show_plots + useRefAttitude=False, # useRefAttitude ) diff --git a/examples/scenarioFuelSlosh.py b/examples/scenarioFuelSlosh.py index 8d2d5a8869..62e360513d 100755 --- a/examples/scenarioFuelSlosh.py +++ b/examples/scenarioFuelSlosh.py @@ -167,8 +167,6 @@ """ - - import inspect import os @@ -176,8 +174,10 @@ import numpy as np from Basilisk.simulation import fuelTank from Basilisk.simulation import linearSpringMassDamper + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros @@ -277,7 +277,11 @@ def run(show_plots, damping_parameter, timeStep): # define hub properties scObject.hub.mHub = 1500 # kg scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] # kg*m^2 + scObject.hub.IHubPntBc_B = [ + [900.0, 0.0, 0.0], + [0.0, 800.0, 0.0], + [0.0, 0.0, 600.0], + ] # kg*m^2 scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] # rad scObject.hub.omega_BN_BInit = [[0.1], [-0.1], [0.1]] # rad/s @@ -309,33 +313,40 @@ def run(show_plots, damping_parameter, timeStep): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n simulationTime = macros.sec2nano(P / 4) # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) scLog = scObject.logger( ["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"], - simulationTimeStep) + simulationTimeStep, + ) scSim.AddModelToTask(simTaskName, scLog) damperRhoLog = None if damping_parameter != 0.0: + def get_rho(CurrentSimNanos, i): - stateName = f'linearSpringMassDamperRho{i}' + stateName = f"linearSpringMassDamperRho{i}" return scObject.dynManager.getStateObject(stateName).getState()[0][0] - damperRhoLog = pythonVariableLogger.PythonVariableLogger({ + damperRhoLog = pythonVariableLogger.PythonVariableLogger( + { "damperRho1": lambda CurrentSimNanos: get_rho(CurrentSimNanos, 1), "damperRho2": lambda CurrentSimNanos: get_rho(CurrentSimNanos, 2), "damperRho3": lambda CurrentSimNanos: get_rho(CurrentSimNanos, 3), - }, simulationTimeStep) + }, + simulationTimeStep, + ) scSim.AddModelToTask(simTaskName, damperRhoLog) @@ -360,7 +371,10 @@ def get_rho(CurrentSimNanos, i): rhojOuts = None if damping_parameter != 0.0: rhojOuts = [ - damperRhoLog.damperRho1, damperRhoLog.damperRho2, damperRhoLog.damperRho3] + damperRhoLog.damperRho1, + damperRhoLog.damperRho2, + damperRhoLog.damperRho3, + ] fileName = os.path.basename(os.path.splitext(__file__)[0]) if damping_parameter == 0.0 and timeStep == 0.75: @@ -390,13 +404,13 @@ def get_rho(CurrentSimNanos, i): x = planetRadius * 1e-3 * np.cos(u) y = planetRadius * 1e-3 * np.sin(u) - ax.add_artist(plt.Circle((0, 0), planetRadius / 1000, color='#008800')) + ax.add_artist(plt.Circle((0, 0), planetRadius / 1000, color="#008800")) plt.plot(rData * np.cos(fData) / 1000, rData * np.sin(fData) / 1000) # ax.plot(x,y,color='g') plt.xlim([-7000, 7000]) plt.ylim([-7000, 7000]) - plt.xlabel('X (km)') - plt.ylabel('Y (km)') + plt.xlabel("X (km)") + plt.ylabel("Y (km)") figureList = {} pltName = fileName + "Orbit" + str(setupNo) @@ -405,30 +419,30 @@ def get_rho(CurrentSimNanos, i): plt.figure(2, figsize=(5, 4)) for i in range(3): plt.plot(time, (orbAngMom_N[:, i] - orbAngMom_N[0, i]) / orbAngMom_N[0, i]) - plt.xlabel('Time (s)') - plt.ylabel('Relative Orbital Angular Momentum Variation') + plt.xlabel("Time (s)") + plt.ylabel("Relative Orbital Angular Momentum Variation") pltName = fileName + "OAM" + str(setupNo) figureList[pltName] = plt.figure(2) plt.figure(3, figsize=(5, 4)) plt.plot(time, (orbEnergy - orbEnergy[0]) / orbEnergy[0]) - plt.xlabel('Time (s)') - plt.ylabel('Relative Orbital Energy Variation') + plt.xlabel("Time (s)") + plt.ylabel("Relative Orbital Energy Variation") pltName = fileName + "OE" + str(setupNo) figureList[pltName] = plt.figure(3) plt.figure(4, figsize=(5, 4)) for i in range(3): plt.plot(time, (rotAngMom_N[:, i] - rotAngMom_N[0, i]) / rotAngMom_N[0, i]) - plt.xlabel('Time (s)') - plt.ylabel('Relative Rotational Angular Momentum Variation') + plt.xlabel("Time (s)") + plt.ylabel("Relative Rotational Angular Momentum Variation") pltName = fileName + "RAM" + str(setupNo) figureList[pltName] = plt.figure(4) plt.figure(5, figsize=(5, 4)) plt.plot(time, (rotEnergy - rotEnergy[0]) / rotEnergy[0]) - plt.xlabel('Time (s)') - plt.ylabel('Relative Rotational Energy Variation') + plt.xlabel("Time (s)") + plt.ylabel("Relative Rotational Energy Variation") pltName = fileName + "RE" + str(setupNo) figureList[pltName] = plt.figure(5) @@ -437,9 +451,9 @@ def get_rho(CurrentSimNanos, i): for rhojOut in rhojOuts: plt.plot(time, rhojOut) - plt.legend(['Particle 1', 'Particle 2', 'Particle 3'], loc='lower right') - plt.xlabel('Time (s)') - plt.ylabel('Displacement (m)') + plt.legend(["Particle 1", "Particle 2", "Particle 3"], loc="lower right") + plt.xlabel("Time (s)") + plt.ylabel("Displacement (m)") pltName = fileName + "ParticleMotion" figureList[pltName] = plt.figure(6) @@ -449,13 +463,12 @@ def get_rho(CurrentSimNanos, i): # close the plots being saved off to avoid over-writing old and new figures plt.close("all") - return time, rhojOuts, figureList if __name__ == "__main__": run( - True, # show_plots - 0.0, # damping_parameter - 0.75, # timeStep + True, # show_plots + 0.0, # damping_parameter + 0.75, # timeStep ) diff --git a/examples/scenarioGaussMarkovRandomWalk.py b/examples/scenarioGaussMarkovRandomWalk.py index 889faa3148..000c7d8c42 100644 --- a/examples/scenarioGaussMarkovRandomWalk.py +++ b/examples/scenarioGaussMarkovRandomWalk.py @@ -89,6 +89,7 @@ from Basilisk.utilities import simIncludeGravBody from Basilisk.simulation import svIntegrators + def run(show_plots, processNoiseLevel=0.5, walkBounds=3.0): # Create simulation variable names simTaskName = "simTask" @@ -112,12 +113,12 @@ def run(show_plots, processNoiseLevel=0.5, walkBounds=3.0): # Set spacecraft mass and inertia properties scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM - scObject.hub.IHubPntBc_B = [ - [900.0, 0.0, 0.0], - [0.0, 800.0, 0.0], - [0.0, 0.0, 600.0] - ] + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM + scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] # Setup gravity model gravFactory = simIncludeGravBody.gravBodyFactory() @@ -150,14 +151,12 @@ def run(show_plots, processNoiseLevel=0.5, walkBounds=3.0): imuSensor1.PMatrixGyro = [ [processNoiseLevel, 0.0, 0.0], [0.0, processNoiseLevel, 0.0], - [0.0, 0.0, processNoiseLevel] - ] - imuSensor1.AMatrixGyro = [ - [-0.1, 0.0, 0.0], - [0.0, -0.1, 0.0], - [0.0, 0.0, -0.1] + [0.0, 0.0, processNoiseLevel], ] - imuSensor1.setWalkBoundsGyro(np.array([walkBounds, walkBounds, walkBounds], dtype=np.float64)) + imuSensor1.AMatrixGyro = [[-0.1, 0.0, 0.0], [0.0, -0.1, 0.0], [0.0, 0.0, -0.1]] + imuSensor1.setWalkBoundsGyro( + np.array([walkBounds, walkBounds, walkBounds], dtype=np.float64) + ) imuSensor1.applySensorErrors = True imuSensor1.scStateInMsg.subscribeTo(scObject.scStateOutMsg) @@ -169,7 +168,7 @@ def run(show_plots, processNoiseLevel=0.5, walkBounds=3.0): imuSensor2.PMatrixGyro = [ [processNoiseLevel, 0.0, 0.0], [0.0, processNoiseLevel, 0.0], - [0.0, 0.0, processNoiseLevel] + [0.0, 0.0, processNoiseLevel], ] imuSensor2.walkBoundsGyro = [-1.0, -1.0, -1.0] imuSensor2.senRotBias = [0.0, 0.0, 0.0] @@ -189,11 +188,7 @@ def run(show_plots, processNoiseLevel=0.5, walkBounds=3.0): scSim.InitializeSimulation() # Set IMU2's A Matrix to zero to demonstrate different error propagation behavior. - imuSensor2.AMatrixGyro = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] + imuSensor2.AMatrixGyro = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] simulationTime = macros.min2nano(10) scSim.ConfigureStopTime(simulationTime) @@ -218,7 +213,9 @@ def run(show_plots, processNoiseLevel=0.5, walkBounds=3.0): # Try to access the gyro data try: - data1.append(msg1.AngVelPlatform[0]) # Use AngVelPlatform instead of sensedValues + data1.append( + msg1.AngVelPlatform[0] + ) # Use AngVelPlatform instead of sensedValues data2.append(msg2.AngVelPlatform[0]) except Exception as e: print(f"\nError accessing gyro data: {str(e)}") @@ -237,31 +234,32 @@ def run(show_plots, processNoiseLevel=0.5, walkBounds=3.0): # Create figure dictionary to store plots figureList = {} - plt.close('all') + plt.close("all") # Create the plot plt.figure(1, figsize=(12, 8)) - plt.plot(timeData, data1, label='IMU 1 (Bounded Random Walk)', alpha=0.7) - plt.plot(timeData, data2, label='IMU 2 (Pure Gaussian)', alpha=0.7) + plt.plot(timeData, data1, label="IMU 1 (Bounded Random Walk)", alpha=0.7) + plt.plot(timeData, data2, label="IMU 2 (Pure Gaussian)", alpha=0.7) # Plot bounds for IMU1 only - plt.axhline(y=walkBounds, color='r', linestyle='--', alpha=0.3, label='IMU1 Bounds') - plt.axhline(y=-walkBounds, color='r', linestyle='--', alpha=0.3) - plt.xlabel('Time (s)') - plt.ylabel('Angular Velocity (rad/s)') - plt.title('IMU Gyro Measurements: Random Walk vs Pure Gaussian') + plt.axhline(y=walkBounds, color="r", linestyle="--", alpha=0.3, label="IMU1 Bounds") + plt.axhline(y=-walkBounds, color="r", linestyle="--", alpha=0.3) + plt.xlabel("Time (s)") + plt.ylabel("Angular Velocity (rad/s)") + plt.title("IMU Gyro Measurements: Random Walk vs Pure Gaussian") plt.legend() plt.grid(True) plt.tight_layout() # Store the figure in the figure dictionary - pltName = 'scenarioGaussMarkovRandomWalk' + pltName = "scenarioGaussMarkovRandomWalk" figureList[pltName] = plt.figure(1) if show_plots: plt.show() - plt.close('all') # Close plots to free memory + plt.close("all") # Close plots to free memory return figureList + if __name__ == "__main__": run(True) diff --git a/examples/scenarioGroundDownlink.py b/examples/scenarioGroundDownlink.py index fa11b2355b..0fc6d1239d 100644 --- a/examples/scenarioGroundDownlink.py +++ b/examples/scenarioGroundDownlink.py @@ -54,6 +54,7 @@ .. image:: /_images/Scenarios/scenarioGroundPassStorage.svg :align: center """ + import inspect import os @@ -62,12 +63,17 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.simulation import simpleInstrument, simpleStorageUnit, partitionedStorageUnit, spaceToGroundTransmitter +from Basilisk.simulation import ( + simpleInstrument, + simpleStorageUnit, + partitionedStorageUnit, + spaceToGroundTransmitter, +) from Basilisk.simulation import groundLocation from Basilisk.utilities import vizSupport from Basilisk.utilities import unitTestSupport @@ -79,19 +85,21 @@ from Basilisk.architecture import astroConstants from Basilisk import __path__ + bskPath = __path__[0] path = os.path.dirname(os.path.abspath(__file__)) + def run(show_plots): - taskName = "unitTask" # arbitrary name (don't change) - processname = "TestProcess" # arbitrary name (don't change) + taskName = "unitTask" # arbitrary name (don't change) + processname = "TestProcess" # arbitrary name (don't change) # Create a sim module as an empty container scenarioSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(10.0) # update process rate update time + testProcessRate = macros.sec2nano(10.0) # update process rate update time testProc = scenarioSim.CreateNewProcess(processname) testProc.addTask(scenarioSim.CreateNewTask(taskName, testProcessRate)) @@ -105,27 +113,28 @@ def run(show_plots): planet = gravFactory.createEarth() planet.isCentralBody = True # ensure this is the central gravitational body - planet.useSphericalHarmonicsGravityModel(bskPath + '/supportData/LocalGravData/GGM03S-J2-only.txt', 2) + planet.useSphericalHarmonicsGravityModel( + bskPath + "/supportData/LocalGravData/GGM03S-J2-only.txt", 2 + ) mu = planet.mu # setup Spice interface for some solar system bodies - timeInitString = '2020 MAY 21 18:28:03 (UTC)' + timeInitString = "2020 MAY 21 18:28:03 (UTC)" spiceObject = gravFactory.createSpiceInterface(time=timeInitString) scenarioSim.AddModelToTask(taskName, spiceObject, -1) - # setup orbit using orbitalMotion library oe = orbitalMotion.ClassicElements() - oe.a = astroConstants.REQ_EARTH*1e3 + 418e3 + oe.a = astroConstants.REQ_EARTH * 1e3 + 418e3 oe.e = 0.00061 - oe.i = 51.6418*macros.D2R + oe.i = 51.6418 * macros.D2R - oe.Omega = 119.2314*macros.D2R - oe.omega = 337.8329*macros.D2R - oe.f = 22.2753*macros.D2R + oe.Omega = 119.2314 * macros.D2R + oe.omega = 337.8329 * macros.D2R + oe.f = 22.2753 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - n = np.sqrt(mu/oe.a/oe.a/oe.a) - P = 2.*np.pi/n + n = np.sqrt(mu / oe.a / oe.a / oe.a) + P = 2.0 * np.pi / n scObject.hub.r_CN_NInit = rN scObject.hub.v_CN_NInit = vN @@ -140,10 +149,10 @@ def run(show_plots): # Create the ground location groundStation = groundLocation.GroundLocation() groundStation.ModelTag = "BoulderGroundStation" - groundStation.planetRadius = astroConstants.REQ_EARTH*1e3 + groundStation.planetRadius = astroConstants.REQ_EARTH * 1e3 groundStation.specifyLocation(np.radians(40.009971), np.radians(-105.243895), 1624) groundStation.planetInMsg.subscribeTo(spiceObject.planetStateOutMsgs[0]) - groundStation.minimumElevation = np.radians(10.) + groundStation.minimumElevation = np.radians(10.0) groundStation.maximumRange = 1e9 groundStation.addSpacecraftToModel(scObject.scStateOutMsg) scenarioSim.AddModelToTask(taskName, groundStation) @@ -151,22 +160,22 @@ def run(show_plots): # Create an instrument instrument = simpleInstrument.SimpleInstrument() instrument.ModelTag = "instrument1" - instrument.nodeBaudRate = 2400. # baud - instrument.nodeDataName = "Instrument 1" # baud + instrument.nodeBaudRate = 2400.0 # baud + instrument.nodeDataName = "Instrument 1" # baud scenarioSim.AddModelToTask(taskName, instrument) # Create another instrument instrument2 = simpleInstrument.SimpleInstrument() instrument2.ModelTag = "instrument2" - instrument2.nodeBaudRate = 2400. # baud + instrument2.nodeBaudRate = 2400.0 # baud instrument2.nodeDataName = "Instrument 2" # baud scenarioSim.AddModelToTask(taskName, instrument2) # Create a "transmitter" transmitter = spaceToGroundTransmitter.SpaceToGroundTransmitter() transmitter.ModelTag = "transmitter" - transmitter.nodeBaudRate = -9600. # baud - transmitter.packetSize = -1E6 # bits + transmitter.nodeBaudRate = -9600.0 # baud + transmitter.packetSize = -1e6 # bits transmitter.numBuffers = 2 transmitter.addAccessMsgToTransmitter(groundStation.accessOutMsgs[-1]) scenarioSim.AddModelToTask(taskName, transmitter) @@ -174,7 +183,7 @@ def run(show_plots): # Create a partitionedStorageUnit and attach the instrument to it dataMonitor = partitionedStorageUnit.PartitionedStorageUnit() dataMonitor.ModelTag = "dataMonitor" - dataMonitor.storageCapacity = 8E9 # bits (1 GB) + dataMonitor.storageCapacity = 8e9 # bits (1 GB) dataMonitor.addDataNodeToModel(instrument.nodeDataOutMsg) dataMonitor.addDataNodeToModel(instrument2.nodeDataOutMsg) dataMonitor.addDataNodeToModel(transmitter.nodeDataOutMsg) @@ -187,7 +196,7 @@ def run(show_plots): # Create a simpleStorageUnit and attach the instrument to it dataMonitor2 = simpleStorageUnit.SimpleStorageUnit() dataMonitor2.ModelTag = "dataMonitor2" - dataMonitor2.storageCapacity = 1E5 # bits + dataMonitor2.storageCapacity = 1e5 # bits dataMonitor2.addDataNodeToModel(instrument.nodeDataOutMsg) dataMonitor2.addDataNodeToModel(instrument2.nodeDataOutMsg) dataMonitor2.addDataNodeToModel(transmitter.nodeDataOutMsg) @@ -213,16 +222,21 @@ def run(show_plots): # setup Vizard support if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scenarioSim, taskName, scObject - # , saveFile=__file__ - ) - vizSupport.addLocation(viz, stationName="Boulder Station" - , parentBodyName=planet.planetName - , r_GP_P=unitTestSupport.EigenVector3d2list(groundStation.r_LP_P_Init) - , fieldOfView=np.radians(160.) - , color='pink' - , range=1000.0*1000 # meters - ) + viz = vizSupport.enableUnityVisualization( + scenarioSim, + taskName, + scObject, + # , saveFile=__file__ + ) + vizSupport.addLocation( + viz, + stationName="Boulder Station", + parentBodyName=planet.planetName, + r_GP_P=unitTestSupport.EigenVector3d2list(groundStation.r_LP_P_Init), + fieldOfView=np.radians(160.0), + color="pink", + range=1000.0 * 1000, # meters + ) viz.settings.spacecraftSizeMultiplier = 1.5 viz.settings.showLocationCommLines = 1 viz.settings.showLocationCones = 1 @@ -235,7 +249,7 @@ def run(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - scenarioSim.ConfigureStopTime(macros.hour2nano(24)) # seconds to stop simulation + scenarioSim.ConfigureStopTime(macros.hour2nano(24)) # seconds to stop simulation # Begin the simulation time run set above scenarioSim.ExecuteSimulation() @@ -265,55 +279,65 @@ def run(show_plots): # Stopped here. Revisiting instrument implementation first. figureList = {} plt.close("all") # clears out plots from earlier test runs - fig=plt.figure(1) - plt.plot(tvec, storageLevel/(8E3), label='Data Unit Total Storage Level (KB)') - plt.plot(tvec, storedData[:, 0]/(8E3), label='Instrument 1 Partition Level (KB)') - plt.plot(tvec, storedData[:, 1]/(8E3), label='Instrument 2 Partition Level (KB)') - plt.xlabel('Time (Hr)') - plt.ylabel('Data Stored (KB)') + fig = plt.figure(1) + plt.plot(tvec, storageLevel / (8e3), label="Data Unit Total Storage Level (KB)") + plt.plot(tvec, storedData[:, 0] / (8e3), label="Instrument 1 Partition Level (KB)") + plt.plot(tvec, storedData[:, 1] / (8e3), label="Instrument 2 Partition Level (KB)") + plt.xlabel("Time (Hr)") + plt.ylabel("Data Stored (KB)") plt.grid(True) plt.legend() - figureList['scenarioGroundPassStorage'] = fig + figureList["scenarioGroundPassStorage"] = fig # Plot the orbit and ground station location data fig = plt.figure() - ax = fig.add_subplot(1, 1, 1, projection='3d') - ax.plot(scPosition[:,0]/1000.,scPosition[:, 1]/1000.,scPosition[:,2]/1000., label='S/C Position') - ax.plot(groundPosition[:,0]/1000.,groundPosition[:, 0]/1000.,groundPosition[:,2]/1000., label='Ground Station Position') + ax = fig.add_subplot(1, 1, 1, projection="3d") + ax.plot( + scPosition[:, 0] / 1000.0, + scPosition[:, 1] / 1000.0, + scPosition[:, 2] / 1000.0, + label="S/C Position", + ) + ax.plot( + groundPosition[:, 0] / 1000.0, + groundPosition[:, 0] / 1000.0, + groundPosition[:, 2] / 1000.0, + label="Ground Station Position", + ) plt.legend() - figureList['scenarioGroundPassECI'] = fig + figureList["scenarioGroundPassECI"] = fig fig = plt.figure() - plt.polar(pass_az, 90.-np.degrees(pass_el)) + plt.polar(pass_az, 90.0 - np.degrees(pass_el)) # ax.set_yticks(range(0, 90, 10)) # Define the yticks # ax.set_yticklabels(map(str, range(90, 0, -10))) - plt.title('Ground Pass Azimuth and Declination') - figureList['scenarioGroundPassPolar'] = fig + plt.title("Ground Pass Azimuth and Declination") + figureList["scenarioGroundPassPolar"] = fig plt.figure() - plt.plot(tvec, np.degrees(azimuthData),label='az') - plt.plot(tvec, np.degrees(elevationData), label='el') + plt.plot(tvec, np.degrees(azimuthData), label="az") + plt.plot(tvec, np.degrees(elevationData), label="el") plt.legend() plt.grid(True) - plt.ylabel('Angles (deg)') - plt.xlabel('Time (hr)') + plt.ylabel("Angles (deg)") + plt.xlabel("Time (hr)") - fig=plt.figure() - plt.plot(tvec, rangeData/1000.) - plt.plot(tvec, accessData*1000.) + fig = plt.figure() + plt.plot(tvec, rangeData / 1000.0) + plt.plot(tvec, accessData * 1000.0) plt.grid(True) - plt.title('Slant Range, Access vs. Time') - plt.ylabel('Slant Range (km)') - plt.xlabel('Time (hr)') - figureList['scenarioGroundPassRange'] = fig + plt.title("Slant Range, Access vs. Time") + plt.ylabel("Slant Range (km)") + plt.xlabel("Time (hr)") + figureList["scenarioGroundPassRange"] = fig fig = plt.figure() - plt.plot(tvec,storageNetBaud / (8E3), label='Net Baud Rate (KB/s)') - plt.xlabel('Time (Hr)') - plt.ylabel('Data Rate (KB/s)') + plt.plot(tvec, storageNetBaud / (8e3), label="Net Baud Rate (KB/s)") + plt.xlabel("Time (Hr)") + plt.ylabel("Data Rate (KB/s)") plt.grid(True) plt.legend() - figureList['scenarioGroundPassBaud'] = fig + figureList["scenarioGroundPassBaud"] = fig if show_plots: plt.show() @@ -321,6 +345,7 @@ def run(show_plots): return figureList + # This statement below ensures that the unitTestScript can be run as a # stand-alone python script diff --git a/examples/scenarioGroundLocationImaging.py b/examples/scenarioGroundLocationImaging.py index 543236a309..e8a3f8c64a 100644 --- a/examples/scenarioGroundLocationImaging.py +++ b/examples/scenarioGroundLocationImaging.py @@ -425,9 +425,8 @@ def run(show_plots): viz = vizSupport.enableUnityVisualization( scSim, simTaskName, - scObject + scObject, # , saveFile=fileName - , genericSensorList=genericSensorHUD, transceiverList=transceiverHUD, genericStorageList=hdDevicePanel, diff --git a/examples/scenarioGroundMapping.py b/examples/scenarioGroundMapping.py index 639aa7753e..6b21bad4f4 100644 --- a/examples/scenarioGroundMapping.py +++ b/examples/scenarioGroundMapping.py @@ -234,7 +234,7 @@ def run(show_plots, useCentral): earth.isCentralBody = useCentral # ensure this is the central gravitational body mu = earth.mu - timeInitString = '2020 MAY 21 18:28:03 (UTC)' + timeInitString = "2020 MAY 21 18:28:03 (UTC)" spiceObject = gravFactory.createSpiceInterface(time=timeInitString) scSim.AddModelToTask(simTaskName, spiceObject, -1) @@ -331,9 +331,7 @@ def run(show_plots, useCentral): # Setup the MRP Feedback control module mrpControl = mrpFeedback.mrpFeedback() mrpControl.ModelTag = "MRP_Feedback" - scSim.AddModelToTask( - simTaskName, mrpControl, ModelPriority=96 - ) + scSim.AddModelToTask(simTaskName, mrpControl, ModelPriority=96) mrpControl.guidInMsg.subscribeTo(locPoint.attGuidOutMsg) mrpControl.K = 5.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback @@ -389,9 +387,8 @@ def run(show_plots, useCentral): viz = vizSupport.enableUnityVisualization( scSim, simTaskName, - scObject + scObject, # , saveFile=fileName - , genericSensorList=genericSensorHUD, ) # the following command sets Viz settings for the first spacecraft in the simulation diff --git a/examples/scenarioHaloOrbit.py b/examples/scenarioHaloOrbit.py index 9f0e2b5dc2..e262ce92f8 100644 --- a/examples/scenarioHaloOrbit.py +++ b/examples/scenarioHaloOrbit.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2024, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -73,13 +72,20 @@ from Basilisk import __path__ from Basilisk.simulation import spacecraft from Basilisk.topLevelModules import pyswice -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport, vizSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, + vizSupport, +) from Basilisk.utilities.pyswice_spk_utilities import spkRead bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) + def run(showPlots=True): """ Args: @@ -113,8 +119,8 @@ def run(showPlots=True): # Setup gravity factory and gravity bodies # Include bodies as a list of SPICE names gravFactory = simIncludeGravBody.gravBodyFactory() - gravBodies = gravFactory.createBodies('moon', 'earth') - gravBodies['earth'].isCentralBody = True + gravBodies = gravFactory.createBodies("moon", "earth") + gravBodies["earth"].isCentralBody = True # Add gravity bodies to the spacecraft dynamics gravFactory.addBodiesTo(scObject) @@ -122,37 +128,50 @@ def run(showPlots=True): # Create default SPICE module, specify start date/time. timeInitString = "2022 August 31 15:00:00.0" bsk_path = __path__[0] - spiceObject = gravFactory.createSpiceInterface(bsk_path + "/supportData/EphemerisData/", time=timeInitString, - epochInMsg=True) - spiceObject.zeroBase = 'earth' + spiceObject = gravFactory.createSpiceInterface( + bsk_path + "/supportData/EphemerisData/", time=timeInitString, epochInMsg=True + ) + spiceObject.zeroBase = "earth" # Add SPICE object to the simulation task list scSim.AddModelToTask(simTaskName, spiceObject, 1) # Import SPICE ephemeris data into the python environment - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'de430.bsp') # solar system bodies - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'naif0012.tls') # leap second file - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'de-403-masses.tpc') # solar system masses - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'pck00010.tpc') # generic Planetary Constants Kernel + pyswice.furnsh_c(spiceObject.SPICEDataPath + "de430.bsp") # solar system bodies + pyswice.furnsh_c(spiceObject.SPICEDataPath + "naif0012.tls") # leap second file + pyswice.furnsh_c( + spiceObject.SPICEDataPath + "de-403-masses.tpc" + ) # solar system masses + pyswice.furnsh_c( + spiceObject.SPICEDataPath + "pck00010.tpc" + ) # generic Planetary Constants Kernel # Set spacecraft ICs # Get initial moon data - moonSpiceName = 'moon' - moonInitialState = 1000 * spkRead(moonSpiceName, timeInitString, 'J2000', 'earth') + moonSpiceName = "moon" + moonInitialState = 1000 * spkRead(moonSpiceName, timeInitString, "J2000", "earth") moon_rN_init = moonInitialState[0:3] moon_vN_init = moonInitialState[3:6] - moon = gravBodies['moon'] - earth = gravBodies['earth'] + moon = gravBodies["moon"] + earth = gravBodies["earth"] oe = orbitalMotion.rv2elem(earth.mu, moon_rN_init, moon_vN_init) moon_a = oe.a # Direction Cosine Matrix (DCM) from earth centered inertial frame to earth-moon rotation frame - DCMInit = np.array([moon_rN_init/np.linalg.norm(moon_rN_init),moon_vN_init/np.linalg.norm(moon_vN_init), - np.cross(moon_rN_init, moon_vN_init) / np.linalg.norm(np.cross(moon_rN_init, moon_vN_init))]) + DCMInit = np.array( + [ + moon_rN_init / np.linalg.norm(moon_rN_init), + moon_vN_init / np.linalg.norm(moon_vN_init), + np.cross(moon_rN_init, moon_vN_init) + / np.linalg.norm(np.cross(moon_rN_init, moon_vN_init)), + ] + ) # Set up non-dimensional parameters - T_ND = np.sqrt(moon_a ** 3 / (earth.mu + moon.mu)) # non-dimensional time for one second - mu_ND = moon.mu/(earth.mu + moon.mu) # non-dimensional mass + T_ND = np.sqrt( + moon_a**3 / (earth.mu + moon.mu) + ) # non-dimensional time for one second + mu_ND = moon.mu / (earth.mu + moon.mu) # non-dimensional mass # Set up initial conditions for the spacecraft x0 = 1.182212 * moon_a + moon_a * mu_ND @@ -172,7 +191,9 @@ def run(showPlots=True): # Setup data logging numDataPoints = 1000 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) # Setup spacecraft data recorder scDataRec = scObject.scStateOutMsg.recorder(samplingTime) @@ -181,9 +202,12 @@ def run(showPlots=True): scSim.AddModelToTask(simTaskName, MoonDataRec) if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - # saveFile=__file__ - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # saveFile=__file__ + ) viz.settings.showCelestialBodyLabels = 1 viz.settings.mainCameraTarget = "earth" viz.settings.trueTrajectoryLinesOn = 4 @@ -213,10 +237,10 @@ def run(showPlots=True): fig = plt.figure(1, figsize=tuple(np.array((1.0, b / oe.a)) * 4.75), dpi=100) plt.axis(np.array([-oe.rApoap, oe.rPeriap, -b, b]) / 1000 * 1.25) ax = fig.gca() - ax.ticklabel_format(style='scientific', scilimits=[-5, 3]) + ax.ticklabel_format(style="scientific", scilimits=[-5, 3]) # Draw 'cartoon' Earth - ax.add_artist(plt.Circle((0, 0), 0.2e5, color='b')) + ax.add_artist(plt.Circle((0, 0), 0.2e5, color="b")) # Plot spacecraft orbit data rDataSpacecraft = [] @@ -225,8 +249,13 @@ def run(showPlots=True): oeDataSpacecraft = orbitalMotion.rv2elem(earth.mu, posData[ii], velData[ii]) rDataSpacecraft.append(oeDataSpacecraft.rmag) fDataSpacecraft.append(oeDataSpacecraft.f + oeDataSpacecraft.omega - oe.omega) - plt.plot(rDataSpacecraft * np.cos(fDataSpacecraft) / 1000, rDataSpacecraft * np.sin(fDataSpacecraft) / 1000, - color='g', linewidth=3.0, label='Spacecraft') + plt.plot( + rDataSpacecraft * np.cos(fDataSpacecraft) / 1000, + rDataSpacecraft * np.sin(fDataSpacecraft) / 1000, + color="g", + linewidth=3.0, + label="Spacecraft", + ) # Plot moon orbit data rDataMoon = [] @@ -235,11 +264,16 @@ def run(showPlots=True): oeDataMoon = orbitalMotion.rv2elem(earth.mu, moonPos[ii], moonVel[ii]) rDataMoon.append(oeDataMoon.rmag) fDataMoon.append(oeDataMoon.f + oeDataMoon.omega - oe.omega) - plt.plot(rDataMoon * np.cos(fDataMoon) / 1000, rDataMoon * np.sin(fDataMoon) / 1000, color='0.5', - linewidth=3.0, label='Moon') + plt.plot( + rDataMoon * np.cos(fDataMoon) / 1000, + rDataMoon * np.sin(fDataMoon) / 1000, + color="0.5", + linewidth=3.0, + label="Moon", + ) - plt.xlabel(r'$i_e$ Coord. [km]') - plt.ylabel(r'$i_p$ Coord. [km]') + plt.xlabel(r"$i_e$ Coord. [km]") + plt.ylabel(r"$i_p$ Coord. [km]") plt.grid() plt.legend() pltName = fileName + "Fig1" @@ -248,12 +282,12 @@ def run(showPlots=True): # Second plot: Draw orbit in frame rotating with the Moon (the center is L2 point) # x axis is moon position vector direction and y axis is moon velocity vector direction fig = plt.figure(2, figsize=tuple(np.array((1.0, b / oe.a)) * 4.75), dpi=100) - plt.axis(np.array([-1e5, 5e5, -3e5, 3e5]) * 1.25) + plt.axis(np.array([-1e5, 5e5, -3e5, 3e5]) * 1.25) ax = fig.gca() - ax.ticklabel_format(style='scientific', scilimits=[-5, 3]) + ax.ticklabel_format(style="scientific", scilimits=[-5, 3]) # Draw 'cartoon' Earth - ax.add_artist(plt.Circle((0, 0), 0.2e5, color='b')) + ax.add_artist(plt.Circle((0, 0), 0.2e5, color="b")) # Plot spacecraft orbit data rSpacecraft = np.zeros((len(posData), 3)) @@ -266,17 +300,25 @@ def run(showPlots=True): # Direction Cosine Matrix (DCM) from earth centered inertial frame to earth-moon rotation frame rSpacecraftMag = np.linalg.norm(posData[ii]) rMoonMag = np.linalg.norm(moon_rN) - DCM = [moon_rN / rMoonMag, moon_vN / np.linalg.norm(moon_vN), - np.cross(moon_rN, moon_vN) / np.linalg.norm(np.cross(moon_rN, moon_vN))] + DCM = [ + moon_rN / rMoonMag, + moon_vN / np.linalg.norm(moon_vN), + np.cross(moon_rN, moon_vN) / np.linalg.norm(np.cross(moon_rN, moon_vN)), + ] # Spacecraft position in rotating frame - rSpacecraft[ii,:] = np.dot(DCM, posData[ii]) - - plt.plot(rSpacecraft[:,0] / 1000, rSpacecraft[:,1] / 1000, - color='g', linewidth=3.0, label='Spacecraft') + rSpacecraft[ii, :] = np.dot(DCM, posData[ii]) + + plt.plot( + rSpacecraft[:, 0] / 1000, + rSpacecraft[:, 1] / 1000, + color="g", + linewidth=3.0, + label="Spacecraft", + ) - plt.xlabel('Earth-Moon axis [km]') - plt.ylabel('Moon Velocity axis [km]') + plt.xlabel("Earth-Moon axis [km]") + plt.ylabel("Moon Velocity axis [km]") plt.grid() plt.legend() pltName = fileName + "Fig2" @@ -288,16 +330,21 @@ def run(showPlots=True): fig = plt.figure(3, figsize=tuple(np.array((1.0, b / oe.a)) * 4.75), dpi=100) plt.axis(np.array([-1e5, 5e5, -3e5, 3e5]) * 1.25) ax = fig.gca() - ax.ticklabel_format(style='scientific', scilimits=[-5, 3]) + ax.ticklabel_format(style="scientific", scilimits=[-5, 3]) # Draw 'cartoon' Earth - ax.add_artist(plt.Circle((0, 0), 0.2e5, color='b')) - - plt.plot(rSpacecraft[:, 0] / 1000, rSpacecraft[:, 2] / 1000, - color='g', linewidth=3.0, label='Spacecraft') + ax.add_artist(plt.Circle((0, 0), 0.2e5, color="b")) + + plt.plot( + rSpacecraft[:, 0] / 1000, + rSpacecraft[:, 2] / 1000, + color="g", + linewidth=3.0, + label="Spacecraft", + ) - plt.xlabel('Earth-Moon axis [km]') - plt.ylabel('Earth-Moon perpendicular axis [km]') + plt.xlabel("Earth-Moon axis [km]") + plt.ylabel("Earth-Moon perpendicular axis [km]") plt.grid() plt.legend() pltName = fileName + "Fig3" @@ -310,15 +357,19 @@ def run(showPlots=True): # Unload spice libraries gravFactory.unloadSpiceKernels() - pyswice.unload_c(spiceObject.SPICEDataPath + 'de430.bsp') # solar system bodies - pyswice.unload_c(spiceObject.SPICEDataPath + 'naif0012.tls') # leap second file - pyswice.unload_c(spiceObject.SPICEDataPath + 'de-403-masses.tpc') # solar system masses - pyswice.unload_c(spiceObject.SPICEDataPath + 'pck00010.tpc') # generic Planetary Constants Kernel + pyswice.unload_c(spiceObject.SPICEDataPath + "de430.bsp") # solar system bodies + pyswice.unload_c(spiceObject.SPICEDataPath + "naif0012.tls") # leap second file + pyswice.unload_c( + spiceObject.SPICEDataPath + "de-403-masses.tpc" + ) # solar system masses + pyswice.unload_c( + spiceObject.SPICEDataPath + "pck00010.tpc" + ) # generic Planetary Constants Kernel return figureList if __name__ == "__main__": run( - True # Show plots + True # Show plots ) diff --git a/examples/scenarioHelioTransSpice.py b/examples/scenarioHelioTransSpice.py index 5bb52b897e..e875db65dc 100644 --- a/examples/scenarioHelioTransSpice.py +++ b/examples/scenarioHelioTransSpice.py @@ -90,9 +90,10 @@ bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) + def run(): """ - Heliocentric mission simulation scenarion. + Heliocentric mission simulation scenarion. """ # Create simulation variable names simTaskName = "simTask" @@ -108,7 +109,7 @@ def run(): dynProcess = scSim.CreateNewProcess(simProcessName) # Create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(5 * 60 * 60.) + simulationTimeStep = macros.sec2nano(5 * 60 * 60.0) # Add the dynamics task to the dynamics process dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) @@ -131,7 +132,9 @@ def run(): # Load the custom Spice files using the SpiceInterface class loadSpiceKernel() method for file in customSpiceFiles: - spiceObject.loadSpiceKernel(file, os.path.join(path, "dataForExamples", "Spice/")) + spiceObject.loadSpiceKernel( + file, os.path.join(path, "dataForExamples", "Spice/") + ) # Add spacecraft name scNames = ["-60000"] @@ -145,11 +148,13 @@ def run(): scSim.AddModelToTask(simTaskName, scObject) # define the spacecraft inertia and other parameters - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # To set the spacecraft initial conditions, the following initial position and velocity variables are set: @@ -158,15 +163,20 @@ def run(): # Configure Vizard settings if vizSupport.vizFound: - colorMsgContent = messaging.ColorMsgPayload(colorRGBA=vizSupport.toRGBA255("Yellow")) + colorMsgContent = messaging.ColorMsgPayload( + colorRGBA=vizSupport.toRGBA255("Yellow") + ) colorMsg = messaging.ColorMsg().write(colorMsgContent) - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - , oscOrbitColorList=[vizSupport.toRGBA255("Magenta")] - , trueOrbitColorInMsgList=colorMsg.addSubscriber() - # , saveFile=__file__ - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + oscOrbitColorList=[vizSupport.toRGBA255("Magenta")], + trueOrbitColorInMsgList=colorMsg.addSubscriber(), + # , saveFile=__file__ + ) viz.epochInMsg.subscribeTo(gravFactory.epochMsg) viz.settings.orbitLinesOn = 1 viz.settings.spacecraftHelioViewSizeMultiplier = 3 diff --git a/examples/scenarioHingedRigidBody.py b/examples/scenarioHingedRigidBody.py index cb8ceb7ee9..65e11bfdaf 100755 --- a/examples/scenarioHingedRigidBody.py +++ b/examples/scenarioHingedRigidBody.py @@ -146,27 +146,38 @@ # import non-basilisk libraries import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # Allows for forces to act on the spacecraft without adding an effector like a thruster from Basilisk.simulation import extForceTorque from Basilisk.simulation import hingedRigidBodyStateEffector + # import simulation related support -from Basilisk.simulation import \ - spacecraft # The base of any spacecraft simulation which deals with spacecraft dynamics +from Basilisk.simulation import ( + spacecraft, +) # The base of any spacecraft simulation which deals with spacecraft dynamics + # import general simulation support files -from Basilisk.utilities import SimulationBaseClass # The class which contains the basilisk simuation environment +from Basilisk.utilities import ( + SimulationBaseClass, +) # The class which contains the basilisk simuation environment from Basilisk.utilities import macros # Some unit conversions from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) + def run(show_plots): """ At the end of the python script you can specify the following example parameters. @@ -230,7 +241,7 @@ def run(show_plots): scSim.panel2.mass = 100.0 scSim.panel2.IPntS_S = [[100.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] scSim.panel2.d = 1.5 - scSim.panel2.k = 1000. + scSim.panel2.k = 1000.0 scSim.panel2.c = 0.0 # c is the rotational damping coefficient for the hinge, which is modeled as a spring. scSim.panel2.r_HB_B = [[-0.5], [0.0], [1.0]] scSim.panel2.dcm_HB = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] @@ -255,7 +266,7 @@ def run(show_plots): # setup extForceTorque module extFTObject = extForceTorque.ExtForceTorque() extFTObject.ModelTag = "maneuverThrust" - extFTObject.extForce_N = [[0.], [0.], [0.]] + extFTObject.extForce_N = [[0.0], [0.0], [0.0]] scObject.addDynamicEffector(extFTObject) scSim.AddModelToTask(simTaskName, extFTObject) # Ending the HingedRigidBody State Effector @@ -265,7 +276,7 @@ def run(show_plots): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rLEO = 7000. * 1000 # meters + rLEO = 7000.0 * 1000 # meters oe.a = rLEO oe.e = 0.0001 oe.i = 0.0 * macros.D2R @@ -278,7 +289,7 @@ def run(show_plots): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n simulationTimeFactor = 0.01 simulationTime = macros.sec2nano(simulationTimeFactor * P) @@ -286,7 +297,9 @@ def run(show_plots): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) pl1Log = scSim.panel1.hingedRigidBodyOutMsg.recorder(samplingTime) pl2Log = scSim.panel2.hingedRigidBodyOutMsg.recorder(samplingTime) @@ -295,9 +308,12 @@ def run(show_plots): scSim.AddModelToTask(simTaskName, pl2Log) # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation @@ -311,8 +327,10 @@ def run(show_plots): scSim.ExecuteSimulation() # compute maneuver Delta_v's - extFTObject.extForce_N = [[-2050.], [-1430.], [-.00076]] - T2 = macros.sec2nano(935.) # this is the amount of time to get a deltaV equal to what the other tutorial has. + extFTObject.extForce_N = [[-2050.0], [-1430.0], [-0.00076]] + T2 = macros.sec2nano( + 935.0 + ) # this is the amount of time to get a deltaV equal to what the other tutorial has. # run simulation for 2nd chunk scSim.ConfigureStopTime(simulationTime + T2) @@ -339,52 +357,58 @@ def run(show_plots): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, posData[:, idx] / 1000., - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [h]') - plt.ylabel('Inertial Position [km]') + plt.plot( + timeAxis * macros.NANO2MIN, + posData[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [h]") + plt.ylabel("Inertial Position [km]") figureList = {} - pltName = fileName + "1" + str(int(0.)) + pltName = fileName + "1" + str(int(0.0)) figureList[pltName] = plt.figure(1) # show SMA plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") rData = [] for idx in range(0, len(posData)): oeData = orbitalMotion.rv2elem_parab(mu, posData[idx], velData[idx]) - rData.append(oeData.rmag / 1000.) - plt.plot(timeAxis * macros.NANO2MIN, rData, color='#aa0000', - ) - plt.xlabel('Time [min]') - plt.ylabel('Radius [km]') - pltName = fileName + "2" + str(int(0.)) + rData.append(oeData.rmag / 1000.0) + plt.plot( + timeAxis * macros.NANO2MIN, + rData, + color="#aa0000", + ) + plt.xlabel("Time [min]") + plt.ylabel("Radius [km]") + pltName = fileName + "2" + str(int(0.0)) figureList[pltName] = plt.figure(2) plt.figure(3) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") plt.plot(timeAxis * macros.NANO2MIN, panel1thetaLog) - plt.xlabel('Time [min]') - plt.ylabel('Panel 1 Angular Displacement [r]') - pltName = fileName + "panel1theta" + str(int(0.)) + plt.xlabel("Time [min]") + plt.ylabel("Panel 1 Angular Displacement [r]") + pltName = fileName + "panel1theta" + str(int(0.0)) figureList[pltName] = plt.figure(3) plt.figure(4) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") plt.plot(timeAxis * macros.NANO2MIN, panel2thetaLog) - plt.xlabel('Time [min]') - plt.ylabel('Panel 2 Angular Displacement [r]') - pltName = fileName + "panel2theta" + str(int(0.)) + plt.xlabel("Time [min]") + plt.ylabel("Panel 2 Angular Displacement [r]") + pltName = fileName + "panel2theta" + str(int(0.0)) figureList[pltName] = plt.figure(4) if show_plots: diff --git a/examples/scenarioHohmann.py b/examples/scenarioHohmann.py index 93055a9df6..730de95124 100644 --- a/examples/scenarioHohmann.py +++ b/examples/scenarioHohmann.py @@ -124,10 +124,10 @@ def plotOrbit(rFirst, rSecond, posData): # plot the earth - ax = plt.axes(projection='3d') - planetColor = '#008800' + ax = plt.axes(projection="3d") + planetColor = "#008800" planetRadius = 6378.1 - u, v = np.mgrid[0:2 * np.pi:40j, 0:np.pi:40j] + u, v = np.mgrid[0 : 2 * np.pi : 40j, 0 : np.pi : 40j] x = planetRadius * np.cos(u) * np.sin(v) y = planetRadius * np.sin(u) * np.sin(v) z = planetRadius * np.cos(v) @@ -142,45 +142,59 @@ def plotOrbit(rFirst, rSecond, posData): ax.plot_surface(x, y, z, color=planetColor) # plot the orbit - ax.plot3D(posData[:, 0] / 1000, posData[:, 1] / 1000, posData[:, 2] / 1000, - color='orangered', label='Simulated Flight') - ax.set_xlabel('x [km]') - ax.set_ylabel('y [km]') - ax.set_zlabel('z [km]') - ax.set_aspect('equal') + ax.plot3D( + posData[:, 0] / 1000, + posData[:, 1] / 1000, + posData[:, 2] / 1000, + color="orangered", + label="Simulated Flight", + ) + ax.set_xlabel("x [km]") + ax.set_ylabel("y [km]") + ax.set_zlabel("z [km]") + ax.set_aspect("equal") def plotAttitudeError(timeAxis, attErrorData): plt.figure(2) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, attErrorData[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='best') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeAxis * macros.NANO2MIN, + attErrorData[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="best") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") def plotAttitude(timeAxis, attData): plt.figure(3) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, attData[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='best') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude $\sigma_{B/N}$') + plt.plot( + timeAxis * macros.NANO2MIN, + attData[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="best") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude $\sigma_{B/N}$") def plotReferenceAttitude(timeAxis, attRefData): plt.figure(4) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, attRefData[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='best') - plt.xlabel('Time [min]') - plt.ylabel(r'Reference Attitude $\sigma_{R/N}$') + plt.plot( + timeAxis * macros.NANO2MIN, + attRefData[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="best") + plt.xlabel("Time [min]") + plt.ylabel(r"Reference Attitude $\sigma_{R/N}$") def run(show_plots, rFirst, rSecond): @@ -213,11 +227,13 @@ def run(show_plots, rFirst, rSecond): # Initialize spacecraft object and set properties scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # Add spacecraft object to the simulation process @@ -231,10 +247,10 @@ def run(show_plots, rFirst, rSecond): # Override information with SPICE timeInitString = "2021 MAY 04 07:47:48.965 (UTC)" - gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/', - timeInitString, - epochInMsg=True) - gravFactory.spiceObject.zeroBase = 'Earth' + gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", timeInitString, epochInMsg=True + ) + gravFactory.spiceObject.zeroBase = "Earth" gravFactory.addBodiesTo(scObject) # Add ephemeris for Hill frame @@ -245,9 +261,9 @@ def run(show_plots, rFirst, rSecond): # Add RW devices rwFactory = simIncludeRW.rwFactory() - RW1 = rwFactory.create('Honeywell_HR16', [1, 0, 0], maxMomentum=50., Omega=100.) - RW2 = rwFactory.create('Honeywell_HR16', [0, 1, 0], maxMomentum=50., Omega=200.) - RW3 = rwFactory.create('Honeywell_HR16', [0, 0, 1], maxMomentum=50., Omega=300.) + RW1 = rwFactory.create("Honeywell_HR16", [1, 0, 0], maxMomentum=50.0, Omega=100.0) + RW2 = rwFactory.create("Honeywell_HR16", [0, 1, 0], maxMomentum=50.0, Omega=200.0) + RW3 = rwFactory.create("Honeywell_HR16", [0, 0, 1], maxMomentum=50.0, Omega=300.0) rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector() rwStateEffector.ModelTag = "RW_cluster" rwFactory.addToSpacecraft(rwStateEffector.ModelTag, rwStateEffector, scObject) @@ -266,12 +282,14 @@ def run(show_plots, rFirst, rSecond): firstBurn = velocityPoint.velocityPoint() firstBurn.ModelTag = "velocityPoint" firstBurn.mu = mu - scSim.fswProcess.addTask(scSim.CreateNewTask("firstBurnTask", simulationTimeStep), 5) + scSim.fswProcess.addTask( + scSim.CreateNewTask("firstBurnTask", simulationTimeStep), 5 + ) scSim.AddModelToTask("firstBurnTask", firstBurn) firstBurnMRPRotation = mrpRotation.mrpRotation() firstBurnMRPRotation.ModelTag = "mrpRotation" - sigma_RR0 = np.array([np.tan(- np.pi / 8), 0, 0]) + sigma_RR0 = np.array([np.tan(-np.pi / 8), 0, 0]) firstBurnMRPRotation.mrpSet = sigma_RR0 scSim.AddModelToTask("firstBurnTask", firstBurnMRPRotation) messaging.AttRefMsg_C_addAuthor(firstBurnMRPRotation.attRefOutMsg, attRefMsg) @@ -280,7 +298,9 @@ def run(show_plots, rFirst, rSecond): secondBurn = velocityPoint.velocityPoint() secondBurn.ModelTag = "velocityPoint" secondBurn.mu = mu - scSim.fswProcess.addTask(scSim.CreateNewTask("secondBurnTask", simulationTimeStep), 5) + scSim.fswProcess.addTask( + scSim.CreateNewTask("secondBurnTask", simulationTimeStep), 5 + ) scSim.AddModelToTask("secondBurnTask", secondBurn) # Need to get this reference attitude to rotate 180 @@ -294,7 +314,9 @@ def run(show_plots, rFirst, rSecond): # Set up hill point guidance module attGuidanceHillPoint = hillPoint.hillPoint() attGuidanceHillPoint.ModelTag = "hillPoint" - scSim.fswProcess.addTask(scSim.CreateNewTask("hillPointTask", simulationTimeStep), 5) + scSim.fswProcess.addTask( + scSim.CreateNewTask("hillPointTask", simulationTimeStep), 5 + ) scSim.AddModelToTask("hillPointTask", attGuidanceHillPoint) messaging.AttRefMsg_C_addAuthor(attGuidanceHillPoint.attRefOutMsg, attRefMsg) @@ -419,25 +441,28 @@ def run(show_plots, rFirst, rSecond): # Set the simulation time variable and the period of the first orbit simulationTime = 0 - P1 = 2 * np.pi * np.sqrt(oe.a ** 3 / mu) + P1 = 2 * np.pi * np.sqrt(oe.a**3 / mu) # if this scenario is to interface with the BSK Viz, uncomment the following line - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) scSim.InitializeSimulation() scSim.ShowExecutionOrder() scSim.SetProgressBar(True) # Start in Hill Point and run 40% of a period on the initial circular orbit - scSim.modeRequest = 'hillPoint' + scSim.modeRequest = "hillPoint" simulationTime += macros.sec2nano(0.4 * P1) scSim.ConfigureStopTime(simulationTime) scSim.ExecuteSimulation() # Change to first burn orientation in preparation for burn - scSim.modeRequest = 'firstBurn' + scSim.modeRequest = "firstBurn" simulationTime += macros.sec2nano(0.1 * P1) scSim.ConfigureStopTime(simulationTime) scSim.ExecuteSimulation() @@ -450,7 +475,7 @@ def run(show_plots, rFirst, rSecond): # Conduct the first burn of a Hohmann transfer from rFirst to rSecond rData = np.linalg.norm(rN) vData = np.linalg.norm(vN) - at = (rData + rSecond) * .5 + at = (rData + rSecond) * 0.5 vt1 = np.sqrt((2 * mu / rData) - (mu / at)) Delta_V_1 = vt1 - vData vHat = vN / np.linalg.norm(vN) @@ -458,7 +483,7 @@ def run(show_plots, rFirst, rSecond): velRef.setState(new_v) # Define the transfer time - time_Transfer = np.pi * np.sqrt(at ** 3 / mu) + time_Transfer = np.pi * np.sqrt(at**3 / mu) # Continue the simulation with new delta v simulationTime += macros.sec2nano(0.2 * time_Transfer) @@ -466,13 +491,13 @@ def run(show_plots, rFirst, rSecond): scSim.ExecuteSimulation() # Change back to hill point for 60% of transfer orbit - scSim.modeRequest = 'hillPoint' + scSim.modeRequest = "hillPoint" simulationTime += macros.sec2nano(0.6 * time_Transfer) scSim.ConfigureStopTime(simulationTime) scSim.ExecuteSimulation() # Change to second burn orientation in preparation for second burn - scSim.modeRequest = 'secondBurn' + scSim.modeRequest = "secondBurn" simulationTime += macros.sec2nano(0.2 * time_Transfer) scSim.ConfigureStopTime(simulationTime) scSim.ExecuteSimulation() @@ -484,7 +509,7 @@ def run(show_plots, rFirst, rSecond): # Conduct the second burn of a Hohmann transfer from rFirst to rSecond rData = np.linalg.norm(rN) vData = np.linalg.norm(vN) - at = (rData + rSecond) * .5 + at = (rData + rSecond) * 0.5 vt2 = np.sqrt((2 * mu / rData) - (mu / at)) Delta_V_2 = vt2 - vData vHat = vN / np.linalg.norm(vN) # unit vec? @@ -493,7 +518,7 @@ def run(show_plots, rFirst, rSecond): # Find the period for second orbit a2 = rSecond - P2 = 2 * np.pi * np.sqrt(a2 ** 3 / mu) + P2 = 2 * np.pi * np.sqrt(a2**3 / mu) # Continue the simulation with new delta v simulationTime += macros.sec2nano(0.1 * P2) @@ -501,7 +526,7 @@ def run(show_plots, rFirst, rSecond): scSim.ExecuteSimulation() # Change back to Hill Point on second orbit - scSim.modeRequest = 'hillPoint' + scSim.modeRequest = "hillPoint" simulationTime += macros.sec2nano(0.2 * P2) scSim.ConfigureStopTime(simulationTime) scSim.ExecuteSimulation() @@ -550,5 +575,5 @@ def run(show_plots, rFirst, rSecond): run( True, # show_plots 7000000, # major axis first orbit (m) - 42164000 # major axis second orbit (m) + 42164000, # major axis second orbit (m) ) diff --git a/examples/scenarioInertialSpiral.py b/examples/scenarioInertialSpiral.py index 0ddf68903d..944c95cefb 100644 --- a/examples/scenarioInertialSpiral.py +++ b/examples/scenarioInertialSpiral.py @@ -85,7 +85,9 @@ # import general simulation support files from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import matplotlib.pyplot as plt from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion @@ -109,6 +111,7 @@ # 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]) @@ -129,7 +132,7 @@ def run(show_plots): scSim = SimulationBaseClass.SimBaseClass() # Define simulation run time and integration time step - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) simulationTimeStep = macros.sec2nano(0.1) # Create the simulation process @@ -140,11 +143,13 @@ def run(show_plots): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bskSat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -180,7 +185,7 @@ def run(show_plots): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # we create 2 dynamic attitude reference modules as we want to do a 1-2 Euler angle rotation # and the modules provide a 3-2-1 sequence. Thus, we do a 0-0-1 321-rotation and then a 0-1-0 321-rotation @@ -211,7 +216,7 @@ def run(show_plots): mrpControl.K = 3.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # # create simulation messages @@ -234,7 +239,9 @@ def run(show_plots): # Set up data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) snAttLog = sNavObject.attOutMsg.recorder(samplingTime) snLog = sNavObject.scStateInMsg.recorder(samplingTime) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) @@ -258,8 +265,8 @@ def run(show_plots): rN, vN = orbitalMotion.elem2rv(mu, oe) scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - scObject.hub.sigma_BNInit = [[0.], [0.], [0.]] # sigma_BN_B - scObject.hub.omega_BN_BInit = [[0.], [0.], [0.]] # rad/s - omega_BN_B + scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] # sigma_BN_B + scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B # # initialize Simulation @@ -291,53 +298,70 @@ def run(show_plots): plt.close("all") # clears out plots from earlier test runs plt.figure(1) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, attErrorLog.sigma_BR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeAxis * macros.NANO2MIN, + attErrorLog.sigma_BR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) plt.figure(2) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, mrpLog.torqueRequestBody[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='upper right') - plt.xlabel('Time [min]') - plt.ylabel(r'Control Torque $L_r$ [Nm]') + plt.plot( + timeAxis * macros.NANO2MIN, + mrpLog.torqueRequestBody[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="upper right") + plt.xlabel("Time [min]") + plt.ylabel(r"Control Torque $L_r$ [Nm]") pltName = fileName + "2" figureList[pltName] = plt.figure(2) plt.figure(3) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, attErrorLog.omega_BR_B[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeAxis * macros.NANO2MIN, + attErrorLog.omega_BR_B[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") pltName = fileName + "3" figureList[pltName] = plt.figure(3) plt.figure(4) - plt.plot(timeLineSet, dataEulerAnglesYaw, - color=unitTestSupport.getLineColor(0, 3),label=r'Yaw') - plt.plot(timeLineSet, dataEulerAnglesPitch, - color=unitTestSupport.getLineColor(1, 3),label=r'Pitch') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Euler Angles [rad]') + plt.plot( + timeLineSet, + dataEulerAnglesYaw, + color=unitTestSupport.getLineColor(0, 3), + label=r"Yaw", + ) + plt.plot( + timeLineSet, + dataEulerAnglesPitch, + color=unitTestSupport.getLineColor(1, 3), + label=r"Pitch", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Euler Angles [rad]") pltName = fileName + "4" figureList[pltName] = plt.figure(4) plt.figure(5) plt.plot(dataEulerAnglesPitch, dataEulerAnglesYaw) - plt.xlabel('Pitch [rad]') - plt.ylabel('Yaw [rad]') + plt.xlabel("Pitch [rad]") + plt.ylabel("Yaw [rad]") pltName = fileName + "5" figureList[pltName] = plt.figure(5) @@ -350,7 +374,6 @@ def run(show_plots): return figureList - # # This statement below ensures that the unit test scrip can be run as a # stand-along python script diff --git a/examples/scenarioIntegrators.py b/examples/scenarioIntegrators.py index afe67ac44b..e5b0e91b42 100755 --- a/examples/scenarioIntegrators.py +++ b/examples/scenarioIntegrators.py @@ -297,7 +297,7 @@ def run(show_plots, integratorCase): vizSupport.enableUnityVisualization( scSim, simTaskName, - scObject + scObject, # , saveFile=fileName ) @@ -328,7 +328,7 @@ def run(show_plots, integratorCase): # draw orbit in perifocal frame b = oe.a * np.sqrt(1 - oe.e * oe.e) p = oe.a * (1 - oe.e * oe.e) - if integratorCase == 'rk4': + if integratorCase == "rk4": plt.figure(1, figsize=tuple(np.array((1.0, b / oe.a)) * 4.75), dpi=100) plt.axis(np.array([-oe.rApoap, oe.rPeriap, -b, b]) / 1000 * 1.25) # draw the planet @@ -347,9 +347,8 @@ def run(show_plots, integratorCase): fData.append(oeData.f + oeData.omega - oe.omega) plt.plot( rData * np.cos(fData) / 1000, - rData * np.sin(fData) / 1000 + rData * np.sin(fData) / 1000, # , color=unitTestSupport.getLineColor(labelStrings.index(integratorCase), len(labelStrings)) - , label=integratorCase, linewidth=3.0, ) @@ -392,5 +391,6 @@ def run(show_plots, integratorCase): # if __name__ == "__main__": run( - True, "rk4" # show_plots + True, + "rk4", # show_plots ) # integrator case(0 - rk4, 1 - rkf45, 2 - rkf78, 3 - euler, 4 - rk2, 5 - rk3, 6 - bogackiShampine) diff --git a/examples/scenarioJupiterArrival.py b/examples/scenarioJupiterArrival.py index 2200d02cbe..486398fd9b 100644 --- a/examples/scenarioJupiterArrival.py +++ b/examples/scenarioJupiterArrival.py @@ -91,9 +91,16 @@ fileName = os.path.basename(os.path.splitext(__file__)[0]) from Basilisk.simulation import spacecraft, gravityEffector -from Basilisk.utilities import SimulationBaseClass, macros, orbitalMotion, simIncludeGravBody, unitTestSupport +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, +) from Basilisk.utilities import vizSupport + def run(show_plots): """ The scenarios can be run with the followings setups parameters: @@ -112,7 +119,7 @@ def run(show_plots): dynProcess = scSim.CreateNewProcess(simProcessName) # Create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) @@ -149,21 +156,33 @@ def run(show_plots): # oe = orbitalMotion.ClassicElements() - sunmu = 132600000000000000000 # [m^3/s^3] - r_J_2_S = 778298361. * 1000 # [m] Distance from Jupiter to Sun - r_E_2_S = 149598023. * 1000 # [m] Distance from Earth to Sun - r_SC_J_park = 800000. * 1000 # [m] Desired circular parking orbit radius - - V_J_C_S = np.sqrt(sunmu/r_J_2_S) # [m/s] (1) "J"upiter "C"ircular speed relative to the "S"un - V_SC_C_J = np.sqrt(jupiter.mu/r_SC_J_park) # [m/s] (2) "S"pace"C"raft "C"ircular parking speed relative to "J"upiter. - V_SC_A_S = np.sqrt( sunmu*r_E_2_S / ((r_J_2_S + r_E_2_S)*r_J_2_S) ) # [m/s] (3) "S"pace"C"raft speed at "A"poapsis of Hohmann transfer ellipse relative to the "S"un - V_SC_A_J = V_SC_A_S - V_J_C_S # [m/s] (4) "S"pace"C"raft speed at "A"poapsis of Hohmann transfer ellipse relative to "J"upiter - - a_H = - ( jupiter.mu / ( V_SC_A_J*V_SC_A_J) ) # [m] Semimajor axis (-) of arrival hyperbola. - - V_SC_P_H = np.sqrt( (V_SC_A_J*V_SC_A_J) + (2*jupiter.mu / r_SC_J_park) ) # [m/s] (5) "S"pace"C"raft speed at "P"eriapsis of "H"ohmann transfer ellipse (Before delta V performed) + sunmu = 132600000000000000000 # [m^3/s^3] + r_J_2_S = 778298361.0 * 1000 # [m] Distance from Jupiter to Sun + r_E_2_S = 149598023.0 * 1000 # [m] Distance from Earth to Sun + r_SC_J_park = 800000.0 * 1000 # [m] Desired circular parking orbit radius + + V_J_C_S = np.sqrt( + sunmu / r_J_2_S + ) # [m/s] (1) "J"upiter "C"ircular speed relative to the "S"un + V_SC_C_J = np.sqrt( + jupiter.mu / r_SC_J_park + ) # [m/s] (2) "S"pace"C"raft "C"ircular parking speed relative to "J"upiter. + V_SC_A_S = np.sqrt( + sunmu * r_E_2_S / ((r_J_2_S + r_E_2_S) * r_J_2_S) + ) # [m/s] (3) "S"pace"C"raft speed at "A"poapsis of Hohmann transfer ellipse relative to the "S"un + V_SC_A_J = ( + V_SC_A_S - V_J_C_S + ) # [m/s] (4) "S"pace"C"raft speed at "A"poapsis of Hohmann transfer ellipse relative to "J"upiter + + a_H = -( + jupiter.mu / (V_SC_A_J * V_SC_A_J) + ) # [m] Semimajor axis (-) of arrival hyperbola. + + V_SC_P_H = np.sqrt( + (V_SC_A_J * V_SC_A_J) + (2 * jupiter.mu / r_SC_J_park) + ) # [m/s] (5) "S"pace"C"raft speed at "P"eriapsis of "H"ohmann transfer ellipse (Before delta V performed) Delta_V_Parking_Orbit = V_SC_C_J - V_SC_P_H - e_H = 1 + ((r_SC_J_park*V_SC_A_J*V_SC_A_J) / jupiter.mu) + e_H = 1 + ((r_SC_J_park * V_SC_A_J * V_SC_A_J) / jupiter.mu) oe.a = a_H oe.e = e_H oe.i = 0.0 * macros.D2R @@ -173,8 +192,11 @@ def run(show_plots): # Determine required simulation time (time of flight) from SC initial position on hyperbola to periapsis. (Time until delta V must be performed) # Method: Use hyperbolic time equation to find TOF: (t-tp) - zeta = 2*np.arctan( np.tan(oe.f / 2) * np.sqrt( (oe.e - 1) / (oe.e + 1) ) ) - t_tp = np.abs( np.sqrt(-oe.a * oe.a * oe.a / jupiter.mu) * ( oe.e * np.tan(zeta) - np.log( np.tan((zeta/2) + (np.pi/4)) ) ) ) + zeta = 2 * np.arctan(np.tan(oe.f / 2) * np.sqrt((oe.e - 1) / (oe.e + 1))) + t_tp = np.abs( + np.sqrt(-oe.a * oe.a * oe.a / jupiter.mu) + * (oe.e * np.tan(zeta) - np.log(np.tan((zeta / 2) + (np.pi / 4)))) + ) # Setting initial position and velocity vectors using orbital elements r_N, v_N = orbitalMotion.elem2rv(jupiter.mu, oe) @@ -184,15 +206,17 @@ def run(show_plots): e_park = 0 # Initialize Spacecraft States with the initialization variables - scObject.hub.r_CN_NInit = r_N # [m] = r_BN_N - scObject.hub.v_CN_NInit = v_N # [m/s] = v_BN_N + scObject.hub.r_CN_NInit = r_N # [m] = r_BN_N + scObject.hub.v_CN_NInit = v_N # [m/s] = v_BN_N # Set the simulation time simulationTime = macros.sec2nano(t_tp) # Simulation Stops At Periapsis # Setup data logging before the simulation is initialized numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataRec = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataRec) @@ -205,9 +229,12 @@ def run(show_plots): # Vizard and played back after running the BSK simulation. # To enable this, uncomment this line: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - # saveFile=__file__ - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # saveFile=__file__ + ) # Initialize and execute simulation for the first section (stops at periapsis of hyperbola before delta V) scSim.InitializeSimulation() @@ -222,7 +249,7 @@ def run(show_plots): # Apply delta V and set new velocity state vHat = vN / np.linalg.norm(vN) - vN = vN + Delta_V_Parking_Orbit*vHat + vN = vN + Delta_V_Parking_Orbit * vHat velRef.setState(vN) # Run the simulation for 2nd chunk @@ -253,45 +280,66 @@ def plotOrbits(timeAxis, posData, jupiter, a_park, e_park): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") for idx in range(3): - plt.plot(timeAxis * macros.NANO2SEC, posData[:,idx] / 1000., - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Inertial Position [km]') + plt.plot( + timeAxis * macros.NANO2SEC, + posData[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Inertial Position [km]") figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) # Figure 2: Plot arrival to Jupiter - plt.figure(2,figsize=(5,5)) + plt.figure(2, figsize=(5, 5)) plt.axis([-16, 16, -16, 16]) # Draw the planet fig = plt.gcf() ax = fig.gca() - ax.set_aspect('equal') - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - ax.get_xaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - planetColor = '#008800' + ax.set_aspect("equal") + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + ax.get_xaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + planetColor = "#008800" planetRadius = 1.0 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # Draw the actual orbit from pulled data (DataRec) - plt.plot(posData[:,0] / jupiter.radEquator, posData[:,1] / jupiter.radEquator, color='orangered', label='Simulated Flight') - plt.xlabel('Jupiter Velocity Direction [DU]') - plt.ylabel('Anti-Sunward Direction [DU]') + plt.plot( + posData[:, 0] / jupiter.radEquator, + posData[:, 1] / jupiter.radEquator, + color="orangered", + label="Simulated Flight", + ) + plt.xlabel("Jupiter Velocity Direction [DU]") + plt.ylabel("Anti-Sunward Direction [DU]") # Draw desired parking orbit - fData = np.linspace(0, 2*np.pi, 100) + fData = np.linspace(0, 2 * np.pi, 100) rData = [] for indx in range(0, len(fData)): - rData.append( ( a_park/jupiter.radEquator * (1 - e_park * e_park) ) / (1 + e_park * np.cos(fData[indx])) ) - plt.plot(rData * np.cos(fData), rData * np.sin(fData), '--', color='#555555', label='Desired Circ. Parking Orbit') - plt.legend(loc='upper right') + rData.append( + (a_park / jupiter.radEquator * (1 - e_park * e_park)) + / (1 + e_park * np.cos(fData[indx])) + ) + plt.plot( + rData * np.cos(fData), + rData * np.sin(fData), + "--", + color="#555555", + label="Desired Circ. Parking Orbit", + ) + plt.legend(loc="upper right") plt.grid() pltName = fileName + "2" figureList[pltName] = plt.figure(2) diff --git a/examples/scenarioLagrangePointOrbit.py b/examples/scenarioLagrangePointOrbit.py index 3797f1ba7f..e5a777d716 100644 --- a/examples/scenarioLagrangePointOrbit.py +++ b/examples/scenarioLagrangePointOrbit.py @@ -136,13 +136,20 @@ from Basilisk.simulation import orbElemConvert from Basilisk.simulation import spacecraft from Basilisk.topLevelModules import pyswice -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport, vizSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, + vizSupport, +) from Basilisk.utilities.pyswice_spk_utilities import spkRead bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) + def run(lagrangePoint, nOrbits, timestep, showPlots=True): """ Args: @@ -182,56 +189,60 @@ def run(lagrangePoint, nOrbits, timestep, showPlots=True): # Setup gravity factory and gravity bodies # Include bodies as a list of SPICE names gravFactory = simIncludeGravBody.gravBodyFactory() - gravBodies = gravFactory.createBodies('moon', 'earth') - gravBodies['earth'].isCentralBody = True + gravBodies = gravFactory.createBodies("moon", "earth") + gravBodies["earth"].isCentralBody = True # Add gravity bodies to the spacecraft dynamics gravFactory.addBodiesTo(scObject) # Create default SPICE module, specify start date/time. timeInitString = "2022 August 31 15:00:00.0" - spiceTimeStringFormat = '%Y %B %d %H:%M:%S.%f' + spiceTimeStringFormat = "%Y %B %d %H:%M:%S.%f" timeInit = datetime.strptime(timeInitString, spiceTimeStringFormat) spiceObject = gravFactory.createSpiceInterface(time=timeInitString, epochInMsg=True) - spiceObject.zeroBase = 'Earth' + spiceObject.zeroBase = "Earth" # Add SPICE object to the simulation task list scSim.AddModelToTask(simTaskName, spiceObject, 1) # Import SPICE ephemeris data into the python environment - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'de430.bsp') # solar system bodies - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'naif0012.tls') # leap second file - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'de-403-masses.tpc') # solar system masses - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'pck00010.tpc') # generic Planetary Constants Kernel + pyswice.furnsh_c(spiceObject.SPICEDataPath + "de430.bsp") # solar system bodies + pyswice.furnsh_c(spiceObject.SPICEDataPath + "naif0012.tls") # leap second file + pyswice.furnsh_c( + spiceObject.SPICEDataPath + "de-403-masses.tpc" + ) # solar system masses + pyswice.furnsh_c( + spiceObject.SPICEDataPath + "pck00010.tpc" + ) # generic Planetary Constants Kernel # Set spacecraft ICs # Use Earth data - moonSpiceName = 'moon' - moonInitialState = 1000 * spkRead(moonSpiceName, timeInitString, 'J2000', 'earth') + moonSpiceName = "moon" + moonInitialState = 1000 * spkRead(moonSpiceName, timeInitString, "J2000", "earth") moon_rN_init = moonInitialState[0:3] moon_vN_init = moonInitialState[3:6] - moon = gravBodies['moon'] - earth = gravBodies['earth'] + moon = gravBodies["moon"] + earth = gravBodies["earth"] oe = orbitalMotion.rv2elem(earth.mu, moon_rN_init, moon_vN_init) moon_a = oe.a # Delay or advance the spacecraft by a few degrees to prevent strange spacecraft-moon interactions when the # spacecraft wanders from the unstable equilibrium points if lagrangePoint == 1: - oe.a = oe.a * (1-np.power(moon.mu / (3*earth.mu), 1./3.)) - oe.f = oe.f + macros.D2R*4 + oe.a = oe.a * (1 - np.power(moon.mu / (3 * earth.mu), 1.0 / 3.0)) + oe.f = oe.f + macros.D2R * 4 elif lagrangePoint == 2: - oe.a = oe.a * (1+np.power(moon.mu / (3*earth.mu), 1./3.)) - oe.f = oe.f - macros.D2R*4 + oe.a = oe.a * (1 + np.power(moon.mu / (3 * earth.mu), 1.0 / 3.0)) + oe.f = oe.f - macros.D2R * 4 elif lagrangePoint == 3: - oe.a = oe.a * (1-(7*moon.mu/(12*earth.mu))) + oe.a = oe.a * (1 - (7 * moon.mu / (12 * earth.mu))) oe.f = oe.f + np.pi elif lagrangePoint == 4: - oe.f = oe.f + np.pi/3 + oe.f = oe.f + np.pi / 3 else: - oe.f = oe.f - np.pi/3 + oe.f = oe.f - np.pi / 3 - oe.f = oe.f - macros.D2R*2 + oe.f = oe.f - macros.D2R * 2 rN, vN = orbitalMotion.elem2rv(earth.mu, oe) @@ -240,20 +251,25 @@ def run(lagrangePoint, nOrbits, timestep, showPlots=True): # Set simulation time n = np.sqrt(earth.mu / np.power(moon_a, 3)) - P = 2 * np.pi/n - simulationTime = macros.sec2nano(nOrbits*P) + P = 2 * np.pi / n + simulationTime = macros.sec2nano(nOrbits * P) # Setup data logging numDataPoints = 1000 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) # Setup spacecraft data recorder scDataRec = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, scDataRec) - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - # saveFile=__file__ - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # saveFile=__file__ + ) # Initialize simulation scSim.InitializeSimulation() @@ -276,10 +292,10 @@ def run(lagrangePoint, nOrbits, timestep, showPlots=True): fig = plt.figure(1, figsize=tuple(np.array((1.0, b / oe.a)) * 4.75), dpi=100) plt.axis(np.array([-oe.rApoap, oe.rPeriap, -b, b]) / 1000 * 1.25) ax = fig.gca() - ax.ticklabel_format(style='scientific', scilimits=[-5, 3]) + ax.ticklabel_format(style="scientific", scilimits=[-5, 3]) # Draw 'cartoon' Earth - ax.add_artist(plt.Circle((0, 0), 0.2e5, color='b')) + ax.add_artist(plt.Circle((0, 0), 0.2e5, color="b")) # Plot spacecraft orbit data rDataSpacecraft = [] @@ -287,9 +303,16 @@ def run(lagrangePoint, nOrbits, timestep, showPlots=True): for ii in range(len(posData)): oeDataSpacecraft = orbitalMotion.rv2elem(earth.mu, posData[ii], velData[ii]) rDataSpacecraft.append(oeDataSpacecraft.rmag) - fDataSpacecraft.append(oeDataSpacecraft.f + oeDataSpacecraft.omega - oe.omega) # Why the add/subtract of omegas? - plt.plot(rDataSpacecraft * np.cos(fDataSpacecraft) / 1000, rDataSpacecraft * np.sin(fDataSpacecraft) / 1000, - color='g', linewidth=3.0, label='Spacecraft') + fDataSpacecraft.append( + oeDataSpacecraft.f + oeDataSpacecraft.omega - oe.omega + ) # Why the add/subtract of omegas? + plt.plot( + rDataSpacecraft * np.cos(fDataSpacecraft) / 1000, + rDataSpacecraft * np.sin(fDataSpacecraft) / 1000, + color="g", + linewidth=3.0, + label="Spacecraft", + ) # Plot moon orbit data rDataMoon = [] @@ -300,17 +323,22 @@ def run(lagrangePoint, nOrbits, timestep, showPlots=True): usec = (simTime - sec) * 1e6 time = timeInit + timedelta(seconds=sec, microseconds=usec) timeString = time.strftime(spiceTimeStringFormat) - moonState = 1000 * spkRead(moonSpiceName, timeString, 'J2000', 'earth') + moonState = 1000 * spkRead(moonSpiceName, timeString, "J2000", "earth") moon_rN = moonState[0:3] moon_vN = moonState[3:6] oeDataMoon = orbitalMotion.rv2elem(earth.mu, moon_rN, moon_vN) rDataMoon.append(oeDataMoon.rmag) fDataMoon.append(oeDataMoon.f + oeDataMoon.omega - oe.omega) - plt.plot(rDataMoon * np.cos(fDataMoon) / 1000, rDataMoon * np.sin(fDataMoon) / 1000, color='0.5', linewidth=3.0, - label='Moon') + plt.plot( + rDataMoon * np.cos(fDataMoon) / 1000, + rDataMoon * np.sin(fDataMoon) / 1000, + color="0.5", + linewidth=3.0, + label="Moon", + ) - plt.xlabel('$i_e$ Coord. [km]') - plt.ylabel('$i_p$ Coord. [km]') + plt.xlabel("$i_e$ Coord. [km]") + plt.ylabel("$i_p$ Coord. [km]") plt.grid() plt.legend() pltName = fileName + "L" + str(lagrangePoint) + "Fig1" @@ -320,10 +348,10 @@ def run(lagrangePoint, nOrbits, timestep, showPlots=True): fig = plt.figure(2, figsize=tuple(np.array((1.0, b / oe.a)) * 4.75), dpi=100) plt.axis(np.array([-oe.rApoap, oe.rPeriap, -b, b]) / 1000 * 1.25) ax = fig.gca() - ax.ticklabel_format(style='scientific', scilimits=[-5, 3]) + ax.ticklabel_format(style="scientific", scilimits=[-5, 3]) # Draw 'cartoon' Earth - ax.add_artist(plt.Circle((0, 0), 0.2e5, color='b')) + ax.add_artist(plt.Circle((0, 0), 0.2e5, color="b")) # Plot spacecraft and Moon orbit data rDataSpacecraft = [] @@ -331,14 +359,13 @@ def run(lagrangePoint, nOrbits, timestep, showPlots=True): rDataMoon = [] fDataMoon = [] for ii in range(len(posData)): - # Get Moon f simTime = timeData[ii] * macros.NANO2SEC sec = int(simTime) usec = (simTime - sec) * 1e6 time = timeInit + timedelta(seconds=sec, microseconds=usec) timeString = time.strftime(spiceTimeStringFormat) - moonState = 1000 * spkRead(moonSpiceName, timeString, 'J2000', 'earth') + moonState = 1000 * spkRead(moonSpiceName, timeString, "J2000", "earth") moon_rN = moonState[0:3] moon_vN = moonState[3:6] oeDataMoon = orbitalMotion.rv2elem(earth.mu, moon_rN, moon_vN) @@ -347,19 +374,31 @@ def run(lagrangePoint, nOrbits, timestep, showPlots=True): # Get spacecraft data, with spacecraft f = oe data f - moon f oeDataSpacecraft = orbitalMotion.rv2elem(earth.mu, posData[ii], velData[ii]) rDataSpacecraft.append(oeDataSpacecraft.rmag) - fDataSpacecraft.append(oeDataSpacecraft.f - moon_f + oeDataSpacecraft.omega - oe.omega) + fDataSpacecraft.append( + oeDataSpacecraft.f - moon_f + oeDataSpacecraft.omega - oe.omega + ) # Get Moon data rDataMoon.append(oeDataMoon.rmag) fDataMoon.append(0) - plt.plot(rDataSpacecraft * np.cos(fDataSpacecraft) / 1000, rDataSpacecraft * np.sin(fDataSpacecraft) / 1000, - color='g', linewidth=3.0, label='Spacecraft') - plt.plot(rDataMoon * np.cos(fDataMoon) / 1000, rDataMoon * np.sin(fDataMoon) / 1000, color='0.5', linewidth=3.0, - label='Moon') + plt.plot( + rDataSpacecraft * np.cos(fDataSpacecraft) / 1000, + rDataSpacecraft * np.sin(fDataSpacecraft) / 1000, + color="g", + linewidth=3.0, + label="Spacecraft", + ) + plt.plot( + rDataMoon * np.cos(fDataMoon) / 1000, + rDataMoon * np.sin(fDataMoon) / 1000, + color="0.5", + linewidth=3.0, + label="Moon", + ) - plt.xlabel('Earth-Moon axis [km]') - plt.ylabel('Earth-Moon perpendicular axis [km]') + plt.xlabel("Earth-Moon axis [km]") + plt.ylabel("Earth-Moon perpendicular axis [km]") plt.grid() plt.legend() pltName = fileName + "L" + str(lagrangePoint) + "Fig2" @@ -372,18 +411,22 @@ def run(lagrangePoint, nOrbits, timestep, showPlots=True): # Unload spice libraries gravFactory.unloadSpiceKernels() - pyswice.unload_c(spiceObject.SPICEDataPath + 'de430.bsp') # solar system bodies - pyswice.unload_c(spiceObject.SPICEDataPath + 'naif0012.tls') # leap second file - pyswice.unload_c(spiceObject.SPICEDataPath + 'de-403-masses.tpc') # solar system masses - pyswice.unload_c(spiceObject.SPICEDataPath + 'pck00010.tpc') # generic Planetary Constants Kernel + pyswice.unload_c(spiceObject.SPICEDataPath + "de430.bsp") # solar system bodies + pyswice.unload_c(spiceObject.SPICEDataPath + "naif0012.tls") # leap second file + pyswice.unload_c( + spiceObject.SPICEDataPath + "de-403-masses.tpc" + ) # solar system masses + pyswice.unload_c( + spiceObject.SPICEDataPath + "pck00010.tpc" + ) # generic Planetary Constants Kernel return figureList if __name__ == "__main__": run( - 5, # Lagrange point - 1, # Number of Moon orbits - 300, # Timestep (seconds) - True # Show plots + 5, # Lagrange point + 1, # Number of Moon orbits + 300, # Timestep (seconds) + True, # Show plots ) diff --git a/examples/scenarioLambertSolver.py b/examples/scenarioLambertSolver.py index 23cf15b7c6..eb5d5af99f 100644 --- a/examples/scenarioLambertSolver.py +++ b/examples/scenarioLambertSolver.py @@ -103,15 +103,21 @@ import matplotlib.pyplot as plt import numpy as np from Basilisk import __path__ -from Basilisk.fswAlgorithms import (lambertPlanner, lambertSolver, lambertValidator) +from Basilisk.fswAlgorithms import lambertPlanner, lambertSolver, lambertValidator from Basilisk.simulation import simpleNav from Basilisk.simulation import spacecraft -from Basilisk.utilities import (SimulationBaseClass, macros, simIncludeGravBody, vizSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + simIncludeGravBody, + vizSupport, +) from Basilisk.utilities import orbitalMotion from Basilisk.utilities import unitTestSupport try: from Basilisk.simulation import vizInterface + vizFound = True except ImportError: vizFound = False @@ -149,8 +155,8 @@ def run(show_plots): fswProcess = scSim.CreateNewProcess(fswProcessName) # create the dynamics task and specify the simulation time step information - simStep = 10. - fswStep = 30. + simStep = 10.0 + fswStep = 30.0 simTimeStep = macros.sec2nano(simStep) fswTimeStep = macros.sec2nano(fswStep) dynProcess.addTask(scSim.CreateNewTask(dynTaskName, simTimeStep)) @@ -169,53 +175,85 @@ def run(show_plots): scObject.ModelTag = "bskSat" oeSC = orbitalMotion.ClassicElements() - oeSC.a = 10000. * 1e3 + oeSC.a = 10000.0 * 1e3 oeSC.e = 0.001 - oeSC.i = 5. * macros.D2R - oeSC.Omega = 10. * macros.D2R - oeSC.omega = 10. * macros.D2R - oeSC.f = 10. * macros.D2R + oeSC.i = 5.0 * macros.D2R + oeSC.Omega = 10.0 * macros.D2R + oeSC.omega = 10.0 * macros.D2R + oeSC.f = 10.0 * macros.D2R # spacecraft state at initial time r_BO_N, v_BO_N = orbitalMotion.elem2rv(mu, oeSC) # Set the truth ICs for the spacecraft position and velocity scObject.hub.r_CN_NInit = r_BO_N # m - r_BN_N scObject.hub.v_CN_NInit = v_BO_N # m/s - v_BN_N - scObject.hub.mHub = 330. # kg - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.hub.mHub = 330.0 # kg + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # orbit transfer parameters - tau = 2*np.pi*np.sqrt(oeSC.a**3/mu) - tm = round(tau/4./simStep)*simStep # maneuver time - tf = round(tau/2./simStep)*simStep # final time - r_TN_N = np.array([-(rEarth + 200 * 1e3), 0., 0.]) # targeted position + tau = 2 * np.pi * np.sqrt(oeSC.a**3 / mu) + tm = round(tau / 4.0 / simStep) * simStep # maneuver time + tf = round(tau / 2.0 / simStep) * simStep # final time + r_TN_N = np.array([-(rEarth + 200 * 1e3), 0.0, 0.0]) # targeted position # Lambert solution validation parameters - maxDistanceTarget = 500. + maxDistanceTarget = 500.0 minOrbitRadius = rEarth # Set up simpleNav for s/c "measurements" simpleNavMeas = simpleNav.SimpleNav() - simpleNavMeas.ModelTag = 'SimpleNav' + simpleNavMeas.ModelTag = "SimpleNav" simpleNavMeas.scStateInMsg.subscribeTo(scObject.scStateOutMsg) - pos_sigma_sc = 1. + pos_sigma_sc = 1.0 vel_sigma_sc = 0.01 att_sigma_sc = 0.0 rate_sigma_sc = 0.0 sun_sigma_sc = 0.0 dv_sigma_sc = 0.0 - p_matrix_sc = np.diag([pos_sigma_sc, pos_sigma_sc, pos_sigma_sc, - vel_sigma_sc, vel_sigma_sc, vel_sigma_sc, - att_sigma_sc, att_sigma_sc, att_sigma_sc, - rate_sigma_sc, rate_sigma_sc, rate_sigma_sc, - sun_sigma_sc, sun_sigma_sc, sun_sigma_sc, - dv_sigma_sc, dv_sigma_sc, dv_sigma_sc]) - walk_bounds_sc = [[10.], [10.], [10.], - [1], [1], [1], - [0.], [0.], [0.], - [0.], [0.], [0.], - [0.], [0.], [0.], - [0.], [0.], [0.]] + p_matrix_sc = np.diag( + [ + pos_sigma_sc, + pos_sigma_sc, + pos_sigma_sc, + vel_sigma_sc, + vel_sigma_sc, + vel_sigma_sc, + att_sigma_sc, + att_sigma_sc, + att_sigma_sc, + rate_sigma_sc, + rate_sigma_sc, + rate_sigma_sc, + sun_sigma_sc, + sun_sigma_sc, + sun_sigma_sc, + dv_sigma_sc, + dv_sigma_sc, + dv_sigma_sc, + ] + ) + walk_bounds_sc = [ + [10.0], + [10.0], + [10.0], + [1], + [1], + [1], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + ] simpleNavMeas.PMatrix = p_matrix_sc simpleNavMeas.walkBounds = walk_bounds_sc @@ -241,10 +279,20 @@ def run(show_plots): lamValidator.setManeuverTime(tm) lamValidator.setMaxDistanceTarget(maxDistanceTarget) lamValidator.setMinOrbitRadius(minOrbitRadius) - lamValidator.setUncertaintyStates(np.diag([pos_sigma_sc, pos_sigma_sc, pos_sigma_sc, - vel_sigma_sc, vel_sigma_sc, vel_sigma_sc])) + lamValidator.setUncertaintyStates( + np.diag( + [ + pos_sigma_sc, + pos_sigma_sc, + pos_sigma_sc, + vel_sigma_sc, + vel_sigma_sc, + vel_sigma_sc, + ] + ) + ) lamValidator.setUncertaintyDV(0.1) - lamValidator.setDvConvergenceTolerance(1.) + lamValidator.setDvConvergenceTolerance(1.0) lamValidator.navTransInMsg.subscribeTo(simpleNavMeas.transOutMsg) lamValidator.lambertProblemInMsg.subscribeTo(lamPlanner.lambertProblemOutMsg) lamValidator.lambertPerformanceInMsg.subscribeTo(lamSolver.lambertPerformanceOutMsg) @@ -281,9 +329,12 @@ def run(show_plots): # be stored in a binary file inside the _VizFiles sub-folder with the scenario folder. This file can be read in by # Vizard and played back after running the BSK simulation. if vizFound: - viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, scObject, - # saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + dynTaskName, + scObject, + # saveFile=fileName + ) viz.settings.showSpacecraftLabels = 1 # initialize Simulation @@ -346,15 +397,21 @@ def run(show_plots): # FSW stops after maneuver # to plot for entire time span and automatically adjust axes, data is extended for plotting - plot_rm(np.append(timeFSW, time[-1]), np.append(r1_N, [np.nan * np.ones(3)], axis=0)) + plot_rm( + np.append(timeFSW, time[-1]), np.append(r1_N, [np.nan * np.ones(3)], axis=0) + ) pltName = fileName + "3" figureList[pltName] = plt.figure(3) - plot_vm(np.append(timeFSW, time[-1]), np.append(v1_N, [np.nan * np.ones(3)], axis=0)) + plot_vm( + np.append(timeFSW, time[-1]), np.append(v1_N, [np.nan * np.ones(3)], axis=0) + ) pltName = fileName + "4" figureList[pltName] = plt.figure(4) - plot_dV(np.append(timeFSW, time[-1]), np.append(dv_N, [np.nan * np.ones(3)], axis=0)) + plot_dV( + np.append(timeFSW, time[-1]), np.append(dv_N, [np.nan * np.ones(3)], axis=0) + ) pltName = fileName + "5" figureList[pltName] = plt.figure(5) @@ -370,7 +427,9 @@ def run(show_plots): pltName = fileName + "8" figureList[pltName] = plt.figure(8) - plot_failedDvConvergence(np.append(timeFSW, time[-1]), np.append(failedDvSolutionConvergence, [np.nan])) + plot_failedDvConvergence( + np.append(timeFSW, time[-1]), np.append(failedDvSolutionConvergence, [np.nan]) + ) pltName = fileName + "9" figureList[pltName] = plt.figure(9) @@ -386,53 +445,53 @@ def run(show_plots): # Plotting functions def plot_position(time, r_BN_N_truth, r_BN_N_meas, r_TN_N): """Plot the position result.""" - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time, r_BN_N_meas[:, 0], 'k*', label='measurement', markersize=2) - ax[1].plot(time, r_BN_N_meas[:, 1], 'k*', markersize=2) - ax[2].plot(time, r_BN_N_meas[:, 2], 'k*', markersize=2) + ax[0].plot(time, r_BN_N_meas[:, 0], "k*", label="measurement", markersize=2) + ax[1].plot(time, r_BN_N_meas[:, 1], "k*", markersize=2) + ax[2].plot(time, r_BN_N_meas[:, 2], "k*", markersize=2) - ax[0].plot(time, r_BN_N_truth[:, 0], label='truth') + ax[0].plot(time, r_BN_N_truth[:, 0], label="truth") ax[1].plot(time, r_BN_N_truth[:, 1]) ax[2].plot(time, r_BN_N_truth[:, 2]) - ax[0].plot(time[-1], r_TN_N[0], 'rx', label='target') - ax[1].plot(time[-1], r_TN_N[1], 'rx') - ax[2].plot(time[-1], r_TN_N[2], 'rx') + ax[0].plot(time[-1], r_TN_N[0], "rx", label="target") + ax[1].plot(time[-1], r_TN_N[1], "rx") + ax[2].plot(time[-1], r_TN_N[2], "rx") - plt.xlabel('Time [sec]') - plt.title('Spacecraft Position') + plt.xlabel("Time [sec]") + plt.title("Spacecraft Position") - ax[0].set_ylabel('${}^Nr_{BN_1}$ [m]') - ax[1].set_ylabel('${}^Nr_{BN_2}$ [m]') - ax[2].set_ylabel('${}^Nr_{BN_3}$ [m]') + ax[0].set_ylabel("${}^Nr_{BN_1}$ [m]") + ax[1].set_ylabel("${}^Nr_{BN_2}$ [m]") + ax[2].set_ylabel("${}^Nr_{BN_3}$ [m]") - ax[0].legend(loc='upper right') + ax[0].legend(loc="upper right") def plot_velocity(time, v_BN_N_truth, v_BN_N_meas): """Plot the velocity result.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time, v_BN_N_meas[:, 0], 'k*', label='measurement', markersize=2) - ax[1].plot(time, v_BN_N_meas[:, 1], 'k*', markersize=2) - ax[2].plot(time, v_BN_N_meas[:, 2], 'k*', markersize=2) + ax[0].plot(time, v_BN_N_meas[:, 0], "k*", label="measurement", markersize=2) + ax[1].plot(time, v_BN_N_meas[:, 1], "k*", markersize=2) + ax[2].plot(time, v_BN_N_meas[:, 2], "k*", markersize=2) - ax[0].plot(time, v_BN_N_truth[:, 0], label='truth') + ax[0].plot(time, v_BN_N_truth[:, 0], label="truth") ax[1].plot(time, v_BN_N_truth[:, 1]) ax[2].plot(time, v_BN_N_truth[:, 2]) - plt.xlabel('Time [sec]') - plt.title('Spacecraft Velocity') + plt.xlabel("Time [sec]") + plt.title("Spacecraft Velocity") - ax[0].set_ylabel('${}^Nv_{BN_1}$ [m/s]') - ax[1].set_ylabel('${}^Nv_{BN_2}$ [m/s]') - ax[2].set_ylabel('${}^Nv_{BN_3}$ [m/s]') + ax[0].set_ylabel("${}^Nv_{BN_1}$ [m/s]") + ax[1].set_ylabel("${}^Nv_{BN_2}$ [m/s]") + ax[2].set_ylabel("${}^Nv_{BN_3}$ [m/s]") ax[0].legend() @@ -440,20 +499,20 @@ def plot_velocity(time, v_BN_N_truth, v_BN_N_meas): def plot_rm(time, rm_BN_N): """Plot the expected position at maneuver time.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time, rm_BN_N[:, 0], 'k*', label='measurement', markersize=2) - ax[1].plot(time, rm_BN_N[:, 1], 'k*', markersize=2) - ax[2].plot(time, rm_BN_N[:, 2], 'k*', markersize=2) + ax[0].plot(time, rm_BN_N[:, 0], "k*", label="measurement", markersize=2) + ax[1].plot(time, rm_BN_N[:, 1], "k*", markersize=2) + ax[2].plot(time, rm_BN_N[:, 2], "k*", markersize=2) - plt.xlabel('Time [sec]') - plt.title('Planner: Expected Spacecraft Position at Maneuver Time') + plt.xlabel("Time [sec]") + plt.title("Planner: Expected Spacecraft Position at Maneuver Time") - ax[0].set_ylabel('${}^Nr_{BN,m_1}$ [m]') - ax[1].set_ylabel('${}^Nr_{BN,m_2}$ [m]') - ax[2].set_ylabel('${}^Nr_{BN,m_3}$ [m]') + ax[0].set_ylabel("${}^Nr_{BN,m_1}$ [m]") + ax[1].set_ylabel("${}^Nr_{BN,m_2}$ [m]") + ax[2].set_ylabel("${}^Nr_{BN,m_3}$ [m]") ax[0].set_xlim([time[0], time[-1]]) ax[1].set_xlim([time[0], time[-1]]) @@ -463,20 +522,20 @@ def plot_rm(time, rm_BN_N): def plot_vm(time, vm_BN_N): """Plot the expected velocity at maneuver time.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time, vm_BN_N[:, 0], 'k*', label='measurement', markersize=2) - ax[1].plot(time, vm_BN_N[:, 1], 'k*', markersize=2) - ax[2].plot(time, vm_BN_N[:, 2], 'k*', markersize=2) + ax[0].plot(time, vm_BN_N[:, 0], "k*", label="measurement", markersize=2) + ax[1].plot(time, vm_BN_N[:, 1], "k*", markersize=2) + ax[2].plot(time, vm_BN_N[:, 2], "k*", markersize=2) - plt.xlabel('Time [sec]') - plt.title('Planner: Expected Spacecraft Velocity at Maneuver Time') + plt.xlabel("Time [sec]") + plt.title("Planner: Expected Spacecraft Velocity at Maneuver Time") - ax[0].set_ylabel('${}^Nv_{BN,m_1}$ [m/s]') - ax[1].set_ylabel('${}^Nv_{BN,m_2}$ [m/s]') - ax[2].set_ylabel('${}^Nv_{BN,m_3}$ [m/s]') + ax[0].set_ylabel("${}^Nv_{BN,m_1}$ [m/s]") + ax[1].set_ylabel("${}^Nv_{BN,m_2}$ [m/s]") + ax[2].set_ylabel("${}^Nv_{BN,m_3}$ [m/s]") ax[0].set_xlim([time[0], time[-1]]) ax[1].set_xlim([time[0], time[-1]]) @@ -486,20 +545,20 @@ def plot_vm(time, vm_BN_N): def plot_dV(time, dv_N): """Plot the required Delta-V for the maneuver.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time, dv_N[:, 0], 'k*', markersize=2) - ax[1].plot(time, dv_N[:, 1], 'k*', markersize=2) - ax[2].plot(time, dv_N[:, 2], 'k*', markersize=2) + ax[0].plot(time, dv_N[:, 0], "k*", markersize=2) + ax[1].plot(time, dv_N[:, 1], "k*", markersize=2) + ax[2].plot(time, dv_N[:, 2], "k*", markersize=2) - plt.xlabel('Time [sec]') - plt.title('Delta-V') + plt.xlabel("Time [sec]") + plt.title("Delta-V") - ax[0].set_ylabel('${}^N\\Delta v_{1}$ [m/s]') - ax[1].set_ylabel('${}^N\\Delta v_{2}$ [m/s]') - ax[2].set_ylabel('${}^N\\Delta v_{3}$ [m/s]') + ax[0].set_ylabel("${}^N\\Delta v_{1}$ [m/s]") + ax[1].set_ylabel("${}^N\\Delta v_{2}$ [m/s]") + ax[2].set_ylabel("${}^N\\Delta v_{3}$ [m/s]") ax[0].set_xlim([time[0], time[-1]]) ax[1].set_xlim([time[0], time[-1]]) @@ -514,9 +573,9 @@ def plot_x(time, x): plt.plot(time, x) - plt.xlabel('Time [sec]') - plt.ylabel('x [-]') - plt.title('Lambert Solver Performance: x Solution') + plt.xlabel("Time [sec]") + plt.ylabel("x [-]") + plt.title("Lambert Solver Performance: x Solution") plt.xlim([time[0], time[-1]]) @@ -529,9 +588,9 @@ def plot_numIter(time, numIter): plt.plot(time, numIter) - plt.xlabel('Time [sec]') - plt.ylabel('number of iterations [-]') - plt.title('Lambert Solver Performance: Number of Iterations') + plt.xlabel("Time [sec]") + plt.ylabel("number of iterations [-]") + plt.title("Lambert Solver Performance: Number of Iterations") plt.xlim([time[0], time[-1]]) @@ -544,9 +603,9 @@ def plot_errX(time, errX): plt.plot(time, errX) - plt.xlabel('Time [sec]') - plt.ylabel('x error') - plt.title('Lambert Solver Performance: Error in x') + plt.xlabel("Time [sec]") + plt.ylabel("x error") + plt.title("Lambert Solver Performance: Error in x") plt.xlim([time[0], time[-1]]) @@ -559,9 +618,9 @@ def plot_failedDvConvergence(time, failedDvSolutionConvergence): plt.plot(time, failedDvSolutionConvergence) - plt.xlabel('Time [sec]') - plt.ylabel('Failed Dv Convergence Flag') - plt.title('Lambert Validator: Failed Delta-V convergence (true if 1)') + plt.xlabel("Time [sec]") + plt.ylabel("Failed Dv Convergence Flag") + plt.title("Lambert Validator: Failed Delta-V convergence (true if 1)") plt.xlim([time[0], time[-1]]) diff --git a/examples/scenarioMagneticFieldCenteredDipole.py b/examples/scenarioMagneticFieldCenteredDipole.py index 5a50d8ce04..7dba23c8ef 100644 --- a/examples/scenarioMagneticFieldCenteredDipole.py +++ b/examples/scenarioMagneticFieldCenteredDipole.py @@ -151,6 +151,7 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ @@ -159,14 +160,21 @@ # import simulation related support from Basilisk.simulation import spacecraft from Basilisk.simulation import magneticFieldCenteredDipole + # general support file with common unit test functions # import general simulation support files -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, +) from Basilisk.utilities import simSetPlanetEnvironment -#attempt to import vizard +# attempt to import vizard from Basilisk.utilities import vizSupport + fileName = os.path.basename(os.path.splitext(__file__)[0]) @@ -181,7 +189,6 @@ def run(show_plots, orbitCase, planetCase): """ - # Create simulation variable names simTaskName = "simTask" simProcessName = "simProcess" @@ -195,7 +202,7 @@ def run(show_plots, orbitCase, planetCase): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -211,33 +218,36 @@ def run(show_plots, orbitCase, planetCase): # setup Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() - if planetCase == 'Jupiter': + if planetCase == "Jupiter": planet = gravFactory.createJupiter() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body else: # Earth planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu req = planet.radEquator # attach gravity model to spacecraft gravFactory.addBodiesTo(scObject) - # create the magnetic field - magModule = magneticFieldCenteredDipole.MagneticFieldCenteredDipole() # default is Earth centered dipole module + magModule = ( + magneticFieldCenteredDipole.MagneticFieldCenteredDipole() + ) # default is Earth centered dipole module magModule.ModelTag = "CenteredDipole" - magModule.addSpacecraftToModel(scObject.scStateOutMsg) # this command can be repeated if multiple + magModule.addSpacecraftToModel( + scObject.scStateOutMsg + ) # this command can be repeated if multiple - if planetCase == 'Jupiter': + if planetCase == "Jupiter": # The following command is a support function that sets up the centered dipole parameters. # These parameters can also be setup manually - simSetPlanetEnvironment.centeredDipoleMagField(magModule, 'jupiter') + simSetPlanetEnvironment.centeredDipoleMagField(magModule, "jupiter") else: - simSetPlanetEnvironment.centeredDipoleMagField(magModule, 'earth') + simSetPlanetEnvironment.centeredDipoleMagField(magModule, "earth") scSim.AddModelToTask(simTaskName, magModule) - if planetCase == 'Earth' and orbitCase == 'elliptical': + if planetCase == "Earth" and orbitCase == "elliptical": # Note that more then one magnetic field can be attached to a planet. # In the elliptic Earth orbit scenario # a second magnetic field module `magModule2` is created with a @@ -249,11 +259,11 @@ def run(show_plots, orbitCase, planetCase): magModule2.addSpacecraftToModel(scObject.scStateOutMsg) # set the 2nd magnetic field through custom dipole settings magModule2.g10 = -30926.00 / 1e9 * 0.5 # Tesla - magModule2.g11 = -2318.00 / 1e9 * 0.5 # Tesla - magModule2.h11 = 5817.00 / 1e9 * 0.5 # Tesla + magModule2.g11 = -2318.00 / 1e9 * 0.5 # Tesla + magModule2.h11 = 5817.00 / 1e9 * 0.5 # Tesla magModule2.planetRadius = 6371.2 * 1000 # meters # set the reach variables such that the fields - magModule2.envMaxReach = req*1.3 + magModule2.envMaxReach = req * 1.3 magModule.envMinReach = magModule2.envMaxReach scSim.AddModelToTask(simTaskName, magModule2) @@ -262,12 +272,12 @@ def run(show_plots, orbitCase, planetCase): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rPeriapses = req*1.1 # meters - if orbitCase == 'circular': + rPeriapses = req * 1.1 # meters + if orbitCase == "circular": oe.a = rPeriapses oe.e = 0.0000 - elif orbitCase == 'elliptical': - rApoapses = req*3.5 + elif orbitCase == "elliptical": + rApoapses = req * 3.5 oe.a = (rPeriapses + rApoapses) / 2.0 oe.e = 1.0 - rPeriapses / oe.a else: @@ -278,7 +288,9 @@ def run(show_plots, orbitCase, planetCase): oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements # with circular or equatorial orbit, some angles are arbitrary # @@ -289,26 +301,31 @@ def run(show_plots, orbitCase, planetCase): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n - simulationTime = macros.sec2nano(1. * P) + P = 2.0 * np.pi / n + simulationTime = macros.sec2nano(1.0 * P) # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) magLog = magModule.envOutMsgs[0].recorder(samplingTime) dataLog = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, magLog) scSim.AddModelToTask(simTaskName, dataLog) - if planetCase == 'Earth' and orbitCase == 'elliptical': + if planetCase == "Earth" and orbitCase == "elliptical": mag2Log = magModule2.envOutMsgs[0].recorder(samplingTime) scSim.AddModelToTask(simTaskName, mag2Log) # if this scenario is to interface with the BSK Viz, uncomment the following line - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) scSim.InitializeSimulation() @@ -323,7 +340,7 @@ def run(show_plots, orbitCase, planetCase): # magData = magLog.magField_N posData = dataLog.r_BN_N - if planetCase == 'Earth' and orbitCase == 'elliptical': + if planetCase == "Earth" and orbitCase == "elliptical": magData2 = mag2Log.magField_N np.set_printoptions(precision=16) @@ -337,16 +354,21 @@ def run(show_plots, orbitCase, planetCase): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) timeAxis = dataLog.times() * macros.NANO2SEC for idx in range(3): - plt.plot(timeAxis / P, posData[:, idx] / 1000., - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Inertial Position [km]') + plt.plot( + timeAxis / P, + posData[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Inertial Position [km]") figureList = {} pltName = fileName + "1" + orbitCase + planetCase figureList[pltName] = plt.figure(1) @@ -354,24 +376,32 @@ def run(show_plots, orbitCase, planetCase): plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) for idx in range(3): - plt.plot(timeAxis / P, magData[:, idx] *1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$B\_N_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Magnetic Field [nT]') - if planetCase == 'Earth' and orbitCase == 'elliptical': + plt.plot( + timeAxis / P, + magData[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$B\_N_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Magnetic Field [nT]") + if planetCase == "Earth" and orbitCase == "elliptical": for idx in range(3): - plt.plot(timeAxis / P, magData2[:, idx] * 1e9, '--', - color=unitTestSupport.getLineColor(idx, 3), - label=r'$B\_N_{' + str(idx) + '}$') + plt.plot( + timeAxis / P, + magData2[:, idx] * 1e9, + "--", + color=unitTestSupport.getLineColor(idx, 3), + label=r"$B\_N_{" + str(idx) + "}$", + ) pltName = fileName + "2" + orbitCase + planetCase figureList[pltName] = plt.figure(2) - if show_plots: plt.show() @@ -381,14 +411,13 @@ def run(show_plots, orbitCase, planetCase): 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 - 'circular', # orbit Case (circular, elliptical) - 'Earth' # planetCase (Earth, Jupiter) + True, # show_plots + "circular", # orbit Case (circular, elliptical) + "Earth", # planetCase (Earth, Jupiter) ) diff --git a/examples/scenarioMagneticFieldWMM.py b/examples/scenarioMagneticFieldWMM.py index 09a8309180..16ad8c6d7c 100644 --- a/examples/scenarioMagneticFieldWMM.py +++ b/examples/scenarioMagneticFieldWMM.py @@ -127,6 +127,7 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ @@ -138,12 +139,18 @@ # import simulation related support from Basilisk.simulation import spacecraft from Basilisk.simulation import magneticFieldWMM + # general support file with common unit test functions # import general simulation support files -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport) - -#attempt to import vizard +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, +) + +# attempt to import vizard from Basilisk.utilities import vizSupport @@ -157,7 +164,6 @@ def run(show_plots, orbitCase): """ - # Create simulation variable names simTaskName = "simTask" simProcessName = "simProcess" @@ -171,7 +177,7 @@ def run(show_plots, orbitCase): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(60.) + simulationTimeStep = macros.sec2nano(60.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -188,7 +194,7 @@ def run(show_plots, orbitCase): # setup Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu req = planet.radEquator @@ -198,18 +204,22 @@ def run(show_plots, orbitCase): # create the magnetic field magModule = magneticFieldWMM.MagneticFieldWMM() magModule.ModelTag = "WMM" - magModule.dataPath = bskPath + '/supportData/MagneticField/' + magModule.dataPath = bskPath + "/supportData/MagneticField/" # set the minReach and maxReach values if on an elliptic orbit - if orbitCase == 'elliptical': - magModule.envMinReach = 10000*1000. - magModule.envMaxReach = 20000*1000. + if orbitCase == "elliptical": + magModule.envMinReach = 10000 * 1000.0 + magModule.envMaxReach = 20000 * 1000.0 # set epoch date/time message - epochMsg = unitTestSupport.timeStringToGregorianUTCMsg('2019 June 27, 10:23:0.0 (UTC)') + epochMsg = unitTestSupport.timeStringToGregorianUTCMsg( + "2019 June 27, 10:23:0.0 (UTC)" + ) # add spacecraft to the magnetic field module so it can read the sc position messages - magModule.addSpacecraftToModel(scObject.scStateOutMsg) # this command can be repeated if multiple + magModule.addSpacecraftToModel( + scObject.scStateOutMsg + ) # this command can be repeated if multiple # add the magnetic field module to the simulation task stack scSim.AddModelToTask(simTaskName, magModule) @@ -219,12 +229,12 @@ def run(show_plots, orbitCase): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rPeriapses = req*1.1 # meters - if orbitCase == 'circular': + rPeriapses = req * 1.1 # meters + if orbitCase == "circular": oe.a = rPeriapses oe.e = 0.0000 - elif orbitCase == 'elliptical': - rApoapses = req*3.5 + elif orbitCase == "elliptical": + rApoapses = req * 3.5 oe.a = (rPeriapses + rApoapses) / 2.0 oe.e = 1.0 - rPeriapses / oe.a else: @@ -247,8 +257,8 @@ def run(show_plots, orbitCase): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n - simulationTime = macros.sec2nano(1. * P) + P = 2.0 * np.pi / n + simulationTime = macros.sec2nano(1.0 * P) # connect messages magModule.epochInMsg.subscribeTo(epochMsg) @@ -257,7 +267,9 @@ def run(show_plots, orbitCase): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) magLog = magModule.envOutMsgs[0].recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) @@ -265,9 +277,12 @@ def run(show_plots, orbitCase): # if this scenario is to interface with the BSK Viz, uncomment the following line if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - # saveFile=fileName, - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # saveFile=fileName, + ) viz.epochInMsg.subscribeTo(epochMsg) viz.settings.show24hrClock = 1 @@ -299,21 +314,32 @@ def run(show_plots, orbitCase): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) rData = [] for idx in range(0, len(posData)): rMag = np.linalg.norm(posData[idx]) - rData.append(rMag / 1000.) - plt.plot(timeAxis / P, rData, color='#aa0000') - if orbitCase == 'elliptical': - plt.plot(timeAxis / P, [magModule.envMinReach/1000.]*len(rData), color='#007700', dashes=[5, 5, 5, 5]) - plt.plot(timeAxis / P, [magModule.envMaxReach / 1000.] * len(rData), - color='#007700', dashes=[5, 5, 5, 5]) - - plt.xlabel('Time [orbits]') - plt.ylabel('Radius [km]') - plt.ylim(min(rData)*0.9, max(rData)*1.1) + rData.append(rMag / 1000.0) + plt.plot(timeAxis / P, rData, color="#aa0000") + if orbitCase == "elliptical": + plt.plot( + timeAxis / P, + [magModule.envMinReach / 1000.0] * len(rData), + color="#007700", + dashes=[5, 5, 5, 5], + ) + plt.plot( + timeAxis / P, + [magModule.envMaxReach / 1000.0] * len(rData), + color="#007700", + dashes=[5, 5, 5, 5], + ) + + plt.xlabel("Time [orbits]") + plt.ylabel("Radius [km]") + plt.ylim(min(rData) * 0.9, max(rData) * 1.1) figureList = {} pltName = fileName + "1" + orbitCase figureList[pltName] = plt.figure(1) @@ -321,19 +347,23 @@ def run(show_plots, orbitCase): plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) for idx in range(3): - plt.plot(timeAxis / P, magData[:, idx] *1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$B\_N_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Magnetic Field [nT]') + plt.plot( + timeAxis / P, + magData[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$B\_N_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Magnetic Field [nT]") pltName = fileName + "2" + orbitCase figureList[pltName] = plt.figure(2) - if show_plots: plt.show() @@ -343,13 +373,12 @@ def run(show_plots, orbitCase): 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 - 'elliptical', # orbit Case (circular, elliptical) + True, # show_plots + "elliptical", # orbit Case (circular, elliptical) ) diff --git a/examples/scenarioMomentumDumping.py b/examples/scenarioMomentumDumping.py index cb3a14c5a4..44fbfa7677 100644 --- a/examples/scenarioMomentumDumping.py +++ b/examples/scenarioMomentumDumping.py @@ -87,20 +87,37 @@ import numpy as np from Basilisk import __path__ from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, inertial3D, rwMotorTorque, - thrMomentumManagement, thrForceMapping, thrMomentumDumping) -from Basilisk.simulation import (reactionWheelStateEffector, thrusterDynamicEffector, simpleNav, spacecraft) -from Basilisk.utilities import (SimulationBaseClass, macros, - orbitalMotion, simIncludeGravBody, - simIncludeRW, simIncludeThruster, unitTestSupport, vizSupport) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + inertial3D, + rwMotorTorque, + thrMomentumManagement, + thrForceMapping, + thrMomentumDumping, +) +from Basilisk.simulation import ( + reactionWheelStateEffector, + thrusterDynamicEffector, + simpleNav, + spacecraft, +) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + simIncludeRW, + simIncludeThruster, + unitTestSupport, + vizSupport, +) bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) - def run(show_plots): - # Create simulation variable names fswTask = "fswTask" dynTask = "dynTask" @@ -136,8 +153,8 @@ def run(show_plots): gravFactory = simIncludeGravBody.gravBodyFactory() # Next a series of gravitational bodies are included - gravBodies = gravFactory.createBodies('earth', 'sun') - planet = gravBodies['earth'] + gravBodies = gravFactory.createBodies("earth", "sun") + planet = gravBodies["earth"] planet.isCentralBody = True mu = planet.mu @@ -151,7 +168,7 @@ def run(show_plots): spiceObject = gravFactory.createSpiceInterface(time=timeInitString, epochInMsg=True) # Earth is gravity center - spiceObject.zeroBase = 'Earth' + spiceObject.zeroBase = "Earth" # The SPICE object is added to the simulation task list. scSim.AddModelToTask(fswTask, spiceObject, 2) @@ -161,7 +178,7 @@ def run(show_plots): # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - oe.a = 7000. * 1000 # meters + oe.a = 7000.0 * 1000 # meters oe.e = 0.0001 oe.i = 33.3 * macros.D2R oe.Omega = 148.2 * macros.D2R @@ -170,17 +187,23 @@ def run(show_plots): rN, vN = orbitalMotion.elem2rv(mu, oe) # To set the spacecraft initial conditions, the following initial position and velocity variables are set: - scObject.hub.r_CN_NInit = rN # m - r_BN_N - scObject.hub.v_CN_NInit = vN # m/s - v_BN_N - scObject.hub.sigma_BNInit = [0, 0., 0.] # MRP set to customize initial inertial attitude - scObject.hub.omega_BN_BInit = [[0.], [0.], [0.]] # rad/s - omega_CN_B + scObject.hub.r_CN_NInit = rN # m - r_BN_N + scObject.hub.v_CN_NInit = vN # m/s - v_BN_N + scObject.hub.sigma_BNInit = [ + 0, + 0.0, + 0.0, + ] # MRP set to customize initial inertial attitude + scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_CN_B # define the simulation inertia - I = [1700, 0., 0., - 0., 1700, 0., - 0., 0., 1800] + I = [1700, 0.0, 0.0, 0.0, 1700, 0.0, 0.0, 0.0, 1800] scObject.hub.mHub = 2500 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [1.28]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [1.28], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # @@ -193,15 +216,35 @@ def run(show_plots): varRWModel = messaging.BalancedWheels # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments - c = 2**(-0.5) - RW1 = rwFactory.create('Honeywell_HR16', [c, 0, c], maxMomentum=100., Omega=4000. # RPM - , RWModel=varRWModel) - RW2 = rwFactory.create('Honeywell_HR16', [0, c, c], maxMomentum=100., Omega=2000. # RPM - , RWModel=varRWModel) - RW3 = rwFactory.create('Honeywell_HR16', [-c, 0, c], maxMomentum=100., Omega=3500. # RPM - , RWModel=varRWModel) - RW4 = rwFactory.create('Honeywell_HR16', [0, -c, c], maxMomentum=100., Omega=0. # RPM - , RWModel=varRWModel) + c = 2 ** (-0.5) + RW1 = rwFactory.create( + "Honeywell_HR16", + [c, 0, c], + maxMomentum=100.0, + Omega=4000.0, # RPM + RWModel=varRWModel, + ) + RW2 = rwFactory.create( + "Honeywell_HR16", + [0, c, c], + maxMomentum=100.0, + Omega=2000.0, # RPM + RWModel=varRWModel, + ) + RW3 = rwFactory.create( + "Honeywell_HR16", + [-c, 0, c], + maxMomentum=100.0, + Omega=3500.0, # RPM + RWModel=varRWModel, + ) + RW4 = rwFactory.create( + "Honeywell_HR16", + [0, -c, c], + maxMomentum=100.0, + Omega=0.0, # RPM + RWModel=varRWModel, + ) numRW = rwFactory.getNumOfDevices() RW = [RW1, RW2, RW3, RW4] @@ -221,24 +264,24 @@ def run(show_plots): a = 1 b = 1.28 location = [ - [-a, -a, b], - [ a, -a, -b], - [ a, -a, b], - [ a, a, -b], - [ a, a, b], - [-a, a, -b], - [-a, a, b], - [-a, -a, -b] + [-a, -a, b], + [a, -a, -b], + [a, -a, b], + [a, a, -b], + [a, a, b], + [-a, a, -b], + [-a, a, b], + [-a, -a, -b], ] direction = [ - [ 1, 0, 0], - [-1, 0, 0], - [ 0, 1, 0], - [ 0, -1, 0], - [-1, 0, 0], - [ 1, 0, 0], - [ 0, -1, 0], - [ 0, 1, 0] + [1, 0, 0], + [-1, 0, 0], + [0, 1, 0], + [0, -1, 0], + [-1, 0, 0], + [1, 0, 0], + [0, -1, 0], + [0, 1, 0], ] # create the set of thruster in the dynamics task @@ -250,7 +293,7 @@ def run(show_plots): # create the thruster devices by specifying the thruster type and its location and direction for pos_B, dir_B in zip(location, direction): - thFactory.create('MOOG_Monarc_5', pos_B, dir_B, MaxThrust=5.0) + thFactory.create("MOOG_Monarc_5", pos_B, dir_B, MaxThrust=5.0) # get number of thruster devices numTh = thFactory.getNumOfDevices() @@ -275,7 +318,7 @@ def run(show_plots): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(fswTask, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -289,9 +332,9 @@ def run(show_plots): decayTime = 10.0 xi = 1.0 mrpControl.Ki = -1 # make value negative to turn off integral feedback - mrpControl.P = 3*np.max(I)/decayTime - mrpControl.K = (mrpControl.P/xi)*(mrpControl.P/xi)/np.max(I) - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.P = 3 * np.max(I) / decayTime + mrpControl.K = (mrpControl.P / xi) * (mrpControl.P / xi) / np.max(I) + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] @@ -306,7 +349,7 @@ def run(show_plots): thrDesatControl = thrMomentumManagement.thrMomentumManagement() thrDesatControl.ModelTag = "thrMomentumManagement" scSim.AddModelToTask(fswTask, thrDesatControl) - thrDesatControl.hs_min = 80 # Nms : maximum wheel momentum + thrDesatControl.hs_min = 80 # Nms : maximum wheel momentum # setup the thruster force mapping module thrForceMappingObj = thrForceMapping.thrForceMapping() @@ -314,14 +357,14 @@ def run(show_plots): scSim.AddModelToTask(fswTask, thrForceMappingObj) thrForceMappingObj.controlAxes_B = controlAxes_B thrForceMappingObj.thrForceSign = 1 - thrForceMappingObj.angErrThresh = 3.15 # this needs to be larger than pi (180 deg) for the module to work in the momentum dumping scenario + thrForceMappingObj.angErrThresh = 3.15 # this needs to be larger than pi (180 deg) for the module to work in the momentum dumping scenario # setup the thruster momentum dumping module thrDump = thrMomentumDumping.thrMomentumDumping() thrDump.ModelTag = "thrDump" scSim.AddModelToTask(fswTask, thrDump) - thrDump.maxCounterValue = 100 # number of control periods (simulationTimeStepFsw) to wait between two subsequent on-times - thrDump.thrMinFireTime = 0.02 # thruster firing resolution + thrDump.maxCounterValue = 100 # number of control periods (simulationTimeStepFsw) to wait between two subsequent on-times + thrDump.thrMinFireTime = 0.02 # thruster firing resolution # # create simulation messages @@ -333,16 +376,21 @@ def run(show_plots): vcMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, dynTask, scObject - # , saveFile=fileName - , rwEffectorList=rwStateEffector - , thrEffectorList=thrusterSet - ) - vizSupport.setActuatorGuiSetting(viz, viewRWPanel=True, - viewRWHUD=True, - viewThrusterPanel=True, - viewThrusterHUD=True - ) + viz = vizSupport.enableUnityVisualization( + scSim, + dynTask, + scObject, + # , saveFile=fileName + rwEffectorList=rwStateEffector, + thrEffectorList=thrusterSet, + ) + vizSupport.setActuatorGuiSetting( + viz, + viewRWPanel=True, + viewRWHUD=True, + viewThrusterPanel=True, + viewThrusterHUD=True, + ) # link messages attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg) @@ -366,12 +414,13 @@ def run(show_plots): thrDump.thrusterImpulseInMsg.subscribeTo(thrForceMappingObj.thrForceCmdOutMsg) thrusterSet.cmdsInMsg.subscribeTo(thrDump.thrusterOnTimeOutMsg) - # # Setup data logging before the simulation is initialized # numDataPoints = 5000 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStepDyn, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStepDyn, numDataPoints + ) sNavRec = sNavObject.attOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, sNavRec) dataRec = scObject.scStateOutMsg.recorder(samplingTime) @@ -380,7 +429,7 @@ def run(show_plots): scSim.AddModelToTask(dynTask, rwMotorLog) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, attErrorLog) - deltaHLog = thrDesatControl.deltaHOutMsg.recorder(samplingTime) + deltaHLog = thrDesatControl.deltaHOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, deltaHLog) thrMapLog = thrForceMappingObj.thrForceCmdOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, thrMapLog) @@ -440,7 +489,6 @@ def run(show_plots): np.set_printoptions(precision=16) - # Displays the plots relative to the S/C attitude and rates errors, wheel momenta, thruster impulses, on times, and thruster firing intervals timeData = rwMotorLog.times() * macros.NANO2SEC @@ -488,98 +536,132 @@ def plot_attitude_error(timeData, dataSigmaBR): """Plot the attitude errors.""" plt.figure(1) for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + r'$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + r"$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") + def plot_rate_error(timeData, dataOmegaBR): """Plot the body angular velocity rate tracking errors.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx+1) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error (rad/s) ') + plt.plot( + timeData, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx + 1) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error (rad/s) ") + def plot_rw_momenta(timeData, dataOmegaRw, RW, numRW): """Plot the RW momenta.""" totMomentumNorm = [] for j in range(len(timeData)): - totMomentum = np.array([0,0,0]) + totMomentum = np.array([0, 0, 0]) for idx in range(numRW): for k in range(3): - totMomentum[k] = totMomentum[k] + dataOmegaRw[j, idx] * RW[idx].Js * RW[idx].gsHat_B[k][0] + totMomentum[k] = ( + totMomentum[k] + + dataOmegaRw[j, idx] * RW[idx].Js * RW[idx].gsHat_B[k][0] + ) totMomentumNorm.append(np.linalg.norm(totMomentum)) plt.figure(3) for idx in range(numRW): - plt.plot(timeData, dataOmegaRw[:, idx] * RW[idx].Js, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$H_{' + str(idx+1) + r'}$') - plt.plot(timeData, totMomentumNorm, '--', - label=r'$\|H\|$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Momentum (Nms)') + plt.plot( + timeData, + dataOmegaRw[:, idx] * RW[idx].Js, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$H_{" + str(idx + 1) + r"}$", + ) + plt.plot(timeData, totMomentumNorm, "--", label=r"$\|H\|$") + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Momentum (Nms)") + def plot_DH(timeData, dataDH): """Plot the body angular velocity rate tracking errors.""" plt.figure(4) for idx in range(3): - plt.plot(timeData, dataDH[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\Delta H_{' + str(idx+1) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Dumped momentum (Nms) ') + plt.plot( + timeData, + dataDH[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\Delta H_{" + str(idx + 1) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Dumped momentum (Nms) ") + def plot_rw_speeds(timeData, dataOmegaRW, numRW): """Plot the RW spin rates.""" plt.figure(5) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx+1) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed (RPM) ') + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx + 1) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed (RPM) ") + def plot_thrImpulse(timeDataFSW, dataMap, numTh): """Plot the Thruster force values.""" plt.figure(5) for idx in range(numTh): - plt.plot(timeDataFSW, dataMap[:, idx], - color=unitTestSupport.getLineColor(idx, numTh), - label=r'$thrImpulse_{' + str(idx+1) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Impulse requested [Ns]') + plt.plot( + timeDataFSW, + dataMap[:, idx], + color=unitTestSupport.getLineColor(idx, numTh), + label=r"$thrImpulse_{" + str(idx + 1) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Impulse requested [Ns]") + def plot_OnTimeRequest(timeData, dataOnTime, numTh): """Plot the thruster on time requests.""" plt.figure(6) for idx in range(numTh): - plt.plot(timeData, dataOnTime[:, idx], - color=unitTestSupport.getLineColor(idx, numTh), - label=r'$OnTimeRequest_{' + str(idx+1) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('OnTimeRequest [sec]') + plt.plot( + timeData, + dataOnTime[:, idx], + color=unitTestSupport.getLineColor(idx, numTh), + label=r"$OnTimeRequest_{" + str(idx + 1) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("OnTimeRequest [sec]") + def plot_thrForce(timeDataFSW, dataThr, numTh): """Plot the Thruster force values.""" plt.figure(7) for idx in range(numTh): - plt.plot(timeDataFSW, dataThr[idx], - color=unitTestSupport.getLineColor(idx, numTh), - label=r'$thrForce_{' + str(idx+1) + r'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Thruster force [N]') + plt.plot( + timeDataFSW, + dataThr[idx], + color=unitTestSupport.getLineColor(idx, numTh), + label=r"$thrForce_{" + str(idx + 1) + r"}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Thruster force [N]") + if __name__ == "__main__": run(True) diff --git a/examples/scenarioMonteCarloAttRW.py b/examples/scenarioMonteCarloAttRW.py index b2dc3e2d4f..f5547a6504 100644 --- a/examples/scenarioMonteCarloAttRW.py +++ b/examples/scenarioMonteCarloAttRW.py @@ -178,7 +178,6 @@ # scenarioAttitudeFeedbackRW with dispersed initial parameters # - import inspect import math import os @@ -193,6 +192,7 @@ from Basilisk import __path__ + bskPath = __path__[0] # import general simulation support files @@ -221,8 +221,12 @@ from Basilisk.architecture import messaging from Basilisk.utilities.MonteCarlo.Controller import Controller, RetentionPolicy -from Basilisk.utilities.MonteCarlo.Dispersions import (UniformEulerAngleMRPDispersion, UniformDispersion, - NormalVectorCartDispersion, InertiaTensorDispersion) +from Basilisk.utilities.MonteCarlo.Dispersions import ( + UniformEulerAngleMRPDispersion, + UniformDispersion, + NormalVectorCartDispersion, + InertiaTensorDispersion, +) # Add this import and check at the beginning of the file import importlib @@ -230,6 +234,7 @@ # Try to import Bokeh, set availability flag try: import bokeh + bokeh_available = True from Basilisk.utilities.MonteCarlo.AnalysisBaseClass import MonteCarloPlotter except ImportError: @@ -254,8 +259,8 @@ # We also will need the simulationTime and samplingTimes numDataPoints = 500 -simulationTime = macros.min2nano(10.) -samplingTime = simulationTime // (numDataPoints-1) +simulationTime = macros.min2nano(10.0) +samplingTime = simulationTime // (numDataPoints - 1) def run(saveFigures, case, show_plots, delete_data=True, useBokeh=False): @@ -306,37 +311,78 @@ def run(saveFigures, case, show_plots, delete_data=True, useBokeh=False): monteCarlo.setArchiveDir(dirName) # Statistical dispersions can be applied to initial parameters using the MonteCarlo module - dispMRPInit = 'TaskList[0].TaskModels[0].hub.sigma_BNInit' - dispOmegaInit = 'TaskList[0].TaskModels[0].hub.omega_BN_BInit' - dispMass = 'TaskList[0].TaskModels[0].hub.mHub' - dispCoMOff = 'TaskList[0].TaskModels[0].hub.r_BcB_B' - dispInertia = 'hubref.IHubPntBc_B' - dispRW1Axis = 'RW1.gsHat_B' - dispRW2Axis = 'RW2.gsHat_B' - dispRW3Axis = 'RW3.gsHat_B' - dispRW1Omega = 'RW1.Omega' - dispRW2Omega = 'RW2.Omega' - dispRW3Omega = 'RW3.Omega' - dispVoltageIO_0 = 'rwVoltageIO.voltage2TorqueGain[0]' - dispVoltageIO_1 = 'rwVoltageIO.voltage2TorqueGain[1]' - dispVoltageIO_2 = 'rwVoltageIO.voltage2TorqueGain[2]' + dispMRPInit = "TaskList[0].TaskModels[0].hub.sigma_BNInit" + dispOmegaInit = "TaskList[0].TaskModels[0].hub.omega_BN_BInit" + dispMass = "TaskList[0].TaskModels[0].hub.mHub" + dispCoMOff = "TaskList[0].TaskModels[0].hub.r_BcB_B" + dispInertia = "hubref.IHubPntBc_B" + dispRW1Axis = "RW1.gsHat_B" + dispRW2Axis = "RW2.gsHat_B" + dispRW3Axis = "RW3.gsHat_B" + dispRW1Omega = "RW1.Omega" + dispRW2Omega = "RW2.Omega" + dispRW3Omega = "RW3.Omega" + dispVoltageIO_0 = "rwVoltageIO.voltage2TorqueGain[0]" + dispVoltageIO_1 = "rwVoltageIO.voltage2TorqueGain[1]" + dispVoltageIO_2 = "rwVoltageIO.voltage2TorqueGain[2]" dispList = [dispMRPInit, dispOmegaInit, dispMass, dispCoMOff, dispInertia] # Add dispersions with their dispersion type monteCarlo.addDispersion(UniformEulerAngleMRPDispersion(dispMRPInit)) - monteCarlo.addDispersion(NormalVectorCartDispersion(dispOmegaInit, 0.0, 0.75 / 3.0 * np.pi / 180)) - monteCarlo.addDispersion(UniformDispersion(dispMass, ([750.0 - 0.05*750, 750.0 + 0.05*750]))) - monteCarlo.addDispersion(NormalVectorCartDispersion(dispCoMOff, [0.0, 0.0, 1.0], [0.05 / 3.0, 0.05 / 3.0, 0.1 / 3.0])) + monteCarlo.addDispersion( + NormalVectorCartDispersion(dispOmegaInit, 0.0, 0.75 / 3.0 * np.pi / 180) + ) + monteCarlo.addDispersion( + UniformDispersion(dispMass, ([750.0 - 0.05 * 750, 750.0 + 0.05 * 750])) + ) + monteCarlo.addDispersion( + NormalVectorCartDispersion( + dispCoMOff, [0.0, 0.0, 1.0], [0.05 / 3.0, 0.05 / 3.0, 0.1 / 3.0] + ) + ) monteCarlo.addDispersion(InertiaTensorDispersion(dispInertia, stdAngle=0.1)) - monteCarlo.addDispersion(NormalVectorCartDispersion(dispRW1Axis, [1.0, 0.0, 0.0], [0.01 / 3.0, 0.005 / 3.0, 0.005 / 3.0])) - monteCarlo.addDispersion(NormalVectorCartDispersion(dispRW2Axis, [0.0, 1.0, 0.0], [0.005 / 3.0, 0.01 / 3.0, 0.005 / 3.0])) - monteCarlo.addDispersion(NormalVectorCartDispersion(dispRW3Axis, [0.0, 0.0, 1.0], [0.005 / 3.0, 0.005 / 3.0, 0.01 / 3.0])) - monteCarlo.addDispersion(UniformDispersion(dispRW1Omega, ([100.0 - 0.05*100, 100.0 + 0.05*100]))) - monteCarlo.addDispersion(UniformDispersion(dispRW2Omega, ([200.0 - 0.05*200, 200.0 + 0.05*200]))) - monteCarlo.addDispersion(UniformDispersion(dispRW3Omega, ([300.0 - 0.05*300, 300.0 + 0.05*300]))) - monteCarlo.addDispersion(UniformDispersion(dispVoltageIO_0, ([0.2/10. - 0.05 * 0.2/10., 0.2/10. + 0.05 * 0.2/10.]))) - monteCarlo.addDispersion(UniformDispersion(dispVoltageIO_1, ([0.2/10. - 0.05 * 0.2/10., 0.2/10. + 0.05 * 0.2/10.]))) - monteCarlo.addDispersion(UniformDispersion(dispVoltageIO_2, ([0.2/10. - 0.05 * 0.2/10., 0.2/10. + 0.05 * 0.2/10.]))) + monteCarlo.addDispersion( + NormalVectorCartDispersion( + dispRW1Axis, [1.0, 0.0, 0.0], [0.01 / 3.0, 0.005 / 3.0, 0.005 / 3.0] + ) + ) + monteCarlo.addDispersion( + NormalVectorCartDispersion( + dispRW2Axis, [0.0, 1.0, 0.0], [0.005 / 3.0, 0.01 / 3.0, 0.005 / 3.0] + ) + ) + monteCarlo.addDispersion( + NormalVectorCartDispersion( + dispRW3Axis, [0.0, 0.0, 1.0], [0.005 / 3.0, 0.005 / 3.0, 0.01 / 3.0] + ) + ) + monteCarlo.addDispersion( + UniformDispersion(dispRW1Omega, ([100.0 - 0.05 * 100, 100.0 + 0.05 * 100])) + ) + monteCarlo.addDispersion( + UniformDispersion(dispRW2Omega, ([200.0 - 0.05 * 200, 200.0 + 0.05 * 200])) + ) + monteCarlo.addDispersion( + UniformDispersion(dispRW3Omega, ([300.0 - 0.05 * 300, 300.0 + 0.05 * 300])) + ) + monteCarlo.addDispersion( + UniformDispersion( + dispVoltageIO_0, + ([0.2 / 10.0 - 0.05 * 0.2 / 10.0, 0.2 / 10.0 + 0.05 * 0.2 / 10.0]), + ) + ) + monteCarlo.addDispersion( + UniformDispersion( + dispVoltageIO_1, + ([0.2 / 10.0 - 0.05 * 0.2 / 10.0, 0.2 / 10.0 + 0.05 * 0.2 / 10.0]), + ) + ) + monteCarlo.addDispersion( + UniformDispersion( + dispVoltageIO_2, + ([0.2 / 10.0 - 0.05 * 0.2 / 10.0, 0.2 / 10.0 + 0.05 * 0.2 / 10.0]), + ) + ) # A `RetentionPolicy` is used to define what data from the simulation should be retained. A `RetentionPolicy` # is a list of messages and variables to log from each simulation run. It also has a callback, @@ -374,10 +420,14 @@ def run(saveFigures, case, show_plots, delete_data=True, useBokeh=False): # Then retained data from any run can then be accessed in the form of a dictionary # with two sub-dictionaries for messages and variables: - retainedData = monteCarloLoaded.getRetainedData(NUMBER_OF_RUNS-1) - assert retainedData is not None, "Retained data should be available after execution" + retainedData = monteCarloLoaded.getRetainedData(NUMBER_OF_RUNS - 1) + assert retainedData is not None, ( + "Retained data should be available after execution" + ) assert "messages" in retainedData, "Retained data should retain messages" - assert guidMsgName + ".sigma_BR" in retainedData["messages"], "Retained messages should exist" + assert guidMsgName + ".sigma_BR" in retainedData["messages"], ( + "Retained messages should exist" + ) # We also can rerun a case using the same parameters and random seeds # If we rerun a properly set-up run, it should output the same data. @@ -385,39 +435,46 @@ def run(saveFigures, case, show_plots, delete_data=True, useBokeh=False): oldOutput = retainedData["messages"][guidMsgName + ".sigma_BR"] # Rerunning the case shouldn't fail - failed = monteCarloLoaded.reRunCases([NUMBER_OF_RUNS-1]) + failed = monteCarloLoaded.reRunCases([NUMBER_OF_RUNS - 1]) assert len(failed) == 0, "Should rerun case successfully" # Now access the newly retained data to see if it changed - retainedData = monteCarloLoaded.getRetainedData(NUMBER_OF_RUNS-1) + retainedData = monteCarloLoaded.getRetainedData(NUMBER_OF_RUNS - 1) newOutput = retainedData["messages"][guidMsgName + ".sigma_BR"] for k1, v1 in enumerate(oldOutput): for k2, v2 in enumerate(v1): - assert math.fabs(oldOutput[k1][k2] - newOutput[k1][k2]) < .001, \ - "Outputs shouldn't change on runs if random seeds are same" + assert math.fabs(oldOutput[k1][k2] - newOutput[k1][k2]) < 0.001, ( + "Outputs shouldn't change on runs if random seeds are same" + ) # We can also access the initial parameters # The random seeds should differ between runs, so we will test that - params1 = monteCarloLoaded.getParameters(NUMBER_OF_RUNS-1) - params2 = monteCarloLoaded.getParameters(NUMBER_OF_RUNS-2) - assert "TaskList[0].TaskModels[0].RNGSeed" in params1, "random number seed should be applied" + params1 = monteCarloLoaded.getParameters(NUMBER_OF_RUNS - 1) + params2 = monteCarloLoaded.getParameters(NUMBER_OF_RUNS - 2) + assert "TaskList[0].TaskModels[0].RNGSeed" in params1, ( + "random number seed should be applied" + ) for dispName in dispList: assert dispName in params1, "dispersion should be applied" # assert two different runs had different parameters. - assert params1[dispName] != params2[dispName], "dispersion should be different in each run" + assert params1[dispName] != params2[dispName], ( + "dispersion should be different in each run" + ) if useBokeh and bokeh_available: # Create the Bokeh application plotter = MonteCarloPlotter(dirName) - plotter.load_data([ - guidMsgName + ".sigma_BR", - guidMsgName + ".omega_BR_B", - rwSpeedMsgName + ".wheelSpeeds", - voltMsgName + ".voltage", - rwOutName[0] + ".u_current", - rwOutName[1] + ".u_current", - rwOutName[2] + ".u_current" - ]) + plotter.load_data( + [ + guidMsgName + ".sigma_BR", + guidMsgName + ".omega_BR_B", + rwSpeedMsgName + ".wheelSpeeds", + voltMsgName + ".voltage", + rwOutName[0] + ".u_current", + rwOutName[1] + ".u_current", + rwOutName[2] + ".u_current", + ] + ) plotter.show_plots() elif show_plots: # Use matplotlib for plotting @@ -453,10 +510,12 @@ def run(saveFigures, case, show_plots, delete_data=True, useBokeh=False): plt.close("all") # Now we clean up data from this test - os.remove(icName + '/' + 'MonteCarlo.data') + os.remove(icName + "/" + "MonteCarlo.data") for i in range(numberICs): - os.remove(icName + '/' + 'run' + str(i) + '.data') - assert not os.path.exists(icName + '/' + 'MonteCarlo.data'), "No leftover data should exist after the test" + os.remove(icName + "/" + "run" + str(i) + ".data") + assert not os.path.exists(icName + "/" + "MonteCarlo.data"), ( + "No leftover data should exist after the test" + ) # Delete Monte Carlo data if configured to do so. (default is True) if delete_data: @@ -464,9 +523,9 @@ def run(saveFigures, case, show_plots, delete_data=True, useBokeh=False): return dirName + # This function creates the simulation to be executed in parallel. def createScenarioAttitudeFeedbackRW(): - # Create simulation variable names simTaskName = "simTask" simProcessName = "simProcess" @@ -489,7 +548,7 @@ def createScenarioAttitudeFeedbackRW(): scSim.dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) scSim.dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -500,11 +559,13 @@ def createScenarioAttitudeFeedbackRW(): scSim.scObject = spacecraft.Spacecraft() scSim.scObject.ModelTag = "spacecraftBody" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scSim.scObject.hub.mHub = 750.0 # kg - spacecraft mass - scSim.scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scSim.scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scSim.scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) scSim.hubref = scSim.scObject.hub @@ -515,7 +576,7 @@ def createScenarioAttitudeFeedbackRW(): scSim.rwVoltageIO.ModelTag = "rwVoltageInterface" # set module parameters(s) - scSim.rwVoltageIO.setGains(np.array([0.2/10.]*3)) # [Nm/V] conversion gain + scSim.rwVoltageIO.setGains(np.array([0.2 / 10.0] * 3)) # [Nm/V] conversion gain # Add test module to runtime call list scSim.AddModelToTask(simTaskName, scSim.rwVoltageIO) @@ -540,30 +601,37 @@ def createScenarioAttitudeFeedbackRW(): scSim.varRWModel = messaging.BalancedWheels # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments - scSim.RW1 = scSim.rwFactory.create('Honeywell_HR16' - , [1, 0, 0] - , maxMomentum=50. - , Omega=100. # RPM - , RWModel= scSim.varRWModel - ) - scSim.RW2 = scSim.rwFactory.create('Honeywell_HR16' - , [0, 1, 0] - , maxMomentum=50. - , Omega=200. # RPM - , RWModel= scSim.varRWModel - ) - scSim.RW3 = scSim.rwFactory.create('Honeywell_HR16' - , [0, 0, 1] - , maxMomentum=50. - , Omega=300. # RPM - , rWB_B = [0.5, 0.5, 0.5] # meters - , RWModel= scSim.varRWModel - ) + scSim.RW1 = scSim.rwFactory.create( + "Honeywell_HR16", + [1, 0, 0], + maxMomentum=50.0, + Omega=100.0, # RPM + RWModel=scSim.varRWModel, + ) + scSim.RW2 = scSim.rwFactory.create( + "Honeywell_HR16", + [0, 1, 0], + maxMomentum=50.0, + Omega=200.0, # RPM + RWModel=scSim.varRWModel, + ) + scSim.RW3 = scSim.rwFactory.create( + "Honeywell_HR16", + [0, 0, 1], + maxMomentum=50.0, + Omega=300.0, # RPM + rWB_B=[0.5, 0.5, 0.5], # meters + RWModel=scSim.varRWModel, + ) numRW = scSim.rwFactory.getNumOfDevices() # create RW object container and tie to spacecraft object scSim.rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector() - scSim.rwFactory.addToSpacecraft(scSim.scObject.ModelTag, scSim.rwStateEffector, scSim.scObject) - scSim.rwStateEffector.rwMotorCmdInMsg.subscribeTo(scSim.rwVoltageIO.motorTorqueOutMsg) + scSim.rwFactory.addToSpacecraft( + scSim.scObject.ModelTag, scSim.rwStateEffector, scSim.scObject + ) + scSim.rwStateEffector.rwMotorCmdInMsg.subscribeTo( + scSim.rwVoltageIO.motorTorqueOutMsg + ) # add RW object array to the simulation process scSim.AddModelToTask(simTaskName, scSim.rwStateEffector, 2) @@ -595,7 +663,11 @@ def createScenarioAttitudeFeedbackRW(): scSim.inertial3DObj = inertial3D.inertial3D() scSim.inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, scSim.inertial3DObj) - scSim.inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + scSim.inertial3DObj.sigma_R0N = [ + 0.0, + 0.0, + 0.0, + ] # set the desired inertial orientation # setup the attitude tracking error evaluation module scSim.attError = attTrackingError.attTrackingError() @@ -612,10 +684,10 @@ def createScenarioAttitudeFeedbackRW(): scSim.mrpControl.vehConfigInMsg.subscribeTo(vcMsg) scSim.mrpControl.rwParamsInMsg.subscribeTo(scSim.fswRwConfMsg) scSim.mrpControl.rwSpeedsInMsg.subscribeTo(scSim.rwStateEffector.rwSpeedOutMsg) - scSim.mrpControl.K = 3.5 - scSim.mrpControl.Ki = -1 # make value negative to turn off integral feedback - scSim.mrpControl.P = 30.0 - scSim.mrpControl.integralLimit = 2./scSim.mrpControl.Ki * 0.1 + scSim.mrpControl.K = 3.5 + scSim.mrpControl.Ki = -1 # make value negative to turn off integral feedback + scSim.mrpControl.P = 30.0 + scSim.mrpControl.integralLimit = 2.0 / scSim.mrpControl.Ki * 0.1 # add module that maps the Lr control torque into the RW motor torques scSim.rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() @@ -625,11 +697,7 @@ def createScenarioAttitudeFeedbackRW(): scSim.rwMotorTorqueObj.vehControlInMsg.subscribeTo(scSim.mrpControl.cmdTorqueOutMsg) scSim.rwMotorTorqueObj.rwParamsInMsg.subscribeTo(scSim.fswRwConfMsg) # Make the RW control all three body axes - controlAxes_B = [ - 1,0,0 - ,0,1,0 - ,0,0,1 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] scSim.rwMotorTorqueObj.controlAxes_B = controlAxes_B scSim.fswRWVoltage = rwMotorVoltage.rwMotorVoltage() @@ -639,7 +707,9 @@ def createScenarioAttitudeFeedbackRW(): scSim.AddModelToTask(simTaskName, scSim.fswRWVoltage) # Initialize the test module configuration data - scSim.fswRWVoltage.torqueInMsg.subscribeTo(scSim.rwMotorTorqueObj.rwMotorTorqueOutMsg) + scSim.fswRWVoltage.torqueInMsg.subscribeTo( + scSim.rwMotorTorqueObj.rwMotorTorqueOutMsg + ) scSim.fswRWVoltage.rwParamsInMsg.subscribeTo(scSim.fswRwConfMsg) scSim.rwVoltageIO.motorVoltageInMsg.subscribeTo(scSim.fswRWVoltage.voltageOutMsg) # set module parameters @@ -651,23 +721,25 @@ def createScenarioAttitudeFeedbackRW(): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - oe.a = 10000000.0 # meters - oe.e = 0.01 - oe.i = 33.3*macros.D2R - oe.Omega = 48.2*macros.D2R - oe.omega = 347.8*macros.D2R - oe.f = 85.3*macros.D2R + oe.a = 10000000.0 # meters + oe.e = 0.01 + oe.i = 33.3 * macros.D2R + oe.Omega = 48.2 * macros.D2R + oe.omega = 347.8 * macros.D2R + oe.f = 85.3 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) scSim.scObject.hub.r_CN_NInit = rN # m - r_CN_N scSim.scObject.hub.v_CN_NInit = vN # m/s - v_CN_N - scSim.scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_CN_B - scSim.scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_CN_B + scSim.scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_CN_B + scSim.scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_CN_B # store the msg recorder modules in a dictionary list so the retention policy class can pull the data # when the simulation ends scSim.msgRecList = {} - scSim.msgRecList[rwMotorTorqueMsgName] = scSim.rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime) + scSim.msgRecList[rwMotorTorqueMsgName] = ( + scSim.rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime) + ) scSim.AddModelToTask(simTaskName, scSim.msgRecList[rwMotorTorqueMsgName]) scSim.msgRecList[guidMsgName] = scSim.attError.attGuidOutMsg.recorder(samplingTime) @@ -676,15 +748,21 @@ def createScenarioAttitudeFeedbackRW(): scSim.msgRecList[transMsgName] = sNavObject.transOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, scSim.msgRecList[transMsgName]) - scSim.msgRecList[rwSpeedMsgName] = scSim.rwStateEffector.rwSpeedOutMsg.recorder(samplingTime) + scSim.msgRecList[rwSpeedMsgName] = scSim.rwStateEffector.rwSpeedOutMsg.recorder( + samplingTime + ) scSim.AddModelToTask(simTaskName, scSim.msgRecList[rwSpeedMsgName]) - scSim.msgRecList[voltMsgName] = scSim.fswRWVoltage.voltageOutMsg.recorder(samplingTime) + scSim.msgRecList[voltMsgName] = scSim.fswRWVoltage.voltageOutMsg.recorder( + samplingTime + ) scSim.AddModelToTask(simTaskName, scSim.msgRecList[voltMsgName]) c = 0 for msgName in rwOutName: - scSim.msgRecList[msgName] = scSim.rwStateEffector.rwOutMsgs[c].recorder(samplingTime) + scSim.msgRecList[msgName] = scSim.rwStateEffector.rwOutMsgs[c].recorder( + samplingTime + ) scSim.AddModelToTask(simTaskName, scSim.msgRecList[msgName]) c += 1 @@ -704,75 +782,95 @@ def executeScenario(sim): # It is called once for each run of the simulation, overlapping the plots def plotSim(data, retentionPolicy): # retrieve the logged data - dataUsReq = data["messages"][rwMotorTorqueMsgName + ".motorTorque"][:,1:] - dataSigmaBR = data["messages"][guidMsgName + ".sigma_BR"][:,1:] - dataOmegaBR = data["messages"][guidMsgName + ".omega_BR_B"][:,1:] - dataPos = data["messages"][transMsgName + ".r_BN_N"][:,1:] - dataOmegaRW = data["messages"][rwSpeedMsgName + ".wheelSpeeds"][:,1:] - dataVolt = data["messages"][voltMsgName + ".voltage"][:,1:] + dataUsReq = data["messages"][rwMotorTorqueMsgName + ".motorTorque"][:, 1:] + dataSigmaBR = data["messages"][guidMsgName + ".sigma_BR"][:, 1:] + dataOmegaBR = data["messages"][guidMsgName + ".omega_BR_B"][:, 1:] + dataPos = data["messages"][transMsgName + ".r_BN_N"][:, 1:] + dataOmegaRW = data["messages"][rwSpeedMsgName + ".wheelSpeeds"][:, 1:] + dataVolt = data["messages"][voltMsgName + ".voltage"][:, 1:] dataRW = [] for msgName in rwOutName: - dataRW.append(data["messages"][msgName+".u_current"][:,1:]) + dataRW.append(data["messages"][msgName + ".u_current"][:, 1:]) np.set_printoptions(precision=16) # # plot the results # - timeData = data["messages"][rwMotorTorqueMsgName + ".motorTorque"][:,0] * macros.NANO2MIN + timeData = ( + data["messages"][rwMotorTorqueMsgName + ".motorTorque"][:, 0] * macros.NANO2MIN + ) figureList = {} plt.figure(1) - pltName = 'AttitudeError' + pltName = "AttitudeError" for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - label='Run ' + str(data["index"]) + r' $\sigma_'+str(idx)+'$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + label="Run " + str(data["index"]) + r" $\sigma_" + str(idx) + "$", + ) # plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") figureList[pltName] = plt.figure(1) plt.figure(2) - pltName = 'RWMotorTorque' + pltName = "RWMotorTorque" for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - label='Run ' + str(data["index"]) + r' $\hat u_{s,'+str(idx)+'}$') - plt.plot(timeData, dataRW[idx], - label='Run ' + str(data["index"]) + ' $u_{s,' + str(idx) + '}$') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + label="Run " + str(data["index"]) + r" $\hat u_{s," + str(idx) + "}$", + ) + plt.plot( + timeData, + dataRW[idx], + label="Run " + str(data["index"]) + " $u_{s," + str(idx) + "}$", + ) # plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") figureList[pltName] = plt.figure(2) plt.figure(3) - pltName = 'RateTrackingError' + pltName = "RateTrackingError" for idx in range(3): - plt.plot(timeData, dataOmegaBR[:, idx], - label='Run ' + str(data["index"]) + r' $\omega_{BR,'+str(idx)+'}$') + plt.plot( + timeData, + dataOmegaBR[:, idx], + label="Run " + str(data["index"]) + r" $\omega_{BR," + str(idx) + "}$", + ) # plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error (rad/s) ') + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error (rad/s) ") figureList[pltName] = plt.figure(3) plt.figure(4) - pltName = 'RWSpeed' + pltName = "RWSpeed" for idx in range(len(rwOutName)): - plt.plot(timeData, dataOmegaRW[:, idx]/macros.RPM, - label='Run ' + str(data["index"]) + r' $\Omega_{'+str(idx)+'}$') + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + label="Run " + str(data["index"]) + r" $\Omega_{" + str(idx) + "}$", + ) # plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed (RPM) ') + plt.xlabel("Time [min]") + plt.ylabel("RW Speed (RPM) ") figureList[pltName] = plt.figure(4) plt.figure(5) - pltName = 'RWVoltage' + pltName = "RWVoltage" for idx in range(len(rwOutName)): - plt.plot(timeData, dataVolt[:, idx], - label='Run ' + str(data["index"]) + ' $V_{' + str(idx) + '}$') + plt.plot( + timeData, + dataVolt[:, idx], + label="Run " + str(data["index"]) + " $V_{" + str(idx) + "}$", + ) # plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Voltage (V) ') + plt.xlabel("Time [min]") + plt.ylabel("RW Voltage (V) ") figureList[pltName] = plt.figure(5) return figureList @@ -783,8 +881,8 @@ def plotSimAndSave(data, retentionPolicy): for pltName, plt in list(figureList.items()): # plt.subplots_adjust(top = 0.6, bottom = 0.4) unitTestSupport.saveScenarioFigure( - fileNameString + "_" + pltName - , plt, path + "/dataForExamples") + fileNameString + "_" + pltName, plt, path + "/dataForExamples" + ) return @@ -792,6 +890,7 @@ def plotSimAndSave(data, retentionPolicy): # Modify the __main__ section if __name__ == "__main__": import sys + delete_data = True useBokeh = False @@ -817,17 +916,25 @@ def plotSimAndSave(data, retentionPolicy): def bk_worker(doc): def update(): - dirName = run(saveFigures=True, case=1, show_plots=True, delete_data=False, useBokeh=True) + dirName = run( + saveFigures=True, + case=1, + show_plots=True, + delete_data=False, + useBokeh=True, + ) plotter = MonteCarloPlotter(dirName) - plotter.load_data([ - rwOutName[0] + ".u_current", - rwOutName[1] + ".u_current", - rwOutName[2] + ".u_current", - guidMsgName + ".sigma_BR", - guidMsgName + ".omega_BR_B", - rwSpeedMsgName + ".wheelSpeeds", - voltMsgName + ".voltage" - ]) + plotter.load_data( + [ + rwOutName[0] + ".u_current", + rwOutName[1] + ".u_current", + rwOutName[2] + ".u_current", + guidMsgName + ".sigma_BR", + guidMsgName + ".omega_BR_B", + rwSpeedMsgName + ".wheelSpeeds", + voltMsgName + ".voltage", + ] + ) layout = plotter.show_plots() # Add close button functionality @@ -842,7 +949,9 @@ def close_callback(): } }, 100); """ - doc.add_next_tick_callback(lambda: doc.js_on_event(None, script)) + doc.add_next_tick_callback( + lambda: doc.js_on_event(None, script) + ) # Stop the Bokeh server doc.remove_root(layout) @@ -854,10 +963,20 @@ def close_callback(): logger.error(f"Error during shutdown: {str(e)}") sys.exit(1) - close_button = Button(label="Close Application", button_type="danger", width=150) + close_button = Button( + label="Close Application", button_type="danger", width=150 + ) close_button.on_click(close_callback) - layout.children.insert(-1, row(Spacer(width=20), close_button, Spacer(width=20), - sizing_mode="fixed", align="center")) + layout.children.insert( + -1, + row( + Spacer(width=20), + close_button, + Spacer(width=20), + sizing_mode="fixed", + align="center", + ), + ) doc.add_root(layout) doc.title = "BSK Monte Carlo Visualization" @@ -865,10 +984,12 @@ def close_callback(): doc.add_next_tick_callback(update) print("Starting Bokeh server") - server = Server({'/': Application(FunctionHandler(bk_worker))}) + server = Server({"/": Application(FunctionHandler(bk_worker))}) server.start() - print('Opening Bokeh application on http://localhost:5006/') + print("Opening Bokeh application on http://localhost:5006/") server.io_loop.add_callback(server.show, "/") server.io_loop.start() else: - dirName = run(saveFigures=True, case=1, show_plots=True, delete_data=True, useBokeh=False) + dirName = run( + saveFigures=True, case=1, show_plots=True, delete_data=True, useBokeh=False + ) diff --git a/examples/scenarioMonteCarloSpice.py b/examples/scenarioMonteCarloSpice.py index 4da783bb53..5655bc4de5 100644 --- a/examples/scenarioMonteCarloSpice.py +++ b/examples/scenarioMonteCarloSpice.py @@ -50,7 +50,6 @@ # Purpose: This Monte Carlo example shows how to properly use Spice in such simulations. # - import inspect import os import shutil @@ -62,6 +61,7 @@ # @endcond from Basilisk import __path__ + bskPath = __path__[0] from Basilisk.utilities import SimulationBaseClass @@ -96,35 +96,34 @@ def __init__(self): simTaskName = "simTask" simProcessName = "simProcess" - self.dynProcess = self.CreateNewProcess(simProcessName) - self.dynProcess.addTask(self.CreateNewTask(simTaskName, macros.sec2nano(10.))) + self.dynProcess.addTask(self.CreateNewTask(simTaskName, macros.sec2nano(10.0))) self.scObject = spacecraft.Spacecraft() self.AddModelToTask(simTaskName, self.scObject, 1) - self.scObject.hub.r_CN_NInit = [7000000.0, 0.0, 0.0] # m - r_CN_N - self.scObject.hub.v_CN_NInit = [0.0, 7500.0, 0.0] # m/s - v_CN_N - + self.scObject.hub.r_CN_NInit = [7000000.0, 0.0, 0.0] # m - r_CN_N + self.scObject.hub.v_CN_NInit = [0.0, 7500.0, 0.0] # m/s - v_CN_N # operate on pyswice dataPath = bskPath + "/supportData/EphemerisData/" - self.scSpiceName = 'HUBBLE SPACE TELESCOPE' - pyswice.furnsh_c(dataPath + 'naif0011.tls') - pyswice.furnsh_c(dataPath + 'pck00010.tpc') - pyswice.furnsh_c(dataPath + 'de-403-masses.tpc') - pyswice.furnsh_c(dataPath + 'de430.bsp') - pyswice.furnsh_c(dataPath + 'hst_edited.bsp') + self.scSpiceName = "HUBBLE SPACE TELESCOPE" + pyswice.furnsh_c(dataPath + "naif0011.tls") + pyswice.furnsh_c(dataPath + "pck00010.tpc") + pyswice.furnsh_c(dataPath + "de-403-masses.tpc") + pyswice.furnsh_c(dataPath + "de430.bsp") + pyswice.furnsh_c(dataPath + "hst_edited.bsp") self.accessSpiceKernel() def accessSpiceKernel(self): - startCalendarTime = '2012 APR 29 15:18:14.907 (UTC)' - zeroBase = 'Sun' - integFrame = 'j2000' + startCalendarTime = "2012 APR 29 15:18:14.907 (UTC)" + zeroBase = "Sun" + integFrame = "j2000" stateOut = spkRead(self.scSpiceName, startCalendarTime, integFrame, zeroBase) print(stateOut) + def run(): """ This is the main function that is called in this script. It illustrates possible ways @@ -166,7 +165,7 @@ def run(): def executeScenario(sim): - sim.ConfigureStopTime(macros.sec2nano(100.)) + sim.ConfigureStopTime(macros.sec2nano(100.0)) sim.InitializeSimulation() # Here is another example where it is allowable to run the python spice routines within a MC simulation setup diff --git a/examples/scenarioMtbMomentumManagement.py b/examples/scenarioMtbMomentumManagement.py index 946d251a47..dd286fee25 100755 --- a/examples/scenarioMtbMomentumManagement.py +++ b/examples/scenarioMtbMomentumManagement.py @@ -79,20 +79,36 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, - inertial3D, rwMotorTorque, - tamComm, mtbMomentumManagement) -from Basilisk.simulation import (reactionWheelStateEffector, - simpleNav, - magneticFieldWMM, magnetometer, MtbEffector, - spacecraft) -from Basilisk.utilities import (SimulationBaseClass, macros, - orbitalMotion, simIncludeGravBody, - simIncludeRW, unitTestSupport, vizSupport) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + inertial3D, + rwMotorTorque, + tamComm, + mtbMomentumManagement, +) +from Basilisk.simulation import ( + reactionWheelStateEffector, + simpleNav, + magneticFieldWMM, + magnetometer, + MtbEffector, + spacecraft, +) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + simIncludeRW, + unitTestSupport, + vizSupport, +) bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) @@ -104,113 +120,151 @@ def plot_attitude_error(timeData, dataSigmaBR): """Plot the attitude errors.""" plt.figure(1) for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") plt.grid(True) + def plot_rw_cmd_torque(timeData, dataUsReq, numRW): """Plot the RW command torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque [Nm]') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque [Nm]") plt.grid(True) + def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW): """Plot the RW actual motor torques.""" plt.figure(2) for idx in range(numRW): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.plot(timeData, dataRW[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque [Nm]') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.plot( + timeData, + dataRW[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque [Nm]") plt.grid(True) + def plot_rate_error(timeData, dataOmegaBR): """Plot the body angular velocity rate tracking errors.""" plt.figure(3) for idx in range(3): - plt.plot(timeData, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeData, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") plt.grid(True) + def plot_rw_speeds(timeData, dataOmegaRW, numRW): """Plot the RW spin rates.""" plt.figure(4) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed [RPM] ') + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed [RPM] ") plt.grid(True) + def plot_magnetic_field(timeData, dataMagField): """Plot the magnetic field.""" plt.figure(5) for idx in range(3): - plt.plot(timeData, dataMagField[:, idx] * 1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$B\_N_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Magnetic Field [nT]') + plt.plot( + timeData, + dataMagField[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$B\_N_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Magnetic Field [nT]") plt.grid(True) + def plot_data_tam(timeData, dataTam): """Plot the magnetic field.""" plt.figure(6) for idx in range(3): - plt.plot(timeData, dataTam[:, idx] * 1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$TAM\_S_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Magnetic Field [nT]') + plt.plot( + timeData, + dataTam[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$TAM\_S_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Magnetic Field [nT]") plt.grid(True) + def plot_data_tam_comm(timeData, dataTamComm): """Plot the magnetic field.""" plt.figure(7) for idx in range(3): - plt.plot(timeData, dataTamComm[:, idx] * 1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$TAM\_B_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Magnetic Field [nT]') + plt.plot( + timeData, + dataTamComm[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$TAM\_B_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Magnetic Field [nT]") plt.grid(True) + def plot_data_mtb_momentum_management(timeData, dataMtbMomentumManegement, numMTB): """Plot the magnetic field.""" plt.figure(8) for idx in range(numMTB): - plt.plot(timeData, dataMtbMomentumManegement[:, idx], - color=unitTestSupport.getLineColor(idx, numMTB), - label=r'$MTB\_T_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Torque Rod Dipoles [A-m2]') + plt.plot( + timeData, + dataMtbMomentumManegement[:, idx], + color=unitTestSupport.getLineColor(idx, numMTB), + label=r"$MTB\_T_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Torque Rod Dipoles [A-m2]") plt.grid(True) @@ -218,14 +272,17 @@ def plot_data_rw_motor_torque_desired(dataUsReq, tauRequested_W, numRW): """Plot the RW desired motor torques.""" plt.figure(9) for idx in range(numRW): - plt.plot(tauRequested_W[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + tauRequested_W[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") plt.grid(True) + def run(show_plots): """ The scenarios can be run with the followings setups parameters: @@ -245,7 +302,7 @@ def run(show_plots): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(120.) + simulationTime = macros.min2nano(120.0) # # create the simulation process @@ -264,11 +321,13 @@ def run(show_plots): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [0.02 / 3, 0., 0., - 0., 0.1256 / 3, 0., - 0., 0., 0.1256 / 3] + I = [0.02 / 3, 0.0, 0.0, 0.0, 0.1256 / 3, 0.0, 0.0, 0.0, 0.1256 / 3] scObject.hub.mHub = 10.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -294,26 +353,45 @@ def run(show_plots): # store the RW dynamical model type varRWModel = messaging.BalancedWheels - beta = 52. * np.pi / 180. - Gs = np.array([ - [0., 0., np.cos(beta), -np.cos(beta)], - [np.cos(beta), np.sin(beta), -np.sin(beta), -np.sin(beta)], - [np.sin(beta), -np.cos(beta), 0., 0.]]) + beta = 52.0 * np.pi / 180.0 + Gs = np.array( + [ + [0.0, 0.0, np.cos(beta), -np.cos(beta)], + [np.cos(beta), np.sin(beta), -np.sin(beta), -np.sin(beta)], + [np.sin(beta), -np.cos(beta), 0.0, 0.0], + ] + ) # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments - RW1 = rwFactory.create('BCT_RWP015', Gs[:, 0], Omega_max=5000. # RPM - , RWModel=varRWModel, useRWfriction=False, - ) - RW2 = rwFactory.create('BCT_RWP015', Gs[:, 1], Omega_max=5000. # RPM - , RWModel=varRWModel, useRWfriction=False, - ) - RW3 = rwFactory.create('BCT_RWP015', Gs[:, 2], Omega_max=5000. # RPM - , RWModel=varRWModel, useRWfriction=False, - ) - - RW4 = rwFactory.create('BCT_RWP015', Gs[:, 3], Omega_max=5000. # RPM - , RWModel=varRWModel, useRWfriction=False, - ) + RW1 = rwFactory.create( + "BCT_RWP015", + Gs[:, 0], + Omega_max=5000.0, # RPM + RWModel=varRWModel, + useRWfriction=False, + ) + RW2 = rwFactory.create( + "BCT_RWP015", + Gs[:, 1], + Omega_max=5000.0, # RPM + RWModel=varRWModel, + useRWfriction=False, + ) + RW3 = rwFactory.create( + "BCT_RWP015", + Gs[:, 2], + Omega_max=5000.0, # RPM + RWModel=varRWModel, + useRWfriction=False, + ) + + RW4 = rwFactory.create( + "BCT_RWP015", + Gs[:, 3], + Omega_max=5000.0, # RPM + RWModel=varRWModel, + useRWfriction=False, + ) # In this simulation the RW objects RW1, RW2 or RW3 are not modified further. However, you can over-ride # any values generate in the `.create()` process using for example RW1.Omega_max = 100. to change the @@ -330,7 +408,6 @@ def run(show_plots): # to be called which logs the RW states scSim.AddModelToTask(simTaskName, rwStateEffector, 2) - # add the simple Navigation sensor module. This sets the SC attitude, rate, position # velocity navigation message sNavObject = simpleNav.SimpleNav() @@ -340,10 +417,14 @@ def run(show_plots): # create magnetic field module magModule = magneticFieldWMM.MagneticFieldWMM() magModule.ModelTag = "WMM" - magModule.dataPath = bskPath + '/supportData/MagneticField/' - epochMsg = unitTestSupport.timeStringToGregorianUTCMsg('2019 June 27, 10:23:0.0 (UTC)') + magModule.dataPath = bskPath + "/supportData/MagneticField/" + epochMsg = unitTestSupport.timeStringToGregorianUTCMsg( + "2019 June 27, 10:23:0.0 (UTC)" + ) magModule.epochInMsg.subscribeTo(epochMsg) - magModule.addSpacecraftToModel(scObject.scStateOutMsg) # this command can be repeated if multiple + magModule.addSpacecraftToModel( + scObject.scStateOutMsg + ) # this command can be repeated if multiple scSim.AddModelToTask(simTaskName, magModule) # add magnetic torque bar effector @@ -352,7 +433,6 @@ def run(show_plots): scObject.addDynamicEffector(mtbEff) scSim.AddModelToTask(simTaskName, mtbEff) - # # setup the FSW algorithm tasks # @@ -361,7 +441,7 @@ def run(show_plots): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -376,7 +456,7 @@ def run(show_plots): mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.K = 0.0001 mrpControl.P = 0.002 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # add module that maps the Lr control torque into the RW motor torques rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() @@ -384,9 +464,7 @@ def run(show_plots): scSim.AddModelToTask(simTaskName, rwMotorTorqueObj) # Make the RW control all three body axes - controlAxes_B = [ - 1, 0, 0, 0, 1, 0, 0, 0, 1 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] rwMotorTorqueObj.controlAxes_B = controlAxes_B # create the minimal TAM module @@ -394,20 +472,24 @@ def run(show_plots): TAM.ModelTag = "TAM_sensor" # specify the optional TAM variables TAM.scaleFactor = 1.0 - TAM.senNoiseStd = [0.0, 0.0, 0.0] + TAM.senNoiseStd = [0.0, 0.0, 0.0] scSim.AddModelToTask(simTaskName, TAM) # setup tamComm module tamCommObj = tamComm.tamComm() - tamCommObj.dcm_BS = [1., 0., 0., 0., 1., 0., 0., 0., 1.] + tamCommObj.dcm_BS = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] tamCommObj.ModelTag = "tamComm" scSim.AddModelToTask(simTaskName, tamCommObj) # setup mtbMomentumManagement module mtbMomentumManagementObj = mtbMomentumManagement.mtbMomentumManagement() # setting the optional RW biases - mtbMomentumManagementObj.wheelSpeedBiases = [800. * macros.rpm2radsec, 600. * macros.rpm2radsec, - 400. * macros.rpm2radsec, 200. * macros.rpm2radsec] + mtbMomentumManagementObj.wheelSpeedBiases = [ + 800.0 * macros.rpm2radsec, + 600.0 * macros.rpm2radsec, + 400.0 * macros.rpm2radsec, + 200.0 * macros.rpm2radsec, + ] mtbMomentumManagementObj.cGain = 0.003 mtbMomentumManagementObj.ModelTag = "mtbMomentumManagement" scSim.AddModelToTask(simTaskName, mtbMomentumManagementObj) @@ -417,19 +499,31 @@ def run(show_plots): mtbConfigParams.numMTB = 4 # row major toque bar alignments - mtbConfigParams.GtMatrix_B =[ - 1., 0., 0., 0.70710678, - 0., 1., 0., 0.70710678, - 0., 0., 1., 0.] + mtbConfigParams.GtMatrix_B = [ + 1.0, + 0.0, + 0.0, + 0.70710678, + 0.0, + 1.0, + 0.0, + 0.70710678, + 0.0, + 0.0, + 1.0, + 0.0, + ] maxDipole = 0.1 - mtbConfigParams.maxMtbDipoles = [maxDipole]*mtbConfigParams.numMTB + mtbConfigParams.maxMtbDipoles = [maxDipole] * mtbConfigParams.numMTB mtbParamsInMsg = messaging.MTBArrayConfigMsg().write(mtbConfigParams) # # Setup data logging before the simulation is initialized # numDataPoints = 200 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) rwMotorLog = rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) magLog = magModule.envOutMsgs[0].recorder(samplingTime) @@ -474,12 +568,12 @@ def run(show_plots): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - oe.a = 6778.14 * 1000. # meters + oe.a = 6778.14 * 1000.0 # meters oe.e = 0.00 - oe.i = 45. * macros.D2R - oe.Omega = 60. * macros.D2R - oe.omega = 0. * macros.D2R - oe.f = 0. * macros.D2R + oe.i = 45.0 * macros.D2R + oe.Omega = 60.0 * macros.D2R + oe.omega = 0.0 * macros.D2R + oe.f = 0.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m/s - v_CN_N @@ -487,10 +581,13 @@ def run(show_plots): scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_CN_B # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - , rwEffectorList=rwStateEffector - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + rwEffectorList=rwStateEffector, + ) # link messages sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg) @@ -512,9 +609,13 @@ def run(show_plots): mtbMomentumManagementObj.mtbParamsInMsg.subscribeTo(mtbParamsInMsg) mtbMomentumManagementObj.tamSensorBodyInMsg.subscribeTo(tamCommObj.tamOutMsg) mtbMomentumManagementObj.rwSpeedsInMsg.subscribeTo(rwStateEffector.rwSpeedOutMsg) - mtbMomentumManagementObj.rwMotorTorqueInMsg.subscribeTo(rwMotorTorqueObj.rwMotorTorqueOutMsg) + mtbMomentumManagementObj.rwMotorTorqueInMsg.subscribeTo( + rwMotorTorqueObj.rwMotorTorqueOutMsg + ) - rwStateEffector.rwMotorCmdInMsg.subscribeTo(mtbMomentumManagementObj.rwMotorTorqueOutMsg) + rwStateEffector.rwMotorCmdInMsg.subscribeTo( + mtbMomentumManagementObj.rwMotorTorqueOutMsg + ) mtbEff.mtbCmdInMsg.subscribeTo(mtbMomentumManagementObj.mtbCmdOutMsg) mtbEff.mtbParamsInMsg.subscribeTo(mtbParamsInMsg) @@ -570,7 +671,9 @@ def run(show_plots): pltName = fileName + "6" figureList[pltName] = plt.figure(7) - plot_data_mtb_momentum_management(timeData, dataMtbDipoleCmds, mtbConfigParams.numMTB) + plot_data_mtb_momentum_management( + timeData, dataMtbDipoleCmds, mtbConfigParams.numMTB + ) pltName = fileName + "7" figureList[pltName] = plt.figure(8) diff --git a/examples/scenarioMtbMomentumManagementSimple.py b/examples/scenarioMtbMomentumManagementSimple.py index 1cb7d157bf..da9052e903 100755 --- a/examples/scenarioMtbMomentumManagementSimple.py +++ b/examples/scenarioMtbMomentumManagementSimple.py @@ -87,22 +87,40 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, - inertial3D, rwMotorTorque, - tamComm, mtbMomentumManagementSimple, - torque2Dipole, dipoleMapping, - mtbFeedforward, rwNullSpace) -from Basilisk.simulation import (reactionWheelStateEffector, - simpleNav, - magneticFieldWMM, magnetometer, MtbEffector, - spacecraft) -from Basilisk.utilities import (SimulationBaseClass, macros, - orbitalMotion, simIncludeGravBody, - simIncludeRW, unitTestSupport, vizSupport) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + inertial3D, + rwMotorTorque, + tamComm, + mtbMomentumManagementSimple, + torque2Dipole, + dipoleMapping, + mtbFeedforward, + rwNullSpace, +) +from Basilisk.simulation import ( + reactionWheelStateEffector, + simpleNav, + magneticFieldWMM, + magnetometer, + MtbEffector, + spacecraft, +) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + simIncludeRW, + unitTestSupport, + vizSupport, +) bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) @@ -113,113 +131,151 @@ def plot_attitude_error(timeData, dataSigmaBR): """Plot the attitude errors.""" plt.figure(1) for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") plt.grid(True) + def plot_rw_cmd_torque(timeData, dataUsReq, numRW): """Plot the RW command torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque [Nm]') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque [Nm]") plt.grid(True) + def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW): """Plot the RW actual motor torques.""" plt.figure(2) for idx in range(numRW): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.plot(timeData, dataRW[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque [Nm]') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.plot( + timeData, + dataRW[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque [Nm]") plt.grid(True) + def plot_rate_error(timeData, dataOmegaBR): """Plot the body angular velocity rate tracking errors.""" plt.figure(3) for idx in range(3): - plt.plot(timeData, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeData, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") plt.grid(True) + def plot_rw_speeds(timeData, dataOmegaRW, numRW): """Plot the RW spin rates.""" plt.figure(4) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed [RPM] ') + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed [RPM] ") plt.grid(True) + def plot_magnetic_field(timeData, dataMagField): """Plot the magnetic field.""" plt.figure(5) for idx in range(3): - plt.plot(timeData, dataMagField[:, idx] * 1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$B\_N_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Magnetic Field [nT]') + plt.plot( + timeData, + dataMagField[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$B\_N_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Magnetic Field [nT]") plt.grid(True) + def plot_data_tam(timeData, dataTam): """Plot the magnetic field.""" plt.figure(6) for idx in range(3): - plt.plot(timeData, dataTam[:, idx] * 1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$TAM\_S_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Magnetic Field [nT]') + plt.plot( + timeData, + dataTam[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$TAM\_S_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Magnetic Field [nT]") plt.grid(True) + def plot_data_tam_comm(timeData, dataTamComm): """Plot the magnetic field.""" plt.figure(7) for idx in range(3): - plt.plot(timeData, dataTamComm[:, idx] * 1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$TAM\_B_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Magnetic Field [nT]') + plt.plot( + timeData, + dataTamComm[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$TAM\_B_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Magnetic Field [nT]") plt.grid(True) + def plot_data_mtb_momentum_management(timeData, dataMtbMomentumManegement, numMTB): """Plot the magnetic field.""" plt.figure(8) for idx in range(numMTB): - plt.plot(timeData, dataMtbMomentumManegement[:, idx], - color=unitTestSupport.getLineColor(idx, numMTB), - label=r'$MTB\_T_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Torque Rod Dipoles [A-m2]') + plt.plot( + timeData, + dataMtbMomentumManegement[:, idx], + color=unitTestSupport.getLineColor(idx, numMTB), + label=r"$MTB\_T_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Torque Rod Dipoles [A-m2]") plt.grid(True) @@ -227,14 +283,17 @@ def plot_data_rw_motor_torque_desired(dataUsReq, tauRequested_W, numRW): """Plot the RW desired motor torques.""" plt.figure(9) for idx in range(numRW): - plt.plot(tauRequested_W[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + tauRequested_W[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") plt.grid(True) + def run(show_plots): """ The scenarios can be run with the followings setups parameters: @@ -252,7 +311,7 @@ def run(show_plots): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(120.) + simulationTime = macros.min2nano(120.0) # # create the simulation process @@ -271,11 +330,13 @@ def run(show_plots): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [0.02 / 3, 0., 0., - 0., 0.1256 / 3, 0., - 0., 0., 0.1256 / 3] + I = [0.02 / 3, 0.0, 0.0, 0.0, 0.1256 / 3, 0.0, 0.0, 0.0, 0.1256 / 3] scObject.hub.mHub = 10.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -301,26 +362,45 @@ def run(show_plots): # store the RW dynamical model type varRWModel = messaging.BalancedWheels - beta = 52. * np.pi / 180. - Gs = np.array([ - [0., 0., np.cos(beta), -np.cos(beta)], - [np.cos(beta), np.sin(beta), -np.sin(beta), -np.sin(beta)], - [np.sin(beta), -np.cos(beta), 0., 0.]]) + beta = 52.0 * np.pi / 180.0 + Gs = np.array( + [ + [0.0, 0.0, np.cos(beta), -np.cos(beta)], + [np.cos(beta), np.sin(beta), -np.sin(beta), -np.sin(beta)], + [np.sin(beta), -np.cos(beta), 0.0, 0.0], + ] + ) # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments - RW1 = rwFactory.create('BCT_RWP015', Gs[:, 0], Omega_max=5000. # RPM - , RWModel=varRWModel, useRWfriction=False, - ) - RW2 = rwFactory.create('BCT_RWP015', Gs[:, 1], Omega_max=5000. # RPM - , RWModel=varRWModel, useRWfriction=False, - ) - RW3 = rwFactory.create('BCT_RWP015', Gs[:, 2], Omega_max=5000. # RPM - , RWModel=varRWModel, useRWfriction=False, - ) - - RW4 = rwFactory.create('BCT_RWP015', Gs[:, 3], Omega_max=5000. # RPM - , RWModel=varRWModel, useRWfriction=False, - ) + RW1 = rwFactory.create( + "BCT_RWP015", + Gs[:, 0], + Omega_max=5000.0, # RPM + RWModel=varRWModel, + useRWfriction=False, + ) + RW2 = rwFactory.create( + "BCT_RWP015", + Gs[:, 1], + Omega_max=5000.0, # RPM + RWModel=varRWModel, + useRWfriction=False, + ) + RW3 = rwFactory.create( + "BCT_RWP015", + Gs[:, 2], + Omega_max=5000.0, # RPM + RWModel=varRWModel, + useRWfriction=False, + ) + + RW4 = rwFactory.create( + "BCT_RWP015", + Gs[:, 3], + Omega_max=5000.0, # RPM + RWModel=varRWModel, + useRWfriction=False, + ) # In this simulation the RW objects RW1, RW2 or RW3 are not modified further. However, you can over-ride # any values generate in the `.create()` process using for example RW1.Omega_max = 100. to change the @@ -337,7 +417,6 @@ def run(show_plots): # to be called which logs the RW states scSim.AddModelToTask(simTaskName, rwStateEffector, 2) - # add the simple Navigation sensor module. This sets the SC attitude, rate, position # velocity navigation message sNavObject = simpleNav.SimpleNav() @@ -347,10 +426,14 @@ def run(show_plots): # create magnetic field module magModule = magneticFieldWMM.MagneticFieldWMM() magModule.ModelTag = "WMM" - magModule.dataPath = bskPath + '/supportData/MagneticField/' - epochMsg = unitTestSupport.timeStringToGregorianUTCMsg('2019 June 27, 10:23:0.0 (UTC)') + magModule.dataPath = bskPath + "/supportData/MagneticField/" + epochMsg = unitTestSupport.timeStringToGregorianUTCMsg( + "2019 June 27, 10:23:0.0 (UTC)" + ) magModule.epochInMsg.subscribeTo(epochMsg) - magModule.addSpacecraftToModel(scObject.scStateOutMsg) # this command can be repeated if multiple + magModule.addSpacecraftToModel( + scObject.scStateOutMsg + ) # this command can be repeated if multiple scSim.AddModelToTask(simTaskName, magModule) # add magnetic torque bar effector @@ -359,7 +442,6 @@ def run(show_plots): scObject.addDynamicEffector(mtbEff) scSim.AddModelToTask(simTaskName, mtbEff) - # # setup the FSW algorithm tasks # @@ -368,7 +450,7 @@ def run(show_plots): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -383,25 +465,26 @@ def run(show_plots): mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.K = 0.0001 mrpControl.P = 0.002 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 - + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # create the minimal TAM module TAM = magnetometer.Magnetometer() TAM.ModelTag = "TAM_sensor" # specify the optional TAM variables TAM.scaleFactor = 1.0 - TAM.senNoiseStd = [0.0, 0.0, 0.0] + TAM.senNoiseStd = [0.0, 0.0, 0.0] scSim.AddModelToTask(simTaskName, TAM) # setup tamComm module tamCommObj = tamComm.tamComm() - tamCommObj.dcm_BS = [1., 0., 0., 0., 1., 0., 0., 0., 1.] + tamCommObj.dcm_BS = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] tamCommObj.ModelTag = "tamComm" scSim.AddModelToTask(simTaskName, tamCommObj) # setup mtbMomentumManagement module - mtbMomentumManagementSimpleObj = mtbMomentumManagementSimple.mtbMomentumManagementSimple() + mtbMomentumManagementSimpleObj = ( + mtbMomentumManagementSimple.mtbMomentumManagementSimple() + ) mtbMomentumManagementSimpleObj.Kp = 0.003 mtbMomentumManagementSimpleObj.ModelTag = "mtbMomentumManagementSimple" scSim.AddModelToTask(simTaskName, mtbMomentumManagementSimpleObj) @@ -416,22 +499,42 @@ def run(show_plots): mtbConfigParams.numMTB = 4 # row major toque bar alignments - mtbConfigParams.GtMatrix_B =[ - 1., 0., 0., 0.70710678, - 0., 1., 0., 0.70710678, - 0., 0., 1., 0.] + mtbConfigParams.GtMatrix_B = [ + 1.0, + 0.0, + 0.0, + 0.70710678, + 0.0, + 1.0, + 0.0, + 0.70710678, + 0.0, + 0.0, + 1.0, + 0.0, + ] maxDipole = 0.1 - mtbConfigParams.maxMtbDipoles = [maxDipole]*mtbConfigParams.numMTB + mtbConfigParams.maxMtbDipoles = [maxDipole] * mtbConfigParams.numMTB mtbParamsInMsg = messaging.MTBArrayConfigMsg().write(mtbConfigParams) # setup dipoleMapping module dipoleMappingObj = dipoleMapping.dipoleMapping() # row major toque bar alignment inverse - dipoleMappingObj.steeringMatrix = [0.75, -0.25, 0., - -0.25, 0.75, 0., - 0., 0., 1., - 0.35355339, 0.35355339, 0.] + dipoleMappingObj.steeringMatrix = [ + 0.75, + -0.25, + 0.0, + -0.25, + 0.75, + 0.0, + 0.0, + 0.0, + 1.0, + 0.35355339, + 0.35355339, + 0.0, + ] dipoleMappingObj.ModelTag = "dipoelMapping" scSim.AddModelToTask(simTaskName, dipoleMappingObj) @@ -446,9 +549,7 @@ def run(show_plots): scSim.AddModelToTask(simTaskName, rwMotorTorqueObj) # Make the RW control all three body axes - controlAxes_B = [ - 1, 0, 0, 0, 1, 0, 0, 0, 1 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] rwMotorTorqueObj.controlAxes_B = controlAxes_B # setup rwNullSpace module @@ -460,28 +561,34 @@ def run(show_plots): # setup RWConstellationMsgPayload rwConstellationConfig = messaging.RWConstellationMsgPayload( numRW=numRW, - reactionWheels = [ + reactionWheels=[ messaging.RWConfigElementMsgPayload( gsHat_B=Gs[:, i], Js=RW.Js, uMax=RW.u_max, ) for i, RW in enumerate((RW1, RW2, RW3, RW4)) - ] + ], + ) + rwConstellationConfigInMsg = messaging.RWConstellationMsg().write( + rwConstellationConfig ) - rwConstellationConfigInMsg = messaging.RWConstellationMsg().write(rwConstellationConfig) # setup RWSpeedMsgPayload for the rwNullSpace - desiredOmega = [1000 * macros.rpm2radsec]*numRW + desiredOmega = [1000 * macros.rpm2radsec] * numRW rwNullSpaceWheelSpeedBias = messaging.RWSpeedMsgPayload(wheelSpeeds=desiredOmega) - rwNullSpaceWheelSpeedBiasInMsg = messaging.RWSpeedMsg().write(rwNullSpaceWheelSpeedBias) + rwNullSpaceWheelSpeedBiasInMsg = messaging.RWSpeedMsg().write( + rwNullSpaceWheelSpeedBias + ) # # Setup data logging before the simulation is initialized # numDataPoints = 200 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) rwMotorLog = rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) magLog = magModule.envOutMsgs[0].recorder(samplingTime) @@ -526,12 +633,12 @@ def run(show_plots): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - oe.a = 6778.14 * 1000. # meters + oe.a = 6778.14 * 1000.0 # meters oe.e = 0.00 - oe.i = 45. * macros.D2R - oe.Omega = 60. * macros.D2R - oe.omega = 0. * macros.D2R - oe.f = 0. * macros.D2R + oe.i = 45.0 * macros.D2R + oe.Omega = 60.0 * macros.D2R + oe.omega = 0.0 * macros.D2R + oe.f = 0.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m/s - v_CN_N @@ -539,10 +646,13 @@ def run(show_plots): scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_CN_B # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - , rwEffectorList=rwStateEffector - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + rwEffectorList=rwStateEffector, + ) # link messages sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg) @@ -558,16 +668,24 @@ def run(show_plots): tamCommObj.tamInMsg.subscribeTo(TAM.tamDataOutMsg) mtbMomentumManagementSimpleObj.rwParamsInMsg.subscribeTo(fswRwParamMsg) - mtbMomentumManagementSimpleObj.rwSpeedsInMsg.subscribeTo(rwStateEffector.rwSpeedOutMsg) + mtbMomentumManagementSimpleObj.rwSpeedsInMsg.subscribeTo( + rwStateEffector.rwSpeedOutMsg + ) - torque2DipoleObj.tauRequestInMsg.subscribeTo(mtbMomentumManagementSimpleObj.tauMtbRequestOutMsg) + torque2DipoleObj.tauRequestInMsg.subscribeTo( + mtbMomentumManagementSimpleObj.tauMtbRequestOutMsg + ) torque2DipoleObj.tamSensorBodyInMsg.subscribeTo(tamCommObj.tamOutMsg) - dipoleMappingObj.dipoleRequestBodyInMsg.subscribeTo(torque2DipoleObj.dipoleRequestOutMsg) + dipoleMappingObj.dipoleRequestBodyInMsg.subscribeTo( + torque2DipoleObj.dipoleRequestOutMsg + ) dipoleMappingObj.mtbArrayConfigParamsInMsg.subscribeTo(mtbParamsInMsg) mtbFeedforwardObj.vehControlInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) - mtbFeedforwardObj.dipoleRequestMtbInMsg.subscribeTo(dipoleMappingObj.dipoleRequestMtbOutMsg) + mtbFeedforwardObj.dipoleRequestMtbInMsg.subscribeTo( + dipoleMappingObj.dipoleRequestMtbOutMsg + ) mtbFeedforwardObj.tamSensorBodyInMsg.subscribeTo(tamCommObj.tamOutMsg) mtbFeedforwardObj.mtbArrayConfigParamsInMsg.subscribeTo(mtbParamsInMsg) @@ -585,7 +703,6 @@ def run(show_plots): mtbEff.mtbParamsInMsg.subscribeTo(mtbParamsInMsg) mtbEff.magInMsg.subscribeTo(magModule.envOutMsgs[0]) - # initialize configure and execute sim scSim.InitializeSimulation() scSim.ConfigureStopTime(simulationTime) @@ -636,7 +753,9 @@ def run(show_plots): pltName = fileName + "6" figureList[pltName] = plt.figure(7) - plot_data_mtb_momentum_management(timeData, dataMtbDipoleCmds, mtbConfigParams.numMTB) + plot_data_mtb_momentum_management( + timeData, dataMtbDipoleCmds, mtbConfigParams.numMTB + ) pltName = fileName + "7" figureList[pltName] = plt.figure(8) diff --git a/examples/scenarioOrbitManeuver.py b/examples/scenarioOrbitManeuver.py index 00953d1308..4bea09b011 100755 --- a/examples/scenarioOrbitManeuver.py +++ b/examples/scenarioOrbitManeuver.py @@ -98,6 +98,7 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ @@ -106,7 +107,10 @@ from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -144,7 +148,7 @@ def run(show_plots, maneuverCase): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -169,7 +173,7 @@ def run(show_plots, maneuverCase): # setup spice library for Earth ephemeris timeInitString = "2024 September 21, 21:00:00.0 TDB" spiceObject = gravFactory.createSpiceInterface(time=timeInitString, epochInMsg=True) - spiceObject.zeroBase = 'Earth' + spiceObject.zeroBase = "Earth" # need spice to run before spacecraft module as it provides the spacecraft translational states scSim.AddModelToTask(simTaskName, spiceObject) @@ -179,8 +183,8 @@ def run(show_plots, maneuverCase): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rLEO = 7000. * 1000 # meters - rGEO = math.pow(earth.mu / math.pow((2. * np.pi) / (24. * 3600.), 2), 1. / 3.) + rLEO = 7000.0 * 1000 # meters + rGEO = math.pow(earth.mu / math.pow((2.0 * np.pi) / (24.0 * 3600.0), 2), 1.0 / 3.0) oe.a = rLEO oe.e = 0.0001 oe.i = 0.0 * macros.D2R @@ -193,29 +197,36 @@ def run(show_plots, maneuverCase): # set the simulation time n = np.sqrt(earth.mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n simulationTime = macros.sec2nano(0.25 * P) # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataRec = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataRec) if vizSupport.vizFound: # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - , oscOrbitColorList=[vizSupport.toRGBA255("yellow")] - , trueOrbitColorList=[vizSupport.toRGBA255("turquoise")] - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + oscOrbitColorList=[vizSupport.toRGBA255("yellow")], + trueOrbitColorList=[vizSupport.toRGBA255("turquoise")], + # , saveFile=fileName + ) viz.settings.mainCameraTarget = "earth" viz.settings.showCelestialBodyLabels = 1 viz.settings.showSpacecraftLabels = 1 viz.settings.truePathRelativeBody = "earth" - viz.settings.trueTrajectoryLinesOn = 3 # relative to celestial body inertial frame + viz.settings.trueTrajectoryLinesOn = ( + 3 # relative to celestial body inertial frame + ) # # initialize Simulation @@ -249,7 +260,7 @@ def run(show_plots, maneuverCase): hHat = hHat / np.linalg.norm(hHat) vHat = np.cross(hHat, rHat) v0 = np.dot(vHat, vVt) - vVt = vVt - (1. - np.cos(Delta_i)) * v0 * vHat + np.sin(Delta_i) * v0 * hHat + vVt = vVt - (1.0 - np.cos(Delta_i)) * v0 * vHat + np.sin(Delta_i) * v0 * hHat # After computing the maneuver specific Delta_v's, the state managers velocity is updated through velRef.setState(vVt) @@ -258,7 +269,7 @@ def run(show_plots, maneuverCase): # Hohmann Transfer to GEO v0 = np.linalg.norm(vVt) r0 = np.linalg.norm(rVt) - at = (r0 + rGEO) * .5 + at = (r0 + rGEO) * 0.5 v0p = np.sqrt(earth.mu / at * rGEO / r0) n1 = np.sqrt(earth.mu / at / at / at) T2 = macros.sec2nano((np.pi) / n1) @@ -285,7 +296,7 @@ def run(show_plots, maneuverCase): hHat = hHat / np.linalg.norm(hHat) vHat = np.cross(hHat, rHat) v0 = np.dot(vHat, vVt) - vVt = vVt - (1. - np.cos(Delta_i)) * v0 * vHat + np.sin(Delta_i) * v0 * hHat + vVt = vVt - (1.0 - np.cos(Delta_i)) * v0 * vHat + np.sin(Delta_i) * v0 * hHat velRef.setState(vVt) T3 = macros.sec2nano(P * 0.25) else: @@ -321,14 +332,17 @@ def run(show_plots, maneuverCase): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") for idx in range(3): - plt.plot(dataRec.times() * macros.NANO2HOUR, posData[:, idx] / 1000., - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [h]') - plt.ylabel('Inertial Position [km]') + plt.plot( + dataRec.times() * macros.NANO2HOUR, + posData[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [h]") + plt.ylabel("Inertial Position [km]") figureList = {} pltName = fileName + "1" + str(int(maneuverCase)) figureList[pltName] = plt.figure(1) @@ -338,33 +352,39 @@ def run(show_plots, maneuverCase): plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") iData = [] for idx in range(0, len(posData)): oeData = orbitalMotion.rv2elem(earth.mu, posData[idx], velData[idx]) iData.append(oeData.i * macros.R2D) - plt.plot(dataRec.times() * macros.NANO2HOUR, np.ones(len(posData[:, 0])) * 8.93845, '--', color='#444444' - ) - plt.plot(dataRec.times() * macros.NANO2HOUR, iData, color='#aa0000' - ) + plt.plot( + dataRec.times() * macros.NANO2HOUR, + np.ones(len(posData[:, 0])) * 8.93845, + "--", + color="#444444", + ) + plt.plot(dataRec.times() * macros.NANO2HOUR, iData, color="#aa0000") plt.ylim([-1, 10]) - plt.xlabel('Time [h]') - plt.ylabel('Inclination [deg]') + plt.xlabel("Time [h]") + plt.ylabel("Inclination [deg]") else: # show SMA plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") rData = [] for idx in range(0, len(posData)): oeData = orbitalMotion.rv2elem_parab(earth.mu, posData[idx], velData[idx]) - rData.append(oeData.rmag / 1000.) - plt.plot(dataRec.times() * macros.NANO2HOUR, rData, color='#aa0000', - ) - plt.xlabel('Time [h]') - plt.ylabel('Radius [km]') + rData.append(oeData.rmag / 1000.0) + plt.plot( + dataRec.times() * macros.NANO2HOUR, + rData, + color="#aa0000", + ) + plt.xlabel("Time [h]") + plt.ylabel("Radius [km]") pltName = fileName + "2" + str(int(maneuverCase)) figureList[pltName] = plt.figure(2) @@ -374,7 +394,6 @@ def run(show_plots, maneuverCase): # close the plots being saved off to avoid over-writing old and new figures plt.close("all") - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found dataPos = posRef.getState() @@ -389,5 +408,5 @@ def run(show_plots, maneuverCase): if __name__ == "__main__": run( True, # show_plots - 0 # Maneuver Case (0 - Hohmann, 1 - Inclination) + 0, # Maneuver Case (0 - Hohmann, 1 - Inclination) ) diff --git a/examples/scenarioOrbitManeuverTH.py b/examples/scenarioOrbitManeuverTH.py index fc3d5de1b5..f899e72738 100644 --- a/examples/scenarioOrbitManeuverTH.py +++ b/examples/scenarioOrbitManeuverTH.py @@ -82,28 +82,36 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging + # import FSW Algorithm related support from Basilisk.fswAlgorithms import attTrackingError from Basilisk.fswAlgorithms import mrpFeedback from Basilisk.fswAlgorithms import velocityPoint from Basilisk.simulation import extForceTorque from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft from Basilisk.simulation import svIntegrators from Basilisk.simulation import thrusterStateEffector + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody from Basilisk.utilities import simIncludeThruster -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -143,9 +151,7 @@ def run(show_plots): # initialize spacecraft object and set properties scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) @@ -167,8 +173,8 @@ def run(show_plots): # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rLEO = 7000. * 1000 # meters - rGEO = math.pow(mu / math.pow((2. * np.pi) / (24. * 3600.), 2), 1. / 3.) + rLEO = 7000.0 * 1000 # meters + rGEO = math.pow(mu / math.pow((2.0 * np.pi) / (24.0 * 3600.0), 2), 1.0 / 3.0) oe.a = rLEO oe.e = 0.0001 oe.i = 0.0 * macros.D2R @@ -183,7 +189,7 @@ def run(show_plots): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n simulationTime = macros.sec2nano(0.25 * P) # setup extForceTorque module @@ -216,12 +222,26 @@ def run(show_plots): direction = [[0, 0, 1]] # create the thruster devices by specifying the thruster type and its location and direction for pos_B, dir_B in zip(location, direction): - thFactory.create('Blank_Thruster', pos_B, dir_B, cutoffFrequency=.1, MaxThrust=3000.0, - areaNozzle=0.046759, steadyIsp=318.0) + thFactory.create( + "Blank_Thruster", + pos_B, + dir_B, + cutoffFrequency=0.1, + MaxThrust=3000.0, + areaNozzle=0.046759, + steadyIsp=318.0, + ) for pos_B, dir_B in zip(location, direction): - thFactory.create('Blank_Thruster', pos_B, dir_B, cutoffFrequency=.1, MaxThrust=420.0, - areaNozzle=0.046759, steadyIsp=318.0) + thFactory.create( + "Blank_Thruster", + pos_B, + dir_B, + cutoffFrequency=0.1, + MaxThrust=420.0, + areaNozzle=0.046759, + steadyIsp=318.0, + ) # create thruster object container and tie to spacecraft object thrModelTag = "GTOThrusterDynamics" @@ -247,7 +267,7 @@ def run(show_plots): attError = attTrackingError.attTrackingError() attError.ModelTag = "attErrorVelocityPoint" scSim.AddModelToTask(simTaskName, attError) - attError.sigma_R0R = [np.tan(np.pi/8), 0, 0] + attError.sigma_R0R = [np.tan(np.pi / 8), 0, 0] attError.attRefInMsg.subscribeTo(attGuidance.attRefOutMsg) attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg) @@ -259,7 +279,7 @@ def run(show_plots): mrpControl.K = 3.5 mrpControl.Ki = -1.0 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # connect torque command to external torque effector extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) @@ -268,7 +288,9 @@ def run(show_plots): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) mrpLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) attErrLog = attError.attGuidOutMsg.recorder(samplingTime) snAttLog = sNavObject.attOutMsg.recorder(samplingTime) @@ -297,14 +319,16 @@ def run(show_plots): mrpControl.vehConfigInMsg.subscribeTo(vcMsg) thrusterSet.cmdsInMsg.subscribeTo(thrOnTimeMsg) - # if this scenario is to interface with the BSK Viz, uncomment the following lines if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - , thrEffectorList=thrusterSet - , thrColors=vizSupport.toRGBA255("red") - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + thrEffectorList=thrusterSet, + thrColors=vizSupport.toRGBA255("red"), + ) vizSupport.setActuatorGuiSetting(viz, showThrusterLabels=True) # @@ -334,7 +358,7 @@ def run(show_plots): # Hohmann Transfer to GEO v0 = np.linalg.norm(vVt) r0 = np.linalg.norm(rVt) - at = (r0 + rGEO) * .5 + at = (r0 + rGEO) * 0.5 v0p = np.sqrt(mu / at * rGEO / r0) dv0 = v0p - v0 @@ -360,8 +384,8 @@ def run(show_plots): oe1 = orbitalMotion.rv2elem(mu, rVt, vVt) n1 = np.sqrt(mu / oe1.a / oe1.a / oe1.a) T3Full = (np.pi) / n1 - E1 = 2*np.arctan(np.sqrt((1-oe1.e)/(1+oe1.e))*np.tan(oe1.f/2)) - T3P = np.sqrt((oe1.a * oe1.a * oe1.a)/mu)*(E1-oe1.e*np.sin(E1)) + E1 = 2 * np.arctan(np.sqrt((1 - oe1.e) / (1 + oe1.e)) * np.tan(oe1.f / 2)) + T3P = np.sqrt((oe1.a * oe1.a * oe1.a) / mu) * (E1 - oe1.e * np.sin(E1)) T3 = macros.sec2nano(T3Full - T3P) # Run the simulation for until apogee is reached @@ -391,7 +415,7 @@ def run(show_plots): # Run the simulation for second burn and quarter orbit after T5 = macros.sec2nano(0.25 * (np.pi) / n2) - scSim.ConfigureStopTime(simulationTime + T2 + T3 + T4 +T5) + scSim.ConfigureStopTime(simulationTime + T2 + T3 + T4 + T5) scSim.ExecuteSimulation() # @@ -410,14 +434,17 @@ def run(show_plots): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") for idx in range(3): - plt.plot(dataRec.times() * macros.NANO2HOUR, posData[:, idx] / 1000., - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [h]') - plt.ylabel('Inertial Position [km]') + plt.plot( + dataRec.times() * macros.NANO2HOUR, + posData[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [h]") + plt.ylabel("Inertial Position [km]") figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) @@ -426,27 +453,39 @@ def run(show_plots): plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") rData = [] for idx in range(0, len(posData)): oeData = orbitalMotion.rv2elem_parab(mu, posData[idx], velData[idx]) - rData.append(oeData.rmag / 1000.) - plt.plot(dataRec.times() * macros.NANO2HOUR, rData, color='#aa0000', - ) - plt.xlabel('Time [h]') - plt.ylabel('Radius [km]') + rData.append(oeData.rmag / 1000.0) + plt.plot( + dataRec.times() * macros.NANO2HOUR, + rData, + color="#aa0000", + ) + plt.xlabel("Time [h]") + plt.ylabel("Radius [km]") pltName = fileName + "2" figureList[pltName] = plt.figure(2) plt.figure(3) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') - plt.plot(thrCmdRec0.times()*macros.NANO2MIN/60, thrCmdRec0.thrustForce, color='#aa0000', label='Thruster 1') - plt.plot(thrCmdRec1.times()*macros.NANO2MIN/60, thrCmdRec1.thrustForce, label='Thruster 2') - plt.legend(loc='upper right') - plt.xlabel('Time [h]') - plt.ylabel('Force implemented[N]') + ax.ticklabel_format(useOffset=False, style="plain") + plt.plot( + thrCmdRec0.times() * macros.NANO2MIN / 60, + thrCmdRec0.thrustForce, + color="#aa0000", + label="Thruster 1", + ) + plt.plot( + thrCmdRec1.times() * macros.NANO2MIN / 60, + thrCmdRec1.thrustForce, + label="Thruster 2", + ) + plt.legend(loc="upper right") + plt.xlabel("Time [h]") + plt.ylabel("Force implemented[N]") pltName = fileName + "3" figureList[pltName] = plt.figure(3) @@ -458,6 +497,7 @@ def run(show_plots): return figureList + # # This statement below ensures that the unit test scrip can be run as a # stand-along python script diff --git a/examples/scenarioOrbitMultiBody.py b/examples/scenarioOrbitMultiBody.py index 9a7a8ed7b9..dcc5d81d8d 100755 --- a/examples/scenarioOrbitMultiBody.py +++ b/examples/scenarioOrbitMultiBody.py @@ -102,17 +102,23 @@ import matplotlib.pyplot as plt import numpy as np from Basilisk import __path__ + # import simulation related support from Basilisk.simulation import spacecraft + # Used to get the location of supporting data. from Basilisk.topLevelModules import pyswice + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.architecture import astroConstants + # attempt to import vizard from Basilisk.utilities import vizSupport from Basilisk.utilities.pyswice_spk_utilities import spkRead @@ -152,7 +158,7 @@ def run(show_plots, scCase): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(5.) + simulationTimeStep = macros.sec2nano(5.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -181,12 +187,16 @@ def run(show_plots, scCase): # Next a series of gravitational bodies are included. Note that it is convenient to include them as a # list of SPICE names. The Earth is included in this scenario with the # spherical harmonics turned on. Note that this is true for both spacecraft simulations. - gravBodies = gravFactory.createBodies('earth', 'mars barycenter', 'sun', 'moon', "jupiter barycenter") - gravBodies['earth'].isCentralBody = True + gravBodies = gravFactory.createBodies( + "earth", "mars barycenter", "sun", "moon", "jupiter barycenter" + ) + gravBodies["earth"].isCentralBody = True # Other possible ways to access specific gravity bodies include the below # earth = gravBodies['earth'] # earth = gravFactory.createEarth() - gravBodies['earth'].useSphericalHarmonicsGravityModel(bskPath + '/supportData/LocalGravData/GGM03S.txt', 100) + gravBodies["earth"].useSphericalHarmonicsGravityModel( + bskPath + "/supportData/LocalGravData/GGM03S.txt", 100 + ) # The configured gravitational bodies are added to the spacecraft dynamics with the usual command: gravFactory.addBodiesTo(scObject) @@ -194,7 +204,7 @@ def run(show_plots, scCase): # Next, the default SPICE support module is created and configured. The first step is to store # the date and time of the start of the simulation. timeInitString = "2012 MAY 1 00:28:30.0" - spiceTimeStringFormat = '%Y %B %d %H:%M:%S.%f' + spiceTimeStringFormat = "%Y %B %d %H:%M:%S.%f" timeInit = datetime.strptime(timeInitString, spiceTimeStringFormat) # The following is a support macro that creates a `spiceObject` instance, and fills in typical @@ -212,7 +222,7 @@ def run(show_plots, scCase): # or SSB for short. The spacecraft() state output message is relative to this SBB frame by default. To change # this behavior, the zero based point must be redefined from SBB to another body. # In this simulation we use the Earth. - spiceObject.zeroBase = 'Earth' + spiceObject.zeroBase = "Earth" # Finally, the SPICE object is added to the simulation task list. scSim.AddModelToTask(simTaskName, spiceObject) @@ -226,23 +236,29 @@ def run(show_plots, scCase): # separate from the earlier SPICE setup that was loaded to BSK. This is why # all required SPICE libraries must be included when setting up and loading # SPICE kernals in Python. - if scCase == 'NewHorizons': - scEphemerisFileName = 'nh_pred_od077.bsp' - scSpiceName = 'NEW HORIZONS' + if scCase == "NewHorizons": + scEphemerisFileName = "nh_pred_od077.bsp" + scSpiceName = "NEW HORIZONS" else: # default case - scEphemerisFileName = 'hst_edited.bsp' - scSpiceName = 'HUBBLE SPACE TELESCOPE' - pyswice.furnsh_c(spiceObject.SPICEDataPath + scEphemerisFileName) # Hubble Space Telescope data - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'de430.bsp') # solar system bodies - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'naif0012.tls') # leap second file - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'de-403-masses.tpc') # solar system masses - pyswice.furnsh_c(spiceObject.SPICEDataPath + 'pck00010.tpc') # generic Planetary Constants Kernel + scEphemerisFileName = "hst_edited.bsp" + scSpiceName = "HUBBLE SPACE TELESCOPE" + pyswice.furnsh_c( + spiceObject.SPICEDataPath + scEphemerisFileName + ) # Hubble Space Telescope data + pyswice.furnsh_c(spiceObject.SPICEDataPath + "de430.bsp") # solar system bodies + pyswice.furnsh_c(spiceObject.SPICEDataPath + "naif0012.tls") # leap second file + pyswice.furnsh_c( + spiceObject.SPICEDataPath + "de-403-masses.tpc" + ) # solar system masses + pyswice.furnsh_c( + spiceObject.SPICEDataPath + "pck00010.tpc" + ) # generic Planetary Constants Kernel # # Setup spacecraft initial states # # The initial spacecraft position and velocity vector is obtained via the SPICE function call: - scInitialState = 1000 * spkRead(scSpiceName, timeInitString, 'J2000', 'EARTH') + scInitialState = 1000 * spkRead(scSpiceName, timeInitString, "J2000", "EARTH") rN = scInitialState[0:3] # meters vN = scInitialState[3:6] # m/s @@ -262,20 +278,25 @@ def run(show_plots, scCase): # # Setup simulation time # - simulationTime = macros.sec2nano(2000.) + simulationTime = macros.sec2nano(2000.0) # # Setup data logging before the simulation is initialized # numDataPoints = 50 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataRec = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataRec) # if this scenario is to interface with the BSK Unity Viz, uncomment the following lines - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation @@ -304,35 +325,38 @@ def run(show_plots, scCase): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') - ax.yaxis.set_major_formatter(matplotlib.ticker.StrMethodFormatter('{x:,.0f}')) - if scCase == 'NewHorizons': - axesScale = astroConstants.AU * 1000. # convert to AU - axesLabel = '[AU]' + ax.ticklabel_format(useOffset=False, style="plain") + ax.yaxis.set_major_formatter(matplotlib.ticker.StrMethodFormatter("{x:,.0f}")) + if scCase == "NewHorizons": + axesScale = astroConstants.AU * 1000.0 # convert to AU + axesLabel = "[AU]" timeScale = macros.NANO2MIN # convert to minutes - timeLabel = '[min]' + timeLabel = "[min]" else: - axesScale = 1000. # convert to km - axesLabel = '[km]' + axesScale = 1000.0 # convert to km + axesLabel = "[km]" timeScale = macros.NANO2MIN # convert to minutes - timeLabel = '[min]' + timeLabel = "[min]" for idx in range(3): - plt.plot(timeAxis * timeScale, posData[:, idx] / axesScale, - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time ' + timeLabel) - plt.ylabel('Inertial Position ' + axesLabel) + plt.plot( + timeAxis * timeScale, + posData[:, idx] / axesScale, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time " + timeLabel) + plt.ylabel("Inertial Position " + axesLabel) figureList = {} pltName = fileName + "1" + scCase figureList[pltName] = plt.figure(1) rBSK = posData[-1] # store the last position to compare to the SPICE position - if scCase == 'Hubble': + if scCase == "Hubble": # # draw orbit in perifocal frame # - oeData = orbitalMotion.rv2elem(gravBodies['earth'].mu, rN, vN) + oeData = orbitalMotion.rv2elem(gravBodies["earth"].mu, rN, vN) omega0 = oeData.omega b = oeData.a * np.sqrt(1 - oeData.e * oeData.e) p = oeData.a * (1 - oeData.e * oeData.e) @@ -342,23 +366,27 @@ def run(show_plots, scCase): # draw the planet fig = plt.gcf() ax = fig.gca() - planetColor = '#008800' - planetRadius = gravBodies['earth'].radEquator / 1000 + planetColor = "#008800" + planetRadius = gravBodies["earth"].radEquator / 1000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # draw the actual orbit rData = [] fData = [] for idx in range(0, len(posData)): - oeData = orbitalMotion.rv2elem(gravBodies['earth'].mu, posData[idx], velData[idx]) + oeData = orbitalMotion.rv2elem( + gravBodies["earth"].mu, posData[idx], velData[idx] + ) rData.append(oeData.rmag) fData.append(oeData.f + oeData.omega - omega0) - plt.plot(rData * np.cos(fData) / 1000, rData * np.sin(fData) / 1000 - , color='#aa0000' - , linewidth=0.5 - , label='Basilisk' - ) - plt.legend(loc='lower right') + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + color="#aa0000", + linewidth=0.5, + label="Basilisk", + ) + plt.legend(loc="lower right") # draw the full SPICE orbit rData = [] @@ -370,38 +398,39 @@ def run(show_plots, scCase): usec = (simTime - sec) * 1000000 time = timeInit + timedelta(seconds=sec, microseconds=usec) timeString = time.strftime(spiceTimeStringFormat) - scState = 1000.0 * spkRead(scSpiceName, timeString, 'J2000', 'EARTH') + scState = 1000.0 * spkRead(scSpiceName, timeString, "J2000", "EARTH") rN = scState[0:3] # meters vN = scState[3:6] # m/s - oeData = orbitalMotion.rv2elem(gravBodies['earth'].mu, rN, vN) + oeData = orbitalMotion.rv2elem(gravBodies["earth"].mu, rN, vN) rData.append(oeData.rmag) fData.append(oeData.f + oeData.omega - omega0) rTrue = rN # store the last position to compare to the BSK position - plt.plot(rData * np.cos(fData) / 1000, rData * np.sin(fData) / 1000 - , '--' - , color='#555555' - , linewidth=1.0 - , label='Spice' - ) - plt.legend(loc='lower right') - plt.xlabel('$i_e$ Cord. [km]') - plt.ylabel('$i_p$ Cord. [km]') + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + "--", + color="#555555", + linewidth=1.0, + label="Spice", + ) + plt.legend(loc="lower right") + plt.xlabel("$i_e$ Cord. [km]") + plt.ylabel("$i_p$ Cord. [km]") plt.grid() pltName = fileName + "2" + scCase figureList[pltName] = plt.figure(2) else: - scState = 1000.0 * spkRead(scSpiceName, - spiceObject.getCurrentTimeString(), - 'J2000', - 'EARTH') + scState = 1000.0 * spkRead( + scSpiceName, spiceObject.getCurrentTimeString(), "J2000", "EARTH" + ) rTrue = scState[0:3] # plot the differences between BSK and SPICE position data plt.figure(3) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") # ax.yaxis.set_major_formatter(matplotlib.ticker.StrMethodFormatter('{x:,.0f}')) posError = [] for idx in range(len(timeAxis)): @@ -410,15 +439,18 @@ def run(show_plots, scCase): usec = (simTime - sec) * 1000000 time = timeInit + timedelta(seconds=sec, microseconds=usec) timeString = time.strftime(spiceTimeStringFormat) - scState = 1000 * spkRead(scSpiceName, timeString, 'J2000', 'EARTH') + scState = 1000 * spkRead(scSpiceName, timeString, "J2000", "EARTH") posError.append(posData[idx] - np.array(scState[0:3])) # meters for idx in range(3): - plt.plot(dataRec.times() * macros.NANO2MIN, np.array(posError)[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\Delta r_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Inertial Position Differences [m]') + plt.plot( + dataRec.times() * macros.NANO2MIN, + np.array(posError)[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\Delta r_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Inertial Position Differences [m]") pltName = fileName + "3" + scCase figureList[pltName] = plt.figure(3) @@ -433,10 +465,14 @@ def run(show_plots, scCase): # gravFactory.unloadSpiceKernels() pyswice.unload_c(scEphemerisFileName) - pyswice.unload_c(spiceObject.SPICEDataPath + 'de430.bsp') # solar system bodies - pyswice.unload_c(spiceObject.SPICEDataPath + 'naif0012.tls') # leap second file - pyswice.unload_c(spiceObject.SPICEDataPath + 'de-403-masses.tpc') # solar system masses - pyswice.unload_c(spiceObject.SPICEDataPath + 'pck00010.tpc') # generic Planetary Constants Kernel + pyswice.unload_c(spiceObject.SPICEDataPath + "de430.bsp") # solar system bodies + pyswice.unload_c(spiceObject.SPICEDataPath + "naif0012.tls") # leap second file + pyswice.unload_c( + spiceObject.SPICEDataPath + "de-403-masses.tpc" + ) # solar system masses + pyswice.unload_c( + spiceObject.SPICEDataPath + "pck00010.tpc" + ) # generic Planetary Constants Kernel # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found @@ -450,5 +486,5 @@ def run(show_plots, scCase): if __name__ == "__main__": run( True, # show_plots - 'Hubble' # 'Hubble' or 'NewHorizons' + "Hubble", # 'Hubble' or 'NewHorizons' ) diff --git a/examples/scenarioPatchedConics.py b/examples/scenarioPatchedConics.py index e1c0d376c5..d91e1efe08 100644 --- a/examples/scenarioPatchedConics.py +++ b/examples/scenarioPatchedConics.py @@ -104,10 +104,17 @@ bskPath = __path__[0] from Basilisk.simulation import spacecraft, gravityEffector -from Basilisk.utilities import SimulationBaseClass, macros, orbitalMotion, simIncludeGravBody, unitTestSupport +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, +) from Basilisk.architecture import messaging from Basilisk.utilities import vizSupport + def run(show_plots): """ The scenarios can be run with the followings setups parameters: @@ -125,7 +132,7 @@ def run(show_plots): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(5.) + simulationTimeStep = macros.sec2nano(5.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) @@ -158,28 +165,28 @@ def run(show_plots): # and to discount Jupiter's velocity upon # entering Jupiter's Sphere of Influence. This ensures the spacecraft has the correct heliocentric and relative # positions and velocities even when the planets are not moving. - rEarth = 149598023. * 1000 - rJupiter = 778298361. * 1000 + rEarth = 149598023.0 * 1000 + rJupiter = 778298361.0 * 1000 earthStateMsg = messaging.SpicePlanetStateMsgPayload( PositionVector=[0.0, -rEarth, 0.0], VelocityVector=[0.0, 0.0, 0.0], ) earthMsg = messaging.SpicePlanetStateMsg().write(earthStateMsg) - gravFactory.gravBodies['earth'].planetBodyInMsg.subscribeTo(earthMsg) + gravFactory.gravBodies["earth"].planetBodyInMsg.subscribeTo(earthMsg) jupiterStateMsg = messaging.SpicePlanetStateMsgPayload( PositionVector=[0.0, rJupiter, 0.0], VelocityVector=[0.0, 0.0, 0.0], ) jupiterMsg = messaging.SpicePlanetStateMsg().write(jupiterStateMsg) - gravFactory.gravBodies['jupiter barycenter'].planetBodyInMsg.subscribeTo(jupiterMsg) + gravFactory.gravBodies["jupiter barycenter"].planetBodyInMsg.subscribeTo(jupiterMsg) sunStateMsg = messaging.SpicePlanetStateMsgPayload( PositionVector=[0.0, 0.0, 0.0], VelocityVector=[0.0, 0.0, 0.0], ) sunMsg = messaging.SpicePlanetStateMsg().write(sunStateMsg) - gravFactory.gravBodies['sun'].planetBodyInMsg.subscribeTo(sunMsg) + gravFactory.gravBodies["sun"].planetBodyInMsg.subscribeTo(sunMsg) # Earth Centered Circular orbit and hyperbolic departure # initialize spacecraft object and set properties @@ -187,7 +194,7 @@ def run(show_plots): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rLEO = 7000. * 1000 # meters + rLEO = 7000.0 * 1000 # meters oe.a = rLEO oe.e = 0.0 oe.i = 0.0 * macros.D2R @@ -195,22 +202,30 @@ def run(show_plots): oe.omega = 0.0 * macros.D2R oe.f = 0.0 * macros.D2R r_E, v_E = orbitalMotion.elem2rv(earth.mu, oe) - oe = orbitalMotion.rv2elem(earth.mu, r_E, v_E) # this stores consistent initial orbit elements + oe = orbitalMotion.rv2elem( + earth.mu, r_E, v_E + ) # this stores consistent initial orbit elements vel_N_Earth = [0.0 * 1000, 0, 0] # Hohmann transfer calculations - at = (rEarth + rJupiter) * .5 + at = (rEarth + rJupiter) * 0.5 vPt = np.sqrt(2 * sun.mu / rEarth - sun.mu / at) vAt = np.sqrt(2 * sun.mu / rJupiter - sun.mu / at) n1 = np.sqrt(sun.mu / at / at / at) T2 = macros.sec2nano((np.pi) / n1) n = np.sqrt(earth.mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n v_Earth = 29.7859 * 1000 # speed of the earth v_Earth_rel = vPt - v_Earth # Required earth relative velocity - aHyp = - earth.mu / (v_Earth_rel * v_Earth_rel) # Semimajor axis of departure hyperbola - eHyp = rLEO * v_Earth_rel * v_Earth_rel / earth.mu + 1 # Eccentricity of hyperbolic departure orbit - v0 = np.sqrt(v_Earth_rel * v_Earth_rel + 2 * earth.mu / rLEO) # Earth relative speed s/c needs post burn + aHyp = -earth.mu / ( + v_Earth_rel * v_Earth_rel + ) # Semimajor axis of departure hyperbola + eHyp = ( + rLEO * v_Earth_rel * v_Earth_rel / earth.mu + 1 + ) # Eccentricity of hyperbolic departure orbit + v0 = np.sqrt( + v_Earth_rel * v_Earth_rel + 2 * earth.mu / rLEO + ) # Earth relative speed s/c needs post burn v_c = np.sqrt(earth.mu / rLEO) deltaV1 = v0 - v_c phi = np.arccos(1 / eHyp) + np.pi # Burn angle @@ -228,13 +243,18 @@ def run(show_plots): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=__file__ - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=__file__ + ) # initialize Simulation: This function clears the simulation log, and runs the self_init() # cross_init() and reset() routines on each module. # If the routine InitializeSimulationAndDiscover() is run instead of InitializeSimulation(), @@ -312,7 +332,12 @@ def run(show_plots): # we break up the integration into chunks of less than 100 days steps = 20 for i in range(steps): - stopTime = simulationTime + macros.sec2nano(110000) + T2*(i+1)/steps - oneWeek*1 + stopTime = ( + simulationTime + + macros.sec2nano(110000) + + T2 * (i + 1) / steps + - oneWeek * 1 + ) scSim.ConfigureStopTime(stopTime) scSim.ExecuteSimulation() @@ -335,10 +360,10 @@ def run(show_plots): # the simulation is propagated until it reaches Jupiter's SOI. Similar to the # Interplanetary section, the position and # velocity states are pulled and manipulated to be Jupiter-centric and then fed back to the simulation. - simulationTimeStep = macros.sec2nano(500.) + simulationTimeStep = macros.sec2nano(500.0) dynProcess.updateTaskPeriod(simTaskName, simulationTimeStep) - dataLog.updateTimeInterval(macros.sec2nano(20*60)) - stopTime = simulationTime + macros.sec2nano(110000) + T2 - oneWeek*0.5 + dataLog.updateTimeInterval(macros.sec2nano(20 * 60)) + stopTime = simulationTime + macros.sec2nano(110000) + T2 - oneWeek * 0.5 scSim.ConfigureStopTime(stopTime) scSim.ExecuteSimulation() @@ -384,7 +409,7 @@ def run(show_plots): # Setup data logging before the simulation is initialized # scSim.TotalSim.logThisMessage(scObject.scStateOutMsgName, simulationTimeStep) - stopTime = simulationTime + macros.sec2nano(110000) + T2 + oneWeek*6 + stopTime = simulationTime + macros.sec2nano(110000) + T2 + oneWeek * 6 scSim.ConfigureStopTime(stopTime) scSim.ExecuteSimulation() @@ -403,7 +428,7 @@ def run(show_plots): # Earth Centered Departure pos_S_EC = [] vel_S_EC = [] - for idx in range (0, endEarthTime): + for idx in range(0, endEarthTime): r_S_E = posData[idx] - pos_N_Earth v_S_E = velData[idx] - vel_N_Earth @@ -429,14 +454,14 @@ def run(show_plots): plt.close("all") # clears out plots from earlier test runs b = oe.a * np.sqrt(1 - oe.e * oe.e) p = oe.a * (1 - oe.e * oe.e) - plt.figure(1) #, figsize=tuple(np.array((1.0, b / oe.a)) * 4.75), dpi=100) + plt.figure(1) # , figsize=tuple(np.array((1.0, b / oe.a)) * 4.75), dpi=100) # plt.axis(np.array([-oe.rApoap, oe.rPeriap, -b, b]) / 1000 * 1.25) - plt.axis('equal') + plt.axis("equal") plt.axis([-20000, 50000, -10000, 10000]) # draw the planet fig = plt.gcf() ax = fig.gca() - planetColor = '#008800' + planetColor = "#008800" planetRadius = earth.radEquator / 1000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # draw the actual orbit @@ -447,21 +472,36 @@ def run(show_plots): 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, - label='Simulated Flight') + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + color="#aa0000", + linewidth=3.0, + label="Simulated Flight", + ) # 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]))) ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - ax.get_xaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - plt.plot(rData * np.cos(fData) / 1000, rData * np.sin(fData) / 1000, '--', color='#555555', label='Orbit Track') - plt.xlabel('Earth velocity direction [km]') - plt.ylabel('Sunward direction [km]') - plt.legend(loc='upper right') + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + ax.get_xaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + "--", + color="#555555", + label="Orbit Track", + ) + plt.xlabel("Earth velocity direction [km]") + plt.ylabel("Sunward direction [km]") + plt.legend(loc="upper right") plt.grid() figureList = {} pltName = fileName + "1" @@ -470,73 +510,101 @@ def run(show_plots): plt.figure(2) fig = plt.gcf() ax = fig.gca() - plt.axis([60, 95, 0 , 25000]) - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - ax.get_xaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) + plt.axis([60, 95, 0, 25000]) + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + ax.get_xaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) rData = [] for idx in range(0, len(posData[0:300])): oeData = orbitalMotion.rv2elem_parab(earth.mu, pos_S_EC[idx], vel_S_EC[idx]) - rData.append(oeData.rmag / 1000.) - plt.plot(timeAxis[0:300] * macros.NANO2MIN, rData, color='#aa0000', - ) - plt.xlabel('Time [min]') - plt.ylabel('Earth Relative Radius [km]') + rData.append(oeData.rmag / 1000.0) + plt.plot( + timeAxis[0:300] * macros.NANO2MIN, + rData, + color="#aa0000", + ) + plt.xlabel("Time [min]") + plt.ylabel("Earth Relative Radius [km]") pltName = fileName + "2" figureList[pltName] = plt.figure(2) - fig = plt.figure(3) ax = fig.gca() - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - ax.get_xaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - planetColor = '#008800' + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + ax.get_xaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + planetColor = "#008800" planetRadius = sun.radEquator / 1000 / 149598000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # draw the actual orbit rData = [] fData = [] for idx in range(0, len(hohmann_PosData)): - oeData = orbitalMotion.rv2elem_parab(sun.mu, hohmann_PosData[idx], hohmann_VelData[idx]) - rData.append(oeData.rmag / 1000.) + oeData = orbitalMotion.rv2elem_parab( + sun.mu, hohmann_PosData[idx], hohmann_VelData[idx] + ) + rData.append(oeData.rmag / 1000.0) fData.append(oeData.f + oeData.omega - oeData.omega) rData = np.array(rData) / 149598000 - plt.plot(rData[endEarthTime:-1] * np.cos(fData[endEarthTime:-1]), - rData[endEarthTime: -1] * np.sin(fData[endEarthTime: -1]), color='#008800', linewidth=3.0, - label='Simulated Flight') - plt.legend(loc='lower left') + plt.plot( + rData[endEarthTime:-1] * np.cos(fData[endEarthTime:-1]), + rData[endEarthTime:-1] * np.sin(fData[endEarthTime:-1]), + color="#008800", + linewidth=3.0, + label="Simulated Flight", + ) + plt.legend(loc="lower left") plt.grid() - plt.xlabel('Y axis - Sunward Direction [AU]') - plt.ylabel('X axis - Velocity Direction [AU]') + plt.xlabel("Y axis - Sunward Direction [AU]") + plt.ylabel("X axis - Velocity Direction [AU]") pltName = fileName + "3" figureList[pltName] = plt.figure(3) - - plt.figure(4,figsize=(5,5)) + plt.figure(4, figsize=(5, 5)) plt.axis([-20000000, 20000000, -20000000, 20000000]) # draw the planet fig = plt.gcf() ax = fig.gca() - ax.set_aspect('equal') - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - ax.get_xaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - planetColor = '#008800' + ax.set_aspect("equal") + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + ax.get_xaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + planetColor = "#008800" planetRadius = jupiter.radEquator / 1000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # draw actual orbit - plt.plot(pos_S_JC[:,0] / 1000., pos_S_JC[:,1] / 1000., color='orangered', label='Simulated Flight') - plt.xlabel('Jupiter velocity direction [km]') - plt.ylabel('Anti-Sunward direction [km]') + plt.plot( + pos_S_JC[:, 0] / 1000.0, + pos_S_JC[:, 1] / 1000.0, + color="orangered", + label="Simulated Flight", + ) + plt.xlabel("Jupiter velocity direction [km]") + plt.ylabel("Anti-Sunward direction [km]") pltName = fileName + "4" figureList[pltName] = plt.figure(4) plt.figure(5) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) - ax.get_xaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) + ax.get_xaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) dataTime_EC = [] rData_EC = [] @@ -549,51 +617,65 @@ def run(show_plots): # To speed up the simulation, only certain data points are recorded for plotting for idx in range(0, len(dataPos)): - if idx <= endEarthTime: dataTime_EC.append(timeAxis[idx]) oeData = orbitalMotion.rv2elem_parab(sun.mu, dataPos[idx], dataVel[idx]) - rData_EC.append(oeData.rmag / 1000.) + rData_EC.append(oeData.rmag / 1000.0) - elif (endEarthTime < idx <= endSunTime): - if idx % 2 == 0: # Records every 2nd data point + elif endEarthTime < idx <= endSunTime: + if idx % 2 == 0: # Records every 2nd data point dataTime_HC.append(timeAxis[idx]) oeData = orbitalMotion.rv2elem_parab(sun.mu, dataPos[idx], dataVel[idx]) - rData_HC.append(oeData.rmag / 1000.) - + rData_HC.append(oeData.rmag / 1000.0) - elif(endSunTime < idx <= endTimeStepSwitchTime): - if idx % 20 == 0: # Records every 20th data point + elif endSunTime < idx <= endTimeStepSwitchTime: + if idx % 20 == 0: # Records every 20th data point dataTime_TS.append(timeAxis[idx]) oeData = orbitalMotion.rv2elem_parab(sun.mu, dataPos[idx], dataVel[idx]) - rData_TS.append(oeData.rmag / 1000.) + rData_TS.append(oeData.rmag / 1000.0) else: # if idx % 5 == 0: # Records every 5th data point dataTime_JC.append(timeAxis[idx]) oeData = orbitalMotion.rv2elem_parab(sun.mu, dataPos[idx], dataVel[idx]) - rData_JC.append(oeData.rmag / 1000.) + rData_JC.append(oeData.rmag / 1000.0) dataTime_EC = np.array(dataTime_EC) dataTime_HC = np.array(dataTime_HC) dataTime_TS = np.array(dataTime_TS) dataTime_JC = np.array(dataTime_JC) - plt.plot(dataTime_EC * macros.NANO2HOUR, rData_EC, color='#aa0000', label='Earth Centered') - plt.plot(dataTime_HC * macros.NANO2HOUR, rData_HC, color='#008800', label='Heliocentric transfer') - plt.plot(dataTime_TS * macros.NANO2HOUR, rData_TS, color='#555555', label='Time Switch') - plt.plot(dataTime_JC * macros.NANO2HOUR, rData_JC, color='orangered', label='Jupiter Centered') - plt.yscale('log') - plt.legend(loc='lower right') - plt.xlabel('Time [h]') - plt.ylabel('Heliocentric Radius [km]') + plt.plot( + dataTime_EC * macros.NANO2HOUR, + rData_EC, + color="#aa0000", + label="Earth Centered", + ) + plt.plot( + dataTime_HC * macros.NANO2HOUR, + rData_HC, + color="#008800", + label="Heliocentric transfer", + ) + plt.plot( + dataTime_TS * macros.NANO2HOUR, rData_TS, color="#555555", label="Time Switch" + ) + plt.plot( + dataTime_JC * macros.NANO2HOUR, + rData_JC, + color="orangered", + label="Jupiter Centered", + ) + plt.yscale("log") + plt.legend(loc="lower right") + plt.xlabel("Time [h]") + plt.ylabel("Heliocentric Radius [km]") pltName = fileName + "5" figureList[pltName] = plt.figure(5) if show_plots: plt.show() else: - # close the plots being saved off to avoid over-writing old and new figures plt.close("all") @@ -601,9 +683,9 @@ def run(show_plots): dataPos = hubPos_N.getState() dataPos = [[dataPos[0][0], dataPos[1][0], dataPos[2][0]]] - return dataPos, figureList + # # This statement below ensures that the unit test script can be run as a # stand-along python script diff --git a/examples/scenarioPowerDemo.py b/examples/scenarioPowerDemo.py index afc0bcff05..994ad7b53e 100644 --- a/examples/scenarioPowerDemo.py +++ b/examples/scenarioPowerDemo.py @@ -80,7 +80,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation @@ -96,10 +96,12 @@ from Basilisk.architecture import astroConstants from Basilisk import __path__ + bskPath = __path__[0] path = os.path.dirname(os.path.abspath(__file__)) + def run(show_plots): """ The scenarios can be run with the followings setups parameters: @@ -108,14 +110,14 @@ def run(show_plots): show_plots (bool): Determines if the script should display plots """ - taskName = "unitTask" # arbitrary name (don't change) - processname = "TestProcess" # arbitrary name (don't change) + taskName = "unitTask" # arbitrary name (don't change) + processname = "TestProcess" # arbitrary name (don't change) # Create a sim module as an empty container scenarioSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(1.0) # update process rate update time + testProcessRate = macros.sec2nano(1.0) # update process rate update time testProc = scenarioSim.CreateNewProcess(processname) testProc.addTask(scenarioSim.CreateNewTask(taskName, testProcessRate)) @@ -128,14 +130,14 @@ def run(show_plots): gravFactory = simIncludeGravBody.gravBodyFactory() planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu sun = gravFactory.createSun() # attach gravity model to spacecraft gravFactory.addBodiesTo(scObject) # setup Spice interface for some solar system bodies - timeInitString = '2021 MAY 04 07:47:48.965 (UTC)' + timeInitString = "2021 MAY 04 07:47:48.965 (UTC)" spiceObject = gravFactory.createSpiceInterface(time=timeInitString) scenarioSim.AddModelToTask(taskName, spiceObject, -1) @@ -145,17 +147,17 @@ def run(show_plots): # setup orbit using orbitalMotion library oe = orbitalMotion.ClassicElements() - oe.a = astroConstants.REQ_EARTH*1e3 + 400e3 + oe.a = astroConstants.REQ_EARTH * 1e3 + 400e3 oe.e = 0.0 - oe.i = 0.0*macros.D2R + oe.i = 0.0 * macros.D2R - oe.Omega = 0.0*macros.D2R - oe.omega = 0.0*macros.D2R - oe.f = 75.0*macros.D2R + oe.Omega = 0.0 * macros.D2R + oe.omega = 0.0 * macros.D2R + oe.f = 75.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - n = np.sqrt(mu/oe.a/oe.a/oe.a) - P = 2.*np.pi/n + n = np.sqrt(mu / oe.a / oe.a / oe.a) + P = 2.0 * np.pi / n scObject.hub.r_CN_NInit = rN scObject.hub.v_CN_NInit = vN @@ -177,13 +179,15 @@ def run(show_plots): solarPanel.stateInMsg.subscribeTo(scObject.scStateOutMsg) solarPanel.sunEclipseInMsg.subscribeTo(eclipseObject.eclipseOutMsgs[0]) solarPanel.sunInMsg.subscribeTo(sunMsg) - solarPanel.setPanelParameters([1,0,0], 0.2*0.3, 0.20) # Set the panel normal vector in the body frame, the area, + solarPanel.setPanelParameters( + [1, 0, 0], 0.2 * 0.3, 0.20 + ) # Set the panel normal vector in the body frame, the area, scenarioSim.AddModelToTask(taskName, solarPanel) # Create a simple power sink powerSink = simplePowerSink.SimplePowerSink() powerSink.ModelTag = "powerSink2" - powerSink.nodePowerOut = -3. # Watts + powerSink.nodePowerOut = -3.0 # Watts scenarioSim.AddModelToTask(taskName, powerSink) # Create a simpleBattery and attach the sources/sinks to it @@ -210,7 +214,7 @@ def run(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - scenarioSim.ConfigureStopTime(macros.sec2nano(P)) # seconds to stop simulation + scenarioSim.ConfigureStopTime(macros.sec2nano(P)) # seconds to stop simulation # Begin the simulation time run set above scenarioSim.ExecuteSimulation() @@ -229,12 +233,12 @@ def run(show_plots): figureList = {} plt.close("all") # clears out plots from earlier test runs plt.figure(1) - plt.plot(tvec, storageData/3600., label='Stored Power (W-Hr)') - plt.plot(tvec, netData, label='Net Power (W)') - plt.plot(tvec, supplyData, label='Panel Power (W)') - plt.plot(tvec, sinkData, label='Power Draw (W)') - plt.xlabel('Time (Hr)') - plt.ylabel('Power (W)') + plt.plot(tvec, storageData / 3600.0, label="Stored Power (W-Hr)") + plt.plot(tvec, netData, label="Net Power (W)") + plt.plot(tvec, supplyData, label="Panel Power (W)") + plt.plot(tvec, sinkData, label="Power Draw (W)") + plt.xlabel("Time (Hr)") + plt.ylabel("Power (W)") plt.grid(True) plt.legend() @@ -247,6 +251,7 @@ def run(show_plots): return figureList + # # This statement below ensures that the unitTestScript can be run as a # stand-alone python script diff --git a/examples/scenarioQuadMaps.py b/examples/scenarioQuadMaps.py index bf26ddbe7d..0c38b63230 100644 --- a/examples/scenarioQuadMaps.py +++ b/examples/scenarioQuadMaps.py @@ -98,8 +98,14 @@ # general support file with common unit test functions # import general simulation support files -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport, vizSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, + vizSupport, +) from Basilisk.architecture import messaging @@ -123,7 +129,7 @@ def run(show_plots): dynProcess = scSim.CreateNewProcess(simProcessName) # Create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # Set up the simulation tasks/objects @@ -137,7 +143,7 @@ def run(show_plots): # Set up Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu planet.radiusRatio = 0.9966 @@ -147,7 +153,7 @@ def run(show_plots): # Setup spice library for Earth ephemeris timeInitString = "2000 November 26, 09:30:00.0 TDB" spiceObject = gravFactory.createSpiceInterface(time=timeInitString, epochInMsg=True) - spiceObject.zeroBase = 'Earth' + spiceObject.zeroBase = "Earth" scSim.AddModelToTask(simTaskName, spiceObject) @@ -156,15 +162,15 @@ def run(show_plots): # # Set up the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rMEO = 11260. * 1000 # meters + rMEO = 11260.0 * 1000 # meters # Elliptic MEO case oe.a = rMEO oe.e = 0.25 - oe.i = 28. * macros.D2R + oe.i = 28.0 * macros.D2R oe.Omega = 10.5 * macros.D2R oe.omega = 20.5 * macros.D2R - oe.f = 10. * macros.D2R + oe.f = 10.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) # To set the spacecraft initial conditions, the following initial position and velocity variables are set: @@ -175,25 +181,28 @@ def run(show_plots): # Set up data logging before the simulation is initialized numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) # create a logging task object of the spacecraft output message at the desired down sampling ratio dataRec = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataRec) if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - # saveFile=__file__ - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # saveFile=__file__ + ) # Del viz.quadMaps[:] at the start of the sim viz.quadMaps.clear() # Create Standard Camera - cam = vizSupport.createStandardCamera(viz, - setMode=0, - bodyTarget="earth", - fieldOfView=np.deg2rad(10) - ) + cam = vizSupport.createStandardCamera( + viz, setMode=0, bodyTarget="earth", fieldOfView=np.deg2rad(10) + ) # In more complex scenarios with pointing modules, a CameraConfigMsg can be used # for more precise camera-pointing. See scenarioVizPoint.py for this camera setup demo. @@ -208,37 +217,47 @@ def run(show_plots): # viz.addCamMsgToModule(messaging.CameraConfigMsg().write(cam)) # Create initial QuadMaps - vizSupport.addQuadMap(viz, - ID=1, - parentBodyName="earth", - vertices=vizSupport.qms.computeRectMesh(planet, - [66.5, 90], - [-180, 180], - 8), - color=[0, 0, 255, 100], - label="Arctic Region" - ) - - vizSupport.addQuadMap(viz, - ID=2, - parentBodyName="earth", - vertices=vizSupport.qms.computeRectMesh(planet, - [37, 41], - [-102.0467, -109.0467], - 5), - color=[0, 255, 0, 100], - label="Colorado" - ) - - vizSupport.addQuadMap(viz, - ID=3, - parentBodyName="bsk-Sat", - vertices=[-2.551, 0.341, 1.01, - -2.804, 0.341, 1.01, - -2.804, 0.111, 1.01, - -2.551, 0.111, 1.01], - color=[255, 128, 0, 100], - label="Solar Cell") + vizSupport.addQuadMap( + viz, + ID=1, + parentBodyName="earth", + vertices=vizSupport.qms.computeRectMesh(planet, [66.5, 90], [-180, 180], 8), + color=[0, 0, 255, 100], + label="Arctic Region", + ) + + vizSupport.addQuadMap( + viz, + ID=2, + parentBodyName="earth", + vertices=vizSupport.qms.computeRectMesh( + planet, [37, 41], [-102.0467, -109.0467], 5 + ), + color=[0, 255, 0, 100], + label="Colorado", + ) + + vizSupport.addQuadMap( + viz, + ID=3, + parentBodyName="bsk-Sat", + vertices=[ + -2.551, + 0.341, + 1.01, + -2.804, + 0.341, + 1.01, + -2.804, + 0.111, + 1.01, + -2.551, + 0.111, + 1.01, + ], + color=[255, 128, 0, 100], + label="Solar Cell", + ) viz.settings.mainCameraTarget = "earth" @@ -262,45 +281,60 @@ def run(show_plots): if len(FOVBox) == 12: # Subdivide region to wrap onto ellipsoid if fieldOfViewSubdivs > 1: - FOVBox = vizSupport.qms.subdivideFOVBox(planet, FOVBox, fieldOfViewSubdivs) - vizSupport.addQuadMap(viz, - ID=camQM_ID, - parentBodyName="earth", - vertices=FOVBox, - color=[255, 0, 0, 60] - ) + FOVBox = vizSupport.qms.subdivideFOVBox( + planet, FOVBox, fieldOfViewSubdivs + ) + vizSupport.addQuadMap( + viz, + ID=camQM_ID, + parentBodyName="earth", + vertices=FOVBox, + color=[255, 0, 0, 60], + ) camQM_ID += 1 - if incrementalStopTime == simulationTime/2: - vizSupport.addQuadMap(viz, - ID=1, - parentBodyName="earth", - vertices=vizSupport.qms.computeRectMesh(planet, - [66.5, 90], - [-180, 180], - 8), - color=[0, 0, 255, 100], - isHidden=True - ) - vizSupport.addQuadMap(viz, - ID=2, - parentBodyName="earth", - vertices=vizSupport.qms.computeRectMesh(planet, - [37, 41], - [-102.0467, -109.0467], - 5), - color=[255, 255, 0, 100], - label="CO (new color!)" - ) - vizSupport.addQuadMap(viz, - ID=3, - parentBodyName="bsk-Sat", - vertices=[-2.551, 0.341, 1.01, - -2.804, 0.341, 1.01, - -2.804, 0.111, 1.01, - -2.551, 0.111, 1.01], - color=[255, 128, 0, 100], - label="NOLABEL") + if incrementalStopTime == simulationTime / 2: + vizSupport.addQuadMap( + viz, + ID=1, + parentBodyName="earth", + vertices=vizSupport.qms.computeRectMesh( + planet, [66.5, 90], [-180, 180], 8 + ), + color=[0, 0, 255, 100], + isHidden=True, + ) + vizSupport.addQuadMap( + viz, + ID=2, + parentBodyName="earth", + vertices=vizSupport.qms.computeRectMesh( + planet, [37, 41], [-102.0467, -109.0467], 5 + ), + color=[255, 255, 0, 100], + label="CO (new color!)", + ) + vizSupport.addQuadMap( + viz, + ID=3, + parentBodyName="bsk-Sat", + vertices=[ + -2.551, + 0.341, + 1.01, + -2.804, + 0.341, + 1.01, + -2.804, + 0.111, + 1.01, + -2.551, + 0.111, + 1.01, + ], + color=[255, 128, 0, 100], + label="NOLABEL", + ) incrementalStopTime += imgTimeStep scSim.ConfigureStopTime(incrementalStopTime) @@ -312,7 +346,7 @@ def run(show_plots): # Unload Spice kernel gravFactory.unloadSpiceKernels() - return {} # no figures to return + return {} # no figures to return if __name__ == "__main__": diff --git a/examples/scenarioRendezVous.py b/examples/scenarioRendezVous.py index 012d9bb3cf..dbabec8270 100755 --- a/examples/scenarioRendezVous.py +++ b/examples/scenarioRendezVous.py @@ -89,12 +89,27 @@ import numpy as np from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import locationPointing -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, - rwMotorTorque, hillPoint) -from Basilisk.simulation import reactionWheelStateEffector, simpleNav, spacecraft, ephemerisConverter -from Basilisk.utilities import (SimulationBaseClass, macros, - orbitalMotion, simIncludeGravBody, - simIncludeRW, unitTestSupport, vizSupport) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + rwMotorTorque, + hillPoint, +) +from Basilisk.simulation import ( + reactionWheelStateEffector, + simpleNav, + spacecraft, + ephemerisConverter, +) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + simIncludeRW, + unitTestSupport, + vizSupport, +) try: from Basilisk.simulation import vizInterface @@ -104,6 +119,7 @@ # 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]) @@ -113,65 +129,83 @@ def plot_attitude_error(timeData, dataSigmaBR): """Plot the attitude errors.""" plt.figure(1) for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") def plot_rw_cmd_torque(timeData, dataUsReq, numRW): """Plot the RW command torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW): """Plot the RW actual motor torques.""" plt.figure(2) for idx in range(3): - plt.plot(timeData, dataUsReq[:, idx], - '--', - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\hat u_{s,' + str(idx) + '}$') - plt.plot(timeData, dataRW[idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$u_{s,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Motor Torque (Nm)') + plt.plot( + timeData, + dataUsReq[:, idx], + "--", + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\hat u_{s," + str(idx) + "}$", + ) + plt.plot( + timeData, + dataRW[idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$u_{s," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Motor Torque (Nm)") def plot_rate_error(timeData, dataOmegaBR): """Plot the body angular velocity rate tracking errors.""" plt.figure(3) for idx in range(3): - plt.plot(timeData, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error (rad/s) ') + plt.plot( + timeData, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error (rad/s) ") def plot_rw_speeds(timeData, dataOmegaRW, numRW): """Plot the RW spin rates.""" plt.figure(4) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Speed (RPM) ') + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Speed (RPM) ") def run(show_plots): @@ -211,18 +245,14 @@ def run(show_plots): scObject = spacecraft.Spacecraft() scObject.ModelTag = "servicer" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # create the debris object states scObject2 = spacecraft.Spacecraft() scObject2.ModelTag = "debris" - I2 = [600., 0., 0., - 0., 650., 0., - 0., 0, 450.] + I2 = [600.0, 0.0, 0.0, 0.0, 650.0, 0.0, 0.0, 0, 450.0] scObject2.hub.mHub = 350.0 # kg scObject2.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I2) @@ -236,9 +266,9 @@ def run(show_plots): # setup Earth Gravity Body # earth = gravFactory.createEarth() # earth.isCentralBody = True # ensure this is the central gravitational body - gravBodies = gravFactory.createBodies('sun', 'earth') - gravBodies['earth'].isCentralBody = True - mu = gravBodies['earth'].mu + gravBodies = gravFactory.createBodies("sun", "earth") + gravBodies["earth"].isCentralBody = True + mu = gravBodies["earth"].mu sunIdx = 0 earthIdx = 1 @@ -249,7 +279,7 @@ def run(show_plots): # setup SPICE interface for celestial objects timeInitString = "2022 MAY 1 00:28:30.0" spiceObject = gravFactory.createSpiceInterface(time=timeInitString, epochInMsg=True) - spiceObject.zeroBase = 'Earth' + spiceObject.zeroBase = "Earth" scSim.AddModelToTask(simTaskName, spiceObject) # @@ -262,16 +292,28 @@ def run(show_plots): varRWModel = messaging.BalancedWheels # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments - RW1 = rwFactory.create('Honeywell_HR16', [1, 0, 0], maxMomentum=50., Omega=100. # RPM - , RWModel=varRWModel - ) - RW2 = rwFactory.create('Honeywell_HR16', [0, 1, 0], maxMomentum=50., Omega=200. # RPM - , RWModel=varRWModel - ) - RW3 = rwFactory.create('Honeywell_HR16', [0, 0, 1], maxMomentum=50., Omega=300. # RPM - , rWB_B=[0.5, 0.5, 0.5] # meters - , RWModel=varRWModel - ) + RW1 = rwFactory.create( + "Honeywell_HR16", + [1, 0, 0], + maxMomentum=50.0, + Omega=100.0, # RPM + RWModel=varRWModel, + ) + RW2 = rwFactory.create( + "Honeywell_HR16", + [0, 1, 0], + maxMomentum=50.0, + Omega=200.0, # RPM + RWModel=varRWModel, + ) + RW3 = rwFactory.create( + "Honeywell_HR16", + [0, 0, 1], + maxMomentum=50.0, + Omega=300.0, # RPM + rWB_B=[0.5, 0.5, 0.5], # meters + RWModel=varRWModel, + ) numRW = rwFactory.getNumOfDevices() @@ -298,7 +340,7 @@ def run(show_plots): # Create an ephemeris converter to convert messages of type # 'SpicePlanetStateMsgPayload' to 'EphemerisMsgPayload' ephemObject = ephemerisConverter.EphemerisConverter() - ephemObject.ModelTag = 'EphemData' + ephemObject.ModelTag = "EphemData" ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[sunIdx]) ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[earthIdx]) scSim.AddModelToTask(simTaskName, ephemObject) @@ -360,7 +402,7 @@ def run(show_plots): mrpControl.K = 3.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # add module that maps the Lr control torque into the RW motor torques rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() @@ -371,16 +413,16 @@ def run(show_plots): rwMotorTorqueObj.rwParamsInMsg.subscribeTo(fswRwMsg) rwStateEffector.rwMotorCmdInMsg.subscribeTo(rwMotorTorqueObj.rwMotorTorqueOutMsg) # Make the RW control all three body axes - controlAxes_B = [ - 1, 0, 0, 0, 1, 0, 0, 0, 1 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] rwMotorTorqueObj.controlAxes_B = controlAxes_B # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) rwCmdLog = rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime) attErrLog = attError.attGuidOutMsg.recorder(samplingTime) sNavLog = sNavObject.transOutMsg.recorder(samplingTime) @@ -411,15 +453,15 @@ def run(show_plots): scObject2.hub.v_CN_NInit = v2N # m/s - v_CN_N scObject2.hub.sigma_BNInit = [[0.3], [0.1], [0.2]] # sigma_CN_B scObject2.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_CN_B - n = np.sqrt(mu/oe2.a/oe2.a/oe2.a) - orbitPeriod = 2*np.pi / n # in seconds + n = np.sqrt(mu / oe2.a / oe2.a / oe2.a) + orbitPeriod = 2 * np.pi / n # in seconds # setup the servicer (deputy) orbit using Hill frame coordinates # oe = copy.deepcopy(oe2) # oe.e = 0.000001 # rN, vN = orbitalMotion.elem2rv(mu, oe) rho_H = [-10.0, -40.0, 0.0] - rho_Prime_H = [0, -1.5*n*rho_H[0], 0] + rho_Prime_H = [0, -1.5 * n * rho_H[0], 0] rN, vN = orbitalMotion.hill2rv(r2N, v2N, rho_H, rho_Prime_H) scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m/s - v_CN_N @@ -438,11 +480,14 @@ def run(show_plots): servicerLight.markerDiameter = 0.1 servicerLight.color = vizInterface.IntVector(vizSupport.toRGBA255("white")) - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, [scObject, scObject2] - , rwEffectorList=[rwStateEffector, None] - , lightList=[[servicerLight], None] - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + [scObject, scObject2], + rwEffectorList=[rwStateEffector, None], + lightList=[[servicerLight], None], + # , saveFile=fileName + ) viz.settings.trueTrajectoryLinesOn = -1 viz.settings.orbitLinesOn = 2 @@ -485,7 +530,7 @@ def relOrbDrift(): rho_H, rho_Prime_H = orbitalMotion.rv2hill(rc, vc, rd, vd) xOff = rho_H[0] rho_Prime_H[0] = 0.0 - rho_Prime_H[1] = -1.5*n*xOff + rho_Prime_H[1] = -1.5 * n * xOff unusedPos, vd = orbitalMotion.hill2rv(rc, vc, rho_H, rho_Prime_H) servicerVel.setState(vd) @@ -495,17 +540,17 @@ def relativeEllipse(A0, xOff, B0=-1): rc = unitTestSupport.EigenVector3d2np(debrisPos.getState()) vc = unitTestSupport.EigenVector3d2np(debrisVel.getState()) rho_H, rho_Prime_H = orbitalMotion.rv2hill(rc, vc, rd, vd) - alpha = np.arccos((rho_H[0] - xOff)/A0) + alpha = np.arccos((rho_H[0] - xOff) / A0) if B0 > 0.01: - beta = np.arccos(rho_H[2]/B0) + beta = np.arccos(rho_H[2] / B0) else: if B0 == 0.0: beta = 0.0 # yOff = rho_H[1] + 2*A0*np.sin(alpha) - rho_Prime_H[0] = -A0*n*np.sin(alpha) - rho_Prime_H[1] = -2*A0*n*np.cos(alpha) - 1.5*n*xOff + rho_Prime_H[0] = -A0 * n * np.sin(alpha) + rho_Prime_H[1] = -2 * A0 * n * np.cos(alpha) - 1.5 * n * xOff if B0 >= 0.0: - rho_Prime_H[2] = -B0*n*np.sin(beta) + rho_Prime_H[2] = -B0 * n * np.sin(beta) unusedPos, vd = orbitalMotion.hill2rv(rc, vc, rho_H, rho_Prime_H) servicerVel.setState(vd) @@ -573,5 +618,5 @@ def relativeEllipse(A0, xOff, B0=-1): # if __name__ == "__main__": run( - True # show_plots + True # show_plots ) diff --git a/examples/scenarioRoboticArm.py b/examples/scenarioRoboticArm.py index 8bbef3d4b3..858737932e 100644 --- a/examples/scenarioRoboticArm.py +++ b/examples/scenarioRoboticArm.py @@ -77,8 +77,19 @@ import matplotlib.pyplot as plt import numpy as np -from Basilisk.utilities import (SimulationBaseClass, vizSupport, simIncludeGravBody, macros, orbitalMotion, unitTestSupport) -from Basilisk.simulation import spacecraft, spinningBodyNDOFStateEffector, prescribedRotation1DOF +from Basilisk.utilities import ( + SimulationBaseClass, + vizSupport, + simIncludeGravBody, + macros, + orbitalMotion, + unitTestSupport, +) +from Basilisk.simulation import ( + spacecraft, + spinningBodyNDOFStateEffector, + prescribedRotation1DOF, +) from Basilisk.architecture import messaging from Basilisk import __path__ @@ -119,7 +130,7 @@ class geometryClass: def createSimBaseClass(): scSim = SimulationBaseClass.SimBaseClass() - scSim.simulationTime = macros.min2nano(5.) + scSim.simulationTime = macros.min2nano(5.0) scSim.dynTaskName = "dynTask" scSim.dynProcess = scSim.CreateNewProcess("dynProcess") dynTimeStep = macros.sec2nano(0.2) @@ -132,9 +143,29 @@ def setUpSpacecraft(scSim, scGeometry): scObject = spacecraft.Spacecraft() scObject.ModelTag = "scObject" scObject.hub.mHub = scGeometry.massHub - scObject.hub.IHubPntBc_B = [[scGeometry.massHub / 12 * (scGeometry.lengthHub ** 2 + scGeometry.heightHub ** 2), 0.0, 0.0], - [0.0, scGeometry.massHub / 12 * (scGeometry.widthHub ** 2 + scGeometry.heightHub ** 2), 0.0], - [0.0, 0.0, scGeometry.massHub / 12 * (scGeometry.lengthHub ** 2 + scGeometry.widthHub ** 2)]] + scObject.hub.IHubPntBc_B = [ + [ + scGeometry.massHub + / 12 + * (scGeometry.lengthHub**2 + scGeometry.heightHub**2), + 0.0, + 0.0, + ], + [ + 0.0, + scGeometry.massHub + / 12 + * (scGeometry.widthHub**2 + scGeometry.heightHub**2), + 0.0, + ], + [ + 0.0, + 0.0, + scGeometry.massHub + / 12 + * (scGeometry.lengthHub**2 + scGeometry.widthHub**2), + ], + ] scSim.AddModelToTask(scSim.dynTaskName, scObject) return scObject @@ -158,7 +189,13 @@ def createFirstLink(scSim, spinningBodyEffector, scGeometry): spinningBody.setISPntSc_S([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]) spinningBody.setDCM_S0P([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) spinningBody.setR_ScS_S([[0.0], [scGeometry.heightLink / 2], [0.0]]) - spinningBody.setR_SP_P([[0], [scGeometry.lengthHub / 2], [scGeometry.heightHub / 2 - scGeometry.diameterLink / 2]]) + spinningBody.setR_SP_P( + [ + [0], + [scGeometry.lengthHub / 2], + [scGeometry.heightHub / 2 - scGeometry.diameterLink / 2], + ] + ) spinningBody.setSHat_S([[1], [0], [0]]) spinningBody.setThetaInit(0 * macros.D2R) spinningBody.setThetaDotInit(0 * macros.D2R) @@ -172,20 +209,44 @@ def createFirstLink(scSim, spinningBodyEffector, scGeometry): profiler.setThetaInit(spinningBody.getThetaInit()) profiler.setSmoothingDuration(10) scSim.AddModelToTask(scSim.dynTaskName, profiler) - spinningBodyEffector.spinningBodyRefInMsgs[0].subscribeTo(profiler.spinningBodyOutMsg) + spinningBodyEffector.spinningBodyRefInMsgs[0].subscribeTo( + profiler.spinningBodyOutMsg + ) hingedRigidBodyMessageData = messaging.HingedRigidBodyMsgPayload( theta=-30 * macros.D2R, # [rad] thetaDot=0.0, # [rad/s] ) - hingedRigidBodyMessage1 = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData) + hingedRigidBodyMessage1 = messaging.HingedRigidBodyMsg().write( + hingedRigidBodyMessageData + ) profiler.spinningBodyInMsg.subscribeTo(hingedRigidBodyMessage1) spinningBody = spinningBodyNDOFStateEffector.SpinningBody() spinningBody.setMass(scGeometry.massLink) - spinningBody.setISPntSc_S([[spinningBody.getMass() / 12 * (3 * (scGeometry.diameterLink / 2) ** 2 + scGeometry.heightLink ** 2), 0.0, 0.0], - [0.0, spinningBody.getMass() / 12 * (scGeometry.diameterLink / 2) ** 2, 0.0], - [0.0, 0.0, spinningBody.getMass() / 12 * (3 * (scGeometry.diameterLink / 2) ** 2 + scGeometry.heightLink ** 2)]]) + spinningBody.setISPntSc_S( + [ + [ + spinningBody.getMass() + / 12 + * (3 * (scGeometry.diameterLink / 2) ** 2 + scGeometry.heightLink**2), + 0.0, + 0.0, + ], + [ + 0.0, + spinningBody.getMass() / 12 * (scGeometry.diameterLink / 2) ** 2, + 0.0, + ], + [ + 0.0, + 0.0, + spinningBody.getMass() + / 12 + * (3 * (scGeometry.diameterLink / 2) ** 2 + scGeometry.heightLink**2), + ], + ] + ) spinningBody.setDCM_S0P([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) spinningBody.setR_ScS_S([[0.0], [scGeometry.heightLink / 2], [0.0]]) spinningBody.setR_SP_P([[0], [0], [0]]) @@ -202,13 +263,17 @@ def createFirstLink(scSim, spinningBodyEffector, scGeometry): profiler.setThetaInit(spinningBody.getThetaInit()) profiler.setSmoothingDuration(10) scSim.AddModelToTask(scSim.dynTaskName, profiler) - spinningBodyEffector.spinningBodyRefInMsgs[1].subscribeTo(profiler.spinningBodyOutMsg) + spinningBodyEffector.spinningBodyRefInMsgs[1].subscribeTo( + profiler.spinningBodyOutMsg + ) hingedRigidBodyMessageData = messaging.HingedRigidBodyMsgPayload( theta=45 * macros.D2R, # [rad] thetaDot=0.0, # [rad/s] ) - hingedRigidBodyMessage2 = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData) + hingedRigidBodyMessage2 = messaging.HingedRigidBodyMsg().write( + hingedRigidBodyMessageData + ) profiler.spinningBodyInMsg.subscribeTo(hingedRigidBodyMessage2) @@ -232,20 +297,44 @@ def createSecondLink(scSim, spinningBodyEffector, scGeometry): profiler.setThetaInit(spinningBody.getThetaInit()) profiler.setSmoothingDuration(10) scSim.AddModelToTask(scSim.dynTaskName, profiler) - spinningBodyEffector.spinningBodyRefInMsgs[2].subscribeTo(profiler.spinningBodyOutMsg) + spinningBodyEffector.spinningBodyRefInMsgs[2].subscribeTo( + profiler.spinningBodyOutMsg + ) hingedRigidBodyMessageData = messaging.HingedRigidBodyMsgPayload( theta=90 * macros.D2R, # [rad] thetaDot=0.0, # [rad/s] ) - hingedRigidBodyMessage1 = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData) + hingedRigidBodyMessage1 = messaging.HingedRigidBodyMsg().write( + hingedRigidBodyMessageData + ) profiler.spinningBodyInMsg.subscribeTo(hingedRigidBodyMessage1) spinningBody = spinningBodyNDOFStateEffector.SpinningBody() spinningBody.setMass(scGeometry.massLink) - spinningBody.setISPntSc_S([[spinningBody.getMass() / 12 * (3 * (scGeometry.diameterLink / 2) ** 2 + scGeometry.heightLink ** 2), 0.0, 0.0], - [0.0, spinningBody.getMass() / 12 * (scGeometry.diameterLink / 2) ** 2, 0.0], - [0.0, 0.0, spinningBody.getMass() / 12 * (3 * (scGeometry.diameterLink / 2) ** 2 + scGeometry.heightLink ** 2)]]) + spinningBody.setISPntSc_S( + [ + [ + spinningBody.getMass() + / 12 + * (3 * (scGeometry.diameterLink / 2) ** 2 + scGeometry.heightLink**2), + 0.0, + 0.0, + ], + [ + 0.0, + spinningBody.getMass() / 12 * (scGeometry.diameterLink / 2) ** 2, + 0.0, + ], + [ + 0.0, + 0.0, + spinningBody.getMass() + / 12 + * (3 * (scGeometry.diameterLink / 2) ** 2 + scGeometry.heightLink**2), + ], + ] + ) spinningBody.setDCM_S0P([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) spinningBody.setR_ScS_S([[0.0], [scGeometry.heightLink / 2], [0.0]]) spinningBody.setR_SP_P([[0], [0], [0]]) @@ -262,26 +351,32 @@ def createSecondLink(scSim, spinningBodyEffector, scGeometry): profiler.setThetaInit(spinningBody.getThetaInit()) profiler.setSmoothingDuration(10) scSim.AddModelToTask(scSim.dynTaskName, profiler) - spinningBodyEffector.spinningBodyRefInMsgs[3].subscribeTo(profiler.spinningBodyOutMsg) + spinningBodyEffector.spinningBodyRefInMsgs[3].subscribeTo( + profiler.spinningBodyOutMsg + ) hingedRigidBodyMessageData = messaging.HingedRigidBodyMsgPayload( - theta=-20 * macros.D2R, # [rad] - thetaDot=0.0, # [rad/s] + theta=-20 * macros.D2R, # [rad] + thetaDot=0.0, # [rad/s] + ) + hingedRigidBodyMessage2 = messaging.HingedRigidBodyMsg().write( + hingedRigidBodyMessageData ) - hingedRigidBodyMessage2 = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData) profiler.spinningBodyInMsg.subscribeTo(hingedRigidBodyMessage2) def setUpGravity(scSim, scObject): gravFactory = simIncludeGravBody.gravBodyFactory() - gravBodies = gravFactory.createBodies(['earth', 'sun']) - gravBodies['earth'].isCentralBody = True - mu = gravBodies['earth'].mu + gravBodies = gravFactory.createBodies(["earth", "sun"]) + gravBodies["earth"].isCentralBody = True + mu = gravBodies["earth"].mu gravFactory.addBodiesTo(scObject) timeInitString = "2012 MAY 1 00:28:30.0" - gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/', timeInitString, epochInMsg=True) - gravFactory.spiceObject.zeroBase = 'earth' + gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", timeInitString, epochInMsg=True + ) + gravFactory.spiceObject.zeroBase = "earth" scSim.AddModelToTask(scSim.dynTaskName, gravFactory.spiceObject) return mu @@ -314,29 +409,48 @@ def setUpRecorders(scSim, scObject, roboticArmEffector): def setUpVizard(scSim, scObject, roboticArmEffector, scGeometry): - scBodyList = [scObject, - ["arm1", roboticArmEffector.spinningBodyConfigLogOutMsgs[1]], - ["arm2", roboticArmEffector.spinningBodyConfigLogOutMsgs[3]]] - - viz = vizSupport.enableUnityVisualization(scSim, scSim.dynTaskName, scBodyList - # , saveFile=fileName - ) - - vizSupport.createCustomModel(viz - , simBodiesToModify=[scObject.ModelTag] - , modelPath="CUBE" - , color=vizSupport.toRGBA255("gold") - , scale=[scGeometry.widthHub, scGeometry.lengthHub, scGeometry.heightHub]) - vizSupport.createCustomModel(viz - , simBodiesToModify=["arm1"] - , modelPath="CYLINDER" - , scale=[scGeometry.diameterLink, scGeometry.diameterLink, scGeometry.heightLink / 2] - , rotation=[np.pi / 2, 0, 0]) - vizSupport.createCustomModel(viz - , simBodiesToModify=["arm2"] - , modelPath="CYLINDER" - , scale=[scGeometry.diameterLink, scGeometry.diameterLink, scGeometry.heightLink / 2] - , rotation=[np.pi / 2, 0, 0]) + scBodyList = [ + scObject, + ["arm1", roboticArmEffector.spinningBodyConfigLogOutMsgs[1]], + ["arm2", roboticArmEffector.spinningBodyConfigLogOutMsgs[3]], + ] + + viz = vizSupport.enableUnityVisualization( + scSim, + scSim.dynTaskName, + scBodyList, + # , saveFile=fileName + ) + + vizSupport.createCustomModel( + viz, + simBodiesToModify=[scObject.ModelTag], + modelPath="CUBE", + color=vizSupport.toRGBA255("gold"), + scale=[scGeometry.widthHub, scGeometry.lengthHub, scGeometry.heightHub], + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=["arm1"], + modelPath="CYLINDER", + scale=[ + scGeometry.diameterLink, + scGeometry.diameterLink, + scGeometry.heightLink / 2, + ], + rotation=[np.pi / 2, 0, 0], + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=["arm2"], + modelPath="CYLINDER", + scale=[ + scGeometry.diameterLink, + scGeometry.diameterLink, + scGeometry.heightLink / 2, + ], + rotation=[np.pi / 2, 0, 0], + ) viz.settings.orbitLinesOn = -1 @@ -363,11 +477,11 @@ def plotting(show_plots, scLog, thetaLog): plt.clf() ax = plt.axes() for idx, angle in enumerate(theta): - plt.plot(dynTimeMin, macros.R2D * angle, label=r'$\theta_' + str(idx + 1) + '$') - plt.legend(fontsize='14') - plt.title('Angles', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\theta$ [deg]', fontsize='18') + plt.plot(dynTimeMin, macros.R2D * angle, label=r"$\theta_" + str(idx + 1) + "$") + plt.legend(fontsize="14") + plt.title("Angles", fontsize="22") + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\theta$ [deg]", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -378,11 +492,15 @@ def plotting(show_plots, scLog, thetaLog): plt.clf() ax = plt.axes() for idx, angleRate in enumerate(thetaDot): - plt.plot(dynTimeMin, macros.R2D * angleRate, label=r'$\dot{\theta}_' + str(idx + 1) + '$') - plt.legend(fontsize='14') - plt.title('Angle Rates', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\dot{\theta}$ [deg/s]', fontsize='18') + plt.plot( + dynTimeMin, + macros.R2D * angleRate, + label=r"$\dot{\theta}_" + str(idx + 1) + "$", + ) + plt.legend(fontsize="14") + plt.title("Angle Rates", fontsize="22") + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\dot{\theta}$ [deg/s]", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -393,13 +511,16 @@ def plotting(show_plots, scLog, thetaLog): plt.clf() ax = plt.axes() for idx in range(3): - plt.plot(dynTimeMin, sigma_BN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(fontsize='14') - plt.title('Attitude', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\sigma_{B/N}$', fontsize='18') + plt.plot( + dynTimeMin, + sigma_BN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(fontsize="14") + plt.title("Attitude", fontsize="22") + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\sigma_{B/N}$", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -410,13 +531,16 @@ def plotting(show_plots, scLog, thetaLog): plt.clf() ax = plt.axes() for idx in range(3): - plt.plot(dynTimeMin, omega_BN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_' + str(idx) + '$') - plt.legend(fontsize='14') - plt.title('Attitude Rate', fontsize='22') - plt.xlabel('time [min]', fontsize='18') - plt.ylabel(r'$\omega_{B/N}$', fontsize='18') + plt.plot( + dynTimeMin, + omega_BN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_" + str(idx) + "$", + ) + plt.legend(fontsize="14") + plt.title("Attitude Rate", fontsize="22") + plt.xlabel("time [min]", fontsize="18") + plt.ylabel(r"$\omega_{B/N}$", fontsize="18") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) diff --git a/examples/scenarioRotatingPanel.py b/examples/scenarioRotatingPanel.py index ab53c4f9a8..490280c3a7 100644 --- a/examples/scenarioRotatingPanel.py +++ b/examples/scenarioRotatingPanel.py @@ -87,6 +87,7 @@ import matplotlib.pyplot as plt import numpy as np + # To play with any scenario scripts as tutorials, you should make a copy of them into a custom folder # outside of the Basilisk directory. # @@ -100,10 +101,17 @@ # import simulation related support from Basilisk.simulation import spacecraft + # general support file with common unit test functions # import general simulation support files -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport, vizSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, + vizSupport, +) from Basilisk.simulation import hingedRigidBodyStateEffector from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.architecture import messaging @@ -111,6 +119,7 @@ from Basilisk.simulation import coarseSunSensor import math + def run(show_plots): """ At the end of the python script you can specify the following example parameters. @@ -131,7 +140,7 @@ def run(show_plots): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(1.) + simulationTimeStep = macros.sec2nano(1.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # create the spacecraft hub @@ -150,14 +159,14 @@ def run(show_plots): # create sun position message sunMessage = messaging.SpicePlanetStateMsgPayload( PlanetName="Sun", - PositionVector=[0, orbitalMotion.AU*1000, 0], + PositionVector=[0, orbitalMotion.AU * 1000, 0], ) sunStateMsg = messaging.SpicePlanetStateMsg().write(sunMessage) # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rLEO = 7000. * 1000 # meters - rGEO = 42000. * 1000 # meters + rLEO = 7000.0 * 1000 # meters + rGEO = 42000.0 * 1000 # meters oe.a = (rLEO + rGEO) / 2.0 oe.e = 1.0 - rLEO / oe.a oe.i = 0.0 * macros.D2R @@ -165,13 +174,19 @@ def run(show_plots): oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements # To set the spacecraft initial conditions, the following initial position and velocity variables are set: scObject.hub.r_CN_NInit = rN # m - r_BN_N scObject.hub.v_CN_NInit = vN # m/s - v_BN_N # point the body 3 axis towards the sun in the inertial n2 direction - scObject.hub.sigma_BNInit = [[math.tan(-90./4.*macros.D2R)], [0.0], [0.0]] # sigma_BN_B + scObject.hub.sigma_BNInit = [ + [math.tan(-90.0 / 4.0 * macros.D2R)], + [0.0], + [0.0], + ] # sigma_BN_B scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B # @@ -208,18 +223,22 @@ def run(show_plots): # CSS1 = coarseSunSensor.CoarseSunSensor() CSS1.ModelTag = "CSS1_sensor" - CSS1.fov = 45. * macros.D2R + CSS1.fov = 45.0 * macros.D2R CSS1.scaleFactor = 1.0 CSS1.sunInMsg.subscribeTo(sunStateMsg) CSS1.nHat_B = [1, 0, 0] - CSS1.stateInMsg.subscribeTo(panel1.hingedRigidBodyConfigLogOutMsg) # states relative to panel states + CSS1.stateInMsg.subscribeTo( + panel1.hingedRigidBodyConfigLogOutMsg + ) # states relative to panel states CSS2 = coarseSunSensor.CoarseSunSensor() CSS2.ModelTag = "CSS2_sensor" - CSS2.fov = 45. * macros.D2R + CSS2.fov = 45.0 * macros.D2R CSS2.scaleFactor = 1.0 CSS2.sunInMsg.subscribeTo(sunStateMsg) CSS2.nHat_B = [0, 0, 1] - CSS2.stateInMsg.subscribeTo(panel1.hingedRigidBodyConfigLogOutMsg) # states relative to panel states + CSS2.stateInMsg.subscribeTo( + panel1.hingedRigidBodyConfigLogOutMsg + ) # states relative to panel states # # add modules to simulation task list @@ -232,12 +251,14 @@ def run(show_plots): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n simulationTime = macros.sec2nano(0.05 * P) # Setup data logging before the simulation is initialized numDataPoints = 200 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) pl1Log = panel1.hingedRigidBodyOutMsg.recorder(samplingTime) spLog = solarPanel.nodePowerOutMsg.recorder(samplingTime) @@ -250,11 +271,14 @@ def run(show_plots): scSim.AddModelToTask(simTaskName, css2Log) # Vizard Visualization Option - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - # saveFile=__file__, - # liveStream=True, - cssList=[[CSS1, CSS2]] - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # saveFile=__file__, + # liveStream=True, + cssList=[[CSS1, CSS2]], + ) scSim.InitializeSimulation() scSim.ConfigureStopTime(simulationTime) @@ -269,7 +293,9 @@ def run(show_plots): np.set_printoptions(precision=16) - figureList = plotOrbits(dataLog.times(), dataSigmaBN, panel1thetaLog, solarPowerLog, css1Log, css2Log) + figureList = plotOrbits( + dataLog.times(), dataSigmaBN, panel1thetaLog, solarPowerLog, css1Log, css2Log + ) if show_plots: plt.show() @@ -287,15 +313,18 @@ def plotOrbits(timeAxis, dataSigmaBN, panel1thetaLog, solarPowerLog, css1Log, cs plt.figure(figCounter) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") timeData = timeAxis * macros.NANO2MIN for idx in range(3): - plt.plot(timeData, dataSigmaBN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'MRP Attitude $\sigma_{B/N}$') + plt.plot( + timeData, + dataSigmaBN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"MRP Attitude $\sigma_{B/N}$") figureList = {} pltName = fileName + str(figCounter) figureList[pltName] = plt.figure(figCounter) @@ -304,17 +333,17 @@ def plotOrbits(timeAxis, dataSigmaBN, panel1thetaLog, solarPowerLog, css1Log, cs figCounter += 1 plt.figure(figCounter) ax1 = plt.figure(figCounter).add_subplot(111) - ax1.plot(timeData, panel1thetaLog*macros.R2D % 360, '--', color='royalblue') - ax1.fill_between(timeData, 0, 90, facecolor='gold') - ax1.fill_between(timeData, 270, 360, facecolor='gold') + ax1.plot(timeData, panel1thetaLog * macros.R2D % 360, "--", color="royalblue") + ax1.fill_between(timeData, 0, 90, facecolor="gold") + ax1.fill_between(timeData, 270, 360, facecolor="gold") ax1.set_yticks([0, 90, 180, 270, 360]) - plt.xlabel('Time [min]') - plt.ylabel('Panel Angle [deg]', color='royalblue') + plt.xlabel("Time [min]") + plt.ylabel("Panel Angle [deg]", color="royalblue") ax2 = plt.figure(figCounter).add_subplot(111, sharex=ax1, frameon=False) - ax2.plot(timeData, solarPowerLog, color='goldenrod') + ax2.plot(timeData, solarPowerLog, color="goldenrod") ax2.yaxis.tick_right() ax2.yaxis.set_label_position("right") - plt.ylabel('Solar Panel Power [W]', color='goldenrod') + plt.ylabel("Solar Panel Power [W]", color="goldenrod") pltName = fileName + str(figCounter) + "panel1theta" figureList[pltName] = plt.figure(figCounter) @@ -322,24 +351,20 @@ def plotOrbits(timeAxis, dataSigmaBN, panel1thetaLog, solarPowerLog, css1Log, cs figCounter += 1 plt.figure(figCounter) ax1 = plt.figure(figCounter).add_subplot(111) - ax1.plot(timeData, panel1thetaLog*macros.R2D % 360, '--', color='royalblue') + ax1.plot(timeData, panel1thetaLog * macros.R2D % 360, "--", color="royalblue") ax1.set_yticks([0, 90, 180, 270, 360]) - plt.xlabel('Time [min]') - plt.ylabel('Panel Angle [deg]', color='royalblue') + plt.xlabel("Time [min]") + plt.ylabel("Panel Angle [deg]", color="royalblue") ax2 = plt.figure(figCounter).add_subplot(111, sharex=ax1, frameon=False) - ax2.plot(timeData, css1Log, - color='tab:pink', - label=r'CSS$_1$') - ax2.plot(timeData, css2Log, - color='tab:olive', - label=r'CSS$_2$') - ax1.fill_between(timeData, 225, 315, facecolor='pink') - ax1.fill_between(timeData, 315, 360, facecolor='palegoldenrod') - ax1.fill_between(timeData, 0, 45, facecolor='palegoldenrod') - plt.legend(loc='lower right') + ax2.plot(timeData, css1Log, color="tab:pink", label=r"CSS$_1$") + ax2.plot(timeData, css2Log, color="tab:olive", label=r"CSS$_2$") + ax1.fill_between(timeData, 225, 315, facecolor="pink") + ax1.fill_between(timeData, 315, 360, facecolor="palegoldenrod") + ax1.fill_between(timeData, 0, 45, facecolor="palegoldenrod") + plt.legend(loc="lower right") ax2.yaxis.tick_right() ax2.yaxis.set_label_position("right") - plt.ylabel(r'CSS Signals') + plt.ylabel(r"CSS Signals") pltName = fileName + str(figCounter) figureList[pltName] = plt.figure(figCounter) diff --git a/examples/scenarioSatelliteConstellation.py b/examples/scenarioSatelliteConstellation.py index 14e81f6250..620d994d3e 100644 --- a/examples/scenarioSatelliteConstellation.py +++ b/examples/scenarioSatelliteConstellation.py @@ -90,14 +90,21 @@ bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) -from Basilisk.simulation import (spacecraft, ephemerisConverter) -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport, vizSupport) +from Basilisk.simulation import spacecraft, ephemerisConverter +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, + vizSupport, +) from Basilisk.fswAlgorithms import locationPointing -from Basilisk.simulation import (simpleNav, planetEphemeris) +from Basilisk.simulation import simpleNav, planetEphemeris # always import the Basilisk messaging support + def run(show_plots, a, i, T, P, F): """ The scenario can be run with the followings setups parameters: @@ -127,7 +134,7 @@ def run(show_plots, a, i, T, P, F): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(30.) + simulationTimeStep = macros.sec2nano(30.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # clear prior gravitational body and SPICE setup definitions @@ -139,38 +146,49 @@ def run(show_plots, a, i, T, P, F): mu = earth.mu # Create the ephemeris converter module - spiceObject = gravFactory.createSpiceInterface(time="2029 June 12 5:30:30.0", epochInMsg=True) - spiceObject.zeroBase = 'Earth' + spiceObject = gravFactory.createSpiceInterface( + time="2029 June 12 5:30:30.0", epochInMsg=True + ) + spiceObject.zeroBase = "Earth" scSim.AddModelToTask(simTaskName, spiceObject) ephemObject = ephemerisConverter.EphemerisConverter() ephemObject.ModelTag = "ephemData" - ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[0]) # Earth + ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[0]) # Earth scSim.AddModelToTask(simTaskName, ephemObject) # spacecraft inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] # set the simulation time to be two orbit periods n = np.sqrt(mu / a / a / a) - Pr = 2. * np.pi / n + Pr = 2.0 * np.pi / n simulationTime = macros.sec2nano(1.5 * Pr) # create a logging task object of the spacecraft output message at the desired down sampling ratio numDataPoints = 500 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) - if (np.mod(T,1) !=0 or np.mod(P,1) !=0 or np.mod(F,1) != 0 or T < 1 or P < 1 or F < 1): - raise Exception('number of satellites T, number of planes P, and relative spaceing F must be positive integer numbers') - if (np.mod(T,P) !=0): - raise Exception('number of satellites T must be an integer multiple of P') + if ( + np.mod(T, 1) != 0 + or np.mod(P, 1) != 0 + or np.mod(F, 1) != 0 + or T < 1 + or P < 1 + or F < 1 + ): + raise Exception( + "number of satellites T, number of planes P, and relative spaceing F must be positive integer numbers" + ) + if np.mod(T, P) != 0: + raise Exception("number of satellites T must be an integer multiple of P") # pre-compute Walker constellation parameters - PU = 360/T # patter unit in degrees - S = T/P # number of satellites in each plane - delta_RAAN = S*PU # delta RAAN in degrees - delta_nu = P*PU # in plane spacing of satellites + PU = 360 / T # patter unit in degrees + S = T / P # number of satellites in each plane + delta_RAAN = S * PU # delta RAAN in degrees + delta_nu = P * PU # in plane spacing of satellites oe = orbitalMotion.ClassicElements() oe.a = a oe.e = 0.01 @@ -184,17 +202,19 @@ def run(show_plots, a, i, T, P, F): for i in range(T): # initialize spacecraft object scList.append(spacecraft.Spacecraft()) - scList[i].ModelTag = "bsk-Sat-"+str(i) + scList[i].ModelTag = "bsk-Sat-" + str(i) scSim.AddModelToTask(simTaskName, scList[i]) # attach gravity model to spacecraft - scList[i].gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scList[i].gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # setup the orbit using classical orbit elements - RAAN = delta_RAAN * np.floor(i/S) + RAAN = delta_RAAN * np.floor(i / S) oe.Omega = RAAN * macros.D2R - oe.omega = F * np.floor(i/S) * macros.D2R - oe.f = delta_nu * np.mod(i,S) * macros.D2R + oe.omega = F * np.floor(i / S) * macros.D2R + oe.f = delta_nu * np.mod(i, S) * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) scList[i].hub.r_CN_NInit = rN # [m] scList[i].hub.v_CN_NInit = vN # [m/s] @@ -205,13 +225,13 @@ def run(show_plots, a, i, T, P, F): # set up prescribed attitude guidance navList.append(simpleNav.SimpleNav()) - navList[i].ModelTag = "SimpleNav"+str(i) + navList[i].ModelTag = "SimpleNav" + str(i) scSim.AddModelToTask(simTaskName, navList[i]) navList[i].scStateInMsg.subscribeTo(scList[i].scStateOutMsg) # setup nadir pointing guidance module guideList.append(locationPointing.locationPointing()) - guideList[i].ModelTag = "nadirPoint"+str(i) + guideList[i].ModelTag = "nadirPoint" + str(i) guideList[i].pHat_B = [1, 0, 0] guideList[i].scTransInMsg.subscribeTo(navList[i].transOutMsg) guideList[i].scAttInMsg.subscribeTo(navList[i].attOutMsg) @@ -226,13 +246,24 @@ def run(show_plots, a, i, T, P, F): # If you wish to transmit the simulation data to the United based Vizard Visualization application, # then uncomment the following saveFile line if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scList, - # saveFile=__file__ - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scList, + # saveFile=__file__ + ) for i in range(T): - vizSupport.createConeInOut(viz, fromBodyName=scList[i].ModelTag, toBodyName='earth', coneColor='teal', - normalVector_B=[1, 0, 0], incidenceAngle=30/macros.D2R, isKeepIn=False, - coneHeight=10.0, coneName='pointingCone') + vizSupport.createConeInOut( + viz, + fromBodyName=scList[i].ModelTag, + toBodyName="earth", + coneColor="teal", + normalVector_B=[1, 0, 0], + incidenceAngle=30 / macros.D2R, + isKeepIn=False, + coneHeight=10.0, + coneName="pointingCone", + ) # initialize simulation scSim.InitializeSimulation() @@ -248,16 +279,16 @@ def run(show_plots, a, i, T, P, F): for n in range(T): # calculate latitude and longitude [lon, lat] = rv2latlon(dataRec[n]) - color = unitTestSupport.getLineColor(int(np.floor(n/S)), 3) + color = unitTestSupport.getLineColor(int(np.floor(n / S)), 3) plt.scatter(lat, lon, s=5, c=[color]) - plt.scatter(lat[0], lon[0], s=20, c='#00A300') - plt.scatter(lat[-1], lon[-1], s=20, c='#A30000') - plt.xlim([-180,180]) - plt.ylim([-90,90]) - plt.xticks(range(-180,181,30)) - plt.yticks(range(-90,91,15)) - plt.xlabel('longitude (degrees)') - plt.ylabel('lattitude (degrees)') + plt.scatter(lat[0], lon[0], s=20, c="#00A300") + plt.scatter(lat[-1], lon[-1], s=20, c="#A30000") + plt.xlim([-180, 180]) + plt.ylim([-90, 90]) + plt.xticks(range(-180, 181, 30)) + plt.yticks(range(-90, 91, 15)) + plt.xlabel("longitude (degrees)") + plt.ylabel("lattitude (degrees)") pltName = fileName figureList[pltName] = plt.figure(1) @@ -270,28 +301,32 @@ def run(show_plots, a, i, T, P, F): return figureList + def rv2latlon(dataRec): times = dataRec.times() * macros.NANO2SEC rECI = dataRec.r_BN_N lat = np.zeros(times.shape) lon = np.zeros(times.shape) for i in range(len(times)): - theta = times[i]*planetEphemeris.OMEGA_EARTH - dcm_ECI2ECEF = [[np.cos(theta), np.sin(theta), 0], - [-np.sin(theta), np.cos(theta), 0], - [0, 0, 1]] - rECEF = dcm_ECI2ECEF@rECI[i,:] - lat[i] = np.arcsin(rECEF[2]/np.linalg.norm(rECEF)) * macros.R2D - lon[i] = np.arctan2(rECEF[1],rECEF[0]) * macros.R2D + theta = times[i] * planetEphemeris.OMEGA_EARTH + dcm_ECI2ECEF = [ + [np.cos(theta), np.sin(theta), 0], + [-np.sin(theta), np.cos(theta), 0], + [0, 0, 1], + ] + rECEF = dcm_ECI2ECEF @ rECI[i, :] + lat[i] = np.arcsin(rECEF[2] / np.linalg.norm(rECEF)) * macros.R2D + lon[i] = np.arctan2(rECEF[1], rECEF[0]) * macros.R2D return lat, lon + if __name__ == "__main__": run( - True, # show_plots - 29994000, # semi-major axis [m] - 56, # orbit inclination [deg] - 24, # total number of satellites (int) - 3, # number of orbit planes (int) - 1 # phasing (int) + True, # show_plots + 29994000, # semi-major axis [m] + 56, # orbit inclination [deg] + 24, # total number of satellites (int) + 3, # number of orbit planes (int) + 1, # phasing (int) ) diff --git a/examples/scenarioSepMomentumManagement.py b/examples/scenarioSepMomentumManagement.py index fafb3226fd..207b499f2c 100644 --- a/examples/scenarioSepMomentumManagement.py +++ b/examples/scenarioSepMomentumManagement.py @@ -103,14 +103,39 @@ import numpy as np from Basilisk import __path__ from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, oneAxisSolarArrayPoint, rwMotorTorque, - hingedRigidBodyPIDMotor, solarArrayReference, thrusterPlatformReference, - thrusterPlatformState, thrustCMEstimation, torqueScheduler) -from Basilisk.simulation import (reactionWheelStateEffector, simpleNav, simpleMassProps, spacecraft, - spinningBodyOneDOFStateEffector, spinningBodyTwoDOFStateEffector, - thrusterStateEffector, facetSRPDynamicEffector, boreAngCalc) -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, simIncludeGravBody, simIncludeRW, - unitTestSupport, vizSupport, RigidBodyKinematics as rbk) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + oneAxisSolarArrayPoint, + rwMotorTorque, + hingedRigidBodyPIDMotor, + solarArrayReference, + thrusterPlatformReference, + thrusterPlatformState, + thrustCMEstimation, + torqueScheduler, +) +from Basilisk.simulation import ( + reactionWheelStateEffector, + simpleNav, + simpleMassProps, + spacecraft, + spinningBodyOneDOFStateEffector, + spinningBodyTwoDOFStateEffector, + thrusterStateEffector, + facetSRPDynamicEffector, + boreAngCalc, +) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + simIncludeRW, + unitTestSupport, + vizSupport, + RigidBodyKinematics as rbk, +) bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) @@ -170,9 +195,9 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) gravFactory = simIncludeGravBody.gravBodyFactory() # Next a series of gravitational bodies are included - gravBodies = gravFactory.createBodies(['sun']) - gravBodies['sun'].isCentralBody = True - mu = gravBodies['sun'].mu + gravBodies = gravFactory.createBodies(["sun"]) + gravBodies["sun"].isCentralBody = True + mu = gravBodies["sun"].mu # The configured gravitational bodies are added to the spacecraft dynamics with the usual command: gravFactory.addBodiesTo(scObject) @@ -181,19 +206,19 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) timeInitString = "2023 OCTOBER 22 00:00:00.0" # The following is a support macro that creates a `gravFactory.spiceObject` instance - gravFactory.createSpiceInterface(bskPath +'/supportData/EphemerisData/', - timeInitString, - epochInMsg=True) + gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", timeInitString, epochInMsg=True + ) # Sun is gravity center - gravFactory.spiceObject.zeroBase = 'Sun' + gravFactory.spiceObject.zeroBase = "Sun" # The SPICE object is added to the simulation task list. scSim.AddModelToTask(fswTask, gravFactory.spiceObject, 2) # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - oe.a = 100e9 # meters + oe.a = 100e9 # meters oe.e = 0.001 oe.i = 0.0 * macros.D2R oe.Omega = 0.0 * macros.D2R @@ -202,17 +227,23 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) rN, vN = orbitalMotion.elem2rv(mu, oe) # To set the spacecraft initial conditions, the following initial position and velocity variables are set: - scObject.hub.r_CN_NInit = rN # m - r_BN_N - scObject.hub.v_CN_NInit = vN # m/s - v_BN_N - scObject.hub.sigma_BNInit = [0, 0., 0.] # MRP set to customize initial inertial attitude - scObject.hub.omega_BN_BInit = [[0.], [0.], [0.]] # rad/s - omega_CN_B + scObject.hub.r_CN_NInit = rN # m - r_BN_N + scObject.hub.v_CN_NInit = vN # m/s - v_BN_N + scObject.hub.sigma_BNInit = [ + 0, + 0.0, + 0.0, + ] # MRP set to customize initial inertial attitude + scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_CN_B # define the simulation inertia - I = [ 1725, -5, -12, - -5, 5525, 43, - -12, 43, 4810] + I = [1725, -5, -12, -5, 5525, 43, -12, 43, 4810] scObject.hub.mHub = 2500 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.008], [-0.010], [1.214]] # [m] - position vector of hub CM relative to the body-fixed point B + scObject.hub.r_BcB_B = [ + [0.008], + [-0.010], + [1.214], + ] # [m] - position vector of hub CM relative to the body-fixed point B scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # @@ -222,22 +253,30 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) rwFactory = simIncludeRW.rwFactory() # specify RW momentum capacity - maxRWMomentum = 100. # Nms + maxRWMomentum = 100.0 # Nms # Define orthogonal RW pyramid # -- Pointing directions rwElAngle = np.array([40.0, 40.0, 40.0, 40.0]) * macros.D2R rwAzimuthAngle = np.array([45.0, 135.0, 225.0, 315.0]) * macros.D2R - rwPosVector = [[0.8, 0.8, 1.8], - [0.8, -0.8, 1.8], - [-0.8, -0.8, 1.8], - [-0.8, 0.8, 1.8]] + rwPosVector = [ + [0.8, 0.8, 1.8], + [0.8, -0.8, 1.8], + [-0.8, -0.8, 1.8], + [-0.8, 0.8, 1.8], + ] Gs = [] for elAngle, azAngle, posVector in zip(rwElAngle, rwAzimuthAngle, rwPosVector): gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot(np.array([1, 0, 0])) Gs.append(gsHat) - rwFactory.create('Honeywell_HR16', gsHat, maxMomentum=maxRWMomentum, rWB_B=posVector, Omega=0.) + rwFactory.create( + "Honeywell_HR16", + gsHat, + maxMomentum=maxRWMomentum, + rWB_B=posVector, + Omega=0.0, + ) numRW = rwFactory.getNumOfDevices() @@ -271,9 +310,7 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) RSAList[0].r_ScS_S = [0.0, 3.75, 0.0] RSAList[0].sHat_S = [0, 1, 0] RSAList[0].dcm_S0B = [[0, 0, 1], [1, 0, 0], [0, 1, 0]] - RSAList[0].IPntSc_S = [[250.0, 0.0, 0.0], - [0.0, 250.0, 0.0], - [0.0, 0.0, 500.0]] + RSAList[0].IPntSc_S = [[250.0, 0.0, 0.0], [0.0, 250.0, 0.0], [0.0, 0.0, 500.0]] RSAList[0].mass = 85 RSAList[0].k = 0 RSAList[0].c = 0 @@ -288,9 +325,7 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) RSAList[1].r_ScS_S = [0.0, 3.75, 0.0] RSAList[1].sHat_S = [0, 1, 0] RSAList[1].dcm_S0B = [[0, 0, -1], [-1, 0, 0], [0, 1, 0]] - RSAList[1].IPntSc_S = [[250.0, 0.0, 0.0], - [0.0, 250.0, 0.0], - [0.0, 0.0, 500.0]] + RSAList[1].IPntSc_S = [[250.0, 0.0, 0.0], [0.0, 250.0, 0.0], [0.0, 0.0, 500.0]] RSAList[1].mass = 85 RSAList[1].k = 0 RSAList[1].c = 0 @@ -376,17 +411,19 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) lenYHub = 1.8 # [m] lenZHub = 2.86 # [m] arrayDiam = 7.262 - area2 = np.pi*(0.5 * arrayDiam)*(0.5 * arrayDiam) # [m^2] - facetAreaList = [lenYHub * lenZHub, - lenXHub * lenZHub, - lenYHub * lenZHub, - lenXHub * lenZHub, - lenXHub * lenYHub, - lenXHub * lenYHub, - area2, - area2, - area2, - area2] + area2 = np.pi * (0.5 * arrayDiam) * (0.5 * arrayDiam) # [m^2] + facetAreaList = [ + lenYHub * lenZHub, + lenXHub * lenZHub, + lenYHub * lenZHub, + lenXHub * lenZHub, + lenXHub * lenYHub, + lenXHub * lenYHub, + area2, + area2, + area2, + area2, + ] # Define the initial facet attitudes relative to B frame prv_F01B = (macros.D2R * -90.0) * np.array([0.0, 0.0, 1.0]) @@ -399,40 +436,46 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) prv_F08B = (macros.D2R * 180.0) * np.array([0.0, 0.0, 1.0]) prv_F09B = (macros.D2R * 0.0) * np.array([0.0, 0.0, 1.0]) prv_F010B = (macros.D2R * 180.0) * np.array([0.0, 0.0, 1.0]) - facetDcm_F0BList = [rbk.PRV2C(prv_F01B), - rbk.PRV2C(prv_F02B), - rbk.PRV2C(prv_F03B), - rbk.PRV2C(prv_F04B), - rbk.PRV2C(prv_F05B), - rbk.PRV2C(prv_F06B), - rbk.PRV2C(prv_F07B), - rbk.PRV2C(prv_F08B), - rbk.PRV2C(prv_F09B), - rbk.PRV2C(prv_F010B)] + facetDcm_F0BList = [ + rbk.PRV2C(prv_F01B), + rbk.PRV2C(prv_F02B), + rbk.PRV2C(prv_F03B), + rbk.PRV2C(prv_F04B), + rbk.PRV2C(prv_F05B), + rbk.PRV2C(prv_F06B), + rbk.PRV2C(prv_F07B), + rbk.PRV2C(prv_F08B), + rbk.PRV2C(prv_F09B), + rbk.PRV2C(prv_F010B), + ] # Define the facet normal vectors in F frame components - facetNHat_FList = [np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0])] + facetNHat_FList = [ + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + ] # Define facet articulation axes in F frame components - facetRotHat_FList = [np.array([0.0, 0.0, 0.0]), - np.array([0.0, 0.0, 0.0]), - np.array([0.0, 0.0, 0.0]), - np.array([0.0, 0.0, 0.0]), - np.array([0.0, 0.0, 0.0]), - np.array([0.0, 0.0, 0.0]), - np.array([1.0, 0.0, 0.0]), - np.array([-1.0, 0.0, 0.0]), - np.array([-1.0, 0.0, 0.0]), - np.array([1.0, 0.0, 0.0])] + facetRotHat_FList = [ + np.array([0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0]), + np.array([1.0, 0.0, 0.0]), + np.array([-1.0, 0.0, 0.0]), + np.array([-1.0, 0.0, 0.0]), + np.array([1.0, 0.0, 0.0]), + ] # Define the facet center of pressure locations with respect to point B in B frame components facetLoc1 = np.array([0.5 * lenXHub, 0.0, 0.5 * lenZHub]) # [m] @@ -445,21 +488,38 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) facetLoc8 = np.array([3.75 + 0.5 * lenXHub, 0.00, 0.45]) # [m] facetLoc9 = np.array([-(3.75 + 0.5 * lenXHub), 0.0, 0.45]) # [m] facetLoc10 = np.array([-(3.75 + 0.5 * lenXHub), 0.0, 0.45]) # [m] - facetR_CopB_BList = [facetLoc1, facetLoc2, facetLoc3, facetLoc4, facetLoc5, facetLoc6, facetLoc7, facetLoc8, facetLoc9, facetLoc10] + facetR_CopB_BList = [ + facetLoc1, + facetLoc2, + facetLoc3, + facetLoc4, + facetLoc5, + facetLoc6, + facetLoc7, + facetLoc8, + facetLoc9, + facetLoc10, + ] # Define the facet optical coefficients - facetSpecularCoeffList = np.array([0.336, 0.336, 0.336, 0.336, 0.336, 0.336, 0.16, 0.00, 0.16, 0.00]) - facetDiffuseCoeffList = np.array([0.139, 0.139, 0.139, 0.139, 0.139, 0.139, 0.16, 0.56, 0.16, 0.56]) + facetSpecularCoeffList = np.array( + [0.336, 0.336, 0.336, 0.336, 0.336, 0.336, 0.16, 0.00, 0.16, 0.00] + ) + facetDiffuseCoeffList = np.array( + [0.139, 0.139, 0.139, 0.139, 0.139, 0.139, 0.16, 0.56, 0.16, 0.56] + ) # Populate the scGeometry structure with the facet information for i in range(len(facetAreaList)): - SRP.addFacet(facetAreaList[i], - facetDcm_F0BList[i], - facetNHat_FList[i], - facetRotHat_FList[i], - facetR_CopB_BList[i], - facetDiffuseCoeffList[i], - facetSpecularCoeffList[i]) + SRP.addFacet( + facetAreaList[i], + facetDcm_F0BList[i], + facetNHat_FList[i], + facetRotHat_FList[i], + facetR_CopB_BList[i], + facetDiffuseCoeffList[i], + facetSpecularCoeffList[i], + ) SRP.ModelTag = "FacetSRP" SRP.addArticulatedFacet(RSAList[0].spinningBodyOutMsg) @@ -485,7 +545,9 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) cmEstimator = thrustCMEstimation.ThrustCMEstimation() cmEstimator.ModelTag = "cmEstimator" cmEstimator.attitudeTol = 1e-6 - cmEstimator.r_CB_B = r_CB_B_0 # Real CoM_B location = [0.113244, 0.025605, 1.239834] + cmEstimator.r_CB_B = ( + r_CB_B_0 # Real CoM_B location = [0.113244, 0.025605, 1.239834] + ) cmEstimator.P0 = [0.0025, 0.0025, 0.0025] cmEstimator.R0 = [1e-10, 1e-10, 1e-10] scSim.AddModelToTask(fswTask, cmEstimator, None, 29) @@ -503,12 +565,12 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) # Set up platform reference module pltReference = thrusterPlatformReference.thrusterPlatformReference() - pltReference.ModelTag = 'thrusterPlatformReference' + pltReference.ModelTag = "thrusterPlatformReference" pltReference.sigma_MB = pltState.sigma_MB pltReference.r_BM_M = pltState.r_BM_M pltReference.r_FM_F = pltState.r_FM_F - pltReference.theta1Max = np.pi/12 - pltReference.theta2Max = np.pi/12 + pltReference.theta1Max = np.pi / 12 + pltReference.theta2Max = np.pi / 12 if thrMomManagement: pltReference.K = 2.5e-4 else: @@ -520,7 +582,7 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) pltController = [] for item in range(2): pltController.append(hingedRigidBodyPIDMotor.hingedRigidBodyPIDMotor()) - pltController[item].ModelTag = "PltMototorGimbal"+str(item+1) + pltController[item].ModelTag = "PltMototorGimbal" + str(item + 1) pltController[item].K = 0.5 pltController[item].P = 3 scSim.AddModelToTask(fswTask, pltController[item], 27) @@ -535,19 +597,23 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) # Set up attitude guidance module sepPoint = oneAxisSolarArrayPoint.oneAxisSolarArrayPoint() sepPoint.ModelTag = "sepPointGuidance" - sepPoint.a1Hat_B = [1, 0, 0] # solar array drive axis - sepPoint.a2Hat_B = [0, 1, 0] # antiparallel direction to the sensitive surface - sepPoint.hHat_N = [1, 0, 0] # random inertial thrust direction + sepPoint.a1Hat_B = [1, 0, 0] # solar array drive axis + sepPoint.a2Hat_B = [0, 1, 0] # antiparallel direction to the sensitive surface + sepPoint.hHat_N = [1, 0, 0] # random inertial thrust direction scSim.AddModelToTask(fswTask, sepPoint, 25) # Set up the solar array reference modules saReference = [] for item in range(numRSA): saReference.append(solarArrayReference.solarArrayReference()) - saReference[item].ModelTag = "SolarArrayReference"+str(item+1) - saReference[item].a1Hat_B = [(-1)**item, 0, 0] + saReference[item].ModelTag = "SolarArrayReference" + str(item + 1) + saReference[item].a1Hat_B = [(-1) ** item, 0, 0] saReference[item].a2Hat_B = [0, 1, 0] - saReference[item].r_AB_B = [(-1)**item * 0.5 * (lenXHub + arrayDiam), 0.0, 0.45] + saReference[item].r_AB_B = [ + (-1) ** item * 0.5 * (lenXHub + arrayDiam), + 0.0, + 0.45, + ] saReference[item].pointingMode = 0 saReference[item].n = 2 saReference[item].sigma = 1e-3 @@ -558,7 +624,7 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) saController = [] for item in range(numRSA): saController.append(hingedRigidBodyPIDMotor.hingedRigidBodyPIDMotor()) - saController[item].ModelTag = "SolarArrayMotor"+str(item+1) + saController[item].ModelTag = "SolarArrayMotor" + str(item + 1) saController[item].K = 1.25 saController[item].P = 50 saController[item].I = 3e-3 @@ -575,7 +641,7 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) mrpControl.Ki = 1e-5 mrpControl.P = 275 mrpControl.K = 9 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 mrpControl.controlLawType = 1 scSim.AddModelToTask(fswTask, mrpControl, 21) @@ -586,7 +652,9 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) scSim.AddModelToTask(fswTask, rwMotorTorqueObj, 20) # Configure thruster on-time message - thrOnTimeMsgData = messaging.THRArrayOnTimeCmdMsgPayload(OnTimeRequest=[3600*24*7]) + thrOnTimeMsgData = messaging.THRArrayOnTimeCmdMsgPayload( + OnTimeRequest=[3600 * 24 * 7] + ) thrOnTimeMsg = messaging.THRArrayOnTimeCmdMsg().write(thrOnTimeMsgData) # Write cmEstimator output msg to the standalone message vcMsg_CoM @@ -604,37 +672,45 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) sc_body_list.append([RSAList[1].ModelTag, RSAList[1].spinningBodyConfigLogOutMsg]) if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scSim, dynTask, sc_body_list - # , saveFile=__file__ - ) - viz.settings.ambient = 0.7 # increase ambient light to make the shaded spacecraft more visible + viz = vizSupport.enableUnityVisualization( + scSim, + dynTask, + sc_body_list, + # , saveFile=__file__ + ) + viz.settings.ambient = ( + 0.7 # increase ambient light to make the shaded spacecraft more visible + ) viz.settings.orbitLinesOn = -1 # turn off osculating orbit line current_path = os.path.dirname(os.path.abspath(__file__)) # Specifying relative model path is useful for sharing scenarios and resources: - texture_path = os.path.join('..', 'dataForExamples', 'texture') + texture_path = os.path.join("..", "dataForExamples", "texture") # Specifying absolute model path is preferable for live-streaming: # texture_path = os.path.join(current_path, 'dataForExamples', 'texture') - vizSupport.createCustomModel(viz - , simBodiesToModify=[sc_body_list[0].ModelTag] - , modelPath="CUBE" - , customTexturePath=os.path.join(texture_path, 'foil_n.png') - , offset=[0, 0, 0] - , scale=[2.5, 2.5, 2.5] - ) - vizSupport.createCustomModel(viz - , simBodiesToModify=[sc_body_list[1][0]] - , modelPath="CYLINDER" - , customTexturePath=os.path.join(texture_path, 'panel.jpg') - , offset=[-0.035, 0.25, -0.087] - , scale=[7, 7, 0.05] - ) - vizSupport.createCustomModel(viz - , simBodiesToModify=[sc_body_list[2][0]] - , modelPath="CYLINDER" - , customTexturePath=os.path.join(texture_path, 'panel.jpg') - , offset=[0.128, 0.25, -0.087] - , scale=[7, 7, 0.05] - ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=[sc_body_list[0].ModelTag], + modelPath="CUBE", + customTexturePath=os.path.join(texture_path, "foil_n.png"), + offset=[0, 0, 0], + scale=[2.5, 2.5, 2.5], + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=[sc_body_list[1][0]], + modelPath="CYLINDER", + customTexturePath=os.path.join(texture_path, "panel.jpg"), + offset=[-0.035, 0.25, -0.087], + scale=[7, 7, 0.05], + ) + vizSupport.createCustomModel( + viz, + simBodiesToModify=[sc_body_list[2][0]], + modelPath="CYLINDER", + customTexturePath=os.path.join(texture_path, "panel.jpg"), + offset=[0.128, 0.25, -0.087], + scale=[7, 7, 0.05], + ) # Connect messages sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg) @@ -653,9 +729,13 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) cmEstimator.attGuidInMsg.subscribeTo(attError.attGuidOutMsg) cmEstimator.vehConfigInMsg.subscribeTo(simpleMassPropsObject.vehicleConfigOutMsg) if cmEstimation: - pltReference.vehConfigInMsg.subscribeTo(vcMsg_CoM) # connect to this msg for estimated CM + pltReference.vehConfigInMsg.subscribeTo( + vcMsg_CoM + ) # connect to this msg for estimated CM else: - pltReference.vehConfigInMsg.subscribeTo(simpleMassPropsObject.vehicleConfigOutMsg) # connect to this msg for exact CM information + pltReference.vehConfigInMsg.subscribeTo( + simpleMassPropsObject.vehicleConfigOutMsg + ) # connect to this msg for exact CM information pltReference.thrusterConfigFInMsg.subscribeTo(thrConfigFMsg) pltReference.rwConfigDataInMsg.subscribeTo(fswRwConfigMsg) pltReference.rwSpeedsInMsg.subscribeTo(rwStateEffector.rwSpeedOutMsg) @@ -677,28 +757,48 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) for item in range(numRSA): saReference[item].attNavInMsg.subscribeTo(sNavObject.attOutMsg) saReference[item].attRefInMsg.subscribeTo(sepPoint.attRefOutMsg) - saReference[item].hingedRigidBodyInMsg.subscribeTo(RSAList[item].spinningBodyOutMsg) + saReference[item].hingedRigidBodyInMsg.subscribeTo( + RSAList[item].spinningBodyOutMsg + ) saReference[item].rwSpeedsInMsg.subscribeTo(rwStateEffector.rwSpeedOutMsg) saReference[item].rwConfigDataInMsg.subscribeTo(fswRwConfigMsg) if cmEstimation: saReference[item].vehConfigInMsg.subscribeTo(vcMsg_CoM) else: - saReference[item].vehConfigInMsg.subscribeTo(simpleMassPropsObject.vehicleConfigOutMsg) - saController[item].hingedRigidBodyInMsg.subscribeTo(RSAList[item].spinningBodyOutMsg) - saController[item].hingedRigidBodyRefInMsg.subscribeTo(saReference[item].hingedRigidBodyRefOutMsg) - saBoresightList[item].scStateInMsg.subscribeTo(RSAList[item].spinningBodyConfigLogOutMsg) - saBoresightList[item].celBodyInMsg.subscribeTo(gravFactory.spiceObject.planetStateOutMsgs[0]) + saReference[item].vehConfigInMsg.subscribeTo( + simpleMassPropsObject.vehicleConfigOutMsg + ) + saController[item].hingedRigidBodyInMsg.subscribeTo( + RSAList[item].spinningBodyOutMsg + ) + saController[item].hingedRigidBodyRefInMsg.subscribeTo( + saReference[item].hingedRigidBodyRefOutMsg + ) + saBoresightList[item].scStateInMsg.subscribeTo( + RSAList[item].spinningBodyConfigLogOutMsg + ) + saBoresightList[item].celBodyInMsg.subscribeTo( + gravFactory.spiceObject.planetStateOutMsgs[0] + ) for item in range(2): - pltController[item].hingedRigidBodyInMsg.subscribeTo(platform.spinningBodyOutMsgs[item]) - pltController[0].hingedRigidBodyRefInMsg.subscribeTo(pltReference.hingedRigidBodyRef1OutMsg) - pltController[1].hingedRigidBodyRefInMsg.subscribeTo(pltReference.hingedRigidBodyRef2OutMsg) + pltController[item].hingedRigidBodyInMsg.subscribeTo( + platform.spinningBodyOutMsgs[item] + ) + pltController[0].hingedRigidBodyRefInMsg.subscribeTo( + pltReference.hingedRigidBodyRef1OutMsg + ) + pltController[1].hingedRigidBodyRefInMsg.subscribeTo( + pltReference.hingedRigidBodyRef2OutMsg + ) sepThruster.cmdsInMsg.subscribeTo(thrOnTimeMsg) # # Setup data logging before the simulation is initialized # numDataPoints = simulationTime / simulationTimeStepFsw - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStepFsw, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStepFsw, numDataPoints + ) vehConfigLog = simpleMassPropsObject.vehicleConfigOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, vehConfigLog) @@ -739,15 +839,21 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) for item in range(numRSA): saAngleLogs.append(RSAList[item].spinningBodyOutMsg.recorder(samplingTime)) scSim.AddModelToTask(dynTask, saAngleLogs[item]) - saRefAngleLogs.append(saReference[item].hingedRigidBodyRefOutMsg.recorder(samplingTime)) + saRefAngleLogs.append( + saReference[item].hingedRigidBodyRefOutMsg.recorder(samplingTime) + ) scSim.AddModelToTask(dynTask, saRefAngleLogs[item]) saBoresightLogs.append(saBoresightList[item].angOutMsg.recorder(samplingTime)) scSim.AddModelToTask(dynTask, saBoresightLogs[item]) pltAngleLogs = [] pltRefAngleLogs = [] - pltRefAngleLogs.append(pltReference.hingedRigidBodyRef1OutMsg.recorder(samplingTime)) - pltRefAngleLogs.append(pltReference.hingedRigidBodyRef2OutMsg.recorder(samplingTime)) + pltRefAngleLogs.append( + pltReference.hingedRigidBodyRef1OutMsg.recorder(samplingTime) + ) + pltRefAngleLogs.append( + pltReference.hingedRigidBodyRef2OutMsg.recorder(samplingTime) + ) for item in range(2): scSim.AddModelToTask(dynTask, pltRefAngleLogs[item]) pltAngleLogs.append(platform.spinningBodyOutMsgs[item].recorder(samplingTime)) @@ -805,7 +911,9 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) dataSRPTorquePntB = srpTorqueLog.torqueExternalPntB_B dataSRPTorquePntC = [] for i in range(len(dataSRPForce)): - dataSRPTorquePntC.append(dataSRPTorquePntB[i] - np.cross(dataRealCM[i], dataSRPForce[i])) + dataSRPTorquePntC.append( + dataSRPTorquePntB[i] - np.cross(dataRealCM[i], dataSRPForce[i]) + ) dataSRPTorquePntC = np.array(dataSRPTorquePntC) thrLoc_F = thrLog.thrusterLocation @@ -813,51 +921,83 @@ def run(swirlTorque, thrMomManagement, saMomManagement, cmEstimation, showPlots) thrForce = thrLog.thrustForce thrVec_F = [] for i in range(len(thrForce)): - thrVec_F.append(thrForce[i]*thrDir_F[i]) + thrVec_F.append(thrForce[i] * thrDir_F[i]) thrVec_F = np.array(thrVec_F) # Plot the results plt.close("all") figureList = {} plot_attitude(timeData, dataSigmaBN, dataSigmaRN, figID=1) - pltName = fileName+"1"+str(int(thrMomManagement))+str(int(cmEstimation)) + pltName = fileName + "1" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(1) plot_attitude_error(timeData, dataSigmaBR, figID=2) - pltName = fileName+"2"+str(int(thrMomManagement))+str(int(cmEstimation)) + pltName = fileName + "2" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(2) plot_rw_speeds(timeData, dataOmegaRW, numRW, figID=3) - pltName = fileName+"3"+str(int(thrMomManagement))+str(int(cmEstimation)) + pltName = fileName + "3" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(3) plot_solar_array_angle(timeData, dataAlpha, dataAlphaRef, figID=4) - pltName = fileName+"4"+str(int(thrMomManagement))+str(int(cmEstimation)) + pltName = fileName + "4" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(4) plot_platform_angle(timeData, dataNu, dataNuRef, figID=5) - pltName = fileName+"5"+str(int(thrMomManagement))+str(int(cmEstimation)) + pltName = fileName + "5" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(5) - plot_thruster_cm_offset(timeData, dataRealCM, dataNu, platform.r_S1B_B, platform.dcm_S10B, thrLoc_F, thrDir_F, figID=6) - pltName = fileName+"6"+str(int(thrMomManagement))+str(int(cmEstimation)) + plot_thruster_cm_offset( + timeData, + dataRealCM, + dataNu, + platform.r_S1B_B, + platform.dcm_S10B, + thrLoc_F, + thrDir_F, + figID=6, + ) + pltName = fileName + "6" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(6) - plot_external_torque(timeData, dataSRPTorquePntC, yString='SRP', figID=7) - pltName = fileName+"7"+str(int(thrMomManagement))+str(int(cmEstimation)) + plot_external_torque(timeData, dataSRPTorquePntC, yString="SRP", figID=7) + pltName = fileName + "7" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(7) - plot_thr_torque(timeData, dataRealCM, dataNu, platform.r_S1B_B, platform.dcm_S10B, thrLoc_F, thrVec_F, THRConfig.swirlTorque, figID=8) - pltName = fileName+"8"+str(int(thrMomManagement))+str(int(cmEstimation)) + plot_thr_torque( + timeData, + dataRealCM, + dataNu, + platform.r_S1B_B, + platform.dcm_S10B, + thrLoc_F, + thrVec_F, + THRConfig.swirlTorque, + figID=8, + ) + pltName = fileName + "8" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(8) - plot_net_torques(timeData, dataRealCM, dataNu, platform.r_S1B_B, platform.dcm_S10B, thrLoc_F, thrVec_F, THRConfig.swirlTorque, dataSRPTorquePntC, figID=9) - pltName = fileName+"9"+str(int(thrMomManagement))+str(int(cmEstimation)) + plot_net_torques( + timeData, + dataRealCM, + dataNu, + platform.r_S1B_B, + platform.dcm_S10B, + thrLoc_F, + thrVec_F, + THRConfig.swirlTorque, + dataSRPTorquePntC, + figID=9, + ) + pltName = fileName + "9" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(9) plot_solar_array_pointing_error(timeData, dataSAPointing, figID=10) - pltName = fileName+"10"+str(int(thrMomManagement))+str(int(cmEstimation)) + pltName = fileName + "10" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(10) plot_neg_Y_pointing_error(timeData, dataNegYPointing, figID=11) - pltName = fileName+"11"+str(int(thrMomManagement))+str(int(cmEstimation)) + pltName = fileName + "11" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(11) if cmEstimation: plot_state_errors(timeData, dataStateError, dataCovariance, figID=12) - pltName = fileName+"12"+str(int(thrMomManagement))+str(int(cmEstimation)) + pltName = fileName + "12" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(12) - plot_residuals(timeData, dataPreFit, dataPostFit, cmEstimator.R0[0][0]**0.5, figID=13) - pltName = fileName+"13"+str(int(thrMomManagement))+str(int(cmEstimation)) + plot_residuals( + timeData, dataPreFit, dataPostFit, cmEstimator.R0[0][0] ** 0.5, figID=13 + ) + pltName = fileName + "13" + str(int(thrMomManagement)) + str(int(cmEstimation)) figureList[pltName] = plt.figure(13) if showPlots: @@ -874,62 +1014,104 @@ def plot_attitude(timeData, dataSigmaBN, dataSigmaRN, figID=None): """Plot the spacecraft attitude w.r.t. reference.""" plt.figure(figID, figsize=(5, 2.75)) for idx in range(3): - plt.plot(timeData, dataSigmaBN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_{BN,' + str(idx + 1) + '}$') + plt.plot( + timeData, + dataSigmaBN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_{BN," + str(idx + 1) + "}$", + ) for idx in range(3): - plt.plot(timeData, dataSigmaRN[:, idx], - color=unitTestSupport.getLineColor(idx, 3), linestyle='dashed', - label=r'$\sigma_{RN,' + str(idx + 1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [hours]') - plt.ylabel(r'Attitude $\sigma$') + plt.plot( + timeData, + dataSigmaRN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + linestyle="dashed", + label=r"$\sigma_{RN," + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [hours]") + plt.ylabel(r"Attitude $\sigma$") + def plot_attitude_error(timeData, dataSigmaBR, figID=None): """Plot the spacecraft attitude error.""" plt.figure(figID, figsize=(5, 2.75)) for idx in range(3): - plt.plot(timeData, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx + 1) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [hours]') - plt.ylabel(r'Attitude Tracking Error $\sigma_{B/R}$') + plt.plot( + timeData, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx + 1) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [hours]") + plt.ylabel(r"Attitude Tracking Error $\sigma_{B/R}$") + def plot_rw_speeds(timeData, dataOmegaRW, numRW, figID=None): """Plot the RW spin rates.""" plt.figure(figID, figsize=(5, 2.75)) for idx in range(numRW): - plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, - color=unitTestSupport.getLineColor(idx, numRW), - label=r'$\Omega_{' + str(idx + 1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [hours]') - plt.ylabel('RW Speed (RPM) ') + plt.plot( + timeData, + dataOmegaRW[:, idx] / macros.RPM, + color=unitTestSupport.getLineColor(idx, numRW), + label=r"$\Omega_{" + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [hours]") + plt.ylabel("RW Speed (RPM) ") + def plot_solar_array_angle(timeData, dataAngle, dataRefAngle, figID=None): """Plot the solar array angles w.r.t references.""" plt.figure(figID, figsize=(5, 2.75)) for i, angle in enumerate(dataAngle): - plt.plot(timeData, angle / np.pi * 180, color='C'+str(i), label=r'$\alpha_' + str(i+1) + '$') + plt.plot( + timeData, + angle / np.pi * 180, + color="C" + str(i), + label=r"$\alpha_" + str(i + 1) + "$", + ) for i, angle in enumerate(dataRefAngle): - plt.plot(timeData, angle / np.pi * 180, color='C'+str(i), linestyle='dashed', label=r'$\alpha_{R,' + str(i+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [hours]') - plt.ylabel(r'Solar Array Angles [deg]') + plt.plot( + timeData, + angle / np.pi * 180, + color="C" + str(i), + linestyle="dashed", + label=r"$\alpha_{R," + str(i + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [hours]") + plt.ylabel(r"Solar Array Angles [deg]") + def plot_platform_angle(timeData, dataAngle, dataRefAngle, figID=None): """Plot the platform tip and tilt angles w.r.t. references.""" - plt.figure(figID, figsize=(5,2.75)) + plt.figure(figID, figsize=(5, 2.75)) for i, angle in enumerate(dataAngle): - plt.plot(timeData, angle / np.pi * 180, color='C'+str(i), label=r'$\nu_' + str(i+1) + '$') + plt.plot( + timeData, + angle / np.pi * 180, + color="C" + str(i), + label=r"$\nu_" + str(i + 1) + "$", + ) for i, angle in enumerate(dataRefAngle): - plt.plot(timeData, angle / np.pi * 180, color='C'+str(i), linestyle='dashed', label=r'$\nu_{R,' + str(i+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [hours]') - plt.ylabel(r'Platform Angles [deg]') - -def plot_thruster_cm_offset(timeData, dataCM, dataNu, dataMB_B, dataM0B, dataThrLoc_F, dataThrDir_F, figID=None): + plt.plot( + timeData, + angle / np.pi * 180, + color="C" + str(i), + linestyle="dashed", + label=r"$\nu_{R," + str(i + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [hours]") + plt.ylabel(r"Platform Angles [deg]") + + +def plot_thruster_cm_offset( + timeData, dataCM, dataNu, dataMB_B, dataM0B, dataThrLoc_F, dataThrDir_F, figID=None +): """Plot the angle between thrust vector and system CM.""" r_MB_B = np.array([dataMB_B[0][0], dataMB_B[1][0], dataMB_B[2][0]]) dataAngOffset = [] @@ -940,33 +1122,52 @@ def plot_thruster_cm_offset(timeData, dataCM, dataNu, dataMB_B, dataM0B, dataThr r_TM_B = np.matmul(BF, dataThrLoc_F[i]) r_CT_B = dataCM[i] - r_TM_B - r_MB_B thrDir_B = np.matmul(BF, dataThrDir_F[i]) - dataAngOffset.append(np.arccos(min(max(np.dot(r_CT_B, thrDir_B) / np.linalg.norm(r_CT_B), -1), 1))) + dataAngOffset.append( + np.arccos( + min(max(np.dot(r_CT_B, thrDir_B) / np.linalg.norm(r_CT_B), -1), 1) + ) + ) cross = np.cross(r_CT_B, thrDir_B) if np.arctan2(cross[1], cross[0]) < 0: dataAngOffset[-1] = -dataAngOffset[-1] dataAngOffset = np.array(dataAngOffset) * macros.R2D plt.figure(figID, figsize=(5, 2.75)) - plt.plot(timeData, dataAngOffset, label=r'$\Delta \theta$') - plt.legend(loc='lower right') - plt.xlabel('Time [hours]') - plt.ylabel('CM Offset Ang [deg]') + plt.plot(timeData, dataAngOffset, label=r"$\Delta \theta$") + plt.legend(loc="lower right") + plt.xlabel("Time [hours]") + plt.ylabel("CM Offset Ang [deg]") + def plot_external_torque(timeData, dataTorque, yString=None, figID=None): """Plot the external torques.""" plt.figure(figID, figsize=(5, 2.75)) for idx in range(3): - plt.plot(timeData, dataTorque[:, idx] * 1000, - color=unitTestSupport.getLineColor(idx, 3), - label=r'${}^BL_' + str(idx+1) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [hours]') + plt.plot( + timeData, + dataTorque[:, idx] * 1000, + color=unitTestSupport.getLineColor(idx, 3), + label=r"${}^BL_" + str(idx + 1) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [hours]") if yString: - plt.ylabel(yString + ' Torque [mNm]') + plt.ylabel(yString + " Torque [mNm]") else: - plt.ylabel('Torque [mNm]') - -def plot_thr_torque(timeData, dataCM, dataNu, dataMB_B, dataM0B, dataThrLoc_F, dataThrVec_F, swirlTorque, figID=None): + plt.ylabel("Torque [mNm]") + + +def plot_thr_torque( + timeData, + dataCM, + dataNu, + dataMB_B, + dataM0B, + dataThrLoc_F, + dataThrVec_F, + swirlTorque, + figID=None, +): """Plot the thruster torque about CM.""" r_MB_B = np.array([dataMB_B[0][0], dataMB_B[1][0], dataMB_B[2][0]]) dataThrTorque = [] @@ -979,9 +1180,21 @@ def plot_thr_torque(timeData, dataCM, dataNu, dataMB_B, dataM0B, dataThrLoc_F, d thrVec_B = np.matmul(BF, dataThrVec_F[i]) dataThrTorque.append(np.cross(r_TC_B, thrVec_B) + thrVec_B * swirlTorque) dataThrTorque = np.array(dataThrTorque) - plot_external_torque(timeData, dataThrTorque, yString=r'Thruster', figID=figID) - -def plot_net_torques(timeData, dataCM, dataNu, dataMB_B, dataM0B, dataThrLoc_F, dataThrVec_F, swirlTorque, dataSRP, figID=None): + plot_external_torque(timeData, dataThrTorque, yString=r"Thruster", figID=figID) + + +def plot_net_torques( + timeData, + dataCM, + dataNu, + dataMB_B, + dataM0B, + dataThrLoc_F, + dataThrVec_F, + swirlTorque, + dataSRP, + figID=None, +): """Plot the net external torques in the plane perpendicular to the thrust vector.""" r_MB_B = np.array([dataMB_B[0][0], dataMB_B[1][0], dataMB_B[2][0]]) dataDeltaL = [] @@ -995,78 +1208,140 @@ def plot_net_torques(timeData, dataCM, dataNu, dataMB_B, dataM0B, dataThrLoc_F, thrTorque_B = np.cross(r_TC_B, thrVec_B) + thrVec_B * swirlTorque dataDeltaL.append(dataSRP[i] + thrTorque_B) dataDeltaL = np.array(dataDeltaL) - plot_external_torque(timeData, dataDeltaL, yString=r'Net Ext.', figID=figID) + plot_external_torque(timeData, dataDeltaL, yString=r"Net Ext.", figID=figID) + def plot_state_errors(timeData, data1, data2, figID=None): """Plot the error between estimated CM and true CM.""" plt.figure(figID, figsize=(5, 6)) - plt.subplot(3,1,1) - plt.plot(timeData, data1[:, 0]*1000, color='C0', linestyle='solid', label=r'$\Delta r_1$') - plt.plot(timeData, 3*data2[:, 0]*1000, color='C0', linestyle='dashed', label=r'$\pm 3\sigma_1$') - plt.plot(timeData, -3*data2[:, 0]*1000, color='C0', linestyle='dashed') - plt.legend(loc='upper right') - plt.ylabel('$r_{CM,1}$ [mm]') + plt.subplot(3, 1, 1) + plt.plot( + timeData, + data1[:, 0] * 1000, + color="C0", + linestyle="solid", + label=r"$\Delta r_1$", + ) + plt.plot( + timeData, + 3 * data2[:, 0] * 1000, + color="C0", + linestyle="dashed", + label=r"$\pm 3\sigma_1$", + ) + plt.plot(timeData, -3 * data2[:, 0] * 1000, color="C0", linestyle="dashed") + plt.legend(loc="upper right") + plt.ylabel("$r_{CM,1}$ [mm]") plt.grid() - plt.subplot(3,1,2) - plt.plot(timeData, data1[:, 1]*1000, color='C1', linestyle='solid', label=r'$\Delta r_2$') - plt.plot(timeData, 3*data2[:, 1]*1000, color='C1', linestyle='dashed', label=r'$\pm 3\sigma_2$') - plt.plot(timeData, -3*data2[:, 1]*1000, color='C1', linestyle='dashed') - plt.legend(loc='upper right') - plt.ylabel('$r_{CM,2}$ [mm]') + plt.subplot(3, 1, 2) + plt.plot( + timeData, + data1[:, 1] * 1000, + color="C1", + linestyle="solid", + label=r"$\Delta r_2$", + ) + plt.plot( + timeData, + 3 * data2[:, 1] * 1000, + color="C1", + linestyle="dashed", + label=r"$\pm 3\sigma_2$", + ) + plt.plot(timeData, -3 * data2[:, 1] * 1000, color="C1", linestyle="dashed") + plt.legend(loc="upper right") + plt.ylabel("$r_{CM,2}$ [mm]") plt.grid() - plt.subplot(3,1,3) - plt.plot(timeData, data1[:, 2]*1000, color='C2', linestyle='solid', label=r'$\Delta r_3$') - plt.plot(timeData, 3*data2[:, 2]*1000, color='C2', linestyle='dashed', label=r'$\pm 3\sigma_3$') - plt.plot(timeData, -3*data2[:, 2]*1000, color='C2', linestyle='dashed') - plt.legend(loc='upper right') - plt.ylabel('$r_{CM,3}$ [mm]') + plt.subplot(3, 1, 3) + plt.plot( + timeData, + data1[:, 2] * 1000, + color="C2", + linestyle="solid", + label=r"$\Delta r_3$", + ) + plt.plot( + timeData, + 3 * data2[:, 2] * 1000, + color="C2", + linestyle="dashed", + label=r"$\pm 3\sigma_3$", + ) + plt.plot(timeData, -3 * data2[:, 2] * 1000, color="C2", linestyle="dashed") + plt.legend(loc="upper right") + plt.ylabel("$r_{CM,3}$ [mm]") plt.grid() - plt.xlabel('Time [hours]') + plt.xlabel("Time [hours]") + def plot_residuals(timeData, preFit, postFit, R, figID=None): """Plot pre-fit and post-fit residuals on integral feedback torque measurements.""" plt.figure(figID, figsize=(5, 6)) plt.subplot(2, 1, 1) - plt.plot(timeData, preFit[:, 0]*1e3, color='C0', linestyle='solid', label=r'$\rho_1$') - plt.plot(timeData, preFit[:, 1]*1e3, color='C1', linestyle='solid', label=r'$\rho_2$') - plt.plot(timeData, preFit[:, 2]*1e3, color='C2', linestyle='solid', label=r'$\rho_3$') - plt.ylabel('Pre-Fit residuals [mNm]') - plt.legend(loc='upper right') + plt.plot( + timeData, preFit[:, 0] * 1e3, color="C0", linestyle="solid", label=r"$\rho_1$" + ) + plt.plot( + timeData, preFit[:, 1] * 1e3, color="C1", linestyle="solid", label=r"$\rho_2$" + ) + plt.plot( + timeData, preFit[:, 2] * 1e3, color="C2", linestyle="solid", label=r"$\rho_3$" + ) + plt.ylabel("Pre-Fit residuals [mNm]") + plt.legend(loc="upper right") plt.grid() - plt.subplot(2,1,2) - plt.plot(timeData, postFit[:, 0]*1e3, color='C0', linestyle='dashed', label=r'$\rho_1$') - plt.plot(timeData, postFit[:, 1]*1e3, color='C1', linestyle='dashed', label=r'$\rho_2$') - plt.plot(timeData, postFit[:, 2]*1e3, color='C2', linestyle='dashed', label=r'$\rho_3$') - plt.plot([timeData[0],timeData[-1]],[3000*R,3000*R], color='C3', linestyle='dashed', label=r'$\pm 3\sigma_R$') - plt.plot([timeData[0],timeData[-1]],[-3000*R,-3000*R], color='C3', linestyle='dashed') - plt.legend(loc='upper right') - plt.ylabel('Post-Fit residuals [mNm]') + plt.subplot(2, 1, 2) + plt.plot( + timeData, postFit[:, 0] * 1e3, color="C0", linestyle="dashed", label=r"$\rho_1$" + ) + plt.plot( + timeData, postFit[:, 1] * 1e3, color="C1", linestyle="dashed", label=r"$\rho_2$" + ) + plt.plot( + timeData, postFit[:, 2] * 1e3, color="C2", linestyle="dashed", label=r"$\rho_3$" + ) + plt.plot( + [timeData[0], timeData[-1]], + [3000 * R, 3000 * R], + color="C3", + linestyle="dashed", + label=r"$\pm 3\sigma_R$", + ) + plt.plot( + [timeData[0], timeData[-1]], + [-3000 * R, -3000 * R], + color="C3", + linestyle="dashed", + ) + plt.legend(loc="upper right") + plt.ylabel("Post-Fit residuals [mNm]") plt.grid() - plt.xlabel('Time [hours]') + plt.xlabel("Time [hours]") + def plot_solar_array_pointing_error(timeData, dataAngle, figID=None): """Plot the solar array angles w.r.t references.""" plt.figure(figID, figsize=(5, 2.75)) for i, angle in enumerate(dataAngle): - plt.plot(timeData, angle / np.pi * 180, color='C'+str(i), label=r'$\gamma_' + str(i+1) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [hours]') - plt.ylabel(r'Solar Array Pointing Error [deg]') + plt.plot( + timeData, + angle / np.pi * 180, + color="C" + str(i), + label=r"$\gamma_" + str(i + 1) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [hours]") + plt.ylabel(r"Solar Array Pointing Error [deg]") + def plot_neg_Y_pointing_error(timeData, dataAngle, figID=None): """Plot the solar array angles w.r.t references.""" plt.figure(figID, figsize=(5, 2.75)) - plt.plot(timeData, dataAngle / np.pi * 180, color='C'+str(3), label=r'$\delta$') - plt.legend(loc='lower right') - plt.xlabel('Time [hours]') - plt.ylabel(r'Sensitive Platform Pointing [deg]') + plt.plot(timeData, dataAngle / np.pi * 180, color="C" + str(3), label=r"$\delta$") + plt.legend(loc="lower right") + plt.xlabel("Time [hours]") + plt.ylabel(r"Sensitive Platform Pointing [deg]") if __name__ == "__main__": - run( - False, - True, - False, - True, - True - ) + run(False, True, False, True, True) diff --git a/examples/scenarioSmallBodyFeedbackControl.py b/examples/scenarioSmallBodyFeedbackControl.py index ba44477c48..e50f3c597e 100644 --- a/examples/scenarioSmallBodyFeedbackControl.py +++ b/examples/scenarioSmallBodyFeedbackControl.py @@ -81,13 +81,19 @@ 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 ( + 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 @@ -100,24 +106,24 @@ # Plotting functions 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, 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) + 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_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}}$') + 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') + 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].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() @@ -127,24 +133,24 @@ def plot_position(time, r_BO_O_truth, r_BO_O_meas): 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, 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) + 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_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[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') + 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].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() @@ -153,23 +159,23 @@ def plot_velocity(time, v_BO_O_truth, v_BO_O_meas): 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, 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) + 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_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[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]') + 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].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() @@ -178,23 +184,23 @@ def plot_sc_att(time, sigma_BN_truth, sigma_BN_meas): 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, 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) + 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_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[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]') + 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].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() @@ -203,19 +209,19 @@ def plot_sc_rate(time, omega_BN_B_truth, omega_BN_B_meas): def plot_control(time, u): plt.gcf() - fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6)) + 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) + 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) + 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]') + 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]') + 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 @@ -247,7 +253,7 @@ def run(show_plots): # Setup celestial object ephemeris module gravBodyEphem = planetEphemeris.PlanetEphemeris() - gravBodyEphem.ModelTag = 'planetEphemeris' + gravBodyEphem.ModelTag = "planetEphemeris" gravBodyEphem.setPlanetNames(planetEphemeris.StringVector(["bennu"])) # specify orbits of gravitational bodies @@ -260,14 +266,18 @@ def run(show_plots): 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(astroConstants.MU_SUN*(1000.**3), oeAsteroid) + r_ON_N, v_ON_N = orbitalMotion.elem2rv( + astroConstants.MU_SUN * (1000.0**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.rightAscension = planetEphemeris.DoubleVector([0.0 * macros.D2R]) + gravBodyEphem.declination = planetEphemeris.DoubleVector([90.0 * macros.D2R]) gravBodyEphem.lst0 = planetEphemeris.DoubleVector([0.0 * macros.D2R]) - gravBodyEphem.rotRate = planetEphemeris.DoubleVector([360 * macros.D2R / (4.297461 * 3600.)]) + gravBodyEphem.rotRate = planetEphemeris.DoubleVector( + [360 * macros.D2R / (4.297461 * 3600.0)] + ) # setup Sun Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() @@ -293,8 +303,12 @@ def run(show_plots): 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 + r_BO_N = np.array( + [-2000.0, 1500.0, 1000.0] + ) # Position of the spacecraft relative to the body + v_BO_N = np.array( + [0.0, 0.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) @@ -306,7 +320,7 @@ def run(show_plots): I = [82.12, 0.0, 0.0, 0.0, 98.40, 0.0, 0.0, 0.0, 121.0] - mass = 330. # kg + mass = 330.0 # kg scObject.hub.mHub = mass scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) @@ -318,12 +332,24 @@ def run(show_plots): 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 - ) + RW1 = rwFactory.create( + "Honeywell_HR16", + [1, 0, 0], + maxMomentum=100.0, + Omega=100.0, # RPM + ) + RW2 = rwFactory.create( + "Honeywell_HR16", + [0, 1, 0], + maxMomentum=100.0, + Omega=200.0, # RPM + ) + RW3 = rwFactory.create( + "Honeywell_HR16", + [0, 0, 1], + maxMomentum=100.0, + Omega=300.0, # RPM + ) # create RW object container and tie to spacecraft object rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector() @@ -332,8 +358,10 @@ def run(show_plots): rwConfigMsg = rwFactory.getConfigMessage() # Create an SRP model - srp = radiationPressure.RadiationPressure() # default model is the SRP_CANNONBALL_MODEL - srp.area = 1. # m^3 + srp = ( + radiationPressure.RadiationPressure() + ) # default model is the SRP_CANNONBALL_MODEL + srp.area = 1.0 # m^3 srp.coefficientReflection = 1.9 scObject.addDynamicEffector(srp) srp.sunEphmInMsg.subscribeTo(sunPlanetStateMsg) @@ -345,7 +373,7 @@ def run(show_plots): # Set up simpleNav for s/c "measurements" simpleNavMeas = simpleNav.SimpleNav() - simpleNavMeas.ModelTag = 'SimpleNav' + simpleNavMeas.ModelTag = "SimpleNav" simpleNavMeas.scStateInMsg.subscribeTo(scObject.scStateOutMsg) pos_sigma_sc = 30.0 vel_sigma_sc = 0.01 @@ -353,25 +381,388 @@ def run(show_plots): 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.]] + p_matrix_sc = [ + [ + pos_sigma_sc, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 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, + 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, + 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, + 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, + 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, + 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, + 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, + 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, + ], + [ + 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, + 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, + 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, + 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, + 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, + 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, + 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, + 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, + 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.0], + [10.0], + [10.0], + [0.001], + [0.001], + [0.001], + [0.005], + [0.005], + [0.005], + [0.002], + [0.002], + [0.002], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + ] simpleNavMeas.PMatrix = p_matrix_sc simpleNavMeas.walkBounds = walk_bounds_sc @@ -383,19 +774,34 @@ def run(show_plots): 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]] + p_matrix_p = [ + [pos_sigma_p, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 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, 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, 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, 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, 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], + [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, 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, 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, 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, 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, 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.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 @@ -423,8 +829,8 @@ def run(show_plots): mrpFeedbackControl.vehConfigInMsg.subscribeTo(vcConfigMsg) mrpFeedbackControl.K = 7.0 mrpFeedbackControl.Ki = -1 - mrpFeedbackControl.P = 30. - mrpFeedbackControl.integralLimit = 2. / mrpFeedbackControl.Ki * 0.1 + mrpFeedbackControl.P = 30.0 + mrpFeedbackControl.integralLimit = 2.0 / mrpFeedbackControl.Ki * 0.1 # add module that maps the Lr control torque into the RW motor torques rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() @@ -444,11 +850,11 @@ def run(show_plots): 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.A_sc = 1.0 # 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.x1_ref = [-2000.0, 0.0, 0.0] waypointFeedback.x2_ref = [0.0, 0.0, 0.0] extForceTorqueModule = extForceTorque.ExtForceTorque() @@ -487,13 +893,18 @@ def run(show_plots): scSim.AddModelToTask(simTaskName, requested_control_recorder) scSim.AddModelToTask(simTaskName, attitude_error_recorder) - fileName = 'scenarioSmallBodyFeedbackControl' + fileName = "scenarioSmallBodyFeedbackControl" if vizSupport.vizFound: - vizInterface = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) - vizSupport.createStandardCamera(vizInterface, setMode=0, bodyTarget='bennu', setView=0) + 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 @@ -505,8 +916,12 @@ def run(show_plots): 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.]) + 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, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + ) # configure a simulation stop time and execute the simulation run scSim.ConfigureStopTime(simulationTime_1) @@ -531,7 +946,9 @@ def run(show_plots): 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): + 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) diff --git a/examples/scenarioSmallBodyLandmarks.py b/examples/scenarioSmallBodyLandmarks.py index 73bbce9e70..6c353da64f 100644 --- a/examples/scenarioSmallBodyLandmarks.py +++ b/examples/scenarioSmallBodyLandmarks.py @@ -141,7 +141,9 @@ from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody from Basilisk.utilities import RigidBodyKinematics -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # attempt to import vizard from Basilisk.utilities import vizSupport @@ -152,26 +154,26 @@ # Landmark distribution function def landmark_distribution(vert_list, face_list, n_vert, n_face, n_lmk): - """Creates a landmark distribution based on a polyhedron shape.""" - pos_vert = np.array(vert_list) - order_face = np.array(face_list) - 1 - pos_face = np.zeros((n_face,3)) - normal_face = np.zeros((n_face,3)) - for i in range(n_face): - pos0 = pos_vert[order_face[i, 0], 0:3] - pos1 = pos_vert[order_face[i, 1], 0:3] - pos2 = pos_vert[order_face[i, 2], 0:3] - pos_face[i, 0:3] = (pos0 + pos1 + pos2) / 3 - normal_face[i, 0:3] = np.cross(pos1-pos0, pos2-pos0) - normal_face[i, 0:3] /= np.linalg.norm(normal_face[i, 0:3]) - - np.random.seed(0) - idx_lmk = np.random.choice(n_face, n_lmk, replace=False) - idx_lmk.sort() - pos_lmk = pos_face[idx_lmk, 0:3] - normal_lmk = normal_face[idx_lmk, 0:3] - - return pos_lmk, normal_lmk + """Creates a landmark distribution based on a polyhedron shape.""" + pos_vert = np.array(vert_list) + order_face = np.array(face_list) - 1 + pos_face = np.zeros((n_face, 3)) + normal_face = np.zeros((n_face, 3)) + for i in range(n_face): + pos0 = pos_vert[order_face[i, 0], 0:3] + pos1 = pos_vert[order_face[i, 1], 0:3] + pos2 = pos_vert[order_face[i, 2], 0:3] + pos_face[i, 0:3] = (pos0 + pos1 + pos2) / 3 + normal_face[i, 0:3] = np.cross(pos1 - pos0, pos2 - pos0) + normal_face[i, 0:3] /= np.linalg.norm(normal_face[i, 0:3]) + + np.random.seed(0) + idx_lmk = np.random.choice(n_face, n_lmk, replace=False) + idx_lmk.sort() + pos_lmk = pos_face[idx_lmk, 0:3] + normal_lmk = normal_face[idx_lmk, 0:3] + + return pos_lmk, normal_lmk def plot_3D(t, r, xyz_vert, order_face, posLmk, isvisibleLmk, dcm_CP): @@ -181,25 +183,76 @@ def plot_3D(t, r, xyz_vert, order_face, posLmk, isvisibleLmk, dcm_CP): color_asteroid = [105 / 255, 105 / 255, 105 / 255] fig = plt.figure() - ax = fig.add_subplot(1, 1, 1, projection='3d') - ax.plot(r[0] / 1000, r[1] / 1000, r[2] / 1000, 'k', marker='s', markersize=5) - ax.plot_trisurf(xyz_vert[:, 0] / 1000, xyz_vert[:, 1] / 1000, xyz_vert[:, 2] / 1000, - triangles=order_face-1, color=color_asteroid, zorder=0, alpha=0.1) - ax.plot(posLmk[idxOn, 0] / 1000, posLmk[idxOn, 1] / 1000, posLmk[idxOn, 2] / 1000, 'b', linestyle='', - marker='.', markersize=5) - ax.plot(posLmk[idxOff, 0] / 1000, posLmk[idxOff, 1] / 1000, posLmk[idxOff, 2] / 1000, 'r', linestyle='', - marker='.', markersize=5, alpha=0.25) - ax.quiver(r[0] / 1000, r[1] / 1000, r[2] / 1000, dcm_CP[0,0], dcm_CP[1,0], dcm_CP[2,0], length=10, normalize=True, - color='black', alpha=0.25) - ax.quiver(r[0] / 1000, r[1] / 1000, r[2] / 1000, dcm_CP[0,1], dcm_CP[1,1], dcm_CP[2,1], length=10, normalize=True, - color='black', alpha=0.25) - ax.quiver(r[0] / 1000, r[1] / 1000, r[2] / 1000, dcm_CP[0,2], dcm_CP[1,2], dcm_CP[2,2], length=10, normalize=True, - color='blue') - ax.set_xlabel('${}^{P}r_{x}$ [km]') - ax.set_ylabel('${}^{P}r_{y}$ [km]') - ax.set_zlabel('${}^{P}r_{z}$ [km]') - t_str = str(int(t/60)) - ax.set_title('Asteroid rotating frame, t=' + t_str + ' min') + ax = fig.add_subplot(1, 1, 1, projection="3d") + ax.plot(r[0] / 1000, r[1] / 1000, r[2] / 1000, "k", marker="s", markersize=5) + ax.plot_trisurf( + xyz_vert[:, 0] / 1000, + xyz_vert[:, 1] / 1000, + xyz_vert[:, 2] / 1000, + triangles=order_face - 1, + color=color_asteroid, + zorder=0, + alpha=0.1, + ) + ax.plot( + posLmk[idxOn, 0] / 1000, + posLmk[idxOn, 1] / 1000, + posLmk[idxOn, 2] / 1000, + "b", + linestyle="", + marker=".", + markersize=5, + ) + ax.plot( + posLmk[idxOff, 0] / 1000, + posLmk[idxOff, 1] / 1000, + posLmk[idxOff, 2] / 1000, + "r", + linestyle="", + marker=".", + markersize=5, + alpha=0.25, + ) + ax.quiver( + r[0] / 1000, + r[1] / 1000, + r[2] / 1000, + dcm_CP[0, 0], + dcm_CP[1, 0], + dcm_CP[2, 0], + length=10, + normalize=True, + color="black", + alpha=0.25, + ) + ax.quiver( + r[0] / 1000, + r[1] / 1000, + r[2] / 1000, + dcm_CP[0, 1], + dcm_CP[1, 1], + dcm_CP[2, 1], + length=10, + normalize=True, + color="black", + alpha=0.25, + ) + ax.quiver( + r[0] / 1000, + r[1] / 1000, + r[2] / 1000, + dcm_CP[0, 2], + dcm_CP[1, 2], + dcm_CP[2, 2], + length=10, + normalize=True, + color="blue", + ) + ax.set_xlabel("${}^{P}r_{x}$ [km]") + ax.set_ylabel("${}^{P}r_{y}$ [km]") + ax.set_zlabel("${}^{P}r_{z}$ [km]") + t_str = str(int(t / 60)) + ax.set_title("Asteroid rotating frame, t=" + t_str + " min") def plot_pixel(pixelLmk, statusLmk): @@ -209,45 +262,54 @@ def plot_pixel(pixelLmk, statusLmk): fig = plt.figure() ax = fig.gca() - plt.plot(pixelOn[:,0], pixelOn[:,1], linestyle='', marker='s', color='b', markersize=5) + plt.plot( + pixelOn[:, 0], pixelOn[:, 1], linestyle="", marker="s", color="b", markersize=5 + ) for i in range(len(pixelOn)): - ax.annotate(str(i), (pixelOn[i,0], pixelOn[i,1])) + ax.annotate(str(i), (pixelOn[i, 0], pixelOn[i, 1])) ax.set_xlim([-1024, 1024]) ax.set_ylim([-768, 768]) - plt.xlabel('$p_x$ [-]') - plt.ylabel('$p_y$ [-]') + plt.xlabel("$p_x$ [-]") + plt.ylabel("$p_y$ [-]") def plot_nLmk(t, nvisibleLmk): """Plot visible landmarks evolution.""" fig = plt.figure() ax = fig.gca() - plt.plot(t/3600, nvisibleLmk, linestyle='--', marker='.', markersize=8) - ax.set_xlim([t[0]/3600, t[-1]/3600]) - plt.xlabel('Time [h]') - plt.ylabel('Visible landmarks [-]') + plt.plot(t / 3600, nvisibleLmk, linestyle="--", marker=".", markersize=8) + ax.set_xlim([t[0] / 3600, t[-1] / 3600]) + plt.xlabel("Time [h]") + plt.ylabel("Visible landmarks [-]") def plot_orientation(t, dcm_HP, dcm_CP): """Plot the camera frame orientation with respect to Hill frame.""" data = np.zeros((len(t), 3)) for i in range(len(t)): - data[i,0:3] = [np.dot(dcm_HP[i, 0:3, 0], dcm_CP[i, 0:3, 2]), - np.dot(dcm_HP[i, 0:3, 1], dcm_CP[i, 0:3, 0]), - np.dot(dcm_HP[i, 0:3, 2], dcm_CP[i, 0:3, 1])] + data[i, 0:3] = [ + np.dot(dcm_HP[i, 0:3, 0], dcm_CP[i, 0:3, 2]), + np.dot(dcm_HP[i, 0:3, 1], dcm_CP[i, 0:3, 0]), + np.dot(dcm_HP[i, 0:3, 2], dcm_CP[i, 0:3, 1]), + ] fig = plt.figure() ax = fig.gca() - labelStrings = (r'$\hat\imath_r\cdot \hat c_3$' - , r'${\hat\imath}_{\theta}\cdot \hat c_1$' - , r'$\hat\imath_h\cdot \hat c_2$') + labelStrings = ( + r"$\hat\imath_r\cdot \hat c_3$", + r"${\hat\imath}_{\theta}\cdot \hat c_1$", + r"$\hat\imath_h\cdot \hat c_2$", + ) for idx in range(3): - plt.plot(t/3600, data[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=labelStrings[idx]) - ax.set_xlim([t[0]/3600, t[-1]/3600]) - plt.legend(loc='lower right') - plt.xlabel('Time [h]') - plt.ylabel('Orientation Illustration') + plt.plot( + t / 3600, + data[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=labelStrings[idx], + ) + ax.set_xlim([t[0] / 3600, t[-1] / 3600]) + plt.legend(loc="lower right") + plt.xlabel("Time [h]") + plt.ylabel("Orientation Illustration") def run(show_plots, useBatch): @@ -268,7 +330,7 @@ def run(show_plots, useBatch): simProcessName = "simProcess" # Define the simulation duration - simulationTime = macros.min2nano(100.) + simulationTime = macros.min2nano(100.0) # Create the process dynProcess = scSim.CreateNewProcess(simProcessName) @@ -283,33 +345,35 @@ def run(show_plots, useBatch): # Setup celestial object ephemeris module gravBodyEphem = planetEphemeris.PlanetEphemeris() - gravBodyEphem.ModelTag = 'erosEphemeris' + gravBodyEphem.ModelTag = "erosEphemeris" gravBodyEphem.setPlanetNames(planetEphemeris.StringVector(["eros"])) # Specify asteroid orbit elements and rotational state January 21st, 2022 # https://ssd.jpl.nasa.gov/horizons.cgi#results oeAsteroid = planetEphemeris.ClassicElements() - oeAsteroid.a = 1.4583 * 149597870.7*1e3 # meters + oeAsteroid.a = 1.4583 * 149597870.7 * 1e3 # meters oeAsteroid.e = 0.2227 - oeAsteroid.i = 10.829 * np.pi/180 - oeAsteroid.Omega = 304.3 * np.pi/180 - oeAsteroid.omega = 178.9 * np.pi/180 - oeAsteroid.f = 246.9 * np.pi/180 - AR = 11.369 * np.pi/180 - dec = 17.227 * np.pi/180 + oeAsteroid.i = 10.829 * np.pi / 180 + oeAsteroid.Omega = 304.3 * np.pi / 180 + oeAsteroid.omega = 178.9 * np.pi / 180 + oeAsteroid.f = 246.9 * np.pi / 180 + AR = 11.369 * np.pi / 180 + dec = 17.227 * np.pi / 180 lst0 = 0 * macros.D2R gravBodyEphem.planetElements = planetEphemeris.classicElementVector([oeAsteroid]) gravBodyEphem.rightAscension = planetEphemeris.DoubleVector([AR]) gravBodyEphem.declination = planetEphemeris.DoubleVector([dec]) gravBodyEphem.lst0 = planetEphemeris.DoubleVector([lst0]) - gravBodyEphem.rotRate = planetEphemeris.DoubleVector([360 * macros.D2R / (5.27 * 3600.)]) - dcm_PN = RigidBodyKinematics.euler3232C([AR, np.pi/2 - dec, lst0]) + gravBodyEphem.rotRate = planetEphemeris.DoubleVector( + [360 * macros.D2R / (5.27 * 3600.0)] + ) + dcm_PN = RigidBodyKinematics.euler3232C([AR, np.pi / 2 - dec, lst0]) # Set up asteroid gravity effector (only Keplerian gravity for simplicity) # https://ssd.jpl.nasa.gov/tools/gravity.html#/vesta gravFactory = simIncludeGravBody.gravBodyFactory() mu = 4.4631 * 1e5 - asteroid = gravFactory.createCustomGravObject("eros", mu=mu, radEquator=16*1000) + asteroid = gravFactory.createCustomGravObject("eros", mu=mu, radEquator=16 * 1000) asteroid.isCentralBody = True asteroid.planetBodyInMsg.subscribeTo(gravBodyEphem.planetOutMsgs[0]) @@ -321,9 +385,7 @@ def run(show_plots, useBatch): # Initialize spacecraft object and set properties scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) @@ -379,26 +441,26 @@ def run(show_plots, useBatch): mrpControl.K = 3.5 mrpControl.Ki = -1.0 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # Connect torque command to external torque effector extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) # Prepare 100 landmarks distribution - polyFile = bskPath + '/supportData/LocalGravData/eros007790.tab' + polyFile = bskPath + "/supportData/LocalGravData/eros007790.tab" vert_list, face_list, n_vert, n_face = loadPolyFromFileToList(polyFile) n_lmk = 100 - pos_lmk, normal_lmk = landmark_distribution(vert_list, face_list, n_vert, n_face, n_lmk) + pos_lmk, normal_lmk = landmark_distribution( + vert_list, face_list, n_vert, n_face, n_lmk + ) # Set the pinhole camera module camera = pinholeCamera.PinholeCamera() - camera.f = 25*1e-3 + camera.f = 25 * 1e-3 camera.nxPixel = 2048 camera.nyPixel = 1536 - camera.wPixel = (17.3*1e-3) / 2048 - dcm_CB = np.array([[0, 0, -1], - [0, 1, 0], - [1, 0, 0]]) + camera.wPixel = (17.3 * 1e-3) / 2048 + dcm_CB = np.array([[0, 0, -1], [0, 1, 0], [1, 0, 0]]) camera.dcm_CB = dcm_CB.tolist() for i in range(n_lmk): camera.addLandmark(pos_lmk[i, 0:3], normal_lmk[i, 0:3]) @@ -507,31 +569,43 @@ def run(show_plots, useBatch): pixelBatchLmk = np.zeros((n, n_lmk, 2)) # Process pinhole camera as a batch - camera.processBatch(r_BP_P, mrp_BP, -r_PN_P / np.linalg.norm(r_PN_P, axis=1)[:, None], False) + camera.processBatch( + r_BP_P, mrp_BP, -r_PN_P / np.linalg.norm(r_PN_P, axis=1)[:, None], False + ) isvisibleBatchLmk = np.array(camera.isvisibleBatchLmk) nvisibleBatchLmk = np.sum(isvisibleBatchLmk, axis=1) pixelBatchLmk[:, :, 0] = np.array(camera.pixelBatchLmk)[:, 0:n_lmk] - pixelBatchLmk[:, :, 1] = np.array(camera.pixelBatchLmk)[:, n_lmk:2*n_lmk] + pixelBatchLmk[:, :, 1] = np.array(camera.pixelBatchLmk)[:, n_lmk : 2 * n_lmk] # Ensure that results are equal as BSK sim - batch_diff = np.array([np.linalg.norm(nvisibleBatchLmk - nvisibleLmk) / np.linalg.norm(nvisibleLmk), - np.linalg.norm(isvisibleBatchLmk - isvisibleLmk) / np.linalg.norm(isvisibleLmk), - np.linalg.norm(pixelBatchLmk - pixelLmk) / np.linalg.norm(pixelLmk)]) + batch_diff = np.array( + [ + np.linalg.norm(nvisibleBatchLmk - nvisibleLmk) + / np.linalg.norm(nvisibleLmk), + np.linalg.norm(isvisibleBatchLmk - isvisibleLmk) + / np.linalg.norm(isvisibleLmk), + np.linalg.norm(pixelBatchLmk - pixelLmk) / np.linalg.norm(pixelLmk), + ] + ) else: batch_diff = 0 # Plot results of interest figureList = {} - plot_3D(t0, r0, np.array(vert_list), np.array(face_list), pos_lmk, isvisibleLmk0, dcm0) + plot_3D( + t0, r0, np.array(vert_list), np.array(face_list), pos_lmk, isvisibleLmk0, dcm0 + ) pltName = fileName + "1" figureList[pltName] = plt.figure(1) - plot_3D(tf, rf, np.array(vert_list), np.array(face_list), pos_lmk, isvisibleLmkf, dcmf) + plot_3D( + tf, rf, np.array(vert_list), np.array(face_list), pos_lmk, isvisibleLmkf, dcmf + ) pltName = fileName + "2" figureList[pltName] = plt.figure(2) - plot_pixel(pixelLmk[-1, : , :], isvisibleLmk[-1, :]) + plot_pixel(pixelLmk[-1, :, :], isvisibleLmk[-1, :]) pltName = fileName + "3" figureList[pltName] = plt.figure(3) @@ -558,6 +632,6 @@ def run(show_plots, useBatch): # if __name__ == "__main__": run( - True, # show_plots - False # useBatch + True, # show_plots + False, # useBatch ) diff --git a/examples/scenarioSmallBodyNav.py b/examples/scenarioSmallBodyNav.py index 52acfcf614..8853be4b51 100644 --- a/examples/scenarioSmallBodyNav.py +++ b/examples/scenarioSmallBodyNav.py @@ -102,7 +102,12 @@ 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 ( + SimulationBaseClass, + macros, + simIncludeGravBody, + vizSupport, +) from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeRW from Basilisk.utilities import unitTestSupport @@ -116,29 +121,29 @@ # Plotting functions def plot_position(time, meas_time, r_BO_O_truth, r_BO_O_est, r_BO_O_meas): """Plot the relative position result.""" - #plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + # 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(meas_time, r_BO_O_meas[:, 0], 'k*', label='measurement', markersize=1) - ax[1].plot(meas_time, r_BO_O_meas[:, 1], 'k*', markersize=1) - ax[2].plot(meas_time, r_BO_O_meas[:, 2], 'k*', markersize=1) + ax[0].plot(meas_time, r_BO_O_meas[:, 0], "k*", label="measurement", markersize=1) + ax[1].plot(meas_time, r_BO_O_meas[:, 1], "k*", markersize=1) + ax[2].plot(meas_time, r_BO_O_meas[:, 2], "k*", markersize=1) - ax[0].plot(time, r_BO_O_truth[:, 0], label=r'${}^Or_{BO_{1}}$') - ax[1].plot(time, r_BO_O_truth[:, 1], label=r'${}^Or_{BO_{2}}$') - ax[2].plot(time, r_BO_O_truth[:, 2], label=r'${}^Or_{BO_{3}}$') + ax[0].plot(time, r_BO_O_truth[:, 0], label=r"${}^Or_{BO_{1}}$") + ax[1].plot(time, r_BO_O_truth[:, 1], label=r"${}^Or_{BO_{2}}$") + ax[2].plot(time, r_BO_O_truth[:, 2], label=r"${}^Or_{BO_{3}}$") - ax[0].plot(time, r_BO_O_est[:, 0], label='estimate') + ax[0].plot(time, r_BO_O_est[:, 0], label="estimate") ax[1].plot(time, r_BO_O_est[:, 1]) ax[2].plot(time, r_BO_O_est[:, 2]) - plt.xlabel('Time [sec]') - plt.title('Relative Spacecraft Position') + plt.xlabel("Time [sec]") + plt.title("Relative Spacecraft Position") - ax[0].set_ylabel(r'${}^O r_{BO_{1}}$ [m]') - ax[1].set_ylabel(r'${}^O r_{BO_{2}}$ [m]') - ax[2].set_ylabel(r'${}^O r_{BO_{3}}$ [m]') + ax[0].set_ylabel(r"${}^O r_{BO_{1}}$ [m]") + ax[1].set_ylabel(r"${}^O r_{BO_{2}}$ [m]") + ax[2].set_ylabel(r"${}^O r_{BO_{3}}$ [m]") ax[0].legend() @@ -148,28 +153,28 @@ def plot_position(time, meas_time, r_BO_O_truth, r_BO_O_est, r_BO_O_meas): def plot_velocity(time, meas_time, v_BO_O_truth, v_BO_O_est, v_BO_O_meas): """Plot the relative velocity result.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(meas_time, v_BO_O_meas[:, 0], 'k*', label='measurement', markersize=1) - ax[1].plot(meas_time, v_BO_O_meas[:, 1], 'k*', markersize=1) - ax[2].plot(meas_time, v_BO_O_meas[:, 2], 'k*', markersize=1) + ax[0].plot(meas_time, v_BO_O_meas[:, 0], "k*", label="measurement", markersize=1) + ax[1].plot(meas_time, v_BO_O_meas[:, 1], "k*", markersize=1) + ax[2].plot(meas_time, v_BO_O_meas[:, 2], "k*", markersize=1) - ax[0].plot(time, v_BO_O_truth[:, 0], label='truth') + 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]) - ax[0].plot(time, v_BO_O_est[:, 0], label='estimate') + ax[0].plot(time, v_BO_O_est[:, 0], label="estimate") ax[1].plot(time, v_BO_O_est[:, 1]) ax[2].plot(time, v_BO_O_est[:, 2]) - plt.xlabel('Time [sec]') - plt.title('Relative Spacecraft Velocity') + plt.xlabel("Time [sec]") + plt.title("Relative Spacecraft Velocity") - ax[0].set_ylabel(r'${}^Ov_{BO_1}$ [m/s]') - ax[1].set_ylabel(r'${}^Ov_{BO_2}$ [m/s]') - ax[2].set_ylabel(r'${}^Ov_{BO_3}$ [m/s]') + ax[0].set_ylabel(r"${}^Ov_{BO_1}$ [m/s]") + ax[1].set_ylabel(r"${}^Ov_{BO_2}$ [m/s]") + ax[2].set_ylabel(r"${}^Ov_{BO_3}$ [m/s]") ax[0].legend() @@ -180,28 +185,28 @@ def plot_pos_error(time, r_err, P): """Plot the position estimation error and associated covariance.""" # plt.figure(3) plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time, r_err[:, 0], label='error') - ax[0].plot(time, 2*np.sqrt(P[:, 0, 0]), 'k--', label=r'$2\sigma$') - ax[0].plot(time, -2*np.sqrt(P[:, 0, 0]), 'k--') + ax[0].plot(time, r_err[:, 0], label="error") + ax[0].plot(time, 2 * np.sqrt(P[:, 0, 0]), "k--", label=r"$2\sigma$") + ax[0].plot(time, -2 * np.sqrt(P[:, 0, 0]), "k--") ax[1].plot(time, r_err[:, 1]) - ax[1].plot(time, 2*np.sqrt(P[:, 1, 1]), 'k--') - ax[1].plot(time, -2*np.sqrt(P[:, 1, 1]), 'k--') + ax[1].plot(time, 2 * np.sqrt(P[:, 1, 1]), "k--") + ax[1].plot(time, -2 * np.sqrt(P[:, 1, 1]), "k--") ax[2].plot(time, r_err[:, 2]) - ax[2].plot(time, 2*np.sqrt(P[:, 2, 2]), 'k--') - ax[2].plot(time, -2*np.sqrt(P[:, 2, 2]), 'k--') + ax[2].plot(time, 2 * np.sqrt(P[:, 2, 2]), "k--") + ax[2].plot(time, -2 * np.sqrt(P[:, 2, 2]), "k--") - plt.xlabel('Time [sec]') - plt.title('Position Error and Covariance') + plt.xlabel("Time [sec]") + plt.title("Position Error and Covariance") - ax[0].set_ylabel(r'${}^Or_{BO_1}$ Error [m]') - ax[1].set_ylabel(r'${}^Or_{BO_2}$ Error [m]') - ax[2].set_ylabel(r'${}^Or_{BO_3}$ Error [m]') + ax[0].set_ylabel(r"${}^Or_{BO_1}$ Error [m]") + ax[1].set_ylabel(r"${}^Or_{BO_2}$ Error [m]") + ax[2].set_ylabel(r"${}^Or_{BO_3}$ Error [m]") ax[0].legend() @@ -212,28 +217,28 @@ def plot_vel_error(time, v_err, P): """Plot the position estimation error and associated covariance.""" # plt.figure(4) plt.gcf() - fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time, v_err[:, 0], label='error') - ax[0].plot(time, 2*np.sqrt(P[:, 3, 3]), 'k--', label=r'$2\sigma$') - ax[0].plot(time, -2*np.sqrt(P[:, 3, 3]), 'k--') + ax[0].plot(time, v_err[:, 0], label="error") + ax[0].plot(time, 2 * np.sqrt(P[:, 3, 3]), "k--", label=r"$2\sigma$") + ax[0].plot(time, -2 * np.sqrt(P[:, 3, 3]), "k--") ax[1].plot(time, v_err[:, 1]) - ax[1].plot(time, 2*np.sqrt(P[:, 4, 4]), 'k--') - ax[1].plot(time, -2*np.sqrt(P[:, 4, 4]), 'k--') + ax[1].plot(time, 2 * np.sqrt(P[:, 4, 4]), "k--") + ax[1].plot(time, -2 * np.sqrt(P[:, 4, 4]), "k--") ax[2].plot(time, v_err[:, 2]) - ax[2].plot(time, 2*np.sqrt(P[:, 5, 5]), 'k--') - ax[2].plot(time, -2*np.sqrt(P[:, 5, 5]), 'k--') + ax[2].plot(time, 2 * np.sqrt(P[:, 5, 5]), "k--") + ax[2].plot(time, -2 * np.sqrt(P[:, 5, 5]), "k--") - plt.xlabel('Time [sec]') - plt.title('Velocity Error and Covariance') + plt.xlabel("Time [sec]") + plt.title("Velocity Error and Covariance") - ax[0].set_ylabel('${}^Ov_{BO_1}$ Error [m/s]') - ax[1].set_ylabel('${}^Ov_{BO_2}$ Error [m/s]') - ax[2].set_ylabel('${}^Ov_{BO_3}$ Error [m/s]') + ax[0].set_ylabel("${}^Ov_{BO_1}$ Error [m/s]") + ax[1].set_ylabel("${}^Ov_{BO_2}$ Error [m/s]") + ax[2].set_ylabel("${}^Ov_{BO_3}$ Error [m/s]") ax[0].legend() @@ -242,27 +247,27 @@ def plot_vel_error(time, v_err, P): def plot_sc_att(time, meas_time, sigma_BN_truth, sigma_BN_est, sigma_BN_meas): plt.gcf() - fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(meas_time, sigma_BN_meas[:, 0], 'k*', label='measurement', markersize=1) - ax[1].plot(meas_time, sigma_BN_meas[:, 1], 'k*', markersize=1) - ax[2].plot(meas_time, sigma_BN_meas[:, 2], 'k*', markersize=1) + ax[0].plot(meas_time, sigma_BN_meas[:, 0], "k*", label="measurement", markersize=1) + ax[1].plot(meas_time, sigma_BN_meas[:, 1], "k*", markersize=1) + ax[2].plot(meas_time, sigma_BN_meas[:, 2], "k*", markersize=1) - ax[0].plot(time, sigma_BN_truth[:, 0], label='truth') + 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]) - ax[0].plot(time, sigma_BN_est[:, 0], label='estimate') + ax[0].plot(time, sigma_BN_est[:, 0], label="estimate") ax[1].plot(time, sigma_BN_est[:, 1]) ax[2].plot(time, sigma_BN_est[:, 2]) - plt.xlabel('Time [sec]') + 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].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() @@ -271,27 +276,29 @@ def plot_sc_att(time, meas_time, sigma_BN_truth, sigma_BN_est, sigma_BN_meas): def plot_sc_rate(time, meas_time, omega_BN_B_truth, omega_BN_B_est, omega_BN_B_meas): plt.gcf() - fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(meas_time, omega_BN_B_meas[:, 0], 'k*', label='measurement', markersize=1) - ax[1].plot(meas_time, omega_BN_B_meas[:, 1], 'k*', markersize=1) - ax[2].plot(meas_time, omega_BN_B_meas[:, 2], 'k*', markersize=1) + ax[0].plot( + meas_time, omega_BN_B_meas[:, 0], "k*", label="measurement", markersize=1 + ) + ax[1].plot(meas_time, omega_BN_B_meas[:, 1], "k*", markersize=1) + ax[2].plot(meas_time, omega_BN_B_meas[:, 2], "k*", markersize=1) - ax[0].plot(time, omega_BN_B_truth[:, 0], label='truth') + 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]) - ax[0].plot(time, omega_BN_B_est[:, 0], label='estimate') + ax[0].plot(time, omega_BN_B_est[:, 0], label="estimate") ax[1].plot(time, omega_BN_B_est[:, 1]) ax[2].plot(time, omega_BN_B_est[:, 2]) - plt.xlabel('Time [sec]') + 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].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() @@ -300,27 +307,27 @@ def plot_sc_rate(time, meas_time, omega_BN_B_truth, omega_BN_B_est, omega_BN_B_m def plot_ast_att(time, meas_time, sigma_AN_truth, sigma_AN_est, sigma_AN_meas): plt.gcf() - fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(meas_time, sigma_AN_meas[:, 0], 'k*', label='measurement', markersize=1) - ax[1].plot(meas_time, sigma_AN_meas[:, 1], 'k*', markersize=1) - ax[2].plot(meas_time, sigma_AN_meas[:, 2], 'k*', markersize=1) + ax[0].plot(meas_time, sigma_AN_meas[:, 0], "k*", label="measurement", markersize=1) + ax[1].plot(meas_time, sigma_AN_meas[:, 1], "k*", markersize=1) + ax[2].plot(meas_time, sigma_AN_meas[:, 2], "k*", markersize=1) - ax[0].plot(time, sigma_AN_truth[:, 0], label='truth') + ax[0].plot(time, sigma_AN_truth[:, 0], label="truth") ax[1].plot(time, sigma_AN_truth[:, 1]) ax[2].plot(time, sigma_AN_truth[:, 2]) - ax[0].plot(time, sigma_AN_est[:, 0], label='estimate') + ax[0].plot(time, sigma_AN_est[:, 0], label="estimate") ax[1].plot(time, sigma_AN_est[:, 1]) ax[2].plot(time, sigma_AN_est[:, 2]) - plt.xlabel('Time [sec]') + plt.xlabel("Time [sec]") - ax[0].set_ylabel(r'$\sigma_{AN_{1}}$ [rad]') - ax[1].set_ylabel(r'$\sigma_{AN_{2}}$ [rad]') - ax[2].set_ylabel(r'$\sigma_{AN_{3}}$ [rad]') + ax[0].set_ylabel(r"$\sigma_{AN_{1}}$ [rad]") + ax[1].set_ylabel(r"$\sigma_{AN_{2}}$ [rad]") + ax[2].set_ylabel(r"$\sigma_{AN_{3}}$ [rad]") ax[0].legend() @@ -329,27 +336,29 @@ def plot_ast_att(time, meas_time, sigma_AN_truth, sigma_AN_est, sigma_AN_meas): def plot_ast_rate(time, meas_time, omega_AN_A_truth, omega_AN_A_est, omega_AN_A_meas): plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(meas_time, omega_AN_A_meas[:, 0], 'k*', label='measurement', markersize=1) - ax[1].plot(meas_time, omega_AN_A_meas[:, 1], 'k*', markersize=1) - ax[2].plot(meas_time, omega_AN_A_meas[:, 2], 'k*', markersize=1) + ax[0].plot( + meas_time, omega_AN_A_meas[:, 0], "k*", label="measurement", markersize=1 + ) + ax[1].plot(meas_time, omega_AN_A_meas[:, 1], "k*", markersize=1) + ax[2].plot(meas_time, omega_AN_A_meas[:, 2], "k*", markersize=1) - ax[0].plot(time, omega_AN_A_truth[:, 0], label='truth') + ax[0].plot(time, omega_AN_A_truth[:, 0], label="truth") ax[1].plot(time, omega_AN_A_truth[:, 1]) ax[2].plot(time, omega_AN_A_truth[:, 2]) - ax[0].plot(time, omega_AN_A_est[:, 0], label='estimate') + ax[0].plot(time, omega_AN_A_est[:, 0], label="estimate") ax[1].plot(time, omega_AN_A_est[:, 1]) ax[2].plot(time, omega_AN_A_est[:, 2]) - ax[0].set_ylabel(r'${}^A\omega_{AN_{1}}$ [rad/s]') - ax[1].set_ylabel(r'${}^A\omega_{AN_{2}}$ [rad/s]') - ax[2].set_ylabel(r'${}^A\omega_{AN_{3}}$ [rad/s]') + ax[0].set_ylabel(r"${}^A\omega_{AN_{1}}$ [rad/s]") + ax[1].set_ylabel(r"${}^A\omega_{AN_{2}}$ [rad/s]") + ax[2].set_ylabel(r"${}^A\omega_{AN_{3}}$ [rad/s]") - plt.xlabel('Time [sec]') + plt.xlabel("Time [sec]") ax[0].legend() @@ -359,28 +368,28 @@ def plot_ast_rate(time, meas_time, omega_AN_A_truth, omega_AN_A_est, omega_AN_A_ def plot_ast_attitude_error(time, sigma_err, P): """Plot the asteroid attitude estimation error and associated covariance.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time, sigma_err[:, 0], label='error') - ax[0].plot(time, 2*np.sqrt(P[:, 6, 6]), 'k--', label=r'$2\sigma$') - ax[0].plot(time, -2*np.sqrt(P[:, 6, 6]), 'k--') + ax[0].plot(time, sigma_err[:, 0], label="error") + ax[0].plot(time, 2 * np.sqrt(P[:, 6, 6]), "k--", label=r"$2\sigma$") + ax[0].plot(time, -2 * np.sqrt(P[:, 6, 6]), "k--") ax[1].plot(time, sigma_err[:, 1]) - ax[1].plot(time, 2*np.sqrt(P[:, 7, 7]), 'k--') - ax[1].plot(time, -2*np.sqrt(P[:, 7, 7]), 'k--') + ax[1].plot(time, 2 * np.sqrt(P[:, 7, 7]), "k--") + ax[1].plot(time, -2 * np.sqrt(P[:, 7, 7]), "k--") ax[2].plot(time, sigma_err[:, 2]) - ax[2].plot(time, 2*np.sqrt(P[:, 8, 8]), 'k--') - ax[2].plot(time, -2*np.sqrt(P[:, 8, 8]), 'k--') + ax[2].plot(time, 2 * np.sqrt(P[:, 8, 8]), "k--") + ax[2].plot(time, -2 * np.sqrt(P[:, 8, 8]), "k--") - plt.xlabel('Time [sec]') - plt.title('Attitude Error and Covariance') + plt.xlabel("Time [sec]") + plt.title("Attitude Error and Covariance") - ax[0].set_ylabel(r'$\sigma_{AN_{1}}$ Error [rad]') - ax[1].set_ylabel(r'$\sigma_{AN_{2}}$ Error [rad]') - ax[2].set_ylabel(r'$\sigma_{AN_{3}}$ Error [rad]') + ax[0].set_ylabel(r"$\sigma_{AN_{1}}$ Error [rad]") + ax[1].set_ylabel(r"$\sigma_{AN_{2}}$ Error [rad]") + ax[2].set_ylabel(r"$\sigma_{AN_{3}}$ Error [rad]") ax[0].legend() @@ -390,28 +399,28 @@ def plot_ast_attitude_error(time, sigma_err, P): def plot_ast_rate_error(time, omega_err, P): """Plot the asteroid rate estimation error and associated covariance.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time, omega_err[:, 0], label='error') - ax[0].plot(time, 2*np.sqrt(P[:, 9, 9]), 'k--', label=r'$2\sigma$') - ax[0].plot(time, -2*np.sqrt(P[:, 9, 9]), 'k--') + ax[0].plot(time, omega_err[:, 0], label="error") + ax[0].plot(time, 2 * np.sqrt(P[:, 9, 9]), "k--", label=r"$2\sigma$") + ax[0].plot(time, -2 * np.sqrt(P[:, 9, 9]), "k--") ax[1].plot(time, omega_err[:, 1]) - ax[1].plot(time, 2*np.sqrt(P[:, 10, 10]), 'k--') - ax[1].plot(time, -2*np.sqrt(P[:, 10, 10]), 'k--') + ax[1].plot(time, 2 * np.sqrt(P[:, 10, 10]), "k--") + ax[1].plot(time, -2 * np.sqrt(P[:, 10, 10]), "k--") ax[2].plot(time, omega_err[:, 2]) - ax[2].plot(time, 2*np.sqrt(P[:, 11, 11]), 'k--') - ax[2].plot(time, -2*np.sqrt(P[:, 11, 11]), 'k--') + ax[2].plot(time, 2 * np.sqrt(P[:, 11, 11]), "k--") + ax[2].plot(time, -2 * np.sqrt(P[:, 11, 11]), "k--") - plt.xlabel('Time [sec]') - plt.title('Position Error and Covariance') + plt.xlabel("Time [sec]") + plt.title("Position Error and Covariance") - ax[0].set_ylabel(r'${}^A\omega_{AN_{1}}$ Error [rad/s]') - ax[1].set_ylabel(r'${}^A\omega_{AN_{2}}$ Error [rad/s]') - ax[2].set_ylabel(r'${}^A\omega_{AN_{3}}$ Error [rad/s]') + ax[0].set_ylabel(r"${}^A\omega_{AN_{1}}$ Error [rad/s]") + ax[1].set_ylabel(r"${}^A\omega_{AN_{2}}$ Error [rad/s]") + ax[2].set_ylabel(r"${}^A\omega_{AN_{3}}$ Error [rad/s]") ax[0].legend() @@ -451,7 +460,7 @@ def run(show_plots): # setup celestial object ephemeris module gravBodyEphem = planetEphemeris.PlanetEphemeris() - gravBodyEphem.ModelTag = 'planetEphemeris' + gravBodyEphem.ModelTag = "planetEphemeris" gravBodyEphem.setPlanetNames(planetEphemeris.StringVector(["bennu"])) # specify orbits of gravitational bodies @@ -464,7 +473,9 @@ def run(show_plots): 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) + r_ON_N, v_ON_N = orbitalMotion.elem2rv( + orbitalMotion.MU_SUN * (1000.0**3), oeAsteroid + ) # specify celestial object orbit gravBodyEphem.planetElements = planetEphemeris.classicElementVector([oeAsteroid]) @@ -472,7 +483,9 @@ def run(show_plots): gravBodyEphem.rightAscension = planetEphemeris.DoubleVector([86.6388 * macros.D2R]) gravBodyEphem.declination = planetEphemeris.DoubleVector([-65.1086 * macros.D2R]) gravBodyEphem.lst0 = planetEphemeris.DoubleVector([0.0 * macros.D2R]) - gravBodyEphem.rotRate = planetEphemeris.DoubleVector([360 * macros.D2R / (4.297461 * 3600.)]) + gravBodyEphem.rotRate = planetEphemeris.DoubleVector( + [360 * macros.D2R / (4.297461 * 3600.0)] + ) # setup Sun Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() @@ -489,10 +502,9 @@ def run(show_plots): sunEphemerisMsg.write(sunEphemerisMsgData) mu = 4.892 # m^3/s^2 - asteroid = gravFactory.createCustomGravObject("bennu", mu - , modelDictionaryKey="Bennu" - , radEquator=565. / 2.0 - ) + asteroid = gravFactory.createCustomGravObject( + "bennu", mu, modelDictionaryKey="Bennu", radEquator=565.0 / 2.0 + ) asteroid.planetBodyInMsg.subscribeTo(gravBodyEphem.planetOutMsgs[0]) # create SC object @@ -501,8 +513,12 @@ def run(show_plots): gravFactory.addBodiesTo(scObject) # Create the position and velocity of states of the s/c wrt the small body hill frame - r_BO_N = np.array([2000., 1500., 1000.]) # Position of the spacecraft relative to the body - v_BO_N = np.array([1., 1., 1.]) # Velocity of the spacecraft relative to the body + r_BO_N = np.array( + [2000.0, 1500.0, 1000.0] + ) # Position of the spacecraft relative to the body + v_BO_N = np.array( + [1.0, 1.0, 1.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) @@ -514,7 +530,7 @@ def run(show_plots): I = [82.12, 0.0, 0.0, 0.0, 98.40, 0.0, 0.0, 0.0, 121.0] - mass = 330. # kg + mass = 330.0 # kg scObject.hub.mHub = mass scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) @@ -526,12 +542,24 @@ def run(show_plots): 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=50., Omega=100. # RPM - ) - RW2 = rwFactory.create('Honeywell_HR16', [0, 1, 0], maxMomentum=50., Omega=200. # RPM - ) - RW3 = rwFactory.create('Honeywell_HR16', [0, 0, 1], maxMomentum=50., Omega=300. # RPM - ) + RW1 = rwFactory.create( + "Honeywell_HR16", + [1, 0, 0], + maxMomentum=50.0, + Omega=100.0, # RPM + ) + RW2 = rwFactory.create( + "Honeywell_HR16", + [0, 1, 0], + maxMomentum=50.0, + Omega=200.0, # RPM + ) + RW3 = rwFactory.create( + "Honeywell_HR16", + [0, 0, 1], + maxMomentum=50.0, + Omega=300.0, # RPM + ) # create RW object container and tie to spacecraft object rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector() @@ -545,8 +573,10 @@ def run(show_plots): thrusterMsg.write(thrusterMsgData) # Create an SRP model - srp = radiationPressure.RadiationPressure() # default model is the SRP_CANNONBALL_MODEL - srp.area = 3. # m^3 + srp = ( + radiationPressure.RadiationPressure() + ) # default model is the SRP_CANNONBALL_MODEL + srp.area = 3.0 # m^3 srp.coefficientReflection = 0.9 scObject.addDynamicEffector(srp) srp.sunEphmInMsg.subscribeTo(sunPlanetStateMsg) @@ -558,38 +588,401 @@ def run(show_plots): # Set up simpleNav for s/c "measurements" simpleNavMeas = simpleNav.SimpleNav() - simpleNavMeas.ModelTag = 'SimpleNav' + simpleNavMeas.ModelTag = "SimpleNav" simpleNavMeas.scStateInMsg.subscribeTo(scObject.scStateOutMsg) pos_sigma_sc = 40.0 vel_sigma_sc = 0.05 - att_sigma_sc = 0. * math.pi / 180.0 - rate_sigma_sc = 0. * math.pi / 180.0 + att_sigma_sc = 0.0 * math.pi / 180.0 + rate_sigma_sc = 0.0 * 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.01], [0.01], [0.01], [0.005], [0.005], [0.005], [0.002], [0.002], [0.002], [0.], [0.], [0.], [0.], [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, + 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, + 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, + 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, + 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, + 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, + 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, + 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, + 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, + ], + [ + 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, + 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, + 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, + 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, + 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, + 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, + 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, + 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, + 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.0], + [10.0], + [10.0], + [0.01], + [0.01], + [0.01], + [0.005], + [0.005], + [0.005], + [0.002], + [0.002], + [0.002], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + ] simpleNavMeas.PMatrix = p_matrix_sc simpleNavMeas.walkBounds = walk_bounds_sc simpleNavMeas2 = simpleNav.SimpleNav() - simpleNavMeas2.ModelTag = 'SimpleNav2' + simpleNavMeas2.ModelTag = "SimpleNav2" simpleNavMeas2.scStateInMsg.subscribeTo(scObject.scStateOutMsg) simpleNavMeas2.PMatrix = p_matrix_sc simpleNavMeas2.walkBounds = walk_bounds_sc @@ -602,19 +995,34 @@ def run(show_plots): vel_sigma_p = 0.0 att_sigma_p = 1.0 * math.pi / 180.0 rate_sigma_p = 0.1 * 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]] + p_matrix_p = [ + [pos_sigma_p, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 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, 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, 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, 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, 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], + [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, 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, 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, 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, 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, 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.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 @@ -637,10 +1045,10 @@ def run(show_plots): mrpFeedbackControl.ModelTag = "mrpFeedbackControl" mrpFeedbackControl.guidInMsg.subscribeTo(trackingError.attGuidOutMsg) mrpFeedbackControl.vehConfigInMsg.subscribeTo(vcConfigMsg) - mrpFeedbackControl.K = 7. + mrpFeedbackControl.K = 7.0 mrpFeedbackControl.Ki = -1.0 - mrpFeedbackControl.P = 35. - mrpFeedbackControl.integralLimit = 2. / mrpFeedbackControl.Ki * 0.1 + mrpFeedbackControl.P = 35.0 + mrpFeedbackControl.integralLimit = 2.0 / mrpFeedbackControl.Ki * 0.1 # add module that maps the Lr control torque into the RW motor torques rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() @@ -656,11 +1064,11 @@ def run(show_plots): waypointFeedback.asteroidEphemerisInMsg.subscribeTo(planetNavMeas.ephemerisOutMsg) waypointFeedback.sunEphemerisInMsg.subscribeTo(sunEphemerisMsg) waypointFeedback.navAttInMsg.subscribeTo(simpleNavMeas2.attOutMsg) - waypointFeedback.A_sc = 1. # Surface area of the spacecraft, m^2 + waypointFeedback.A_sc = 1.0 # 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.x1_ref = [-2000.0, 0.0, 0.0] waypointFeedback.x2_ref = [0.0, 0.0, 0.0] extForceTorqueModule = extForceTorque.ExtForceTorque() @@ -672,35 +1080,35 @@ def run(show_plots): smallBodyNav.ModelTag = "smallBodyNavEKF" # Set the filter parameters (sc area, mass, gravitational constants, etc.) - smallBodyNav.A_sc = 1. # Surface area of the spacecraft, m^2 + smallBodyNav.A_sc = 1.0 # Surface area of the spacecraft, m^2 smallBodyNav.M_sc = mass # Mass of the spacecraft, kg smallBodyNav.mu_ast = mu # Gravitational constant of the asteroid # Set the process noise - Q = np.zeros((12,12)) - Q[0,0] = Q[1,1] = Q[2,2] = 0.0000001 - Q[3,3] = Q[4,4] = Q[5,5] = 0.000001 - Q[6,6] = Q[7,7] = Q[8,8] = 0.000001 - Q[9,9] = Q[10,10] = Q[11,11] = 0.0000001 + Q = np.zeros((12, 12)) + Q[0, 0] = Q[1, 1] = Q[2, 2] = 0.0000001 + Q[3, 3] = Q[4, 4] = Q[5, 5] = 0.000001 + Q[6, 6] = Q[7, 7] = Q[8, 8] = 0.000001 + Q[9, 9] = Q[10, 10] = Q[11, 11] = 0.0000001 smallBodyNav.Q = Q.tolist() # Set the measurement noise - R = np.zeros((12,12)) - R[0,0] = R[1,1] = R[2,2] = pos_sigma_sc # position sigmas - R[3,3] = R[4,4] = R[5,5] = vel_sigma_sc # velocity sigmas - R[6,6] = R[7,7] = R[8,8] = att_sigma_p - R[9,9] = R[10,10] = R[11,11] = rate_sigma_p + R = np.zeros((12, 12)) + R[0, 0] = R[1, 1] = R[2, 2] = pos_sigma_sc # position sigmas + R[3, 3] = R[4, 4] = R[5, 5] = vel_sigma_sc # velocity sigmas + R[6, 6] = R[7, 7] = R[8, 8] = att_sigma_p + R[9, 9] = R[10, 10] = R[11, 11] = rate_sigma_p smallBodyNav.R = np.multiply(R, R).tolist() # Measurement Noise # Set the initial guess, x_0 x_0 = np.zeros(18) - x_0[0:3] = np.array([2458., -704.08, 844.275]) + x_0[0:3] = np.array([2458.0, -704.08, 844.275]) x_0[3:6] = np.array([1.475, -0.176, 0.894]) x_0[6:9] = np.array([-0.58, 0.615, 0.125]) x_0[11] = 0.0004 smallBodyNav.x_hat_k = x_0 # Set the covariance to something large - smallBodyNav.P_k = (0.1*np.identity(12)).tolist() + smallBodyNav.P_k = (0.1 * np.identity(12)).tolist() # Connect the relevant modules to the smallBodyEKF input messages smallBodyNav.navTransInMsg.subscribeTo(simpleNavMeas.transOutMsg) @@ -716,8 +1124,12 @@ def run(show_plots): waypointFeedback.navTransInMsg.subscribeTo(smallBodyNav.navTransOutMsg) # Set the waypoint feedback gains - 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.]) + 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, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + ) # Add all models to the task scSim.AddModelToTask(simTaskName, scObject, 100) @@ -755,9 +1167,12 @@ def run(show_plots): scSim.AddModelToTask(measTaskName, ast_ephemeris_meas_recorder) if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) viz.settings.showSpacecraftLabels = 1 # initialize Simulation @@ -768,19 +1183,19 @@ def run(show_plots): scSim.ConfigureStopTime(simulationTime) scSim.ExecuteSimulation() - scSim.ConfigureStopTime(simulationTime + macros.sec2nano(1000.)) + scSim.ConfigureStopTime(simulationTime + macros.sec2nano(1000.0)) scSim.disableTask(measTaskName) scSim.ExecuteSimulation() - scSim.ConfigureStopTime(simulationTime + macros.sec2nano(2000.)) + scSim.ConfigureStopTime(simulationTime + macros.sec2nano(2000.0)) scSim.enableTask(measTaskName) scSim.ExecuteSimulation() - scSim.ConfigureStopTime(simulationTime + macros.sec2nano(3000.)) + scSim.ConfigureStopTime(simulationTime + macros.sec2nano(3000.0)) scSim.disableTask(measTaskName) scSim.ExecuteSimulation() - scSim.ConfigureStopTime(simulationTime + macros.sec2nano(4000.)) + scSim.ConfigureStopTime(simulationTime + macros.sec2nano(4000.0)) scSim.enableTask(measTaskName) scSim.ExecuteSimulation() @@ -809,8 +1224,8 @@ def run(show_plots): for rd_N, vd_N, rc_N, vc_N in zip(r_BN_N_truth, v_BN_N_truth, r_AN_N, v_AN_N): dcm_ON = orbitalMotion.hillFrame(rc_N, vc_N) - r_BO_O_truth.append(np.matmul(dcm_ON, rd_N-rc_N)) - v_BO_O_truth.append(np.matmul(dcm_ON, vd_N-vc_N)) + r_BO_O_truth.append(np.matmul(dcm_ON, rd_N - rc_N)) + v_BO_O_truth.append(np.matmul(dcm_ON, vd_N - vc_N)) for idx, t in enumerate(meas_time): truth_idx = np.where(time == t)[0][0] @@ -822,42 +1237,58 @@ def run(show_plots): dcm_ON = orbitalMotion.hillFrame(rc_N, vc_N) - r_BO_O_meas.append(np.matmul(dcm_ON, rd_N_meas-rc_N)) - v_BO_O_meas.append(np.matmul(dcm_ON, vd_N_meas-vc_N)) + r_BO_O_meas.append(np.matmul(dcm_ON, rd_N_meas - rc_N)) + v_BO_O_meas.append(np.matmul(dcm_ON, vd_N_meas - vc_N)) # # plot the results # - plot_position(time, meas_time, np.array(r_BO_O_truth), x_hat[:,0:3], np.array(r_BO_O_meas)) + plot_position( + time, meas_time, np.array(r_BO_O_truth), x_hat[:, 0:3], np.array(r_BO_O_meas) + ) figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) - plot_velocity(time, meas_time, np.array(v_BO_O_truth), x_hat[:,3:6], np.array(v_BO_O_meas)) + plot_velocity( + time, meas_time, np.array(v_BO_O_truth), x_hat[:, 3:6], np.array(v_BO_O_meas) + ) pltName = fileName + "2" figureList[pltName] = plt.figure(2) - plot_pos_error(time, np.subtract(r_BO_O_truth, x_hat[:,0:3]), P) + plot_pos_error(time, np.subtract(r_BO_O_truth, x_hat[:, 0:3]), P) pltName = fileName + "3" figureList[pltName] = plt.figure(3) - plot_vel_error(time, np.subtract(v_BO_O_truth,x_hat[:,3:6]), P) + plot_vel_error(time, np.subtract(v_BO_O_truth, x_hat[:, 3:6]), P) pltName = fileName + "4" figureList[pltName] = plt.figure(4) - plot_ast_att(time, meas_time, np.array(sigma_AN_truth), x_hat[:,6:9], np.array(sigma_AN_meas)) + plot_ast_att( + time, + meas_time, + np.array(sigma_AN_truth), + x_hat[:, 6:9], + np.array(sigma_AN_meas), + ) pltName = fileName + "5" figureList[pltName] = plt.figure(5) - plot_ast_rate(time, meas_time, np.array(omega_AN_A_truth), x_hat[:,9:12], np.array(omega_AN_A_meas)) + plot_ast_rate( + time, + meas_time, + np.array(omega_AN_A_truth), + x_hat[:, 9:12], + np.array(omega_AN_A_meas), + ) pltName = fileName + "6" figureList[pltName] = plt.figure(6) - plot_ast_attitude_error(time, np.subtract(sigma_AN_truth, x_hat[:,6:9]), P) + plot_ast_attitude_error(time, np.subtract(sigma_AN_truth, x_hat[:, 6:9]), P) pltName = fileName + "7" figureList[pltName] = plt.figure(7) - plot_ast_rate_error(time, np.subtract(omega_AN_A_truth, x_hat[:,9:12]), P) + plot_ast_rate_error(time, np.subtract(omega_AN_A_truth, x_hat[:, 9:12]), P) pltName = fileName + "8" figureList[pltName] = plt.figure(8) diff --git a/examples/scenarioSmallBodyNavUKF.py b/examples/scenarioSmallBodyNavUKF.py index 03d932e9d0..d31dd8c625 100644 --- a/examples/scenarioSmallBodyNavUKF.py +++ b/examples/scenarioSmallBodyNavUKF.py @@ -93,7 +93,7 @@ from Basilisk.simulation import simpleNav from Basilisk.simulation import spacecraft from Basilisk.utilities import RigidBodyKinematics -from Basilisk.utilities import (SimulationBaseClass, macros, simIncludeGravBody) +from Basilisk.utilities import SimulationBaseClass, macros, simIncludeGravBody from Basilisk.utilities import orbitalMotion from Basilisk.utilities import unitTestSupport @@ -101,125 +101,130 @@ # Used to get the location of supporting data. fileName = os.path.basename(os.path.splitext(__file__)[0]) + # Plotting functions -def plot_3Dposition(r_truth, title='None'): +def plot_3Dposition(r_truth, title="None"): """Plot the relative position in 3D.""" fig = plt.figure() - ax = fig.add_subplot(1, 1, 1, projection='3d') - ax.plot(r_truth[:, 0] / 1000., r_truth[:, 1]/1000., r_truth[:,2]/1000., 'b') - if title == 'inertial': - ax.set_xlabel('${}^{N}r_{x}$ [km]') - ax.set_ylabel('${}^{N}r_{y}$ [km]') - ax.set_zlabel('${}^{N}r_{z}$ [km]') - ax.set_title('Inertial frame') - elif title == 'asteroid': - ax.set_xlabel('${}^{A}r_{x}$ [km]') - ax.set_ylabel('${}^{A}r_{y}$ [km]') - ax.set_zlabel('${}^{A}r_{z}$ [km]') - ax.set_title('Small body fixed frame') + ax = fig.add_subplot(1, 1, 1, projection="3d") + ax.plot(r_truth[:, 0] / 1000.0, r_truth[:, 1] / 1000.0, r_truth[:, 2] / 1000.0, "b") + if title == "inertial": + ax.set_xlabel("${}^{N}r_{x}$ [km]") + ax.set_ylabel("${}^{N}r_{y}$ [km]") + ax.set_zlabel("${}^{N}r_{z}$ [km]") + ax.set_title("Inertial frame") + elif title == "asteroid": + ax.set_xlabel("${}^{A}r_{x}$ [km]") + ax.set_ylabel("${}^{A}r_{y}$ [km]") + ax.set_zlabel("${}^{A}r_{z}$ [km]") + ax.set_title("Small body fixed frame") + def plot_position(time, r_truth, r_est): """Plot the relative position result.""" - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time/(3600*24), r_truth[:, 0]/1000, 'b', label='truth') - ax[1].plot(time/(3600*24), r_truth[:, 1]/1000, 'b') - ax[2].plot(time/(3600*24), r_truth[:, 2]/1000, 'b') + ax[0].plot(time / (3600 * 24), r_truth[:, 0] / 1000, "b", label="truth") + ax[1].plot(time / (3600 * 24), r_truth[:, 1] / 1000, "b") + ax[2].plot(time / (3600 * 24), r_truth[:, 2] / 1000, "b") - ax[0].plot(time/(3600*24), r_est[:, 0]/1000, color='orange', label='estimate') - ax[1].plot(time/(3600*24), r_est[:, 1]/1000, color='orange') - ax[2].plot(time/(3600*24), r_est[:, 2]/1000, color='orange') + ax[0].plot(time / (3600 * 24), r_est[:, 0] / 1000, color="orange", label="estimate") + ax[1].plot(time / (3600 * 24), r_est[:, 1] / 1000, color="orange") + ax[2].plot(time / (3600 * 24), r_est[:, 2] / 1000, color="orange") - plt.xlabel('Time [days]') - plt.title('Spacecraft Position') + plt.xlabel("Time [days]") + plt.title("Spacecraft Position") - ax[0].set_ylabel('${}^{A}r_{x}$ [km]') - ax[1].set_ylabel('${}^{A}r_{y}$ [km]') - ax[2].set_ylabel('${}^{A}r_{z}$ [km]') + ax[0].set_ylabel("${}^{A}r_{x}$ [km]") + ax[1].set_ylabel("${}^{A}r_{y}$ [km]") + ax[2].set_ylabel("${}^{A}r_{z}$ [km]") ax[0].legend() return + def plot_velocity(time, v_truth, v_est): """Plot the relative velocity result.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time/(3600*24), v_truth[:, 0], 'b', label='truth') - ax[1].plot(time/(3600*24), v_truth[:, 1], 'b') - ax[2].plot(time/(3600*24), v_truth[:, 2], 'b') + ax[0].plot(time / (3600 * 24), v_truth[:, 0], "b", label="truth") + ax[1].plot(time / (3600 * 24), v_truth[:, 1], "b") + ax[2].plot(time / (3600 * 24), v_truth[:, 2], "b") - ax[0].plot(time/(3600*24), v_est[:, 0], color='orange', label='estimate') - ax[1].plot(time/(3600*24), v_est[:, 1], color='orange') - ax[2].plot(time/(3600*24), v_est[:, 2], color='orange') + ax[0].plot(time / (3600 * 24), v_est[:, 0], color="orange", label="estimate") + ax[1].plot(time / (3600 * 24), v_est[:, 1], color="orange") + ax[2].plot(time / (3600 * 24), v_est[:, 2], color="orange") - plt.xlabel('Time [days]') - plt.title('Spacecraft Velocity') + plt.xlabel("Time [days]") + plt.title("Spacecraft Velocity") - ax[0].set_ylabel('${}^{A}v_{x}$ [m/s]') - ax[1].set_ylabel('${}^{A}v_{y}$ [m/s]') - ax[2].set_ylabel('${}^{A}v_{z}$ [m/s]') + ax[0].set_ylabel("${}^{A}v_{x}$ [m/s]") + ax[1].set_ylabel("${}^{A}v_{y}$ [m/s]") + ax[2].set_ylabel("${}^{A}v_{z}$ [m/s]") ax[0].legend() return + def plot_acceleration(time, a_truth, a_est): """Plot the non-Keplerian acceleration result.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time/(3600*24), a_truth[:, 0]*1000, 'b', label='truth') - ax[1].plot(time/(3600*24), a_truth[:, 1]*1000, 'b') - ax[2].plot(time/(3600*24), a_truth[:, 2]*1000, 'b') + ax[0].plot(time / (3600 * 24), a_truth[:, 0] * 1000, "b", label="truth") + ax[1].plot(time / (3600 * 24), a_truth[:, 1] * 1000, "b") + ax[2].plot(time / (3600 * 24), a_truth[:, 2] * 1000, "b") - ax[0].plot(time/(3600*24), a_est[:, 0]*1000, color='orange', label='estimate') - ax[1].plot(time/(3600*24), a_est[:, 1]*1000, color='orange') - ax[2].plot(time/(3600*24), a_est[:, 2]*1000, color='orange') + ax[0].plot(time / (3600 * 24), a_est[:, 0] * 1000, color="orange", label="estimate") + ax[1].plot(time / (3600 * 24), a_est[:, 1] * 1000, color="orange") + ax[2].plot(time / (3600 * 24), a_est[:, 2] * 1000, color="orange") - plt.xlabel('Time [days]') - plt.title('Inhomogeneous gravity acceleration') + plt.xlabel("Time [days]") + plt.title("Inhomogeneous gravity acceleration") - ax[0].set_ylabel('${}^{A}a_{x}$ [mm/s$^2$]') - ax[1].set_ylabel('${}^{A}a_{y}$ [mm/s$^2$]') - ax[2].set_ylabel('${}^{A}a_{z}$ [mm/s$^2$]') + ax[0].set_ylabel("${}^{A}a_{x}$ [mm/s$^2$]") + ax[1].set_ylabel("${}^{A}a_{y}$ [mm/s$^2$]") + ax[2].set_ylabel("${}^{A}a_{z}$ [mm/s$^2$]") ax[0].legend() return + def plot_pos_error(time, r_err, P): """Plot the position estimation error and associated covariance.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time/(3600*24), r_err[:, 0], 'b', label='error') - ax[0].plot(time/(3600*24), 2*np.sqrt(P[:, 0, 0]), 'k--', label=r'$2\sigma$') - ax[0].plot(time/(3600*24), -2*np.sqrt(P[:, 0, 0]), 'k--') + ax[0].plot(time / (3600 * 24), r_err[:, 0], "b", label="error") + ax[0].plot(time / (3600 * 24), 2 * np.sqrt(P[:, 0, 0]), "k--", label=r"$2\sigma$") + ax[0].plot(time / (3600 * 24), -2 * np.sqrt(P[:, 0, 0]), "k--") - ax[1].plot(time/(3600*24), r_err[:, 1], 'b') - ax[1].plot(time/(3600*24), 2*np.sqrt(P[:, 1, 1]), 'k--') - ax[1].plot(time/(3600*24), -2*np.sqrt(P[:, 1, 1]), 'k--') + ax[1].plot(time / (3600 * 24), r_err[:, 1], "b") + ax[1].plot(time / (3600 * 24), 2 * np.sqrt(P[:, 1, 1]), "k--") + ax[1].plot(time / (3600 * 24), -2 * np.sqrt(P[:, 1, 1]), "k--") - ax[2].plot(time/(3600*24), r_err[:, 2], 'b') - ax[2].plot(time/(3600*24), 2*np.sqrt(P[:, 2, 2]), 'k--') - ax[2].plot(time/(3600*24), -2*np.sqrt(P[:, 2, 2]), 'k--') + ax[2].plot(time / (3600 * 24), r_err[:, 2], "b") + ax[2].plot(time / (3600 * 24), 2 * np.sqrt(P[:, 2, 2]), "k--") + ax[2].plot(time / (3600 * 24), -2 * np.sqrt(P[:, 2, 2]), "k--") - plt.xlabel('Time [days]') - plt.title('Position Error and Covariance') + plt.xlabel("Time [days]") + plt.title("Position Error and Covariance") - ax[0].set_ylabel('${}^{A}r_{x}$ [m]') - ax[1].set_ylabel('${}^{A}r_{y}$ [m]') - ax[2].set_ylabel('${}^{A}r_{z}$ [m]') + ax[0].set_ylabel("${}^{A}r_{x}$ [m]") + ax[1].set_ylabel("${}^{A}r_{y}$ [m]") + ax[2].set_ylabel("${}^{A}r_{z}$ [m]") ax[0].legend() @@ -229,58 +234,61 @@ def plot_pos_error(time, r_err, P): def plot_vel_error(time, v_err, P): """Plot the velocity estimation error and associated covariance.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time/(3600*24), v_err[:, 0], 'b', label='error') - ax[0].plot(time/(3600*24), 2*np.sqrt(P[:, 3, 3]), 'k--', label=r'$2\sigma$') - ax[0].plot(time/(3600*24), -2*np.sqrt(P[:, 3, 3]), 'k--') + ax[0].plot(time / (3600 * 24), v_err[:, 0], "b", label="error") + ax[0].plot(time / (3600 * 24), 2 * np.sqrt(P[:, 3, 3]), "k--", label=r"$2\sigma$") + ax[0].plot(time / (3600 * 24), -2 * np.sqrt(P[:, 3, 3]), "k--") - ax[1].plot(time/(3600*24), v_err[:, 1], 'b') - ax[1].plot(time/(3600*24), 2*np.sqrt(P[:, 4, 4]), 'k--') - ax[1].plot(time/(3600*24), -2*np.sqrt(P[:, 4, 4]), 'k--') + ax[1].plot(time / (3600 * 24), v_err[:, 1], "b") + ax[1].plot(time / (3600 * 24), 2 * np.sqrt(P[:, 4, 4]), "k--") + ax[1].plot(time / (3600 * 24), -2 * np.sqrt(P[:, 4, 4]), "k--") - ax[2].plot(time/(3600*24), v_err[:, 2], 'b') - ax[2].plot(time/(3600*24), 2*np.sqrt(P[:, 5, 5]), 'k--') - ax[2].plot(time/(3600*24), -2*np.sqrt(P[:, 5, 5]), 'k--') + ax[2].plot(time / (3600 * 24), v_err[:, 2], "b") + ax[2].plot(time / (3600 * 24), 2 * np.sqrt(P[:, 5, 5]), "k--") + ax[2].plot(time / (3600 * 24), -2 * np.sqrt(P[:, 5, 5]), "k--") - plt.xlabel('Time [days]') - plt.title('Velocity Error and Covariance') + plt.xlabel("Time [days]") + plt.title("Velocity Error and Covariance") - ax[0].set_ylabel('${}^{A}v_{x}$ [m/s]') - ax[1].set_ylabel('${}^{A}v_{y}$ [m/s]') - ax[2].set_ylabel('${}^{A}v_{z}$ [m/s]') + ax[0].set_ylabel("${}^{A}v_{x}$ [m/s]") + ax[1].set_ylabel("${}^{A}v_{y}$ [m/s]") + ax[2].set_ylabel("${}^{A}v_{z}$ [m/s]") ax[0].legend() return + def plot_acc_error(time, a_err, P): """Plot the non-Keplerian acceleration estimation error and associated covariance.""" plt.gcf() - fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6)) + 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) + plt.tick_params(labelcolor="none", top=False, bottom=False, left=False, right=False) - ax[0].plot(time/(3600*24), a_err[:, 0]*1000, 'b', label='error') - ax[0].plot(time/(3600*24), 2*np.sqrt(P[:, 6, 6])*1000, 'k--', label=r'$2\sigma$') - ax[0].plot(time/(3600*24), -2*np.sqrt(P[:, 6, 6])*1000, 'k--') + ax[0].plot(time / (3600 * 24), a_err[:, 0] * 1000, "b", label="error") + ax[0].plot( + time / (3600 * 24), 2 * np.sqrt(P[:, 6, 6]) * 1000, "k--", label=r"$2\sigma$" + ) + ax[0].plot(time / (3600 * 24), -2 * np.sqrt(P[:, 6, 6]) * 1000, "k--") - ax[1].plot(time/(3600*24), a_err[:, 1]*1000, 'b') - ax[1].plot(time/(3600*24), 2*np.sqrt(P[:, 7, 7])*1000, 'k--') - ax[1].plot(time/(3600*24), -2*np.sqrt(P[:, 7, 7])*1000, 'k--') + ax[1].plot(time / (3600 * 24), a_err[:, 1] * 1000, "b") + ax[1].plot(time / (3600 * 24), 2 * np.sqrt(P[:, 7, 7]) * 1000, "k--") + ax[1].plot(time / (3600 * 24), -2 * np.sqrt(P[:, 7, 7]) * 1000, "k--") - ax[2].plot(time/(3600*24), a_err[:, 2]*1000, 'b') - ax[2].plot(time/(3600*24), 2*np.sqrt(P[:, 8, 8])*1000, 'k--') - ax[2].plot(time/(3600*24), -2*np.sqrt(P[:, 8, 8])*1000, 'k--') + ax[2].plot(time / (3600 * 24), a_err[:, 2] * 1000, "b") + ax[2].plot(time / (3600 * 24), 2 * np.sqrt(P[:, 8, 8]) * 1000, "k--") + ax[2].plot(time / (3600 * 24), -2 * np.sqrt(P[:, 8, 8]) * 1000, "k--") - plt.xlabel('Time [days]') - plt.title('Acceleration Error and Covariance') + plt.xlabel("Time [days]") + plt.title("Acceleration Error and Covariance") - ax[0].set_ylabel('${}^{A}a_{x}$ [mm/s$^2$]') - ax[1].set_ylabel('${}^{A}a_{y}$ [mm/s$^2$]') - ax[2].set_ylabel('${}^{A}a_{z}$ [mm/s$^2$]') + ax[0].set_ylabel("${}^{A}a_{x}$ [mm/s$^2$]") + ax[1].set_ylabel("${}^{A}a_{y}$ [mm/s$^2$]") + ax[2].set_ylabel("${}^{A}a_{z}$ [mm/s$^2$]") ax[0].legend() @@ -289,6 +297,7 @@ def plot_acc_error(time, a_err, P): def run(show_plots): from Basilisk import __path__ + bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) @@ -304,12 +313,12 @@ def run(show_plots): # create the dynamics task and specify the simulation time step information simulationTimeStep = macros.sec2nano(15) - simulationTime = macros.sec2nano(4*24*3600.0) + simulationTime = macros.sec2nano(4 * 24 * 3600.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # setup celestial object ephemeris module gravBodyEphem = planetEphemeris.PlanetEphemeris() - gravBodyEphem.ModelTag = 'vestaEphemeris' + gravBodyEphem.ModelTag = "vestaEphemeris" gravBodyEphem.setPlanetNames(planetEphemeris.StringVector(["vesta"])) # specify small body o.e. and rotational state January 21st, 2022 @@ -317,10 +326,10 @@ def run(show_plots): oeAsteroid = planetEphemeris.ClassicElements() oeAsteroid.a = 2.3612 * orbitalMotion.AU * 1000 # meters oeAsteroid.e = 0.08823 - oeAsteroid.i = 7.1417*macros.D2R - oeAsteroid.Omega = 103.8*macros.D2R - oeAsteroid.omega = 151.1*macros.D2R - oeAsteroid.f = 7.0315*macros.D2R + oeAsteroid.i = 7.1417 * macros.D2R + oeAsteroid.Omega = 103.8 * macros.D2R + oeAsteroid.omega = 151.1 * macros.D2R + oeAsteroid.f = 7.0315 * macros.D2R # the rotational state would be prescribed to AR = 309.03 * macros.D2R @@ -330,21 +339,25 @@ def run(show_plots): gravBodyEphem.rightAscension = planetEphemeris.DoubleVector([AR]) gravBodyEphem.declination = planetEphemeris.DoubleVector([dec]) gravBodyEphem.lst0 = planetEphemeris.DoubleVector([lst0]) - gravBodyEphem.rotRate = planetEphemeris.DoubleVector([360 * macros.D2R / (5.3421 * 3600.)]) + gravBodyEphem.rotRate = planetEphemeris.DoubleVector( + [360 * macros.D2R / (5.3421 * 3600.0)] + ) # initialize small body fixed frame dcm w.r.t. inertial and its angular velocity - dcm_AN = RigidBodyKinematics.euler3232C([AR , np.pi/2 - dec, lst0]) - omega_AN_A = np.array([0,0,360 * macros.D2R / (5.3421 * 3600.)]) + dcm_AN = RigidBodyKinematics.euler3232C([AR, np.pi / 2 - dec, lst0]) + omega_AN_A = np.array([0, 0, 360 * macros.D2R / (5.3421 * 3600.0)]) # setup small body gravity effector (no Sun 3rd perturbation included) # https://ssd.jpl.nasa.gov/tools/gravity.html#/vesta gravFactory = simIncludeGravBody.gravBodyFactory() - mu = 17.2882449693*1e9 # m^3/s^2 - asteroid = gravFactory.createCustomGravObject("vesta", mu, radEquator=265*1000) + mu = 17.2882449693 * 1e9 # m^3/s^2 + asteroid = gravFactory.createCustomGravObject("vesta", mu, radEquator=265 * 1000) asteroid.isCentralBody = True nSpherHarm = 14 - asteroid.useSphericalHarmonicsGravityModel(bskPath + "/supportData/LocalGravData/VESTA20H.txt", nSpherHarm) + asteroid.useSphericalHarmonicsGravityModel( + bskPath + "/supportData/LocalGravData/VESTA20H.txt", nSpherHarm + ) asteroid.planetBodyInMsg.subscribeTo(gravBodyEphem.planetOutMsgs[0]) # create an ephemeris converter @@ -359,7 +372,7 @@ def run(show_plots): # setup orbit initial conditions of the sc oe = orbitalMotion.ClassicElements() - oe.a = 500*1000 # meters + oe.a = 500 * 1000 # meters oe.e = 0.001 oe.i = 175 * macros.D2R oe.Omega = 48.2 * macros.D2R @@ -373,7 +386,7 @@ def run(show_plots): # set up simpleNav for s/c "measurements" simpleNavMeas = simpleNav.SimpleNav() - simpleNavMeas.ModelTag = 'SimpleNav' + simpleNavMeas.ModelTag = "SimpleNav" simpleNavMeas.scStateInMsg.subscribeTo(scObject.scStateOutMsg) pos_sigma_sc = 10.0 vel_sigma_sc = 0.1 @@ -381,25 +394,388 @@ def run(show_plots): rate_sigma_sc = 0.0 * 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.1], [0.1], [0.1], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [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, + 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, + 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, + 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, + 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, + 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, + 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, + 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, + 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, + ], + [ + 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, + 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, + 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, + 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, + 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, + 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, + 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, + 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, + 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.0], + [10.0], + [10.0], + [0.1], + [0.1], + [0.1], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + ] simpleNavMeas.PMatrix = p_matrix_sc simpleNavMeas.walkBounds = walk_bounds_sc @@ -412,19 +788,34 @@ def run(show_plots): vel_sigma_p = 0.0 att_sigma_p = 0 * math.pi / 180.0 rate_sigma_p = 0 * 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.], [0.], [0.], [0.], [0.], [0.]] + p_matrix_p = [ + [pos_sigma_p, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 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, 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, 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, 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, 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], + [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, 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, 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, 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, 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, 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.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + ] planetNavMeas.PMatrix = p_matrix_p planetNavMeas.walkBounds = walk_bounds_p @@ -439,41 +830,45 @@ def run(show_plots): P_proc_pos = 1000 P_proc_vel = 1 P_proc_acc = 1e-6 - P_proc = [[P_proc_pos, 0., 0., 0., 0., 0., 0., 0., 0.], - [0., P_proc_pos, 0., 0., 0., 0., 0., 0., 0.], - [0., 0., P_proc_pos, 0., 0., 0., 0., 0., 0.], - [0., 0., 0., P_proc_vel, 0., 0., 0., 0., 0.], - [0., 0., 0., 0., P_proc_vel, 0., 0., 0., 0.], - [0., 0., 0., 0., 0., P_proc_vel, 0., 0., 0.], - [0., 0., 0., 0., 0., 0., P_proc_acc, 0., 0.], - [0., 0., 0., 0., 0., 0., 0., P_proc_acc, 0.], - [0., 0., 0., 0., 0., 0., 0., 0., P_proc_acc]] + P_proc = [ + [P_proc_pos, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, P_proc_pos, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, P_proc_pos, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, P_proc_vel, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, P_proc_vel, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, P_proc_vel, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, P_proc_acc, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, P_proc_acc, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, P_proc_acc], + ] smallBodyNav.P_proc = P_proc # set the measurement noise R_meas = np.identity(3) - R_meas[0,0] = R_meas[1,1] = R_meas[2,2] = 100 # position sigmas + R_meas[0, 0] = R_meas[1, 1] = R_meas[2, 2] = 100 # position sigmas smallBodyNav.R_meas = R_meas.tolist() # Measurement Noise # set the initial guess, x_0 x_0 = np.zeros(9) x_0[0:3] = r_CA_A - x_0[3:6] = v_CA_A - np.cross(omega_AN_A,r_CA_A) + x_0[3:6] = v_CA_A - np.cross(omega_AN_A, r_CA_A) smallBodyNav.x_hat_k = x_0 # set the initial state covariance P_k_pos = 1e4 P_k_vel = 0.1 P_k_acc = 1e-4 - P_k = [[P_k_pos, 0., 0., 0., 0., 0., 0., 0., 0.], - [0., P_k_pos, 0., 0., 0., 0., 0., 0., 0.], - [0., 0., P_k_pos, 0., 0., 0., 0., 0., 0.], - [0., 0., 0., P_k_vel, 0., 0., 0., 0., 0.], - [0., 0., 0., 0., P_k_vel, 0., 0., 0., 0.], - [0., 0., 0., 0., 0., P_k_vel, 0., 0., 0.], - [0., 0., 0., 0., 0., 0., P_k_acc, 0., 0.], - [0., 0., 0., 0., 0., 0., 0., P_k_acc, 0.], - [0., 0., 0., 0., 0., 0., 0., 0., P_k_acc]] + P_k = [ + [P_k_pos, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, P_k_pos, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, P_k_pos, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, P_k_vel, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, P_k_vel, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, P_k_vel, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, P_k_acc, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, P_k_acc, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, P_k_acc], + ] smallBodyNav.P_k = P_k # set UKF hyperparameters @@ -527,8 +922,8 @@ def run(show_plots): omega_AN_A = ast_truth_recorder.omega_BN_B # initialize truth inertial position and velocity w.r.t. asteroid centered fixed frame - r_truth_A = np.zeros((N_points,3)) - v_truth_A = np.zeros((N_points,3)) + r_truth_A = np.zeros((N_points, 3)) + v_truth_A = np.zeros((N_points, 3)) a_truth_A = np.zeros((N_points, 3)) # loop through simulation points @@ -538,10 +933,14 @@ def run(show_plots): # rotate position and velocity r_truth_A[ii][0:3] = R_AN.dot(r_truth_N[ii][0:3]) - v_truth_A[ii][0:3] = R_AN.dot(v_truth_N[ii][0:3]) - np.cross(omega_AN_A[ii][0:3], r_truth_A[ii][0:3]) + v_truth_A[ii][0:3] = R_AN.dot(v_truth_N[ii][0:3]) - np.cross( + omega_AN_A[ii][0:3], r_truth_A[ii][0:3] + ) # compute gravity acceleration and substract Keplerian term - a_truth_A[ii][0:3] = np.array(asteroid.spherHarm.computeField(r_truth_A[ii][0:3], nSpherHarm, False)).reshape(3) + a_truth_A[ii][0:3] = np.array( + asteroid.spherHarm.computeField(r_truth_A[ii][0:3], nSpherHarm, False) + ).reshape(3) # get filter output x_hat = sc_nav_recorder.state @@ -550,46 +949,47 @@ def run(show_plots): # plot the results figureList = {} - plot_3Dposition(r_truth_N, title='inertial') + plot_3Dposition(r_truth_N, title="inertial") pltName = fileName + "1" figureList[pltName] = plt.figure(1) - plot_3Dposition(r_truth_A, title='asteroid') + plot_3Dposition(r_truth_A, title="asteroid") pltName = fileName + "2" figureList[pltName] = plt.figure(2) - plot_position(time, r_truth_A, x_hat[:,0:3]) + plot_position(time, r_truth_A, x_hat[:, 0:3]) pltName = fileName + "3" figureList[pltName] = plt.figure(3) - plot_velocity(time, v_truth_A, x_hat[:,3:6]) + plot_velocity(time, v_truth_A, x_hat[:, 3:6]) pltName = fileName + "4" figureList[pltName] = plt.figure(4) - plot_acceleration(time, a_truth_A, x_hat[:,6:9]) + plot_acceleration(time, a_truth_A, x_hat[:, 6:9]) pltName = fileName + "5" figureList[pltName] = plt.figure(5) - plot_pos_error(time, np.subtract(r_truth_A, x_hat[:,0:3]), P) + plot_pos_error(time, np.subtract(r_truth_A, x_hat[:, 0:3]), P) pltName = fileName + "6" figureList[pltName] = plt.figure(6) - plot_vel_error(time, np.subtract(v_truth_A, x_hat[:,3:6]), P) + plot_vel_error(time, np.subtract(v_truth_A, x_hat[:, 3:6]), P) pltName = fileName + "7" figureList[pltName] = plt.figure(7) - plot_acc_error(time, np.subtract(a_truth_A,x_hat[:,6:9]), P) + plot_acc_error(time, np.subtract(a_truth_A, x_hat[:, 6:9]), P) pltName = fileName + "8" figureList[pltName] = plt.figure(8) if show_plots: - plt.show() + 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 script can be run as a # stand-along python script diff --git a/examples/scenarioSpacecraftLocation.py b/examples/scenarioSpacecraftLocation.py index f32bc403ac..7e0c86b7f7 100755 --- a/examples/scenarioSpacecraftLocation.py +++ b/examples/scenarioSpacecraftLocation.py @@ -49,18 +49,24 @@ import matplotlib.pyplot as plt import numpy as np from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, hillPoint) +from Basilisk.fswAlgorithms import mrpFeedback, attTrackingError, hillPoint from Basilisk.simulation import extForceTorque from Basilisk.simulation import simpleNav, spacecraft from Basilisk.simulation import spacecraftLocation -from Basilisk.utilities import (SimulationBaseClass, macros, - orbitalMotion, simIncludeGravBody, - unitTestSupport, vizSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, + vizSupport, +) # 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]) @@ -81,7 +87,7 @@ def run(show_plots): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(140.) + simulationTime = macros.min2nano(140.0) # # create the simulation process @@ -100,18 +106,14 @@ def run(show_plots): scObject = spacecraft.Spacecraft() scObject.ModelTag = "Servicer" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # create the debris object states scObject2 = spacecraft.Spacecraft() scObject2.ModelTag = "Debris" - I2 = [600., 0., 0., - 0., 650., 0., - 0., 0, 450.] + I2 = [600.0, 0.0, 0.0, 0.0, 650.0, 0.0, 0.0, 0, 450.0] scObject2.hub.mHub = 350.0 # kg scObject2.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I2) @@ -150,10 +152,10 @@ def run(show_plots): scLocation.addSpacecraftToModel(scObject2.scStateOutMsg) scLocation.primaryScStateInMsg.subscribeTo(scObject.scStateOutMsg) scLocation.rEquator = earth.radEquator - scLocation.rPolar = earth.radEquator*0.98 + scLocation.rPolar = earth.radEquator * 0.98 scLocation.aHat_B = [0, 1, 0] - scLocation.theta = np.radians(10.) - scLocation.maximumRange = 55. + scLocation.theta = np.radians(10.0) + scLocation.maximumRange = 55.0 scSim.AddModelToTask(simTaskName, scLocation) # @@ -187,7 +189,7 @@ def run(show_plots): mrpControl.K = 3.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) # @@ -216,7 +218,7 @@ def run(show_plots): # setup 1st debris object states oe2 = copy.deepcopy(oe) oe2.e += 0.000001 - oe2.f += 40./oe2.a + oe2.f += 40.0 / oe2.a r2N, v2N = orbitalMotion.elem2rv(mu, oe2) scObject2.hub.r_CN_NInit = r2N # m - r_CN_N scObject2.hub.v_CN_NInit = v2N # m/s - v_CN_N @@ -226,17 +228,22 @@ def run(show_plots): # 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: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, [scObject, scObject2] - # , saveFile=fileName, - ) - vizSupport.addLocation(viz, stationName="antenna" - , parentBodyName='Servicer' - , r_GP_P=[0, 2, 0] - , gHat_P=[0, 1, 0] - , fieldOfView=2*scLocation.theta - , range=scLocation.maximumRange - , color='pink' - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + [scObject, scObject2], + # , saveFile=fileName, + ) + vizSupport.addLocation( + viz, + stationName="antenna", + parentBodyName="Servicer", + r_GP_P=[0, 2, 0], + gHat_P=[0, 1, 0], + fieldOfView=2 * scLocation.theta, + range=scLocation.maximumRange, + color="pink", + ) viz.settings.showLocationCommLines = 1 viz.settings.showLocationCones = 1 viz.settings.showLocationLabels = 1 @@ -254,13 +261,13 @@ def run(show_plots): # # plot the results # - dataLog = accessRec.hasAccess + dataLog = accessRec.hasAccess timeData = accessRec.times() * macros.NANO2MIN plt.figure(1) plt.plot(timeData, dataLog) - plt.xlabel('Time [min]') - plt.ylabel('Sat-Sat Access') + plt.xlabel("Time [min]") + plt.ylabel("Sat-Sat Access") figureList = {} pltName = fileName + "1" @@ -281,5 +288,5 @@ def run(show_plots): # if __name__ == "__main__": run( - True # show_plots + True # show_plots ) diff --git a/examples/scenarioSpiceSpacecraft.py b/examples/scenarioSpiceSpacecraft.py index f453751afd..4bc7174dda 100755 --- a/examples/scenarioSpiceSpacecraft.py +++ b/examples/scenarioSpiceSpacecraft.py @@ -81,7 +81,9 @@ # import general simulation support files from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import matplotlib.pyplot as plt from Basilisk.utilities import macros @@ -106,6 +108,7 @@ # 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]) @@ -127,7 +130,7 @@ def run(show_plots): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -135,7 +138,7 @@ def run(show_plots): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -146,9 +149,7 @@ def run(show_plots): scObject = spacecraft.Spacecraft() scObject.ModelTag = "Hubble" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) @@ -168,10 +169,12 @@ def run(show_plots): # setup spice library for Earth ephemeris and Hubble states timeInitString = "2015 February 10, 00:00:00.0 TDB" spiceObject = gravFactory.createSpiceInterface(time=timeInitString, epochInMsg=True) - spiceObject.zeroBase = 'Earth' + spiceObject.zeroBase = "Earth" scNames = ["HUBBLE SPACE TELESCOPE"] spiceObject.addSpacecraftNames(messaging.StringVector(scNames)) - spiceObject.loadSpiceKernel("hst_edited.bsp", bskPath + '/supportData/EphemerisData/') + spiceObject.loadSpiceKernel( + "hst_edited.bsp", bskPath + "/supportData/EphemerisData/" + ) # need spice to run before spacecraft module as it provides the spacecraft translational states scSim.AddModelToTask(simTaskName, spiceObject) @@ -198,7 +201,7 @@ def run(show_plots): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -212,7 +215,7 @@ def run(show_plots): mrpControl.K = 3.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # # create simulation messages @@ -238,7 +241,9 @@ def run(show_plots): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) snLog = sNavObject.scStateInMsg.recorder(samplingTime) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) mrpLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) @@ -253,9 +258,12 @@ def run(show_plots): scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B # if this scenario is to interface with the BSK Viz, uncomment the following line - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation @@ -270,7 +278,9 @@ def run(show_plots): # unload custom Spice kernel gravFactory.unloadSpiceKernels() - spiceObject.unloadSpiceKernel("hst_edited.bsp", bskPath + '/supportData/EphemerisData/') + spiceObject.unloadSpiceKernel( + "hst_edited.bsp", bskPath + "/supportData/EphemerisData/" + ) # # plot the results @@ -279,43 +289,55 @@ def run(show_plots): plt.close("all") # clears out plots from earlier test runs plt.figure(1) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, attErrorLog.sigma_BR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeAxis * macros.NANO2MIN, + attErrorLog.sigma_BR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) plt.figure(2) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, mrpLog.torqueRequestBody[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Control Torque $L_r$ [Nm]') + plt.plot( + timeAxis * macros.NANO2MIN, + mrpLog.torqueRequestBody[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Control Torque $L_r$ [Nm]") pltName = fileName + "2" plt.figure(3) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, attErrorLog.omega_BR_B[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeAxis * macros.NANO2MIN, + attErrorLog.omega_BR_B[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") plt.figure(4) for idx in range(3): - plt.plot(timeAxis * macros.NANO2MIN, snLog.r_BN_N[:, idx] / 1000., - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Inertial Position [km]') + plt.plot( + timeAxis * macros.NANO2MIN, + snLog.r_BN_N[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Inertial Position [km]") figureList[pltName] = plt.figure(4) if show_plots: diff --git a/examples/scenarioSpinningBodiesTwoDOF.py b/examples/scenarioSpinningBodiesTwoDOF.py index 5b47b042f3..d35899be79 100755 --- a/examples/scenarioSpinningBodiesTwoDOF.py +++ b/examples/scenarioSpinningBodiesTwoDOF.py @@ -103,6 +103,7 @@ from Basilisk.utilities import macros, orbitalMotion, unitTestSupport from Basilisk import __path__ + bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) @@ -128,7 +129,7 @@ def run(show_plots, numberPanels): dynProcess = scSim.CreateNewProcess(simProcessName) # Create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.01) + simulationTimeStep = macros.sec2nano(0.01) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -145,9 +146,11 @@ def run(show_plots, numberPanels): 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]] + 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 @@ -169,12 +172,12 @@ def run(show_plots, numberPanels): # 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.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]] @@ -201,18 +204,22 @@ def run(show_plots, numberPanels): # 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.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.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 @@ -255,32 +262,43 @@ def run(show_plots, numberPanels): 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")) + 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")) + 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")) + 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 @@ -308,45 +326,63 @@ def run(show_plots, numberPanels): 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.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]') + 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.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]') + 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.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]') + 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.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]') + plt.xlabel("time [s]") + plt.ylabel(r"Angular Velocity [rad/s]") pltName = fileName + "angularVelocity" + str(int(numberPanels)) figureList[pltName] = plt.figure(4) diff --git a/examples/scenarioSweepingSpacecraft.py b/examples/scenarioSweepingSpacecraft.py index 8775aa3f2e..7d782e8297 100644 --- a/examples/scenarioSweepingSpacecraft.py +++ b/examples/scenarioSweepingSpacecraft.py @@ -1,4 +1,3 @@ - # # ISC License # @@ -99,9 +98,11 @@ import os import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ + # import message declarations from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import attTrackingError @@ -112,15 +113,20 @@ from Basilisk.fswAlgorithms import mrpFeedback from Basilisk.simulation import extForceTorque from Basilisk.simulation import simpleNav + # import simulation related support from Basilisk.simulation import spacecraft from Basilisk.utilities import RigidBodyKinematics + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -130,6 +136,7 @@ # Plotting functions + def plot_attitude_error(timeLineSet, dataSigmaBR): """Plot the attitude result.""" plt.figure(1) @@ -137,36 +144,47 @@ def plot_attitude_error(timeLineSet, dataSigmaBR): ax = fig.gca() vectorData = dataSigmaBR sNorm = np.array([np.linalg.norm(v) for v in vectorData]) - plt.plot(timeLineSet, sNorm, - color=unitTestSupport.getLineColor(1, 3), - ) - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error Norm $|\sigma_{B/R}|$') - ax.set_yscale('log') + plt.plot( + timeLineSet, + sNorm, + color=unitTestSupport.getLineColor(1, 3), + ) + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error Norm $|\sigma_{B/R}|$") + ax.set_yscale("log") + def plot_control_torque(timeLineSet, dataLr): """Plot the control torque response.""" plt.figure(2) for idx in range(3): - plt.plot(timeLineSet, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Control Torque $L_r$ [Nm]') + plt.plot( + timeLineSet, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Control Torque $L_r$ [Nm]") + def plot_rate_error(timeLineSet, dataOmegaBR): """Plot the body angular velocity tracking error.""" plt.figure(3) for idx in range(3): - plt.plot(timeLineSet, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeLineSet, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") return + def plot_orientation(timeLineSet, dataPos, dataVel, dataSigmaBN): """Plot the spacecraft orientation.""" vectorPosData = dataPos @@ -179,23 +197,33 @@ def plot_orientation(timeLineSet, dataPos, dataVel, dataSigmaBN): ih = hv / np.linalg.norm(hv) itheta = np.cross(ih, ir) dcmBN = RigidBodyKinematics.MRP2C(vectorMRPData[idx]) - data[idx] = [np.dot(ir, dcmBN[0]), np.dot(itheta, dcmBN[1]), np.dot(ih, dcmBN[2])] + data[idx] = [ + np.dot(ir, dcmBN[0]), + np.dot(itheta, dcmBN[1]), + np.dot(ih, dcmBN[2]), + ] plt.figure(4) - labelStrings = (r'$\hat\imath_r\cdot \hat b_1$' - , r'${\hat\imath}_{\theta}\cdot \hat b_2$' - , r'$\hat\imath_h\cdot \hat b_3$') + labelStrings = ( + r"$\hat\imath_r\cdot \hat b_1$", + r"${\hat\imath}_{\theta}\cdot \hat b_2$", + r"$\hat\imath_h\cdot \hat b_3$", + ) for idx in range(3): - plt.plot(timeLineSet, data[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=labelStrings[idx]) - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Orientation Illustration') + plt.plot( + timeLineSet, + data[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=labelStrings[idx], + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Orientation Illustration") # Run function -def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): + +def run(show_plots, useAltBodyFrame, angle_rate_command, time_command): """ The scenarios can be run with the followings setups parameters: @@ -213,7 +241,7 @@ def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): scSim = SimulationBaseClass.SimBaseClass() # Set the simulation time variable used later on - simulationTime = macros.min2nano(time_command[0]) # convert mins to nano-seconds + simulationTime = macros.min2nano(time_command[0]) # convert mins to nano-seconds # Create the simulation process dynProcess = scSim.CreateNewProcess(simProcessName) @@ -229,11 +257,13 @@ def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): # Initialize spacecraft object and its properties scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] # Inertia of the spacecraft + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] # Inertia of the spacecraft scObject.hub.mHub = 750.0 # kg - Mass of the spacecraft - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) scSim.AddModelToTask(simTaskName, scObject) @@ -254,19 +284,23 @@ def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): sNavObject = simpleNav.SimpleNav() sNavObject.ModelTag = "SimpleNavigation" scSim.AddModelToTask(simTaskName, sNavObject) - sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg) # scStateInMsg Input message name for spacecraft state + sNavObject.scStateInMsg.subscribeTo( + scObject.scStateOutMsg + ) # scStateInMsg Input message name for spacecraft state # - #Create the FSW modules + # Create the FSW modules # - #Setup HillPoint Guidance Module + # Setup HillPoint Guidance Module attGuidance = hillPoint.hillPoint() attGuidance.ModelTag = "hillPoint" - attGuidance.transNavInMsg.subscribeTo(sNavObject.transOutMsg) # Incoming spacecraft tranlational message + attGuidance.transNavInMsg.subscribeTo( + sNavObject.transOutMsg + ) # Incoming spacecraft tranlational message scSim.AddModelToTask(simTaskName, attGuidance) - #Setup Euler rotation of the Hill reference frame + # Setup Euler rotation of the Hill reference frame attGuidanceEuler = eulerRotation.eulerRotation() attGuidanceEuler.ModelTag = "eulerRotation" attGuidanceEuler.attRefInMsg.subscribeTo(attGuidance.attRefOutMsg) @@ -275,8 +309,7 @@ def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): attGuidanceEuler.angleSet = [0, np.pi, 0] attGuidanceEuler.angleRates = angle_rate_command[0] - - #Setup the attitude tracking error evaluation module + # Setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() attError.ModelTag = "attErrorrotatingHill" scSim.AddModelToTask(simTaskName, attError) @@ -290,11 +323,13 @@ def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): mrpControl.guidInMsg.subscribeTo(attError.attGuidOutMsg) configData = messaging.VehicleConfigMsgPayload(ISCPntB_B=I) configDataMsg = messaging.VehicleConfigMsg().write(configData) - mrpControl.vehConfigInMsg.subscribeTo(configDataMsg) # The MRP feedback algorithm requires the vehicle configuration structure. + mrpControl.vehConfigInMsg.subscribeTo( + configDataMsg + ) # The MRP feedback algorithm requires the vehicle configuration structure. mrpControl.K = 3.5 mrpControl.Ki = -1.0 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # Connect torque command to external torque effector extFTObject.cmdTorqueInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) @@ -304,7 +339,9 @@ def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) mrpLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) attErrLog = attError.attGuidOutMsg.recorder(samplingTime) snAttLog = sNavObject.attOutMsg.recorder(samplingTime) @@ -314,7 +351,7 @@ def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): scSim.AddModelToTask(simTaskName, snAttLog) scSim.AddModelToTask(simTaskName, snTransLog) - #Inititialize spacecraft state + # Inititialize spacecraft state oe = orbitalMotion.ClassicElements() oe.a = 10000000.0 # meters oe.e = 0.1 @@ -323,23 +360,43 @@ def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - scObject.hub.r_CN_NInit = rN # m - r_CN_N Initial position with respect to the inertial planet frame - scObject.hub.v_CN_NInit = vN # m/s - v_CN_N Initial velocity with respect to the inertial planet frame - scObject.hub.sigma_BNInit = [[0.1], [0.2], [-0.3]] # sigma_BN_B Initial attitude with respect to the inertial planet frame - scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_BN_B Initial attitude rate with respect to the inertial planet frame + scObject.hub.r_CN_NInit = ( + rN # m - r_CN_N Initial position with respect to the inertial planet frame + ) + scObject.hub.v_CN_NInit = ( + vN # m/s - v_CN_N Initial velocity with respect to the inertial planet frame + ) + scObject.hub.sigma_BNInit = [ + [0.1], + [0.2], + [-0.3], + ] # sigma_BN_B Initial attitude with respect to the inertial planet frame + scObject.hub.omega_BN_BInit = [ + [0.001], + [-0.01], + [0.03], + ] # rad/s - omega_BN_B Initial attitude rate with respect to the inertial planet frame # # Interface the scenario with the BSK Viz # if vizSupport.vizFound: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) - vizSupport.createCameraConfigMsg(viz, parentName=scObject.ModelTag, - cameraID=1, fieldOfView=20 * macros.D2R, - resolution=[1024, 1024], renderRate=0., - cameraPos_B=[1., 0., .0], sigma_CB=[0., np.tan(np.pi/2/4), 0.] - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) + vizSupport.createCameraConfigMsg( + viz, + parentName=scObject.ModelTag, + cameraID=1, + fieldOfView=20 * macros.D2R, + resolution=[1024, 1024], + renderRate=0.0, + cameraPos_B=[1.0, 0.0, 0.0], + sigma_CB=[0.0, np.tan(np.pi / 2 / 4), 0.0], + ) viz.settings.viewCameraConeHUD = 1 # @@ -348,7 +405,6 @@ def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): scSim.InitializeSimulation() - # # Execute the simulation for the first angle rate and simulation time # @@ -360,7 +416,7 @@ def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): # Update of the angle rate and simulation time # - for i,j in zip(time_command[1:],angle_rate_command[1:]): + for i, j in zip(time_command[1:], angle_rate_command[1:]): attGuidanceEuler.angleRates = j simulationTime += macros.min2nano(i) scSim.ConfigureStopTime(simulationTime) @@ -419,6 +475,8 @@ def run (show_plots, useAltBodyFrame, angle_rate_command, time_command): run( True, # show_plots True, # useAltBodyFrame - np.array([[0.0,0,0.0],[0.0,0.002,0.0],[0.0,-0.002,0.0],[0.0,0,0.0]]), # angle_rate_command - np.array([10,10,10,10]) # time_command + np.array( + [[0.0, 0, 0.0], [0.0, 0.002, 0.0], [0.0, -0.002, 0.0], [0.0, 0, 0.0]] + ), # angle_rate_command + np.array([10, 10, 10, 10]), # time_command ) diff --git a/examples/scenarioTAM.py b/examples/scenarioTAM.py index cfb8443952..54f43e1619 100644 --- a/examples/scenarioTAM.py +++ b/examples/scenarioTAM.py @@ -119,17 +119,24 @@ # general support file with common unit test functions # import general simulation support files -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, +) from Basilisk.utilities import simSetPlanetEnvironment # import simulation related support from Basilisk.simulation import spacecraft -#attempt to import vizard +# attempt to import vizard from Basilisk.utilities import vizSupport + fileName = os.path.basename(os.path.splitext(__file__)[0]) + def run(show_plots, orbitCase, planetCase, useBias, useBounds): """ The scenarios can be run with the followings setups parameters: @@ -156,7 +163,7 @@ def run(show_plots, orbitCase, planetCase, useBias, useBounds): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -172,37 +179,43 @@ def run(show_plots, orbitCase, planetCase, useBias, useBounds): # setup Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() - if planetCase == 'Jupiter': + if planetCase == "Jupiter": planet = gravFactory.createJupiter() - planet.isCentralBody = True # ensure this is the central gravitational body - elif planetCase == 'Earth': + planet.isCentralBody = True # ensure this is the central gravitational body + elif planetCase == "Earth": planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu req = planet.radEquator # attach gravity model to spacecraft gravFactory.addBodiesTo(scObject) - if planetCase == 'Jupiter': - magModule = magneticFieldCenteredDipole.MagneticFieldCenteredDipole() # default is Earth centered dipole module + if planetCase == "Jupiter": + magModule = ( + magneticFieldCenteredDipole.MagneticFieldCenteredDipole() + ) # default is Earth centered dipole module magModule.ModelTag = "CenteredDipole" # The following command is a support function that sets up the centered dipole parameters. # These parameters can also be setup manually - simSetPlanetEnvironment.centeredDipoleMagField(magModule, 'jupiter') - elif planetCase == 'Earth': + simSetPlanetEnvironment.centeredDipoleMagField(magModule, "jupiter") + elif planetCase == "Earth": magModule = magneticFieldWMM.MagneticFieldWMM() magModule.ModelTag = "WMM" - magModule.dataPath = bskPath + '/supportData/MagneticField/' + magModule.dataPath = bskPath + "/supportData/MagneticField/" # set epoch date/time message - epochMsg = unitTestSupport.timeStringToGregorianUTCMsg('2019 June 27, 10:23:0.0 (UTC)') + epochMsg = unitTestSupport.timeStringToGregorianUTCMsg( + "2019 June 27, 10:23:0.0 (UTC)" + ) magModule.epochInMsg.subscribeTo(epochMsg) - if orbitCase == 'elliptical': - magModule.envMinReach = 10000 * 1000. - magModule.envMaxReach = 20000 * 1000. + if orbitCase == "elliptical": + magModule.envMinReach = 10000 * 1000.0 + magModule.envMaxReach = 20000 * 1000.0 # add spacecraft to the magnetic field module so it can read the sc position messages - magModule.addSpacecraftToModel(scObject.scStateOutMsg) # this command can be repeated if multiple + magModule.addSpacecraftToModel( + scObject.scStateOutMsg + ) # this command can be repeated if multiple # add the magnetic field module to the simulation task stack scSim.AddModelToTask(simTaskName, magModule) @@ -212,7 +225,7 @@ def run(show_plots, orbitCase, planetCase, useBias, useBounds): TAM.ModelTag = "TAM_sensor" # specify the optional TAM variables TAM.scaleFactor = 1.0 - TAM.senNoiseStd = [100e-9, 100e-9, 100e-9] + TAM.senNoiseStd = [100e-9, 100e-9, 100e-9] if useBias: TAM.senBias = [0, 0, -1e-6] # Tesla if useBounds: @@ -226,12 +239,12 @@ def run(show_plots, orbitCase, planetCase, useBias, useBounds): # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rPeriapses = req*1.1 # meters - if orbitCase == 'circular': + rPeriapses = req * 1.1 # meters + if orbitCase == "circular": oe.a = rPeriapses oe.e = 0.0000 - elif orbitCase == 'elliptical': - rApoapses = req*3.5 + elif orbitCase == "elliptical": + rApoapses = req * 3.5 oe.a = (rPeriapses + rApoapses) / 2.0 oe.e = 1.0 - rPeriapses / oe.a else: @@ -254,14 +267,16 @@ def run(show_plots, orbitCase, planetCase, useBias, useBounds): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n - simulationTime = macros.sec2nano(1. * P) + P = 2.0 * np.pi / n + simulationTime = macros.sec2nano(1.0 * P) # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) magLog = magModule.envOutMsgs[0].recorder(samplingTime) tamLog = TAM.tamDataOutMsg.recorder(samplingTime) dataLog = scObject.scStateOutMsg.recorder(samplingTime) @@ -271,9 +286,12 @@ def run(show_plots, orbitCase, planetCase, useBias, useBounds): TAM.magInMsg.subscribeTo(magModule.envOutMsgs[0]) # if this scenario is to interface with the BSK Viz, uncomment the following line - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) scSim.InitializeSimulation() @@ -301,21 +319,31 @@ def run(show_plots, orbitCase, planetCase, useBias, useBounds): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) rData = [] timeAxis = dataLog.times() for idx in range(len(posData)): rMag = np.linalg.norm(posData[idx]) - rData.append(rMag / 1000.) - plt.plot(timeAxis * macros.NANO2SEC / P, rData, color='#aa0000') - if orbitCase == 'elliptical': - plt.plot(timeAxis * macros.NANO2SEC / P, [magModule.envMinReach / 1000.] * len(rData), color='#007700', - dashes=[5, 5, 5, 5]) - plt.plot(timeAxis * macros.NANO2SEC / P, [magModule.envMaxReach / 1000.] * len(rData), - color='#007700', dashes=[5, 5, 5, 5]) - plt.xlabel('Time [orbits]') - plt.ylabel('Radius [km]') + rData.append(rMag / 1000.0) + plt.plot(timeAxis * macros.NANO2SEC / P, rData, color="#aa0000") + if orbitCase == "elliptical": + plt.plot( + timeAxis * macros.NANO2SEC / P, + [magModule.envMinReach / 1000.0] * len(rData), + color="#007700", + dashes=[5, 5, 5, 5], + ) + plt.plot( + timeAxis * macros.NANO2SEC / P, + [magModule.envMaxReach / 1000.0] * len(rData), + color="#007700", + dashes=[5, 5, 5, 5], + ) + plt.xlabel("Time [orbits]") + plt.ylabel("Radius [km]") plt.ylim(min(rData) * 0.9, max(rData) * 1.1) figureList = {} pltName = fileName + "1" + orbitCase + planetCase @@ -324,15 +352,20 @@ def run(show_plots, orbitCase, planetCase, useBias, useBounds): plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) for idx in range(3): - plt.plot(timeAxis * macros.NANO2SEC / P, tamData[:, idx] * 1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$TAM_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Magnetic Field [nT] ') + plt.plot( + timeAxis * macros.NANO2SEC / P, + tamData[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$TAM_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Magnetic Field [nT] ") pltName = fileName + "2" + orbitCase + planetCase figureList[pltName] = plt.figure(2) @@ -344,15 +377,16 @@ def run(show_plots, orbitCase, planetCase, useBias, useBounds): return magData, tamData, figureList, simulationTime + # # 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 (True, False) - 'elliptical', # orbit Case (circular, elliptical) - 'Jupiter', # planet Case (Earth, Jupiter) - False, # useBias - False # useBounds + True, # show_plots (True, False) + "elliptical", # orbit Case (circular, elliptical) + "Jupiter", # planet Case (Earth, Jupiter) + False, # useBias + False, # useBounds ) diff --git a/examples/scenarioTAMcomparison.py b/examples/scenarioTAMcomparison.py index 4d2b12f925..a2ce993352 100644 --- a/examples/scenarioTAMcomparison.py +++ b/examples/scenarioTAMcomparison.py @@ -115,6 +115,7 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ @@ -130,13 +131,19 @@ # general support file with common unit test functions # import general simulation support files -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - simIncludeGravBody, unitTestSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, +) from Basilisk.utilities import simSetPlanetEnvironment -#attempt to import vizard +# attempt to import vizard from Basilisk.utilities import vizSupport + def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): """ The scenarios can be run with the following setups parameters: @@ -162,7 +169,7 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -179,7 +186,7 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): # setup Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu req = planet.radEquator @@ -189,11 +196,13 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): # # create the centered dipole magnetic field # - magModule1 = magneticFieldCenteredDipole.MagneticFieldCenteredDipole() # default is Earth centered dipole module + magModule1 = ( + magneticFieldCenteredDipole.MagneticFieldCenteredDipole() + ) # default is Earth centered dipole module magModule1.ModelTag = "CenteredDipole" - simSetPlanetEnvironment.centeredDipoleMagField(magModule1, 'earth') + simSetPlanetEnvironment.centeredDipoleMagField(magModule1, "earth") - if orbitCase == 'elliptical': + if orbitCase == "elliptical": # Note that more then one magnetic field can be attached to a planet. # In the elliptic Earth orbit scenario # a second magnetic field module `magModule2` is created with a @@ -219,14 +228,16 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): magModule3 = magneticFieldWMM.MagneticFieldWMM() magModule3.ModelTag = "WMM" - magModule3.dataPath = bskPath + '/supportData/MagneticField/' + magModule3.dataPath = bskPath + "/supportData/MagneticField/" # set epoch date/time message - epochMsg = unitTestSupport.timeStringToGregorianUTCMsg('2019 June 27, 10:23:0.0 (UTC)') + epochMsg = unitTestSupport.timeStringToGregorianUTCMsg( + "2019 June 27, 10:23:0.0 (UTC)" + ) magModule3.epochInMsg.subscribeTo(epochMsg) # set the minReach and maxReach values if on an elliptic orbit - if orbitCase == 'elliptical': - magModule3.envMinReach = 10000 * 1000. - magModule3.envMaxReach = 20000 * 1000. + if orbitCase == "elliptical": + magModule3.envMinReach = 10000 * 1000.0 + magModule3.envMaxReach = 20000 * 1000.0 # add spacecraft to the magnetic field modules so it can read the sc position messages magModule1.addSpacecraftToModel(scObject.scStateOutMsg) @@ -247,45 +258,47 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): TAM1.senNoiseStd = [100e-9, 100e-9, 100e-9] TAM2.senNoiseStd = [100e-9, 100e-9, 100e-9] - if orbitCase == 'elliptical': - TAM3 = magnetometer.Magnetometer() # TAM3 is a dummy TAM used to plot Dipole Magnetic Model 2 + if orbitCase == "elliptical": + TAM3 = ( + magnetometer.Magnetometer() + ) # TAM3 is a dummy TAM used to plot Dipole Magnetic Model 2 TAM3.ModelTag = "TAM3_sensor" TAM3.scaleFactor = 1.0 TAM3.senNoiseStd = [100e-9, 100e-9, 100e-9] if useBias1: - useBias1_str = 'True' + useBias1_str = "True" TAM1.senBias = [0, 0, -1e-6] # Tesla - if orbitCase == 'elliptical': + if orbitCase == "elliptical": TAM3.senBias = [0, 0, -1e-6] # Tesla else: - useBias1_str = 'False' + useBias1_str = "False" if useBounds1: - useBounds1_str = 'True' - TAM1.maxOutput = 2.5E-5 # Tesla - TAM1.minOutput = -2.5E-5 # Tesla - if orbitCase == 'elliptical': - TAM3.maxOutput = 2.5E-5 # Tesla - TAM3.minOutput = -2.5E-5 # Tesla + useBounds1_str = "True" + TAM1.maxOutput = 2.5e-5 # Tesla + TAM1.minOutput = -2.5e-5 # Tesla + if orbitCase == "elliptical": + TAM3.maxOutput = 2.5e-5 # Tesla + TAM3.minOutput = -2.5e-5 # Tesla else: - useBounds1_str = 'False' + useBounds1_str = "False" TAM1.stateInMsg.subscribeTo(scObject.scStateOutMsg) scSim.AddModelToTask(simTaskName, TAM1) - if orbitCase == 'elliptical': + if orbitCase == "elliptical": TAM3.stateInMsg.subscribeTo(scObject.scStateOutMsg) scSim.AddModelToTask(simTaskName, TAM3) if useBias2: - useBias2_str = 'True' + useBias2_str = "True" TAM2.senBias = [0, 0, -1e-6] # Tesla else: - useBias2_str = 'False' + useBias2_str = "False" if useBounds2: - useBounds2_str = 'True' - TAM2.maxOutput = 2.5E-5 # Tesla - TAM2.minOutput = -2.5E-5 # Tesla + useBounds2_str = "True" + TAM2.maxOutput = 2.5e-5 # Tesla + TAM2.minOutput = -2.5e-5 # Tesla else: - useBounds2_str = 'False' + useBounds2_str = "False" TAM2.stateInMsg.subscribeTo(scObject.scStateOutMsg) scSim.AddModelToTask(simTaskName, TAM2) @@ -295,10 +308,10 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() rPeriapses = req * 1.1 # meters - if orbitCase == 'circular': + if orbitCase == "circular": oe.a = rPeriapses oe.e = 0.0000 - elif orbitCase == 'elliptical': + elif orbitCase == "elliptical": rApoapses = req * 3.5 oe.a = (rPeriapses + rApoapses) / 2.0 oe.e = 1.0 - rPeriapses / oe.a @@ -310,7 +323,9 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements # with circular or equatorial orbit, some angles are arbitrary # @@ -321,14 +336,16 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n - simulationTime = macros.sec2nano(1. * P) + P = 2.0 * np.pi / n + simulationTime = macros.sec2nano(1.0 * P) # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) mag1Log = magModule1.envOutMsgs[0].recorder(samplingTime) mag3Log = magModule3.envOutMsgs[0].recorder(samplingTime) tam1Log = TAM1.tamDataOutMsg.recorder(samplingTime) @@ -341,7 +358,7 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): scSim.AddModelToTask(simTaskName, dataLog) TAM1.magInMsg.subscribeTo(magModule1.envOutMsgs[0]) TAM2.magInMsg.subscribeTo(magModule3.envOutMsgs[0]) - if orbitCase == 'elliptical': + if orbitCase == "elliptical": mag2Log = magModule2.envOutMsgs[0].recorder(samplingTime) tam3Log = TAM3.tamDataOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, mag2Log) @@ -349,9 +366,12 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): TAM3.magInMsg.subscribeTo(magModule2.envOutMsgs[0]) # if this scenario is to interface with the BSK Viz, uncomment the following line - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) scSim.InitializeSimulation() @@ -368,7 +388,7 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): mag3Data = mag3Log.magField_N tam1Data = tam1Log.tam_S tam2Data = tam2Log.tam_S - if orbitCase == 'elliptical': + if orbitCase == "elliptical": mag2Data = mag2Log.magField_N tam3Data = tam3Log.tam_S posData = dataLog.r_BN_N @@ -384,66 +404,113 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) rData = [] timeAxis = dataLog.times() for idx in range(len(posData)): rMag = np.linalg.norm(posData[idx]) - rData.append(rMag / 1000.) - plt.plot(timeAxis * macros.NANO2SEC / P, rData, color='#aa0000') - if orbitCase == 'elliptical': - plt.plot(timeAxis * macros.NANO2SEC / P, [magModule3.envMinReach / 1000.] * len(rData), color='#007700', - dashes=[5, 5, 5, 5]) - plt.plot(timeAxis * macros.NANO2SEC / P, [magModule3.envMaxReach / 1000.] * len(rData), - color='#007700', dashes=[5, 5, 5, 5]) - plt.xlabel('Time [orbits]') - plt.ylabel('Radius [km]') + rData.append(rMag / 1000.0) + plt.plot(timeAxis * macros.NANO2SEC / P, rData, color="#aa0000") + if orbitCase == "elliptical": + plt.plot( + timeAxis * macros.NANO2SEC / P, + [magModule3.envMinReach / 1000.0] * len(rData), + color="#007700", + dashes=[5, 5, 5, 5], + ) + plt.plot( + timeAxis * macros.NANO2SEC / P, + [magModule3.envMaxReach / 1000.0] * len(rData), + color="#007700", + dashes=[5, 5, 5, 5], + ) + plt.xlabel("Time [orbits]") + plt.ylabel("Radius [km]") plt.ylim(min(rData) * 0.9, max(rData) * 1.1) figureList = {} - pltName = fileName + "1" + orbitCase + useBias1_str + useBounds1_str + useBias2_str + useBounds2_str + pltName = ( + fileName + + "1" + + orbitCase + + useBias1_str + + useBounds1_str + + useBias2_str + + useBounds2_str + ) figureList[pltName] = plt.figure(1) # plot 2 plt.figure(2) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) for idx in range(3): - plt.plot(timeAxis * macros.NANO2SEC / P, tam1Data[:, idx] * 1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$TAM_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Magnetic Field [nT] ') - plt.title('Centered Dipole Model') - if orbitCase == 'elliptical': + plt.plot( + timeAxis * macros.NANO2SEC / P, + tam1Data[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$TAM_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Magnetic Field [nT] ") + plt.title("Centered Dipole Model") + if orbitCase == "elliptical": for idx in range(3): - plt.plot(timeAxis * macros.NANO2SEC / P, tam3Data[:, idx] * 1e9, '--', - color=unitTestSupport.getLineColor(idx, 3), - label=r'$TAM_{' + str(idx) + '}$') - pltName = fileName + "2" + orbitCase + useBias1_str + useBounds1_str + useBias2_str + useBounds2_str + plt.plot( + timeAxis * macros.NANO2SEC / P, + tam3Data[:, idx] * 1e9, + "--", + color=unitTestSupport.getLineColor(idx, 3), + label=r"$TAM_{" + str(idx) + "}$", + ) + pltName = ( + fileName + + "2" + + orbitCase + + useBias1_str + + useBounds1_str + + useBias2_str + + useBounds2_str + ) figureList[pltName] = plt.figure(2) # plot 3 plt.figure(3) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - ax.get_yaxis().set_major_formatter(plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x)))) + ax.ticklabel_format(useOffset=False, style="sci") + ax.get_yaxis().set_major_formatter( + plt.FuncFormatter(lambda x, loc: "{:,}".format(int(x))) + ) for idx in range(3): - plt.plot(timeAxis * macros.NANO2SEC / P, tam2Data[:, idx] * 1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$TAM_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Magnetic Field [nT] ') - plt.title('WMM Model') - pltName = fileName + "3" + orbitCase + useBias1_str + useBounds1_str + useBias2_str + useBounds2_str + plt.plot( + timeAxis * macros.NANO2SEC / P, + tam2Data[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$TAM_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Magnetic Field [nT] ") + plt.title("WMM Model") + pltName = ( + fileName + + "3" + + orbitCase + + useBias1_str + + useBounds1_str + + useBias2_str + + useBounds2_str + ) figureList[pltName] = plt.figure(3) - if show_plots: plt.show() @@ -460,9 +527,9 @@ def run(show_plots, orbitCase, useBias1, useBias2, useBounds1, useBounds2): if __name__ == "__main__": run( True, # show_plots (True,False) - 'circular', # orbit Case (circular, elliptical) - False, #use Bias 1 (True,False) - False, #use Bias 2 (True,False) - True, #Use sensor bounds 1 (True,False) - False #Use sensor bounds 2 (True,False) + "circular", # orbit Case (circular, elliptical) + False, # use Bias 1 (True,False) + False, # use Bias 2 (True,False) + True, # Use sensor bounds 1 (True,False) + False, # Use sensor bounds 2 (True,False) ) diff --git a/examples/scenarioTempMeasurementAttitude.py b/examples/scenarioTempMeasurementAttitude.py index 498d217081..cc03b6004e 100755 --- a/examples/scenarioTempMeasurementAttitude.py +++ b/examples/scenarioTempMeasurementAttitude.py @@ -63,17 +63,34 @@ import matplotlib.pyplot as plt import numpy as np import time + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, - inertial3D, rwMotorTorque) -from Basilisk.simulation import (reactionWheelStateEffector, simpleNav, - spacecraft, motorThermal, tempMeasurement) -from Basilisk.utilities import (SimulationBaseClass, fswSetupRW, macros, - orbitalMotion, simIncludeGravBody, - simIncludeRW, unitTestSupport, vizSupport) +from Basilisk.fswAlgorithms import ( + mrpFeedback, + attTrackingError, + inertial3D, + rwMotorTorque, +) +from Basilisk.simulation import ( + reactionWheelStateEffector, + simpleNav, + spacecraft, + motorThermal, + tempMeasurement, +) +from Basilisk.utilities import ( + SimulationBaseClass, + fswSetupRW, + macros, + orbitalMotion, + simIncludeGravBody, + simIncludeRW, + unitTestSupport, + vizSupport, +) bskPath = __path__[0] fileName = os.path.basename(os.path.splitext(__file__)[0]) @@ -84,26 +101,34 @@ def plot_rw_temperature(timeData, dataTemp, numRW, id=None): """Plot the reaction wheel temperatures""" plt.figure(id) for idx in range(numRW): - plt.plot(timeData, dataTemp[:,idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$T_{rw,' + str(idx+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('RW Temperatures [ºC]') + plt.plot( + timeData, + dataTemp[:, idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$T_{rw," + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("RW Temperatures [ºC]") return + def plot_rw_temp_measurement(timeData, dataTemp, numRW, id=None): """Plot the reaction wheel temperature measurements""" plt.figure(id) for idx in range(numRW): - plt.plot(timeData, dataTemp[:,idx], - color=unitTestSupport.getLineColor(idx, numRW), - label='$T_{rw,' + str(idx+1) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Temp Measurements [ºC]') + plt.plot( + timeData, + dataTemp[:, idx], + color=unitTestSupport.getLineColor(idx, numRW), + label="$T_{rw," + str(idx + 1) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Temp Measurements [ºC]") return + def run(show_plots): """ The scenarios can be run with the followings setups parameters: @@ -120,7 +145,7 @@ def run(show_plots): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -128,7 +153,7 @@ def run(show_plots): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -139,11 +164,13 @@ def run(show_plots): scObject = spacecraft.Spacecraft() scObject.ModelTag = "bsk-Sat" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # add spacecraft object to the simulation process @@ -170,16 +197,28 @@ def run(show_plots): varRWModel = messaging.BalancedWheels # create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments - RW1 = rwFactory.create('Honeywell_HR16', [1, 0, 0], maxMomentum=50., Omega=100. # RPM - , RWModel=varRWModel - ) - RW2 = rwFactory.create('Honeywell_HR16', [0, 1, 0], maxMomentum=50., Omega=200. # RPM - , RWModel=varRWModel - ) - RW3 = rwFactory.create('Honeywell_HR16', [0, 0, 1], maxMomentum=50., Omega=300. # RPM - , rWB_B=[0.5, 0.5, 0.5] # meters - , RWModel=varRWModel - ) + RW1 = rwFactory.create( + "Honeywell_HR16", + [1, 0, 0], + maxMomentum=50.0, + Omega=100.0, # RPM + RWModel=varRWModel, + ) + RW2 = rwFactory.create( + "Honeywell_HR16", + [0, 1, 0], + maxMomentum=50.0, + Omega=200.0, # RPM + RWModel=varRWModel, + ) + RW3 = rwFactory.create( + "Honeywell_HR16", + [0, 0, 1], + maxMomentum=50.0, + Omega=300.0, # RPM + rWB_B=[0.5, 0.5, 0.5], # meters + RWModel=varRWModel, + ) # In this simulation the RW objects RW1, RW2 or RW3 are not modified further. However, you can over-ride # any values generate in the `.create()` process using for example RW1.Omega_max = 100. to change the # maximum wheel speed. @@ -212,15 +251,23 @@ def run(show_plots): # initialize the temperature measurement tempMeasList[item].ModelTag = "tempMeasurementModel" + str(item) - tempMeasList[item].senBias = 0.0 # [C] bias amount + tempMeasList[item].senBias = 0.0 # [C] bias amount tempMeasList[item].senNoiseStd = 0.5 # [C] noise standard deviation tempMeasList[item].walkBounds = 0.1 # [C] noise wald bounds - tempMeasList[item].stuckValue = 0.0 # [C] if the sensor gets stuck, stuck at 10 degrees C - tempMeasList[item].spikeProbability = 0.0 # [-] 30% chance of spiking at each time step - tempMeasList[item].spikeAmount = 0.0 # [-] 10x the actual sensed value if the spike happens + tempMeasList[ + item + ].stuckValue = 0.0 # [C] if the sensor gets stuck, stuck at 10 degrees C + tempMeasList[ + item + ].spikeProbability = 0.0 # [-] 30% chance of spiking at each time step + tempMeasList[ + item + ].spikeAmount = 0.0 # [-] 10x the actual sensed value if the spike happens tempMeasList[item].faultState = tempMeasurement.TEMP_FAULT_NOMINAL # tempMeasList[item].RNGSeed = 123 # Seed number (same seed) - tempMeasList[item].RNGSeed = time.time_ns() % (2**32) # Seed number (random for every run) + tempMeasList[item].RNGSeed = time.time_ns() % ( + 2**32 + ) # Seed number (random for every run) # add RW temperature and measurement object array to the simulation process scSim.AddModelToTask(simTaskName, rwTempList[item], 2) @@ -240,7 +287,7 @@ def run(show_plots): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = [0., 0., 0.] # set the desired inertial orientation + inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0] # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -254,7 +301,7 @@ def run(show_plots): mrpControl.K = 3.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # add module that maps the Lr control torque into the RW motor torques rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() @@ -262,16 +309,16 @@ def run(show_plots): scSim.AddModelToTask(simTaskName, rwMotorTorqueObj) # Make the RW control all three body axes - controlAxes_B = [ - 1, 0, 0, 0, 1, 0, 0, 0, 1 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] rwMotorTorqueObj.controlAxes_B = controlAxes_B # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) # A message is created that stores an array of the true temperature and temperature measurement. # This is logged here to be plotted later on. @@ -317,10 +364,13 @@ def run(show_plots): scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] # rad/s - omega_CN_B # if this scenario is to interface with the BSK Viz, uncomment the following lines - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - , rwEffectorList=rwStateEffector - ) + viz = vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + rwEffectorList=rwStateEffector, + ) # link messages sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg) diff --git a/examples/scenarioTwoChargedSC.py b/examples/scenarioTwoChargedSC.py index b79c222b10..fd458e4384 100644 --- a/examples/scenarioTwoChargedSC.py +++ b/examples/scenarioTwoChargedSC.py @@ -72,9 +72,16 @@ import numpy as np from Basilisk.architecture import messaging from Basilisk.simulation import spacecraft, extForceTorque, msmForceTorque -from Basilisk.utilities import (SimulationBaseClass, macros, - orbitalMotion, simIncludeGravBody, - unitTestSupport, RigidBodyKinematics, vizSupport, SpherePlot) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + simIncludeGravBody, + unitTestSupport, + RigidBodyKinematics, + vizSupport, + SpherePlot, +) # The path to the location of Basilisk # Used to get the location of supporting data. @@ -143,17 +150,21 @@ def run(show_plots): scSim.AddModelToTask(dynTaskName, MSMmodule) # define electric potentials - voltLeaderInMsgData = messaging.VoltMsgPayload(voltage=-500) # [V] servicer potential + voltLeaderInMsgData = messaging.VoltMsgPayload( + voltage=-500 + ) # [V] servicer potential voltLeaderInMsg = messaging.VoltMsg().write(voltLeaderInMsgData) - voltFollowerInMsgData = messaging.VoltMsgPayload(voltage=500) # [V] debris potential + voltFollowerInMsgData = messaging.VoltMsgPayload( + voltage=500 + ) # [V] debris potential voltFollowerInMsg = messaging.VoltMsg().write(voltFollowerInMsgData) # Import multi-sphere model of GOESR bus and read them into an array of strings # For each list of 4, the first 3 values are the spacial location of an individual sphere relative to a center of # [0,0,0] and the forth value is the radius of the sphere path = os.path.dirname(os.path.abspath(__file__)) - dataFileName = os.path.join(path, 'dataForExamples', 'GOESR_bus_80_sphs.csv') + dataFileName = os.path.join(path, "dataForExamples", "GOESR_bus_80_sphs.csv") scSphMod = open(dataFileName) type(scSphMod) csvreader = csv.reader(scSphMod) @@ -168,25 +179,34 @@ def run(show_plots): radii.append(float(row.pop(3))) rownum = [float(i) for i in row] spherelocation.append(rownum) - spPosListLeader_H = spherelocation # The location of each sphere for the leader spacecraft + spPosListLeader_H = ( + spherelocation # The location of each sphere for the leader spacecraft + ) rListLeader = radii # radius of each sphere in the leader spacecraft - spPosListFollower_H = spherelocation # The location of each sphere for the follower spacecraft + spPosListFollower_H = ( + spherelocation # The location of each sphere for the follower spacecraft + ) rListFollower = radii # radius of each sphere in the follower spacecraft - -# If you would like to simulate each spacecraft by a single sphere, uncomment this section (line186 - line189) of + # If you would like to simulate each spacecraft by a single sphere, uncomment this section (line186 - line189) of # code and comment out the previous section lines (162-181) -# create a list of sphere body-fixed locations and associated radii using one sphere for each spacecraft -# spPosListLeader_H = [[0,0,0]] # one sphere located at origin of body frame -# rListLeader = [2] # radius of sphere is 2m -# spPosListFollower_H = [[0,0,0]] # one sphere located at origin of body frame -# rListFollower = [2] # radius of sphere is 2m + # create a list of sphere body-fixed locations and associated radii using one sphere for each spacecraft + # spPosListLeader_H = [[0,0,0]] # one sphere located at origin of body frame + # rListLeader = [2] # radius of sphere is 2m + # spPosListFollower_H = [[0,0,0]] # one sphere located at origin of body frame + # rListFollower = [2] # radius of sphere is 2m # add spacecraft to state - MSMmodule.addSpacecraftToModel(scObjectLeader.scStateOutMsg, messaging.DoubleVector(rListLeader), - unitTestSupport.npList2EigenXdVector(spPosListLeader_H)) - MSMmodule.addSpacecraftToModel(scObjectFollower.scStateOutMsg, messaging.DoubleVector(rListFollower), - unitTestSupport.npList2EigenXdVector(spPosListFollower_H)) + MSMmodule.addSpacecraftToModel( + scObjectLeader.scStateOutMsg, + messaging.DoubleVector(rListLeader), + unitTestSupport.npList2EigenXdVector(spPosListLeader_H), + ) + MSMmodule.addSpacecraftToModel( + scObjectFollower.scStateOutMsg, + messaging.DoubleVector(rListFollower), + unitTestSupport.npList2EigenXdVector(spPosListFollower_H), + ) # subscribe input messages to module MSMmodule.voltInMsgs[0].subscribeTo(voltLeaderInMsg) @@ -213,24 +233,26 @@ def run(show_plots): # set up the Leader orbit using classical orbit elements oe = orbitalMotion.ClassicElements() oe.a = 42164 * 1e3 # [m] Geosynchronous Orbit - oe.e = 0. - oe.i = 0. - oe.Omega = 0. + oe.e = 0.0 + oe.i = 0.0 + oe.Omega = 0.0 oe.omega = 0 - oe.f = 0. + oe.f = 0.0 r_LN, v_LN = orbitalMotion.elem2rv(mu, oe) scObjectLeader.hub.r_CN_NInit = r_LN # m scObjectLeader.hub.v_CN_NInit = v_LN # m/s oe = orbitalMotion.rv2elem(mu, r_LN, v_LN) # setup Follower object states - r_FS = np.array([0, -50.0, 0.0]) # relative position of follower, 10m behind servicer in along-track direction + r_FS = np.array( + [0, -50.0, 0.0] + ) # relative position of follower, 10m behind servicer in along-track direction r_FN = r_FS + r_LN v_FN = v_LN scObjectFollower.hub.r_CN_NInit = r_FN # m scObjectFollower.hub.v_CN_NInit = v_FN # m/s n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n # orbit period + P = 2.0 * np.pi / n # orbit period # # Setup data logging before the simulation is initialized @@ -248,9 +270,12 @@ def run(show_plots): # 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: - viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, [scObjectLeader, scObjectFollower] - # , saveFile=fileName, - ) + viz = vizSupport.enableUnityVisualization( + scSim, + dynTaskName, + [scObjectLeader, scObjectFollower], + # , saveFile=fileName, + ) # # initialize Simulation @@ -264,8 +289,12 @@ def run(show_plots): scSim.ExecuteSimulation() # Retrieve the charge data of the spheres - LeaderSpCharges = unitTestSupport.columnToRowList(MSMmodule.chargeMsmOutMsgs[0].read().q) - FollowerSpCharges = unitTestSupport.columnToRowList(MSMmodule.chargeMsmOutMsgs[1].read().q) + LeaderSpCharges = unitTestSupport.columnToRowList( + MSMmodule.chargeMsmOutMsgs[0].read().q + ) + FollowerSpCharges = unitTestSupport.columnToRowList( + MSMmodule.chargeMsmOutMsgs[1].read().q + ) # retrieve the logged data from the recorders posDataL_N = dataRecL.r_BN_N @@ -286,9 +315,12 @@ def run(show_plots): relZPosData_H = [] for i in range(len(relPosData_N)): # Calculate the discrete cosine matrix for mapping from inertial frame to the Hill frame of the leader spacecraft - nrn = posDataL_N[i, :]/math.sqrt(posDataL_N[i, 0]**2 + posDataL_N[i, 1]**2 + posDataL_N[i, 2]**2) - nrh = np.cross(posDataL_N[i, 0:3], velDataL_N[i, 0:3])/np.linalg.norm(np.cross(posDataL_N[i, 0:3], - velDataL_N[i, 0:3])) + nrn = posDataL_N[i, :] / math.sqrt( + posDataL_N[i, 0] ** 2 + posDataL_N[i, 1] ** 2 + posDataL_N[i, 2] ** 2 + ) + nrh = np.cross(posDataL_N[i, 0:3], velDataL_N[i, 0:3]) / np.linalg.norm( + np.cross(posDataL_N[i, 0:3], velDataL_N[i, 0:3]) + ) nre = np.cross(nrh, nrn) HN = nrn, nre, nrh @@ -304,9 +336,24 @@ def run(show_plots): np.set_printoptions(precision=16) - figureList = plotOrbits(timeData, posDataL_N, posDataF_N, relPosMagn, attDataL_N, attDataF_N, P, spPosListLeader_H, - rListLeader, LeaderSpCharges, spPosListFollower_H, rListFollower, FollowerSpCharges, - relXPosData_H, relYPosData_H, relZPosData_H) + figureList = plotOrbits( + timeData, + posDataL_N, + posDataF_N, + relPosMagn, + attDataL_N, + attDataF_N, + P, + spPosListLeader_H, + rListLeader, + LeaderSpCharges, + spPosListFollower_H, + rListFollower, + FollowerSpCharges, + relXPosData_H, + relYPosData_H, + relZPosData_H, + ) if show_plots: plt.show() @@ -317,10 +364,24 @@ def run(show_plots): return figureList -def plotOrbits(timeData, posDataL_N, posDataF_N, relPosMagn, attDataL_N, attDataF_N, P, spPosListLeader_H, rListLeader, - LeaderSpCharges, spPosListFollower_H, rListFollower, FollowerSpCharges, relXPosData_H, relYPosData_H, - relZPosData_H): - +def plotOrbits( + timeData, + posDataL_N, + posDataF_N, + relPosMagn, + attDataL_N, + attDataF_N, + P, + spPosListLeader_H, + rListLeader, + LeaderSpCharges, + spPosListFollower_H, + rListFollower, + FollowerSpCharges, + relXPosData_H, + relYPosData_H, + relZPosData_H, +): # # draw the total separation of the spacecrafts # @@ -329,13 +390,13 @@ def plotOrbits(timeData, posDataL_N, posDataF_N, relPosMagn, attDataL_N, attData plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") plt.plot(timeData * macros.NANO2SEC / P, relPosMagn[:]) - plt.xlabel('Time [orbits]') - plt.ylabel('Separation [m]') - plt.title('Total separation') + plt.xlabel("Time [orbits]") + plt.ylabel("Separation [m]") + plt.title("Total separation") figureList = {} - pltName = 'scenarioTwoChargedSC1' + pltName = "scenarioTwoChargedSC1" figureList[pltName] = plt.figure(1) # @@ -343,9 +404,9 @@ def plotOrbits(timeData, posDataL_N, posDataF_N, relPosMagn, attDataL_N, attData # plt.figure(2, figsize=(5, 4)) - ax = plt.axes(projection='3d') + ax = plt.axes(projection="3d") # Set the Leader S/C as the center of the plot - r_LN_N = np.array([0., 0., 0.]) + r_LN_N = np.array([0.0, 0.0, 0.0]) # get sphere locations dcm_NL = RigidBodyKinematics.MRP2C(attDataF_N[0, 0:3]).transpose() spPosL_N = np.dot(dcm_NL, np.array(spPosListLeader_H).transpose()).transpose() @@ -358,24 +419,39 @@ def plotOrbits(timeData, posDataL_N, posDataF_N, relPosMagn, attDataL_N, attData z = np.outer(np.cos(u), np.ones_like(v)) for ii in range(0, len(radiiL)): r_SpN_N = r_LN_N + spPosL_N[ii, 0:3] - ax.plot_surface(r_SpN_N[0] + radiiL[ii] * x, r_SpN_N[1] + radiiL[ii] * y, r_SpN_N[2] + radiiL[ii] * z, color="b") + ax.plot_surface( + r_SpN_N[0] + radiiL[ii] * x, + r_SpN_N[1] + radiiL[ii] * y, + r_SpN_N[2] + radiiL[ii] * z, + color="b", + ) # Plot the relative position of the Follower spacecraft ax.plot(relXPosData_H, relYPosData_H, relZPosData_H) - ax.set_xlabel('Radial(m)') - ax.set_ylabel('Along Track(m)') - ax.set_zlabel('Orbit Normal (m)') - pltName = 'scenarioTwoChargedSC2' + ax.set_xlabel("Radial(m)") + ax.set_ylabel("Along Track(m)") + ax.set_zlabel("Orbit Normal (m)") + pltName = "scenarioTwoChargedSC2" figureList[pltName] = plt.figure(2) # # Draw the sphere representation of the satellites used by the MSM in the Hill frame of the Leader spacecraft # - SpherePlotList = SpherePlot.plotSpheres(posDataL_N, posDataF_N, attDataL_N, attDataF_N, spPosListLeader_H, rListLeader, - LeaderSpCharges, spPosListFollower_H, rListFollower, FollowerSpCharges) + SpherePlotList = SpherePlot.plotSpheres( + posDataL_N, + posDataF_N, + attDataL_N, + attDataF_N, + spPosListLeader_H, + rListLeader, + LeaderSpCharges, + spPosListFollower_H, + rListFollower, + FollowerSpCharges, + ) - figureList['scenarioTwoChargedSC3'] = SpherePlotList['Charged_Spheres'] - figureList['scenarioTwoChargedSC4'] = SpherePlotList['Colorbar'] + figureList["scenarioTwoChargedSC3"] = SpherePlotList["Charged_Spheres"] + figureList["scenarioTwoChargedSC4"] = SpherePlotList["Colorbar"] return figureList @@ -383,6 +459,7 @@ def plotOrbits(timeData, posDataL_N, posDataF_N, relPosMagn, attDataL_N, attData def NormalizeData(data): return (data - np.min(data)) / (np.max(data) - np.min(data)) + # # This statement below ensures that the unit test scrip can be run as a # stand-along python script diff --git a/examples/scenarioVariableTimeStepIntegrators.py b/examples/scenarioVariableTimeStepIntegrators.py index 74ea4fb460..6975ad91c6 100644 --- a/examples/scenarioVariableTimeStepIntegrators.py +++ b/examples/scenarioVariableTimeStepIntegrators.py @@ -72,17 +72,22 @@ import matplotlib.pyplot as plt import numpy as np + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ from Basilisk.simulation import spacecraft from Basilisk.simulation import svIntegrators + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions + # attempt to import vizard from Basilisk.utilities import vizSupport @@ -127,7 +132,7 @@ def run(show_plots, integratorCase, relTol, absTol): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.hour2nano(2.) + simulationTimeStep = macros.hour2nano(2.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -187,21 +192,26 @@ def run(show_plots, integratorCase, relTol, absTol): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n simulationTime = macros.sec2nano(0.9 * P) # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) # if this scenario is to interface with the BSK Viz, uncomment the following lines - vizSupport.enableUnityVisualization(scSim, simTaskName, scObject - # , saveFile=fileName - ) + vizSupport.enableUnityVisualization( + scSim, + simTaskName, + scObject, + # , saveFile=fileName + ) # # initialize Simulation @@ -235,8 +245,8 @@ def run(show_plots, integratorCase, relTol, absTol): # draw the planet fig = plt.gcf() ax = fig.gca() - ax.set_aspect('equal') - planetColor = '#008800' + ax.set_aspect("equal") + planetColor = "#008800" planetRadius = 1.0 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # draw the actual orbit @@ -245,25 +255,31 @@ def run(show_plots, integratorCase, relTol, absTol): labelStrings = ("rk4", "rkf45", "rkf78") for idx in range(0, len(posData)): oeData = orbitalMotion.rv2elem(mu, posData[idx], velData[idx]) - rData.append(oeData.rmag/earth.radEquator) + rData.append(oeData.rmag / earth.radEquator) fData.append(oeData.f + oeData.omega - oe.omega) - plt.plot(rData * np.cos(fData), rData * np.sin(fData) - , color=unitTestSupport.getLineColor(labelStrings.index(integratorCase), len(labelStrings)) - , label=integratorCase - , linewidth=3.0 - ) + plt.plot( + rData * np.cos(fData), + rData * np.sin(fData), + color=unitTestSupport.getLineColor( + labelStrings.index(integratorCase), len(labelStrings) + ), + label=integratorCase, + 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)/earth.radEquator, rData * np.sin(fData)/earth.radEquator - , '--' - , color='#555555' - ) - plt.xlabel('$i_e$ Cord. [DU]') - plt.ylabel('$i_p$ Cord. [DU]') - plt.legend(loc='lower right') + plt.plot( + rData * np.cos(fData) / earth.radEquator, + rData * np.sin(fData) / earth.radEquator, + "--", + color="#555555", + ) + plt.xlabel("$i_e$ Cord. [DU]") + plt.ylabel("$i_p$ Cord. [DU]") + plt.legend(loc="lower right") plt.grid() figureList = {} pltName = fileName @@ -286,6 +302,7 @@ def run(show_plots, integratorCase, relTol, absTol): if __name__ == "__main__": run( True, # show_plots - 'rkf78', # integrator case(0 - rk4, 1 - rkf45, 2 - rkf78) + "rkf78", # integrator case(0 - rk4, 1 - rkf45, 2 - rkf78) 1e-5, # relative tolerance - 1e-8) # absolute tolerance + 1e-8, + ) # absolute tolerance diff --git a/examples/scenarioVizPoint.py b/examples/scenarioVizPoint.py index 70b998e788..6e674b7c27 100755 --- a/examples/scenarioVizPoint.py +++ b/examples/scenarioVizPoint.py @@ -105,7 +105,9 @@ # import general simulation support files from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import matplotlib.pyplot as plt from Basilisk.utilities import macros, orbitalMotion from Basilisk.utilities import RigidBodyKinematics as rbk @@ -147,10 +149,14 @@ def run(show_plots, missionType, saveVizardFile): """ - missionOptions = ['dscovr', 'marsOrbit']; + missionOptions = ["dscovr", "marsOrbit"] if missionType not in missionOptions: - print("ERROR: scenarioVizPoint received the wrong mission type " + missionType - + ". Options include " + str(missionOptions)) + print( + "ERROR: scenarioVizPoint received the wrong mission type " + + missionType + + ". Options include " + + str(missionOptions) + ) exit(1) # Create simulation variable names @@ -161,7 +167,7 @@ def run(show_plots, missionType, saveVizardFile): scSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.min2nano(10.) + simulationTime = macros.min2nano(10.0) # # create the simulation process @@ -169,27 +175,31 @@ def run(show_plots, missionType, saveVizardFile): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(.1) + simulationTimeStep = macros.sec2nano(0.1) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # if this scenario is to interface with the BSK Viz, uncomment the following lines - if missionType == 'dscovr': + if missionType == "dscovr": # setup Grav Bodies and Spice messages gravFactory = simIncludeGravBody.gravBodyFactory() - bodies = gravFactory.createBodies('earth', 'sun') - bodies['earth'].isCentralBody = True # ensure this is the central gravitational body - spiceObject = gravFactory.createSpiceInterface(time='2018 OCT 23 04:35:25.000 (UTC)', epochInMsg=True) + bodies = gravFactory.createBodies("earth", "sun") + bodies[ + "earth" + ].isCentralBody = True # ensure this is the central gravitational body + spiceObject = gravFactory.createSpiceInterface( + time="2018 OCT 23 04:35:25.000 (UTC)", epochInMsg=True + ) - spiceObject.zeroBase = 'earth' + spiceObject.zeroBase = "earth" scSim.AddModelToTask(simTaskName, spiceObject) # Setup Camera. cameraConfig = messaging.CameraConfigMsgPayload( cameraID=1, renderRate=0, sigma_CB=[-0.333333, 0.333333, -0.333333], - cameraPos_B=[5000. * 1E-3, 0., 0.], # m - fieldOfView=0.62*macros.D2R, # rad - resolution=[2048, 2048], # pixels + cameraPos_B=[5000.0 * 1e-3, 0.0, 0.0], # m + fieldOfView=0.62 * macros.D2R, # rad + resolution=[2048, 2048], # pixels ) else: simulationTime = macros.min2nano(6.25) @@ -202,9 +212,9 @@ def run(show_plots, missionType, saveVizardFile): cameraID=1, renderRate=0, sigma_CB=[-0.333333, 0.333333, -0.333333], - cameraPos_B=[5000. * 1E-3, 0., 0.], # m - fieldOfView=50.*macros.D2R, # rad - resolution=[512, 512], # pixels + cameraPos_B=[5000.0 * 1e-3, 0.0, 0.0], # m + fieldOfView=50.0 * macros.D2R, # rad + resolution=[512, 512], # pixels ) camMsg = messaging.CameraConfigMsg().write(cameraConfig) @@ -215,11 +225,13 @@ def run(show_plots, missionType, saveVizardFile): scObject = spacecraft.Spacecraft() scObject.ModelTag = "spacecraftBody" # define the simulation inertia - I = [900., 0., 0., - 0., 800., 0., - 0., 0., 600.] + I = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] scObject.hub.mHub = 750.0 # kg - spacecraft mass - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # attach gravity model to spacecraft gravFactory.addBodiesTo(scObject) @@ -231,7 +243,7 @@ def run(show_plots, missionType, saveVizardFile): # the control torque is read in through the messaging system extFTObject = extForceTorque.ExtForceTorque() extFTObject.ModelTag = "externalDisturbance" -# extFTObject.extTorquePntB_B = [[0.25], [-0.25], [0.1]] + # extFTObject.extTorquePntB_B = [[0.25], [-0.25], [0.1]] scObject.addDynamicEffector(extFTObject) scSim.AddModelToTask(simTaskName, extFTObject) @@ -246,30 +258,36 @@ def run(show_plots, missionType, saveVizardFile): # setup the FSW algorithm tasks # - if missionType == 'dscovr': + if missionType == "dscovr": # Set up pointing frame and camera position given the initial conditions on Oct 23rd 2018 4:35 UTC # and the DDSCOVR data - earthVec = np.array([129559501208.24178, 68180766143.44236,29544768114.76163]) - normal = np.array([0.,0.,1.]) + earthVec = np.array([129559501208.24178, 68180766143.44236, 29544768114.76163]) + normal = np.array([0.0, 0.0, 1.0]) sunVec = np.array([-32509693.54023, 1002377617.77831, 423017670.86700]) - dscovrEarthDistance = 1405708000. + dscovrEarthDistance = 1405708000.0 SEVangle = 7.28 - r_sc = dscovrEarthDistance * (sunVec-earthVec)/np.linalg.norm(sunVec-earthVec) + r_sc = ( + dscovrEarthDistance + * (sunVec - earthVec) + / np.linalg.norm(sunVec - earthVec) + ) v_sc = np.zeros(3) - b1_n = -(sunVec-earthVec)/np.linalg.norm(sunVec-earthVec) - b3_n = (normal - np.dot(normal, b1_n)*b1_n)/np.linalg.norm(normal - np.dot(normal, b1_n)*b1_n) - assert np.abs(np.dot(b1_n, b3_n)) < 1E-10, 'Wrong dcm' - b2_n = np.cross(b3_n, b1_n)/np.linalg.norm( np.cross(b3_n, b1_n)) - NB = np.zeros([3,3]) + b1_n = -(sunVec - earthVec) / np.linalg.norm(sunVec - earthVec) + b3_n = (normal - np.dot(normal, b1_n) * b1_n) / np.linalg.norm( + normal - np.dot(normal, b1_n) * b1_n + ) + assert np.abs(np.dot(b1_n, b3_n)) < 1e-10, "Wrong dcm" + b2_n = np.cross(b3_n, b1_n) / np.linalg.norm(np.cross(b3_n, b1_n)) + NB = np.zeros([3, 3]) NB[:, 0] = b1_n NB[:, 1] = b2_n NB[:, 2] = b3_n earthPoint = rbk.C2MRP(NB.T) else: - earthPoint = np.array([0.,0.,0.1]) + earthPoint = np.array([0.0, 0.0, 0.1]) # create the FSW vehicle configuration message # use the same inertia in the FSW algorithm as in the simulation @@ -280,7 +298,9 @@ def run(show_plots, missionType, saveVizardFile): inertial3DObj = inertial3D.inertial3D() inertial3DObj.ModelTag = "inertial3D" scSim.AddModelToTask(simTaskName, inertial3DObj) - inertial3DObj.sigma_R0N = earthPoint.tolist() # set the desired inertial orientation + inertial3DObj.sigma_R0N = ( + earthPoint.tolist() + ) # set the desired inertial orientation # setup the attitude tracking error evaluation module attError = attTrackingError.attTrackingError() @@ -299,13 +319,15 @@ def run(show_plots, missionType, saveVizardFile): mrpControl.K = 3.5 mrpControl.Ki = -1 # make value negative to turn off integral feedback mrpControl.P = 30.0 - mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 + mrpControl.integralLimit = 2.0 / mrpControl.Ki * 0.1 # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) cmdRec = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) attErrRec = attError.attGuidOutMsg.recorder(samplingTime) dataLog = sNavObject.transOutMsg.recorder(samplingTime) @@ -318,14 +340,14 @@ def run(show_plots, missionType, saveVizardFile): # # setup the orbit using classical orbit elements # for orbit around Earth - if missionType == 'marsOrbit': + if missionType == "marsOrbit": oe = orbitalMotion.ClassicElements() - oe.a = 16000000 # meters + oe.a = 16000000 # meters oe.e = 0.1 - oe.i = 10. * macros.D2R - oe.Omega = 25. * macros.D2R - oe.omega = 10. * macros.D2R - oe.f = 160. * macros.D2R + oe.i = 10.0 * macros.D2R + oe.Omega = 25.0 * macros.D2R + oe.omega = 10.0 * macros.D2R + oe.f = 160.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) else: rN = r_sc @@ -339,8 +361,9 @@ def run(show_plots, missionType, saveVizardFile): # initialize Simulation # if saveVizardFile: - viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject, - saveFile=fileNamePath) + viz = vizSupport.enableUnityVisualization( + scSim, simTaskName, scObject, saveFile=fileNamePath + ) viz.addCamMsgToModule(camMsg) viz.settings.viewCameraConeHUD = 1 scSim.InitializeSimulation() @@ -360,7 +383,6 @@ def run(show_plots, missionType, saveVizardFile): dataPos = dataLog.r_BN_N np.set_printoptions(precision=16) - # # plot the results # @@ -368,35 +390,44 @@ def run(show_plots, missionType, saveVizardFile): timeAxis = cmdRec.times() * macros.NANO2MIN plt.figure(1) for idx in range(3): - plt.plot(timeAxis, dataSigmaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Attitude Error $\sigma_{B/R}$') + plt.plot( + timeAxis, + dataSigmaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Attitude Error $\sigma_{B/R}$") figureList = {} pltName = fileName + "1" figureList[pltName] = plt.figure(1) plt.figure(2) for idx in range(3): - plt.plot(timeAxis, dataLr[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label='$L_{r,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Control Torque $L_r$ [Nm]') + plt.plot( + timeAxis, + dataLr[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label="$L_{r," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Control Torque $L_r$ [Nm]") pltName = fileName + "2" figureList[pltName] = plt.figure(2) plt.figure(3) for idx in range(3): - plt.plot(timeAxis, dataOmegaBR[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\omega_{BR,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel('Rate Tracking Error [rad/s] ') + plt.plot( + timeAxis, + dataOmegaBR[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\omega_{BR," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel("Rate Tracking Error [rad/s] ") pltName = fileName + "3" figureList[pltName] = plt.figure(3) @@ -415,7 +446,7 @@ def run(show_plots, missionType, saveVizardFile): # if __name__ == "__main__": run( - True, # show_plots - 'marsOrbit', # missionType: dscovr or marsOrbit - True # saveVizardFile: flag to save the Vizard data file + True, # show_plots + "marsOrbit", # missionType: dscovr or marsOrbit + True, # saveVizardFile: flag to save the Vizard data file ) diff --git a/libs/mujoco/conanfile.py b/libs/mujoco/conanfile.py index aba7dd0eae..cd6f2783c1 100644 --- a/libs/mujoco/conanfile.py +++ b/libs/mujoco/conanfile.py @@ -6,12 +6,15 @@ from conan.tools.files import get, copy, load, download from conan import ConanFile + class MujocoConan(ConanFile): name = "mujoco" settings = "os", "arch" def set_version(self): - self.version = load(self, os.path.join(self.recipe_folder, "version.txt")).strip() + self.version = load( + self, os.path.join(self.recipe_folder, "version.txt") + ).strip() def package(self): url_format = "https://github.com/google-deepmind/mujoco/releases/download/{version}/mujoco-{version}-{file}" @@ -34,7 +37,7 @@ def package(self): if os_setting == "Linux" or os_setting == "Windows": self._package_linux_windows(url) - else: # Macos + else: # Macos self._package_macos(url) def _package_linux_windows(self, url: str): @@ -42,7 +45,9 @@ def _package_linux_windows(self, url: str): try: get(self, url, strip_root=self.settings.os == "Linux") except Exception as ex: - raise Exception(f"Failed to download MuJoCo source code from: '{url}'. Is the link reachable?") from ex + raise Exception( + f"Failed to download MuJoCo source code from: '{url}'. Is the link reachable?" + ) from ex package_include = os.path.join(self.package_folder, "include") package_lib = os.path.join(self.package_folder, "lib") @@ -64,7 +69,9 @@ def _package_macos(self, url): try: download(self, url, filename) except Exception as ex: - raise Exception(f"Failed to download MuJoCo source code from: '{url}'. Is the link reachable?") from ex + raise Exception( + f"Failed to download MuJoCo source code from: '{url}'. Is the link reachable?" + ) from ex disk = None @@ -72,22 +79,29 @@ def _package_macos(self, url): disk, mount_points = MujocoConan._mount_dmg_with_disk(filename) if not mount_points: - self.output.error(f"No mount points found at {filename}, downloaded from {url}") + self.output.error( + f"No mount points found at {filename}, downloaded from {url}" + ) self.output.info(f"Mounted DMG at: {', '.join(mount_points)}") possible_framework_dirs = [ - os.path.join(d, "mujoco.framework") - for d in mount_points + os.path.join(d, "mujoco.framework") for d in mount_points ] - framework_dir = next((d for d in possible_framework_dirs if os.path.isdir(d)), None) + framework_dir = next( + (d for d in possible_framework_dirs if os.path.isdir(d)), None + ) if framework_dir is None: - self.output.error("Failed to find any mount that contains the folder 'mujoco.framework'") + self.output.error( + "Failed to find any mount that contains the folder 'mujoco.framework'" + ) # Copy the headers to the expected include/mujoco folder headers_dir = os.path.join(framework_dir, "Versions", "A", "Headers") - package_include_mujoco = os.path.join(self.package_folder, "include", "mujoco") + package_include_mujoco = os.path.join( + self.package_folder, "include", "mujoco" + ) copy(self, "*.h", headers_dir, package_include_mujoco) # Copy the entire mujoco.framework dir @@ -103,7 +117,6 @@ def _package_macos(self, url): except Exception as e: self.output.error(f"Failed to detach disk: {e}") - def package_info(self): if self.settings.os == "Macos": self.cpp_info.frameworkdirs = [os.path.join(self.package_folder, "lib")] @@ -119,7 +132,8 @@ def _mount_dmg_with_disk(dmg_path): """ result = subprocess.run( ["hdiutil", "attach", dmg_path, "-nobrowse", "-plist"], - capture_output=True, check=True + capture_output=True, + check=True, ) plist_data = plistlib.loads(result.stdout) # The disk identifier is available in one of the system-entities; diff --git a/run_all_test.py b/run_all_test.py index d099c747a8..8425da8700 100644 --- a/run_all_test.py +++ b/run_all_test.py @@ -1,4 +1,4 @@ from os import system -system('cd dist3 && ctest -C Release') -system('cd src && pytest -n auto') +system("cd dist3 && ctest -C Release") +system("cd src && pytest -n auto") diff --git a/setup.py b/setup.py index 6d93ea07ab..ad4c3371a6 100644 --- a/setup.py +++ b/setup.py @@ -1,23 +1,23 @@ -''' - ISC License +""" +ISC License - Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder +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. +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. +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. -''' +""" -#------------------------------------------------------------------------------- +# ------------------------------------------------------------------------------- # XXX: Check that `setuptools` is the correct version. This check is necessary # because older versions of pip<22.3 can install site-package versions instead # of the requested package versions. But we can't check the `pip` version @@ -25,22 +25,26 @@ # So instead, we check it indirectly by ensuring that setuptools is an # acceptable version. See https://github.com/pypa/pip/issues/6264 from importlib.metadata import version # Supported on Python 3.8+ -setuptools_version = version('setuptools') + +setuptools_version = version("setuptools") if int(setuptools_version.split(".")[0]) < 64: - raise RuntimeError(f"setuptools>=64 is required to install Basilisk, but found setuptools=={setuptools_version}. " \ - f"This can happen on old versions of pip. Please upgrade with `pip install --upgrade \"pip>=22.3\"`.") -#------------------------------------------------------------------------------- + raise RuntimeError( + f"setuptools>=64 is required to install Basilisk, but found setuptools=={setuptools_version}. " + f'This can happen on old versions of pip. Please upgrade with `pip install --upgrade "pip>=22.3"`.' + ) +# ------------------------------------------------------------------------------- -#------------------------------------------------------------------------------- +# ------------------------------------------------------------------------------- # Allow user to pass arguments to Conan through environment variables. # TODO: Should allow these to be passed in as arguments, e.g. pip's # `--config-settings`. However, this is not implemented by setuptools yet. # See https://github.com/pypa/setuptools/issues/3896 import shlex import os + USER_CONAN_ARGS = shlex.split(os.getenv("CONAN_ARGS") or "") -#------------------------------------------------------------------------------- +# ------------------------------------------------------------------------------- import sys @@ -61,8 +65,10 @@ class ConanExtension(Extension): args: list[str] def __post_init__(self): - self.conanfile = Path(self.src)/"conanfile.py" - assert self.conanfile.is_file(), f"Expected to find conanfile.py file at {self.conanfile}" + self.conanfile = Path(self.src) / "conanfile.py" + assert self.conanfile.is_file(), ( + f"Expected to find conanfile.py file at {self.conanfile}" + ) class BuildConanExtCommand(Command, SubCommand): @@ -72,16 +78,27 @@ def initialize_options(self) -> None: def finalize_options(self) -> None: # NOTE: Leave the extensions in self.distribution.ext_modules to # ensure that setuptools builds this as a "platform Wheel". - self.conan_extensions = [ext for ext in self.distribution.ext_modules if isinstance(ext, ConanExtension)] + self.conan_extensions = [ + ext + for ext in self.distribution.ext_modules + if isinstance(ext, ConanExtension) + ] # Set limited ABI compatibility by default, targeting the minimum required Python version. # See https://docs.python.org/3/c-api/stable.html # NOTE: Swig 4.2.1 or higher is required, see https://github.com/swig/swig/pull/2727 - min_version = next(self.distribution.python_requires.filter([f"3.{i}" for i in range(2, 100)])).replace(".", "") - bdist_wheel = self.reinitialize_command("bdist_wheel", py_limited_api=f"cp{min_version}") + min_version = next( + self.distribution.python_requires.filter([f"3.{i}" for i in range(2, 100)]) + ).replace(".", "") + bdist_wheel = self.reinitialize_command( + "bdist_wheel", py_limited_api=f"cp{min_version}" + ) bdist_wheel.ensure_finalized() for ext in self.conan_extensions: - ext.args += ["--pyLimitedAPI", f"0x{min_version[0]:>02}{min_version[1]:>02}00f0"] + ext.args += [ + "--pyLimitedAPI", + f"0x{min_version[0]:>02}{min_version[1]:>02}00f0", + ] def get_source_files(self) -> list[str]: # NOTE: This is necessary for building sdists, and is populated @@ -103,14 +120,18 @@ def run(self) -> None: for pkg in find_packages(ext.build_dir): pkg_dir = Path(ext.build_dir, *pkg.split(".")) self.distribution.packages.append(pkg) - self.distribution.package_dir[pkg] = os.path.relpath(pkg_dir, start=HERE) + self.distribution.package_dir[pkg] = os.path.relpath( + pkg_dir, start=HERE + ) pd = self.distribution.package_data.setdefault(pkg, []) pd += ["*.dll", "**/*.dll", "*.pyd", "**/*.pyd"] if self.editable_mode and len(self.distribution.packages) == 0: - raise Exception("Tried to install in editable mode, but packages have not been prepared yet! " \ - "Please install via `python conanfile.py` instead!") + raise Exception( + "Tried to install in editable mode, but packages have not been prepared yet! " + "Please install via `python conanfile.py` instead!" + ) # Refresh `build_py` to ensure it can find the newly generated packages. build_py = self.reinitialize_command("build_py") @@ -119,8 +140,8 @@ def run(self) -> None: # XXX: Forcibly override build to run ConanExtension builder before build_py. build.sub_commands = [ - ('build_ext', build.has_ext_modules), - ('build_py', build.has_pure_modules) + ("build_ext", build.has_ext_modules), + ("build_py", build.has_pure_modules), ] @@ -134,21 +155,22 @@ def run(self) -> None: src=HERE, args=[ # (defaults) - "--buildType", "Release", - "--buildProject", "True", + "--buildType", + "Release", + "--buildProject", + "True", "--clean", # (user arguments) *USER_CONAN_ARGS, # (overrides) - "--managePipEnvironment", "False" # Force conanfile to leave pip alone. - ] + "--managePipEnvironment", + "False", # Force conanfile to leave pip alone. + ], ) ], - url="https://avslab.github.io/basilisk/", # Ensure this field is populated - # XXX: Override build_ext with ConanExtension builder. - cmdclass={'build_ext': BuildConanExtCommand}, + cmdclass={"build_ext": BuildConanExtCommand}, zip_safe=False, include_package_data=True, ) diff --git a/src/architecture/messaging/_UnitTest/test_CMsgTimeWritten.py b/src/architecture/messaging/_UnitTest/test_CMsgTimeWritten.py index eb65df90cd..3749df6aab 100644 --- a/src/architecture/messaging/_UnitTest/test_CMsgTimeWritten.py +++ b/src/architecture/messaging/_UnitTest/test_CMsgTimeWritten.py @@ -39,7 +39,7 @@ def test_CMsgTimeWritten(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.0))) # create modules mod1 = cModuleTemplate.cModuleTemplate() @@ -58,12 +58,14 @@ def test_CMsgTimeWritten(): scSim.ConfigureStopTime(macros.sec2nano(1.0)) scSim.ExecuteSimulation() - testFailCount, testMessages = uts.compareVector(msgRec.timesWritten() - , msgRec.times() - , 0.01 - , "recorded msg timesWritten was not correct." - , testFailCount - , testMessages) + testFailCount, testMessages = uts.compareVector( + msgRec.timesWritten(), + msgRec.times(), + 0.01, + "recorded msg timesWritten was not correct.", + testFailCount, + testMessages, + ) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found diff --git a/src/architecture/messaging/_UnitTest/test_MessagingInclude.py b/src/architecture/messaging/_UnitTest/test_MessagingInclude.py index 58b0a0145a..b8b9c7bf55 100644 --- a/src/architecture/messaging/_UnitTest/test_MessagingInclude.py +++ b/src/architecture/messaging/_UnitTest/test_MessagingInclude.py @@ -18,6 +18,7 @@ from Basilisk.moduleTemplates import cppModuleTemplate + def test_messagingInclude(show_plots): """test script to ensure any BSK include has access to messaging""" @@ -28,6 +29,7 @@ def test_messagingInclude(show_plots): except: assert False, "messaging inclusion test failed" + if __name__ == "__main__": test_messagingInclude( False # show_plots diff --git a/src/architecture/messaging/_UnitTest/test_NonSwigMessaging.py b/src/architecture/messaging/_UnitTest/test_NonSwigMessaging.py index fa6f0ce051..e6ff38fd12 100644 --- a/src/architecture/messaging/_UnitTest/test_NonSwigMessaging.py +++ b/src/architecture/messaging/_UnitTest/test_NonSwigMessaging.py @@ -47,7 +47,7 @@ def test_RecordingInputMessages(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.0))) # create modules mod1 = cModuleTemplate.cModuleTemplate() @@ -109,60 +109,74 @@ def test_RecordingInputMessages(): # print(dataOutRec.dataVector) # print(dataOut2Rec.dataVector) - testFailCount, testMessages = uts.compareArray([inputData.dataVector]*3 - , dataInRec.dataVector - , 0.01 - , "recorded input message was not correct." - , testFailCount - , testMessages) - - testFailCount, testMessages = uts.compareArray([[0, 0, 0], [0, 0, 0], [3, 2, 3]] - , dataOutRec.dataVector - , 0.01 - , "recorded module output message was not correct." - , testFailCount - , testMessages) - - testFailCount, testMessages = uts.compareArray([[2, 2, 3], [3, 2, 3], [4, 2, 3]] - , dataOut2Rec.dataVector - , 0.01 - , "recorded redirected module output message was not correct." - , testFailCount - , testMessages) - - testFailCount, testMessages = uts.compareArray([[4., 2., 3.]] - , [mod1.dataOutMsg.read().dataVector] - , 0.01 - , "read of module output message was not correct." - , testFailCount - , testMessages) - - testFailCount, testMessages = uts.compareArray([[4, 2, 3]] - , [attGuidMsg.read().dataVector] - , 0.01 - , "read of module redirected output message was not correct." - , testFailCount - , testMessages) - #Check that the input message subscribed with address reflects data right - testFailCount, testMessages = uts.compareArray(dataOutRec.dataVector - , dataInRecCPP.dataVector - , 0.01 - , "recorded input message from addr was not correct." - , testFailCount - , testMessages) + testFailCount, testMessages = uts.compareArray( + [inputData.dataVector] * 3, + dataInRec.dataVector, + 0.01, + "recorded input message was not correct.", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = uts.compareArray( + [[0, 0, 0], [0, 0, 0], [3, 2, 3]], + dataOutRec.dataVector, + 0.01, + "recorded module output message was not correct.", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = uts.compareArray( + [[2, 2, 3], [3, 2, 3], [4, 2, 3]], + dataOut2Rec.dataVector, + 0.01, + "recorded redirected module output message was not correct.", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = uts.compareArray( + [[4.0, 2.0, 3.0]], + [mod1.dataOutMsg.read().dataVector], + 0.01, + "read of module output message was not correct.", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = uts.compareArray( + [[4, 2, 3]], + [attGuidMsg.read().dataVector], + 0.01, + "read of module redirected output message was not correct.", + testFailCount, + testMessages, + ) + # Check that the input message subscribed with address reflects data right + testFailCount, testMessages = uts.compareArray( + dataOutRec.dataVector, + dataInRecCPP.dataVector, + 0.01, + "recorded input message from addr was not correct.", + testFailCount, + testMessages, + ) dataRecExpectation = dataInRecCPP.dataVector dummyVal = 1 for i in range(dataRecExpectation.shape[0]): dataRecExpectation[i, 0] += dummyVal dummyVal += 1 - #Check that the output message using input subscribed with address is right - testFailCount, testMessages = uts.compareArray(dataRecExpectation - , dataOutRecCPP.dataVector - , 0.01 - , "recorded output message from addr was not correct." - , testFailCount - , testMessages) + # Check that the output message using input subscribed with address is right + testFailCount, testMessages = uts.compareArray( + dataRecExpectation, + dataOutRecCPP.dataVector, + 0.01, + "recorded output message from addr was not correct.", + testFailCount, + testMessages, + ) if testFailCount: print(testMessages) diff --git a/src/architecture/messaging/_UnitTest/test_RecordInputMessages.py b/src/architecture/messaging/_UnitTest/test_RecordInputMessages.py index 1dbf8e38ff..c47558324d 100644 --- a/src/architecture/messaging/_UnitTest/test_RecordInputMessages.py +++ b/src/architecture/messaging/_UnitTest/test_RecordInputMessages.py @@ -45,7 +45,7 @@ def test_RecordingInputMessages(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.0))) # create modules mod1 = cModuleTemplate.cModuleTemplate() @@ -91,40 +91,50 @@ def test_RecordingInputMessages(): # print(dataOutRec.dataVector) # print(dataOut2Rec.dataVector) - testFailCount, testMessages = uts.compareArray([inputData.dataVector]*3 - , dataInRec.dataVector - , 0.01 - , "recorded input message was not correct." - , testFailCount - , testMessages) - - testFailCount, testMessages = uts.compareArray([[0, 0, 0], [0, 0, 0], [3, 2, 3]] - , dataOutRec.dataVector - , 0.01 - , "recorded module output message was not correct." - , testFailCount - , testMessages) - - testFailCount, testMessages = uts.compareArray([[2, 2, 3], [3, 2, 3], [4, 2, 3]] - , dataOut2Rec.dataVector - , 0.01 - , "recorded redirected module output message was not correct." - , testFailCount - , testMessages) - - testFailCount, testMessages = uts.compareArray([[4., 2., 3.]] - , [mod1.dataOutMsg.read().dataVector] - , 0.01 - , "read of module output message was not correct." - , testFailCount - , testMessages) - - testFailCount, testMessages = uts.compareArray([[4, 2, 3]] - , [attGuidMsg.read().dataVector] - , 0.01 - , "read of module redirected output message was not correct." - , testFailCount - , testMessages) + testFailCount, testMessages = uts.compareArray( + [inputData.dataVector] * 3, + dataInRec.dataVector, + 0.01, + "recorded input message was not correct.", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = uts.compareArray( + [[0, 0, 0], [0, 0, 0], [3, 2, 3]], + dataOutRec.dataVector, + 0.01, + "recorded module output message was not correct.", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = uts.compareArray( + [[2, 2, 3], [3, 2, 3], [4, 2, 3]], + dataOut2Rec.dataVector, + 0.01, + "recorded redirected module output message was not correct.", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = uts.compareArray( + [[4.0, 2.0, 3.0]], + [mod1.dataOutMsg.read().dataVector], + 0.01, + "read of module output message was not correct.", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = uts.compareArray( + [[4, 2, 3]], + [attGuidMsg.read().dataVector], + 0.01, + "read of module redirected output message was not correct.", + testFailCount, + testMessages, + ) if testFailCount: print(testMessages) diff --git a/src/architecture/messaging/_UnitTest/test_deprecatedRecorderFormat.py b/src/architecture/messaging/_UnitTest/test_deprecatedRecorderFormat.py index 606aadc4a4..28d888d86e 100644 --- a/src/architecture/messaging/_UnitTest/test_deprecatedRecorderFormat.py +++ b/src/architecture/messaging/_UnitTest/test_deprecatedRecorderFormat.py @@ -41,6 +41,7 @@ from Basilisk.architecture import messaging from Basilisk.utilities import deprecated + def test_deprecatedRecorderFormat(): """test script to ensure any BSK include has access to messaging""" @@ -58,9 +59,12 @@ def test_deprecatedRecorderFormat(): # ignore the deprecation warning, but trigger it anyway # when the deprecation time limit expires, this will turn into a # non-ignored error - with deprecated.ignore(r"Basilisk\.architecture\.messaging\..*Payload\..*Recorder\.__getattr__"): + with deprecated.ignore( + r"Basilisk\.architecture\.messaging\..*Payload\..*Recorder\.__getattr__" + ): result = recorder.accPkts - assert isinstance(result[0], dict) # expected, deprecated format + assert isinstance(result[0], dict) # expected, deprecated format + if __name__ == "__main__": test_deprecatedRecorderFormat() diff --git a/src/architecture/messaging/_UnitTest/test_unsubscribe.py b/src/architecture/messaging/_UnitTest/test_unsubscribe.py index f90b9ac380..482a1ca6bf 100644 --- a/src/architecture/messaging/_UnitTest/test_unsubscribe.py +++ b/src/architecture/messaging/_UnitTest/test_unsubscribe.py @@ -21,11 +21,12 @@ from Basilisk.architecture import messaging from Basilisk.architecture import bskLogging + def assertConnectionWithValues( inMsg: Union[messaging.CModuleTemplateMsg_C, messaging.CModuleTemplateMsgReader], - outMsg: Union[messaging.CModuleTemplateMsg_C, messaging.CModuleTemplateMsg] + outMsg: Union[messaging.CModuleTemplateMsg_C, messaging.CModuleTemplateMsg], ): - val = time.time() # unique value + val = time.time() # unique value payload = messaging.CModuleTemplateMsgPayload() payload.dataVector = [val, 0, 0] @@ -38,6 +39,7 @@ def assertConnectionWithValues( assert readPayload.dataVector[0] == val + def test_unsubscribe(): """ testing the unsubscribe functions of C and C++ messages @@ -101,5 +103,6 @@ def test_unsubscribe(): assert not messaging.CModuleTemplateMsg_C_isLinked(cMod.dataInMsg) assert not cppMod.dataInMsg.isLinked() + if __name__ == "__main__": test_unsubscribe() diff --git a/src/architecture/messaging/cMsgCInterfacePy/__init__.py b/src/architecture/messaging/cMsgCInterfacePy/__init__.py index 5b9625d083..081c9819fc 100644 --- a/src/architecture/messaging/cMsgCInterfacePy/__init__.py +++ b/src/architecture/messaging/cMsgCInterfacePy/__init__.py @@ -1,7 +1,12 @@ """ Kept only for backwards-compatibility. """ + from warnings import warn -warn("The use of `cMsgCInterfacePy` is deprecated and will be removed after 2025/08/07. Use `messaging` instead", DeprecationWarning) + +warn( + "The use of `cMsgCInterfacePy` is deprecated and will be removed after 2025/08/07. Use `messaging` instead", + DeprecationWarning, +) from Basilisk.architecture.messaging import * diff --git a/src/architecture/messaging/msgAutoSource/generatePackageInit.py b/src/architecture/messaging/msgAutoSource/generatePackageInit.py index 44ab82c79a..cd3e89ddac 100644 --- a/src/architecture/messaging/msgAutoSource/generatePackageInit.py +++ b/src/architecture/messaging/msgAutoSource/generatePackageInit.py @@ -2,23 +2,27 @@ import sys path = os.path.dirname(os.path.abspath(__file__)) -sys.path.append(path + '/../../../../../Basilisk/src/architecture/messaging/msgAutoSource') +sys.path.append( + path + "/../../../../../Basilisk/src/architecture/messaging/msgAutoSource" +) if __name__ == "__main__": moduleOutputPath = sys.argv[1] isExist = os.path.exists(moduleOutputPath) if not isExist: os.makedirs(moduleOutputPath, exist_ok=True) - mainImportFid = open(moduleOutputPath + '/__init__.py', 'w') + mainImportFid = open(moduleOutputPath + "/__init__.py", "w") for i in range(2, len(sys.argv)): headerInputPath = sys.argv[i] for filePre in os.listdir(headerInputPath): - if(filePre.endswith(".h") or filePre.endswith(".hpp")): + if filePre.endswith(".h") or filePre.endswith(".hpp"): className = os.path.splitext(filePre)[0] - msgName = className.split('Payload')[0] - mainImportFid.write('from Basilisk.architecture.messaging.' + className + ' import *\n') + msgName = className.split("Payload")[0] + mainImportFid.write( + "from Basilisk.architecture.messaging." + className + " import *\n" + ) mainImportFid.close() - setOldPath = moduleOutputPath.split('messaging')[0] + '/cMsgCInterfacePy' + setOldPath = moduleOutputPath.split("messaging")[0] + "/cMsgCInterfacePy" # XXX: Disabled: don't make a symbolic link here, because when we build a # Python wheel, the contents of the folder get copied, effectively doubling diff --git a/src/architecture/messaging/msgAutoSource/generateSWIGModules.py b/src/architecture/messaging/msgAutoSource/generateSWIGModules.py index cfdcfdbe08..ec36730112 100644 --- a/src/architecture/messaging/msgAutoSource/generateSWIGModules.py +++ b/src/architecture/messaging/msgAutoSource/generateSWIGModules.py @@ -7,53 +7,42 @@ C_TYPE_TO_NPY_ENUM = { # Signed integers "int8_t": "NPY_INT8", - "short": "NPY_INT16", "short int": "NPY_INT16", "signed short": "NPY_INT16", "signed short int": "NPY_INT16", "int16_t": "NPY_INT16", - "int": "NPY_INT32", "signed int": "NPY_INT32", "int32_t": "NPY_INT32", - "long": "NPY_LONG", "long int": "NPY_LONG", "signed long": "NPY_LONG", "signed long int": "NPY_LONG", - "long long": "NPY_INT64", "long long int": "NPY_INT64", "signed long long": "NPY_INT64", "signed long long int": "NPY_INT64", "int64_t": "NPY_INT64", - # Unsigned integers "uint8_t": "NPY_UINT8", - "unsigned short": "NPY_UINT16", "unsigned short int": "NPY_UINT16", "uint16_t": "NPY_UINT16", - "unsigned": "NPY_UINT32", "unsigned int": "NPY_UINT32", "uint32_t": "NPY_UINT32", - "unsigned long": "NPY_ULONG", "unsigned long int": "NPY_ULONG", - "unsigned long long": "NPY_UINT64", "unsigned long long int": "NPY_UINT64", "uint64_t": "NPY_UINT64", - # Platform-size signed/unsigned integers "intptr_t": "NPY_INTP", "uintptr_t": "NPY_UINTP", "ptrdiff_t": "NPY_INTP", "ssize_t": "NPY_INTP", "size_t": "NPY_UINTP", - # Floating point types "float16": "NPY_FLOAT16", "half": "NPY_FLOAT16", @@ -64,6 +53,7 @@ "long double": "NPY_LONGDOUBLE", } + def cleanSwigType(cppType: str) -> str: """ Cleans and normalizes a C++ template type string extracted from SWIG-generated XML, @@ -90,7 +80,7 @@ def cleanSwigType(cppType: str) -> str: cppType = html.unescape(cppType) # Tokenize while preserving C++ symbols - tokens = re.findall(r'\w+::|::|\w+|<|>|,|\(|\)|\[|\]|\*|&|&&|\S', cppType) + tokens = re.findall(r"\w+::|::|\w+|<|>|,|\(|\)|\[|\]|\*|&|&&|\S", cppType) def stripParensIfWrapped(typeStr: str) -> str: """ @@ -103,12 +93,12 @@ def stripParensIfWrapped(typeStr: str) -> str: str: The string with parentheses removed if they are unnecessary """ typeStr = typeStr.strip() - if typeStr.startswith('(') and typeStr.endswith(')'): + if typeStr.startswith("(") and typeStr.endswith(")"): depth = 0 for i, ch in enumerate(typeStr): - if ch == '(': + if ch == "(": depth += 1 - elif ch == ')': + elif ch == ")": depth -= 1 # If we close early, parentheses are internal – keep them if depth == 0 and i != len(typeStr) - 1: @@ -118,7 +108,7 @@ def stripParensIfWrapped(typeStr: str) -> str: def isWord(token: str) -> bool: """Return True if the token is a C++ identifier (e.g., 'int', 'const')""" - return re.fullmatch(r'\w+', token) is not None + return re.fullmatch(r"\w+", token) is not None def parseType(index: int) -> Tuple[str, int]: """ @@ -136,20 +126,20 @@ def parseType(index: int) -> Tuple[str, int]: while index < len(tokens): token = tokens[index] - if token == '<': + if token == "<": index += 1 - inner, index = parseBracketBlock(index, '<', '>', parseType) + inner, index = parseBracketBlock(index, "<", ">", parseType) parts.append(f"<{inner}>") - elif token == '(': + elif token == "(": index += 1 - inner, index = parseBracketBlock(index, '(', ')', parseType) + inner, index = parseBracketBlock(index, "(", ")", parseType) parts.append(f"({inner})") - elif token in [')', '>']: + elif token in [")", ">"]: break - elif token == ',': + elif token == ",": index += 1 break @@ -158,14 +148,18 @@ def parseType(index: int) -> Tuple[str, int]: if parts: prev = parts[-1] if isWord(prev) and isWord(token): - parts.append(' ') + parts.append(" ") parts.append(token) index += 1 - return ''.join(parts), index + return "".join(parts), index - def parseBracketBlock(index: int, openSym: str, closeSym: str, - parseFunc: Callable[[int], Tuple[str, int]]) -> Tuple[str, int]: + def parseBracketBlock( + index: int, + openSym: str, + closeSym: str, + parseFunc: Callable[[int], Tuple[str, int]], + ) -> Tuple[str, int]: """ Parses a bracketed block like <...> or (...) or [...] with nested content. @@ -184,17 +178,18 @@ def parseBracketBlock(index: int, openSym: str, closeSym: str, while index < len(tokens): if tokens[index] == closeSym: cleaned = [stripParensIfWrapped(i) for i in items] - return ', '.join(cleaned), index + 1 - elif tokens[index] == ',': + return ", ".join(cleaned), index + 1 + elif tokens[index] == ",": index += 1 # skip separator else: item, index = parseFunc(index) items.append(item) - return ', '.join(items), index + return ", ".join(items), index cleaned, _ = parseType(0) return cleaned.strip() + def parseSwigDecl(decl: str): """ Parses a SWIG `decl` string and converts it into components of a C++ declarator. @@ -237,27 +232,30 @@ def parseSwigDecl(decl: str): >>> parseSwigDecl("p.p.a(2).a(2).r.") ('**', '&', ['2', '2']) # reference to pointer to pointer to 2x2 array """ - pointerPart = '' - referencePart = '' + pointerPart = "" + referencePart = "" arrayParts = [] # Match each declarator component - tokens = re.findall(r'(a\([^)]+\)|p\.|r\.|f\(\)\.)', decl) + tokens = re.findall(r"(a\([^)]+\)|p\.|r\.|f\(\)\.)", decl) for token in tokens: - if token.startswith('a('): + if token.startswith("a("): # Array: a(N) -> N - size = re.match(r'a\(([^)]+)\)', token).group(1) + size = re.match(r"a\(([^)]+)\)", token).group(1) arrayParts.append(size) - elif token == 'p.': - pointerPart += '*' - elif token == 'r.': - referencePart = '&' # References appear after pointer + elif token == "p.": + pointerPart += "*" + elif token == "r.": + referencePart = "&" # References appear after pointer # Note: We skip f(). (function pointer) for now return pointerPart, referencePart, arrayParts -def parseSwigXml(xmlPath: str, targetStructName: str, cpp: bool, recorderPropertyRollback: bool): + +def parseSwigXml( + xmlPath: str, targetStructName: str, cpp: bool, recorderPropertyRollback: bool +): """ Parses a SWIG-generated XML file and emits RECORDER_PROPERTY macros for all struct/class member fields. @@ -310,11 +308,14 @@ def parseSwigXml(xmlPath: str, targetStructName: str, cpp: bool, recorderPropert macroName = f"RECORDER_PROPERTY_NUMERIC_{len(typeArrayParts)}" result += f"{macroName}({targetStructName}, {fieldName}, {typeWithPointerRef}, {npyType});\n" elif not recorderPropertyRollback: - fullType = f"{typeWithPointerRef}{''.join(f'[{i}]' for i in typeArrayParts)}" + fullType = ( + f"{typeWithPointerRef}{''.join(f'[{i}]' for i in typeArrayParts)}" + ) result += f"RECORDER_PROPERTY({targetStructName}, {fieldName}, ({fullType}));\n" return result + def extractAttributeMap(attributeListNode: Optional[ET.Element]): """ Converts an XML node into a Python dict. @@ -328,30 +329,37 @@ def extractAttributeMap(attributeListNode: Optional[ET.Element]): if attributeListNode is None: return {} return { - attr.attrib['name']: attr.attrib['value'] + attr.attrib["name"]: attr.attrib["value"] for attr in attributeListNode.findall("attribute") - if 'name' in attr.attrib and 'value' in attr.attrib + if "name" in attr.attrib and "value" in attr.attrib } + if __name__ == "__main__": moduleOutputPath = sys.argv[1] headerinputPath = sys.argv[2] payloadTypeName = sys.argv[3] - structType = payloadTypeName.split('Payload')[0] + structType = payloadTypeName.split("Payload")[0] baseDir = sys.argv[4] - generateCInfo = sys.argv[5] == 'True' + generateCInfo = sys.argv[5] == "True" xmlWrapPath = sys.argv[6] recorderPropertyRollback = bool(int(sys.argv[7])) - with open('msgInterfacePy.i.in', 'r') as f: + with open("msgInterfacePy.i.in", "r") as f: swigTemplateData = f.read() - with open('cMsgCInterfacePy.i.in', 'r') as f: + with open("cMsgCInterfacePy.i.in", "r") as f: swigCTemplateData = f.read() - extraContent = parseSwigXml(xmlWrapPath, payloadTypeName, not generateCInfo, recorderPropertyRollback) - - with open(moduleOutputPath, 'w') as moduleFileOut: - moduleFileOut.write(swigTemplateData.format(type=structType, baseDir=baseDir, extraContent=extraContent)) - if(generateCInfo): + extraContent = parseSwigXml( + xmlWrapPath, payloadTypeName, not generateCInfo, recorderPropertyRollback + ) + + with open(moduleOutputPath, "w") as moduleFileOut: + moduleFileOut.write( + swigTemplateData.format( + type=structType, baseDir=baseDir, extraContent=extraContent + ) + ) + if generateCInfo: moduleFileOut.write(swigCTemplateData.format(type=structType)) diff --git a/src/architecture/system_model/_UnitTest/test_PySysModel.py b/src/architecture/system_model/_UnitTest/test_PySysModel.py index d1e338d679..54add88241 100644 --- a/src/architecture/system_model/_UnitTest/test_PySysModel.py +++ b/src/architecture/system_model/_UnitTest/test_PySysModel.py @@ -28,6 +28,7 @@ import numpy as np + def test_PySysModel(): testResults, testMessage = 0, [] @@ -38,7 +39,7 @@ def test_PySysModel(): dynProcess = scSim.CreateNewProcess("dynamicsProcess") # create the dynamics task and specify the integration update time - dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(5.))) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(5.0))) # create copies of the Basilisk modules mod1 = cModuleTemplate.cModuleTemplate() @@ -75,15 +76,20 @@ def test_PySysModel(): testResults += 1 testMessage.append("TestPythonModule::UpdateState was not called") - if mod2MsgRecorder.dataVector[1,1] == 0: + if mod2MsgRecorder.dataVector[1, 1] == 0: testResults += 1 - testMessage.append("Message from TestPythonModule was not connected to message in mod2") - elif mod2MsgRecorder.dataVector[1,1] == 1: + testMessage.append( + "Message from TestPythonModule was not connected to message in mod2" + ) + elif mod2MsgRecorder.dataVector[1, 1] == 1: testResults += 1 - testMessage.append("TestPythonModule does not run before mod2 despite having greater priority") + testMessage.append( + "TestPythonModule does not run before mod2 despite having greater priority" + ) assert testResults < 1, testMessage + def test_ErrorPySysModel(): """This method tests that exceptions happening in Python module Reset and UpdateState are always printed to sys.stderr""" @@ -124,32 +130,36 @@ def test_ErrorPySysModel(): assert len(testMessage) == 0, testMessage -class PythonModule(sysModel.SysModel): +class PythonModule(sysModel.SysModel): def __init__(self, *args): super().__init__(*args) self.dataOutMsg = messaging.CModuleTemplateMsg() def Reset(self, CurrentSimNanos): payload = self.dataOutMsg.zeroMsgPayload - payload.dataVector = np.array([0,0,0]) + payload.dataVector = np.array([0, 0, 0]) self.dataOutMsg.write(payload, CurrentSimNanos, self.moduleID) self.bskLogger.bskLog(bskLogging.BSK_INFORMATION, "Reset in TestPythonModule") def UpdateState(self, CurrentSimNanos): payload = self.dataOutMsg.zeroMsgPayload - payload.dataVector = self.dataOutMsg.read().dataVector + np.array([0,1,0]) + payload.dataVector = self.dataOutMsg.read().dataVector + np.array([0, 1, 0]) self.dataOutMsg.write(payload, CurrentSimNanos, self.moduleID) - self.bskLogger.bskLog(bskLogging.BSK_INFORMATION, f"Python Module ID {self.moduleID} ran Update at {CurrentSimNanos*1e-9}s") + self.bskLogger.bskLog( + bskLogging.BSK_INFORMATION, + f"Python Module ID {self.moduleID} ran Update at {CurrentSimNanos * 1e-9}s", + ) -class ErroringPythonModule(sysModel.SysModel): +class ErroringPythonModule(sysModel.SysModel): def Reset(self): raise ValueError("Error in Reset") def UpdateState(self, CurrentSimNanos): raise ValueError("Error in UpdateState") + if __name__ == "__main__": test_PySysModel() test_ErrorPySysModel() diff --git a/src/architecture/utilitiesSelfCheck/_UnitTest/test_BSpline.py b/src/architecture/utilitiesSelfCheck/_UnitTest/test_BSpline.py index 469b6f6f13..d819800ff1 100644 --- a/src/architecture/utilitiesSelfCheck/_UnitTest/test_BSpline.py +++ b/src/architecture/utilitiesSelfCheck/_UnitTest/test_BSpline.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -33,13 +32,14 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) + + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. @pytest.mark.parametrize("P", [5, 6]) @pytest.mark.parametrize("XDot_flag", [False, True]) @pytest.mark.parametrize("XDDot_flag", [False, True]) @pytest.mark.parametrize("accuracy", [1e-6]) - def test_BSpline(show_plots, P, XDot_flag, XDDot_flag, accuracy): r""" **Validation Test Description** @@ -100,9 +100,8 @@ def test_BSpline(show_plots, P, XDot_flag, XDDot_flag, accuracy): def BSplineTestFunction(P, XDot_flag, XDDot_flag, accuracy): - - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages X1 = np.array([0, 1, 2, 3, 4, 5, 6]) X2 = np.array([5, 4, 3, 2, 1, 0, 1]) @@ -125,28 +124,56 @@ def BSplineTestFunction(P, XDot_flag, XDDot_flag, accuracy): if abs(Output.T[i][0] - Input.T[j][0]) < accuracy: if not abs(Output.X1[i][0] - X1[j]) < accuracy: testFailCount += 1 - testMessages.append("FAILED: BSpline." + " Function of order {} failed coordinate #1 check at time t = {}".format(P,Input.T[j][0])) + testMessages.append( + "FAILED: BSpline." + + " Function of order {} failed coordinate #1 check at time t = {}".format( + P, Input.T[j][0] + ) + ) if not abs(Output.X2[i][0] - X2[j]) < accuracy: testFailCount += 1 - testMessages.append("FAILED: BSpline." + " Function of order {} failed coordinate #2 check at time t = {}".format(P,Input.T[j][0])) + testMessages.append( + "FAILED: BSpline." + + " Function of order {} failed coordinate #2 check at time t = {}".format( + P, Input.T[j][0] + ) + ) if not abs(Output.X3[i][0] - X3[j]) < accuracy: testFailCount += 1 - testMessages.append("FAILED: BSpline." + " Function of order {} failed coordinate #3 check at time t = {}".format(P,Input.T[j][0])) + testMessages.append( + "FAILED: BSpline." + + " Function of order {} failed coordinate #3 check at time t = {}".format( + P, Input.T[j][0] + ) + ) if XDot_flag: - if not ((abs(Output.XD1[0][0]-Input.XDot_0[0][0]) < accuracy) and - (abs(Output.XD2[0][0]-Input.XDot_0[1][0]) < accuracy) and - (abs(Output.XD3[0][0]-Input.XDot_0[2][0]) < accuracy)): + if not ( + (abs(Output.XD1[0][0] - Input.XDot_0[0][0]) < accuracy) + and (abs(Output.XD2[0][0] - Input.XDot_0[1][0]) < accuracy) + and (abs(Output.XD3[0][0] - Input.XDot_0[2][0]) < accuracy) + ): testFailCount += 1 - testMessages.append("FAILED: BSpline." + " Function of order {} failed first derivative at starting point".format(P)) + testMessages.append( + "FAILED: BSpline." + + " Function of order {} failed first derivative at starting point".format( + P + ) + ) if XDDot_flag: - if not ((abs(Output.XDD1[0][0]-Input.XDDot_0[0][0]) < accuracy) and - (abs(Output.XDD2[0][0]-Input.XDDot_0[1][0]) < accuracy) and - (abs(Output.XDD3[0][0]-Input.XDDot_0[2][0]) < accuracy)): + if not ( + (abs(Output.XDD1[0][0] - Input.XDDot_0[0][0]) < accuracy) + and (abs(Output.XDD2[0][0] - Input.XDDot_0[1][0]) < accuracy) + and (abs(Output.XDD3[0][0] - Input.XDDot_0[2][0]) < accuracy) + ): testFailCount += 1 - testMessages.append("FAILED: BSpline." + " Function of order {} failed second derivative at starting point".format(P)) - + testMessages.append( + "FAILED: BSpline." + + " Function of order {} failed second derivative at starting point".format( + P + ) + ) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -155,7 +182,8 @@ def BSplineTestFunction(P, XDot_flag, XDDot_flag, accuracy): # if __name__ == "__main__": BSplineTestFunction( - 5, # polynomial order - True, # XDot_flag - False, # XDDot_flag - 1e-6) + 5, # polynomial order + True, # XDot_flag + False, # XDDot_flag + 1e-6, + ) diff --git a/src/architecture/utilitiesSelfCheck/_UnitTest/test_avsLibrarySelfCheck.py b/src/architecture/utilitiesSelfCheck/_UnitTest/test_avsLibrarySelfCheck.py index 75c865e65f..c60c74d63c 100644 --- a/src/architecture/utilitiesSelfCheck/_UnitTest/test_avsLibrarySelfCheck.py +++ b/src/architecture/utilitiesSelfCheck/_UnitTest/test_avsLibrarySelfCheck.py @@ -29,13 +29,17 @@ from Basilisk.utilities import unitTestSupport -@pytest.mark.parametrize("testName", - ["testRigidBodyKinematics" - , "testOrbitalElements" - , "testOrbitalAnomalies" - , "testLinearAlgebra" - , "testOrbitalHill" - , "testEnvironment"]) +@pytest.mark.parametrize( + "testName", + [ + "testRigidBodyKinematics", + "testOrbitalElements", + "testOrbitalAnomalies", + "testLinearAlgebra", + "testOrbitalHill", + "testEnvironment", + ], +) # provide a unique test method name, starting with test_ def test_unitDynamicsModes(testName): """AVS Library Self Check""" @@ -52,7 +56,9 @@ def unitAVSLibrarySelfCheck(testName): errorCount = avsLibrarySelfCheck.testRigidBodyKinematics(1e-10) if errorCount: testFailCount += errorCount - testMessages.append("ERROR: Rigid Body Kinematics Library Failed Self Test.\n") + testMessages.append( + "ERROR: Rigid Body Kinematics Library Failed Self Test.\n" + ) if testName == "testOrbitalAnomalies": errorCount = avsLibrarySelfCheck.testOrbitalAnomalies(1e-10) if errorCount: @@ -72,13 +78,15 @@ def unitAVSLibrarySelfCheck(testName): if testFailCount == 0: print("PASSED ") passFailText = "PASSED" - colorText = 'ForestGreen' # color to write auto-documented "PASSED" message in in LATEX + colorText = ( + "ForestGreen" # color to write auto-documented "PASSED" message in in LATEX + ) snippetContent = "" else: print(testFailCount) print(testMessages) - passFailText = 'FAILED' - colorText = 'Red' # color to write auto-documented "FAILED" message in in LATEX + passFailText = "FAILED" + colorText = "Red" # color to write auto-documented "FAILED" message in in LATEX snippetContent = "" for message in testMessages: snippetContent += message @@ -86,16 +94,20 @@ def unitAVSLibrarySelfCheck(testName): fileName = os.path.basename(os.path.splitext(__file__)[0]) path = os.path.dirname(os.path.abspath(__file__)) - snippetMsgName = fileName + 'Msg-' + testName - unitTestSupport.writeTeXSnippet(snippetMsgName, snippetContent, path + "/../_Documentation/") + snippetMsgName = fileName + "Msg-" + testName + unitTestSupport.writeTeXSnippet( + snippetMsgName, snippetContent, path + "/../_Documentation/" + ) - snippetPassFailName = fileName + 'TestMsg-' + testName - snippetContent = r'\textcolor{' + colorText + '}{' + passFailText + '}' - unitTestSupport.writeTeXSnippet(snippetPassFailName, snippetContent, path + "/../_Documentation/") + snippetPassFailName = fileName + "TestMsg-" + testName + snippetContent = r"\textcolor{" + colorText + "}{" + passFailText + "}" + unitTestSupport.writeTeXSnippet( + snippetPassFailName, snippetContent, path + "/../_Documentation/" + ) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -103,6 +115,4 @@ def unitAVSLibrarySelfCheck(testName): # stand-along python script # if __name__ == "__main__": - unitAVSLibrarySelfCheck( - "testOrbitalHill" - ) + unitAVSLibrarySelfCheck("testOrbitalHill") diff --git a/src/architecture/utilitiesSelfCheck/_UnitTest/test_keplerianOrbit.py b/src/architecture/utilitiesSelfCheck/_UnitTest/test_keplerianOrbit.py index 57691f9314..81c71312bc 100644 --- a/src/architecture/utilitiesSelfCheck/_UnitTest/test_keplerianOrbit.py +++ b/src/architecture/utilitiesSelfCheck/_UnitTest/test_keplerianOrbit.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,8 +15,6 @@ # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - - # # Keplerian OrbitUnit Test # @@ -37,6 +34,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) + def test_unitKeplerianOrbit(show_plots=False): """Module Unit Test""" [testResults, testMessage] = unitKeplerianOrbit(show_plots) @@ -74,10 +72,10 @@ def unitKeplerianOrbit(show_plots=False): # constructor without arguments orb = keplerianOrbit.KeplerianOrbit() - assert orb.a() == 100000000. - if not orb.a() == 100000000.: + assert orb.a() == 100000000.0 + if not orb.a() == 100000000.0: testFailCount += 1 - testMessages.append('default constructor failure') + testMessages.append("default constructor failure") # constructor with arguments oe = orb.oe() @@ -85,54 +83,54 @@ def unitKeplerianOrbit(show_plots=False): assert orb2.r_BP_P() == orb.r_BP_P() if not orb2.r_BP_P() == orb.r_BP_P(): testFailCount += 1 - testMessages.append('Argumented constructor failure') + testMessages.append("Argumented constructor failure") # copy constructor orb3 = keplerianOrbit.KeplerianOrbit(orb2) assert orb2.v_BP_P() == orb3.v_BP_P() if not orb2.v_BP_P() == orb3.v_BP_P(): testFailCount += 1 - testMessages.append('Copy Constructor Failure') + testMessages.append("Copy Constructor Failure") try: orb4 = copy(orb3) except: assert False testFailCount += 1 - testMessages.append('python copy not working') + testMessages.append("python copy not working") # changing orbital elements orb3.set_f(0.0) init_r = orb3.r() - orb3.set_f(1.) + orb3.set_f(1.0) assert init_r != orb3.r() if init_r == orb3.r(): testFailCount += 1 - testMessages.append('Failure to change element') + testMessages.append("Failure to change element") orb3.set_f(0.0) assert init_r == orb3.r() if init_r != orb3.r(): testFailCount += 1 - testMessages.append('Failure to change element') + testMessages.append("Failure to change element") # mean motion calc - expected_n = np.sqrt(orbitalMotion.MU_EARTH / orb3.a()**3) + expected_n = np.sqrt(orbitalMotion.MU_EARTH / orb3.a() ** 3) assert orb3.n() == expected_n if not orb3.n() == expected_n: testFailCount += 1 - testMessages.append('Bad mean motion calc') + testMessages.append("Bad mean motion calc") # orbital period calc assert orb3.P() == 2 * np.pi / expected_n if not orb3.P() == 2 * np.pi / expected_n: testFailCount += 1 - testMessages.append('Bad period calc') + testMessages.append("Bad period calc") # orbital energy calc expected_E = -orbitalMotion.MU_EARTH / 2.0 / orb3.a() assert orb3.Energy() == expected_E if not orb3.Energy() == expected_E: testFailCount += 1 - testMessages.append('Bad energy calc') + testMessages.append("Bad energy calc") # rv calc expected_r, expected_v = orbitalMotion.elem2rv(orbitalMotion.MU_EARTH, orb3.oe()) @@ -140,9 +138,10 @@ def unitKeplerianOrbit(show_plots=False): assert dist == 0.0 if not dist == 0.0: testFailCount += 1 - testMessages.append('RV conversion failure') + testMessages.append("RV conversion failure") + + return [testFailCount, "".join(testMessages)] - return [testFailCount, ''.join(testMessages)] if __name__ == "__main__": unitKeplerianOrbit() diff --git a/src/conftest.py b/src/conftest.py index 834f4b6177..b7c1ea338b 100644 --- a/src/conftest.py +++ b/src/conftest.py @@ -30,29 +30,34 @@ # remove the old report because we don't want stale data around, even without pytest-html # for more see reportconf.py -if os.path.exists('tests/report/'): - shutil.rmtree('tests/report/') +if os.path.exists("tests/report/"): + shutil.rmtree("tests/report/") def pytest_addoption(parser): - parser.addoption("--show_plots", action="store_true", - help="test(s) shall display plots") - parser.addoption("--report", action="store_true", # --report is easier, more controlled than --html= - help="whether or not to gen a pytest-html report. The report is saved in ./tests/report") + parser.addoption( + "--show_plots", action="store_true", help="test(s) shall display plots" + ) + parser.addoption( + "--report", + action="store_true", # --report is easier, more controlled than --html= + help="whether or not to gen a pytest-html report. The report is saved in ./tests/report", + ) @pytest.fixture(scope="module") def show_plots(request): return request.config.getoption("--show_plots") + # we don't want to reconfigure pytest per pytest-html unless we have it # for more on this, see the reportconf.py file. -reqs = subprocess.check_output([sys.executable, '-m', 'pip', 'freeze']) -installed_packages = [r.decode().split('==')[0] for r in reqs.split()] +reqs = subprocess.check_output([sys.executable, "-m", "pip", "freeze"]) +installed_packages = [r.decode().split("==")[0] for r in reqs.split()] -if ('--report' in sys.argv) and ('pytest-html' not in installed_packages): - print('ERROR: you need to pip install pytest-html package to use the --report flag') +if ("--report" in sys.argv) and ("pytest-html" not in installed_packages): + print("ERROR: you need to pip install pytest-html package to use the --report flag") quit() -if 'pytest-html' in installed_packages: +if "pytest-html" in installed_packages: exec(open(path + "/reportconf.py").read(), globals()) diff --git a/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_UnitTest/test_lowPassFilterTorqueCommand.py b/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_UnitTest/test_lowPassFilterTorqueCommand.py index 15e53cfd99..332b3e5a81 100755 --- a/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_UnitTest/test_lowPassFilterTorqueCommand.py +++ b/src/fswAlgorithms/attControl/lowPassFilterTorqueCommand/_UnitTest/test_lowPassFilterTorqueCommand.py @@ -25,54 +25,61 @@ import matplotlib.pyplot as plt from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import lowPassFilterTorqueCommand # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + lowPassFilterTorqueCommand, +) # import the module that is to be tested + # Import all of the modules that we are going to call in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # import packages as needed e.g. 'numpy', 'ctypes, 'math' etc. + # uncomment this line is 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(conditionstring) # provide a unique test method name, starting with test_ -def test_lowPassFilterControlTorque(show_plots): # update "subModule" in this function name to reflect the module name +def test_lowPassFilterControlTorque( + show_plots, +): # update "subModule" in this function name to reflect the module name """Module Unit Test""" [testResults, testMessage] = subModuleTestFunction(show_plots) assert testResults < 1, testMessage + def subModuleTestFunction(show_plots): # zero all unit test result gather variables - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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() - # this create a fresh and consistent simulation environment for each test run + # this create a fresh and consistent simulation environment for each test run # Create test thread - testProcessRate = macros.sec2nano(0.5) # process rate update time + testProcessRate = macros.sec2nano(0.5) # process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = lowPassFilterTorqueCommand.lowPassFilterTorqueCommand() - module.ModelTag = "lowPassFilterTorqueCommand" # python name of test module. + module.ModelTag = "lowPassFilterTorqueCommand" # python name of test module. # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - module.wc = 0.1*math.pi*2 # [rad/s] continous time critical filter frequency - module.h = 0.5 # [s] filter time step - module.reset = 1 # flag to initialize module states on first run - + module.wc = 0.1 * math.pi * 2 # [rad/s] continous time critical filter frequency + module.h = 0.5 # [s] filter time step + module.reset = 1 # flag to initialize module states on first run # Create input message and size it because the regular creator of that message # is not part of the test. @@ -91,47 +98,50 @@ def subModuleTestFunction(show_plots): unitTestSim.InitializeSimulation() # Step the simulation to 3*process rate so 4 total steps including zero - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - module.Reset(1) # this module reset function needs a time input (in NanoSeconds) + module.Reset(1) # this module reset function needs a time input (in NanoSeconds) - unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() LrF = outLog.torqueRequestBody # set the filtered output truth states LrFtrue = [ - [0.2734574719946391,-0.1367287359973196,0.1914202303962474], - [0.4721359549995794,-0.2360679774997897,0.3304951684997055], - [0.6164843223022588,-0.3082421611511294,0.4315390256115811], - [0.2734574719946391,-0.1367287359973196,0.1914202303962474], - [0.4721359549995794,-0.2360679774997897,0.3304951684997055], - ] + [0.2734574719946391, -0.1367287359973196, 0.1914202303962474], + [0.4721359549995794, -0.2360679774997897, 0.3304951684997055], + [0.6164843223022588, -0.3082421611511294, 0.4315390256115811], + [0.2734574719946391, -0.1367287359973196, 0.1914202303962474], + [0.4721359549995794, -0.2360679774997897, 0.3304951684997055], + ] # compare the module and truth results - for i in range(0,len(LrFtrue)): + for i in range(0, len(LrFtrue)): if not unitTestSupport.isArrayEqual(LrF[i], LrFtrue[i], 3, 1e-12): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed LrFtrue unit test at t=" + str(LrF[i,0]*unitTestSupport.NANO2SEC) + "sec\n") - - + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed LrFtrue unit test at t=" + + str(LrF[i, 0] * unitTestSupport.NANO2SEC) + + "sec\n" + ) # If the argument provided at commandline "--show_plots" evaluates as true, # plot all figures if show_plots: plt.show() - plt.close('all') + plt.close("all") # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag) - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attControl/mrpFeedback/_UnitTest/test_mrpFeedback.py b/src/fswAlgorithms/attControl/mrpFeedback/_UnitTest/test_mrpFeedback.py index 68c14e82be..860ad77db9 100644 --- a/src/fswAlgorithms/attControl/mrpFeedback/_UnitTest/test_mrpFeedback.py +++ b/src/fswAlgorithms/attControl/mrpFeedback/_UnitTest/test_mrpFeedback.py @@ -36,7 +36,7 @@ # uncomment this line is 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() +# @pytest.mark.xfail() # provide a unique test method name, starting with test_ @@ -45,64 +45,66 @@ @pytest.mark.parametrize("integralLimit", [0, 20]) @pytest.mark.parametrize("ctrlLaw", [0, 1]) @pytest.mark.parametrize("useRwAvailability", ["NO", "ON", "OFF"]) - -def test_MRP_Feedback(show_plots, intGain, rwNum, integralLimit, ctrlLaw, useRwAvailability): +def test_MRP_Feedback( + show_plots, intGain, rwNum, integralLimit, ctrlLaw, useRwAvailability +): r""" - **Validation Test Description** - - The unit test for this module tests a set of gains :math:`K`, :math:`K_i`, :math:`P` on a rigid body - with no external torques, and with a fixed input reference attitude message. The torque requested - by the controller is evaluated against python computed torques at 0s, 0.5s, 1s, 1.5s and 2s to - within a tolerance of :math:`10^{-8}`. After 1s the simulation is stopped and the ``Reset()`` - function is called to check that integral feedback related variables are properly reset. - The following permutations are run: - - - The test is run for a case with error integration feedback (:math:`k_i`=0.01) and one case - where :math:`k_i` is set to a negative value, resulting in a case with no integrator. - - The RW array number is configured either to 4 or 0 - - The integral limit term is set to either 0 or 20 - - The RW availability message is tested in 3 manners. Either the availability message is not - written where all wheels should default to being available. If the availability message is - written, then the RWs are either zero to available or not available. - - The control parameter :math:`\delta\omega_{0}` is set to either a zero or non-zero vector - - All permutations of these test cases are expected to pass. - - - **Test Parameters** - - Args: - intGain (float): value of the integral gain :math:`K_i` - rwNum (int): number of RW devices to simulate - integralLimit (float): value of the integral limit - ctrlLaw (int): type of control law used - useRwAvailability (string): Flag to not use RW availabillity (``NO``), use the availability - message and turn on the RW devices (``ON``) and use the message and turn off the devices (``OFF``) + **Validation Test Description** + + The unit test for this module tests a set of gains :math:`K`, :math:`K_i`, :math:`P` on a rigid body + with no external torques, and with a fixed input reference attitude message. The torque requested + by the controller is evaluated against python computed torques at 0s, 0.5s, 1s, 1.5s and 2s to + within a tolerance of :math:`10^{-8}`. After 1s the simulation is stopped and the ``Reset()`` + function is called to check that integral feedback related variables are properly reset. + The following permutations are run: + + - The test is run for a case with error integration feedback (:math:`k_i`=0.01) and one case + where :math:`k_i` is set to a negative value, resulting in a case with no integrator. + - The RW array number is configured either to 4 or 0 + - The integral limit term is set to either 0 or 20 + - The RW availability message is tested in 3 manners. Either the availability message is not + written where all wheels should default to being available. If the availability message is + written, then the RWs are either zero to available or not available. + - The control parameter :math:`\delta\omega_{0}` is set to either a zero or non-zero vector + + All permutations of these test cases are expected to pass. + + + **Test Parameters** + + Args: + intGain (float): value of the integral gain :math:`K_i` + rwNum (int): number of RW devices to simulate + integralLimit (float): value of the integral limit + ctrlLaw (int): type of control law used + useRwAvailability (string): Flag to not use RW availabillity (``NO``), use the availability + message and turn on the RW devices (``ON``) and use the message and turn off the devices (``OFF``) """ # each test method requires a single assert method to be called - [testResults, testMessage] = run(show_plots,intGain, rwNum, integralLimit, ctrlLaw, useRwAvailability) + [testResults, testMessage] = run( + show_plots, intGain, rwNum, integralLimit, ctrlLaw, useRwAvailability + ) assert testResults < 1, testMessage def run(show_plots, intGain, rwNum, integralLimit, ctrlLaw, useRwAvailability): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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() - # this creates a fresh and consistent simulation environment for each test run + # this creates a fresh and consistent simulation environment for each test run # Create test thread - testProcessRate = macros.sec2nano(0.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = mrpFeedback.mrpFeedback() module.ModelTag = "mrpFeedback" @@ -116,7 +118,7 @@ def run(show_plots, intGain, rwNum, integralLimit, ctrlLaw, useRwAvailability): module.P = 150.0 module.integralLimit = integralLimit module.controlLawType = ctrlLaw - module.knownTorquePntB_B = [1., 1., 1.] + module.knownTorquePntB_B = [1.0, 1.0, 1.0] # create input messages # AttGuidFswMsg Message: @@ -133,9 +135,7 @@ def run(show_plots, intGain, rwNum, integralLimit, ctrlLaw, useRwAvailability): # vehicleConfigData Message: vehicleConfig = messaging.VehicleConfigMsgPayload() - I = [1000., 0., 0., - 0., 800., 0., - 0., 0., 800.] + I = [1000.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 800.0] vehicleConfig.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfig) @@ -148,14 +148,23 @@ def run(show_plots, intGain, rwNum, integralLimit, ctrlLaw, useRwAvailability): # wheelConfigData message jsList = [] GsMatrix_B = [] + def writeMsgInWheelConfiguration(): rwConfigParams = messaging.RWArrayConfigMsgPayload() rwConfigParams.GsMatrix_B = [ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - 0.577350269190, 0.577350269190, 0.577350269190 + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.577350269190, + 0.577350269190, + 0.577350269190, ] rwConfigParams.JsList = [0.1, 0.1, 0.1, 0.1] rwConfigParams.numRW = rwNum @@ -169,21 +178,42 @@ def writeMsgInWheelConfiguration(): rwAvailabilityMessage = messaging.RWAvailabilityMsgPayload() if useRwAvailability != "NO": if useRwAvailability == "ON": - rwAvailabilityMessage.wheelAvailability = [messaging.AVAILABLE, messaging.AVAILABLE, - messaging.AVAILABLE, messaging.AVAILABLE] + rwAvailabilityMessage.wheelAvailability = [ + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + ] elif useRwAvailability == "OFF": - rwAvailabilityMessage.wheelAvailability = [messaging.UNAVAILABLE, messaging.UNAVAILABLE, - messaging.UNAVAILABLE, messaging.UNAVAILABLE] + rwAvailabilityMessage.wheelAvailability = [ + messaging.UNAVAILABLE, + messaging.UNAVAILABLE, + messaging.UNAVAILABLE, + messaging.UNAVAILABLE, + ] else: print("WARNING: unknown rw availability status") rwAvailInMsg = messaging.RWAvailabilityMsg().write(rwAvailabilityMessage) else: # set default availability - rwAvailabilityMessage.wheelAvailability = [messaging.AVAILABLE, messaging.AVAILABLE, - messaging.AVAILABLE, messaging.AVAILABLE] + rwAvailabilityMessage.wheelAvailability = [ + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + ] - LrTrue = findTrueTorques(module, guidCmdData, rwSpeedMessage, vehicleConfig, jsList, - rwNum, GsMatrix_B, rwAvailabilityMessage, ctrlLaw) + LrTrue = findTrueTorques( + module, + guidCmdData, + rwSpeedMessage, + vehicleConfig, + jsList, + rwNum, + GsMatrix_B, + rwAvailabilityMessage, + ctrlLaw, + ) # Setup logging on the test module output message so that we get all the writes to it dataLog = module.cmdTorqueOutMsg.recorder() @@ -202,22 +232,29 @@ def writeMsgInWheelConfiguration(): unitTestSim.InitializeSimulation() # Step the simulation to 3*process rate so 4 total steps including zero - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - module.Reset(1) # this module reset function needs a time input (in NanoSeconds) + module.Reset(1) # this module reset function needs a time input (in NanoSeconds) - unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() # compare the module results to the truth values accuracy = 1e-8 for i in range(0, len(LrTrue)): # check vector values - if not unitTestSupport.isArrayEqual(dataLog.torqueRequestBody[i], LrTrue[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.torqueRequestBody[i], LrTrue[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed mrpFeedback unit test at t=" - + str(dataLog.times()[i]*macros.NANO2SEC) + "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed mrpFeedback unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) # print out success message if no error were found if testFailCount == 0: @@ -227,18 +264,28 @@ def writeMsgInWheelConfiguration(): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] - - -def findTrueTorques(module,guidCmdData,rwSpeedMessage,vehicleConfigOut,jsList,numRW,GsMatrix_B,rwAvailMsg,ctrlLaw): + return [testFailCount, "".join(testMessages)] + + +def findTrueTorques( + module, + guidCmdData, + rwSpeedMessage, + vehicleConfigOut, + jsList, + numRW, + GsMatrix_B, + rwAvailMsg, + ctrlLaw, +): Lr = [] - #Read in variables + # Read in variables L = np.asarray(module.knownTorquePntB_B) - steps = [0, 0, .5, 0, .5] + steps = [0, 0, 0.5, 0, 0.5] omega_BR_B = np.asarray(guidCmdData.omega_BR_B) omega_RN_B = np.asarray(guidCmdData.omega_RN_B) - omega_BN_B = omega_BR_B + omega_RN_B #find body rate + omega_BN_B = omega_BR_B + omega_RN_B # find body rate domega_RN_B = np.asarray(guidCmdData.domega_RN_B) sigma_BR = np.asarray(guidCmdData.sigma_BR) Isc = np.asarray(vehicleConfigOut.ISCPntB_B) @@ -248,59 +295,73 @@ def findTrueTorques(module,guidCmdData,rwSpeedMessage,vehicleConfigOut,jsList,nu P = module.P jsVec = jsList GsMatrix_B_array = np.asarray(GsMatrix_B) - GsMatrix_B_array = np.reshape(GsMatrix_B_array[0:numRW * 3], (numRW, 3)) + GsMatrix_B_array = np.reshape(GsMatrix_B_array[0 : numRW * 3], (numRW, 3)) sigmaInt = np.asarray([0, 0, 0]) - #Compute toruqes + # Compute toruqes for i in range(len(steps)): dt = steps[i] if dt == 0: sigmaInt = np.asarray([0, 0, 0]) - #evaluate integral term - if Ki > 0: #if integral feedback is on + # evaluate integral term + if Ki > 0: # if integral feedback is on sigmaInt = K * dt * sigma_BR + sigmaInt for n in range(3): if abs(sigmaInt[n]) > module.integralLimit: - sigmaInt[n] *= module.integralLimit/sigmaInt[n] #check elementwise if integral term is greater than limit; preserve direction (+/-) + sigmaInt[n] *= ( + module.integralLimit / sigmaInt[n] + ) # check elementwise if integral term is greater than limit; preserve direction (+/-) zVec = sigmaInt + Isc.dot(omega_BR_B) - else: #integral gain turned off/negative setting + else: # integral gain turned off/negative setting zVec = np.asarray([0, 0, 0]) - #compute torque Lr + # compute torque Lr Lr0 = K * sigma_BR # +K*sigmaBR Lr1 = Lr0 + P * omega_BR_B # +P*deltaOmega Lr2 = Lr1 + P * Ki * zVec # +P*Ki*z - GsHs = np.array([0,0,0]) + GsHs = np.array([0, 0, 0]) - if numRW>0: + if numRW > 0: for i in range(numRW): - if rwAvailMsg.wheelAvailability[i] == 0: #Make RW availability check - GsHs = GsHs + np.dot(GsMatrix_B_array[i, :], jsVec[i]*(np.dot(omega_BN_B, GsMatrix_B_array[i, :])+rwSpeedMessage.wheelSpeeds[i])) - #J_s*(dot(omegaBN_B,Gs_vec)+Omega_wheel) + if rwAvailMsg.wheelAvailability[i] == 0: # Make RW availability check + GsHs = GsHs + np.dot( + GsMatrix_B_array[i, :], + jsVec[i] + * ( + np.dot(omega_BN_B, GsMatrix_B_array[i, :]) + + rwSpeedMessage.wheelSpeeds[i] + ), + ) + # J_s*(dot(omegaBN_B,Gs_vec)+Omega_wheel) if ctrlLaw == 0: - Lr3 = Lr2 - np.cross((omega_RN_B+Ki*zVec), (Isc.dot(omega_BN_B)+GsHs)) # -[v3Tilde(omega_r+Ki*z)]([I]omega + [Gs]h_s) + Lr3 = Lr2 - np.cross( + (omega_RN_B + Ki * zVec), (Isc.dot(omega_BN_B) + GsHs) + ) # -[v3Tilde(omega_r+Ki*z)]([I]omega + [Gs]h_s) else: - Lr3 = Lr2 - np.cross(omega_BN_B, (Isc.dot(omega_BN_B)+GsHs)) # -[v3Tilde(omega)]([I]omega + [Gs]h_s) + Lr3 = Lr2 - np.cross( + omega_BN_B, (Isc.dot(omega_BN_B) + GsHs) + ) # -[v3Tilde(omega)]([I]omega + [Gs]h_s) - Lr4 = Lr3 + Isc.dot(-domega_RN_B + np.cross(omega_BN_B, omega_RN_B)) #+[I](-d(omega_r)/dt + omega x omega_r) + Lr4 = Lr3 + Isc.dot( + -domega_RN_B + np.cross(omega_BN_B, omega_RN_B) + ) # +[I](-d(omega_r)/dt + omega x omega_r) Lr5 = Lr4 + L Lr5 = -Lr5 Lr.append(np.ndarray.tolist(Lr5)) return Lr - - # This statement below ensures that the unitTestScript can be run as a stand-along python scripts # automatically executes the test_MRP_Feedback() method # if __name__ == "__main__": - test_MRP_Feedback(False, # showplots - 0.01, # intGain - 0, # rwNum - 0.0, # integralLimit - "NO" # useRwAvailability ("NO", "ON", "OFF") - ) + test_MRP_Feedback( + False, # showplots + 0.01, # intGain + 0, # rwNum + 0.0, # integralLimit + "NO", # useRwAvailability ("NO", "ON", "OFF") + ) diff --git a/src/fswAlgorithms/attControl/mrpPD/_UnitTest/Support/results_MRP_PD.py b/src/fswAlgorithms/attControl/mrpPD/_UnitTest/Support/results_MRP_PD.py index 33ca89b99f..5dc2983525 100644 --- a/src/fswAlgorithms/attControl/mrpPD/_UnitTest/Support/results_MRP_PD.py +++ b/src/fswAlgorithms/attControl/mrpPD/_UnitTest/Support/results_MRP_PD.py @@ -16,6 +16,7 @@ # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. # import numpy as np + np.set_printoptions(precision=12) # Initial Conditions @@ -24,11 +25,7 @@ omega_RN_B = np.array([-0.02, -0.01, 0.005]) domega_RN_B = np.array([0.0002, 0.0003, 0.0001]) -I = np.array([ - [1000., 0., 0.], - [0., 800., 0.], - [0., 0., 800.] -]) +I = np.array([[1000.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 800.0]]) K = 0.15 P = 150.0 @@ -43,4 +40,4 @@ Lr *= -1.0 # Print Results -print('Lr = ', Lr) +print("Lr = ", Lr) diff --git a/src/fswAlgorithms/attControl/mrpPD/_UnitTest/test_mrpPD.py b/src/fswAlgorithms/attControl/mrpPD/_UnitTest/test_mrpPD.py index d89be23a59..0a314b015a 100644 --- a/src/fswAlgorithms/attControl/mrpPD/_UnitTest/test_mrpPD.py +++ b/src/fswAlgorithms/attControl/mrpPD/_UnitTest/test_mrpPD.py @@ -25,13 +25,10 @@ path = os.path.dirname(os.path.abspath(filename)) - - - - - from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.fswAlgorithms import mrpPD # import the module that is to be tested from Basilisk.utilities import macros from Basilisk.architecture import messaging @@ -42,8 +39,8 @@ # @pytest.mark.xfail() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("setExtTorque", [False, True]) +@pytest.mark.parametrize("setExtTorque", [False, True]) def test_mrp_PD_tracking(show_plots, setExtTorque): r""" **Validation Test Description** @@ -111,9 +108,7 @@ def mrp_PD_tracking(show_plots, setExtTorque): # vehicleConfig FSW Message: vehicleConfigIn = messaging.VehicleConfigMsgPayload() - vehicleConfigIn.ISCPntB_B = [1000., 0., 0., - 0., 800., 0., - 0., 0., 800.] + vehicleConfigIn.ISCPntB_B = [1000.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 800.0] vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigIn) # Setup logging on the test module output message so that we get all the writes to it @@ -131,29 +126,35 @@ def mrp_PD_tracking(show_plots, setExtTorque): unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - trueVector = [findTrueTorques(module, guidCmdData, vehicleConfigIn)]*3 + trueVector = [findTrueTorques(module, guidCmdData, vehicleConfigIn)] * 3 # print trueVector # compare the module results to the truth values accuracy = 1e-12 print("accuracy = " + str(accuracy)) - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, dataLog.torqueRequestBody, accuracy, - "torqueRequestBody", testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + dataLog.torqueRequestBody, + accuracy, + "torqueRequestBody", + testFailCount, + testMessages, + ) snippentName = "passFail" + str(setExtTorque) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def findTrueTorques(module, guidCmdData, vehicleConfigOut): @@ -182,6 +183,5 @@ def findTrueTorques(module, guidCmdData, vehicleConfigOut): return Lr - if __name__ == "__main__": test_mrp_PD_tracking(False, False) diff --git a/src/fswAlgorithms/attControl/mrpSteering/_UnitTest/test_MRP_steeringInt.py b/src/fswAlgorithms/attControl/mrpSteering/_UnitTest/test_MRP_steeringInt.py index 81427677ab..95a92465bc 100644 --- a/src/fswAlgorithms/attControl/mrpSteering/_UnitTest/test_MRP_steeringInt.py +++ b/src/fswAlgorithms/attControl/mrpSteering/_UnitTest/test_MRP_steeringInt.py @@ -24,7 +24,9 @@ from Basilisk.utilities import RigidBodyKinematics from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -33,12 +35,11 @@ # @pytest.mark.xfail() # need to update how the RW states are defined # provide a unique test method name, starting with test_ + @pytest.mark.parametrize("K1", [0.15, 0]) @pytest.mark.parametrize("K3", [1, 0]) @pytest.mark.parametrize("omegaMax", [1.5 * macros.D2R, 0.001]) - - -def test_mrp_steering_tracking(show_plots,K1, K3, omegaMax): +def test_mrp_steering_tracking(show_plots, K1, K3, omegaMax): r""" **Validation Test Description** @@ -59,11 +60,11 @@ def test_mrp_steering_tracking(show_plots,K1, K3, omegaMax): :return: void """ - [testResults, testMessage] = mrp_steering_tracking(show_plots,K1, K3, omegaMax) + [testResults, testMessage] = mrp_steering_tracking(show_plots, K1, K3, omegaMax) assert testResults < 1, testMessage -def mrp_steering_tracking(show_plots,K1, K3, omegaMax): +def mrp_steering_tracking(show_plots, K1, K3, omegaMax): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. @@ -98,13 +99,15 @@ def mrp_steering_tracking(show_plots,K1, K3, omegaMax): module.omega_max = omegaMax servo.Ki = 0.01 servo.P = 150.0 - servo.integralLimit = 2. / servo.Ki * 0.1 - servo.knownTorquePntB_B = [0., 0., 0.] + servo.integralLimit = 2.0 / servo.Ki * 0.1 + servo.knownTorquePntB_B = [0.0, 0.0, 0.0] # Create input message and size it because the regular creator of that message # is not part of the test. # attGuidOut Message: - guidCmdData = messaging.AttGuidMsgPayload() # Create a structure for the input message + guidCmdData = ( + messaging.AttGuidMsgPayload() + ) # Create a structure for the input message guidCmdData.sigma_BR = [0.3, -0.5, 0.7] guidCmdData.omega_BR_B = [0.010, -0.020, 0.015] guidCmdData.omega_RN_B = [-0.02, -0.01, 0.005] @@ -113,9 +116,7 @@ def mrp_steering_tracking(show_plots,K1, K3, omegaMax): # vehicleConfigData Message: vehicleConfigOut = messaging.VehicleConfigMsgPayload() - I = [1000., 0., 0., - 0., 800., 0., - 0., 0., 800.] + I = [1000.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 800.0] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) @@ -129,10 +130,18 @@ def mrp_steering_tracking(show_plots,K1, K3, omegaMax): def writeMsgInWheelConfiguration(): rwConfigParams = messaging.RWArrayConfigMsgPayload() rwConfigParams.GsMatrix_B = [ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - 0.5773502691896258, 0.5773502691896258, 0.5773502691896258 + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.5773502691896258, + 0.5773502691896258, + 0.5773502691896258, ] rwConfigParams.JsList = [0.1, 0.1, 0.1, 0.1] rwConfigParams.numRW = 4 @@ -146,7 +155,12 @@ def writeMsgInWheelConfiguration(): # wheelAvailability message rwAvailList = [] rwAvailabilityMessage = messaging.RWAvailabilityMsgPayload() - rwAvail = [messaging.AVAILABLE, messaging.AVAILABLE, messaging.AVAILABLE, messaging.AVAILABLE] + rwAvail = [ + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + ] rwAvailabilityMessage.wheelAvailability = rwAvail rwAvailInMsg = messaging.RWAvailabilityMsg().write(rwAvailabilityMessage) rwAvailList.append(rwAvail) @@ -178,18 +192,26 @@ def writeMsgInWheelConfiguration(): unitTestSim.ExecuteSimulation() # Compute true values - trueVals = findTrueTorques(module, servo, guidCmdData, rwSpeedMessage, vehicleConfigOut, rwAvailList) + trueVals = findTrueTorques( + module, servo, guidCmdData, rwSpeedMessage, vehicleConfigOut, rwAvailList + ) # set the filtered output truth states # compare the module results to the truth values accuracy = 1e-12 for i in range(0, len(trueVals)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.torqueRequestBody[i], trueVals[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.torqueRequestBody[i], trueVals[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed torqueRequestBody unit test at t=" - + str(dataLog.times[i] * macros.NANO2SEC) + "sec \n") - + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed torqueRequestBody unit test at t=" + + str(dataLog.times[i] * macros.NANO2SEC) + + "sec \n" + ) # If the argument provided at commandline "--show_plots" evaluates as true, # plot all figures @@ -202,51 +224,67 @@ def writeMsgInWheelConfiguration(): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def findTrueValues(guidCmdData, module): - omegaMax = module.omega_max sigma = np.asarray(guidCmdData.sigma_BR) K1 = np.asarray(module.K1) K3 = np.asarray(module.K3) Bmat = RigidBodyKinematics.BmatMRP(sigma) - omegaAst = []#np.asarray([0, 0, 0]) + omegaAst = [] # np.asarray([0, 0, 0]) omegaAst_P = [] for i in range(len(sigma)): - steerRate = -1*(2*omegaMax/np.pi)*np.arctan((K1*sigma[i]+K3*sigma[i]*sigma[i]*sigma[i])*np.pi/(2*omegaMax)) + steerRate = ( + -1 + * (2 * omegaMax / np.pi) + * np.arctan( + (K1 * sigma[i] + K3 * sigma[i] * sigma[i] * sigma[i]) + * np.pi + / (2 * omegaMax) + ) + ) omegaAst.append(steerRate) - #print omegaAst + # print omegaAst - - if 1:#module.ignoreOuterLoopFeedforward: #should be "if not" - sigmaP = 0.25*Bmat.dot(omegaAst) + if 1: # module.ignoreOuterLoopFeedforward: #should be "if not" + sigmaP = 0.25 * Bmat.dot(omegaAst) for i in range(len(sigma)): - omegaAstRate = (K1+3*K3*sigma[i]**2)/(1+((K1*sigma[i]+K3*sigma[i]**3)**2)*(np.pi/(2*omegaMax))**2)*sigmaP[i] + omegaAstRate = ( + (K1 + 3 * K3 * sigma[i] ** 2) + / ( + 1 + + ((K1 * sigma[i] + K3 * sigma[i] ** 3) ** 2) + * (np.pi / (2 * omegaMax)) ** 2 + ) + * sigmaP[i] + ) omegaAst_P.append(-omegaAstRate) else: omegaAst_P = np.asarray([0, 0, 0]) return omegaAst, omegaAst_P -def findTrueTorques(module,servo, guidCmdData,rwSpeedMessage,vehicleConfigOut, rwAvailMsg): + +def findTrueTorques( + module, servo, guidCmdData, rwSpeedMessage, vehicleConfigOut, rwAvailMsg +): Lr = [] - #Read in variables + # Read in variables numRW = servo.rwConfigParams.numRW L = np.asarray(servo.knownTorquePntB_B) - steps = [0, 0, .5, 0, .5] + steps = [0, 0, 0.5, 0, 0.5] omega_BR_B = np.asarray(guidCmdData.omega_BR_B) omega_RN_B = np.asarray(guidCmdData.omega_RN_B) - omega_BN_B = omega_BR_B + omega_RN_B #find body rate + omega_BN_B = omega_BR_B + omega_RN_B # find body rate domega_RN_B = np.asarray(guidCmdData.domega_RN_B) - omega_BastR_B, omegap_BastR_B = findTrueValues(guidCmdData, module) - omega_BastN_B = omega_BastR_B+omega_RN_B + omega_BastN_B = omega_BastR_B + omega_RN_B omega_BBast_B = omega_BN_B - omega_BastN_B Isc = np.asarray(vehicleConfigOut.ISCPntB_B) @@ -254,47 +292,59 @@ def findTrueTorques(module,servo, guidCmdData,rwSpeedMessage,vehicleConfigOut, r Ki = servo.Ki P = servo.P jsVec = servo.rwConfigParams.JsList[0:numRW] - #GsMatrix_B_array = np.asarray(GsMatrix) - GsMatrix = (servo.rwConfigParams.GsMatrix_B) - GsMatrix_B_array = np.reshape(GsMatrix[0:numRW * 3], (numRW, 3)) + # GsMatrix_B_array = np.asarray(GsMatrix) + GsMatrix = servo.rwConfigParams.GsMatrix_B + GsMatrix_B_array = np.reshape(GsMatrix[0 : numRW * 3], (numRW, 3)) - #Compute toruqes + # Compute toruqes for i in range(len(steps)): dt = steps[i] if dt == 0: zVec = np.asarray([0, 0, 0]) - #evaluate integral term - if Ki > 0 and abs(servo.integralLimit) > 0: #if integral feedback is on + # evaluate integral term + if Ki > 0 and abs(servo.integralLimit) > 0: # if integral feedback is on zVec = dt * omega_BBast_B + zVec # z = integral(del_omega) # Make sure each component is less than the integral limit for i in range(3): if zVec[i] > servo.integralLimit: - zVec[i] = zVec[i]/abs(zVec[i])*servo.integralLimit + zVec[i] = zVec[i] / abs(zVec[i]) * servo.integralLimit - else: #integral gain turned off/negative setting + else: # integral gain turned off/negative setting zVec = np.asarray([0, 0, 0]) - #compute torque Lr + # compute torque Lr Lr0 = Ki * zVec # +K*sigmaBR Lr1 = Lr0 + P * omega_BBast_B # +P*deltaOmega - GsHs = np.array([0,0,0]) + GsHs = np.array([0, 0, 0]) if numRW > 0: for i in range(numRW): if rwAvailMsg[0][i] == 0: # Make RW availability check - GsHs = GsHs + np.dot(GsMatrix_B_array[i, :], jsVec[i]*(np.dot(omega_BN_B, GsMatrix_B_array[i, :]) + rwSpeedMessage.wheelSpeeds[i])) + GsHs = GsHs + np.dot( + GsMatrix_B_array[i, :], + jsVec[i] + * ( + np.dot(omega_BN_B, GsMatrix_B_array[i, :]) + + rwSpeedMessage.wheelSpeeds[i] + ), + ) # J_s*(dot(omegaBN_B,Gs_vec)+Omega_wheel) - Lr2 = Lr1 - np.cross(omega_BastN_B, (Isc.dot(omega_BN_B)+GsHs)) # - omega_BastN x ([I]omega + [Gs]h_s) + Lr2 = Lr1 - np.cross( + omega_BastN_B, (Isc.dot(omega_BN_B) + GsHs) + ) # - omega_BastN x ([I]omega + [Gs]h_s) - Lr3 = Lr2 - Isc.dot(omegap_BastR_B + domega_RN_B - np.cross(omega_BN_B, omega_RN_B)) + Lr3 = Lr2 - Isc.dot( + omegap_BastR_B + domega_RN_B - np.cross(omega_BN_B, omega_RN_B) + ) # - [I](d(omega_B^ast/R)/dt + d(omega_r)/dt - omega x omega_r) Lr4 = Lr3 + L Lr4 = -Lr4 Lr.append(np.ndarray.tolist(Lr4)) return Lr + if __name__ == "__main__": test_mrp_steering_tracking(False, 0.15, 1.0, 0.025) diff --git a/src/fswAlgorithms/attControl/mrpSteering/_UnitTest/test_MRP_steeringUnit.py b/src/fswAlgorithms/attControl/mrpSteering/_UnitTest/test_MRP_steeringUnit.py index 9b877cff39..b20acbce7a 100644 --- a/src/fswAlgorithms/attControl/mrpSteering/_UnitTest/test_MRP_steeringUnit.py +++ b/src/fswAlgorithms/attControl/mrpSteering/_UnitTest/test_MRP_steeringUnit.py @@ -24,14 +24,15 @@ from Basilisk.utilities import RigidBodyKinematics from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions @pytest.mark.parametrize("K1", [0.15, 0]) @pytest.mark.parametrize("K3", [1, 0]) @pytest.mark.parametrize("omegaMax", [1.5 * macros.D2R, 0.001 * macros.D2R]) - # uncomment this line is 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 @@ -84,7 +85,6 @@ def mrp_steering_tracking(show_plots, K1, K3, omegaMax): module = mrpSteering.mrpSteering() module.ModelTag = "mrpSteering" - # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) @@ -95,7 +95,9 @@ def mrp_steering_tracking(show_plots, K1, K3, omegaMax): # Create input message and size it because the regular creator of that message # is not part of the test. - guidCmdData = messaging.AttGuidMsgPayload() # Create a structure for the input message + guidCmdData = ( + messaging.AttGuidMsgPayload() + ) # Create a structure for the input message sigma_BR = np.array([0.3, -0.5, 0.7]) guidCmdData.sigma_BR = sigma_BR omega_BR_B = np.array([0.010, -0.020, 0.015]) @@ -127,19 +129,33 @@ def mrp_steering_tracking(show_plots, K1, K3, omegaMax): accuracy = 1e-12 for i in range(0, len(omegaAstTrue)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.omega_BastR_B[i], omegaAstTrue[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.omega_BastR_B[i], omegaAstTrue[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_BastR_B unit test at t=" - + str(dataLog.times()[i] * macros.NANO2SEC) + "sec \n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_BastR_B unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec \n" + ) # compare the module results to the truth values accuracy = 1e-12 for i in range(0, len(omegaAstPTrue)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.omegap_BastR_B[i], omegaAstPTrue[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.omegap_BastR_B[i], omegaAstPTrue[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omegap_BastR_B unit test at t=" - + str(dataLog.times()[i] * macros.NANO2SEC) + "sec \n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omegap_BastR_B unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec \n" + ) # If the argument provided at commandline "--show_plots" evaluates as true, # plot all figures @@ -152,28 +168,42 @@ def mrp_steering_tracking(show_plots, K1, K3, omegaMax): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def findTrueValues(guidCmdData, module): - omegaMax = module.omega_max sigma = np.asarray(guidCmdData.sigma_BR) K1 = np.asarray(module.K1) K3 = np.asarray(module.K3) Bmat = RigidBodyKinematics.BmatMRP(sigma) - omegaAst = [] #np.asarray([0, 0, 0]) + omegaAst = [] # np.asarray([0, 0, 0]) omegaAst_P = [] for i in range(len(sigma)): - steerRate = -1*(2*omegaMax/np.pi)*np.arctan((K1*sigma[i]+K3*sigma[i]*sigma[i]*sigma[i])*np.pi/(2*omegaMax)) + steerRate = ( + -1 + * (2 * omegaMax / np.pi) + * np.arctan( + (K1 * sigma[i] + K3 * sigma[i] * sigma[i] * sigma[i]) + * np.pi + / (2 * omegaMax) + ) + ) omegaAst.append(steerRate) - - if 1: #module.ignoreOuterLoopFeedforward: #should be "if not" - sigmaP = 0.25*Bmat.dot(omegaAst) + if 1: # module.ignoreOuterLoopFeedforward: #should be "if not" + sigmaP = 0.25 * Bmat.dot(omegaAst) for i in range(len(sigma)): - omegaAstRate = (K1+3*K3*sigma[i]**2)/(1+((K1*sigma[i]+K3*sigma[i]**3)**2)*(np.pi/(2*omegaMax))**2)*sigmaP[i] + omegaAstRate = ( + (K1 + 3 * K3 * sigma[i] ** 2) + / ( + 1 + + ((K1 * sigma[i] + K3 * sigma[i] ** 3) ** 2) + * (np.pi / (2 * omegaMax)) ** 2 + ) + * sigmaP[i] + ) omegaAst_P.append(-omegaAstRate) else: omegaAst_P = np.asarray([0, 0, 0]) diff --git a/src/fswAlgorithms/attControl/mtbFeedforward/_UnitTest/test_mtbFeedforward.py b/src/fswAlgorithms/attControl/mtbFeedforward/_UnitTest/test_mtbFeedforward.py index d7e002318c..be33c4b73a 100644 --- a/src/fswAlgorithms/attControl/mtbFeedforward/_UnitTest/test_mtbFeedforward.py +++ b/src/fswAlgorithms/attControl/mtbFeedforward/_UnitTest/test_mtbFeedforward.py @@ -1,4 +1,3 @@ - # # ISC License # @@ -28,13 +27,18 @@ import numpy as np from Basilisk.architecture import bskLogging from Basilisk.architecture import messaging # import the message definitions -from Basilisk.fswAlgorithms import mtbFeedforward # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + mtbFeedforward, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions -accuracy = 1E-12 +accuracy = 1e-12 # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed # @pytest.mark.skipif(conditionstring) @@ -42,7 +46,8 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ -def test_mtbFeedforward_module(): # update "module" in this function name to reflect the module name + +def test_mtbFeedforward_module(): # update "module" in this function name to reflect the module name r""" **Validation Test Description** @@ -61,47 +66,60 @@ def test_mtbFeedforward_module(): # update "module" in this function name to [testResults, testMessage] = mtbFeedforwardModuleTestFunction() assert testResults < 1, testMessage + def mtbFeedforwardModuleTestFunction(): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.01) # update process rate update time + testProcessRate = macros.sec2nano(0.01) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Initialize module under test's config message and add module to runtime call list module = mtbFeedforward.mtbFeedforward() - module.ModelTag = "mrpFeedback" # update python name of test module + module.ModelTag = "mrpFeedback" # update python name of test module unitTestSim.AddModelToTask(unitTaskName, module) # Initialize CmdTorqueBodyMsg vehControlInMsgContainer = messaging.CmdTorqueBodyMsgPayload() - vehControlInMsgContainer.torqueRequestBody = [0., 0., 0.] + vehControlInMsgContainer.torqueRequestBody = [0.0, 0.0, 0.0] vehControlInMsg = messaging.CmdTorqueBodyMsg().write(vehControlInMsgContainer) # Initialize DipoleRequestBodyMsg dipoleRequestMtbInMsgContainer = messaging.MTBCmdMsgPayload() - dipoleRequestMtbInMsgContainer.mtbDipoleCmds = [1., 2., 3.] + dipoleRequestMtbInMsgContainer.mtbDipoleCmds = [1.0, 2.0, 3.0] dipoleRequestMtbInMsg = messaging.MTBCmdMsg().write(dipoleRequestMtbInMsgContainer) # Initialize TAMSensorBodyMsg tamSensorBodyInMsgContainer = messaging.TAMSensorBodyMsgPayload() - tamSensorBodyInMsgContainer.tam_B = [ 1E-5, -3E-5, 5E-5] + tamSensorBodyInMsgContainer.tam_B = [1e-5, -3e-5, 5e-5] tamSensorBodyInMsg = messaging.TAMSensorBodyMsg().write(tamSensorBodyInMsgContainer) # Initialize MTBArrayConfigMsg mtbArrayConfigParamsInMsgContainer = messaging.MTBArrayConfigMsgPayload() mtbArrayConfigParamsInMsgContainer.numMTB = 3 - mtbArrayConfigParamsInMsgContainer.maxMtbDipoles = [1E3, 1E3, 1E3] - mtbArrayConfigParamsInMsgContainer.GtMatrix_B = [1., 0., 0., 0., 1., 0., 0., 0., 1.] - mtbArrayConfigParamsInMsg = messaging.MTBArrayConfigMsg().write(mtbArrayConfigParamsInMsgContainer) + mtbArrayConfigParamsInMsgContainer.maxMtbDipoles = [1e3, 1e3, 1e3] + mtbArrayConfigParamsInMsgContainer.GtMatrix_B = [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + ] + mtbArrayConfigParamsInMsg = messaging.MTBArrayConfigMsg().write( + mtbArrayConfigParamsInMsgContainer + ) # Setup logging on the test module output message so that we get all the writes to it resultVehControlOutMsg = module.vehControlOutMsg.recorder() @@ -114,77 +132,95 @@ def mtbFeedforwardModuleTestFunction(): module.mtbArrayConfigParamsInMsg.subscribeTo(mtbArrayConfigParamsInMsg) # Set the simulation time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) # seconds to stop simulation unitTestSim.InitializeSimulation() - ''' + """ TEST 1: Check that dipoles are non-zero expected value. - ''' + """ unitTestSim.ExecuteSimulation() m = np.array(dipoleRequestMtbInMsgContainer.mtbDipoleCmds[0:3]) b = np.array(tamSensorBodyInMsgContainer.tam_B) expectedTorque = -np.cross(m, b) - testFailCount, testMessages = unitTestSupport.compareVector(expectedTorque, - resultVehControlOutMsg.torqueRequestBody[0], - accuracy, - "torqueRequestBody", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareVector( + expectedTorque, + resultVehControlOutMsg.torqueRequestBody[0], + accuracy, + "torqueRequestBody", + testFailCount, + testMessages, + ) - ''' + """ TEST 2: Check that torqueRequestBody is zero when b field is zero. - ''' - tamSensorBodyInMsgContainer.tam_B = [0., 0., 0.] + """ + tamSensorBodyInMsgContainer.tam_B = [0.0, 0.0, 0.0] tamSensorBodyInMsg = messaging.TAMSensorBodyMsg().write(tamSensorBodyInMsgContainer) module.tamSensorBodyInMsg.subscribeTo(tamSensorBodyInMsg) unitTestSim.InitializeSimulation() unitTestSim.ExecuteSimulation() - expectedTorque = [0., 0., 0.] - testFailCount, testMessages = unitTestSupport.compareVector(expectedTorque, - resultVehControlOutMsg.torqueRequestBody[0], - accuracy, - "torqueRequestBody", - testFailCount, testMessages) - - ''' + expectedTorque = [0.0, 0.0, 0.0] + testFailCount, testMessages = unitTestSupport.compareVector( + expectedTorque, + resultVehControlOutMsg.torqueRequestBody[0], + accuracy, + "torqueRequestBody", + testFailCount, + testMessages, + ) + + """ TEST 3: Check that torqueRequestBody is zero when dipoles are zero. - ''' - tamSensorBodyInMsgContainer.tam_B = [1E-5, -3E-5, 5E-5] + """ + tamSensorBodyInMsgContainer.tam_B = [1e-5, -3e-5, 5e-5] tamSensorBodyInMsg = messaging.TAMSensorBodyMsg().write(tamSensorBodyInMsgContainer) module.tamSensorBodyInMsg.subscribeTo(tamSensorBodyInMsg) - dipoleRequestMtbInMsgContainer.mtbDipoleCmds = [0., 0., 0.] + dipoleRequestMtbInMsgContainer.mtbDipoleCmds = [0.0, 0.0, 0.0] dipoleRequestMtbInMsg = messaging.MTBCmdMsg().write(dipoleRequestMtbInMsgContainer) module.dipoleRequestMtbInMsg.subscribeTo(dipoleRequestMtbInMsg) unitTestSim.InitializeSimulation() unitTestSim.ExecuteSimulation() - expectedTorque = [0., 0., 0.] - testFailCount, testMessages = unitTestSupport.compareVector(expectedTorque, - resultVehControlOutMsg.torqueRequestBody[0], - accuracy, - "torqueRequestBody", - testFailCount, testMessages) - - ''' + expectedTorque = [0.0, 0.0, 0.0] + testFailCount, testMessages = unitTestSupport.compareVector( + expectedTorque, + resultVehControlOutMsg.torqueRequestBody[0], + accuracy, + "torqueRequestBody", + testFailCount, + testMessages, + ) + + """ TEST 4: Check that torqueRequestBody is non-zero expected value with non-trivial Gt matrix. - ''' - dipoleRequestMtbInMsgContainer.mtbDipoleCmds = [7., -3.] + """ + dipoleRequestMtbInMsgContainer.mtbDipoleCmds = [7.0, -3.0] dipoleRequestMtbInMsg = messaging.MTBCmdMsg().write(dipoleRequestMtbInMsgContainer) module.dipoleRequestMtbInMsg.subscribeTo(dipoleRequestMtbInMsg) - beta = 45. * np.pi / 180. - Gt = np.array([[np.cos(beta), -np.sin(beta)],[np.sin(beta), np.cos(beta)], [0., 0.]]) + beta = 45.0 * np.pi / 180.0 + Gt = np.array( + [[np.cos(beta), -np.sin(beta)], [np.sin(beta), np.cos(beta)], [0.0, 0.0]] + ) mtbArrayConfigParamsInMsgContainer.numMTB = 2 - mtbArrayConfigParamsInMsgContainer.GtMatrix_B = [Gt[0, 0], Gt[0, 1], - Gt[1, 0], Gt[1, 1], - Gt[2, 0], Gt[2, 1]] - mtbArrayConfigParamsInMsg = messaging.MTBArrayConfigMsg().write(mtbArrayConfigParamsInMsgContainer) + mtbArrayConfigParamsInMsgContainer.GtMatrix_B = [ + Gt[0, 0], + Gt[0, 1], + Gt[1, 0], + Gt[1, 1], + Gt[2, 0], + Gt[2, 1], + ] + mtbArrayConfigParamsInMsg = messaging.MTBArrayConfigMsg().write( + mtbArrayConfigParamsInMsgContainer + ) module.mtbArrayConfigParamsInMsg.subscribeTo(mtbArrayConfigParamsInMsg) unitTestSim.InitializeSimulation() @@ -192,18 +228,21 @@ def mtbFeedforwardModuleTestFunction(): m = Gt @ np.array(dipoleRequestMtbInMsgContainer.mtbDipoleCmds[0:2]) b = np.array(tamSensorBodyInMsgContainer.tam_B) expectedTorque = -np.cross(m, b) - testFailCount, testMessages = unitTestSupport.compareVector(expectedTorque, - resultVehControlOutMsg.torqueRequestBody[0], - accuracy, - "torqueRequestBody", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareVector( + expectedTorque, + resultVehControlOutMsg.torqueRequestBody[0], + accuracy, + "torqueRequestBody", + testFailCount, + testMessages, + ) print("Accuracy used: " + str(accuracy)) if testFailCount == 0: print("PASSED: mtbFeedforward unit test") else: print("Failed: mtbFeedforward unit test") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attControl/mtbMomentumManagement/_UnitTest/test_mtbMomentumManagement.py b/src/fswAlgorithms/attControl/mtbMomentumManagement/_UnitTest/test_mtbMomentumManagement.py index f237d29b59..34d561b110 100644 --- a/src/fswAlgorithms/attControl/mtbMomentumManagement/_UnitTest/test_mtbMomentumManagement.py +++ b/src/fswAlgorithms/attControl/mtbMomentumManagement/_UnitTest/test_mtbMomentumManagement.py @@ -27,11 +27,16 @@ import numpy as np from Basilisk.architecture import bskLogging from Basilisk.architecture import messaging # import the message definitions -from Basilisk.fswAlgorithms import mtbMomentumManagement # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + mtbMomentumManagement, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # CONSTANTS MAX_EFF_CNT = 36 @@ -42,7 +47,8 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ -def test_mtbMomentumManagement(): # update "module" in this function name to reflect the module name + +def test_mtbMomentumManagement(): # update "module" in this function name to reflect the module name r""" **Validation Test Description** @@ -62,32 +68,46 @@ def test_mtbMomentumManagement(): # update "module" in this function name to [testResults, testMessage] = mtbMomentumManagementModuleTestFunction() assert testResults < 1, testMessage + def mtbMomentumManagementModuleTestFunction(): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.01) # update process rate update time + testProcessRate = macros.sec2nano(0.01) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container module = mtbMomentumManagement.mtbMomentumManagement() module.cGain = 0.005 - module.wheelSpeedBiases = [0., 0., 0, 0.] - module.ModelTag = "mtbMomentumManagement" # update python name of test module + module.wheelSpeedBiases = [0.0, 0.0, 0, 0.0] + module.ModelTag = "mtbMomentumManagement" # update python name of test module unitTestSim.AddModelToTask(unitTaskName, module) # wheelConfigData message (array is ordered c11, c22, c33, c44, ...) rwConfigParams = messaging.RWArrayConfigMsgPayload() - beta = 45. * np.pi / 180. - rwConfigParams.GsMatrix_B = [0., np.cos(beta), np.sin(beta), 0., np.sin(beta), -np.cos(beta), np.cos(beta), -np.sin(beta), 0., -np.cos(beta), -np.sin(beta), 0.] + beta = 45.0 * np.pi / 180.0 + rwConfigParams.GsMatrix_B = [ + 0.0, + np.cos(beta), + np.sin(beta), + 0.0, + np.sin(beta), + -np.cos(beta), + np.cos(beta), + -np.sin(beta), + 0.0, + -np.cos(beta), + -np.sin(beta), + 0.0, + ] rwConfigParams.JsList = [0.002, 0.002, 0.002, 0.002] rwConfigParams.numRW = 4 rwParamsInMsg = messaging.RWArrayConfigMsg().write(rwConfigParams) @@ -96,28 +116,30 @@ def mtbMomentumManagementModuleTestFunction(): mtbConfigParams = messaging.MTBArrayConfigMsgPayload() mtbConfigParams.numMTB = 3 # row major toque bar alignments - mtbConfigParams.GtMatrix_B = [ - 1., 0., 0., - 0., 1., 0., - 0., 0., 1. - ] - mtbConfigParams.maxMtbDipoles = [10.]*mtbConfigParams.numMTB + mtbConfigParams.GtMatrix_B = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + mtbConfigParams.maxMtbDipoles = [10.0] * mtbConfigParams.numMTB mtbParamsInMsg = messaging.MTBArrayConfigMsg().write(mtbConfigParams) # TAMSensorBodyMsg message (leads to non-invertible matrix) tamSensorBodyInMsgContainer = messaging.TAMSensorBodyMsgPayload() - tamSensorBodyInMsgContainer.tam_B = [ 5E3 * 0.03782347, 5E3 * 0.49170516, 5E3 * -0.8699399] + tamSensorBodyInMsgContainer.tam_B = [ + 5e3 * 0.03782347, + 5e3 * 0.49170516, + 5e3 * -0.8699399, + ] tamSensorBodyInMsg = messaging.TAMSensorBodyMsg().write(tamSensorBodyInMsgContainer) # rwSpeeds message rwSpeedsInMsgContainer = messaging.RWSpeedMsgPayload() - rwSpeedsInMsgContainer.wheelSpeeds = [100., 200., 300., 400.] + rwSpeedsInMsgContainer.wheelSpeeds = [100.0, 200.0, 300.0, 400.0] rwSpeedsInMsg = messaging.RWSpeedMsg().write(rwSpeedsInMsgContainer) # attControl message rwMotorTorqueInMsgContainer = messaging.ArrayMotorTorqueMsgPayload() - rwMotorTorqueInMsgContainer.motorTorque = [0., 0., 0., 0.] - rwMotorTorqueInMsg = messaging.ArrayMotorTorqueMsg().write(rwMotorTorqueInMsgContainer) + rwMotorTorqueInMsgContainer.motorTorque = [0.0, 0.0, 0.0, 0.0] + rwMotorTorqueInMsg = messaging.ArrayMotorTorqueMsg().write( + rwMotorTorqueInMsgContainer + ) # Setup logging on the test module output message so that we get all the writes to it resultMtbCmdOutMsg = module.mtbCmdOutMsg.recorder() @@ -139,64 +161,77 @@ def mtbMomentumManagementModuleTestFunction(): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) # seconds to stop simulation - accuracy = 1E-8 - + unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) # seconds to stop simulation + accuracy = 1e-8 - ''' + """ TEST 0: Check that mtbDipoleCmds and are non-zero. - ''' + """ unitTestSim.InitializeSimulation() unitTestSim.ExecuteSimulation() - testFailCount, testMessages = unitTestSupport.compareVector([0., 0., 0.], - resultMtbCmdOutMsg.mtbDipoleCmds[0][0:3], - accuracy, - "tauMtbRequestOutMsg", - testFailCount, testMessages, ExpectedResult=0) - - testFailCount, testMessages = unitTestSupport.compareVector([0., 0., 0., 0.], - resultRwMotorTorqueOutMsg.motorTorque[0][0:4], - accuracy, - "rwMotorTorqueOutMsg", - testFailCount, testMessages, ExpectedResult=0) - ''' + testFailCount, testMessages = unitTestSupport.compareVector( + [0.0, 0.0, 0.0], + resultMtbCmdOutMsg.mtbDipoleCmds[0][0:3], + accuracy, + "tauMtbRequestOutMsg", + testFailCount, + testMessages, + ExpectedResult=0, + ) + + testFailCount, testMessages = unitTestSupport.compareVector( + [0.0, 0.0, 0.0, 0.0], + resultRwMotorTorqueOutMsg.motorTorque[0][0:4], + accuracy, + "rwMotorTorqueOutMsg", + testFailCount, + testMessages, + ExpectedResult=0, + ) + """ TEST 1: Check that the mtbDipoleCmds are zero and that the resulting torque on the body is zero when the b field is zero. - ''' - tamSensorBodyInMsgContainer.tam_B = [0., 0., 0.] + """ + tamSensorBodyInMsgContainer.tam_B = [0.0, 0.0, 0.0] tamSensorBodyInMsg = messaging.TAMSensorBodyMsg().write(tamSensorBodyInMsgContainer) module.tamSensorBodyInMsg.subscribeTo(tamSensorBodyInMsg) unitTestSim.InitializeSimulation() unitTestSim.ExecuteSimulation() - testFailCount, testMessages = unitTestSupport.compareVector([0., 0., 0.], - resultMtbCmdOutMsg.mtbDipoleCmds[0][0:3], - accuracy, - "tauMtbRequestOutMsg", - testFailCount, testMessages, ExpectedResult=1) + testFailCount, testMessages = unitTestSupport.compareVector( + [0.0, 0.0, 0.0], + resultMtbCmdOutMsg.mtbDipoleCmds[0][0:3], + accuracy, + "tauMtbRequestOutMsg", + testFailCount, + testMessages, + ExpectedResult=1, + ) Gs = np.array(rwConfigParams.GsMatrix_B[0:12]).reshape(4, 3).T tauBody = Gs @ np.array(resultRwMotorTorqueOutMsg.motorTorque[0][0:4]) - testFailCount, testMessages = unitTestSupport.compareVector([0., 0., 0.], - tauBody, - accuracy, - "rwMotorTorqueOutMsg", - testFailCount, testMessages, ExpectedResult=1) - + testFailCount, testMessages = unitTestSupport.compareVector( + [0.0, 0.0, 0.0], + tauBody, + accuracy, + "rwMotorTorqueOutMsg", + testFailCount, + testMessages, + ExpectedResult=1, + ) # reset the module to test this functionality - module.Reset(0) # this module reset function needs a time input (in NanoSeconds) - + module.Reset(0) # this module reset function needs a time input (in NanoSeconds) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found print("fail count", testFailCount) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attControl/mtbMomentumManagementSimple/_UnitTest/test_mtbMomentumManagementSimple.py b/src/fswAlgorithms/attControl/mtbMomentumManagementSimple/_UnitTest/test_mtbMomentumManagementSimple.py index a622c81739..c65a86bc60 100644 --- a/src/fswAlgorithms/attControl/mtbMomentumManagementSimple/_UnitTest/test_mtbMomentumManagementSimple.py +++ b/src/fswAlgorithms/attControl/mtbMomentumManagementSimple/_UnitTest/test_mtbMomentumManagementSimple.py @@ -27,11 +27,16 @@ import numpy as np from Basilisk.architecture import bskLogging from Basilisk.architecture import messaging # import the message definitions -from Basilisk.fswAlgorithms import mtbMomentumManagementSimple # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + mtbMomentumManagementSimple, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -40,7 +45,8 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ -def test_mtbMomentumManagementSimple(): # update "module" in this function name to reflect the module name + +def test_mtbMomentumManagementSimple(): # update "module" in this function name to reflect the module name r""" **Validation Test Description** @@ -58,38 +64,52 @@ def test_mtbMomentumManagementSimple(): # update "module" in this function n [testResults, testMessage] = mtbMomentumManagementSimpleTestFunction() assert testResults < 1, testMessage + def mtbMomentumManagementSimpleTestFunction(): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.01) # update process rate update time + testProcessRate = macros.sec2nano(0.01) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container module = mtbMomentumManagementSimple.mtbMomentumManagementSimple() module.Kp = 0.003 - module.ModelTag = "mtbMomentumManagementSimple" # update python name of test module + module.ModelTag = "mtbMomentumManagementSimple" # update python name of test module unitTestSim.AddModelToTask(unitTaskName, module) # wheelConfigData message (column major format) rwConfigParams = messaging.RWArrayConfigMsgPayload() - beta = 45. * np.pi / 180. - rwConfigParams.GsMatrix_B = [0., np.cos(beta), np.sin(beta), 0., np.sin(beta), -np.cos(beta), np.cos(beta), -np.sin(beta), 0., -np.cos(beta), -np.sin(beta), 0.] + beta = 45.0 * np.pi / 180.0 + rwConfigParams.GsMatrix_B = [ + 0.0, + np.cos(beta), + np.sin(beta), + 0.0, + np.sin(beta), + -np.cos(beta), + np.cos(beta), + -np.sin(beta), + 0.0, + -np.cos(beta), + -np.sin(beta), + 0.0, + ] rwConfigParams.JsList = [0.002, 0.002, 0.002, 0.002] rwConfigParams.numRW = 4 rwParamsInMsg = messaging.RWArrayConfigMsg().write(rwConfigParams) # rwSpeeds message rwSpeedsInMsgContainer = messaging.RWSpeedMsgPayload() - rwSpeedsInMsgContainer.wheelSpeeds = [100., 200., 300., 400.] + rwSpeedsInMsgContainer.wheelSpeeds = [100.0, 200.0, 300.0, 400.0] rwSpeedsInMsg = messaging.RWSpeedMsg().write(rwSpeedsInMsgContainer) # Setup logging on the test module output message so that we get all the writes to it @@ -107,74 +127,86 @@ def mtbMomentumManagementSimpleTestFunction(): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) # seconds to stop simulation - accuracy = 1E-8 + unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) # seconds to stop simulation + accuracy = 1e-8 - ''' + """ TEST 1: Check that tauMtbRequestOutMsg is non-zero when the wheel speeds are non-zero. - ''' + """ unitTestSim.InitializeSimulation() unitTestSim.ExecuteSimulation() - Gs = np.array([ - [0., 0., np.cos(beta), -np.cos(beta)], - [np.cos(beta), np.sin(beta), -np.sin(beta), -np.sin(beta)], - [np.sin(beta), -np.cos(beta), 0., 0.]]) + Gs = np.array( + [ + [0.0, 0.0, np.cos(beta), -np.cos(beta)], + [np.cos(beta), np.sin(beta), -np.sin(beta), -np.sin(beta)], + [np.sin(beta), -np.cos(beta), 0.0, 0.0], + ] + ) wheelSpeeds = np.array(rwSpeedsInMsgContainer.wheelSpeeds[0:4]) hWheels_W = wheelSpeeds * rwConfigParams.JsList[0] - hWheels_B = Gs @ hWheels_W - tauExpected = - module.Kp * hWheels_B - - testFailCount, testMessages = unitTestSupport.compareVector(tauExpected, - resultTauMtbRequestOutMsg.torqueRequestBody[0][0:3], - accuracy, - "tauMtbRequestOutMsg", - testFailCount, testMessages, ExpectedResult=1) + hWheels_B = Gs @ hWheels_W + tauExpected = -module.Kp * hWheels_B + + testFailCount, testMessages = unitTestSupport.compareVector( + tauExpected, + resultTauMtbRequestOutMsg.torqueRequestBody[0][0:3], + accuracy, + "tauMtbRequestOutMsg", + testFailCount, + testMessages, + ExpectedResult=1, + ) - ''' + """ TEST 2: Check that tauMtbRequestOutMsg is the zero vector when the wheels speeds are zero. - ''' - rwSpeedsInMsgContainer.wheelSpeeds = [0., 0., 0., 0.] + """ + rwSpeedsInMsgContainer.wheelSpeeds = [0.0, 0.0, 0.0, 0.0] rwSpeedsInMsg = messaging.RWSpeedMsg().write(rwSpeedsInMsgContainer) module.rwSpeedsInMsg.subscribeTo(rwSpeedsInMsg) unitTestSim.InitializeSimulation() unitTestSim.ExecuteSimulation() - testFailCount, testMessages = unitTestSupport.compareVector([0., 0., 0.], - resultTauMtbRequestOutMsg.torqueRequestBody[0][0:3], - accuracy, - "tauMtbRequestOutMsg", - testFailCount, testMessages, ExpectedResult=1) + testFailCount, testMessages = unitTestSupport.compareVector( + [0.0, 0.0, 0.0], + resultTauMtbRequestOutMsg.torqueRequestBody[0][0:3], + accuracy, + "tauMtbRequestOutMsg", + testFailCount, + testMessages, + ExpectedResult=1, + ) - ''' + """ TEST 3: Check that tauMtbRequestOutMsg is the zero vector when the wheels speeds are non-zero and the feedback gain is zero. - ''' - rwSpeedsInMsgContainer.wheelSpeeds = [100., 200., 300., 400.] + """ + rwSpeedsInMsgContainer.wheelSpeeds = [100.0, 200.0, 300.0, 400.0] rwSpeedsInMsg = messaging.RWSpeedMsg().write(rwSpeedsInMsgContainer) module.rwSpeedsInMsg.subscribeTo(rwSpeedsInMsg) - module.Kp = 0. + module.Kp = 0.0 unitTestSim.InitializeSimulation() unitTestSim.ExecuteSimulation() - testFailCount, testMessages = unitTestSupport.compareVector([0., 0., 0.], - resultTauMtbRequestOutMsg.torqueRequestBody[0][0:3], - accuracy, - "tauMtbRequestOutMsg", - testFailCount, testMessages, ExpectedResult=1) - + testFailCount, testMessages = unitTestSupport.compareVector( + [0.0, 0.0, 0.0], + resultTauMtbRequestOutMsg.torqueRequestBody[0][0:3], + accuracy, + "tauMtbRequestOutMsg", + testFailCount, + testMessages, + ExpectedResult=1, + ) # reset the module to test this functionality - module.Reset(0) # this module reset function needs a time input (in NanoSeconds) - - + module.Reset(0) # this module reset function needs a time input (in NanoSeconds) print("Accuracy used: " + str(accuracy)) if testFailCount == 0: @@ -182,7 +214,7 @@ def mtbMomentumManagementSimpleTestFunction(): else: print("Failed: mtbMomentumManagementSimple unit test") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attControl/prvSteering/_UnitTest/test_PRV_Steering.py b/src/fswAlgorithms/attControl/prvSteering/_UnitTest/test_PRV_Steering.py index 90629a60f1..2eef2b3163 100755 --- a/src/fswAlgorithms/attControl/prvSteering/_UnitTest/test_PRV_Steering.py +++ b/src/fswAlgorithms/attControl/prvSteering/_UnitTest/test_PRV_Steering.py @@ -22,12 +22,14 @@ # Creation Date: December 18, 2015 # import matplotlib.pyplot as plt + # import packages as needed e.g. 'numpy', 'ctypes, 'math' etc. import numpy as np import pytest from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import prvSteering from Basilisk.fswAlgorithms import rateServoFullNonlinear + # Import all of the modules that we are going to call in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros @@ -40,7 +42,9 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ @pytest.mark.parametrize("simCase", [0, 1]) -def test_prvSteering(show_plots, simCase): # update "subModule" in this function name to reflect the module name +def test_prvSteering( + show_plots, simCase +): # update "subModule" in this function name to reflect the module name """Module Unit Test""" # each test method requires a single assert method to be called [testResults, testMessage] = subModuleTestFunction(show_plots, simCase) @@ -48,20 +52,19 @@ def test_prvSteering(show_plots, simCase): # update "subModule" in this func def subModuleTestFunction(show_plots, simCase): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = prvSteering.prvSteering() module.ModelTag = "prvSteering" @@ -76,18 +79,19 @@ def subModuleTestFunction(show_plots, simCase): # configure BSK modules module.K1 = 0.15 module.K3 = 1.0 - module.omega_max = 1.5*macros.D2R + module.omega_max = 1.5 * macros.D2R servo.Ki = 0.01 servo.P = 150.0 - servo.integralLimit = 2./servo.Ki * 0.1 - servo.knownTorquePntB_B = [0., 0., 0.] - + servo.integralLimit = 2.0 / servo.Ki * 0.1 + servo.knownTorquePntB_B = [0.0, 0.0, 0.0] # Create input message and size it because the regular creator of that message # is not part of the test. # attGuidOut Message: - guidCmdData = messaging.AttGuidMsgPayload() # Create a structure for the input message + guidCmdData = ( + messaging.AttGuidMsgPayload() + ) # Create a structure for the input message sigma_BR = [] if simCase == 0: @@ -106,9 +110,7 @@ def subModuleTestFunction(show_plots, simCase): # vehicleConfigData Message: vehicleConfigOut = messaging.VehicleConfigMsgPayload() - I = [1000., 0., 0., - 0., 800., 0., - 0., 0., 800.] + I = [1000.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 800.0] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) @@ -122,10 +124,18 @@ def subModuleTestFunction(show_plots, simCase): def writeMsgInWheelConfiguration(): rwConfigParams = messaging.RWArrayConfigMsgPayload() rwConfigParams.GsMatrix_B = [ - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0 + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, ] rwConfigParams.JsList = [0.1, 0.1, 0.1, 0.1] rwConfigParams.numRW = 4 @@ -137,14 +147,18 @@ def writeMsgInWheelConfiguration(): # wheelAvailability message def writeMsgInWheelAvailability(): rwAvailabilityMessage = messaging.RWAvailabilityMsgPayload() - avail = [messaging.AVAILABLE, messaging.AVAILABLE, messaging.AVAILABLE, messaging.AVAILABLE] + avail = [ + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + ] rwAvailabilityMessage.wheelAvailability = avail rwAvailInMsg = messaging.RWAvailabilityMsg().write(rwAvailabilityMessage) return rwAvailInMsg rwAvailInMsg = writeMsgInWheelAvailability() - # Setup logging on the test module output message so that we get all the writes to it dataLog = servo.cmdTorqueOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) @@ -162,62 +176,60 @@ def writeMsgInWheelAvailability(): unitTestSim.InitializeSimulation() # Step the simulation to 3*process rate so 4 total steps including zero - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - servo.Reset(1) # this module reset function needs a time input (in NanoSeconds) + servo.Reset(1) # this module reset function needs a time input (in NanoSeconds) - unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() # set the filtered output truth states trueVector = [] if simCase == 0: trueVector = [ - [-2.9352922876097969, +6.2831737715827778, -4.0554726129822907] - ,[-2.9352922876097969, +6.2831737715827778, -4.0554726129822907] - ,[-2.9353853745179044, +6.2833455830962901, -4.0556481491012084] - ,[-2.9352922876097969, +6.2831737715827778, -4.0554726129822907] - ,[-2.9353853745179044, +6.2833455830962901, -4.0556481491012084] - ] + [-2.9352922876097969, +6.2831737715827778, -4.0554726129822907], + [-2.9352922876097969, +6.2831737715827778, -4.0554726129822907], + [-2.9353853745179044, +6.2833455830962901, -4.0556481491012084], + [-2.9352922876097969, +6.2831737715827778, -4.0554726129822907], + [-2.9353853745179044, +6.2833455830962901, -4.0556481491012084], + ] if simCase == 1: trueVector = [ - [-1.39, 3.79, -1.39] - ,[-1.39, 3.79, -1.39] - ,[-1.39005, 3.7901, -1.390075] - ,[-1.39, 3.79, -1.39] - ,[-1.39005, 3.7901, -1.390075] - ] + [-1.39, 3.79, -1.39], + [-1.39, 3.79, -1.39], + [-1.39005, 3.7901, -1.390075], + [-1.39, 3.79, -1.39], + [-1.39005, 3.7901, -1.390075], + ] # compare the module results to the truth values accuracy = 1e-12 - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.torqueRequestBody[i], trueVector[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.torqueRequestBody[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed torqueRequestBody unit test at t=" - + str(dataLog.times()[i]*macros.NANO2SEC) + "sec\n") - - - - - - + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed torqueRequestBody unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) # If the argument provided at commandline "--show_plots" evaluates as true, # plot all figures if show_plots: - plt.show() + plt.show() if testFailCount == 0: print("PASSED: " + module.ModelTag) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] - - - + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attControl/rateServoFullNonlinear/_UnitTest/test_rateServoFullNonlinear.py b/src/fswAlgorithms/attControl/rateServoFullNonlinear/_UnitTest/test_rateServoFullNonlinear.py index 78654404c9..c1b6b4df71 100644 --- a/src/fswAlgorithms/attControl/rateServoFullNonlinear/_UnitTest/test_rateServoFullNonlinear.py +++ b/src/fswAlgorithms/attControl/rateServoFullNonlinear/_UnitTest/test_rateServoFullNonlinear.py @@ -20,10 +20,14 @@ import numpy as np import pytest from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import rateServoFullNonlinear # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + rateServoFullNonlinear, +) # import the module that is to be tested from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -35,28 +39,51 @@ @pytest.mark.parametrize("rwNum", [4, 0]) @pytest.mark.parametrize("intGain", [0.01, -1]) -@pytest.mark.parametrize("omegap_BastR_B", [(1.87766650e-04, -3.91233583e-05, 3.56369489e-05), (0, 0, 0)]) -@pytest.mark.parametrize("omega_BastR_B", [(-2.23886891e-02, 2.47942516e-02, -2.55601849e-02), (0, 0, 0)]) +@pytest.mark.parametrize( + "omegap_BastR_B", [(1.87766650e-04, -3.91233583e-05, 3.56369489e-05), (0, 0, 0)] +) +@pytest.mark.parametrize( + "omega_BastR_B", [(-2.23886891e-02, 2.47942516e-02, -2.55601849e-02), (0, 0, 0)] +) @pytest.mark.parametrize("integralLimit", [0, 20]) @pytest.mark.parametrize("useRwAvailability", ["NO", "ON", "OFF"]) - - -def test_rate_servo_full_nonlinear(show_plots, rwNum, intGain, omegap_BastR_B, omega_BastR_B, integralLimit, - useRwAvailability): +def test_rate_servo_full_nonlinear( + show_plots, + rwNum, + intGain, + omegap_BastR_B, + omega_BastR_B, + integralLimit, + useRwAvailability, +): """Module Unit Test""" - [testResults, testMessage] = rate_servo_full_nonlinear(show_plots, rwNum, intGain, omegap_BastR_B, omega_BastR_B, - integralLimit, useRwAvailability) + [testResults, testMessage] = rate_servo_full_nonlinear( + show_plots, + rwNum, + intGain, + omegap_BastR_B, + omega_BastR_B, + integralLimit, + useRwAvailability, + ) assert testResults < 1, testMessage -def rate_servo_full_nonlinear(show_plots,rwNum, intGain, omegap_BastR_B, omega_BastR_B, integralLimit, - useRwAvailability): +def rate_servo_full_nonlinear( + show_plots, + rwNum, + intGain, + omegap_BastR_B, + omega_BastR_B, + integralLimit, + useRwAvailability, +): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. - #__tracebackhide__ = True + # __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages @@ -82,12 +109,14 @@ def rate_servo_full_nonlinear(show_plots,rwNum, intGain, omegap_BastR_B, omega_B module.Ki = intGain module.P = 150.0 module.integralLimit = integralLimit - module.knownTorquePntB_B = (1,1,1) + module.knownTorquePntB_B = (1, 1, 1) # Create input message and size it because the regular creator of that message # is not part of the test. # attGuidOut Message: - guidCmdData = messaging.AttGuidMsgPayload() # Create a structure for the input message + guidCmdData = ( + messaging.AttGuidMsgPayload() + ) # Create a structure for the input message sigma_BR = np.array([0.3, -0.5, 0.7]) guidCmdData.sigma_BR = sigma_BR omega_BR_B = np.array([0.010, -0.020, 0.015]) @@ -100,9 +129,7 @@ def rate_servo_full_nonlinear(show_plots,rwNum, intGain, omegap_BastR_B, omega_B # vehicleConfigData Message: vehicleConfigOut = messaging.VehicleConfigMsgPayload() - I = [1000., 0., 0., - 0., 800., 0., - 0., 0., 800.] + I = [1000.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 800.0] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) @@ -115,13 +142,22 @@ def rate_servo_full_nonlinear(show_plots,rwNum, intGain, omegap_BastR_B, omega_B # wheelConfigData message jsList = [] GsMatrix_B = [] + def writeMsgInWheelConfiguration(): rwConfigParams = messaging.RWArrayConfigMsgPayload() rwConfigParams.GsMatrix_B = [ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - 0.5773502691896258, 0.5773502691896258, 0.5773502691896258 + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.5773502691896258, + 0.5773502691896258, + 0.5773502691896258, ] rwConfigParams.JsList = [0.1, 0.1, 0.1, 0.1] rwConfigParams.numRW = rwNum @@ -134,19 +170,30 @@ def writeMsgInWheelConfiguration(): rwAvailabilityMessage = messaging.RWAvailabilityMsgPayload() if useRwAvailability != "NO": if useRwAvailability == "ON": - rwAvailabilityMessage.wheelAvailability = [messaging.AVAILABLE, messaging.AVAILABLE, - messaging.AVAILABLE, messaging.AVAILABLE] + rwAvailabilityMessage.wheelAvailability = [ + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + ] elif useRwAvailability == "OFF": - rwAvailabilityMessage.wheelAvailability = [messaging.UNAVAILABLE, messaging.UNAVAILABLE, - messaging.UNAVAILABLE, messaging.UNAVAILABLE] + rwAvailabilityMessage.wheelAvailability = [ + messaging.UNAVAILABLE, + messaging.UNAVAILABLE, + messaging.UNAVAILABLE, + messaging.UNAVAILABLE, + ] else: print("WARNING: unknown rw availability status") rwAvailInMsg = messaging.RWAvailabilityMsg().write(rwAvailabilityMessage) else: # set default availability - rwAvailabilityMessage.wheelAvailability = [messaging.AVAILABLE, messaging.AVAILABLE, - messaging.AVAILABLE, messaging.AVAILABLE] - + rwAvailabilityMessage.wheelAvailability = [ + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + ] # rateSteering message rateSteeringMsg = messaging.RateCmdMsgPayload() @@ -181,18 +228,33 @@ def writeMsgInWheelConfiguration(): unitTestSim.ExecuteSimulation() # set the filtered output truth states - LrTrue = findTrueTorques(module, guidCmdData, rwSpeedMessage, vehicleConfigOut, jsList, - rwNum, GsMatrix_B, rwAvailabilityMessage,rateSteeringMsg) - + LrTrue = findTrueTorques( + module, + guidCmdData, + rwSpeedMessage, + vehicleConfigOut, + jsList, + rwNum, + GsMatrix_B, + rwAvailabilityMessage, + rateSteeringMsg, + ) # compare the module results to the truth values accuracy = 1e-8 for i in range(0, len(LrTrue)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.torqueRequestBody[i], LrTrue[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.torqueRequestBody[i], LrTrue[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed torqueRequestBody unit test at t=" - + str(dataLog.times()[i] * macros.NANO2SEC) + "sec \n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed torqueRequestBody unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec \n" + ) # If the argument provided at commandline "--show_plots" evaluates as true, # plot all figures @@ -205,26 +267,34 @@ def writeMsgInWheelConfiguration(): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] - - - - - - -def findTrueTorques(module,guidCmdData,rwSpeedMessage,vehicleConfigOut,jsList,numRW,GsMatrix_B,rwAvailMsg,rateSteeringMsg ): + return [testFailCount, "".join(testMessages)] + + +def findTrueTorques( + module, + guidCmdData, + rwSpeedMessage, + vehicleConfigOut, + jsList, + numRW, + GsMatrix_B, + rwAvailMsg, + rateSteeringMsg, +): Lr = [] - #Read in variables + # Read in variables L = np.asarray(module.knownTorquePntB_B) - steps = [0, 0, .5, 0, .5] + steps = [0, 0, 0.5, 0, 0.5] omega_BR_B = np.asarray(guidCmdData.omega_BR_B) omega_RN_B = np.asarray(guidCmdData.omega_RN_B) - omega_BN_B = omega_BR_B + omega_RN_B #find body rate + omega_BN_B = omega_BR_B + omega_RN_B # find body rate domega_RN_B = np.asarray(guidCmdData.domega_RN_B) omega_BastR_B = np.asarray(rateSteeringMsg.omega_BastR_B) - omegap_BastR_B = np.asarray(rateSteeringMsg.omegap_BastR_B) #body-frame derivative of omega_BastR_B - omega_BastN_B = omega_BastR_B+omega_RN_B + omegap_BastR_B = np.asarray( + rateSteeringMsg.omegap_BastR_B + ) # body-frame derivative of omega_BastR_B + omega_BastN_B = omega_BastR_B + omega_RN_B omega_BBast_B = omega_BN_B - omega_BastN_B Isc = np.asarray(vehicleConfigOut.ISCPntB_B) @@ -233,40 +303,51 @@ def findTrueTorques(module,guidCmdData,rwSpeedMessage,vehicleConfigOut,jsList,nu P = module.P jsVec = jsList GsMatrix_B_array = np.asarray(GsMatrix_B) - GsMatrix_B_array = np.reshape(GsMatrix_B_array[0:numRW * 3], (numRW, 3)) + GsMatrix_B_array = np.reshape(GsMatrix_B_array[0 : numRW * 3], (numRW, 3)) - #Compute toruqes + # Compute toruqes for i in range(len(steps)): dt = steps[i] if dt == 0: zVec = np.asarray([0, 0, 0]) - #evaluate integral term - if Ki > 0 and abs(module.integralLimit) > 0: #if integral feedback is on + # evaluate integral term + if Ki > 0 and abs(module.integralLimit) > 0: # if integral feedback is on zVec = dt * omega_BBast_B + zVec # z = integral(del_omega) # Make sure each component is less than the integral limit for i in range(3): if zVec[i] > module.integralLimit: - zVec[i] = zVec[i]/abs(zVec[i])*module.integralLimit + zVec[i] = zVec[i] / abs(zVec[i]) * module.integralLimit - else: #integral gain turned off/negative setting + else: # integral gain turned off/negative setting zVec = np.asarray([0, 0, 0]) - #compute torque Lr + # compute torque Lr Lr0 = Ki * zVec # +K*sigmaBR Lr1 = Lr0 + P * omega_BBast_B # +P*deltaOmega - GsHs = np.array([0,0,0]) + GsHs = np.array([0, 0, 0]) if numRW > 0: for i in range(numRW): if rwAvailMsg.wheelAvailability[i] == 0: # Make RW availability check - GsHs = GsHs + np.dot(GsMatrix_B_array[i, :], jsVec[i]*(np.dot(omega_BN_B, GsMatrix_B_array[i, :]) + rwSpeedMessage.wheelSpeeds[i])) + GsHs = GsHs + np.dot( + GsMatrix_B_array[i, :], + jsVec[i] + * ( + np.dot(omega_BN_B, GsMatrix_B_array[i, :]) + + rwSpeedMessage.wheelSpeeds[i] + ), + ) # J_s*(dot(omegaBN_B,Gs_vec)+Omega_wheel) - Lr2 = Lr1 - np.cross(omega_BastN_B, (Isc.dot(omega_BN_B)+GsHs)) # - omega_BastN x ([I]omega + [Gs]h_s) + Lr2 = Lr1 - np.cross( + omega_BastN_B, (Isc.dot(omega_BN_B) + GsHs) + ) # - omega_BastN x ([I]omega + [Gs]h_s) - Lr3 = Lr2 - Isc.dot(omegap_BastR_B + domega_RN_B - np.cross(omega_BN_B, omega_RN_B)) + Lr3 = Lr2 - Isc.dot( + omegap_BastR_B + domega_RN_B - np.cross(omega_BN_B, omega_RN_B) + ) # - [I](d(omega_B^ast/R)/dt + d(omega_r)/dt - omega x omega_r) Lr4 = Lr3 + L Lr4 = -Lr4 @@ -274,12 +355,13 @@ def findTrueTorques(module,guidCmdData,rwSpeedMessage,vehicleConfigOut,jsList,nu return Lr - if __name__ == "__main__": - test_rate_servo_full_nonlinear(False, #show plots T/F - 4, # Number of RW - 0.01, # Integral Gain - (0, 0, 0), # omegap_BastR_B - (0, 0, 0), # omega_BastR_B - 20, # integraLimit - "ON") # useRwAvailability + test_rate_servo_full_nonlinear( + False, # show plots T/F + 4, # Number of RW + 0.01, # Integral Gain + (0, 0, 0), # omegap_BastR_B + (0, 0, 0), # omega_BastR_B + 20, # integraLimit + "ON", + ) # useRwAvailability diff --git a/src/fswAlgorithms/attControl/thrMomentumManagement/_UnitTest/test_thrMomentumManagement.py b/src/fswAlgorithms/attControl/thrMomentumManagement/_UnitTest/test_thrMomentumManagement.py index b1da84ff32..692bc57c58 100755 --- a/src/fswAlgorithms/attControl/thrMomentumManagement/_UnitTest/test_thrMomentumManagement.py +++ b/src/fswAlgorithms/attControl/thrMomentumManagement/_UnitTest/test_thrMomentumManagement.py @@ -31,16 +31,14 @@ path = os.path.dirname(os.path.abspath(filename)) - - - - - - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import thrMomentumManagement # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import ( + thrMomentumManagement, +) # import the module that is to be tested from Basilisk.utilities import macros from Basilisk.utilities import fswSetupRW from Basilisk.architecture import messaging @@ -53,34 +51,32 @@ # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("hsMinCheck", [ - (0), - (1) -]) +@pytest.mark.parametrize("hsMinCheck", [(0), (1)]) # update "module" in this function name to reflect the module name def test_thrMomentumManagement(show_plots, hsMinCheck): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = thrMomentumManagementTestFunction(show_plots, hsMinCheck) + [testResults, testMessage] = thrMomentumManagementTestFunction( + show_plots, hsMinCheck + ) assert testResults < 1, testMessage def thrMomentumManagementTestFunction(show_plots, hsMinCheck): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = thrMomentumManagement.thrMomentumManagement() module.ModelTag = "thrMomentumManagement" @@ -90,17 +86,15 @@ def thrMomentumManagementTestFunction(show_plots, hsMinCheck): # Initialize the test module configuration data if hsMinCheck: - module.hs_min = 1000./6000.*100. # Nms + module.hs_min = 1000.0 / 6000.0 * 100.0 # Nms else: - module.hs_min = 100./6000.*100. # Nms - + module.hs_min = 100.0 / 6000.0 * 100.0 # Nms # wheelSpeeds Message rwSpeedMessage = messaging.RWSpeedMsgPayload() - rwSpeedMessage.wheelSpeeds = [10.0, -25.0, 50.0, 100.] + rwSpeedMessage.wheelSpeeds = [10.0, -25.0, 50.0, 100.0] rwSpeedInMsg = messaging.RWSpeedMsg().write(rwSpeedMessage) - # wheelConfigData Message fswSetupRW.clearSetup() Js = 0.1 @@ -110,8 +104,6 @@ def thrMomentumManagementTestFunction(show_plots, hsMinCheck): fswSetupRW.create([0.5773502691896258, 0.5773502691896258, 0.5773502691896258], Js) rwConfigInMsg = fswSetupRW.writeConfigMessage() - - # Setup logging on the test module output message so that we get all the writes to it dataLog = module.deltaHOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) @@ -127,42 +119,43 @@ def thrMomentumManagementTestFunction(show_plots, hsMinCheck): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() # set the filtered output truth states if hsMinCheck == 1: - trueVector = [ - [0.0, 0.0, 0.0] - ]*2 + trueVector = [[0.0, 0.0, 0.0]] * 2 else: - trueVector = [ - [-5.914369484146579, -2.858300248464629, -9.407020039211664] - ]*2 + trueVector = [[-5.914369484146579, -2.858300248464629, -9.407020039211664]] * 2 # compare the module results to the truth values accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, dataLog.torqueRequestBody, accuracy, - "torqueRequestBody", testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + dataLog.torqueRequestBody, + accuracy, + "torqueRequestBody", + testFailCount, + testMessages, + ) snippetName = "passFail" + str(hsMinCheck) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippetName, passedText, path) - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -170,7 +163,7 @@ def thrMomentumManagementTestFunction(show_plots, hsMinCheck): # stand-along python script # if __name__ == "__main__": - test_thrMomentumManagement( # update "module" in function name - True, - 0 # hsMinCheck - ) + test_thrMomentumManagement( # update "module" in function name + True, + 0, # hsMinCheck + ) diff --git a/src/fswAlgorithms/attDetermination/CSSEst/_UnitTest/test_cssWlsEstUnitTest.py b/src/fswAlgorithms/attDetermination/CSSEst/_UnitTest/test_cssWlsEstUnitTest.py index ea18983128..a9942172af 100644 --- a/src/fswAlgorithms/attDetermination/CSSEst/_UnitTest/test_cssWlsEstUnitTest.py +++ b/src/fswAlgorithms/attDetermination/CSSEst/_UnitTest/test_cssWlsEstUnitTest.py @@ -32,25 +32,25 @@ import pytest from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import cssWlsEst + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) - - - # Function that takes a sun pointing vector and array of CSS normal vectors and # returns the measurements associated with those normal vectors. def createCosList(sunPointVec, sensorPointList): outList = [] for sensorPoint in sensorPointList: outList.append(numpy.dot(sunPointVec, sensorPoint)) - if(outList[-1] < 0.0): + if outList[-1] < 0.0: outList[-1] = 0.0 return outList @@ -64,18 +64,18 @@ def checkNumActiveAccuracy(measVec, numActiveUse, numActiveFailCriteria, thresh) # Iterate through measVec and find all valid signals for i in range(0, 32): obsVal = measVec.CosValue[i] - if (obsVal > thresh): + if obsVal > thresh: numActivePred += 1 # Iterate through the numActive array and sum up all numActive estimates - numActiveTotal = numpy.array([0.]) + numActiveTotal = numpy.array([0.0]) j = 0 while j < numActiveUse.shape[0]: numActiveTotal += numActiveUse[j, 1:] j += 1 numActiveTotal /= j # Mean number of numActive # If we violate the test criteria, increment the failure count and alert user - if (abs(numActiveTotal[0] - numActivePred) > numActiveFailCriteria): + if abs(numActiveTotal[0] - numActivePred) > numActiveFailCriteria: testFailCount += 1 errorString = "Active number failure for count of: " errorString += str(numActivePred) @@ -97,14 +97,14 @@ def checksHatAccuracy(testVec, sHatEstUse, angleFailCriteria, TotalSim): sHatTotal /= j # mean sHat estimate # This logic is to protect cases where the dot product numerically breaks acos dot_value = numpy.dot(sHatTotal, testVec) - if (abs(dot_value > 1.0)): + if abs(dot_value > 1.0): dot_value -= 2.0 * (dot_value - math.copysign(1.0, dot_value)) # If we violate the failure criteria, increment failure count and alert user - if (abs(math.acos(dot_value)) > angleFailCriteria): + if abs(math.acos(dot_value)) > angleFailCriteria: testFailCount += 1 errorString = "Angle fail criteria violated for test vector:" - errorString += str(testVec).strip('[]') + "\n" + errorString += str(testVec).strip("[]") + "\n" errorString += "Criteria violation of: " errorString += str(abs(math.acos(numpy.dot(sHatTotal, testVec)))) logging.error(errorString) @@ -119,11 +119,11 @@ def checkResidAccuracy(testVec, sResids, sThresh, TotalSim): testFailCount = 0 # Sum up all of the sHat estimates from the execution while j < sResids.shape[0]: - sNormObs = numpy.linalg.norm(sResids[j,1:]) - if(sNormObs > sThresh): + sNormObs = numpy.linalg.norm(sResids[j, 1:]) + if sNormObs > sThresh: testFailCount += 1 errorString = "Residual error computation failure:" - errorString += str(testVec).strip('[]') + "\n" + errorString += str(testVec).strip("[]") + "\n" errorString += "Criteria violation of: " errorString += str(sNormObs) logging.error(errorString) @@ -136,14 +136,15 @@ def checkResidAccuracy(testVec, sResids, sThresh, TotalSim): # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(conditionstring) -@pytest.mark.parametrize("testSunHeading, testRate", [ - ("True", "False") - ,("False", "True") -]) +@pytest.mark.parametrize( + "testSunHeading, testRate", [("True", "False"), ("False", "True")] +) # provide a unique test method name, starting with test_ -def test_module(show_plots, testSunHeading, testRate): # update "module" in this function name to reflect the module name +def test_module( + show_plots, testSunHeading, testRate +): # update "module" in this function name to reflect the module name """Module Unit Test""" # each test method requires a single assert method to be called # pass on the testPlotFixture so that the main test function may set the DataStore attributes @@ -158,17 +159,17 @@ def test_module(show_plots, testSunHeading, testRate): # update "module" in def cssWlsEstTestFunction(show_plots): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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 testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, int(1E8))) + testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, int(1e8))) # Construct algorithm and associated C++ container CSSWlsEstFSW = cssWlsEst.cssWlsEst() @@ -209,9 +210,11 @@ def cssWlsEstTestFunction(show_plots): cssDataMsg = messaging.CSSArraySensorMsgPayload() cssDataInMsg = messaging.CSSArraySensorMsg().write(cssDataMsg) - angleFailCriteria = 17.5 * math.pi / 180.0 # Get 95% effective charging in this case + angleFailCriteria = ( + 17.5 * math.pi / 180.0 + ) # Get 95% effective charging in this case numActiveFailCriteria = 0.000001 # basically zero - residFailCriteria = 1.0E-12 # Essentially numerically "small" + residFailCriteria = 1.0e-12 # Essentially numerically "small" # Log the output message as well as the internal numACtiveCss variables navData = CSSWlsEstFSW.navStateOutMsg.recorder() @@ -226,16 +229,20 @@ def cssWlsEstTestFunction(show_plots): CSSWlsEstFSW.cssConfigInMsg.subscribeTo(cssConfigDataInMsg) # Initial test is all of the principal body axes - TestVectors = [[-1.0, 0.0, 0.0], - [0.0, -1.0, 0.0], - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, -1.0], - [0.0, 0.0, 1.0]] + TestVectors = [ + [-1.0, 0.0, 0.0], + [0.0, -1.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, -1.0], + [0.0, 0.0, 1.0], + ] # Initialize test and then step through all of the test vectors in a loop unitTestSim.InitializeSimulation() - CSSWlsEstFSW.Reset(0) # this module reset function needs a time input (in NanoSeconds) + CSSWlsEstFSW.Reset( + 0 + ) # this module reset function needs a time input (in NanoSeconds) stepCount = 0 logLengthPrev = 0 @@ -252,7 +259,7 @@ def cssWlsEstTestFunction(show_plots): cssDataInMsg.write(cssDataMsg) # Increment the stop time to new termination value - unitTestSim.ConfigureStopTime(int((stepCount + 1) * 1E9)) + unitTestSim.ConfigureStopTime(int((stepCount + 1) * 1e9)) # Execute simulation to current stop time unitTestSim.ExecuteSimulation() stepCount += 1 @@ -260,19 +267,27 @@ def cssWlsEstTestFunction(show_plots): # Pull logged data out into workspace for analysis sHatEst = navData.vehSunPntBdy - numActive = unitTestSupport.addTimeColumn(numActiveData.times(), numActiveData.numActiveCss) + numActive = unitTestSupport.addTimeColumn( + numActiveData.times(), numActiveData.numActiveCss + ) sHatEstUse = sHatEst[logLengthPrev:, :] # Only data for this subtest - numActiveUse = numActive[logLengthPrev + 1:, :] # Only data for this subtest + numActiveUse = numActive[logLengthPrev + 1 :, :] # Only data for this subtest # Check failure criteria and add test failures - testFailCount += checksHatAccuracy(testVec, sHatEstUse, angleFailCriteria, - unitTestSim) - testFailCount += checkNumActiveAccuracy(cssDataMsg, numActiveUse, - numActiveFailCriteria, CSSWlsEstFSW.sensorUseThresh) + testFailCount += checksHatAccuracy( + testVec, sHatEstUse, angleFailCriteria, unitTestSim + ) + testFailCount += checkNumActiveAccuracy( + cssDataMsg, + numActiveUse, + numActiveFailCriteria, + CSSWlsEstFSW.sensorUseThresh, + ) filtRes = filterData.postFitRes - testFailCount += checkResidAccuracy(testVec, filtRes, residFailCriteria, - unitTestSim) + testFailCount += checkResidAccuracy( + testVec, filtRes, residFailCriteria, unitTestSim + ) # Pop truth state onto end of array for plotting purposes currentRow = [sHatEstUse[0, 0]] @@ -286,20 +301,25 @@ def cssWlsEstTestFunction(show_plots): # Hand construct case where we get low coverage (2 valid sensors) LonVal = 0.0 LatVal = 40.68 * math.pi / 180.0 - doubleTestVec = [math.sin(LatVal), math.cos(LatVal) * math.sin(LonVal), - math.cos(LatVal) * math.cos(LonVal)] + doubleTestVec = [ + math.sin(LatVal), + math.cos(LatVal) * math.sin(LonVal), + math.cos(LatVal) * math.cos(LonVal), + ] cssDataMsg.CosValue = createCosList(doubleTestVec, CSSOrientationList) # Write in double coverage conditions and ensure that we get correct outputs cssDataInMsg.write(cssDataMsg) - unitTestSim.ConfigureStopTime(int((stepCount + 1) * 1E9)) + unitTestSim.ConfigureStopTime(int((stepCount + 1) * 1e9)) unitTestSim.ExecuteSimulation() stepCount += 1 sHatEst = navData.vehSunPntBdy - numActive = unitTestSupport.addTimeColumn(numActiveData.times(), numActiveData.numActiveCss) + numActive = unitTestSupport.addTimeColumn( + numActiveData.times(), numActiveData.numActiveCss + ) sHatEstUse = sHatEst[logLengthPrev:, :] - numActiveUse = numActive[logLengthPrev + 1:, :] + numActiveUse = numActive[logLengthPrev + 1 :, :] logLengthPrev = sHatEst.shape[0] currentRow = [sHatEstUse[0, 0]] currentRow.extend(doubleTestVec) @@ -309,25 +329,30 @@ def cssWlsEstTestFunction(show_plots): truthData.append(currentRow) # Check test criteria again - testFailCount += checksHatAccuracy(doubleTestVec, sHatEstUse, angleFailCriteria, - unitTestSim) - testFailCount += checkNumActiveAccuracy(cssDataMsg, numActiveUse, - numActiveFailCriteria, CSSWlsEstFSW.sensorUseThresh) + testFailCount += checksHatAccuracy( + doubleTestVec, sHatEstUse, angleFailCriteria, unitTestSim + ) + testFailCount += checkNumActiveAccuracy( + cssDataMsg, numActiveUse, numActiveFailCriteria, CSSWlsEstFSW.sensorUseThresh + ) # Same test as above, but zero first element to get to a single coverage case cssDataMsg.CosValue[0] = 0.0 cssDataInMsg.write(cssDataMsg) - unitTestSim.ConfigureStopTime(int((stepCount + 1) * 1E9)) + unitTestSim.ConfigureStopTime(int((stepCount + 1) * 1e9)) unitTestSim.ExecuteSimulation() stepCount += 1 - numActive = unitTestSupport.addTimeColumn(numActiveData.times(), numActiveData.numActiveCss) - numActiveUse = numActive[logLengthPrev + 1:, :] + numActive = unitTestSupport.addTimeColumn( + numActiveData.times(), numActiveData.numActiveCss + ) + numActiveUse = numActive[logLengthPrev + 1 :, :] sHatEst = navData.vehSunPntBdy - sHatEstUse = sHatEst[logLengthPrev + 1:, :] + sHatEstUse = sHatEst[logLengthPrev + 1 :, :] logLengthPrev = sHatEst.shape[0] - testFailCount += checkNumActiveAccuracy(cssDataMsg, numActiveUse, - numActiveFailCriteria, CSSWlsEstFSW.sensorUseThresh) + testFailCount += checkNumActiveAccuracy( + cssDataMsg, numActiveUse, numActiveFailCriteria, CSSWlsEstFSW.sensorUseThresh + ) currentRow = [sHatEstUse[0, 0]] currentRow.extend(doubleTestVec) truthData.append(currentRow) @@ -340,73 +365,80 @@ def cssWlsEstTestFunction(show_plots): cssDataMsg.CosValue[3] = 0.0 cssDataInMsg.write(cssDataMsg) - unitTestSim.ConfigureStopTime(int((stepCount + 1) * 1E9)) + unitTestSim.ConfigureStopTime(int((stepCount + 1) * 1e9)) unitTestSim.ExecuteSimulation() - numActive = unitTestSupport.addTimeColumn(numActiveData.times(), numActiveData.numActiveCss) + numActive = unitTestSupport.addTimeColumn( + numActiveData.times(), numActiveData.numActiveCss + ) numActiveUse = numActive[logLengthPrev:, :] logLengthPrev = numActive.shape[0] - testFailCount += checkNumActiveAccuracy(cssDataMsg, numActiveUse, - numActiveFailCriteria, CSSWlsEstFSW.sensorUseThresh) + testFailCount += checkNumActiveAccuracy( + cssDataMsg, numActiveUse, numActiveFailCriteria, CSSWlsEstFSW.sensorUseThresh + ) # Format data for plotting truthData = numpy.array(truthData) sHatEst = navData.vehSunPntBdy - numActive = unitTestSupport.addTimeColumn(numActiveData.times(), numActiveData.numActiveCss) - + numActive = unitTestSupport.addTimeColumn( + numActiveData.times(), numActiveData.numActiveCss + ) # # test the case where all CSS signals are zero # cssDataMsg.CosValue = numpy.zeros(len(CSSOrientationList)) cssDataInMsg.write(cssDataMsg) - unitTestSim.ConfigureStopTime(int((stepCount + 2) * 1E9)) + unitTestSim.ConfigureStopTime(int((stepCount + 2) * 1e9)) unitTestSim.ExecuteSimulation() sHatEstZero = navData.vehSunPntBdy - sHatEstZeroUse = sHatEstZero[logLengthPrev + 1:, :] + sHatEstZeroUse = sHatEstZero[logLengthPrev + 1 :, :] - trueVector = [[0.0, 0.0, 0.0]]*len(sHatEstZeroUse) + trueVector = [[0.0, 0.0, 0.0]] * len(sHatEstZeroUse) for i in range(0, len(trueVector)): # check a vector values if not unitTestSupport.isArrayEqual(sHatEstZeroUse[i], trueVector[i], 3, 1e-12): testFailCount += 1 - testMessages.append("FAILED: " + CSSWlsEstFSW.ModelTag + " Module failed unit test at t=" + - str(navData.times()[i] * macros.NANO2SEC) + "sec\n") - - + testMessages.append( + "FAILED: " + + CSSWlsEstFSW.ModelTag + + " Module failed unit test at t=" + + str(navData.times()[i] * macros.NANO2SEC) + + "sec\n" + ) if show_plots: plt.figure(1) - plt.plot(sHatEst[:, 0] * 1.0E-9, sHatEst[:, 0], label='x-Sun') - plt.plot(sHatEst[:, 0] * 1.0E-9, sHatEst[:, 1], label='y-Sun') - plt.plot(sHatEst[:, 0] * 1.0E-9, sHatEst[:, 2], label='z-Sun') - plt.legend(loc='upper left') - plt.xlabel('Time (s)') - plt.ylabel('Unit Component (--)') + plt.plot(sHatEst[:, 0] * 1.0e-9, sHatEst[:, 0], label="x-Sun") + plt.plot(sHatEst[:, 0] * 1.0e-9, sHatEst[:, 1], label="y-Sun") + plt.plot(sHatEst[:, 0] * 1.0e-9, sHatEst[:, 2], label="z-Sun") + plt.legend(loc="upper left") + plt.xlabel("Time (s)") + plt.ylabel("Unit Component (--)") plt.figure(2) - plt.plot(numActive[:, 0] * 1.0E-9, numActive[:, 1]) - plt.xlabel('Time (s)') - plt.ylabel('Number Active CSS (--)') + plt.plot(numActive[:, 0] * 1.0e-9, numActive[:, 1]) + plt.xlabel("Time (s)") + plt.ylabel("Number Active CSS (--)") plt.figure(3) plt.subplot(3, 1, 1) - plt.plot(sHatEst[:, 0] * 1.0E-9, sHatEst[:, 0], label='Est') - plt.plot(truthData[:, 0] * 1.0E-9, truthData[:, 0], 'r--', label='Truth') - plt.xlabel('Time (s)') - plt.ylabel('X Component (--)') - plt.legend(loc='lower right') + plt.plot(sHatEst[:, 0] * 1.0e-9, sHatEst[:, 0], label="Est") + plt.plot(truthData[:, 0] * 1.0e-9, truthData[:, 0], "r--", label="Truth") + plt.xlabel("Time (s)") + plt.ylabel("X Component (--)") + plt.legend(loc="lower right") plt.subplot(3, 1, 2) - plt.plot(sHatEst[:, 0] * 1.0E-9, sHatEst[:, 1], label='Est') - plt.plot(truthData[:, 0] * 1.0E-9, truthData[:, 1], 'r--', label='Truth') - plt.xlabel('Time (s)') - plt.ylabel('Y Component (--)') + plt.plot(sHatEst[:, 0] * 1.0e-9, sHatEst[:, 1], label="Est") + plt.plot(truthData[:, 0] * 1.0e-9, truthData[:, 1], "r--", label="Truth") + plt.xlabel("Time (s)") + plt.ylabel("Y Component (--)") plt.subplot(3, 1, 3) - plt.plot(sHatEst[:, 0] * 1.0E-9, sHatEst[:, 2], label='Est') - plt.plot(truthData[:, 0] * 1.0E-9, truthData[:, 2], 'r--', label='Truth') - plt.xlabel('Time (s)') - plt.ylabel('Z Component (--)') + plt.plot(sHatEst[:, 0] * 1.0e-9, sHatEst[:, 2], label="Est") + plt.plot(truthData[:, 0] * 1.0e-9, truthData[:, 2], "r--", label="Truth") + plt.xlabel("Time (s)") + plt.ylabel("Z Component (--)") plt.show() - plt.close('all') + plt.close("all") # print out success message if no error were found if testFailCount == 0: @@ -414,14 +446,14 @@ def cssWlsEstTestFunction(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def cssRateTestFunction(show_plots): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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() @@ -493,7 +525,7 @@ def cssRateTestFunction(show_plots): unitTestSim.ExecuteSimulation() # test the module reset function - module.Reset(1) # this module reset function needs a time input (in NanoSeconds) + module.Reset(1) # this module reset function needs a time input (in NanoSeconds) unitTestSim.ConfigureStopTime(macros.sec2nano(2.5)) unitTestSim.ExecuteSimulation() cssDataMsg.CosValue = createCosList([1.0, 0.0, 0.0], CSSOrientationList) @@ -509,27 +541,32 @@ def cssRateTestFunction(show_plots): [0.0, 0.0, -3.14159265], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], - [0.0, 0.0, +3.14159265] + [0.0, 0.0, +3.14159265], ] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, dataLog.omega_BN_B, - accuracy, "CSS Rate Vector", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + dataLog.omega_BN_B, + accuracy, + "CSS Rate Vector", + testFailCount, + testMessages, + ) # print out success message if no error were found snippentName = "passFailRate" if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -538,7 +575,7 @@ def cssRateTestFunction(show_plots): # if __name__ == "__main__": test_module( - True, # show_plots - False, # testSunHeading Flag - True # testRate Flag + True, # show_plots + False, # testSunHeading Flag + True, # testRate Flag ) diff --git a/src/fswAlgorithms/attDetermination/InertialUKF/_UnitTest/test_inertialKF.py b/src/fswAlgorithms/attDetermination/InertialUKF/_UnitTest/test_inertialKF.py index d4a6e91762..b4a9e873de 100644 --- a/src/fswAlgorithms/attDetermination/InertialUKF/_UnitTest/test_inertialKF.py +++ b/src/fswAlgorithms/attDetermination/InertialUKF/_UnitTest/test_inertialKF.py @@ -25,15 +25,17 @@ from Basilisk.fswAlgorithms import inertialUKF # import the module that is to be tested from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -textSnippetPassed = r'\textcolor{ForestGreen}{' + "PASSED" + '}' -textSnippetFailed = r'\textcolor{Red}{' + "Failed" + '}' +textSnippetPassed = r"\textcolor{ForestGreen}{" + "PASSED" + "}" +textSnippetFailed = r"\textcolor{Red}{" + "Failed" + "}" -def setupFilterData(filterObject): +def setupFilterData(filterObject): filterObject.alpha = 0.02 filterObject.beta = 2.0 filterObject.kappa = 0.0 @@ -41,36 +43,85 @@ def setupFilterData(filterObject): ST1Data = inertialUKF.STMessage() - ST1Data.noise = [0.00017 * 0.00017, 0.0, 0.0, - 0.0, 0.00017 * 0.00017, 0.0, - 0.0, 0.0, 0.00017 * 0.00017] + ST1Data.noise = [ + 0.00017 * 0.00017, + 0.0, + 0.0, + 0.0, + 0.00017 * 0.00017, + 0.0, + 0.0, + 0.0, + 0.00017 * 0.00017, + ] ST2Data = inertialUKF.STMessage() - ST2Data.noise = [0.00017 * 0.00017, 0.0, 0.0, - 0.0, 0.00017 * 0.00017, 0.0, - 0.0, 0.0, 0.00017 * 0.00017] + ST2Data.noise = [ + 0.00017 * 0.00017, + 0.0, + 0.0, + 0.0, + 0.00017 * 0.00017, + 0.0, + 0.0, + 0.0, + 0.00017 * 0.00017, + ] STList = [ST1Data, ST2Data] filterObject.STDatasStruct.STMessages = STList filterObject.STDatasStruct.numST = len(STList) filterObject.stateInit = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0] - filterObject.covarInit = [0.04, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.004] + filterObject.covarInit = [ + 0.04, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.04, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.04, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.004, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.004, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.004, + ] qNoiseIn = numpy.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3]*0.0017*0.0017 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6]*0.00017*0.00017 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 0.0017 * 0.0017 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 0.00017 * 0.00017 filterObject.qNoise = qNoiseIn.reshape(36).tolist() lpDataUse = inertialUKF.LowPassFilterData() lpDataUse.hStep = 0.5 - lpDataUse.omegCutoff = 15.0/(2.0*math.pi) + lpDataUse.omegCutoff = 15.0 / (2.0 * math.pi) filterObject.gyroFilt = [lpDataUse, lpDataUse, lpDataUse] + # uncomment this line is 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 @@ -92,9 +143,12 @@ def all_inertial_kfTest(show_plots): # [testResults, testMessage] = faultScenarios() # assert testResults < 1, testMessage + def test_FilterMethods(): [testResults, testMessage] = filterMethods() assert testResults < 1, testMessage + + def filterMethods(): """Module Unit Test""" testFailCount = 0 @@ -111,7 +165,7 @@ def filterMethods(): testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - accuracy = 1E-10 + accuracy = 1e-10 # Construct algorithm and associated C++ container module = inertialUKF.inertialUKF() module.ModelTag = "inertialUKF" @@ -136,12 +190,12 @@ def filterMethods(): STList = [ST1Data, ST2Data, ST3Data] state = inertialUKF.new_doubleArray(6) - stateInput = numpy.array([1., 0., 0., 0.1, 0.1, 0.1]) + stateInput = numpy.array([1.0, 0.0, 0.0, 0.1, 0.1, 0.1]) for i in range(len(stateInput)): inertialUKF.doubleArray_setitem(state, i, stateInput[i]) - wheelAccel = numpy.array([-5, 5]) / 1. * numpy.array([1., 1]) - angAccel = -0.5 * (wheelAccel[0] + wheelAccel[1]) * numpy.array([1., 0., 0]) + wheelAccel = numpy.array([-5, 5]) / 1.0 * numpy.array([1.0, 1]) + angAccel = -0.5 * (wheelAccel[0] + wheelAccel[1]) * numpy.array([1.0, 0.0, 0]) expectedRate = numpy.array(stateInput[3:]) + angAccel inertialUKF.inertialStateProp(module.getConfig(), state, 0.5) @@ -155,9 +209,7 @@ def filterMethods(): setupFilterData(module) vehicleConfigOut = messaging.VehicleConfigMsgPayload() - I = [1000., 0., 0., - 0., 800., 0., - 0., 0., 800.] + I = [1000.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 800.0] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) @@ -188,27 +240,35 @@ def filterMethods(): # Star Tracker Read Message and Order method unitTestSim.InitializeSimulation() - unitTestSim.ConfigureStopTime(1E9) + unitTestSim.ConfigureStopTime(1e9) unitTestSim.ExecuteSimulation() - stOrdered = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.stSensorOrder) - if numpy.linalg.norm(numpy.array(stOrdered[0]) - numpy.array([0., 2, 1, 0, 0])) > accuracy: + stOrdered = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.stSensorOrder + ) + if ( + numpy.linalg.norm(numpy.array(stOrdered[0]) - numpy.array([0.0, 2, 1, 0, 0])) + > accuracy + ): testFailCount += 1 testMessages.append("ST order test failed") unitTestSupport.writeTeXSnippet("toleranceValue00", str(accuracy), path) if testFailCount == 0: - print('Passed: test_FilterMethods') + print("Passed: test_FilterMethods") unitTestSupport.writeTeXSnippet("passFail00", textSnippetPassed, path) else: - print('Failed: test_FilterMethods') + print("Failed: test_FilterMethods") unitTestSupport.writeTeXSnippet("passFail00", textSnippetFailed, path) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def test_stateUpdateInertialAttitude(show_plots): [testResults, testMessage] = stateUpdateInertialAttitude(show_plots) assert testResults < 1, testMessage + + def stateUpdateInertialAttitude(show_plots): """Module Unit Test""" # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -241,9 +301,7 @@ def stateUpdateInertialAttitude(show_plots): module.maxTimeJump = 10 vehicleConfigOut = messaging.VehicleConfigMsgPayload() - I = [1000., 0., 0., - 0., 800., 0., - 0., 0., 800.] + I = [1000.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 800.0] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) @@ -255,10 +313,10 @@ def stateUpdateInertialAttitude(show_plots): stMessage2.MRP_BdyInrtl = [0.3, 0.4, 0.5] st2InMsg = messaging.STAttMsg() -# stateTarget = testVector.tolist() -# stateTarget.extend([0.0, 0.0, 0.0]) -# module.state = [0.7, 0.7, 0.0] - inertialUKFLog = module.logger(["covar", "state"], testProcessRate*10) + # stateTarget = testVector.tolist() + # stateTarget.extend([0.0, 0.0, 0.0]) + # module.state = [0.7, 0.7, 0.0] + inertialUKFLog = module.logger(["covar", "state"], testProcessRate * 10) unitTestSim.AddModelToTask(unitTaskName, inertialUKFLog) # make input messages but don't write to them @@ -278,85 +336,111 @@ def stateUpdateInertialAttitude(show_plots): for i in range(20000): if i > 21: - stMessage1.timeTag = int(i*0.5*1E9) - stMessage2.timeTag = int(i*0.5*1E9) + stMessage1.timeTag = int(i * 0.5 * 1e9) + stMessage2.timeTag = int(i * 0.5 * 1e9) st1InMsg.write(stMessage1, unitTestSim.TotalSim.CurrentNanos) st2InMsg.write(stMessage2, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i+1)*0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * 0.5)) unitTestSim.ExecuteSimulation() - covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) - stateLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.state) - accuracy = 1.0E-5 + covarLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.covar + ) + stateLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.state + ) + accuracy = 1.0e-5 unitTestSupport.writeTeXSnippet("toleranceValue11", str(accuracy), path) for i in range(3): - if(covarLog[-1, i*6+1+i] > covarLog[0, i*6+1+i]): + if covarLog[-1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i]: testFailCount += 1 testMessages.append("Covariance update failure") - unitTestSupport.writeTeXSnippet('passFail11', textSnippetFailed, path) + unitTestSupport.writeTeXSnippet("passFail11", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail11', textSnippetPassed, path) - if(abs(stateLog[-1, i+1] - stMessage1.MRP_BdyInrtl[i]) > accuracy): - print(abs(stateLog[-1, i+1] - stMessage1.MRP_BdyInrtl[i])) + unitTestSupport.writeTeXSnippet("passFail11", textSnippetPassed, path) + if abs(stateLog[-1, i + 1] - stMessage1.MRP_BdyInrtl[i]) > accuracy: + print(abs(stateLog[-1, i + 1] - stMessage1.MRP_BdyInrtl[i])) testFailCount += 1 testMessages.append("State update failure") - unitTestSupport.writeTeXSnippet('passFail11', textSnippetFailed, path) + unitTestSupport.writeTeXSnippet("passFail11", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail11', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail11", textSnippetPassed, path) stMessage1.MRP_BdyInrtl = [1.2, 0.0, 0.0] stMessage2.MRP_BdyInrtl = [1.2, 0.0, 0.0] for i in range(20000): if i > 20: - stMessage1.timeTag = int((i+20000)*0.25*1E9) - stMessage2.timeTag = int((i+20000)*0.5*1E9) + stMessage1.timeTag = int((i + 20000) * 0.25 * 1e9) + stMessage2.timeTag = int((i + 20000) * 0.5 * 1e9) st1InMsg.write(stMessage1, unitTestSim.TotalSim.CurrentNanos) st2InMsg.write(stMessage2, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i+20000+1)*0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + 20000 + 1) * 0.5)) unitTestSim.ExecuteSimulation() - - covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) - stateLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.state) + covarLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.covar + ) + stateLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.state + ) for i in range(3): - if(covarLog[-1, i*6+1+i] > covarLog[0, i*6+1+i]): + if covarLog[-1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i]: testFailCount += 1 testMessages.append("Covariance update large failure") - unitTestSupport.writeTeXSnippet('passFail11', textSnippetFailed, path) + unitTestSupport.writeTeXSnippet("passFail11", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail11', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail11", textSnippetPassed, path) plt.figure() for i in range(module.numStates): - plt.plot(stateLog[:,0]*1.0E-9, stateLog[:,i+1], label='State_' +str(i)) + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, i + 1], label="State_" + str(i)) plt.legend() plt.ylim([-1, 1]) - unitTestSupport.writeFigureLaTeX('Test11', 'Test 1 State convergence', plt, 'width=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "Test11", + "Test 1 State convergence", + plt, + "width=0.9\\textwidth, keepaspectratio", + path, + ) plt.figure() for i in range(module.numStates): - plt.plot(covarLog[:,0]*1.0E-9, covarLog[:,i*module.numStates+i+1], label='Covar_' +str(i)) + plt.plot( + covarLog[:, 0] * 1.0e-9, + covarLog[:, i * module.numStates + i + 1], + label="Covar_" + str(i), + ) plt.legend() - plt.ylim([0, 2.E-7]) - - unitTestSupport.writeFigureLaTeX('Test12', 'Test 1 Covariance convergence', plt, 'width=0.9\\textwidth, keepaspectratio', path) - if(show_plots): + plt.ylim([0, 2.0e-7]) + + unitTestSupport.writeFigureLaTeX( + "Test12", + "Test 1 Covariance convergence", + plt, + "width=0.9\\textwidth, keepaspectratio", + path, + ) + if show_plots: plt.show() - plt.close('all') + plt.close("all") # print out success message if no error were found if testFailCount == 0: - print('Passed: test_StateUpdateInertialAttitude') + print("Passed: test_StateUpdateInertialAttitude") else: - print('Failed: test_StateUpdateInertialAttitude') + print("Failed: test_StateUpdateInertialAttitude") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def BROKENtest_statePropInertialAttitude(show_plots): [testResults, testMessage] = statePropInertialAttitude(show_plots) assert testResults < 1, testMessage + + def statePropInertialAttitude(show_plots): """Module Unit Test""" @@ -388,14 +472,11 @@ def statePropInertialAttitude(show_plots): setupFilterData(module) vehicleConfigOut = messaging.VehicleConfigMsgPayload() - I = [1000., 0., 0., - 0., 800., 0., - 0., 0., 800.] + I = [1000.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 800.0] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) - - inertialUKFLog = module.logger(["covar", "state"], testProcessRate*10) + inertialUKFLog = module.logger(["covar", "state"], testProcessRate * 10) # make input messages but don't write to them rwSpeedInMsg = messaging.RWSpeedMsg() @@ -412,43 +493,49 @@ def statePropInertialAttitude(show_plots): module.rwParamsInMsg.subscribeTo(rwConfigInMsg) module.gyrBuffInMsg.subscribeTo(gyroInMsg) - unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(macros.sec2nano(8000.0)) unitTestSim.ExecuteSimulation() - covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) - stateLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.state) + covarLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.covar + ) + stateLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.state + ) - accuracy = 1.0E-10 + accuracy = 1.0e-10 unitTestSupport.writeTeXSnippet("toleranceValue22", str(accuracy), path) for i in range(6): - if(abs(stateLog[-1, i+1] - stateLog[0, i+1]) > accuracy): + if abs(stateLog[-1, i + 1] - stateLog[0, i + 1]) > accuracy: testFailCount += 1 testMessages.append("State propagation failure") - unitTestSupport.writeTeXSnippet('passFail22', textSnippetFailed, path) + unitTestSupport.writeTeXSnippet("passFail22", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail22', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail22", textSnippetPassed, path) for i in range(6): - if(covarLog[-1, i*6+i+1] <= covarLog[0, i*6+i+1]): - testFailCount += 1 - testMessages.append("State covariance failure i="+str(i)) + if covarLog[-1, i * 6 + i + 1] <= covarLog[0, i * 6 + i + 1]: + testFailCount += 1 + testMessages.append("State covariance failure i=" + str(i)) # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag + " state propagation") else: - print('Failed: test_StatePropInertialAttitude') + print("Failed: test_StatePropInertialAttitude") print(testMessages) # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def test_stateUpdateRWInertialAttitude(show_plots): [testResults, testMessage] = stateUpdateRWInertialAttitude(show_plots) assert testResults < 1, testMessage + + def stateUpdateRWInertialAttitude(show_plots): """Module Unit Test""" # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -480,9 +567,7 @@ def stateUpdateRWInertialAttitude(show_plots): setupFilterData(module) vehicleConfigOut = messaging.VehicleConfigMsgPayload() - I = [1000., 0., 0., - 0., 800., 0., - 0., 0., 800.] + I = [1000.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 800.0] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) @@ -490,10 +575,9 @@ def stateUpdateRWInertialAttitude(show_plots): rwArrayConfigOut.numRW = 3 rwConfigInMsg = messaging.RWArrayConfigMsg().write(rwArrayConfigOut) - rwSpeedIntMsg = messaging.RWSpeedMsgPayload() rwSpeedIntMsg.wheelSpeeds = [0.1, 0.01, 0.1] - rwSpeedIntMsg.wheelThetas = [0.,0.,0.] + rwSpeedIntMsg.wheelThetas = [0.0, 0.0, 0.0] rwSpeedInMsg = messaging.RWSpeedMsg().write(rwSpeedIntMsg) stMessage1 = messaging.STAttMsgPayload() @@ -507,7 +591,7 @@ def stateUpdateRWInertialAttitude(show_plots): # stateTarget = testVector.tolist() # stateTarget.extend([0.0, 0.0, 0.0]) # module.state = [0.7, 0.7, 0.0] - inertialUKFLog = module.logger(["covar", "state"], testProcessRate*10) + inertialUKFLog = module.logger(["covar", "state"], testProcessRate * 10) unitTestSim.AddModelToTask(unitTaskName, inertialUKFLog) # make input messages but don't write to them @@ -525,75 +609,99 @@ def stateUpdateRWInertialAttitude(show_plots): for i in range(20000): if i > 20: - stMessage1.timeTag = int(i * 0.5 * 1E9) - stMessage2.timeTag = int(i * 0.5 * 1E9) + stMessage1.timeTag = int(i * 0.5 * 1e9) + stMessage2.timeTag = int(i * 0.5 * 1e9) st1InMsg.write(stMessage1, unitTestSim.TotalSim.CurrentNanos) st2InMsg.write(stMessage2, unitTestSim.TotalSim.CurrentNanos) - if i==10000: + if i == 10000: rwSpeedIntMsg.wheelSpeeds = [0.5, 0.1, 0.05] rwSpeedInMsg.write(rwSpeedIntMsg, 0) unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * 0.5)) unitTestSim.ExecuteSimulation() - covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) - stateLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.state) + covarLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.covar + ) + stateLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.state + ) print(inertialUKFLog.covar, covarLog) - accuracy = 1.0E-5 + accuracy = 1.0e-5 unitTestSupport.writeTeXSnippet("toleranceValue33", str(accuracy), path) for i in range(3): - if (covarLog[-1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i]): + if covarLog[-1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i]: testFailCount += 1 testMessages.append("Covariance update with RW failure") - if (abs(stateLog[-1, i + 1] - stMessage1.MRP_BdyInrtl[i]) > accuracy): + if abs(stateLog[-1, i + 1] - stMessage1.MRP_BdyInrtl[i]) > accuracy: print(abs(stateLog[-1, i + 1] - stMessage1.MRP_BdyInrtl[i])) testFailCount += 1 testMessages.append("State update with RW failure") - unitTestSupport.writeTeXSnippet('passFail33', textSnippetFailed, path) + unitTestSupport.writeTeXSnippet("passFail33", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail33', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail33", textSnippetPassed, path) stMessage1.MRP_BdyInrtl = [1.2, 0.0, 0.0] stMessage2.MRP_BdyInrtl = [1.2, 0.0, 0.0] for i in range(20000): if i > 20: - stMessage1.timeTag = int((i + 20000) * 0.25 * 1E9) - stMessage2.timeTag = int((i + 20000) * 0.5 * 1E9) + stMessage1.timeTag = int((i + 20000) * 0.25 * 1e9) + stMessage2.timeTag = int((i + 20000) * 0.5 * 1e9) st1InMsg.write(stMessage1, unitTestSim.TotalSim.CurrentNanos) st2InMsg.write(stMessage2, unitTestSim.TotalSim.CurrentNanos) unitTestSim.ConfigureStopTime(macros.sec2nano((i + 20000 + 1) * 0.5)) unitTestSim.ExecuteSimulation() - covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) - stateLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.state) + covarLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.covar + ) + stateLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.state + ) for i in range(3): - if (covarLog[-1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i]): + if covarLog[-1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i]: testFailCount += 1 testMessages.append("Covariance update large failure") - unitTestSupport.writeTeXSnippet('passFail33', textSnippetFailed, path) + unitTestSupport.writeTeXSnippet("passFail33", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail33', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail33", textSnippetPassed, path) plt.figure() for i in range(module.numStates): - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, i + 1], label='State_' +str(i)) + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, i + 1], label="State_" + str(i)) plt.legend() plt.ylim([-1, 1]) - unitTestSupport.writeFigureLaTeX('Test31', 'Test 3 State convergence', plt, 'width=0.7\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "Test31", + "Test 3 State convergence", + plt, + "width=0.7\\textwidth, keepaspectratio", + path, + ) plt.figure() for i in range(module.numStates): - plt.plot(covarLog[:, 0] * 1.0E-9, covarLog[:, i * module.numStates + i + 1], label='Covar_' +str(i)) + plt.plot( + covarLog[:, 0] * 1.0e-9, + covarLog[:, i * module.numStates + i + 1], + label="Covar_" + str(i), + ) plt.legend() - plt.ylim([0., 2E-7]) - - unitTestSupport.writeFigureLaTeX('Test32', 'Test 3 Covariance convergence', plt, 'width=0.7\\textwidth, keepaspectratio', path) - if (show_plots): + plt.ylim([0.0, 2e-7]) + + unitTestSupport.writeFigureLaTeX( + "Test32", + "Test 3 Covariance convergence", + plt, + "width=0.7\\textwidth, keepaspectratio", + path, + ) + if show_plots: plt.show() - plt.close('all') + plt.close("all") # print out success message if no error were found if testFailCount == 0: @@ -601,11 +709,14 @@ def stateUpdateRWInertialAttitude(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def BROKENtest_StatePropRateInertialAttitude(show_plots): [testResults, testMessage] = statePropRateInertialAttitude(show_plots) assert testResults < 1, testMessage + + def statePropRateInertialAttitude(show_plots): """Module Unit Test""" @@ -641,21 +752,61 @@ def statePropRateInertialAttitude(show_plots): module.switchMag = 1.2 module.stateInit = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0] - module.covarInit = [0.04, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.004] + module.covarInit = [ + 0.04, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.04, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.04, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.004, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.004, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.004, + ] qNoiseIn = numpy.identity(6) qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 0.0017 * 0.0017 qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 0.00017 * 0.00017 module.qNoise = qNoiseIn.reshape(36).tolist() ST1Data = inertialUKF.STMessage() - ST1Data.noise = [0.00017 * 0.00017, 0.0, 0.0, - 0.0, 0.00017 * 0.00017, 0.0, - 0.0, 0.0, 0.00017 * 0.00017] + ST1Data.noise = [ + 0.00017 * 0.00017, + 0.0, + 0.0, + 0.0, + 0.00017 * 0.00017, + 0.0, + 0.0, + 0.0, + 0.00017 * 0.00017, + ] STList = [ST1Data] module.STDatasStruct.STMessages = STList module.STDatasStruct.numST = len(STList) @@ -666,19 +817,19 @@ def statePropRateInertialAttitude(show_plots): module.gyroFilt = [lpDataUse, lpDataUse, lpDataUse] vehicleConfigOut = messaging.VehicleConfigMsgPayload() - I = [1000., 0., 0., - 0., 800., 0., - 0., 0., 800.] + I = [1000.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 800.0] vehicleConfigOut.ISCPntB_B = I vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) - stateInit = [0.0, 0.0, 0.0, math.pi/18.0, 0.0, 0.0] + stateInit = [0.0, 0.0, 0.0, math.pi / 18.0, 0.0, 0.0] module.stateInit = stateInit - inertialUKFLog = module.logger(["covar", "sigma_BNOut", "omega_BN_BOut"], testProcessRate*10) + inertialUKFLog = module.logger( + ["covar", "sigma_BNOut", "omega_BN_BOut"], testProcessRate * 10 + ) stMessage1 = messaging.STAttMsgPayload() - stMessage1.MRP_BdyInrtl = [0., 0., 0.] - stMessage1.timeTag = int(1* 1E9) + stMessage1.MRP_BdyInrtl = [0.0, 0.0, 0.0] + stMessage1.timeTag = int(1 * 1e9) st1InMsg = messaging.STAttMsg() # make input messages but don't write to them @@ -694,35 +845,46 @@ def statePropRateInertialAttitude(show_plots): module.gyrBuffInMsg.subscribeTo(gyroInMsg) unitTestSim.InitializeSimulation() - st1InMsg.write(stMessage1, int(1 * 1E9)) + st1InMsg.write(stMessage1, int(1 * 1e9)) gyroBufferData = messaging.AccDataMsgPayload() - for i in range(3600*2+1): - gyroBufferData.accPkts[i%inertialUKF.MAX_ACC_BUF_PKT].measTime = (int(i*0.5*1E9)) - gyroBufferData.accPkts[i%inertialUKF.MAX_ACC_BUF_PKT].gyro_B = \ - [math.pi/18.0, 0.0, 0.0] - gyroInMsg.write(gyroBufferData, (int(i*0.5*1E9))) + for i in range(3600 * 2 + 1): + gyroBufferData.accPkts[i % inertialUKF.MAX_ACC_BUF_PKT].measTime = int( + i * 0.5 * 1e9 + ) + gyroBufferData.accPkts[i % inertialUKF.MAX_ACC_BUF_PKT].gyro_B = [ + math.pi / 18.0, + 0.0, + 0.0, + ] + gyroInMsg.write(gyroBufferData, (int(i * 0.5 * 1e9))) - unitTestSim.ConfigureStopTime(macros.sec2nano((i+1)*0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * 0.5)) unitTestSim.ExecuteSimulation() - covarLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.covar) - sigmaLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.sigma_BNOut) - omegaLog = unitTestSupport.addTimeColumn(inertialUKFLog.times(), inertialUKFLog.omega_BN_BOut) - accuracy = 1.0E-3 + covarLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.covar + ) + sigmaLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.sigma_BNOut + ) + omegaLog = unitTestSupport.addTimeColumn( + inertialUKFLog.times(), inertialUKFLog.omega_BN_BOut + ) + accuracy = 1.0e-3 unitTestSupport.writeTeXSnippet("toleranceValue44", str(accuracy), path) for i in range(3): - if(abs(omegaLog[-1, i+1] - stateInit[i+3]) > accuracy): - print(abs(omegaLog[-1, i+1] - stateInit[i+3])) + if abs(omegaLog[-1, i + 1] - stateInit[i + 3]) > accuracy: + print(abs(omegaLog[-1, i + 1] - stateInit[i + 3])) testFailCount += 1 testMessages.append("State omega propagation failure") - unitTestSupport.writeTeXSnippet('passFail44', textSnippetFailed, path) + unitTestSupport.writeTeXSnippet("passFail44", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail44', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail44", textSnippetPassed, path) for i in range(6): - if(covarLog[-1, i*6+i+1] <= covarLog[0, i*6+i+1]): - testFailCount += 1 - testMessages.append("State covariance failure") + if covarLog[-1, i * 6 + i + 1] <= covarLog[0, i * 6 + i + 1]: + testFailCount += 1 + testMessages.append("State covariance failure") # print out success message if no error were found if testFailCount == 0: @@ -732,11 +894,14 @@ def statePropRateInertialAttitude(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def BROKENtest_FaultScenarios(show_plots): [testResults, testMessage] = faultScenarios(show_plots) assert testResults < 1, testMessage + + def faultScenarios(show_plots): """Module Unit Test""" # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -761,42 +926,188 @@ def faultScenarios(show_plots): # Clean methods for Measurement and Time Updates moduleConfigClean1 = inertialUKF.InertialUKFConfig() moduleConfigClean1.numStates = 6 - moduleConfigClean1.state = [0., 0., 0., 0., 0., 0.] - moduleConfigClean1.statePrev = [0., 0., 0., 0., 0., 0.] - moduleConfigClean1.sBar = [0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0.] - moduleConfigClean1.sBarPrev = [1., 0., 0., 0., 0., 0., - 0., 1., 0., 0., 0., 0., - 0., 0., 1., 0., 0., 0., - 0., 0., 0., 1., 0., 0., - 0., 0., 0., 0., 1., 0., - 0., 0., 0., 0., 0., 1.] - moduleConfigClean1.covar = [0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0.] - moduleConfigClean1.covarPrev = [2., 0., 0., 0., 0., 0., - 0., 2., 0., 0., 0., 0., - 0., 0., 2., 0., 0., 0., - 0., 0., 0., 2., 0., 0., - 0., 0., 0., 0., 2., 0., - 0., 0., 0., 0., 0., 2.] + moduleConfigClean1.state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + moduleConfigClean1.statePrev = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + moduleConfigClean1.sBar = [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + moduleConfigClean1.sBarPrev = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + ] + moduleConfigClean1.covar = [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + moduleConfigClean1.covarPrev = [ + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + ] inertialUKF.inertialUKFCleanUpdate(moduleConfigClean1) - if numpy.linalg.norm(numpy.array(moduleConfigClean1.covarPrev) - numpy.array(moduleConfigClean1.covar)) > 1E10: + if ( + numpy.linalg.norm( + numpy.array(moduleConfigClean1.covarPrev) + - numpy.array(moduleConfigClean1.covar) + ) + > 1e10 + ): testFailCount += 1 testMessages.append("inertialUKFClean Covar failed") - if numpy.linalg.norm(numpy.array(moduleConfigClean1.statePrev) - numpy.array(moduleConfigClean1.state)) > 1E10: + if ( + numpy.linalg.norm( + numpy.array(moduleConfigClean1.statePrev) + - numpy.array(moduleConfigClean1.state) + ) + > 1e10 + ): testFailCount += 1 testMessages.append("inertialUKFClean States failed") - if numpy.linalg.norm(numpy.array(moduleConfigClean1.sBar) - numpy.array(moduleConfigClean1.sBarPrev)) > 1E10: + if ( + numpy.linalg.norm( + numpy.array(moduleConfigClean1.sBar) + - numpy.array(moduleConfigClean1.sBarPrev) + ) + > 1e10 + ): testFailCount += 1 testMessages.append("inertialUKFClean sBar failed") @@ -804,20 +1115,18 @@ def faultScenarios(show_plots): moduleConfigClean1.rwConfigParams.numRW = 2 moduleConfigClean1.rwSpeeds.wheelSpeeds = [10, 5] moduleConfigClean1.rwSpeedPrev.wheelSpeeds = [15, 10] - moduleConfigClean1.rwConfigParams.JsList = [1., 1.] - moduleConfigClean1.rwConfigParams.GsMatrix_B = [1., 0., 0., 1., 0., 0.] - moduleConfigClean1.speedDt = 1. - #moduleConfigClean1.IInv = [1., 0., 0., 0., 1., 0., 0., 0., 1.] + moduleConfigClean1.rwConfigParams.JsList = [1.0, 1.0] + moduleConfigClean1.rwConfigParams.GsMatrix_B = [1.0, 0.0, 0.0, 1.0, 0.0, 0.0] + moduleConfigClean1.speedDt = 1.0 + # moduleConfigClean1.IInv = [1., 0., 0., 0., 1., 0., 0., 0., 1.] # Bad Time and Measurement Update st1 = messaging.STAttMsgPayload() - st1.timeTag = macros.sec2nano(1.) + st1.timeTag = macros.sec2nano(1.0) st1.MRP_BdyInrtl = [0.1, 0.2, 0.3] ST1Data = inertialUKF.STMessage() - ST1Data.noise = [1., 0., 0., - 0., 1., 0., - 0., 0., 1.] + ST1Data.noise = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] STList = [ST1Data] @@ -863,7 +1172,7 @@ def faultScenarios(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": diff --git a/src/fswAlgorithms/attDetermination/headingSuKF/_UnitTest/headingSuKF_test_utilities.py b/src/fswAlgorithms/attDetermination/headingSuKF/_UnitTest/headingSuKF_test_utilities.py index 11671df327..51e3125830 100644 --- a/src/fswAlgorithms/attDetermination/headingSuKF/_UnitTest/headingSuKF_test_utilities.py +++ b/src/fswAlgorithms/attDetermination/headingSuKF/_UnitTest/headingSuKF_test_utilities.py @@ -23,116 +23,122 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('fswAlgorithms') - - +splitPath = path.split("fswAlgorithms") import matplotlib.pyplot as plt def StateCovarPlot(time, x, Pflat, string, show_plots): - numStates = len(x[0, :]) P = np.zeros([len(Pflat[:, 0]), numStates, numStates]) t = np.zeros(len(Pflat[:, 0])) for i in range(len(Pflat[:, 0])): - t[i] = time[i]*1E-9 - P[i, :, :] = Pflat[i, 0:(numStates*numStates+1)].reshape([numStates, numStates]) + t[i] = time[i] * 1e-9 + P[i, :, :] = Pflat[i, 0 : (numStates * numStates + 1)].reshape( + [numStates, numStates] + ) - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t , x[:, 0], "b", label='Error Filter') - plt.plot(t , x[:, 0]+3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t , x[:, 0]-3 * np.sqrt(P[:, 0, 0]), 'r--') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(t, x[:, 0], "b", label="Error Filter") + plt.plot(t, x[:, 0] + 3 * np.sqrt(P[:, 0, 0]), "r--", label="Covar Filter") + plt.plot(t, x[:, 0] - 3 * np.sqrt(P[:, 0, 0]), "r--") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() - plt.subplot(323) - plt.plot(t , x[:, 1], "b") - plt.plot(t , x[:, 1]+3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.plot(t , x[:, 1]-3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.title('Second LOS component') + plt.plot(t, x[:, 1], "b") + plt.plot(t, x[:, 1] + 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.plot(t, x[:, 1] - 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(t , x[:, 3], "b") - plt.plot(t , x[:, 3]+3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.plot(t , x[:, 3]-3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.title('Second rate component') + plt.plot(t, x[:, 3], "b") + plt.plot(t, x[:, 3] + 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.plot(t, x[:, 3] - 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(t , x[:, 2], "b") - plt.plot(t , x[:, 2]+3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.plot(t , x[:, 2]-3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(t, x[:, 2], "b") + plt.plot(t, x[:, 2] + 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.plot(t, x[:, 2] - 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(t , x[:, 4], "b") - plt.plot(t , x[:, 4]+3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.plot(t , x[:, 4]-3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(t, x[:, 4], "b") + plt.plot(t, x[:, 4] + 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.plot(t, x[:, 4] - 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesPlot' + string, 'State error and covariance', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesPlot" + string, + "State error and covariance", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() - def PostFitResiduals(time, Res, noise, string, show_plots): - - MeasNoise = np.zeros(len(Res[:,0])) - t= np.zeros(len(Res[:,0])) - numObs = len(Res[0,:]) - for i in range(len(Res[:,0])): - t[i] = time[i]*1E-9 - MeasNoise[i] = 3*noise + MeasNoise = np.zeros(len(Res[:, 0])) + t = np.zeros(len(Res[:, 0])) + numObs = len(Res[0, :]) + for i in range(len(Res[:, 0])): + t[i] = time[i] * 1e-9 + MeasNoise[i] = 3 * noise # Don't plot zero values, since they mean that no measurement is taken - for j in range(len(Res[0,:])-1): - if -1E-10 < Res[i,j+1] < 1E-10: - Res[i, j+1] = np.nan + for j in range(len(Res[0, :]) - 1): + if -1e-10 < Res[i, j + 1] < 1e-10: + Res[i, j + 1] = np.nan - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(t , Res[:, 0], "b.", label='Residual') - plt.plot(t , MeasNoise, 'r--', label='Covar') - plt.plot(t , -MeasNoise, 'r--') - plt.legend(loc='lower right') - if noise >1E-5: - plt.ylim([-10*noise, 10*noise]) - plt.title('First Position Component') + plt.plot(t, Res[:, 0], "b.", label="Residual") + plt.plot(t, MeasNoise, "r--", label="Covar") + plt.plot(t, -MeasNoise, "r--") + plt.legend(loc="lower right") + if noise > 1e-5: + plt.ylim([-10 * noise, 10 * noise]) + plt.title("First Position Component") plt.grid() plt.subplot(312) - plt.plot(t , Res[:, 1], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - if noise >1E-5: - plt.ylim([-10*noise, 10*noise]) - plt.title('Second Position Component') + plt.plot(t, Res[:, 1], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + if noise > 1e-5: + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Second Position Component") plt.grid() plt.subplot(313) - plt.plot(t , Res[:, 2], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - if noise > 1E-5: + plt.plot(t, Res[:, 2], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + if noise > 1e-5: plt.ylim([-10 * noise, 10 * noise]) - plt.title('Third Position Component') + plt.title("Third Position Component") plt.grid() - - - unitTestSupport.writeFigureLaTeX('PostFit' +string , 'Post Fit Residuals', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "PostFit" + string, + "Post Fit Residuals", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() diff --git a/src/fswAlgorithms/attDetermination/headingSuKF/_UnitTest/test_headingSuKF.py b/src/fswAlgorithms/attDetermination/headingSuKF/_UnitTest/test_headingSuKF.py index d65db33b86..781e3317d2 100644 --- a/src/fswAlgorithms/attDetermination/headingSuKF/_UnitTest/test_headingSuKF.py +++ b/src/fswAlgorithms/attDetermination/headingSuKF/_UnitTest/test_headingSuKF.py @@ -1,5 +1,6 @@ -''' ''' -''' +""" """ + +""" ISC License Copyright (c) 2016-2018, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,7 +17,7 @@ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. -''' +""" import math import matplotlib.pyplot as plt @@ -29,24 +30,46 @@ def setupFilterData(filterObject): - filterObject.alpha = 0.02 filterObject.beta = 2.0 filterObject.kappa = 0.0 # filterObject.state = [0.0, 0., 0., 0., 0.] filterObject.stateInit = [0.0, 0.0, 1.0, 0.0, 0.0] - filterObject.covarInit = [0.1, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.1, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.1, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.01] + filterObject.covarInit = [ + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + ] qNoiseIn = np.identity(5) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3]*0.01*0.01 - qNoiseIn[3:5, 3:5] = qNoiseIn[3:5, 3:5]*0.001*0.001 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 0.01 * 0.01 + qNoiseIn[3:5, 3:5] = qNoiseIn[3:5, 3:5] * 0.001 * 0.001 filterObject.qNoise = qNoiseIn.reshape(25).tolist() + def test_functions_ukf(show_plots): """ **Validation Test Description** @@ -67,12 +90,14 @@ def test_functions_ukf(show_plots): [testResults, testMessage] = heading_utilities_test(show_plots) assert testResults < 1, testMessage + # uncomment this line is 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() # need to update how the RW states are defined # provide a unique test method name, starting with test_ + def test_all_heading_kf(show_plots): """ **Validation Test Description** @@ -99,6 +124,7 @@ def test_all_heading_kf(show_plots): [testResults, testMessage] = StateUpdateSunLine(show_plots) assert testResults < 1, testMessage + def heading_utilities_test(show_plots): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the @@ -109,12 +135,32 @@ def heading_utilities_test(show_plots): testMessages = [] # create empty list to store test log messages # Initialize the test module configuration data - AMatrix = [0.488894, 0.888396, 0.325191, 0.319207, - 1.03469, -1.14707, -0.754928, 0.312859, - 0.726885, -1.06887, 1.3703, -0.86488, - -0.303441, -0.809499, -1.71152, -0.0300513, - 0.293871, -2.94428, -0.102242, -0.164879, - -0.787283, 1.43838, -0.241447, 0.627707] + AMatrix = [ + 0.488894, + 0.888396, + 0.325191, + 0.319207, + 1.03469, + -1.14707, + -0.754928, + 0.312859, + 0.726885, + -1.06887, + 1.3703, + -0.86488, + -0.303441, + -0.809499, + -1.71152, + -0.0300513, + 0.293871, + -2.94428, + -0.102242, + -0.164879, + -0.787283, + 1.43838, + -0.241447, + 0.627707, + ] RVector = headingSuKF.new_doubleArray(len(AMatrix)) AVector = headingSuKF.new_doubleArray(len(AMatrix)) @@ -124,23 +170,40 @@ def heading_utilities_test(show_plots): headingSuKF.ukfQRDJustR(AVector, 6, 4, RVector) RMatrix = [] - for i in range(4*4): + for i in range(4 * 4): RMatrix.append(headingSuKF.doubleArray_getitem(RVector, i)) - RBaseNumpy = np.array(RMatrix).reshape(4,4) - AMatNumpy = np.array(AMatrix).reshape(6,4) - q,r = np.linalg.qr(AMatNumpy) + RBaseNumpy = np.array(RMatrix).reshape(4, 4) + AMatNumpy = np.array(AMatrix).reshape(6, 4) + q, r = np.linalg.qr(AMatNumpy) for i in range(r.shape[0]): - if r[i,i] < 0.0: - r[i,:] *= -1.0 - if np.linalg.norm(r - RBaseNumpy) > 1.0E-15: + if r[i, i] < 0.0: + r[i, :] *= -1.0 + if np.linalg.norm(r - RBaseNumpy) > 1.0e-15: testFailCount += 1 testMessages.append("QR Decomposition accuracy failure") - AMatrix = [1.09327, 1.10927, -0.863653, 1.32288, - -1.21412, -1.1135, -0.00684933, -2.43508, - -0.769666, 0.371379, -0.225584, -1.76492, - -1.08906, 0.0325575, 0.552527, -1.6256, - 1.54421, 0.0859311, -1.49159, 1.59683] + AMatrix = [ + 1.09327, + 1.10927, + -0.863653, + 1.32288, + -1.21412, + -1.1135, + -0.00684933, + -2.43508, + -0.769666, + 0.371379, + -0.225584, + -1.76492, + -1.08906, + 0.0325575, + 0.552527, + -1.6256, + 1.54421, + 0.0859311, + -1.49159, + 1.59683, + ] RVector = headingSuKF.new_doubleArray(len(AMatrix)) AVector = headingSuKF.new_doubleArray(len(AMatrix)) @@ -150,24 +213,19 @@ def heading_utilities_test(show_plots): headingSuKF.ukfQRDJustR(AVector, 5, 4, RVector) RMatrix = [] - for i in range(4*4): + for i in range(4 * 4): RMatrix.append(headingSuKF.doubleArray_getitem(RVector, i)) - RBaseNumpy = np.array(RMatrix).reshape(4,4) - AMatNumpy = np.array(AMatrix).reshape(5,4) - q,r = np.linalg.qr(AMatNumpy) + RBaseNumpy = np.array(RMatrix).reshape(4, 4) + AMatNumpy = np.array(AMatrix).reshape(5, 4) + q, r = np.linalg.qr(AMatNumpy) for i in range(r.shape[0]): - if r[i,i] < 0.0: - r[i,:] *= -1.0 - if np.linalg.norm(r - RBaseNumpy) > 1.0E-14: + if r[i, i] < 0.0: + r[i, :] *= -1.0 + if np.linalg.norm(r - RBaseNumpy) > 1.0e-14: testFailCount += 1 testMessages.append("QR Decomposition accuracy failure") - AMatrix = [ 0.2236, 0, - 0, 0.2236, - -0.2236, 0, - 0, -0.2236, - 0.0170, 0, - 0, 0.0170] + AMatrix = [0.2236, 0, 0, 0.2236, -0.2236, 0, 0, -0.2236, 0.0170, 0, 0, 0.0170] RVector = headingSuKF.new_doubleArray(len(AMatrix)) AVector = headingSuKF.new_doubleArray(len(AMatrix)) @@ -177,21 +235,20 @@ def heading_utilities_test(show_plots): headingSuKF.ukfQRDJustR(AVector, 6, 2, RVector) RMatrix = [] - for i in range(2*2): + for i in range(2 * 2): RMatrix.append(headingSuKF.doubleArray_getitem(RVector, i)) - RBaseNumpy = np.array(RMatrix).reshape(2,2) - AMatNumpy = np.array(AMatrix).reshape(6,2) - q,r = np.linalg.qr(AMatNumpy) + RBaseNumpy = np.array(RMatrix).reshape(2, 2) + AMatNumpy = np.array(AMatrix).reshape(6, 2) + q, r = np.linalg.qr(AMatNumpy) for i in range(r.shape[0]): - if r[i,i] < 0.0: - r[i,:] *= -1.0 + if r[i, i] < 0.0: + r[i, :] *= -1.0 - if np.linalg.norm(r - RBaseNumpy) > 1.0E-15: + if np.linalg.norm(r - RBaseNumpy) > 1.0e-15: testFailCount += 1 testMessages.append("QR Decomposition accuracy failure") - - LUSourceMat = [8,1,6,3,5,7,4,9,2] + LUSourceMat = [8, 1, 6, 3, 5, 7, 4, 9, 2] LUSVector = headingSuKF.new_doubleArray(len(LUSourceMat)) LVector = headingSuKF.new_doubleArray(len(LUSourceMat)) UVector = headingSuKF.new_doubleArray(len(LUSourceMat)) @@ -203,65 +260,66 @@ def heading_utilities_test(show_plots): headingSuKF.doubleArray_setitem(LVector, i, 0.0) exCount = headingSuKF.ukfLUD(LUSVector, 3, 3, LVector, intSwapVector) - #headingSuKF.ukfUInv(LUSVector, 3, 3, UVector) + # headingSuKF.ukfUInv(LUSVector, 3, 3, UVector) LMatrix = [] UMatrix = [] - #UMatrix = [] + # UMatrix = [] for i in range(3): currRow = headingSuKF.intArray_getitem(intSwapVector, i) for j in range(3): - if(ji): + elif j > i: LMatrix.append(0.0) - UMatrix.append(headingSuKF.doubleArray_getitem(LVector, i*3+j)) + UMatrix.append(headingSuKF.doubleArray_getitem(LVector, i * 3 + j)) else: LMatrix.append(1.0) - UMatrix.append(headingSuKF.doubleArray_getitem(LVector, i*3+j)) + UMatrix.append(headingSuKF.doubleArray_getitem(LVector, i * 3 + j)) # UMatrix.append(headingSuKF.doubleArray_getitem(UVector, i)) - LMatrix = np.array(LMatrix).reshape(3,3) - UMatrix = np.array(UMatrix).reshape(3,3) + LMatrix = np.array(LMatrix).reshape(3, 3) + UMatrix = np.array(UMatrix).reshape(3, 3) outMat = np.dot(LMatrix, UMatrix) - outMatSwap = np.zeros((3,3)) + outMatSwap = np.zeros((3, 3)) for i in range(3): currRow = headingSuKF.intArray_getitem(intSwapVector, i) - outMatSwap[i,:] = outMat[currRow, :] - outMat[currRow,:] = outMat[i, :] - LuSourceArray = np.array(LUSourceMat).reshape(3,3) + outMatSwap[i, :] = outMat[currRow, :] + outMat[currRow, :] = outMat[i, :] + LuSourceArray = np.array(LUSourceMat).reshape(3, 3) - if(np.linalg.norm(outMatSwap - LuSourceArray) > 1.0E-14): + if np.linalg.norm(outMatSwap - LuSourceArray) > 1.0e-14: testFailCount += 1 testMessages.append("LU Decomposition accuracy failure") EqnSourceMat = [2.0, 1.0, 3.0, 2.0, 6.0, 8.0, 6.0, 8.0, 18.0] BVector = [1.0, 3.0, 5.0] EqnVector = headingSuKF.new_doubleArray(len(EqnSourceMat)) - EqnBVector = headingSuKF.new_doubleArray(len(LUSourceMat)//3) - EqnOutVector = headingSuKF.new_doubleArray(len(LUSourceMat)//3) + EqnBVector = headingSuKF.new_doubleArray(len(LUSourceMat) // 3) + EqnOutVector = headingSuKF.new_doubleArray(len(LUSourceMat) // 3) for i in range(len(EqnSourceMat)): headingSuKF.doubleArray_setitem(EqnVector, i, EqnSourceMat[i]) - headingSuKF.doubleArray_setitem(EqnBVector, i//3, BVector[i//3]) - headingSuKF.intArray_setitem(intSwapVector, i//3, 0) + headingSuKF.doubleArray_setitem(EqnBVector, i // 3, BVector[i // 3]) + headingSuKF.intArray_setitem(intSwapVector, i // 3, 0) headingSuKF.doubleArray_setitem(LVector, i, 0.0) exCount = headingSuKF.ukfLUD(EqnVector, 3, 3, LVector, intSwapVector) headingSuKF.ukfLUBckSlv(LVector, 3, 3, intSwapVector, EqnBVector, EqnOutVector) - expectedSol = [3.0/10.0, 4.0/10.0, 0.0] + expectedSol = [3.0 / 10.0, 4.0 / 10.0, 0.0] errorVal = 0.0 for i in range(3): - errorVal += abs(headingSuKF.doubleArray_getitem(EqnOutVector, i) -expectedSol[i]) + errorVal += abs( + headingSuKF.doubleArray_getitem(EqnOutVector, i) - expectedSol[i] + ) - if(errorVal > 1.0E-14): + if errorVal > 1.0e-14: testFailCount += 1 testMessages.append("LU Back-Solve accuracy failure") - - InvSourceMat = [8,1,6,3,5,7,4,9,2] + InvSourceMat = [8, 1, 6, 3, 5, 7, 4, 9, 2] SourceVector = headingSuKF.new_doubleArray(len(InvSourceMat)) InvVector = headingSuKF.new_doubleArray(len(InvSourceMat)) for i in range(len(InvSourceMat)): @@ -275,13 +333,12 @@ def heading_utilities_test(show_plots): InvOut.append(headingSuKF.doubleArray_getitem(InvVector, i)) InvOut = np.array(InvOut).reshape(nRow, nRow) - expectIdent = np.dot(InvOut, np.array(InvSourceMat).reshape(3,3)) + expectIdent = np.dot(InvOut, np.array(InvSourceMat).reshape(3, 3)) errorNorm = np.linalg.norm(expectIdent - np.identity(3)) - if(errorNorm > 1.0E-14): + if errorNorm > 1.0e-14: testFailCount += 1 testMessages.append("LU Matrix Inverse accuracy failure") - cholTestMat = [1.0, 0.0, 0.0, 0.0, 10.0, 5.0, 0.0, 5.0, 10.0] SourceVector = headingSuKF.new_doubleArray(len(cholTestMat)) CholVector = headingSuKF.new_doubleArray(len(cholTestMat)) @@ -297,15 +354,28 @@ def heading_utilities_test(show_plots): cholOut = np.array(cholOut).reshape(nRow, nRow) cholComp = np.linalg.cholesky(np.array(cholTestMat).reshape(nRow, nRow)) errorNorm = np.linalg.norm(cholOut - cholComp) - if(errorNorm > 1.0E-14): + if errorNorm > 1.0e-14: testFailCount += 1 testMessages.append("Cholesky Matrix Decomposition accuracy failure") - - InvSourceMat = [2.1950926119414667, 0.0, 0.0, 0.0, - 1.0974804773131115, 1.9010439702743847, 0.0, 0.0, - 0.0, 1.2672359635912551, 1.7923572711881284, 0.0, - 1.0974804773131113, -0.63357997864171967, 1.7920348101787789, 0.033997451205364251] + InvSourceMat = [ + 2.1950926119414667, + 0.0, + 0.0, + 0.0, + 1.0974804773131115, + 1.9010439702743847, + 0.0, + 0.0, + 0.0, + 1.2672359635912551, + 1.7923572711881284, + 0.0, + 1.0974804773131113, + -0.63357997864171967, + 1.7920348101787789, + 0.033997451205364251, + ] SourceVector = headingSuKF.new_doubleArray(len(InvSourceMat)) InvVector = headingSuKF.new_doubleArray(len(InvSourceMat)) @@ -320,14 +390,18 @@ def heading_utilities_test(show_plots): InvOut.append(headingSuKF.doubleArray_getitem(InvVector, i)) InvOut = np.array(InvOut).reshape(nRow, nRow) - expectIdent = np.dot(InvOut, np.array(InvSourceMat).reshape(nRow,nRow)) + expectIdent = np.dot(InvOut, np.array(InvSourceMat).reshape(nRow, nRow)) errorNorm = np.linalg.norm(expectIdent - np.identity(nRow)) - if(errorNorm > 1.0E-12): + if errorNorm > 1.0e-12: print(errorNorm) testFailCount += 1 testMessages.append("L Matrix Inverse accuracy failure") - InvSourceMat = np.transpose(np.array(InvSourceMat).reshape(nRow, nRow)).reshape(nRow*nRow).tolist() + InvSourceMat = ( + np.transpose(np.array(InvSourceMat).reshape(nRow, nRow)) + .reshape(nRow * nRow) + .tolist() + ) SourceVector = headingSuKF.new_doubleArray(len(InvSourceMat)) InvVector = headingSuKF.new_doubleArray(len(InvSourceMat)) for i in range(len(InvSourceMat)): @@ -341,14 +415,13 @@ def heading_utilities_test(show_plots): InvOut.append(headingSuKF.doubleArray_getitem(InvVector, i)) InvOut = np.array(InvOut).reshape(nRow, nRow) - expectIdent = np.dot(InvOut, np.array(InvSourceMat).reshape(nRow,nRow)) + expectIdent = np.dot(InvOut, np.array(InvSourceMat).reshape(nRow, nRow)) errorNorm = np.linalg.norm(expectIdent - np.identity(nRow)) - if(errorNorm > 1.0E-12): + if errorNorm > 1.0e-12: print(errorNorm) testFailCount += 1 testMessages.append("U Matrix Inverse accuracy failure") - # If the argument provided at commandline "--show_plots" evaluates as true, # plot all figures if show_plots: @@ -360,10 +433,10 @@ def heading_utilities_test(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] -def StateUpdateSunLine(show_plots): +def StateUpdateSunLine(show_plots): __tracebackhide__ = True testFailCount = 0 # zero unit test result counter @@ -400,7 +473,7 @@ def StateUpdateSunLine(show_plots): stateTarget = testVector.tolist() inputData.r_BN_B = stateTarget stateTarget.extend([0.0, 0.0]) - module.stateInit = [1., 0.2, 0.1, 0.01, 0.001] + module.stateInit = [1.0, 0.2, 0.1, 0.01, 0.001] # setup message connections module.opnavDataInMsg.subscribeTo(opnavDataInMsg) @@ -408,28 +481,28 @@ def StateUpdateSunLine(show_plots): unitTestSim.InitializeSimulation() t1 = 1000 for i in range(t1): - if i > 0 and i%20 == 0: + if i > 0 and i % 20 == 0: inputData.timeTag = macros.sec2nano(i * 0.5) inputData.valid = 1 inputData.r_BN_B += np.random.normal(0, 0.001, 3) inputData.covar_B = [0.0001**2, 0, 0, 0, 0.0001**2, 0, 0, 0, 0.0001**2] opnavDataInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i+1)*0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * 0.5)) unitTestSim.ExecuteSimulation() stateLog = dataLog.state postFitLog = dataLog.postFitRes covarLog = dataLog.covar - stateTarget[:3] = (-testVector[:3]/np.linalg.norm(testVector[:3])).tolist() + stateTarget[:3] = (-testVector[:3] / np.linalg.norm(testVector[:3])).tolist() for i in range(5): # check covariance immediately after measurement is taken, # ensure order of magnitude less than initial covariance. - if(np.abs(covarLog[t1-10, i*5+i] - covarLog[0, i*5+i]/10) > 1E-1): + if np.abs(covarLog[t1 - 10, i * 5 + i] - covarLog[0, i * 5 + i] / 10) > 1e-1: testFailCount += 1 testMessages.append("Covariance update failure") - if(abs(stateLog[-1, i] - stateTarget[i-1]) > 1.0E-1): - print(abs(stateLog[-1, i] - stateTarget[i-1])) + if abs(stateLog[-1, i] - stateTarget[i - 1]) > 1.0e-1: + print(abs(stateLog[-1, i] - stateTarget[i - 1])) testFailCount += 1 testMessages.append("State update failure") @@ -439,33 +512,42 @@ def StateUpdateSunLine(show_plots): stateTarget.extend([0.0, 0.0]) for i in range(t1): - if i%20 == 0: - inputData.timeTag = macros.sec2nano(i*0.5 +t1 +1) + if i % 20 == 0: + inputData.timeTag = macros.sec2nano(i * 0.5 + t1 + 1) inputData.r_BN_B += np.random.normal(0, 0.001, 3) inputData.valid = 1 - inputData.covar_B = [0.0001**2,0,0,0,0.0001**2,0,0,0,0.0001**2] + inputData.covar_B = [0.0001**2, 0, 0, 0, 0.0001**2, 0, 0, 0, 0.0001**2] opnavDataInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i+t1 +1)*0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + t1 + 1) * 0.5)) unitTestSim.ExecuteSimulation() stateLog = dataLog.state stateErrorLog = dataLog.stateError postFitLog = dataLog.postFitRes covarLog = dataLog.covar - stateTarget[:3] = (-testVector[:3]/np.linalg.norm(testVector[:3])).tolist() + stateTarget[:3] = (-testVector[:3] / np.linalg.norm(testVector[:3])).tolist() for i in range(5): - if(np.abs(covarLog[2*t1-10, i*5+i] - covarLog[0, i*5+i]/10)>1E-1): + if ( + np.abs(covarLog[2 * t1 - 10, i * 5 + i] - covarLog[0, i * 5 + i] / 10) + > 1e-1 + ): testFailCount += 1 testMessages.append("Covariance update failure") - if(abs(stateLog[-1, i] - stateTarget[i-1]) > 1.0E-1): - print(abs(stateLog[-1, i] - stateTarget[i-1])) + if abs(stateLog[-1, i] - stateTarget[i - 1]) > 1.0e-1: + print(abs(stateLog[-1, i] - stateTarget[i - 1])) testFailCount += 1 testMessages.append("State update failure") - FilterPlots.StateCovarPlot(dataLog.times(), stateLog, covarLog, 'Update', show_plots) - FilterPlots.StateCovarPlot(dataLog.times(),stateErrorLog, covarLog, 'Update_Error', show_plots) - FilterPlots.PostFitResiduals(dataLog.times(), postFitLog, 0.001, 'Update', show_plots) + FilterPlots.StateCovarPlot( + dataLog.times(), stateLog, covarLog, "Update", show_plots + ) + FilterPlots.StateCovarPlot( + dataLog.times(), stateErrorLog, covarLog, "Update_Error", show_plots + ) + FilterPlots.PostFitResiduals( + dataLog.times(), postFitLog, 0.001, "Update", show_plots + ) # print out success message if no error were found if testFailCount == 0: @@ -473,7 +555,8 @@ def StateUpdateSunLine(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def StatePropSunLine(show_plots): __tracebackhide__ = True @@ -518,24 +601,25 @@ def StatePropSunLine(show_plots): postFitLog = dataLog.postFitRes covarLog = dataLog.covar - FilterPlots.StateCovarPlot(dataLog.times(), stateLog, covarLog, 'Prop', show_plots) - FilterPlots.PostFitResiduals(dataLog.times(), postFitLog, module.qObsVal, 'Prop', show_plots) + FilterPlots.StateCovarPlot(dataLog.times(), stateLog, covarLog, "Prop", show_plots) + FilterPlots.PostFitResiduals( + dataLog.times(), postFitLog, module.qObsVal, "Prop", show_plots + ) for i in range(5): - if(abs(stateLog[-1, i] - stateLog[0, i]) > 1.0E-10): + if abs(stateLog[-1, i] - stateLog[0, i]) > 1.0e-10: print(abs(stateLog[-1, i] - stateLog[0, i])) testFailCount += 1 testMessages.append("State propagation failure") - - # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag + " state propagation") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": # test_all_heading_kf(True) diff --git a/src/fswAlgorithms/attDetermination/okeefeEKF/_UnitTest/SunLineOEKF_test_utilities.py b/src/fswAlgorithms/attDetermination/okeefeEKF/_UnitTest/SunLineOEKF_test_utilities.py index 415d887906..0e150b76b9 100644 --- a/src/fswAlgorithms/attDetermination/okeefeEKF/_UnitTest/SunLineOEKF_test_utilities.py +++ b/src/fswAlgorithms/attDetermination/okeefeEKF/_UnitTest/SunLineOEKF_test_utilities.py @@ -20,210 +20,230 @@ import matplotlib.pyplot as plt import numpy as np -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) def StatesPlot(time, x, Pflat, show_plots): + P = np.zeros([len(Pflat[:, 0]), 3, 3]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = time[i] * 1e-9 + P[i, :, :] = Pflat[i, 0 : 3 * 3].reshape([3, 3]) - P = np.zeros([len(Pflat[:,0]),3,3]) - t = np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = time[i]*1E-9 - P[i, :, :] = Pflat[i, 0:3*3].reshape([3, 3]) - - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(t , x[:, 1], "b", label='Error Filter') - plt.plot(t , 3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t , -3 * np.sqrt(P[:, 0, 0]), 'r--') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.plot(t, 3 * np.sqrt(P[:, 0, 0]), "r--", label="Covar Filter") + plt.plot(t, -3 * np.sqrt(P[:, 0, 0]), "r--") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(312) - plt.plot(t , x[:, 2], "b") - plt.plot(t , 3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.title('First rate component') + plt.plot(t, x[:, 2], "b") + plt.plot(t, 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 1, 1]), "r--") + plt.title("First rate component") plt.grid() plt.subplot(313) - plt.plot(t , x[:, 3], "b") - plt.plot(t , 3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.title('Second LOS component') + plt.plot(t, x[:, 3], "b") + plt.plot(t, 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 2, 2]), "r--") + plt.title("Second LOS component") plt.grid() - - - unitTestSupport.writeFigureLaTeX('StatesPlot', 'State error and covariance', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesPlot", + "State error and covariance", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() - plt.close('all') + plt.close("all") def StatesPlotCompare(x, x2, Pflat, Pflat2, show_plots): - - - P = np.zeros([len(Pflat[:,0]),3,3]) - P2 = np.zeros([len(Pflat[:,0]),3,3]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*1E-9 - P[i,:,:] = Pflat[i,1:3*3+1].reshape([3,3]) - P2[i, :, :] = Pflat2[i, 1:3*3+1].reshape([3, 3]) - - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + P = np.zeros([len(Pflat[:, 0]), 3, 3]) + P2 = np.zeros([len(Pflat[:, 0]), 3, 3]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * 1e-9 + P[i, :, :] = Pflat[i, 1 : 3 * 3 + 1].reshape([3, 3]) + P2[i, :, :] = Pflat2[i, 1 : 3 * 3 + 1].reshape([3, 3]) + + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(t[0:30] , x[0:30, 1], "b", label='Error Filter') - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 0, 0]), 'r--') - plt.plot(t[0:30] , x2[0:30, 1], "g", label='Error Expected') - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 0, 0]), 'c--', label='Covar Expected') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 0, 0]), 'c--') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(t[0:30], x[0:30, 1], "b", label="Error Filter") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 0, 0]), "r--", label="Covar Filter") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 0, 0]), "r--") + plt.plot(t[0:30], x2[0:30, 1], "g", label="Error Expected") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 0, 0]), "c--", label="Covar Expected") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 0, 0]), "c--") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(312) - plt.plot(t[0:30] , x[0:30, 2], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 1, 1]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 1, 1]), 'r--') - plt.plot(t[0:30] , x2[0:30, 2], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.title('First rate component') + plt.plot(t[0:30], x[0:30, 2], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 1, 1]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 1, 1]), "r--") + plt.plot(t[0:30], x2[0:30, 2], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.title("First rate component") plt.grid() plt.subplot(313) - plt.plot(t[0:30] , x[0:30, 3], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 2, 2]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 2, 2]), 'r--') - plt.plot(t[0:30] , x2[0:30, 3], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.title('Second LOS component') + plt.plot(t[0:30], x[0:30, 3], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 2, 2]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 2, 2]), "r--") + plt.plot(t[0:30], x2[0:30, 3], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.title("Second LOS component") plt.grid() - - - unitTestSupport.writeFigureLaTeX('StatesCompare', 'State error and covariance vs expected Values', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesCompare", + "State error and covariance vs expected Values", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() -def PostFitResiduals(time, Res, noise, show_plots): - MeasNoise = np.zeros(len(Res[:,0])) - t= np.zeros(len(Res[:,0])) - for i in range(len(Res[:,0])): - t[i] = time[i]*1E-9 - MeasNoise[i] = 3*noise +def PostFitResiduals(time, Res, noise, show_plots): + MeasNoise = np.zeros(len(Res[:, 0])) + t = np.zeros(len(Res[:, 0])) + for i in range(len(Res[:, 0])): + t[i] = time[i] * 1e-9 + MeasNoise[i] = 3 * noise # Don't plot zero values, since they mean that no measurement is taken - for j in range(len(Res[0,:])-1): - if -1E-10 < Res[i,j+1] < 1E-10: - Res[i, j+1] = np.nan + for j in range(len(Res[0, :]) - 1): + if -1e-10 < Res[i, j + 1] < 1e-10: + Res[i, j + 1] = np.nan - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(421) - plt.plot(t , Res[:, 0], "b.", label='Residual') - plt.plot(t , MeasNoise, 'r--', label='Covar') - plt.plot(t , -MeasNoise, 'r--') - plt.legend(loc='lower right') - plt.ylim([-10*noise, 10*noise]) - plt.title('First CSS') + plt.plot(t, Res[:, 0], "b.", label="Residual") + plt.plot(t, MeasNoise, "r--", label="Covar") + plt.plot(t, -MeasNoise, "r--") + plt.legend(loc="lower right") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("First CSS") plt.grid() plt.subplot(422) - plt.plot(t , Res[:, 4], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Fifth CSS') + plt.plot(t, Res[:, 4], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Fifth CSS") plt.grid() plt.subplot(423) - plt.plot(t , Res[:, 1], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Second CSS') + plt.plot(t, Res[:, 1], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Second CSS") plt.grid() plt.subplot(424) - plt.plot(t , Res[:, 5], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Sixth CSS') + plt.plot(t, Res[:, 5], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Sixth CSS") plt.grid() plt.subplot(425) - plt.plot(t , Res[:, 2], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Third CSS') + plt.plot(t, Res[:, 2], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Third CSS") plt.grid() plt.subplot(426) - plt.plot(t , Res[:, 6], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Seventh CSS') + plt.plot(t, Res[:, 6], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Seventh CSS") plt.grid() plt.subplot(427) - plt.plot(t , Res[:, 3], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.xlabel('t(s)') - plt.title('Fourth CSS') + plt.plot(t, Res[:, 3], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.xlabel("t(s)") + plt.title("Fourth CSS") plt.grid() plt.subplot(428) - plt.plot(t , Res[:, 7], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.xlabel('t(s)') - plt.title('Eight CSS') + plt.plot(t, Res[:, 7], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.xlabel("t(s)") + plt.title("Eight CSS") plt.grid() - unitTestSupport.writeFigureLaTeX('PostFit', 'Post Fit Residuals', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "PostFit", + "Post Fit Residuals", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() + def StatesVsExpected(stateLog, expectedStateArray, show_plots): - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 1], 'b--', label='Expected') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1], 'r', label='Filter') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 1], "b--", label="Expected") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1], "r", label="Filter") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(312) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 2], 'b--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2], 'r') - plt.title('First rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 2], "b--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2], "r") + plt.title("First rate component") plt.grid() plt.subplot(313) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 3], 'b--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3], 'r') - plt.title('Second LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 3], "b--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3], "r") + plt.title("Second LOS component") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesExpected', 'States vs true states in static case', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesExpected", + "States vs true states in static case", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() @@ -231,66 +251,72 @@ def StatesVsExpected(stateLog, expectedStateArray, show_plots): def StatesVsTargets(time, target1, target2, stateLog, show_plots): - - - target = np.ones([len(stateLog[:, 0]),3]) - for i in range((len(stateLog[:, 0])-1)//2): + target = np.ones([len(stateLog[:, 0]), 3]) + for i in range((len(stateLog[:, 0]) - 1) // 2): target[i, :] = target1 - target[i+(len(stateLog[:, 0]) - 1) // 2,:] = target2 + target[i + (len(stateLog[:, 0]) - 1) // 2, :] = target2 - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(time[:] * 1.0E-9, stateLog[:, 0], 'b', label='Filter') - plt.plot(time[:] * 1.0E-9, target[:, 0], 'r--', label='Expected') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(time[:] * 1.0e-9, stateLog[:, 0], "b", label="Filter") + plt.plot(time[:] * 1.0e-9, target[:, 0], "r--", label="Expected") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(312) - plt.plot(time[:] * 1.0E-9, stateLog[:, 1], 'b') - plt.plot(time[:] * 1.0E-9, target[:, 1], 'r--') - plt.title('First rate component') + plt.plot(time[:] * 1.0e-9, stateLog[:, 1], "b") + plt.plot(time[:] * 1.0e-9, target[:, 1], "r--") + plt.title("First rate component") plt.grid() plt.subplot(313) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 2], 'r--') - plt.title('Second LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 2], "r--") + plt.title("Second LOS component") plt.grid() - - - unitTestSupport.writeFigureLaTeX('StatesTarget', 'States tracking target values', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesTarget", + "States tracking target values", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() -def OmegaVsExpected(omegaExp, omegaSim, show_plots): - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') +def OmegaVsExpected(omegaExp, omegaSim, show_plots): + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(omegaSim[:, 0] * 1.0E-9, omegaSim[:, 1], 'b', label='Filter') - plt.plot(omegaSim[:, 0] * 1.0E-9, omegaExp[:, 1], 'r--', label='Expected') - plt.legend(loc='lower right') - plt.title('First omega component') + plt.plot(omegaSim[:, 0] * 1.0e-9, omegaSim[:, 1], "b", label="Filter") + plt.plot(omegaSim[:, 0] * 1.0e-9, omegaExp[:, 1], "r--", label="Expected") + plt.legend(loc="lower right") + plt.title("First omega component") plt.grid() plt.subplot(312) - plt.plot(omegaSim[:, 0] * 1.0E-9, omegaSim[:, 2], 'b') - plt.plot(omegaSim[:, 0] * 1.0E-9, omegaExp[:, 2], 'r--') - plt.title('Second omega component') + plt.plot(omegaSim[:, 0] * 1.0e-9, omegaSim[:, 2], "b") + plt.plot(omegaSim[:, 0] * 1.0e-9, omegaExp[:, 2], "r--") + plt.title("Second omega component") plt.grid() plt.subplot(313) - plt.plot(omegaSim[:, 0] * 1.0E-9, omegaSim[:, 3], 'b') - plt.plot(omegaSim[:, 0] * 1.0E-9, omegaExp[:, 3], 'r--') - plt.title('Third omega component') + plt.plot(omegaSim[:, 0] * 1.0e-9, omegaSim[:, 3], "b") + plt.plot(omegaSim[:, 0] * 1.0e-9, omegaExp[:, 3], "r--") + plt.title("Third omega component") plt.grid() - - - unitTestSupport.writeFigureLaTeX('StatesTarget', 'States tracking target values', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesTarget", + "States tracking target values", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() diff --git a/src/fswAlgorithms/attDetermination/okeefeEKF/_UnitTest/test_okeefeEKF.py b/src/fswAlgorithms/attDetermination/okeefeEKF/_UnitTest/test_okeefeEKF.py index 4b162d4dd4..a5d7710b27 100644 --- a/src/fswAlgorithms/attDetermination/okeefeEKF/_UnitTest/test_okeefeEKF.py +++ b/src/fswAlgorithms/attDetermination/okeefeEKF/_UnitTest/test_okeefeEKF.py @@ -24,30 +24,33 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('FswAlgorithms') -sys.path.append(splitPath[0] + '/modules') -sys.path.append(splitPath[0] + '/PythonModules') +splitPath = path.split("FswAlgorithms") +sys.path.append(splitPath[0] + "/modules") +sys.path.append(splitPath[0] + "/PythonModules") import SunLineOEKF_test_utilities as FilterPlots from Basilisk.fswAlgorithms import okeefeEKF from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.architecture import messaging def setupFilterData(filterObject): - filterObject.sensorUseThresh = 0. + filterObject.sensorUseThresh = 0.0 filterObject.state = [1.0, 1.0, 1.0] filterObject.omega = [0.1, 0.2, 0.1] filterObject.x = [1.0, 0.0, 1.0] - filterObject.covar = [0.4, 0.0, 0.0, - 0.0, 0.4, 0.0, - 0.0, 0.0, 0.4] + filterObject.covar = [0.4, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.4] filterObject.qProcVal = 0.1**2 filterObject.qObsVal = 0.001 - filterObject.eKFSwitch = 5. #If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF + filterObject.eKFSwitch = ( + 5.0 # If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF + ) + def test_all_functions_oekf(show_plots): """Module Unit Test""" @@ -58,30 +61,38 @@ def test_all_functions_oekf(show_plots): [testResults, testMessage] = StatePropVariable(show_plots) assert testResults < 1, testMessage + # uncomment this line is 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(True) + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("SimHalfLength, AddMeasNoise , testVector1 , testVector2, stateGuess", [ - (200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0]), - (2000, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0]), - (200, False ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0]), - (200, False ,[0., 0.4, -0.4] ,[0., 0.7, 0.2], [0.3, 0.0, 0.6]), - (200, True ,[0., 0.4, -0.4] ,[0.4, 0.5, 0.], [0.7, 0.7, 0.0]), - (200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0]) -]) - +@pytest.mark.parametrize( + "SimHalfLength, AddMeasNoise , testVector1 , testVector2, stateGuess", + [ + (200, True, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0]), + (2000, True, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0]), + (200, False, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0]), + (200, False, [0.0, 0.4, -0.4], [0.0, 0.7, 0.2], [0.3, 0.0, 0.6]), + (200, True, [0.0, 0.4, -0.4], [0.4, 0.5, 0.0], [0.7, 0.7, 0.0]), + (200, True, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0]), + ], +) # uncomment this line is 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() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -def test_all_sunline_oekf(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess): - [testResults, testMessage] = StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess) +def test_all_sunline_oekf( + show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess +): + [testResults, testMessage] = StateUpdateSunLine( + show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess + ) assert testResults < 1, testMessage @@ -100,41 +111,51 @@ def sunline_individual_test(): ################################################################################### inputOmega = [0.1, 0.2, 0.1] - dt =0.5 - - expDynMat = - np.array([[0., -inputOmega[2], inputOmega[1]], - [inputOmega[2], 0., -inputOmega[0]], - [ -inputOmega[1], inputOmega[0], 0.]]) + dt = 0.5 + expDynMat = -np.array( + [ + [0.0, -inputOmega[2], inputOmega[1]], + [inputOmega[2], 0.0, -inputOmega[0]], + [-inputOmega[1], inputOmega[0], 0.0], + ] + ) - dynMat = okeefeEKF.new_doubleArray(3*3) + dynMat = okeefeEKF.new_doubleArray(3 * 3) for i in range(9): okeefeEKF.doubleArray_setitem(dynMat, i, 0.0) okeefeEKF.sunlineDynMatrixOkeefe(inputOmega, dt, dynMat) DynOut = [] - for i in range(NUMSTATES*NUMSTATES): + for i in range(NUMSTATES * NUMSTATES): DynOut.append(okeefeEKF.doubleArray_getitem(dynMat, i)) DynOut = np.array(DynOut).reshape(3, 3) errorNorm = np.linalg.norm(expDynMat - DynOut) - if(errorNorm > 1.0E-12): + if errorNorm > 1.0e-12: print(errorNorm) testFailCount += 1 testMessages.append("Dynamics Matrix generation Failure \n") - ################################################################################### ## Testing omega computation ################################################################################### - inputStates = [2,1,0.75] - inputPrevStates = [1,0.1,0.5] + inputStates = [2, 1, 0.75] + inputPrevStates = [1, 0.1, 0.5] norm1 = np.linalg.norm(np.array(inputStates)) - norm2 = np.linalg.norm(np.array(inputPrevStates)) - dt =0.5 + norm2 = np.linalg.norm(np.array(inputPrevStates)) + dt = 0.5 - expOmega = 1./dt*np.cross(np.array(inputStates),np.array(inputPrevStates))/(norm1*norm2)*np.arccos(np.dot(np.array(inputStates),np.array(inputPrevStates))/(norm1*norm2)) + expOmega = ( + 1.0 + / dt + * np.cross(np.array(inputStates), np.array(inputPrevStates)) + / (norm1 * norm2) + * np.arccos( + np.dot(np.array(inputStates), np.array(inputPrevStates)) / (norm1 * norm2) + ) + ) omega = okeefeEKF.new_doubleArray(NUMSTATES) for i in range(3): @@ -147,7 +168,7 @@ def sunline_individual_test(): omegaOut = np.array(omegaOut) errorNorm = np.linalg.norm(expOmega - omegaOut) - if(errorNorm > 1.0E-12): + if errorNorm > 1.0e-12: print(errorNorm) testFailCount += 1 testMessages.append("Dynamics Matrix generation Failure \n") @@ -156,93 +177,142 @@ def sunline_individual_test(): ## STM and State Test ################################################################################### - inputStates = [2,1,0.75] + inputStates = [2, 1, 0.75] inputOmega = [0.1, 0.2, 0.1] - dt =0.5 - stateTransition = okeefeEKF.new_doubleArray(NUMSTATES*NUMSTATES) + dt = 0.5 + stateTransition = okeefeEKF.new_doubleArray(NUMSTATES * NUMSTATES) states = okeefeEKF.new_doubleArray(NUMSTATES) prev_states = okeefeEKF.new_doubleArray(NUMSTATES) for i in range(NUMSTATES): okeefeEKF.doubleArray_setitem(states, i, inputStates[i]) for j in range(NUMSTATES): - if i==j: - okeefeEKF.doubleArray_setitem(stateTransition, NUMSTATES*i+j, 1.0) + if i == j: + okeefeEKF.doubleArray_setitem(stateTransition, NUMSTATES * i + j, 1.0) else: - okeefeEKF.doubleArray_setitem(stateTransition, NUMSTATES*i+j, 0.0) + okeefeEKF.doubleArray_setitem(stateTransition, NUMSTATES * i + j, 0.0) - okeefeEKF.sunlineStateSTMProp(expDynMat.flatten().tolist(), dt, inputOmega, states, prev_states, stateTransition) + okeefeEKF.sunlineStateSTMProp( + expDynMat.flatten().tolist(), + dt, + inputOmega, + states, + prev_states, + stateTransition, + ) PropStateOut = [] PropSTMOut = [] for i in range(NUMSTATES): PropStateOut.append(okeefeEKF.doubleArray_getitem(states, i)) - for i in range(NUMSTATES*NUMSTATES): + for i in range(NUMSTATES * NUMSTATES): PropSTMOut.append(okeefeEKF.doubleArray_getitem(stateTransition, i)) - STMout = np.array(PropSTMOut).reshape([NUMSTATES,NUMSTATES]) + STMout = np.array(PropSTMOut).reshape([NUMSTATES, NUMSTATES]) StatesOut = np.array(PropStateOut) - expectedSTM = dt*np.dot(expDynMat, np.eye(NUMSTATES)) + np.eye(NUMSTATES) + expectedSTM = dt * np.dot(expDynMat, np.eye(NUMSTATES)) + np.eye(NUMSTATES) expectedStates = np.zeros(NUMSTATES) inputStatesArray = np.array(inputStates) ## Equations when removing the unobservable states from d_dot - expectedStates[0:3] = np.array(inputStatesArray - dt*(np.cross(np.array(inputOmega), np.array(inputStatesArray)))) + expectedStates[0:3] = np.array( + inputStatesArray + - dt * (np.cross(np.array(inputOmega), np.array(inputStatesArray))) + ) errorNormSTM = np.linalg.norm(expectedSTM - STMout) errorNormStates = np.linalg.norm(expectedStates - StatesOut) - if(errorNormSTM > 1.0E-12): + if errorNormSTM > 1.0e-12: print(errorNormSTM) testFailCount += 1 testMessages.append("STM Propagation Failure \n") - - if(errorNormStates > 1.0E-12): + if errorNormStates > 1.0e-12: print(errorNormStates) testFailCount += 1 testMessages.append("State Propagation Failure \n") - - ################################################################################### # ## Test the H and yMeas matrix generation as well as the observation count # ################################################################################### numCSS = 4 - cssCos = [np.cos(np.deg2rad(10.)), np.cos(np.deg2rad(25.)), np.cos(np.deg2rad(5.)), np.cos(np.deg2rad(90.))] - sensorTresh = np.cos(np.deg2rad(50.)) - cssNormals = [1.,0.,0.,0.,1.,0., 0.,0.,1., 1./np.sqrt(2), 1./np.sqrt(2),0.] + cssCos = [ + np.cos(np.deg2rad(10.0)), + np.cos(np.deg2rad(25.0)), + np.cos(np.deg2rad(5.0)), + np.cos(np.deg2rad(90.0)), + ] + sensorTresh = np.cos(np.deg2rad(50.0)) + cssNormals = [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 1.0 / np.sqrt(2), + 1.0 / np.sqrt(2), + 0.0, + ] cssBias = [1.0 for i in range(numCSS)] - measMat = okeefeEKF.new_doubleArray(8*NUMSTATES) + measMat = okeefeEKF.new_doubleArray(8 * NUMSTATES) obs = okeefeEKF.new_doubleArray(8) yMeas = okeefeEKF.new_doubleArray(8) numObs = okeefeEKF.new_intArray(1) - for i in range(8*NUMSTATES): - okeefeEKF.doubleArray_setitem(measMat, i, 0.) + for i in range(8 * NUMSTATES): + okeefeEKF.doubleArray_setitem(measMat, i, 0.0) for i in range(8): okeefeEKF.doubleArray_setitem(obs, i, 0.0) okeefeEKF.doubleArray_setitem(yMeas, i, 0.0) - okeefeEKF.sunlineHMatrixYMeas(inputStates, numCSS, cssCos, sensorTresh, cssNormals, cssBias, obs, yMeas, numObs, measMat) + okeefeEKF.sunlineHMatrixYMeas( + inputStates, + numCSS, + cssCos, + sensorTresh, + cssNormals, + cssBias, + obs, + yMeas, + numObs, + measMat, + ) obsOut = [] yMeasOut = [] numObsOut = [] HOut = [] - for i in range(8*NUMSTATES): + for i in range(8 * NUMSTATES): HOut.append(okeefeEKF.doubleArray_getitem(measMat, i)) for i in range(8): yMeasOut.append(okeefeEKF.doubleArray_getitem(yMeas, i)) obsOut.append(okeefeEKF.doubleArray_getitem(obs, i)) numObsOut.append(okeefeEKF.intArray_getitem(numObs, 0)) - #Fill in expected values for test - expectedH = np.zeros([8,NUMSTATES]) + # Fill in expected values for test + expectedH = np.zeros([8, NUMSTATES]) expectedY = np.zeros(8) for j in range(3): - expectedH[j,0:3] = np.eye(3)[j,:] - expectedY[j] =np.array(cssCos[j]) - np.dot(np.array(inputStates)[0:3], np.array(cssNormals)[j*3:(j+1)*3]) - expectedObs = np.array([np.cos(np.deg2rad(10.)), np.cos(np.deg2rad(25.)), np.cos(np.deg2rad(5.)),0.,0.,0.,0.,0.]) + expectedH[j, 0:3] = np.eye(3)[j, :] + expectedY[j] = np.array(cssCos[j]) - np.dot( + np.array(inputStates)[0:3], np.array(cssNormals)[j * 3 : (j + 1) * 3] + ) + expectedObs = np.array( + [ + np.cos(np.deg2rad(10.0)), + np.cos(np.deg2rad(25.0)), + np.cos(np.deg2rad(5.0)), + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + ) expectedNumObs = 3 HOut = np.array(HOut).reshape([8, NUMSTATES]) @@ -252,7 +322,7 @@ def sunline_individual_test(): errorNorm[2] = np.linalg.norm(obsOut - expectedObs) errorNorm[3] = np.linalg.norm(numObsOut[0] - expectedNumObs) for i in range(4): - if(errorNorm[i] > 1.0E-12): + if errorNorm[i] > 1.0e-12: testFailCount += 1 testMessages.append("H and yMeas update failure \n") @@ -261,17 +331,39 @@ def sunline_individual_test(): # ################################################################################### numObs = 3 - h = [1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0.] - covar = [1., 0., 1., - 0., 1., 0., - 1., 0., 1. ] - noise= 0.01 + h = [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + covar = [1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0] + noise = 0.01 Kalman = okeefeEKF.new_doubleArray(NUMSTATES * 8) for i in range(8 * NUMSTATES): - okeefeEKF.doubleArray_setitem(Kalman, i, 0.) + okeefeEKF.doubleArray_setitem(Kalman, i, 0.0) okeefeEKF.sunlineKalmanGainOkeefe(covar, h, noise, numObs, Kalman) @@ -280,16 +372,21 @@ def sunline_individual_test(): KalmanOut.append(okeefeEKF.doubleArray_getitem(Kalman, i)) # Fill in expected values for test - Hmat = np.array(h).reshape([8,NUMSTATES]) - Pk = np.array(covar).reshape([NUMSTATES,NUMSTATES]) - R = noise*np.eye(3) - expectedK = np.dot(np.dot(Pk, Hmat[0:numObs,:].T), np.linalg.inv(np.dot(np.dot(Hmat[0:numObs,:], Pk), Hmat[0:numObs,:].T) + R[0:numObs,0:numObs])) - - KalmanOut = np.array(KalmanOut)[0:NUMSTATES*numObs].reshape([NUMSTATES, 3]) - errorNorm = np.linalg.norm(KalmanOut[:,0:numObs] - expectedK) - - - if (errorNorm > 1.0E-12): + Hmat = np.array(h).reshape([8, NUMSTATES]) + Pk = np.array(covar).reshape([NUMSTATES, NUMSTATES]) + R = noise * np.eye(3) + expectedK = np.dot( + np.dot(Pk, Hmat[0:numObs, :].T), + np.linalg.inv( + np.dot(np.dot(Hmat[0:numObs, :], Pk), Hmat[0:numObs, :].T) + + R[0:numObs, 0:numObs] + ), + ) + + KalmanOut = np.array(KalmanOut)[0 : NUMSTATES * numObs].reshape([NUMSTATES, 3]) + errorNorm = np.linalg.norm(KalmanOut[:, 0:numObs] - expectedK) + + if errorNorm > 1.0e-12: print(errorNorm) testFailCount += 1 testMessages.append("Kalman Gain update failure \n") @@ -298,36 +395,70 @@ def sunline_individual_test(): # ## Test the EKF update # ################################################################################### - KGain = [1.,2.,3., 0., 1., 2., 1., 0., 1.] - for i in range(NUMSTATES*8-NUMSTATES*3): - KGain.append(0.) - inputStates = [2,1,0.75] + KGain = [1.0, 2.0, 3.0, 0.0, 1.0, 2.0, 1.0, 0.0, 1.0] + for i in range(NUMSTATES * 8 - NUMSTATES * 3): + KGain.append(0.0) + inputStates = [2, 1, 0.75] xbar = [0.1, 0.2, 0.01] numObs = 3 - h = [1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., 0., 0., 0.] - covar = [1., 0., 1., - 0., 1., 0., - 1., 0., 1., - ] + h = [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + covar = [ + 1.0, + 0.0, + 1.0, + 0.0, + 1.0, + 0.0, + 1.0, + 0.0, + 1.0, + ] noise = 0.01 inputY = np.zeros(3) for j in range(3): - inputY[j] = np.array(cssCos[j]) - np.dot(np.array(inputStates)[0:3], np.array(cssNormals)[j * 3:(j + 1) * 3]) + inputY[j] = np.array(cssCos[j]) - np.dot( + np.array(inputStates)[0:3], np.array(cssNormals)[j * 3 : (j + 1) * 3] + ) inputY = inputY.tolist() stateError = okeefeEKF.new_doubleArray(NUMSTATES) - covarMat = okeefeEKF.new_doubleArray(NUMSTATES*NUMSTATES) + covarMat = okeefeEKF.new_doubleArray(NUMSTATES * NUMSTATES) inputs = okeefeEKF.new_doubleArray(NUMSTATES) - for i in range(NUMSTATES): - okeefeEKF.doubleArray_setitem(stateError, i, 0.) + okeefeEKF.doubleArray_setitem(stateError, i, 0.0) okeefeEKF.doubleArray_setitem(inputs, i, inputStates[i]) for j in range(NUMSTATES): - okeefeEKF.doubleArray_setitem(covarMat,i+j,0.) + okeefeEKF.doubleArray_setitem(covarMat, i + j, 0.0) - okeefeEKF.okeefeEKFUpdate(KGain, covar, noise, numObs, inputY, h, inputs, stateError, covarMat) + okeefeEKF.okeefeEKFUpdate( + KGain, covar, noise, numObs, inputY, h, inputs, stateError, covarMat + ) stateOut = [] covarOut = [] @@ -335,22 +466,27 @@ def sunline_individual_test(): for i in range(NUMSTATES): stateOut.append(okeefeEKF.doubleArray_getitem(inputs, i)) errorOut.append(okeefeEKF.doubleArray_getitem(stateError, i)) - for j in range(NUMSTATES*NUMSTATES): + for j in range(NUMSTATES * NUMSTATES): covarOut.append(okeefeEKF.doubleArray_getitem(covarMat, j)) # Fill in expected values for test - KK = np.array(KGain)[0:NUMSTATES*3].reshape([NUMSTATES,3]) + KK = np.array(KGain)[0 : NUMSTATES * 3].reshape([NUMSTATES, 3]) expectedStates = np.array(inputStates) + np.dot(KK, np.array(inputY)) - H = np.array(h).reshape([8,NUMSTATES])[0:3,:] + H = np.array(h).reshape([8, NUMSTATES])[0:3, :] Pk = np.array(covar).reshape([NUMSTATES, NUMSTATES]) R = noise * np.eye(3) - expectedP = np.dot(np.dot(np.eye(NUMSTATES) - np.dot(KK, H), Pk), np.transpose(np.eye(NUMSTATES) - np.dot(KK, H))) + np.dot(KK, np.dot(R,KK.T)) + expectedP = np.dot( + np.dot(np.eye(NUMSTATES) - np.dot(KK, H), Pk), + np.transpose(np.eye(NUMSTATES) - np.dot(KK, H)), + ) + np.dot(KK, np.dot(R, KK.T)) errorNorm = np.zeros(2) errorNorm[0] = np.linalg.norm(np.array(stateOut) - expectedStates) - errorNorm[1] = np.linalg.norm(expectedP - np.array(covarOut).reshape([NUMSTATES,NUMSTATES])) + errorNorm[1] = np.linalg.norm( + expectedP - np.array(covarOut).reshape([NUMSTATES, NUMSTATES]) + ) for i in range(2): - if(errorNorm[i] > 1.0E-12): + if errorNorm[i] > 1.0e-12: testFailCount += 1 testMessages.append("EKF update failure \n") # @@ -358,22 +494,45 @@ def sunline_individual_test(): # ## Test the CKF update # ################################################################################### - KGain = [1., 2., 3.] + KGain = [1.0, 2.0, 3.0] for i in range(NUMSTATES * 8 - NUMSTATES * 3): - KGain.append(0.) + KGain.append(0.0) inputStates = [2, 1, 0.75] xbar = [0.1, 0.2, 0.01] numObs = 3 - h = [1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0.] - covar = [1., 0., 1., - 0., 1., 0., - 1., 0., 1.] - noise =0.01 + h = [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + covar = [1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0] + noise = 0.01 inputY = np.zeros(3) for j in range(3): - inputY[j] = np.array(cssCos[j]) - np.dot(np.array(inputStates)[0:3], - np.array(cssNormals)[j * 3:(j + 1) * 3]) + inputY[j] = np.array(cssCos[j]) - np.dot( + np.array(inputStates)[0:3], np.array(cssNormals)[j * 3 : (j + 1) * 3] + ) inputY = inputY.tolist() stateError = okeefeEKF.new_doubleArray(NUMSTATES) @@ -382,32 +541,38 @@ def sunline_individual_test(): for i in range(NUMSTATES): okeefeEKF.doubleArray_setitem(stateError, i, xbar[i]) for j in range(NUMSTATES): - okeefeEKF.doubleArray_setitem(covarMat, i + j, 0.) + okeefeEKF.doubleArray_setitem(covarMat, i + j, 0.0) - okeefeEKF.sunlineCKFUpdateOkeefe(xbar, KGain, covar, noise, numObs, inputY, h, stateError, covarMat) + okeefeEKF.sunlineCKFUpdateOkeefe( + xbar, KGain, covar, noise, numObs, inputY, h, stateError, covarMat + ) covarOut = [] errorOut = [] for i in range(NUMSTATES): errorOut.append(okeefeEKF.doubleArray_getitem(stateError, i)) - for j in range(NUMSTATES*NUMSTATES): + for j in range(NUMSTATES * NUMSTATES): covarOut.append(okeefeEKF.doubleArray_getitem(covarMat, j)) # Fill in expected values for test - KK = np.array(KGain)[0:NUMSTATES * 3].reshape([NUMSTATES, 3]) + KK = np.array(KGain)[0 : NUMSTATES * 3].reshape([NUMSTATES, 3]) H = np.array(h).reshape([8, NUMSTATES])[0:3, :] - expectedStateError = np.array(xbar) + np.dot(KK, (np.array(inputY) - np.dot(H, np.array(xbar)))) + expectedStateError = np.array(xbar) + np.dot( + KK, (np.array(inputY) - np.dot(H, np.array(xbar))) + ) Pk = np.array(covar).reshape([NUMSTATES, NUMSTATES]) - expectedP = np.dot(np.dot(np.eye(NUMSTATES) - np.dot(KK, H), Pk), np.transpose(np.eye(NUMSTATES) - np.dot(KK, H))) + np.dot(KK, - np.dot( - R, - KK.T)) + expectedP = np.dot( + np.dot(np.eye(NUMSTATES) - np.dot(KK, H), Pk), + np.transpose(np.eye(NUMSTATES) - np.dot(KK, H)), + ) + np.dot(KK, np.dot(R, KK.T)) errorNorm = np.zeros(2) errorNorm[0] = np.linalg.norm(np.array(errorOut) - expectedStateError) - errorNorm[1] = np.linalg.norm(expectedP - np.array(covarOut).reshape([NUMSTATES, NUMSTATES])) + errorNorm[1] = np.linalg.norm( + expectedP - np.array(covarOut).reshape([NUMSTATES, NUMSTATES]) + ) for i in range(2): - if (errorNorm[i] > 1.0E-12): + if errorNorm[i] > 1.0e-12: testFailCount += 1 testMessages.append("CKF update failure \n") @@ -417,7 +582,8 @@ def sunline_individual_test(): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + #################################################################################### # Test for the time and update with static states (zero d_dot) @@ -428,7 +594,7 @@ def StatePropStatic(): # --fulltrace command line option is specified. __tracebackhide__ = True - NUMSTATES=3 + NUMSTATES = 3 testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages @@ -451,9 +617,9 @@ def StatePropStatic(): unitTestSim.AddModelToTask(unitTaskName, module) setupFilterData(module) - module.omega = [0.,0.,0.] + module.omega = [0.0, 0.0, 0.0] - kfLog = module.logger(["covar", "state"], testProcessRate*10) + kfLog = module.logger(["covar", "state"], testProcessRate * 10) unitTestSim.AddModelToTask(unitTaskName, kfLog) # connect messages @@ -462,7 +628,6 @@ def StatePropStatic(): module.cssDataInMsg.subscribeTo(cssDataInMsg) module.cssConfigInMsg.subscribeTo(cssConfigInMsg) - unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(macros.sec2nano(8000.0)) unitTestSim.ExecuteSimulation() @@ -470,7 +635,7 @@ def StatePropStatic(): stateLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.state) for i in range(NUMSTATES): - if (abs(stateLog[-1, i + 1] - stateLog[0, i + 1]) > 1.0E-10): + if abs(stateLog[-1, i + 1] - stateLog[0, i + 1]) > 1.0e-10: print(abs(stateLog[-1, i + 1] - stateLog[0, i + 1])) testFailCount += 1 testMessages.append("Static state propagation failure \n") @@ -481,7 +646,7 @@ def StatePropStatic(): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] #################################################################################### @@ -493,7 +658,7 @@ def StatePropVariable(show_plots): # --fulltrace command line option is specified. __tracebackhide__ = True - NUMSTATES=3 + NUMSTATES = 3 testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages @@ -524,7 +689,9 @@ def StatePropVariable(show_plots): InitOmega = module.omega module.state = InitialState - kfLog = module.logger(["covar", "state", "stateTransition", "x", "omega"], testProcessRate) + kfLog = module.logger( + ["covar", "state", "stateTransition", "x", "omega"], testProcessRate + ) unitTestSim.AddModelToTask(unitTaskName, kfLog) # connect messages @@ -537,7 +704,6 @@ def StatePropVariable(show_plots): unitTestSim.ConfigureStopTime(macros.sec2nano(1000.0)) unitTestSim.ExecuteSimulation() - covarLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.covar) stateLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.state) stateErrorLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.x) @@ -546,79 +712,141 @@ def StatePropVariable(show_plots): dt = 0.5 expectedOmega = np.zeros([2001, (NUMSTATES + 1)]) - expectedStateArray = np.zeros([2001,(NUMSTATES+1)]) - expectedPrevArray = np.zeros([2001,(NUMSTATES+1)]) - expectedStateArray[0,1:(NUMSTATES+1)] = np.array(InitialState) - expectedOmega[0,1:(NUMSTATES+1)] = np.array(InitOmega) - - expectedXBar = np.zeros([2001,NUMSTATES+1]) - expectedXBar[0,1:(NUMSTATES+1)] = np.array(Initialx) - - expectedSTM = np.zeros([2001,NUMSTATES,NUMSTATES]) - expectedSTM[0,:,:] = np.eye(NUMSTATES) - - expectedCovar = np.zeros([2001,NUMSTATES*NUMSTATES+1]) - expectedCovar[0,1:(NUMSTATES*NUMSTATES+1)] = np.array(InitialCovar) - - expDynMat = np.zeros([2001,NUMSTATES,NUMSTATES]) - Gamma = dt ** 2. / 2. * np.eye(3) - ProcNoiseCovar = np.dot(Gamma, np.dot(module.qProcVal*np.eye(3),Gamma.T)) - - for i in range(1,2001): - expectedStateArray[i,0] = dt*i*1E9 - expectedPrevArray[i,0] = dt*i*1E9 - expectedOmega[i,0] = dt*i*1E9 - expectedCovar[i,0] = dt*i*1E9 - expectedXBar[i,0] = dt*i*1E9 - - #Simulate sunline Dyn Mat - expDynMat[i-1, :, :] = - np.array([[0., -expectedOmega[i-1, 3], expectedOmega[i-1,2]], - [expectedOmega[i-1,3], 0., -expectedOmega[i-1,1]], - [ -expectedOmega[i-1,2], expectedOmega[i-1,1], 0.]]) - - #Simulate STM State prop - expectedStateArray[i,1:(NUMSTATES+1)] = np.array(expectedStateArray[i-1,1:(NUMSTATES+1)] - dt*(np.cross(np.array(expectedOmega[i-1,1:(NUMSTATES+1)]), np.array(expectedStateArray[i-1,1:(NUMSTATES+1)])))) - expectedPrevArray[i, 1:(NUMSTATES + 1)] = expectedStateArray[i-1,1:(NUMSTATES+1)] - expectedSTM[i,:,:] = dt * np.dot(expDynMat[i-1,:,:], np.eye(NUMSTATES)) + np.eye(NUMSTATES) + expectedStateArray = np.zeros([2001, (NUMSTATES + 1)]) + expectedPrevArray = np.zeros([2001, (NUMSTATES + 1)]) + expectedStateArray[0, 1 : (NUMSTATES + 1)] = np.array(InitialState) + expectedOmega[0, 1 : (NUMSTATES + 1)] = np.array(InitOmega) + + expectedXBar = np.zeros([2001, NUMSTATES + 1]) + expectedXBar[0, 1 : (NUMSTATES + 1)] = np.array(Initialx) + + expectedSTM = np.zeros([2001, NUMSTATES, NUMSTATES]) + expectedSTM[0, :, :] = np.eye(NUMSTATES) + + expectedCovar = np.zeros([2001, NUMSTATES * NUMSTATES + 1]) + expectedCovar[0, 1 : (NUMSTATES * NUMSTATES + 1)] = np.array(InitialCovar) + + expDynMat = np.zeros([2001, NUMSTATES, NUMSTATES]) + Gamma = dt**2.0 / 2.0 * np.eye(3) + ProcNoiseCovar = np.dot(Gamma, np.dot(module.qProcVal * np.eye(3), Gamma.T)) + + for i in range(1, 2001): + expectedStateArray[i, 0] = dt * i * 1e9 + expectedPrevArray[i, 0] = dt * i * 1e9 + expectedOmega[i, 0] = dt * i * 1e9 + expectedCovar[i, 0] = dt * i * 1e9 + expectedXBar[i, 0] = dt * i * 1e9 + + # Simulate sunline Dyn Mat + expDynMat[i - 1, :, :] = -np.array( + [ + [0.0, -expectedOmega[i - 1, 3], expectedOmega[i - 1, 2]], + [expectedOmega[i - 1, 3], 0.0, -expectedOmega[i - 1, 1]], + [-expectedOmega[i - 1, 2], expectedOmega[i - 1, 1], 0.0], + ] + ) + + # Simulate STM State prop + expectedStateArray[i, 1 : (NUMSTATES + 1)] = np.array( + expectedStateArray[i - 1, 1 : (NUMSTATES + 1)] + - dt + * ( + np.cross( + np.array(expectedOmega[i - 1, 1 : (NUMSTATES + 1)]), + np.array(expectedStateArray[i - 1, 1 : (NUMSTATES + 1)]), + ) + ) + ) + expectedPrevArray[i, 1 : (NUMSTATES + 1)] = expectedStateArray[ + i - 1, 1 : (NUMSTATES + 1) + ] + expectedSTM[i, :, :] = dt * np.dot( + expDynMat[i - 1, :, :], np.eye(NUMSTATES) + ) + np.eye(NUMSTATES) # Simulate Rate compute - normdk = np.linalg.norm(expectedStateArray[i, 1:(NUMSTATES + 1)]) - nomrdkmin1 = np.linalg.norm(expectedPrevArray[i, 1:(NUMSTATES + 1)]) - arg = np.dot(expectedStateArray[i, 1:(NUMSTATES + 1)], expectedPrevArray[i , 1:(NUMSTATES + 1)]) / (normdk * nomrdkmin1) - if arg>1: - expectedOmega[i, 1:(NUMSTATES + 1)] = 1./dt*np.cross(expectedStateArray[i, 1:(NUMSTATES + 1)], - expectedPrevArray[i, 1:(NUMSTATES + 1)]) / (normdk * nomrdkmin1) * np.arccos(1) - elif arg<-1: - expectedOmega[i, 1:(NUMSTATES + 1)] = 1./dt*np.cross(expectedStateArray[i, 1:(NUMSTATES + 1)], - expectedPrevArray[i, 1:(NUMSTATES + 1)]) / ( - normdk * nomrdkmin1) * np.arccos(-1) + normdk = np.linalg.norm(expectedStateArray[i, 1 : (NUMSTATES + 1)]) + nomrdkmin1 = np.linalg.norm(expectedPrevArray[i, 1 : (NUMSTATES + 1)]) + arg = np.dot( + expectedStateArray[i, 1 : (NUMSTATES + 1)], + expectedPrevArray[i, 1 : (NUMSTATES + 1)], + ) / (normdk * nomrdkmin1) + if arg > 1: + expectedOmega[i, 1 : (NUMSTATES + 1)] = ( + 1.0 + / dt + * np.cross( + expectedStateArray[i, 1 : (NUMSTATES + 1)], + expectedPrevArray[i, 1 : (NUMSTATES + 1)], + ) + / (normdk * nomrdkmin1) + * np.arccos(1) + ) + elif arg < -1: + expectedOmega[i, 1 : (NUMSTATES + 1)] = ( + 1.0 + / dt + * np.cross( + expectedStateArray[i, 1 : (NUMSTATES + 1)], + expectedPrevArray[i, 1 : (NUMSTATES + 1)], + ) + / (normdk * nomrdkmin1) + * np.arccos(-1) + ) else: - expectedOmega[i, 1:(NUMSTATES + 1)] = 1./dt*np.cross(expectedStateArray[i, 1:(NUMSTATES + 1)],expectedPrevArray[i, 1:(NUMSTATES + 1)]) / (normdk * nomrdkmin1) * np.arccos(arg) - - expectedXBar[i, 1:(NUMSTATES+1)] = np.dot(expectedSTM[i, :, :], expectedXBar[i - 1, 1:(NUMSTATES+1)]) - expectedCovar[i,1:(NUMSTATES*NUMSTATES+1)] = (np.dot(expectedSTM[i,:,:], np.dot(np.reshape(expectedCovar[i-1,1:(NUMSTATES*NUMSTATES+1)],[NUMSTATES,NUMSTATES]), np.transpose(expectedSTM[i,:,:])))+ ProcNoiseCovar).flatten() + expectedOmega[i, 1 : (NUMSTATES + 1)] = ( + 1.0 + / dt + * np.cross( + expectedStateArray[i, 1 : (NUMSTATES + 1)], + expectedPrevArray[i, 1 : (NUMSTATES + 1)], + ) + / (normdk * nomrdkmin1) + * np.arccos(arg) + ) + + expectedXBar[i, 1 : (NUMSTATES + 1)] = np.dot( + expectedSTM[i, :, :], expectedXBar[i - 1, 1 : (NUMSTATES + 1)] + ) + expectedCovar[i, 1 : (NUMSTATES * NUMSTATES + 1)] = ( + np.dot( + expectedSTM[i, :, :], + np.dot( + np.reshape( + expectedCovar[i - 1, 1 : (NUMSTATES * NUMSTATES + 1)], + [NUMSTATES, NUMSTATES], + ), + np.transpose(expectedSTM[i, :, :]), + ), + ) + + ProcNoiseCovar + ).flatten() FilterPlots.StatesVsExpected(stateLog, expectedStateArray, show_plots) - FilterPlots.StatesPlotCompare(stateErrorLog, expectedXBar, covarLog, expectedCovar, show_plots) + FilterPlots.StatesPlotCompare( + stateErrorLog, expectedXBar, covarLog, expectedCovar, show_plots + ) FilterPlots.OmegaVsExpected(expectedOmega, omegaLog, show_plots) - for j in range(1,2001): + for j in range(1, 2001): for i in range(NUMSTATES): - if (abs(stateLog[j, i + 1] - expectedStateArray[j, i + 1]) > 1.0E-10): + if abs(stateLog[j, i + 1] - expectedStateArray[j, i + 1]) > 1.0e-10: testFailCount += 1 testMessages.append("General state propagation failure: State Prop \n") - if (abs(stateErrorLog[j, i + 1] - expectedXBar[j, i + 1]) > 1.0E-10): + if abs(stateErrorLog[j, i + 1] - expectedXBar[j, i + 1]) > 1.0e-10: testFailCount += 1 - testMessages.append("General state propagation failure: State Error Prop \n") + testMessages.append( + "General state propagation failure: State Error Prop \n" + ) - for i in range(NUMSTATES*NUMSTATES): - if (abs(covarLog[j, i + 1] - expectedCovar[j, i + 1]) > 1.0E-8): + for i in range(NUMSTATES * NUMSTATES): + if abs(covarLog[j, i + 1] - expectedCovar[j, i + 1]) > 1.0e-8: print(abs(covarLog[j, i + 1] - expectedCovar[j, i + 1])) abs(covarLog[j, i + 1] - expectedCovar[j, i + 1]) testFailCount += 1 # testMessages.append("General state propagation failure: Covariance Prop \n") - if (abs(stmLog[j, i + 1] - expectedSTM[j,:].flatten()[i]) > 1.0E-10): + if abs(stmLog[j, i + 1] - expectedSTM[j, :].flatten()[i]) > 1.0e-10: testFailCount += 1 testMessages.append("General state propagation failure: STM Prop \n") @@ -630,18 +858,20 @@ def StatePropVariable(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] #################################################################################### # Test for the full filter with time and measurement update #################################################################################### -def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess): +def StateUpdateSunLine( + show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess +): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. __tracebackhide__ = True - NUMSTATES=3 + NUMSTATES = 3 testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages @@ -664,7 +894,7 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) setupFilterData(module) - module.omega = [0.,0.,0.] + module.omega = [0.0, 0.0, 0.0] # Set up some test parameters @@ -682,7 +912,7 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes ] CSSBias = [1.0 for i in range(len(CSSOrientationList))] totalCSSList = [] - i=0 + i = 0 for CSSHat in CSSOrientationList: newCSS = messaging.CSSUnitConfigMsgPayload() newCSS.CBias = CSSBias[i] @@ -700,7 +930,7 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes module.cssDataInMsg.subscribeTo(cssDataInMsg) module.cssConfigInMsg.subscribeTo(cssConstInMsg) - dt =0.5 + dt = 0.5 stateTarget1 = testVector1 module.state = stateGuess module.x = (np.array(stateTarget1) - np.array(stateGuess)).tolist() @@ -711,14 +941,14 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes unitTestSim.InitializeSimulation() - - for i in range(SimHalfLength): if i > 20: dotList = [] for element in CSSOrientationList: if AddMeasNoise: - dotProd = np.dot(np.array(element), np.array(testVector1)[0:3]) + np.random.normal(0., module.qObsVal) + dotProd = np.dot( + np.array(element), np.array(testVector1)[0:3] + ) + np.random.normal(0.0, module.qObsVal) else: dotProd = np.dot(np.array(element), np.array(testVector1)[0:3]) dotList.append(dotProd) @@ -733,22 +963,33 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes if not AddMeasNoise: for i in range(NUMSTATES): - if (abs(covarLog[-1, i * NUMSTATES + i] - covarLog[0, i * NUMSTATES + i] / 100.) > 1E-1): + if ( + abs( + covarLog[-1, i * NUMSTATES + i] + - covarLog[0, i * NUMSTATES + i] / 100.0 + ) + > 1e-1 + ): testFailCount += 1 testMessages.append("Covariance update failure") - if (abs(stateLog[-1, i] - stateTarget1[i]) > 1.0E-10): + if abs(stateLog[-1, i] - stateTarget1[i]) > 1.0e-10: testFailCount += 1 testMessages.append("State update failure") else: for i in range(NUMSTATES): - if (abs(covarLog[-1, i * NUMSTATES + i] - covarLog[0, i * NUMSTATES + i] / 100.) > 1E-1): + if ( + abs( + covarLog[-1, i * NUMSTATES + i] + - covarLog[0, i * NUMSTATES + i] / 100.0 + ) + > 1e-1 + ): testFailCount += 1 testMessages.append("Covariance update failure with noise") - if (abs(stateLog[-1, i] - stateTarget1[i]) > 1.0E-2): + if abs(stateLog[-1, i] - stateTarget1[i]) > 1.0e-2: testFailCount += 1 testMessages.append("State update failure with noise") - stateTarget2 = testVector2 inputData = messaging.CSSArraySensorMsgPayload() @@ -757,14 +998,16 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes dotList = [] for element in CSSOrientationList: if AddMeasNoise: - dotProd = np.dot(np.array(element), np.array(testVector2)[0:3]) + np.random.normal(0., module.qObsVal) + dotProd = np.dot( + np.array(element), np.array(testVector2)[0:3] + ) + np.random.normal(0.0, module.qObsVal) else: dotProd = np.dot(np.array(element), np.array(testVector2)[0:3]) dotList.append(dotProd) inputData.CosValue = dotList cssDataInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i + SimHalfLength+1) * 0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + SimHalfLength + 1) * 0.5)) unitTestSim.ExecuteSimulation() stateErrorLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.x) @@ -772,30 +1015,42 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes postFitLog = dataLog.postFitRes covarLog = dataLog.covar - if not AddMeasNoise: for i in range(NUMSTATES): - if (abs(covarLog[-1, i * NUMSTATES + i] - covarLog[0, i * NUMSTATES + i] / 100.) > 1E-1): + if ( + abs( + covarLog[-1, i * NUMSTATES + i] + - covarLog[0, i * NUMSTATES + i] / 100.0 + ) + > 1e-1 + ): testFailCount += 1 testMessages.append("Covariance update failure") - if (abs(stateLog[-1, i] - stateTarget2[i]) > 1.0E-10): + if abs(stateLog[-1, i] - stateTarget2[i]) > 1.0e-10: testFailCount += 1 testMessages.append("State update failure") else: for i in range(NUMSTATES): - if (abs(covarLog[-1, i * NUMSTATES + i] - covarLog[0, i * NUMSTATES + i] / 100.) > 1E-1): + if ( + abs( + covarLog[-1, i * NUMSTATES + i] + - covarLog[0, i * NUMSTATES + i] / 100.0 + ) + > 1e-1 + ): testFailCount += 1 testMessages.append("Covariance update failure") - if (abs(stateLog[-1, i] - stateTarget2[i]) > 1.0E-2): + if abs(stateLog[-1, i] - stateTarget2[i]) > 1.0e-2: testFailCount += 1 testMessages.append("State update failure") - target1 = np.array(testVector1) target2 = np.array(testVector2) FilterPlots.StatesPlot(dataLog.times(), stateErrorLog, covarLog, show_plots) FilterPlots.StatesVsTargets(dataLog.times(), target1, target2, stateLog, show_plots) - FilterPlots.PostFitResiduals(dataLog.times(), postFitLog, module.qObsVal, show_plots) + FilterPlots.PostFitResiduals( + dataLog.times(), postFitLog, module.qObsVal, show_plots + ) # print out success message if no error were found if testFailCount == 0: @@ -805,12 +1060,13 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] - + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": - StateUpdateSunLine(True, 200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0]) + StateUpdateSunLine( + True, 200, True, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0] + ) # sunline_individual_test() # StatePropStatic() # StatePropVariable(True) diff --git a/src/fswAlgorithms/attDetermination/sunlineEKF/_UnitTest/SunLineEKF_test_utilities.py b/src/fswAlgorithms/attDetermination/sunlineEKF/_UnitTest/SunLineEKF_test_utilities.py index e8b61634d0..48fddc82f5 100644 --- a/src/fswAlgorithms/attDetermination/sunlineEKF/_UnitTest/SunLineEKF_test_utilities.py +++ b/src/fswAlgorithms/attDetermination/sunlineEKF/_UnitTest/SunLineEKF_test_utilities.py @@ -23,336 +23,358 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('fswAlgorithms') - - +splitPath = path.split("fswAlgorithms") import matplotlib.pyplot as plt def StateErrorCovarPlot(x, Pflat, show_plots): - - - P = np.zeros([len(Pflat[:,0]),6,6]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*1E-9 - P[i,:,:] = Pflat[i,1:37].reshape([6,6]) - for j in range(len(P[0,0,:])): - P[i,j,j] = np.sqrt(P[i,j,j]) - - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + P = np.zeros([len(Pflat[:, 0]), 6, 6]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * 1e-9 + P[i, :, :] = Pflat[i, 1:37].reshape([6, 6]) + for j in range(len(P[0, 0, :])): + P[i, j, j] = np.sqrt(P[i, j, j]) + + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t , x[:, 1], "b", label='Error Filter') - plt.plot(t , 3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t , -3 * np.sqrt(P[:, 0, 0]), 'r--') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.plot(t, 3 * np.sqrt(P[:, 0, 0]), "r--", label="Covar Filter") + plt.plot(t, -3 * np.sqrt(P[:, 0, 0]), "r--") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(322) - plt.plot(t , x[:, 4], "b") - plt.plot(t , 3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.title('First rate component') + plt.plot(t, x[:, 4], "b") + plt.plot(t, 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 3, 3]), "r--") + plt.title("First rate component") plt.grid() plt.subplot(323) - plt.plot(t , x[:, 2], "b") - plt.plot(t , 3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.title('Second LOS component') + plt.plot(t, x[:, 2], "b") + plt.plot(t, 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 1, 1]), "r--") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(t , x[:, 5], "b") - plt.plot(t , 3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.title('Second rate component') + plt.plot(t, x[:, 5], "b") + plt.plot(t, 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 4, 4]), "r--") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(t , x[:, 3], "b") - plt.plot(t , 3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(t, x[:, 3], "b") + plt.plot(t, 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 2, 2]), "r--") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(t , x[:, 6], "b") - plt.plot(t , 3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(t, x[:, 6], "b") + plt.plot(t, 3 * np.sqrt(P[:, 5, 5]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 5, 5]), "r--") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesPlot', 'State error and covariance', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesPlot", + "State error and covariance", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() - plt.close('all') + plt.close("all") def StatesPlotCompare(x, x2, Pflat, Pflat2, show_plots): - - - P = np.zeros([len(Pflat[:,0]),6,6]) - P2 = np.zeros([len(Pflat[:,0]),6,6]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*1E-9 - P[i,:,:] = Pflat[i,1:37].reshape([6,6]) + P = np.zeros([len(Pflat[:, 0]), 6, 6]) + P2 = np.zeros([len(Pflat[:, 0]), 6, 6]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * 1e-9 + P[i, :, :] = Pflat[i, 1:37].reshape([6, 6]) P2[i, :, :] = Pflat2[i, 1:37].reshape([6, 6]) - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t[0:30] , x[0:30, 1], "b", label='Error Filter') - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 0, 0]), 'r--') - plt.plot(t[0:30] , x2[0:30, 1], "g", label='Error Expected') - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 0, 0]), 'c--', label='Covar Expected') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 0, 0]), 'c--') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(t[0:30], x[0:30, 1], "b", label="Error Filter") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 0, 0]), "r--", label="Covar Filter") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 0, 0]), "r--") + plt.plot(t[0:30], x2[0:30, 1], "g", label="Error Expected") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 0, 0]), "c--", label="Covar Expected") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 0, 0]), "c--") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(322) - plt.plot(t[0:30] , x[0:30, 4], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 3, 3]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 3, 3]), 'r--') - plt.plot(t[0:30] , x2[0:30, 4], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 3, 3]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 3, 3]), 'c--') - plt.title('First rate component') + plt.plot(t[0:30], x[0:30, 4], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 3, 3]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 3, 3]), "r--") + plt.plot(t[0:30], x2[0:30, 4], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 3, 3]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 3, 3]), "c--") + plt.title("First rate component") plt.grid() plt.subplot(323) - plt.plot(t[0:30] , x[0:30, 2], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 1, 1]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 1, 1]), 'r--') - plt.plot(t[0:30] , x2[0:30, 2], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.title('Second LOS component') + plt.plot(t[0:30], x[0:30, 2], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 1, 1]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 1, 1]), "r--") + plt.plot(t[0:30], x2[0:30, 2], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(t[0:30] , x[0:30, 5], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 4, 4]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 4, 4]), 'r--') - plt.plot(t[0:30] , x2[0:30, 5], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 4, 4]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 4, 4]), 'c--') - plt.title('Second rate component') + plt.plot(t[0:30], x[0:30, 5], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 4, 4]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 4, 4]), "r--") + plt.plot(t[0:30], x2[0:30, 5], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 4, 4]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 4, 4]), "c--") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(t[0:30] , x[0:30, 3], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 2, 2]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 2, 2]), 'r--') - plt.plot(t[0:30] , x2[0:30, 3], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(t[0:30], x[0:30, 3], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 2, 2]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 2, 2]), "r--") + plt.plot(t[0:30], x2[0:30, 3], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(t[0:30] , x[0:30, 6], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 5, 5]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 5, 5]), 'r--') - plt.plot(t[0:30] , x2[0:30, 6], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 5, 5]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 5, 5]), 'c--') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(t[0:30], x[0:30, 6], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 5, 5]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 5, 5]), "r--") + plt.plot(t[0:30], x2[0:30, 6], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 5, 5]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 5, 5]), "c--") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesCompare', 'State error and covariance vs expected Values', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesCompare", + "State error and covariance vs expected Values", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() -def PostFitResiduals(Res, noise, show_plots): - MeasNoise = np.zeros(len(Res[:,0])) - t= np.zeros(len(Res[:,0])) - for i in range(len(Res[:,0])): - t[i] = Res[i, 0]*1E-9 - MeasNoise[i] = 3*noise +def PostFitResiduals(Res, noise, show_plots): + MeasNoise = np.zeros(len(Res[:, 0])) + t = np.zeros(len(Res[:, 0])) + for i in range(len(Res[:, 0])): + t[i] = Res[i, 0] * 1e-9 + MeasNoise[i] = 3 * noise # Don't plot zero values, since they mean that no measurement is taken - for j in range(len(Res[0,:])-1): - if -1E-10 < Res[i,j+1] < 1E-10: - Res[i, j+1] = np.nan + for j in range(len(Res[0, :]) - 1): + if -1e-10 < Res[i, j + 1] < 1e-10: + Res[i, j + 1] = np.nan - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(421) - plt.plot(t , Res[:, 1], "b.", label='Residual') - plt.plot(t , MeasNoise, 'r--', label='Covar') - plt.plot(t , -MeasNoise, 'r--') - plt.legend(loc='lower right') - plt.ylim([-10*noise, 10*noise]) - plt.title('First CSS') + plt.plot(t, Res[:, 1], "b.", label="Residual") + plt.plot(t, MeasNoise, "r--", label="Covar") + plt.plot(t, -MeasNoise, "r--") + plt.legend(loc="lower right") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("First CSS") plt.grid() plt.subplot(422) - plt.plot(t , Res[:, 5], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Fifth CSS') + plt.plot(t, Res[:, 5], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Fifth CSS") plt.grid() plt.subplot(423) - plt.plot(t , Res[:, 2], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Second CSS') + plt.plot(t, Res[:, 2], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Second CSS") plt.grid() plt.subplot(424) - plt.plot(t , Res[:, 6], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Sixth CSS') + plt.plot(t, Res[:, 6], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Sixth CSS") plt.grid() plt.subplot(425) - plt.plot(t , Res[:, 3], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Third CSS') + plt.plot(t, Res[:, 3], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Third CSS") plt.grid() plt.subplot(426) - plt.plot(t , Res[:, 7], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Seventh CSS') + plt.plot(t, Res[:, 7], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Seventh CSS") plt.grid() plt.subplot(427) - plt.plot(t , Res[:, 4], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.xlabel('t(s)') - plt.title('Fourth CSS') + plt.plot(t, Res[:, 4], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.xlabel("t(s)") + plt.title("Fourth CSS") plt.grid() plt.subplot(428) - plt.plot(t , Res[:, 8], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.xlabel('t(s)') - plt.title('Eight CSS') + plt.plot(t, Res[:, 8], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.xlabel("t(s)") + plt.title("Eight CSS") plt.grid() - unitTestSupport.writeFigureLaTeX('PostFit', 'Post Fit Residuals', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "PostFit", + "Post Fit Residuals", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() + def StatesVsExpected(stateLog, expectedStateArray, show_plots): - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 1], 'b--', label='Expected') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1], 'r', label='Filter') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 1], "b--", label="Expected") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1], "r", label="Filter") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(323) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 2], 'b--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2], 'r') - plt.title('Second LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 2], "b--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2], "r") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 4], 'b--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 4], 'r') - plt.title('Second rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 4], "b--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 4], "r") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 3], 'b--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3], 'r') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 3], "b--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3], "r") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 5], 'b--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 5], 'r') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 5], "b--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 5], "r") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesExpected', 'States vs true states in static case', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesExpected", + "States vs true states in static case", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() - def StatesVsTargets(target1, target2, stateLog, show_plots): - - - target = np.ones([len(stateLog[:, 0]),6]) - for i in range((len(stateLog[:, 0])-1)//2): + target = np.ones([len(stateLog[:, 0]), 6]) + for i in range((len(stateLog[:, 0]) - 1) // 2): target[i, :] = target1 - target[i+(len(stateLog[:, 0]) - 1) // 2,:] = target2 + target[i + (len(stateLog[:, 0]) - 1) // 2, :] = target2 - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1], 'b', label='Filter') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 0], 'r--', label='Expected') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1], "b", label="Filter") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 0], "r--", label="Expected") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(322) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 4], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 3], 'r--') - plt.title('First rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 4], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 3], "r--") + plt.title("First rate component") plt.grid() plt.subplot(323) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 1], 'r--') - plt.title('Second LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 1], "r--") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 5], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 4], 'r--') - plt.title('Second rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 5], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 4], "r--") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 2], 'r--') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 2], "r--") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 6], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 5], 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 6], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 5], "r--") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesTarget', 'States tracking target values', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesTarget", + "States tracking target values", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() diff --git a/src/fswAlgorithms/attDetermination/sunlineEKF/_UnitTest/test_SunLineEKF.py b/src/fswAlgorithms/attDetermination/sunlineEKF/_UnitTest/test_SunLineEKF.py index 621c8e1dda..ff0f01e946 100644 --- a/src/fswAlgorithms/attDetermination/sunlineEKF/_UnitTest/test_SunLineEKF.py +++ b/src/fswAlgorithms/attDetermination/sunlineEKF/_UnitTest/test_SunLineEKF.py @@ -26,7 +26,9 @@ from Basilisk.fswAlgorithms import sunlineEKF from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import SunLineEKF_test_utilities as FilterPlots @@ -36,20 +38,54 @@ def addTimeColumn(time, data): def setupFilterData(filterObject): - - filterObject.sensorUseThresh = 0. + filterObject.sensorUseThresh = 0.0 filterObject.state = [1.0, 1.0, 1.0, 0.0, 0.0, 0.0] filterObject.x = [1.0, 0.0, 1.0, 0.0, 0.1, 0.0] - filterObject.covar = [0.4, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.004] + filterObject.covar = [ + 0.4, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.4, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.4, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.004, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.004, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.004, + ] filterObject.qProcVal = 0.1**2 filterObject.qObsVal = 0.001 - filterObject.eKFSwitch = 5. #If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF + filterObject.eKFSwitch = ( + 5.0 # If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF + ) + def test_all_functions_ekf(show_plots): """Module Unit Test""" @@ -60,31 +96,39 @@ def test_all_functions_ekf(show_plots): [testResults, testMessage] = StatePropVariable(show_plots) assert testResults < 1, testMessage + # uncomment this line is 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(True) + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("SimHalfLength, AddMeasNoise , testVector1 , testVector2, stateGuess", [ - (200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]), - (2000, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]), - (200, False ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]), - (200, False ,[0., 0.4, -0.4] ,[0., 0.7, 0.2], [0.3, 0.0, 0.6, 0.0, 0.0, 0.0]), - (200, True ,[0., 0.4, -0.4] ,[0.4, 0.5, 0.], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]), - (200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]) -]) - +@pytest.mark.parametrize( + "SimHalfLength, AddMeasNoise , testVector1 , testVector2, stateGuess", + [ + (200, True, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]), + (2000, True, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]), + (200, False, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]), + (200, False, [0.0, 0.4, -0.4], [0.0, 0.7, 0.2], [0.3, 0.0, 0.6, 0.0, 0.0, 0.0]), + (200, True, [0.0, 0.4, -0.4], [0.4, 0.5, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]), + (200, True, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]), + ], +) # uncomment this line is 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() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -def test_all_sunline_ekf(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess): +def test_all_sunline_ekf( + show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess +): """Module Unit Test""" - [testResults, testMessage] = StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess) + [testResults, testMessage] = StateUpdateSunLine( + show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess + ) assert testResults < 1, testMessage @@ -101,20 +145,51 @@ def sunline_individual_test(): ## Testing dynamics matrix computation ################################################################################### - inputStates = [2,1,0.75,0.1,0.4,0.05] - dt =0.5 + inputStates = [2, 1, 0.75, 0.1, 0.4, 0.05] + dt = 0.5 - expDynMat = np.zeros([6,6]) - expDynMat[0:3, 0:3] = -(np.outer(inputStates[0:3],inputStates[3:6])/np.linalg.norm(inputStates[0:3])**2. + - np.dot(inputStates[3:6],inputStates[0:3])*(np.linalg.norm(inputStates[0:3])**2.*np.eye(3)- 2*np.outer(inputStates[0:3],inputStates[0:3]))/np.linalg.norm(inputStates[0:3])**4.) - expDynMat[0:3, 3:6] = np.eye(3) - np.outer(inputStates[0:3],inputStates[0:3])/np.linalg.norm(inputStates[0:3])**2 + expDynMat = np.zeros([6, 6]) + expDynMat[0:3, 0:3] = -( + np.outer(inputStates[0:3], inputStates[3:6]) + / np.linalg.norm(inputStates[0:3]) ** 2.0 + + np.dot(inputStates[3:6], inputStates[0:3]) + * ( + np.linalg.norm(inputStates[0:3]) ** 2.0 * np.eye(3) + - 2 * np.outer(inputStates[0:3], inputStates[0:3]) + ) + / np.linalg.norm(inputStates[0:3]) ** 4.0 + ) + expDynMat[0:3, 3:6] = ( + np.eye(3) + - np.outer(inputStates[0:3], inputStates[0:3]) + / np.linalg.norm(inputStates[0:3]) ** 2 + ) ## Equations when removing the unobservable states from d_dot - expDynMat[3:6, 0:3] = -1/dt*(np.outer(inputStates[0:3],inputStates[3:6])/np.linalg.norm(inputStates[0:3])**2. + - np.dot(inputStates[3:6],inputStates[0:3])*(np.linalg.norm(inputStates[0:3])**2.*np.eye(3)- 2*np.outer(inputStates[0:3],inputStates[0:3]))/np.linalg.norm(inputStates[0:3])**4.) - expDynMat[3:6, 3:6] =- 1/dt*(np.outer(inputStates[0:3],inputStates[0:3])/np.linalg.norm(inputStates[0:3])**2) - - dynMat = sunlineEKF.new_doubleArray(6*6) + expDynMat[3:6, 0:3] = ( + -1 + / dt + * ( + np.outer(inputStates[0:3], inputStates[3:6]) + / np.linalg.norm(inputStates[0:3]) ** 2.0 + + np.dot(inputStates[3:6], inputStates[0:3]) + * ( + np.linalg.norm(inputStates[0:3]) ** 2.0 * np.eye(3) + - 2 * np.outer(inputStates[0:3], inputStates[0:3]) + ) + / np.linalg.norm(inputStates[0:3]) ** 4.0 + ) + ) + expDynMat[3:6, 3:6] = ( + -1 + / dt + * ( + np.outer(inputStates[0:3], inputStates[0:3]) + / np.linalg.norm(inputStates[0:3]) ** 2 + ) + ) + + dynMat = sunlineEKF.new_doubleArray(6 * 6) for i in range(36): sunlineEKF.doubleArray_setitem(dynMat, i, 0.0) sunlineEKF.sunlineDynMatrix(inputStates, dt, dynMat) @@ -125,7 +200,7 @@ def sunline_individual_test(): DynOut = np.array(DynOut).reshape(6, 6) errorNorm = np.linalg.norm(expDynMat - DynOut) - if(errorNorm > 1.0E-10): + if errorNorm > 1.0e-10: print(errorNorm) testFailCount += 1 testMessages.append("Dynamics Matrix generation Failure \n") @@ -134,19 +209,21 @@ def sunline_individual_test(): ## STM and State Test ################################################################################### - inputStates = [2,1,0.75, 1.5, 0.5, 0.5] - dt =0.5 + inputStates = [2, 1, 0.75, 1.5, 0.5, 0.5] + dt = 0.5 stateTransition = sunlineEKF.new_doubleArray(36) states = sunlineEKF.new_doubleArray(6) for i in range(6): sunlineEKF.doubleArray_setitem(states, i, inputStates[i]) for j in range(6): - if i==j: - sunlineEKF.doubleArray_setitem(stateTransition, 6*i+j, 1.0) + if i == j: + sunlineEKF.doubleArray_setitem(stateTransition, 6 * i + j, 1.0) else: - sunlineEKF.doubleArray_setitem(stateTransition, 6*i+j, 0.0) + sunlineEKF.doubleArray_setitem(stateTransition, 6 * i + j, 0.0) - sunlineEKF.sunlineStateSTMProp(expDynMat.flatten().tolist(), dt, states, stateTransition) + sunlineEKF.sunlineStateSTMProp( + expDynMat.flatten().tolist(), dt, states, stateTransition + ) PropStateOut = [] PropSTMOut = [] @@ -155,23 +232,37 @@ def sunline_individual_test(): for i in range(36): PropSTMOut.append(sunlineEKF.doubleArray_getitem(stateTransition, i)) - STMout = np.array(PropSTMOut).reshape([6,6]) + STMout = np.array(PropSTMOut).reshape([6, 6]) StatesOut = np.array(PropStateOut) - expectedSTM = dt*np.dot(expDynMat, np.eye(6)) + np.eye(6) + expectedSTM = dt * np.dot(expDynMat, np.eye(6)) + np.eye(6) expectedStates = np.zeros(6) inputStatesArray = np.array(inputStates) ## Equations when removing the unobservable states from d_dot - expectedStates[3:6] = np.array(inputStatesArray[3:6] - np.dot(inputStatesArray[3:6], inputStatesArray[0:3])*inputStatesArray[0:3]/np.linalg.norm(inputStatesArray[0:3])**2.) - expectedStates[0:3] = np.array(inputStatesArray[0:3] + dt*(inputStatesArray[3:6] - np.dot(inputStatesArray[3:6], inputStatesArray[0:3])*inputStatesArray[0:3]/np.linalg.norm(inputStatesArray[0:3])**2.)) + expectedStates[3:6] = np.array( + inputStatesArray[3:6] + - np.dot(inputStatesArray[3:6], inputStatesArray[0:3]) + * inputStatesArray[0:3] + / np.linalg.norm(inputStatesArray[0:3]) ** 2.0 + ) + expectedStates[0:3] = np.array( + inputStatesArray[0:3] + + dt + * ( + inputStatesArray[3:6] + - np.dot(inputStatesArray[3:6], inputStatesArray[0:3]) + * inputStatesArray[0:3] + / np.linalg.norm(inputStatesArray[0:3]) ** 2.0 + ) + ) errorNormSTM = np.linalg.norm(expectedSTM - STMout) errorNormStates = np.linalg.norm(expectedStates - StatesOut) - if(errorNormSTM > 1.0E-10): + if errorNormSTM > 1.0e-10: testFailCount += 1 testMessages.append("STM Propagation Failure \n") - if(errorNormStates > 1.0E-10): + if errorNormStates > 1.0e-10: testFailCount += 1 testMessages.append("State Propagation Failure \n") @@ -180,42 +271,84 @@ def sunline_individual_test(): ################################################################################### numCSS = 4 - cssCos = [np.cos(np.deg2rad(10.)), np.cos(np.deg2rad(25.)), np.cos(np.deg2rad(5.)), np.cos(np.deg2rad(90.))] - sensorTresh = np.cos(np.deg2rad(50.)) - cssNormals = [1.,0.,0.,0.,1.,0., 0.,0.,1., 1./np.sqrt(2), 1./np.sqrt(2),0.] + cssCos = [ + np.cos(np.deg2rad(10.0)), + np.cos(np.deg2rad(25.0)), + np.cos(np.deg2rad(5.0)), + np.cos(np.deg2rad(90.0)), + ] + sensorTresh = np.cos(np.deg2rad(50.0)) + cssNormals = [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 1.0 / np.sqrt(2), + 1.0 / np.sqrt(2), + 0.0, + ] cssBias = [1.0 for i in range(numCSS)] - measMat = sunlineEKF.new_doubleArray(8*6) + measMat = sunlineEKF.new_doubleArray(8 * 6) obs = sunlineEKF.new_doubleArray(8) yMeas = sunlineEKF.new_doubleArray(8) numObs = sunlineEKF.new_intArray(1) - for i in range(8*6): - sunlineEKF.doubleArray_setitem(measMat, i, 0.) + for i in range(8 * 6): + sunlineEKF.doubleArray_setitem(measMat, i, 0.0) for i in range(8): sunlineEKF.doubleArray_setitem(obs, i, 0.0) sunlineEKF.doubleArray_setitem(yMeas, i, 0.0) - sunlineEKF.sunlineHMatrixYMeas(inputStates, numCSS, cssCos, sensorTresh, cssNormals, cssBias, obs, yMeas, numObs, measMat) + sunlineEKF.sunlineHMatrixYMeas( + inputStates, + numCSS, + cssCos, + sensorTresh, + cssNormals, + cssBias, + obs, + yMeas, + numObs, + measMat, + ) obsOut = [] yMeasOut = [] numObsOut = [] HOut = [] - for i in range(8*6): + for i in range(8 * 6): HOut.append(sunlineEKF.doubleArray_getitem(measMat, i)) for i in range(8): yMeasOut.append(sunlineEKF.doubleArray_getitem(yMeas, i)) obsOut.append(sunlineEKF.doubleArray_getitem(obs, i)) numObsOut.append(sunlineEKF.intArray_getitem(numObs, 0)) - #Fill in expected values for test - expectedH = np.zeros([8,6]) + # Fill in expected values for test + expectedH = np.zeros([8, 6]) expectedY = np.zeros(8) for j in range(3): - expectedH[j,0:3] = np.eye(3)[j,:] - expectedY[j] =np.array(cssCos[j]) - np.dot(np.array(inputStates)[0:3], np.array(cssNormals)[j*3:(j+1)*3]) - expectedObs = np.array([np.cos(np.deg2rad(10.)), np.cos(np.deg2rad(25.)), np.cos(np.deg2rad(5.)),0.,0.,0.,0.,0.]) + expectedH[j, 0:3] = np.eye(3)[j, :] + expectedY[j] = np.array(cssCos[j]) - np.dot( + np.array(inputStates)[0:3], np.array(cssNormals)[j * 3 : (j + 1) * 3] + ) + expectedObs = np.array( + [ + np.cos(np.deg2rad(10.0)), + np.cos(np.deg2rad(25.0)), + np.cos(np.deg2rad(5.0)), + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + ) expectedNumObs = 3 HOut = np.array(HOut).reshape([8, 6]) @@ -226,7 +359,7 @@ def sunline_individual_test(): errorNorm[3] = np.linalg.norm(numObsOut[0] - expectedNumObs) for i in range(4): - if(errorNorm[i] > 1.0E-10): + if errorNorm[i] > 1.0e-10: testFailCount += 1 testMessages.append("H and yMeas update failure \n") @@ -235,20 +368,100 @@ def sunline_individual_test(): ################################################################################### numObs = 3 - h = [1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.] - covar = [1., 0., 0., 1., 0., 0., - 0., 1., 0., 0., 1., 0., - 0., 0., 1., 0., 0., 1., - 1., 0., 0., 1., 0., 0., - 0., 1., 0., 0., 1., 0., - 0., 0., 1., 0., 0., 1.] - noise= 0.01 + h = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + covar = [ + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + ] + noise = 0.01 Kalman = sunlineEKF.new_doubleArray(6 * 8) for i in range(8 * 6): - sunlineEKF.doubleArray_setitem(Kalman, i, 0.) + sunlineEKF.doubleArray_setitem(Kalman, i, 0.0) sunlineEKF.sunlineKalmanGain(covar, h, noise, numObs, Kalman) @@ -257,15 +470,21 @@ def sunline_individual_test(): KalmanOut.append(sunlineEKF.doubleArray_getitem(Kalman, i)) # Fill in expected values for test - Hmat = np.array(h).reshape([8,6]) - Pk = np.array(covar).reshape([6,6]) - R = noise*np.eye(3) - expectedK = np.dot(np.dot(Pk, Hmat[0:numObs,:].T), np.linalg.inv(np.dot(np.dot(Hmat[0:numObs,:], Pk), Hmat[0:numObs,:].T) + R[0:numObs,0:numObs])) - - KalmanOut = np.array(KalmanOut)[0:6*numObs].reshape([6, 3]) - errorNorm = np.linalg.norm(KalmanOut[:,0:numObs] - expectedK) - - if (errorNorm > 1.0E-10): + Hmat = np.array(h).reshape([8, 6]) + Pk = np.array(covar).reshape([6, 6]) + R = noise * np.eye(3) + expectedK = np.dot( + np.dot(Pk, Hmat[0:numObs, :].T), + np.linalg.inv( + np.dot(np.dot(Hmat[0:numObs, :], Pk), Hmat[0:numObs, :].T) + + R[0:numObs, 0:numObs] + ), + ) + + KalmanOut = np.array(KalmanOut)[0 : 6 * numObs].reshape([6, 3]) + errorNorm = np.linalg.norm(KalmanOut[:, 0:numObs] - expectedK) + + if errorNorm > 1.0e-10: print(errorNorm) testFailCount += 1 testMessages.append("Kalman Gain update failure \n") @@ -274,38 +493,140 @@ def sunline_individual_test(): ## Test the EKF update ################################################################################### - KGain = [1.,2.,3., 0., 1., 2., 1., 0., 1., 0., 1., 0., 3., 0., 1., 0., 2., 0.] - for i in range(6*8-6*3): - KGain.append(0.) - inputStates = [2,1,0.75,0.1,0.4,0.05] + KGain = [ + 1.0, + 2.0, + 3.0, + 0.0, + 1.0, + 2.0, + 1.0, + 0.0, + 1.0, + 0.0, + 1.0, + 0.0, + 3.0, + 0.0, + 1.0, + 0.0, + 2.0, + 0.0, + ] + for i in range(6 * 8 - 6 * 3): + KGain.append(0.0) + inputStates = [2, 1, 0.75, 0.1, 0.4, 0.05] xbar = [0.1, 0.2, 0.01, 0.005, 0.009, 0.001] numObs = 3 - h = [1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.] - covar = [1., 0., 0., 1., 0., 0., - 0., 1., 0., 0., 1., 0., - 0., 0., 1., 0., 0., 1., - 1., 0., 0., 1., 0., 0., - 0., 1., 0., 0., 1., 0., - 0., 0., 1., 0., 0., 1.] + h = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + covar = [ + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + ] noise = 0.01 inputY = np.zeros(3) for j in range(3): - inputY[j] = np.array(cssCos[j]) - np.dot(np.array(inputStates)[0:3], np.array(cssNormals)[j * 3:(j + 1) * 3]) + inputY[j] = np.array(cssCos[j]) - np.dot( + np.array(inputStates)[0:3], np.array(cssNormals)[j * 3 : (j + 1) * 3] + ) inputY = inputY.tolist() stateError = sunlineEKF.new_doubleArray(6) - covarMat = sunlineEKF.new_doubleArray(6*6) + covarMat = sunlineEKF.new_doubleArray(6 * 6) inputs = sunlineEKF.new_doubleArray(6) - for i in range(6): - sunlineEKF.doubleArray_setitem(stateError, i, 0.) + sunlineEKF.doubleArray_setitem(stateError, i, 0.0) sunlineEKF.doubleArray_setitem(inputs, i, inputStates[i]) for j in range(6): - sunlineEKF.doubleArray_setitem(covarMat,i+j,0.) + sunlineEKF.doubleArray_setitem(covarMat, i + j, 0.0) - sunlineEKF.sunlineEKFUpdate(KGain, covar, noise, numObs, inputY, h, inputs, stateError, covarMat) + sunlineEKF.sunlineEKFUpdate( + KGain, covar, noise, numObs, inputY, h, inputs, stateError, covarMat + ) stateOut = [] covarOut = [] @@ -317,19 +638,21 @@ def sunline_individual_test(): covarOut.append(sunlineEKF.doubleArray_getitem(covarMat, j)) # Fill in expected values for test - KK = np.array(KGain)[0:6*3].reshape([6,3]) + KK = np.array(KGain)[0 : 6 * 3].reshape([6, 3]) expectedStates = np.array(inputStates) + np.dot(KK, np.array(inputY)) - H = np.array(h).reshape([8,6])[0:3,:] + H = np.array(h).reshape([8, 6])[0:3, :] Pk = np.array(covar).reshape([6, 6]) R = noise * np.eye(3) - expectedP = np.dot(np.dot(np.eye(6) - np.dot(KK, H), Pk), np.transpose(np.eye(6) - np.dot(KK, H))) + np.dot(KK, np.dot(R,KK.T)) + expectedP = np.dot( + np.dot(np.eye(6) - np.dot(KK, H), Pk), np.transpose(np.eye(6) - np.dot(KK, H)) + ) + np.dot(KK, np.dot(R, KK.T)) errorNorm = np.zeros(2) errorNorm[0] = np.linalg.norm(np.array(stateOut) - expectedStates) - errorNorm[1] = np.linalg.norm(expectedP - np.array(covarOut).reshape([6,6])) + errorNorm[1] = np.linalg.norm(expectedP - np.array(covarOut).reshape([6, 6])) for i in range(2): - if(errorNorm[i] > 1.0E-10): + if errorNorm[i] > 1.0e-10: testFailCount += 1 testMessages.append("EKF update failure \n") @@ -337,25 +660,125 @@ def sunline_individual_test(): ## Test the CKF update ################################################################################### - KGain = [1., 2., 3., 0., 1., 2., 1., 0., 1., 0., 1., 0., 3., 0., 1., 0., 2., 0.] + KGain = [ + 1.0, + 2.0, + 3.0, + 0.0, + 1.0, + 2.0, + 1.0, + 0.0, + 1.0, + 0.0, + 1.0, + 0.0, + 3.0, + 0.0, + 1.0, + 0.0, + 2.0, + 0.0, + ] for i in range(6 * 8 - 6 * 3): - KGain.append(0.) + KGain.append(0.0) inputStates = [2, 1, 0.75, 0.1, 0.4, 0.05] xbar = [0.1, 0.2, 0.01, 0.005, 0.009, 0.001] numObs = 3 - h = [1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.] - covar = [1., 0., 0., 1., 0., 0., - 0., 1., 0., 0., 1., 0., - 0., 0., 1., 0., 0., 1., - 1., 0., 0., 1., 0., 0., - 0., 1., 0., 0., 1., 0., - 0., 0., 1., 0., 0., 1.] - noise =0.01 + h = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + covar = [ + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + ] + noise = 0.01 inputY = np.zeros(3) for j in range(3): - inputY[j] = np.array(cssCos[j]) - np.dot(np.array(inputStates)[0:3], - np.array(cssNormals)[j * 3:(j + 1) * 3]) + inputY[j] = np.array(cssCos[j]) - np.dot( + np.array(inputStates)[0:3], np.array(cssNormals)[j * 3 : (j + 1) * 3] + ) inputY = inputY.tolist() stateError = sunlineEKF.new_doubleArray(6) @@ -364,9 +787,11 @@ def sunline_individual_test(): for i in range(6): sunlineEKF.doubleArray_setitem(stateError, i, xbar[i]) for j in range(6): - sunlineEKF.doubleArray_setitem(covarMat, i + j, 0.) + sunlineEKF.doubleArray_setitem(covarMat, i + j, 0.0) - sunlineEKF.sunlineCKFUpdate(xbar, KGain, covar, noise, numObs, inputY, h, stateError, covarMat) + sunlineEKF.sunlineCKFUpdate( + xbar, KGain, covar, noise, numObs, inputY, h, stateError, covarMat + ) covarOut = [] errorOut = [] @@ -376,21 +801,22 @@ def sunline_individual_test(): covarOut.append(sunlineEKF.doubleArray_getitem(covarMat, j)) # Fill in expected values for test - KK = np.array(KGain)[0:6 * 3].reshape([6, 3]) + KK = np.array(KGain)[0 : 6 * 3].reshape([6, 3]) H = np.array(h).reshape([8, 6])[0:3, :] - expectedStateError = np.array(xbar) + np.dot(KK, (np.array(inputY) - np.dot(H, np.array(xbar)))) + expectedStateError = np.array(xbar) + np.dot( + KK, (np.array(inputY) - np.dot(H, np.array(xbar))) + ) Pk = np.array(covar).reshape([6, 6]) - expectedP = np.dot(np.dot(np.eye(6) - np.dot(KK, H), Pk), np.transpose(np.eye(6) - np.dot(KK, H))) + np.dot(KK, - np.dot( - R, - KK.T)) + expectedP = np.dot( + np.dot(np.eye(6) - np.dot(KK, H), Pk), np.transpose(np.eye(6) - np.dot(KK, H)) + ) + np.dot(KK, np.dot(R, KK.T)) errorNorm = np.zeros(2) errorNorm[0] = np.linalg.norm(np.array(errorOut) - expectedStateError) errorNorm[1] = np.linalg.norm(expectedP - np.array(covarOut).reshape([6, 6])) for i in range(2): - if (errorNorm[i] > 1.0E-10): + if errorNorm[i] > 1.0e-10: testFailCount += 1 testMessages.append("CKF update failure \n") @@ -402,7 +828,8 @@ def sunline_individual_test(): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + #################################################################################### # Test for the time and update with static states (zero d_dot) @@ -435,7 +862,7 @@ def StatePropStatic(): unitTestSim.AddModelToTask(unitTaskName, module) setupFilterData(module) - kfLog = module.logger(["covar", "state"], testProcessRate*10) + kfLog = module.logger(["covar", "state"], testProcessRate * 10) unitTestSim.AddModelToTask(unitTaskName, kfLog) # connect messages @@ -451,7 +878,7 @@ def StatePropStatic(): stateLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.state) for i in range(6): - if (abs(stateLog[-1, i + 1] - stateLog[0, i + 1]) > 1.0E-10): + if abs(stateLog[-1, i + 1] - stateLog[0, i + 1]) > 1.0e-10: testFailCount += 1 testMessages.append("State propagation failure \n") @@ -463,7 +890,7 @@ def StatePropStatic(): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] #################################################################################### @@ -493,14 +920,14 @@ def StatePropVariable(show_plots): module = sunlineEKF.sunlineEKF() module.ModelTag = "SunlineEKF" - - # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) setupFilterData(module) - InitialState = (np.array(module.state)+ +np.array([0.,0.,0.,0.0001,0.002, 0.001])).tolist() + InitialState = ( + np.array(module.state) + +np.array([0.0, 0.0, 0.0, 0.0001, 0.002, 0.001]) + ).tolist() Initialx = module.x InitialCovar = module.covar @@ -519,72 +946,129 @@ def StatePropVariable(show_plots): unitTestSim.ConfigureStopTime(macros.sec2nano(1000.0)) unitTestSim.ExecuteSimulation() - covarLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.covar) stateLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.state) stateErrorLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.x) stmLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.stateTransition) - dt = 0.5 - expectedStateArray = np.zeros([2001,7]) - expectedStateArray[0,1:7] = np.array(InitialState) - - for i in range(1,2001): - expectedStateArray[i,0] = dt*i*1E9 - expectedStateArray[i,1:4] = expectedStateArray[i-1,1:4] + dt*(expectedStateArray[i-1,4:7] - (np.dot(expectedStateArray[i-1,4:7],expectedStateArray[i-1,1:4]))*expectedStateArray[i-1,1:4]/np.linalg.norm(expectedStateArray[i-1,1:4])**2.) + expectedStateArray = np.zeros([2001, 7]) + expectedStateArray[0, 1:7] = np.array(InitialState) + + for i in range(1, 2001): + expectedStateArray[i, 0] = dt * i * 1e9 + expectedStateArray[i, 1:4] = expectedStateArray[i - 1, 1:4] + dt * ( + expectedStateArray[i - 1, 4:7] + - (np.dot(expectedStateArray[i - 1, 4:7], expectedStateArray[i - 1, 1:4])) + * expectedStateArray[i - 1, 1:4] + / np.linalg.norm(expectedStateArray[i - 1, 1:4]) ** 2.0 + ) ## Equations when removing the unobservable states from d_dot - expectedStateArray[i, 4:7] = expectedStateArray[i-1,4:7] - (np.dot(expectedStateArray[i-1,4:7],expectedStateArray[i-1,1:4]))*expectedStateArray[i-1,1:4]/np.linalg.norm(expectedStateArray[i-1,1:4])**2. - - expDynMat = np.zeros([2001,6,6]) - for i in range(0,2001): - expDynMat[i, 0:3, 0:3] = -(np.outer(expectedStateArray[i,1:4],expectedStateArray[i,4:7])/np.linalg.norm(expectedStateArray[i,1:4])**2. + - np.dot(expectedStateArray[i,4:7], expectedStateArray[i,1:4])*(np.linalg.norm(expectedStateArray[i,1:4])**2.*np.eye(3)- 2*np.outer(expectedStateArray[i,1:4],expectedStateArray[i,1:4]))/np.linalg.norm(expectedStateArray[i,1:4])**4.) - expDynMat[i, 0:3, 3:6] = np.eye(3) - np.outer(expectedStateArray[i,1:4],expectedStateArray[i,1:4])/np.linalg.norm(expectedStateArray[i,1:4])**2 + expectedStateArray[i, 4:7] = ( + expectedStateArray[i - 1, 4:7] + - (np.dot(expectedStateArray[i - 1, 4:7], expectedStateArray[i - 1, 1:4])) + * expectedStateArray[i - 1, 1:4] + / np.linalg.norm(expectedStateArray[i - 1, 1:4]) ** 2.0 + ) + + expDynMat = np.zeros([2001, 6, 6]) + for i in range(0, 2001): + expDynMat[i, 0:3, 0:3] = -( + np.outer(expectedStateArray[i, 1:4], expectedStateArray[i, 4:7]) + / np.linalg.norm(expectedStateArray[i, 1:4]) ** 2.0 + + np.dot(expectedStateArray[i, 4:7], expectedStateArray[i, 1:4]) + * ( + np.linalg.norm(expectedStateArray[i, 1:4]) ** 2.0 * np.eye(3) + - 2 * np.outer(expectedStateArray[i, 1:4], expectedStateArray[i, 1:4]) + ) + / np.linalg.norm(expectedStateArray[i, 1:4]) ** 4.0 + ) + expDynMat[i, 0:3, 3:6] = ( + np.eye(3) + - np.outer(expectedStateArray[i, 1:4], expectedStateArray[i, 1:4]) + / np.linalg.norm(expectedStateArray[i, 1:4]) ** 2 + ) ## Equations when removing the unobservable states from d_dot - expDynMat[i, 3:6, 0:3] = -1/dt*(np.outer(expectedStateArray[i,1:4],expectedStateArray[i,4:7])/np.linalg.norm(expectedStateArray[i,1:4])**2. + - np.dot(expectedStateArray[i,4:7], expectedStateArray[i,1:4])*(np.linalg.norm(expectedStateArray[i,1:4])**2.*np.eye(3)- 2*np.outer(expectedStateArray[i,1:4],expectedStateArray[i,1:4]))/np.linalg.norm(expectedStateArray[i,1:4])**4.) - expDynMat[i, 3:6, 3:6] = -1/dt*(np.outer(expectedStateArray[i,1:4],expectedStateArray[i,1:4])/np.linalg.norm(expectedStateArray[i,1:4])**2) - - expectedSTM = np.zeros([2001,6,6]) - expectedSTM[0,:,:] = np.eye(6) - for i in range(1,2001): - expectedSTM[i,:,:] = dt * np.dot(expDynMat[i-1,:,:], np.eye(6)) + np.eye(6) - - expectedXBar = np.zeros([2001,7]) - expectedXBar[0,1:7] = np.array(Initialx) - for i in range(1,2001): - expectedXBar[i,0] = dt*i*1E9 + expDynMat[i, 3:6, 0:3] = ( + -1 + / dt + * ( + np.outer(expectedStateArray[i, 1:4], expectedStateArray[i, 4:7]) + / np.linalg.norm(expectedStateArray[i, 1:4]) ** 2.0 + + np.dot(expectedStateArray[i, 4:7], expectedStateArray[i, 1:4]) + * ( + np.linalg.norm(expectedStateArray[i, 1:4]) ** 2.0 * np.eye(3) + - 2 + * np.outer(expectedStateArray[i, 1:4], expectedStateArray[i, 1:4]) + ) + / np.linalg.norm(expectedStateArray[i, 1:4]) ** 4.0 + ) + ) + expDynMat[i, 3:6, 3:6] = ( + -1 + / dt + * ( + np.outer(expectedStateArray[i, 1:4], expectedStateArray[i, 1:4]) + / np.linalg.norm(expectedStateArray[i, 1:4]) ** 2 + ) + ) + + expectedSTM = np.zeros([2001, 6, 6]) + expectedSTM[0, :, :] = np.eye(6) + for i in range(1, 2001): + expectedSTM[i, :, :] = dt * np.dot(expDynMat[i - 1, :, :], np.eye(6)) + np.eye( + 6 + ) + + expectedXBar = np.zeros([2001, 7]) + expectedXBar[0, 1:7] = np.array(Initialx) + for i in range(1, 2001): + expectedXBar[i, 0] = dt * i * 1e9 expectedXBar[i, 1:7] = np.dot(expectedSTM[i, :, :], expectedXBar[i - 1, 1:7]) - expectedCovar = np.zeros([2001,37]) - expectedCovar[0,1:37] = np.array(InitialCovar) + expectedCovar = np.zeros([2001, 37]) + expectedCovar[0, 1:37] = np.array(InitialCovar) Gamma = np.zeros([6, 3]) - Gamma[0:3, 0:3] = dt ** 2. / 2. * np.eye(3) + Gamma[0:3, 0:3] = dt**2.0 / 2.0 * np.eye(3) Gamma[3:6, 0:3] = dt * np.eye(3) - ProcNoiseCovar = np.dot(Gamma, np.dot(module.qProcVal*np.eye(3),Gamma.T)) - for i in range(1,2001): - expectedCovar[i,0] = dt*i*1E9 - expectedCovar[i,1:37] = (np.dot(expectedSTM[i,:,:], np.dot(np.reshape(expectedCovar[i-1,1:37],[6,6]), np.transpose(expectedSTM[i,:,:])))+ ProcNoiseCovar).flatten() + ProcNoiseCovar = np.dot(Gamma, np.dot(module.qProcVal * np.eye(3), Gamma.T)) + for i in range(1, 2001): + expectedCovar[i, 0] = dt * i * 1e9 + expectedCovar[i, 1:37] = ( + np.dot( + expectedSTM[i, :, :], + np.dot( + np.reshape(expectedCovar[i - 1, 1:37], [6, 6]), + np.transpose(expectedSTM[i, :, :]), + ), + ) + + ProcNoiseCovar + ).flatten() FilterPlots.StatesVsExpected(stateLog, expectedStateArray, show_plots) - FilterPlots.StatesPlotCompare(stateErrorLog, expectedXBar, covarLog, expectedCovar, show_plots) + FilterPlots.StatesPlotCompare( + stateErrorLog, expectedXBar, covarLog, expectedCovar, show_plots + ) - for j in range(1,2001): + for j in range(1, 2001): for i in range(6): - if (abs(stateLog[j, i + 1] - expectedStateArray[j, i + 1]) > 1.0E-4): + if abs(stateLog[j, i + 1] - expectedStateArray[j, i + 1]) > 1.0e-4: testFailCount += 1 testMessages.append("General state propagation failure: State Prop \n") - if (abs(stateErrorLog[j, i + 1] - expectedXBar[j, i + 1]) > 1.0E-4): + if abs(stateErrorLog[j, i + 1] - expectedXBar[j, i + 1]) > 1.0e-4: testFailCount += 1 - testMessages.append("General state propagation failure: State Error Prop \n") + testMessages.append( + "General state propagation failure: State Error Prop \n" + ) for i in range(36): - if (abs(covarLog[j, i + 1] - expectedCovar[j, i + 1]) > 1.0E-4): + if abs(covarLog[j, i + 1] - expectedCovar[j, i + 1]) > 1.0e-4: abs(covarLog[j, i + 1] - expectedCovar[j, i + 1]) testFailCount += 1 - testMessages.append("General state propagation failure: Covariance Prop \n") - if (abs(stmLog[j, i + 1] - expectedSTM[j,:].flatten()[i]) > 1.0E-4): + testMessages.append( + "General state propagation failure: Covariance Prop \n" + ) + if abs(stmLog[j, i + 1] - expectedSTM[j, :].flatten()[i]) > 1.0e-4: testFailCount += 1 testMessages.append("General state propagation failure: STM Prop \n") @@ -596,13 +1080,15 @@ def StatePropVariable(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] #################################################################################### # Test for the full filter with time and measurement update #################################################################################### -def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess): +def StateUpdateSunLine( + show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess +): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. @@ -655,7 +1141,7 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes newCSS.CBias = CSSBias[i] newCSS.nHat_B = CSSHat totalCSSList.append(newCSS) - i = i+1 + i = i + 1 cssConstelation.nCSS = len(CSSOrientationList) cssConstelation.cssVals = totalCSSList @@ -684,7 +1170,9 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes dotList = [] for element in CSSOrientationList: if AddMeasNoise: - dotProd = np.dot(np.array(element), np.array(testVector1)[0:3]) + np.random.normal(0., module.qObsVal) + dotProd = np.dot( + np.array(element), np.array(testVector1)[0:3] + ) + np.random.normal(0.0, module.qObsVal) else: dotProd = np.dot(np.array(element), np.array(testVector1)[0:3]) dotList.append(dotProd) @@ -698,16 +1186,15 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes covarLog = addTimeColumn(dataLog.times(), dataLog.covar) for i in range(6): - if (abs(covarLog[-1, i * 6 + 1 + i] - covarLog[0, i * 6 + 1 + i] / 100.) > 1E-2): + if abs(covarLog[-1, i * 6 + 1 + i] - covarLog[0, i * 6 + 1 + i] / 100.0) > 1e-2: testFailCount += 1 testMessages.append("Covariance update failure") - if (abs(stateLog[-1, i + 1] - stateTarget1[i]) > 1.0E-2): + if abs(stateLog[-1, i + 1] - stateTarget1[i]) > 1.0e-2: testFailCount += 1 testMessages.append("State update failure") - stateTarget2 = testVector2 - stateTarget2 = stateTarget2+[0.,0.,0.] + stateTarget2 = stateTarget2 + [0.0, 0.0, 0.0] inputData = messaging.CSSArraySensorMsgPayload() for i in range(SimHalfLength): @@ -715,14 +1202,16 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes dotList = [] for element in CSSOrientationList: if AddMeasNoise: - dotProd = np.dot(np.array(element), np.array(testVector2)[0:3]) + np.random.normal(0., module.qObsVal) + dotProd = np.dot( + np.array(element), np.array(testVector2)[0:3] + ) + np.random.normal(0.0, module.qObsVal) else: dotProd = np.dot(np.array(element), np.array(testVector2)[0:3]) dotList.append(dotProd) inputData.CosValue = dotList cssDataInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i + SimHalfLength+1) * 0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + SimHalfLength + 1) * 0.5)) unitTestSim.ExecuteSimulation() stateErrorLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.x) @@ -731,15 +1220,15 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes covarLog = addTimeColumn(dataLog.times(), dataLog.covar) for i in range(6): - if (abs(covarLog[-1, i * 6 + 1 + i] - covarLog[0, i * 6 + 1 + i] / 100.) > 1E-2): + if abs(covarLog[-1, i * 6 + 1 + i] - covarLog[0, i * 6 + 1 + i] / 100.0) > 1e-2: testFailCount += 1 testMessages.append("Covariance update failure") - if (abs(stateLog[-1, i + 1] - stateTarget2[i]) > 1.0E-2): + if abs(stateLog[-1, i + 1] - stateTarget2[i]) > 1.0e-2: testFailCount += 1 testMessages.append("State update failure") target1 = np.array(testVector1) - target2 = np.array(testVector2+[0.,0.,0.]) + target2 = np.array(testVector2 + [0.0, 0.0, 0.0]) FilterPlots.StateErrorCovarPlot(stateErrorLog, covarLog, show_plots) FilterPlots.StatesVsTargets(target1, target2, stateLog, show_plots) FilterPlots.PostFitResiduals(postFitLog, module.qObsVal, show_plots) @@ -750,12 +1239,18 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] - + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": # test_all_sunline_ekf(True, 200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]) # StatePropVariable(True) # StatePropStatic() - StateUpdateSunLine(True, 200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0, 0.0]) + StateUpdateSunLine( + True, + 200, + True, + [-0.7, 0.7, 0.0], + [0.8, 0.9, 0.0], + [0.7, 0.7, 0.0, 0.0, 0.0, 0.0], + ) diff --git a/src/fswAlgorithms/attDetermination/sunlineEphem/_UnitTest/test_sunlineEphem.py b/src/fswAlgorithms/attDetermination/sunlineEphem/_UnitTest/test_sunlineEphem.py index 1f3332fdd7..7ab71aec93 100644 --- a/src/fswAlgorithms/attDetermination/sunlineEphem/_UnitTest/test_sunlineEphem.py +++ b/src/fswAlgorithms/attDetermination/sunlineEphem/_UnitTest/test_sunlineEphem.py @@ -26,11 +26,16 @@ import numpy as np import pytest from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import sunlineEphem # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + sunlineEphem, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # import packages as needed e.g. 'numpy', 'ctypes, 'math' etc. @@ -39,22 +44,26 @@ class DataStore: """Container for developer defined variables to be used in test data post-processing and plotting. - Attributes: - variableState (list): an example variable to hold test result data. + Attributes: + variableState (list): an example variable to hold test result data. """ def __init__(self): - self.variableState = None # replace/add with appropriate variables for test result data storing + self.variableState = ( + None # replace/add with appropriate variables for test result data storing + ) def plotData(self): - """All test plotting to be performed here. - - """ - plt.figure(1) # plot a sample variable. - plt.plot(self.variableState[:, 0]*macros.NANO2SEC, self.variableState[:, 1], label='Sample Variable') - plt.legend(loc='upper left') - plt.xlabel('Time [s]') - plt.ylabel('Variable Description [unit]') + """All test plotting to be performed here.""" + plt.figure(1) # plot a sample variable. + plt.plot( + self.variableState[:, 0] * macros.NANO2SEC, + self.variableState[:, 1], + label="Sample Variable", + ) + plt.legend(loc="upper left") + plt.xlabel("Time [s]") + plt.ylabel("Variable Description [unit]") plt.show() @@ -71,7 +80,9 @@ def plotFixture(show_plots): # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ -def test_module(show_plots): # update "module" in this function name to reflect the module name +def test_module( + show_plots, +): # update "module" in this function name to reflect the module name """Module Unit Test""" # each test method requires a single assert method to be called # pass on the testPlotFixture so that the main test function may set the DataStore attributes @@ -80,22 +91,22 @@ def test_module(show_plots): # update "module" in this function name to refl def sunlineEphemTestFunction(show_plots): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container sunlineEphemObj = sunlineEphem.sunlineEphem() - sunlineEphemObj.ModelTag = "sunlineEphem" # update python name of test module + sunlineEphemObj.ModelTag = "sunlineEphem" # update python name of test module # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, sunlineEphemObj) @@ -107,20 +118,20 @@ def sunlineEphemTestFunction(show_plots): vehPosData = messaging.NavTransMsgPayload() sunData = messaging.EphemerisMsgPayload() - # Artificially put sun at the origin. sunData.r_BdyZero_N = [0.0, 0.0, 0.0] vehAttInMsg = messaging.NavAttMsg().write(vehAttData) - # Place spacecraft unit length away on each coordinate axis vehAttData.sigma_BN = [0.0, 0.0, 0.0] - TestVectors = [[-1.0, 0.0, 0.0], - [0.0, -1.0, 0.0], - [0.0, 0.0, -1.0], - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, 1.0]] + TestVectors = [ + [-1.0, 0.0, 0.0], + [0.0, -1.0, 0.0], + [0.0, 0.0, -1.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0], + ] estVector = np.zeros((6, 3)) @@ -140,35 +151,39 @@ def sunlineEphemTestFunction(show_plots): # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime( + macros.sec2nano(1.0) + ) # seconds to stop simulation unitTestSim.ExecuteSimulation() estVector[i] = dataLog.vehSunPntBdy[-1] # reset the module to test this functionality sunlineEphemObj.Reset(1) - # set the filtered output truth states trueVector = [ - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, 1.0], - [-1.0, 0.0, 0.0], - [0.0, -1.0, 0.0], - [0.0, 0.0, -1.0] - ] + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0], + [-1.0, 0.0, 0.0], + [0.0, -1.0, 0.0], + [0.0, 0.0, -1.0], + ] # compare the module results to the truth values accuracy = 1e-12 - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values if not unitTestSupport.isArrayEqual(estVector[i], trueVector[i], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + sunlineEphemObj.ModelTag + " Module failed sunlineEphem " + - " unit test at t=" + str(dataLog.times()[i]*macros.NANO2SEC) + "sec\n") - - - + testMessages.append( + "FAILED: " + + sunlineEphemObj.ModelTag + + " Module failed sunlineEphem " + + " unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) # print out success message if no error were found if testFailCount == 0: @@ -178,7 +193,7 @@ def sunlineEphemTestFunction(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -186,6 +201,6 @@ def sunlineEphemTestFunction(show_plots): # stand-along python script # if __name__ == "__main__": - test_module( # update "subModule" in function name - False # show_plots + test_module( # update "subModule" in function name + False # show_plots ) diff --git a/src/fswAlgorithms/attDetermination/sunlineSEKF/_UnitTest/SunLineSEKF_test_utilities.py b/src/fswAlgorithms/attDetermination/sunlineSEKF/_UnitTest/SunLineSEKF_test_utilities.py index 9b67cdc6f4..664a319692 100644 --- a/src/fswAlgorithms/attDetermination/sunlineSEKF/_UnitTest/SunLineSEKF_test_utilities.py +++ b/src/fswAlgorithms/attDetermination/sunlineSEKF/_UnitTest/SunLineSEKF_test_utilities.py @@ -1,5 +1,6 @@ -''' ''' -''' +""" """ + +""" ISC License Copyright (c) 2016-2017, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,7 +17,7 @@ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. -''' +""" import inspect import os @@ -25,263 +26,290 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('fswAlgorithms') - - +splitPath = path.split("fswAlgorithms") import matplotlib.pyplot as plt -def StatesPlot(x, Pflat, show_plots): - numStates = len(x[0,:])-1 +def StatesPlot(x, Pflat, show_plots): + numStates = len(x[0, :]) - 1 - P = np.zeros([len(Pflat[:,0]),numStates,numStates]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*1E-9 - P[i,:,:] = Pflat[i,1:(numStates*numStates+1)].reshape([numStates,numStates]) + P = np.zeros([len(Pflat[:, 0]), numStates, numStates]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * 1e-9 + P[i, :, :] = Pflat[i, 1 : (numStates * numStates + 1)].reshape( + [numStates, numStates] + ) - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t , x[:, 1], "b", label='Error Filter') - plt.plot(t , 3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t , -3 * np.sqrt(P[:, 0, 0]), 'r--') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.plot(t, 3 * np.sqrt(P[:, 0, 0]), "r--", label="Covar Filter") + plt.plot(t, -3 * np.sqrt(P[:, 0, 0]), "r--") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() - plt.subplot(323) - plt.plot(t , x[:, 2], "b") - plt.plot(t , 3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.title('Second LOS component') + plt.plot(t, x[:, 2], "b") + plt.plot(t, 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 1, 1]), "r--") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(t , x[:, 4], "b") - plt.plot(t , 3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.title('Second rate component') + plt.plot(t, x[:, 4], "b") + plt.plot(t, 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 3, 3]), "r--") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(t , x[:, 3], "b") - plt.plot(t , 3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(t, x[:, 3], "b") + plt.plot(t, 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 2, 2]), "r--") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(t , x[:, 5], "b") - plt.plot(t , 3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.plot(t , -3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(t, x[:, 5], "b") + plt.plot(t, 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.plot(t, -3 * np.sqrt(P[:, 4, 4]), "r--") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesPlot', 'State error and covariance', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesPlot", + "State error and covariance", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() def StatesPlotCompare(x, x2, Pflat, Pflat2, show_plots): - - numStates = len(x[0,:])-1 - - P = np.zeros([len(Pflat[:,0]),numStates,numStates]) - P2 = np.zeros([len(Pflat[:,0]),numStates,numStates]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*1E-9 - P[i,:,:] = Pflat[i,1:(numStates*numStates+1)].reshape([numStates,numStates]) - P2[i, :, :] = Pflat2[i, 1:(numStates*numStates+1)].reshape([numStates, numStates]) - - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + numStates = len(x[0, :]) - 1 + + P = np.zeros([len(Pflat[:, 0]), numStates, numStates]) + P2 = np.zeros([len(Pflat[:, 0]), numStates, numStates]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * 1e-9 + P[i, :, :] = Pflat[i, 1 : (numStates * numStates + 1)].reshape( + [numStates, numStates] + ) + P2[i, :, :] = Pflat2[i, 1 : (numStates * numStates + 1)].reshape( + [numStates, numStates] + ) + + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t[0:30] , x[0:30, 1], "b", label='Error Filter') - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 0, 0]), 'r--') - plt.plot(t[0:30] , x2[0:30, 1], "g", label='Error Expected') - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 0, 0]), 'c--', label='Covar Expected') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 0, 0]), 'c--') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(t[0:30], x[0:30, 1], "b", label="Error Filter") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 0, 0]), "r--", label="Covar Filter") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 0, 0]), "r--") + plt.plot(t[0:30], x2[0:30, 1], "g", label="Error Expected") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 0, 0]), "c--", label="Covar Expected") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 0, 0]), "c--") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(323) - plt.plot(t[0:30] , x[0:30, 2], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 1, 1]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 1, 1]), 'r--') - plt.plot(t[0:30] , x2[0:30, 2], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 1, 1]), 'c--') - plt.title('Second LOS component') + plt.plot(t[0:30], x[0:30, 2], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 1, 1]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 1, 1]), "r--") + plt.plot(t[0:30], x2[0:30, 2], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 1, 1]), "c--") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(t[0:30] , x[0:30, 4], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 3, 3]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 3, 3]), 'r--') - plt.plot(t[0:30] , x2[0:30, 4], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 3, 3]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 3, 3]), 'c--') - plt.title('Second rate component') + plt.plot(t[0:30], x[0:30, 4], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 3, 3]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 3, 3]), "r--") + plt.plot(t[0:30], x2[0:30, 4], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 3, 3]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 3, 3]), "c--") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(t[0:30] , x[0:30, 3], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 2, 2]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 2, 2]), 'r--') - plt.plot(t[0:30] , x2[0:30, 3], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 2, 2]), 'c--') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(t[0:30], x[0:30, 3], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 2, 2]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 2, 2]), "r--") + plt.plot(t[0:30], x2[0:30, 3], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 2, 2]), "c--") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(t[0:30] , x[0:30, 5], "b") - plt.plot(t[0:30] , 3 * np.sqrt(P[0:30, 4, 4]), 'r--') - plt.plot(t[0:30] , -3 * np.sqrt(P[0:30, 4, 4]), 'r--') - plt.plot(t[0:30] , x2[0:30, 5], "g") - plt.plot(t[0:30] , 3 * np.sqrt(P2[0:30, 4, 4]), 'c--') - plt.plot(t[0:30] , -3 * np.sqrt(P2[0:30, 4, 4]), 'c--') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(t[0:30], x[0:30, 5], "b") + plt.plot(t[0:30], 3 * np.sqrt(P[0:30, 4, 4]), "r--") + plt.plot(t[0:30], -3 * np.sqrt(P[0:30, 4, 4]), "r--") + plt.plot(t[0:30], x2[0:30, 5], "g") + plt.plot(t[0:30], 3 * np.sqrt(P2[0:30, 4, 4]), "c--") + plt.plot(t[0:30], -3 * np.sqrt(P2[0:30, 4, 4]), "c--") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesCompare', 'State error and covariance vs expected Values', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesCompare", + "State error and covariance vs expected Values", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() -def PostFitResiduals(Res, noise, show_plots): - MeasNoise = np.zeros(len(Res[:,0])) - t= np.zeros(len(Res[:,0])) - for i in range(len(Res[:,0])): - t[i] = Res[i, 0]*1E-9 - MeasNoise[i] = 3*noise +def PostFitResiduals(Res, noise, show_plots): + MeasNoise = np.zeros(len(Res[:, 0])) + t = np.zeros(len(Res[:, 0])) + for i in range(len(Res[:, 0])): + t[i] = Res[i, 0] * 1e-9 + MeasNoise[i] = 3 * noise # Don't plot zero values, since they mean that no measurement is taken - for j in range(len(Res[0,:])-1): - if -1E-10 < Res[i,j+1] < 1E-10: - Res[i, j+1] = np.nan + for j in range(len(Res[0, :]) - 1): + if -1e-10 < Res[i, j + 1] < 1e-10: + Res[i, j + 1] = np.nan - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(421) - plt.plot(t , Res[:, 1], "b.", label='Residual') - plt.plot(t , MeasNoise, 'r--', label='Covar') - plt.plot(t , -MeasNoise, 'r--') - plt.legend(loc='lower right') - plt.ylim([-10*noise, 10*noise]) - plt.title('First CSS') + plt.plot(t, Res[:, 1], "b.", label="Residual") + plt.plot(t, MeasNoise, "r--", label="Covar") + plt.plot(t, -MeasNoise, "r--") + plt.legend(loc="lower right") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("First CSS") plt.grid() plt.subplot(422) - plt.plot(t , Res[:, 5], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Fifth CSS') + plt.plot(t, Res[:, 5], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Fifth CSS") plt.grid() plt.subplot(423) - plt.plot(t , Res[:, 2], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Second CSS') + plt.plot(t, Res[:, 2], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Second CSS") plt.grid() plt.subplot(424) - plt.plot(t , Res[:, 6], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Sixth CSS') + plt.plot(t, Res[:, 6], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Sixth CSS") plt.grid() plt.subplot(425) - plt.plot(t , Res[:, 3], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Third CSS') + plt.plot(t, Res[:, 3], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Third CSS") plt.grid() plt.subplot(426) - plt.plot(t , Res[:, 7], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Seventh CSS') + plt.plot(t, Res[:, 7], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Seventh CSS") plt.grid() plt.subplot(427) - plt.plot(t , Res[:, 4], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.xlabel('t(s)') - plt.title('Fourth CSS') + plt.plot(t, Res[:, 4], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.xlabel("t(s)") + plt.title("Fourth CSS") plt.grid() plt.subplot(428) - plt.plot(t , Res[:, 8], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.xlabel('t(s)') - plt.title('Eight CSS') + plt.plot(t, Res[:, 8], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.xlabel("t(s)") + plt.title("Eight CSS") plt.grid() - unitTestSupport.writeFigureLaTeX('PostFit', 'Post Fit Residuals', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "PostFit", + "Post Fit Residuals", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() + def StatesVsExpected(stateLog, expectedStateArray, show_plots): - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 1], 'b--', label='Expected') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1], 'r', label='Filter') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 1], "b--", label="Expected") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1], "r", label="Filter") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(323) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 2], 'b--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2], 'r') - plt.title('Second LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 2], "b--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2], "r") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 4], 'b--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 4], 'r') - plt.title('Second rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 4], "b--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 4], "r") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 3], 'b--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3], 'r') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 3], "b--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3], "r") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(stateLog[:, 0] * 1.0E-9, expectedStateArray[:, 5], 'b--') - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 5], 'r') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, expectedStateArray[:, 5], "b--") + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 5], "r") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesExpected', 'States vs true states in static case', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesExpected", + "States vs true states in static case", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() @@ -289,49 +317,54 @@ def StatesVsExpected(stateLog, expectedStateArray, show_plots): def StatesVsTargets(target1, target2, stateLog, show_plots): + numStates = len(stateLog[0, :]) - 1 - numStates = len(stateLog[0,:])-1 - - target = np.ones([len(stateLog[:, 0]),5]) - for i in range((len(stateLog[:, 0])-1)//2): + target = np.ones([len(stateLog[:, 0]), 5]) + for i in range((len(stateLog[:, 0]) - 1) // 2): target[i, :] = target1 - target[i+(len(stateLog[:, 0]) - 1) // 2,:] = target2 + target[i + (len(stateLog[:, 0]) - 1) // 2, :] = target2 - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 1], 'b', label='Filter') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 0], 'r--', label='Expected') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 1], "b", label="Filter") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 0], "r--", label="Expected") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(323) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 2], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 1], 'r--') - plt.title('Second LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 2], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 1], "r--") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 4], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 3], 'r--') - plt.title('Second rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 4], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 3], "r--") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 3], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 2], 'r--') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 3], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 2], "r--") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(stateLog[:, 0] * 1.0E-9, stateLog[:, 5], 'b') - plt.plot(stateLog[:, 0] * 1.0E-9, target[:, 4], 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(stateLog[:, 0] * 1.0e-9, stateLog[:, 5], "b") + plt.plot(stateLog[:, 0] * 1.0e-9, target[:, 4], "r--") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesTarget', 'States tracking target values', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesTarget", + "States tracking target values", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() diff --git a/src/fswAlgorithms/attDetermination/sunlineSEKF/_UnitTest/test_SunLineSEKF.py b/src/fswAlgorithms/attDetermination/sunlineSEKF/_UnitTest/test_SunLineSEKF.py index cc54dff6ab..dd2780560f 100644 --- a/src/fswAlgorithms/attDetermination/sunlineSEKF/_UnitTest/test_SunLineSEKF.py +++ b/src/fswAlgorithms/attDetermination/sunlineSEKF/_UnitTest/test_SunLineSEKF.py @@ -1,5 +1,6 @@ -''' ''' -''' +""" """ + +""" ISC License Copyright (c) 2016-2017, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,7 +17,7 @@ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. -''' +""" # This test validates the EKF module by running several # scenarios on both individual functions and the full module. @@ -28,7 +29,9 @@ from Basilisk.fswAlgorithms import sunlineSEKF from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros, RigidBodyKinematics -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import SunLineSEKF_test_utilities as FilterPlots @@ -38,19 +41,42 @@ def addTimeColumn(time, data): def setupFilterData(filterObject): - - filterObject.sensorUseThresh = 0. + filterObject.sensorUseThresh = 0.0 filterObject.state = [0.1, 0.9, 0.1, 0.0, 0.0] filterObject.x = [1.0, 0.0, 1.0, 0.0, 0.1] - filterObject.covar = [0.4, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.4, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.4, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.004, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.004] + filterObject.covar = [ + 0.4, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.4, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.4, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.004, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.004, + ] filterObject.qProcVal = 0.1**2 filterObject.qObsVal = 0.001 - filterObject.eKFSwitch = (4./3)**2 #If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF + filterObject.eKFSwitch = ( + 4.0 / 3 + ) ** 2 # If low (0-5), the CKF kicks in easily, if high (>10) it's mostly only EKF def test_all_functions_sekf(show_plots): @@ -62,29 +88,37 @@ def test_all_functions_sekf(show_plots): [testResults, testMessage] = StatePropVariable(show_plots) assert testResults < 1, testMessage + # uncomment this line is 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(True) + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("SimHalfLength, AddMeasNoise , testVector1 , testVector2, stateGuess", [ - (200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0]), - (2000, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0]), - (200, False ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0]), - (200, False ,[0., 1., 0.] ,[1., 0., 0.], [0.3, 0.0, 0.6, 0.0, 0.0]), - (200, True ,[0.5, 0.5, 0.] ,[0., 1., 0.], [0.7, 0.7, 0.0, 0.0, 0.0]) -]) - +@pytest.mark.parametrize( + "SimHalfLength, AddMeasNoise , testVector1 , testVector2, stateGuess", + [ + (200, True, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0]), + (2000, True, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0]), + (200, False, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0]), + (200, False, [0.0, 1.0, 0.0], [1.0, 0.0, 0.0], [0.3, 0.0, 0.6, 0.0, 0.0]), + (200, True, [0.5, 0.5, 0.0], [0.0, 1.0, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0]), + ], +) # uncomment this line is 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() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -def test_all_sunline_sekf(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess): - [testResults, testMessage] = StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess) +def test_all_sunline_sekf( + show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess +): + [testResults, testMessage] = StateUpdateSunLine( + show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess + ) assert testResults < 1, testMessage @@ -103,14 +137,12 @@ def sunline_individual_test(): ################################################################################### ## Testing dynamics matrix computation ################################################################################### - inputStates = [2,1,0.75,0.1,0.4] - inputOmega_SB_S = [0.,0.1, 0.4] - bVec = [1.,0.,0.] - dt =0.5 + inputStates = [2, 1, 0.75, 0.1, 0.4] + inputOmega_SB_S = [0.0, 0.1, 0.4] + bVec = [1.0, 0.0, 0.0] + dt = 0.5 - dcm_BS = [1., 0., 0., - 0., 1., 0., - 0., 0., 1.] + dcm_BS = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] # Fill in the variables for the test dcm = sunlineSEKF.new_doubleArray(3 * 3) @@ -122,28 +154,28 @@ def sunline_individual_test(): for j in range(9): dcmOut.append(sunlineSEKF.doubleArray_getitem(dcm, j)) - DCM_BS = np.array(dcmOut).reshape([3,3]) + DCM_BS = np.array(dcmOut).reshape([3, 3]) omega_SB_B = np.dot(DCM_BS, np.array(inputOmega_SB_S)) dtilde = RigidBodyKinematics.v3Tilde(np.array(inputStates)[:3]) dBS = np.dot(dtilde, DCM_BS) - expDynMat = np.zeros([numStates,numStates]) - expDynMat[0:3, 0:3] = np.array(RigidBodyKinematics.v3Tilde(omega_SB_B)) + expDynMat = np.zeros([numStates, numStates]) + expDynMat[0:3, 0:3] = np.array(RigidBodyKinematics.v3Tilde(omega_SB_B)) expDynMat[0:3, 3:numStates] = -dBS[:, 1:] - dynMat = sunlineSEKF.new_doubleArray(numStates*numStates) - for i in range(numStates*numStates): + dynMat = sunlineSEKF.new_doubleArray(numStates * numStates) + for i in range(numStates * numStates): sunlineSEKF.doubleArray_setitem(dynMat, i, 0.0) sunlineSEKF.sunlineDynMatrix(inputStates, bVec, dt, dynMat) DynOut = [] - for i in range(numStates*numStates): + for i in range(numStates * numStates): DynOut.append(sunlineSEKF.doubleArray_getitem(dynMat, i)) DynOut = np.array(DynOut).reshape(numStates, numStates) errorNorm = np.linalg.norm(expDynMat - DynOut) - if(errorNorm > 1.0E-10): + if errorNorm > 1.0e-10: print(errorNorm, "Dyn Matrix") testFailCount += 1 testMessages.append("Dynamics Matrix generation Failure Dyn " + "\n") @@ -152,11 +184,11 @@ def sunline_individual_test(): ## STM and State Test ################################################################################### - inputStates = [2,1,0.75,0.1,0.4] - inputOmega = [0.,0.1, 0.4] - bVec_test = [1,0,0] + inputStates = [2, 1, 0.75, 0.1, 0.4] + inputOmega = [0.0, 0.1, 0.4] + bVec_test = [1, 0, 0] dt = 0.5 - stateTransition = sunlineSEKF.new_doubleArray(numStates*numStates) + stateTransition = sunlineSEKF.new_doubleArray(numStates * numStates) states = sunlineSEKF.new_doubleArray(numStates) bVec = sunlineSEKF.new_doubleArray(3) for k in range(3): @@ -164,23 +196,23 @@ def sunline_individual_test(): for i in range(numStates): sunlineSEKF.doubleArray_setitem(states, i, inputStates[i]) for j in range(numStates): - if i==j: - sunlineSEKF.doubleArray_setitem(stateTransition, numStates*i+j, 1.0) + if i == j: + sunlineSEKF.doubleArray_setitem(stateTransition, numStates * i + j, 1.0) else: - sunlineSEKF.doubleArray_setitem(stateTransition, numStates*i+j, 0.0) + sunlineSEKF.doubleArray_setitem(stateTransition, numStates * i + j, 0.0) - sunlineSEKF.sunlineStateSTMProp(expDynMat.flatten().tolist(), bVec_test, dt, states, stateTransition) + sunlineSEKF.sunlineStateSTMProp( + expDynMat.flatten().tolist(), bVec_test, dt, states, stateTransition + ) PropStateOut = [] PropSTMOut = [] for i in range(numStates): PropStateOut.append(sunlineSEKF.doubleArray_getitem(states, i)) - for i in range(numStates*numStates): + for i in range(numStates * numStates): PropSTMOut.append(sunlineSEKF.doubleArray_getitem(stateTransition, i)) - dcm_BS = [1., 0., 0., - 0., 1., 0., - 0., 0., 1.] + dcm_BS = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] # Fill in the variables for the test dcm = sunlineSEKF.new_doubleArray(3 * 3) @@ -194,23 +226,25 @@ def sunline_individual_test(): for j in range(9): dcmOut.append(sunlineSEKF.doubleArray_getitem(dcm, j)) - DCM_BS = np.array(dcmOut).reshape([3,3]) - STMout = np.array(PropSTMOut).reshape([numStates,numStates]) + DCM_BS = np.array(dcmOut).reshape([3, 3]) + STMout = np.array(PropSTMOut).reshape([numStates, numStates]) StatesOut = np.array(PropStateOut) - expectedSTM = dt*np.dot(expDynMat, np.eye(numStates)) + np.eye(numStates) + expectedSTM = dt * np.dot(expDynMat, np.eye(numStates)) + np.eye(numStates) expectedStates = np.zeros(numStates) ## Equations when removing the unobservable states from d_dot expectedStates[3:numStates] = np.array(inputOmega)[1:3] - expectedStates[0:3] = np.array(inputStates)[0:3]+dt*np.cross(np.dot(DCM_BS,np.array(inputOmega)), np.array(inputStates)[0:3]) + expectedStates[0:3] = np.array(inputStates)[0:3] + dt * np.cross( + np.dot(DCM_BS, np.array(inputOmega)), np.array(inputStates)[0:3] + ) errorNormSTM = np.linalg.norm(expectedSTM - STMout) errorNormStates = np.linalg.norm(expectedStates - StatesOut) - if(errorNormSTM > 1.0E-10): + if errorNormSTM > 1.0e-10: testFailCount += 1 - testMessages.append("STM Propagation Failure Dyn " + "\n") + testMessages.append("STM Propagation Failure Dyn " + "\n") - if(errorNormStates > 1.0E-10): + if errorNormStates > 1.0e-10: testFailCount += 1 testMessages.append("State Propagation Failure Dyn " + "\n") @@ -219,43 +253,84 @@ def sunline_individual_test(): ################################################################################### numCSS = 4 - cssCos = [np.cos(np.deg2rad(10.)), np.cos(np.deg2rad(25.)), np.cos(np.deg2rad(5.)), np.cos(np.deg2rad(90.))] - sensorTresh = np.cos(np.deg2rad(50.)) - cssNormals = [1.,0.,0.,0.,1.,0., 0.,0.,1., 1./np.sqrt(2), 1./np.sqrt(2),0.] - dcmArray_BS = RigidBodyKinematics.MRP2C([0.1,-0.15,0.2]) + cssCos = [ + np.cos(np.deg2rad(10.0)), + np.cos(np.deg2rad(25.0)), + np.cos(np.deg2rad(5.0)), + np.cos(np.deg2rad(90.0)), + ] + sensorTresh = np.cos(np.deg2rad(50.0)) + cssNormals = [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 1.0 / np.sqrt(2), + 1.0 / np.sqrt(2), + 0.0, + ] + dcmArray_BS = RigidBodyKinematics.MRP2C([0.1, -0.15, 0.2]) dcm_BS = (dcmArray_BS.flatten()).tolist() - measMat = sunlineSEKF.new_doubleArray(8*numStates) + measMat = sunlineSEKF.new_doubleArray(8 * numStates) obs = sunlineSEKF.new_doubleArray(8) yMeas = sunlineSEKF.new_doubleArray(8) numObs = sunlineSEKF.new_intArray(1) - for i in range(8*numStates): - sunlineSEKF.doubleArray_setitem(measMat, i, 0.) + for i in range(8 * numStates): + sunlineSEKF.doubleArray_setitem(measMat, i, 0.0) for i in range(8): sunlineSEKF.doubleArray_setitem(obs, i, 0.0) sunlineSEKF.doubleArray_setitem(yMeas, i, 0.0) - sunlineSEKF.sunlineHMatrixYMeas(inputStates, numCSS, cssCos, sensorTresh, cssNormals, obs, yMeas, numObs, measMat) + sunlineSEKF.sunlineHMatrixYMeas( + inputStates, + numCSS, + cssCos, + sensorTresh, + cssNormals, + obs, + yMeas, + numObs, + measMat, + ) obsOut = [] yMeasOut = [] numObsOut = [] HOut = [] - for i in range(8*numStates): + for i in range(8 * numStates): HOut.append(sunlineSEKF.doubleArray_getitem(measMat, i)) for i in range(8): yMeasOut.append(sunlineSEKF.doubleArray_getitem(yMeas, i)) obsOut.append(sunlineSEKF.doubleArray_getitem(obs, i)) numObsOut.append(sunlineSEKF.intArray_getitem(numObs, 0)) - #Fill in expected values for test - expectedH = np.zeros([8,numStates]) + # Fill in expected values for test + expectedH = np.zeros([8, numStates]) expectedY = np.zeros(8) for j in range(3): - expectedH[j,0:3] = np.eye(3)[j,:] - expectedY[j] =np.array(cssCos[j]) - np.dot( np.array(inputStates)[0:3], np.array(cssNormals)[j*3:(j+1)*3]) - expectedObs = np.array([np.cos(np.deg2rad(10.)), np.cos(np.deg2rad(25.)), np.cos(np.deg2rad(5.)),0.,0.,0.,0.,0.]) + expectedH[j, 0:3] = np.eye(3)[j, :] + expectedY[j] = np.array(cssCos[j]) - np.dot( + np.array(inputStates)[0:3], np.array(cssNormals)[j * 3 : (j + 1) * 3] + ) + expectedObs = np.array( + [ + np.cos(np.deg2rad(10.0)), + np.cos(np.deg2rad(25.0)), + np.cos(np.deg2rad(5.0)), + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + ) expectedNumObs = 3 HOut = np.array(HOut).reshape([8, numStates]) @@ -266,7 +341,7 @@ def sunline_individual_test(): errorNorm[3] = np.linalg.norm(numObsOut[0] - expectedNumObs) for i in range(4): - if(errorNorm[i] > 1.0E-10): + if errorNorm[i] > 1.0e-10: testFailCount += 1 testMessages.append("H and yMeas update failure \n") @@ -275,19 +350,81 @@ def sunline_individual_test(): ################################################################################### numObs = 3 - h = [1., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.] - covar = [1., 0., 0., 1., 0., - 0., 1., 0., 0., 1., - 0., 0., 1., 0., 0., - 1., 0., 0., 1., 0., - 0., 1., 0., 0., 1.] - noise= 0.01 + h = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + covar = [ + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + ] + noise = 0.01 Kalman = sunlineSEKF.new_doubleArray(numStates * 8) for i in range(8 * numStates): - sunlineSEKF.doubleArray_setitem(Kalman, i, 0.) + sunlineSEKF.doubleArray_setitem(Kalman, i, 0.0) sunlineSEKF.sunlineKalmanGain(covar, h, noise, numObs, Kalman) @@ -296,15 +433,21 @@ def sunline_individual_test(): KalmanOut.append(sunlineSEKF.doubleArray_getitem(Kalman, i)) # Fill in expected values for test - Hmat = np.array(h).reshape([8,numStates]) - Pk = np.array(covar).reshape([numStates,numStates]) - R = noise*np.eye(numObs) - expectedK = np.dot(np.dot(Pk, Hmat[0:numObs,:].T), np.linalg.inv(np.dot(np.dot(Hmat[0:numObs,:], Pk), Hmat[0:numObs,:].T) + R[0:numObs,0:numObs])) - - KalmanOut = np.array(KalmanOut)[0:numStates*numObs].reshape([numStates, numObs]) - errorNorm = np.linalg.norm(KalmanOut[:,0:numObs] - expectedK) - - if (errorNorm > 1.0E-10): + Hmat = np.array(h).reshape([8, numStates]) + Pk = np.array(covar).reshape([numStates, numStates]) + R = noise * np.eye(numObs) + expectedK = np.dot( + np.dot(Pk, Hmat[0:numObs, :].T), + np.linalg.inv( + np.dot(np.dot(Hmat[0:numObs, :], Pk), Hmat[0:numObs, :].T) + + R[0:numObs, 0:numObs] + ), + ) + + KalmanOut = np.array(KalmanOut)[0 : numStates * numObs].reshape([numStates, numObs]) + errorNorm = np.linalg.norm(KalmanOut[:, 0:numObs] - expectedK) + + if errorNorm > 1.0e-10: print(errorNorm, "Kalman Gain Error") testFailCount += 1 testMessages.append("Kalman Gain update failure \n") @@ -313,37 +456,102 @@ def sunline_individual_test(): ## Test the EKF update ################################################################################### - KGain = [1., 2., 3., 0., 1., 1., 0., 1., 0., 1., 3., 0., 1., 0., 2.] - for i in range(numStates*8-numStates*numObs): - KGain.append(0.) - inputStates = [2,1,0.75,0.1,0.4] + KGain = [1.0, 2.0, 3.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 3.0, 0.0, 1.0, 0.0, 2.0] + for i in range(numStates * 8 - numStates * numObs): + KGain.append(0.0) + inputStates = [2, 1, 0.75, 0.1, 0.4] xbar = [0.1, 0.2, 0.01, 0.005, 0.009] numObs = 3 - h = [1., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.] - covar = [1., 0., 0., 1., 0., - 0., 1., 0., 0., 1., - 0., 0., 1., 0., 0., - 1., 0., 0., 1., 0., - 0., 1., 0., 0., 1.] + h = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + covar = [ + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + ] noise = 0.01 inputY = np.zeros(3) for j in range(3): - inputY[j] = np.array(cssCos[j]) - np.dot(np.array(inputStates)[0:3], np.array(cssNormals)[j * 3:(j + 1) * 3]) + inputY[j] = np.array(cssCos[j]) - np.dot( + np.array(inputStates)[0:3], np.array(cssNormals)[j * 3 : (j + 1) * 3] + ) inputY = inputY.tolist() stateError = sunlineSEKF.new_doubleArray(numStates) - covarMat = sunlineSEKF.new_doubleArray(numStates*numStates) + covarMat = sunlineSEKF.new_doubleArray(numStates * numStates) inputs = sunlineSEKF.new_doubleArray(numStates) - for i in range(numStates): - sunlineSEKF.doubleArray_setitem(stateError, i, 0.) + sunlineSEKF.doubleArray_setitem(stateError, i, 0.0) sunlineSEKF.doubleArray_setitem(inputs, i, inputStates[i]) for j in range(numStates): - sunlineSEKF.doubleArray_setitem(covarMat,i+j,0.) + sunlineSEKF.doubleArray_setitem(covarMat, i + j, 0.0) - sunlineSEKF.sunlineSEKFUpdate(KGain, covar, noise, numObs, inputY, h, inputs, stateError, covarMat) + sunlineSEKF.sunlineSEKFUpdate( + KGain, covar, noise, numObs, inputY, h, inputs, stateError, covarMat + ) stateOut = [] covarOut = [] @@ -351,23 +559,28 @@ def sunline_individual_test(): for i in range(numStates): stateOut.append(sunlineSEKF.doubleArray_getitem(inputs, i)) errorOut.append(sunlineSEKF.doubleArray_getitem(stateError, i)) - for j in range(numStates*numStates): + for j in range(numStates * numStates): covarOut.append(sunlineSEKF.doubleArray_getitem(covarMat, j)) # Fill in expected values for test - KK = np.array(KGain)[0:numStates*3].reshape([numStates,3]) + KK = np.array(KGain)[0 : numStates * 3].reshape([numStates, 3]) expectedStates = np.array(inputStates) + np.dot(KK, np.array(inputY)) - H = np.array(h).reshape([8,numStates])[0:3,:] + H = np.array(h).reshape([8, numStates])[0:3, :] Pk = np.array(covar).reshape([numStates, numStates]) R = noise * np.eye(3) - expectedP = np.dot(np.dot(np.eye(numStates) - np.dot(KK, H), Pk), np.transpose(np.eye(numStates) - np.dot(KK, H))) + np.dot(KK, np.dot(R,KK.T)) + expectedP = np.dot( + np.dot(np.eye(numStates) - np.dot(KK, H), Pk), + np.transpose(np.eye(numStates) - np.dot(KK, H)), + ) + np.dot(KK, np.dot(R, KK.T)) errorNorm = np.zeros(2) errorNorm[0] = np.linalg.norm(np.array(stateOut) - expectedStates) - errorNorm[1] = np.linalg.norm(expectedP - np.array(covarOut).reshape([numStates,numStates])) + errorNorm[1] = np.linalg.norm( + expectedP - np.array(covarOut).reshape([numStates, numStates]) + ) for i in range(2): - if(errorNorm[i] > 1.0E-10): + if errorNorm[i] > 1.0e-10: testFailCount += 1 testMessages.append("EKF update failure \n") @@ -375,23 +588,86 @@ def sunline_individual_test(): ## Test the CKF update ################################################################################### - KGain = [1., 2., 3., 0., 1., 1., 0., 1., 0., 1., 3., 0., 1., 0., 2.] + KGain = [1.0, 2.0, 3.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 3.0, 0.0, 1.0, 0.0, 2.0] for i in range(numStates * 8 - numStates * 3): - KGain.append(0.) - inputStates = [2,1,0.75,0.1,0.4] + KGain.append(0.0) + inputStates = [2, 1, 0.75, 0.1, 0.4] xbar = [0.1, 0.2, 0.01, 0.005, 0.009] - h = [1., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.] - covar = [1., 0., 0., 1., 0., - 0., 1., 0., 0., 1., - 0., 0., 1., 0., 0., - 1., 0., 0., 1., 0., - 0., 1., 0., 0., 1.] - noise =0.01 + h = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + covar = [ + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + ] + noise = 0.01 inputY = np.zeros(numObs) for j in range(numObs): - inputY[j] = np.array(cssCos[j]) - np.dot(np.array(inputStates)[0:3], - np.array(cssNormals)[j * 3:(j + 1) * 3]) + inputY[j] = np.array(cssCos[j]) - np.dot( + np.array(inputStates)[0:3], np.array(cssNormals)[j * 3 : (j + 1) * 3] + ) inputY = inputY.tolist() stateError = sunlineSEKF.new_doubleArray(numStates) @@ -400,33 +676,39 @@ def sunline_individual_test(): for i in range(numStates): sunlineSEKF.doubleArray_setitem(stateError, i, xbar[i]) for j in range(numStates): - sunlineSEKF.doubleArray_setitem(covarMat, i + j, 0.) + sunlineSEKF.doubleArray_setitem(covarMat, i + j, 0.0) - sunlineSEKF.sunlineCKFUpdate(xbar, KGain, covar, noise, numObs, inputY, h, stateError, covarMat) + sunlineSEKF.sunlineCKFUpdate( + xbar, KGain, covar, noise, numObs, inputY, h, stateError, covarMat + ) covarOut = [] errorOut = [] for i in range(numStates): errorOut.append(sunlineSEKF.doubleArray_getitem(stateError, i)) - for j in range(numStates*numStates): + for j in range(numStates * numStates): covarOut.append(sunlineSEKF.doubleArray_getitem(covarMat, j)) # Fill in expected values for test - KK = np.array(KGain)[0:numStates * numObs].reshape([numStates, numObs]) + KK = np.array(KGain)[0 : numStates * numObs].reshape([numStates, numObs]) H = np.array(h).reshape([8, numStates])[0:3, :] - expectedStateError = np.array(xbar) + np.dot(KK, (np.array(inputY) - np.dot(H, np.array(xbar)))) + expectedStateError = np.array(xbar) + np.dot( + KK, (np.array(inputY) - np.dot(H, np.array(xbar))) + ) Pk = np.array(covar).reshape([numStates, numStates]) - expectedP = np.dot(np.dot(np.eye(numStates) - np.dot(KK, H), Pk), np.transpose(np.eye(numStates) - np.dot(KK, H))) + np.dot(KK, - np.dot( - R, - KK.T)) + expectedP = np.dot( + np.dot(np.eye(numStates) - np.dot(KK, H), Pk), + np.transpose(np.eye(numStates) - np.dot(KK, H)), + ) + np.dot(KK, np.dot(R, KK.T)) errorNorm = np.zeros(2) errorNorm[0] = np.linalg.norm(np.array(errorOut) - expectedStateError) - errorNorm[1] = np.linalg.norm(expectedP - np.array(covarOut).reshape([numStates, numStates])) + errorNorm[1] = np.linalg.norm( + expectedP - np.array(covarOut).reshape([numStates, numStates]) + ) for i in range(2): - if (errorNorm[i] > 1.0E-10): + if errorNorm[i] > 1.0e-10: testFailCount += 1 testMessages.append("CKF update failure \n") @@ -436,22 +718,25 @@ def sunline_individual_test(): inputStates = [2, 1, 0.75, 0.1, 0.4] sunheading = inputStates[:3] - bvec1 = [0., 1., 0.] + bvec1 = [0.0, 1.0, 0.0] b1 = np.array(bvec1) - dcm_BS = [1., 0., 0., - 0., 1., 0., - 0., 0., 1.] + dcm_BS = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] # Fill in expected values for test - DCM_exp = np.zeros([3,3]) + DCM_exp = np.zeros([3, 3]) W_exp = np.eye(numStates) - DCM_exp[:, 0] = np.array(inputStates[0:3]) / (np.linalg.norm(np.array(inputStates[0:3]))) - DCM_exp[:, 1] = np.cross(DCM_exp[:, 0], b1) / np.linalg.norm(np.array(np.cross(DCM_exp[:, 0], b1))) + DCM_exp[:, 0] = np.array(inputStates[0:3]) / ( + np.linalg.norm(np.array(inputStates[0:3])) + ) + DCM_exp[:, 1] = np.cross(DCM_exp[:, 0], b1) / np.linalg.norm( + np.array(np.cross(DCM_exp[:, 0], b1)) + ) DCM_exp[:, 2] = np.cross(DCM_exp[:, 0], DCM_exp[:, 1]) / np.linalg.norm( - np.cross(DCM_exp[:, 0], DCM_exp[:, 1])) + np.cross(DCM_exp[:, 0], DCM_exp[:, 1]) + ) # Fill in the variables for the test dcm = sunlineSEKF.new_doubleArray(3 * 3) @@ -466,12 +751,11 @@ def sunline_individual_test(): for j in range(9): dcmOut.append(sunlineSEKF.doubleArray_getitem(dcm, j)) - errorNorm = np.zeros(1) errorNorm[0] = np.linalg.norm(DCM_exp - np.array(dcmOut).reshape([3, 3])) for i in range(len(errorNorm)): - if (errorNorm[i] > 1.0E-10): + if errorNorm[i] > 1.0e-10: testFailCount += 1 testMessages.append("Frame switch failure \n") @@ -479,37 +763,71 @@ def sunline_individual_test(): ## Test the Switching method ################################################################################### - inputStates = [2,1,0.75,0.1,0.4] - bvec1 = [0.,1.,0.] + inputStates = [2, 1, 0.75, 0.1, 0.4] + bvec1 = [0.0, 1.0, 0.0] b1 = np.array(bvec1) - covar = [1., 0., 0., 1., 0., - 0., 1., 0., 0., 1., - 0., 0., 1., 0., 0., - 1., 0., 0., 1., 0., - 0., 1., 0., 0., 1.] - noise =0.01 + covar = [ + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + ] + noise = 0.01 # Fill in expected values for test - DCM_BSold = np.zeros([3,3]) - DCM_BSnew = np.zeros([3,3]) + DCM_BSold = np.zeros([3, 3]) + DCM_BSnew = np.zeros([3, 3]) Switch = np.eye(numStates) SwitchBSold = np.eye(numStates) SwitchBSnew = np.eye(numStates) - DCM_BSold[:,0] = np.array(inputStates[0:3])/(np.linalg.norm(np.array(inputStates[0:3]))) - DCM_BSold[:,1] = np.cross(DCM_BSold[:,0], b1)/np.linalg.norm(np.array(np.cross(DCM_BSold[:,0], b1))) - DCM_BSold[:,2] = np.cross(DCM_BSold[:,0], DCM_BSold[:,1])/np.linalg.norm(np.cross(DCM_BSold[:,0], DCM_BSold[:,1])) + DCM_BSold[:, 0] = np.array(inputStates[0:3]) / ( + np.linalg.norm(np.array(inputStates[0:3])) + ) + DCM_BSold[:, 1] = np.cross(DCM_BSold[:, 0], b1) / np.linalg.norm( + np.array(np.cross(DCM_BSold[:, 0], b1)) + ) + DCM_BSold[:, 2] = np.cross(DCM_BSold[:, 0], DCM_BSold[:, 1]) / np.linalg.norm( + np.cross(DCM_BSold[:, 0], DCM_BSold[:, 1]) + ) SwitchBSold[3:5, 3:5] = DCM_BSold[1:3, 1:3] - b2 = np.array([1.,0.,0.]) - DCM_BSnew[:,0] = np.array(inputStates[0:3])/(np.linalg.norm(np.array(inputStates[0:3]))) - DCM_BSnew[:,1] = np.cross(DCM_BSnew[:,0], b2)/np.linalg.norm(np.array(np.cross(DCM_BSnew[:,0], b2))) - DCM_BSnew[:,2] = np.cross(DCM_BSnew[:,0], DCM_BSnew[:,1])/np.linalg.norm(np.cross(DCM_BSnew[:,0], DCM_BSnew[:,1])) + b2 = np.array([1.0, 0.0, 0.0]) + DCM_BSnew[:, 0] = np.array(inputStates[0:3]) / ( + np.linalg.norm(np.array(inputStates[0:3])) + ) + DCM_BSnew[:, 1] = np.cross(DCM_BSnew[:, 0], b2) / np.linalg.norm( + np.array(np.cross(DCM_BSnew[:, 0], b2)) + ) + DCM_BSnew[:, 2] = np.cross(DCM_BSnew[:, 0], DCM_BSnew[:, 1]) / np.linalg.norm( + np.cross(DCM_BSnew[:, 0], DCM_BSnew[:, 1]) + ) SwitchBSnew[3:5, 3:5] = DCM_BSnew[1:3, 1:3] DCM_newOld = np.dot(DCM_BSnew.T, DCM_BSold) - Switch[3:5, 3:5] = DCM_newOld[1:3,1:3] + Switch[3:5, 3:5] = DCM_newOld[1:3, 1:3] # Fill in the variables for the test bvec = sunlineSEKF.new_doubleArray(3) @@ -521,7 +839,7 @@ def sunline_individual_test(): sunlineSEKF.doubleArray_setitem(bvec, i, bvec1[i]) for i in range(numStates): sunlineSEKF.doubleArray_setitem(states, i, inputStates[i]) - for j in range(numStates*numStates): + for j in range(numStates * numStates): sunlineSEKF.doubleArray_setitem(covarMat, j, covar[j]) # sunlineSEKF.doubleArray_setitem(switchBS, j, switchInput[j]) @@ -535,35 +853,36 @@ def sunline_individual_test(): bvecOut.append(sunlineSEKF.doubleArray_getitem(bvec, i)) for i in range(numStates): stateOut.append(sunlineSEKF.doubleArray_getitem(states, i)) - for j in range(numStates*numStates): + for j in range(numStates * numStates): covarOut.append(sunlineSEKF.doubleArray_getitem(covarMat, j)) - expectedState = np.dot(Switch, np.array(inputStates)) Pk = np.array(covar).reshape([numStates, numStates]) expectedP = np.dot(Switch, np.dot(Pk, Switch.T)) errorNorm = np.zeros(3) errorNorm[0] = np.linalg.norm(np.array(stateOut) - expectedState) - errorNorm[1] = np.linalg.norm(expectedP - np.array(covarOut).reshape([numStates, numStates])) + errorNorm[1] = np.linalg.norm( + expectedP - np.array(covarOut).reshape([numStates, numStates]) + ) errorNorm[2] = np.linalg.norm(np.array(bvecOut) - b2) # errorNorm[3] = np.linalg.norm(SwitchBSnew - np.array(switchBSout).reshape([numStates, numStates])) for i in range(len(errorNorm)): - if (errorNorm[i] > 1.0E-10): + if errorNorm[i] > 1.0e-10: testFailCount += 1 testMessages.append("Frame switch failure \n") - # print out success message if no error were found if testFailCount == 0: print("PASSED: " + " SEKF individual tests") else: - print(str(testFailCount) + ' tests failed') + print(str(testFailCount) + " tests failed") print(testMessages) # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + #################################################################################### # Test for the time and update with static states (zero d_dot) @@ -599,7 +918,7 @@ def StatePropStatic(): setupFilterData(module) - kfLog = module.logger(["covar", "state"], testProcessRate*10) + kfLog = module.logger(["covar", "state"], testProcessRate * 10) unitTestSim.AddModelToTask(unitTaskName, kfLog) # connect messages @@ -615,7 +934,7 @@ def StatePropStatic(): stateLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.state) for i in range(numStates): - if (abs(stateLog[-1, i + 1] - stateLog[0, i + 1]) > 1.0E-10): + if abs(stateLog[-1, i + 1] - stateLog[0, i + 1]) > 1.0e-10: testFailCount += 1 testMessages.append("State propagation failure \n") @@ -625,7 +944,8 @@ def StatePropStatic(): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + #################################################################################### # Test for the time and update with changing states (non-zero d_dot) @@ -660,7 +980,9 @@ def StatePropVariable(show_plots): setupFilterData(module) - InitialState = (np.array(module.state)+ +np.array([0.,0.,0.,0.0001,0.002])).tolist() + InitialState = ( + np.array(module.state) + +np.array([0.0, 0.0, 0.0, 0.0001, 0.002]) + ).tolist() Initialx = module.x InitialCovar = module.covar module.state = InitialState @@ -678,32 +1000,38 @@ def StatePropVariable(show_plots): unitTestSim.ConfigureStopTime(macros.sec2nano(1000.0)) unitTestSim.ExecuteSimulation() - covarLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.covar) stateLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.state) stateErrorLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.x) stmLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.stateTransition) - bVec = [1.,0.,0.] + bVec = [1.0, 0.0, 0.0] dt = 0.5 - expectedStateArray = np.zeros([2001,numStates+1]) - DCM_BS = np.zeros([2001,3,3]) - omega_S = np.zeros([2001,3]) - omega_B = np.zeros([2001,3]) - expectedStateArray[0,1:numStates+1] = np.array(InitialState) - expDynMat = np.zeros([2001,numStates,numStates]) - - DCM_BS[0,:,0] = np.array(InitialState[0:3])/(np.linalg.norm(np.array(InitialState[0:3]))) - DCM_BS[0,:,1] = np.cross(DCM_BS[0,:,0], bVec)/np.linalg.norm(np.array(np.cross(DCM_BS[0,:,0], bVec))) - DCM_BS[0,:,2] = np.cross(DCM_BS[0,:,0], DCM_BS[0,:,1])/np.linalg.norm(np.cross(DCM_BS[0,:,0], DCM_BS[0,:,1])) - omega_S[0,1:] = InitialState[3:] - omega_B[0,:] = np.dot(DCM_BS[0, :, :], omega_S[0,:]) - - for i in range(1,2001): - expectedStateArray[i,0] = dt*i*1E9 - expectedStateArray[i,1:4] = expectedStateArray[i-1,1:4] + dt * np.cross(omega_B[i-1,:], - expectedStateArray[i - 1, 1:4]) - expectedStateArray[i, 4:6] = expectedStateArray[i-1, 4:6] + expectedStateArray = np.zeros([2001, numStates + 1]) + DCM_BS = np.zeros([2001, 3, 3]) + omega_S = np.zeros([2001, 3]) + omega_B = np.zeros([2001, 3]) + expectedStateArray[0, 1 : numStates + 1] = np.array(InitialState) + expDynMat = np.zeros([2001, numStates, numStates]) + + DCM_BS[0, :, 0] = np.array(InitialState[0:3]) / ( + np.linalg.norm(np.array(InitialState[0:3])) + ) + DCM_BS[0, :, 1] = np.cross(DCM_BS[0, :, 0], bVec) / np.linalg.norm( + np.array(np.cross(DCM_BS[0, :, 0], bVec)) + ) + DCM_BS[0, :, 2] = np.cross(DCM_BS[0, :, 0], DCM_BS[0, :, 1]) / np.linalg.norm( + np.cross(DCM_BS[0, :, 0], DCM_BS[0, :, 1]) + ) + omega_S[0, 1:] = InitialState[3:] + omega_B[0, :] = np.dot(DCM_BS[0, :, :], omega_S[0, :]) + + for i in range(1, 2001): + expectedStateArray[i, 0] = dt * i * 1e9 + expectedStateArray[i, 1:4] = expectedStateArray[i - 1, 1:4] + dt * np.cross( + omega_B[i - 1, :], expectedStateArray[i - 1, 1:4] + ) + expectedStateArray[i, 4:6] = expectedStateArray[i - 1, 4:6] # Fill in the variables for the test dcm = sunlineSEKF.new_doubleArray(3 * 3) @@ -713,57 +1041,81 @@ def StatePropVariable(show_plots): dcmOut = [] for j in range(9): dcmOut.append(sunlineSEKF.doubleArray_getitem(dcm, j)) - DCM_BS[i,:,:] = np.array(dcmOut).reshape([3, 3]) + DCM_BS[i, :, :] = np.array(dcmOut).reshape([3, 3]) omega_S[i, 1:] = expectedStateArray[i, 4:] - omega_B[i,:] = np.dot(DCM_BS[i, :, :], omega_S[i,:]) + omega_B[i, :] = np.dot(DCM_BS[i, :, :], omega_S[i, :]) for i in range(0, 2001): dtilde = -np.array(RigidBodyKinematics.v3Tilde(expectedStateArray[i, 1:4])) - dBS = np.dot(dtilde, DCM_BS[i,:,:]) + dBS = np.dot(dtilde, DCM_BS[i, :, :]) - expDynMat[i,0:3, 0:3] = np.array(RigidBodyKinematics.v3Tilde(omega_B[i,:])) + expDynMat[i, 0:3, 0:3] = np.array(RigidBodyKinematics.v3Tilde(omega_B[i, :])) expDynMat[i, 0:3, 3:numStates] = dBS[:, 1:] - expectedSTM = np.zeros([2001,numStates,numStates]) - expectedSTM[0,:,:] = np.eye(numStates) - for i in range(1,2001): - expectedSTM[i,:,:] = dt * np.dot(expDynMat[i-1,:,:], np.eye(numStates)) + np.eye(numStates) - - expectedXBar = np.zeros([2001,numStates+1]) - expectedXBar[0,1:6] = np.array(Initialx) - for i in range(1,2001): - expectedXBar[i,0] = dt*i*1E9 + expectedSTM = np.zeros([2001, numStates, numStates]) + expectedSTM[0, :, :] = np.eye(numStates) + for i in range(1, 2001): + expectedSTM[i, :, :] = dt * np.dot( + expDynMat[i - 1, :, :], np.eye(numStates) + ) + np.eye(numStates) + + expectedXBar = np.zeros([2001, numStates + 1]) + expectedXBar[0, 1:6] = np.array(Initialx) + for i in range(1, 2001): + expectedXBar[i, 0] = dt * i * 1e9 expectedXBar[i, 1:6] = np.dot(expectedSTM[i, :, :], expectedXBar[i - 1, 1:6]) - expectedCovar = np.zeros([2001,26]) - expectedCovar[0,1:26] = np.array(InitialCovar) - Gamma = np.zeros([2001,numStates, 2]) - ProcNoiseCovar = np.zeros([2001,numStates,numStates]) - for i in range(0,2001): - s_skew = np.array([[0., -expectedStateArray[i,3], expectedStateArray[i,2]], - [expectedStateArray[i,3], 0., -expectedStateArray[i,1]], - [-expectedStateArray[i,2], expectedStateArray[i,1], 0.]]) - s_BS = np.dot(s_skew, DCM_BS[i,:,:]) - Gamma[i, 0:3, 0:2] = dt ** 2. / 2. * s_BS[:,1:3] - Gamma[i,3:numStates, 0:2] = dt * np.eye(2) - ProcNoiseCovar[i,:,:] = np.dot(Gamma[i,:,:], np.dot(module.qProcVal*np.eye(2),Gamma[i,:,:].T)) - for i in range(1,2001): - expectedCovar[i,0] = dt*i*1E9 - expectedCovar[i,1:26] = (np.dot(expectedSTM[i,:,:], np.dot(np.reshape(expectedCovar[i-1,1:26],[numStates,numStates]), np.transpose(expectedSTM[i,:,:])))+ ProcNoiseCovar[i,:,:]).flatten() + expectedCovar = np.zeros([2001, 26]) + expectedCovar[0, 1:26] = np.array(InitialCovar) + Gamma = np.zeros([2001, numStates, 2]) + ProcNoiseCovar = np.zeros([2001, numStates, numStates]) + for i in range(0, 2001): + s_skew = np.array( + [ + [0.0, -expectedStateArray[i, 3], expectedStateArray[i, 2]], + [expectedStateArray[i, 3], 0.0, -expectedStateArray[i, 1]], + [-expectedStateArray[i, 2], expectedStateArray[i, 1], 0.0], + ] + ) + s_BS = np.dot(s_skew, DCM_BS[i, :, :]) + Gamma[i, 0:3, 0:2] = dt**2.0 / 2.0 * s_BS[:, 1:3] + Gamma[i, 3:numStates, 0:2] = dt * np.eye(2) + ProcNoiseCovar[i, :, :] = np.dot( + Gamma[i, :, :], np.dot(module.qProcVal * np.eye(2), Gamma[i, :, :].T) + ) + for i in range(1, 2001): + expectedCovar[i, 0] = dt * i * 1e9 + expectedCovar[i, 1:26] = ( + np.dot( + expectedSTM[i, :, :], + np.dot( + np.reshape(expectedCovar[i - 1, 1:26], [numStates, numStates]), + np.transpose(expectedSTM[i, :, :]), + ), + ) + + ProcNoiseCovar[i, :, :] + ).flatten() FilterPlots.StatesVsExpected(stateLog, expectedStateArray, show_plots) - FilterPlots.StatesPlotCompare(stateErrorLog, expectedXBar, covarLog, expectedCovar, show_plots) + FilterPlots.StatesPlotCompare( + stateErrorLog, expectedXBar, covarLog, expectedCovar, show_plots + ) - if (np.linalg.norm(np.array(stateLog)[:, 1:] - expectedStateArray[:, 1:]) > 1.0E-10): + if np.linalg.norm(np.array(stateLog)[:, 1:] - expectedStateArray[:, 1:]) > 1.0e-10: testFailCount += 1 testMessages.append("General state propagation failure: State Prop \n") - if (np.linalg.norm(np.array(stateErrorLog)[:, 1:] - expectedXBar[:,1:]) > 1.0E-4): + if np.linalg.norm(np.array(stateErrorLog)[:, 1:] - expectedXBar[:, 1:]) > 1.0e-4: testFailCount += 1 testMessages.append("General state propagation failure: State Error Prop \n") - if (np.linalg.norm(np.array(covarLog)[:, 1:] - expectedCovar[:, 1:]) > 1.0E-4): + if np.linalg.norm(np.array(covarLog)[:, 1:] - expectedCovar[:, 1:]) > 1.0e-4: testFailCount += 1 testMessages.append("General state propagation failure: Covariance Prop \n") - if (np.linalg.norm(np.array(stmLog)[:, 1:] - expectedSTM[:,:,:].reshape([2001,25])) > 1.0E-4): + if ( + np.linalg.norm( + np.array(stmLog)[:, 1:] - expectedSTM[:, :, :].reshape([2001, 25]) + ) + > 1.0e-4 + ): testFailCount += 1 testMessages.append("General state propagation failure: STM Prop \n") @@ -773,13 +1125,15 @@ def StatePropVariable(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] #################################################################################### # Test for the full filter with time and measurement update #################################################################################### -def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess): +def StateUpdateSunLine( + show_plots, SimHalfLength, AddMeasNoise, testVector1, testVector2, stateGuess +): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. @@ -864,7 +1218,9 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes dotList = [] for element in CSSOrientationList: if AddMeasNoise: - dotProd = np.dot(np.array(element), np.array(testVector1)[0:3]) + np.random.normal(0., module.qObsVal) + dotProd = np.dot( + np.array(element), np.array(testVector1)[0:3] + ) + np.random.normal(0.0, module.qObsVal) else: dotProd = np.dot(np.array(element), np.array(testVector1)[0:3]) dotList.append(dotProd) @@ -878,17 +1234,27 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes covarLog = addTimeColumn(dataLog.times(), dataLog.covar) for i in range(numStates): - if (abs(covarLog[-1, i *numStates + 1 + i] - covarLog[0, i * numStates + 1 + i] / 100.) > 1E-1): - print(abs(covarLog[-1, i *numStates + 1 + i] - covarLog[0, i * numStates + 1 + i] / 100.)) + if ( + abs( + covarLog[-1, i * numStates + 1 + i] + - covarLog[0, i * numStates + 1 + i] / 100.0 + ) + > 1e-1 + ): + print( + abs( + covarLog[-1, i * numStates + 1 + i] + - covarLog[0, i * numStates + 1 + i] / 100.0 + ) + ) testFailCount += 1 testMessages.append("Covariance update failure") - if (abs(stateLog[-1, i + 1] - stateTarget1[i]) > 1.0E-1): + if abs(stateLog[-1, i + 1] - stateTarget1[i]) > 1.0e-1: testFailCount += 1 testMessages.append("State update failure") - stateTarget2 = testVector2 - stateTarget2 = stateTarget2+[0.,0.] + stateTarget2 = stateTarget2 + [0.0, 0.0] inputData = messaging.CSSArraySensorMsgPayload() for i in range(SimHalfLength): @@ -896,14 +1262,16 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes dotList = [] for element in CSSOrientationList: if AddMeasNoise: - dotProd = np.dot(np.array(element), np.array(testVector2)[0:3]) + np.random.normal(0., module.qObsVal) + dotProd = np.dot( + np.array(element), np.array(testVector2)[0:3] + ) + np.random.normal(0.0, module.qObsVal) else: dotProd = np.dot(np.array(element), np.array(testVector2)[0:3]) dotList.append(dotProd) inputData.CosValue = dotList cssDataInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i + SimHalfLength+1) * 0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + SimHalfLength + 1) * 0.5)) unitTestSim.ExecuteSimulation() stateErrorLog = unitTestSupport.addTimeColumn(kfLog.times(), kfLog.x) @@ -911,18 +1279,22 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes postFitLog = addTimeColumn(dataLog.times(), dataLog.postFitRes) covarLog = addTimeColumn(dataLog.times(), dataLog.covar) - - for i in range(numStates): - if (abs(covarLog[-1, i * numStates + 1 + i] - covarLog[0, i * numStates + 1 + i] / 100.) > 1E-1): + if ( + abs( + covarLog[-1, i * numStates + 1 + i] + - covarLog[0, i * numStates + 1 + i] / 100.0 + ) + > 1e-1 + ): testFailCount += 1 testMessages.append("Covariance update failure at end") - if (abs(stateLog[-1, i + 1] - stateTarget2[i]) > 1.0E-1): + if abs(stateLog[-1, i + 1] - stateTarget2[i]) > 1.0e-1: testFailCount += 1 testMessages.append("State update failure at end") target1 = np.array(testVector1) - target2 = np.array(testVector2+[0.,0.]) + target2 = np.array(testVector2 + [0.0, 0.0]) FilterPlots.StatesPlot(stateErrorLog, covarLog, show_plots) FilterPlots.StatesVsTargets(target1, target2, stateLog, show_plots) FilterPlots.PostFitResiduals(postFitLog, module.qObsVal, show_plots) @@ -935,11 +1307,12 @@ def StateUpdateSunLine(show_plots, SimHalfLength, AddMeasNoise, testVector1, tes # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] - + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": # StatePropVariable(True) # sunline_individual_test() - test_all_sunline_sekf(True, 200, True ,[-0.7, 0.7, 0.0] ,[0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0]) + test_all_sunline_sekf( + True, 200, True, [-0.7, 0.7, 0.0], [0.8, 0.9, 0.0], [0.7, 0.7, 0.0, 0.0, 0.0] + ) diff --git a/src/fswAlgorithms/attDetermination/sunlineSuKF/_UnitTest/SunLineSuKF_test_utilities.py b/src/fswAlgorithms/attDetermination/sunlineSuKF/_UnitTest/SunLineSuKF_test_utilities.py index f3e1cc8e5f..6a270d0e1b 100644 --- a/src/fswAlgorithms/attDetermination/sunlineSuKF/_UnitTest/SunLineSuKF_test_utilities.py +++ b/src/fswAlgorithms/attDetermination/sunlineSuKF/_UnitTest/SunLineSuKF_test_utilities.py @@ -23,159 +23,168 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('fswAlgorithms') - - +splitPath = path.split("fswAlgorithms") import matplotlib.pyplot as plt def StateCovarPlot(x, Pflat, show_plots): + numStates = len(x[0, :]) - 1 - numStates = len(x[0,:])-1 - - P = np.zeros([len(Pflat[:,0]),numStates,numStates]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*1E-9 - P[i,:,:] = Pflat[i,1:(numStates*numStates+1)].reshape([numStates,numStates]) + P = np.zeros([len(Pflat[:, 0]), numStates, numStates]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * 1e-9 + P[i, :, :] = Pflat[i, 1 : (numStates * numStates + 1)].reshape( + [numStates, numStates] + ) - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t , x[:, 1], "b", label='Error Filter') - plt.plot(t , x[:, 1]+3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t , x[:, 1]-3 * np.sqrt(P[:, 0, 0]), 'r--') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.plot(t, x[:, 1] + 3 * np.sqrt(P[:, 0, 0]), "r--", label="Covar Filter") + plt.plot(t, x[:, 1] - 3 * np.sqrt(P[:, 0, 0]), "r--") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() plt.subplot(322) - plt.plot(t , x[:, 4], "b") - plt.plot(t , x[:, 4]+3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.plot(t , x[:, 4]-3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.title('Second rate component') + plt.plot(t, x[:, 4], "b") + plt.plot(t, x[:, 4] + 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.plot(t, x[:, 4] - 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.title("Second rate component") plt.grid() plt.subplot(323) - plt.plot(t , x[:, 2], "b") - plt.plot(t , x[:, 2]+3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.plot(t , x[:, 2]-3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.title('Second LOS component') + plt.plot(t, x[:, 2], "b") + plt.plot(t, x[:, 2] + 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.plot(t, x[:, 2] - 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(t , x[:, 5], "b") - plt.plot(t , x[:, 5]+3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.plot(t , x[:, 5]-3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(t, x[:, 5], "b") + plt.plot(t, x[:, 5] + 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.plot(t, x[:, 5] - 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() plt.subplot(325) - plt.plot(t , x[:, 3], "b") - plt.plot(t , x[:, 3]+3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.plot(t , x[:, 3]-3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(t, x[:, 3], "b") + plt.plot(t, x[:, 3] + 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.plot(t, x[:, 3] - 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(t , x[:, 6], "b") - plt.plot(t , x[:, 6]+3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.plot(t , x[:, 6]-3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.xlabel('t(s)') - plt.title('Solar Intensity') + plt.plot(t, x[:, 6], "b") + plt.plot(t, x[:, 6] + 3 * np.sqrt(P[:, 5, 5]), "r--") + plt.plot(t, x[:, 6] - 3 * np.sqrt(P[:, 5, 5]), "r--") + plt.xlabel("t(s)") + plt.title("Solar Intensity") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesPlot', 'State error and covariance', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesPlot", + "State error and covariance", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() - def PostFitResiduals(Res, noise, show_plots): - - MeasNoise = np.zeros(len(Res[:,0])) - t= np.zeros(len(Res[:,0])) - for i in range(len(Res[:,0])): - t[i] = Res[i, 0]*1E-9 - MeasNoise[i] = 3*noise + MeasNoise = np.zeros(len(Res[:, 0])) + t = np.zeros(len(Res[:, 0])) + for i in range(len(Res[:, 0])): + t[i] = Res[i, 0] * 1e-9 + MeasNoise[i] = 3 * noise # Don't plot zero values, since they mean that no measurement is taken - for j in range(len(Res[0,:])-1): - if -1E-10 < Res[i,j+1] < 1E-10: - Res[i, j+1] = np.nan + for j in range(len(Res[0, :]) - 1): + if -1e-10 < Res[i, j + 1] < 1e-10: + Res[i, j + 1] = np.nan - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(421) - plt.plot(t , Res[:, 1], "b.", label='Residual') - plt.plot(t , MeasNoise, 'r--', label='Covar') - plt.plot(t , -MeasNoise, 'r--') - plt.legend(loc='lower right') - plt.ylim([-10*noise, 10*noise]) - plt.title('First CSS') + plt.plot(t, Res[:, 1], "b.", label="Residual") + plt.plot(t, MeasNoise, "r--", label="Covar") + plt.plot(t, -MeasNoise, "r--") + plt.legend(loc="lower right") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("First CSS") plt.grid() plt.subplot(422) - plt.plot(t , Res[:, 5], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Fifth CSS') + plt.plot(t, Res[:, 5], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Fifth CSS") plt.grid() plt.subplot(423) - plt.plot(t , Res[:, 2], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Second CSS') + plt.plot(t, Res[:, 2], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Second CSS") plt.grid() plt.subplot(424) - plt.plot(t , Res[:, 6], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Sixth CSS') + plt.plot(t, Res[:, 6], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Sixth CSS") plt.grid() plt.subplot(425) - plt.plot(t , Res[:, 3], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Third CSS') + plt.plot(t, Res[:, 3], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Third CSS") plt.grid() plt.subplot(426) - plt.plot(t , Res[:, 7], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Seventh CSS') + plt.plot(t, Res[:, 7], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Seventh CSS") plt.grid() plt.subplot(427) - plt.plot(t , Res[:, 4], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.xlabel('t(s)') - plt.title('Fourth CSS') + plt.plot(t, Res[:, 4], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.xlabel("t(s)") + plt.title("Fourth CSS") plt.grid() plt.subplot(428) - plt.plot(t , Res[:, 8], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.xlabel('t(s)') - plt.title('Eight CSS') + plt.plot(t, Res[:, 8], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.xlabel("t(s)") + plt.title("Eight CSS") plt.grid() - unitTestSupport.writeFigureLaTeX('PostFit' , 'Post Fit Residuals', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "PostFit", + "Post Fit Residuals", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() diff --git a/src/fswAlgorithms/attDetermination/sunlineSuKF/_UnitTest/test_SunLineSuKF.py b/src/fswAlgorithms/attDetermination/sunlineSuKF/_UnitTest/test_SunLineSuKF.py index 12f43fcfd8..c9de844bb8 100644 --- a/src/fswAlgorithms/attDetermination/sunlineSuKF/_UnitTest/test_SunLineSuKF.py +++ b/src/fswAlgorithms/attDetermination/sunlineSuKF/_UnitTest/test_SunLineSuKF.py @@ -1,5 +1,6 @@ -''' ''' -''' +""" """ + +""" ISC License Copyright (c) 2016-2018, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,7 +17,7 @@ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. -''' +""" import numpy import pytest from Basilisk.architecture import messaging @@ -29,27 +30,60 @@ def addTimeColumn(time, data): return numpy.transpose(numpy.vstack([[time], numpy.transpose(data)])) + def setupFilterData(filterObject, initialized): filterObject.alpha = 0.02 filterObject.beta = 2.0 filterObject.kappa = 0.0 if initialized: - filterObject.stateInit = [0.0, 0.0, 1.0, 0.0, 0.0, 1.] + filterObject.stateInit = [0.0, 0.0, 1.0, 0.0, 0.0, 1.0] filterObject.filterInitialized = 1 else: filterObject.filterInitialized = 0 - filterObject.covarInit = [1., 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1., 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1., 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1E-4] + filterObject.covarInit = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.02, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.02, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1e-4, + ] qNoiseIn = numpy.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3]*0.001*0.001 - qNoiseIn[3:5, 3:5] = qNoiseIn[3:5, 3:5]*0.001*0.001 - qNoiseIn[5, 5] = qNoiseIn[5, 5]*0.0000002*0.0000002 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 0.001 * 0.001 + qNoiseIn[3:5, 3:5] = qNoiseIn[3:5, 3:5] * 0.001 * 0.001 + qNoiseIn[5, 5] = qNoiseIn[5, 5] * 0.0000002 * 0.0000002 filterObject.qNoise = qNoiseIn.reshape(36).tolist() filterObject.qObsVal = 0.002 filterObject.sensorUseThresh = 0.0 @@ -60,12 +94,7 @@ def setupFilterData(filterObject, initialized): # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("kellyOn", [ - (False), - (True) -]) - - +@pytest.mark.parametrize("kellyOn", [(False), (True)]) def test_all_sunline_kf(show_plots, kellyOn): """Module Unit Test""" [testResults, testMessage] = SwitchMethods() @@ -91,24 +120,27 @@ def SwitchMethods(): ################################################################################### numStates = 6 - inputStates = [2, 1, 0.75, 0.1, 0.4, 0.] + inputStates = [2, 1, 0.75, 0.1, 0.4, 0.0] sunheading = inputStates[:3] - bvec1 = [0., 1., 0.] + bvec1 = [0.0, 1.0, 0.0] b1 = numpy.array(bvec1) - dcm_BS = [1., 0., 0., - 0., 1., 0., - 0., 0., 1.] + dcm_BS = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] # Fill in expected values for test - DCM_exp = numpy.zeros([3,3]) + DCM_exp = numpy.zeros([3, 3]) W_exp = numpy.eye(numStates) - DCM_exp[:, 0] = numpy.array(inputStates[0:3]) / (numpy.linalg.norm(numpy.array(inputStates[0:3]))) - DCM_exp[:, 1] = numpy.cross(DCM_exp[:, 0], b1) / numpy.linalg.norm(numpy.array(numpy.cross(DCM_exp[:, 0], b1))) + DCM_exp[:, 0] = numpy.array(inputStates[0:3]) / ( + numpy.linalg.norm(numpy.array(inputStates[0:3])) + ) + DCM_exp[:, 1] = numpy.cross(DCM_exp[:, 0], b1) / numpy.linalg.norm( + numpy.array(numpy.cross(DCM_exp[:, 0], b1)) + ) DCM_exp[:, 2] = numpy.cross(DCM_exp[:, 0], DCM_exp[:, 1]) / numpy.linalg.norm( - numpy.cross(DCM_exp[:, 0], DCM_exp[:, 1])) + numpy.cross(DCM_exp[:, 0], DCM_exp[:, 1]) + ) # Fill in the variables for the test dcm = sunlineSuKF.new_doubleArray(3 * 3) @@ -123,12 +155,11 @@ def SwitchMethods(): for j in range(9): dcmOut.append(sunlineSuKF.doubleArray_getitem(dcm, j)) - errorNorm = numpy.zeros(1) errorNorm[0] = numpy.linalg.norm(DCM_exp - numpy.array(dcmOut).reshape([3, 3])) for i in range(len(errorNorm)): - if (errorNorm[i] > 1.0E-10): + if errorNorm[i] > 1.0e-10: testFailCount += 1 testMessages.append("Frame switch failure \n") @@ -136,38 +167,82 @@ def SwitchMethods(): ## Test the Switching method ################################################################################### - inputStates = [2,1,0.75,0.1,0.4, 1.] - bvec1 = [0.,1.,0.] + inputStates = [2, 1, 0.75, 0.1, 0.4, 1.0] + bvec1 = [0.0, 1.0, 0.0] b1 = numpy.array(bvec1) - covar = [1., 0., 0., 1., 0., 0., - 0., 1., 0., 0., 1., 0., - 0., 0., 1., 0., 0., 1., - 1., 0., 0., 1., 0., 0., - 0., 1., 0., 0., 1., 0., - 0., 0., 1., 0., 0., 1.] - noise =0.01 + covar = [ + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 1.0, + ] + noise = 0.01 # Fill in expected values for test - DCM_BSold = numpy.zeros([3,3]) - DCM_BSnew = numpy.zeros([3,3]) + DCM_BSold = numpy.zeros([3, 3]) + DCM_BSnew = numpy.zeros([3, 3]) Switch = numpy.eye(numStates) SwitchBSold = numpy.eye(numStates) SwitchBSnew = numpy.eye(numStates) - DCM_BSold[:,0] = numpy.array(inputStates[0:3])/(numpy.linalg.norm(numpy.array(inputStates[0:3]))) - DCM_BSold[:,1] = numpy.cross(DCM_BSold[:,0], b1)/numpy.linalg.norm(numpy.array(numpy.cross(DCM_BSold[:,0], b1))) - DCM_BSold[:,2] = numpy.cross(DCM_BSold[:,0], DCM_BSold[:,1])/numpy.linalg.norm(numpy.cross(DCM_BSold[:,0], DCM_BSold[:,1])) + DCM_BSold[:, 0] = numpy.array(inputStates[0:3]) / ( + numpy.linalg.norm(numpy.array(inputStates[0:3])) + ) + DCM_BSold[:, 1] = numpy.cross(DCM_BSold[:, 0], b1) / numpy.linalg.norm( + numpy.array(numpy.cross(DCM_BSold[:, 0], b1)) + ) + DCM_BSold[:, 2] = numpy.cross(DCM_BSold[:, 0], DCM_BSold[:, 1]) / numpy.linalg.norm( + numpy.cross(DCM_BSold[:, 0], DCM_BSold[:, 1]) + ) SwitchBSold[3:5, 3:5] = DCM_BSold[1:3, 1:3] - b2 = numpy.array([1.,0.,0.]) - DCM_BSnew[:,0] = numpy.array(inputStates[0:3])/(numpy.linalg.norm(numpy.array(inputStates[0:3]))) - DCM_BSnew[:,1] = numpy.cross(DCM_BSnew[:,0], b2)/numpy.linalg.norm(numpy.array(numpy.cross(DCM_BSnew[:,0], b2))) - DCM_BSnew[:,2] = numpy.cross(DCM_BSnew[:,0], DCM_BSnew[:,1])/numpy.linalg.norm(numpy.cross(DCM_BSnew[:,0], DCM_BSnew[:,1])) + b2 = numpy.array([1.0, 0.0, 0.0]) + DCM_BSnew[:, 0] = numpy.array(inputStates[0:3]) / ( + numpy.linalg.norm(numpy.array(inputStates[0:3])) + ) + DCM_BSnew[:, 1] = numpy.cross(DCM_BSnew[:, 0], b2) / numpy.linalg.norm( + numpy.array(numpy.cross(DCM_BSnew[:, 0], b2)) + ) + DCM_BSnew[:, 2] = numpy.cross(DCM_BSnew[:, 0], DCM_BSnew[:, 1]) / numpy.linalg.norm( + numpy.cross(DCM_BSnew[:, 0], DCM_BSnew[:, 1]) + ) SwitchBSnew[3:5, 3:5] = DCM_BSnew[1:3, 1:3] DCM_newOld = numpy.dot(DCM_BSnew.T, DCM_BSold) - Switch[3:5, 3:5] = DCM_newOld[1:3,1:3] + Switch[3:5, 3:5] = DCM_newOld[1:3, 1:3] # Fill in the variables for the test bvec = sunlineSuKF.new_doubleArray(3) @@ -178,7 +253,7 @@ def SwitchMethods(): sunlineSuKF.doubleArray_setitem(bvec, i, bvec1[i]) for i in range(numStates): sunlineSuKF.doubleArray_setitem(states, i, inputStates[i]) - for j in range(numStates*numStates): + for j in range(numStates * numStates): sunlineSuKF.doubleArray_setitem(covarMat, j, covar[j]) # sunlineSEKF.doubleArray_setitem(switchBS, j, switchInput[j]) @@ -192,34 +267,35 @@ def SwitchMethods(): bvecOut.append(sunlineSuKF.doubleArray_getitem(bvec, i)) for i in range(numStates): stateOut.append(sunlineSuKF.doubleArray_getitem(states, i)) - for j in range(numStates*numStates): + for j in range(numStates * numStates): covarOut.append(sunlineSuKF.doubleArray_getitem(covarMat, j)) - expectedState = numpy.dot(Switch, numpy.array(inputStates)) Pk = numpy.array(covar).reshape([numStates, numStates]) expectedP = numpy.dot(Switch, numpy.dot(Pk, Switch.T)) errorNorm = numpy.zeros(3) errorNorm[0] = numpy.linalg.norm(numpy.array(stateOut) - expectedState) - errorNorm[1] = numpy.linalg.norm(expectedP - numpy.array(covarOut).reshape([numStates, numStates])) + errorNorm[1] = numpy.linalg.norm( + expectedP - numpy.array(covarOut).reshape([numStates, numStates]) + ) errorNorm[2] = numpy.linalg.norm(numpy.array(bvecOut) - b2) for i in range(len(errorNorm)): - if (errorNorm[i] > 1.0E-10): + if errorNorm[i] > 1.0e-10: testFailCount += 1 testMessages.append("Frame switch failure \n") - # print out success message if no error were found if testFailCount == 0: print("PASSED: " + " SuKF switch tests") else: - print(str(testFailCount) + ' tests failed') + print(str(testFailCount) + " tests failed") print(testMessages) # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def StateUpdateSunLine(show_plots, kellyOn): # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -252,14 +328,14 @@ def StateUpdateSunLine(show_plots, kellyOn): cssConstelation = messaging.CSSConfigMsgPayload() CSSOrientationList = [ - [0.70710678118654746, -0.5, 0.5], - [0.70710678118654746, -0.5, -0.5], - [0.70710678118654746, 0.5, -0.5], - [0.70710678118654746, 0.5, 0.5], - [-0.70710678118654746, 0, 0.70710678118654757], - [-0.70710678118654746, 0.70710678118654757, 0.0], - [-0.70710678118654746, 0, -0.70710678118654757], - [-0.70710678118654746, -0.70710678118654757, 0.0], + [0.70710678118654746, -0.5, 0.5], + [0.70710678118654746, -0.5, -0.5], + [0.70710678118654746, 0.5, -0.5], + [0.70710678118654746, 0.5, 0.5], + [-0.70710678118654746, 0, 0.70710678118654757], + [-0.70710678118654746, 0.70710678118654757, 0.0], + [-0.70710678118654746, 0, -0.70710678118654757], + [-0.70710678118654746, -0.70710678118654757, 0.0], ] totalCSSList = [] for CSSHat in CSSOrientationList: @@ -280,24 +356,26 @@ def StateUpdateSunLine(show_plots, kellyOn): for j in range(len(CSSOrientationList)): kellyData = sunlineSuKF.SunlineSuKFCFit() kellyData.cssKellFact = 0.05 - kellyData.cssKellPow = 2. - kellyData.cssRelScale = 1. + kellyData.cssKellPow = 2.0 + kellyData.cssRelScale = 1.0 kellList.append(kellyData) module.kellFits = kellList testVector = numpy.array([-0.7, 0.7, 0.0]) - testVector/=numpy.linalg.norm(testVector) + testVector /= numpy.linalg.norm(testVector) inputData = messaging.CSSArraySensorMsgPayload() dotList = [] for element in CSSOrientationList: - dotProd = numpy.dot(numpy.array(element), testVector)/(numpy.linalg.norm(element)*numpy.linalg.norm(testVector)) + dotProd = numpy.dot(numpy.array(element), testVector) / ( + numpy.linalg.norm(element) * numpy.linalg.norm(testVector) + ) dotList.append(dotProd) inputData.CosValue = dotList cssDataInMsg = messaging.CSSArraySensorMsg() stateTarget = testVector.tolist() - stateTarget.extend([0.0, 0.0, 1.]) + stateTarget.extend([0.0, 0.0, 1.0]) # module.stateInit = [0.7, 0.7, 0.0, 0.01, 0.001, 1.] # connect messages @@ -309,32 +387,49 @@ def StateUpdateSunLine(show_plots, kellyOn): if kellyOn: time = 1000 else: - time = 500 + time = 500 for i in range(time): cssDataInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i+1)*0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * 0.5)) unitTestSim.ExecuteSimulation() stateLog = addTimeColumn(dataLog.times(), dataLog.state) postFitLog = addTimeColumn(dataLog.times(), dataLog.postFitRes) covarLog = addTimeColumn(dataLog.times(), dataLog.covar) - accuracy = 1.0E-3 + accuracy = 1.0e-3 if kellyOn: - accuracy = 1.0E-2 # 1% Error test for the kelly curves given errors + accuracy = 1.0e-2 # 1% Error test for the kelly curves given errors for i in range(numStates): - if(covarLog[-1, i*numStates+1+i] > covarLog[0, i*numStates+1+i]): + if covarLog[-1, i * numStates + 1 + i] > covarLog[0, i * numStates + 1 + i]: testFailCount += 1 testMessages.append("Covariance update failure first part") - if(numpy.arccos(numpy.dot(stateLog[-1, 1:4], stateTarget[0:3])/(numpy.linalg.norm(stateLog[-1, 1:4])*numpy.linalg.norm(stateTarget[0:3]))) > accuracy): - print(numpy.arccos(numpy.dot(stateLog[-1, 1:4], stateTarget[0:3])/(numpy.linalg.norm(stateLog[-1, 1:4])*numpy.linalg.norm(stateTarget[0:3])))) + if ( + numpy.arccos( + numpy.dot(stateLog[-1, 1:4], stateTarget[0:3]) + / ( + numpy.linalg.norm(stateLog[-1, 1:4]) + * numpy.linalg.norm(stateTarget[0:3]) + ) + ) + > accuracy + ): + print( + numpy.arccos( + numpy.dot(stateLog[-1, 1:4], stateTarget[0:3]) + / ( + numpy.linalg.norm(stateLog[-1, 1:4]) + * numpy.linalg.norm(stateTarget[0:3]) + ) + ) + ) testFailCount += 1 testMessages.append("Pointing update failure") - if(numpy.linalg.norm(stateLog[-1, 4:7] - stateTarget[3:6]) > accuracy): - print(numpy.linalg.norm(stateLog[-1, 4:7] - stateTarget[3:6])) + if numpy.linalg.norm(stateLog[-1, 4:7] - stateTarget[3:6]) > accuracy: + print(numpy.linalg.norm(stateLog[-1, 4:7] - stateTarget[3:6])) testFailCount += 1 testMessages.append("Rate update failure") - if(abs(stateLog[-1, 6] - stateTarget[5]) > accuracy): + if abs(stateLog[-1, 6] - stateTarget[5]) > accuracy: print(abs(stateLog[-1, 6] - stateTarget[5])) testFailCount += 1 testMessages.append("Sun Intensity update failure") @@ -351,7 +446,7 @@ def StateUpdateSunLine(show_plots, kellyOn): for i in range(time): if i > 20: cssDataInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i+time+1)*0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + time + 1) * 0.5)) unitTestSim.ExecuteSimulation() stateLog = addTimeColumn(dataLog.times(), dataLog.state) @@ -362,19 +457,38 @@ def StateUpdateSunLine(show_plots, kellyOn): stateTarget.extend([0.0, 0.0, 1.0]) for i in range(numStates): - if(covarLog[-1, i*numStates+1+i] > covarLog[0, i*numStates+1+i]): - print(covarLog[-1, i*numStates+1+i] - covarLog[0, i*numStates+1+i]) + if covarLog[-1, i * numStates + 1 + i] > covarLog[0, i * numStates + 1 + i]: + print( + covarLog[-1, i * numStates + 1 + i] - covarLog[0, i * numStates + 1 + i] + ) testFailCount += 1 testMessages.append("Covariance update failure") - if(numpy.arccos(numpy.dot(stateLog[-1, 1:4], stateTarget[0:3])/(numpy.linalg.norm(stateLog[-1, 1:4])*numpy.linalg.norm(stateTarget[0:3]))) > accuracy): - print(numpy.arccos(numpy.dot(stateLog[-1, 1:4], stateTarget[0:3])/(numpy.linalg.norm(stateLog[-1, 1:4])*numpy.linalg.norm(stateTarget[0:3])))) + if ( + numpy.arccos( + numpy.dot(stateLog[-1, 1:4], stateTarget[0:3]) + / ( + numpy.linalg.norm(stateLog[-1, 1:4]) + * numpy.linalg.norm(stateTarget[0:3]) + ) + ) + > accuracy + ): + print( + numpy.arccos( + numpy.dot(stateLog[-1, 1:4], stateTarget[0:3]) + / ( + numpy.linalg.norm(stateLog[-1, 1:4]) + * numpy.linalg.norm(stateTarget[0:3]) + ) + ) + ) testFailCount += 1 testMessages.append("Pointing update failure") - if(numpy.linalg.norm(stateLog[-1, 4:7] - stateTarget[3:6]) > accuracy): - print(numpy.linalg.norm(stateLog[-1, 4:7] - stateTarget[3:6])) + if numpy.linalg.norm(stateLog[-1, 4:7] - stateTarget[3:6]) > accuracy: + print(numpy.linalg.norm(stateLog[-1, 4:7] - stateTarget[3:6])) testFailCount += 1 testMessages.append("Rate update failure") - if(abs(stateLog[-1, 6] - stateTarget[5]) > accuracy): + if abs(stateLog[-1, 6] - stateTarget[5]) > accuracy: print(abs(stateLog[-1, 6] - stateTarget[5])) testFailCount += 1 testMessages.append("Sun Intensity update failure") @@ -390,7 +504,8 @@ def StateUpdateSunLine(show_plots, kellyOn): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def StatePropSunLine(show_plots): # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -443,20 +558,19 @@ def StatePropSunLine(show_plots): FilterPlots.PostFitResiduals(postFitLog, module.qObsVal, show_plots) for i in range(numStates): - if(abs(stateLog[-1, i+1] - stateLog[0, i+1]) > 1.0E-10): - print(abs(stateLog[-1, i+1] - stateLog[0, i+1])) + if abs(stateLog[-1, i + 1] - stateLog[0, i + 1]) > 1.0e-10: + print(abs(stateLog[-1, i + 1] - stateLog[0, i + 1])) testFailCount += 1 testMessages.append("State propagation failure") - - # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag + " state propagation") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def FaultScenarios(): # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -482,42 +596,185 @@ def FaultScenarios(): moduleClean1 = sunlineSuKF.SunlineSuKFConfig() moduleClean1.numStates = 6 moduleClean1.countHalfSPs = moduleClean1.numStates - moduleClean1.state = [0., 0., 0., 0., 0., 0.] - moduleClean1.statePrev = [0., 0., 0., 0., 0., 0.] - moduleClean1.sBar = [0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0.] - moduleClean1.sBarPrev = [1., 0., 0., 0., 0., 0., - 0., 1., 0., 0., 0., 0., - 0., 0., 1., 0., 0., 0., - 0., 0., 0., 1., 0., 0., - 0., 0., 0., 0., 1., 0., - 0., 0., 0., 0., 0., 1.] - moduleClean1.covar = [0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0., - 0., 0., 0., 0., 0., 0.] - moduleClean1.covarPrev = [2., 0., 0., 0., 0., 0., - 0., 2., 0., 0., 0., 0., - 0., 0., 2., 0., 0., 0., - 0., 0., 0., 2., 0., 0., - 0., 0., 0., 0., 2., 0., - 0., 0., 0., 0., 0., 2.] + moduleClean1.state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + moduleClean1.statePrev = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + moduleClean1.sBar = [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + moduleClean1.sBarPrev = [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + ] + moduleClean1.covar = [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + moduleClean1.covarPrev = [ + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + ] sunlineSuKF.sunlineSuKFCleanUpdate(moduleClean1) - if numpy.linalg.norm(numpy.array(moduleClean1.covarPrev) - numpy.array(moduleClean1.covar)) > 1E10: + if ( + numpy.linalg.norm( + numpy.array(moduleClean1.covarPrev) - numpy.array(moduleClean1.covar) + ) + > 1e10 + ): testFailCount += 1 testMessages.append("sunlineSuKFClean Covar failed") - if numpy.linalg.norm(numpy.array(moduleClean1.statePrev) - numpy.array(moduleClean1.state)) > 1E10: + if ( + numpy.linalg.norm( + numpy.array(moduleClean1.statePrev) - numpy.array(moduleClean1.state) + ) + > 1e10 + ): testFailCount += 1 testMessages.append("sunlineSuKFClean States failed") - if numpy.linalg.norm(numpy.array(moduleClean1.sBar) - numpy.array(moduleClean1.sBarPrev)) > 1E10: + if ( + numpy.linalg.norm( + numpy.array(moduleClean1.sBar) - numpy.array(moduleClean1.sBarPrev) + ) + > 1e10 + ): testFailCount += 1 testMessages.append("sunlineSuKFClean sBar failed") @@ -543,14 +800,14 @@ def FaultScenarios(): testFailCount += 1 testMessages.append("Failed to catch bad Update and clean in Meas update") - # print out success message if no error were found if testFailCount == 0: print("PASSED: fault detection test") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": # test_all_sunline_kf(True) diff --git a/src/fswAlgorithms/attDetermination/sunlineUKF/_UnitTest/SunLineuKF_test_utilities.py b/src/fswAlgorithms/attDetermination/sunlineUKF/_UnitTest/SunLineuKF_test_utilities.py index 31ce7a5f60..144c54736a 100644 --- a/src/fswAlgorithms/attDetermination/sunlineUKF/_UnitTest/SunLineuKF_test_utilities.py +++ b/src/fswAlgorithms/attDetermination/sunlineUKF/_UnitTest/SunLineuKF_test_utilities.py @@ -23,152 +23,160 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('fswAlgorithms') - - +splitPath = path.split("fswAlgorithms") import matplotlib.pyplot as plt def StateCovarPlot(x, Pflat, testNum, show_plots): + numStates = len(x[0, :]) - 1 - numStates = len(x[0,:])-1 - - P = np.zeros([len(Pflat[:,0]),numStates,numStates]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*1E-9 - P[i,:,:] = Pflat[i,1:(numStates*numStates+1)].reshape([numStates,numStates]) + P = np.zeros([len(Pflat[:, 0]), numStates, numStates]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * 1e-9 + P[i, :, :] = Pflat[i, 1 : (numStates * numStates + 1)].reshape( + [numStates, numStates] + ) - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t , x[:, 1], "b", label='Error Filter') - plt.plot(t , x[:, 1]+3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t , x[:, 1]-3 * np.sqrt(P[:, 0, 0]), 'r--') - plt.legend(loc='lower right') - plt.title('First LOS component') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.plot(t, x[:, 1] + 3 * np.sqrt(P[:, 0, 0]), "r--", label="Covar Filter") + plt.plot(t, x[:, 1] - 3 * np.sqrt(P[:, 0, 0]), "r--") + plt.legend(loc="lower right") + plt.title("First LOS component") plt.grid() - plt.subplot(323) - plt.plot(t , x[:, 2], "b") - plt.plot(t , x[:, 2]+3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.plot(t , x[:, 2]-3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.title('Second LOS component') + plt.plot(t, x[:, 2], "b") + plt.plot(t, x[:, 2] + 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.plot(t, x[:, 2] - 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.title("Second LOS component") plt.grid() plt.subplot(324) - plt.plot(t , x[:, 4], "b") - plt.plot(t , x[:, 4]+3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.plot(t , x[:, 4]-3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.title('Second rate component') + plt.plot(t, x[:, 4], "b") + plt.plot(t, x[:, 4] + 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.plot(t, x[:, 4] - 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.title("Second rate component") plt.grid() plt.subplot(325) - plt.plot(t , x[:, 3], "b") - plt.plot(t , x[:, 3]+3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.plot(t , x[:, 3]-3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.xlabel('t(s)') - plt.title('Third LOS component') + plt.plot(t, x[:, 3], "b") + plt.plot(t, x[:, 3] + 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.plot(t, x[:, 3] - 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.xlabel("t(s)") + plt.title("Third LOS component") plt.grid() plt.subplot(326) - plt.plot(t , x[:, 5], "b") - plt.plot(t , x[:, 5]+3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.plot(t , x[:, 5]-3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component') + plt.plot(t, x[:, 5], "b") + plt.plot(t, x[:, 5] + 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.plot(t, x[:, 5] - 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.xlabel("t(s)") + plt.title("Third rate component") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesPlot' + str(testNum), 'State error and covariance', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesPlot" + str(testNum), + "State error and covariance", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() - def PostFitResiduals(Res, noise, testNum, show_plots): - - MeasNoise = np.zeros(len(Res[:,0])) - t= np.zeros(len(Res[:,0])) - for i in range(len(Res[:,0])): - t[i] = Res[i, 0]*1E-9 - MeasNoise[i] = 3*noise + MeasNoise = np.zeros(len(Res[:, 0])) + t = np.zeros(len(Res[:, 0])) + for i in range(len(Res[:, 0])): + t[i] = Res[i, 0] * 1e-9 + MeasNoise[i] = 3 * noise # Don't plot zero values, since they mean that no measurement is taken - for j in range(len(Res[0,:])-1): - if -1E-10 < Res[i,j+1] < 1E-10: - Res[i, j+1] = np.nan + for j in range(len(Res[0, :]) - 1): + if -1e-10 < Res[i, j + 1] < 1e-10: + Res[i, j + 1] = np.nan - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(421) - plt.plot(t , Res[:, 1], "b.", label='Residual') - plt.plot(t , MeasNoise, 'r--', label='Covar') - plt.plot(t , -MeasNoise, 'r--') - plt.legend(loc='lower right') - plt.ylim([-10*noise, 10*noise]) - plt.title('First CSS') + plt.plot(t, Res[:, 1], "b.", label="Residual") + plt.plot(t, MeasNoise, "r--", label="Covar") + plt.plot(t, -MeasNoise, "r--") + plt.legend(loc="lower right") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("First CSS") plt.grid() plt.subplot(422) - plt.plot(t , Res[:, 5], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Fifth CSS') + plt.plot(t, Res[:, 5], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Fifth CSS") plt.grid() plt.subplot(423) - plt.plot(t , Res[:, 2], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Second CSS') + plt.plot(t, Res[:, 2], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Second CSS") plt.grid() plt.subplot(424) - plt.plot(t , Res[:, 6], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Sixth CSS') + plt.plot(t, Res[:, 6], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Sixth CSS") plt.grid() plt.subplot(425) - plt.plot(t , Res[:, 3], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Third CSS') + plt.plot(t, Res[:, 3], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Third CSS") plt.grid() plt.subplot(426) - plt.plot(t , Res[:, 7], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Seventh CSS') + plt.plot(t, Res[:, 7], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Seventh CSS") plt.grid() plt.subplot(427) - plt.plot(t , Res[:, 4], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.xlabel('t(s)') - plt.title('Fourth CSS') + plt.plot(t, Res[:, 4], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.xlabel("t(s)") + plt.title("Fourth CSS") plt.grid() plt.subplot(428) - plt.plot(t , Res[:, 8], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.xlabel('t(s)') - plt.title('Eight CSS') + plt.plot(t, Res[:, 8], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.xlabel("t(s)") + plt.title("Eight CSS") plt.grid() - unitTestSupport.writeFigureLaTeX('PostFit' + str(testNum), 'Post Fit Residuals', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "PostFit" + str(testNum), + "Post Fit Residuals", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() diff --git a/src/fswAlgorithms/attDetermination/sunlineUKF/_UnitTest/test_SunLineUKF.py b/src/fswAlgorithms/attDetermination/sunlineUKF/_UnitTest/test_SunLineUKF.py index 9afd6b03a7..de3c8696e2 100644 --- a/src/fswAlgorithms/attDetermination/sunlineUKF/_UnitTest/test_SunLineUKF.py +++ b/src/fswAlgorithms/attDetermination/sunlineUKF/_UnitTest/test_SunLineUKF.py @@ -30,24 +30,58 @@ def addTimeColumn(time, data): return numpy.transpose(numpy.vstack([[time], numpy.transpose(data)])) + def setupFilterData(filterObject): filterObject.alpha = 0.02 filterObject.beta = 2.0 filterObject.kappa = 0.0 filterObject.state = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0] - filterObject.covar = [0.4, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.04] + filterObject.covar = [ + 0.4, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.4, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.4, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.04, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.04, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.04, + ] qNoiseIn = numpy.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3]*0.01*0.01 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6]*0.001*0.001 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 0.01 * 0.01 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 0.001 * 0.001 filterObject.qNoise = qNoiseIn.reshape(36).tolist() filterObject.qObsVal = 0.001 + # uncomment this line is 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 @@ -55,10 +89,10 @@ def setupFilterData(filterObject): # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("function", ["sunline_utilities_test" - , "checkStatePropSunLine" - , "checkStateUpdateSunLine" - ]) +@pytest.mark.parametrize( + "function", + ["sunline_utilities_test", "checkStatePropSunLine", "checkStateUpdateSunLine"], +) def test_all_sunline_kf(show_plots, function): """Module Unit Test""" testFunction = globals().get(function) @@ -81,12 +115,32 @@ def sunline_utilities_test(show_plots): testMessages = [] # create empty list to store test log messages # Initialize the test module configuration data - AMatrix = [0.488894, 0.888396, 0.325191, 0.319207, - 1.03469, -1.14707, -0.754928, 0.312859, - 0.726885, -1.06887, 1.3703, -0.86488, - -0.303441, -0.809499, -1.71152, -0.0300513, - 0.293871, -2.94428, -0.102242, -0.164879, - -0.787283, 1.43838, -0.241447, 0.627707] + AMatrix = [ + 0.488894, + 0.888396, + 0.325191, + 0.319207, + 1.03469, + -1.14707, + -0.754928, + 0.312859, + 0.726885, + -1.06887, + 1.3703, + -0.86488, + -0.303441, + -0.809499, + -1.71152, + -0.0300513, + 0.293871, + -2.94428, + -0.102242, + -0.164879, + -0.787283, + 1.43838, + -0.241447, + 0.627707, + ] RVector = sunlineUKF.new_doubleArray(len(AMatrix)) AVector = sunlineUKF.new_doubleArray(len(AMatrix)) @@ -96,23 +150,40 @@ def sunline_utilities_test(show_plots): sunlineUKF.ukfQRDJustR(AVector, 6, 4, RVector) RMatrix = [] - for i in range(4*4): + for i in range(4 * 4): RMatrix.append(sunlineUKF.doubleArray_getitem(RVector, i)) - RBaseNumpy = numpy.array(RMatrix).reshape(4,4) - AMatNumpy = numpy.array(AMatrix).reshape(6,4) - q,r = numpy.linalg.qr(AMatNumpy) + RBaseNumpy = numpy.array(RMatrix).reshape(4, 4) + AMatNumpy = numpy.array(AMatrix).reshape(6, 4) + q, r = numpy.linalg.qr(AMatNumpy) for i in range(r.shape[0]): - if r[i,i] < 0.0: - r[i,:] *= -1.0 - if numpy.linalg.norm(r - RBaseNumpy) > 1.0E-15: + if r[i, i] < 0.0: + r[i, :] *= -1.0 + if numpy.linalg.norm(r - RBaseNumpy) > 1.0e-15: testFailCount += 1 testMessages.append("QR Decomposition accuracy failure") - AMatrix = [1.09327, 1.10927, -0.863653, 1.32288, - -1.21412, -1.1135, -0.00684933, -2.43508, - -0.769666, 0.371379, -0.225584, -1.76492, - -1.08906, 0.0325575, 0.552527, -1.6256, - 1.54421, 0.0859311, -1.49159, 1.59683] + AMatrix = [ + 1.09327, + 1.10927, + -0.863653, + 1.32288, + -1.21412, + -1.1135, + -0.00684933, + -2.43508, + -0.769666, + 0.371379, + -0.225584, + -1.76492, + -1.08906, + 0.0325575, + 0.552527, + -1.6256, + 1.54421, + 0.0859311, + -1.49159, + 1.59683, + ] RVector = sunlineUKF.new_doubleArray(len(AMatrix)) AVector = sunlineUKF.new_doubleArray(len(AMatrix)) @@ -122,24 +193,19 @@ def sunline_utilities_test(show_plots): sunlineUKF.ukfQRDJustR(AVector, 5, 4, RVector) RMatrix = [] - for i in range(4*4): + for i in range(4 * 4): RMatrix.append(sunlineUKF.doubleArray_getitem(RVector, i)) - RBaseNumpy = numpy.array(RMatrix).reshape(4,4) - AMatNumpy = numpy.array(AMatrix).reshape(5,4) - q,r = numpy.linalg.qr(AMatNumpy) + RBaseNumpy = numpy.array(RMatrix).reshape(4, 4) + AMatNumpy = numpy.array(AMatrix).reshape(5, 4) + q, r = numpy.linalg.qr(AMatNumpy) for i in range(r.shape[0]): - if r[i,i] < 0.0: - r[i,:] *= -1.0 - if numpy.linalg.norm(r - RBaseNumpy) > 1.0E-14: + if r[i, i] < 0.0: + r[i, :] *= -1.0 + if numpy.linalg.norm(r - RBaseNumpy) > 1.0e-14: testFailCount += 1 testMessages.append("QR Decomposition accuracy failure") - AMatrix = [ 0.2236, 0, - 0, 0.2236, - -0.2236, 0, - 0, -0.2236, - 0.0170, 0, - 0, 0.0170] + AMatrix = [0.2236, 0, 0, 0.2236, -0.2236, 0, 0, -0.2236, 0.0170, 0, 0, 0.0170] RVector = sunlineUKF.new_doubleArray(len(AMatrix)) AVector = sunlineUKF.new_doubleArray(len(AMatrix)) @@ -149,21 +215,20 @@ def sunline_utilities_test(show_plots): sunlineUKF.ukfQRDJustR(AVector, 6, 2, RVector) RMatrix = [] - for i in range(2*2): + for i in range(2 * 2): RMatrix.append(sunlineUKF.doubleArray_getitem(RVector, i)) - RBaseNumpy = numpy.array(RMatrix).reshape(2,2) - AMatNumpy = numpy.array(AMatrix).reshape(6,2) - q,r = numpy.linalg.qr(AMatNumpy) + RBaseNumpy = numpy.array(RMatrix).reshape(2, 2) + AMatNumpy = numpy.array(AMatrix).reshape(6, 2) + q, r = numpy.linalg.qr(AMatNumpy) for i in range(r.shape[0]): - if r[i,i] < 0.0: - r[i,:] *= -1.0 + if r[i, i] < 0.0: + r[i, :] *= -1.0 - if numpy.linalg.norm(r - RBaseNumpy) > 1.0E-15: + if numpy.linalg.norm(r - RBaseNumpy) > 1.0e-15: testFailCount += 1 testMessages.append("QR Decomposition accuracy failure") - - LUSourceMat = [8,1,6,3,5,7,4,9,2] + LUSourceMat = [8, 1, 6, 3, 5, 7, 4, 9, 2] LUSVector = sunlineUKF.new_doubleArray(len(LUSourceMat)) LVector = sunlineUKF.new_doubleArray(len(LUSourceMat)) UVector = sunlineUKF.new_doubleArray(len(LUSourceMat)) @@ -175,65 +240,66 @@ def sunline_utilities_test(show_plots): sunlineUKF.doubleArray_setitem(LVector, i, 0.0) exCount = sunlineUKF.ukfLUD(LUSVector, 3, 3, LVector, intSwapVector) - #sunlineUKF.ukfUInv(LUSVector, 3, 3, UVector) + # sunlineUKF.ukfUInv(LUSVector, 3, 3, UVector) LMatrix = [] UMatrix = [] - #UMatrix = [] + # UMatrix = [] for i in range(3): currRow = sunlineUKF.intArray_getitem(intSwapVector, i) for j in range(3): - if(ji): + elif j > i: LMatrix.append(0.0) - UMatrix.append(sunlineUKF.doubleArray_getitem(LVector, i*3+j)) + UMatrix.append(sunlineUKF.doubleArray_getitem(LVector, i * 3 + j)) else: LMatrix.append(1.0) - UMatrix.append(sunlineUKF.doubleArray_getitem(LVector, i*3+j)) + UMatrix.append(sunlineUKF.doubleArray_getitem(LVector, i * 3 + j)) # UMatrix.append(sunlineUKF.doubleArray_getitem(UVector, i)) - LMatrix = numpy.array(LMatrix).reshape(3,3) - UMatrix = numpy.array(UMatrix).reshape(3,3) + LMatrix = numpy.array(LMatrix).reshape(3, 3) + UMatrix = numpy.array(UMatrix).reshape(3, 3) outMat = numpy.dot(LMatrix, UMatrix) - outMatSwap = numpy.zeros((3,3)) + outMatSwap = numpy.zeros((3, 3)) for i in range(3): currRow = sunlineUKF.intArray_getitem(intSwapVector, i) - outMatSwap[i,:] = outMat[currRow, :] - outMat[currRow,:] = outMat[i, :] - LuSourceArray = numpy.array(LUSourceMat).reshape(3,3) + outMatSwap[i, :] = outMat[currRow, :] + outMat[currRow, :] = outMat[i, :] + LuSourceArray = numpy.array(LUSourceMat).reshape(3, 3) - if(numpy.linalg.norm(outMatSwap - LuSourceArray) > 1.0E-14): + if numpy.linalg.norm(outMatSwap - LuSourceArray) > 1.0e-14: testFailCount += 1 testMessages.append("LU Decomposition accuracy failure") EqnSourceMat = [2.0, 1.0, 3.0, 2.0, 6.0, 8.0, 6.0, 8.0, 18.0] BVector = [1.0, 3.0, 5.0] EqnVector = sunlineUKF.new_doubleArray(len(EqnSourceMat)) - EqnBVector = sunlineUKF.new_doubleArray(len(LUSourceMat)//3) - EqnOutVector = sunlineUKF.new_doubleArray(len(LUSourceMat)//3) + EqnBVector = sunlineUKF.new_doubleArray(len(LUSourceMat) // 3) + EqnOutVector = sunlineUKF.new_doubleArray(len(LUSourceMat) // 3) for i in range(len(EqnSourceMat)): sunlineUKF.doubleArray_setitem(EqnVector, i, EqnSourceMat[i]) - sunlineUKF.doubleArray_setitem(EqnBVector, i//3, BVector[i//3]) - sunlineUKF.intArray_setitem(intSwapVector, i//3, 0) + sunlineUKF.doubleArray_setitem(EqnBVector, i // 3, BVector[i // 3]) + sunlineUKF.intArray_setitem(intSwapVector, i // 3, 0) sunlineUKF.doubleArray_setitem(LVector, i, 0.0) exCount = sunlineUKF.ukfLUD(EqnVector, 3, 3, LVector, intSwapVector) sunlineUKF.ukfLUBckSlv(LVector, 3, 3, intSwapVector, EqnBVector, EqnOutVector) - expectedSol = [3.0/10.0, 4.0/10.0, 0.0] + expectedSol = [3.0 / 10.0, 4.0 / 10.0, 0.0] errorVal = 0.0 for i in range(3): - errorVal += abs(sunlineUKF.doubleArray_getitem(EqnOutVector, i) -expectedSol[i]) + errorVal += abs( + sunlineUKF.doubleArray_getitem(EqnOutVector, i) - expectedSol[i] + ) - if(errorVal > 1.0E-14): + if errorVal > 1.0e-14: testFailCount += 1 testMessages.append("LU Back-Solve accuracy failure") - - InvSourceMat = [8,1,6,3,5,7,4,9,2] + InvSourceMat = [8, 1, 6, 3, 5, 7, 4, 9, 2] SourceVector = sunlineUKF.new_doubleArray(len(InvSourceMat)) InvVector = sunlineUKF.new_doubleArray(len(InvSourceMat)) for i in range(len(InvSourceMat)): @@ -247,13 +313,12 @@ def sunline_utilities_test(show_plots): InvOut.append(sunlineUKF.doubleArray_getitem(InvVector, i)) InvOut = numpy.array(InvOut).reshape(nRow, nRow) - expectIdent = numpy.dot(InvOut, numpy.array(InvSourceMat).reshape(3,3)) + expectIdent = numpy.dot(InvOut, numpy.array(InvSourceMat).reshape(3, 3)) errorNorm = numpy.linalg.norm(expectIdent - numpy.identity(3)) - if(errorNorm > 1.0E-14): + if errorNorm > 1.0e-14: testFailCount += 1 testMessages.append("LU Matrix Inverse accuracy failure") - cholTestMat = [1.0, 0.0, 0.0, 0.0, 10.0, 5.0, 0.0, 5.0, 10.0] SourceVector = sunlineUKF.new_doubleArray(len(cholTestMat)) CholVector = sunlineUKF.new_doubleArray(len(cholTestMat)) @@ -269,15 +334,28 @@ def sunline_utilities_test(show_plots): cholOut = numpy.array(cholOut).reshape(nRow, nRow) cholComp = numpy.linalg.cholesky(numpy.array(cholTestMat).reshape(nRow, nRow)) errorNorm = numpy.linalg.norm(cholOut - cholComp) - if(errorNorm > 1.0E-14): + if errorNorm > 1.0e-14: testFailCount += 1 testMessages.append("Cholesky Matrix Decomposition accuracy failure") - - InvSourceMat = [2.1950926119414667, 0.0, 0.0, 0.0, - 1.0974804773131115, 1.9010439702743847, 0.0, 0.0, - 0.0, 1.2672359635912551, 1.7923572711881284, 0.0, - 1.0974804773131113, -0.63357997864171967, 1.7920348101787789, 0.033997451205364251] + InvSourceMat = [ + 2.1950926119414667, + 0.0, + 0.0, + 0.0, + 1.0974804773131115, + 1.9010439702743847, + 0.0, + 0.0, + 0.0, + 1.2672359635912551, + 1.7923572711881284, + 0.0, + 1.0974804773131113, + -0.63357997864171967, + 1.7920348101787789, + 0.033997451205364251, + ] SourceVector = sunlineUKF.new_doubleArray(len(InvSourceMat)) InvVector = sunlineUKF.new_doubleArray(len(InvSourceMat)) @@ -292,14 +370,18 @@ def sunline_utilities_test(show_plots): InvOut.append(sunlineUKF.doubleArray_getitem(InvVector, i)) InvOut = numpy.array(InvOut).reshape(nRow, nRow) - expectIdent = numpy.dot(InvOut, numpy.array(InvSourceMat).reshape(nRow,nRow)) + expectIdent = numpy.dot(InvOut, numpy.array(InvSourceMat).reshape(nRow, nRow)) errorNorm = numpy.linalg.norm(expectIdent - numpy.identity(nRow)) - if(errorNorm > 1.0E-12): + if errorNorm > 1.0e-12: print(errorNorm) testFailCount += 1 testMessages.append("L Matrix Inverse accuracy failure") - InvSourceMat = numpy.transpose(numpy.array(InvSourceMat).reshape(nRow, nRow)).reshape(nRow*nRow).tolist() + InvSourceMat = ( + numpy.transpose(numpy.array(InvSourceMat).reshape(nRow, nRow)) + .reshape(nRow * nRow) + .tolist() + ) SourceVector = sunlineUKF.new_doubleArray(len(InvSourceMat)) InvVector = sunlineUKF.new_doubleArray(len(InvSourceMat)) for i in range(len(InvSourceMat)): @@ -313,19 +395,18 @@ def sunline_utilities_test(show_plots): InvOut.append(sunlineUKF.doubleArray_getitem(InvVector, i)) InvOut = numpy.array(InvOut).reshape(nRow, nRow) - expectIdent = numpy.dot(InvOut, numpy.array(InvSourceMat).reshape(nRow,nRow)) + expectIdent = numpy.dot(InvOut, numpy.array(InvSourceMat).reshape(nRow, nRow)) errorNorm = numpy.linalg.norm(expectIdent - numpy.identity(nRow)) - if(errorNorm > 1.0E-12): + if errorNorm > 1.0e-12: print(errorNorm) testFailCount += 1 testMessages.append("U Matrix Inverse accuracy failure") - # If the argument provided at commandline "--show_plots" evaluates as true, # plot all figures if show_plots: plt.show() - plt.close('all') + plt.close("all") # print out success message if no error were found if testFailCount == 0: @@ -335,7 +416,8 @@ def sunline_utilities_test(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def checkStateUpdateSunLine(show_plots): # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -369,14 +451,14 @@ def checkStateUpdateSunLine(show_plots): cssConstelation = messaging.CSSConfigMsgPayload() CSSOrientationList = [ - [0.70710678118654746, -0.5, 0.5], - [0.70710678118654746, -0.5, -0.5], - [0.70710678118654746, 0.5, -0.5], - [0.70710678118654746, 0.5, 0.5], - [-0.70710678118654746, 0, 0.70710678118654757], - [-0.70710678118654746, 0.70710678118654757, 0.0], - [-0.70710678118654746, 0, -0.70710678118654757], - [-0.70710678118654746, -0.70710678118654757, 0.0], + [0.70710678118654746, -0.5, 0.5], + [0.70710678118654746, -0.5, -0.5], + [0.70710678118654746, 0.5, -0.5], + [0.70710678118654746, 0.5, 0.5], + [-0.70710678118654746, 0, 0.70710678118654757], + [-0.70710678118654746, 0.70710678118654757, 0.0], + [-0.70710678118654746, 0, -0.70710678118654757], + [-0.70710678118654746, -0.70710678118654757, 0.0], ] totalCSSList = [] for CSSHat in CSSOrientationList: @@ -388,7 +470,6 @@ def checkStateUpdateSunLine(show_plots): cssConstelation.cssVals = totalCSSList cssConstInMsg = messaging.CSSConfigMsg().write(cssConstelation) - testVector = numpy.array([-0.7, 0.7, 0.0]) inputData = messaging.CSSArraySensorMsgPayload() dotList = [] @@ -414,7 +495,7 @@ def checkStateUpdateSunLine(show_plots): for i in range(400): if i > 20: cssDataInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i+1)*0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * 0.5)) unitTestSim.ExecuteSimulation() stateLog = addTimeColumn(dataLog.times(), dataLog.state) @@ -422,11 +503,11 @@ def checkStateUpdateSunLine(show_plots): covarLog = addTimeColumn(dataLog.times(), dataLog.covar) for i in range(6): - if(covarLog[-1, i*6+1+i] > covarLog[0, i*6+1+i]/100): + if covarLog[-1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i] / 100: testFailCount += 1 testMessages.append("Covariance update failure") - if(abs(stateLog[-1, i+1] - stateTarget[i]) > 1.0E-5): - print(abs(stateLog[-1, i+1] - stateTarget[i])) + if abs(stateLog[-1, i + 1] - stateTarget[i]) > 1.0e-5: + print(abs(stateLog[-1, i + 1] - stateTarget[i])) testFailCount += 1 testMessages.append("State update failure") @@ -441,7 +522,7 @@ def checkStateUpdateSunLine(show_plots): for i in range(400): if i > 20: cssDataInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i+401)*0.5)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + 401) * 0.5)) unitTestSim.ExecuteSimulation() stateLog = addTimeColumn(dataLog.times(), dataLog.state) @@ -451,16 +532,16 @@ def checkStateUpdateSunLine(show_plots): stateTarget = testVector.tolist() stateTarget.extend([0.0, 0.0, 0.0]) for i in range(6): - if(covarLog[-1, i*6+1+i] > covarLog[0, i*6+1+i]/100): + if covarLog[-1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i] / 100: testFailCount += 1 testMessages.append("Covariance update failure") - if(abs(stateLog[-1, i+1] - stateTarget[i]) > 1.0E-5): - print(abs(stateLog[-1, i+1] - stateTarget[i])) + if abs(stateLog[-1, i + 1] - stateTarget[i]) > 1.0e-5: + print(abs(stateLog[-1, i + 1] - stateTarget[i])) testFailCount += 1 testMessages.append("State update failure") - FilterPlots.StateCovarPlot(stateLog, covarLog, 'update', show_plots) - FilterPlots.PostFitResiduals(postFitLog, module.qObsVal, 'update', show_plots) + FilterPlots.StateCovarPlot(stateLog, covarLog, "update", show_plots) + FilterPlots.PostFitResiduals(postFitLog, module.qObsVal, "update", show_plots) # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag + " state update") @@ -469,11 +550,10 @@ def checkStateUpdateSunLine(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def checkStatePropSunLine(show_plots): - # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. @@ -519,17 +599,15 @@ def checkStatePropSunLine(show_plots): postFitLog = addTimeColumn(dataLog.times(), dataLog.postFitRes) covarLog = addTimeColumn(dataLog.times(), dataLog.covar) - FilterPlots.StateCovarPlot(stateLog, covarLog, 'prop', show_plots) - FilterPlots.PostFitResiduals(postFitLog, module.qObsVal, 'prop', show_plots) + FilterPlots.StateCovarPlot(stateLog, covarLog, "prop", show_plots) + FilterPlots.PostFitResiduals(postFitLog, module.qObsVal, "prop", show_plots) for i in range(6): - if(abs(stateLog[-1, i+1] - stateLog[0, i+1]) > 1.0E-10): - print(abs(stateLog[-1, i+1] - stateLog[0, i+1])) + if abs(stateLog[-1, i + 1] - stateLog[0, i + 1]) > 1.0e-10: + print(abs(stateLog[-1, i + 1] - stateLog[0, i + 1])) testFailCount += 1 testMessages.append("State propagation failure") - - # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag + " state propagation") @@ -538,7 +616,8 @@ def checkStatePropSunLine(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": test_all_sunline_kf(True) diff --git a/src/fswAlgorithms/attGuidance/attRefCorrection/_UnitTest/test_attRefCorrection.py b/src/fswAlgorithms/attGuidance/attRefCorrection/_UnitTest/test_attRefCorrection.py index 0fe33191bb..6d71b1bbc3 100644 --- a/src/fswAlgorithms/attGuidance/attRefCorrection/_UnitTest/test_attRefCorrection.py +++ b/src/fswAlgorithms/attGuidance/attRefCorrection/_UnitTest/test_attRefCorrection.py @@ -28,7 +28,6 @@ @pytest.mark.parametrize("accuracy", [1e-12]) - def test_attRefCorrection(show_plots, accuracy): r""" **Validation Test Description** @@ -64,11 +63,11 @@ def attRefCorrectionTestFunction(show_plots, accuracy): module = attRefCorrection.attRefCorrection() module.ModelTag = "attRefCorrectionTag" unitTestSim.AddModelToTask(unitTaskName, module) - module.sigma_BcB = [math.tan(math.pi/4), 0.0, 0.0] + module.sigma_BcB = [math.tan(math.pi / 4), 0.0, 0.0] # Configure blank module input messages attRefInMsgData = messaging.AttRefMsgPayload() - attRefInMsgData.sigma_RN = [math.tan(math.pi/8), 0.0, 0.0] + attRefInMsgData.sigma_RN = [math.tan(math.pi / 8), 0.0, 0.0] attRefInMsg = messaging.AttRefMsg().write(attRefInMsgData) # subscribe input messages to module @@ -86,16 +85,22 @@ def attRefCorrectionTestFunction(show_plots, accuracy): trueVector = [ [-math.tan(math.pi / 8), 0.0, 0.0], [-math.tan(math.pi / 8), 0.0, 0.0], - [-math.tan(math.pi / 8), 0.0, 0.0] + [-math.tan(math.pi / 8), 0.0, 0.0], ] # compare the module results to the truth values for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(attRefOutMsgRec.sigma_RN[i], trueVector[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + attRefOutMsgRec.sigma_RN[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_RN unit test at t=" + - str(attRefOutMsgRec.times()[i] * macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_RN unit test at t=" + + str(attRefOutMsgRec.times()[i] * macros.NANO2SEC) + + "sec\n" + ) if testFailCount == 0: print("PASSED: " + module.ModelTag) diff --git a/src/fswAlgorithms/attGuidance/attTrackingError/_UnitTest/test_attTrackingError.py b/src/fswAlgorithms/attGuidance/attTrackingError/_UnitTest/test_attTrackingError.py index dc4b8b36e9..d1808c04d3 100755 --- a/src/fswAlgorithms/attGuidance/attTrackingError/_UnitTest/test_attTrackingError.py +++ b/src/fswAlgorithms/attGuidance/attTrackingError/_UnitTest/test_attTrackingError.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -34,19 +33,19 @@ path = os.path.dirname(os.path.abspath(filename)) - - - - - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import attTrackingError # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import ( + attTrackingError, +) # import the module that is to be tested from Basilisk.utilities import macros from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.architecture import messaging + # uncomment this line is 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 @@ -60,20 +59,19 @@ def test_attTrackingError(show_plots): def subModuleTestFunction(show_plots): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = attTrackingError.attTrackingError() module.ModelTag = "attTrackingError" @@ -87,7 +85,9 @@ def subModuleTestFunction(show_plots): # # Navigation Message # - NavStateOutData = messaging.NavAttMsgPayload() # Create a structure for the input message + NavStateOutData = ( + messaging.NavAttMsgPayload() + ) # Create a structure for the input message sigma_BN = [0.25, -0.45, 0.75] NavStateOutData.sigma_BN = sigma_BN omega_BN_B = [-0.015, -0.012, 0.005] @@ -97,7 +97,9 @@ def subModuleTestFunction(show_plots): # # Reference Frame Message # - RefStateOutData = messaging.AttRefMsgPayload() # Create a structure for the input message + RefStateOutData = ( + messaging.AttRefMsgPayload() + ) # Create a structure for the input message sigma_RN = [0.35, -0.25, 0.15] RefStateOutData.sigma_RN = sigma_RN omega_RN_N = [0.018, -0.032, 0.015] @@ -121,7 +123,7 @@ def subModuleTestFunction(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.3)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.3)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -142,7 +144,9 @@ def subModuleTestFunction(show_plots): accuracy = 1e-12 if not unitTestSupport.isArrayEqual(moduleOutput, trueVector, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_BR unit test\n") + testMessages.append( + "FAILED: " + module.ModelTag + " Module failed sigma_BR unit test\n" + ) unitTestSupport.writeTeXSnippet("passFail_sigBR", "FAILED", path) else: unitTestSupport.writeTeXSnippet("passFail_sigBR", "PASSED", path) @@ -158,7 +162,9 @@ def subModuleTestFunction(show_plots): # compare the module results to the truth values if not unitTestSupport.isArrayEqual(moduleOutput, trueVector, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_BR_B unit test\n") + testMessages.append( + "FAILED: " + module.ModelTag + " Module failed omega_BR_B unit test\n" + ) unitTestSupport.writeTeXSnippet("passFail_omega_BR_B", "FAILED", path) else: unitTestSupport.writeTeXSnippet("passFail_omega_BR_B", "PASSED", path) @@ -172,9 +178,11 @@ def subModuleTestFunction(show_plots): trueVector = np.dot(BN, np.array(omega_RN_N)) # compare the module results to the truth values - if not unitTestSupport.isArrayEqual(moduleOutput,trueVector,3,accuracy): + if not unitTestSupport.isArrayEqual(moduleOutput, trueVector, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_N unit test\n") + testMessages.append( + "FAILED: " + module.ModelTag + " Module failed omega_RN_N unit test\n" + ) unitTestSupport.writeTeXSnippet("passFail_omega_RN_B", "FAILED", path) else: unitTestSupport.writeTeXSnippet("passFail_omega_RN_B", "PASSED", path) @@ -188,16 +196,18 @@ def subModuleTestFunction(show_plots): trueVector = np.dot(BN, np.array(domega_RN_N)) # compare the module results to the truth values - if not unitTestSupport.isArrayEqual(moduleOutput,trueVector,3,accuracy): + if not unitTestSupport.isArrayEqual(moduleOutput, trueVector, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed domega_RN_B unit test\n") + testMessages.append( + "FAILED: " + module.ModelTag + " Module failed domega_RN_B unit test\n" + ) unitTestSupport.writeTeXSnippet("passFail_domega_RN_B", "FAILED", path) else: unitTestSupport.writeTeXSnippet("passFail_domega_RN_B", "PASSED", path) # Note that we can continue to step the simulation however we feel like. # Just because we stop and query data does not mean everything has to stop for good - unitTestSim.ConfigureStopTime(macros.sec2nano(0.6)) # run an additional 0.6 seconds + unitTestSim.ConfigureStopTime(macros.sec2nano(0.6)) # run an additional 0.6 seconds unitTestSim.ExecuteSimulation() if testFailCount == 0: @@ -207,7 +217,7 @@ def subModuleTestFunction(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attGuidance/celestialTwoBodyPoint/_UnitTest/test_celestialTwoBodyPoint.py b/src/fswAlgorithms/attGuidance/celestialTwoBodyPoint/_UnitTest/test_celestialTwoBodyPoint.py index 6425cb782f..f4b271b4bb 100755 --- a/src/fswAlgorithms/attGuidance/celestialTwoBodyPoint/_UnitTest/test_celestialTwoBodyPoint.py +++ b/src/fswAlgorithms/attGuidance/celestialTwoBodyPoint/_UnitTest/test_celestialTwoBodyPoint.py @@ -30,18 +30,21 @@ from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import celestialTwoBodyPoint # module that is to be tested from Basilisk.utilities import RigidBodyKinematics as rbk + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import astroFunctions as af from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.architecture import astroConstants from numpy import linalg as la filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -textSnippetPassed = r'\textcolor{ForestGreen}{' + "PASSED" + '}' -textSnippetFailed = r'\textcolor{Red}{' + "Failed" + '}' +textSnippetPassed = r"\textcolor{ForestGreen}{" + "PASSED" + "}" +textSnippetFailed = r"\textcolor{Red}{" + "Failed" + "}" # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -50,16 +53,16 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ -def computeCelestialTwoBodyPoint(R_P1, v_P1, a_P1, R_P2, v_P2, a_P2): +def computeCelestialTwoBodyPoint(R_P1, v_P1, a_P1, R_P2, v_P2, a_P2): # Beforehand computations R_n = np.cross(R_P1, R_P2) v_n = np.cross(v_P1, R_P2) + np.cross(R_P1, v_P2) a_n = np.cross(a_P1, R_P2) + np.cross(R_P1, a_P2) + 2 * np.cross(v_P1, v_P2) # Reference Frame generation - r1_hat = R_P1/la.norm(R_P1) - r3_hat = R_n/la.norm(R_n) + r1_hat = R_P1 / la.norm(R_P1) + r3_hat = R_n / la.norm(R_n) r2_hat = np.cross(r3_hat, r1_hat) RN = np.array([r1_hat, r2_hat, r3_hat]) sigma_RN = rbk.C2MRP(RN) @@ -73,11 +76,9 @@ def computeCelestialTwoBodyPoint(R_P1, v_P1, a_P1, R_P2, v_P2, a_P2): dr2_hat = np.cross(dr3_hat, r1_hat) + np.cross(r3_hat, dr1_hat) # Angular Velocity computation - omega_RN_R = np.array([ - np.dot(r3_hat, dr2_hat), - np.dot(r1_hat, dr3_hat), - np.dot(r2_hat, dr1_hat) - ]) + omega_RN_R = np.array( + [np.dot(r3_hat, dr2_hat), np.dot(r1_hat, dr3_hat), np.dot(r2_hat, dr1_hat)] + ) omega_RN_N = np.dot(RN.T, omega_RN_R) # Reference base-vectors second time-derivative @@ -85,19 +86,31 @@ def computeCelestialTwoBodyPoint(R_P1, v_P1, a_P1, R_P2, v_P2, a_P2): ddr1_hat = 1.0 / la.norm(R_P1) * (np.dot(C1, a_P1) - np.dot(temp33_1, v_P1)) temp33_3 = 2 * np.outer(dr3_hat, r3_hat) + np.outer(r3_hat, dr3_hat) ddr3_hat = 1.0 / la.norm(R_n) * (np.dot(C3, a_n) - np.dot(temp33_3, v_n)) - ddr2_hat = np.cross(ddr3_hat, r1_hat) + np.cross(ddr1_hat, r3_hat) + 2 * np.cross(dr3_hat, dr1_hat) + ddr2_hat = ( + np.cross(ddr3_hat, r1_hat) + + np.cross(ddr1_hat, r3_hat) + + 2 * np.cross(dr3_hat, dr1_hat) + ) # Angular Acceleration computation - domega_RN_R = np.array([ - np.dot(dr3_hat, dr2_hat) + np.dot(r3_hat, ddr2_hat) - np.dot(omega_RN_R, dr1_hat), - np.dot(dr1_hat, dr3_hat) + np.dot(r1_hat, ddr3_hat) - np.dot(omega_RN_R, dr2_hat), - np.dot(dr2_hat, dr1_hat) + np.dot(r2_hat, ddr1_hat) - np.dot(omega_RN_R, dr3_hat) - - ]) + domega_RN_R = np.array( + [ + np.dot(dr3_hat, dr2_hat) + + np.dot(r3_hat, ddr2_hat) + - np.dot(omega_RN_R, dr1_hat), + np.dot(dr1_hat, dr3_hat) + + np.dot(r1_hat, ddr3_hat) + - np.dot(omega_RN_R, dr2_hat), + np.dot(dr2_hat, dr1_hat) + + np.dot(r2_hat, ddr1_hat) + - np.dot(omega_RN_R, dr3_hat), + ] + ) domega_RN_N = np.dot(RN.T, domega_RN_R) return sigma_RN, omega_RN_N, domega_RN_N + def test_celestialTwoBodyPointTestFunction(show_plots): """Module Unit Test""" @@ -105,9 +118,7 @@ def test_celestialTwoBodyPointTestFunction(show_plots): assert testResults < 1, testMessage - def celestialTwoBodyPointTestFunction(show_plots): - testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages unitTaskName = "unitTask" # arbitrary name (don't change) @@ -131,7 +142,6 @@ def celestialTwoBodyPointTestFunction(show_plots): # Initialize the test module configuration data module.singularityThresh = 1.0 * macros.D2R - # Previous Computation of Initial Conditions for the test a = astroConstants.REQ_EARTH * 2.8 * 1000 # m e = 0.0 @@ -139,9 +149,9 @@ def celestialTwoBodyPointTestFunction(show_plots): Omega = 0.0 omega = 0.0 f = 60 * macros.D2R - (r, v) = af.OE2RV(astroConstants.MU_EARTH*1e9, a, e, i, Omega, omega, f) - r_BN_N = np.array([0., 0., 0.]) - v_BN_N = np.array([0., 0., 0.]) + (r, v) = af.OE2RV(astroConstants.MU_EARTH * 1e9, a, e, i, Omega, omega, f) + r_BN_N = np.array([0.0, 0.0, 0.0]) + v_BN_N = np.array([0.0, 0.0, 0.0]) celPositionVec = r celVelocityVec = v @@ -149,7 +159,9 @@ def celestialTwoBodyPointTestFunction(show_plots): # is not part of the test. # Navigation Input Message - NavStateOutData = messaging.NavTransMsgPayload() # Create a structure for the input message + NavStateOutData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message NavStateOutData.r_BN_N = r_BN_N NavStateOutData.v_BN_N = v_BN_N navMsg = messaging.NavTransMsg().write(NavStateOutData) @@ -161,7 +173,6 @@ def celestialTwoBodyPointTestFunction(show_plots): CelBodyData.v_BdyZero_N = celVelocityVec celBodyMsg = messaging.EphemerisMsg().write(CelBodyData) - # Setup logging on the test module output message so that we get all the writes to it dataLog = module.attRefOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) @@ -170,7 +181,6 @@ def celestialTwoBodyPointTestFunction(show_plots): module.transNavInMsg.subscribeTo(navMsg) module.celBodyInMsg.subscribeTo(celBodyMsg) - # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() @@ -178,7 +188,7 @@ def celestialTwoBodyPointTestFunction(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -190,22 +200,24 @@ def celestialTwoBodyPointTestFunction(show_plots): Omega = 0.0 omega = 0.0 f = 60 * macros.D2R - mu = astroConstants.MU_EARTH*1e9 # m^3/s^2 + mu = astroConstants.MU_EARTH * 1e9 # m^3/s^2 (r, v) = af.OE2RV(mu, a, e, i, Omega, omega, f) - r_BN_N = np.array([0., 0., 0.]) - v_BN_N = np.array([0., 0., 0.]) + r_BN_N = np.array([0.0, 0.0, 0.0]) + v_BN_N = np.array([0.0, 0.0, 0.0]) celPositionVec = r celVelocityVec = v # Begin Method R_P1 = celPositionVec - r_BN_N v_P1 = celVelocityVec - v_BN_N - a_P1 = np.array([0., 0., 0.]) + a_P1 = np.array([0.0, 0.0, 0.0]) R_P2 = np.cross(R_P1, v_P1) v_P2 = np.cross(R_P1, a_P1) a_P2 = np.cross(v_P1, a_P1) - sigma_RN, omega_RN_N, domega_RN_N = computeCelestialTwoBodyPoint(R_P1, v_P1, a_P1, R_P2, v_P2, a_P2) + sigma_RN, omega_RN_N, domega_RN_N = computeCelestialTwoBodyPoint( + R_P1, v_P1, a_P1, R_P2, v_P2, a_P2 + ) # This pulls the actual data log from the simulation run. # Note that range(3) will provide [0, 1, 2] Those are the elements you get from the vector (all of them) @@ -217,12 +229,16 @@ def celestialTwoBodyPointTestFunction(show_plots): for i in range(0, len(moduleOutput)): if not unitTestSupport.isArrayEqual(moduleOutput[i], sigma_RN, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_RN unit test at t=" + - str(moduleOutput[i, 0] * macros.NANO2SEC) + - "sec\n") - unitTestSupport.writeTeXSnippet('passFail11', textSnippetFailed, path) + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_RN unit test at t=" + + str(moduleOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) + unitTestSupport.writeTeXSnippet("passFail11", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail11', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail11", textSnippetPassed, path) # check omega_RN_N moduleOutput = dataLog.omega_RN_N @@ -232,12 +248,16 @@ def celestialTwoBodyPointTestFunction(show_plots): for i in range(0, len(moduleOutput)): if not unitTestSupport.isArrayEqual(moduleOutput[i], omega_RN_N, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_N unit test at t=" + - str(moduleOutput[i, 0] * macros.NANO2SEC) + - "sec\n") - unitTestSupport.writeTeXSnippet('passFail12', textSnippetFailed, path) + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_RN_N unit test at t=" + + str(moduleOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) + unitTestSupport.writeTeXSnippet("passFail12", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail12', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail12", textSnippetPassed, path) # check domega_RN_N moduleOutput = dataLog.domega_RN_N @@ -247,19 +267,24 @@ def celestialTwoBodyPointTestFunction(show_plots): for i in range(0, len(moduleOutput)): if not unitTestSupport.isArrayEqual(moduleOutput[i], domega_RN_N, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed domega_RN_N unit test at t=" + - str(moduleOutput[i, 0] * macros.NANO2SEC) + - "sec\n") - unitTestSupport.writeTeXSnippet('passFail13', textSnippetFailed, path) + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed domega_RN_N unit test at t=" + + str(moduleOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) + unitTestSupport.writeTeXSnippet("passFail13", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail13', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail13", textSnippetPassed, path) if testFailCount == 0: print("PASSED: " + "celestialTwoBodyPointTestFunction") else: print(testMessages) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def test_secBodyCelestialTwoBodyPointTestFunction(show_plots): """Module Unit Test""" @@ -268,8 +293,8 @@ def test_secBodyCelestialTwoBodyPointTestFunction(show_plots): [testResults, testMessage] = secBodyCelestialTwoBodyPointTestFunction(show_plots) assert testResults < 1, testMessage -def secBodyCelestialTwoBodyPointTestFunction(show_plots): +def secBodyCelestialTwoBodyPointTestFunction(show_plots): testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages unitTaskName = "unitTask" # arbitrary name (don't change) @@ -300,10 +325,10 @@ def secBodyCelestialTwoBodyPointTestFunction(show_plots): Omega = 0.0 omega = 0.0 f = 60 * macros.D2R - mu = astroConstants.MU_EARTH*1e9 # m^3/s^2 + mu = astroConstants.MU_EARTH * 1e9 # m^3/s^2 (r, v) = af.OE2RV(mu, a, e, i, Omega, omega, f) - r_BN_N = np.array([0., 0., 0.]) - v_BN_N = np.array([0., 0., 0.]) + r_BN_N = np.array([0.0, 0.0, 0.0]) + v_BN_N = np.array([0.0, 0.0, 0.0]) celPositionVec = r celVelocityVec = v @@ -312,7 +337,9 @@ def secBodyCelestialTwoBodyPointTestFunction(show_plots): # is not part of the test. # Navigation Input Message - NavStateOutData = messaging.NavTransMsgPayload() # Create a structure for the input message + NavStateOutData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message NavStateOutData.r_BN_N = r_BN_N NavStateOutData.v_BN_N = v_BN_N navMsg = messaging.NavTransMsg().write(NavStateOutData) @@ -325,9 +352,9 @@ def secBodyCelestialTwoBodyPointTestFunction(show_plots): # Spice Input Message of Secondary Body SecBodyData = messaging.EphemerisMsgPayload() - secPositionVec = [500., 500., 500.] + secPositionVec = [500.0, 500.0, 500.0] SecBodyData.r_BdyZero_N = secPositionVec - secVelocityVec = [0., 0., 0.] + secVelocityVec = [0.0, 0.0, 0.0] SecBodyData.v_BdyZero_N = secVelocityVec cel2ndBodyMsg = messaging.EphemerisMsg().write(SecBodyData) @@ -347,7 +374,7 @@ def secBodyCelestialTwoBodyPointTestFunction(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -358,7 +385,7 @@ def secBodyCelestialTwoBodyPointTestFunction(show_plots): moduleOutput = dataLog.sigma_RN # set the filtered output truth states - trueVector = [0.474475084038, 0.273938317493, 0.191443718765] + trueVector = [0.474475084038, 0.273938317493, 0.191443718765] # compare the module results to the truth values accuracy = 1e-10 @@ -368,47 +395,59 @@ def secBodyCelestialTwoBodyPointTestFunction(show_plots): # check a vector values if not unitTestSupport.isArrayEqual(moduleOutput[i], trueVector, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_RN unit test at t=" + - str(dataLog.times()[i] * macros.NANO2SEC) + - "sec\n") - unitTestSupport.writeTeXSnippet('passFail21', textSnippetFailed, path) + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_RN unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) + unitTestSupport.writeTeXSnippet("passFail21", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail21', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail21", textSnippetPassed, path) # check omega_RN_N moduleOutput = dataLog.omega_RN_N # set the filtered output truth states - trueVector = [1.59336987e-04, 2.75979758e-04, 2.64539877e-04] + trueVector = [1.59336987e-04, 2.75979758e-04, 2.64539877e-04] # compare the module results to the truth values for i in range(0, len(moduleOutput)): # check a vector values if not unitTestSupport.isArrayEqual(moduleOutput[i], trueVector, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_N unit test at t=" + - str(dataLog.times()[i] * macros.NANO2SEC) + - "sec\n") - unitTestSupport.writeTeXSnippet('passFail22', textSnippetFailed, path) + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_RN_N unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) + unitTestSupport.writeTeXSnippet("passFail22", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail22', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail22", textSnippetPassed, path) # check domega_RN_N moduleOutput = dataLog.domega_RN_N # set the filtered output truth states - trueVector = [-2.12284893e-07, 5.69968291e-08, -4.83648052e-08] + trueVector = [-2.12284893e-07, 5.69968291e-08, -4.83648052e-08] # compare the module results to the truth values for i in range(0, len(moduleOutput)): # check a vector values if not unitTestSupport.isArrayEqual(moduleOutput[i], trueVector, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed domega_RN_N unit test at t=" + - str(dataLog.times()[i] * macros.NANO2SEC) + - "sec\n") - unitTestSupport.writeTeXSnippet('passFail23', textSnippetFailed, path) + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed domega_RN_N unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) + unitTestSupport.writeTeXSnippet("passFail23", textSnippetFailed, path) else: - unitTestSupport.writeTeXSnippet('passFail23', textSnippetPassed, path) + unitTestSupport.writeTeXSnippet("passFail23", textSnippetPassed, path) # Note that we can continue to step the simulation however we feel like. # Just because we stop and query data does not mean everything has to stop for good @@ -422,7 +461,7 @@ def secBodyCelestialTwoBodyPointTestFunction(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attGuidance/constrainedAttitudeManeuver/_UnitTest/test_ConstrainedAttitudeManeuver.py b/src/fswAlgorithms/attGuidance/constrainedAttitudeManeuver/_UnitTest/test_ConstrainedAttitudeManeuver.py index a4f9c57804..373a051902 100644 --- a/src/fswAlgorithms/attGuidance/constrainedAttitudeManeuver/_UnitTest/test_ConstrainedAttitudeManeuver.py +++ b/src/fswAlgorithms/attGuidance/constrainedAttitudeManeuver/_UnitTest/test_ConstrainedAttitudeManeuver.py @@ -33,6 +33,7 @@ from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import constrainedAttitudeManeuver from Basilisk.utilities import RigidBodyKinematics as rbk + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros @@ -45,7 +46,7 @@ def shadowSetMap(sigma, switch): sigma = np.array(sigma) if switch: - s2 = (sigma[0]**2 + sigma[1]**2 + sigma[2]**2) + s2 = sigma[0] ** 2 + sigma[1] ** 2 + sigma[2] ** 2 if not s2 == 0: return -sigma / s2 else: @@ -56,7 +57,7 @@ def shadowSetMap(sigma, switch): class constraint: def __init__(self, axis, color): - self.axis = axis / np.linalg.norm(axis) + self.axis = axis / np.linalg.norm(axis) self.color = color @@ -65,45 +66,55 @@ def __init__(self, sigma_BN, constraints, **kwargs): self.sigma_BN = np.array(sigma_BN) s = np.linalg.norm(self.sigma_BN) if s > 1: - self.sigma_BN = shadowSetMap(sigma_BN, True) # mapping to shadow set if |sigma| > 1 - if np.abs(s-1) < 1e-5: - s = 1. + self.sigma_BN = shadowSetMap( + sigma_BN, True + ) # mapping to shadow set if |sigma| > 1 + if np.abs(s - 1) < 1e-5: + s = 1.0 self.isBoundary = False self.s = s if s == 1: self.isBoundary = True self.isFree = True - self.color = '' + self.color = "" self.neighbors = {} self.heuristic = 0 self.priority = 0 self.backpointer = self # check cosntraint compliance - sigma_tilde = np.array([ [0, -sigma_BN[2], sigma_BN[1]], - [sigma_BN[2], 0, -sigma_BN[0]], - [-sigma_BN[1], sigma_BN[0], 0 ] ]) - BN = np.identity(3) + ( 8*np.matmul(sigma_tilde, sigma_tilde) -4*(1-s**2)*sigma_tilde ) / (1 + s**2)**2 + sigma_tilde = np.array( + [ + [0, -sigma_BN[2], sigma_BN[1]], + [sigma_BN[2], 0, -sigma_BN[0]], + [-sigma_BN[1], sigma_BN[0], 0], + ] + ) + BN = ( + np.identity(3) + + (8 * np.matmul(sigma_tilde, sigma_tilde) - 4 * (1 - s**2) * sigma_tilde) + / (1 + s**2) ** 2 + ) NB = BN.transpose() # checking for keepOut constraint violation - if 'keepOut_b' in kwargs: - for i in range(len(kwargs['keepOut_b'])): - b_B = np.array(kwargs['keepOut_b'][i]) + if "keepOut_b" in kwargs: + for i in range(len(kwargs["keepOut_b"])): + b_B = np.array(kwargs["keepOut_b"][i]) b_N = np.matmul(NB, b_B) - for c in constraints['keepOut']: - if np.dot(b_N, c.axis) >= np.cos(kwargs['keepOut_fov'][i]): + for c in constraints["keepOut"]: + if np.dot(b_N, c.axis) >= np.cos(kwargs["keepOut_fov"][i]): self.isFree = False self.color = c.color return # checking for keepIn constraint violation (at least one SS must see the Sun) - if 'keepIn_b' in kwargs: + if "keepIn_b" in kwargs: b_N = [] - for i in range(len(kwargs['keepIn_b'])): - b_B = np.array(kwargs['keepIn_b'][i]) + for i in range(len(kwargs["keepIn_b"])): + b_B = np.array(kwargs["keepIn_b"][i]) b_N.append(np.matmul(NB, b_B)) isIn = False - for c in constraints['keepIn']: + for c in constraints["keepIn"]: for i in range(len(b_N)): - if np.dot(b_N[i], c.axis) >= np.cos(kwargs['keepIn_fov'][i]): + if np.dot(b_N[i], c.axis) >= np.cos(kwargs["keepIn_fov"][i]): isIn = True self.color = c.color if not isIn: @@ -112,8 +123,8 @@ def __init__(self, sigma_BN, constraints, **kwargs): def distanceCart(n1, n2): d1 = np.linalg.norm(n1.sigma_BN - n2.sigma_BN) - sigma1norm2 = n1.sigma_BN[0]**2 + n1.sigma_BN[1]**2 + n1.sigma_BN[2]**2 - sigma2norm2 = n2.sigma_BN[0]**2 + n2.sigma_BN[1]**2 + n2.sigma_BN[2]**2 + sigma1norm2 = n1.sigma_BN[0] ** 2 + n1.sigma_BN[1] ** 2 + n1.sigma_BN[2] ** 2 + sigma2norm2 = n2.sigma_BN[0] ** 2 + n2.sigma_BN[1] ** 2 + n2.sigma_BN[2] ** 2 if sigma2norm2 > 1e-8: sigma2_SS = shadowSetMap(n2.sigma_BN, True) d2 = np.linalg.norm(n1.sigma_BN - sigma2_SS) @@ -135,67 +146,134 @@ def distanceCart(n1, n2): def distanceMRP(n1, n2): s1 = n1.sigma_BN s2 = n2.sigma_BN - sigma1norm2 = n1.sigma_BN[0]**2 + n1.sigma_BN[1]**2 + n1.sigma_BN[2]**2 - sigma2norm2 = n2.sigma_BN[0]**2 + n2.sigma_BN[1]**2 + n2.sigma_BN[2]**2 + sigma1norm2 = n1.sigma_BN[0] ** 2 + n1.sigma_BN[1] ** 2 + n1.sigma_BN[2] ** 2 + sigma2norm2 = n2.sigma_BN[0] ** 2 + n2.sigma_BN[1] ** 2 + n2.sigma_BN[2] ** 2 - D = 1 + (sigma1norm2*sigma2norm2)**2 + 2*np.dot(s1, s2) + D = 1 + (sigma1norm2 * sigma2norm2) ** 2 + 2 * np.dot(s1, s2) if abs(D) < 1e-5: s2 = shadowSetMap(s2, True) sigma2norm2 = 1 / sigma2norm2 - D = 1 + (sigma1norm2*sigma2norm2)**2 + 2*np.dot(s1, s2) + D = 1 + (sigma1norm2 * sigma2norm2) ** 2 + 2 * np.dot(s1, s2) - s12 = ( (1-sigma2norm2)*s1 - (1-sigma1norm2)*s2 + 2*np.cross(s1, s2) ) / D - sigma12norm2 = np.linalg.norm(s12)**2 + s12 = ((1 - sigma2norm2) * s1 - (1 - sigma1norm2) * s2 + 2 * np.cross(s1, s2)) / D + sigma12norm2 = np.linalg.norm(s12) ** 2 if sigma12norm2 > 1: s12 = shadowSetMap(s12, True) - return 4*np.arctan(np.linalg.norm(s12)) + return 4 * np.arctan(np.linalg.norm(s12)) def mirrorFunction(i, j, k): - return [ [i, j, k], [-i, j, k], [i, -j, k], [i, j, -k], [-i, -j, k], [-i, j, -k], [i, -j, -k], [-i, -j, -k] ] + return [ + [i, j, k], + [-i, j, k], + [i, -j, k], + [i, j, -k], + [-i, -j, k], + [-i, j, -k], + [i, -j, -k], + [-i, -j, -k], + ] def neighboringNodes(i, j, k): - return [ [i-1, j, k], [i+1, j, k], [i, j-1, k], [i, j+1, k], [i, j, k-1], [i, j, k+1], - [i-1, j-1, k], [i+1, j-1, k], [i-1, j+1, k], [i+1, j+1, k], [i-1, j, k-1], [i+1, j, k-1], - [i-1, j, k+1], [i+1, j, k+1], [i, j-1, k-1], [i, j+1, k-1], [i, j-1, k+1], [i, j+1, k+1], - [i-1,j-1,k-1], [i+1,j-1,k-1], [i-1,j+1,k-1], [i-1,j-1,k+1], [i+1,j+1,k-1], [i+1,j-1,k+1], [i-1,j+1,k+1], [i+1,j+1,k+1] ] + return [ + [i - 1, j, k], + [i + 1, j, k], + [i, j - 1, k], + [i, j + 1, k], + [i, j, k - 1], + [i, j, k + 1], + [i - 1, j - 1, k], + [i + 1, j - 1, k], + [i - 1, j + 1, k], + [i + 1, j + 1, k], + [i - 1, j, k - 1], + [i + 1, j, k - 1], + [i - 1, j, k + 1], + [i + 1, j, k + 1], + [i, j - 1, k - 1], + [i, j + 1, k - 1], + [i, j - 1, k + 1], + [i, j + 1, k + 1], + [i - 1, j - 1, k - 1], + [i + 1, j - 1, k - 1], + [i - 1, j + 1, k - 1], + [i - 1, j - 1, k + 1], + [i + 1, j + 1, k - 1], + [i + 1, j - 1, k + 1], + [i - 1, j + 1, k + 1], + [i + 1, j + 1, k + 1], + ] def generateGrid(n_start, n_goal, N, constraints, data): - - u = np.linspace(0, 1, N, endpoint = True) + u = np.linspace(0, 1, N, endpoint=True) nodes = {} # add internal nodes (|sigma| <= 1) for i in range(N): for j in range(N): for k in range(N): - if (u[i]**2+u[j]**2+u[k]**2) <= 1: + if (u[i] ** 2 + u[j] ** 2 + u[k] ** 2) <= 1: for m in mirrorFunction(i, j, k): if (m[0], m[1], m[2]) not in nodes: - nodes[(m[0], m[1], m[2])] = node([np.sign(m[0])*u[i], np.sign(m[1])*u[j], np.sign(m[2])*u[k]], constraints, **data) + nodes[(m[0], m[1], m[2])] = node( + [ + np.sign(m[0]) * u[i], + np.sign(m[1]) * u[j], + np.sign(m[2]) * u[k], + ], + constraints, + **data, + ) # add missing boundary nodes (|sigma| = 1) - for i in range(N-1): - for j in range(N-1): - for k in range(N-1): + for i in range(N - 1): + for j in range(N - 1): + for k in range(N - 1): if (i, j, k) in nodes: if not nodes[(i, j, k)].isBoundary: - if (i+1, j, k) not in nodes: - for m in mirrorFunction(i+1, j, k): + if (i + 1, j, k) not in nodes: + for m in mirrorFunction(i + 1, j, k): if (m[0], m[1], m[2]) not in nodes: - nodes[(m[0], m[1], m[2])] = node([np.sign(m[0])*(1-u[j]**2-u[k]**2)**0.5, np.sign(m[1])*u[j], np.sign(m[2])*u[k]], constraints, **data) - if (i, j+1, k) not in nodes: - for m in mirrorFunction(i, j+1, k): + nodes[(m[0], m[1], m[2])] = node( + [ + np.sign(m[0]) + * (1 - u[j] ** 2 - u[k] ** 2) ** 0.5, + np.sign(m[1]) * u[j], + np.sign(m[2]) * u[k], + ], + constraints, + **data, + ) + if (i, j + 1, k) not in nodes: + for m in mirrorFunction(i, j + 1, k): if (m[0], m[1], m[2]) not in nodes: - nodes[(m[0], m[1], m[2])] = node([np.sign(m[0])*u[i], np.sign(m[1])*(1-u[i]**2-u[k]**2)**0.5, np.sign(m[2])*u[k]], constraints, **data) - if (i, j, k+1) not in nodes: - for m in mirrorFunction(i, j, k+1): + nodes[(m[0], m[1], m[2])] = node( + [ + np.sign(m[0]) * u[i], + np.sign(m[1]) + * (1 - u[i] ** 2 - u[k] ** 2) ** 0.5, + np.sign(m[2]) * u[k], + ], + constraints, + **data, + ) + if (i, j, k + 1) not in nodes: + for m in mirrorFunction(i, j, k + 1): if (m[0], m[1], m[2]) not in nodes: - nodes[(m[0], m[1], m[2])] = node([np.sign(m[0])*u[i], np.sign(m[1])*u[j], np.sign(m[2])*(1-u[i]**2-u[j]**2)**0.5], constraints, **data) + nodes[(m[0], m[1], m[2])] = node( + [ + np.sign(m[0]) * u[i], + np.sign(m[1]) * u[j], + np.sign(m[2]) + * (1 - u[i] ** 2 - u[j] ** 2) ** 0.5, + ], + constraints, + **data, + ) # link nodes for key1 in nodes: @@ -212,7 +290,11 @@ def generateGrid(n_start, n_goal, N, constraints, data): if nodes[key1].isBoundary: if (-i, -j, -k) in nodes: for key2 in nodes[(-i, -j, -k)].neighbors: - if nodes[key1].isFree and nodes[key2].isFree and key2 not in nodes[key1].neighbors: + if ( + nodes[key1].isFree + and nodes[key2].isFree + and key2 not in nodes[key1].neighbors + ): nodes[key1].neighbors[key2] = nodes[key2] # add start and goal nodes @@ -222,8 +304,10 @@ def generateGrid(n_start, n_goal, N, constraints, data): for key in nodes: if nodes[key].isFree: d1 = distanceCart(nodes[key], n_start) - if abs(d1-ds) < 1e-6: - if np.linalg.norm(nodes[key].sigma_BN) < np.linalg.norm(nodes[key_s].sigma_BN): + if abs(d1 - ds) < 1e-6: + if np.linalg.norm(nodes[key].sigma_BN) < np.linalg.norm( + nodes[key_s].sigma_BN + ): ds = d1 n_s = nodes[key] key_s = key @@ -233,8 +317,10 @@ def generateGrid(n_start, n_goal, N, constraints, data): n_s = nodes[key] key_s = key d2 = distanceCart(nodes[key], n_goal) - if abs(d2-dg) < 1e-6: - if np.linalg.norm(nodes[key].sigma_BN) < np.linalg.norm(nodes[key_g].sigma_BN): + if abs(d2 - dg) < 1e-6: + if np.linalg.norm(nodes[key].sigma_BN) < np.linalg.norm( + nodes[key_g].sigma_BN + ): dg = d2 n_g = nodes[key] key_g = key @@ -264,32 +350,31 @@ def backtrack(n, n_start): def pathHandle(path, avgOmega): - T = [0] S = 0 - for n in range(len(path)-1): - T.append(T[n] + distanceCart(path[n], path[n+1])) - S += T[n+1] - T[n] + for n in range(len(path) - 1): + T.append(T[n] + distanceCart(path[n], path[n + 1])) + S += T[n + 1] - T[n] X1 = [] X2 = [] X3 = [] shadowSet = False - for n in range(len(path)-1): + for n in range(len(path) - 1): if not shadowSet: sigma = path[n].sigma_BN else: - if unitTestSupport.isVectorEqual(path[n].sigma_BN, [0,0,0], 1e-6): - for m in range(0,n): - s2 = (X1[m]**2 + X2[m]**2 + X3[m]**2)**0.5 + if unitTestSupport.isVectorEqual(path[n].sigma_BN, [0, 0, 0], 1e-6): + for m in range(0, n): + s2 = (X1[m] ** 2 + X2[m] ** 2 + X3[m] ** 2) ** 0.5 X1[m] = -X1[m] / s2 X2[m] = -X2[m] / s2 X3[m] = -X3[m] / s2 shadowSet = not shadowSet sigma = rbk.MRPswitch(path[n].sigma_BN, 0) - delSigma = path[n+1].sigma_BN - path[n].sigma_BN - if (np.linalg.norm(delSigma) > 1): + delSigma = path[n + 1].sigma_BN - path[n].sigma_BN + if np.linalg.norm(delSigma) > 1: shadowSet = not shadowSet X1.append(sigma[0]) X2.append(sigma[1]) @@ -303,14 +388,13 @@ def pathHandle(path, avgOmega): X3.append(sigma[2]) Input = BSpline.InputDataSet(X1, X2, X3) - Input.setT( np.array(T) * 4 * S / (T[-1] * avgOmega) ) + Input.setT(np.array(T) * 4 * S / (T[-1] * avgOmega)) return Input def spline(Input, omegaS, omegaG): - - sigmaS = [Input.X1[0][0], Input.X2[0][0], Input.X3[0][0]] + sigmaS = [Input.X1[0][0], Input.X2[0][0], Input.X3[0][0]] sigmaG = [Input.X1[-1][0], Input.X2[-1][0], Input.X3[-1][0]] sigmaDotS = rbk.dMRP(sigmaS, omegaS) sigmaDotG = rbk.dMRP(sigmaG, omegaG) @@ -325,7 +409,6 @@ def spline(Input, omegaS, omegaG): def computeTorque(sigma, sigmaDot, sigmaDDot, I): - omega = rbk.dMRP2Omega(sigma, sigmaDot) omegaDot = rbk.ddMRP2dOmega(sigma, sigmaDot, sigmaDDot) @@ -333,19 +416,26 @@ def computeTorque(sigma, sigmaDot, sigmaDDot, I): def effortEvaluation(Output, I): - effort = 0 - sigma = [Output.X1[0][0], Output.X2[0][0], Output.X3[0][0]] - sigmaDot = [Output.XD1[0][0], Output.XD2[0][0], Output.XD3[0][0]] + sigma = [Output.X1[0][0], Output.X2[0][0], Output.X3[0][0]] + sigmaDot = [Output.XD1[0][0], Output.XD2[0][0], Output.XD3[0][0]] sigmaDDot = [Output.XDD1[0][0], Output.XDD2[0][0], Output.XDD3[0][0]] L_a = computeTorque(sigma, sigmaDot, sigmaDDot, I) - for n in range(len(Output.T)-1): - sigma = [Output.X1[n+1][0], Output.X2[n+1][0], Output.X3[n+1][0]] - sigmaDot = [Output.XD1[n+1][0], Output.XD2[n+1][0], Output.XD3[n+1][0]] - sigmaDDot = [Output.XDD1[n+1][0], Output.XDD2[n+1][0], Output.XDD3[n+1][0]] + for n in range(len(Output.T) - 1): + sigma = [Output.X1[n + 1][0], Output.X2[n + 1][0], Output.X3[n + 1][0]] + sigmaDot = [Output.XD1[n + 1][0], Output.XD2[n + 1][0], Output.XD3[n + 1][0]] + sigmaDDot = [ + Output.XDD1[n + 1][0], + Output.XDD2[n + 1][0], + Output.XDD3[n + 1][0], + ] L_b = computeTorque(sigma, sigmaDot, sigmaDDot, I) - effort += (np.linalg.norm(L_a) + np.linalg.norm(L_b)) * (Output.T[n+1][0] - Output.T[n][0]) / 2 + effort += ( + (np.linalg.norm(L_a) + np.linalg.norm(L_b)) + * (Output.T[n + 1][0] - Output.T[n][0]) + / 2 + ) L_a = L_b @@ -353,7 +443,6 @@ def effortEvaluation(Output, I): def AStar(nodes, n_start, n_goal): - for key in nodes: nodes[key].heuristic = distanceCart(nodes[key], n_goal) @@ -366,7 +455,12 @@ def AStar(nodes, n_start, n_goal): C.append(O[0]) for key in O[0].neighbors: if nodes[key] not in C: - p = nodes[key].heuristic + distanceCart(O[0], nodes[key]) + O[0].priority - O[0].heuristic + p = ( + nodes[key].heuristic + + distanceCart(O[0], nodes[key]) + + O[0].priority + - O[0].heuristic + ) if nodes[key] in O: if p < nodes[key].priority: nodes[key].priority = p @@ -380,7 +474,7 @@ def AStar(nodes, n_start, n_goal): if not O: print("Dead end") else: - O.sort(key = lambda x: x.priority) + O.sort(key=lambda x: x.priority) path = backtrack(O[0], n_start) @@ -388,7 +482,6 @@ def AStar(nodes, n_start, n_goal): def effortBasedAStar(nodes, n_start, n_goal, omegaS, omegaG, avgOmega, I): - O = [n_start] C = [] n = 0 @@ -419,20 +512,23 @@ def effortBasedAStar(nodes, n_start, n_goal, omegaS, omegaG, avgOmega, I): if not O: print("Dead end") else: - O.sort(key = lambda x: x.priority) + O.sort(key=lambda x: x.priority) path = backtrack(O[0], n_start) return path + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("N", [5,6]) +@pytest.mark.parametrize("N", [5, 6]) @pytest.mark.parametrize("keepOutFov", [20]) @pytest.mark.parametrize("keepInFov", [70]) -@pytest.mark.parametrize("costFcnType", [0,1]) +@pytest.mark.parametrize("costFcnType", [0, 1]) @pytest.mark.parametrize("accuracy", [1e-12]) -def test_constrainedAttitudeManeuver(show_plots, N, keepOutFov, keepInFov, costFcnType, accuracy): +def test_constrainedAttitudeManeuver( + show_plots, N, keepOutFov, keepInFov, costFcnType, accuracy +): r""" **Validation Test Description** @@ -472,20 +568,20 @@ def test_constrainedAttitudeManeuver(show_plots, N, keepOutFov, keepInFov, costF """ # each test method requires a single assert method to be called - [testResults, testMessage] = CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy) + [testResults, testMessage] = CAMTestFunction( + N, keepOutFov, keepInFov, costFcnType, accuracy + ) assert testResults < 1, testMessage -def CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) +def CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy): + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) - Inertia = [0.02 / 3, 0., 0., - 0., 0.1256 / 3, 0., - 0., 0., 0.1256 / 3] + Inertia = [0.02 / 3, 0.0, 0.0, 0.0, 0.1256 / 3, 0.0, 0.0, 0.0, 0.1256 / 3] InertiaTensor = unitTestSupport.np2EigenMatrix3d(Inertia) PlanetInertialPosition = np.array([10, 0, 0]) SCInertialPosition = np.array([1, 0, 0]) @@ -500,18 +596,34 @@ def CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy): keepOutFov = keepOutFov * macros.D2R keepInFov = keepInFov * macros.D2R - constraints = {'keepOut' : [], 'keepIn' : []} - constraints['keepOut'].append( constraint(PlanetInertialPosition-SCInertialPosition, 'r') ) - constraints['keepIn'].append( constraint(PlanetInertialPosition-SCInertialPosition, 'g') ) - data = {'keepOut_b' : keepOutBoresight_B, 'keepOut_fov' : [keepOutFov], - 'keepIn_b' : keepInBoresight_B, 'keepIn_fov' : [keepInFov, keepInFov]} + constraints = {"keepOut": [], "keepIn": []} + constraints["keepOut"].append( + constraint(PlanetInertialPosition - SCInertialPosition, "r") + ) + constraints["keepIn"].append( + constraint(PlanetInertialPosition - SCInertialPosition, "g") + ) + data = { + "keepOut_b": keepOutBoresight_B, + "keepOut_fov": [keepOutFov], + "keepIn_b": keepInBoresight_B, + "keepIn_fov": [keepInFov, keepInFov], + } n_start = node(SCInitialAttitude, constraints, **data) - n_goal = node(SCTargetAttitude, constraints, **data) + n_goal = node(SCTargetAttitude, constraints, **data) nodes = generateGrid(n_start, n_goal, N, constraints, data) if costFcnType == 0: path = AStar(nodes, n_start, n_goal) else: - path = effortBasedAStar(nodes, n_start, n_goal, SCInitialAngRate, SCTargetAngRate, SCAvgAngRate, InertiaTensor) + path = effortBasedAStar( + nodes, + n_start, + n_goal, + SCInitialAngRate, + SCTargetAngRate, + SCAvgAngRate, + InertiaTensor, + ) Input = pathHandle(path, SCAvgAngRate) Output = spline(Input, SCInitialAngRate, SCTargetAngRate) pathCost = effortEvaluation(Output, InertiaTensor) @@ -521,7 +633,7 @@ def CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy): # Create test thread simulationTime = macros.min2nano(2) - testProcessRate = macros.sec2nano(0.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -555,7 +667,9 @@ def CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy): testModule.keepInCelBodyInMsg.subscribeTo(PlanetStateMsg) numDataPoints = 200 - samplingTime = unitTestSupport.samplingTime(simulationTime, testProcessRate, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, testProcessRate, numDataPoints + ) CAMLog = testModule.attRefOutMsg.recorder(samplingTime) unitTestSim.AddModelToTask(unitTaskName, CAMLog) @@ -569,7 +683,7 @@ def CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(simulationTime) # seconds to stop simulation + unitTestSim.ConfigureStopTime(simulationTime) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -579,68 +693,126 @@ def CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy): # print(len(CAMLog.sigma_RN)) # check correctness of grid points: - for i in range(-N,N+1): - for j in range(-N,N+1): - for k in range(-N,N+1): + for i in range(-N, N + 1): + for j in range(-N, N + 1): + for k in range(-N, N + 1): if (i, j, k) in nodes: sigma_BN = nodes[(i, j, k)].sigma_BN sigma_BN_BSK = [] for p in range(3): - sigma_BN_BSK.append( testModule.returnNodeCoord([i, j, k], p) ) - if not unitTestSupport.isVectorEqual(sigma_BN, sigma_BN_BSK, accuracy): + sigma_BN_BSK.append(testModule.returnNodeCoord([i, j, k], p)) + if not unitTestSupport.isVectorEqual( + sigma_BN, sigma_BN_BSK, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in the coordinates of node ({},{},{}) \n".format(i, j, k)) - if not nodes[(i, j, k)].isFree == testModule.returnNodeState([i, j, k]): + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in the coordinates of node ({},{},{}) \n".format( + i, j, k + ) + ) + if not nodes[(i, j, k)].isFree == testModule.returnNodeState( + [i, j, k] + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in the state of node ({},{},{}) \n".format(i, j, k)) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in the state of node ({},{},{}) \n".format( + i, j, k + ) + ) # check that the same path is produced for p in range(len(path)): sigma_BN = path[p].sigma_BN sigma_BN_BSK = [] for j in range(3): - sigma_BN_BSK.append(testModule.returnPathCoord(p,j)) + sigma_BN_BSK.append(testModule.returnPathCoord(p, j)) if not unitTestSupport.isVectorEqual(sigma_BN, sigma_BN_BSK, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in waypoint number {} in path \n".format(p)) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in waypoint number {} in path \n".format(p) + ) # check interpolated path compliance for n in range(len(Output.T)): T_BSK = testModule.Output.T[n][0] - sigma_BSK = np.array([testModule.Output.X1[n][0], testModule.Output.X2[n][0], testModule.Output.X3[n][0]]) - sigmaDot_BSK = np.array([testModule.Output.XD1[n][0], testModule.Output.XD2[n][0], testModule.Output.XD3[n][0]]) - sigmaDDot_BSK = np.array([testModule.Output.XDD1[n][0], testModule.Output.XDD2[n][0], testModule.Output.XDD3[n][0]]) + sigma_BSK = np.array( + [ + testModule.Output.X1[n][0], + testModule.Output.X2[n][0], + testModule.Output.X3[n][0], + ] + ) + sigmaDot_BSK = np.array( + [ + testModule.Output.XD1[n][0], + testModule.Output.XD2[n][0], + testModule.Output.XD3[n][0], + ] + ) + sigmaDDot_BSK = np.array( + [ + testModule.Output.XDD1[n][0], + testModule.Output.XDD2[n][0], + testModule.Output.XDD3[n][0], + ] + ) - T = Output.T[n][0] - sigma = np.array([Output.X1[n][0], Output.X2[n][0], Output.X3[n][0]]) - sigmaDot = np.array([Output.XD1[n][0], Output.XD2[n][0], Output.XD3[n][0]]) + T = Output.T[n][0] + sigma = np.array([Output.X1[n][0], Output.X2[n][0], Output.X3[n][0]]) + sigmaDot = np.array([Output.XD1[n][0], Output.XD2[n][0], Output.XD3[n][0]]) sigmaDDot = np.array([Output.XDD1[n][0], Output.XDD2[n][0], Output.XDD3[n][0]]) if not unitTestSupport.isDoubleEqual(T, T_BSK, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in time at index #{} in trajectory \n".format(n)) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in time at index #{} in trajectory \n".format(n) + ) if not unitTestSupport.isVectorEqual(sigma, sigma_BSK, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in sigma at index #{} in trajectory \n".format(n)) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in sigma at index #{} in trajectory \n".format(n) + ) if not unitTestSupport.isVectorEqual(sigmaDot, sigmaDot_BSK, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in sigmaDot at index #{} in trajectory \n".format(n)) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in sigmaDot at index #{} in trajectory \n".format(n) + ) if not unitTestSupport.isVectorEqual(sigmaDDot, sigmaDDot_BSK, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in sigmaDDot at index #{} in trajectory \n".format(n)) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in sigmaDDot at index #{} in trajectory \n".format(n) + ) # check same path cost for every spline point for n in range(len(Output.T)): c1 = testModule.computeTorqueNorm(n, Inertia) - sigma = [Output.X1[n][0], Output.X2[n][0], Output.X3[n][0]] - sigmaDot = [Output.XD1[n][0], Output.XD2[n][0], Output.XD3[n][0]] + sigma = [Output.X1[n][0], Output.X2[n][0], Output.X3[n][0]] + sigmaDot = [Output.XD1[n][0], Output.XD2[n][0], Output.XD3[n][0]] sigmaDDot = [Output.XDD1[n][0], Output.XDD2[n][0], Output.XDD3[n][0]] c2 = np.linalg.norm(computeTorque(sigma, sigmaDot, sigmaDDot, InertiaTensor)) if not unitTestSupport.isDoubleEqual(c1, c2, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error torque norm in point {} in trajectory \n".format(n)) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error torque norm in point {} in trajectory \n".format(n) + ) if not unitTestSupport.isDoubleEqual(pathCost, testModule.pathCost, accuracy): testFailCount += 1 @@ -649,9 +821,21 @@ def CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy): # check correctness of output message for n in range(len(timeData)): t = timeData[n] - sigma_RN = [Output.getStates(t,0,0), Output.getStates(t,0,1), Output.getStates(t,0,2)] - sigmaDot_RN = [Output.getStates(t,1,0), Output.getStates(t,1,1), Output.getStates(t,1,2)] - sigmaDDot_RN = [Output.getStates(t,2,0), Output.getStates(t,2,1), Output.getStates(t,2,2)] + sigma_RN = [ + Output.getStates(t, 0, 0), + Output.getStates(t, 0, 1), + Output.getStates(t, 0, 2), + ] + sigmaDot_RN = [ + Output.getStates(t, 1, 0), + Output.getStates(t, 1, 1), + Output.getStates(t, 1, 2), + ] + sigmaDDot_RN = [ + Output.getStates(t, 2, 0), + Output.getStates(t, 2, 1), + Output.getStates(t, 2, 2), + ] RN = rbk.MRP2C(sigma_RN) omega_RN_R = rbk.dMRP2Omega(sigma_RN, sigmaDot_RN) omega_RN_N = np.matmul(omega_RN_R, RN) @@ -660,26 +844,57 @@ def CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy): if not unitTestSupport.isVectorEqual(sigma_RN, CAMLog.sigma_RN[n], accuracy): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in attitude reference message at t = {} sec \n".format(t)) - if not unitTestSupport.isVectorEqual(omega_RN_N, CAMLog.omega_RN_N[n], accuracy): + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in attitude reference message at t = {} sec \n".format(t) + ) + if not unitTestSupport.isVectorEqual( + omega_RN_N, CAMLog.omega_RN_N[n], accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in attitude reference message at t = {} sec \n".format(t)) - if not unitTestSupport.isVectorEqual(omegaDot_RN_N, CAMLog.domega_RN_N[n], accuracy): + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in attitude reference message at t = {} sec \n".format(t) + ) + if not unitTestSupport.isVectorEqual( + omegaDot_RN_N, CAMLog.domega_RN_N[n], accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in attitude reference message at t = {} sec \n".format(t)) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in attitude reference message at t = {} sec \n".format(t) + ) if not unitTestSupport.isVectorEqual(sigma_RN, CAMLogC.sigma_RN[n], accuracy): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in C attitude reference message at t = {} sec \n".format(t)) - if not unitTestSupport.isVectorEqual(omega_RN_N, CAMLogC.omega_RN_N[n], accuracy): + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in C attitude reference message at t = {} sec \n".format(t) + ) + if not unitTestSupport.isVectorEqual( + omega_RN_N, CAMLogC.omega_RN_N[n], accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in C attitude reference message at t = {} sec \n".format(t)) - if not unitTestSupport.isVectorEqual(omegaDot_RN_N, CAMLogC.domega_RN_N[n], accuracy): + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in C attitude reference message at t = {} sec \n".format(t) + ) + if not unitTestSupport.isVectorEqual( + omegaDot_RN_N, CAMLogC.domega_RN_N[n], accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Error in C attitude reference message at t = {} sec \n".format(t)) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Error in C attitude reference message at t = {} sec \n".format(t) + ) - - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -688,9 +903,9 @@ def CAMTestFunction(N, keepOutFov, keepInFov, costFcnType, accuracy): # if __name__ == "__main__": CAMTestFunction( - 5, # grid coarsness N - 20, # keepOutFov - 70, # keepInFov - 1, # costFcnType - 1e-12 # accuracy - ) + 5, # grid coarsness N + 20, # keepOutFov + 70, # keepInFov + 1, # costFcnType + 1e-12, # accuracy + ) diff --git a/src/fswAlgorithms/attGuidance/eulerRotation/_UnitTest/Support/results_eulerRotation.py b/src/fswAlgorithms/attGuidance/eulerRotation/_UnitTest/Support/results_eulerRotation.py index d3d85852f1..536a58473f 100644 --- a/src/fswAlgorithms/attGuidance/eulerRotation/_UnitTest/Support/results_eulerRotation.py +++ b/src/fswAlgorithms/attGuidance/eulerRotation/_UnitTest/Support/results_eulerRotation.py @@ -24,18 +24,26 @@ from Basilisk.utilities import macros as mc - - -def printResults_eulerRotation(configData, sigma_R0N, omega_R0N_N, domega_R0N_N, callTime): +def printResults_eulerRotation( + configData, sigma_R0N, omega_R0N_N, domega_R0N_N, callTime +): def computeEuler321_Binv_derivative(angleSet, angleRates): theta = angleSet[1] phi = angleSet[2] thetaDot = angleRates[1] phiDot = angleRates[2] B_inv_deriv = [ - [-thetaDot*cos(theta), 0, 0], - [phiDot*cos(phi)*cos(theta)-thetaDot*sin(phi)*sin(theta), -phiDot*sin(phi), 0], - [-phiDot*sin(phi)*cos(theta)-thetaDot*cos(phi)*cos(theta), -phiDot*cos(phi), 0] + [-thetaDot * cos(theta), 0, 0], + [ + phiDot * cos(phi) * cos(theta) - thetaDot * sin(phi) * sin(theta), + -phiDot * sin(phi), + 0, + ], + [ + -phiDot * sin(phi) * cos(theta) - thetaDot * cos(phi) * cos(theta), + -phiDot * cos(phi), + 0, + ], ] return B_inv_deriv @@ -64,32 +72,39 @@ def computeEuler321_Binv_derivative(angleSet, angleRates): # Print results def printData(): - print('callTime = ', callTime) - print('eulerAngleSet = ', angleSet) - print('B_inv_deriv = ', B_inv_deriv) - print('sigma_RN = ', sigma_RN) - print('omega_RN_N = ', omega_RN_N) - print('domega_RN_N = ', domega_RN_N) - print('\n') + print("callTime = ", callTime) + print("eulerAngleSet = ", angleSet) + print("B_inv_deriv = ", B_inv_deriv) + print("sigma_RN = ", sigma_RN) + print("omega_RN_N = ", omega_RN_N) + print("domega_RN_N = ", domega_RN_N) + print("\n") + printData() return angleSet # Initial Conditions -sigma_R0N = np.array([ 0.1, 0.2, 0.3 ]) +sigma_R0N = np.array([0.1, 0.2, 0.3]) omega_R0N_N = np.array([0.1, 0.0, 0.0]) domega_R0N_N = np.array([0.0, 0.0, 0.0]) dt = 0.5 -angleRates = np.array([0.1, 0., 0.]) * mc.D2R +angleRates = np.array([0.1, 0.0, 0.0]) * mc.D2R angleSet = np.array([0.0, 0.0, 0.0]) * mc.D2R configData = (angleSet, angleRates, 0.0) -angleSet = printResults_eulerRotation(configData, sigma_R0N, omega_R0N_N, domega_R0N_N, dt*0.0) +angleSet = printResults_eulerRotation( + configData, sigma_R0N, omega_R0N_N, domega_R0N_N, dt * 0.0 +) configData = (angleSet, angleRates, dt) -angleSet = printResults_eulerRotation(configData, sigma_R0N, omega_R0N_N, domega_R0N_N, dt*1.0) +angleSet = printResults_eulerRotation( + configData, sigma_R0N, omega_R0N_N, domega_R0N_N, dt * 1.0 +) configData = (angleSet, angleRates, dt) -angleSet = printResults_eulerRotation(configData, sigma_R0N, omega_R0N_N, domega_R0N_N, dt*2.0) +angleSet = printResults_eulerRotation( + configData, sigma_R0N, omega_R0N_N, domega_R0N_N, dt * 2.0 +) # t0 = 0.0 diff --git a/src/fswAlgorithms/attGuidance/eulerRotation/_UnitTest/test_eulerRotation.py b/src/fswAlgorithms/attGuidance/eulerRotation/_UnitTest/test_eulerRotation.py index c122135496..4b1b2c332c 100644 --- a/src/fswAlgorithms/attGuidance/eulerRotation/_UnitTest/test_eulerRotation.py +++ b/src/fswAlgorithms/attGuidance/eulerRotation/_UnitTest/test_eulerRotation.py @@ -25,11 +25,16 @@ import numpy as np import pytest from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import eulerRotation # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + eulerRotation, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros as mc -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -38,9 +43,8 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("function", ["run" - , "run2" - ]) + +@pytest.mark.parametrize("function", ["run", "run2"]) def test_all_test_eulerRotation(show_plots, function): """Module Unit Test""" testFunction = globals().get(function) @@ -53,16 +57,16 @@ def test_all_test_eulerRotation(show_plots, function): def run(show_plots): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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() # Test times - updateTime = 0.5 # update process rate update time + updateTime = 0.5 # update process rate update time totalTestSimTime = 1.5 # Create test thread @@ -70,7 +74,6 @@ def run(show_plots): testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = eulerRotation.eulerRotation() module.ModelTag = "eulerRotation" @@ -90,7 +93,9 @@ def run(show_plots): # # Reference Frame Message # - RefStateOutData = messaging.AttRefMsgPayload() # Create a structure for the input message + RefStateOutData = ( + messaging.AttRefMsgPayload() + ) # Create a structure for the input message sigma_R0N = np.array([0.1, 0.2, 0.3]) RefStateOutData.sigma_RN = sigma_R0N omega_R0N_N = np.array([0.1, 0.0, 0.0]) @@ -113,7 +118,9 @@ def run(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(mc.sec2nano(totalTestSimTime)) # seconds to stop simulation + unitTestSim.ConfigureStopTime( + mc.sec2nano(totalTestSimTime) + ) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -130,12 +137,12 @@ def run(show_plots): trueVector = [ [-0.193031238249, 0.608048400483, 0.386062476497], [-0.193031238249, 0.608048400483, 0.386062476497], - [-0.193144351314, 0.607931107381, 0.386360300559], - [-0.193257454832, 0.607813704445, 0.386658117585] + [-0.193144351314, 0.607931107381, 0.386360300559], + [-0.193257454832, 0.607813704445, 0.386658117585], ] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "sigma_RN Set", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, moduleOutput, accuracy, "sigma_RN Set", testFailCount, testMessages + ) # print '\n sigma_RN = ', moduleOutput[:, 1:], '\n' # # check omega_RN_N @@ -143,14 +150,19 @@ def run(show_plots): moduleOutput = dataLog.omega_RN_N # set the filtered output truth states trueVector = [ - [0.101246280045, 0.000182644489, 0.001208139578], - [0.101246280045, 0.000182644489, 0.001208139578], - [0.101246280045, 0.000182644489, 0.001208139578], - [0.101246280045, 0.000182644489, 0.001208139578] + [0.101246280045, 0.000182644489, 0.001208139578], + [0.101246280045, 0.000182644489, 0.001208139578], + [0.101246280045, 0.000182644489, 0.001208139578], + [0.101246280045, 0.000182644489, 0.001208139578], ] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "omega_RN_N Vector", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + moduleOutput, + accuracy, + "omega_RN_N Vector", + testFailCount, + testMessages, + ) # # check domega_RN_N @@ -158,30 +170,35 @@ def run(show_plots): moduleOutput = dataLog.domega_RN_N # set the filtered output truth states trueVector = [ - [0.000000000000e+00, -1.208139577635e-04, 1.826444892823e-05], - [0.000000000000e+00, -1.208139577635e-04, 1.826444892823e-05], - [0.000000000000e+00, -1.208139577635e-04, 1.826444892823e-05], - [0.000000000000e+00, -1.208139577635e-04, 1.826444892823e-05] + [0.000000000000e00, -1.208139577635e-04, 1.826444892823e-05], + [0.000000000000e00, -1.208139577635e-04, 1.826444892823e-05], + [0.000000000000e00, -1.208139577635e-04, 1.826444892823e-05], + [0.000000000000e00, -1.208139577635e-04, 1.826444892823e-05], ] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "domega_RN_N Vector", - testFailCount, testMessages) - + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + moduleOutput, + accuracy, + "domega_RN_N Vector", + testFailCount, + testMessages, + ) # If the argument provided at commandline "--show_plots" evaluates as true, # plot all figures -# if show_plots: -# # plot a sample variable. -# plt.figure(1) -# plt.plot(variableState[:,0]*macros.NANO2SEC, variableState[:,1], label='Sample Variable') -# plt.legend(loc='upper left') -# plt.xlabel('Time [s]') -# plt.ylabel('Variable Description [unit]') -# plt.show() + # if show_plots: + # # plot a sample variable. + # plt.figure(1) + # plt.plot(variableState[:,0]*macros.NANO2SEC, variableState[:,1], label='Sample Variable') + # plt.legend(loc='upper left') + # plt.xlabel('Time [s]') + # plt.ylabel('Variable Description [unit]') + # plt.show() # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def run2(show_plots): testFailCount = 0 # zero unit test result counter @@ -220,7 +237,9 @@ def run2(show_plots): # # Reference Frame Message # - RefStateOutData = messaging.AttRefMsgPayload() # Create a structure for the input message + RefStateOutData = ( + messaging.AttRefMsgPayload() + ) # Create a structure for the input message sigma_R0N = np.array([0.1, 0.2, 0.3]) RefStateOutData.sigma_RN = sigma_R0N omega_R0N_N = np.array([0.1, 0.0, 0.0]) @@ -252,7 +271,9 @@ def run2(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(mc.sec2nano(totalTestSimTime)) # seconds to stop simulation + unitTestSim.ConfigureStopTime( + mc.sec2nano(totalTestSimTime) + ) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -269,11 +290,11 @@ def run2(show_plots): [-0.193031238249, 0.608048400483, 0.386062476497], [-0.193031238249, 0.608048400483, 0.386062476497], [-0.193144351314, 0.607931107381, 0.386360300559], - [-0.193257454832, 0.607813704445, 0.386658117585] + [-0.193257454832, 0.607813704445, 0.386658117585], ] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "sigma_RN Set", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, moduleOutput, accuracy, "sigma_RN Set", testFailCount, testMessages + ) # print '\n sigma_RN = ', moduleOutput[:, 1:], '\n' # # check omega_RN_N @@ -284,11 +305,16 @@ def run2(show_plots): [0.101246280045, 0.000182644489, 0.001208139578], [0.101246280045, 0.000182644489, 0.001208139578], [0.101246280045, 0.000182644489, 0.001208139578], - [0.101246280045, 0.000182644489, 0.001208139578] + [0.101246280045, 0.000182644489, 0.001208139578], ] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "omega_RN_N Vector", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + moduleOutput, + accuracy, + "omega_RN_N Vector", + testFailCount, + testMessages, + ) # # check domega_RN_N @@ -296,19 +322,23 @@ def run2(show_plots): moduleOutput = dataLog.domega_RN_N # set the filtered output truth states trueVector = [ - [0.000000000000e+00, -1.208139577635e-04, 1.826444892823e-05], - [0.000000000000e+00, -1.208139577635e-04, 1.826444892823e-05], - [0.000000000000e+00, -1.208139577635e-04, 1.826444892823e-05], - [0.000000000000e+00, -1.208139577635e-04, 1.826444892823e-05] + [0.000000000000e00, -1.208139577635e-04, 1.826444892823e-05], + [0.000000000000e00, -1.208139577635e-04, 1.826444892823e-05], + [0.000000000000e00, -1.208139577635e-04, 1.826444892823e-05], + [0.000000000000e00, -1.208139577635e-04, 1.826444892823e-05], ] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "domega_RN_N Vector", - testFailCount, testMessages) - + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + moduleOutput, + accuracy, + "domega_RN_N Vector", + testFailCount, + testMessages, + ) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attGuidance/hillPoint/_UnitTest/Support/results_hillPoint.py b/src/fswAlgorithms/attGuidance/hillPoint/_UnitTest/Support/results_hillPoint.py index 89206ce293..184852ca25 100755 --- a/src/fswAlgorithms/attGuidance/hillPoint/_UnitTest/Support/results_hillPoint.py +++ b/src/fswAlgorithms/attGuidance/hillPoint/_UnitTest/Support/results_hillPoint.py @@ -17,20 +17,20 @@ # import numpy as np from numpy import linalg as la -np.set_printoptions(precision=12) - - +np.set_printoptions(precision=12) from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.utilities import astroFunctions as af + def normalize(v): - norm=np.linalg.norm(v) - if norm==0: - return v - return v/norm + norm = np.linalg.norm(v) + if norm == 0: + return v + return v / norm + def printResults_HillPoint(r_BN_N, v_BN_N, celBodyPosVec, celBodyVelVec): r = r_BN_N - celBodyPosVec @@ -39,7 +39,7 @@ def printResults_HillPoint(r_BN_N, v_BN_N, celBodyPosVec, celBodyVelVec): i_r = normalize(r) i_h = normalize(h) i_theta = np.cross(i_h, i_r) - HN = np.array([ i_r, i_theta, i_h ]) + HN = np.array([i_r, i_theta, i_h]) sigma_HN = rbk.C2MRP(HN) hm = la.norm(h) @@ -51,16 +51,17 @@ def printResults_HillPoint(r_BN_N, v_BN_N, celBodyPosVec, celBodyVelVec): omega_HN_N = dfdt * i_h domega_HN_N = ddfdt2 * i_h - print('sigma_HN = ', sigma_HN) - print('omega_HN_N = ', omega_HN_N) - print('domega_HN_N = ', domega_HN_N) + print("sigma_HN = ", sigma_HN) + print("omega_HN_N = ", omega_HN_N) + print("domega_HN_N = ", domega_HN_N) HN = rbk.MRP2C(sigma_HN) - M = rbk.Mi(0.5*np.pi, 1) + M = rbk.Mi(0.5 * np.pi, 1) sigma = rbk.C2MRP(np.dot(M, HN)) print(sigma) return (sigma_HN, omega_HN_N, domega_HN_N) + # MAIN # Initial Conditions (IC) a = af.E_radius * 2.8 diff --git a/src/fswAlgorithms/attGuidance/hillPoint/_UnitTest/test_hillPoint.py b/src/fswAlgorithms/attGuidance/hillPoint/_UnitTest/test_hillPoint.py index 5864b9d4b3..91ac39d4f3 100755 --- a/src/fswAlgorithms/attGuidance/hillPoint/_UnitTest/test_hillPoint.py +++ b/src/fswAlgorithms/attGuidance/hillPoint/_UnitTest/test_hillPoint.py @@ -26,12 +26,15 @@ import pytest from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import hillPoint # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import astroFunctions as af from Basilisk.architecture import astroConstants from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -40,7 +43,6 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ @pytest.mark.parametrize("celMsgSet", [True, False]) - def test_hillPoint(show_plots, celMsgSet): """Module Unit Test""" # each test method requires a single assert method to be called @@ -49,20 +51,19 @@ def test_hillPoint(show_plots, celMsgSet): def hillPointTestFunction(show_plots, celMsgSet): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = hillPoint.hillPoint() module.ModelTag = "hillPoint" @@ -72,13 +73,13 @@ def hillPointTestFunction(show_plots, celMsgSet): # Initialize the test module configuration data - a = astroConstants.REQ_EARTH * 2.8 * 1000 # m + a = astroConstants.REQ_EARTH * 2.8 * 1000 # m e = 0.0 i = 0.0 Omega = 0.0 omega = 0.0 f = 60 * astroConstants.D2R - mu = astroConstants.MU_EARTH*1e9 + mu = astroConstants.MU_EARTH * 1e9 (r, v) = af.OE2RV(mu, a, e, i, Omega, omega, f) r_BN_N = r v_BN_N = v @@ -90,7 +91,9 @@ def hillPointTestFunction(show_plots, celMsgSet): # # Navigation Input Message # - NavStateOutData = messaging.NavTransMsgPayload() # Create a structure for the input message + NavStateOutData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message NavStateOutData.r_BN_N = r_BN_N NavStateOutData.v_BN_N = v_BN_N navMsg = messaging.NavTransMsg().write(NavStateOutData) @@ -99,7 +102,7 @@ def hillPointTestFunction(show_plots, celMsgSet): # # Spice Input Message # - if (celMsgSet): + if celMsgSet: CelBodyData = messaging.EphemerisMsgPayload() CelBodyData.r_BdyZero_N = planetPos CelBodyData.v_BdyZero_N = planetVel @@ -119,7 +122,7 @@ def hillPointTestFunction(show_plots, celMsgSet): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -133,68 +136,82 @@ def hillPointTestFunction(show_plots, celMsgSet): # set the filtered output truth states trueVector = [ - [0., 0., 0.267949192431], - [0., 0., 0.267949192431], - [0., 0., 0.267949192431] - ] + [0.0, 0.0, 0.267949192431], + [0.0, 0.0, 0.267949192431], + [0.0, 0.0, 0.267949192431], + ] # compare the module results to the truth values accuracy = 1e-12 - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleOutput[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + moduleOutput[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_RN unit test at t=" + - str(moduleOutput[i,0]*macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_RN unit test at t=" + + str(moduleOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) # # check omega_RN_N # moduleOutput = dataLog.omega_RN_N # set the filtered output truth states - fDot = np.sqrt(mu/(a*a*a)) - trueVector = [ - [0., 0., fDot], - [0., 0., fDot], - [0., 0., fDot] - ] + fDot = np.sqrt(mu / (a * a * a)) + trueVector = [[0.0, 0.0, fDot], [0.0, 0.0, fDot], [0.0, 0.0, fDot]] # compare the module results to the truth values accuracy = 1e-12 - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleOutput[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + moduleOutput[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_N unit test at t=" + - str(moduleOutput[i,0]*macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_RN_N unit test at t=" + + str(moduleOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) # # check domega_RN_N # moduleOutput = dataLog.domega_RN_N # set the filtered output truth states trueVector = [ - [0.0, 0.0, 1.315647475046e-23], - [0.0, 0.0, 1.315647475046e-23], - [0.0, 0.0, 1.315647475046e-23] - ] + [0.0, 0.0, 1.315647475046e-23], + [0.0, 0.0, 1.315647475046e-23], + [0.0, 0.0, 1.315647475046e-23], + ] # compare the module results to the truth values accuracy = 1e-12 - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleOutput[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + moduleOutput[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed domega_RN_N unit test at t=" + - str(moduleOutput[i,0]*macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed domega_RN_N unit test at t=" + + str(moduleOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) # Note that we can continue to step the simulation however we feel like. # Just because we stop and query data does not mean everything has to stop for good - unitTestSim.ConfigureStopTime(macros.sec2nano(0.6)) # run an additional 0.6 seconds + unitTestSim.ConfigureStopTime(macros.sec2nano(0.6)) # run an additional 0.6 seconds unitTestSim.ExecuteSimulation() # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attGuidance/inertial3D/_UnitTest/test_inertial3D.py b/src/fswAlgorithms/attGuidance/inertial3D/_UnitTest/test_inertial3D.py index 9a052d4260..02cd41f5ee 100755 --- a/src/fswAlgorithms/attGuidance/inertial3D/_UnitTest/test_inertial3D.py +++ b/src/fswAlgorithms/attGuidance/inertial3D/_UnitTest/test_inertial3D.py @@ -29,14 +29,12 @@ path = os.path.dirname(os.path.abspath(filename)) - - - - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import inertial3D # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import inertial3D # import the module that is to be tested from Basilisk.utilities import macros @@ -53,20 +51,19 @@ def test_inertial3D(show_plots): def subModuleTestFunction(show_plots): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = inertial3D.inertial3D() module.ModelTag = "inertial3D" @@ -81,7 +78,6 @@ def subModuleTestFunction(show_plots): dataLog = module.attRefOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) - # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() @@ -89,7 +85,7 @@ def subModuleTestFunction(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -101,80 +97,85 @@ def subModuleTestFunction(show_plots): # moduleOutput = dataLog.sigma_RN # set the filtered output truth states - trueVector = [ - [0.1, 0.2, 0.3], - [0.1, 0.2, 0.3], - [0.1, 0.2, 0.3] - ] + trueVector = [[0.1, 0.2, 0.3], [0.1, 0.2, 0.3], [0.1, 0.2, 0.3]] # compare the module results to the truth values accuracy = 1e-12 - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleOutput[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + moduleOutput[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_RN unit test at t=" + - str(moduleOutput[i,0]*macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_RN unit test at t=" + + str(moduleOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) # # check omega_RN_N # moduleOutput = dataLog.omega_RN_N # set the filtered output truth states - trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] + trueVector = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # compare the module results to the truth values accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleOutput[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + moduleOutput[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_N unit test at t=" + - str(moduleOutput[i,0]*macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_RN_N unit test at t=" + + str(moduleOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) # # check domega_RN_B # moduleOutput = dataLog.domega_RN_N # set the filtered output truth states - trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] + trueVector = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # compare the module results to the truth values accuracy = 1e-12 - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleOutput[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + moduleOutput[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed domega_RN_N unit test at t=" + - str(moduleOutput[i,0]*macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed domega_RN_N unit test at t=" + + str(moduleOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) snippentName = "passFail" if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attGuidance/inertial3DSpin/_UnitTest/Support/results_inertial3DSpin.py b/src/fswAlgorithms/attGuidance/inertial3DSpin/_UnitTest/Support/results_inertial3DSpin.py index 314183ed73..14f5d04ffc 100755 --- a/src/fswAlgorithms/attGuidance/inertial3DSpin/_UnitTest/Support/results_inertial3DSpin.py +++ b/src/fswAlgorithms/attGuidance/inertial3DSpin/_UnitTest/Support/results_inertial3DSpin.py @@ -23,8 +23,8 @@ np.set_printoptions(precision=12) -def printResults3DSpin(sigma_R0N, omega_R0N_N, domega_R0N_N, omega_RR0_R, dt): +def printResults3DSpin(sigma_R0N, omega_R0N_N, domega_R0N_N, omega_RR0_R, dt): # Compute angular Rate RN = rbk.MRP2C(sigma_R0N) omega_RR0_N = np.dot(RN.T, omega_RR0_R) @@ -37,22 +37,22 @@ def printResults3DSpin(sigma_R0N, omega_R0N_N, domega_R0N_N, omega_RR0_R, dt): omega_RN_R = np.dot(RN, omega_RN_N) B = rbk.BmatMRP(sigma_R0N) dsigma_RN = 0.25 * np.dot(B, omega_RN_R) - sigma_RN = sigma_R0N + dsigma_RN * dt + sigma_RN = sigma_R0N + dsigma_RN * dt rbk.MRPswitch(sigma_RN, 1) # Print results - print('sigma_RN = ', sigma_RN) - print('omega_RN_N = ', omega_RN_N) - print('domega_RN_N = ', domega_RN_N) - print('\n') + print("sigma_RN = ", sigma_RN) + print("omega_RN_N = ", omega_RN_N) + print("domega_RN_N = ", domega_RN_N) + print("\n") return sigma_RN sigma_R0N = np.array([0.1, 0.2, 0.3]) -omega_R0N_N = np.array([0., 0., 0.]) -domega_R0N_N = np.array([0., 0., 0.]) -omega_spin = np.array([1., -1., 0.5]) * mc.D2R -print('CallTime = 0.0') +omega_R0N_N = np.array([0.0, 0.0, 0.0]) +domega_R0N_N = np.array([0.0, 0.0, 0.0]) +omega_spin = np.array([1.0, -1.0, 0.5]) * mc.D2R +print("CallTime = 0.0") dt = 0.0 sigma_RN = printResults3DSpin(sigma_R0N, omega_R0N_N, domega_R0N_N, omega_spin, dt) dt = 0.5 diff --git a/src/fswAlgorithms/attGuidance/inertial3DSpin/_UnitTest/test_inertial3DSpin.py b/src/fswAlgorithms/attGuidance/inertial3DSpin/_UnitTest/test_inertial3DSpin.py index ed2a609e22..3dd2f5fc5a 100755 --- a/src/fswAlgorithms/attGuidance/inertial3DSpin/_UnitTest/test_inertial3DSpin.py +++ b/src/fswAlgorithms/attGuidance/inertial3DSpin/_UnitTest/test_inertial3DSpin.py @@ -26,22 +26,28 @@ import numpy as np import pytest from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import inertial3DSpin # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + inertial3DSpin, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros as mc -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is 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(conditionstring) +# @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("function", ["subModuleTestFunction" - , "subModuleTestFunction2" - ]) + +@pytest.mark.parametrize( + "function", ["subModuleTestFunction", "subModuleTestFunction2"] +) def test_stateArchitectureAllTests(show_plots, function): """Module Unit Test""" testFunction = globals().get(function) @@ -54,20 +60,19 @@ def test_stateArchitectureAllTests(show_plots, function): def subModuleTestFunction(show_plots): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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 = mc.sec2nano(0.5) # update process rate update time + testProcessRate = mc.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = inertial3DSpin.inertial3DSpin() module.ModelTag = "inertial3DSpin" @@ -76,13 +81,15 @@ def subModuleTestFunction(show_plots): unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - omega_RR0_R0 = np.array([1., -1., 0.5]) * mc.D2R + omega_RR0_R0 = np.array([1.0, -1.0, 0.5]) * mc.D2R module.omega_RR0_R0 = omega_RR0_R0 # # Reference Frame Message # - RefStateOutData = messaging.AttRefMsgPayload() # Create a structure for the input message + RefStateOutData = ( + messaging.AttRefMsgPayload() + ) # Create a structure for the input message sigma_R0N = np.array([0.1, 0.2, 0.3]) RefStateOutData.sigma_RN = sigma_R0N omega_R0N_N = np.array([0.0, 0.0, 0.0]) @@ -105,7 +112,7 @@ def subModuleTestFunction(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(mc.sec2nano(1.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(mc.sec2nano(1.5)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -114,21 +121,27 @@ def subModuleTestFunction(show_plots): # check sigma_RN # trueVector = [ - [0.1, 0.2, 0.3], - [0.1, 0.2, 0.3], - [0.103643374814, 0.199258235068, 0.299694567381], - [0.10728593457, 0.198511279747, 0.299381655572] - ] + [0.1, 0.2, 0.3], + [0.1, 0.2, 0.3], + [0.103643374814, 0.199258235068, 0.299694567381], + [0.10728593457, 0.198511279747, 0.299381655572], + ] # compare the module results to the truth values accuracy = 1e-12 for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleLog.sigma_RN[i], trueVector[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + moduleLog.sigma_RN[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_RN unit test at t=" + - str(moduleLog.times()[i] * mc.NANO2SEC) + "sec\n") - + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_RN unit test at t=" + + str(moduleLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # # check omega_RN_N @@ -137,39 +150,48 @@ def subModuleTestFunction(show_plots): [0.02142849611, 0.01021197571, -0.011041933756], [0.02142849611, 0.01021197571, -0.011041933756], [0.02142849611, 0.01021197571, -0.011041933756], - [0.021428270863, 0.010212299678, -0.011042071256] + [0.021428270863, 0.010212299678, -0.011042071256], ] # compare the module results to the truth values accuracy = 1e-12 - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleLog.omega_RN_N[i], trueVector[i] , 3, accuracy): + if not unitTestSupport.isArrayEqual( + moduleLog.omega_RN_N[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_N unit test at t=" + - str(moduleLog.times()[i] * mc.NANO2SEC) + "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_RN_N unit test at t=" + + str(moduleLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # # check domega_RN_N # - trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] + trueVector = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # compare the module results to the truth values accuracy = 1e-12 - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleLog.domega_RN_N[i], trueVector[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + moduleLog.domega_RN_N[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed domega_RN_N unit test at t=" + - str(moduleLog.times()[i] * mc.NANO2SEC) +"sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed domega_RN_N unit test at t=" + + str(moduleLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # Note that we can continue to step the simulation however we feel like. # Just because we stop and query data does not mean everything has to stop for good - unitTestSim.ConfigureStopTime(mc.sec2nano(0.6)) # run an additional 0.6 seconds + unitTestSim.ConfigureStopTime(mc.sec2nano(0.6)) # run an additional 0.6 seconds unitTestSim.ExecuteSimulation() if testFailCount: @@ -179,7 +201,7 @@ def subModuleTestFunction(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def subModuleTestFunction2(show_plots): @@ -204,14 +226,16 @@ def subModuleTestFunction2(show_plots): unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - omega_RR0_R0 = np.array([1., -1., 0.5]) * mc.D2R + omega_RR0_R0 = np.array([1.0, -1.0, 0.5]) * mc.D2R module.omega_RR0_R0 = omega_RR0_R0 # Create input message and size it because the regular creator of that message # is not part of the test. # # Reference Frame Message # - RefStateOutData = messaging.AttRefMsgPayload() # Create a structure for the input message + RefStateOutData = ( + messaging.AttRefMsgPayload() + ) # Create a structure for the input message sigma_R0N = np.array([0.1, 0.2, 0.3]) RefStateOutData.sigma_RN = sigma_R0N @@ -250,18 +274,24 @@ def subModuleTestFunction2(show_plots): [0.1, 0.2, 0.3], [0.1, 0.2, 0.3], [0.103643374814, 0.199258235068, 0.299694567381], - [0.10728593457, 0.198511279747, 0.299381655572] + [0.10728593457, 0.198511279747, 0.299381655572], ] # compare the module results to the truth values accuracy = 1e-12 for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleLog.sigma_RN[i], trueVector[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + moduleLog.sigma_RN[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_RN unit test at t=" + - str(moduleLog.times()[i] * mc.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_RN unit test at t=" + + str(moduleLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # # check omega_RN_N @@ -271,39 +301,46 @@ def subModuleTestFunction2(show_plots): [0.02142849611, 0.01021197571, -0.011041933756], [0.02142849611, 0.01021197571, -0.011041933756], [0.02142849611, 0.01021197571, -0.011041933756], - [0.021428270863, 0.010212299678, -0.011042071256] + [0.021428270863, 0.010212299678, -0.011042071256], ] # compare the module results to the truth values accuracy = 1e-12 for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleLog.omega_RN_N[i], trueVector[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + moduleLog.omega_RN_N[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_N unit test at t=" + - str(moduleLog.times()[i] * mc.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_RN_N unit test at t=" + + str(moduleLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # # check domega_RN_N # # set the filtered output truth states - trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] + trueVector = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # compare the module results to the truth values accuracy = 1e-12 for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleLog.domega_RN_N[i], trueVector[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + moduleLog.domega_RN_N[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed domega_RN_N unit test at t=" + - str(moduleLog.times()[i] * mc.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed domega_RN_N unit test at t=" + + str(moduleLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # Note that we can continue to step the simulation however we feel like. # Just because we stop and query data does not mean everything has to stop for good @@ -317,7 +354,7 @@ def subModuleTestFunction2(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attGuidance/locationPointing/_UnitTest/test_locationPointing.py b/src/fswAlgorithms/attGuidance/locationPointing/_UnitTest/test_locationPointing.py index d8ab1c2e87..905f3f1b0a 100644 --- a/src/fswAlgorithms/attGuidance/locationPointing/_UnitTest/test_locationPointing.py +++ b/src/fswAlgorithms/attGuidance/locationPointing/_UnitTest/test_locationPointing.py @@ -35,7 +35,10 @@ @pytest.mark.parametrize("accuracy", [1e-12]) -@pytest.mark.parametrize("r_LS_N", [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.], [0, -1, 0.], [1, 1, 1]]) +@pytest.mark.parametrize( + "r_LS_N", + [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [0, -1, 0.0], [1, 1, 1]], +) @pytest.mark.parametrize("locationType", [0, 1, 2]) @pytest.mark.parametrize("use3DRate", [True, False]) def test_locationPointing(show_plots, r_LS_N, locationType, use3DRate, accuracy): @@ -61,12 +64,15 @@ def test_locationPointing(show_plots, r_LS_N, locationType, use3DRate, accuracy) The script checks the attitude and rate outputs. """ - [testResults, testMessage] = locationPointingTestFunction(show_plots, r_LS_N, locationType, - use3DRate, accuracy) + [testResults, testMessage] = locationPointingTestFunction( + show_plots, r_LS_N, locationType, use3DRate, accuracy + ) assert testResults < 1, testMessage -def locationPointingTestFunction(show_plots, r_LS_NIn, locationType, use3DRate, accuracy): +def locationPointingTestFunction( + show_plots, r_LS_NIn, locationType, use3DRate, accuracy +): """Test method""" testFailCount = 0 testMessages = [] @@ -84,7 +90,7 @@ def locationPointingTestFunction(show_plots, r_LS_NIn, locationType, use3DRate, r_SN_N = np.array([10, 11, 12]) r_LS_N = np.array(r_LS_NIn) omega_BN_B = np.array([0.001, 0.002, 0.003]) - sigma_BN = np.array([0., 0., 0.]) + sigma_BN = np.array([0.0, 0.0, 0.0]) r_LN_N = r_LS_N + r_SN_N # setup module to be tested @@ -146,42 +152,73 @@ def locationPointingTestFunction(show_plots, r_LS_NIn, locationType, use3DRate, unitTestSim.ExecuteSimulation() counter += 1 - truthSigmaBR, truthOmegaBR, truthSigmaRN, truthOmegaRN = \ - truthValues(pHat_B, r_LN_N, r_SN_N, scAttRec.sigma_BN, scAttRec.omega_BN_B, eps, timeStep, - use3DRate) + truthSigmaBR, truthOmegaBR, truthSigmaRN, truthOmegaRN = truthValues( + pHat_B, + r_LN_N, + r_SN_N, + scAttRec.sigma_BN, + scAttRec.omega_BN_B, + eps, + timeStep, + use3DRate, + ) # compare the module results to the truth values for i in range(0, len(truthSigmaBR)): # check a vector values - if not unitTestSupport.isArrayEqual(attGuidOutMsgRec.sigma_BR[i], truthSigmaBR[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + attGuidOutMsgRec.sigma_BR[i], truthSigmaBR[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_BR unit test at t=" + - str(attGuidOutMsgRec.times()[i] * macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_BR unit test at t=" + + str(attGuidOutMsgRec.times()[i] * macros.NANO2SEC) + + "sec\n" + ) for i in range(0, len(truthOmegaBR)): # check a vector values - if not unitTestSupport.isArrayEqual(attGuidOutMsgRec.omega_BR_B[i], truthOmegaBR[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + attGuidOutMsgRec.omega_BR_B[i], truthOmegaBR[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_BR_B unit test at t=" + - str(attGuidOutMsgRec.times()[i] * macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_BR_B unit test at t=" + + str(attGuidOutMsgRec.times()[i] * macros.NANO2SEC) + + "sec\n" + ) for i in range(0, len(truthSigmaRN)): # check a vector values - if not unitTestSupport.isArrayEqual(attRefOutMsgRec.sigma_RN[i], truthSigmaRN[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + attRefOutMsgRec.sigma_RN[i], truthSigmaRN[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_RN unit test at t=" + - str(attRefOutMsgRec.times()[i] * macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_RN unit test at t=" + + str(attRefOutMsgRec.times()[i] * macros.NANO2SEC) + + "sec\n" + ) for i in range(0, len(truthOmegaRN)): # check a vector values - if not unitTestSupport.isArrayEqual(attRefOutMsgRec.omega_RN_N[i], truthOmegaRN[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + attRefOutMsgRec.omega_RN_N[i], truthOmegaRN[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_N unit test at t=" + - str(attRefOutMsgRec.times()[i] * macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_RN_N unit test at t=" + + str(attRefOutMsgRec.times()[i] * macros.NANO2SEC) + + "sec\n" + ) if testFailCount == 0: print("PASSED: " + module.ModelTag) @@ -191,17 +228,19 @@ def locationPointingTestFunction(show_plots, r_LS_NIn, locationType, use3DRate, return [testFailCount, "".join(testMessages)] -def truthValues(pHat_B, r_LN_N, r_SN_N, sigma_BNList, omega_BNList, smallAngle, dt, use3DRate): +def truthValues( + pHat_B, r_LN_N, r_SN_N, sigma_BNList, omega_BNList, smallAngle, dt, use3DRate +): # setup eHat180_B - eHat180_B = np.cross(pHat_B, np.array([1., 0., 0.])) + eHat180_B = np.cross(pHat_B, np.array([1.0, 0.0, 0.0])) if np.linalg.norm(eHat180_B) < 0.1: - eHat180_B = np.cross(pHat_B, np.array([0., 1., 0.])) + eHat180_B = np.cross(pHat_B, np.array([0.0, 1.0, 0.0])) eHat180_B = eHat180_B / np.linalg.norm(eHat180_B) r_LS_N = r_LN_N - r_SN_N counter = 0 - omega_BR_B = np.array([0., 0., 0.]) + omega_BR_B = np.array([0.0, 0.0, 0.0]) sigma_BR_Out = [] omega_BR_B_Out = [] sigma_RN_Out = [] @@ -212,14 +251,14 @@ def truthValues(pHat_B, r_LN_N, r_SN_N, sigma_BNList, omega_BNList, smallAngle, r_LS_B = dcmBN.dot(r_LS_N) phi = math.acos(pHat_B.dot(r_LS_B) / np.linalg.norm(r_LS_B)) if phi < smallAngle: - sigma_BR = np.array([0., 0., 0.]) + sigma_BR = np.array([0.0, 0.0, 0.0]) else: if math.pi - phi < smallAngle: eHat_B = eHat180_B else: eHat_B = np.cross(pHat_B, r_LS_B) eHat_B = eHat_B / np.linalg.norm(eHat_B) - sigma_BR = - math.tan(phi / 4.) * eHat_B + sigma_BR = -math.tan(phi / 4.0) * eHat_B if counter >= 1: dsigma = (sigma_BR - sigma_BR_Out[counter - 1]) / dt @@ -228,13 +267,17 @@ def truthValues(pHat_B, r_LN_N, r_SN_N, sigma_BNList, omega_BNList, smallAngle, if use3DRate: rHat_LS_B = r_LS_B / np.linalg.norm(r_LS_B) - omega_BR_B = omega_BR_B + (omega_BNList[counter].dot(rHat_LS_B))*rHat_LS_B + omega_BR_B = omega_BR_B + (omega_BNList[counter].dot(rHat_LS_B)) * rHat_LS_B # store truth results sigma_BR_Out.append(sigma_BR) omega_BR_B_Out.append(omega_BR_B) - sigma_RN_Out.append(RigidBodyKinematics.addMRP(sigma_BNList[counter], -sigma_BR)) - omega_RN_N_Out.append(np.transpose(dcmBN).dot(omega_BNList[counter] - omega_BR_B_Out[counter])) + sigma_RN_Out.append( + RigidBodyKinematics.addMRP(sigma_BNList[counter], -sigma_BR) + ) + omega_RN_N_Out.append( + np.transpose(dcmBN).dot(omega_BNList[counter] - omega_BR_B_Out[counter]) + ) counter += 1 diff --git a/src/fswAlgorithms/attGuidance/mrpRotation/_UnitTest/Support/truth_mrpRotation.py b/src/fswAlgorithms/attGuidance/mrpRotation/_UnitTest/Support/truth_mrpRotation.py index 6a5353c96c..0b65bff3a1 100644 --- a/src/fswAlgorithms/attGuidance/mrpRotation/_UnitTest/Support/truth_mrpRotation.py +++ b/src/fswAlgorithms/attGuidance/mrpRotation/_UnitTest/Support/truth_mrpRotation.py @@ -16,16 +16,14 @@ # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. # import numpy as np + np.set_printoptions(precision=12) from Basilisk.utilities import RigidBodyKinematics as rbk - - def results(sigma_RR0, omega_RR0_R, RefStateInData, dt, cmdStateFlag, testReset): - ansSigma = [] ansOmega_RN_N = [] ansdOmega_RN_N = [] @@ -37,7 +35,7 @@ def results(sigma_RR0, omega_RR0_R, RefStateInData, dt, cmdStateFlag, testReset) # compute 0th time step s0 = np.array(sigma_RR0) - s1=rbk.addMRP(np.array(sigma_R0N), np.array(sigma_RR0)) + s1 = rbk.addMRP(np.array(sigma_R0N), np.array(sigma_RR0)) RR0 = rbk.MRP2C(sigma_RR0) RN = np.dot(RR0, R0N) @@ -55,7 +53,7 @@ def results(sigma_RR0, omega_RR0_R, RefStateInData, dt, cmdStateFlag, testReset) ansdOmega_RN_N.append(domega_RN_N.tolist()) # compute 1st time step - B = rbk.BmatMRP(sigma_RR0) + B = rbk.BmatMRP(sigma_RR0) sigma_RR0 += dt * 0.25 * np.dot(B, omega_RR0_R) RR0 = rbk.MRP2C(sigma_RR0) RN = np.dot(RR0, R0N) @@ -70,9 +68,8 @@ def results(sigma_RR0, omega_RR0_R, RefStateInData, dt, cmdStateFlag, testReset) domega_RN_N = domega_RR0_N + domega_R0N_N ansdOmega_RN_N.append(domega_RN_N.tolist()) - # compute 2nd time step - B = rbk.BmatMRP(sigma_RR0) + B = rbk.BmatMRP(sigma_RR0) sigma_RR0 += dt * 0.25 * np.dot(B, omega_RR0_R) RR0 = rbk.MRP2C(sigma_RR0) RN = np.dot(RR0, R0N) @@ -87,7 +84,6 @@ def results(sigma_RR0, omega_RR0_R, RefStateInData, dt, cmdStateFlag, testReset) domega_RN_N = domega_RR0_N + domega_R0N_N ansdOmega_RN_N.append(domega_RN_N.tolist()) - # Testing Reset function if testReset: if cmdStateFlag: @@ -123,5 +119,4 @@ def results(sigma_RR0, omega_RR0_R, RefStateInData, dt, cmdStateFlag, testReset) domega_RN_N = domega_RR0_N + domega_R0N_N ansdOmega_RN_N.append(domega_RN_N.tolist()) - return ansSigma, ansOmega_RN_N, ansdOmega_RN_N diff --git a/src/fswAlgorithms/attGuidance/mrpRotation/_UnitTest/test_mrpRotation.py b/src/fswAlgorithms/attGuidance/mrpRotation/_UnitTest/test_mrpRotation.py index c4162d3911..8db10f8d33 100644 --- a/src/fswAlgorithms/attGuidance/mrpRotation/_UnitTest/test_mrpRotation.py +++ b/src/fswAlgorithms/attGuidance/mrpRotation/_UnitTest/test_mrpRotation.py @@ -36,13 +36,15 @@ # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import mrpRotation # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import mrpRotation # import the module that is to be tested from Basilisk.utilities import macros as mc from Basilisk.architecture import messaging -sys.path.append(path + '/Support') +sys.path.append(path + "/Support") import truth_mrpRotation as truth @@ -51,11 +53,10 @@ # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(conditionstring) + @pytest.mark.parametrize("cmdStateFlag", [False, True]) @pytest.mark.parametrize("testReset", [False, True]) - - # provide a unique test method name, starting with test_ def test_mrpRotation(show_plots, cmdStateFlag, testReset): """Module Unit Test""" @@ -65,17 +66,16 @@ def test_mrpRotation(show_plots, cmdStateFlag, testReset): def run(show_plots, cmdStateFlag, testReset): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) - + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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() # Test times - updateTime = 0.5 # update process rate update time + updateTime = 0.5 # update process rate update time totalTestSimTime = 1.5 # Create test thread @@ -83,7 +83,6 @@ def run(show_plots, cmdStateFlag, testReset): testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = mrpRotation.mrpRotation() module.ModelTag = "mrpRotation" @@ -92,13 +91,14 @@ def run(show_plots, cmdStateFlag, testReset): unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - sigma_RR0 = np.array([0.3, .5, 0.0]) + sigma_RR0 = np.array([0.3, 0.5, 0.0]) module.mrpSet = sigma_RR0 omega_RR0_R = np.array([0.1, 0.0, 0.0]) * mc.D2R module.omega_RR0_R = omega_RR0_R unitTestSupport.writeTeXSnippet("sigma_RR0", str(sigma_RR0), path) - unitTestSupport.writeTeXSnippet("omega_RR0_R", str(omega_RR0_R*mc.R2D) + "deg/sec", path) - + unitTestSupport.writeTeXSnippet( + "omega_RR0_R", str(omega_RR0_R * mc.R2D) + "deg/sec", path + ) if cmdStateFlag: desiredAtt = messaging.AttStateMsgPayload() @@ -110,13 +110,16 @@ def run(show_plots, cmdStateFlag, testReset): module.desiredAttInMsg.subscribeTo(desInMsg) unitTestSupport.writeTeXSnippet("sigma_RR0Cmd", str(sigma_RR0), path) - unitTestSupport.writeTeXSnippet("omega_RR0_RCmd", str(omega_RR0_R * mc.R2D) + "deg/sec", path) - + unitTestSupport.writeTeXSnippet( + "omega_RR0_RCmd", str(omega_RR0_R * mc.R2D) + "deg/sec", path + ) # # Reference Frame Message # - RefStateInData = messaging.AttRefMsgPayload() # Create a structure for the input message + RefStateInData = ( + messaging.AttRefMsgPayload() + ) # Create a structure for the input message sigma_R0N = np.array([0.1, 0.2, 0.3]) RefStateInData.sigma_RN = sigma_R0N omega_R0N_N = np.array([0.1, 0.0, 0.0]) @@ -137,60 +140,81 @@ def run(show_plots, cmdStateFlag, testReset): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(mc.sec2nano(totalTestSimTime)) # seconds to stop simulation + unitTestSim.ConfigureStopTime( + mc.sec2nano(totalTestSimTime) + ) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() if testReset: module.Reset(1) - unitTestSim.ConfigureStopTime(mc.sec2nano(totalTestSimTime+1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime( + mc.sec2nano(totalTestSimTime + 1.0) + ) # seconds to stop simulation unitTestSim.ExecuteSimulation() - # This pulls the actual data log from the simulation run. # Note that range(3) will provide [0, 1, 2] Those are the elements you get from the vector (all of them) accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) - trueSigma, trueOmega, truedOmega, \ - = truth.results(sigma_RR0,omega_RR0_R,RefStateInData,updateTime, cmdStateFlag, testReset) + ( + trueSigma, + trueOmega, + truedOmega, + ) = truth.results( + sigma_RR0, omega_RR0_R, RefStateInData, updateTime, cmdStateFlag, testReset + ) # # check sigma_RN # - testFailCount, testMessages = unitTestSupport.compareArray(trueSigma, dataLog.sigma_RN, - accuracy, "sigma_RN Set", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueSigma, + dataLog.sigma_RN, + accuracy, + "sigma_RN Set", + testFailCount, + testMessages, + ) # # check omega_RN_N # - testFailCount, testMessages = unitTestSupport.compareArray(trueOmega, dataLog.omega_RN_N, - accuracy, "omega_RN_N Vector", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueOmega, + dataLog.omega_RN_N, + accuracy, + "omega_RN_N Vector", + testFailCount, + testMessages, + ) # # check domega_RN_N # - testFailCount, testMessages = unitTestSupport.compareArray(truedOmega, dataLog.domega_RN_N, - accuracy, "domega_RN_N Vector", - testFailCount, testMessages) - + testFailCount, testMessages = unitTestSupport.compareArray( + truedOmega, + dataLog.domega_RN_N, + accuracy, + "domega_RN_N Vector", + testFailCount, + testMessages, + ) snippentName = "passFail" + str(cmdStateFlag) + str(testReset) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -199,7 +223,7 @@ def run(show_plots, cmdStateFlag, testReset): # if __name__ == "__main__": test_mrpRotation( - False # show plots - , False # cmdStateFlag - , True # testReset + False, # show plots + False, # cmdStateFlag + True, # testReset ) diff --git a/src/fswAlgorithms/attGuidance/oneAxisSolarArrayPoint/_UnitTest/test_oneAxisSolarArrayPoint.py b/src/fswAlgorithms/attGuidance/oneAxisSolarArrayPoint/_UnitTest/test_oneAxisSolarArrayPoint.py index 76aa5c277e..aeeb08ab14 100755 --- a/src/fswAlgorithms/attGuidance/oneAxisSolarArrayPoint/_UnitTest/test_oneAxisSolarArrayPoint.py +++ b/src/fswAlgorithms/attGuidance/oneAxisSolarArrayPoint/_UnitTest/test_oneAxisSolarArrayPoint.py @@ -31,7 +31,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) @@ -46,19 +46,18 @@ def computeGamma(alpha, delta): - - if alpha >= 0 and alpha <= np.pi/2: - if delta < np.pi/2 - alpha: - gamma = np.pi/2 - alpha - delta - elif delta > alpha + np.pi/2: - gamma = - np.pi/2 - alpha + delta + if alpha >= 0 and alpha <= np.pi / 2: + if delta < np.pi / 2 - alpha: + gamma = np.pi / 2 - alpha - delta + elif delta > alpha + np.pi / 2: + gamma = -np.pi / 2 - alpha + delta else: gamma = 0 else: - if delta < alpha - np.pi/2: - gamma = - np.pi/2 + alpha - delta - elif delta > 3/2*np.pi - alpha: - gamma = alpha + delta - 3/2*np.pi + if delta < alpha - np.pi / 2: + gamma = -np.pi / 2 + alpha - delta + elif delta > 3 / 2 * np.pi - alpha: + gamma = alpha + delta - 3 / 2 * np.pi else: gamma = 0 @@ -70,14 +69,22 @@ def computeGamma(alpha, delta): ang = np.linspace(0, np.pi, 5, endpoint=False) ang = list(ang) + @pytest.mark.parametrize("alpha", ang) @pytest.mark.parametrize("delta", ang) -@pytest.mark.parametrize("bodyAxisInput", [0,1]) -@pytest.mark.parametrize("inertialAxisInput", [0,1,2]) -@pytest.mark.parametrize("alignmentPriority", [0,1]) +@pytest.mark.parametrize("bodyAxisInput", [0, 1]) +@pytest.mark.parametrize("inertialAxisInput", [0, 1, 2]) +@pytest.mark.parametrize("alignmentPriority", [0, 1]) @pytest.mark.parametrize("accuracy", [1e-12]) - -def test_oneAxisSolarArrayPointTestFunction(show_plots, alpha, delta, bodyAxisInput, inertialAxisInput, alignmentPriority, accuracy): +def test_oneAxisSolarArrayPointTestFunction( + show_plots, + alpha, + delta, + bodyAxisInput, + inertialAxisInput, + alignmentPriority, + accuracy, +): r""" **Validation Test Description** @@ -119,18 +126,33 @@ def test_oneAxisSolarArrayPointTestFunction(show_plots, alpha, delta, bodyAxisIn module for multiple update calls. To ensure fast execution of the unit test, this is avoided. """ # each test method requires a single assert method to be called - [testResults, testMessage] = oneAxisSolarArrayPointTestFunction(show_plots, alpha, delta, bodyAxisInput, inertialAxisInput, alignmentPriority, accuracy) + [testResults, testMessage] = oneAxisSolarArrayPointTestFunction( + show_plots, + alpha, + delta, + bodyAxisInput, + inertialAxisInput, + alignmentPriority, + accuracy, + ) assert testResults < 1, testMessage -def oneAxisSolarArrayPointTestFunction(show_plots, alpha, delta, bodyAxisInput, inertialAxisInput, alignmentPriority, accuracy): - +def oneAxisSolarArrayPointTestFunction( + show_plots, + alpha, + delta, + bodyAxisInput, + inertialAxisInput, + alignmentPriority, + accuracy, +): gamma_true = computeGamma(alpha, delta) - rHat_SB_N = np.array([1, 2, 3]) # Sun direction in inertial coordinates + rHat_SB_N = np.array([1, 2, 3]) # Sun direction in inertial coordinates rHat_SB_N = rHat_SB_N / np.linalg.norm(rHat_SB_N) - a1Hat_B = np.array([9, 8, 7]) # array axis direction in body frame + a1Hat_B = np.array([9, 8, 7]) # array axis direction in body frame a1Hat_B = a1Hat_B / np.linalg.norm(a1Hat_B) a = np.cross(rHat_SB_N, [4, 5, 6]) @@ -142,13 +164,17 @@ def oneAxisSolarArrayPointTestFunction(show_plots, alpha, delta, bodyAxisInput, DCM1 = rbk.PRV2C(a * alpha) DCM2 = rbk.PRV2C(d * delta) - hHat_N = np.matmul(DCM1, rHat_SB_N) # required thrust direction in inertial frame, at an angle alpha from rHat_SB_N - hHat_B = np.matmul(DCM2, a1Hat_B) # required thrust direction in inertial frame, at an angle alpha from rHat_SB_N - - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + hHat_N = np.matmul( + DCM1, rHat_SB_N + ) # required thrust direction in inertial frame, at an angle alpha from rHat_SB_N + hHat_B = np.matmul( + DCM2, a1Hat_B + ) # required thrust direction in inertial frame, at an angle alpha from rHat_SB_N + + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container @@ -209,7 +235,6 @@ def oneAxisSolarArrayPointTestFunction(show_plots, alpha, delta, bodyAxisInput, ephemerisMsg = messaging.EphemerisMsg().write(ephemerisData) attReferenceCongfig.ephemerisInMsg.subscribeTo(ephemerisMsg) - # Setup logging on the test module output message so that we get all the writes to it dataLog = attReferenceCongfig.attRefOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) @@ -231,28 +256,38 @@ def oneAxisSolarArrayPointTestFunction(show_plots, alpha, delta, bodyAxisInput, RN = rbk.MRP2C(sigma_RN) NR = RN.transpose() a1Hat_N = np.matmul(NR, a1Hat_B) - gamma_sim = np.arcsin( abs( np.clip( np.dot(rHat_SB_N, a1Hat_N), -1, 1 ) ) ) + gamma_sim = np.arcsin(abs(np.clip(np.dot(rHat_SB_N, a1Hat_N), -1, 1))) # set the filtered output truth states if alignmentPriority == 0: if not unitTestSupport.isDoubleEqual(gamma_sim, gamma_true, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + attReferenceCongfig.ModelTag + " Module failed incidence angle for " + testMessages.append( + "FAILED: " + + attReferenceCongfig.ModelTag + + " Module failed incidence angle for " "bodyAxisInput = {}, inertialAxisInput = {} and priorityFlag = {}".format( - bodyAxisInput, inertialAxisInput, alignmentPriority)) + bodyAxisInput, inertialAxisInput, alignmentPriority + ) + ) else: if not unitTestSupport.isDoubleEqual(gamma_sim, 0, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + attReferenceCongfig.ModelTag + " Module failed incidence angle for " + testMessages.append( + "FAILED: " + + attReferenceCongfig.ModelTag + + " Module failed incidence angle for " "bodyAxisInput = {}, inertialAxisInput = {} and priorityFlag = {}".format( - bodyAxisInput, inertialAxisInput, alignmentPriority)) + bodyAxisInput, inertialAxisInput, alignmentPriority + ) + ) if testFailCount: print(testMessages) else: print("Unit Test Passed") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -261,11 +296,11 @@ def oneAxisSolarArrayPointTestFunction(show_plots, alpha, delta, bodyAxisInput, # if __name__ == "__main__": oneAxisSolarArrayPointTestFunction( - False, - np.pi, # alpha - np.pi, # delta - 0, # flagB - 1, # flagN - 0, # priorityFlag - 1e-9 # accuracy - ) + False, + np.pi, # alpha + np.pi, # delta + 0, # flagB + 1, # flagN + 0, # priorityFlag + 1e-9, # accuracy + ) diff --git a/src/fswAlgorithms/attGuidance/opNavPoint/_UnitTest/test_opNavPoint.py b/src/fswAlgorithms/attGuidance/opNavPoint/_UnitTest/test_opNavPoint.py index f1fcfcd4ca..31747606c5 100755 --- a/src/fswAlgorithms/attGuidance/opNavPoint/_UnitTest/test_opNavPoint.py +++ b/src/fswAlgorithms/attGuidance/opNavPoint/_UnitTest/test_opNavPoint.py @@ -33,11 +33,12 @@ path = os.path.dirname(os.path.abspath(filename)) - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import opNavPoint # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import opNavPoint # import the module that is to be tested from Basilisk.architecture import messaging from Basilisk.utilities import macros as mc @@ -45,18 +46,21 @@ # uncomment this line is 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(conditionstring) +# @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("case", [ - (1) # target is visible, vectors are not aligned - ,(2) # target is not visible, vectors are not aligned - ,(3) # target is visible, vectors are aligned - ,(4) # target is not visible, search -]) - +@pytest.mark.parametrize( + "case", + [ + (1), # target is visible, vectors are not aligned + (2), # target is not visible, vectors are not aligned + (3), # target is visible, vectors are aligned + (4), # target is not visible, search + ], +) def test_module(show_plots, case): """Module Unit Test""" # each test method requires a single assert method to be called @@ -65,20 +69,19 @@ def test_module(show_plots, case): def opNavPointTestFunction(show_plots, case): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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 = mc.sec2nano(0.5) # update process rate update time + testProcessRate = mc.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = opNavPoint.opNavPoint() module.ModelTag = "opNavPoint" @@ -87,39 +90,42 @@ def opNavPointTestFunction(show_plots, case): unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - camera_Z = [0.,0.,1.] + camera_Z = [0.0, 0.0, 1.0] module.alignAxis_C = camera_Z module.minUnitMag = 0.01 - module.smallAngle = 0.01*mc.D2R + module.smallAngle = 0.01 * mc.D2R module.timeOut = 100 # Create input messages # - planet_B = [1.,1.,0.] - inputOpNavData = messaging.OpNavMsgPayload() # Create a structure for the input message + planet_B = [1.0, 1.0, 0.0] + inputOpNavData = ( + messaging.OpNavMsgPayload() + ) # Create a structure for the input message inputOpNavData.r_BN_C = planet_B inputOpNavData.valid = 1 - if (case == 2): #No valid measurement + if case == 2: # No valid measurement inputOpNavData.valid = 0 - if (case == 3): #No valid measurement - inputOpNavData.r_BN_C = [0.,0.,-1.] - if (case == 4): #No valid measurement + if case == 3: # No valid measurement + inputOpNavData.r_BN_C = [0.0, 0.0, -1.0] + if case == 4: # No valid measurement inputOpNavData.valid = 0 opnavInMsg = messaging.OpNavMsg().write(inputOpNavData) - inputIMUData = messaging.NavAttMsgPayload() # Create a structure for the input message + inputIMUData = ( + messaging.NavAttMsgPayload() + ) # Create a structure for the input message omega_BN_B = np.array([0.01, 0.50, -0.2]) inputIMUData.omega_BN_B = omega_BN_B imuInMsg = messaging.NavAttMsg().write(inputIMUData) omega_RN_B_Search = np.array([0.0, 0.0, 0.1]) - if (case ==2 or case==4): + if case == 2 or case == 4: module.omega_RN_B = omega_RN_B_Search cam = messaging.CameraConfigMsgPayload() # Create a structure for the input message - cam.sigma_CB = [0.,0.,0] + cam.sigma_CB = [0.0, 0.0, 0] camInMsg = messaging.CameraConfigMsg().write(cam) - # Setup logging on the test module output message so that we get all the writes to it dataLog = module.attGuidanceOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) @@ -129,10 +135,9 @@ def opNavPointTestFunction(show_plots, case): module.imuInMsg.subscribeTo(imuInMsg) module.cameraConfigInMsg.subscribeTo(camInMsg) - # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() - unitTestSim.ConfigureStopTime(mc.sec2nano(1.)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(mc.sec2nano(1.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() # This pulls the actual data log from the simulation run. @@ -144,114 +149,117 @@ def opNavPointTestFunction(show_plots, case): eHat = np.cross(-np.array(planet_B), np.array(camera_Z)) eHat = eHat / np.linalg.norm(eHat) - Phi = np.arccos(np.dot(-np.array(planet_B)/np.linalg.norm(-np.array(planet_B)),np.array(camera_Z))) - sigmaTrue = eHat * np.tan(Phi/4.0) - trueVector = [ - sigmaTrue.tolist(), - sigmaTrue.tolist(), - sigmaTrue.tolist() - ] - if (case == 2 or case == 3 or case == 4): - trueVector = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, 0] - ] + Phi = np.arccos( + np.dot( + -np.array(planet_B) / np.linalg.norm(-np.array(planet_B)), + np.array(camera_Z), + ) + ) + sigmaTrue = eHat * np.tan(Phi / 4.0) + trueVector = [sigmaTrue.tolist(), sigmaTrue.tolist(), sigmaTrue.tolist()] + if case == 2 or case == 3 or case == 4: + trueVector = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] # compare the module results to the truth values accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.sigma_BR[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.sigma_BR[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_BR unit test at t=" + - str(dataLog.times()[i] * mc.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_BR unit test at t=" + + str(dataLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # # check omega_BR_B # # set the filtered output truth states - trueVector = [ - omega_BN_B.tolist(), - omega_BN_B.tolist(), - omega_BN_B.tolist() - ] - if (case == 2 or case==4): + trueVector = [omega_BN_B.tolist(), omega_BN_B.tolist(), omega_BN_B.tolist()] + if case == 2 or case == 4: trueVector = [ (omega_BN_B - omega_RN_B_Search).tolist(), (omega_BN_B - omega_RN_B_Search).tolist(), - (omega_BN_B - omega_RN_B_Search).tolist() + (omega_BN_B - omega_RN_B_Search).tolist(), ] # compare the module results to the truth values - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.omega_BR_B[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.omega_BR_B[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_BR_B unit test at t=" + - str(dataLog.times()[i] * mc.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_BR_B unit test at t=" + + str(dataLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # # check omega_RN_B # # set the filtered output truth states - trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] - if (case == 2 or case == 4): - trueVector = [ - omega_RN_B_Search, - omega_RN_B_Search, - omega_RN_B_Search - ] + trueVector = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + if case == 2 or case == 4: + trueVector = [omega_RN_B_Search, omega_RN_B_Search, omega_RN_B_Search] # compare the module results to the truth values - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.omega_RN_B[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.omega_RN_B[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_B unit test at t=" + - str(dataLog.times()[i] * mc.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_RN_B unit test at t=" + + str(dataLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # # check domega_RN_B # # set the filtered output truth states - trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] + trueVector = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # compare the module results to the truth values - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.domega_RN_B[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.domega_RN_B[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed domega_RN_B unit test at t=" + - str(dataLog.times()[i] * mc.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed domega_RN_B unit test at t=" + + str(dataLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # print out success message if no error were found snippentName = "passFail" + str(case) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("FAILED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attGuidance/sunSafePoint/_UnitTest/test_sunSafePoint.py b/src/fswAlgorithms/attGuidance/sunSafePoint/_UnitTest/test_sunSafePoint.py index b62c36d726..29e9f2dd24 100755 --- a/src/fswAlgorithms/attGuidance/sunSafePoint/_UnitTest/test_sunSafePoint.py +++ b/src/fswAlgorithms/attGuidance/sunSafePoint/_UnitTest/test_sunSafePoint.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -33,14 +32,14 @@ path = os.path.dirname(os.path.abspath(filename)) - - - - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import sunSafePoint # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import ( + sunSafePoint, +) # import the module that is to be tested from Basilisk.architecture import messaging from Basilisk.utilities import macros as mc @@ -48,21 +47,28 @@ # uncomment this line is 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(conditionstring) +# @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("case", [ - (1) # sun is visible, vectors are not aligned - ,(2) # sun is not visible, vectors are not aligned - ,(3) # sun is visible, vectors are aligned - ,(4) # sun is visible, vectors are oppositely aligned - ,(5) # sun is visible, vectors are oppositely aligned, and command sc is b1 - ,(6) # sun is not visible, vectors are not aligned, no specified omega_RN_B value - ,(7) # sun is visible, vectors not aligned, nominal spin rate specified about sun heading vector -]) - +@pytest.mark.parametrize( + "case", + [ + (1), # sun is visible, vectors are not aligned + (2), # sun is not visible, vectors are not aligned + (3), # sun is visible, vectors are aligned + (4), # sun is visible, vectors are oppositely aligned + (5), # sun is visible, vectors are oppositely aligned, and command sc is b1 + ( + 6 + ), # sun is not visible, vectors are not aligned, no specified omega_RN_B value + ( + 7 + ), # sun is visible, vectors not aligned, nominal spin rate specified about sun heading vector + ], +) def test_module(show_plots, case): """Module Unit Test""" # each test method requires a single assert method to be called @@ -71,20 +77,19 @@ def test_module(show_plots, case): def sunSafePointTestFunction(show_plots, case): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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 = mc.sec2nano(0.5) # update process rate update time + testProcessRate = mc.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = sunSafePoint.sunSafePoint() module.ModelTag = "sunSafePoint" @@ -93,7 +98,7 @@ def sunSafePointTestFunction(show_plots, case): unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - sHat_Cmd_B = np.array([0.0, 0.0 ,1.0]) + sHat_Cmd_B = np.array([0.0, 0.0, 1.0]) if case == 5: sHat_Cmd_B = np.array([1.0, 0.0, 0.0]) module.sHatBdyCmd = sHat_Cmd_B @@ -101,31 +106,35 @@ def sunSafePointTestFunction(show_plots, case): if case == 2: omega_RN_B_Search = np.array([0.0, 0.0, 0.1]) module.omega_RN_B = omega_RN_B_Search - module.smallAngle = 0.01*mc.D2R + module.smallAngle = 0.01 * mc.D2R # Create input messages # - inputSunVecData = messaging.NavAttMsgPayload() # Create a structure for the input message + inputSunVecData = ( + messaging.NavAttMsgPayload() + ) # Create a structure for the input message sunVec_B = np.array([1.0, 1.0, 0.0]) - if (case == 2 or case == 6): # no sun visible, providing a near zero norm direction vector */ - sunVec_B = [0.0, module.minUnitMag/2, 0.0] - if (case == 3): + if ( + case == 2 or case == 6 + ): # no sun visible, providing a near zero norm direction vector */ + sunVec_B = [0.0, module.minUnitMag / 2, 0.0] + if case == 3: sunVec_B = sHat_Cmd_B - if (case == 4 or case == 5): + if case == 4 or case == 5: sunVec_B = -sHat_Cmd_B inputSunVecData.vehSunPntBdy = sunVec_B sunInMsg = messaging.NavAttMsg().write(inputSunVecData) - inputIMUData = messaging.NavAttMsgPayload() # Create a structure for the input message + inputIMUData = ( + messaging.NavAttMsgPayload() + ) # Create a structure for the input message omega_BN_B = np.array([0.01, 0.50, -0.2]) inputIMUData.omega_BN_B = omega_BN_B imuInMsg = messaging.NavAttMsg().write(inputIMUData) if case == 7: - module.sunAxisSpinRate = 1.5*mc.D2R - omega_RN_B_Search = sunVec_B/np.linalg.norm(sunVec_B) * module.sunAxisSpinRate - - + module.sunAxisSpinRate = 1.5 * mc.D2R + omega_RN_B_Search = sunVec_B / np.linalg.norm(sunVec_B) * module.sunAxisSpinRate # Setup logging on the test module output message so that we get all the writes to it dataLog = module.attGuidanceOutMsg.recorder() @@ -135,7 +144,6 @@ def sunSafePointTestFunction(show_plots, case): module.sunDirectionInMsg.subscribeTo(sunInMsg) module.imuInMsg.subscribeTo(imuInMsg) - # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() @@ -143,10 +151,10 @@ def sunSafePointTestFunction(show_plots, case): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(mc.sec2nano(1.)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(mc.sec2nano(1.0)) # seconds to stop simulation # run the Reset() routine - module.Reset(0) # this module reset function needs a time input (in NanoSeconds) + module.Reset(0) # this module reset function needs a time input (in NanoSeconds) # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -157,143 +165,132 @@ def sunSafePointTestFunction(show_plots, case): # check sigma_BR # # set the filtered output truth states - if (case == 1 or case == 7): - eHat = np.cross(sunVec_B,sHat_Cmd_B) + if case == 1 or case == 7: + eHat = np.cross(sunVec_B, sHat_Cmd_B) eHat = eHat / np.linalg.norm(eHat) - Phi = np.arccos(np.dot(sunVec_B/np.linalg.norm(sunVec_B),sHat_Cmd_B)) - sigmaTrue = eHat * np.tan(Phi/4.0) - trueVector = [ - sigmaTrue.tolist(), - sigmaTrue.tolist(), - sigmaTrue.tolist() - ] - if (case == 2 or case == 3 or case == 6): - trueVector = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, 0] - ] - if (case == 4): - eHat = np.cross(sHat_Cmd_B,np.array([1,0,0])) + Phi = np.arccos(np.dot(sunVec_B / np.linalg.norm(sunVec_B), sHat_Cmd_B)) + sigmaTrue = eHat * np.tan(Phi / 4.0) + trueVector = [sigmaTrue.tolist(), sigmaTrue.tolist(), sigmaTrue.tolist()] + if case == 2 or case == 3 or case == 6: + trueVector = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] + if case == 4: + eHat = np.cross(sHat_Cmd_B, np.array([1, 0, 0])) eHat = eHat / np.linalg.norm(eHat) - Phi = np.arccos(np.dot(sunVec_B/np.linalg.norm(sunVec_B),sHat_Cmd_B)) - sigmaTrue = eHat * np.tan(Phi/4.0) - trueVector = [ - sigmaTrue.tolist(), - sigmaTrue.tolist(), - sigmaTrue.tolist() - ] - if (case == 5): + Phi = np.arccos(np.dot(sunVec_B / np.linalg.norm(sunVec_B), sHat_Cmd_B)) + sigmaTrue = eHat * np.tan(Phi / 4.0) + trueVector = [sigmaTrue.tolist(), sigmaTrue.tolist(), sigmaTrue.tolist()] + if case == 5: eHat = np.cross(sHat_Cmd_B, np.array([0, 1, 0])) eHat = eHat / np.linalg.norm(eHat) - Phi = np.arccos(np.dot(sunVec_B/np.linalg.norm(sunVec_B), sHat_Cmd_B)) + Phi = np.arccos(np.dot(sunVec_B / np.linalg.norm(sunVec_B), sHat_Cmd_B)) sigmaTrue = eHat * np.tan(Phi / 4.0) - trueVector = [ - sigmaTrue.tolist(), - sigmaTrue.tolist(), - sigmaTrue.tolist() - ] + trueVector = [sigmaTrue.tolist(), sigmaTrue.tolist(), sigmaTrue.tolist()] # compare the module results to the truth values accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.sigma_BR[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.sigma_BR[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_BR unit test at t=" + - str(dataLog.times()[i] * mc.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_BR unit test at t=" + + str(dataLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # # check omega_BR_B # # set the filtered output truth states - if (case == 1 or case == 3 or case == 4 or case == 5 or case == 6): - trueVector = [ - omega_BN_B.tolist(), - omega_BN_B.tolist(), - omega_BN_B.tolist() - ] - if (case == 2 or case == 7): + if case == 1 or case == 3 or case == 4 or case == 5 or case == 6: + trueVector = [omega_BN_B.tolist(), omega_BN_B.tolist(), omega_BN_B.tolist()] + if case == 2 or case == 7: trueVector = [ (omega_BN_B - omega_RN_B_Search).tolist(), (omega_BN_B - omega_RN_B_Search).tolist(), - (omega_BN_B - omega_RN_B_Search).tolist() + (omega_BN_B - omega_RN_B_Search).tolist(), ] # compare the module results to the truth values - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.omega_BR_B[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.omega_BR_B[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_BR_B unit test at t=" + - str(dataLog.times()[i] * mc.NANO2SEC) + - "sec\n") - + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_BR_B unit test at t=" + + str(dataLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # # check omega_RN_B # # set the filtered output truth states - if (case == 1 or case == 3 or case == 4 or case == 5 or case == 6): - trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] - if (case == 2 or case == 7): - trueVector = [ - omega_RN_B_Search, - omega_RN_B_Search, - omega_RN_B_Search - ] + if case == 1 or case == 3 or case == 4 or case == 5 or case == 6: + trueVector = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + if case == 2 or case == 7: + trueVector = [omega_RN_B_Search, omega_RN_B_Search, omega_RN_B_Search] # compare the module results to the truth values - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.omega_RN_B[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.omega_RN_B[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_B unit test at t=" + - str(dataLog.times()[i] * mc.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_RN_B unit test at t=" + + str(dataLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # # check domega_RN_B # # set the filtered output truth states - trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] + trueVector = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # compare the module results to the truth values - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.domega_RN_B[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.domega_RN_B[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed domega_RN_B unit test at t=" + - str(dataLog.times()[i] * mc.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed domega_RN_B unit test at t=" + + str(dataLog.times()[i] * mc.NANO2SEC) + + "sec\n" + ) # print out success message if no error were found snippentName = "passFail" + str(case) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("FAILED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" print(testMessages) unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attGuidance/velocityPoint/_UnitTest/Support/results_velocityPoint.py b/src/fswAlgorithms/attGuidance/velocityPoint/_UnitTest/Support/results_velocityPoint.py index 66e8ef646e..097d4d7f11 100755 --- a/src/fswAlgorithms/attGuidance/velocityPoint/_UnitTest/Support/results_velocityPoint.py +++ b/src/fswAlgorithms/attGuidance/velocityPoint/_UnitTest/Support/results_velocityPoint.py @@ -18,16 +18,19 @@ import numpy as np from numpy import linalg as la from numpy import sin, cos + np.set_printoptions(precision=12) from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.utilities import astroFunctions as af + def normalize(v): - norm=np.linalg.norm(v) - if norm==0: - return v - return v/norm + norm = np.linalg.norm(v) + if norm == 0: + return v + return v / norm + def printResults_VelocityPoint(r_BN_N, v_BN_N, celBodyPosVec, celBodyVelVec, mu): r = r_BN_N - celBodyPosVec @@ -38,7 +41,7 @@ def printResults_VelocityPoint(r_BN_N, v_BN_N, celBodyPosVec, celBodyVelVec, mu) i_v = normalize(v) i_h = normalize(h) i_n = np.cross(i_v, i_h) - VN = np.array([ i_n, i_v, i_h ]) + VN = np.array([i_n, i_v, i_h]) sigma_VN = rbk.C2MRP(VN) hm = la.norm(h) @@ -56,12 +59,13 @@ def printResults_VelocityPoint(r_BN_N, v_BN_N, celBodyPosVec, celBodyVelVec, mu) omega_VN_N = (-dBdt + dfdt) * i_h domega_VN_N = (-ddBdt2 + ddfdt2) * i_h - print('sigma_VN = ', sigma_VN) - print('omega_VN_N = ', omega_VN_N) - print('domega_VN_N = ', domega_VN_N) + print("sigma_VN = ", sigma_VN) + print("omega_VN_N = ", omega_VN_N) + print("domega_VN_N = ", domega_VN_N) return (sigma_VN, omega_VN_N, domega_VN_N) + # MAIN # Initial Conditions (IC) a = af.E_radius * 2.8 diff --git a/src/fswAlgorithms/attGuidance/velocityPoint/_UnitTest/test_velocityPoint.py b/src/fswAlgorithms/attGuidance/velocityPoint/_UnitTest/test_velocityPoint.py index 834a6f0658..7a5a781f59 100755 --- a/src/fswAlgorithms/attGuidance/velocityPoint/_UnitTest/test_velocityPoint.py +++ b/src/fswAlgorithms/attGuidance/velocityPoint/_UnitTest/test_velocityPoint.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -25,13 +24,18 @@ import numpy as np from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import velocityPoint # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + velocityPoint, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import astroFunctions as af from Basilisk.architecture import astroConstants from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -47,20 +51,19 @@ def test_velocityPoint(show_plots): def velocityPointTestFunction(show_plots): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = velocityPoint.velocityPoint() module.ModelTag = "velocityPoint" @@ -70,7 +73,7 @@ def velocityPointTestFunction(show_plots): # Initialize the test module configuration data - module.mu = astroConstants.MU_EARTH*1e9 # m^3/s^2 + module.mu = astroConstants.MU_EARTH * 1e9 # m^3/s^2 a = astroConstants.REQ_EARTH * 2.8 * 1000 # m e = 0.0 @@ -89,7 +92,9 @@ def velocityPointTestFunction(show_plots): # # Navigation Input Message # - NavStateOutData = messaging.NavTransMsgPayload() # Create a structure for the input message + NavStateOutData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message NavStateOutData.r_BN_N = r_BN_N NavStateOutData.v_BN_N = v_BN_N navInMsg = messaging.NavTransMsg().write(NavStateOutData) @@ -117,7 +122,7 @@ def velocityPointTestFunction(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -129,48 +134,53 @@ def velocityPointTestFunction(show_plots): # # set the filtered output truth states trueVector = [ - [0., 0., 0.267949192431], - [0., 0., 0.267949192431], - [0., 0., 0.267949192431] - ] + [0.0, 0.0, 0.267949192431], + [0.0, 0.0, 0.267949192431], + [0.0, 0.0, 0.267949192431], + ] # compare the module results to the truth values accuracy = 1e-12 - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, dataLog.sigma_RN, accuracy, - 'sigma_RN', testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, dataLog.sigma_RN, accuracy, "sigma_RN", testFailCount, testMessages + ) # # check omega_RN_N # # set the filtered output truth states - fDot = np.sqrt(module.mu / (a*a*a)) - trueVector = [ - [0., 0., fDot], - [0., 0., fDot], - [0., 0., fDot] - ] + fDot = np.sqrt(module.mu / (a * a * a)) + trueVector = [[0.0, 0.0, fDot], [0.0, 0.0, fDot], [0.0, 0.0, fDot]] # compare the module results to the truth values accuracy = 1e-12 - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, dataLog.omega_RN_N, accuracy, - 'omega_RN_N', testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + dataLog.omega_RN_N, + accuracy, + "omega_RN_N", + testFailCount, + testMessages, + ) # # check domega_RN_N # # set the filtered output truth states - trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] + trueVector = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # compare the module results to the truth values accuracy = 1e-12 - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, dataLog.domega_RN_N, accuracy, - 'domega_RN_N', testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + dataLog.domega_RN_N, + accuracy, + "domega_RN_N", + testFailCount, + testMessages, + ) # Note that we can continue to step the simulation however we feel like. # Just because we stop and query data does not mean everything has to stop for good - unitTestSim.ConfigureStopTime(macros.sec2nano(0.6)) # run an additional 0.6 seconds + unitTestSim.ConfigureStopTime(macros.sec2nano(0.6)) # run an additional 0.6 seconds unitTestSim.ExecuteSimulation() if testFailCount: @@ -180,7 +190,7 @@ def velocityPointTestFunction(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/attGuidance/waypointReference/_UnitTest/test_WaypointReference.py b/src/fswAlgorithms/attGuidance/waypointReference/_UnitTest/test_WaypointReference.py index ece40dc574..d23baf2496 100644 --- a/src/fswAlgorithms/attGuidance/waypointReference/_UnitTest/test_WaypointReference.py +++ b/src/fswAlgorithms/attGuidance/waypointReference/_UnitTest/test_WaypointReference.py @@ -32,6 +32,7 @@ from Basilisk.architecture import bskLogging from Basilisk.fswAlgorithms import waypointReference from Basilisk.utilities import RigidBodyKinematics as rbk + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros @@ -39,6 +40,8 @@ path = os.path.dirname(os.path.abspath(__file__)) dataFileName = None + + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. @pytest.mark.parametrize("attType", [0, 1, 2]) @@ -47,7 +50,9 @@ @pytest.mark.parametrize("accuracy", [1e-12]) # provide a unique test method name, starting with test_ -def test_waypointReference(show_plots, attType, MRPswitching, useReferenceFrame, accuracy): +def test_waypointReference( + show_plots, attType, MRPswitching, useReferenceFrame, accuracy +): r""" **Validation Test Description** @@ -94,20 +99,22 @@ def test_waypointReference(show_plots, attType, MRPswitching, useReferenceFrame, """ # each test method requires a single assert method to be called - [testResults, testMessage] = waypointReferenceTestFunction(attType, MRPswitching, useReferenceFrame, accuracy) + [testResults, testMessage] = waypointReferenceTestFunction( + attType, MRPswitching, useReferenceFrame, accuracy + ) global dataFileName if os.path.exists(dataFileName): os.remove(dataFileName) assert testResults < 1, testMessage -def waypointReferenceTestFunction(attType, MRPswitching, useReferenceFrame, accuracy): +def waypointReferenceTestFunction(attType, MRPswitching, useReferenceFrame, accuracy): bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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() @@ -144,13 +151,17 @@ def waypointReferenceTestFunction(attType, MRPswitching, useReferenceFrame, accu s = 0.8 else: s = 0.1 - attRealMRP = np.array([s + 0.05*i, 0.2 + 0.05*i, 0.3 + 0.05*i]) + attRealMRP = np.array([s + 0.05 * i, 0.2 + 0.05 * i, 0.3 + 0.05 * i]) if np.linalg.norm(attRealMRP) <= 1: attReal_RN.append(attRealMRP) else: - attReal_RN.append(-attRealMRP / (np.linalg.norm(attRealMRP))**2 ) - omegaReal_RN_N.append(np.array([0.4 + 0.05*i, 0.5 + 0.05*i, 0.6 + 0.05*i])) - omegaDotReal_RN_N.append(np.array([0.7 + 0.05*i, 0.8 + 0.05*i, 0.9 + 0.05*i])) + attReal_RN.append(-attRealMRP / (np.linalg.norm(attRealMRP)) ** 2) + omegaReal_RN_N.append( + np.array([0.4 + 0.05 * i, 0.5 + 0.05 * i, 0.6 + 0.05 * i]) + ) + omegaDotReal_RN_N.append( + np.array([0.7 + 0.05 * i, 0.8 + 0.05 * i, 0.9 + 0.05 * i]) + ) lineString = str(t[-1]) + delimiter @@ -168,12 +179,22 @@ def waypointReferenceTestFunction(attType, MRPswitching, useReferenceFrame, accu return if not useReferenceFrame: - lineString += str(omegaReal_RN_N[-1].tolist())[1:-1] + delimiter + str(omegaDotReal_RN_N[-1].tolist())[1:-1] + '\n' + lineString += ( + str(omegaReal_RN_N[-1].tolist())[1:-1] + + delimiter + + str(omegaDotReal_RN_N[-1].tolist())[1:-1] + + "\n" + ) else: RN = rbk.MRP2C(attReal_RN[-1]) omegaReal_RN_R = np.matmul(RN, omegaReal_RN_N[-1]) omegaDotReal_RN_R = np.matmul(RN, omegaDotReal_RN_N[-1]) - lineString += str(omegaReal_RN_R.tolist())[1:-1] + delimiter + str(omegaDotReal_RN_R.tolist())[1:-1] + '\n' + lineString += ( + str(omegaReal_RN_R.tolist())[1:-1] + + delimiter + + str(omegaDotReal_RN_R.tolist())[1:-1] + + "\n" + ) # write line on file fDataFile.write(lineString) @@ -214,60 +235,135 @@ def waypointReferenceTestFunction(attType, MRPswitching, useReferenceFrame, accu sigma_RN = [[], [], []] # checking attitude msg for t < t_min - for i in range(len(timeData)-1): - + for i in range(len(timeData) - 1): for n in range(3): sigma_RN[n].append(dataLog.sigma_RN[i][n]) if timeData[i] < t[0]: - if not unitTestSupport.isVectorEqual(dataLog.sigma_RN[i], attReal_RN[0], accuracy): + if not unitTestSupport.isVectorEqual( + dataLog.sigma_RN[i], attReal_RN[0], accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Module failed attitude check at time t = {}".format(timeData[i])) - if not unitTestSupport.isVectorEqual(dataLog.omega_RN_N[i], np.array([0.0, 0.0, 0.0]), accuracy): + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Module failed attitude check at time t = {}".format(timeData[i]) + ) + if not unitTestSupport.isVectorEqual( + dataLog.omega_RN_N[i], np.array([0.0, 0.0, 0.0]), accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular rate check at time t = {}".format(timeData[i])) - if not unitTestSupport.isVectorEqual(dataLog.domega_RN_N[i], np.array([0.0, 0.0, 0.0]), accuracy): + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Module failed angular rate check at time t = {}".format( + timeData[i] + ) + ) + if not unitTestSupport.isVectorEqual( + dataLog.domega_RN_N[i], np.array([0.0, 0.0, 0.0]), accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular acceleration check at time t = {}".format(timeData[i])) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Module failed angular acceleration check at time t = {}".format( + timeData[i] + ) + ) # checking attitude msg for t_min <= t <= t_max elif timeData[i] >= t[0] and timeData[i] <= t[-1]: - while (timeData[i] >= t[j] and timeData[i] <= t[j+1]) == False: + while (timeData[i] >= t[j] and timeData[i] <= t[j + 1]) == False: j += 1 sigma_RN_int = np.array([0.0, 0.0, 0.0]) omega_RN_N_int = np.array([0.0, 0.0, 0.0]) omegaDot_RN_N_int = np.array([0.0, 0.0, 0.0]) # interpolating between attitudes for times t = timeData[i] - if np.linalg.norm( attReal_RN[j+1] - attReal_RN[j] ) <= 1: - sigma_RN_int = attReal_RN[j] + (attReal_RN[j+1] - attReal_RN[j]) / (t[j+1] - t[j]) * (timeData[i] - t[j]) + if np.linalg.norm(attReal_RN[j + 1] - attReal_RN[j]) <= 1: + sigma_RN_int = attReal_RN[j] + (attReal_RN[j + 1] - attReal_RN[j]) / ( + t[j + 1] - t[j] + ) * (timeData[i] - t[j]) else: - attReal_RN_SS = -attReal_RN[j+1] / (np.linalg.norm(attReal_RN[j+1]))**2 - sigma_RN_int = attReal_RN[j] + (attReal_RN_SS - attReal_RN[j]) / (t[j+1] - t[j]) * (timeData[i] - t[j]) - omega_RN_N_int = omegaReal_RN_N[j] + (omegaReal_RN_N[j+1] - omegaReal_RN_N[j]) / (t[j+1] - t[j]) * (timeData[i] - t[j]) - omegaDot_RN_N_int = omegaDotReal_RN_N[j] + (omegaDotReal_RN_N[j+1] - omegaDotReal_RN_N[j]) / (t[j+1] - t[j]) * (timeData[i] - t[j]) - if not unitTestSupport.isVectorEqual(dataLog.sigma_RN[i], sigma_RN_int, accuracy): + attReal_RN_SS = ( + -attReal_RN[j + 1] / (np.linalg.norm(attReal_RN[j + 1])) ** 2 + ) + sigma_RN_int = attReal_RN[j] + (attReal_RN_SS - attReal_RN[j]) / ( + t[j + 1] - t[j] + ) * (timeData[i] - t[j]) + omega_RN_N_int = omegaReal_RN_N[j] + ( + omegaReal_RN_N[j + 1] - omegaReal_RN_N[j] + ) / (t[j + 1] - t[j]) * (timeData[i] - t[j]) + omegaDot_RN_N_int = omegaDotReal_RN_N[j] + ( + omegaDotReal_RN_N[j + 1] - omegaDotReal_RN_N[j] + ) / (t[j + 1] - t[j]) * (timeData[i] - t[j]) + if not unitTestSupport.isVectorEqual( + dataLog.sigma_RN[i], sigma_RN_int, accuracy + ): print(timeData[i], dataLog.sigma_RN[i], sigma_RN_int) testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Module failed attitude check at time t = {}".format(timeData[i])) - if not unitTestSupport.isVectorEqual(dataLog.omega_RN_N[i], omega_RN_N_int, accuracy): + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Module failed attitude check at time t = {}".format(timeData[i]) + ) + if not unitTestSupport.isVectorEqual( + dataLog.omega_RN_N[i], omega_RN_N_int, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular rate check at time t = {}".format(timeData[i])) - if not unitTestSupport.isVectorEqual(dataLog.domega_RN_N[i], omegaDot_RN_N_int, accuracy): + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Module failed angular rate check at time t = {}".format( + timeData[i] + ) + ) + if not unitTestSupport.isVectorEqual( + dataLog.domega_RN_N[i], omegaDot_RN_N_int, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular acceleration check at time t = {}".format(timeData[i])) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Module failed angular acceleration check at time t = {}".format( + timeData[i] + ) + ) # checking attitude msg for t < t_max else: - if not unitTestSupport.isVectorEqual(dataLog.sigma_RN[i], attReal_RN[-1], accuracy): + if not unitTestSupport.isVectorEqual( + dataLog.sigma_RN[i], attReal_RN[-1], accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Module failed attitude check at time t = {}".format(timeData[i])) - if not unitTestSupport.isVectorEqual(dataLog.omega_RN_N[i], [0.0, 0.0, 0.0], accuracy): + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Module failed attitude check at time t = {}".format(timeData[i]) + ) + if not unitTestSupport.isVectorEqual( + dataLog.omega_RN_N[i], [0.0, 0.0, 0.0], accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular rate check at time t = {}".format(timeData[i])) - if not unitTestSupport.isVectorEqual(dataLog.domega_RN_N[i], [0.0, 0.0, 0.0], accuracy): + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Module failed angular rate check at time t = {}".format( + timeData[i] + ) + ) + if not unitTestSupport.isVectorEqual( + dataLog.domega_RN_N[i], [0.0, 0.0, 0.0], accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + testModule.ModelTag + " Module failed angular acceleration check at time t = {}".format(timeData[i])) + testMessages.append( + "FAILED: " + + testModule.ModelTag + + " Module failed angular acceleration check at time t = {}".format( + timeData[i] + ) + ) # print out success or failure message if testFailCount == 0: @@ -276,7 +372,8 @@ def waypointReferenceTestFunction(attType, MRPswitching, useReferenceFrame, accu print("FAILED: " + testModule.ModelTag) print(testMessages) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + # # This statement below ensures that the unitTestScript can be run as a @@ -284,9 +381,10 @@ def waypointReferenceTestFunction(attType, MRPswitching, useReferenceFrame, accu # if __name__ == "__main__": waypointReferenceTestFunction( - 2, # attType (0 -> MRP, 1 -> quaternion [q0, q1, q2, q3], 2 -> quaternion [q1, q2, q3, qs]) - False, # MRPswitching - False, # useReferenceFrame - 1e-12) + 2, # attType (0 -> MRP, 1 -> quaternion [q0, q1, q2, q3], 2 -> quaternion [q1, q2, q3, qs]) + False, # MRPswitching + False, # useReferenceFrame + 1e-12, + ) if os.path.exists(dataFileName): os.remove(dataFileName) diff --git a/src/fswAlgorithms/dvGuidance/dvAttGuidance/_UnitTest/test_dvGuidance.py b/src/fswAlgorithms/dvGuidance/dvAttGuidance/_UnitTest/test_dvGuidance.py index b1367840d3..b7f42a935f 100644 --- a/src/fswAlgorithms/dvGuidance/dvAttGuidance/_UnitTest/test_dvGuidance.py +++ b/src/fswAlgorithms/dvGuidance/dvAttGuidance/_UnitTest/test_dvGuidance.py @@ -12,21 +12,23 @@ from Basilisk.fswAlgorithms import dvGuidance from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) - def test_dv_guidance(show_plots): - """ Test dvGuidance. """ + """Test dvGuidance.""" [testResults, testMessage] = dvGuidanceTestFunction(show_plots) assert testResults < 1, testMessage + def dvGuidanceTestFunction(show_plots): - """ Test the dvGuidance module. Setup a simulation, write a DvBurnCmdFswMsg, and confirm that dvGuidance outputs the - correct values. """ + """Test the dvGuidance module. Setup a simulation, write a DvBurnCmdFswMsg, and confirm that dvGuidance outputs the + correct values.""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -42,7 +44,9 @@ def dvGuidanceTestFunction(show_plots): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process # Construct the dvGuidance module module = dvGuidance.dvGuidance() @@ -58,7 +62,7 @@ def dvGuidanceTestFunction(show_plots): # NOTE: This is nonsense. These are random numbers dvBurnCmdMsg.dvInrtlCmd = [5, 5, 5] dvBurnCmdMsg.dvRotVecUnit = [1, 0, 0] - dvBurnCmdMsg.dvRotVecMag = .5 + dvBurnCmdMsg.dvRotVecMag = 0.5 dvBurnCmdMsg.burnStartTime = macros.sec2nano(0.5) # Write this message dvBurnInMsg = messaging.DvBurnCmdMsg().write(dvBurnCmdMsg) @@ -71,30 +75,35 @@ def dvGuidanceTestFunction(show_plots): # connect messages module.burnDataInMsg.subscribeTo(dvBurnInMsg) - # Initialize the simulation unitTestSim.InitializeSimulation() # Step the simulation to 3*process rate so 4 total steps including zero - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() # Get the output from this simulation - moduleOutputName = 'dvAttGuidance' + moduleOutputName = "dvAttGuidance" outSigma = dataLog.sigma_RN outOmega = dataLog.omega_RN_N outDOmega = dataLog.domega_RN_N # NOTE: these values are just from a previous run. These should be validated - trueSigma = [[5.69822629e-01, 1.99143700e-01, 2.72649472e-01], - [6.12361487e-01, 1.31298090e-01, 3.16981631e-01], - [6.50967464e-01, 5.62624705e-02, 3.61117890e-01]] - trueOmega = [[4.08248290e-01, -2.04124145e-01, -2.04124145e-01], - [4.08248290e-01, -2.04124145e-01, -2.04124145e-01], - [4.08248290e-01, -2.04124145e-01, -2.04124145e-01]] - trueDOmega =[[0.00000000e+00, 0.00000000e+00, 0.00000000e+00], - [0.00000000e+00, 0.00000000e+00, 0.00000000e+00], - [0.00000000e+00, 0.00000000e+00, 0.00000000e+00]] + trueSigma = [ + [5.69822629e-01, 1.99143700e-01, 2.72649472e-01], + [6.12361487e-01, 1.31298090e-01, 3.16981631e-01], + [6.50967464e-01, 5.62624705e-02, 3.61117890e-01], + ] + trueOmega = [ + [4.08248290e-01, -2.04124145e-01, -2.04124145e-01], + [4.08248290e-01, -2.04124145e-01, -2.04124145e-01], + [4.08248290e-01, -2.04124145e-01, -2.04124145e-01], + ] + trueDOmega = [ + [0.00000000e00, 0.00000000e00, 0.00000000e00], + [0.00000000e00, 0.00000000e00, 0.00000000e00], + [0.00000000e00, 0.00000000e00, 0.00000000e00], + ] accuracy = 1e-9 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) @@ -104,18 +113,30 @@ def dvGuidanceTestFunction(show_plots): if not unitTestSupport.isArrayEqual(outSigma[i], trueSigma[i], 3, accuracy): testFailCount += 1 testMessages.append( - "FAILED: " + module.ModelTag + " Module failed sigma_RN unit test at t=" + str( - dataLog.times()[i] * macros.NANO2SEC) + "sec\n") + "FAILED: " + + module.ModelTag + + " Module failed sigma_RN unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) if not unitTestSupport.isArrayEqual(outOmega[i], trueOmega[i], 3, accuracy): testFailCount += 1 testMessages.append( - "FAILED: " + module.ModelTag + " Module failed omega_RN_N unit test at t=" + str( - dataLog.times()[i] * macros.NANO2SEC) + "sec\n") + "FAILED: " + + module.ModelTag + + " Module failed omega_RN_N unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) if not unitTestSupport.isArrayEqual(outDOmega[i], trueDOmega[i], 3, accuracy): testFailCount += 1 testMessages.append( - "FAILED: " + module.ModelTag + " Module failed domega_RN_N unit test at t=" + str( - dataLog.times()[i] * macros.NANO2SEC) + "sec\n") + "FAILED: " + + module.ModelTag + + " Module failed domega_RN_N unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) # print(outSigma) # print(outOmega) @@ -125,41 +146,42 @@ def dvGuidanceTestFunction(show_plots): plt.plot(dataLog.times() * macros.NANO2SEC, outSigma[:, 0], label="Sigma 1") plt.plot(dataLog.times() * macros.NANO2SEC, outSigma[:, 1], label="Sigma 2") plt.plot(dataLog.times() * macros.NANO2SEC, outSigma[:, 2], label="Sigma 3") - plt.legend(loc='upper left') - plt.xlabel('Time [s]') - plt.ylabel('Sigma') + plt.legend(loc="upper left") + plt.xlabel("Time [s]") + plt.ylabel("Sigma") plt.figure() plt.plot(dataLog.times() * macros.NANO2SEC, outOmega[:, 0], label="Omega 1") plt.plot(dataLog.times() * macros.NANO2SEC, outOmega[:, 1], label="Omega 2") plt.plot(dataLog.times() * macros.NANO2SEC, outOmega[:, 2], label="Omega 3") - plt.legend(loc='upper left') - plt.xlabel('Time [s]') - plt.ylabel('Omega [rad/s]') + plt.legend(loc="upper left") + plt.xlabel("Time [s]") + plt.ylabel("Omega [rad/s]") plt.figure() plt.plot(dataLog.times() * macros.NANO2SEC, outDOmega[:, 0], label="DOmega 1") plt.plot(dataLog.times() * macros.NANO2SEC, outDOmega[:, 1], label="DOmega 2") plt.plot(dataLog.times() * macros.NANO2SEC, outDOmega[:, 2], label="DOmega 3") - plt.legend(loc='upper left') - plt.xlabel('Time [s]') - plt.ylabel('DOmega') + plt.legend(loc="upper left") + plt.xlabel("Time [s]") + plt.ylabel("DOmega") if show_plots: plt.show() snippentName = "passFail" if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + -if __name__ == '__main__': +if __name__ == "__main__": test_dv_guidance(show_plots=False) diff --git a/src/fswAlgorithms/effectorInterfaces/dipoleMapping/_UnitTest/test_dipoleMapping.py b/src/fswAlgorithms/effectorInterfaces/dipoleMapping/_UnitTest/test_dipoleMapping.py index ee7a068c19..60b0bd3d1d 100644 --- a/src/fswAlgorithms/effectorInterfaces/dipoleMapping/_UnitTest/test_dipoleMapping.py +++ b/src/fswAlgorithms/effectorInterfaces/dipoleMapping/_UnitTest/test_dipoleMapping.py @@ -27,13 +27,18 @@ import numpy as np from Basilisk.architecture import bskLogging from Basilisk.architecture import messaging # import the message definitions -from Basilisk.fswAlgorithms import dipoleMapping # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + dipoleMapping, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions -accuracy = 1E-12 +accuracy = 1e-12 # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed # @pytest.mark.skipif(conditionstring) @@ -41,7 +46,8 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ -def test_dipoleMapping_module(): # update "module" in this function name to reflect the module name + +def test_dipoleMapping_module(): # update "module" in this function name to reflect the module name r""" **Validation Test Description** @@ -60,38 +66,53 @@ def test_dipoleMapping_module(): # update "module" in this function name to [testResults, testMessage] = dipoleMappingModuleTestFunction() assert testResults < 1, testMessage + def dipoleMappingModuleTestFunction(): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.01) # update process rate update time + testProcessRate = macros.sec2nano(0.01) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Initialize module under test's config message and add module to runtime call list module = dipoleMapping.dipoleMapping() - module.steeringMatrix = [1., 0., 0., 0., 1., 0., 0., 0., 1.] - module.ModelTag = "dipoleMapping" # update python name of test module + module.steeringMatrix = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + module.ModelTag = "dipoleMapping" # update python name of test module unitTestSim.AddModelToTask(unitTaskName, module) # Initialize DipoleRequestBodyMsg dipoleRequestBodyInMsgContainer = messaging.DipoleRequestBodyMsgPayload() - dipoleRequestBodyInMsgContainer.dipole_B = [1., 2., 3.] - dipoleRequestBodyInMsg = messaging.DipoleRequestBodyMsg().write(dipoleRequestBodyInMsgContainer) + dipoleRequestBodyInMsgContainer.dipole_B = [1.0, 2.0, 3.0] + dipoleRequestBodyInMsg = messaging.DipoleRequestBodyMsg().write( + dipoleRequestBodyInMsgContainer + ) # Initialize MTBArrayConfigMsg mtbArrayConfigParamsInMsgContainer = messaging.MTBArrayConfigMsgPayload() mtbArrayConfigParamsInMsgContainer.numMTB = 3 - mtbArrayConfigParamsInMsgContainer.maxMtbDipoles = [1E3, 1E3, 1E3] - mtbArrayConfigParamsInMsgContainer.GtMatrix_B = [1., 0., 0., 0., 1., 0., 0., 0., 1.] - mtbArrayConfigParamsInMsg = messaging.MTBArrayConfigMsg().write(mtbArrayConfigParamsInMsgContainer) + mtbArrayConfigParamsInMsgContainer.maxMtbDipoles = [1e3, 1e3, 1e3] + mtbArrayConfigParamsInMsgContainer.GtMatrix_B = [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + ] + mtbArrayConfigParamsInMsg = messaging.MTBArrayConfigMsg().write( + mtbArrayConfigParamsInMsgContainer + ) # Setup logging on the test module output message so that we get all the writes to it resultDipoleRequestMtbOutMsg = module.dipoleRequestMtbOutMsg.recorder() @@ -102,76 +123,104 @@ def dipoleMappingModuleTestFunction(): module.mtbArrayConfigParamsInMsg.subscribeTo(mtbArrayConfigParamsInMsg) # Set the simulation time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) # seconds to stop simulation unitTestSim.InitializeSimulation() - ''' + """ TEST 1: Check that dipoles is non-zero expected value with trivial steeringMatrix. - ''' + """ unitTestSim.ExecuteSimulation() - expectedDipole = [0.] * messaging.MAX_EFF_CNT - expectedDipole[0:3] = [1., 2., 3.] - testFailCount, testMessages = unitTestSupport.compareVector(expectedDipole, - resultDipoleRequestMtbOutMsg.mtbDipoleCmds[0], - accuracy, - "dipoles", - testFailCount, testMessages) - ''' + expectedDipole = [0.0] * messaging.MAX_EFF_CNT + expectedDipole[0:3] = [1.0, 2.0, 3.0] + testFailCount, testMessages = unitTestSupport.compareVector( + expectedDipole, + resultDipoleRequestMtbOutMsg.mtbDipoleCmds[0], + accuracy, + "dipoles", + testFailCount, + testMessages, + ) + """ TEST 2: Check that dipoles is non-zero with non-trivial steeringMatrix. - ''' - beta = 45. * np.pi / 180. - Gt = np.array([[np.cos(beta), -np.sin(beta)],[np.sin(beta), np.cos(beta)], [0., 0.]]) + """ + beta = 45.0 * np.pi / 180.0 + Gt = np.array( + [[np.cos(beta), -np.sin(beta)], [np.sin(beta), np.cos(beta)], [0.0, 0.0]] + ) GtInverse = np.linalg.pinv(Gt) mtbArrayConfigParamsInMsgContainer.numMTB = 2 - mtbArrayConfigParamsInMsgContainer.GtMatrix_B = [Gt[0, 0], Gt[0, 1], - Gt[1, 0], Gt[1, 1], - Gt[2, 0], Gt[2, 1]] - mtbArrayConfigParamsInMsg = messaging.MTBArrayConfigMsg().write(mtbArrayConfigParamsInMsgContainer) + mtbArrayConfigParamsInMsgContainer.GtMatrix_B = [ + Gt[0, 0], + Gt[0, 1], + Gt[1, 0], + Gt[1, 1], + Gt[2, 0], + Gt[2, 1], + ] + mtbArrayConfigParamsInMsg = messaging.MTBArrayConfigMsg().write( + mtbArrayConfigParamsInMsgContainer + ) module.mtbArrayConfigParamsInMsg.subscribeTo(mtbArrayConfigParamsInMsg) - module.steeringMatrix = [GtInverse[0, 0], GtInverse[0, 1], GtInverse[0, 2], - GtInverse[1, 0], GtInverse[1, 1], GtInverse[1, 2]] + module.steeringMatrix = [ + GtInverse[0, 0], + GtInverse[0, 1], + GtInverse[0, 2], + GtInverse[1, 0], + GtInverse[1, 1], + GtInverse[1, 2], + ] unitTestSim.InitializeSimulation() unitTestSim.ExecuteSimulation() - expectedDipole = [0.] * messaging.MAX_EFF_CNT + expectedDipole = [0.0] * messaging.MAX_EFF_CNT expectedDipole[0:2] = GtInverse @ np.array(dipoleRequestBodyInMsgContainer.dipole_B) - testFailCount, testMessages = unitTestSupport.compareVector(expectedDipole, - resultDipoleRequestMtbOutMsg.mtbDipoleCmds[0], - accuracy, - "dipoles", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareVector( + expectedDipole, + resultDipoleRequestMtbOutMsg.mtbDipoleCmds[0], + accuracy, + "dipoles", + testFailCount, + testMessages, + ) - ''' + """ TEST 3: Check that dipoles is zero with zero input dipole. - ''' - dipoleRequestBodyInMsgContainer.dipole_B = [0., 0., 0.] - dipoleRequestBodyInMsg = messaging.DipoleRequestBodyMsg().write(dipoleRequestBodyInMsgContainer) + """ + dipoleRequestBodyInMsgContainer.dipole_B = [0.0, 0.0, 0.0] + dipoleRequestBodyInMsg = messaging.DipoleRequestBodyMsg().write( + dipoleRequestBodyInMsgContainer + ) module.dipoleRequestBodyInMsg.subscribeTo(dipoleRequestBodyInMsg) unitTestSim.InitializeSimulation() unitTestSim.ExecuteSimulation() unitTestSim.ExecuteSimulation() - expectedDipole = [0.] * messaging.MAX_EFF_CNT - testFailCount, testMessages = unitTestSupport.compareVector(expectedDipole, - resultDipoleRequestMtbOutMsg.mtbDipoleCmds[0], - accuracy, - "dipoles", - testFailCount, testMessages) - - ''' + expectedDipole = [0.0] * messaging.MAX_EFF_CNT + testFailCount, testMessages = unitTestSupport.compareVector( + expectedDipole, + resultDipoleRequestMtbOutMsg.mtbDipoleCmds[0], + accuracy, + "dipoles", + testFailCount, + testMessages, + ) + + """ TEST 4: Check that Reset() zeros the output message. - ''' + """ # First set a non-zero dipole request - dipoleRequestBodyInMsgContainer.dipole_B = [1., 2., 3.] - dipoleRequestBodyInMsg = messaging.DipoleRequestBodyMsg().write(dipoleRequestBodyInMsgContainer) + dipoleRequestBodyInMsgContainer.dipole_B = [1.0, 2.0, 3.0] + dipoleRequestBodyInMsg = messaging.DipoleRequestBodyMsg().write( + dipoleRequestBodyInMsgContainer + ) module.dipoleRequestBodyInMsg.subscribeTo(dipoleRequestBodyInMsg) unitTestSim.InitializeSimulation() @@ -179,19 +228,22 @@ def dipoleMappingModuleTestFunction(): # Now call Reset() and verify output is zeroed module.Reset(0) # Pass 0 as currentSimNanos - expectedDipole = [0.] * messaging.MAX_EFF_CNT - testFailCount, testMessages = unitTestSupport.compareVector(np.array(expectedDipole), - np.array(module.dipoleRequestMtbOutMsg.read().mtbDipoleCmds), - 1e-3, - "Reset() zeroed dipole", - testFailCount, testMessages) + expectedDipole = [0.0] * messaging.MAX_EFF_CNT + testFailCount, testMessages = unitTestSupport.compareVector( + np.array(expectedDipole), + np.array(module.dipoleRequestMtbOutMsg.read().mtbDipoleCmds), + 1e-3, + "Reset() zeroed dipole", + testFailCount, + testMessages, + ) print("Accuracy used: " + str(accuracy)) if testFailCount == 0: print("PASSED: dipoleMapping unit test") else: print("Failed: dipoleMapping unit test") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/effectorInterfaces/forceTorqueThrForceMapping/_UnitTest/test_forceTorqueThrForceMapping.py b/src/fswAlgorithms/effectorInterfaces/forceTorqueThrForceMapping/_UnitTest/test_forceTorqueThrForceMapping.py index 9e0eec4b32..9fd4d4717d 100644 --- a/src/fswAlgorithms/effectorInterfaces/forceTorqueThrForceMapping/_UnitTest/test_forceTorqueThrForceMapping.py +++ b/src/fswAlgorithms/effectorInterfaces/forceTorqueThrForceMapping/_UnitTest/test_forceTorqueThrForceMapping.py @@ -36,36 +36,46 @@ def test_forceTorqueThrForceMapping1(): """ # Test 1 - No thrusters pointing in one direction, CoM offset - rcsLocationData = [[-0.86360, -0.82550, 1.79070], - [-0.82550, -0.86360, 1.79070], - [0.82550, 0.86360, 1.79070], - [0.86360, 0.82550, 1.79070], - [-0.86360, -0.82550, -1.79070], - [-0.82550, -0.86360, -1.79070], - [0.82550, 0.86360, -1.79070], - [0.86360, 0.82550, -1.79070]] - - rcsDirectionData = [[1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, -1.0, 0.0], - [-1.0, 0.0, 0.0], - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, -1.0, 0.0], - [-1.0, 0.0, 0.0]] + rcsLocationData = [ + [-0.86360, -0.82550, 1.79070], + [-0.82550, -0.86360, 1.79070], + [0.82550, 0.86360, 1.79070], + [0.86360, 0.82550, 1.79070], + [-0.86360, -0.82550, -1.79070], + [-0.82550, -0.86360, -1.79070], + [0.82550, 0.86360, -1.79070], + [0.86360, 0.82550, -1.79070], + ] + + rcsDirectionData = [ + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, -1.0, 0.0], + [-1.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, -1.0, 0.0], + [-1.0, 0.0, 0.0], + ] requested_torque = [0.4, 0.2, 0.4] - requested_force = [0.9, 1.1, 0.] + requested_force = [0.9, 1.1, 0.0] CoM_B = [0.1, 0.1, 0.1] - [testResults, testMessage] = forceTorqueThrForceMappingTestFunction(rcsLocationData, rcsDirectionData, - requested_torque, requested_force, CoM_B, - True) + [testResults, testMessage] = forceTorqueThrForceMappingTestFunction( + rcsLocationData, + rcsDirectionData, + requested_torque, + requested_force, + CoM_B, + True, + ) assert testResults < 1, testMessage + def test_forceTorqueThrForceMapping2(): r""" **Test Description** @@ -76,36 +86,46 @@ def test_forceTorqueThrForceMapping2(): """ # Test 1 - No thrusters pointing in one direction, CoM offset - rcsLocationData = [[-0.86360, -0.82550, 1.79070], - [-0.82550, -0.86360, 1.79070], - [0.82550, 0.86360, 1.79070], - [0.86360, 0.82550, 1.79070], - [-0.86360, -0.82550, -1.79070], - [-0.82550, -0.86360, -1.79070], - [0.82550, 0.86360, -1.79070], - [0.86360, 0.82550, -1.79070]] - - rcsDirectionData = [[1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, -1.0, 0.0], - [-1.0, 0.0, 0.0], - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, -1.0, 0.0], - [-1.0, 0.0, 0.0]] - - requested_force = [0.9, 1.1, 0.] + rcsLocationData = [ + [-0.86360, -0.82550, 1.79070], + [-0.82550, -0.86360, 1.79070], + [0.82550, 0.86360, 1.79070], + [0.86360, 0.82550, 1.79070], + [-0.86360, -0.82550, -1.79070], + [-0.82550, -0.86360, -1.79070], + [0.82550, 0.86360, -1.79070], + [0.86360, 0.82550, -1.79070], + ] + + rcsDirectionData = [ + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, -1.0, 0.0], + [-1.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, -1.0, 0.0], + [-1.0, 0.0, 0.0], + ] + + requested_force = [0.9, 1.1, 0.0] CoM_B = [0.1, 0.1, 0.1] requested_torque = [0.0, 0.0, 0.0] - [testResults, testMessage] = forceTorqueThrForceMappingTestFunction(rcsLocationData, rcsDirectionData, - requested_torque, requested_force, CoM_B, - True) + [testResults, testMessage] = forceTorqueThrForceMappingTestFunction( + rcsLocationData, + rcsDirectionData, + requested_torque, + requested_force, + CoM_B, + True, + ) assert testResults < 1, testMessage + def test_forceTorqueThrForceMapping3(): r""" **Test Description** @@ -116,33 +136,42 @@ def test_forceTorqueThrForceMapping3(): """ # Test 1 - No thrusters pointing in one direction, CoM offset - rcsLocationData = [[-0.86360, -0.82550, 1.79070], - [-0.82550, -0.86360, 1.79070], - [0.82550, 0.86360, 1.79070], - [0.86360, 0.82550, 1.79070], - [-0.86360, -0.82550, -1.79070], - [-0.82550, -0.86360, -1.79070], - [0.82550, 0.86360, -1.79070], - [0.86360, 0.82550, -1.79070]] - - rcsDirectionData = [[1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, -1.0, 0.0], - [-1.0, 0.0, 0.0], - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, -1.0, 0.0], - [-1.0, 0.0, 0.0]] - - requested_force = [0.9, 1.1, 0.] + rcsLocationData = [ + [-0.86360, -0.82550, 1.79070], + [-0.82550, -0.86360, 1.79070], + [0.82550, 0.86360, 1.79070], + [0.86360, 0.82550, 1.79070], + [-0.86360, -0.82550, -1.79070], + [-0.82550, -0.86360, -1.79070], + [0.82550, 0.86360, -1.79070], + [0.86360, 0.82550, -1.79070], + ] + + rcsDirectionData = [ + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, -1.0, 0.0], + [-1.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, -1.0, 0.0], + [-1.0, 0.0, 0.0], + ] + + requested_force = [0.9, 1.1, 0.0] CoM_B = [0.1, 0.1, 0.1] requested_torque = [0.0, 0.0, 0.0] - [testResults, testMessage] = forceTorqueThrForceMappingTestFunction(rcsLocationData, rcsDirectionData, - requested_torque, requested_force, CoM_B, - False) + [testResults, testMessage] = forceTorqueThrForceMappingTestFunction( + rcsLocationData, + rcsDirectionData, + requested_torque, + requested_force, + CoM_B, + False, + ) assert testResults < 1, testMessage @@ -156,39 +185,48 @@ def test_forceTorqueThrForceMapping4(): """ - rcsLocationData = [[-1, -1, 1], - [-1, -1, 1], - [-1, -1, 1], - [1, 1, 1], - [1, 1, 1], - [1, 1, 1], - [1, 1, -1], - [1, 1, -1], - [1, 1, -1], - [-1, -1, -1], - [-1, -1, -1], - [-1, -1, -1]] - - rcsDirectionData = [[1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, -1.0], - [0.0, 0.0, -1.0], - [0.0, -1.0, 0.0], - [-1.0, 0.0, 0.0], - [0.0, -1.0, 0.0], - [-1.0, 0.0, 0.0], - [0.0, 0.0, 1.0], - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, 1.0]] + rcsLocationData = [ + [-1, -1, 1], + [-1, -1, 1], + [-1, -1, 1], + [1, 1, 1], + [1, 1, 1], + [1, 1, 1], + [1, 1, -1], + [1, 1, -1], + [1, 1, -1], + [-1, -1, -1], + [-1, -1, -1], + [-1, -1, -1], + ] + + rcsDirectionData = [ + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, -1.0], + [0.0, 0.0, -1.0], + [0.0, -1.0, 0.0], + [-1.0, 0.0, 0.0], + [0.0, -1.0, 0.0], + [-1.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0], + ] CoM_B = [0.1, 0.1, 0.1] requested_torque = [0.0, 0.0, 0.0] - requested_force = [0.9, 1.1, 1.] - - [testResults, testMessage] = forceTorqueThrForceMappingTestFunction(rcsLocationData, rcsDirectionData, - requested_torque, requested_force, CoM_B, - True) + requested_force = [0.9, 1.1, 1.0] + + [testResults, testMessage] = forceTorqueThrForceMappingTestFunction( + rcsLocationData, + rcsDirectionData, + requested_torque, + requested_force, + CoM_B, + True, + ) assert testResults < 1, testMessage @@ -209,8 +247,10 @@ def calculate_force_torque(thruster_forces, locations, directions, CoM_B): return total_torque, total_force -def forceTorqueThrForceMappingTestFunction(rcsLocation, rcsDirection, requested_torque, requested_force, CoM_B, - torqueInMsgFlag): + +def forceTorqueThrForceMappingTestFunction( + rcsLocation, rcsDirection, requested_torque, requested_force, CoM_B, torqueInMsgFlag +): """Test method""" testFailCount = 0 testMessages = [] @@ -242,9 +282,9 @@ def forceTorqueThrForceMappingTestFunction(rcsLocation, rcsDirection, requested_ rcsLocationData = np.zeros((MAX_EFF_CNT, 3)) rcsDirectionData = np.zeros((MAX_EFF_CNT, 3)) - rcsLocationData[0:len(rcsLocation)] = rcsLocation + rcsLocationData[0 : len(rcsLocation)] = rcsLocation - rcsDirectionData[0:len(rcsLocation)] = rcsDirection + rcsDirectionData[0 : len(rcsLocation)] = rcsDirection fswSetupThrusters.clearSetup() for i in range(numThrusters): @@ -267,13 +307,15 @@ def forceTorqueThrForceMappingTestFunction(rcsLocation, rcsDirection, requested_ unitTestSim.ExecuteSimulation() # Get actual thruster forces - actual_thruster_forces = np.array(module.thrForceCmdOutMsg.read().thrForce[0:len(rcsLocation)]) + actual_thruster_forces = np.array( + module.thrForceCmdOutMsg.read().thrForce[0 : len(rcsLocation)] + ) actual_torque, actual_force = calculate_force_torque( actual_thruster_forces, np.array(rcsLocation), np.array(rcsDirection), - np.array(CoM_B) + np.array(CoM_B), ) # Print detailed comparison @@ -292,7 +334,9 @@ def forceTorqueThrForceMappingTestFunction(rcsLocation, rcsDirection, requested_ testMessages.append(f"FAILED: Thruster {i} has negative force: {force}\n") elif force > maxThrust + 1e-10: testFailCount += 1 - testMessages.append(f"FAILED: Thruster {i} exceeds max thrust ({maxThrust} N): {force}\n") + testMessages.append( + f"FAILED: Thruster {i} exceeds max thrust ({maxThrust} N): {force}\n" + ) # Test force/torque accuracy using compareVector tolerance = 1e-3 @@ -300,8 +344,13 @@ def forceTorqueThrForceMappingTestFunction(rcsLocation, rcsDirection, requested_ actual_torque_force = np.array([np.concatenate([actual_torque, actual_force])]) testFailCount, testMessages = unitTestSupport.compareVector( - requested_torque_force, actual_torque_force, tolerance, - "Force/Torque accuracy", testFailCount, testMessages) + requested_torque_force, + actual_torque_force, + tolerance, + "Force/Torque accuracy", + testFailCount, + testMessages, + ) # First set a non-zero force/torque request cmdTorqueInMsgData.torqueRequestBody = [1.0, 0.5, 0.7] @@ -317,12 +366,15 @@ def forceTorqueThrForceMappingTestFunction(rcsLocation, rcsDirection, requested_ # Now call Reset() and verify output is zeroed module.Reset(0) # Pass 0 as currentSimNanos - expectedForce = [0.] * messaging.MAX_EFF_CNT - testFailCount, testMessages = unitTestSupport.compareVector(np.array(expectedForce), - np.array(module.thrForceCmdOutMsg.read().thrForce), - tolerance, - "Reset() zeroed thruster force", - testFailCount, testMessages) + expectedForce = [0.0] * messaging.MAX_EFF_CNT + testFailCount, testMessages = unitTestSupport.compareVector( + np.array(expectedForce), + np.array(module.thrForceCmdOutMsg.read().thrForce), + tolerance, + "Reset() zeroed thruster force", + testFailCount, + testMessages, + ) print(f"Accuracy used: {tolerance}") diff --git a/src/fswAlgorithms/effectorInterfaces/hingedRigidBodyPIDMotor/_UnitTest/test_hingedRigidBodyPIDMotor.py b/src/fswAlgorithms/effectorInterfaces/hingedRigidBodyPIDMotor/_UnitTest/test_hingedRigidBodyPIDMotor.py index 3586b30f07..5b930f60f3 100644 --- a/src/fswAlgorithms/effectorInterfaces/hingedRigidBodyPIDMotor/_UnitTest/test_hingedRigidBodyPIDMotor.py +++ b/src/fswAlgorithms/effectorInterfaces/hingedRigidBodyPIDMotor/_UnitTest/test_hingedRigidBodyPIDMotor.py @@ -32,17 +32,20 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import hingedRigidBodyPIDMotor # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import ( + hingedRigidBodyPIDMotor, +) # import the module that is to be tested from Basilisk.utilities import macros -from Basilisk.architecture import messaging # import the message definitions +from Basilisk.architecture import messaging # import the message definitions from Basilisk.architecture import bskLogging @@ -55,18 +58,19 @@ # of the multiple test runs for this test. Note that the order in that you add the parametrize method # matters for the documentation in that it impacts the order in which the test arguments are shown. # The first parametrize arguments are shown last in the pytest argument list -@pytest.mark.parametrize("thetaR", [np.pi/2, np.pi/6]) +@pytest.mark.parametrize("thetaR", [np.pi / 2, np.pi / 6]) @pytest.mark.parametrize("thetaDotR", [0.1, 0.2]) -@pytest.mark.parametrize("theta", [7*np.pi/6, 3*np.pi/2]) +@pytest.mark.parametrize("theta", [7 * np.pi / 6, 3 * np.pi / 2]) @pytest.mark.parametrize("thetaDot", [-0.1, -0.5]) @pytest.mark.parametrize("K", [0, 2]) @pytest.mark.parametrize("P", [0, 5]) @pytest.mark.parametrize("I", [0, 1]) @pytest.mark.parametrize("accuracy", [1e-8]) - # update "module" in this function name to reflect the module name -def test_hingedRigidBodyPIDMotor(show_plots, thetaR, thetaDotR, theta, thetaDot, K, P, I, accuracy): +def test_hingedRigidBodyPIDMotor( + show_plots, thetaR, thetaDotR, theta, thetaDot, K, P, I, accuracy +): r""" **Validation Test Description** @@ -96,23 +100,26 @@ def test_hingedRigidBodyPIDMotor(show_plots, thetaR, thetaDotR, theta, thetaDot, only checks the output at the first time step, for which the integral term does not contribute. """ # each test method requires a single assert method to be called - [testResults, testMessage] = hingedRigidBodyPIDMotorTestFunction(show_plots, thetaR, thetaDotR, theta, thetaDot, K, P, I, accuracy) + [testResults, testMessage] = hingedRigidBodyPIDMotorTestFunction( + show_plots, thetaR, thetaDotR, theta, thetaDot, K, P, I, accuracy + ) assert testResults < 1, testMessage -def hingedRigidBodyPIDMotorTestFunction(show_plots, thetaR, thetaDotR, theta, thetaDot, K, P, I, accuracy): - - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) +def hingedRigidBodyPIDMotorTestFunction( + show_plots, thetaR, thetaDotR, theta, thetaDot, K, P, I, accuracy +): + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(1) # update process rate update time + testProcessRate = macros.sec2nano(1) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -149,7 +156,7 @@ def hingedRigidBodyPIDMotorTestFunction(show_plots, thetaR, thetaDotR, theta, th # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -159,11 +166,17 @@ def hingedRigidBodyPIDMotorTestFunction(show_plots, thetaR, thetaDotR, theta, th # compare the module results to the truth values if not unitTestSupport.isDoubleEqual(dataLog.motorTorque[0][0], T, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + motor.ModelTag + " module failed unit test for thetaR = {}, thetaDotR = {}, theta = {}, thetaDot = {}, K = {}, P = {} \n".format(thetaR, thetaDotR, theta, thetaDot, K, P)) + testMessages.append( + "FAILED: " + + motor.ModelTag + + " module failed unit test for thetaR = {}, thetaDotR = {}, theta = {}, thetaDot = {}, K = {}, P = {} \n".format( + thetaR, thetaDotR, theta, thetaDot, K, P + ) + ) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -172,13 +185,13 @@ def hingedRigidBodyPIDMotorTestFunction(show_plots, thetaR, thetaDotR, theta, th # if __name__ == "__main__": test_hingedRigidBodyPIDMotor( - False, - np.pi/3, - 0, - np.pi/2, - 0.1, - 1, - 5, - 0, - 1e-12 # accuracy - ) + False, + np.pi / 3, + 0, + np.pi / 2, + 0.1, + 1, + 5, + 0, + 1e-12, # accuracy + ) diff --git a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/_UnitTest/test_prescribedRot2DOF.py b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/_UnitTest/test_prescribedRot2DOF.py index e12f09618a..5e240f91d7 100755 --- a/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/_UnitTest/test_prescribedRot2DOF.py +++ b/src/fswAlgorithms/effectorInterfaces/prescribedRot2DOF/_UnitTest/test_prescribedRot2DOF.py @@ -31,7 +31,9 @@ import os from Basilisk.architecture import bskLogging from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import prescribedRot2DOF # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + prescribedRot2DOF, +) # import the module that is to be tested from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros @@ -39,18 +41,28 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) + # Parametrize the user-configurable variables @pytest.mark.parametrize("thetaInit", [0.01]) -@pytest.mark.parametrize("thetaRef1a", [0.0, 2*np.pi/3]) # Rotation 1 -@pytest.mark.parametrize("thetaRef2a", [np.pi/3, 2*np.pi/3]) # Rotation 1 -@pytest.mark.parametrize("thetaRef1b", [0.0, 2*np.pi/3]) # Rotation 2 -@pytest.mark.parametrize("thetaRef2b", [np.pi/3, 2*np.pi/3]) # Rotation 2 +@pytest.mark.parametrize("thetaRef1a", [0.0, 2 * np.pi / 3]) # Rotation 1 +@pytest.mark.parametrize("thetaRef2a", [np.pi / 3, 2 * np.pi / 3]) # Rotation 1 +@pytest.mark.parametrize("thetaRef1b", [0.0, 2 * np.pi / 3]) # Rotation 2 +@pytest.mark.parametrize("thetaRef2b", [np.pi / 3, 2 * np.pi / 3]) # Rotation 2 @pytest.mark.parametrize("phiDDotMax", [0.004]) @pytest.mark.parametrize("accuracy", [1e-5]) -def test_PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, thetaRef1b, thetaRef2b, phiDDotMax, accuracy): +def test_PrescribedRot2DOFTestFunction( + show_plots, + thetaInit, + thetaRef1a, + thetaRef2a, + thetaRef1b, + thetaRef2b, + phiDDotMax, + accuracy, +): r""" **Validation Test Description** @@ -83,12 +95,30 @@ def test_PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaR to the reference angular velocity magnitude, ``thetaDot_Ref``. """ - [testResults, testMessage] = PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, thetaRef1b, thetaRef2b, phiDDotMax, accuracy) + [testResults, testMessage] = PrescribedRot2DOFTestFunction( + show_plots, + thetaInit, + thetaRef1a, + thetaRef2a, + thetaRef1b, + thetaRef2b, + phiDDotMax, + accuracy, + ) assert testResults < 1, testMessage -def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, thetaRef1b, thetaRef2b, phiDDotMax, accuracy): +def PrescribedRot2DOFTestFunction( + show_plots, + thetaInit, + thetaRef1a, + thetaRef2a, + thetaRef1b, + thetaRef2b, + phiDDotMax, + accuracy, +): """Call this routine directly to run the unit test.""" testFailCount = 0 testMessages = [] @@ -100,7 +130,7 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, unitTestSim = SimulationBaseClass.SimBaseClass() # Create the test thread - testProcessRate = macros.sec2nano(0.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -109,14 +139,24 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, prescribedRot2DOFObj.ModelTag = "PrescribedRot2DOF" # Initialize the test module configuration data - rotAxis1_M = np.array([0.0, 1.0, 0.0]) # Rotation axis for the first reference rotation angle, thetaRef1a - rotAxis2_P1 = np.array([0.0, 0.0, 1.0]) # Rotation axis for the second reference rotation angle, thetaRef2a + rotAxis1_M = np.array( + [0.0, 1.0, 0.0] + ) # Rotation axis for the first reference rotation angle, thetaRef1a + rotAxis2_P1 = np.array( + [0.0, 0.0, 1.0] + ) # Rotation axis for the second reference rotation angle, thetaRef2a prescribedRot2DOFObj.rotAxis1_M = rotAxis1_M prescribedRot2DOFObj.rotAxis2_P1 = rotAxis2_P1 prescribedRot2DOFObj.phiDDotMax = phiDDotMax - prescribedRot2DOFObj.omega_PM_P = np.array([0.0, 0.0, 0.0]) # [rad/s] Angular velocity of frame P relative to frame M in P frame components - prescribedRot2DOFObj.omegaPrime_PM_P = np.array([0.0, 0.0, 0.0]) # [rad/s^2] B frame time derivative of omega_FB_F in P frame components - prescribedRot2DOFObj.sigma_PM = np.array([0.0, 0.0, 0.0]) # MRP attitude of frame P relative to frame M + prescribedRot2DOFObj.omega_PM_P = np.array( + [0.0, 0.0, 0.0] + ) # [rad/s] Angular velocity of frame P relative to frame M in P frame components + prescribedRot2DOFObj.omegaPrime_PM_P = np.array( + [0.0, 0.0, 0.0] + ) # [rad/s^2] B frame time derivative of omega_FB_F in P frame components + prescribedRot2DOFObj.sigma_PM = np.array( + [0.0, 0.0, 0.0] + ) # MRP attitude of frame P relative to frame M # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, prescribedRot2DOFObj) @@ -129,8 +169,12 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, hingedRigidBodyMessageData2.theta = thetaRef2a hingedRigidBodyMessageData1.thetaDot = thetaDot_Ref hingedRigidBodyMessageData2.thetaDot = thetaDot_Ref - HingedRigidBodyMessage1 = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData1) - HingedRigidBodyMessage2 = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData2) + HingedRigidBodyMessage1 = messaging.HingedRigidBodyMsg().write( + hingedRigidBodyMessageData1 + ) + HingedRigidBodyMessage2 = messaging.HingedRigidBodyMsg().write( + hingedRigidBodyMessageData2 + ) prescribedRot2DOFObj.spinningBodyRef1InMsg.subscribeTo(HingedRigidBodyMessage1) prescribedRot2DOFObj.spinningBodyRef2InMsg.subscribeTo(HingedRigidBodyMessage2) @@ -146,11 +190,21 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, unitTestSim.InitializeSimulation() # Calculate the two reference PRVs for the first rotation - prv_P0M_a = thetaRef1a * rotAxis1_M[0], thetaRef1a * rotAxis1_M[1], thetaRef1a * rotAxis1_M[2] - prv_P1P0_a = thetaRef2a * rotAxis2_P1[0], thetaRef2a * rotAxis2_P1[1], thetaRef2a * rotAxis2_P1[2] + prv_P0M_a = ( + thetaRef1a * rotAxis1_M[0], + thetaRef1a * rotAxis1_M[1], + thetaRef1a * rotAxis1_M[2], + ) + prv_P1P0_a = ( + thetaRef2a * rotAxis2_P1[0], + thetaRef2a * rotAxis2_P1[1], + thetaRef2a * rotAxis2_P1[2], + ) # Calculate a single reference PRV for the first rotation and the associated MRP attitude - if (thetaRef1a == 0 and thetaRef2a == 0): # Prevent a (0,0,0) error using rbk.addPRV() + if ( + thetaRef1a == 0 and thetaRef2a == 0 + ): # Prevent a (0,0,0) error using rbk.addPRV() prv_P1M_a = np.array([0.0, 0.0, 0.0]) phi_P1M_a = 0.0 sigma_PM_Ref1 = np.array([0.0, 0.0, 0.0]) @@ -171,11 +225,21 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, sigma_PM_Final1 = sigma_PM_FirstMan[-1, :] # Calculate the two reference PRVs for the second rotation - prv_P2M_b = thetaRef1b * rotAxis1_M[0], thetaRef1b * rotAxis1_M[1], thetaRef1b * rotAxis1_M[2] - prv_P3P2_b = thetaRef2b * rotAxis2_P1[0], thetaRef2b * rotAxis2_P1[1], thetaRef2b * rotAxis2_P1[2] + prv_P2M_b = ( + thetaRef1b * rotAxis1_M[0], + thetaRef1b * rotAxis1_M[1], + thetaRef1b * rotAxis1_M[2], + ) + prv_P3P2_b = ( + thetaRef2b * rotAxis2_P1[0], + thetaRef2b * rotAxis2_P1[1], + thetaRef2b * rotAxis2_P1[2], + ) # Calculate a single reference PRV (prv_P3M_b) for the second rotation beginning from the M frame - if (thetaRef1b == 0 and thetaRef2b == 0): # Prevent a (0,0,0) error using rbk.addPRV() + if ( + thetaRef1b == 0 and thetaRef2b == 0 + ): # Prevent a (0,0,0) error using rbk.addPRV() prv_P3M_b = np.array([0.0, 0.0, 0.0]) else: prv_P3M_b = rbk.addPRV(prv_P2M_b, prv_P3P2_b) @@ -197,8 +261,12 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, hingedRigidBodyMessageData2.theta = thetaRef2b hingedRigidBodyMessageData1.thetaDot = thetaDot_Ref hingedRigidBodyMessageData2.thetaDot = thetaDot_Ref - HingedRigidBodyMessage1 = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData1, macros.sec2nano(simTime1)) - HingedRigidBodyMessage2 = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData2, macros.sec2nano(simTime1)) + HingedRigidBodyMessage1 = messaging.HingedRigidBodyMsg().write( + hingedRigidBodyMessageData1, macros.sec2nano(simTime1) + ) + HingedRigidBodyMessage2 = messaging.HingedRigidBodyMsg().write( + hingedRigidBodyMessageData2, macros.sec2nano(simTime1) + ) prescribedRot2DOFObj.spinningBodyRef1InMsg.subscribeTo(HingedRigidBodyMessage1) prescribedRot2DOFObj.spinningBodyRef2InMsg.subscribeTo(HingedRigidBodyMessage2) @@ -233,13 +301,15 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, # Plot omega_FB_F plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, omega_PM_P[:, 0], label=r'$\omega_{1}$') - plt.plot(timespan * macros.NANO2SEC, omega_PM_P[:, 1], label=r'$\omega_{2}$') - plt.plot(timespan * macros.NANO2SEC, omega_PM_P[:, 2], label=r'$\omega_{3}$') - plt.title(r'Prescribed Angular Velocity ${}^\mathcal{P} \omega_{\mathcal{P}/\mathcal{M}}$') - plt.xlabel('Time (s)') - plt.ylabel('(rad/s)') - plt.legend(loc='upper right', prop={'size': 12}) + plt.plot(timespan * macros.NANO2SEC, omega_PM_P[:, 0], label=r"$\omega_{1}$") + plt.plot(timespan * macros.NANO2SEC, omega_PM_P[:, 1], label=r"$\omega_{2}$") + plt.plot(timespan * macros.NANO2SEC, omega_PM_P[:, 2], label=r"$\omega_{3}$") + plt.title( + r"Prescribed Angular Velocity ${}^\mathcal{P} \omega_{\mathcal{P}/\mathcal{M}}$" + ) + plt.xlabel("Time (s)") + plt.ylabel("(rad/s)") + plt.legend(loc="upper right", prop={"size": 12}) # Plot phi thetaRef1_plotting = np.ones(len(timespan)) * phi_P1M_a @@ -247,22 +317,26 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, thetaInit_plotting = np.ones(len(timespan)) * thetaInit plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, phi, label=r'$\Phi$') - plt.plot(timespan * macros.NANO2SEC, thetaInit_plotting, '--', label=r'$\Phi_{0}$') - plt.plot(timespan * macros.NANO2SEC, thetaRef1_plotting, '--', label=r'$\Phi_{1_{Ref}}$') - plt.plot(timespan * macros.NANO2SEC, thetaRef2_plotting, '--', label=r'$\Phi_{2_{Ref}}$') - plt.title(r'Prescribed Principal Rotation Vector (PRV) Angles $\Phi$') - plt.xlabel('Time (s)') - plt.ylabel('(rad)') - plt.legend(loc='upper right', prop={'size': 12}) + plt.plot(timespan * macros.NANO2SEC, phi, label=r"$\Phi$") + plt.plot(timespan * macros.NANO2SEC, thetaInit_plotting, "--", label=r"$\Phi_{0}$") + plt.plot( + timespan * macros.NANO2SEC, thetaRef1_plotting, "--", label=r"$\Phi_{1_{Ref}}$" + ) + plt.plot( + timespan * macros.NANO2SEC, thetaRef2_plotting, "--", label=r"$\Phi_{2_{Ref}}$" + ) + plt.title(r"Prescribed Principal Rotation Vector (PRV) Angles $\Phi$") + plt.xlabel("Time (s)") + plt.ylabel("(rad)") + plt.legend(loc="upper right", prop={"size": 12}) # Plot the accumulated PRV angle plt.figure() plt.clf() plt.plot(timespan * macros.NANO2SEC, phiAccum) - plt.title(r'Accumulated Principal Rotation Vector (PRV) Angle $\Phi$') - plt.xlabel('Time (s)') - plt.ylabel('(rad)') + plt.title(r"Accumulated Principal Rotation Vector (PRV) Angle $\Phi$") + plt.xlabel("Time (s)") + plt.ylabel("(rad)") if show_plots: plt.show() @@ -271,7 +345,11 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, # Compare the reference and simulated data and output failure messages as necessary if not unitTestSupport.isDoubleEqual(thetaDot_Final, thetaDot_Ref, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + prescribedRot2DOFObj.ModelTag + " thetaDot_Final and thetaDot_Ref do not match") + testMessages.append( + "FAILED: " + + prescribedRot2DOFObj.ModelTag + + " thetaDot_Final and thetaDot_Ref do not match" + ) print("thetaDot_Final: ") print(thetaDot_Final) print("thetaDot_Ref: ") @@ -279,7 +357,11 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, if not unitTestSupport.isArrayEqual(sigma_PM_Final1, sigma_PM_Ref1, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + prescribedRot2DOFObj.ModelTag + " MRPs sigma_PM_Final1 and sigma_PM_Ref1 do not match") + testMessages.append( + "FAILED: " + + prescribedRot2DOFObj.ModelTag + + " MRPs sigma_PM_Final1 and sigma_PM_Ref1 do not match" + ) print("sigma_PM_Final1: ") print(sigma_PM_Final1) print("sigma_PM_Ref1: ") @@ -287,13 +369,17 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, if not unitTestSupport.isArrayEqual(sigma_PM_Final2, sigma_PM_Ref2, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + prescribedRot2DOFObj.ModelTag + " MRPs sigma_PM_Final2 and sigma_PM_Ref2 do not match") + testMessages.append( + "FAILED: " + + prescribedRot2DOFObj.ModelTag + + " MRPs sigma_PM_Final2 and sigma_PM_Ref2 do not match" + ) print("sigma_PM_Final2: ") print(sigma_PM_Final2) print("sigma_PM_Ref2: ") print(sigma_PM_Ref2) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -302,12 +388,12 @@ def PrescribedRot2DOFTestFunction(show_plots, thetaInit, thetaRef1a, thetaRef2a, # if __name__ == "__main__": PrescribedRot2DOFTestFunction( - True, - 0.0, # thetaInit - 2 * np.pi / 3, # thetaRef1a - np.pi / 6, # thetaRef2a - 0.0, # thetaRef1b - 2 * np.pi / 3, # thetaRef2b - 0.008, # phiDDotMax - 1e-5 # accuracy - ) + True, + 0.0, # thetaInit + 2 * np.pi / 3, # thetaRef1a + np.pi / 6, # thetaRef2a + 0.0, # thetaRef1b + 2 * np.pi / 3, # thetaRef2b + 0.008, # phiDDotMax + 1e-5, # accuracy + ) diff --git a/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/Support/results_rwMotorTorque.py b/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/Support/results_rwMotorTorque.py index d01fbbe29b..e643c899de 100644 --- a/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/Support/results_rwMotorTorque.py +++ b/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/Support/results_rwMotorTorque.py @@ -21,67 +21,59 @@ def controlAxes3D(): - C = np.array([ - [1.0, 0.0, 0.0] - , [0.0, 1.0, 0.0] - , [0.0, 0.0, 1.0] - ]) + C = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) return C + + def controlAxes2D(): - C = np.array([ - [1.0, 0.0, 0.0] - , [0.0, 0.0, 1.0] - ]) + C = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]) return C + + def controlAxes1D(): - C = np.array([ - [1.0, 0.0, 0.0] - ]) + C = np.array([[1.0, 0.0, 0.0]]) return C - def computeTorqueU(CArray, Gs_B, Lr, availMsg): - - numControlAxes = len(CArray)//3 + numControlAxes = len(CArray) // 3 numWheels = len(availMsg) nonAvailWheels = 0 # Build Control Frame (doesn't need to be a complete frame) - C = np.zeros((3,3)) + C = np.zeros((3, 3)) for i in range(3): if numControlAxes > i: - C[i,:] = CArray[3*i:3*(i+1)] + C[i, :] = CArray[3 * i : 3 * (i + 1)] else: - C[i,:] = [0.0, 0.0, 0.0] + C[i, :] = [0.0, 0.0, 0.0] # Remove wheels that are deemed unavailable - for i in range(len(Gs_B[0])): # + for i in range(len(Gs_B[0])): # if numWheels > i: if availMsg[i] is not rwMotorTorque.AVAILABLE: - Gs_B[:,i] = [0.0, 0.0, 0.0] + Gs_B[:, i] = [0.0, 0.0, 0.0] nonAvailWheels += 1 else: - Gs_B[:,i] = [0.0, 0.0, 0.0] + Gs_B[:, i] = [0.0, 0.0, 0.0] # If fewer wheels than number of control axes, output no torque - if (numWheels-nonAvailWheels) < numControlAxes: - return [0.0]*len(Gs_B[0]) + if (numWheels - nonAvailWheels) < numControlAxes: + return [0.0] * len(Gs_B[0]) - - Lr_C = np.dot(C,Lr) # Project torque onto control axes - CGs = np.dot(C, Gs_B) # Map the control axes onto the wheels + Lr_C = np.dot(C, Lr) # Project torque onto control axes + CGs = np.dot(C, Gs_B) # Map the control axes onto the wheels # Build minimum norm framework M = np.dot(CGs, CGs.T) - M_rep = np.identity(3) # Need to keep the matrix non-singular for inversion - for i in range(0,numControlAxes): - for j in range(0,numControlAxes): + M_rep = np.identity(3) # Need to keep the matrix non-singular for inversion + for i in range(0, numControlAxes): + for j in range(0, numControlAxes): M_rep[i][j] = M[i][j] M_inv = la.inv(M_rep) # Remove projection to any non-defined control axes - for i in range(numControlAxes,3): + for i in range(numControlAxes, 3): M_inv[i][i] = 0.0 # Determine the solution @@ -92,13 +84,16 @@ def computeTorqueU(CArray, Gs_B, Lr, availMsg): return -u_s + def exampleComputation(): - Gs_B = np.array([ - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, 1.0], - [0.5773502691896258, 0.5773502691896258, 0.5773502691896258] - ]).T + Gs_B = np.array( + [ + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0], + [0.5773502691896258, 0.5773502691896258, 0.5773502691896258], + ] + ).T JsList = np.array([0.1, 0.1, 0.1, 0.1]) numRW = 4 @@ -107,14 +102,14 @@ def exampleComputation(): Lr = np.array([1.0, -0.5, 0.7]) rwAvailability = np.array([1, 1, 1, 1]) - print('3D Control') + print("3D Control") u_s = computeTorqueU(controlAxes3D(), Gs_B, Lr) - print('U_s = ', u_s, '\n') + print("U_s = ", u_s, "\n") - print('2D Control') + print("2D Control") u_s = computeTorqueU(controlAxes2D(), Gs_B, Lr) - print('U_s = ', u_s) + print("U_s = ", u_s) - print('1D Control') + print("1D Control") u_s = computeTorqueU(controlAxes1D(), Gs_B, Lr) - print('U_s = ', u_s) + print("U_s = ", u_s) diff --git a/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorque.py b/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorque.py index 41573d9799..39eb242f6b 100644 --- a/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorque.py +++ b/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorque.py @@ -24,10 +24,13 @@ from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import rwMotorTorque + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # Uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed. @@ -38,6 +41,7 @@ # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. + # update "module" in this function name to reflect the module name def test_rwMotorTorque(show_plots): """Module Unit Test""" @@ -47,16 +51,16 @@ def test_rwMotorTorque(show_plots): def rwMotorTorqueTest(show_plots): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -65,31 +69,37 @@ def rwMotorTorqueTest(show_plots): module.ModelTag = "rwMotorTorque" # Initialize module variables - controlAxes_B = [ - 1,0,0 - ,0,1,0 - ,0,0,1 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] module.controlAxes_B = controlAxes_B - # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) - # attControl message - inputMessageData = messaging.CmdTorqueBodyMsgPayload() # Create a structure for the input message - requestedTorque = [1.0, -0.5, 0.7] # Set up a list as a 3-vector - inputMessageData.torqueRequestBody = requestedTorque # write torque request to input message + inputMessageData = ( + messaging.CmdTorqueBodyMsgPayload() + ) # Create a structure for the input message + requestedTorque = [1.0, -0.5, 0.7] # Set up a list as a 3-vector + inputMessageData.torqueRequestBody = ( + requestedTorque # write torque request to input message + ) cmdTorqueInMsg = messaging.CmdTorqueBodyMsg().write(inputMessageData) # wheelConfigData message rwConfigParams = messaging.RWArrayConfigMsgPayload() rwConfigParams.GsMatrix_B = [ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - 0.5773502691896258, 0.5773502691896258, 0.5773502691896258 + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.5773502691896258, + 0.5773502691896258, + 0.5773502691896258, ] rwConfigParams.JsList = [0.1, 0.1, 0.1, 0.1] rwConfigParams.numRW = 4 @@ -97,7 +107,12 @@ def rwMotorTorqueTest(show_plots): # wheelAvailability message rwAvailabilityMessage = messaging.RWAvailabilityMsgPayload() - avail = [messaging.AVAILABLE, messaging.AVAILABLE, messaging.AVAILABLE, messaging.AVAILABLE] + avail = [ + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + messaging.AVAILABLE, + ] rwAvailabilityMessage.wheelAvailability = avail rwAvailInMsg = messaging.RWAvailabilityMsg().write(rwAvailabilityMessage) @@ -119,7 +134,7 @@ def rwMotorTorqueTest(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -130,22 +145,25 @@ def rwMotorTorqueTest(show_plots): # print('\n', moduleOutput) # set the output truth states - ans = [0]*messaging.MAX_EFF_CNT + ans = [0] * messaging.MAX_EFF_CNT ans[0:4] = [-0.8, 0.7000000000000001, -0.5, -0.3464101615137755] - trueVector = [ - ans, - ans - ] + trueVector = [ans, ans] # compare the module results to the truth values accuracy = 1e-12 - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleOutput[i], trueVector[i], rwConfigParams.numRW, accuracy): + if not unitTestSupport.isArrayEqual( + moduleOutput[i], trueVector[i], rwConfigParams.numRW, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed motorTorque unit test at t=" + - str(dataLog.times()[i]*macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed motorTorque unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) # print out success message if no error were found if testFailCount == 0: @@ -155,7 +173,7 @@ def rwMotorTorqueTest(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorqueParametrized.py b/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorqueParametrized.py index 5155146882..237c197245 100644 --- a/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorqueParametrized.py +++ b/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorqueParametrized.py @@ -33,8 +33,6 @@ path = os.path.dirname(os.path.abspath(filename)) - - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import unitTestSupport @@ -43,6 +41,7 @@ from Basilisk.architecture import messaging from Support import results_rwMotorTorque + # Uncomment this line is 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. @@ -52,67 +51,59 @@ # of the multiple test runs for this test. @pytest.mark.parametrize("numControlAxes", [0, 1, 2, 3]) @pytest.mark.parametrize("numWheels", [2, 4, messaging.MAX_EFF_CNT]) -@pytest.mark.parametrize("RWAvailMsg",["NO", "ON", "OFF", "MIXED"]) - +@pytest.mark.parametrize("RWAvailMsg", ["NO", "ON", "OFF", "MIXED"]) # update "module" in this function name to reflect the module name def test_rwMotorTorque(show_plots, numControlAxes, numWheels, RWAvailMsg): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = rwMotorTorqueTest(show_plots, numControlAxes, numWheels, RWAvailMsg) + [testResults, testMessage] = rwMotorTorqueTest( + show_plots, numControlAxes, numWheels, RWAvailMsg + ) assert testResults < 1, testMessage def rwMotorTorqueTest(show_plots, numControlAxes, numWheels, RWAvailMsg): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = rwMotorTorque.rwMotorTorque() module.ModelTag = "rwMotorTorque" - # Initialize module variables if numControlAxes == 3: - controlAxes_B = [ - 1, 0, 0 - , 0, 1, 0 - , 0, 0, 1 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] elif numControlAxes == 2: - controlAxes_B = [ - 1,0,0 - ,0,1,0 - ] + controlAxes_B = [1, 0, 0, 0, 1, 0] elif numControlAxes == 1: - controlAxes_B = [ - 1, 0, 0 - ] + controlAxes_B = [1, 0, 0] else: controlAxes_B = [] module.controlAxes_B = controlAxes_B - # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) - # attControl message - inputMessageData = messaging.CmdTorqueBodyMsgPayload() # Create a structure for the input message - requestedTorque = [1.0, -0.5, 0.7] # Set up a list as a 3-vector - inputMessageData.torqueRequestBody = requestedTorque # write torque request to input message + inputMessageData = ( + messaging.CmdTorqueBodyMsgPayload() + ) # Create a structure for the input message + requestedTorque = [1.0, -0.5, 0.7] # Set up a list as a 3-vector + inputMessageData.torqueRequestBody = ( + requestedTorque # write torque request to input message + ) cmdTorqueInMsg = messaging.CmdTorqueBodyMsg().write(inputMessageData) # wheelConfigData message @@ -121,50 +112,131 @@ def rwMotorTorqueTest(show_plots, numControlAxes, numWheels, RWAvailMsg): if numWheels == MAX_EFF_CNT: rwConfigParams.GsMatrix_B = [ - 0.4835867893995201, 0.7025829597277155, 0.5220354411517549, - 0.6274167231454653, 0.4634123147571517, 0.6257773422303058, - 0.4927675437195689, 0.3909468277672152, 0.7773935462269635, - 0.2791305379092009, 0.20278639222840245, 0.9385967301954065, - 0.1742148051521812, 0.9353106472878886, 0.3079662233682429, - 0.7408864742367625, 0.30733781515416325, 0.5971856492492805, - 0.49166240509756476, 0.11024265612126483, 0.863779275153674, - 0.08522980139648922, 0.5635691254043687, 0.8216603445736381, - 0.5169183283391889, 0.6482094982986043, 0.5591242153068406, - 0.5539478507672101, 0.4352935184619988, 0.7096910112262675, - 0.08177103922211226, 0.7185493168899821, 0.6906521384470449, - 0.5424303480563135, 0.8034905566669417, 0.24530031156636306, - 0.6791649825098244, 0.25103926707369056, 0.6897203874901293, - 0.6662787689368599, 0.6695372377111813, 0.32831766535181106, - 0.28428078464167594, 0.5440295499812461, 0.7894404880867942, - 0.8881073966834958, 0.007176386091829566, 0.4595799728433832, - 0.7043700914244455, 0.20398698108861654, 0.6798912308987893, - 0.5913513581668906, 0.7154722881784563, 0.3720255045596441, - 0.5353927164036736, 0.8292977052562882, 0.1600623480977027, - 0.5626385603464779, 0.5530980227747188, 0.6144269099038059, - 0.8047402627946283, 0.5179828986694456, 0.2899772855298006, - 0.6435726414836709, 0.49863310510036174, 0.5806714059015666, - 0.2533767502100278, 0.8066673674024603, 0.533936307831739, - 0.051675625147813466, 0.741898369799065, 0.6685180914942186, - 0.6705007071467579, 0.243658731626882, 0.700756180292173, - 0.6124322825812726, 0.6044312394389204, 0.5094993386086216, - 0.5025822950964116, 0.49662160344788164, 0.7076567103083798, - 0.4875326918964735, 0.8575174427431412, 0.16424283766253403, - 0.3659744927810267, 0.8415919620749859, 0.39722240622155974, - 0.6205921515961875, 0.5508152351685801, 0.5580931446303532, - 0.20125257120061574, 0.7022636474963218, 0.6828785924235018, - 0.4318909377763495, 0.6786025351852008, 0.5941117883924572, - 0.6839787443692367, 0.6598940110591041, 0.31098709204629277, - 0.35743175000357147, 0.8343049491885353, 0.4197353878920623, - 0.8124751056450826, 0.35669421673672336, 0.46114362020262967, - 0.04721328350343224, 0.8901899787392832, 0.45313652204714083] + 0.4835867893995201, + 0.7025829597277155, + 0.5220354411517549, + 0.6274167231454653, + 0.4634123147571517, + 0.6257773422303058, + 0.4927675437195689, + 0.3909468277672152, + 0.7773935462269635, + 0.2791305379092009, + 0.20278639222840245, + 0.9385967301954065, + 0.1742148051521812, + 0.9353106472878886, + 0.3079662233682429, + 0.7408864742367625, + 0.30733781515416325, + 0.5971856492492805, + 0.49166240509756476, + 0.11024265612126483, + 0.863779275153674, + 0.08522980139648922, + 0.5635691254043687, + 0.8216603445736381, + 0.5169183283391889, + 0.6482094982986043, + 0.5591242153068406, + 0.5539478507672101, + 0.4352935184619988, + 0.7096910112262675, + 0.08177103922211226, + 0.7185493168899821, + 0.6906521384470449, + 0.5424303480563135, + 0.8034905566669417, + 0.24530031156636306, + 0.6791649825098244, + 0.25103926707369056, + 0.6897203874901293, + 0.6662787689368599, + 0.6695372377111813, + 0.32831766535181106, + 0.28428078464167594, + 0.5440295499812461, + 0.7894404880867942, + 0.8881073966834958, + 0.007176386091829566, + 0.4595799728433832, + 0.7043700914244455, + 0.20398698108861654, + 0.6798912308987893, + 0.5913513581668906, + 0.7154722881784563, + 0.3720255045596441, + 0.5353927164036736, + 0.8292977052562882, + 0.1600623480977027, + 0.5626385603464779, + 0.5530980227747188, + 0.6144269099038059, + 0.8047402627946283, + 0.5179828986694456, + 0.2899772855298006, + 0.6435726414836709, + 0.49863310510036174, + 0.5806714059015666, + 0.2533767502100278, + 0.8066673674024603, + 0.533936307831739, + 0.051675625147813466, + 0.741898369799065, + 0.6685180914942186, + 0.6705007071467579, + 0.243658731626882, + 0.700756180292173, + 0.6124322825812726, + 0.6044312394389204, + 0.5094993386086216, + 0.5025822950964116, + 0.49662160344788164, + 0.7076567103083798, + 0.4875326918964735, + 0.8575174427431412, + 0.16424283766253403, + 0.3659744927810267, + 0.8415919620749859, + 0.39722240622155974, + 0.6205921515961875, + 0.5508152351685801, + 0.5580931446303532, + 0.20125257120061574, + 0.7022636474963218, + 0.6828785924235018, + 0.4318909377763495, + 0.6786025351852008, + 0.5941117883924572, + 0.6839787443692367, + 0.6598940110591041, + 0.31098709204629277, + 0.35743175000357147, + 0.8343049491885353, + 0.4197353878920623, + 0.8124751056450826, + 0.35669421673672336, + 0.46114362020262967, + 0.04721328350343224, + 0.8901899787392832, + 0.45313652204714083, + ] else: rwConfigParams.GsMatrix_B = [ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - 0.5773502691896258, 0.5773502691896258, 0.5773502691896258 + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.5773502691896258, + 0.5773502691896258, + 0.5773502691896258, ] - rwConfigParams.JsList = [0.1]*numWheels + rwConfigParams.JsList = [0.1] * numWheels rwConfigParams.numRW = numWheels rwConfigInMsg = messaging.RWArrayConfigMsg().write(rwConfigParams) @@ -188,7 +260,9 @@ def rwMotorTorqueTest(show_plots, numControlAxes, numWheels, RWAvailMsg): module.rwAvailInMsg.subscribeTo(rwAvailInMsg) else: - avail = [rwMotorTorque.AVAILABLE] * numWheels # this is used purely for the python level solution + avail = [ + rwMotorTorque.AVAILABLE + ] * numWheels # this is used purely for the python level solution # Setup logging on the test module output message so that we get all the writes to it dataLog = module.rwMotorTorqueOutMsg.recorder() @@ -207,7 +281,7 @@ def rwMotorTorqueTest(show_plots, numControlAxes, numWheels, RWAvailMsg): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -216,38 +290,51 @@ def rwMotorTorqueTest(show_plots, numControlAxes, numWheels, RWAvailMsg): # Note that range(3) will provide [0, 1, 2] Those are the elements you get from the vector (all of them) moduleOutput = dataLog.motorTorque - trueVector = np.array([ - [0.0] * MAX_EFF_CNT, - [0.0] * MAX_EFF_CNT - ]) + trueVector = np.array([[0.0] * MAX_EFF_CNT, [0.0] * MAX_EFF_CNT]) # set the output truth states - trueVector[0] = results_rwMotorTorque.computeTorqueU(np.array(controlAxes_B), - np.array(rwConfigParams.GsMatrix_B).reshape(( - 3, MAX_EFF_CNT), order='F'), - requestedTorque, - avail) + trueVector[0] = results_rwMotorTorque.computeTorqueU( + np.array(controlAxes_B), + np.array(rwConfigParams.GsMatrix_B).reshape((3, MAX_EFF_CNT), order="F"), + requestedTorque, + avail, + ) trueVector[1] = trueVector[0] # compare the module results to the truth values accuracy = 1e-8 - testFailCount, testMessages = unitTestSupport.compareArrayND(trueVector, moduleOutput, accuracy, "rwMotorTorques", - MAX_EFF_CNT, testFailCount, testMessages) - - - GsMatrix = np.transpose(np.reshape(rwConfigParams.GsMatrix_B, (MAX_EFF_CNT, 3), order="C")) + testFailCount, testMessages = unitTestSupport.compareArrayND( + trueVector, + moduleOutput, + accuracy, + "rwMotorTorques", + MAX_EFF_CNT, + testFailCount, + testMessages, + ) + + GsMatrix = np.transpose( + np.reshape(rwConfigParams.GsMatrix_B, (MAX_EFF_CNT, 3), order="C") + ) F = np.transpose(moduleOutput[0]) - receivedTorque = -1.0*np.array([np.matmul(GsMatrix,F)]) + receivedTorque = -1.0 * np.array([np.matmul(GsMatrix, F)]) receivedTorque = np.append(np.array([]), receivedTorque) if numWheels >= numControlAxes and numControlAxes > 0: if (len(avail) - np.sum(avail)) > numControlAxes: - testFailCount, testMessages = unitTestSupport.compareArrayND(np.array([requestedTorque]), - np.array([receivedTorque]), accuracy, - "CompareTorques", - numControlAxes, testFailCount, testMessages) - - snippetName = "LrBReq_LrBRec_"+str(numControlAxes) + "_" + str(numWheels) + "_" + RWAvailMsg + testFailCount, testMessages = unitTestSupport.compareArrayND( + np.array([requestedTorque]), + np.array([receivedTorque]), + accuracy, + "CompareTorques", + numControlAxes, + testFailCount, + testMessages, + ) + + snippetName = ( + "LrBReq_LrBRec_" + str(numControlAxes) + "_" + str(numWheels) + "_" + RWAvailMsg + ) requestedTex = str(requestedTorque) receivedTex = str(receivedTorque[1:4]) snippetTex = "Requested:\t" + requestedTex + "\n" @@ -256,23 +343,22 @@ def rwMotorTorqueTest(show_plots, numControlAxes, numWheels, RWAvailMsg): unitTestSupport.writeTeXSnippet(snippetName, snippetTex, path) # print out success message if no error were found - unitTestSupport.writeTeXSnippet('toleranceValue', str(accuracy), path) + unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) - snippentName = "passFail_"+str(numControlAxes) + str(numWheels) + RWAvailMsg + snippentName = "passFail_" + str(numControlAxes) + str(numWheels) + RWAvailMsg if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -280,8 +366,9 @@ def rwMotorTorqueTest(show_plots, numControlAxes, numWheels, RWAvailMsg): # stand-along python script # if __name__ == "__main__": - test_rwMotorTorque(False, - 3, # numControlAxes - 36, # numWheels - "NO" # RWAvailMsg ("NO", "ON", "OFF") - ) + test_rwMotorTorque( + False, + 3, # numControlAxes + 36, # numWheels + "NO", # RWAvailMsg ("NO", "ON", "OFF") + ) diff --git a/src/fswAlgorithms/effectorInterfaces/rwMotorVoltage/_UnitTest/test_rwMotorVoltage.py b/src/fswAlgorithms/effectorInterfaces/rwMotorVoltage/_UnitTest/test_rwMotorVoltage.py index 55a308b9ea..48f3fad31c 100755 --- a/src/fswAlgorithms/effectorInterfaces/rwMotorVoltage/_UnitTest/test_rwMotorVoltage.py +++ b/src/fswAlgorithms/effectorInterfaces/rwMotorVoltage/_UnitTest/test_rwMotorVoltage.py @@ -30,7 +30,9 @@ # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.fswAlgorithms import rwMotorVoltage from Basilisk.utilities import fswSetupRW from Basilisk.utilities import macros @@ -40,9 +42,11 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) + def addTimeColumn(time, data): return np.transpose(np.vstack([[time], np.transpose(data)])) + # Uncomment this line is 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. @@ -50,20 +54,26 @@ def addTimeColumn(time, data): # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("useLargeVoltage, useAvailability, useTorqueLoop, testName", [ - (False, False, False, "One") - , (True, False, False, "Two") - , (False, True, False, "Three") - , (False, False, True, "Four") -]) +@pytest.mark.parametrize( + "useLargeVoltage, useAvailability, useTorqueLoop, testName", + [ + (False, False, False, "One"), + (True, False, False, "Two"), + (False, True, False, "Three"), + (False, False, True, "Four"), + ], +) # update "module" in this function name to reflect the module name def test_module(show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = run(show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName) + [testResults, testMessage] = run( + show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName + ) assert testResults < 1, testMessage + def test_reset(): """Test that Reset() zeros the output message""" testFailCount = 0 @@ -87,21 +97,18 @@ def test_reset(): unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - module.VMin = 1.0 # Volts - module.VMax = 11.0 # Volts + module.VMin = 1.0 # Volts + module.VMax = 11.0 # Volts # Create RW configuration parameter input message - GsMatrix_B = [ - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, 1.0], - [1.0, 1.0, 1.0] - ] + GsMatrix_B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [1.0, 1.0, 1.0]] fswSetupRW.clearSetup() for i in range(4): - fswSetupRW.create(GsMatrix_B[i], # spin axis - 0.1, # kg*m^2 J2 - 0.2) # Nm uMax + fswSetupRW.create( + GsMatrix_B[i], # spin axis + 0.1, # kg*m^2 J2 + 0.2, + ) # Nm uMax rwConfigInMsg = fswSetupRW.writeConfigMessage() module.rwParamsInMsg.subscribeTo(rwConfigInMsg) @@ -120,12 +127,15 @@ def test_reset(): # Now call Reset() and verify output is zeroed module.Reset(0) # Pass 0 as currentSimNanos - expectedVoltage = [0.] * messaging.MAX_EFF_CNT - testFailCount, testMessages = unitTestSupport.compareVector(np.array(expectedVoltage), - np.array(module.voltageOutMsg.read().voltage), - 1e-3, - "Reset() zeroed voltage", - testFailCount, testMessages) + expectedVoltage = [0.0] * messaging.MAX_EFF_CNT + testFailCount, testMessages = unitTestSupport.compareVector( + np.array(expectedVoltage), + np.array(module.voltageOutMsg.read().voltage), + 1e-3, + "Reset() zeroed voltage", + testFailCount, + testMessages, + ) # Print success message if no errors were found if testFailCount == 0: @@ -133,19 +143,20 @@ def test_reset(): else: print("Failed: " + module.ModelTag + " Reset() test") - assert testFailCount == 0, ''.join(testMessages) + assert testFailCount == 0, "".join(testMessages) + def run(show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -158,18 +169,20 @@ def run(show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName): # Initialize the test module configuration data # set module parameters - module.VMin = 1.0 # Volts - module.VMax = 11.0 # Volts + module.VMin = 1.0 # Volts + module.VMax = 11.0 # Volts if useTorqueLoop: module.K = 1.5 rwSpeedMessage = messaging.RWSpeedMsgPayload() - rwSpeedMessage.wheelSpeeds = [1.0, 2.0, 1.5, -3.0] # rad/sec Omega's + rwSpeedMessage.wheelSpeeds = [1.0, 2.0, 1.5, -3.0] # rad/sec Omega's rwSpeedInMsg = messaging.RWSpeedMsg().write(rwSpeedMessage) module.rwSpeedInMsg.subscribeTo(rwSpeedInMsg) - unitTestSupport.writeTeXSnippet("Omega1", r"$\bm\Omega = " \ - + str(rwSpeedMessage.wheelSpeeds[0:4]) + "$" - , path) + unitTestSupport.writeTeXSnippet( + "Omega1", + r"$\bm\Omega = " + str(rwSpeedMessage.wheelSpeeds[0:4]) + "$", + path, + ) # # create BSK messages @@ -179,13 +192,15 @@ def run(show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName): [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0], - [1.0, 1.0, 1.0] # the create routine below normalizes these vectors + [1.0, 1.0, 1.0], # the create routine below normalizes these vectors ] fswSetupRW.clearSetup() for i in range(4): - fswSetupRW.create(GsMatrix_B[i], # spin axis - 0.1, # kg*m^2 J2 - 0.2) # Nm uMax + fswSetupRW.create( + GsMatrix_B[i], # spin axis + 0.1, # kg*m^2 J2 + 0.2, + ) # Nm uMax rwConfigInMsg = fswSetupRW.writeConfigMessage() module.rwParamsInMsg.subscribeTo(rwConfigInMsg) numRW = fswSetupRW.getNumOfDevices() @@ -193,9 +208,14 @@ def run(show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName): # Create RW motor torque input message usMessageData = messaging.ArrayMotorTorqueMsgPayload() if useLargeVoltage: - usMessageData.motorTorque = [0.5, 0.0, -0.15, -0.5] # [Nm] RW motor torque cmds + usMessageData.motorTorque = [0.5, 0.0, -0.15, -0.5] # [Nm] RW motor torque cmds else: - usMessageData.motorTorque = [0.05, 0.0, -0.15, -0.2] # [Nm] RW motor torque cmds + usMessageData.motorTorque = [ + 0.05, + 0.0, + -0.15, + -0.2, + ] # [Nm] RW motor torque cmds rwMotorTorqueInMsg = messaging.ArrayMotorTorqueMsg().write(usMessageData) module.torqueInMsg.subscribeTo(rwMotorTorqueInMsg) @@ -204,7 +224,7 @@ def run(show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName): rwAvailabilityMessage = messaging.RWAvailabilityMsgPayload() rwAvailArray = np.zeros(messaging.MAX_EFF_CNT, dtype=int) rwAvailArray.fill(messaging.AVAILABLE) - rwAvailArray[2] = messaging.UNAVAILABLE # make 3rd RW unavailable + rwAvailArray[2] = messaging.UNAVAILABLE # make 3rd RW unavailable rwAvailabilityMessage.wheelAvailability = rwAvailArray rwAvailInMsg = messaging.RWAvailabilityMsg().write(rwAvailabilityMessage) module.rwAvailInMsg.subscribeTo(rwAvailInMsg) @@ -220,7 +240,7 @@ def run(show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -228,76 +248,74 @@ def run(show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName): if useTorqueLoop: rwSpeedMessage.wheelSpeeds = [1.1, 2.1, 1.1, -4.1] # rad/sec Omega's rwSpeedInMsg.write(rwSpeedMessage) - unitTestSupport.writeTeXSnippet("Omega2", r"$\bm\Omega = " \ - + str(rwSpeedMessage.wheelSpeeds[0:4]) + "$" - , path) - unitTestSim.ConfigureStopTime(macros.sec2nano(1.5)) # seconds to stop simulation + unitTestSupport.writeTeXSnippet( + "Omega2", + r"$\bm\Omega = " + str(rwSpeedMessage.wheelSpeeds[0:4]) + "$", + path, + ) + unitTestSim.ConfigureStopTime(macros.sec2nano(1.5)) # seconds to stop simulation unitTestSim.ExecuteSimulation() # reset the module to test this functionality - module.Reset(1) # this module reset function needs a time input (in NanoSeconds) + module.Reset(1) # this module reset function needs a time input (in NanoSeconds) # run the module again for an additional 1.0 seconds - unitTestSim.ConfigureStopTime(macros.sec2nano(3.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(3.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - # This pulls the actual data log from the simulation run. moduleOutput = dataLog.voltage[:, :numRW] print(moduleOutput) - # set the filtered output truth states - trueVector=[]; + trueVector = [] if not useLargeVoltage and not useAvailability and not useTorqueLoop: trueVector = [ - [3.5, 0., -8.5, -11.] - , [3.5, 0., -8.5, -11.] - , [3.5, 0., -8.5, -11.] - , [3.5, 0., -8.5, -11.] - , [3.5, 0., -8.5, -11.] - , [3.5, 0., -8.5, -11.] - , [3.5, 0., -8.5, -11.] - ] + [3.5, 0.0, -8.5, -11.0], + [3.5, 0.0, -8.5, -11.0], + [3.5, 0.0, -8.5, -11.0], + [3.5, 0.0, -8.5, -11.0], + [3.5, 0.0, -8.5, -11.0], + [3.5, 0.0, -8.5, -11.0], + [3.5, 0.0, -8.5, -11.0], + ] if useLargeVoltage and not useAvailability and not useTorqueLoop: trueVector = [ - [11., 0., -8.5, -11.] - , [11., 0., -8.5, -11.] - , [11., 0., -8.5, -11.] - , [11., 0., -8.5, -11.] - , [11., 0., -8.5, -11.] - , [11., 0., -8.5, -11.] - , [11., 0., -8.5, -11.] - ] + [11.0, 0.0, -8.5, -11.0], + [11.0, 0.0, -8.5, -11.0], + [11.0, 0.0, -8.5, -11.0], + [11.0, 0.0, -8.5, -11.0], + [11.0, 0.0, -8.5, -11.0], + [11.0, 0.0, -8.5, -11.0], + [11.0, 0.0, -8.5, -11.0], + ] if not useLargeVoltage and useAvailability and not useTorqueLoop: trueVector = [ - [3.5, 0., 0., -11.] - , [3.5, 0., 0., -11.] - , [3.5, 0., 0., -11.] - , [3.5, 0., 0., -11.] - , [3.5, 0., 0., -11.] - , [3.5, 0., 0., -11.] - , [3.5, 0., 0., -11.] - ] + [3.5, 0.0, 0.0, -11.0], + [3.5, 0.0, 0.0, -11.0], + [3.5, 0.0, 0.0, -11.0], + [3.5, 0.0, 0.0, -11.0], + [3.5, 0.0, 0.0, -11.0], + [3.5, 0.0, 0.0, -11.0], + [3.5, 0.0, 0.0, -11.0], + ] if not useLargeVoltage and not useAvailability and useTorqueLoop: trueVector = [ - [3.5, 0., -8.5, -11.] - , [3.5, 0., -8.5, -11.] - , [3.5, 0., -8.5, -11.] - , [5.75, -2.5, -11., -9.5] - , [3.5, 0., -8.5, -11.] - , [3.5, 0., -8.5, -11.] - , [7.25, 0., -11., -11.] - ] + [3.5, 0.0, -8.5, -11.0], + [3.5, 0.0, -8.5, -11.0], + [3.5, 0.0, -8.5, -11.0], + [5.75, -2.5, -11.0, -9.5], + [3.5, 0.0, -8.5, -11.0], + [3.5, 0.0, -8.5, -11.0], + [7.25, 0.0, -11.0, -11.0], + ] # compare the module results to the truth values accuracy = 1e-10 - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "Output Vector", - testFailCount, testMessages) - - + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, moduleOutput, accuracy, "Output Vector", testFailCount, testMessages + ) # If the argument provided at commandline "--show_plots" evaluates as true, # plot all figures @@ -316,39 +334,56 @@ def run(show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName): # print out success message if no error were found snippentName = "passFail" + testName if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + colorText = "Red" + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) # write TeX Tables for documentation - moduleOutput = addTimeColumn(dataLog.times(), dataLog.voltage)[:, :numRW+1] + moduleOutput = addTimeColumn(dataLog.times(), dataLog.voltage)[:, : numRW + 1] resultTable = moduleOutput resultTable[:, 0] = macros.NANO2SEC * resultTable[:, 0] diff = np.delete(moduleOutput, 0, 1) - trueVector - resultTable = np.insert(resultTable, list(range(2, 2 + len(diff.transpose()))), diff, axis=1) - - tableName = "test" + str(useLargeVoltage) + str(useAvailability) + str(useTorqueLoop) - tableHeaders = ["time [s]", "$V_{s,1}$", "Error", "$V_{s,2}$", "Error", "$V_{s,3}$", "Error", "$V_{s,4}$", "Error"] - caption = 'RW voltage output for case {\\tt useLargeVoltage = ' + str(useLargeVoltage) \ - + ', useAvailability = ' + str(useAvailability) \ - + ', useTorqueLoop = ' + str(useTorqueLoop) + '}.' - unitTestSupport.writeTableLaTeX( - tableName, - tableHeaders, - caption, - resultTable, - path) - unitTestSupport.writeTeXSnippet("us"+ str(useLargeVoltage) + str(useAvailability) + str(useTorqueLoop) - , "$\\bm u_s = " + str(usMessageData.motorTorque[0:numRW]) + "$" - , path) + resultTable = np.insert( + resultTable, list(range(2, 2 + len(diff.transpose()))), diff, axis=1 + ) + + tableName = ( + "test" + str(useLargeVoltage) + str(useAvailability) + str(useTorqueLoop) + ) + tableHeaders = [ + "time [s]", + "$V_{s,1}$", + "Error", + "$V_{s,2}$", + "Error", + "$V_{s,3}$", + "Error", + "$V_{s,4}$", + "Error", + ] + caption = ( + "RW voltage output for case {\\tt useLargeVoltage = " + + str(useLargeVoltage) + + ", useAvailability = " + + str(useAvailability) + + ", useTorqueLoop = " + + str(useTorqueLoop) + + "}." + ) + unitTestSupport.writeTableLaTeX(tableName, tableHeaders, caption, resultTable, path) + unitTestSupport.writeTeXSnippet( + "us" + str(useLargeVoltage) + str(useAvailability) + str(useTorqueLoop), + "$\\bm u_s = " + str(usMessageData.motorTorque[0:numRW]) + "$", + path, + ) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -356,10 +391,10 @@ def run(show_plots, useLargeVoltage, useAvailability, useTorqueLoop, testName): # stand-along python script # if __name__ == "__main__": - test_module( # update "module" in function name - False - ,False # useLargeVoltage - ,False # useAvailability - ,True # useTorqueLoop - ,"Four" # testName - ) + test_module( # update "module" in function name + False, + False, # useLargeVoltage + False, # useAvailability + True, # useTorqueLoop + "Four", # testName + ) diff --git a/src/fswAlgorithms/effectorInterfaces/rwNullSpace/_UnitTest/test_rwNullSpace.py b/src/fswAlgorithms/effectorInterfaces/rwNullSpace/_UnitTest/test_rwNullSpace.py index ee9b0ac2ba..036ce356ff 100644 --- a/src/fswAlgorithms/effectorInterfaces/rwNullSpace/_UnitTest/test_rwNullSpace.py +++ b/src/fswAlgorithms/effectorInterfaces/rwNullSpace/_UnitTest/test_rwNullSpace.py @@ -17,6 +17,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) + # Uncomment this line is 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. @@ -24,21 +25,17 @@ # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("numWheels, defaultDesired", [ - (3, True) - , (4, True) - , (3, False) - , (4, False) -]) - - +@pytest.mark.parametrize( + "numWheels, defaultDesired", [(3, True), (4, True), (3, False), (4, False)] +) def test_rwNullSpace(numWheels, defaultDesired): - """ Test rwNullSpace. """ + """Test rwNullSpace.""" [testResults, testMessage] = rwNullSpaceTestFunction(numWheels, defaultDesired) assert testResults < 1, testMessage + def rwNullSpaceTestFunction(numWheels, defaultDesired): - """ Test the rwNullSpace module. Setup a simulation, """ + """Test the rwNullSpace module. Setup a simulation,""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -54,14 +51,18 @@ def rwNullSpaceTestFunction(numWheels, defaultDesired): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process # Construct the rwNullSpace module # Set the names for the input messages module = rwNullSpace.rwNullSpace() # Set the necessary data in the module. NOTE: This information is more or less random - module.OmegaGain = .5 # The feedback gain value applied for the RW despin control law + module.OmegaGain = ( + 0.5 # The feedback gain value applied for the RW despin control law + ) # This calls the algContain to setup the selfInit, update, and reset module.ModelTag = "rwNullSpace" @@ -78,30 +79,32 @@ def rwNullSpaceTestFunction(numWheels, defaultDesired): inputSpeedMsg = messaging.RWSpeedMsgPayload() if defaultDesired: - desiredOmega = [0]*numRW + desiredOmega = [0] * numRW else: - desiredOmega = [5]*numRW + desiredOmega = [5] * numRW inputDesiredSpeedMsg = messaging.RWSpeedMsgPayload() inputDesiredSpeedMsg.wheelSpeeds = desiredOmega - gsHat = [[1, 0, 0], [0,1,0], [0, 0, 1]] + gsHat = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] if numWheels == 4: - gs4Hat = np.array([1,1,1]) - gs4Hat = gs4Hat/np.sqrt(gs4Hat.dot(gs4Hat)) + gs4Hat = np.array([1, 1, 1]) + gs4Hat = gs4Hat / np.sqrt(gs4Hat.dot(gs4Hat)) gsHat.append(gs4Hat.tolist()) # Iterate over all of the reaction wheels, create a rwConfigElementFswMsg, and add them to the rwConstellationFswMsg rwConfigElementList = list() for rw in range(numRW): rwConfigElementMsg = messaging.RWConfigElementMsgPayload() - rwConfigElementMsg.gsHat_B = gsHat[rw] # Spin axis unit vector of the wheel in structure - rwConfigElementMsg.Js = 0.08 # Spin axis inertia of wheel [kgm2] - rwConfigElementMsg.uMax = 0.2 # maximum RW motor torque [Nm] + rwConfigElementMsg.gsHat_B = gsHat[ + rw + ] # Spin axis unit vector of the wheel in structure + rwConfigElementMsg.Js = 0.08 # Spin axis inertia of wheel [kgm2] + rwConfigElementMsg.uMax = 0.2 # maximum RW motor torque [Nm] # Add this to the list rwConfigElementList.append(rwConfigElementMsg) - rwSpeeds = [10, 20, 30] # [rad/sec] The current angular velocities of the RW wheel + rwSpeeds = [10, 20, 30] # [rad/sec] The current angular velocities of the RW wheel if numWheels == 4: rwSpeeds.append(40) # [rad/sec] inputSpeedMsg.wheelSpeeds = rwSpeeds @@ -110,12 +113,11 @@ def rwNullSpaceTestFunction(numWheels, defaultDesired): inputRWConstellationMsg.reactionWheels = rwConfigElementList inputRWCmdMsg = messaging.ArrayMotorTorqueMsgPayload() - usControl = [0.1, 0.2, 0.15] # [Nm] RW motor torque array + usControl = [0.1, 0.2, 0.15] # [Nm] RW motor torque array if numWheels == 4: - usControl.append(-0.2) # [Nm] + usControl.append(-0.2) # [Nm] inputRWCmdMsg.motorTorque = usControl - # Set these messages rwSpeedMsg = messaging.RWSpeedMsg().write(inputSpeedMsg) rwConfigMsg = messaging.RWConstellationMsg().write(inputRWConstellationMsg) @@ -144,11 +146,13 @@ def rwNullSpaceTestFunction(numWheels, defaultDesired): if numWheels == 3: # in this case there is no nullspace of the RW configuration. The output torque should be the input torque - trueVector = [inputRWCmdMsg.motorTorque, - inputRWCmdMsg.motorTorque, - inputRWCmdMsg.motorTorque, - inputRWCmdMsg.motorTorque, - inputRWCmdMsg.motorTorque] + trueVector = [ + inputRWCmdMsg.motorTorque, + inputRWCmdMsg.motorTorque, + inputRWCmdMsg.motorTorque, + inputRWCmdMsg.motorTorque, + inputRWCmdMsg.motorTorque, + ] elif numWheels == 4: # in this case there is a 1D nullspace of [Gs] GsT = np.array(gsHat) @@ -157,7 +161,7 @@ def rwNullSpaceTestFunction(numWheels, defaultDesired): tmp = GsT.dot(inv(tmp)) tmp = tmp.dot(Gs) tau = np.identity(numWheels) - tmp - d = - module.OmegaGain * (np.array(rwSpeeds) - np.array(desiredOmega)) + d = -module.OmegaGain * (np.array(rwSpeeds) - np.array(desiredOmega)) uNull = tau.dot(d) trueTorque = np.array(usControl) + uNull trueVector = [ @@ -165,33 +169,36 @@ def rwNullSpaceTestFunction(numWheels, defaultDesired): trueTorque.tolist(), trueTorque.tolist(), trueTorque.tolist(), - trueTorque.tolist() + trueTorque.tolist(), ] - accuracy = 1e-6 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) # At each timestep, make sure the vehicleConfig values haven't changed from the initial values - testFailCount, testMessages = unitTestSupport.compareArrayND(trueVector, outputCrtlData, - accuracy, - "numWheels = " + str(numWheels), - 2, testFailCount, testMessages) - + testFailCount, testMessages = unitTestSupport.compareArrayND( + trueVector, + outputCrtlData, + accuracy, + "numWheels = " + str(numWheels), + 2, + testFailCount, + testMessages, + ) snippentName = "passFail" + str(numWheels) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) + return [testFailCount, "".join(testMessages)] - return [testFailCount, ''.join(testMessages)] -if __name__ == '__main__': +if __name__ == "__main__": test_rwNullSpace(4, True) diff --git a/src/fswAlgorithms/effectorInterfaces/solarArrayReference/_UnitTest/test_solarArrayReference.py b/src/fswAlgorithms/effectorInterfaces/solarArrayReference/_UnitTest/test_solarArrayReference.py index 0b2e51aa9f..ad23872c55 100644 --- a/src/fswAlgorithms/effectorInterfaces/solarArrayReference/_UnitTest/test_solarArrayReference.py +++ b/src/fswAlgorithms/effectorInterfaces/solarArrayReference/_UnitTest/test_solarArrayReference.py @@ -31,17 +31,20 @@ # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import solarArrayReference # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import ( + solarArrayReference, +) # import the module that is to be tested from Basilisk.utilities import macros from Basilisk.utilities import RigidBodyKinematics as rbk -from Basilisk.architecture import messaging # import the message definitions +from Basilisk.architecture import messaging # import the message definitions from Basilisk.architecture import bskLogging # this python function computes the same reference angle as the tested module def computeSunReferenceAngle(sigma_RN, rHat_SB_N, a1Hat_B, a2Hat_B, theta0): - RN = rbk.MRP2C(sigma_RN) rS_R = np.matmul(RN, rHat_SB_N) @@ -53,18 +56,19 @@ def computeSunReferenceAngle(sigma_RN, rHat_SB_N, a1Hat_B, a2Hat_B, theta0): a2_R_norm = np.linalg.norm(a2_R) if a2_R_norm > 1e-6: a2_R = a2_R / a2_R_norm - theta = np.arccos(min(max(np.dot(a2Hat_B, a2_R),-1),1)) + theta = np.arccos(min(max(np.dot(a2Hat_B, a2_R), -1), 1)) if np.dot(a1Hat_B, np.cross(a2Hat_B, a2_R)) < 0: theta = -theta else: theta = theta0 - if theta-theta0 > np.pi: + if theta - theta0 > np.pi: theta -= np.pi - elif theta-theta0 < -np.pi: + elif theta - theta0 < -np.pi: theta += np.pi return theta + # Uncomment this line is 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. @@ -74,16 +78,15 @@ def computeSunReferenceAngle(sigma_RN, rHat_SB_N, a1Hat_B, a2Hat_B, theta0): # of the multiple test runs for this test. Note that the order in that you add the parametrize method # matters for the documentation in that it impacts the order in which the test arguments are shown. # The first parametrize arguments are shown last in the pytest argument list -@pytest.mark.parametrize("rHat_SB_N", [[1, 0, 0], - [0, 0, 1]]) -@pytest.mark.parametrize("sigma_BN", [[0.1, 0.2, 0.3], - [0.5, 0.4, 0.3]]) -@pytest.mark.parametrize("sigma_RN", [[0.3, 0.2, 0.1], - [0.9, 0.7, 0.8]]) +@pytest.mark.parametrize("rHat_SB_N", [[1, 0, 0], [0, 0, 1]]) +@pytest.mark.parametrize("sigma_BN", [[0.1, 0.2, 0.3], [0.5, 0.4, 0.3]]) +@pytest.mark.parametrize("sigma_RN", [[0.3, 0.2, 0.1], [0.9, 0.7, 0.8]]) @pytest.mark.parametrize("bodyFrame", [0, 1]) @pytest.mark.parametrize("pointingMode", [0, 1]) @pytest.mark.parametrize("accuracy", [1e-12]) -def test_solarArrayRotation(show_plots, rHat_SB_N, sigma_BN, sigma_RN, bodyFrame, pointingMode, accuracy): +def test_solarArrayRotation( + show_plots, rHat_SB_N, sigma_BN, sigma_RN, bodyFrame, pointingMode, accuracy +): r""" **Validation Test Description** @@ -112,12 +115,15 @@ def test_solarArrayRotation(show_plots, rHat_SB_N, sigma_BN, sigma_RN, bodyFrame The reference angle derivative ``thetaDot`` is checked versus zero, as the module is run for only one Update call. """ # each test method requires a single assert method to be called - [testResults, testMessage] = solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, bodyFrame, pointingMode, accuracy) + [testResults, testMessage] = solarArrayRotationTestFunction( + show_plots, rHat_SB_N, sigma_BN, sigma_RN, bodyFrame, pointingMode, accuracy + ) assert testResults < 1, testMessage -def solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, attitudeFrame, pointingMode, accuracy): - +def solarArrayRotationTestFunction( + show_plots, rHat_SB_N, sigma_BN, sigma_RN, attitudeFrame, pointingMode, accuracy +): a1Hat_B = np.array([1, 0, 0]) a2Hat_B = np.array([0, 1, 0]) r_AB_B = np.array([4.5, 0, 0.5]) @@ -126,17 +132,17 @@ def solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, at thetaC = 0 thetaDotC = 0 - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(1) # update process rate update time + testProcessRate = macros.sec2nano(1) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -153,7 +159,7 @@ def solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, at solarArray.r_AB_B = r_AB_B solarArray.pointingMode = pointingMode solarArray.attitudeFrame = attitudeFrame - solarArray.ThetaMax = np.pi/2 + solarArray.ThetaMax = np.pi / 2 solarArray.sigma = 1 solarArray.n = 1 @@ -174,7 +180,9 @@ def solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, at hingedRigidBodyInMsgData = messaging.HingedRigidBodyMsgPayload() hingedRigidBodyInMsgData.theta = thetaC hingedRigidBodyInMsgData.thetaDot = thetaDotC - hingedRigidBodyInMsg = messaging.HingedRigidBodyMsg().write(hingedRigidBodyInMsgData) + hingedRigidBodyInMsg = messaging.HingedRigidBodyMsg().write( + hingedRigidBodyInMsgData + ) solarArray.hingedRigidBodyInMsg.subscribeTo(hingedRigidBodyInMsg) if pointingMode == 1: @@ -187,7 +195,17 @@ def solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, at # Create RW configuration message rwArrayConfigInMsgData = messaging.RWArrayConfigMsgPayload() rwArrayConfigInMsgData.numRW = 3 - rwArrayConfigInMsgData.GsMatrix_B = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + rwArrayConfigInMsgData.GsMatrix_B = [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + ] rwArrayConfigInMsgData.JsList = [0.15, 0.15, 0.15] rwArrayConfigInMsg = messaging.RWArrayConfigMsg().write(rwArrayConfigInMsgData) solarArray.rwConfigDataInMsg.subscribeTo(rwArrayConfigInMsg) @@ -209,14 +227,18 @@ def solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, at # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() if attitudeFrame == 0: - thetaSunR = computeSunReferenceAngle(sigma_RN, rHat_SB_N, a1Hat_B, a2Hat_B, thetaC) + thetaSunR = computeSunReferenceAngle( + sigma_RN, rHat_SB_N, a1Hat_B, a2Hat_B, thetaC + ) else: - thetaSunR = computeSunReferenceAngle(sigma_BN, rHat_SB_N, a1Hat_B, a2Hat_B, thetaC) + thetaSunR = computeSunReferenceAngle( + sigma_BN, rHat_SB_N, a1Hat_B, a2Hat_B, thetaC + ) if pointingMode == 0: thetaR = thetaSunR @@ -225,8 +247,12 @@ def solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, at H_B = np.array([0.0, 0.0, 0.0]) for i in range(rwArrayConfigInMsgData.numRW): for j in range(3): - H_B[j] = (H_B[j] + rwArrayConfigInMsgData.GsMatrix_B[3*i+j] - * rwArrayConfigInMsgData.JsList[i] * rwSpeedsInMsgData.wheelSpeeds[i]) + H_B[j] = ( + H_B[j] + + rwArrayConfigInMsgData.GsMatrix_B[3 * i + j] + * rwArrayConfigInMsgData.JsList[i] + * rwSpeedsInMsgData.wheelSpeeds[i] + ) if attitudeFrame == 0: BN = rbk.MRP2C(sigma_BN) @@ -242,37 +268,57 @@ def solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, at f = np.dot(F_R, H_R) if f > 0: - thetaR = thetaSunR + 2 * solarArray.ThetaMax / np.pi * np.arctan(solarArray.sigma * f**(solarArray.n)) + thetaR = thetaSunR + 2 * solarArray.ThetaMax / np.pi * np.arctan( + solarArray.sigma * f ** (solarArray.n) + ) else: thetaR = thetaSunR # If the angle difference is greater than PI, adjust it to the range [-PI, PI] - thetaDiff = math.fmod(thetaR-thetaC, 2 * np.pi) + thetaDiff = math.fmod(thetaR - thetaC, 2 * np.pi) if thetaDiff > np.pi: - thetaR -= 2*np.pi + thetaR -= 2 * np.pi elif thetaDiff < -np.pi: - thetaR += 2*np.pi + thetaR += 2 * np.pi # compare the module results to the truth values if not unitTestSupport.isDoubleEqual(dataLog.theta[0], thetaR, accuracy): testFailCount += 1 - testMessages.append("FAILED: " - + solarArray.ModelTag - + "solarArrayRotation module failed unit test on thetaR for sigma_BN = [{},{},{}], " - "sigma_RN = [{},{},{}] and attitudeFrame = {} \n".format( - sigma_BN[0], sigma_BN[1], sigma_BN[2], sigma_RN[0], sigma_RN[1], sigma_RN[2], attitudeFrame)) + testMessages.append( + "FAILED: " + + solarArray.ModelTag + + "solarArrayRotation module failed unit test on thetaR for sigma_BN = [{},{},{}], " + "sigma_RN = [{},{},{}] and attitudeFrame = {} \n".format( + sigma_BN[0], + sigma_BN[1], + sigma_BN[2], + sigma_RN[0], + sigma_RN[1], + sigma_RN[2], + attitudeFrame, + ) + ) if not unitTestSupport.isDoubleEqual(dataLog.thetaDot[0], 0, accuracy): testFailCount += 1 - testMessages.append("FAILED: " - + solarArray.ModelTag - + "solarArrayRotation module failed unit test on thetaDotR for sigma_BN = [{},{},{}], " - "sigma_RN = [{},{},{}] and attitudeFrame = {} \n".format( - sigma_BN[0], sigma_BN[1], sigma_BN[2], sigma_RN[0], sigma_RN[1], sigma_RN[2], attitudeFrame)) + testMessages.append( + "FAILED: " + + solarArray.ModelTag + + "solarArrayRotation module failed unit test on thetaDotR for sigma_BN = [{},{},{}], " + "sigma_RN = [{},{},{}] and attitudeFrame = {} \n".format( + sigma_BN[0], + sigma_BN[1], + sigma_BN[2], + sigma_RN[0], + sigma_RN[1], + sigma_RN[2], + attitudeFrame, + ) + ) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -281,11 +327,11 @@ def solarArrayRotationTestFunction(show_plots, rHat_SB_N, sigma_BN, sigma_RN, at # if __name__ == "__main__": test_solarArrayRotation( - False, - np.array([0, 0, 1]), - np.array([0.5, 0.4, 0.3]), - np.array([0.9, 0.7, 0.8]), - 1, - 0, - 1e-12, - ) + False, + np.array([0, 0, 1]), + np.array([0.5, 0.4, 0.3]), + np.array([0.9, 0.7, 0.8]), + 1, + 0, + 1e-12, + ) diff --git a/src/fswAlgorithms/effectorInterfaces/thrFiringRemainder/_UnitTest/test_thrFiringRemainder.py b/src/fswAlgorithms/effectorInterfaces/thrFiringRemainder/_UnitTest/test_thrFiringRemainder.py index dde00807db..275e923a65 100755 --- a/src/fswAlgorithms/effectorInterfaces/thrFiringRemainder/_UnitTest/test_thrFiringRemainder.py +++ b/src/fswAlgorithms/effectorInterfaces/thrFiringRemainder/_UnitTest/test_thrFiringRemainder.py @@ -34,16 +34,14 @@ path = os.path.dirname(os.path.abspath(filename)) - - - - - - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import thrFiringRemainder # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import ( + thrFiringRemainder, +) # import the module that is to be tested from Basilisk.utilities import macros from Basilisk.utilities import fswSetupThrusters from Basilisk.architecture import messaging @@ -56,36 +54,34 @@ # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("resetCheck, dvOn", [ - (False,False), - (True,False), - (False,True), - (True,True) -]) +@pytest.mark.parametrize( + "resetCheck, dvOn", [(False, False), (True, False), (False, True), (True, True)] +) # update "module" in this function name to reflect the module name def test_thrFiringRemainder(show_plots, resetCheck, dvOn): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = thrFiringRemainderTestFunction(show_plots, resetCheck, dvOn) + [testResults, testMessage] = thrFiringRemainderTestFunction( + show_plots, resetCheck, dvOn + ) assert testResults < 1, testMessage def thrFiringRemainderTestFunction(show_plots, resetCheck, dvOn): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = thrFiringRemainder.thrFiringRemainder() module.ModelTag = "thrFiringRemainder" @@ -110,8 +106,8 @@ def thrFiringRemainderTestFunction(show_plots, resetCheck, dvOn): [-0.86360, -0.82550, -1.79070], [-0.82550, -0.86360, -1.79070], [0.82550, 0.86360, -1.79070], - [0.86360, 0.82550, -1.79070] - ] + [0.86360, 0.82550, -1.79070], + ] rcsDirectionData = [ [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], @@ -120,15 +116,14 @@ def thrFiringRemainderTestFunction(show_plots, resetCheck, dvOn): [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 1.0, 0.0], - [1.0, 0.0, 0.0] - ] + [1.0, 0.0, 0.0], + ] for i in range(len(rcsLocationData)): fswSetupThrusters.create(rcsLocationData[i], rcsDirectionData[i], 0.5) thrConfigMsg = fswSetupThrusters.writeConfigMessage() numThrusters = fswSetupThrusters.getNumOfDevices() - # setup thruster impulse request message thrMessageData = messaging.THRArrayCmdForceMsgPayload() if dvOn: @@ -137,13 +132,10 @@ def thrFiringRemainderTestFunction(show_plots, resetCheck, dvOn): thrMessageData.thrForce = [0.5, 0.05, 0.1, 0.15, 0.19, 0.0, 0.2, 0.49] thrForceMsg = messaging.THRArrayCmdForceMsg().write(thrMessageData) - - # Setup logging on the test module output message so that we get all the writes to it dataLog = module.onTimeOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) - # connect messages module.thrConfInMsg.subscribeTo(thrConfigMsg) module.thrForceInMsg.subscribeTo(thrForceMsg) @@ -155,90 +147,95 @@ def thrFiringRemainderTestFunction(show_plots, resetCheck, dvOn): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(3.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(3.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() if resetCheck: # reset the module to test this functionality - module.Reset(macros.sec2nano(3.0)) # this module reset function needs a time input (in NanoSeconds) + module.Reset( + macros.sec2nano(3.0) + ) # this module reset function needs a time input (in NanoSeconds) # run the module again for an additional 1.0 seconds - unitTestSim.ConfigureStopTime(macros.sec2nano(5.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime( + macros.sec2nano(5.5) + ) # seconds to stop simulation unitTestSim.ExecuteSimulation() moduleOutput = dataLog.OnTimeRequest[:, :numThrusters] # set the filtered output truth states - if resetCheck==1: + if resetCheck == 1: if dvOn: trueVector = [ - [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], - [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.24], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], - [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.24], - ] + [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], + [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.24], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], + [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.24], + ] else: trueVector = [ - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], - [0.55, 0.0, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], - [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], - [0.55, 0.2, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], - [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], - [0.55, 0.0, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], - [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], - [0.55, 0.2, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], - ] + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], + [0.55, 0.0, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], + [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], + [0.55, 0.2, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], + [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], + [0.55, 0.0, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], + [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], + [0.55, 0.2, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], + ] else: if dvOn: trueVector = [ - [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], - [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.24], - [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], - ] + [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], + [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.32, 0.22, 0.24], + [0.0, 0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0], + ] else: trueVector = [ - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], - [0.55, 0.0, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], - [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], - [0.55, 0.2, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], - [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], - ] + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], + [0.55, 0.0, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], + [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], + [0.55, 0.2, 0.2, 0.3, 0.38, 0.0, 0.2, 0.49], + [0.55, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.49], + ] # compare the module results to the truth values accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, accuracy, - "OnTimeRequest", testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, moduleOutput, accuracy, "OnTimeRequest", testFailCount, testMessages + ) snippentName = "passFail" + str(resetCheck) + str(dvOn) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) # First set a non-zero thruster force request @@ -251,18 +248,21 @@ def thrFiringRemainderTestFunction(show_plots, resetCheck, dvOn): # Now call Reset() and verify output is zeroed module.Reset(0) # Pass 0 as currentSimNanos - expectedOnTime = [0.] * messaging.MAX_EFF_CNT - testFailCount, testMessages = unitTestSupport.compareVector(np.array(expectedOnTime), - np.array(module.onTimeOutMsg.read().OnTimeRequest), - 1e-3, - "Reset() zeroed onTime", - testFailCount, testMessages) + expectedOnTime = [0.0] * messaging.MAX_EFF_CNT + testFailCount, testMessages = unitTestSupport.compareVector( + np.array(expectedOnTime), + np.array(module.onTimeOutMsg.read().OnTimeRequest), + 1e-3, + "Reset() zeroed onTime", + testFailCount, + testMessages, + ) print("Accuracy used: " + str(accuracy)) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -271,7 +271,7 @@ def thrFiringRemainderTestFunction(show_plots, resetCheck, dvOn): # if __name__ == "__main__": test_thrFiringRemainder( - True, # show plots - False, # - False - ) + True, # show plots + False, # + False, + ) diff --git a/src/fswAlgorithms/effectorInterfaces/thrFiringSchmitt/_UnitTest/test_thrFiringSchmitt.py b/src/fswAlgorithms/effectorInterfaces/thrFiringSchmitt/_UnitTest/test_thrFiringSchmitt.py index 2e2a1de6ed..057485e6a4 100755 --- a/src/fswAlgorithms/effectorInterfaces/thrFiringSchmitt/_UnitTest/test_thrFiringSchmitt.py +++ b/src/fswAlgorithms/effectorInterfaces/thrFiringSchmitt/_UnitTest/test_thrFiringSchmitt.py @@ -33,16 +33,14 @@ path = os.path.dirname(os.path.abspath(filename)) - - - - - - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import thrFiringSchmitt # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import ( + thrFiringSchmitt, +) # import the module that is to be tested from Basilisk.utilities import macros from Basilisk.utilities import fswSetupThrusters from Basilisk.architecture import messaging @@ -55,36 +53,40 @@ # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("resetCheck, dvOn", [ - (False, False), - (True, False), - (False, True), - (True, True), -]) +@pytest.mark.parametrize( + "resetCheck, dvOn", + [ + (False, False), + (True, False), + (False, True), + (True, True), + ], +) # update "module" in this function name to reflect the module name def test_thrFiringSchmitt(show_plots, resetCheck, dvOn): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = thrFiringSchmittTestFunction(show_plots, resetCheck, dvOn) + [testResults, testMessage] = thrFiringSchmittTestFunction( + show_plots, resetCheck, dvOn + ) assert testResults < 1, testMessage def thrFiringSchmittTestFunction(show_plots, resetCheck, dvOn): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = thrFiringSchmitt.thrFiringSchmitt() module.ModelTag = "thrFiringSchmitt" @@ -99,8 +101,8 @@ def thrFiringSchmittTestFunction(show_plots, resetCheck, dvOn): else: module.baseThrustState = 0 - module.level_on = .75 - module.level_off = .25 + module.level_on = 0.75 + module.level_off = 0.25 # setup thruster cluster message fswSetupThrusters.clearSetup() @@ -112,8 +114,8 @@ def thrFiringSchmittTestFunction(show_plots, resetCheck, dvOn): [-0.86360, -0.82550, -1.79070], [-0.82550, -0.86360, -1.79070], [0.82550, 0.86360, -1.79070], - [0.86360, 0.82550, -1.79070] - ] + [0.86360, 0.82550, -1.79070], + ] rcsDirectionData = [ [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], @@ -122,8 +124,8 @@ def thrFiringSchmittTestFunction(show_plots, resetCheck, dvOn): [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 1.0, 0.0], - [1.0, 0.0, 0.0] - ] + [1.0, 0.0, 0.0], + ] for i in range(len(rcsLocationData)): fswSetupThrusters.create(rcsLocationData[i], rcsDirectionData[i], 0.5) @@ -162,94 +164,94 @@ def thrFiringSchmittTestFunction(show_plots, resetCheck, dvOn): inputMessageData.thrForce = effReq1 thrCmdMsg.write(inputMessageData) - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - inputMessageData.thrForce = effReq2 thrCmdMsg.write(inputMessageData) - unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - inputMessageData.thrForce = effReq3 thrCmdMsg.write(inputMessageData) - unitTestSim.ConfigureStopTime(macros.sec2nano(2.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.5)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - inputMessageData.thrForce = effReq4 thrCmdMsg.write(inputMessageData) - unitTestSim.ConfigureStopTime(macros.sec2nano(3.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(3.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() if resetCheck: # reset the module to test this functionality - module.Reset(macros.sec2nano(3.0)) # this module reset function needs a time input (in NanoSeconds) + module.Reset( + macros.sec2nano(3.0) + ) # this module reset function needs a time input (in NanoSeconds) # run the module again for an additional 1.0 seconds - unitTestSim.ConfigureStopTime(macros.sec2nano(5.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime( + macros.sec2nano(5.5) + ) # seconds to stop simulation unitTestSim.ExecuteSimulation() - # This pulls the actual data log from the simulation run. moduleOutput = dataLog.OnTimeRequest[:, :numThrusters] # print moduleOutput # set the filtered output truth states - if resetCheck==1: + if resetCheck == 1: if dvOn == 1: trueVector = [ - [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], - [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], - [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], - [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], - [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], - [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], - [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], - [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], - [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], - [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], - [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], - [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], - ] + [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], + [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], + [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], + [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], + [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], + [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], + [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], + [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], + [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], + [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], + [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], + [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], + ] else: trueVector = [ - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.49], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.2], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.2], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], - ] + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.49], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.2], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.2], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], + ] else: if dvOn == 1: trueVector = [ - [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], - [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], - [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], - [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], - [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], - [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], - [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], - ] + [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], + [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], + [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], + [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], + [0.55, 0.4, 0.3, 0.2, 0.2, 0.0, 0.0, 0.0], + [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], + [0.55, 0.4, 0.3, 0.2, 0.0, 0.0, 0.0, 0.0], + ] else: trueVector = [ - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.49], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.2], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.2], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], - [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], - ] + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.49], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.2], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.2], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], + [0.55, 0.0, 0.0, 0.0, 0.2, 0.2, 0.2, 0.0], + ] # else: # testFailCount+=1 @@ -259,18 +261,19 @@ def thrFiringSchmittTestFunction(show_plots, resetCheck, dvOn): accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, accuracy, - "OnTimeRequest", testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, moduleOutput, accuracy, "OnTimeRequest", testFailCount, testMessages + ) snippentName = "passFail" + str(resetCheck) + str(dvOn) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) # First set a non-zero thruster force request @@ -282,18 +285,21 @@ def thrFiringSchmittTestFunction(show_plots, resetCheck, dvOn): # Now call Reset() and verify output is zeroed module.Reset(0) # Pass 0 as currentSimNanos - expectedOnTime = [0.] * messaging.MAX_EFF_CNT - testFailCount, testMessages = unitTestSupport.compareVector(np.array(expectedOnTime), - np.array(module.onTimeOutMsg.read().OnTimeRequest), - 1e-3, - "Reset() zeroed onTime", - testFailCount, testMessages) + expectedOnTime = [0.0] * messaging.MAX_EFF_CNT + testFailCount, testMessages = unitTestSupport.compareVector( + np.array(expectedOnTime), + np.array(module.onTimeOutMsg.read().OnTimeRequest), + 1e-3, + "Reset() zeroed onTime", + testFailCount, + testMessages, + ) print("Accuracy used: " + str(accuracy)) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -301,8 +307,8 @@ def thrFiringSchmittTestFunction(show_plots, resetCheck, dvOn): # stand-along python script # if __name__ == "__main__": - test_thrFiringSchmitt( # update "module" in function name - False, - True, # resetOn - False # dvOn - ) + test_thrFiringSchmitt( # update "module" in function name + False, + True, # resetOn + False, # dvOn + ) diff --git a/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/Support/Results_thrForceMapping.py b/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/Support/Results_thrForceMapping.py index 38349041ad..328c4fef22 100644 --- a/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/Support/Results_thrForceMapping.py +++ b/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/Support/Results_thrForceMapping.py @@ -1,22 +1,40 @@ -''' ''' +""" """ + import numpy as np -class Results_thrForceMapping(): - def __init__(self, Lr, COrig, COM, rData, gData, thrForceSign, thrForceMag, angErrThresh, numThrusters, epsilon, use2ndLoop): +class Results_thrForceMapping: + def __init__( + self, + Lr, + COrig, + COM, + rData, + gData, + thrForceSign, + thrForceMag, + angErrThresh, + numThrusters, + epsilon, + use2ndLoop, + ): self.rData = np.array(rData) self.gData = np.array(gData) - self.Lr_B = np.array(Lr) #Original Requested Torque in B Frame + self.Lr_B = np.array(Lr) # Original Requested Torque in B Frame self.COM = COM - self.thrForceSign = thrForceSign # -1 is DV thrusters; +1 is RCS thrusters - self.thrForceMag = thrForceMag # specifies the max thrust for each thruster - self.angErrThresh = angErrThresh # Determines when the thrusters are considered saturated + self.thrForceSign = thrForceSign # -1 is DV thrusters; +1 is RCS thrusters + self.thrForceMag = thrForceMag # specifies the max thrust for each thruster + self.angErrThresh = ( + angErrThresh # Determines when the thrusters are considered saturated + ) - self.numThrusters = numThrusters # number of explicitly configured thrusters + self.numThrusters = numThrusters # number of explicitly configured thrusters - self.C = np.array(COrig) # Control "Frame" (could be 1, 2, or 3 axii controllable) - self.C = np.reshape(self.C, ((len(self.C)//3), 3), order='C') + self.C = np.array( + COrig + ) # Control "Frame" (could be 1, 2, or 3 axii controllable) + self.C = np.reshape(self.C, ((len(self.C) // 3), 3), order="C") self.epsilon = epsilon self.use2ndLoop = use2ndLoop @@ -28,11 +46,11 @@ def results_thrForceMapping(self): CT = np.transpose(self.C) # Compute D Matrix and Determine Force - D = np.zeros((3,len(self.rData))) + D = np.zeros((3, len(self.rData))) for i in range(len(self.rData)): - D[:,i] = np.cross((self.rData[i,:] - self.COM), self.gData[i,:]) - if(self.thrForceSign < 0): - Lr_offset -= self.thrForceMag[i]*D[:,i] + D[:, i] = np.cross((self.rData[i, :] - self.COM), self.gData[i, :]) + if self.thrForceSign < 0: + Lr_offset -= self.thrForceMag[i] * D[:, i] self.Lr_B = self.Lr_B + Lr_offset Lr_Bar = np.dot(self.C, self.Lr_B) @@ -40,25 +58,29 @@ def results_thrForceMapping(self): # Subtract off minimum force (remove null space contribution) if self.thrForceSign > 0: - #F = self.subtractPairwiseNullSpace(F, D) + # F = self.subtractPairwiseNullSpace(F, D) F = self.subtractMin(F, self.numThrusters) # Identify any negative forces - t = (F[:]*self.thrForceSign > self.epsilon) + t = F[:] * self.thrForceSign > self.epsilon # Recompute the D Matrix with negative forces removed and compute Force if self.thrForceSign < 0 or self.use2ndLoop: DNew = np.array([]) - for i in range(0,len(F)): + for i in range(0, len(F)): if t[i]: - DNew = np.append(DNew, np.cross((self.rData[i,:] - self.COM), self.gData[i])) - DNew = np.reshape(DNew, (3, (len(DNew) // 3)), order='F') + DNew = np.append( + DNew, np.cross((self.rData[i, :] - self.COM), self.gData[i]) + ) + DNew = np.reshape(DNew, (3, (len(DNew) // 3)), order="F") FNew = self.mapToForce(DNew, Lr_Bar) - if (self.thrForceSign > 0): - FNew = self.subtractMin(FNew,len(DNew[0])) # Produced negative forces when doing 2nd loop, dropped thruster, and COM offset + if self.thrForceSign > 0: + FNew = self.subtractMin( + FNew, len(DNew[0]) + ) # Produced negative forces when doing 2nd loop, dropped thruster, and COM offset # Remove minumum force count = 0 - for i in range(0,len(F)): + for i in range(0, len(F)): if t[i]: F[i] = FNew[count] count += 1 @@ -69,13 +91,15 @@ def results_thrForceMapping(self): angle = self.results_computeAngErr(D, Lr_Bar, F) if angle > self.angErrThresh: - maxFractUse = 0.0 for i in range(0, self.numThrusters): - if self.thrForceMag[i] > 0 and abs(F[i])/self.thrForceMag[i] > maxFractUse: - maxFractUse = abs(F[i])/self.thrForceMag[i] + if ( + self.thrForceMag[i] > 0 + and abs(F[i]) / self.thrForceMag[i] > maxFractUse + ): + maxFractUse = abs(F[i]) / self.thrForceMag[i] if maxFractUse > 1.0: - F = F/maxFractUse + F = F / maxFractUse angleErr = self.results_computeAngErr(D, Lr_Bar, F) return F, DNew @@ -84,7 +108,7 @@ def results_computeAngErr(self, D, BLr_B, F): returnAngle = 0.0 DT = np.transpose(D) - if np.linalg.norm(BLr_B) > 10 ** -9: + if np.linalg.norm(BLr_B) > 10**-9: tauActual_B = [0.0, 0.0, 0.0] BLr_B_hat = BLr_B / np.linalg.norm(BLr_B) for i in range(0, self.numThrusters): @@ -140,11 +164,10 @@ def mapToForce(self, D, Lr_Bar): return F def subtractPairwiseNullSpace(self, F, D): - for i in range(self.numThrusters): if F[i] < 0.0: for j in range(self.numThrusters): - if (np.allclose(D[:, i], D[:, j], atol=self.epsilon) and i != j): + if np.allclose(D[:, i], D[:, j], atol=self.epsilon) and i != j: F[j] -= F[i] break F[i] = 0.0 diff --git a/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/test_thrForceMap_configErr.py b/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/test_thrForceMap_configErr.py index f3083bd4fa..0d8fe63076 100644 --- a/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/test_thrForceMap_configErr.py +++ b/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/test_thrForceMap_configErr.py @@ -1,19 +1,18 @@ - - # 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. +# 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. # @@ -51,6 +50,7 @@ # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. + @pytest.mark.parametrize("useDVThruster", [True, False]) @pytest.mark.parametrize("useCOMOffset", [False]) @pytest.mark.parametrize("dropThruster", [0, 1, 2]) @@ -59,31 +59,54 @@ @pytest.mark.parametrize("saturateThrusters", [0]) @pytest.mark.parametrize("misconfigThruster", [True, False]) - - - # update "module" in this function name to reflect the module name -def test_module(show_plots, useDVThruster, useCOMOffset, dropThruster, asymmetricDrop, numControlAxis, saturateThrusters, misconfigThruster): +def test_module( + show_plots, + useDVThruster, + useCOMOffset, + dropThruster, + asymmetricDrop, + numControlAxis, + saturateThrusters, + misconfigThruster, +): """Module Unit Test""" # each test method requires a single assert method to be called expect_error = misconfigThruster or numControlAxis == 0 with pytest.raises(BasiliskError) if expect_error else nullcontext(): - [testResults, testMessage] = thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asymmetricDrop, - numControlAxis, saturateThrusters, misconfigThruster) + [testResults, testMessage] = thrusterForceTest( + show_plots, + useDVThruster, + useCOMOffset, + dropThruster, + asymmetricDrop, + numControlAxis, + saturateThrusters, + misconfigThruster, + ) assert testResults < 1, testMessage -def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asymmetricDrop, numControlAxis, saturateThrusters, misconfigThruster): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) +def thrusterForceTest( + show_plots, + useDVThruster, + useCOMOffset, + dropThruster, + asymmetricDrop, + numControlAxis, + saturateThrusters, + misconfigThruster, +): + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -97,24 +120,30 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy # write vehicle configuration message vehicleConfigOut = messaging.VehicleConfigMsgPayload() if useCOMOffset == 1: - CoM_B = [0.03,0.001,0.02] + CoM_B = [0.03, 0.001, 0.02] else: - CoM_B = [0,0,0] + CoM_B = [0, 0, 0] vehicleConfigOut.CoM_B = CoM_B vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) # Create input message and size it because the regular creator of that message # is not part of the test. - inputMessageData = messaging.CmdTorqueBodyMsgPayload() # Create a structure for the input message - requestedTorque = [1.0, -0.5, 0.7] # Set up a list as a 3-vector - if saturateThrusters>0: # default angErrThresh is 0, thus this should trigger scaling + inputMessageData = ( + messaging.CmdTorqueBodyMsgPayload() + ) # Create a structure for the input message + requestedTorque = [1.0, -0.5, 0.7] # Set up a list as a 3-vector + if ( + saturateThrusters > 0 + ): # default angErrThresh is 0, thus this should trigger scaling requestedTorque = [10.0, -5.0, 7.0] - if saturateThrusters==2: # angle is set and small enough to trigger scaling - module.angErrThresh = 10.0*macros.D2R - if saturateThrusters==3: # angle is too large enough to trigger scaling - module.angErrThresh = 40.0*macros.D2R + if saturateThrusters == 2: # angle is set and small enough to trigger scaling + module.angErrThresh = 10.0 * macros.D2R + if saturateThrusters == 3: # angle is too large enough to trigger scaling + module.angErrThresh = 40.0 * macros.D2R - inputMessageData.torqueRequestBody = requestedTorque # write torque request to input message + inputMessageData.torqueRequestBody = ( + requestedTorque # write torque request to input message + ) cmdTorqueInMsg = messaging.CmdTorqueBodyMsg().write(inputMessageData) module.epsilon = 0.0005 @@ -123,11 +152,7 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy rcsLocationData = np.zeros((MAX_EFF_CNT, 3)) rcsDirectionData = np.zeros((MAX_EFF_CNT, 3)) - controlAxes_B = np.array([ - [1, 0, 0], - [0, 1, 0], - [0, 0, 1] - ]) + controlAxes_B = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) controlAxes_B = controlAxes_B[0:numControlAxis] if len(controlAxes_B) == 0: @@ -140,38 +165,38 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy # DV thruster setup module.thrForceSign = -1 numThrusters = 6 - rcsLocationData[0:6] = [ \ + rcsLocationData[0:6] = [ [0, 0.413, -0.1671], [0, -0.413, -0.1671], [0.35766849176297305, 0.20650000000000013, -0.1671], [0.3576684917629732, -0.20649999999999988, -0.1671], [-0.35766849176297333, 0.20649999999999968, -0.1671], - [-0.35766849176297305, -0.20650000000000018, -0.1671] \ - ] - rcsDirectionData[0:6] = [ \ + [-0.35766849176297305, -0.20650000000000018, -0.1671], + ] + rcsDirectionData[0:6] = [ [0.0, 0.0, 1.0], [0.0, 0.0, 1.0], [0.0, 0.0, 1.0], [0.0, 0.0, 1.0], [0.0, 0.0, 1.0], - [0.0, 0.0, 1.0] \ - ] + [0.0, 0.0, 1.0], + ] else: # RCS thruster setup module.thrForceSign = +1 numThrusters = 8 - rcsLocationData[0:8] = [ \ - [-0.86360, -0.82550, 1.79070], - [-0.82550, -0.86360, 1.79070], - [0.82550, 0.86360, 1.79070], - [0.86360, 0.82550, 1.79070], - [-0.86360, -0.82550, -1.79070], - [-0.82550, -0.86360, -1.79070], - [0.82550, 0.86360, -1.79070], - [0.86360, 0.82550, -1.79070] \ - ] - - rcsDirectionData[0:8] = [ \ + rcsLocationData[0:8] = [ + [-0.86360, -0.82550, 1.79070], + [-0.82550, -0.86360, 1.79070], + [0.82550, 0.86360, 1.79070], + [0.86360, 0.82550, 1.79070], + [-0.86360, -0.82550, -1.79070], + [-0.82550, -0.86360, -1.79070], + [0.82550, 0.86360, -1.79070], + [0.86360, 0.82550, -1.79070], + ] + + rcsDirectionData[0:8] = [ [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, -1.0, 0.0], @@ -179,12 +204,13 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, -1.0, 0.0], - [-1.0, 0.0, 0.0] \ - ] - + [-1.0, 0.0, 0.0], + ] if dropThruster > 0: - if (dropThruster % 2==0) and asymmetricDrop: # Drop thrusters that dont share the same torque direction + if ( + (dropThruster % 2 == 0) and asymmetricDrop + ): # Drop thrusters that dont share the same torque direction removedThrusters = 0 for i in range(0, numThrusters, 2): rcsLocationData[i] = [0.0, 0.0, 0.0] @@ -205,10 +231,10 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy offset = 0 for i in indices: - idx = (int) (i - offset) + idx = (int)(i - offset) rcsLocationData = np.delete(rcsLocationData, idx, axis=0) rcsDirectionData = np.delete(rcsDirectionData, idx, axis=0) - rcsLocationData = np.append(rcsLocationData,[[0.0, 0.0, 0.0]], axis=0) + rcsLocationData = np.append(rcsLocationData, [[0.0, 0.0, 0.0]], axis=0) rcsDirectionData = np.append(rcsDirectionData, [[0.0, 0.0, 0.0]], axis=0) offset = offset + 1 @@ -217,13 +243,14 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy if useDVThruster: maxThrust = 10.0 - for i in range(numThrusters): if misconfigThruster and i == 0: maxThrustConfig = 0.0 else: maxThrustConfig = maxThrust - fswSetupThrusters.create(rcsLocationData[i], rcsDirectionData[i], maxThrustConfig) + fswSetupThrusters.create( + rcsLocationData[i], rcsDirectionData[i], maxThrustConfig + ) thrConfigInMsg = fswSetupThrusters.writeConfigMessage() # Setup logging on the test module output message so that we get all the writes to it @@ -242,7 +269,7 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -250,42 +277,72 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy # This pulls the actual data log from the simulation run. moduleOutput = dataLog.thrForce if misconfigThruster: - return [testFailCount, ''.join(testMessages)] # We don't handle cases where a thruster is configured incorrectly. + return [ + testFailCount, + "".join(testMessages), + ] # We don't handle cases where a thruster is configured incorrectly. if useDVThruster and numControlAxis == 3: - return [testFailCount, ''.join(testMessages)] # 3 control axes doesn't work for dv thrusters (only two axes controllable) - - - results = thrForceMapping.Results_thrForceMapping(requestedTorque, module.controlAxes_B, - vehicleConfigOut.CoM_B, rcsLocationData, - rcsDirectionData, module.thrForceSign, - module.thrForcMag, module.angErrThresh, - numThrusters, module.epsilon, False) + return [ + testFailCount, + "".join(testMessages), + ] # 3 control axes doesn't work for dv thrusters (only two axes controllable) + + results = thrForceMapping.Results_thrForceMapping( + requestedTorque, + module.controlAxes_B, + vehicleConfigOut.CoM_B, + rcsLocationData, + rcsDirectionData, + module.thrForceSign, + module.thrForcMag, + module.angErrThresh, + numThrusters, + module.epsilon, + False, + ) F, DNew = results.results_thrForceMapping() - accuracy = 1E-6 + accuracy = 1e-6 # Check that Python Math and C Math are Identical - testFailCount, testMessages = unitTestSupport.compareArrayND(np.array([F]), np.array([moduleOutput[0]]), accuracy, - "CompareForces", - numThrusters, testFailCount, testMessages) - - - unitTestSupport.writeTeXSnippet('toleranceValue', str(accuracy), path) + testFailCount, testMessages = unitTestSupport.compareArrayND( + np.array([F]), + np.array([moduleOutput[0]]), + accuracy, + "CompareForces", + numThrusters, + testFailCount, + testMessages, + ) - snippentName = "passFail_" + str(useDVThruster) + "_" + str(useCOMOffset) + "_" + str(dropThruster) + "_" + str( - numControlAxis) + "_" + str(saturateThrusters) + "_" + str(misconfigThruster) + unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) + + snippentName = ( + "passFail_" + + str(useDVThruster) + + "_" + + str(useCOMOffset) + + "_" + + str(dropThruster) + + "_" + + str(numControlAxis) + + "_" + + str(saturateThrusters) + + "_" + + str(misconfigThruster) + ) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -293,14 +350,13 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy # stand-along python script # if __name__ == "__main__": - test_module( # update "module" in function name - False, - True, # useDVThruster - False, # use COM offset - 0, # num drop thruster(s) - False, # asymmetric drop - 1, # num control axis - 0, # saturateThrusters - False # misconfigThruster - + test_module( # update "module" in function name + False, + True, # useDVThruster + False, # use COM offset + 0, # num drop thruster(s) + False, # asymmetric drop + 1, # num control axis + 0, # saturateThrusters + False, # misconfigThruster ) diff --git a/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/test_thrForceMapping.py b/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/test_thrForceMapping.py index af77863737..4814461e1f 100644 --- a/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/test_thrForceMapping.py +++ b/src/fswAlgorithms/effectorInterfaces/thrForceMapping/_UnitTest/test_thrForceMapping.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -32,7 +31,9 @@ # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.fswAlgorithms import thrForceMapping from Basilisk.utilities import macros from Basilisk.utilities import fswSetupThrusters @@ -49,49 +50,82 @@ # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. + @pytest.mark.parametrize("useDVThruster", [True, False]) -@pytest.mark.parametrize(["useCOMOffset","dropThruster", "use2ndLoop"],[ - (False, 0, False), - (False, 1, True), - (False, 2, True), # Any time we drop a thruster we should recompute the solution - (True, 0, False)]) # We don't handle the case where there is a dropped thruster and and COM offset--see performance analysis. +@pytest.mark.parametrize( + ["useCOMOffset", "dropThruster", "use2ndLoop"], + [ + (False, 0, False), + (False, 1, True), + ( + False, + 2, + True, + ), # Any time we drop a thruster we should recompute the solution + (True, 0, False), + ], +) # We don't handle the case where there is a dropped thruster and and COM offset--see performance analysis. @pytest.mark.parametrize("asymmetricDrop", [False]) @pytest.mark.parametrize("numControlAxis", [1, 2, 3]) -@pytest.mark.parametrize("saturateThrusters", [0,1,2]) +@pytest.mark.parametrize("saturateThrusters", [0, 1, 2]) @pytest.mark.parametrize("misconfigThruster", [False]) - - # update "module" in this function name to reflect the module name -def test_module(show_plots, useDVThruster, useCOMOffset, dropThruster, asymmetricDrop, numControlAxis, saturateThrusters, misconfigThruster, use2ndLoop): +def test_module( + show_plots, + useDVThruster, + useCOMOffset, + dropThruster, + asymmetricDrop, + numControlAxis, + saturateThrusters, + misconfigThruster, + use2ndLoop, +): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asymmetricDrop, - numControlAxis, saturateThrusters, misconfigThruster, use2ndLoop) + [testResults, testMessage] = thrusterForceTest( + show_plots, + useDVThruster, + useCOMOffset, + dropThruster, + asymmetricDrop, + numControlAxis, + saturateThrusters, + misconfigThruster, + use2ndLoop, + ) assert testResults < 1, testMessage -def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asymmetricDrop, numControlAxis, - saturateThrusters, misconfigThruster,use2ndLoop): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) +def thrusterForceTest( + show_plots, + useDVThruster, + useCOMOffset, + dropThruster, + asymmetricDrop, + numControlAxis, + saturateThrusters, + misconfigThruster, + use2ndLoop, +): + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = thrForceMapping.thrForceMapping() module.ModelTag = "thrForceMapping" - # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) @@ -101,24 +135,30 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy # write vehicle configuration message vehicleConfigOut = messaging.VehicleConfigMsgPayload() if useCOMOffset == 1: - CoM_B = [0.03,0.001,0.02] + CoM_B = [0.03, 0.001, 0.02] else: - CoM_B = [0,0,0] + CoM_B = [0, 0, 0] vehicleConfigOut.CoM_B = CoM_B vcInMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) # Create input message and size it because the regular creator of that message # is not part of the test. - inputMessageData = messaging.CmdTorqueBodyMsgPayload() # Create a structure for the input message - requestedTorque = [1.0, -0.5, 0.7] # Set up a list as a 3-vector - if saturateThrusters>0: # default angErrThresh is 0, thus this should trigger scaling + inputMessageData = ( + messaging.CmdTorqueBodyMsgPayload() + ) # Create a structure for the input message + requestedTorque = [1.0, -0.5, 0.7] # Set up a list as a 3-vector + if ( + saturateThrusters > 0 + ): # default angErrThresh is 0, thus this should trigger scaling requestedTorque = [10.0, -5.0, 7.0] - if saturateThrusters==2: # angle is set and small enough to trigger scaling - module.angErrThresh = 10.0*macros.D2R - if saturateThrusters==3: # angle is too large enough to trigger scaling - module.angErrThresh = 40.0*macros.D2R + if saturateThrusters == 2: # angle is set and small enough to trigger scaling + module.angErrThresh = 10.0 * macros.D2R + if saturateThrusters == 3: # angle is too large enough to trigger scaling + module.angErrThresh = 40.0 * macros.D2R - inputMessageData.torqueRequestBody = requestedTorque # write torque request to input message + inputMessageData.torqueRequestBody = ( + requestedTorque # write torque request to input message + ) cmdTorqueInMsg = messaging.CmdTorqueBodyMsg().write(inputMessageData) module.epsilon = 0.0005 @@ -127,11 +167,7 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy rcsLocationData = np.zeros((MAX_EFF_CNT, 3)) rcsDirectionData = np.zeros((MAX_EFF_CNT, 3)) - controlAxes_B = np.array([ - [1, 0, 0], - [0, 1, 0], - [0, 0, 1] - ]) + controlAxes_B = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) controlAxes_B = controlAxes_B[0:numControlAxis] if len(controlAxes_B) == 0: @@ -144,38 +180,38 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy # DV thruster setup module.thrForceSign = -1 numThrusters = 6 - rcsLocationData[0:6] = [ \ + rcsLocationData[0:6] = [ [0, 0.413, -0.1671], [0, -0.413, -0.1671], [0.35766849176297305, 0.20650000000000013, -0.1671], [0.3576684917629732, -0.20649999999999988, -0.1671], [-0.35766849176297333, 0.20649999999999968, -0.1671], - [-0.35766849176297305, -0.20650000000000018, -0.1671] \ - ] - rcsDirectionData[0:6] = [ \ + [-0.35766849176297305, -0.20650000000000018, -0.1671], + ] + rcsDirectionData[0:6] = [ + [0.0, 0.0, 1.0], [0.0, 0.0, 1.0], [0.0, 0.0, 1.0], [0.0, 0.0, 1.0], [0.0, 0.0, 1.0], [0.0, 0.0, 1.0], - [0.0, 0.0, 1.0] \ - ] + ] else: # RCS thruster setup module.thrForceSign = +1 numThrusters = 8 - rcsLocationData[0:8] = [ \ - [-0.86360, -0.82550, 1.79070], - [-0.82550, -0.86360, 1.79070], - [0.82550, 0.86360, 1.79070], - [0.86360, 0.82550, 1.79070], - [-0.86360, -0.82550, -1.79070], - [-0.82550, -0.86360, -1.79070], - [0.82550, 0.86360, -1.79070], - [0.86360, 0.82550, -1.79070] \ - ] - - rcsDirectionData[0:8] = [ \ + rcsLocationData[0:8] = [ + [-0.86360, -0.82550, 1.79070], + [-0.82550, -0.86360, 1.79070], + [0.82550, 0.86360, 1.79070], + [0.86360, 0.82550, 1.79070], + [-0.86360, -0.82550, -1.79070], + [-0.82550, -0.86360, -1.79070], + [0.82550, 0.86360, -1.79070], + [0.86360, 0.82550, -1.79070], + ] + + rcsDirectionData[0:8] = [ [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, -1.0, 0.0], @@ -183,12 +219,13 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, -1.0, 0.0], - [-1.0, 0.0, 0.0] \ - ] - + [-1.0, 0.0, 0.0], + ] if dropThruster > 0: - if (dropThruster % 2==0) and asymmetricDrop: # Drop thrusters that don't share the same torque direction + if ( + (dropThruster % 2 == 0) and asymmetricDrop + ): # Drop thrusters that don't share the same torque direction removedThrusters = 0 for i in range(0, numThrusters, 2): rcsLocationData[i] = [0.0, 0.0, 0.0] @@ -209,10 +246,10 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy offset = 0 for i in indices: - idx = (int) (i - offset) + idx = (int)(i - offset) rcsLocationData = np.delete(rcsLocationData, idx, axis=0) rcsDirectionData = np.delete(rcsDirectionData, idx, axis=0) - rcsLocationData = np.append(rcsLocationData,[[0.0, 0.0, 0.0]], axis=0) + rcsLocationData = np.append(rcsLocationData, [[0.0, 0.0, 0.0]], axis=0) rcsDirectionData = np.append(rcsDirectionData, [[0.0, 0.0, 0.0]], axis=0) offset = offset + 1 @@ -221,13 +258,14 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy if useDVThruster: maxThrust = 10.0 - for i in range(numThrusters): if misconfigThruster and i == 0: maxThrustConfig = 0.0 else: maxThrustConfig = maxThrust - fswSetupThrusters.create(rcsLocationData[i], rcsDirectionData[i], maxThrustConfig) + fswSetupThrusters.create( + rcsLocationData[i], rcsDirectionData[i], maxThrustConfig + ) thrConfigInMsg = fswSetupThrusters.writeConfigMessage() # Setup logging on the test module output message so that we get all the writes to it @@ -246,7 +284,7 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -255,34 +293,52 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy moduleOutput = dataLog.thrForce if misconfigThruster: - return [testFailCount, ''.join(testMessages)] # We don't handle cases where a thruster is configured incorrectly. + return [ + testFailCount, + "".join(testMessages), + ] # We don't handle cases where a thruster is configured incorrectly. if useDVThruster and numControlAxis == 3: - return [testFailCount, ''.join(testMessages)] # 3 control axes doesn't work for dv thrusters (only two axes controllable) - - results = thrForceMapping.Results_thrForceMapping(requestedTorque, module.controlAxes_B, - vehicleConfigOut.CoM_B, rcsLocationData, - rcsDirectionData, module.thrForceSign, - module.thrForcMag, module.angErrThresh, - numThrusters, module.epsilon, use2ndLoop) + return [ + testFailCount, + "".join(testMessages), + ] # 3 control axes doesn't work for dv thrusters (only two axes controllable) + + results = thrForceMapping.Results_thrForceMapping( + requestedTorque, + module.controlAxes_B, + vehicleConfigOut.CoM_B, + rcsLocationData, + rcsDirectionData, + module.thrForceSign, + module.thrForcMag, + module.angErrThresh, + numThrusters, + module.epsilon, + use2ndLoop, + ) F, DNew = results.results_thrForceMapping() trueVector = np.zeros((2, MAX_EFF_CNT)) - trueVector[0,:] = F - trueVector[1,:] = F + trueVector[0, :] = F + trueVector[1, :] = F C = np.reshape(controlAxes_B, (numControlAxis, 3)) CT = np.transpose(C) - D = np.cross(rcsDirectionData,rcsLocationData-CoM_B) - receivedTorque = -1.0*np.array([np.matmul(np.transpose(D), np.transpose(moduleOutput[0]))]) + D = np.cross(rcsDirectionData, rcsLocationData - CoM_B) + receivedTorque = -1.0 * np.array( + [np.matmul(np.transpose(D), np.transpose(moduleOutput[0]))] + ) receivedTorque = np.append(np.array([]), receivedTorque) Lr_offset = np.array([0.0, 0.0, 0.0]) Lr_B = np.array([0.0, 0.0, 0.0]) - for i in range(0,numThrusters): + for i in range(0, numThrusters): if module.thrForceSign < 0 and module.thrForcMag[i] >= 0: - Lr_offset -= module.thrForcMag[i]*np.cross(rcsLocationData[i,:]-CoM_B,rcsDirectionData[i,:]) # off pulsing + Lr_offset -= module.thrForcMag[i] * np.cross( + rcsLocationData[i, :] - CoM_B, rcsDirectionData[i, :] + ) # off pulsing Lr_B = requestedTorque + Lr_offset @@ -295,17 +351,23 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy Lr_Rec_Bar_B = np.matmul(CT, np.matmul(C, receivedTorque)) # This computes the projected requested and received control torque directions - Lr_Req_Bar_B_Unit = Lr_Req_Bar_B/np.linalg.norm(Lr_Req_Bar_B) - Lr_Rec_Bar_B_Unit = Lr_Rec_Bar_B/np.linalg.norm(Lr_Rec_Bar_B) + Lr_Req_Bar_B_Unit = Lr_Req_Bar_B / np.linalg.norm(Lr_Req_Bar_B) + Lr_Rec_Bar_B_Unit = Lr_Rec_Bar_B / np.linalg.norm(Lr_Rec_Bar_B) if np.linalg.norm(Lr_Rec_Bar_B) == 0.0: Lr_Rec_Bar_B_Unit = [0.0, 0.0, 0.0] - accuracy = 1E-6 + accuracy = 1e-6 # Check that Python Math and C Math are Identical - testFailCount, testMessages = unitTestSupport.compareArrayND(np.array([F]), np.array([moduleOutput[0]]), accuracy, - "CompareForces", - MAX_EFF_CNT, testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + np.array([F]), + np.array([moduleOutput[0]]), + accuracy, + "CompareForces", + MAX_EFF_CNT, + testFailCount, + testMessages, + ) # Checks to make sure that no forces are negative if not useDVThruster and np.any(moduleOutput[0] < 0): @@ -314,32 +376,54 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy if not useDVThruster and np.any(F < 0): testFailCount += 1 - print("A negative force exists in the Python RCS solution. This is not allowed!\n") + print( + "A negative force exists in the Python RCS solution. This is not allowed!\n" + ) if testFailCount > 0: - return [testFailCount, ''.join(testMessages)] - + return [testFailCount, "".join(testMessages)] # Check that Torques are Sensible print("\nReq Lr_Bar [B]: " + str(Lr_Req_Bar_B)) print("Rec Lr_Bar [B]: " + str(Lr_Rec_Bar_B)) - testFailCount, testMessages = unitTestSupport.compareArrayND(np.array([Lr_Req_Bar_B_Unit]), - np.array([Lr_Rec_Bar_B_Unit]), accuracy, - "CompareTorques", - 3, testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + np.array([Lr_Req_Bar_B_Unit]), + np.array([Lr_Rec_Bar_B_Unit]), + accuracy, + "CompareTorques", + 3, + testFailCount, + testMessages, + ) - snippetName = "LrData_" + str(useDVThruster) + "_" + str(dropThruster) + "_" + str(numControlAxis) + "_" + str(useCOMOffset) + "____" + str(asymmetricDrop) + "_" + str(saturateThrusters) + "_" + str(misconfigThruster) + snippetName = ( + "LrData_" + + str(useDVThruster) + + "_" + + str(dropThruster) + + "_" + + str(numControlAxis) + + "_" + + str(useCOMOffset) + + "____" + + str(asymmetricDrop) + + "_" + + str(saturateThrusters) + + "_" + + str(misconfigThruster) + ) snippetTex = "DV Thrusters:\t" + str(useDVThruster) + "\n" - snippetTex += "Number of Dropped Thrusters:\t" + str(dropThruster)+ "\n" + snippetTex += "Number of Dropped Thrusters:\t" + str(dropThruster) + "\n" snippetTex += "Number of Control Axes:\t" + str(numControlAxis) + "\n" snippetTex += "COM Offset:\t" + str(useCOMOffset) + "\n\n" - snippetTex += "Was the drop asymmetric about the COM?\t" + str(asymmetricDrop) + "\n" + snippetTex += ( + "Was the drop asymmetric about the COM?\t" + str(asymmetricDrop) + "\n" + ) snippetTex += "Number of Saturated Thrusters:\t" + str(saturateThrusters) + "\n" snippetTex += "Misconfigured Thruster?:\t" + str(misconfigThruster) + "\n\n" - snippetTex += "Original [B]:\t" + str(requestedTorque) + "\n" snippetTex += "Requested (Original + Offset) [B]:\t" + str(Lr_B) + "\n" @@ -347,7 +431,11 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy snippetTex += "Requested Unit:\t\t" + str(Lr_Req_B_Unit) + "\n" snippetTex += "Received Unit:\t\t" + str(Lr_Rec_B_Unit) + "\n\n" - snippetTex += "Requested On Control Axes (Original + Offset) [B]:\t" + str(Lr_Req_Bar_B) + "\n" + snippetTex += ( + "Requested On Control Axes (Original + Offset) [B]:\t" + + str(Lr_Req_Bar_B) + + "\n" + ) snippetTex += "Received On Control Axes [B]:\t\t" + str(Lr_Rec_Bar_B) + "\n\n" snippetTex += "Requested On Control Axes Unit:\t\t" + str(Lr_Req_Bar_B_Unit) + "\n" snippetTex += "Received On Control Axes Unit:\t\t" + str(Lr_Rec_Bar_B_Unit) + "\n\n" @@ -360,35 +448,54 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy # Any solutions that dont have the correct torque, but do have the correct unit direction are called successful. if testFailCount > 0: - unitTestSupport.writeTeXSnippet(directory+"Failed/"+snippetName, snippetTex, path) + unitTestSupport.writeTeXSnippet( + directory + "Failed/" + snippetName, snippetTex, path + ) print("FAILED: " + module.ModelTag) - testMessages.append("FAILED: " + module.ModelTag + " Module failed unit test at t=" + - str(dataLog.times()[0] * macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed unit test at t=" + + str(dataLog.times()[0] * macros.NANO2SEC) + + "sec\n" + ) else: - unitTestSupport.writeTeXSnippet(directory+"/Passed/" + snippetName, snippetTex, path) + unitTestSupport.writeTeXSnippet( + directory + "/Passed/" + snippetName, snippetTex, path + ) print("PASSED: " + module.ModelTag) - - unitTestSupport.writeTeXSnippet('toleranceValue', str(accuracy), path) - - snippentName = "passFail_" + str(useDVThruster) + "_" + str(useCOMOffset) + "_" + str(dropThruster) + "_" + str( - numControlAxis) + "_" + str(saturateThrusters) + "_" + str(misconfigThruster) + unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) + + snippentName = ( + "passFail_" + + str(useDVThruster) + + "_" + + str(useCOMOffset) + + "_" + + str(dropThruster) + + "_" + + str(numControlAxis) + + "_" + + str(saturateThrusters) + + "_" + + str(misconfigThruster) + ) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) if testFailCount > 0: print("Python:\t " + str(F)) print("C: \t:" + str(moduleOutput[0])) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -396,14 +503,14 @@ def thrusterForceTest(show_plots, useDVThruster, useCOMOffset, dropThruster, asy # stand-along python script # if __name__ == "__main__": - test_module( # update "module" in function name - False, - False, # useDVThruster - False, # use COM offset - 2, # num drop thruster(s) - False, # asymmetric drop - 3, # num control axis - 2, # saturateThrusters - False, # misconfigThruster - False # Use 2nd loop + test_module( # update "module" in function name + False, + False, # useDVThruster + False, # use COM offset + 2, # num drop thruster(s) + False, # asymmetric drop + 3, # num control axis + 2, # saturateThrusters + False, # misconfigThruster + False, # Use 2nd loop ) diff --git a/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/_UnitTest/test_thrMomentumDumping.py b/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/_UnitTest/test_thrMomentumDumping.py index 540960f936..d5dbea6ee1 100755 --- a/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/_UnitTest/test_thrMomentumDumping.py +++ b/src/fswAlgorithms/effectorInterfaces/thrMomentumDumping/_UnitTest/test_thrMomentumDumping.py @@ -33,12 +33,17 @@ # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import thrMomentumDumping # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import ( + thrMomentumDumping, +) # import the module that is to be tested from Basilisk.utilities import macros from Basilisk.utilities import fswSetupThrusters from Basilisk.architecture import messaging + # Uncomment this line is 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. @@ -46,35 +51,34 @@ # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("resetCheck, largeMinFireTime", [ - (False, False) - ,(True, False) - ,(False, True) -]) +@pytest.mark.parametrize( + "resetCheck, largeMinFireTime", [(False, False), (True, False), (False, True)] +) # update "module" in this function name to reflect the module name def test_thrMomentumDumping(show_plots, resetCheck, largeMinFireTime): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime) + [testResults, testMessage] = thrMomentumDumpingTestFunction( + show_plots, resetCheck, largeMinFireTime + ) assert testResults < 1, testMessage def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = thrMomentumDumping.thrMomentumDumping() module.ModelTag = "thrMomentumDumping" @@ -85,9 +89,9 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): # Initialize the test module configuration data module.maxCounterValue = 2 if largeMinFireTime: - module.thrMinFireTime = 0.200 # seconds + module.thrMinFireTime = 0.200 # seconds else: - module.thrMinFireTime = 0.020 # seconds + module.thrMinFireTime = 0.020 # seconds # setup thruster cluster message fswSetupThrusters.clearSetup() @@ -99,8 +103,8 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): [-0.86360, -0.82550, -1.79070], [-0.82550, -0.86360, -1.79070], [0.82550, 0.86360, -1.79070], - [0.86360, 0.82550, -1.79070] - ] + [0.86360, 0.82550, -1.79070], + ] rcsDirectionData = [ [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], @@ -109,8 +113,8 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 1.0, 0.0], - [1.0, 0.0, 0.0] - ] + [1.0, 0.0, 0.0], + ] for i in range(len(rcsLocationData)): fswSetupThrusters.create(rcsLocationData[i], rcsDirectionData[i], 2.0) @@ -124,7 +128,7 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): # setup the commanded angular momentum change message DeltaHInMsgData = messaging.CmdTorqueBodyMsgPayload() - DeltaHInMsgData.torqueRequestBody = [0., 0., 0.] + DeltaHInMsgData.torqueRequestBody = [0.0, 0.0, 0.0] deltaHInMsg = messaging.CmdTorqueBodyMsg() # Setup logging on the test module output message so that we get all the writes to it @@ -143,49 +147,55 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation unitTestSim.ExecuteSimulation() # write the input Delta_H message deltaHInMsg.write(DeltaHInMsgData, macros.sec2nano(0.5)) - unitTestSim.ConfigureStopTime(macros.sec2nano(3.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(3.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() if resetCheck: # reset the module to test this functionality - module.Reset(macros.sec2nano(3.0)) # this module reset function needs a time input (in NanoSeconds) + module.Reset( + macros.sec2nano(3.0) + ) # this module reset function needs a time input (in NanoSeconds) # run the module again for an additional 1.0 seconds - unitTestSim.ConfigureStopTime(macros.sec2nano(3.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime( + macros.sec2nano(3.5) + ) # seconds to stop simulation unitTestSim.ExecuteSimulation() # re-write the input Delta_H message so that it checks for a new message deltaHInMsg.write(DeltaHInMsgData, macros.sec2nano(3.5)) - unitTestSim.ConfigureStopTime(macros.sec2nano(5.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime( + macros.sec2nano(5.5) + ) # seconds to stop simulation unitTestSim.ExecuteSimulation() # This pulls the actual data log from the simulation run. moduleOutput = dataLog.OnTimeRequest[:, :numThrusters] # set the filtered output truth states - if resetCheck==1: + if resetCheck == 1: trueVector = [ - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.5, 0.1, 0.0, 0.5, 0.5, 0.1, 0.5, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.1, 0.0, 0.0, 0.3, 0.1, 0.0, 0.3, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.5, 0.1, 0.0, 0.5, 0.5, 0.1, 0.5, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.1, 0.0, 0.0, 0.3, 0.1, 0.0, 0.3, 0.0] - ] + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.5, 0.1, 0.0, 0.5, 0.5, 0.1, 0.5, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.1, 0.0, 0.0, 0.3, 0.1, 0.0, 0.3, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.5, 0.1, 0.0, 0.5, 0.5, 0.1, 0.5, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.1, 0.0, 0.0, 0.3, 0.1, 0.0, 0.3, 0.0], + ] else: if largeMinFireTime: trueVector = [ @@ -195,40 +205,41 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.3, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], ] else: trueVector = [ - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.5, 0.1, 0.0, 0.5, 0.5, 0.1, 0.5, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.1, 0.0, 0.0, 0.3, 0.1, 0.0, 0.3, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - ] + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.5, 0.1, 0.0, 0.5, 0.5, 0.1, 0.5, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.1, 0.0, 0.0, 0.3, 0.1, 0.0, 0.3, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ] # compare the module results to the truth values accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, accuracy, - "OnTimeRequest", testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, moduleOutput, accuracy, "OnTimeRequest", testFailCount, testMessages + ) snippentName = "passFail" + str(resetCheck) + str(largeMinFireTime) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -236,8 +247,8 @@ def thrMomentumDumpingTestFunction(show_plots, resetCheck, largeMinFireTime): # stand-along python script # if __name__ == "__main__": - test_thrMomentumDumping( # update "module" in function name - True, - False, # resetCheck - False # largeMinFireTime - ) + test_thrMomentumDumping( # update "module" in function name + True, + False, # resetCheck + False, # largeMinFireTime + ) diff --git a/src/fswAlgorithms/effectorInterfaces/thrustRWDesat/_UnitTest/test_thrustRWDesat.py b/src/fswAlgorithms/effectorInterfaces/thrustRWDesat/_UnitTest/test_thrustRWDesat.py index 8db3e7dce8..bd36bf6505 100644 --- a/src/fswAlgorithms/effectorInterfaces/thrustRWDesat/_UnitTest/test_thrustRWDesat.py +++ b/src/fswAlgorithms/effectorInterfaces/thrustRWDesat/_UnitTest/test_thrustRWDesat.py @@ -6,17 +6,23 @@ from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import thrustRWDesat -from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros, fswSetupThrusters +from Basilisk.utilities import ( + SimulationBaseClass, + unitTestSupport, + macros, + fswSetupThrusters, +) import numpy as np def test_thrustRWDesat(): - """ Test thrustRWDesat. """ + """Test thrustRWDesat.""" [testResults, testMessage] = thrustRWDesatTestFunction() assert testResults < 1, testMessage + def thrustRWDesatTestFunction(): - """ Test the thrustRWDesat module. Setup a simulation, """ + """Test the thrustRWDesat module. Setup a simulation,""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -32,17 +38,21 @@ def thrustRWDesatTestFunction(): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process # Construct the thrustRWDesat module # Set the names for the input messages module = thrustRWDesat.thrustRWDesat() # Set the necessary data in the module. NOTE: This information is more or less random - module.thrFiringPeriod = .5 # The amount of time to rest between thruster firings [s] - module.DMThresh = 20 # The point at which to stop decrementing momentum [r/s] - module.currDMDir = [1, 0, 0] # The current direction of momentum reduction - module.maxFiring = 5 # Maximum time to fire a jet for [s] + module.thrFiringPeriod = ( + 0.5 # The amount of time to rest between thruster firings [s] + ) + module.DMThresh = 20 # The point at which to stop decrementing momentum [r/s] + module.currDMDir = [1, 0, 0] # The current direction of momentum reduction + module.maxFiring = 5 # Maximum time to fire a jet for [s] # This calls the algContain to setup the selfInit, update, and reset module.ModelTag = "thrustRWDesat" @@ -64,21 +74,27 @@ def thrustRWDesatTestFunction(): rwConfigElementList = list() for rw in range(numRW): rwConfigElementMsg = messaging.RWConfigElementMsgPayload() - rwConfigElementMsg.gsHat_B = gsHat[rw] # Spin axis unit vector of the wheel in structure # [1, 0, 0] + rwConfigElementMsg.gsHat_B = gsHat[ + rw + ] # Spin axis unit vector of the wheel in structure # [1, 0, 0] rwConfigElementMsg.Js = 0.08 # Spin axis inertia of wheel [kgm2] rwConfigElementMsg.uMax = 0.2 # maximum RW motor torque [Nm] # Add this to the list rwConfigElementList.append(rwConfigElementMsg) - inputSpeedMsg.wheelSpeeds = [20, 10, 30] # The current angular velocities of the RW wheel + inputSpeedMsg.wheelSpeeds = [ + 20, + 10, + 30, + ] # The current angular velocities of the RW wheel # Set the array of the reaction wheels in RWConstellationFswMsg to the list created above inputRWConstellationMsg.reactionWheels = rwConfigElementList # Initialize the msg that gives the mass properties. This just needs the center of mass value inputVehicleMsg = messaging.VehicleConfigMsgPayload() - inputVehicleMsg.CoM_B = [0, 0, 0] # This is random. + inputVehicleMsg.CoM_B = [0, 0, 0] # This is random. # setup thruster cluster message fswSetupThrusters.clearSetup() @@ -90,8 +106,8 @@ def thrustRWDesatTestFunction(): [-0.86360, -0.82550, -1.79070], [-0.82550, -0.86360, -1.79070], [0.82550, 0.86360, -1.79070], - [0.86360, 0.82550, -1.79070] - ] + [0.86360, 0.82550, -1.79070], + ] rcsDirectionData = [ [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], @@ -100,8 +116,8 @@ def thrustRWDesatTestFunction(): [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 1.0, 0.0], - [1.0, 0.0, 0.0] - ] + [1.0, 0.0, 0.0], + ] for i in range(len(rcsLocationData)): fswSetupThrusters.create(rcsLocationData[i], rcsDirectionData[i], 2.0) @@ -131,7 +147,7 @@ def thrustRWDesatTestFunction(): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -146,12 +162,15 @@ def thrustRWDesatTestFunction(): # Now call Reset() and verify output is zeroed module.Reset(0) # Pass 0 as currentSimNanos - expectedOnTime = [0.] * numThrusters - testFailCount, testMessages = unitTestSupport.compareVector(np.array(expectedOnTime), - np.array(dataLog.OnTimeRequest[0][:numThrusters]), - 1e-3, - "Reset() zeroed onTime", - testFailCount, testMessages) + expectedOnTime = [0.0] * numThrusters + testFailCount, testMessages = unitTestSupport.compareVector( + np.array(expectedOnTime), + np.array(dataLog.OnTimeRequest[0][:numThrusters]), + 1e-3, + "Reset() zeroed onTime", + testFailCount, + testMessages, + ) print("Accuracy used: " + str(1e-3)) @@ -161,24 +180,68 @@ def thrustRWDesatTestFunction(): # print(outputThrData) # This is just what is outputted... - trueVector = [[0., 0., 0., 0., 0., 0., 0., 0.], - [0.00000000e+00, 1.97181559e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.0, 0.0, 0.0], - [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.0, 1.28062495e+00, 0.0, 0.0], - [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.0, 1.28062495e+00, 0.0, 0.0], - [ 0.00000000e+00, 1.97181559e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.0, 0.0, 0.0]] + trueVector = [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [ + 0.00000000e00, + 1.97181559e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.0, + 0.0, + 0.0, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.0, + 1.28062495e00, + 0.0, + 0.0, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.0, + 1.28062495e00, + 0.0, + 0.0, + ], + [ + 0.00000000e00, + 1.97181559e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.0, + 0.0, + 0.0, + ], + ] accuracy = 1e-6 # At each timestep, make sure the vehicleConfig values haven't changed from the initial values - testFailCount, testMessages = unitTestSupport.compareArrayND(trueVector[:len(outputThrData)], outputThrData, - accuracy, - "ThrustRWDesat output", - numThrusters, testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + trueVector[: len(outputThrData)], + outputThrData, + accuracy, + "ThrustRWDesat output", + numThrusters, + testFailCount, + testMessages, + ) if testFailCount == 0: print("Passed") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + -if __name__ == '__main__': +if __name__ == "__main__": test_thrustRWDesat() diff --git a/src/fswAlgorithms/effectorInterfaces/thrusterPlatformReference/_UnitTest/test_thrusterPlatformReference.py b/src/fswAlgorithms/effectorInterfaces/thrusterPlatformReference/_UnitTest/test_thrusterPlatformReference.py index 47fdaf62b6..963af244c1 100644 --- a/src/fswAlgorithms/effectorInterfaces/thrusterPlatformReference/_UnitTest/test_thrusterPlatformReference.py +++ b/src/fswAlgorithms/effectorInterfaces/thrusterPlatformReference/_UnitTest/test_thrusterPlatformReference.py @@ -22,7 +22,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) @@ -39,10 +39,10 @@ # of the multiple test runs for this test. Note that the order in that you add the parametrize method # matters for the documentation in that it impacts the order in which the test arguments are shown. # The first parametrize arguments are shown last in the pytest argument list -@pytest.mark.parametrize("seed", list(np.linspace(1,10,10))) +@pytest.mark.parametrize("seed", list(np.linspace(1, 10, 10))) @pytest.mark.parametrize("delta_CM", [0.1, 0.2, 0.3]) -@pytest.mark.parametrize("K", [0,1,5,10]) -@pytest.mark.parametrize("thetaMax", [-1, np.pi/36]) +@pytest.mark.parametrize("K", [0, 1, 5, 10]) +@pytest.mark.parametrize("thetaMax", [-1, np.pi / 36]) @pytest.mark.parametrize("accuracy", [1e-10]) # update "module" in this function name to reflect the module name def test_platformRotation(show_plots, delta_CM, K, thetaMax, seed, accuracy): @@ -88,7 +88,6 @@ def test_platformRotation(show_plots, delta_CM, K, thetaMax, seed, accuracy): def platformRotationTestFunction(show_plots, delta_CM, K, thetaMax, seed, accuracy): - random.seed(seed) euler_angles_123 = np.array([5.0 * macros.D2R, 10.0 * macros.D2R, 0.0]) @@ -96,20 +95,20 @@ def platformRotationTestFunction(show_plots, delta_CM, K, thetaMax, seed, accura r_BM_M = np.array([0.0, 0.1, 1.4]) r_FM_F = np.array([0.0, 0.0, -0.1]) r_TF_F = np.array([-0.01, 0.03, 0.02]) - T_F = np.array([1.0, 1.0, 10.0]) + T_F = np.array([1.0, 1.0, 10.0]) - r_CB_B = np.array([0,0,0]) + np.random.rand(3) + r_CB_B = np.array([0, 0, 0]) + np.random.rand(3) r_CB_B = r_CB_B / np.linalg.norm(r_CB_B) * delta_CM - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(1) # update process rate update time + testProcessRate = macros.sec2nano(1) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -145,7 +144,7 @@ def platformRotationTestFunction(show_plots, delta_CM, K, thetaMax, seed, accura # Create input RW configuration msg inputRWConfigMsgData = messaging.RWArrayConfigMsgPayload() - inputRWConfigMsgData.GsMatrix_B = [1,0,0,0,1,0,0,0,1] + inputRWConfigMsgData.GsMatrix_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] inputRWConfigMsgData.JsList = [0.01, 0.01, 0.01] inputRWConfigMsgData.numRW = 3 inputRWConfigMsgData.uMax = [0.001, 0.001, 0.001] @@ -177,7 +176,7 @@ def platformRotationTestFunction(show_plots, delta_CM, K, thetaMax, seed, accura # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -193,23 +192,29 @@ def platformRotationTestFunction(show_plots, delta_CM, K, thetaMax, seed, accura r_CM_F = np.matmul(FM, r_CM_M) r_CT_F = r_CM_F - r_FM_F - r_TF_F - offset = np.linalg.norm(np.cross(r_CT_F,T_F) / np.linalg.norm(np.array(r_CT_F)) / np.linalg.norm(np.array(T_F))) + offset = np.linalg.norm( + np.cross(r_CT_F, T_F) + / np.linalg.norm(np.array(r_CT_F)) + / np.linalg.norm(np.array(T_F)) + ) # check if the CM offset is zero if control gain K is also 0 if K == 0 and thetaMax < 0: np.testing.assert_allclose(offset, 0.0, rtol=0, atol=accuracy, verbose=True) - T_B_hat_sim = bodyHeadingLog.rHat_XB_B[0] # simulation result + T_B_hat_sim = bodyHeadingLog.rHat_XB_B[0] # simulation result FB = np.matmul(FM, MB) T_B = np.matmul(FB.transpose(), T_F) - T_B_hat = T_B / np.linalg.norm(T_B) # truth value + T_B_hat = T_B / np.linalg.norm(T_B) # truth value # compare the module results to the python computation for body-frame thruster direction - np.testing.assert_allclose(T_B_hat_sim, T_B_hat, rtol=0, atol=accuracy, verbose=True) + np.testing.assert_allclose( + T_B_hat_sim, T_B_hat, rtol=0, atol=accuracy, verbose=True + ) - L_B_sim = thrusterTorqueLog.torqueRequestBody[0] # simulation result + L_B_sim = thrusterTorqueLog.torqueRequestBody[0] # simulation result L_F = np.cross(r_CT_F, T_F) - L_B = np.matmul(FB.transpose(),L_F) + L_B = np.matmul(FB.transpose(), L_F) # compare the module results to the python computation for body-frame cmd torque np.testing.assert_allclose(L_B_sim, L_B, rtol=0, atol=accuracy, verbose=True) @@ -221,7 +226,9 @@ def platformRotationTestFunction(show_plots, delta_CM, K, thetaMax, seed, accura tMax_sim = thrConfigBLog.maxThrust[0] np.testing.assert_allclose(r_TB_B_sim, r_TB_B, rtol=0, atol=accuracy, verbose=True) np.testing.assert_allclose(tHat_B_sim, T_B_hat, rtol=0, atol=accuracy, verbose=True) - np.testing.assert_allclose(tMax_sim, np.linalg.norm(T_B), rtol=0, atol=accuracy, verbose=True) + np.testing.assert_allclose( + tMax_sim, np.linalg.norm(T_B), rtol=0, atol=accuracy, verbose=True + ) # compare the output reference angle if thetaMax > 0: @@ -237,10 +244,10 @@ def platformRotationTestFunction(show_plots, delta_CM, K, thetaMax, seed, accura # if __name__ == "__main__": test_platformRotation( - False, # show_plots - 0.1, # delta_CM - 0, # K - -1, # thetaMax - np.random.rand(1)[0], # seed - 1e-10 # accuracy - ) + False, # show_plots + 0.1, # delta_CM + 0, # K + -1, # thetaMax + np.random.rand(1)[0], # seed + 1e-10, # accuracy + ) diff --git a/src/fswAlgorithms/effectorInterfaces/thrusterPlatformState/_UnitTest/test_thrusterPlatformState.py b/src/fswAlgorithms/effectorInterfaces/thrusterPlatformState/_UnitTest/test_thrusterPlatformState.py index 4ac2a4e07f..ac7920a69a 100644 --- a/src/fswAlgorithms/effectorInterfaces/thrusterPlatformState/_UnitTest/test_thrusterPlatformState.py +++ b/src/fswAlgorithms/effectorInterfaces/thrusterPlatformState/_UnitTest/test_thrusterPlatformState.py @@ -23,7 +23,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) @@ -37,8 +37,8 @@ from Basilisk.utilities import unitTestSupport -@pytest.mark.parametrize("theta1", [0, np.pi/36, np.pi/18]) -@pytest.mark.parametrize("theta2", [0, np.pi/36, np.pi/18]) +@pytest.mark.parametrize("theta1", [0, np.pi / 36, np.pi / 18]) +@pytest.mark.parametrize("theta2", [0, np.pi / 36, np.pi / 18]) @pytest.mark.parametrize("accuracy", [1e-10]) # update "module" in this function name to reflect the module name def test_platformRotation(show_plots, theta1, theta2, accuracy): @@ -73,23 +73,22 @@ def test_platformRotation(show_plots, theta1, theta2, accuracy): def platformRotationTestFunction(show_plots, theta1, theta2, accuracy): - - sigma_MB = np.array([0., 0., 0.]) + sigma_MB = np.array([0.0, 0.0, 0.0]) r_BM_M = np.array([0.0, 0.1, 1.4]) r_FM_F = np.array([0.0, 0.0, -0.1]) r_TF_F = np.array([-0.01, 0.03, 0.02]) - T_F = np.array([1.0, 1.0, 10.0]) + T_F = np.array([1.0, 1.0, 10.0]) swirlFactor = 0.1 - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(1) # update process rate update time + testProcessRate = macros.sec2nano(1) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -135,7 +134,7 @@ def platformRotationTestFunction(show_plots, theta1, theta2, accuracy): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.5)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -149,13 +148,23 @@ def platformRotationTestFunction(show_plots, theta1, theta2, accuracy): MB = rbk.MRP2C(sigma_MB) FB = np.matmul(FM, MB) - r_TB_B = np.matmul(FB.transpose(), r_TF_F + r_FM_F - np.matmul(FM, r_BM_M)) # thrust application point - tHat_B = np.matmul(FB.transpose(), T_F) / np.linalg.norm(T_F) # thrust unit direction vector + r_TB_B = np.matmul( + FB.transpose(), r_TF_F + r_FM_F - np.matmul(FM, r_BM_M) + ) # thrust application point + tHat_B = np.matmul(FB.transpose(), T_F) / np.linalg.norm( + T_F + ) # thrust unit direction vector np.testing.assert_allclose(rThrust_B, r_TB_B, rtol=0, atol=accuracy, verbose=True) - np.testing.assert_allclose(tHatThrust_B, tHat_B, rtol=0, atol=accuracy, verbose=True) - np.testing.assert_allclose(tMax, np.linalg.norm(T_F), rtol=0, atol=accuracy, verbose=True) - np.testing.assert_allclose(tSwirl, np.linalg.norm(T_F) * swirlFactor, rtol=0, atol=accuracy, verbose=True) + np.testing.assert_allclose( + tHatThrust_B, tHat_B, rtol=0, atol=accuracy, verbose=True + ) + np.testing.assert_allclose( + tMax, np.linalg.norm(T_F), rtol=0, atol=accuracy, verbose=True + ) + np.testing.assert_allclose( + tSwirl, np.linalg.norm(T_F) * swirlFactor, rtol=0, atol=accuracy, verbose=True + ) return @@ -172,14 +181,14 @@ def test_reset(): unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(1) # update process rate update time + testProcessRate = macros.sec2nano(1) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C container platform = thrusterPlatformState.thrusterPlatformState() platform.ModelTag = "platformState" - platform.sigma_MB = [0., 0., 0.] + platform.sigma_MB = [0.0, 0.0, 0.0] platform.r_BM_M = [0.0, 0.1, 1.4] platform.r_FM_F = [0.0, 0.0, -0.1] unitTestSim.AddModelToTask(unitTaskName, platform) @@ -195,12 +204,12 @@ def test_reset(): # Create input hinged rigid body messages with non-zero angles hingedBodyMsg1 = messaging.HingedRigidBodyMsgPayload() - hingedBodyMsg1.theta = np.pi/36 + hingedBodyMsg1.theta = np.pi / 36 hingedBody1InMsg = messaging.HingedRigidBodyMsg().write(hingedBodyMsg1) platform.hingedRigidBody1InMsg.subscribeTo(hingedBody1InMsg) hingedBodyMsg2 = messaging.HingedRigidBodyMsgPayload() - hingedBodyMsg2.theta = np.pi/36 + hingedBodyMsg2.theta = np.pi / 36 hingedBody2InMsg = messaging.HingedRigidBodyMsg().write(hingedBodyMsg2) platform.hingedRigidBody2InMsg.subscribeTo(hingedBody2InMsg) @@ -220,29 +229,41 @@ def test_reset(): # Check that thruster config output is zeroed expectedConfig = messaging.THRConfigMsgPayload() # This creates a zeroed message - testFailCount, testMessages = unitTestSupport.compareVector(np.array(expectedConfig.rThrust_B), - np.array(platform.thrusterConfigBOutMsg.read().rThrust_B), - 1e-12, - "Reset() zeroed rThrust_B", - testFailCount, testMessages) - - testFailCount, testMessages = unitTestSupport.compareVector(np.array(expectedConfig.tHatThrust_B), - np.array(platform.thrusterConfigBOutMsg.read().tHatThrust_B), - 1e-12, - "Reset() zeroed tHatThrust_B", - testFailCount, testMessages) - - testFailCount, testMessages = unitTestSupport.compareVector(np.array([expectedConfig.maxThrust]), - np.array([platform.thrusterConfigBOutMsg.read().maxThrust]), - 1e-12, - "Reset() zeroed maxThrust", - testFailCount, testMessages) - - testFailCount, testMessages = unitTestSupport.compareVector(np.array([expectedConfig.swirlTorque]), - np.array([platform.thrusterConfigBOutMsg.read().swirlTorque]), - 1e-12, - "Reset() zeroed swirlTorque", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareVector( + np.array(expectedConfig.rThrust_B), + np.array(platform.thrusterConfigBOutMsg.read().rThrust_B), + 1e-12, + "Reset() zeroed rThrust_B", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = unitTestSupport.compareVector( + np.array(expectedConfig.tHatThrust_B), + np.array(platform.thrusterConfigBOutMsg.read().tHatThrust_B), + 1e-12, + "Reset() zeroed tHatThrust_B", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = unitTestSupport.compareVector( + np.array([expectedConfig.maxThrust]), + np.array([platform.thrusterConfigBOutMsg.read().maxThrust]), + 1e-12, + "Reset() zeroed maxThrust", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = unitTestSupport.compareVector( + np.array([expectedConfig.swirlTorque]), + np.array([platform.thrusterConfigBOutMsg.read().swirlTorque]), + 1e-12, + "Reset() zeroed swirlTorque", + testFailCount, + testMessages, + ) # Print success message if no errors were found if testFailCount == 0: @@ -250,7 +271,7 @@ def test_reset(): else: print("Failed: " + platform.ModelTag + " Reset() test") - assert testFailCount == 0, ''.join(testMessages) + assert testFailCount == 0, "".join(testMessages) # @@ -259,8 +280,8 @@ def test_reset(): # if __name__ == "__main__": test_platformRotation( - False, # show_plots - 0, # theta1 - np.pi/36, # theta2 - 1e-10 # accuracy + False, # show_plots + 0, # theta1 + np.pi / 36, # theta2 + 1e-10, # accuracy ) diff --git a/src/fswAlgorithms/effectorInterfaces/torque2Dipole/_UnitTest/test_torque2Dipole.py b/src/fswAlgorithms/effectorInterfaces/torque2Dipole/_UnitTest/test_torque2Dipole.py index 60ba1b010b..a39b452832 100644 --- a/src/fswAlgorithms/effectorInterfaces/torque2Dipole/_UnitTest/test_torque2Dipole.py +++ b/src/fswAlgorithms/effectorInterfaces/torque2Dipole/_UnitTest/test_torque2Dipole.py @@ -27,13 +27,18 @@ import numpy as np from Basilisk.architecture import bskLogging from Basilisk.architecture import messaging # import the message definitions -from Basilisk.fswAlgorithms import torque2Dipole # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + torque2Dipole, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions -accuracy = 1E-12 +accuracy = 1e-12 # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed # @pytest.mark.skipif(conditionstring) @@ -41,7 +46,8 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ -def test_torque2Dipole_module(): # update "module" in this function name to reflect the module name + +def test_torque2Dipole_module(): # update "module" in this function name to reflect the module name r""" **Validation Test Description** @@ -60,34 +66,35 @@ def test_torque2Dipole_module(): # update "module" in this function name to [testResults, testMessage] = torque2DipoleModuleTestFunction() assert testResults < 1, testMessage + def torque2DipoleModuleTestFunction(): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.01) # update process rate update time + testProcessRate = macros.sec2nano(0.01) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Initialize module under test's config message and add module to runtime call list module = torque2Dipole.torque2Dipole() - module.ModelTag = "mtbMomentumManagement" # update python name of test module + module.ModelTag = "mtbMomentumManagement" # update python name of test module unitTestSim.AddModelToTask(unitTaskName, module) # Initialize TAMSensorBodyMsg tamSensorBodyInMsgContainer = messaging.TAMSensorBodyMsgPayload() - tamSensorBodyInMsgContainer.tam_B = [1E-5, 0.0, 0.0] + tamSensorBodyInMsgContainer.tam_B = [1e-5, 0.0, 0.0] tamSensorBodyInMsg = messaging.TAMSensorBodyMsg().write(tamSensorBodyInMsgContainer) # Initialize CmdTorqueBodyMsg tauRequestInMsgContainer = messaging.CmdTorqueBodyMsgPayload() - tauRequestInMsgContainer.torqueRequestBody = [10.* 1E-3, 20. * 1E-3, 30 * 1E-3] + tauRequestInMsgContainer.torqueRequestBody = [10.0 * 1e-3, 20.0 * 1e-3, 30 * 1e-3] tauRequestInMsg = messaging.CmdTorqueBodyMsg().write(tauRequestInMsgContainer) # Setup logging on the test module output message so that we get all the writes to it @@ -99,67 +106,76 @@ def torque2DipoleModuleTestFunction(): module.tauRequestInMsg.subscribeTo(tauRequestInMsg) # Set the simulation time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) # seconds to stop simulation unitTestSim.InitializeSimulation() - ''' + """ TEST 1: Check that dipole_B is non-zero expected value. - ''' + """ unitTestSim.ExecuteSimulation() b = np.array(tamSensorBodyInMsgContainer.tam_B) tau = np.array(tauRequestInMsgContainer.torqueRequestBody) expectedDipole = 1 / np.dot(b, b) * np.cross(b, tau) - testFailCount, testMessages = unitTestSupport.compareVector(expectedDipole, - resultDipoleRequestOutMsg.dipole_B[0], - accuracy, - "dipole_B", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareVector( + expectedDipole, + resultDipoleRequestOutMsg.dipole_B[0], + accuracy, + "dipole_B", + testFailCount, + testMessages, + ) - ''' + """ TEST 2: Check that dipole_B is zero when tam_B is zero. - ''' - tamSensorBodyInMsgContainer.tam_B = [0., 0., 0.] + """ + tamSensorBodyInMsgContainer.tam_B = [0.0, 0.0, 0.0] tamSensorBodyInMsg = messaging.TAMSensorBodyMsg().write(tamSensorBodyInMsgContainer) module.tamSensorBodyInMsg.subscribeTo(tamSensorBodyInMsg) unitTestSim.InitializeSimulation() unitTestSim.ExecuteSimulation() - expectedDipole = [0., 0., 0.] - testFailCount, testMessages = unitTestSupport.compareVector(expectedDipole, - resultDipoleRequestOutMsg.dipole_B[0], - accuracy, - "dipole_B", - testFailCount, testMessages) + expectedDipole = [0.0, 0.0, 0.0] + testFailCount, testMessages = unitTestSupport.compareVector( + expectedDipole, + resultDipoleRequestOutMsg.dipole_B[0], + accuracy, + "dipole_B", + testFailCount, + testMessages, + ) - ''' + """ TEST 3: Check that dipole_B is zero when torqueRequestBody is zero. - ''' - tamSensorBodyInMsgContainer.tam_B = [1E-5, 0.0, 0.0] + """ + tamSensorBodyInMsgContainer.tam_B = [1e-5, 0.0, 0.0] tamSensorBodyInMsg = messaging.TAMSensorBodyMsg().write(tamSensorBodyInMsgContainer) module.tamSensorBodyInMsg.subscribeTo(tamSensorBodyInMsg) - tauRequestInMsgContainer.torqueRequestBody = [0., 0., 0.] + tauRequestInMsgContainer.torqueRequestBody = [0.0, 0.0, 0.0] tauRequestInMsg = messaging.CmdTorqueBodyMsg().write(tauRequestInMsgContainer) module.tauRequestInMsg.subscribeTo(tauRequestInMsg) unitTestSim.InitializeSimulation() unitTestSim.ExecuteSimulation() - expectedDipole = [0., 0., 0.] - testFailCount, testMessages = unitTestSupport.compareVector(expectedDipole, - resultDipoleRequestOutMsg.dipole_B[0], - accuracy, - "dipole_B", - testFailCount, testMessages) + expectedDipole = [0.0, 0.0, 0.0] + testFailCount, testMessages = unitTestSupport.compareVector( + expectedDipole, + resultDipoleRequestOutMsg.dipole_B[0], + accuracy, + "dipole_B", + testFailCount, + testMessages, + ) - ''' + """ TEST 4: Check that Reset() zeros the output message. - ''' + """ # First set a non-zero torque request torqueMessageData = messaging.CmdTorqueBodyMsgPayload() torqueMessageData.torqueRequest = [1.0, 0.5, 0.7] @@ -171,19 +187,22 @@ def torque2DipoleModuleTestFunction(): # Now call Reset() and verify output is zeroed module.Reset(0) # Pass 0 as currentSimNanos - expectedDipole = [0., 0., 0.] - testFailCount, testMessages = unitTestSupport.compareVector(np.array(expectedDipole), - np.array(module.dipoleRequestOutMsg.read().dipole_B), - 1e-3, - "Reset() zeroed dipole request", - testFailCount, testMessages) + expectedDipole = [0.0, 0.0, 0.0] + testFailCount, testMessages = unitTestSupport.compareVector( + np.array(expectedDipole), + np.array(module.dipoleRequestOutMsg.read().dipole_B), + 1e-3, + "Reset() zeroed dipole request", + testFailCount, + testMessages, + ) print("Accuracy used: " + str(accuracy)) if testFailCount == 0: print("PASSED: torque2Dipole unit test") else: print("Failed: torque2Dipole unit test") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/effectorInterfaces/torqueScheduler/_UnitTest/test_torqueScheduler.py b/src/fswAlgorithms/effectorInterfaces/torqueScheduler/_UnitTest/test_torqueScheduler.py index a0bba3e275..bbd7ebdfc2 100644 --- a/src/fswAlgorithms/effectorInterfaces/torqueScheduler/_UnitTest/test_torqueScheduler.py +++ b/src/fswAlgorithms/effectorInterfaces/torqueScheduler/_UnitTest/test_torqueScheduler.py @@ -27,11 +27,16 @@ import pytest from Basilisk.architecture import bskLogging from Basilisk.architecture import messaging # import the message definitions -from Basilisk.fswAlgorithms import torqueScheduler # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + torqueScheduler, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import numpy as np @@ -47,8 +52,6 @@ @pytest.mark.parametrize("lockFlag", [0, 1, 2, 3]) @pytest.mark.parametrize("tSwitch", [3, 6]) @pytest.mark.parametrize("accuracy", [1e-12]) - - def test_torqueScheduler(lockFlag, tSwitch, accuracy): r""" **Validation Test Description** @@ -74,23 +77,24 @@ def test_torqueScheduler(lockFlag, tSwitch, accuracy): and that the flags contained in ``effectorLockOutMsg`` are consistent with the schedule logic that the user is requesting. """ # each test method requires a single assert method to be called - [testResults, testMessage] = torqueSchedulerTestFunction(lockFlag, tSwitch, accuracy) + [testResults, testMessage] = torqueSchedulerTestFunction( + lockFlag, tSwitch, accuracy + ) assert testResults < 1, testMessage def torqueSchedulerTestFunction(lockFlag, tSwitch, accuracy): - - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(1) # update process rate update time + testProcessRate = macros.sec2nano(1) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -126,7 +130,7 @@ def torqueSchedulerTestFunction(lockFlag, tSwitch, accuracy): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(10)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(10)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -135,61 +139,145 @@ def torqueSchedulerTestFunction(lockFlag, tSwitch, accuracy): time = torqueLog.times() * macros.NANO2SEC for i in range(len(time)): - if not unitTestSupport.isDoubleEqual(torqueLog.motorTorque[i][0], motorTorque1InMsgData.motorTorque[0], accuracy): + if not unitTestSupport.isDoubleEqual( + torqueLog.motorTorque[i][0], motorTorque1InMsgData.motorTorque[0], accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at passing motor torque #1 value") - if not unitTestSupport.isDoubleEqual(torqueLog.motorTorque[i][1], motorTorque2InMsgData.motorTorque[0], accuracy): + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at passing motor torque #1 value" + ) + if not unitTestSupport.isDoubleEqual( + torqueLog.motorTorque[i][1], motorTorque2InMsgData.motorTorque[0], accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at passing motor torque #2 value") + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at passing motor torque #2 value" + ) if lockFlag == 0: - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][0], 0, accuracy): + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][0], 0, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #1") - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][1], 0, accuracy): + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #1" + ) + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][1], 0, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #2") + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #2" + ) elif lockFlag == 1: if time[i] > tSwitch: - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][0], 1, accuracy): + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][0], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #1") - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][1], 0, accuracy): + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #1" + ) + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][1], 0, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #2") + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #2" + ) else: - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][0], 0, accuracy): + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][0], 0, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #1") - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][1], 1, accuracy): + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #1" + ) + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][1], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #2") + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #2" + ) elif lockFlag == 2: if time[i] > tSwitch: - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][0], 0, accuracy): + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][0], 0, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #1") - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][1], 1, accuracy): + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #1" + ) + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][1], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #2") + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #2" + ) else: - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][0], 1, accuracy): + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][0], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #1") - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][1], 0, accuracy): + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #1" + ) + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][1], 0, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #2") + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #2" + ) else: - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][0], 1, accuracy): + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][0], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #1") - if not unitTestSupport.isDoubleEqual(lockLog.effectorLockFlag[i][1], 1, accuracy): + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #1" + ) + if not unitTestSupport.isDoubleEqual( + lockLog.effectorLockFlag[i][1], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + scheduler.ModelTag + " module failed at outputting effector flag #2") + testMessages.append( + "FAILED: " + + scheduler.ModelTag + + " module failed at outputting effector flag #2" + ) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def test_reset(): @@ -204,7 +292,7 @@ def test_reset(): unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(1) # update process rate update time + testProcessRate = macros.sec2nano(1) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -217,13 +305,17 @@ def test_reset(): # Create input array motor torque msg #1 motorTorque1InMsgData = messaging.ArrayMotorTorqueMsgPayload() - motorTorque1InMsgData.motorTorque = [1.0] * messaging.MAX_EFF_CNT # Fill with non-zero values + motorTorque1InMsgData.motorTorque = [ + 1.0 + ] * messaging.MAX_EFF_CNT # Fill with non-zero values motorTorque1InMsg = messaging.ArrayMotorTorqueMsg().write(motorTorque1InMsgData) scheduler.motorTorque1InMsg.subscribeTo(motorTorque1InMsg) # Create input array motor torque msg #2 motorTorque2InMsgData = messaging.ArrayMotorTorqueMsgPayload() - motorTorque2InMsgData.motorTorque = [3.0] * messaging.MAX_EFF_CNT # Fill with non-zero values + motorTorque2InMsgData.motorTorque = [ + 3.0 + ] * messaging.MAX_EFF_CNT # Fill with non-zero values motorTorque2InMsg = messaging.ArrayMotorTorqueMsg().write(motorTorque2InMsgData) scheduler.motorTorque2InMsg.subscribeTo(motorTorque2InMsg) @@ -245,19 +337,25 @@ def test_reset(): # Check that motor torque output is zeroed expectedTorque = [0.0] * messaging.MAX_EFF_CNT # Array of zeros with correct size - testFailCount, testMessages = unitTestSupport.compareVector(np.array(expectedTorque), - np.array(scheduler.motorTorqueOutMsg.read().motorTorque), - 1e-12, - "Reset() zeroed motor torque", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareVector( + np.array(expectedTorque), + np.array(scheduler.motorTorqueOutMsg.read().motorTorque), + 1e-12, + "Reset() zeroed motor torque", + testFailCount, + testMessages, + ) # Check that effector lock output is zeroed expectedLock = [0] * messaging.MAX_EFF_CNT # Array of zeros with correct size - testFailCount, testMessages = unitTestSupport.compareVector(np.array(expectedLock), - np.array(scheduler.effectorLockOutMsg.read().effectorLockFlag), - 1e-12, - "Reset() zeroed effector lock", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareVector( + np.array(expectedLock), + np.array(scheduler.effectorLockOutMsg.read().effectorLockFlag), + 1e-12, + "Reset() zeroed effector lock", + testFailCount, + testMessages, + ) # Print success message if no errors were found if testFailCount == 0: @@ -265,7 +363,7 @@ def test_reset(): else: print("Failed: " + scheduler.ModelTag + " Reset() test") - assert testFailCount == 0, ''.join(testMessages) + assert testFailCount == 0, "".join(testMessages) # @@ -273,8 +371,4 @@ def test_reset(): # stand-along python script # if __name__ == "__main__": - test_torqueScheduler( - 1, - 5, - 1e-12 - ) + test_torqueScheduler(1, 5, 1e-12) diff --git a/src/fswAlgorithms/effectorInterfaces/vscmgGimbalRateServo/_UnitTest/test_vscmgGimbalRateServo.py b/src/fswAlgorithms/effectorInterfaces/vscmgGimbalRateServo/_UnitTest/test_vscmgGimbalRateServo.py index 8464c7c6ab..59db0d4ccd 100644 --- a/src/fswAlgorithms/effectorInterfaces/vscmgGimbalRateServo/_UnitTest/test_vscmgGimbalRateServo.py +++ b/src/fswAlgorithms/effectorInterfaces/vscmgGimbalRateServo/_UnitTest/test_vscmgGimbalRateServo.py @@ -30,58 +30,64 @@ from Basilisk.utilities import macros from Basilisk.fswAlgorithms import vscmgGimbalRateServo + def defaultVSCMG(): VSCMG = messaging.VSCMGConfigMsgPayload() - VSCMG.rGB_B = [[0.],[0.],[0.]] - VSCMG.gsHat0_B = [[0.],[0.],[0.]] - VSCMG.gtHat0_B = [[0.],[0.],[0.]] - VSCMG.ggHat_B = [[0.],[0.],[0.]] + VSCMG.rGB_B = [[0.0], [0.0], [0.0]] + VSCMG.gsHat0_B = [[0.0], [0.0], [0.0]] + VSCMG.gtHat0_B = [[0.0], [0.0], [0.0]] + VSCMG.ggHat_B = [[0.0], [0.0], [0.0]] VSCMG.u_s_max = -1 VSCMG.u_s_min = -1 - VSCMG.u_s_f = 0. + VSCMG.u_s_f = 0.0 VSCMG.wheelLinearFrictionRatio = -1 - VSCMG.u_g_current = 0. + VSCMG.u_g_current = 0.0 VSCMG.u_g_max = -1 VSCMG.u_g_min = -1 - VSCMG.u_g_f = 0. + VSCMG.u_g_f = 0.0 VSCMG.gimbalLinearFrictionRatio = -1 VSCMG.Omega = 14.4 - VSCMG.gamma = 0. - VSCMG.gammaDot = 0. - VSCMG.Omega_max = 6000. * macros.RPM + VSCMG.gamma = 0.0 + VSCMG.gammaDot = 0.0 + VSCMG.Omega_max = 6000.0 * macros.RPM VSCMG.gammaDot_max = -1 VSCMG.IW1 = 0.1 VSCMG.IW2 = 0.04 VSCMG.IW3 = 0.03 VSCMG.IG1 = 0.03 - VSCMG.IG2 = 0. - VSCMG.IG3 = 0. - VSCMG.U_s = 0. - VSCMG.U_d = 0. - VSCMG.l = 0. - VSCMG.L = 0. - VSCMG.rGcG_G = [[0.],[0.],[0.]] - VSCMG.massW = 0. - VSCMG.massG = 0. + VSCMG.IG2 = 0.0 + VSCMG.IG3 = 0.0 + VSCMG.U_s = 0.0 + VSCMG.U_d = 0.0 + VSCMG.l = 0.0 + VSCMG.L = 0.0 + VSCMG.rGcG_G = [[0.0], [0.0], [0.0]] + VSCMG.massW = 0.0 + VSCMG.massG = 0.0 VSCMG.VSCMGModel = 0 return VSCMG + def setupVSCMGs(numVSCMGs): VSCMGs = [] - ang = 54.75 * np.pi/180 + ang = 54.75 * np.pi / 180 VSCMGs.append(defaultVSCMG()) VSCMGs[0].ggHat_B = [[math.cos(ang)], [0.0], [math.sin(ang)]] VSCMGs[0].gsHat0_B = [[0.0], [1.0], [0.0]] - VSCMGs[0].gtHat0_B = np.cross(np.array([math.cos(ang), 0.0, math.sin(ang)]),np.array([0.0, 1.0, 0.0])) + VSCMGs[0].gtHat0_B = np.cross( + np.array([math.cos(ang), 0.0, math.sin(ang)]), np.array([0.0, 1.0, 0.0]) + ) if numVSCMGs > 1: VSCMGs.append(defaultVSCMG()) VSCMGs[1].gsHat0_B = [[0.0], [-1.0], [0.0]] VSCMGs[1].ggHat_B = [[-math.cos(ang)], [0.0], [math.sin(ang)]] - VSCMGs[1].gtHat0_B = np.cross(np.array([-math.cos(ang), 0.0, math.sin(ang)]),np.array([0.0, -1.0, 0.0])) + VSCMGs[1].gtHat0_B = np.cross( + np.array([-math.cos(ang), 0.0, math.sin(ang)]), np.array([0.0, -1.0, 0.0]) + ) if numVSCMGs == 4: VSCMGs.append(defaultVSCMG()) @@ -90,14 +96,14 @@ def setupVSCMGs(numVSCMGs): ggHat2_B = np.array([0.0, math.cos(ang), math.sin(ang)]) gtHat2_t0_B = np.cross(ggHat2_B, gsHat2_t0_B) gtHat2_t0_B /= np.linalg.norm(gtHat2_t0_B) - BG2 = np.column_stack([gsHat2_t0_B, - gtHat2_t0_B, - ggHat2_B]) - GG02 = np.array([ - [-math.cos(VSCMGs[2].gamma), math.sin(VSCMGs[2].gamma), 0.0], - [-math.sin(VSCMGs[2].gamma), -math.cos(VSCMGs[2].gamma), 0.0], - [0.0, 0.0, 1.0] - ]) + BG2 = np.column_stack([gsHat2_t0_B, gtHat2_t0_B, ggHat2_B]) + GG02 = np.array( + [ + [-math.cos(VSCMGs[2].gamma), math.sin(VSCMGs[2].gamma), 0.0], + [-math.sin(VSCMGs[2].gamma), -math.cos(VSCMGs[2].gamma), 0.0], + [0.0, 0.0, 1.0], + ] + ) BG02 = BG2 @ GG02 VSCMGs[2].gsHat0_B = [[BG02[0][0]], [BG02[1][0]], [BG02[2][0]]] VSCMGs[2].gtHat0_B = [[BG02[0][1]], [BG02[1][1]], [BG02[2][1]]] @@ -109,14 +115,14 @@ def setupVSCMGs(numVSCMGs): ggHat3_B = np.array([0.0, -math.cos(ang), math.sin(ang)]) gtHat3_t0_B = np.cross(ggHat3_B, gsHat3_t0_B) gtHat3_t0_B /= np.linalg.norm(gtHat3_t0_B) - BG3 = np.column_stack([gsHat3_t0_B, - gtHat3_t0_B, - ggHat3_B]) - GG03 = np.array([ - [-math.cos(VSCMGs[3].gamma), math.sin(VSCMGs[3].gamma), 0.0], - [-math.sin(VSCMGs[3].gamma), -math.cos(VSCMGs[3].gamma), 0.0], - [0.0, 0.0, 1.0] - ]) + BG3 = np.column_stack([gsHat3_t0_B, gtHat3_t0_B, ggHat3_B]) + GG03 = np.array( + [ + [-math.cos(VSCMGs[3].gamma), math.sin(VSCMGs[3].gamma), 0.0], + [-math.sin(VSCMGs[3].gamma), -math.cos(VSCMGs[3].gamma), 0.0], + [0.0, 0.0, 1.0], + ] + ) BG03 = BG3 @ GG03 VSCMGs[3].gsHat0_B = [[BG03[0][0]], [BG03[1][0]], [BG03[2][0]]] VSCMGs[3].gtHat0_B = [[BG03[0][1]], [BG03[1][1]], [BG03[2][1]]] @@ -124,21 +130,23 @@ def setupVSCMGs(numVSCMGs): return VSCMGs -def computeTorques(VSCMGs, numVSCMGs, omega_BN_B, K_gamma, gamma_dot_d, Omega_dot_d): +def computeTorques(VSCMGs, numVSCMGs, omega_BN_B, K_gamma, gamma_dot_d, Omega_dot_d): u_s_list = np.zeros(numVSCMGs) u_g_list = np.zeros(numVSCMGs) for i in range(numVSCMGs): - BG0 = np.column_stack(( - np.array(VSCMGs[i].gsHat0_B).flatten(), - np.array(VSCMGs[i].gtHat0_B).flatten(), - np.array(VSCMGs[i].ggHat_B).flatten() - )) + BG0 = np.column_stack( + ( + np.array(VSCMGs[i].gsHat0_B).flatten(), + np.array(VSCMGs[i].gtHat0_B).flatten(), + np.array(VSCMGs[i].ggHat_B).flatten(), + ) + ) GG0 = np.identity(3) - GG0[0,0] = np.cos(VSCMGs[i].gamma) - GG0[0,1] = np.sin(VSCMGs[i].gamma) - GG0[1,0] = -GG0[0,1] - GG0[1,1] = GG0[0,0] + GG0[0, 0] = np.cos(VSCMGs[i].gamma) + GG0[0, 1] = np.sin(VSCMGs[i].gamma) + GG0[1, 0] = -GG0[0, 1] + GG0[1, 1] = GG0[0, 0] BG = BG0 @ GG0.T gsHat = BG[:, 0] gtHat = BG[:, 1] @@ -154,19 +162,24 @@ def computeTorques(VSCMGs, numVSCMGs, omega_BN_B, K_gamma, gamma_dot_d, Omega_do u_s_list[i] = VSCMGs[i].IW1 * (Omega_dot_d[i] + VSCMGs[i].gammaDot * omega_t) # Calculate the gimbal acceleration - gamma_ddot = -K_gamma*(VSCMGs[i].gammaDot - gamma_dot_d[i]) + gamma_ddot = -K_gamma * (VSCMGs[i].gammaDot - gamma_dot_d[i]) # Calculate the gimbal torque - u_g_list[i] = Jg * gamma_ddot - (Js - Jt) * omega_s * omega_t - Iws * VSCMGs[i].Omega * omega_t + u_g_list[i] = ( + Jg * gamma_ddot + - (Js - Jt) * omega_s * omega_t + - Iws * VSCMGs[i].Omega * omega_t + ) return u_s_list, u_g_list -@pytest.mark.parametrize("numVSCMGs", [1,2,4]) +@pytest.mark.parametrize("numVSCMGs", [1, 2, 4]) def test_vscmgGimbalRateServo(show_plots, numVSCMGs): - """Module Unit Test""" - [testResults, testMessage] = vscmgGimbalRateServoTest(show_plots, numVSCMGs) - assert testResults < 1, testMessage + """Module Unit Test""" + [testResults, testMessage] = vscmgGimbalRateServoTest(show_plots, numVSCMGs) + assert testResults < 1, testMessage + def vscmgGimbalRateServoTest(show_plots, numVSCMGs): r""" @@ -214,10 +227,19 @@ def vscmgGimbalRateServoTest(show_plots, numVSCMGs): fswSetupVSCMGs.clearSetup() for i in range(numVSCMGs): fswSetupVSCMGs.create( - VSCMGs[i].gsHat0_B, VSCMGs[i].gtHat0_B, VSCMGs[i].ggHat_B, - VSCMGs[i].IG1, VSCMGs[i].IG2, VSCMGs[i].IG3, - VSCMGs[i].IW1, VSCMGs[i].IW2, VSCMGs[i].IW3, - VSCMGs[i].Omega, VSCMGs[i].gamma, VSCMGs[i].gammaDot) + VSCMGs[i].gsHat0_B, + VSCMGs[i].gtHat0_B, + VSCMGs[i].ggHat_B, + VSCMGs[i].IG1, + VSCMGs[i].IG2, + VSCMGs[i].IG3, + VSCMGs[i].IW1, + VSCMGs[i].IW2, + VSCMGs[i].IW3, + VSCMGs[i].Omega, + VSCMGs[i].gamma, + VSCMGs[i].gammaDot, + ) # setup reference states message refArray = messaging.VSCMGRefStatesMsgPayload() @@ -267,25 +289,41 @@ def vscmgGimbalRateServoTest(show_plots, numVSCMGs): unitTestSim.ExecuteSimulation() # pull module data and make sure it is correct - wheelTorques = dataLog.wheelTorque[0,:numVSCMGs] - gimbalTorques = dataLog.gimbalTorque[0,:numVSCMGs] - wheelTorquesC = dataLogC.wheelTorque[0,:numVSCMGs] - gimbalTorquesC = dataLogC.gimbalTorque[0,:numVSCMGs] + wheelTorques = dataLog.wheelTorque[0, :numVSCMGs] + gimbalTorques = dataLog.gimbalTorque[0, :numVSCMGs] + wheelTorquesC = dataLogC.wheelTorque[0, :numVSCMGs] + gimbalTorquesC = dataLogC.gimbalTorque[0, :numVSCMGs] moduleTorques = np.concatenate((wheelTorques, gimbalTorques)) moduleTorquesC = np.concatenate((wheelTorquesC, gimbalTorquesC)) # set the output truth values - trueWheelTorques, trueGimbalTorques = computeTorques(VSCMGs, numVSCMGs, omega_BN_B, K_gammaDot, refGimbalRates, refWheelAccels) + trueWheelTorques, trueGimbalTorques = computeTorques( + VSCMGs, numVSCMGs, omega_BN_B, K_gammaDot, refGimbalRates, refWheelAccels + ) trueTorques = np.concatenate((trueWheelTorques, trueGimbalTorques)) - #compare the module output values to the truth values + # compare the module output values to the truth values accuracy = 1e-12 - testFailCount, testMessages = unitTestSupport.compareArrayND([trueTorques], [moduleTorques], accuracy, "VSCMGTorques", - 2 * numVSCMGs, testFailCount, testMessages) - - testFailCount, testMessages = unitTestSupport.compareArrayND([moduleTorques], [moduleTorquesC], accuracy, "cMsgTorques", - 2 * numVSCMGs, testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + [trueTorques], + [moduleTorques], + accuracy, + "VSCMGTorques", + 2 * numVSCMGs, + testFailCount, + testMessages, + ) + + testFailCount, testMessages = unitTestSupport.compareArrayND( + [moduleTorques], + [moduleTorquesC], + accuracy, + "cMsgTorques", + 2 * numVSCMGs, + testFailCount, + testMessages, + ) if testFailCount == 0: print("PASSED: " + module.ModelTag) diff --git a/src/fswAlgorithms/effectorInterfaces/vscmgVelocitySteering/_UnitTest/test_vscmgVelocitySteering.py b/src/fswAlgorithms/effectorInterfaces/vscmgVelocitySteering/_UnitTest/test_vscmgVelocitySteering.py index 3e7372cdcb..c87eee0c06 100644 --- a/src/fswAlgorithms/effectorInterfaces/vscmgVelocitySteering/_UnitTest/test_vscmgVelocitySteering.py +++ b/src/fswAlgorithms/effectorInterfaces/vscmgVelocitySteering/_UnitTest/test_vscmgVelocitySteering.py @@ -30,58 +30,64 @@ from Basilisk.utilities import macros from Basilisk.fswAlgorithms import vscmgVelocitySteering + def defaultVSCMG(): VSCMG = messaging.VSCMGConfigMsgPayload() - VSCMG.rGB_B = [[0.],[0.],[0.]] - VSCMG.gsHat0_B = [[0.],[0.],[0.]] - VSCMG.gtHat0_B = [[0.],[0.],[0.]] - VSCMG.ggHat_B = [[0.],[0.],[0.]] + VSCMG.rGB_B = [[0.0], [0.0], [0.0]] + VSCMG.gsHat0_B = [[0.0], [0.0], [0.0]] + VSCMG.gtHat0_B = [[0.0], [0.0], [0.0]] + VSCMG.ggHat_B = [[0.0], [0.0], [0.0]] VSCMG.u_s_max = -1 VSCMG.u_s_min = -1 - VSCMG.u_s_f = 0. + VSCMG.u_s_f = 0.0 VSCMG.wheelLinearFrictionRatio = -1 - VSCMG.u_g_current = 0. + VSCMG.u_g_current = 0.0 VSCMG.u_g_max = -1 VSCMG.u_g_min = -1 - VSCMG.u_g_f = 0. + VSCMG.u_g_f = 0.0 VSCMG.gimbalLinearFrictionRatio = -1 VSCMG.Omega = 14.4 - VSCMG.gamma = 0. - VSCMG.gammaDot = 0. - VSCMG.Omega_max = 6000. * macros.RPM + VSCMG.gamma = 0.0 + VSCMG.gammaDot = 0.0 + VSCMG.Omega_max = 6000.0 * macros.RPM VSCMG.gammaDot_max = -1 VSCMG.IW1 = 0.1 VSCMG.IW2 = 0.04 VSCMG.IW3 = 0.03 VSCMG.IG1 = 0.03 - VSCMG.IG2 = 0. - VSCMG.IG3 = 0. - VSCMG.U_s = 0. - VSCMG.U_d = 0. - VSCMG.l = 0. - VSCMG.L = 0. - VSCMG.rGcG_G = [[0.],[0.],[0.]] - VSCMG.massW = 0. - VSCMG.massG = 0. + VSCMG.IG2 = 0.0 + VSCMG.IG3 = 0.0 + VSCMG.U_s = 0.0 + VSCMG.U_d = 0.0 + VSCMG.l = 0.0 + VSCMG.L = 0.0 + VSCMG.rGcG_G = [[0.0], [0.0], [0.0]] + VSCMG.massW = 0.0 + VSCMG.massG = 0.0 VSCMG.VSCMGModel = 0 return VSCMG + def setupVSCMGs(numVSCMGs): VSCMGs = [] - ang = 54.75 * np.pi/180 + ang = 54.75 * np.pi / 180 VSCMGs.append(defaultVSCMG()) VSCMGs[0].ggHat_B = [[math.cos(ang)], [0.0], [math.sin(ang)]] VSCMGs[0].gsHat0_B = [[0.0], [1.0], [0.0]] - VSCMGs[0].gtHat0_B = np.cross(np.array([math.cos(ang), 0.0, math.sin(ang)]),np.array([0.0, 1.0, 0.0])) + VSCMGs[0].gtHat0_B = np.cross( + np.array([math.cos(ang), 0.0, math.sin(ang)]), np.array([0.0, 1.0, 0.0]) + ) if numVSCMGs > 1: VSCMGs.append(defaultVSCMG()) VSCMGs[1].gsHat0_B = [[0.0], [-1.0], [0.0]] VSCMGs[1].ggHat_B = [[-math.cos(ang)], [0.0], [math.sin(ang)]] - VSCMGs[1].gtHat0_B = np.cross(np.array([-math.cos(ang), 0.0, math.sin(ang)]),np.array([0.0, -1.0, 0.0])) + VSCMGs[1].gtHat0_B = np.cross( + np.array([-math.cos(ang), 0.0, math.sin(ang)]), np.array([0.0, -1.0, 0.0]) + ) if numVSCMGs == 4: VSCMGs.append(defaultVSCMG()) @@ -90,14 +96,14 @@ def setupVSCMGs(numVSCMGs): ggHat2_B = np.array([0.0, math.cos(ang), math.sin(ang)]) gtHat2_t0_B = np.cross(ggHat2_B, gsHat2_t0_B) gtHat2_t0_B /= np.linalg.norm(gtHat2_t0_B) - BG2 = np.column_stack([gsHat2_t0_B, - gtHat2_t0_B, - ggHat2_B]) - GG02 = np.array([ - [-math.cos(VSCMGs[2].gamma), math.sin(VSCMGs[2].gamma), 0.0], - [-math.sin(VSCMGs[2].gamma), -math.cos(VSCMGs[2].gamma), 0.0], - [0.0, 0.0, 1.0] - ]) + BG2 = np.column_stack([gsHat2_t0_B, gtHat2_t0_B, ggHat2_B]) + GG02 = np.array( + [ + [-math.cos(VSCMGs[2].gamma), math.sin(VSCMGs[2].gamma), 0.0], + [-math.sin(VSCMGs[2].gamma), -math.cos(VSCMGs[2].gamma), 0.0], + [0.0, 0.0, 1.0], + ] + ) BG02 = BG2 @ GG02 VSCMGs[2].gsHat0_B = [[BG02[0][0]], [BG02[1][0]], [BG02[2][0]]] VSCMGs[2].gtHat0_B = [[BG02[0][1]], [BG02[1][1]], [BG02[2][1]]] @@ -109,14 +115,14 @@ def setupVSCMGs(numVSCMGs): ggHat3_B = np.array([0.0, -math.cos(ang), math.sin(ang)]) gtHat3_t0_B = np.cross(ggHat3_B, gsHat3_t0_B) gtHat3_t0_B /= np.linalg.norm(gtHat3_t0_B) - BG3 = np.column_stack([gsHat3_t0_B, - gtHat3_t0_B, - ggHat3_B]) - GG03 = np.array([ - [-math.cos(VSCMGs[3].gamma), math.sin(VSCMGs[3].gamma), 0.0], - [-math.sin(VSCMGs[3].gamma), -math.cos(VSCMGs[3].gamma), 0.0], - [0.0, 0.0, 1.0] - ]) + BG3 = np.column_stack([gsHat3_t0_B, gtHat3_t0_B, ggHat3_B]) + GG03 = np.array( + [ + [-math.cos(VSCMGs[3].gamma), math.sin(VSCMGs[3].gamma), 0.0], + [-math.sin(VSCMGs[3].gamma), -math.cos(VSCMGs[3].gamma), 0.0], + [0.0, 0.0, 1.0], + ] + ) BG03 = BG3 @ GG03 VSCMGs[3].gsHat0_B = [[BG03[0][0]], [BG03[1][0]], [BG03[2][0]]] VSCMGs[3].gtHat0_B = [[BG03[0][1]], [BG03[1][1]], [BG03[2][1]]] @@ -124,8 +130,8 @@ def setupVSCMGs(numVSCMGs): return VSCMGs -def etaDot(VSCMGs,numVSCMGs, omega_BN_B, omega_RN_B, mu, W0_s, W_g, Lr): +def etaDot(VSCMGs, numVSCMGs, omega_BN_B, omega_RN_B, mu, W0_s, W_g, Lr): h_bar = np.zeros(numVSCMGs) # Calculate the D matrices D0 = np.zeros((3, numVSCMGs)) @@ -134,16 +140,18 @@ def etaDot(VSCMGs,numVSCMGs, omega_BN_B, omega_RN_B, mu, W0_s, W_g, Lr): D3 = np.zeros((3, numVSCMGs)) D4 = np.zeros((3, numVSCMGs)) for i in range(numVSCMGs): - BG0 = np.column_stack(( - np.array(VSCMGs[i].gsHat0_B).flatten(), - np.array(VSCMGs[i].gtHat0_B).flatten(), - np.array(VSCMGs[i].ggHat_B).flatten() - )) + BG0 = np.column_stack( + ( + np.array(VSCMGs[i].gsHat0_B).flatten(), + np.array(VSCMGs[i].gtHat0_B).flatten(), + np.array(VSCMGs[i].ggHat_B).flatten(), + ) + ) GG0 = np.identity(3) - GG0[0,0] = np.cos(VSCMGs[i].gamma) - GG0[0,1] = np.sin(VSCMGs[i].gamma) - GG0[1,0] = -GG0[0,1] - GG0[1,1] = GG0[0,0] + GG0[0, 0] = np.cos(VSCMGs[i].gamma) + GG0[0, 1] = np.sin(VSCMGs[i].gamma) + GG0[1, 0] = -GG0[0, 1] + GG0[1, 1] = GG0[0, 0] BG = BG0 @ GG0.T gsHat = BG[:, 0] gtHat = BG[:, 1] @@ -160,38 +168,44 @@ def etaDot(VSCMGs,numVSCMGs, omega_BN_B, omega_RN_B, mu, W0_s, W_g, Lr): Omega = VSCMGs[i].Omega - D0[:,i] = gsHat*Iws - D1[:,i] = (Iws*Omega + Js/2*omega_s)*gtHat + Js/2*omega_t*gsHat - D2[:,i] = 1/2*Jt*(omega_t*gsHat + omega_s*gtHat) - D3[:,i] = Jg*(omega_t*gsHat - omega_s*gtHat) - D4[:,i] = 1/2*(Js-Jt)*(np.outer(gsHat,gtHat) @ omega_RN + np.outer(gtHat,gsHat) @ omega_RN) + D0[:, i] = gsHat * Iws + D1[:, i] = (Iws * Omega + Js / 2 * omega_s) * gtHat + Js / 2 * omega_t * gsHat + D2[:, i] = 1 / 2 * Jt * (omega_t * gsHat + omega_s * gtHat) + D3[:, i] = Jg * (omega_t * gsHat - omega_s * gtHat) + D4[:, i] = ( + 1 + / 2 + * (Js - Jt) + * (np.outer(gsHat, gtHat) @ omega_RN + np.outer(gtHat, gsHat) @ omega_RN) + ) - h_bar[i] = Js*Omega + h_bar[i] = Js * Omega - D = D1-D2+D3+D4 - Q = np.column_stack([D0,D]) + D = D1 - D2 + D3 + D4 + Q = np.column_stack([D0, D]) mean_h_bar = np.mean(h_bar) h_bar_squared = mean_h_bar**2 - delta = np.linalg.det(1/h_bar_squared * (D1 @ D1.T)) + delta = np.linalg.det(1 / h_bar_squared * (D1 @ D1.T)) - W = np.zeros((2*numVSCMGs, 2*numVSCMGs)) + W = np.zeros((2 * numVSCMGs, 2 * numVSCMGs)) for i in range(numVSCMGs): - W_si = W0_s[i] * np.exp(-mu*delta) - W[i,i] = W_si - W[numVSCMGs+i,numVSCMGs+i] = W_g[i] + W_si = W0_s[i] * np.exp(-mu * delta) + W[i, i] = W_si + W[numVSCMGs + i, numVSCMGs + i] = W_g[i] QWQT = Q @ W @ Q.T etaDot = W @ Q.T @ np.linalg.solve(QWQT, -np.array(Lr)) return etaDot -@pytest.mark.parametrize("numVSCMGs", [2,4]) +@pytest.mark.parametrize("numVSCMGs", [2, 4]) def test_vscmgVelocitySteering(show_plots, numVSCMGs): - """Module Unit Test""" - [testResults, testMessage] = vscmgVelocitySteeringTest(show_plots, numVSCMGs) - assert testResults < 1, testMessage + """Module Unit Test""" + [testResults, testMessage] = vscmgVelocitySteeringTest(show_plots, numVSCMGs) + assert testResults < 1, testMessage + def vscmgVelocitySteeringTest(show_plots, numVSCMGs): r""" @@ -239,7 +253,7 @@ def vscmgVelocitySteeringTest(show_plots, numVSCMGs): attIn.omega_BN_B = omega_BN_B # setup hub reference angular velocity - omega_RN_B = [ 0.00649856, -0.01231292, -0.01172612] + omega_RN_B = [0.00649856, -0.01231292, -0.01172612] guideIn = messaging.AttGuidMsgPayload() guideIn.omega_RN_B = omega_RN_B @@ -248,10 +262,19 @@ def vscmgVelocitySteeringTest(show_plots, numVSCMGs): fswSetupVSCMGs.clearSetup() for i in range(numVSCMGs): fswSetupVSCMGs.create( - VSCMGs[i].gsHat0_B, VSCMGs[i].gtHat0_B, VSCMGs[i].ggHat_B, - VSCMGs[i].IG1, VSCMGs[i].IG2, VSCMGs[i].IG3, - VSCMGs[i].IW1, VSCMGs[i].IW2, VSCMGs[i].IW3, - VSCMGs[i].Omega, VSCMGs[i].gamma, VSCMGs[i].gammaDot) + VSCMGs[i].gsHat0_B, + VSCMGs[i].gtHat0_B, + VSCMGs[i].ggHat_B, + VSCMGs[i].IG1, + VSCMGs[i].IG2, + VSCMGs[i].IG3, + VSCMGs[i].IW1, + VSCMGs[i].IW2, + VSCMGs[i].IW3, + VSCMGs[i].Omega, + VSCMGs[i].gamma, + VSCMGs[i].gammaDot, + ) # setup speeds message speedsArray = messaging.VSCMGSpeedMsgPayload() @@ -296,24 +319,38 @@ def vscmgVelocitySteeringTest(show_plots, numVSCMGs): unitTestSim.ExecuteSimulation() # pull module data and make sure it is correct - wheelAccels = dataLog.wheelAccels[0,:numVSCMGs] - gimbalRates = dataLog.gimbalRates[0,:numVSCMGs] - wheelAccelsC = dataLogC.wheelAccels[0,:numVSCMGs] - gimbalRatesC = dataLogC.gimbalRates[0,:numVSCMGs] + wheelAccels = dataLog.wheelAccels[0, :numVSCMGs] + gimbalRates = dataLog.gimbalRates[0, :numVSCMGs] + wheelAccelsC = dataLogC.wheelAccels[0, :numVSCMGs] + gimbalRatesC = dataLogC.gimbalRates[0, :numVSCMGs] moduleOutputs = np.concatenate((wheelAccels, gimbalRates)) moduleOutputsC = np.concatenate((wheelAccelsC, gimbalRatesC)) # set the output truth values - trueOutputs = etaDot(VSCMGs,numVSCMGs, omega_BN_B, omega_RN_B, mu, W0_s, W_g, Lr) + trueOutputs = etaDot(VSCMGs, numVSCMGs, omega_BN_B, omega_RN_B, mu, W0_s, W_g, Lr) # compare the module output values to the truth values accuracy = 1e-12 - testFailCount, testMessages = unitTestSupport.compareArrayND([trueOutputs], [moduleOutputs], accuracy, "VSCMGDesiredStates", - 2*numVSCMGs, testFailCount, testMessages) - - testFailCount, testMessages = unitTestSupport.compareArrayND([moduleOutputs], [moduleOutputsC], accuracy, "cMsgTorques", - 2 * numVSCMGs, testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + [trueOutputs], + [moduleOutputs], + accuracy, + "VSCMGDesiredStates", + 2 * numVSCMGs, + testFailCount, + testMessages, + ) + + testFailCount, testMessages = unitTestSupport.compareArrayND( + [moduleOutputs], + [moduleOutputsC], + accuracy, + "cMsgTorques", + 2 * numVSCMGs, + testFailCount, + testMessages, + ) if testFailCount == 0: print("PASSED: " + module.ModelTag) diff --git a/src/fswAlgorithms/formationFlying/etSphericalControl/_UnitTest/test_etSphericalControl.py b/src/fswAlgorithms/formationFlying/etSphericalControl/_UnitTest/test_etSphericalControl.py index 13053ad2a7..0baf175c4b 100644 --- a/src/fswAlgorithms/formationFlying/etSphericalControl/_UnitTest/test_etSphericalControl.py +++ b/src/fswAlgorithms/formationFlying/etSphericalControl/_UnitTest/test_etSphericalControl.py @@ -28,11 +28,16 @@ import pytest from Basilisk.architecture import bskLogging from Basilisk.architecture import messaging # import the message definitions -from Basilisk.fswAlgorithms import etSphericalControl # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + etSphericalControl, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros, RigidBodyKinematics, orbitalMotion -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # import packages as needed e.g. 'numpy', 'ctypes, 'math' etc. @@ -43,8 +48,11 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ + @pytest.mark.parametrize("accuracy", [1e-8]) -def test_etSphericalControl(show_plots, accuracy): # update "module" in this function name to reflect the module name +def test_etSphericalControl( + show_plots, accuracy +): # update "module" in this function name to reflect the module name r""" **Validation Test Description** @@ -71,24 +79,23 @@ def test_etSphericalControl(show_plots, accuracy): # update "module" in this def etSphericalControlTestFunction(show_plots, accuracy): """Test method""" - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = etSphericalControl.etSphericalControl() - module.ModelTag = "ETcontrol" # update python name of test module + module.ModelTag = "ETcontrol" # update python name of test module # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) @@ -96,69 +103,76 @@ def etSphericalControlTestFunction(show_plots, accuracy): # Initialize the test module configuration data mu = 3.986004418e14 # [m^3/s^2] Earth's gravitational parameter - L0 = 20. + L0 = 20.0 Ki = 4e-7 - Pi = 1.85 * Ki ** 0.5 - module.K = [Ki, 0.0, 0.0, - 0.0, Ki, 0.0, - 0.0, 0.0, Ki] - module.P = [Pi, 0.0, 0.0, - 0.0, Pi, 0.0, - 0.0, 0.0, Pi] - module.L_r = 30. - module.theta_r = 0. - module.phi_r = 0. + Pi = 1.85 * Ki**0.5 + module.K = [Ki, 0.0, 0.0, 0.0, Ki, 0.0, 0.0, 0.0, Ki] + module.P = [Pi, 0.0, 0.0, 0.0, Pi, 0.0, 0.0, 0.0, Pi] + module.L_r = 30.0 + module.theta_r = 0.0 + module.phi_r = 0.0 module.mu = mu # Create input message and size it because the regular creator of that message # is not part of the test. oe = orbitalMotion.ClassicElements() - oe.a = 42164. * 1e3 # [m] geostationary orbit - oe.e = 0. - oe.i = 10.*macros.D2R - oe.Omega = 20.*macros.D2R - oe.omega = 30.*macros.D2R - oe.f = 40.*macros.D2R + oe.a = 42164.0 * 1e3 # [m] geostationary orbit + oe.e = 0.0 + oe.i = 10.0 * macros.D2R + oe.Omega = 20.0 * macros.D2R + oe.omega = 30.0 * macros.D2R + oe.f = 40.0 * macros.D2R r_TN_N, v_TN_N = orbitalMotion.elem2rv(mu, oe) - servicerNavTransOutData = messaging.NavTransMsgPayload() # Create a structure for the input message + servicerNavTransOutData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message servicerNavTransOutData.r_BN_N = r_TN_N servicerNavTransOutData.v_BN_N = v_TN_N servicerTransMsg = messaging.NavTransMsg().write(servicerNavTransOutData) - r_DT_N = np.array([2., -L0, -3.]) # relative position between debris and servicer + r_DT_N = np.array([2.0, -L0, -3.0]) # relative position between debris and servicer r_DN_N = r_TN_N + r_DT_N v_DN_N = v_TN_N - debrisNavTransOutData = messaging.NavTransMsgPayload() # Create a structure for the input message + debrisNavTransOutData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message debrisNavTransOutData.r_BN_N = r_DN_N debrisNavTransOutData.v_BN_N = v_DN_N debrisTransMsg = messaging.NavTransMsg().write(debrisNavTransOutData) - beta_TH = [0.972960339471760, 0.107600839071972, -0.0289291519077161, 0.202319898714648] # initial EP + beta_TH = [ + 0.972960339471760, + 0.107600839071972, + -0.0289291519077161, + 0.202319898714648, + ] # initial EP sigma_TN = RigidBodyKinematics.EP2MRP(beta_TH) # MRP servicerNavAttOutData = messaging.NavAttMsgPayload() servicerNavAttOutData.sigma_BN = sigma_TN servicerAttMsg = messaging.NavAttMsg().write(servicerNavAttOutData) servicerConfigOutData = messaging.VehicleConfigMsgPayload() - servicerConfigOutData.massSC = 500. + servicerConfigOutData.massSC = 500.0 servicerVehicleConfigMsg = messaging.VehicleConfigMsg().write(servicerConfigOutData) debrisConfigOutData = messaging.VehicleConfigMsgPayload() - debrisConfigOutData.massSC = 2000. + debrisConfigOutData.massSC = 2000.0 debrisVehicleConfigMsg = messaging.VehicleConfigMsg().write(debrisConfigOutData) # compute electrostatic force between servicer and debris using single sphere for both S/C - R_T = 2. - R_D = 3. - V_T = 25000. - V_D = -25000. + R_T = 2.0 + R_D = 3.0 + V_T = 25000.0 + V_D = -25000.0 kc = 8.9875517923e9 L = np.linalg.norm(r_DT_N) - q_T = (L*(L*R_T*V_T-R_T*R_D*V_D))/(kc*(L**2.-R_T*R_D)) - q_D = (L * (L * R_D * V_D - R_T * R_D * V_T)) / (kc * (L ** 2. - R_T * R_D)) - Fc = kc*q_T*q_D/L**2. - Fc_N = Fc*(-r_DT_N/np.linalg.norm(r_DT_N)) # electrostatic force acting on servicer + q_T = (L * (L * R_T * V_T - R_T * R_D * V_D)) / (kc * (L**2.0 - R_T * R_D)) + q_D = (L * (L * R_D * V_D - R_T * R_D * V_T)) / (kc * (L**2.0 - R_T * R_D)) + Fc = kc * q_T * q_D / L**2.0 + Fc_N = Fc * ( + -r_DT_N / np.linalg.norm(r_DT_N) + ) # electrostatic force acting on servicer eForceOutData = messaging.CmdForceInertialMsgPayload() eForceOutData.forceRequestInertial = Fc_N eForceMsg = messaging.CmdForceInertialMsg().write(eForceOutData) @@ -186,28 +200,42 @@ def etSphericalControlTestFunction(show_plots, accuracy): forceBodyOutput = dataLogBody.forceRequestBody # set the filtered output truth states - trueInertialVector = [[-0.00714223893615245, - 0.00267752848271998, - 0.000883681113161883]] - trueBodyVector = [[-0.00541988234898216, - 0.00542736415350300, - 0.000360862543207430]] + trueInertialVector = [ + [-0.00714223893615245, 0.00267752848271998, 0.000883681113161883] + ] + trueBodyVector = [[-0.00541988234898216, 0.00542736415350300, 0.000360862543207430]] # compare the module results to the truth values for i in range(0, len(trueInertialVector)): # check vector values - if not unitTestSupport.isArrayEqual(forceInertialOutput[i], trueInertialVector[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + forceInertialOutput[i], trueInertialVector[i], 3, accuracy + ): testFailCount += 1 print(forceInertialOutput[i]) - testMessages.append("FAILED: " + module.ModelTag + " Module failed " - + "Inertial Force Output" + " unit test at t=" - + str(forceInertialOutput[i, 0] * macros.NANO2SEC) + "sec\n") - if not unitTestSupport.isArrayEqual(forceBodyOutput[i], trueBodyVector[i], 3, accuracy): + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed " + + "Inertial Force Output" + + " unit test at t=" + + str(forceInertialOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) + if not unitTestSupport.isArrayEqual( + forceBodyOutput[i], trueBodyVector[i], 3, accuracy + ): testFailCount += 1 print(forceBodyOutput[i]) - testMessages.append("FAILED: " + module.ModelTag + " Module failed " - + "Body Force Output" + " unit test at t=" - + str(forceBodyOutput[i, 0] * macros.NANO2SEC) + "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed " + + "Body Force Output" + + " unit test at t=" + + str(forceBodyOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) # print out success message if no error were found if testFailCount == 0: @@ -219,7 +247,7 @@ def etSphericalControlTestFunction(show_plots, accuracy): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -228,6 +256,6 @@ def etSphericalControlTestFunction(show_plots, accuracy): # if __name__ == "__main__": test_etSphericalControl( - False, # show_plots - 1e-8 # accuracy + False, # show_plots + 1e-8, # accuracy ) diff --git a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter.py b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter.py index a3ed865705..66c09ccec1 100644 --- a/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter.py +++ b/src/fswAlgorithms/formationFlying/formationBarycenter/_UnitTest/test_formationBarycenter.py @@ -23,12 +23,15 @@ import pytest from Basilisk.architecture import messaging, astroConstants from Basilisk.fswAlgorithms import formationBarycenter -from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros, orbitalMotion +from Basilisk.utilities import ( + SimulationBaseClass, + unitTestSupport, + macros, + orbitalMotion, +) @pytest.mark.parametrize("accuracy", [1e-1]) - - def test_formationBarycenter(show_plots, accuracy): r""" **Validation Test Description** @@ -70,7 +73,7 @@ def formationBarycenterTestFunction(show_plots, accuracy): unitProcessName = "TestProcess" unitTestSim = SimulationBaseClass.SimBaseClass() - testProcessRate = macros.sec2nano(1.) + testProcessRate = macros.sec2nano(1.0) testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -80,10 +83,10 @@ def formationBarycenterTestFunction(show_plots, accuracy): unitTestSim.AddModelToTask(unitTaskName, barycenterModule) # Configure each spacecraft's position and velocity - mu = astroConstants.MU_EARTH*1e9 # m^3/s^2 + mu = astroConstants.MU_EARTH * 1e9 # m^3/s^2 oe1 = orbitalMotion.ClassicElements() - oe1.a = 1.1 * astroConstants.REQ_EARTH*1e3 # m + oe1.a = 1.1 * astroConstants.REQ_EARTH * 1e3 # m oe1.e = 0.01 oe1.i = 45.0 * macros.D2R oe1.Omega = 48.2 * macros.D2R @@ -157,18 +160,42 @@ def formationBarycenterTestFunction(show_plots, accuracy): barycenterC = barycenterOutMsgC.r_BN_N barycenterVelocityC = barycenterOutMsgC.v_BN_N elements = orbitalMotion.rv2elem(mu, barycenter[1], barycenterVelocity[1]) - elementsArray = [elements.a, elements.e, elements.i, elements.Omega, elements.omega, elements.f] + elementsArray = [ + elements.a, + elements.e, + elements.i, + elements.Omega, + elements.omega, + elements.f, + ] elementsC = orbitalMotion.rv2elem(mu, barycenterC[1], barycenterVelocityC[1]) - elementsArrayC = [elementsC.a, elementsC.e, elementsC.i, elementsC.Omega, elementsC.omega, elementsC.f] + elementsArrayC = [ + elementsC.a, + elementsC.e, + elementsC.i, + elementsC.Omega, + elementsC.omega, + elementsC.f, + ] # Set the true values trueBarycenter = np.array([-2795611.0423523, 4349073.25701624, 4711567.73659507]) trueBarycenterVelocity = np.array([-5738.71806601, -4744.64063227, 1079.61501999]) - trueElements = [7015950.259999997, 0.0099, 0.7579092276785376, 0.8412486994612671, 6.07025513843626, 1.5855546253383273] + trueElements = [ + 7015950.259999997, + 0.0099, + 0.7579092276785376, + 0.8412486994612671, + 6.07025513843626, + 1.5855546253383273, + ] # Verify the data - if not unitTestSupport.isArrayEqual(barycenter[0], trueBarycenter, 3, accuracy) or \ - not unitTestSupport.isArrayEqual(barycenterVelocity[0], trueBarycenterVelocity, 3, accuracy): + if not unitTestSupport.isArrayEqual( + barycenter[0], trueBarycenter, 3, accuracy + ) or not unitTestSupport.isArrayEqual( + barycenterVelocity[0], trueBarycenterVelocity, 3, accuracy + ): testFailCount += 1 testMessages.append("FAILED: formationBarycenter cartesian unit test.") @@ -176,14 +203,21 @@ def formationBarycenterTestFunction(show_plots, accuracy): testFailCount += 1 testMessages.append("FAILED: formationBarycenter orbital element unit test.") - if not unitTestSupport.isArrayEqual(barycenterC[0], trueBarycenter, 3, accuracy) or \ - not unitTestSupport.isArrayEqual(barycenterVelocityC[0], trueBarycenterVelocity, 3, accuracy): + if not unitTestSupport.isArrayEqual( + barycenterC[0], trueBarycenter, 3, accuracy + ) or not unitTestSupport.isArrayEqual( + barycenterVelocityC[0], trueBarycenterVelocity, 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: formationBarycenter C message cartesian unit test.") + testMessages.append( + "FAILED: formationBarycenter C message cartesian unit test." + ) if not unitTestSupport.isArrayEqual(elementsArrayC, trueElements, 6, accuracy): testFailCount += 1 - testMessages.append("FAILED: formationBarycenter C message orbital element unit test.") + testMessages.append( + "FAILED: formationBarycenter C message orbital element unit test." + ) if testFailCount == 0: print("PASSED: formationBarycenter unit test.") @@ -196,5 +230,5 @@ def formationBarycenterTestFunction(show_plots, accuracy): if __name__ == "__main__": test_formationBarycenter( False, # show_plots - 1e-8 # accuracy + 1e-8, # accuracy ) diff --git a/src/fswAlgorithms/formationFlying/hillStateConverter/_UnitTest/test_hillStateConverter.py b/src/fswAlgorithms/formationFlying/hillStateConverter/_UnitTest/test_hillStateConverter.py index 9ac239041a..0176d3245b 100644 --- a/src/fswAlgorithms/formationFlying/hillStateConverter/_UnitTest/test_hillStateConverter.py +++ b/src/fswAlgorithms/formationFlying/hillStateConverter/_UnitTest/test_hillStateConverter.py @@ -1,15 +1,17 @@ - # 3rd party / std lib imports import pytest from Basilisk.architecture import messaging + # Modules to test from Basilisk.fswAlgorithms import hillStateConverter + # Utilities/macros from Basilisk.utilities import SimulationBaseClass as sbc from Basilisk.utilities import macros -#from Basilisk.simulation import simFswInterfaceMessages +# from Basilisk.simulation import simFswInterfaceMessages + def test_hillStateConverter(show_plots): """ @@ -18,17 +20,17 @@ def test_hillStateConverter(show_plots): 2. Correctly converts those messages into the hill frame. """ sim = sbc.SimBaseClass() - procName = 'process' - taskName = 'task' + procName = "process" + taskName = "task" proc = sim.CreateNewProcess(procName) - task = sim.CreateNewTask(taskName, macros.sec2nano(1.0)) + task = sim.CreateNewTask(taskName, macros.sec2nano(1.0)) proc.addTask(task) # Set up two spacecraft position messages - chief_r = [7100,0,0] - chief_v = [0,7.000,0] + chief_r = [7100, 0, 0] + chief_v = [0, 7.000, 0] dep_r = [7101, 0, 0] - dep_v = [0,7.010,0] + dep_v = [0, 7.010, 0] chiefNavMsgData = messaging.NavTransMsgPayload() chiefNavMsgData.r_BN_N = chief_r @@ -57,14 +59,15 @@ def test_hillStateConverter(show_plots): hill_positions = hillRecorder.r_DC_H hill_velocities = hillRecorder.v_DC_H - ref_pos = [1,0,0] - ref_vel = [0,0.00901408,0] + ref_pos = [1, 0, 0] + ref_vel = [0, 0.00901408, 0] # Test the position calculation: for val1, val2 in zip(hill_positions[-1], ref_pos): - assert val1 == pytest.approx(val2) + assert val1 == pytest.approx(val2) - for val1, val2 in zip(hill_velocities[-1], ref_vel): + for val1, val2 in zip(hill_velocities[-1], ref_vel): assert val1 == pytest.approx(val2) -if __name__=="__main__": + +if __name__ == "__main__": test_hillStateConverter(False) diff --git a/src/fswAlgorithms/formationFlying/hillToAttRef/_UnitTest/test_hillToAttRef.py b/src/fswAlgorithms/formationFlying/hillToAttRef/_UnitTest/test_hillToAttRef.py index 5ff8faa4b3..ebdba1c380 100644 --- a/src/fswAlgorithms/formationFlying/hillToAttRef/_UnitTest/test_hillToAttRef.py +++ b/src/fswAlgorithms/formationFlying/hillToAttRef/_UnitTest/test_hillToAttRef.py @@ -1,21 +1,23 @@ - # 3rd party / std lib imports import numpy as np import pytest from Basilisk.architecture import messaging + # Modules to test from Basilisk.fswAlgorithms import hillToAttRef + # Utilities/macros from Basilisk.utilities import SimulationBaseClass as sbc from Basilisk.utilities import macros -#from Basilisk.simulation import simFswInterfaceMessages +# from Basilisk.simulation import simFswInterfaceMessages + -@pytest.mark.parametrize("msg_type", ['AttRefMsg','NavAttMsg']) +@pytest.mark.parametrize("msg_type", ["AttRefMsg", "NavAttMsg"]) @pytest.mark.parametrize("use_limits", [True, False]) def test_hillToAttRef(show_plots, use_limits, msg_type): - """ + """ **Validation Test Description** Unit test for hillToAttRef. The unit test specifically covers: @@ -25,38 +27,41 @@ def test_hillToAttRef(show_plots, use_limits, msg_type): 2. Limit enforcement: When set, does the module correctly use the limits specified by the user? """ - runner(show_plots, use_limits, msg_type) + runner(show_plots, use_limits, msg_type) + def runner(show_plots, use_limits, msg_type): sim = sbc.SimBaseClass() - procName = 'process' - taskName = 'task' + procName = "process" + taskName = "task" proc = sim.CreateNewProcess(procName) - task = sim.CreateNewTask(taskName, macros.sec2nano(1.0)) + task = sim.CreateNewTask(taskName, macros.sec2nano(1.0)) proc.addTask(task) # Set up a test relative state vector - relative_state = [1000, 0, 0, - 0, 5, 0] # m / m/s + relative_state = [1000, 0, 0, 0, 5, 0] # m / m/s # Set up a dummy gain matrix - lqr_gain_set = np.array([[0,1,0], - [0,0,0], - [0,0,0], - [0,0,0], - [0,0,0.25], - [0,0,0], ]).T + lqr_gain_set = np.array( + [ + [0, 1, 0], + [0, 0, 0], + [0, 0, 0], + [0, 0, 0], + [0, 0, 0.25], + [0, 0, 0], + ] + ).T hillStateMsgData = messaging.HillRelStateMsgPayload() hillStateMsgData.r_DC_H = relative_state[0:3] hillStateMsgData.v_DC_H = relative_state[3:] hillStateMsg = messaging.HillRelStateMsg().write(hillStateMsgData) - # Set up the hillStateConverter depAttRef = hillToAttRef.hillToAttRef() depAttRef.ModelTag = "dep_hillControl" depAttRef.gainMatrix = hillToAttRef.MultiArray(lqr_gain_set) depAttRef.hillStateInMsg.subscribeTo(hillStateMsg) - if msg_type == 'NavAttMsg': + if msg_type == "NavAttMsg": attRefMsgData = messaging.NavAttMsgPayload() attRefMsgData.sigma_BN = [0.5, 0.5, 0.5] attRefMsg = messaging.NavAttMsg().write(attRefMsgData) @@ -68,17 +73,17 @@ def runner(show_plots, use_limits, msg_type): depAttRef.attRefInMsg.subscribeTo(attRefMsg) if use_limits: - depAttRef.relMRPMin = -0.2 # Configure minimum MRP + depAttRef.relMRPMin = -0.2 # Configure minimum MRP depAttRef.relMRPMax = 0.2 # Configure maximum MRP # ref_vals = [0.2, -0.2, 0.2] - if msg_type == 'NavAttMsg': + if msg_type == "NavAttMsg": ref_vals = [-0.37398374, -0.25203252, -0.57723577] else: - ref_vals = [0.2165725, 0.32956685, 0.51789077] + ref_vals = [0.2165725, 0.32956685, 0.51789077] else: # ref_vals = lqr_gain_set.dot(relative_state) - if msg_type == 'NavAttMsg': + if msg_type == "NavAttMsg": ref_vals = [0.5, 0.5, 0.5] else: ref_vals = [0.2, 0.2, 0.2] @@ -98,5 +103,6 @@ def runner(show_plots, use_limits, msg_type): for val1, val2 in zip(hill_positions[-1], ref_vals): assert val1 == pytest.approx(val2) -if __name__=="__main__": - test_hillToAttRef(False, True, 'AttRefMsg') + +if __name__ == "__main__": + test_hillToAttRef(False, True, "AttRefMsg") diff --git a/src/fswAlgorithms/formationFlying/meanOEFeedback/_UnitTest/test_meanOEFeedback.py b/src/fswAlgorithms/formationFlying/meanOEFeedback/_UnitTest/test_meanOEFeedback.py index ef14e5c1be..313c5b8f0f 100644 --- a/src/fswAlgorithms/formationFlying/meanOEFeedback/_UnitTest/test_meanOEFeedback.py +++ b/src/fswAlgorithms/formationFlying/meanOEFeedback/_UnitTest/test_meanOEFeedback.py @@ -26,12 +26,17 @@ import pytest from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import meanOEFeedback # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + meanOEFeedback, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -40,13 +45,15 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ + @pytest.mark.parametrize("useClassicElem", [True, False]) @pytest.mark.parametrize("accuracy", [1e-6]) - def test_meanOEFeedback(show_plots, useClassicElem, accuracy): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = meanOEFeedbackTestFunction(show_plots, useClassicElem, accuracy) + [testResults, testMessage] = meanOEFeedbackTestFunction( + show_plots, useClassicElem, accuracy + ) assert testResults < 1, testMessage @@ -60,21 +67,55 @@ def meanOEFeedbackTestFunction(show_plots, useClassicElem, accuracy): # Create test thread testProcessRate = macros.sec2nano(0.1) # process rate testProc = unitTestSim.CreateNewProcess(unitProcessName) # create new process - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # create new task + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # create new task # Construct algorithm and associated C++ container module = meanOEFeedback.meanOEFeedback() module.ModelTag = "meanOEFeedback" # update python name of test meanOEFeedback module.targetDiffOeMean = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] module.mu = orbitalMotion.MU_EARTH * 1e9 # [m^3/s^2] module.req = orbitalMotion.REQ_EARTH * 1e3 # [m] - module.J2 = orbitalMotion.J2_EARTH # [] - module.K = [1e7, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1e7, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1e7, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1e7, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1e7, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1e7] - if(useClassicElem): + module.J2 = orbitalMotion.J2_EARTH # [] + module.K = [ + 1e7, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1e7, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1e7, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1e7, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1e7, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1e7, + ] + if useClassicElem: module.oeType = 0 # 0: classic else: module.oeType = 1 # 1: equinoctial @@ -92,8 +133,10 @@ def meanOEFeedbackTestFunction(show_plots, useClassicElem, accuracy): oe.Omega = 0.3 oe.omega = 0.4 oe.f = 0.5 - (r_BN_N, v_BN_N) = orbitalMotion.elem2rv(orbitalMotion.MU_EARTH*1e9, oe) - chiefNavStateOutData = messaging.NavTransMsgPayload() # Create a structure for the input message + (r_BN_N, v_BN_N) = orbitalMotion.elem2rv(orbitalMotion.MU_EARTH * 1e9, oe) + chiefNavStateOutData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message chiefNavStateOutData.timeTag = 0 chiefNavStateOutData.r_BN_N = r_BN_N chiefNavStateOutData.v_BN_N = v_BN_N @@ -110,8 +153,10 @@ def meanOEFeedbackTestFunction(show_plots, useClassicElem, accuracy): oe2.Omega = 0.0 + 0.0003 oe2.omega = 0.0 + 0.0002 oe2.f = 0.0001 - (r_BN_N2, v_BN_N2) = orbitalMotion.elem2rv(orbitalMotion.MU_EARTH*1e9, oe2) - deputyNavStateOutData = messaging.NavTransMsgPayload() # Create a structure for the input message + (r_BN_N2, v_BN_N2) = orbitalMotion.elem2rv(orbitalMotion.MU_EARTH * 1e9, oe2) + deputyNavStateOutData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message deputyNavStateOutData.timeTag = 0 deputyNavStateOutData.r_BN_N = r_BN_N2 deputyNavStateOutData.v_BN_N = v_BN_N2 @@ -143,22 +188,36 @@ def meanOEFeedbackTestFunction(show_plots, useClassicElem, accuracy): # set the filtered output truth states if useClassicElem: - trueVector = [[-849.57347406544340628897771239280701, - 1849.77641265032843875815160572528839, - 136.07817734479317550722043961286545]] + trueVector = [ + [ + -849.57347406544340628897771239280701, + 1849.77641265032843875815160572528839, + 136.07817734479317550722043961286545, + ] + ] else: - trueVector = [[-1655.37188207880308254971168935298920, - 1788.61776379042521512019447982311249, - 52.54814237453938119415397522971034]] + trueVector = [ + [ + -1655.37188207880308254971168935298920, + 1788.61776379042521512019447982311249, + 52.54814237453938119415397522971034, + ] + ] # compare the meanOEFeedback results to the truth values for i in range(0, len(trueVector)): # check a vector values if not unitTestSupport.isArrayEqual(forceOutput[i], trueVector[i], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed " - + ".forceRequestInertial" + " unit test at t=" - + str(dataLog.times()[i]*macros.NANO2SEC) + "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed " + + ".forceRequestInertial" + + " unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) # print out success message if no error were found if testFailCount == 0: @@ -167,7 +226,7 @@ def meanOEFeedbackTestFunction(show_plots, useClassicElem, accuracy): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -178,5 +237,5 @@ def meanOEFeedbackTestFunction(show_plots, useClassicElem, accuracy): test_meanOEFeedback( False, # show_plots True, # useClassicElem - 1e-6 # accuracy + 1e-6, # accuracy ) diff --git a/src/fswAlgorithms/formationFlying/spacecraftPointing/_UnitTest/test_spacecraftPointing.py b/src/fswAlgorithms/formationFlying/spacecraftPointing/_UnitTest/test_spacecraftPointing.py index d8c6caa1c8..d083b7dfab 100755 --- a/src/fswAlgorithms/formationFlying/spacecraftPointing/_UnitTest/test_spacecraftPointing.py +++ b/src/fswAlgorithms/formationFlying/spacecraftPointing/_UnitTest/test_spacecraftPointing.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -39,16 +38,24 @@ # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.fswAlgorithms import spacecraftPointing # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.fswAlgorithms import ( + spacecraftPointing, +) # import the module that is to be tested from Basilisk.utilities import macros import numpy as np from Basilisk.architecture import messaging -@pytest.mark.parametrize("case", [ - (1) # Regular alignment vector - ,(2) # Alignment vector aligns with the z-axis of the body frame -]) + +@pytest.mark.parametrize( + "case", + [ + (1), # Regular alignment vector + (2), # Alignment vector aligns with the z-axis of the body frame + ], +) # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed # @pytest.mark.skipif(conditionstring) @@ -63,16 +70,16 @@ def test_spacecraftPointing(show_plots, case): def spacecraftPointingTestFunction(show_plots, case): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.1) # update process rate update time + testProcessRate = macros.sec2nano(0.1) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -85,20 +92,24 @@ def spacecraftPointingTestFunction(show_plots, case): # Initialize the test module configuration data module.alignmentVector_B = [1.0, 0.0, 0.0] - if (case == 2): + if case == 2: module.alignmentVector_B = [0.0, 0.0, 1.0] - r_BN_N = [[np.cos(0.0), np.sin(0.0), 0.0], - [np.cos(0.001), np.sin(0.001), 0.0], - [np.cos(0.002), np.sin(0.002), 0.0], - [np.cos(0.003), np.sin(0.003), 0.0], - [np.cos(0.004), np.sin(0.004), 0.0]] - - r_BN_N2 = [[0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0]] + r_BN_N = [ + [np.cos(0.0), np.sin(0.0), 0.0], + [np.cos(0.001), np.sin(0.001), 0.0], + [np.cos(0.002), np.sin(0.002), 0.0], + [np.cos(0.003), np.sin(0.003), 0.0], + [np.cos(0.004), np.sin(0.004), 0.0], + ] + + r_BN_N2 = [ + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + ] # Create input message and size it because the regular creator of that message # is not part of the test. @@ -106,14 +117,18 @@ def spacecraftPointingTestFunction(show_plots, case): # # Chief Input Message # - chiefInputData = messaging.NavTransMsgPayload() # Create a structure for the input message + chiefInputData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message chiefInputData.r_BN_N = r_BN_N[0] chiefInMsg = messaging.NavTransMsg().write(chiefInputData) # # Deputy Input Message # - deputyInputData = messaging.NavTransMsgPayload() # Create a structure for the input message + deputyInputData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message deputyInputData.r_BN_N = r_BN_N2[0] deputyInMsg = messaging.NavTransMsg().write(deputyInputData) @@ -132,7 +147,7 @@ def spacecraftPointingTestFunction(show_plots, case): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(0.1)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(0.1)) # seconds to stop simulation # Begin the simulation time run set above # Because it is decided to give the module a set of coordinates for each timestep, a new message has @@ -179,7 +194,7 @@ def spacecraftPointingTestFunction(show_plots, case): unitTestSim.ExecuteSimulation() - if (case == 1): + if case == 1: # This pulls the actual data log from the simulation run. # Note that range(3) will provide [0, 1, 2] Those are the elements you get from the vector (all of them) # @@ -189,23 +204,29 @@ def spacecraftPointingTestFunction(show_plots, case): moduleOutput = dataLog.sigma_RN # set the filtered output truth states trueVector = [ - [0., 0., 0.0], - [0., 0., 0.0], - [0., 0., 0.0002500000052], - [0., 0., 0.0005000000417], - [0., 0., 0.0007500001406], - [0., 0., 0.001000000333] - ] + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0002500000052], + [0.0, 0.0, 0.0005000000417], + [0.0, 0.0, 0.0007500001406], + [0.0, 0.0, 0.001000000333], + ] # compare the module results to the truth values accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue1", str(accuracy), path) - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleOutput[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + moduleOutput[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_RN unit test at t=" + - str(dataLog.times()[i]*macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_RN unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) # # check omega_RN_N @@ -214,25 +235,31 @@ def spacecraftPointingTestFunction(show_plots, case): # set the filtered output truth states trueVector = [ - [0., 0., 0.0], - [0., 0., 0.0], - [0., 0., 0.01], - [0., 0., 0.01], - [0., 0., 0.01], - [0., 0., 0.01] - ] + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.01], + [0.0, 0.0, 0.01], + [0.0, 0.0, 0.01], + [0.0, 0.0, 0.01], + ] # compare the module results to the truth values # The first three values of the simulation have to be ignored for omega_RN_N. For this reason, comparing from index 3. accuracy = 1e-9 unitTestSupport.writeTeXSnippet("toleranceValue2", str(accuracy), path) - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleOutput[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + moduleOutput[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_N unit test at t=" + - str(dataLog.times()[i]*macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed omega_RN_N unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) # # check domega_RN_N @@ -241,49 +268,61 @@ def spacecraftPointingTestFunction(show_plots, case): # set the filtered output truth states trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + ] # compare the module results to the truth values # The first three values of the simulation have to be ignored for domega_RN_N. For this reason, comparing from index 3. accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue3", str(accuracy), path) - for i in range(0,len(trueVector)): + for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(moduleOutput[i],trueVector[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + moduleOutput[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed domega_RN_N unit test at t=" + - str(dataLog.times()[i]*macros.NANO2SEC) + - "sec\n") - elif (case == 2): - trueVector = [-1.0/3.0, 1.0/3.0, -1.0/3.0] + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed domega_RN_N unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) + elif case == 2: + trueVector = [-1.0 / 3.0, 1.0 / 3.0, -1.0 / 3.0] # compare the module results to the truth values accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue4", str(accuracy), path) # check a vector values - if not unitTestSupport.isVectorEqual(np.array(module.sigma_BA), np.array(trueVector), accuracy): + if not unitTestSupport.isVectorEqual( + np.array(module.sigma_BA), np.array(trueVector), accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed, sigma_BA is calculated incorrectly\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed, sigma_BA is calculated incorrectly\n" + ) # print out success message if no error were found snippentName = "passFail" + str(case) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("FAILED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/formationFlying/spacecraftReconfig/_UnitTest/test_spacecraftReconfig.py b/src/fswAlgorithms/formationFlying/spacecraftReconfig/_UnitTest/test_spacecraftReconfig.py index 587483bf8f..e7025bb167 100644 --- a/src/fswAlgorithms/formationFlying/spacecraftReconfig/_UnitTest/test_spacecraftReconfig.py +++ b/src/fswAlgorithms/formationFlying/spacecraftReconfig/_UnitTest/test_spacecraftReconfig.py @@ -26,13 +26,18 @@ import pytest from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import spacecraftReconfig # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + spacecraftReconfig, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import fswSetupThrusters from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -41,13 +46,15 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ + @pytest.mark.parametrize("useRefAttitude", [True, False]) @pytest.mark.parametrize("accuracy", [1e-9]) - def test_spacecraftReconfig(show_plots, useRefAttitude, accuracy): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = spacecraftReconfigTestFunction(show_plots, useRefAttitude, accuracy) + [testResults, testMessage] = spacecraftReconfigTestFunction( + show_plots, useRefAttitude, accuracy + ) assert testResults < 1, testMessage @@ -61,10 +68,14 @@ def spacecraftReconfigTestFunction(show_plots, useRefAttitude, accuracy): # Create test thread testProcessRate = macros.sec2nano(0.1) # process rate testProc = unitTestSim.CreateNewProcess(unitProcessName) # create new process - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # create new task + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # create new task # Construct algorithm and associated C++ container module = spacecraftReconfig.spacecraftReconfig() - module.ModelTag = "spacecraftReconfig" # update python name of test spacecraftReconfig + module.ModelTag = ( + "spacecraftReconfig" # update python name of test spacecraftReconfig + ) module.targetClassicOED = [0.0000, 0.0000, 0.0000, 0.0001, 0.0002, 0.0003] module.attControlTime = 400 # [s] module.mu = orbitalMotion.MU_EARTH * 1e9 # [m^3/s^2] @@ -82,8 +93,10 @@ def spacecraftReconfigTestFunction(show_plots, useRefAttitude, accuracy): oe.Omega = 0.3 oe.omega = 0.4 oe.f = 0.5 - (r_BN_N, v_BN_N) = orbitalMotion.elem2rv(orbitalMotion.MU_EARTH*1e9, oe) - chiefNavStateOutData = messaging.NavTransMsgPayload() # Create a structure for the input message + (r_BN_N, v_BN_N) = orbitalMotion.elem2rv(orbitalMotion.MU_EARTH * 1e9, oe) + chiefNavStateOutData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message chiefNavStateOutData.timeTag = 0 chiefNavStateOutData.r_BN_N = r_BN_N chiefNavStateOutData.v_BN_N = v_BN_N @@ -100,8 +113,10 @@ def spacecraftReconfigTestFunction(show_plots, useRefAttitude, accuracy): oe2.Omega = 0.0 + 0.0003 oe2.omega = 0.0 + 0.0002 oe2.f = 0.0001 - (r_BN_N2, v_BN_N2) = orbitalMotion.elem2rv(orbitalMotion.MU_EARTH*1e9, oe2) - deputyNavStateOutData = messaging.NavTransMsgPayload() # Create a structure for the input message + (r_BN_N2, v_BN_N2) = orbitalMotion.elem2rv(orbitalMotion.MU_EARTH * 1e9, oe2) + deputyNavStateOutData = ( + messaging.NavTransMsgPayload() + ) # Create a structure for the input message deputyNavStateOutData.timeTag = 0 deputyNavStateOutData.r_BN_N = r_BN_N2 deputyNavStateOutData.v_BN_N = v_BN_N2 @@ -159,26 +174,36 @@ def spacecraftReconfigTestFunction(show_plots, useRefAttitude, accuracy): # This pulls the actual data log from the simulation run. attOutput = dataLog.sigma_RN - resetPeriod = unitTestSupport.addTimeColumn(moduleLog.times(), moduleLog.resetPeriod) + resetPeriod = unitTestSupport.addTimeColumn( + moduleLog.times(), moduleLog.resetPeriod + ) # set the filtered output truth states if useRefAttitude: - trueVector = [[1.0,0.0,0.0]] + trueVector = [[1.0, 0.0, 0.0]] else: - trueVector = [[0.38532697209248595, - -0.7016349090839732, - -0.4026194572440069]] + trueVector = [[0.38532697209248595, -0.7016349090839732, -0.4026194572440069]] trueResetPeriod = 28148.5466910579925752244889736 # compare the spacecraftReconfig results to the truth values for i in range(0, len(trueVector)): # check a vector values if not unitTestSupport.isArrayEqual(attOutput[i], trueVector[i], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_RN" + " unit test at t=" - + str(attOutput[i, 0]*macros.NANO2SEC) + "sec\n") - - if (not unitTestSupport.isDoubleEqualRelative(resetPeriod[0,1], trueResetPeriod, accuracy)): + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed sigma_RN" + + " unit test at t=" + + str(attOutput[i, 0] * macros.NANO2SEC) + + "sec\n" + ) + + if not unitTestSupport.isDoubleEqualRelative( + resetPeriod[0, 1], trueResetPeriod, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed " + "resetPeriod") + testMessages.append( + "FAILED: " + module.ModelTag + " Module failed " + "resetPeriod" + ) # print out success message if no error were found if testFailCount == 0: print("PASSED: " + module.ModelTag) @@ -186,7 +211,7 @@ def spacecraftReconfigTestFunction(show_plots, useRefAttitude, accuracy): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -197,5 +222,5 @@ def spacecraftReconfigTestFunction(show_plots, useRefAttitude, accuracy): test_spacecraftReconfig( False, # show_plots True, # useRefAttitude - 1e-9 # accuracy + 1e-9, # accuracy ) diff --git a/src/fswAlgorithms/imageProcessing/centerRadiusCNN/_UnitTest/test_centerRadiusCNN.py b/src/fswAlgorithms/imageProcessing/centerRadiusCNN/_UnitTest/test_centerRadiusCNN.py index ca693bdc9f..a525b48a79 100755 --- a/src/fswAlgorithms/imageProcessing/centerRadiusCNN/_UnitTest/test_centerRadiusCNN.py +++ b/src/fswAlgorithms/imageProcessing/centerRadiusCNN/_UnitTest/test_centerRadiusCNN.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -31,7 +30,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation @@ -41,7 +40,9 @@ from PIL import Image, ImageDraw except ImportError: importErr = True - reasonErr = "python Pillow package not installed---can't test CenterRadiusCNN module" + reasonErr = ( + "python Pillow package not installed---can't test CenterRadiusCNN module" + ) # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass @@ -60,12 +61,12 @@ # @pytest.mark.xfail(conditionstring) # Provide a unique test method name, starting with 'test_'. -@pytest.mark.skipif(importErr, reason= reasonErr) -@pytest.mark.parametrize("image, saveImage", [ - ("mars.jpg", False), - ("mars2.jpg", False), - ("mars3.jpg", False) -]) + +@pytest.mark.skipif(importErr, reason=reasonErr) +@pytest.mark.parametrize( + "image, saveImage", + [("mars.jpg", False), ("mars2.jpg", False), ("mars3.jpg", False)], +) # update "module" in this function name to reflect the module name def test_module(show_plots, image, saveImage): @@ -80,23 +81,22 @@ def test_module(show_plots, image, saveImage): def cnnTest(show_plots, image, saveImage): - # Truth values from python - imagePath = path + '/' + image + imagePath = path + "/" + image input_image = Image.open(imagePath) input_image.load() ################################################# - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -111,7 +111,7 @@ def cnnTest(show_plots, image, saveImage): module.pathToNetwork = path + "/../CAD.onnx" module.filename = imagePath - module.pixelNoise = [5,5,5] + module.pixelNoise = [5, 5, 5] circles = [] if image == "mars.jpg": @@ -130,12 +130,12 @@ def cnnTest(show_plots, image, saveImage): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() - centers = dataLog.circlesCenters[:, :10*2] + centers = dataLog.circlesCenters[:, : 10 * 2] radii = dataLog.circlesRadii[:, :10] # Output image: @@ -145,31 +145,31 @@ def cnnTest(show_plots, image, saveImage): imageProcCircles = [] for j in range(len(radii[-1, 0:])): - if radii[-1,j] > 0: - imageProcCircles.append((centers[-1, 2*j], centers[-1, 2*j+1], radii[-1, j])) + if radii[-1, j] > 0: + imageProcCircles.append( + (centers[-1, 2 * j], centers[-1, 2 * j + 1], radii[-1, j]) + ) for x, y, r in imageProcCircles: draw_result.ellipse((x - r, y - r, x + r, y + r), outline=(255, 0, 0, 0)) # Save output image if saveImage: - output_image.save("result_"+ image) + output_image.save("result_" + image) if show_plots: print(imageProcCircles[0]) output_image.show() - # print out success message if no error were found for testCircle, refCircle in zip(imageProcCircles, circles): for i in range(3): - if np.abs((testCircle[i] - refCircle[i])/refCircle[i])>1: - testFailCount+=1 + if np.abs((testCircle[i] - refCircle[i]) / refCircle[i]) > 1: + testFailCount += 1 testMessages.append("Test failed processing " + image) - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -180,4 +180,4 @@ def cnnTest(show_plots, image, saveImage): if importErr: print(reasonErr) exit(1) - cnnTest(True, "mars.jpg", True) # mars images + cnnTest(True, "mars.jpg", True) # mars images diff --git a/src/fswAlgorithms/imageProcessing/horizonOpNav/_UnitTest/test_horizonOpNav.py b/src/fswAlgorithms/imageProcessing/horizonOpNav/_UnitTest/test_horizonOpNav.py index 891bfa6e4d..40e0c044f8 100644 --- a/src/fswAlgorithms/imageProcessing/horizonOpNav/_UnitTest/test_horizonOpNav.py +++ b/src/fswAlgorithms/imageProcessing/horizonOpNav/_UnitTest/test_horizonOpNav.py @@ -17,6 +17,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) + def back_substitution(A, b): n = b.size x = np.zeros_like(b) @@ -24,12 +25,12 @@ def back_substitution(A, b): if A[-1, -1] == 0: raise ValueError - x[-1] = b[-1]/ A[-1, -1] - for i in range(n-2, -1, -1): - sum=0 + x[-1] = b[-1] / A[-1, -1] + for i in range(n - 2, -1, -1): + sum = 0 for j in range(i, n): - sum += A[i, j]*x[j] - x[i] = (b[i] - sum)/A[i,i] + sum += A[i, j] * x[j] + x[i] = (b[i] - sum) / A[i, i] return x @@ -52,19 +53,22 @@ def test_horizonOpNav(): [testResults, testMessage] = horizonOpNav_update() assert testResults < 1, testMessage + def horizonOpNav_methods(): testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages ################################################################################### ## Testing QR decomp ################################################################################### - Hinput = np.array([[1,2,3],[1,20,3],[3,0,1],[2,1,0],[20,-1, -5],[0,10,-5]]) + Hinput = np.array( + [[1, 2, 3], [1, 20, 3], [3, 0, 1], [2, 1, 0], [20, -1, -5], [0, 10, -5]] + ) numStates = np.shape(Hinput)[0] # Fill in the variables for the test Qin = horizonOpNav.new_doubleArray(3 * numStates) Rin = horizonOpNav.new_doubleArray(3 * 3) Hin = horizonOpNav.new_doubleArray(numStates * 3) - for j in range(numStates*3): + for j in range(numStates * 3): horizonOpNav.doubleArray_setitem(Qin, j, 0) for j in range(3 * 3): horizonOpNav.doubleArray_setitem(Rin, j, 0) @@ -79,38 +83,37 @@ def horizonOpNav_methods(): for j in range(3 * 3): Rout.append(horizonOpNav.doubleArray_getitem(Rin, j)) - q,r = np.linalg.qr(Hinput) + q, r = np.linalg.qr(Hinput) - Rpy = np.zeros([3,3]) + Rpy = np.zeros([3, 3]) Qpy = np.zeros([numStates, 3]) - for i in range(0,3): - Qpy[:,i] = Hinput[:,i] + for i in range(0, 3): + Qpy[:, i] = Hinput[:, i] for j in range(i): - Rpy[j,i] = np.dot(Qpy[:,j], Hinput[:,i]) - Qpy[:,i]= Qpy[:,i] - Rpy[j,i]*Qpy[:,j] - Rpy[i,i] = np.linalg.norm(Qpy[:,i]) - Qpy[:,i] = 1 / Rpy[i,i] * Qpy[:,i] - + Rpy[j, i] = np.dot(Qpy[:, j], Hinput[:, i]) + Qpy[:, i] = Qpy[:, i] - Rpy[j, i] * Qpy[:, j] + Rpy[i, i] = np.linalg.norm(Qpy[:, i]) + Qpy[:, i] = 1 / Rpy[i, i] * Qpy[:, i] - Qtest = np.array(Qout).reshape([numStates,3]) + Qtest = np.array(Qout).reshape([numStates, 3]) Rtest = np.array(Rout).reshape(3, 3) errorNorm1 = np.linalg.norm(Qpy - Qtest) errorNorm2 = np.linalg.norm(Rpy - Rtest) - if (errorNorm1 > 1.0E-10): + if errorNorm1 > 1.0e-10: print(errorNorm1, "QR decomp") testFailCount += 1 testMessages.append("QR decomp Failure in Q" + "\n") - if (errorNorm2 > 1.0E-10): + if errorNorm2 > 1.0e-10: print(errorNorm2, "QR decomp") testFailCount += 1 testMessages.append("QR decomp Failure in R" + "\n") errorNorm1 = np.linalg.norm(q + Qtest) - errorNorm2 = np.linalg.norm(r[:3,:3] + Rtest) - if (errorNorm1 > 1.0E-10): + errorNorm2 = np.linalg.norm(r[:3, :3] + Rtest) + if errorNorm1 > 1.0e-10: print(errorNorm1, "QR decomp") testFailCount += 1 testMessages.append("QR decomp Failure in Q" + "\n") - if (errorNorm2 > 1.0E-10): + if errorNorm2 > 1.0e-10: print(errorNorm2, "QR decomp") testFailCount += 1 testMessages.append("QR decomp Failure in R" + "\n") @@ -121,10 +124,10 @@ def horizonOpNav_methods(): V = np.ones(3) nIn = horizonOpNav.new_doubleArray(3) VIn = horizonOpNav.new_doubleArray(3) - RIn = horizonOpNav.new_doubleArray(numStates*3) + RIn = horizonOpNav.new_doubleArray(numStates * 3) for i in range(3): horizonOpNav.doubleArray_setitem(nIn, i, 0.0) - for i in range(3*3): + for i in range(3 * 3): horizonOpNav.doubleArray_setitem(RIn, i, r.flatten().tolist()[i]) for i in range(3): horizonOpNav.doubleArray_setitem(VIn, i, V.flatten().tolist()[i]) @@ -134,16 +137,17 @@ def horizonOpNav_methods(): for i in range(3): BackSubOut.append(horizonOpNav.doubleArray_getitem(nIn, i)) - exp = back_substitution(r[:3,:3], V) + exp = back_substitution(r[:3, :3], V) BackSubOut = np.array(BackSubOut) errorNorm = np.linalg.norm(exp - BackSubOut) - if(errorNorm > 1.0E-10): + if errorNorm > 1.0e-10: print(errorNorm, "BackSub") testFailCount += 1 testMessages.append("BackSub Failure " + "\n") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + ################################################################################### ## Testing dynamics matrix computation @@ -159,7 +163,9 @@ def horizonOpNav_update(): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process # Construct the ephemNavConverter module # Set the names for the input messages @@ -174,76 +180,776 @@ def horizonOpNav_update(): unitTestSim.AddModelToTask(unitTaskName, opNav) # These are example points for fitting used from an image processing algorithm - inputPoints = [226., 113., 227., 113., 223., 114., 224., 114., 225., 114., 219., - 115., 220., 115., 221., 115., 222., 115., 215., 116., 216., 116., - 217., 116., 218., 116., 212., 117., 213., 117., 214., 117., 209., - 118., 210., 118., 211., 118., 205., 119., 206., 119., 207., 119., - 208., 119., 204., 120., 205., 120., 201., 121., 202., 121., 203., - 121., 199., 122., 200., 122., 197., 123., 198., 123., 195., 124., - 196., 124., 193., 125., 194., 125., 191., 126., 192., 126., 189., - 127., 190., 127., 187., 128., 188., 128., 185., 129., 186., 129., - 183., 130., 184., 130., 181., 131., 182., 131., 180., 132., 181., - 132., 178., 133., 179., 133., 177., 134., 178., 134., 175., 135., - 176., 135., 174., 136., 175., 136., 172., 137., 173., 137., 171., - 138., 172., 138., 170., 139., 171., 139., 168., 140., 169., 140., - 167., 141., 168., 141., 166., 142., 167., 142., 164., 143., 165., - 143., 163., 144., 164., 144., 162., 145., 163., 145., 161., 146., - 162., 146., 160., 147., 161., 147., 159., 148., 160., 148., 158., - 149., 159., 149., 156., 150., 157., 150., 155., 151., 156., 151., - 154., 152., 155., 152., 153., 153., 154., 153., 153., 154., 152., - 155., 151., 156., 152., 156., 150., 157., 151., 157., 149., 158., - 150., 158., 148., 159., 149., 159., 147., 160., 148., 160., 146., - 161., 147., 161., 145., 162., 146., 162., 145., 163., 144., 164., - 143., 165., 144., 165., 142., 166., 143., 166., 142., 167., 141., - 168., 140., 169., 141., 169., 139., 170., 140., 170., 139., 171., - 138., 172., 137., 173., 138., 173., 137., 174., 136., 175., 135., - 176., 136., 176., 135., 177., 134., 178., 133., 179., 134., 179., - 133., 180., 132., 181., 132., 182., 131., 183., 131., 184., 130., - 185., 129., 186., 130., 186., 129., 187., 128., 188., 128., 189., - 127., 190., 127., 191., 126., 192., 126., 193., 125., 194., 125., - 195., 125., 196., 124., 197., 124., 198., 123., 199., 123., 200., - 122., 201., 122., 202., 122., 203., 121., 204., 120., 205., 121., - 205., 120., 206., 120., 207., 120., 208., 119., 209., 119., 210., - 119., 211., 118., 212., 118., 213., 118., 214., 117., 215., 117., - 216., 117., 217., 117., 218., 116., 219., 116., 220., 116., 221., - 116., 222., 115., 223., 115., 224., 115., 225., 115., 226., 114., - 227., 114., 228., 114., 229., 114., 230., 114., 231., 114., 232., - 113., 233., 113., 234., 113., 235., 113., 236., 113., 237., 113., - 238., 113., 239., 112., 240., 112., 241., 112., 242., 112., 243., - 112., 244., 112., 245., 112., 246., 112., 247., 112., 248., 112., - 249., 112., 250., 112., 251., 112., 252., 112., 253., 112., 254., - 111., 255., 111., 256., 112., 257., 112., 258., 112., 259., 112., - 260., 112., 261., 112., 262., 112., 263., 112., 264., 112., 265., - 112., 266., 112., 267., 112., 268., 112., 269., 112., 270., 112., - 271., 113., 272., 113., 273., 113., 274., 113., 275., 113., 276., - 113., 277., 113., 278., 114., 279., 114., 280., 114., 281., 114., - 282., 114., 283., 114., 284., 115., 285., 115., 286., 115., 287., - 115., 288., 116., 289., 116., 290., 116., 291., 116., 292., 117., - 293., 117., 294., 117., 295., 117., 296., 118., 297., 118., 298., - 118., 299., 119., 300., 119., 301., 119., 302., 120., 303., 120., - 304., 120., 305., 121., 306., 121., 307., 122., 308., 122., 309., - 122., 310., 123., 311., 123., 312., 124., 313., 124., 314., 125., - 315., 125., 316., 125., 317., 126., 318., 126., 319., 127., 320., - 127., 321., 128., 322., 128., 323., 129., 324., 129., 325., 130., - 325., 130., 326., 131., 327., 131., 328., 132., 329., 132., 330., - 133., 331., 133., 332., 134., 332., 134., 333., 135., 334., 135., - 335., 136., 335., 136., 336., 137., 337., 137., 338., 138., 338., - 138., 339., 139., 340., 139., 341., 140., 341., 140., 342., 141., - 342., 141., 343., 142., 344., 142., 345., 143., 345., 143., 346., - 144., 346., 144., 347., 145., 348., 145., 349., 146., 349., 146., - 350., 147., 350., 147., 351., 148., 351., 148., 352., 149., 352., - 149., 353., 150., 353., 150., 354., 151., 354., 151., 355., 152., - 356., 152., 357., 153., 357., 153., 358., 154., 358., 154., 359., - 155., 359., 155., 360., 156., 360., 156., 361., 157., 361., 158., - 362., 159., 362., 159., 363., 160., 363., 160., 364., 161., 364., - 161., 365., 162., 365., 162., 366., 163., 366., 163., 367., 164., - 367., 164., 368., 165., 368., 166., 369., 167., 369., 167., 370., - 168., 370., 168., 371., 169., 371., 169., 372., 170., 372., 171., - 373., 172., 373., 172., 374., 173., 374., 174., 375., 175., 375., - 175., 376., 176., 376., 177., 377., 178., 377., 178., 378., 179., - 378., 180., 379., 181., 379., 181., 380., 182., 380., 183., 381., - 184., 381., 185., 382., 186., 382., 187., 383., 188., 383., 188., - 384., 189., 384., 190., 385., 191., 385., 192., 386.] + inputPoints = [ + 226.0, + 113.0, + 227.0, + 113.0, + 223.0, + 114.0, + 224.0, + 114.0, + 225.0, + 114.0, + 219.0, + 115.0, + 220.0, + 115.0, + 221.0, + 115.0, + 222.0, + 115.0, + 215.0, + 116.0, + 216.0, + 116.0, + 217.0, + 116.0, + 218.0, + 116.0, + 212.0, + 117.0, + 213.0, + 117.0, + 214.0, + 117.0, + 209.0, + 118.0, + 210.0, + 118.0, + 211.0, + 118.0, + 205.0, + 119.0, + 206.0, + 119.0, + 207.0, + 119.0, + 208.0, + 119.0, + 204.0, + 120.0, + 205.0, + 120.0, + 201.0, + 121.0, + 202.0, + 121.0, + 203.0, + 121.0, + 199.0, + 122.0, + 200.0, + 122.0, + 197.0, + 123.0, + 198.0, + 123.0, + 195.0, + 124.0, + 196.0, + 124.0, + 193.0, + 125.0, + 194.0, + 125.0, + 191.0, + 126.0, + 192.0, + 126.0, + 189.0, + 127.0, + 190.0, + 127.0, + 187.0, + 128.0, + 188.0, + 128.0, + 185.0, + 129.0, + 186.0, + 129.0, + 183.0, + 130.0, + 184.0, + 130.0, + 181.0, + 131.0, + 182.0, + 131.0, + 180.0, + 132.0, + 181.0, + 132.0, + 178.0, + 133.0, + 179.0, + 133.0, + 177.0, + 134.0, + 178.0, + 134.0, + 175.0, + 135.0, + 176.0, + 135.0, + 174.0, + 136.0, + 175.0, + 136.0, + 172.0, + 137.0, + 173.0, + 137.0, + 171.0, + 138.0, + 172.0, + 138.0, + 170.0, + 139.0, + 171.0, + 139.0, + 168.0, + 140.0, + 169.0, + 140.0, + 167.0, + 141.0, + 168.0, + 141.0, + 166.0, + 142.0, + 167.0, + 142.0, + 164.0, + 143.0, + 165.0, + 143.0, + 163.0, + 144.0, + 164.0, + 144.0, + 162.0, + 145.0, + 163.0, + 145.0, + 161.0, + 146.0, + 162.0, + 146.0, + 160.0, + 147.0, + 161.0, + 147.0, + 159.0, + 148.0, + 160.0, + 148.0, + 158.0, + 149.0, + 159.0, + 149.0, + 156.0, + 150.0, + 157.0, + 150.0, + 155.0, + 151.0, + 156.0, + 151.0, + 154.0, + 152.0, + 155.0, + 152.0, + 153.0, + 153.0, + 154.0, + 153.0, + 153.0, + 154.0, + 152.0, + 155.0, + 151.0, + 156.0, + 152.0, + 156.0, + 150.0, + 157.0, + 151.0, + 157.0, + 149.0, + 158.0, + 150.0, + 158.0, + 148.0, + 159.0, + 149.0, + 159.0, + 147.0, + 160.0, + 148.0, + 160.0, + 146.0, + 161.0, + 147.0, + 161.0, + 145.0, + 162.0, + 146.0, + 162.0, + 145.0, + 163.0, + 144.0, + 164.0, + 143.0, + 165.0, + 144.0, + 165.0, + 142.0, + 166.0, + 143.0, + 166.0, + 142.0, + 167.0, + 141.0, + 168.0, + 140.0, + 169.0, + 141.0, + 169.0, + 139.0, + 170.0, + 140.0, + 170.0, + 139.0, + 171.0, + 138.0, + 172.0, + 137.0, + 173.0, + 138.0, + 173.0, + 137.0, + 174.0, + 136.0, + 175.0, + 135.0, + 176.0, + 136.0, + 176.0, + 135.0, + 177.0, + 134.0, + 178.0, + 133.0, + 179.0, + 134.0, + 179.0, + 133.0, + 180.0, + 132.0, + 181.0, + 132.0, + 182.0, + 131.0, + 183.0, + 131.0, + 184.0, + 130.0, + 185.0, + 129.0, + 186.0, + 130.0, + 186.0, + 129.0, + 187.0, + 128.0, + 188.0, + 128.0, + 189.0, + 127.0, + 190.0, + 127.0, + 191.0, + 126.0, + 192.0, + 126.0, + 193.0, + 125.0, + 194.0, + 125.0, + 195.0, + 125.0, + 196.0, + 124.0, + 197.0, + 124.0, + 198.0, + 123.0, + 199.0, + 123.0, + 200.0, + 122.0, + 201.0, + 122.0, + 202.0, + 122.0, + 203.0, + 121.0, + 204.0, + 120.0, + 205.0, + 121.0, + 205.0, + 120.0, + 206.0, + 120.0, + 207.0, + 120.0, + 208.0, + 119.0, + 209.0, + 119.0, + 210.0, + 119.0, + 211.0, + 118.0, + 212.0, + 118.0, + 213.0, + 118.0, + 214.0, + 117.0, + 215.0, + 117.0, + 216.0, + 117.0, + 217.0, + 117.0, + 218.0, + 116.0, + 219.0, + 116.0, + 220.0, + 116.0, + 221.0, + 116.0, + 222.0, + 115.0, + 223.0, + 115.0, + 224.0, + 115.0, + 225.0, + 115.0, + 226.0, + 114.0, + 227.0, + 114.0, + 228.0, + 114.0, + 229.0, + 114.0, + 230.0, + 114.0, + 231.0, + 114.0, + 232.0, + 113.0, + 233.0, + 113.0, + 234.0, + 113.0, + 235.0, + 113.0, + 236.0, + 113.0, + 237.0, + 113.0, + 238.0, + 113.0, + 239.0, + 112.0, + 240.0, + 112.0, + 241.0, + 112.0, + 242.0, + 112.0, + 243.0, + 112.0, + 244.0, + 112.0, + 245.0, + 112.0, + 246.0, + 112.0, + 247.0, + 112.0, + 248.0, + 112.0, + 249.0, + 112.0, + 250.0, + 112.0, + 251.0, + 112.0, + 252.0, + 112.0, + 253.0, + 112.0, + 254.0, + 111.0, + 255.0, + 111.0, + 256.0, + 112.0, + 257.0, + 112.0, + 258.0, + 112.0, + 259.0, + 112.0, + 260.0, + 112.0, + 261.0, + 112.0, + 262.0, + 112.0, + 263.0, + 112.0, + 264.0, + 112.0, + 265.0, + 112.0, + 266.0, + 112.0, + 267.0, + 112.0, + 268.0, + 112.0, + 269.0, + 112.0, + 270.0, + 112.0, + 271.0, + 113.0, + 272.0, + 113.0, + 273.0, + 113.0, + 274.0, + 113.0, + 275.0, + 113.0, + 276.0, + 113.0, + 277.0, + 113.0, + 278.0, + 114.0, + 279.0, + 114.0, + 280.0, + 114.0, + 281.0, + 114.0, + 282.0, + 114.0, + 283.0, + 114.0, + 284.0, + 115.0, + 285.0, + 115.0, + 286.0, + 115.0, + 287.0, + 115.0, + 288.0, + 116.0, + 289.0, + 116.0, + 290.0, + 116.0, + 291.0, + 116.0, + 292.0, + 117.0, + 293.0, + 117.0, + 294.0, + 117.0, + 295.0, + 117.0, + 296.0, + 118.0, + 297.0, + 118.0, + 298.0, + 118.0, + 299.0, + 119.0, + 300.0, + 119.0, + 301.0, + 119.0, + 302.0, + 120.0, + 303.0, + 120.0, + 304.0, + 120.0, + 305.0, + 121.0, + 306.0, + 121.0, + 307.0, + 122.0, + 308.0, + 122.0, + 309.0, + 122.0, + 310.0, + 123.0, + 311.0, + 123.0, + 312.0, + 124.0, + 313.0, + 124.0, + 314.0, + 125.0, + 315.0, + 125.0, + 316.0, + 125.0, + 317.0, + 126.0, + 318.0, + 126.0, + 319.0, + 127.0, + 320.0, + 127.0, + 321.0, + 128.0, + 322.0, + 128.0, + 323.0, + 129.0, + 324.0, + 129.0, + 325.0, + 130.0, + 325.0, + 130.0, + 326.0, + 131.0, + 327.0, + 131.0, + 328.0, + 132.0, + 329.0, + 132.0, + 330.0, + 133.0, + 331.0, + 133.0, + 332.0, + 134.0, + 332.0, + 134.0, + 333.0, + 135.0, + 334.0, + 135.0, + 335.0, + 136.0, + 335.0, + 136.0, + 336.0, + 137.0, + 337.0, + 137.0, + 338.0, + 138.0, + 338.0, + 138.0, + 339.0, + 139.0, + 340.0, + 139.0, + 341.0, + 140.0, + 341.0, + 140.0, + 342.0, + 141.0, + 342.0, + 141.0, + 343.0, + 142.0, + 344.0, + 142.0, + 345.0, + 143.0, + 345.0, + 143.0, + 346.0, + 144.0, + 346.0, + 144.0, + 347.0, + 145.0, + 348.0, + 145.0, + 349.0, + 146.0, + 349.0, + 146.0, + 350.0, + 147.0, + 350.0, + 147.0, + 351.0, + 148.0, + 351.0, + 148.0, + 352.0, + 149.0, + 352.0, + 149.0, + 353.0, + 150.0, + 353.0, + 150.0, + 354.0, + 151.0, + 354.0, + 151.0, + 355.0, + 152.0, + 356.0, + 152.0, + 357.0, + 153.0, + 357.0, + 153.0, + 358.0, + 154.0, + 358.0, + 154.0, + 359.0, + 155.0, + 359.0, + 155.0, + 360.0, + 156.0, + 360.0, + 156.0, + 361.0, + 157.0, + 361.0, + 158.0, + 362.0, + 159.0, + 362.0, + 159.0, + 363.0, + 160.0, + 363.0, + 160.0, + 364.0, + 161.0, + 364.0, + 161.0, + 365.0, + 162.0, + 365.0, + 162.0, + 366.0, + 163.0, + 366.0, + 163.0, + 367.0, + 164.0, + 367.0, + 164.0, + 368.0, + 165.0, + 368.0, + 166.0, + 369.0, + 167.0, + 369.0, + 167.0, + 370.0, + 168.0, + 370.0, + 168.0, + 371.0, + 169.0, + 371.0, + 169.0, + 372.0, + 170.0, + 372.0, + 171.0, + 373.0, + 172.0, + 373.0, + 172.0, + 374.0, + 173.0, + 374.0, + 174.0, + 375.0, + 175.0, + 375.0, + 175.0, + 376.0, + 176.0, + 376.0, + 177.0, + 377.0, + 178.0, + 377.0, + 178.0, + 378.0, + 179.0, + 378.0, + 180.0, + 379.0, + 181.0, + 379.0, + 181.0, + 380.0, + 182.0, + 380.0, + 183.0, + 381.0, + 184.0, + 381.0, + 185.0, + 382.0, + 186.0, + 382.0, + 187.0, + 383.0, + 188.0, + 383.0, + 188.0, + 384.0, + 189.0, + 384.0, + 190.0, + 385.0, + 191.0, + 385.0, + 192.0, + 386.0, + ] # Create the input messages. inputCamera = messaging.CameraConfigMsgPayload() @@ -251,27 +957,27 @@ def horizonOpNav_update(): inputAtt = messaging.NavAttMsgPayload() # Set camera - inputCamera.fieldOfView = 2.0 * np.arctan(10*1e-3 / 2.0 / 1. ) # 2*arctan(s/2 / f) + inputCamera.fieldOfView = 2.0 * np.arctan( + 10 * 1e-3 / 2.0 / 1.0 + ) # 2*arctan(s/2 / f) inputCamera.resolution = [512, 512] - inputCamera.sigma_CB = [1.,0.2,0.3] + inputCamera.sigma_CB = [1.0, 0.2, 0.3] camInMsg = messaging.CameraConfigMsg().write(inputCamera) opNav.cameraConfigInMsg.subscribeTo(camInMsg) # Set circles inputLimbMsg.valid = 1 inputLimbMsg.limbPoints = inputPoints - inputLimbMsg.numLimbPoints = int(len(inputPoints)/2) + inputLimbMsg.numLimbPoints = int(len(inputPoints) / 2) inputLimbMsg.timeTag = 12345 limbInMsg = messaging.OpNavLimbMsg().write(inputLimbMsg) opNav.limbInMsg.subscribeTo(limbInMsg) - # Set attitude - inputAtt.sigma_BN = [0.6, 1., 0.1] + inputAtt.sigma_BN = [0.6, 1.0, 0.1] attInMsg = messaging.NavAttMsg().write(inputAtt) opNav.attInMsg.subscribeTo(attInMsg) - # Set module for Mars opNav.planetTarget = 2 dataLog = opNav.opNavOutMsg.recorder() @@ -286,57 +992,64 @@ def horizonOpNav_update(): # Truth Vlaues ############################ Q = np.eye(3) - B = np.zeros([3,3]) - Q *= 1/(3396.19*1E3) # km + B = np.zeros([3, 3]) + Q *= 1 / (3396.19 * 1e3) # km # Q[2,2] = 1/(3376.2*1E3) - numPoints = int(len(inputPoints)/2) + numPoints = int(len(inputPoints) / 2) CB = rbk.MRP2C(inputCamera.sigma_CB) BN = rbk.MRP2C(inputAtt.sigma_BN) - CN = np.dot(CB,BN) + CN = np.dot(CB, BN) B = np.dot(Q, CN.T) # Transf camera to meters - alpha =0 + alpha = 0 up = inputCamera.resolution[0] / 2 vp = inputCamera.resolution[1] / 2 - pX = 2. * np.tan(inputCamera.fieldOfView * inputCamera.resolution[0] / inputCamera.resolution[1] / 2.0) - pY = 2. * np.tan(inputCamera.fieldOfView / 2.0) + pX = 2.0 * np.tan( + inputCamera.fieldOfView + * inputCamera.resolution[0] + / inputCamera.resolution[1] + / 2.0 + ) + pY = 2.0 * np.tan(inputCamera.fieldOfView / 2.0) d_x = inputCamera.resolution[0] / pX d_y = inputCamera.resolution[1] / pY - transf = np.zeros([3,3]) + transf = np.zeros([3, 3]) transf[0, 0] = 1 / d_x transf[1, 1] = 1 / d_y transf[2, 2] = 1 - transf[0, 1] = -alpha/(d_x*d_y) - transf[0, 2] = (alpha*vp - d_y*up)/ (d_x * d_y) + transf[0, 1] = -alpha / (d_x * d_y) + transf[0, 2] = (alpha * vp - d_y * up) / (d_x * d_y) transf[1, 2] = -vp / (d_y) - s = np.zeros([numPoints,3]) - sBar = np.zeros([numPoints,3]) - sBarPrime = np.zeros([numPoints,3]) - H = np.zeros([numPoints,3]) + s = np.zeros([numPoints, 3]) + sBar = np.zeros([numPoints, 3]) + sBarPrime = np.zeros([numPoints, 3]) + H = np.zeros([numPoints, 3]) for i in range(numPoints): - s[i,:] = np.dot(transf, np.array([inputPoints[2*i], inputPoints[2*i+1], 1])) - sBar[i,:] = np.dot(B, s[i,:]) - sBarPrime[i,:] = sBar[i,:]/np.linalg.norm(sBar[i,:]) - H[i,:] = sBarPrime[i,:] + s[i, :] = np.dot( + transf, np.array([inputPoints[2 * i], inputPoints[2 * i + 1], 1]) + ) + sBar[i, :] = np.dot(B, s[i, :]) + sBarPrime[i, :] = sBar[i, :] / np.linalg.norm(sBar[i, :]) + H[i, :] = sBarPrime[i, :] # QR H - Rpy = np.zeros([3,3]) + Rpy = np.zeros([3, 3]) Qpy = np.zeros([numPoints, 3]) - for i in range(0,3): - Qpy[:,i] = H[:,i] + for i in range(0, 3): + Qpy[:, i] = H[:, i] for j in range(i): - Rpy[j,i] = np.dot(Qpy[:,j], H[:,i]) - Qpy[:,i]= Qpy[:,i] - Rpy[j,i]*Qpy[:,j] - Rpy[i,i] = np.linalg.norm(Qpy[:,i]) - Qpy[:,i] = 1 / Rpy[i,i] * Qpy[:,i] + Rpy[j, i] = np.dot(Qpy[:, j], H[:, i]) + Qpy[:, i] = Qpy[:, i] - Rpy[j, i] * Qpy[:, j] + Rpy[i, i] = np.linalg.norm(Qpy[:, i]) + Qpy[:, i] = 1 / Rpy[i, i] * Qpy[:, i] errorNorm1 = np.linalg.norm(np.dot(Qpy, Rpy) - H) - if (errorNorm1 > 1.0E-8): + if errorNorm1 > 1.0e-8: print(errorNorm1, "QR decomp") testFailCount += 1 testMessages.append("QR decomp Failure in update test " + "\n") @@ -346,26 +1059,36 @@ def horizonOpNav_update(): n = back_substitution(Rpy, RHS) n_test = np.dot(np.linalg.inv(Rpy), RHS) - R_s = (opNav.noiseSF*inputCamera.resolution[0]/(numPoints))**2/d_x**2*np.array([[1,0,0],[0,1,0],[0,0,0]]) + R_s = ( + (opNav.noiseSF * inputCamera.resolution[0] / (numPoints)) ** 2 + / d_x**2 + * np.array([[1, 0, 0], [0, 1, 0], [0, 0, 0]]) + ) R_s = np.dot(np.dot(B, R_s), B.T) R_yInv = np.zeros([numPoints, numPoints]) for i in range(numPoints): - J = 1./np.linalg.norm(sBar[i,:])*np.dot(n, np.eye(3) - np.outer(sBarPrime[i,:], sBarPrime[i,:])) + J = ( + 1.0 + / np.linalg.norm(sBar[i, :]) + * np.dot(n, np.eye(3) - np.outer(sBarPrime[i, :], sBarPrime[i, :])) + ) temp = np.dot(R_s, J) - R_yInv[i,i] = 1./np.dot(temp, J) + R_yInv[i, i] = 1.0 / np.dot(temp, J) - Pn = np.linalg.inv(np.dot(np.dot(H.T, R_yInv),H)) - F = -(np.dot(n,n) - 1)**(-0.5)*np.dot(np.linalg.inv(B), np.eye(3) - np.outer(n,n)/(np.dot(n,n)-1)) + Pn = np.linalg.inv(np.dot(np.dot(H.T, R_yInv), H)) + F = -((np.dot(n, n) - 1) ** (-0.5)) * np.dot( + np.linalg.inv(B), np.eye(3) - np.outer(n, n) / (np.dot(n, n) - 1) + ) Covar_C_test = np.dot(np.dot(F, Pn), F.T) errorNorm1 = np.linalg.norm(n_test - n) - if (errorNorm1 > 1.0E-8): + if errorNorm1 > 1.0e-8: print(errorNorm1, "Back Sub") testFailCount += 1 testMessages.append("Back Sub Failure in update test " + "\n") - r_BN_C = - (np.dot(n,n) - 1.)**(-0.5)*np.dot(np.linalg.inv(B), n) + r_BN_C = -((np.dot(n, n) - 1.0) ** (-0.5)) * np.dot(np.linalg.inv(B), n) - posErr = 1e-3 #(m) + posErr = 1e-3 # (m) covarErr = 1e-5 unitTestSupport.writeTeXSnippet("toleranceValuePos", str(posErr), path) unitTestSupport.writeTeXSnippet("toleranceValueVel", str(covarErr), path) @@ -375,31 +1098,44 @@ def horizonOpNav_update(): outputTime = dataLog.timeTag for i in range(len(outputR[-1, 1:])): - if np.abs((r_BN_C[i] - outputR[0, i])/r_BN_C[i]) > posErr or np.isnan(outputR.any()): + if np.abs((r_BN_C[i] - outputR[0, i]) / r_BN_C[i]) > posErr or np.isnan( + outputR.any() + ): testFailCount += 1 - testMessages.append("FAILED: Position Check in Horizon Nav for index "+ str(i) + " with error " + str(np.abs((r_BN_C[i] - outputR[-1, i+1])/r_BN_C[i]))) + testMessages.append( + "FAILED: Position Check in Horizon Nav for index " + + str(i) + + " with error " + + str(np.abs((r_BN_C[i] - outputR[-1, i + 1]) / r_BN_C[i])) + ) for i in range(len(outputCovar[-1, 1:])): - if np.abs((Covar_C_test.flatten()[i] - outputCovar[0, i])/Covar_C_test.flatten()[i]) > covarErr or np.isnan(outputTime.any()): + if np.abs( + (Covar_C_test.flatten()[i] - outputCovar[0, i]) / Covar_C_test.flatten()[i] + ) > covarErr or np.isnan(outputTime.any()): testFailCount += 1 - testMessages.append("FAILED: Covar Check in Horizon Nav for index "+ str(i) + " with error " + str(np.abs((Covar_C_test.flatten()[i] - outputCovar[-1, i+1])))) + testMessages.append( + "FAILED: Covar Check in Horizon Nav for index " + + str(i) + + " with error " + + str(np.abs((Covar_C_test.flatten()[i] - outputCovar[-1, i + 1]))) + ) snippentName = "passFail" if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + opNav.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + opNav.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" print(testMessages) unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] -if __name__ == '__main__': +if __name__ == "__main__": # horizonOpNav_methods() horizonOpNav_update() diff --git a/src/fswAlgorithms/imageProcessing/houghCircles/_UnitTest/test_houghCirlces.py b/src/fswAlgorithms/imageProcessing/houghCircles/_UnitTest/test_houghCirlces.py index 2ef09e76f2..585ababa05 100755 --- a/src/fswAlgorithms/imageProcessing/houghCircles/_UnitTest/test_houghCirlces.py +++ b/src/fswAlgorithms/imageProcessing/houghCircles/_UnitTest/test_houghCirlces.py @@ -1,19 +1,18 @@ - - # 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. +# 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. # @@ -31,7 +30,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation @@ -60,14 +59,29 @@ # @pytest.mark.xfail(conditionstring) # Provide a unique test method name, starting with 'test_'. -@pytest.mark.skipif(importErr, reason= reasonErr) -@pytest.mark.parametrize("image, blur, maxCircles, minDist, minRad, cannyLow, cannyHigh, dp, saveImage", [ - ("mars.png", 5, 1, 50, 20, 20, 200, 1, False), #Mars image - ("moons.png", 5, 10, 25, 10, 20, 200, 1, False) # Moon images - ]) + +@pytest.mark.skipif(importErr, reason=reasonErr) +@pytest.mark.parametrize( + "image, blur, maxCircles, minDist, minRad, cannyLow, cannyHigh, dp, saveImage", + [ + ("mars.png", 5, 1, 50, 20, 20, 200, 1, False), # Mars image + ("moons.png", 5, 10, 25, 10, 20, 200, 1, False), # Moon images + ], +) # update "module" in this function name to reflect the module name -def test_module(show_plots, image, blur, maxCircles , minDist , minRad, cannyLow, cannyHigh, dp, saveImage): +def test_module( + show_plots, + image, + blur, + maxCircles, + minDist, + minRad, + cannyLow, + cannyHigh, + dp, + saveImage, +): """ Unit test for Hough Circles. The unit test specifically runs on 2 images: @@ -78,32 +92,52 @@ def test_module(show_plots, image, blur, maxCircles , minDist , minRad, cannyLow This modules compares directly to the expected circles from the images. """ # each test method requires a single assert method to be called - [testResults, testMessage] = houghCirclesTest(show_plots, image, blur, maxCircles , minDist , minRad, cannyLow, cannyHigh, dp, saveImage) + [testResults, testMessage] = houghCirclesTest( + show_plots, + image, + blur, + maxCircles, + minDist, + minRad, + cannyLow, + cannyHigh, + dp, + saveImage, + ) assert testResults < 1, testMessage -def houghCirclesTest(show_plots, image, blur, maxCircles , minDist , minRad, cannyLow, cannyHigh, dp, saveImage): - +def houghCirclesTest( + show_plots, + image, + blur, + maxCircles, + minDist, + minRad, + cannyLow, + cannyHigh, + dp, + saveImage, +): # Truth values from python - imagePath = path + '/' + image + imagePath = path + "/" + image input_image = Image.open(imagePath) input_image.load() ################################################# - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm module = houghCircles.HoughCircles() module.ModelTag = "houghCircles" @@ -119,17 +153,24 @@ def houghCirclesTest(show_plots, image, blur, maxCircles , minDist , minRad, can module.houghMinRadius = minRad module.blurrSize = blur module.dpValue = dp - module.houghMaxRadius = int(input_image.size[0]/1.25) + module.houghMaxRadius = int(input_image.size[0] / 1.25) circles = [] if image == "mars.png": circles = [(250, 260, 110)] if image == "moons.png": - circles = [(205, 155, 48.900001525878906), (590, 313, 46.29999923706055), (590, 165, 46.29999923706055), (400, 313, 43.79999923706055), (400, 151.5, 45), (210, 313, 45)] + circles = [ + (205, 155, 48.900001525878906), + (590, 313, 46.29999923706055), + (590, 165, 46.29999923706055), + (400, 313, 43.79999923706055), + (400, 151.5, 45), + (210, 313, 45), + ] # Create input message and size it because the regular creator of that message # is not part of the test. inputMessageData = messaging.CameraImageMsgPayload() - inputMessageData.timeTag = int(1E9) + inputMessageData.timeTag = int(1e9) inputMessageData.cameraID = 1 imgInMsg = messaging.CameraImageMsg().write(inputMessageData) module.imageInMsg.subscribeTo(imgInMsg) @@ -145,7 +186,7 @@ def houghCirclesTest(show_plots, image, blur, maxCircles , minDist , minRad, can # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -159,32 +200,32 @@ def houghCirclesTest(show_plots, image, blur, maxCircles , minDist , minRad, can draw_result = ImageDraw.Draw(output_image) imageProcCircles = [] - for j in range(len(radii[-1,1:])): - if radii[-1,j] > 0: - imageProcCircles.append((centers[-1, 2*j], centers[-1, 2*j+1], radii[-1, j])) + for j in range(len(radii[-1, 1:])): + if radii[-1, j] > 0: + imageProcCircles.append( + (centers[-1, 2 * j], centers[-1, 2 * j + 1], radii[-1, j]) + ) for x, y, r in imageProcCircles: draw_result.ellipse((x - r, y - r, x + r, y + r), outline=(255, 0, 0, 0)) # Save output image if saveImage: - output_image.save("result_"+ image) + output_image.save("result_" + image) if show_plots: print(imageProcCircles[0]) output_image.show() - # print out success message if no error were found for testCircle, refCircle in zip(imageProcCircles, circles): for i in range(3): - if np.abs((testCircle[i] - refCircle[i])/refCircle[i])>1: - testFailCount+=1 + if np.abs((testCircle[i] - refCircle[i]) / refCircle[i]) > 1: + testFailCount += 1 testMessages.append("Test failed processing " + image) - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -192,4 +233,4 @@ def houghCirclesTest(show_plots, image, blur, maxCircles , minDist , minRad, can # stand-along python script # if __name__ == "__main__": - houghCirclesTest(True, "moons.png", 5, 10, 25, 10, 20, 200, 1, True) # Moon images + houghCirclesTest(True, "moons.png", 5, 10, 25, 10, 20, 200, 1, True) # Moon images diff --git a/src/fswAlgorithms/imageProcessing/limbFinding/_UnitTest/test_limbFinding.py b/src/fswAlgorithms/imageProcessing/limbFinding/_UnitTest/test_limbFinding.py index b6d9e036e3..1628a97e1d 100755 --- a/src/fswAlgorithms/imageProcessing/limbFinding/_UnitTest/test_limbFinding.py +++ b/src/fswAlgorithms/imageProcessing/limbFinding/_UnitTest/test_limbFinding.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -30,7 +29,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation @@ -46,6 +45,7 @@ from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.architecture import messaging + try: from Basilisk.fswAlgorithms import limbFinding except ImportError: @@ -58,12 +58,16 @@ # @pytest.mark.xfail(conditionstring) # Provide a unique test method name, starting with 'test_'. -@pytest.mark.skipif(importErr, reason= reasonErr) -@pytest.mark.parametrize("image, blur, cannyLow, cannyHigh, saveImage", [ - ("MarsBright.jpg", 1, 100, 200, False), #Mars image - ("MarsDark.jpg", 1, 100, 200, False), # Mars image - ("moons.jpg", 3, 200, 300, False) # Moon images - ]) + +@pytest.mark.skipif(importErr, reason=reasonErr) +@pytest.mark.parametrize( + "image, blur, cannyLow, cannyHigh, saveImage", + [ + ("MarsBright.jpg", 1, 100, 200, False), # Mars image + ("MarsDark.jpg", 1, 100, 200, False), # Mars image + ("moons.jpg", 3, 200, 300, False), # Moon images + ], +) # update "module" in this function name to reflect the module name def test_module(show_plots, image, blur, cannyLow, cannyHigh, saveImage): @@ -80,32 +84,32 @@ def test_module(show_plots, image, blur, cannyLow, cannyHigh, saveImage): on the limb. """ # each test method requires a single assert method to be called - [testResults, testMessage] = limbFindingTest(show_plots, image, blur, cannyLow, cannyHigh, saveImage) + [testResults, testMessage] = limbFindingTest( + show_plots, image, blur, cannyLow, cannyHigh, saveImage + ) assert testResults < 1, testMessage def limbFindingTest(show_plots, image, blur, cannyLow, cannyHigh, saveImage): - # Truth values from python - imagePath = path + '/' + image + imagePath = path + "/" + image input_image = Image.open(imagePath) input_image.load() ################################################# - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = limbFinding.LimbFinding() module.ModelTag = "limbFind" @@ -122,17 +126,17 @@ def limbFindingTest(show_plots, image, blur, cannyLow, cannyHigh, saveImage): refPoints = 0 if image == "MarsBright.jpg": reference = [253.0, 110.0] - refPoints = 2*475.0 + refPoints = 2 * 475.0 if image == "MarsDark.jpg": reference = [187.0, 128.0] - refPoints = 2*192.0 + refPoints = 2 * 192.0 if image == "moons.jpg": reference = [213.0, 66.0] - refPoints = 2*270.0 + refPoints = 2 * 270.0 # Create input message and size it because the regular creator of that message # is not part of the test. inputMessageData = messaging.CameraImageMsgPayload() - inputMessageData.timeTag = int(1E9) + inputMessageData.timeTag = int(1e9) inputMessageData.cameraID = 1 imageInMsg = messaging.CameraImageMsg().write(inputMessageData) module.imageInMsg.subscribeTo(imageInMsg) @@ -141,7 +145,6 @@ def limbFindingTest(show_plots, image, blur, cannyLow, cannyHigh, saveImage): dataLog = module.opnavLimbOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) - # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() @@ -149,13 +152,13 @@ def limbFindingTest(show_plots, image, blur, cannyLow, cannyHigh, saveImage): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() valid = dataLog.valid - points = dataLog.limbPoints[:, :2*1000] + points = dataLog.limbPoints[:, : 2 * 1000] numPoints = dataLog.numLimbPoints # Output image: @@ -164,30 +167,29 @@ def limbFindingTest(show_plots, image, blur, cannyLow, cannyHigh, saveImage): draw_result = ImageDraw.Draw(output_image) imageProcLimb = [] - for j in range(int(len(points[-1,1:])/2)): - if points[-1,2*j]>1E-2: - imageProcLimb.append((points[-1,2*j], points[-1,2*j+1])) + for j in range(int(len(points[-1, 1:]) / 2)): + if points[-1, 2 * j] > 1e-2: + imageProcLimb.append((points[-1, 2 * j], points[-1, 2 * j + 1])) draw_result.point(imageProcLimb, fill=128) # Save output image if saveImage: - output_image.save("result_"+ image) + output_image.save("result_" + image) if show_plots: output_image.show() - # print out success message if no error were found for i in range(2): - if np.abs((reference[i] - imageProcLimb[0][i])/reference[i])>1: - print(np.abs((reference[i] - imageProcLimb[0][i])/reference[i])) + if np.abs((reference[i] - imageProcLimb[0][i]) / reference[i]) > 1: + print(np.abs((reference[i] - imageProcLimb[0][i]) / reference[i])) testFailCount += 1 testMessages.append("Limb Test failed processing " + image) if valid[-1] != 1: testFailCount += 1 testMessages.append("Validity test failed processing " + image) - if np.abs(numPoints[-1]-refPoints)>10: + if np.abs(numPoints[-1] - refPoints) > 10: testFailCount += 1 testMessages.append("NumPoints test failed processing " + image) @@ -198,7 +200,7 @@ def limbFindingTest(show_plots, image, blur, cannyLow, cannyHigh, saveImage): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -206,4 +208,4 @@ def limbFindingTest(show_plots, image, blur, cannyLow, cannyHigh, saveImage): # stand-along python script # if __name__ == "__main__": - limbFindingTest(True, "MarsBright.jpg", 1, 100, 200, True) # Moon images + limbFindingTest(True, "MarsBright.jpg", 1, 100, 200, True) # Moon images diff --git a/src/fswAlgorithms/imageProcessing/pixelLineConverter/_UnitTest/test_pixelLineConverter.py b/src/fswAlgorithms/imageProcessing/pixelLineConverter/_UnitTest/test_pixelLineConverter.py index 600b66ff43..77cd17f3f5 100644 --- a/src/fswAlgorithms/imageProcessing/pixelLineConverter/_UnitTest/test_pixelLineConverter.py +++ b/src/fswAlgorithms/imageProcessing/pixelLineConverter/_UnitTest/test_pixelLineConverter.py @@ -16,46 +16,60 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) + def mapState(state, planet, camera): D = planet["diameter"] - pX = 2. * np.tan(camera.fieldOfView * camera.resolution[0] / camera.resolution[1] / 2.0) - pY = 2. * np.tan(camera.fieldOfView/2.0) - d_x = pX/camera.resolution[0] - d_y = pY/camera.resolution[1] + pX = 2.0 * np.tan( + camera.fieldOfView * camera.resolution[0] / camera.resolution[1] / 2.0 + ) + pY = 2.0 * np.tan(camera.fieldOfView / 2.0) + d_x = pX / camera.resolution[0] + d_y = pY / camera.resolution[1] - A = 2 * np.arctan(state[2]*d_x) + A = 2 * np.arctan(state[2] * d_x) - norm = 0.5 * D/np.sin(0.5*A) - vec = np.array([state[0]*d_x, state[1]*d_y, 1.]) - return norm*vec/np.linalg.norm(vec) + norm = 0.5 * D / np.sin(0.5 * A) + vec = np.array([state[0] * d_x, state[1] * d_y, 1.0]) + return norm * vec / np.linalg.norm(vec) def mapCovar(CovarXYR, rho, planet, camera): D = planet["diameter"] - pX = 2. * np.tan(camera.fieldOfView * camera.resolution[0] / camera.resolution[1] / 2.0) - pY = 2. * np.tan(camera.fieldOfView/2.0) + pX = 2.0 * np.tan( + camera.fieldOfView * camera.resolution[0] / camera.resolution[1] / 2.0 + ) + pY = 2.0 * np.tan(camera.fieldOfView / 2.0) d_x = pX / camera.resolution[0] d_y = pY / camera.resolution[1] - A = 2 * np.arctan(rho*d_x) + A = 2 * np.arctan(rho * d_x) # rho_map = (0.33 * D * np.cos(A)/np.sin(A/2.)**2. * 2./f * 1./(1. + (rho/f)**2.) * (d_x/f) ) - rho_map = 0.5*D*(-np.sqrt(1 + rho**2*d_x**2)/(rho**2*d_x) + d_x/(np.sqrt(1 + rho**2*d_x**2))) - x_map = 0.5 * D/np.sin(0.5*A)*(d_x) - y_map = 0.5 * D/np.sin(0.5*A)*(d_y) - CovarMap = np.array([[x_map,0.,0.],[0., y_map, 0.],[0.,0., rho_map]]) - CoarIn = np.array(CovarXYR).reshape([3,3]) + rho_map = ( + 0.5 + * D + * ( + -np.sqrt(1 + rho**2 * d_x**2) / (rho**2 * d_x) + + d_x / (np.sqrt(1 + rho**2 * d_x**2)) + ) + ) + x_map = 0.5 * D / np.sin(0.5 * A) * (d_x) + y_map = 0.5 * D / np.sin(0.5 * A) * (d_y) + CovarMap = np.array([[x_map, 0.0, 0.0], [0.0, y_map, 0.0], [0.0, 0.0, rho_map]]) + CoarIn = np.array(CovarXYR).reshape([3, 3]) return np.dot(CovarMap, np.dot(CoarIn, CovarMap.T)) + def test_pixelLine_converter(): - """ Test ephemNavConverter. """ + """Test ephemNavConverter.""" [testResults, testMessage] = pixelLineConverterTestFunction() assert testResults < 1, testMessage + def pixelLineConverterTestFunction(): - """ Test the ephemNavConverter module. Setup a simulation """ + """Test the ephemNavConverter module. Setup a simulation""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -68,7 +82,9 @@ def pixelLineConverterTestFunction(): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process # Construct the ephemNavConverter module # Set the names for the input messages @@ -86,22 +102,24 @@ def pixelLineConverterTestFunction(): inputAtt = messaging.NavAttMsgPayload() # Set camera - inputCamera.fieldOfView = 2.0 * np.arctan(10*1e-3 / 2.0 / (1.*1e-3) ) # 2*arctan(s/2 / f) + inputCamera.fieldOfView = 2.0 * np.arctan( + 10 * 1e-3 / 2.0 / (1.0 * 1e-3) + ) # 2*arctan(s/2 / f) inputCamera.resolution = [512, 512] - inputCamera.sigma_CB = [1., 0.3, 0.1] + inputCamera.sigma_CB = [1.0, 0.3, 0.1] camInMsg = messaging.CameraConfigMsg().write(inputCamera) pixelLine.cameraConfigInMsg.subscribeTo(camInMsg) # Set circles inputCircles.circlesCenters = [152, 251] inputCircles.circlesRadii = [75] - inputCircles.uncertainty = [0.5, 0., 0., 0., 0.5, 0., 0., 0., 1.] + inputCircles.uncertainty = [0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0] inputCircles.timeTag = 12345 circlesInMsg = messaging.OpNavCirclesMsg().write(inputCircles) pixelLine.circlesInMsg.subscribeTo(circlesInMsg) # Set attitude - inputAtt.sigma_BN = [0.6, 1., 0.1] + inputAtt.sigma_BN = [0.6, 1.0, 0.1] attInMsg = messaging.NavAttMsg().write(inputAtt) pixelLine.attInMsg.subscribeTo(attInMsg) @@ -123,7 +141,11 @@ def pixelLineConverterTestFunction(): planet["name"] = "Mars" planet["diameter"] = 3396.19 * 2 # km - state = [inputCircles.circlesCenters[0], inputCircles.circlesCenters[1], inputCircles.circlesRadii[0]] + state = [ + inputCircles.circlesCenters[0], + inputCircles.circlesCenters[1], + inputCircles.circlesRadii[0], + ] r_Cexp = mapState(state, planet, inputCamera) covar_Cexp = mapCovar(inputCircles.uncertainty, state[2], planet, inputCamera) @@ -148,34 +170,37 @@ def pixelLineConverterTestFunction(): # # for i in range(len(outputR[-1, 1:])): - if np.abs(r_Nexp[i] - outputR[-1, i]) > 1E-10 and np.isnan(outputR.any()): + if np.abs(r_Nexp[i] - outputR[-1, i]) > 1e-10 and np.isnan(outputR.any()): testFailCount += 1 testMessages.append("FAILED: Position Check in pixelLine") for i in range(len(outputCovar[-1, 0:])): - if np.abs((covar_Nexp[i] - outputCovar[-1, i])) > 1E-10 and np.isnan(outputTime.any()): + if np.abs((covar_Nexp[i] - outputCovar[-1, i])) > 1e-10 and np.isnan( + outputTime.any() + ): testFailCount += 1 testMessages.append("FAILED: Covar Check in pixelLine") - if np.abs((timTagExp - outputTime[-1])/timTagExp) > 1E-10 and np.isnan(outputTime.any()): + if np.abs((timTagExp - outputTime[-1]) / timTagExp) > 1e-10 and np.isnan( + outputTime.any() + ): testFailCount += 1 testMessages.append("FAILED: Time Check in pixelLine") # # print out success message if no error were found snippentName = "passFail" if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + pixelLine.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + pixelLine.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] -if __name__ == '__main__': +if __name__ == "__main__": test_pixelLine_converter() diff --git a/src/fswAlgorithms/opticalNavigation/faultDetection/_UnitTest/test_faultDetection.py b/src/fswAlgorithms/opticalNavigation/faultDetection/_UnitTest/test_faultDetection.py index e64e9ed68d..d73d28ca90 100644 --- a/src/fswAlgorithms/opticalNavigation/faultDetection/_UnitTest/test_faultDetection.py +++ b/src/fswAlgorithms/opticalNavigation/faultDetection/_UnitTest/test_faultDetection.py @@ -17,22 +17,24 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -@pytest.mark.parametrize("r_c1, r_c2, valid1, valid2, faultMode", [ - ([10, 10, 1], [10, 10, 1], 1, 1, 1), # No merge, all valid, no fault - ([10, 10, 1], [10, 10, 1], 1, 0, 1), #No merge, one valid, no fault - ([10, 10, 1], [10, 10, 1], 1, 0, 2), #No merge, one valid, no fault, mode 2 - ([10, 10, 1], [10, 10, 1], 0, 1, 1), # No merge, other valid, no fault - ([10, 10, 1], [10, 10, 1], 0, 1, 2), # No merge, other valid, no fault, mode 2 - ([10, 10, 1], [10, 10, 1], 0, 1, 0), # Merge, other valid, no fault - ([10, 10, 1], [10, 10, 1], 0, 0, 1), # No merge, none valid, no fault - ([10, 10, 1], [100, 10, 1], 1, 1, 1), # No merge, all valid, fault - ([10, 10, 1], [100, 10, 1], 1, 1, 0), # merge, all valid, fault - ([10, 10, 1], [11, 9, 0.5], 1, 1, 0), # merge, all valid, no fault - ([10, 10, 1], [11, 9, 0.5], 1, 1, 2), # merge, all valid, no fault, mode 2 - ([10, 10, 1], [11, 9, 0.5], 1, 1, 0), # merge, all valid, no fault - -]) +@pytest.mark.parametrize( + "r_c1, r_c2, valid1, valid2, faultMode", + [ + ([10, 10, 1], [10, 10, 1], 1, 1, 1), # No merge, all valid, no fault + ([10, 10, 1], [10, 10, 1], 1, 0, 1), # No merge, one valid, no fault + ([10, 10, 1], [10, 10, 1], 1, 0, 2), # No merge, one valid, no fault, mode 2 + ([10, 10, 1], [10, 10, 1], 0, 1, 1), # No merge, other valid, no fault + ([10, 10, 1], [10, 10, 1], 0, 1, 2), # No merge, other valid, no fault, mode 2 + ([10, 10, 1], [10, 10, 1], 0, 1, 0), # Merge, other valid, no fault + ([10, 10, 1], [10, 10, 1], 0, 0, 1), # No merge, none valid, no fault + ([10, 10, 1], [100, 10, 1], 1, 1, 1), # No merge, all valid, fault + ([10, 10, 1], [100, 10, 1], 1, 1, 0), # merge, all valid, fault + ([10, 10, 1], [11, 9, 0.5], 1, 1, 0), # merge, all valid, no fault + ([10, 10, 1], [11, 9, 0.5], 1, 1, 2), # merge, all valid, no fault, mode 2 + ([10, 10, 1], [11, 9, 0.5], 1, 1, 0), # merge, all valid, no fault + ], +) def test_faultdetection(show_plots, r_c1, r_c2, valid1, valid2, faultMode): """ **Validation Test Description** @@ -74,11 +76,14 @@ def test_faultdetection(show_plots, r_c1, r_c2, valid1, valid2, faultMode): These are ``r_BN_N``, ``covar_N``, ``time``, ``faultDetected`` """ - [testResults, testMessage] = faultdetection(show_plots, r_c1, r_c2, valid1, valid2, faultMode) + [testResults, testMessage] = faultdetection( + show_plots, r_c1, r_c2, valid1, valid2, faultMode + ) assert testResults < 1, testMessage + def faultdetection(show_plots, r_c1, r_c2, valid1, valid2, faultMode): - """ Test the faultdetection module. Setup a simulation """ + """Test the faultdetection module. Setup a simulation""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -91,7 +96,9 @@ def faultdetection(show_plots, r_c1, r_c2, valid1, valid2, faultMode): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process # Construct the ephemNavConverter module # Set the names for the input messages @@ -113,14 +120,16 @@ def faultdetection(show_plots, r_c1, r_c2, valid1, valid2, faultMode): inputAtt = messaging.NavAttMsgPayload() # Set camera - inputCamera.fieldOfView = 2.0 * np.arctan(10*1e-3 / 2.0 / (1.*1e-3) ) # 2*arctan(s/2 / f) + inputCamera.fieldOfView = 2.0 * np.arctan( + 10 * 1e-3 / 2.0 / (1.0 * 1e-3) + ) # 2*arctan(s/2 / f) inputCamera.resolution = [512, 512] - inputCamera.sigma_CB = [1.,0.3,0.1] + inputCamera.sigma_CB = [1.0, 0.3, 0.1] camInMsg = messaging.CameraConfigMsg().write(inputCamera) faults.cameraConfigInMsg.subscribeTo(camInMsg) # Set attitude - inputAtt.sigma_BN = [0.6, 1., 0.1] + inputAtt.sigma_BN = [0.6, 1.0, 0.1] attInMsg = messaging.NavAttMsg().write(inputAtt) faults.attInMsg.subscribeTo(attInMsg) @@ -131,8 +140,18 @@ def faultdetection(show_plots, r_c1, r_c2, valid1, valid2, faultMode): inputPrimary.r_BN_C = r_c1 inputPrimary.r_BN_N = np.dot(NC, np.array(r_c1)).tolist() inputPrimary.valid = valid1 - inputPrimary.covar_C = [0.5, 0., 0., 0., 0.5, 0., 0., 0., 1.] - inputPrimary.covar_N = np.dot(np.dot(NC, np.array([0.5, 0., 0., 0., 0.5, 0., 0., 0., 1.]).reshape([3,3])), NC.T).flatten().tolist() + inputPrimary.covar_C = [0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0] + inputPrimary.covar_N = ( + np.dot( + np.dot( + NC, + np.array([0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0]).reshape([3, 3]), + ), + NC.T, + ) + .flatten() + .tolist() + ) inputPrimary.timeTag = 12345 op1InMsg = messaging.OpNavMsg().write(inputPrimary) faults.navMeasPrimaryInMsg.subscribeTo(op1InMsg) @@ -141,8 +160,18 @@ def faultdetection(show_plots, r_c1, r_c2, valid1, valid2, faultMode): inputSecondary.r_BN_C = r_c2 inputSecondary.r_BN_N = np.dot(NC, np.array(r_c2)).tolist() inputSecondary.valid = valid2 - inputSecondary.covar_C = [0.5, 0., 0., 0., 0.5, 0., 0., 0., 1.] - inputSecondary.covar_N = np.dot(np.dot(NC, np.array([0.5, 0., 0., 0., 0.5, 0., 0., 0., 1.]).reshape([3,3])), NC.T).flatten().tolist() + inputSecondary.covar_C = [0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0] + inputSecondary.covar_N = ( + np.dot( + np.dot( + NC, + np.array([0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0]).reshape([3, 3]), + ), + NC.T, + ) + .flatten() + .tolist() + ) inputSecondary.timeTag = 12345 op2InMsg = messaging.OpNavMsg().write(inputSecondary) faults.navMeasSecondaryInMsg.subscribeTo(op2InMsg) @@ -157,47 +186,53 @@ def faultdetection(show_plots, r_c1, r_c2, valid1, valid2, faultMode): unitTestSim.ExecuteSimulation() # Truth Vlaues - faultDetectedTrue= 0 + faultDetectedTrue = 0 timTagExp = inputPrimary.timeTag - if valid1 ==0 and valid2==0: + if valid1 == 0 and valid2 == 0: timTagExp = 0 - r_Cexp = [0,0,0] - covar_Cexp = [0,0,0,0,0,0,0,0,0] - if valid1 == 0 and valid2 ==1: + r_Cexp = [0, 0, 0] + covar_Cexp = [0, 0, 0, 0, 0, 0, 0, 0, 0] + if valid1 == 0 and valid2 == 1: if faultMode > 0: timTagExp = 0 r_Cexp = [0, 0, 0] covar_Cexp = [0, 0, 0, 0, 0, 0, 0, 0, 0] if faultMode == 0: r_Cexp = r_c2 - covar_Cexp = [0.5, 0., 0., 0., 0.5, 0., 0., 0., 1.] - if valid1 == 1 and valid2 ==0: + covar_Cexp = [0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0] + if valid1 == 1 and valid2 == 0: if faultMode < 2: r_Cexp = r_c1 - covar_Cexp = [0.5, 0., 0., 0., 0.5, 0., 0., 0., 1.] + covar_Cexp = [0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0] if faultMode == 2: timTagExp = 0 r_Cexp = [0, 0, 0] covar_Cexp = [0, 0, 0, 0, 0, 0, 0, 0, 0] - if valid1 == 1 and valid2 ==1: + if valid1 == 1 and valid2 == 1: r1 = np.array(r_c1) r2 = np.array(r_c2) faultNorm = np.linalg.norm(r2 - r1) - covarz = np.array([0.5, 0., 0., 0., 0.5, 0., 0., 0., 1.]).reshape([3,3]) - z1 = covarz[:,2] + covarz = np.array([0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0]).reshape([3, 3]) + z1 = covarz[:, 2] z2 = np.copy(z1) - if (faultNorm > faults.sigmaFault*(np.linalg.norm(z1)+np.linalg.norm(z2))): + if faultNorm > faults.sigmaFault * (np.linalg.norm(z1) + np.linalg.norm(z2)): faultDetectedTrue = 1 r_Cexp = r_c2 - covar_Cexp = [0.5, 0., 0., 0., 0.5, 0., 0., 0., 1.] + covar_Cexp = [0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0] elif faultMode > 0: r_Cexp = r_c1 - covar_Cexp = [0.5, 0., 0., 0., 0.5, 0., 0., 0., 1.] + covar_Cexp = [0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0] elif faultMode == 0: - covar_Cexp = np.linalg.inv(np.linalg.inv(covarz) + np.linalg.inv(covarz)).flatten().tolist() - r_Cexp = np.dot(np.linalg.inv(np.linalg.inv(covarz) + np.linalg.inv(covarz)), np.dot(np.linalg.inv(covarz), r1) + np.dot(np.linalg.inv(covarz), r2)).tolist() - + covar_Cexp = ( + np.linalg.inv(np.linalg.inv(covarz) + np.linalg.inv(covarz)) + .flatten() + .tolist() + ) + r_Cexp = np.dot( + np.linalg.inv(np.linalg.inv(covarz) + np.linalg.inv(covarz)), + np.dot(np.linalg.inv(covarz), r1) + np.dot(np.linalg.inv(covarz), r2), + ).tolist() posErr = 1e-10 print(posErr) @@ -210,34 +245,35 @@ def faultdetection(show_plots, r_c1, r_c2, valid1, valid2, faultMode): # # for i in range(len(outputR[-1, 1:])): - if np.abs(r_Cexp[i] - outputR[-1, i]) > 1E-10 or np.isnan(outputR.any()): + if np.abs(r_Cexp[i] - outputR[-1, i]) > 1e-10 or np.isnan(outputR.any()): testFailCount += 1 testMessages.append("FAILED: Position Check in pixelLine") for i in range(len(outputCovar[-1, 0:])): - if np.abs((covar_Cexp[i] - outputCovar[-1, i])) > 1E-10 or np.isnan(outputTime.any()): + if np.abs((covar_Cexp[i] - outputCovar[-1, i])) > 1e-10 or np.isnan( + outputTime.any() + ): testFailCount += 1 testMessages.append("FAILED: Covar Check in pixelLine") - if np.abs((timTagExp - outputTime[-1])) > 1E-10 or np.isnan(outputTime.any()): + if np.abs((timTagExp - outputTime[-1])) > 1e-10 or np.isnan(outputTime.any()): testFailCount += 1 testMessages.append("FAILED: Time Check in pixelLine") - if np.abs(faultDetectedTrue - detected[-1]) > 1E-10 or np.isnan(outputTime.any()): + if np.abs(faultDetectedTrue - detected[-1]) > 1e-10 or np.isnan(outputTime.any()): testFailCount += 1 testMessages.append("FAILED: Time Check in pixelLine") # # print out success message if no error were found if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + faults.ModelTag) else: - colorText = 'Red' + colorText = "Red" print("Failed: " + faults.ModelTag) - - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] -if __name__ == '__main__': - faultdetection(False ,[10, 10, 1], [11, 9, 0.5], 1, 1, 1) +if __name__ == "__main__": + faultdetection(False, [10, 10, 1], [11, 9, 0.5], 1, 1, 1) diff --git a/src/fswAlgorithms/opticalNavigation/pixelLineBiasUKF/_UnitTest/pixelLineBias_test_utilities.py b/src/fswAlgorithms/opticalNavigation/pixelLineBiasUKF/_UnitTest/pixelLineBias_test_utilities.py index aaaa87fb91..52bee026da 100644 --- a/src/fswAlgorithms/opticalNavigation/pixelLineBiasUKF/_UnitTest/pixelLineBias_test_utilities.py +++ b/src/fswAlgorithms/opticalNavigation/pixelLineBiasUKF/_UnitTest/pixelLineBias_test_utilities.py @@ -23,202 +23,235 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('fswAlgorithms') +splitPath = path.split("fswAlgorithms") import matplotlib.pyplot as plt -color_x = 'dodgerblue' -color_y = 'salmon' -color_z = 'lightgreen' +color_x = "dodgerblue" +color_y = "salmon" +color_z = "lightgreen" m2km = 1.0 / 1000.0 -def StatePlot(x, testName, show_plots): - numStates = len(x[0,:])-1 +def StatePlot(x, testName, show_plots): + numStates = len(x[0, :]) - 1 - t= np.zeros(len(x[:,0])) + t = np.zeros(len(x[:, 0])) for i in range(len(t)): - t[i] = x[i, 0]*1E-9 + t[i] = x[i, 0] * 1e-9 - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t , x[:, 1], "b", label='Error Filter') - plt.legend(loc='lower right') - plt.title('First pos component (m)') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.legend(loc="lower right") + plt.title("First pos component (m)") plt.grid() plt.subplot(322) - plt.plot(t , x[:, 4], "b") - plt.title('Second rate component (m/s)') + plt.plot(t, x[:, 4], "b") + plt.title("Second rate component (m/s)") plt.grid() plt.subplot(323) - plt.plot(t , x[:, 2], "b") - plt.title('Second pos component (m)') + plt.plot(t, x[:, 2], "b") + plt.title("Second pos component (m)") plt.grid() plt.subplot(324) - plt.plot(t , x[:, 5], "b") - plt.xlabel('t(s)') - plt.title('Third rate component (m/s)') + plt.plot(t, x[:, 5], "b") + plt.xlabel("t(s)") + plt.title("Third rate component (m/s)") plt.grid() plt.subplot(325) - plt.plot(t , x[:, 3], "b") - plt.xlabel('t(s)') - plt.title('Third pos component (m)') + plt.plot(t, x[:, 3], "b") + plt.xlabel("t(s)") + plt.title("Third pos component (m)") plt.grid() plt.subplot(326) - plt.plot(t , x[:, 6], "b") - plt.xlabel('t(s)') - plt.title('Third rate component (m/s)') + plt.plot(t, x[:, 6], "b") + plt.xlabel("t(s)") + plt.title("Third rate component (m/s)") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesPlot' + testName, 'State error', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesPlot" + testName, + "State error", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() -def EnergyPlot(t, energy, testName, show_plots): - conserved= np.zeros(len(t)) +def EnergyPlot(t, energy, testName, show_plots): + conserved = np.zeros(len(t)) for i in range(len(t)): - conserved[i] = (energy[i] - energy[0])/energy[0] + conserved[i] = (energy[i] - energy[0]) / energy[0] - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') - plt.plot(t , conserved, "b", label='Energy') - plt.legend(loc='lower right') - plt.title('Energy ' + testName) + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") + plt.plot(t, conserved, "b", label="Energy") + plt.legend(loc="lower right") + plt.title("Energy " + testName) plt.grid() - - unitTestSupport.writeFigureLaTeX('Energy' + testName, 'Orbital Energy', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "Energy" + testName, + "Orbital Energy", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() def StateCovarPlot(x, Pflat, testName, show_plots): + numStates = len(x[0, :]) - 1 - numStates = len(x[0,:])-1 - - P = np.zeros([len(Pflat[:,0]),numStates,numStates]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*1E-9 - P[i,:,:] = Pflat[i,1:(numStates*numStates+1)].reshape([numStates,numStates]) + P = np.zeros([len(Pflat[:, 0]), numStates, numStates]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * 1e-9 + P[i, :, :] = Pflat[i, 1 : (numStates * numStates + 1)].reshape( + [numStates, numStates] + ) - - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t , x[:, 1], "b", label='Error Filter') - plt.plot(t , x[:, 1]+3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t , x[:, 1]-3 * np.sqrt(P[:, 0, 0]), 'r--') - plt.legend(loc='lower right') - plt.title('First pos component (m)') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.plot(t, x[:, 1] + 3 * np.sqrt(P[:, 0, 0]), "r--", label="Covar Filter") + plt.plot(t, x[:, 1] - 3 * np.sqrt(P[:, 0, 0]), "r--") + plt.legend(loc="lower right") + plt.title("First pos component (m)") plt.grid() plt.subplot(322) - plt.plot(t , x[:, 4], "b") - plt.plot(t , x[:, 4]+3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.plot(t , x[:, 4]-3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.title('Second rate component (m/s)') + plt.plot(t, x[:, 4], "b") + plt.plot(t, x[:, 4] + 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.plot(t, x[:, 4] - 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.title("Second rate component (m/s)") plt.grid() plt.subplot(323) - plt.plot(t , x[:, 2], "b") - plt.plot(t , x[:, 2]+3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.plot(t , x[:, 2]-3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.title('Second pos component (m)') + plt.plot(t, x[:, 2], "b") + plt.plot(t, x[:, 2] + 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.plot(t, x[:, 2] - 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.title("Second pos component (m)") plt.grid() plt.subplot(324) - plt.plot(t , x[:, 5], "b") - plt.plot(t , x[:, 5]+3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.plot(t , x[:, 5]-3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component (m/s)') + plt.plot(t, x[:, 5], "b") + plt.plot(t, x[:, 5] + 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.plot(t, x[:, 5] - 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.xlabel("t(s)") + plt.title("Third rate component (m/s)") plt.grid() plt.subplot(325) - plt.plot(t , x[:, 3], "b") - plt.plot(t , x[:, 3]+3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.plot(t , x[:, 3]-3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.xlabel('t(s)') - plt.title('Third pos component (m)') + plt.plot(t, x[:, 3], "b") + plt.plot(t, x[:, 3] + 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.plot(t, x[:, 3] - 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.xlabel("t(s)") + plt.title("Third pos component (m)") plt.grid() plt.subplot(326) - plt.plot(t , x[:, 6], "b") - plt.plot(t , x[:, 6]+3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.plot(t , x[:, 6]-3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component (m/s)') + plt.plot(t, x[:, 6], "b") + plt.plot(t, x[:, 6] + 3 * np.sqrt(P[:, 5, 5]), "r--") + plt.plot(t, x[:, 6] - 3 * np.sqrt(P[:, 5, 5]), "r--") + plt.xlabel("t(s)") + plt.title("Third rate component (m/s)") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesPlot' + testName, 'State error and covariance', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesPlot" + testName, + "State error and covariance", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() - def PostFitResiduals(Res, noise, testName, show_plots): - - MeasNoise = np.zeros(len(Res[:,0])) - t= np.zeros(len(Res[:,0])) - for i in range(len(Res[:,0])): - t[i] = Res[i, 0]*1E-9 - MeasNoise[i] = 3*noise + MeasNoise = np.zeros(len(Res[:, 0])) + t = np.zeros(len(Res[:, 0])) + for i in range(len(Res[:, 0])): + t[i] = Res[i, 0] * 1e-9 + MeasNoise[i] = 3 * noise # Don't plot zero values, since they mean that no measurement is taken - for j in range(len(Res[0,:])-1): - if -1E-10 < Res[i,j+1] < 1E-10: - Res[i, j+1] = np.nan + for j in range(len(Res[0, :]) - 1): + if -1e-10 < Res[i, j + 1] < 1e-10: + Res[i, j + 1] = np.nan - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(t , Res[:, 1], "b.", label='Residual') - plt.plot(t , MeasNoise, 'r--', label='Covar') - plt.plot(t , -MeasNoise, 'r--') - plt.legend(loc='lower right') - plt.ylim([-10*noise, 10*noise]) - plt.title('First Meas Comp (m)') + plt.plot(t, Res[:, 1], "b.", label="Residual") + plt.plot(t, MeasNoise, "r--", label="Covar") + plt.plot(t, -MeasNoise, "r--") + plt.legend(loc="lower right") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("First Meas Comp (m)") plt.grid() plt.subplot(312) - plt.plot(t , Res[:, 2], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Second Meas Comp (m)') + plt.plot(t, Res[:, 2], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Second Meas Comp (m)") plt.grid() plt.subplot(313) - plt.plot(t , Res[:, 3], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Third Meas Comp (m)') + plt.plot(t, Res[:, 3], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Third Meas Comp (m)") plt.grid() - - unitTestSupport.writeFigureLaTeX('PostFit' + testName, 'Post Fit Residuals', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "PostFit" + testName, + "Post Fit Residuals", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() + def plot_TwoOrbits(r_BN, r_BN2): fig = plt.figure() - ax = fig.add_subplot(projection='3d') - ax.set_xlabel('$R_x$, km') - ax.set_ylabel('$R_y$, km') - ax.set_zlabel('$R_z$, km') - ax.plot(r_BN[:, 1] * m2km, r_BN[:, 2] * m2km, r_BN[:, 3] * m2km, color_x, label="True orbit") - for i in range(len(r_BN2[:,0])): - if np.abs(r_BN2[i, 1])>0 or np.abs(r_BN2[i, 2])>0: - ax.scatter(r_BN2[i, 1] * m2km, r_BN2[i, 2] * m2km, r_BN2[i, 3] * m2km, color=color_y, label="Meas orbit") - ax.scatter(0, 0, color='r') - ax.set_title('Spacecraft Orbits') + ax = fig.add_subplot(projection="3d") + ax.set_xlabel("$R_x$, km") + ax.set_ylabel("$R_y$, km") + ax.set_zlabel("$R_z$, km") + ax.plot( + r_BN[:, 1] * m2km, + r_BN[:, 2] * m2km, + r_BN[:, 3] * m2km, + color_x, + label="True orbit", + ) + for i in range(len(r_BN2[:, 0])): + if np.abs(r_BN2[i, 1]) > 0 or np.abs(r_BN2[i, 2]) > 0: + ax.scatter( + r_BN2[i, 1] * m2km, + r_BN2[i, 2] * m2km, + r_BN2[i, 3] * m2km, + color=color_y, + label="Meas orbit", + ) + ax.scatter(0, 0, color="r") + ax.set_title("Spacecraft Orbits") return diff --git a/src/fswAlgorithms/opticalNavigation/pixelLineBiasUKF/_UnitTest/test_pixelLineBiasUKF.py b/src/fswAlgorithms/opticalNavigation/pixelLineBiasUKF/_UnitTest/test_pixelLineBiasUKF.py index a1be75dd8d..6175151631 100644 --- a/src/fswAlgorithms/opticalNavigation/pixelLineBiasUKF/_UnitTest/test_pixelLineBiasUKF.py +++ b/src/fswAlgorithms/opticalNavigation/pixelLineBiasUKF/_UnitTest/test_pixelLineBiasUKF.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016-2018, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -18,7 +17,9 @@ import numpy as np from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import pixelLineBiasUKF # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + pixelLineBiasUKF, +) # import the module that is to be tested from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.utilities import SimulationBaseClass, macros, orbitalMotion @@ -28,64 +29,140 @@ def addTimeColumn(time, data): return np.transpose(np.vstack([[time], np.transpose(data)])) + def rk4(f, t, x0): - x = np.zeros([len(t),len(x0)+1]) - h = (t[len(t)-1] - t[0])/len(t) - x[0,0] = t[0] - x[0,1:] = x0 - for i in range(len(t)-1): - h = t[i+1] - t[i] - x[i,0] = t[i] - k1 = h * f(t[i], x[i,1:]) - k2 = h * f(t[i] + 0.5 * h, x[i,1:] + 0.5 * k1) - k3 = h * f(t[i] + 0.5 * h, x[i,1:] + 0.5 * k2) - k4 = h * f(t[i] + h, x[i,1:] + k3) - x[i+1,1:] = x[i,1:] + (k1 + 2.*k2 + 2.*k3 + k4) / 6. - x[i+1,0] = t[i+1] + x = np.zeros([len(t), len(x0) + 1]) + h = (t[len(t) - 1] - t[0]) / len(t) + x[0, 0] = t[0] + x[0, 1:] = x0 + for i in range(len(t) - 1): + h = t[i + 1] - t[i] + x[i, 0] = t[i] + k1 = h * f(t[i], x[i, 1:]) + k2 = h * f(t[i] + 0.5 * h, x[i, 1:] + 0.5 * k1) + k3 = h * f(t[i] + 0.5 * h, x[i, 1:] + 0.5 * k2) + k4 = h * f(t[i] + h, x[i, 1:] + k3) + x[i + 1, 1:] = x[i, 1:] + (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0 + x[i + 1, 0] = t[i + 1] return x -def twoBodyGrav(t, x, mu = 42828.314*1E9): + +def twoBodyGrav(t, x, mu=42828.314 * 1e9): dxdt = np.zeros(np.shape(x)) dxdt[0:3] = x[3:6] - dxdt[3:6] = -mu/np.linalg.norm(x[0:3])**3.*x[0:3] + dxdt[3:6] = -mu / np.linalg.norm(x[0:3]) ** 3.0 * x[0:3] return dxdt def setupFilterData(filterObject): - filterObject.planetIdInit = 2 filterObject.alpha = 0.02 filterObject.beta = 2.0 filterObject.kappa = 0.0 filterObject.gamma = 0.9 - mu = 42828.314*1E9 #m^3/s^2 + mu = 42828.314 * 1e9 # m^3/s^2 elementsInit = orbitalMotion.ClassicElements() - elementsInit.a = 8000*1E3 #m + elementsInit.a = 8000 * 1e3 # m elementsInit.e = 0.2 elementsInit.i = 10 elementsInit.Omega = 0.001 elementsInit.omega = 0.01 elementsInit.f = 0.1 r, v = orbitalMotion.elem2rv(mu, elementsInit) - bias = [1,1,-2] + bias = [1, 1, -2] filterObject.stateInit = r.tolist() + v.tolist() + bias - filterObject.covarInit = [1000.*1E6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1000.*1E6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1000.*1E6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 5*1E6, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 5*1E6, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 5*1E6, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0,] + filterObject.covarInit = [ + 1000.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1000.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1000.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 5 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 5 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 5 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 3.0, + ] qNoiseIn = np.identity(9) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3]*0.00001*0.00001*1E-6 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6]*0.0001*0.0001*1E-6 - qNoiseIn[6:9, 6:9] = qNoiseIn[3:6, 3:6]*0.0001*0.0001 - filterObject.qNoise = qNoiseIn.reshape(9*9).tolist() + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 0.00001 * 0.00001 * 1e-6 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 0.0001 * 0.0001 * 1e-6 + qNoiseIn[6:9, 6:9] = qNoiseIn[3:6, 3:6] * 0.0001 * 0.0001 + filterObject.qNoise = qNoiseIn.reshape(9 * 9).tolist() + # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed # @pytest.mark.skipif(conditionstring) @@ -93,10 +170,13 @@ def setupFilterData(filterObject): # @pytest.mark.xfail() # need to update how the RW states are defined # provide a unique test method name, starting with test_ + def test_methods_kf(show_plots): """Module Unit Test""" [testResults, testMessage] = relOD_method_test(show_plots) assert testResults < 1, testMessage + + def test_propagation_kf(show_plots): """Module Unit Test""" [testResults, testMessage] = StatePropRelOD(show_plots, 10.0) @@ -113,7 +193,7 @@ def relOD_method_test(show_plots): testMessages = [] # create empty list to store test log messages state = [250, 32000, 1000, 5, 3, 2, 1, 1, 1] - covar = 10* np.eye(len(state)) + covar = 10 * np.eye(len(state)) dt = 10 mu = 42828.314 # Measurement Model Test @@ -139,9 +219,12 @@ def relOD_method_test(show_plots): propedState = [] for i in range(len(state)): propedState.append(pixelLineBiasUKF.doubleArray_getitem(stateIn, i)) - expected = rk4(twoBodyGrav, [0, dt], np.array(state)*1E3) - expected[:,1:]*=1E-3 - if np.linalg.norm((np.array(propedState) - expected[-1,1:])/(expected[-1,1:])) > 1.0E-15: + expected = rk4(twoBodyGrav, [0, dt], np.array(state) * 1e3) + expected[:, 1:] *= 1e-3 + if ( + np.linalg.norm((np.array(propedState) - expected[-1, 1:]) / (expected[-1, 1:])) + > 1.0e-15 + ): testFailCount += 1 testMessages.append("State Prop Failure") @@ -159,70 +242,77 @@ def relOD_method_test(show_plots): # Set up attitud message att = messaging.NavAttMsgPayload() - att.sigma_BN = [0, 0.2,-0.1] - att.omega_BN_B = [0.,0.,0.] + att.sigma_BN = [0, 0.2, -0.1] + att.omega_BN_B = [0.0, 0.0, 0.0] data.attInfo = att # Set up a camera message cam = messaging.CameraConfigMsgPayload() - cam.sigma_CB = [-0.2, 0., 0.3] - cam.fieldOfView = 2.0 * np.arctan(10*1e-3 / 2.0 / (1.*1e-3) ) # 2*arctan(s/2 / f) + cam.sigma_CB = [-0.2, 0.0, 0.3] + cam.fieldOfView = 2.0 * np.arctan( + 10 * 1e-3 / 2.0 / (1.0 * 1e-3) + ) # 2*arctan(s/2 / f) cam.resolution = [512, 512] data.cameraSpecs = cam # Populate sigma points - SP = np.zeros([len(state), 2*len(state) +1]) - for i in range(2*len(state) + 1): - if i ==0: + SP = np.zeros([len(state), 2 * len(state) + 1]) + for i in range(2 * len(state) + 1): + if i == 0: SP[:, i] = np.array(state) - if i < len(state) + 1 and i>0: - SP[:,i] = np.array(state) + covar[:,i-1] + if i < len(state) + 1 and i > 0: + SP[:, i] = np.array(state) + covar[:, i - 1] if i > len(state): - SP[:,i] = np.array(state) - covar[:,i-(len(state)+1)] + SP[:, i] = np.array(state) - covar[:, i - (len(state) + 1)] data.SP = np.transpose(SP).flatten().tolist() data.state = state pixelLineBiasUKF.pixelLineBiasUKFMeasModel(data) yMeasOut = data.yMeas - expectedMeas = np.zeros([6, 2*len(state)+1]) + expectedMeas = np.zeros([6, 2 * len(state) + 1]) dcm_CB = rbk.MRP2C(cam.sigma_CB) dcm_BN = rbk.MRP2C(att.sigma_BN) dcm_CN = np.dot(dcm_CB, dcm_BN) - pX = 2. * np.tan(cam.fieldOfView * cam.resolution[0] / cam.resolution[1] / 2.0) - pY = 2. * np.tan(cam.fieldOfView / 2.0) + pX = 2.0 * np.tan(cam.fieldOfView * cam.resolution[0] / cam.resolution[1] / 2.0) + pY = 2.0 * np.tan(cam.fieldOfView / 2.0) X = pX / cam.resolution[0] Y = pY / cam.resolution[1] planetRad = 3396.19 - obs = np.array([msg.circlesCenters[0], msg.circlesCenters[1], msg.circlesRadii[0], 0, 0, 0]) - for i in range(2*len(state)+1): - r_C = np.dot(dcm_CN, SP[0:3,i]) - rNorm = np.linalg.norm(SP[0:3,i]) - r_C = -1./r_C[2]*r_C + obs = np.array( + [msg.circlesCenters[0], msg.circlesCenters[1], msg.circlesRadii[0], 0, 0, 0] + ) + for i in range(2 * len(state) + 1): + r_C = np.dot(dcm_CN, SP[0:3, i]) + rNorm = np.linalg.norm(SP[0:3, i]) + r_C = -1.0 / r_C[2] * r_C centerX = r_C[0] / X centerY = r_C[1] / Y - centerX += cam.resolution[0]/2 - 0.5 + centerX += cam.resolution[0] / 2 - 0.5 centerY += cam.resolution[1] / 2 - 0.5 - rad = 1.0/X*np.tan(np.arcsin(planetRad/rNorm)) + rad = 1.0 / X * np.tan(np.arcsin(planetRad / rNorm)) if i == 0: obs[3:5] = np.array(msg.circlesCenters[0:2]) - obs[0:2] obs[5] = rad - obs[2] for j in range(3): - obs[3+j] = round(obs[3+j]) - expectedMeas[0,i] = centerX - SP[6, i] - expectedMeas[1,i] = centerY - SP[7, i] + obs[3 + j] = round(obs[3 + j]) + expectedMeas[0, i] = centerX - SP[6, i] + expectedMeas[1, i] = centerY - SP[7, i] expectedMeas[2, i] = rad - SP[8, i] expectedMeas[3:, i] = SP[6:, i] - yMeasTest = np.zeros([6, 2*len(state)+1]) - for i in range(2*len(state)+1): - yMeasTest[:,i] = yMeasOut[i*6:i*6+6] - if np.linalg.norm((yMeasTest - expectedMeas))/np.linalg.norm(expectedMeas[:,0]) > 1.0E-15: + yMeasTest = np.zeros([6, 2 * len(state) + 1]) + for i in range(2 * len(state) + 1): + yMeasTest[:, i] = yMeasOut[i * 6 : i * 6 + 6] + if ( + np.linalg.norm((yMeasTest - expectedMeas)) / np.linalg.norm(expectedMeas[:, 0]) + > 1.0e-15 + ): testFailCount += 1 testMessages.append("State Prop Failure") @@ -231,8 +321,7 @@ def relOD_method_test(show_plots): else: print(testMessages) - return [testFailCount, ''.join(testMessages)] - + return [testFailCount, "".join(testMessages)] def StatePropRelOD(show_plots, dt): @@ -269,14 +358,16 @@ def StatePropRelOD(show_plots, dt): inputAtt = messaging.NavAttMsgPayload() # Set camera - inputCamera.fieldOfView = 2.0 * np.arctan(10*1e-3 / 2.0 / 0.01) # 2*arctan(s/2 / f) + inputCamera.fieldOfView = 2.0 * np.arctan( + 10 * 1e-3 / 2.0 / 0.01 + ) # 2*arctan(s/2 / f) inputCamera.resolution = [512, 512] - inputCamera.sigma_CB = [1., 0.3, 0.1] + inputCamera.sigma_CB = [1.0, 0.3, 0.1] camInMsg = messaging.CameraConfigMsg().write(inputCamera) module.cameraConfigInMsg.subscribeTo(camInMsg) # Set attitude - inputAtt.sigma_BN = [0.6, 1., 0.1] + inputAtt.sigma_BN = [0.6, 1.0, 0.1] attInMsg = messaging.NavAttMsg().write(inputAtt) module.attInMsg.subscribeTo(attInMsg) @@ -291,32 +382,36 @@ def StatePropRelOD(show_plots, dt): unitTestSim.ConfigureStopTime(macros.min2nano(timeSim)) unitTestSim.ExecuteSimulation() - time = np.linspace(0, timeSim*60, (int) (timeSim*60/dt+1)) + time = np.linspace(0, timeSim * 60, (int)(timeSim * 60 / dt + 1)) dydt = np.zeros(len(module.stateInit)) energy = np.zeros(len(time)) - expected=np.zeros([len(time), len(module.stateInit)+1]) - expected[0,1:] = module.stateInit - mu = 42828.314*1E9 - energy[0] = -mu/(2*orbitalMotion.rv2elem(mu, expected[0,1:4], expected[0,4:7]).a) + expected = np.zeros([len(time), len(module.stateInit) + 1]) + expected[0, 1:] = module.stateInit + mu = 42828.314 * 1e9 + energy[0] = -mu / ( + 2 * orbitalMotion.rv2elem(mu, expected[0, 1:4], expected[0, 4:7]).a + ) expected = rk4(twoBodyGrav, time, module.stateInit) for i in range(1, len(time)): - energy[i] = - mu / (2 * orbitalMotion.rv2elem(mu, expected[i, 1:4], expected[i, 4:7]).a) + energy[i] = -mu / ( + 2 * orbitalMotion.rv2elem(mu, expected[i, 1:4], expected[i, 4:7]).a + ) stateLog = addTimeColumn(dataLog.times(), dataLog.state) covarLog = addTimeColumn(dataLog.times(), dataLog.covar) diff = np.copy(stateLog) - diff[:,1:]-=expected[:,1:] - FilterPlots.plot_TwoOrbits(expected[:,0:4], stateLog[:,0:4]) - FilterPlots.EnergyPlot(time, energy, 'Prop', show_plots) - FilterPlots.StateCovarPlot(stateLog, covarLog, 'Prop', show_plots) - FilterPlots.StatePlot(diff, 'Prop', show_plots) + diff[:, 1:] -= expected[:, 1:] + FilterPlots.plot_TwoOrbits(expected[:, 0:4], stateLog[:, 0:4]) + FilterPlots.EnergyPlot(time, energy, "Prop", show_plots) + FilterPlots.StateCovarPlot(stateLog, covarLog, "Prop", show_plots) + FilterPlots.StatePlot(diff, "Prop", show_plots) - if (np.linalg.norm(diff[-1,1:]/expected[-1,1:]) > 1.0E-10): + if np.linalg.norm(diff[-1, 1:] / expected[-1, 1:]) > 1.0e-10: testFailCount += 1 testMessages.append("State propagation failure") - if (energy[0] - energy[-1])/energy[0] > 1.0E-10: + if (energy[0] - energy[-1]) / energy[0] > 1.0e-10: testFailCount += 1 testMessages.append("State propagation failure") @@ -328,7 +423,7 @@ def StatePropRelOD(show_plots, dt): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": diff --git a/src/fswAlgorithms/opticalNavigation/relativeODuKF/_UnitTest/relativeODuKF_test_utilities.py b/src/fswAlgorithms/opticalNavigation/relativeODuKF/_UnitTest/relativeODuKF_test_utilities.py index aaaa87fb91..52bee026da 100644 --- a/src/fswAlgorithms/opticalNavigation/relativeODuKF/_UnitTest/relativeODuKF_test_utilities.py +++ b/src/fswAlgorithms/opticalNavigation/relativeODuKF/_UnitTest/relativeODuKF_test_utilities.py @@ -23,202 +23,235 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('fswAlgorithms') +splitPath = path.split("fswAlgorithms") import matplotlib.pyplot as plt -color_x = 'dodgerblue' -color_y = 'salmon' -color_z = 'lightgreen' +color_x = "dodgerblue" +color_y = "salmon" +color_z = "lightgreen" m2km = 1.0 / 1000.0 -def StatePlot(x, testName, show_plots): - numStates = len(x[0,:])-1 +def StatePlot(x, testName, show_plots): + numStates = len(x[0, :]) - 1 - t= np.zeros(len(x[:,0])) + t = np.zeros(len(x[:, 0])) for i in range(len(t)): - t[i] = x[i, 0]*1E-9 + t[i] = x[i, 0] * 1e-9 - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t , x[:, 1], "b", label='Error Filter') - plt.legend(loc='lower right') - plt.title('First pos component (m)') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.legend(loc="lower right") + plt.title("First pos component (m)") plt.grid() plt.subplot(322) - plt.plot(t , x[:, 4], "b") - plt.title('Second rate component (m/s)') + plt.plot(t, x[:, 4], "b") + plt.title("Second rate component (m/s)") plt.grid() plt.subplot(323) - plt.plot(t , x[:, 2], "b") - plt.title('Second pos component (m)') + plt.plot(t, x[:, 2], "b") + plt.title("Second pos component (m)") plt.grid() plt.subplot(324) - plt.plot(t , x[:, 5], "b") - plt.xlabel('t(s)') - plt.title('Third rate component (m/s)') + plt.plot(t, x[:, 5], "b") + plt.xlabel("t(s)") + plt.title("Third rate component (m/s)") plt.grid() plt.subplot(325) - plt.plot(t , x[:, 3], "b") - plt.xlabel('t(s)') - plt.title('Third pos component (m)') + plt.plot(t, x[:, 3], "b") + plt.xlabel("t(s)") + plt.title("Third pos component (m)") plt.grid() plt.subplot(326) - plt.plot(t , x[:, 6], "b") - plt.xlabel('t(s)') - plt.title('Third rate component (m/s)') + plt.plot(t, x[:, 6], "b") + plt.xlabel("t(s)") + plt.title("Third rate component (m/s)") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesPlot' + testName, 'State error', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesPlot" + testName, + "State error", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() -def EnergyPlot(t, energy, testName, show_plots): - conserved= np.zeros(len(t)) +def EnergyPlot(t, energy, testName, show_plots): + conserved = np.zeros(len(t)) for i in range(len(t)): - conserved[i] = (energy[i] - energy[0])/energy[0] + conserved[i] = (energy[i] - energy[0]) / energy[0] - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') - plt.plot(t , conserved, "b", label='Energy') - plt.legend(loc='lower right') - plt.title('Energy ' + testName) + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") + plt.plot(t, conserved, "b", label="Energy") + plt.legend(loc="lower right") + plt.title("Energy " + testName) plt.grid() - - unitTestSupport.writeFigureLaTeX('Energy' + testName, 'Orbital Energy', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "Energy" + testName, + "Orbital Energy", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() def StateCovarPlot(x, Pflat, testName, show_plots): + numStates = len(x[0, :]) - 1 - numStates = len(x[0,:])-1 - - P = np.zeros([len(Pflat[:,0]),numStates,numStates]) - t= np.zeros(len(Pflat[:,0])) - for i in range(len(Pflat[:,0])): - t[i] = x[i, 0]*1E-9 - P[i,:,:] = Pflat[i,1:(numStates*numStates+1)].reshape([numStates,numStates]) + P = np.zeros([len(Pflat[:, 0]), numStates, numStates]) + t = np.zeros(len(Pflat[:, 0])) + for i in range(len(Pflat[:, 0])): + t[i] = x[i, 0] * 1e-9 + P[i, :, :] = Pflat[i, 1 : (numStates * numStates + 1)].reshape( + [numStates, numStates] + ) - - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(321) - plt.plot(t , x[:, 1], "b", label='Error Filter') - plt.plot(t , x[:, 1]+3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') - plt.plot(t , x[:, 1]-3 * np.sqrt(P[:, 0, 0]), 'r--') - plt.legend(loc='lower right') - plt.title('First pos component (m)') + plt.plot(t, x[:, 1], "b", label="Error Filter") + plt.plot(t, x[:, 1] + 3 * np.sqrt(P[:, 0, 0]), "r--", label="Covar Filter") + plt.plot(t, x[:, 1] - 3 * np.sqrt(P[:, 0, 0]), "r--") + plt.legend(loc="lower right") + plt.title("First pos component (m)") plt.grid() plt.subplot(322) - plt.plot(t , x[:, 4], "b") - plt.plot(t , x[:, 4]+3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.plot(t , x[:, 4]-3 * np.sqrt(P[:, 3, 3]), 'r--') - plt.title('Second rate component (m/s)') + plt.plot(t, x[:, 4], "b") + plt.plot(t, x[:, 4] + 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.plot(t, x[:, 4] - 3 * np.sqrt(P[:, 3, 3]), "r--") + plt.title("Second rate component (m/s)") plt.grid() plt.subplot(323) - plt.plot(t , x[:, 2], "b") - plt.plot(t , x[:, 2]+3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.plot(t , x[:, 2]-3 * np.sqrt(P[:, 1, 1]), 'r--') - plt.title('Second pos component (m)') + plt.plot(t, x[:, 2], "b") + plt.plot(t, x[:, 2] + 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.plot(t, x[:, 2] - 3 * np.sqrt(P[:, 1, 1]), "r--") + plt.title("Second pos component (m)") plt.grid() plt.subplot(324) - plt.plot(t , x[:, 5], "b") - plt.plot(t , x[:, 5]+3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.plot(t , x[:, 5]-3 * np.sqrt(P[:, 4, 4]), 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component (m/s)') + plt.plot(t, x[:, 5], "b") + plt.plot(t, x[:, 5] + 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.plot(t, x[:, 5] - 3 * np.sqrt(P[:, 4, 4]), "r--") + plt.xlabel("t(s)") + plt.title("Third rate component (m/s)") plt.grid() plt.subplot(325) - plt.plot(t , x[:, 3], "b") - plt.plot(t , x[:, 3]+3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.plot(t , x[:, 3]-3 * np.sqrt(P[:, 2, 2]), 'r--') - plt.xlabel('t(s)') - plt.title('Third pos component (m)') + plt.plot(t, x[:, 3], "b") + plt.plot(t, x[:, 3] + 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.plot(t, x[:, 3] - 3 * np.sqrt(P[:, 2, 2]), "r--") + plt.xlabel("t(s)") + plt.title("Third pos component (m)") plt.grid() plt.subplot(326) - plt.plot(t , x[:, 6], "b") - plt.plot(t , x[:, 6]+3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.plot(t , x[:, 6]-3 * np.sqrt(P[:, 5, 5]), 'r--') - plt.xlabel('t(s)') - plt.title('Third rate component (m/s)') + plt.plot(t, x[:, 6], "b") + plt.plot(t, x[:, 6] + 3 * np.sqrt(P[:, 5, 5]), "r--") + plt.plot(t, x[:, 6] - 3 * np.sqrt(P[:, 5, 5]), "r--") + plt.xlabel("t(s)") + plt.title("Third rate component (m/s)") plt.grid() - unitTestSupport.writeFigureLaTeX('StatesPlot' + testName, 'State error and covariance', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "StatesPlot" + testName, + "State error and covariance", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() - def PostFitResiduals(Res, noise, testName, show_plots): - - MeasNoise = np.zeros(len(Res[:,0])) - t= np.zeros(len(Res[:,0])) - for i in range(len(Res[:,0])): - t[i] = Res[i, 0]*1E-9 - MeasNoise[i] = 3*noise + MeasNoise = np.zeros(len(Res[:, 0])) + t = np.zeros(len(Res[:, 0])) + for i in range(len(Res[:, 0])): + t[i] = Res[i, 0] * 1e-9 + MeasNoise[i] = 3 * noise # Don't plot zero values, since they mean that no measurement is taken - for j in range(len(Res[0,:])-1): - if -1E-10 < Res[i,j+1] < 1E-10: - Res[i, j+1] = np.nan + for j in range(len(Res[0, :]) - 1): + if -1e-10 < Res[i, j + 1] < 1e-10: + Res[i, j + 1] = np.nan - plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor="w", edgecolor="k") plt.subplot(311) - plt.plot(t , Res[:, 1], "b.", label='Residual') - plt.plot(t , MeasNoise, 'r--', label='Covar') - plt.plot(t , -MeasNoise, 'r--') - plt.legend(loc='lower right') - plt.ylim([-10*noise, 10*noise]) - plt.title('First Meas Comp (m)') + plt.plot(t, Res[:, 1], "b.", label="Residual") + plt.plot(t, MeasNoise, "r--", label="Covar") + plt.plot(t, -MeasNoise, "r--") + plt.legend(loc="lower right") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("First Meas Comp (m)") plt.grid() plt.subplot(312) - plt.plot(t , Res[:, 2], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Second Meas Comp (m)') + plt.plot(t, Res[:, 2], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Second Meas Comp (m)") plt.grid() plt.subplot(313) - plt.plot(t , Res[:, 3], "b.") - plt.plot(t , MeasNoise, 'r--') - plt.plot(t , -MeasNoise, 'r--') - plt.ylim([-10*noise, 10*noise]) - plt.title('Third Meas Comp (m)') + plt.plot(t, Res[:, 3], "b.") + plt.plot(t, MeasNoise, "r--") + plt.plot(t, -MeasNoise, "r--") + plt.ylim([-10 * noise, 10 * noise]) + plt.title("Third Meas Comp (m)") plt.grid() - - unitTestSupport.writeFigureLaTeX('PostFit' + testName, 'Post Fit Residuals', plt, 'height=0.9\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "PostFit" + testName, + "Post Fit Residuals", + plt, + "height=0.9\\textwidth, keepaspectratio", + path, + ) if show_plots: plt.show() plt.close() + def plot_TwoOrbits(r_BN, r_BN2): fig = plt.figure() - ax = fig.add_subplot(projection='3d') - ax.set_xlabel('$R_x$, km') - ax.set_ylabel('$R_y$, km') - ax.set_zlabel('$R_z$, km') - ax.plot(r_BN[:, 1] * m2km, r_BN[:, 2] * m2km, r_BN[:, 3] * m2km, color_x, label="True orbit") - for i in range(len(r_BN2[:,0])): - if np.abs(r_BN2[i, 1])>0 or np.abs(r_BN2[i, 2])>0: - ax.scatter(r_BN2[i, 1] * m2km, r_BN2[i, 2] * m2km, r_BN2[i, 3] * m2km, color=color_y, label="Meas orbit") - ax.scatter(0, 0, color='r') - ax.set_title('Spacecraft Orbits') + ax = fig.add_subplot(projection="3d") + ax.set_xlabel("$R_x$, km") + ax.set_ylabel("$R_y$, km") + ax.set_zlabel("$R_z$, km") + ax.plot( + r_BN[:, 1] * m2km, + r_BN[:, 2] * m2km, + r_BN[:, 3] * m2km, + color_x, + label="True orbit", + ) + for i in range(len(r_BN2[:, 0])): + if np.abs(r_BN2[i, 1]) > 0 or np.abs(r_BN2[i, 2]) > 0: + ax.scatter( + r_BN2[i, 1] * m2km, + r_BN2[i, 2] * m2km, + r_BN2[i, 3] * m2km, + color=color_y, + label="Meas orbit", + ) + ax.scatter(0, 0, color="r") + ax.set_title("Spacecraft Orbits") return diff --git a/src/fswAlgorithms/opticalNavigation/relativeODuKF/_UnitTest/test_relativeODuKF.py b/src/fswAlgorithms/opticalNavigation/relativeODuKF/_UnitTest/test_relativeODuKF.py index 7a2e04ff49..86c9335b63 100644 --- a/src/fswAlgorithms/opticalNavigation/relativeODuKF/_UnitTest/test_relativeODuKF.py +++ b/src/fswAlgorithms/opticalNavigation/relativeODuKF/_UnitTest/test_relativeODuKF.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016-2018, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -18,7 +17,9 @@ import numpy as np from Basilisk.architecture import messaging -from Basilisk.fswAlgorithms import relativeODuKF # import the module that is to be tested +from Basilisk.fswAlgorithms import ( + relativeODuKF, +) # import the module that is to be tested from Basilisk.utilities import SimulationBaseClass, macros, orbitalMotion import relativeODuKF_test_utilities as FilterPlots @@ -27,39 +28,40 @@ def addTimeColumn(time, data): return np.transpose(np.vstack([[time], np.transpose(data)])) + def rk4(f, t, x0): - x = np.zeros([len(t),len(x0)+1]) - h = (t[len(t)-1] - t[0])/len(t) - x[0,0] = t[0] - x[0,1:] = x0 - for i in range(len(t)-1): - h = t[i+1] - t[i] - x[i,0] = t[i] - k1 = h * f(t[i], x[i,1:]) - k2 = h * f(t[i] + 0.5 * h, x[i,1:] + 0.5 * k1) - k3 = h * f(t[i] + 0.5 * h, x[i,1:] + 0.5 * k2) - k4 = h * f(t[i] + h, x[i,1:] + k3) - x[i+1,1:] = x[i,1:] + (k1 + 2.*k2 + 2.*k3 + k4) / 6. - x[i+1,0] = t[i+1] + x = np.zeros([len(t), len(x0) + 1]) + h = (t[len(t) - 1] - t[0]) / len(t) + x[0, 0] = t[0] + x[0, 1:] = x0 + for i in range(len(t) - 1): + h = t[i + 1] - t[i] + x[i, 0] = t[i] + k1 = h * f(t[i], x[i, 1:]) + k2 = h * f(t[i] + 0.5 * h, x[i, 1:] + 0.5 * k1) + k3 = h * f(t[i] + 0.5 * h, x[i, 1:] + 0.5 * k2) + k4 = h * f(t[i] + h, x[i, 1:] + k3) + x[i + 1, 1:] = x[i, 1:] + (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0 + x[i + 1, 0] = t[i + 1] return x -def twoBodyGrav(t, x, mu = 42828.314*1E9): + +def twoBodyGrav(t, x, mu=42828.314 * 1e9): dxdt = np.zeros(np.shape(x)) dxdt[0:3] = x[3:] - dxdt[3:] = -mu/np.linalg.norm(x[0:3])**3.*x[0:3] + dxdt[3:] = -mu / np.linalg.norm(x[0:3]) ** 3.0 * x[0:3] return dxdt def setupFilterData(filterObject): - filterObject.planetIdInit = 2 filterObject.alpha = 0.02 filterObject.beta = 2.0 filterObject.kappa = 0.0 - mu = 42828.314*1E9 #m^3/s^2 + mu = 42828.314 * 1e9 # m^3/s^2 elementsInit = orbitalMotion.ClassicElements() - elementsInit.a = 4000*1E3 #m + elementsInit.a = 4000 * 1e3 # m elementsInit.e = 0.2 elementsInit.i = 10 elementsInit.Omega = 0.001 @@ -68,33 +70,71 @@ def setupFilterData(filterObject): r, v = orbitalMotion.elem2rv(mu, elementsInit) filterObject.stateInit = r.tolist() + v.tolist() - filterObject.covarInit = [1000.*1E6, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1000.*1E6, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1000.*1E6, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 5*1E6, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 5*1E6, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 5*1E6] + filterObject.covarInit = [ + 1000.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1000.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1000.0 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 5 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 5 * 1e6, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 5 * 1e6, + ] qNoiseIn = np.identity(6) - qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3]*0.00001*0.00001*1E-6 - qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6]*0.0001*0.0001*1E-6 + qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 0.00001 * 0.00001 * 1e-6 + qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 0.0001 * 0.0001 * 1e-6 filterObject.qNoise = qNoiseIn.reshape(36).tolist() filterObject.noiseSF = 1 + # uncomment this line is 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() # need to update how the RW states are defined # provide a unique test method name, starting with test_ + def test_methods_kf(show_plots): """Module Unit Test""" [testResults, testMessage] = relOD_method_test(show_plots) assert testResults < 1, testMessage + + def test_propagation_kf(show_plots): """Module Unit Test""" [testResults, testMessage] = StatePropRelOD(show_plots, 10.0) assert testResults < 1, testMessage + + def test_measurements_kf(show_plots): """Module Unit Test""" [testResults, testMessage] = StateUpdateRelOD(show_plots) @@ -123,19 +163,21 @@ def relOD_method_test(show_plots): data.noiseSF = 1 Covar = np.eye(6) - SPexp = np.zeros([6, 2*6+1]) - SPexp[:,0] = np.array(state) - for i in range(1, 6+1): - SPexp[:,i] = np.array(state) + Covar[:,i-1] - SPexp[:, i+6] = np.array(state) - Covar[:,i-1] + SPexp = np.zeros([6, 2 * 6 + 1]) + SPexp[:, 0] = np.array(state) + for i in range(1, 6 + 1): + SPexp[:, i] = np.array(state) + Covar[:, i - 1] + SPexp[:, i + 6] = np.array(state) - Covar[:, i - 1] - - data.SP = np.transpose(SPexp).flatten().tolist() + data.SP = np.transpose(SPexp).flatten().tolist() relativeODuKF.relODuKFMeasModel(data) measurements = data.yMeas - if np.linalg.norm(np.array(measurements) - np.transpose(SPexp[0:3,:]).flatten()) > 1.0E-15: + if ( + np.linalg.norm(np.array(measurements) - np.transpose(SPexp[0:3, :]).flatten()) + > 1.0e-15 + ): testFailCount += 1 testMessages.append("Measurement Model Failure") @@ -151,9 +193,12 @@ def relOD_method_test(show_plots): propedState = [] for i in range(6): propedState.append(relativeODuKF.doubleArray_getitem(stateIn, i)) - expected = rk4(twoBodyGrav, [0, dt], np.array(state)*1E3) - expected[:,1:]*=1E-3 - if np.linalg.norm((np.array(propedState) - expected[-1,1:])/(expected[-1,1:])) > 1.0E-15: + expected = rk4(twoBodyGrav, [0, dt], np.array(state) * 1e3) + expected[:, 1:] *= 1e-3 + if ( + np.linalg.norm((np.array(propedState) - expected[-1, 1:]) / (expected[-1, 1:])) + > 1.0e-15 + ): testFailCount += 1 testMessages.append("State Prop Failure") @@ -161,7 +206,7 @@ def relOD_method_test(show_plots): print(testMessages) else: print("Passed") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def StateUpdateRelOD(show_plots): @@ -201,20 +246,26 @@ def StateUpdateRelOD(show_plots): dataLog = module.filtDataOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) - time = np.linspace(0, int(multT1*t1), int(multT1*t1//dt)+1) + time = np.linspace(0, int(multT1 * t1), int(multT1 * t1 // dt) + 1) dydt = np.zeros(6) energy = np.zeros(len(time)) - expected=np.zeros([len(time), 7]) - expected[0,1:] = module.stateInit - mu = 42828.314*1E9 - energy[0] = -mu/(2*orbitalMotion.rv2elem(mu, expected[0,1:4], expected[0,4:]).a) - - kick = np.array([0., 0., 0., -0.01, 0.01, 0.02]) * 10 *1E3 - - expected[0:t1,:] = rk4(twoBodyGrav, time[0:t1], module.stateInit) - expected[t1:multT1*t1+1, :] = rk4(twoBodyGrav, time[t1:len(time)], expected[t1-1, 1:] + kick) + expected = np.zeros([len(time), 7]) + expected[0, 1:] = module.stateInit + mu = 42828.314 * 1e9 + energy[0] = -mu / ( + 2 * orbitalMotion.rv2elem(mu, expected[0, 1:4], expected[0, 4:]).a + ) + + kick = np.array([0.0, 0.0, 0.0, -0.01, 0.01, 0.02]) * 10 * 1e3 + + expected[0:t1, :] = rk4(twoBodyGrav, time[0:t1], module.stateInit) + expected[t1 : multT1 * t1 + 1, :] = rk4( + twoBodyGrav, time[t1 : len(time)], expected[t1 - 1, 1:] + kick + ) for i in range(1, len(time)): - energy[i] = - mu / (2 * orbitalMotion.rv2elem(mu, expected[i, 1:4], expected[i, 4:]).a) + energy[i] = -mu / ( + 2 * orbitalMotion.rv2elem(mu, expected[i, 1:4], expected[i, 4:]).a + ) inputData = messaging.OpNavMsgPayload() opnavInMsg = messaging.OpNavMsg() @@ -229,11 +280,19 @@ def StateUpdateRelOD(show_plots): for i in range(t1): if i > 0 and i % 50 == 0: inputData.timeTag = macros.sec2nano(i * dt) - inputData.r_BN_N = expected[i,1:4] + np.random.normal(0, 5*1E-2, 3) + inputData.r_BN_N = expected[i, 1:4] + np.random.normal(0, 5 * 1e-2, 3) inputData.valid = 1 - inputData.covar_N = [5.*1E-2, 0., 0., - 0., 5.*1E-2, 0., - 0., 0., 5.*1E-2] + inputData.covar_N = [ + 5.0 * 1e-2, + 0.0, + 0.0, + 0.0, + 5.0 * 1e-2, + 0.0, + 0.0, + 0.0, + 5.0 * 1e-2, + ] opnavInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * dt)) unitTestSim.ExecuteSimulation() @@ -241,20 +300,28 @@ def StateUpdateRelOD(show_plots): covarLog = addTimeColumn(dataLog.times(), dataLog.covar) for i in range(6): - if (covarLog[t1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i] / 100): + if covarLog[t1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i] / 100: testFailCount += 1 testMessages.append("Covariance update failure at " + str(t1)) - for i in range(t1, multT1*t1): + for i in range(t1, multT1 * t1): if i % 50 == 0: inputData.timeTag = macros.sec2nano(i * dt + 1) - inputData.r_BN_N = expected[i,1:4] + np.random.normal(0, 5*1E-2, 3) + inputData.r_BN_N = expected[i, 1:4] + np.random.normal(0, 5 * 1e-2, 3) inputData.valid = 1 - inputData.covar_N = [5.*1E-2, 0.,0., - 0., 5.*1E-2, 0., - 0., 0., 5.*1E-2] + inputData.covar_N = [ + 5.0 * 1e-2, + 0.0, + 0.0, + 0.0, + 5.0 * 1e-2, + 0.0, + 0.0, + 0.0, + 5.0 * 1e-2, + ] opnavInMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) - unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1)*dt)) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * dt)) unitTestSim.ExecuteSimulation() stateLog = addTimeColumn(dataLog.times(), dataLog.state) @@ -262,21 +329,22 @@ def StateUpdateRelOD(show_plots): postFitLog = addTimeColumn(dataLog.times(), dataLog.postFitRes) covarLog = addTimeColumn(dataLog.times(), dataLog.covar) - diff = np.copy(stateLog) - diff[:,1:]-=expected[:,1:] - FilterPlots.EnergyPlot(time, energy, 'Update', show_plots) - FilterPlots.StateCovarPlot(stateLog, covarLog, 'Update', show_plots) - FilterPlots.StatePlot(diff, 'Update', show_plots) - FilterPlots.plot_TwoOrbits(expected[:,0:4], stateLog[:,0:4]) - FilterPlots.PostFitResiduals(postFitLog, np.sqrt(5*1E-2*1E6), 'Update', show_plots) + diff[:, 1:] -= expected[:, 1:] + FilterPlots.EnergyPlot(time, energy, "Update", show_plots) + FilterPlots.StateCovarPlot(stateLog, covarLog, "Update", show_plots) + FilterPlots.StatePlot(diff, "Update", show_plots) + FilterPlots.plot_TwoOrbits(expected[:, 0:4], stateLog[:, 0:4]) + FilterPlots.PostFitResiduals( + postFitLog, np.sqrt(5 * 1e-2 * 1e6), "Update", show_plots + ) for i in range(6): - if (covarLog[t1*multT1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i] / 100): + if covarLog[t1 * multT1, i * 6 + 1 + i] > covarLog[0, i * 6 + 1 + i] / 100: testFailCount += 1 - testMessages.append("Covariance update failure at " + str(t1*multT1)) + testMessages.append("Covariance update failure at " + str(t1 * multT1)) - if (np.linalg.norm(diff[-1, 1:]/expected[-1,1:]) > 1.0E-1): + if np.linalg.norm(diff[-1, 1:] / expected[-1, 1:]) > 1.0e-1: testFailCount += 1 testMessages.append("State propagation failure") @@ -288,7 +356,7 @@ def StateUpdateRelOD(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def StatePropRelOD(show_plots, dt): @@ -335,32 +403,36 @@ def StatePropRelOD(show_plots, dt): unitTestSim.ConfigureStopTime(macros.min2nano(timeSim)) unitTestSim.ExecuteSimulation() - time = np.linspace(0, int(timeSim*60), int(timeSim*60//dt)+1) + time = np.linspace(0, int(timeSim * 60), int(timeSim * 60 // dt) + 1) dydt = np.zeros(6) energy = np.zeros(len(time)) - expected=np.zeros([len(time), 7]) - expected[0,1:] = module.stateInit - mu = 42828.314*1E9 - energy[0] = -mu/(2*orbitalMotion.rv2elem(mu, expected[0,1:4], expected[0,4:]).a) + expected = np.zeros([len(time), 7]) + expected[0, 1:] = module.stateInit + mu = 42828.314 * 1e9 + energy[0] = -mu / ( + 2 * orbitalMotion.rv2elem(mu, expected[0, 1:4], expected[0, 4:]).a + ) expected = rk4(twoBodyGrav, time, module.stateInit) for i in range(1, len(time)): - energy[i] = - mu / (2 * orbitalMotion.rv2elem(mu, expected[i, 1:4], expected[i, 4:]).a) + energy[i] = -mu / ( + 2 * orbitalMotion.rv2elem(mu, expected[i, 1:4], expected[i, 4:]).a + ) stateLog = addTimeColumn(dataLog.times(), dataLog.state) covarLog = addTimeColumn(dataLog.times(), dataLog.covar) diff = np.copy(stateLog) - diff[:,1:]-=expected[:,1:] - FilterPlots.plot_TwoOrbits(expected[:,0:4], stateLog[:,0:4]) - FilterPlots.EnergyPlot(time, energy, 'Prop', show_plots) - FilterPlots.StateCovarPlot(stateLog, covarLog, 'Prop', show_plots) - FilterPlots.StatePlot(diff, 'Prop', show_plots) + diff[:, 1:] -= expected[:, 1:] + FilterPlots.plot_TwoOrbits(expected[:, 0:4], stateLog[:, 0:4]) + FilterPlots.EnergyPlot(time, energy, "Prop", show_plots) + FilterPlots.StateCovarPlot(stateLog, covarLog, "Prop", show_plots) + FilterPlots.StatePlot(diff, "Prop", show_plots) - if (np.linalg.norm(diff[-1,1:]/expected[-1,1:]) > 1.0E-10): + if np.linalg.norm(diff[-1, 1:] / expected[-1, 1:]) > 1.0e-10: testFailCount += 1 testMessages.append("State propagation failure") - if (energy[0] - energy[-1])/energy[0] > 1.0E-10: + if (energy[0] - energy[-1]) / energy[0] > 1.0e-10: testFailCount += 1 testMessages.append("State propagation failure") @@ -370,7 +442,7 @@ def StatePropRelOD(show_plots, dt): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": diff --git a/src/fswAlgorithms/orbitControl/lambertPlanner/_UnitTest/test_lambertPlanner.py b/src/fswAlgorithms/orbitControl/lambertPlanner/_UnitTest/test_lambertPlanner.py index 4ccfdefab7..5dbcaede4b 100644 --- a/src/fswAlgorithms/orbitControl/lambertPlanner/_UnitTest/test_lambertPlanner.py +++ b/src/fswAlgorithms/orbitControl/lambertPlanner/_UnitTest/test_lambertPlanner.py @@ -40,9 +40,9 @@ # create list with all combinations of parameters paramList = list(itertools.product(*paramArray)) + @pytest.mark.parametrize("accuracy", [1e-2]) @pytest.mark.parametrize("p1_revs, p2_tm, p3_tf, p4_eccs", paramList) - def test_lambertPlanner(show_plots, p1_revs, p2_tm, p3_tf, p4_eccs, accuracy): r""" **Validation Test Description** @@ -83,16 +83,16 @@ def lambertPlannerTestFunction(show_plots, p1_revs, p2_tm, p3_tf, p4_eccs, accur solver = messaging.IZZO muBody = 3.986004418e14 - t0 = 100. + t0 = 100.0 revs = p1_revs tm = p2_tm tf = p3_tf ecc = p4_eccs - targetPosition = np.array([7000*1000., -1000*1000., 2000*1000.]) + targetPosition = np.array([7000 * 1000.0, -1000 * 1000.0, 2000 * 1000.0]) # set up orbit using classical orbit elements oe1 = orbitalMotion.ClassicElements() - r = 10000. * 1000 # meters + r = 10000.0 * 1000 # meters if ecc < 1.0: # elliptic case oe1.a = r @@ -100,10 +100,10 @@ def lambertPlannerTestFunction(show_plots, p1_revs, p2_tm, p3_tf, p4_eccs, accur # parabolic and hyperbolic case oe1.a = -r oe1.e = ecc - oe1.i = 10. * macros.D2R - oe1.Omega = 20. * macros.D2R - oe1.omega = 30. * macros.D2R - oe1.f = 40. * macros.D2R + oe1.i = 10.0 * macros.D2R + oe1.Omega = 20.0 * macros.D2R + oe1.omega = 30.0 * macros.D2R + oe1.f = 40.0 * macros.D2R # spacecraft state at initial time r0_BN_N, v0_BN_N = orbitalMotion.elem2rv_parab(muBody, oe1) @@ -112,23 +112,23 @@ def lambertPlannerTestFunction(show_plots, p1_revs, p2_tm, p3_tf, p4_eccs, accur if ecc < 1.0: # elliptic case M1 = orbitalMotion.E2M(orbitalMotion.f2E(oe1.f, ecc), ecc) - n = np.sqrt(muBody/(oe1.a)**3) - M2 = M1 + n*(tm-t0) + n = np.sqrt(muBody / (oe1.a) ** 3) + M2 = M1 + n * (tm - t0) oe2.f = orbitalMotion.E2f(orbitalMotion.M2E(M2, ecc), ecc) elif ecc == 1.0: # parabolic case - D1 = np.tan(oe1.f/2) - M1 = D1 + 1/3*D1**3 - n = np.sqrt(muBody/(2*(-oe1.a)**3)) - M2 = M1 + n*(tm-t0) - A = 3./2.*M2 - B = (A + np.sqrt(A**2 + 1))**(1./3.) - oe2.f = 2.*np.arctan(B-1./B) + D1 = np.tan(oe1.f / 2) + M1 = D1 + 1 / 3 * D1**3 + n = np.sqrt(muBody / (2 * (-oe1.a) ** 3)) + M2 = M1 + n * (tm - t0) + A = 3.0 / 2.0 * M2 + B = (A + np.sqrt(A**2 + 1)) ** (1.0 / 3.0) + oe2.f = 2.0 * np.arctan(B - 1.0 / B) else: # hyperbolic case N1 = orbitalMotion.H2N(orbitalMotion.f2H(oe1.f, ecc), ecc) - n = np.sqrt(muBody/(-oe1.a)**3) - N2 = N1 + n*(tm-t0) + n = np.sqrt(muBody / (-oe1.a) ** 3) + N2 = N1 + n * (tm - t0) oe2.f = orbitalMotion.H2f(orbitalMotion.N2H(N2, ecc), ecc) # spacecraft state at maneuver time @@ -189,50 +189,59 @@ def lambertPlannerTestFunction(show_plots, p1_revs, p2_tm, p3_tf, p4_eccs, accur solverNameTrue = "Izzo" # make sure module output data is correct - paramsString = ' for rev={}, maneuver time={}, final time={}, eccentricity={}, accuracy={}'.format( - str(p1_revs), - str(p2_tm), - str(p3_tf), - str(p4_eccs), - str(accuracy)) + paramsString = " for rev={}, maneuver time={}, final time={}, eccentricity={}, accuracy={}".format( + str(p1_revs), str(p2_tm), str(p3_tf), str(p4_eccs), str(accuracy) + ) np.testing.assert_string_equal(solverName, solverNameTrue) - np.testing.assert_allclose(r1_N, - r1vecTrue, - rtol=accuracy, - atol=0, - err_msg=('Variable: r1_N,' + paramsString), - verbose=True) - - np.testing.assert_allclose(r2_N, - r2vecTrue, - rtol=accuracy, - atol=0, - err_msg=('Variable: r2_N,' + paramsString), - verbose=True) - - np.testing.assert_allclose(transferTime, - transferTimeTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: transferTime,' + paramsString), - verbose=True) - - np.testing.assert_allclose(mu, - muTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: mu,' + paramsString), - verbose=True) - - np.testing.assert_allclose(numRevolutions, - numRevolutionsTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: numRevolutions,' + paramsString), - verbose=True) + np.testing.assert_allclose( + r1_N, + r1vecTrue, + rtol=accuracy, + atol=0, + err_msg=("Variable: r1_N," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + r2_N, + r2vecTrue, + rtol=accuracy, + atol=0, + err_msg=("Variable: r2_N," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + transferTime, + transferTimeTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: transferTime," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + mu, + muTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: mu," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + numRevolutions, + numRevolutionsTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: numRevolutions," + paramsString), + verbose=True, + ) if __name__ == "__main__": - test_lambertPlanner(False, revs[0], time_maneuver[0], time_final[1], eccentricities[0], 1e-2) + test_lambertPlanner( + False, revs[0], time_maneuver[0], time_final[1], eccentricities[0], 1e-2 + ) diff --git a/src/fswAlgorithms/orbitControl/lambertSecondDV/_UnitTest/test_lambertSecondDV.py b/src/fswAlgorithms/orbitControl/lambertSecondDV/_UnitTest/test_lambertSecondDV.py index 4abcd48b42..1788670aca 100644 --- a/src/fswAlgorithms/orbitControl/lambertSecondDV/_UnitTest/test_lambertSecondDV.py +++ b/src/fswAlgorithms/orbitControl/lambertSecondDV/_UnitTest/test_lambertSecondDV.py @@ -27,19 +27,19 @@ from Basilisk.utilities import orbitalMotion # parameters -DVs = np.array([[1., 3., 4.], [500., -100., 200.]]) -time_maneuver = [1.e3, 2.e3] +DVs = np.array([[1.0, 3.0, 4.0], [500.0, -100.0, 200.0]]) +time_maneuver = [1.0e3, 2.0e3] validFlag = [1] paramArray = [DVs, time_maneuver, validFlag] # create list with all combinations of parameters paramList = list(itertools.product(*paramArray)) # add a case where lambert valid flag is false (0) to make sure module returns zeroed DV -paramList.append((DVs[0], 1.e3, 0)) +paramList.append((DVs[0], 1.0e3, 0)) + @pytest.mark.parametrize("accuracy", [1e-4]) @pytest.mark.parametrize("p1_dv, p2_tm, p3_valid", paramList) - def test_lambertSecondDV(show_plots, p1_dv, p2_tm, p3_valid, accuracy): r""" **Validation Test Description** @@ -80,7 +80,7 @@ def lambertSecondDVTestFunction(show_plots, p1_dv, p2_tm, p3_valid, accuracy): # set up orbit using classical orbit elements oe1 = orbitalMotion.ClassicElements() - r = 10000. * 1000 # meters + r = 10000.0 * 1000 # meters if ecc < 1.0: # elliptic case oe1.a = r @@ -88,10 +88,10 @@ def lambertSecondDVTestFunction(show_plots, p1_dv, p2_tm, p3_valid, accuracy): # parabolic and hyperbolic case oe1.a = -r oe1.e = ecc - oe1.i = 10. * macros.D2R - oe1.Omega = 20. * macros.D2R - oe1.omega = 30. * macros.D2R - oe1.f = 40. * macros.D2R + oe1.i = 10.0 * macros.D2R + oe1.Omega = 20.0 * macros.D2R + oe1.omega = 30.0 * macros.D2R + oe1.f = 40.0 * macros.D2R # spacecraft state at maneuver time, just before maneuver r1_BN_N, v1_BN_N = orbitalMotion.elem2rv_parab(muBody, oe1) @@ -107,12 +107,16 @@ def lambertSecondDVTestFunction(show_plots, p1_dv, p2_tm, p3_valid, accuracy): lambertSolutionInMsgData = messaging.LambertSolutionMsgPayload() lambertSolutionInMsgData.v2_N = v1_BN_N lambertSolutionInMsgData.valid = validLambert - lambertSolutionInMsg = messaging.LambertSolutionMsg().write(lambertSolutionInMsgData) + lambertSolutionInMsg = messaging.LambertSolutionMsg().write( + lambertSolutionInMsgData + ) desiredVelocityInMsgData = messaging.DesiredVelocityMsgPayload() desiredVelocityInMsgData.vDesired_N = v2_BN_N desiredVelocityInMsgData.maneuverTime = tm - desiredVelocityInMsg = messaging.DesiredVelocityMsg().write(desiredVelocityInMsgData) + desiredVelocityInMsg = messaging.DesiredVelocityMsg().write( + desiredVelocityInMsgData + ) # subscribe input messages to module module.lambertSolutionInMsg.subscribeTo(lambertSolutionInMsg) @@ -133,29 +137,31 @@ def lambertSecondDVTestFunction(show_plots, p1_dv, p2_tm, p3_valid, accuracy): dvTrue = dv_N burnStartTimeTrue = macros.sec2nano(tm) else: - dvTrue = np.array([0., 0., 0.]) + dvTrue = np.array([0.0, 0.0, 0.0]) burnStartTimeTrue = 0 # make sure module output data is correct - paramsString = ' for DV={}, maneuver time={}, valid flag={}, accuracy={}'.format( - str(p1_dv), - str(p2_tm), - str(p3_valid), - str(accuracy)) - - np.testing.assert_allclose(dv, - dvTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: dv_N,' + paramsString), - verbose=True) - - np.testing.assert_allclose(burnStartTime, - burnStartTimeTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: burnStartTime,' + paramsString), - verbose=True) + paramsString = " for DV={}, maneuver time={}, valid flag={}, accuracy={}".format( + str(p1_dv), str(p2_tm), str(p3_valid), str(accuracy) + ) + + np.testing.assert_allclose( + dv, + dvTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: dv_N," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + burnStartTime, + burnStartTimeTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: burnStartTime," + paramsString), + verbose=True, + ) if __name__ == "__main__": diff --git a/src/fswAlgorithms/orbitControl/lambertSolver/_UnitTest/Support/IzzoLambert.py b/src/fswAlgorithms/orbitControl/lambertSolver/_UnitTest/Support/IzzoLambert.py index 4d1498810c..382a2e66ee 100644 --- a/src/fswAlgorithms/orbitControl/lambertSolver/_UnitTest/Support/IzzoLambert.py +++ b/src/fswAlgorithms/orbitControl/lambertSolver/_UnitTest/Support/IzzoLambert.py @@ -1,11 +1,11 @@ - ## Documentation is Izzo's 2015 paper "Revisiting Lambert's Problem" # cpp source https://github.com/esa/pykep/blob/master/src/lambert_problem.cpp import numpy as np -class IzzoSolve(): - def __init__(self, r1, r2, tof, mu, max_multiRev = 10, max_iter = 12): + +class IzzoSolve: + def __init__(self, r1, r2, tof, mu, max_multiRev=10, max_iter=12): self.mu = mu self.max_multiRev = max_multiRev self.max_iter = max_iter @@ -20,38 +20,50 @@ def __init__(self, r1, r2, tof, mu, max_multiRev = 10, max_iter = 12): R2 = np.linalg.norm(r2) c_norm = np.linalg.norm(self.c) - self.s = 0.5*(R1 + R2 + c_norm) - self.r1_hat = self.r1/R1 - self.r2_hat = self.r2/R2 - h_hat = np.cross(self.r1_hat, self.r2_hat)/np.linalg.norm(np.cross(self.r1_hat, self.r2_hat)) - assert h_hat[2]!= 0 , print("r1 and r2 don't define a plane") + self.s = 0.5 * (R1 + R2 + c_norm) + self.r1_hat = self.r1 / R1 + self.r2_hat = self.r2 / R2 + h_hat = np.cross(self.r1_hat, self.r2_hat) / np.linalg.norm( + np.cross(self.r1_hat, self.r2_hat) + ) + assert h_hat[2] != 0, print("r1 and r2 don't define a plane") - self.lambda2 = 1. - c_norm / self.s + self.lambda2 = 1.0 - c_norm / self.s self.lambd = np.sqrt(self.lambda2) - self.lambd = np.sign(h_hat[2])*self.lambd - self.t1_hat = -np.sign(h_hat[2])*np.cross(self.r1_hat, h_hat) - self.t2_hat = -np.sign(h_hat[2])*np.cross(self.r2_hat, h_hat) + self.lambd = np.sign(h_hat[2]) * self.lambd + self.t1_hat = -np.sign(h_hat[2]) * np.cross(self.r1_hat, h_hat) + self.t2_hat = -np.sign(h_hat[2]) * np.cross(self.r2_hat, h_hat) - self.lambda3 = self.lambd ** 3. - self.T = np.sqrt(2. * self.mu / self.s ** 3) * self.tof + self.lambda3 = self.lambd**3.0 + self.T = np.sqrt(2.0 * self.mu / self.s**3) * self.tof self.solve() def solve(self): x_list = self.findxy(self.lambd, self.T) self.x = x_list - gamma = np.sqrt(self.mu * self.s / 2.) - rho = (np.linalg.norm(self.r1) - np.linalg.norm(self.r2)) / np.linalg.norm(self.c) - sigma = np.sqrt(1. - rho ** 2) + gamma = np.sqrt(self.mu * self.s / 2.0) + rho = (np.linalg.norm(self.r1) - np.linalg.norm(self.r2)) / np.linalg.norm( + self.c + ) + sigma = np.sqrt(1.0 - rho**2) self.v1 = np.zeros([len(x_list), 3]) self.v2 = np.zeros([len(x_list), 3]) for i in range(len(x_list)): - y = np.sqrt(1. - self.lambda2 + self.lambda2 * x_list[i] ** 2) - vr1 = gamma * ((self.lambd * y - x_list[i]) - rho * (self.lambd * y + x_list[i])) / np.linalg.norm(self.r1) - vr2 = -gamma * ((self.lambd * y - x_list[i]) + rho * (self.lambd * y + x_list[i])) / np.linalg.norm(self.r2) + y = np.sqrt(1.0 - self.lambda2 + self.lambda2 * x_list[i] ** 2) + vr1 = ( + gamma + * ((self.lambd * y - x_list[i]) - rho * (self.lambd * y + x_list[i])) + / np.linalg.norm(self.r1) + ) + vr2 = ( + -gamma + * ((self.lambd * y - x_list[i]) + rho * (self.lambd * y + x_list[i])) + / np.linalg.norm(self.r2) + ) vt = gamma * sigma * (y + self.lambd * x_list[i]) vt1 = vt / np.linalg.norm(self.r1) vt2 = vt / np.linalg.norm(self.r2) @@ -60,19 +72,19 @@ def solve(self): self.v2[i, :] = vr2 * self.r2_hat + vt2 * self.t2_hat def findxy(self, lambd, T): - M_max = np.floor(T/np.pi) - T_00 = np.arccos(lambd) + lambd*np.sqrt(1. - lambd**2) + M_max = np.floor(T / np.pi) + T_00 = np.arccos(lambd) + lambd * np.sqrt(1.0 - lambd**2) T_0 = T_00 + M_max * np.pi if T < T_0 and M_max > 0: - x_old = 0. - x_new = 0. + x_old = 0.0 + x_new = 0.0 T_min = T_0 for it in range(12): DT, D2T, D3T = self.dTdx(x_old, T_min) - if DT != 0.: - x_new = x_old - 2. * DT * D2T / (2. * D2T ** 2 - DT * D3T) + if DT != 0.0: + x_new = x_old - 2.0 * DT * D2T / (2.0 * D2T**2 - DT * D3T) err = np.abs(x_old - x_new) - if err < 1E-13: + if err < 1e-13: break self.tof = self.x2tof(x_new, M_max) T_min = self.tof @@ -80,28 +92,28 @@ def findxy(self, lambd, T): if T_min > T: M_max -= 1 - T_1 = 2. / 3. * (1. - self.lambd ** 3) + T_1 = 2.0 / 3.0 * (1.0 - self.lambd**3) M_max = np.min([M_max, self.max_multiRev]) - x = np.zeros([2*int(M_max) + 1]) - iters = np.zeros([2*int(M_max) + 1]) + x = np.zeros([2 * int(M_max) + 1]) + iters = np.zeros([2 * int(M_max) + 1]) if T >= T_00: self.x0 = -(T - T_00) / (T - T_00 + 4) elif T <= T_1: - self.x0 = 5./2. * T_1 * (T_1 - T) / (T*(1 - self.lambd ** 5)) + 1 + self.x0 = 5.0 / 2.0 * T_1 * (T_1 - T) / (T * (1 - self.lambd**5)) + 1 else: - self.x0 = (T_00 / T) ** (np.log(T_1 / T_00)/np.log(2)) - 1. + self.x0 = (T_00 / T) ** (np.log(T_1 / T_00) / np.log(2)) - 1.0 iters[0] = self.householder(T, self.x0, 0, 1e-5, self.max_iter) x[0] = self.x0 for i in range(1, int(M_max) + 1): - tmp = ((i * np.pi + np.pi) / (8.0 * T)) ** (2. / 3.) - x[2*i - 1] = (tmp - 1) / (tmp + 1) - iters[2*i - 1] = self.householder(T, x[2*i - 1], i, 1e-8, self.max_iter) - x[2*i - 1] = self.x0 - - tmp = ((8.0 * T) / (i * np.pi)) ** (2. / 3.) - x[2*i] = (tmp - 1) / (tmp + 1) - iters[2*i] = self.householder(T, x[2*i], i, 1e-8, self.max_iter) - x[2*i] = self.x0 + tmp = ((i * np.pi + np.pi) / (8.0 * T)) ** (2.0 / 3.0) + x[2 * i - 1] = (tmp - 1) / (tmp + 1) + iters[2 * i - 1] = self.householder(T, x[2 * i - 1], i, 1e-8, self.max_iter) + x[2 * i - 1] = self.x0 + + tmp = ((8.0 * T) / (i * np.pi)) ** (2.0 / 3.0) + x[2 * i] = (tmp - 1) / (tmp + 1) + iters[2 * i] = self.householder(T, x[2 * i], i, 1e-8, self.max_iter) + x[2 * i] = self.x0 return x def get_v1(self): @@ -111,47 +123,72 @@ def get_v2(self): return self.v2 def dTdx(self, x, T): - umx2 = 1. - x ** 2 - y = np.sqrt(1. - self.lambda2 * umx2) - - DT = 1. / umx2 * (3. * T * x - 2. + 2. * self.lambda3 * x / y) - D2T = 1. / umx2 * (3. * T + 5. * x * DT + 2. * (1. - self.lambda2) * self.lambda3 / y**3) - D3T = 1. / umx2 * (7. * x * D2T + 8. * DT - 6. * (1. - self.lambda2) * self.lambda2 * self.lambda3 * x / y**5) + umx2 = 1.0 - x**2 + y = np.sqrt(1.0 - self.lambda2 * umx2) + + DT = 1.0 / umx2 * (3.0 * T * x - 2.0 + 2.0 * self.lambda3 * x / y) + D2T = ( + 1.0 + / umx2 + * ( + 3.0 * T + + 5.0 * x * DT + + 2.0 * (1.0 - self.lambda2) * self.lambda3 / y**3 + ) + ) + D3T = ( + 1.0 + / umx2 + * ( + 7.0 * x * D2T + + 8.0 * DT + - 6.0 * (1.0 - self.lambda2) * self.lambda2 * self.lambda3 * x / y**5 + ) + ) return DT, D2T, D3T def x2tof2(self, x, N): - a = 1.0 / (1.0 - x ** 2) - if a > 0: #Eclipse case + a = 1.0 / (1.0 - x**2) + if a > 0: # Eclipse case alfa = 2.0 * np.arccos(x) beta = 2.0 * np.arcsin(np.sqrt(self.lambda2 / a)) if self.lambd < 0.0: beta = -beta - tof = ((a * np.sqrt(a) * ((alfa - np.sin(alfa)) - (beta - np.sin(beta)) + 2.0 * np.pi * N)) / 2.0) + tof = ( + a + * np.sqrt(a) + * ((alfa - np.sin(alfa)) - (beta - np.sin(beta)) + 2.0 * np.pi * N) + ) / 2.0 else: alfa = 2.0 * np.arccosh(x) beta = 2.0 * np.arcsinh(np.sqrt(-self.lambda2 / a)) if self.lambd < 0.0: beta = -beta - tof = (-a * np.sqrt(-a) * ((beta - np.sinh(beta)) - (alfa - np.sinh(alfa))) / 2.0) + tof = ( + -a + * np.sqrt(-a) + * ((beta - np.sinh(beta)) - (alfa - np.sinh(alfa))) + / 2.0 + ) return tof def x2tof(self, x, N): battin = 0.01 lagrange = 0.2 dist = np.abs(x - 1) - if dist < lagrange and dist > battin: # Use langrange formulation + if dist < lagrange and dist > battin: # Use langrange formulation return self.x2tof2(x, N) K = self.lambda2 - E = x ** 2 - 1. + E = x**2 - 1.0 rho = np.abs(E) z = np.sqrt(1 + K * E) - if dist < battin: # Use Battin formulation + if dist < battin: # Use Battin formulation eta = z - self.lambd * x S1 = 0.5 * (1.0 - self.lambd - x * eta) Q = self.hypergeometricF(S1, 1e-11) Q = 4.0 / 3.0 * Q - tof = (eta ** 3 * Q + 4.0 * self.lambd * eta) / 2.0 + N * np.pi / rho ** 1.5 + tof = (eta**3 * Q + 4.0 * self.lambd * eta) / 2.0 + N * np.pi / rho**1.5 else: # Use Lancaster formulation y = np.sqrt(rho) g = x * z - self.lambd * E @@ -169,8 +206,10 @@ def householder(self, T, x0, N, eps, iter_max): tof = self.x2tof(x0, N) DT, DDT, DDDT = self.dTdx(x0, tof) delta = tof - T - DT2 = DT ** 2 - xnew = x0 - delta * (DT2 - delta * DDT / 2.0) / (DT * (DT2 - delta * DDT) + DDDT * delta * delta / 6.0) + DT2 = DT**2 + xnew = x0 - delta * (DT2 - delta * DDT / 2.0) / ( + DT * (DT2 - delta * DDT) + DDDT * delta * delta / 6.0 + ) err = np.abs(x0 - xnew) x0 = xnew self.x0 = xnew @@ -179,11 +218,11 @@ def householder(self, T, x0, N, eps, iter_max): return it def hypergeometricF(self, z, tol): - Sj = 1. - Cj = 1. - err = 1. + Sj = 1.0 + Cj = 1.0 + err = 1.0 for j in range(12): - Cj1 = Cj * (3. + j) * (1.0 + j) / (2.5 + j) * z / (j + 1) + Cj1 = Cj * (3.0 + j) * (1.0 + j) / (2.5 + j) * z / (j + 1) Sj1 = Sj + Cj1 err = np.abs(Cj1) Sj = Sj1 diff --git a/src/fswAlgorithms/orbitControl/lambertSolver/_UnitTest/test_lambertSolver.py b/src/fswAlgorithms/orbitControl/lambertSolver/_UnitTest/test_lambertSolver.py index 67becfcb66..88d63deb7d 100644 --- a/src/fswAlgorithms/orbitControl/lambertSolver/_UnitTest/test_lambertSolver.py +++ b/src/fswAlgorithms/orbitControl/lambertSolver/_UnitTest/test_lambertSolver.py @@ -40,24 +40,32 @@ revs = [0, 1, 4] times = [1e2, 1e6] eccentricities = [0.0, 0.05, 1.0, 1.2] -transferAngle = [30., 90., 180., 210., -60., 500.] -alignmentThreshold = [1.] +transferAngle = [30.0, 90.0, 180.0, 210.0, -60.0, 500.0] +alignmentThreshold = [1.0] paramArray = [solver, revs, times, eccentricities, transferAngle, alignmentThreshold] # create list with all combinations of parameters paramList = list(itertools.product(*paramArray)) # transfer angles >= 180 or < 0 not applicable for parabolic and hyperbolic transfer. Delete from list. -paramList = [item for item in paramList if not (item[3] >= 1.0 and (item[4] >= 180. or item[4] < 0.))] +paramList = [ + item + for item in paramList + if not (item[3] >= 1.0 and (item[4] >= 180.0 or item[4] < 0.0)) +] # add cases to check alignmentThreshold module parameter -paramList.append((solver[0], revs[0], times[0], eccentricities[0], 0.5, 1.)) -paramList.append((solver[0], revs[0], times[0], eccentricities[0], 1.5, 1.)) -paramList.append((solver[0], revs[0], times[0], eccentricities[0], 1.5, 2.)) -paramList.append((solver[0], revs[0], times[0], eccentricities[0], 2.5, 2.)) +paramList.append((solver[0], revs[0], times[0], eccentricities[0], 0.5, 1.0)) +paramList.append((solver[0], revs[0], times[0], eccentricities[0], 1.5, 1.0)) +paramList.append((solver[0], revs[0], times[0], eccentricities[0], 1.5, 2.0)) +paramList.append((solver[0], revs[0], times[0], eccentricities[0], 2.5, 2.0)) -@pytest.mark.parametrize("accuracy", [1e-2]) -@pytest.mark.parametrize("p1_solver, p2_revs, p3_times, p4_eccs, p5_angles, p6_align", paramList) -def test_lambertSolver(show_plots, p1_solver, p2_revs, p3_times, p4_eccs, p5_angles, p6_align, accuracy): +@pytest.mark.parametrize("accuracy", [1e-2]) +@pytest.mark.parametrize( + "p1_solver, p2_revs, p3_times, p4_eccs, p5_angles, p6_align", paramList +) +def test_lambertSolver( + show_plots, p1_solver, p2_revs, p3_times, p4_eccs, p5_angles, p6_align, accuracy +): r""" **Validation Test Description** @@ -89,10 +97,14 @@ def test_lambertSolver(show_plots, p1_solver, p2_revs, p3_times, p4_eccs, p5_ang For the multi-revolution case, the solution for the free variable x is also tested. """ - lambertSolverTestFunction(show_plots, p1_solver, p2_revs, p3_times, p4_eccs, p5_angles, p6_align, accuracy) + lambertSolverTestFunction( + show_plots, p1_solver, p2_revs, p3_times, p4_eccs, p5_angles, p6_align, accuracy + ) -def lambertSolverTestFunction(show_plots, p1_solver, p2_revs, p3_times, p4_eccs, p5_angles, p6_align, accuracy): +def lambertSolverTestFunction( + show_plots, p1_solver, p2_revs, p3_times, p4_eccs, p5_angles, p6_align, accuracy +): unitTaskName = "unitTask" unitProcessName = "TestProcess" @@ -112,7 +124,7 @@ def lambertSolverTestFunction(show_plots, p1_solver, p2_revs, p3_times, p4_eccs, # set up the transfer orbit using classical orbit elements mu = 3.986004418e14 oe1 = orbitalMotion.ClassicElements() - r = 10000. * 1000 # meters + r = 10000.0 * 1000 # meters if p4_eccs < 1.0: # elliptic case oe1.a = r @@ -120,16 +132,16 @@ def lambertSolverTestFunction(show_plots, p1_solver, p2_revs, p3_times, p4_eccs, # parabolic and hyperbolic case oe1.a = -r oe1.e = p4_eccs - oe1.i = 5. * macros.D2R - oe1.Omega = 25. * macros.D2R - oe1.omega = 30. * macros.D2R - oe1.f = 10. * macros.D2R + oe1.i = 5.0 * macros.D2R + oe1.Omega = 25.0 * macros.D2R + oe1.omega = 30.0 * macros.D2R + oe1.f = 10.0 * macros.D2R r1_N, v1_N = orbitalMotion.elem2rv_parab(mu, oe1) oe2 = copy.deepcopy(oe1) oe2.f = oe1.f + p5_angles * macros.D2R # Izzo and Gooding Lambert algorithms only consider positive transfer angles. Convert to 0 < angle < 2pi - oe2.f = (oe2.f*macros.R2D % 360) * macros.D2R + oe2.f = (oe2.f * macros.R2D % 360) * macros.D2R r2_N, v2_N = orbitalMotion.elem2rv_parab(mu, oe2) # determine time-of-flight for given transfer orbit and position vectors (true anomalies) @@ -140,22 +152,22 @@ def lambertSolverTestFunction(show_plots, p1_solver, p2_revs, p3_times, p4_eccs, # elliptic case M1 = orbitalMotion.E2M(orbitalMotion.f2E(oe1.f, p4_eccs), p4_eccs) M2 = orbitalMotion.E2M(orbitalMotion.f2E(oe2.f, p4_eccs), p4_eccs) - n = np.sqrt(mu/(oe1.a)**3) - t_transfer = np.abs(M2-M1)/n + n = np.sqrt(mu / (oe1.a) ** 3) + t_transfer = np.abs(M2 - M1) / n elif p4_eccs == 1.0: # parabolic case - D1 = np.tan(oe1.f/2) - D2 = np.tan(oe2.f/2) - M1 = D1 + 1/3*D1**3 - M2 = D2 + 1/3*D2**3 - n = np.sqrt(mu/(2*(-oe1.a)**3)) - t_transfer = np.abs(M2-M1)/n + D1 = np.tan(oe1.f / 2) + D2 = np.tan(oe2.f / 2) + M1 = D1 + 1 / 3 * D1**3 + M2 = D2 + 1 / 3 * D2**3 + n = np.sqrt(mu / (2 * (-oe1.a) ** 3)) + t_transfer = np.abs(M2 - M1) / n else: # hyperbolic case N1 = orbitalMotion.H2N(orbitalMotion.f2H(oe1.f, p4_eccs), p4_eccs) N2 = orbitalMotion.H2N(orbitalMotion.f2H(oe2.f, p4_eccs), p4_eccs) - n = np.sqrt(mu/(-oe1.a)**3) - t_transfer = np.abs(N2-N1)/n + n = np.sqrt(mu / (-oe1.a) ** 3) + t_transfer = np.abs(N2 - N1) / n solverName = p1_solver time = t_transfer @@ -197,7 +209,7 @@ def lambertSolverTestFunction(show_plots, p1_solver, p2_revs, p3_times, p4_eccs, validFlagSol2 = lambertSolutionOutMsgRec.validSol2[0] # for multi-revolution case, use external Lambert solver - if revs > 0 and p5_angles != 180.: + if revs > 0 and p5_angles != 180.0: Izzo = IzzoSolve(np.array(r1_N), np.array(r2_N), time, mu, revs) Izzo.solve() numSolutions = len(Izzo.x) @@ -207,29 +219,29 @@ def lambertSolverTestFunction(show_plots, p1_solver, p2_revs, p3_times, p4_eccs, # if the transfer angle is smaller than alignmentThreshold, the two position vectors are too aligned. # They might not define a plane, so no solution should be returned. - if abs(np.sin(p5_angles*macros.D2R)) < abs(np.sin(p6_align*macros.D2R)): + if abs(np.sin(p5_angles * macros.D2R)) < abs(np.sin(p6_align * macros.D2R)): alignmentFlag = True else: alignmentFlag = False - if alignmentFlag or (revs > 0 and idx+1 > numSolutions): + if alignmentFlag or (revs > 0 and idx + 1 > numSolutions): # 1. if transfer angle is smaller than alignmentThreshold, no solution should be returned # 2. external Lambert solver does not compute solution if requested transfer time is less than minimum # time-of-flight for multi-revolution solution. In this case set all true outputs to zero # (so does the lambert module as well, since no solution exists for the requested time-of-flight) - v1True_N = np.array([0., 0., 0.]) - v2True_N = np.array([0., 0., 0.]) + v1True_N = np.array([0.0, 0.0, 0.0]) + v2True_N = np.array([0.0, 0.0, 0.0]) validFlagTrue = 0 - v1TrueSol2_N = np.array([0., 0., 0.]) - v2TrueSol2_N = np.array([0., 0., 0.]) + v1TrueSol2_N = np.array([0.0, 0.0, 0.0]) + v2TrueSol2_N = np.array([0.0, 0.0, 0.0]) validFlagTrueSol2 = 0 elif revs == 0: # for zero-revolution case, obtain true velocity vectors from computed transfer orbit v1True_N = v1_N v2True_N = v2_N validFlagTrue = 1 - v1TrueSol2_N = np.array([0., 0., 0.]) - v2TrueSol2_N = np.array([0., 0., 0.]) + v1TrueSol2_N = np.array([0.0, 0.0, 0.0]) + v2TrueSol2_N = np.array([0.0, 0.0, 0.0]) validFlagTrueSol2 = 0 else: # for multi-revolution case, obtain true velocity vectors from external Lambert solver script if solution exist @@ -242,85 +254,112 @@ def lambertSolverTestFunction(show_plots, p1_solver, p2_revs, p3_times, p4_eccs, # only compare solution for free variable x for multi-revolution case (for 0 revolution case, # external lambert solver is not called so no true value is available) - if (alignmentFlag == False and revs > 0 and idx < numSolutions): + if alignmentFlag == False and revs > 0 and idx < numSolutions: x = lambertPerformanceOutMsgRec.x[0] xSol2 = lambertPerformanceOutMsgRec.xSol2[0] xTrue = Izzo.x[idx] xTrueSol2 = Izzo.x[idxSol2] else: - x = 0. - xSol2 = 0. - xTrue = 0. - xTrueSol2 = 0. + x = 0.0 + xSol2 = 0.0 + xTrue = 0.0 + xTrueSol2 = 0.0 # make sure module output data is correct - paramsString = ' for solver={}, rev={}, time={}, eccentricity={}, angle={}, alignmentThreshold={}, ' \ - 'accuracy={}'.format( - str(p1_solver), - str(p2_revs), - str(p3_times), - str(p4_eccs), - str(p5_angles), - str(p6_align), - str(accuracy)) - - np.testing.assert_allclose(v1_N, - v1True_N, - rtol=0, - atol=accuracy, - err_msg=('Variable: v1_N,' + paramsString), - verbose=True) - - np.testing.assert_allclose(v2_N, - v2True_N, - rtol=0, - atol=accuracy, - err_msg=('Variable: v2_N,' + paramsString), - verbose=True) - - np.testing.assert_allclose(validFlag, - validFlagTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: validFlag,' + paramsString), - verbose=True) - - np.testing.assert_allclose(v1Sol2_N, - v1TrueSol2_N, - rtol=0, - atol=accuracy, - err_msg=('Variable: v1Sol2_N,' + paramsString), - verbose=True) - - np.testing.assert_allclose(v2Sol2_N, - v2TrueSol2_N, - rtol=0, - atol=accuracy, - err_msg=('Variable: v2Sol2_N,' + paramsString), - verbose=True) - - np.testing.assert_allclose(validFlagSol2, - validFlagTrueSol2, - rtol=0, - atol=accuracy, - err_msg=('Variable: validFlagSol2,' + paramsString), - verbose=True) - - np.testing.assert_allclose(x, - xTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: x,' + paramsString), - verbose=True) - - np.testing.assert_allclose(xSol2, - xTrueSol2, - rtol=0, - atol=accuracy, - err_msg=('Variable: xSol2,' + paramsString), - verbose=True) + paramsString = ( + " for solver={}, rev={}, time={}, eccentricity={}, angle={}, alignmentThreshold={}, " + "accuracy={}".format( + str(p1_solver), + str(p2_revs), + str(p3_times), + str(p4_eccs), + str(p5_angles), + str(p6_align), + str(accuracy), + ) + ) + + np.testing.assert_allclose( + v1_N, + v1True_N, + rtol=0, + atol=accuracy, + err_msg=("Variable: v1_N," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + v2_N, + v2True_N, + rtol=0, + atol=accuracy, + err_msg=("Variable: v2_N," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + validFlag, + validFlagTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: validFlag," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + v1Sol2_N, + v1TrueSol2_N, + rtol=0, + atol=accuracy, + err_msg=("Variable: v1Sol2_N," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + v2Sol2_N, + v2TrueSol2_N, + rtol=0, + atol=accuracy, + err_msg=("Variable: v2Sol2_N," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + validFlagSol2, + validFlagTrueSol2, + rtol=0, + atol=accuracy, + err_msg=("Variable: validFlagSol2," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + x, + xTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: x," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + xSol2, + xTrueSol2, + rtol=0, + atol=accuracy, + err_msg=("Variable: xSol2," + paramsString), + verbose=True, + ) if __name__ == "__main__": - test_lambertSolver(False, solver[1], revs[0], - times[1], eccentricities[1], transferAngle[0], alignmentThreshold[0], 1e-2) + test_lambertSolver( + False, + solver[1], + revs[0], + times[1], + eccentricities[1], + transferAngle[0], + alignmentThreshold[0], + 1e-2, + ) diff --git a/src/fswAlgorithms/orbitControl/lambertSurfaceRelativeVelocity/_UnitTest/test_lambertSurfaceRelativeVelocity.py b/src/fswAlgorithms/orbitControl/lambertSurfaceRelativeVelocity/_UnitTest/test_lambertSurfaceRelativeVelocity.py index 55f0344fdc..eabcd5de6e 100644 --- a/src/fswAlgorithms/orbitControl/lambertSurfaceRelativeVelocity/_UnitTest/test_lambertSurfaceRelativeVelocity.py +++ b/src/fswAlgorithms/orbitControl/lambertSurfaceRelativeVelocity/_UnitTest/test_lambertSurfaceRelativeVelocity.py @@ -28,20 +28,28 @@ from Basilisk.utilities import orbitalMotion # parameters -relativeVelocities = np.array([[0., 0., 0.], [100., -300., 200.]]) -time_maneuver = [1.e3, 2.e3] -trueAnomalies = [0., 70.] -rotAngles = [0., 230.] -angularVelocities = np.array([[0., 0., 0.], [1., -3., 2.]]) - -paramArray = [relativeVelocities, time_maneuver, trueAnomalies, rotAngles, angularVelocities] +relativeVelocities = np.array([[0.0, 0.0, 0.0], [100.0, -300.0, 200.0]]) +time_maneuver = [1.0e3, 2.0e3] +trueAnomalies = [0.0, 70.0] +rotAngles = [0.0, 230.0] +angularVelocities = np.array([[0.0, 0.0, 0.0], [1.0, -3.0, 2.0]]) + +paramArray = [ + relativeVelocities, + time_maneuver, + trueAnomalies, + rotAngles, + angularVelocities, +] # create list with all combinations of parameters paramList = list(itertools.product(*paramArray)) + @pytest.mark.parametrize("accuracy", [1e-4]) @pytest.mark.parametrize("p1_vr, p2_tm, p3_f, p4_rot, p5_omega", paramList) - -def test_lambertSurfaceRelativeVelocity(show_plots, p1_vr, p2_tm, p3_f, p4_rot, p5_omega, accuracy): +def test_lambertSurfaceRelativeVelocity( + show_plots, p1_vr, p2_tm, p3_f, p4_rot, p5_omega, accuracy +): r""" **Validation Test Description** @@ -64,10 +72,14 @@ def test_lambertSurfaceRelativeVelocity(show_plots, p1_vr, p2_tm, p3_f, p4_rot, The content of the DesiredVelocityMsg output message (velocity vector and maneuver time) is compared with the true values. """ - lambertSurfaceRelativeVelocityTestFunction(show_plots, p1_vr, p2_tm, p3_f, p4_rot, p5_omega, accuracy) + lambertSurfaceRelativeVelocityTestFunction( + show_plots, p1_vr, p2_tm, p3_f, p4_rot, p5_omega, accuracy + ) -def lambertSurfaceRelativeVelocityTestFunction(show_plots, p1_vr, p2_tm, p3_f, p4_rot, p5_omega, accuracy): +def lambertSurfaceRelativeVelocityTestFunction( + show_plots, p1_vr, p2_tm, p3_f, p4_rot, p5_omega, accuracy +): unitTaskName = "unitTask" unitProcessName = "TestProcess" @@ -83,7 +95,7 @@ def lambertSurfaceRelativeVelocityTestFunction(show_plots, p1_vr, p2_tm, p3_f, p omega_PN_N = p5_omega if np.linalg.norm(omega_PN_N) > 1e-6: - omegaHat_PN_N = omega_PN_N/np.linalg.norm(omega_PN_N) + omegaHat_PN_N = omega_PN_N / np.linalg.norm(omega_PN_N) else: omegaHat_PN_N = np.array([0.0, 0.0, 1.0]) @@ -94,12 +106,12 @@ def lambertSurfaceRelativeVelocityTestFunction(show_plots, p1_vr, p2_tm, p3_f, p # set up orbit using classical orbit elements oe0 = orbitalMotion.ClassicElements() - r = 10000. * 1000 # meters + r = 10000.0 * 1000 # meters oe0.a = r oe0.e = ecc - oe0.i = 10. * macros.D2R - oe0.Omega = 20. * macros.D2R - oe0.omega = 30. * macros.D2R + oe0.i = 10.0 * macros.D2R + oe0.Omega = 20.0 * macros.D2R + oe0.omega = 30.0 * macros.D2R oe0.f = p3_f * macros.D2R # spacecraft state at initial time r0_BN_N, v0_BN_N = orbitalMotion.elem2rv_parab(muBody, oe0) @@ -118,7 +130,9 @@ def lambertSurfaceRelativeVelocityTestFunction(show_plots, p1_vr, p2_tm, p3_f, p ephemerisInMsg = messaging.EphemerisMsg().write(ephemerisInMsgData) lambertProblemInMsgData = messaging.LambertProblemMsgPayload() - lambertProblemInMsgData.r2_N = r0_BN_N # only info of LambertProblemMsg needed by module + lambertProblemInMsgData.r2_N = ( + r0_BN_N # only info of LambertProblemMsg needed by module + ) lambertProblemInMsg = messaging.LambertProblemMsg().write(lambertProblemInMsgData) # subscribe input messages to module @@ -138,41 +152,53 @@ def lambertSurfaceRelativeVelocityTestFunction(show_plots, p1_vr, p2_tm, p3_f, p # true values if np.linalg.norm(omega_PN_N) > 1e-6: - s1Hat = np.cross(omega_PN_N, r0_BN_N)/np.linalg.norm(np.cross(omega_PN_N, r0_BN_N)) + s1Hat = np.cross(omega_PN_N, r0_BN_N) / np.linalg.norm( + np.cross(omega_PN_N, r0_BN_N) + ) else: - s1Hat = (np.cross(np.array([0.0, 0.0, 1.0]), r0_BN_N)/ - np.linalg.norm(np.cross(np.array([0.0, 0.0, 1.0]), r0_BN_N))) - s3Hat = r0_BN_N/np.linalg.norm(r0_BN_N) - s2Hat = np.cross(s3Hat, s1Hat)/np.linalg.norm(np.cross(s3Hat, s1Hat)) + s1Hat = np.cross(np.array([0.0, 0.0, 1.0]), r0_BN_N) / np.linalg.norm( + np.cross(np.array([0.0, 0.0, 1.0]), r0_BN_N) + ) + s3Hat = r0_BN_N / np.linalg.norm(r0_BN_N) + s2Hat = np.cross(s3Hat, s1Hat) / np.linalg.norm(np.cross(s3Hat, s1Hat)) dcm_SN = np.array([s1Hat, s2Hat, s3Hat]) vDesiredTrue = np.cross(omega_PN_N, r0_BN_N) + np.dot(dcm_SN.T, vRelativeDesired_S) timeTrue = tm # make sure module output data is correct - paramsString = ' for desired relative velocity={}, maneuver time={}, true anomaly={}, rotation angle={}, ' \ - 'angular velocity={}, accuracy={}'.format( - str(p1_vr), - str(p2_tm), - str(p3_f), - str(p4_rot), - str(p5_omega), - str(accuracy)) - - np.testing.assert_allclose(vDesired, - vDesiredTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: desired velocity,' + paramsString), - verbose=True) - - np.testing.assert_allclose(time, - timeTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: maneuver time,' + paramsString), - verbose=True) + paramsString = ( + " for desired relative velocity={}, maneuver time={}, true anomaly={}, rotation angle={}, " + "angular velocity={}, accuracy={}".format( + str(p1_vr), str(p2_tm), str(p3_f), str(p4_rot), str(p5_omega), str(accuracy) + ) + ) + + np.testing.assert_allclose( + vDesired, + vDesiredTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: desired velocity," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + time, + timeTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: maneuver time," + paramsString), + verbose=True, + ) if __name__ == "__main__": - test_lambertSurfaceRelativeVelocity(False, relativeVelocities[1], time_maneuver[0], trueAnomalies[0], rotAngles[0], - angularVelocities[0], 1e-4) + test_lambertSurfaceRelativeVelocity( + False, + relativeVelocities[1], + time_maneuver[0], + trueAnomalies[0], + rotAngles[0], + angularVelocities[0], + 1e-4, + ) diff --git a/src/fswAlgorithms/orbitControl/lambertValidator/_UnitTest/test_lambertValidator.py b/src/fswAlgorithms/orbitControl/lambertValidator/_UnitTest/test_lambertValidator.py index 4425545e7b..87379b8c99 100644 --- a/src/fswAlgorithms/orbitControl/lambertValidator/_UnitTest/test_lambertValidator.py +++ b/src/fswAlgorithms/orbitControl/lambertValidator/_UnitTest/test_lambertValidator.py @@ -31,9 +31,9 @@ from Basilisk.utilities import orbitalMotion # parameters -DVs = np.array([[1., 3., 4.], [500., -100., 200.]]) -time_maneuver = [1.e3, 2.e3] -time_final = [2.1e3, 4.e3] +DVs = np.array([[1.0, 3.0, 4.0], [500.0, -100.0, 200.0]]) +time_maneuver = [1.0e3, 2.0e3] +time_final = [2.1e3, 4.0e3] iterations = [3] errorsX = [1e-9] @@ -41,14 +41,14 @@ # create list with all combinations of parameters paramList = list(itertools.product(*paramArray)) # add cases to check Lambert solver iterations and error limits, plus final time occurring before maneuver time -paramList.append((DVs[0], 1.e3, 2.e3, 7, 1.e-9)) -paramList.append((DVs[0], 1.e3, 2.e3, 3, 1.e-5)) -paramList.append((DVs[0], 2.e3, 1.e3, 3, 1.e-9)) -paramList.append((DVs[0], 2.e3, 2.e3, 3, 1.e-9)) +paramList.append((DVs[0], 1.0e3, 2.0e3, 7, 1.0e-9)) +paramList.append((DVs[0], 1.0e3, 2.0e3, 3, 1.0e-5)) +paramList.append((DVs[0], 2.0e3, 1.0e3, 3, 1.0e-9)) +paramList.append((DVs[0], 2.0e3, 2.0e3, 3, 1.0e-9)) + @pytest.mark.parametrize("accuracy", [1e-4]) @pytest.mark.parametrize("p1_dv, p2_tm, p3_tf, p4_iter, p5_errX", paramList) - def test_lambertValidator(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_errX, accuracy): r""" **Validation Test Description** @@ -74,7 +74,9 @@ def test_lambertValidator(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_errX, acc """ expect_error = p2_tm > p3_tf with pytest.raises(BasiliskError) if expect_error else nullcontext(): - lambertValidatorTestFunction(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_errX, accuracy) + lambertValidatorTestFunction( + show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_errX, accuracy + ) def getInitialStates(r1_BN_N, v1_BN_N, dv_N, errStates, errDV): @@ -99,12 +101,12 @@ def getInitialStates(r1_BN_N, v1_BN_N, dv_N, errStates, errDV): for ii in range(N): for jj in range(2): if jj == 0: - multiplier = 1. + multiplier = 1.0 else: - multiplier = -1. + multiplier = -1.0 # add state uncertainty - X1_H = X1nom_H + multiplier*Psqrt[:, ii] + X1_H = X1nom_H + multiplier * Psqrt[:, ii] X1minDV_H = copy.deepcopy(X1_H) X1maxDV_H = copy.deepcopy(X1_H) @@ -112,8 +114,10 @@ def getInitialStates(r1_BN_N, v1_BN_N, dv_N, errStates, errDV): X1minDV_H[3:6] += dv_H - errDV * dvHat_H X1maxDV_H[3:6] += dv_H + errDV * dvHat_H - initialStates[:, jj*N + ii] = dcm_HN_states.transpose().dot(X1minDV_H) - initialStates[:, 2*N + jj*N + ii] = dcm_HN_states.transpose().dot(X1maxDV_H) + initialStates[:, jj * N + ii] = dcm_HN_states.transpose().dot(X1minDV_H) + initialStates[:, 2 * N + jj * N + ii] = dcm_HN_states.transpose().dot( + X1maxDV_H + ) X1_H = copy.deepcopy(X1nom_H) X1minDV_H = copy.deepcopy(X1_H) @@ -124,13 +128,16 @@ def getInitialStates(r1_BN_N, v1_BN_N, dv_N, errStates, errDV): X1minDV_H[3:6] += dv_H - errDV * dvHat_H X1maxDV_H[3:6] += dv_H + errDV * dvHat_H - initialStates[:, 4*N] = dcm_HN_states.transpose().dot(X1_H) - initialStates[:, 4*N + 1] = dcm_HN_states.transpose().dot(X1minDV_H) - initialStates[:, 4*N + 2] = dcm_HN_states.transpose().dot(X1maxDV_H) + initialStates[:, 4 * N] = dcm_HN_states.transpose().dot(X1_H) + initialStates[:, 4 * N + 1] = dcm_HN_states.transpose().dot(X1minDV_H) + initialStates[:, 4 * N + 2] = dcm_HN_states.transpose().dot(X1maxDV_H) return initialStates -def countViolations(initialStates, muBody, tm, tf, r_TN_N, maxDistanceTarget, minOrbitRadius): + +def countViolations( + initialStates, muBody, tm, tf, r_TN_N, maxDistanceTarget, minOrbitRadius +): violationsDistanceTarget = 0 violationsOrbitRadius = 0 @@ -149,22 +156,22 @@ def countViolations(initialStates, muBody, tm, tf, r_TN_N, maxDistanceTarget, mi # elliptic case M2 = orbitalMotion.E2M(orbitalMotion.f2E(oem.f, eccf), eccf) n = np.sqrt(muBody / (oem.a) ** 3) - M3 = M2 + n*(tf-tm) + M3 = M2 + n * (tf - tm) oef.f = orbitalMotion.E2f(orbitalMotion.M2E(M3, eccf), eccf) elif eccf == 1.0: # parabolic case D2 = np.tan(oem.f / 2) - M2 = D2 + 1/3*D2**3 + M2 = D2 + 1 / 3 * D2**3 n = np.sqrt(muBody / (2 * (-oem.a) ** 3)) - M3 = M2 + n*(tf-tm) - A = 3./2.*M3 - B = (A + np.sqrt(A**2 + 1))**(1./3.) - oef.f = 2. * np.arctan(B - 1. / B) + M3 = M2 + n * (tf - tm) + A = 3.0 / 2.0 * M3 + B = (A + np.sqrt(A**2 + 1)) ** (1.0 / 3.0) + oef.f = 2.0 * np.arctan(B - 1.0 / B) else: # hyperbolic case N2 = orbitalMotion.H2N(orbitalMotion.f2H(oem.f, eccf), eccf) n = np.sqrt(muBody / (-oem.a) ** 3) - N3 = N2 + n*(tf-tm) + N3 = N2 + n * (tf - tm) oef.f = orbitalMotion.H2f(orbitalMotion.N2H(N3, eccf), eccf) # spacecraft state at final time @@ -174,10 +181,10 @@ def countViolations(initialStates, muBody, tm, tf, r_TN_N, maxDistanceTarget, mi rf_BT_N = rf_BN_N - r_TN_N # find the smallest orbit radius along transfer arc - f1 = math.remainder(oem.f, 2*np.pi) - f2 = math.remainder(oef.f, 2*np.pi) - if f1*f2 < 0.: - f_rmin = 0. + f1 = math.remainder(oem.f, 2 * np.pi) + f2 = math.remainder(oef.f, 2 * np.pi) + if f1 * f2 < 0.0: + f_rmin = 0.0 else: f_rmin = min([abs(f1), abs(f2)]) oe_rmin = copy.deepcopy(oem) @@ -193,7 +200,9 @@ def countViolations(initialStates, muBody, tm, tf, r_TN_N, maxDistanceTarget, mi return violationsDistanceTarget, violationsOrbitRadius -def lambertValidatorTestFunction(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_errX, accuracy): +def lambertValidatorTestFunction( + show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_errX, accuracy +): unitTaskName = "unitTask" unitProcessName = "TestProcess" @@ -204,21 +213,23 @@ def lambertValidatorTestFunction(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_er solverMethod = messaging.IZZO muBody = 3.986004418e14 - t0 = 0. + t0 = 0.0 tm = p2_tm tf = p3_tf numIter = p4_iter errX = p5_errX ecc = 0.1 dv_N = p1_dv - errStates = np.diag([5., 5., 5., 0.01, 0.01, 0.001]) # 6x6 state uncertainty matrix + errStates = np.diag( + [5.0, 5.0, 5.0, 0.01, 0.01, 0.001] + ) # 6x6 state uncertainty matrix errDV = 0.1 # [m/s] Delta-V magnitude uncertainty - maxDistanceTarget = 3000. - minOrbitRadius = 6378 * 1000. + maxDistanceTarget = 3000.0 + minOrbitRadius = 6378 * 1000.0 # set up orbit using classical orbit elements oe0 = orbitalMotion.ClassicElements() - r = 10000. * 1000 # meters + r = 10000.0 * 1000 # meters if ecc < 1.0: # elliptic case oe0.a = r @@ -226,10 +237,10 @@ def lambertValidatorTestFunction(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_er # parabolic and hyperbolic case oe0.a = -r oe0.e = ecc - oe0.i = 10. * macros.D2R - oe0.Omega = 20. * macros.D2R - oe0.omega = 30. * macros.D2R - oe0.f = 40. * macros.D2R + oe0.i = 10.0 * macros.D2R + oe0.Omega = 20.0 * macros.D2R + oe0.omega = 30.0 * macros.D2R + oe0.f = 40.0 * macros.D2R # spacecraft state at initial time r0_BN_N, v0_BN_N = orbitalMotion.elem2rv_parab(muBody, oe0) oe0 = orbitalMotion.rv2elem_parab(muBody, r0_BN_N, v0_BN_N) @@ -240,22 +251,22 @@ def lambertValidatorTestFunction(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_er # elliptic case M0 = orbitalMotion.E2M(orbitalMotion.f2E(oe0.f, ecc), ecc) n = np.sqrt(muBody / (oe0.a) ** 3) - M1 = M0 + n*(tm-t0) + M1 = M0 + n * (tm - t0) oe1.f = orbitalMotion.E2f(orbitalMotion.M2E(M1, ecc), ecc) elif ecc == 1.0: # parabolic case D0 = np.tan(oe0.f / 2) - M0 = D0 + 1/3*D0**3 + M0 = D0 + 1 / 3 * D0**3 n = np.sqrt(muBody / (2 * (-oe0.a) ** 3)) - M1 = M0 + n*(tm-t0) - A = 3./2.*M1 - B = (A + np.sqrt(A**2 + 1))**(1./3.) - oe1.f = 2. * np.arctan(B - 1. / B) + M1 = M0 + n * (tm - t0) + A = 3.0 / 2.0 * M1 + B = (A + np.sqrt(A**2 + 1)) ** (1.0 / 3.0) + oe1.f = 2.0 * np.arctan(B - 1.0 / B) else: # hyperbolic case N0 = orbitalMotion.H2N(orbitalMotion.f2H(oe0.f, ecc), ecc) n = np.sqrt(muBody / (-oe0.a) ** 3) - N1 = N0 + n*(tm-t0) + N1 = N0 + n * (tm - t0) oe1.f = orbitalMotion.H2f(orbitalMotion.N2H(N1, ecc), ecc) # spacecraft state at maneuver time, just before maneuver @@ -273,22 +284,22 @@ def lambertValidatorTestFunction(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_er # elliptic case M2 = orbitalMotion.E2M(orbitalMotion.f2E(oe2.f, ecc3), ecc3) n = np.sqrt(muBody / (oe2.a) ** 3) - M3 = M2 + n*(tf-tm) + M3 = M2 + n * (tf - tm) oe3.f = orbitalMotion.E2f(orbitalMotion.M2E(M3, ecc3), ecc3) elif ecc3 == 1.0: # parabolic case D2 = np.tan(oe2.f / 2) - M2 = D2 + 1/3*D2**3 + M2 = D2 + 1 / 3 * D2**3 n = np.sqrt(muBody / (2 * (-oe2.a) ** 3)) - M3 = M2 + n*(tf-tm) - A = 3./2.*M3 - B = (A + np.sqrt(A**2 + 1))**(1./3.) - oe3.f = 2. * np.arctan(B - 1. / B) + M3 = M2 + n * (tf - tm) + A = 3.0 / 2.0 * M3 + B = (A + np.sqrt(A**2 + 1)) ** (1.0 / 3.0) + oe3.f = 2.0 * np.arctan(B - 1.0 / B) else: # hyperbolic case N2 = orbitalMotion.H2N(orbitalMotion.f2H(oe2.f, ecc3), ecc3) n = np.sqrt(muBody / (-oe2.a) ** 3) - N3 = N2 + n*(tf-tm) + N3 = N2 + n * (tf - tm) oe3.f = orbitalMotion.H2f(orbitalMotion.N2H(N3, ecc3), ecc3) # spacecraft state at final time @@ -303,7 +314,7 @@ def lambertValidatorTestFunction(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_er module.setMinOrbitRadius(minOrbitRadius) module.setUncertaintyStates(errStates) module.setUncertaintyDV(errDV) - module.setDvConvergenceTolerance(np.linalg.norm(dv_N)/1000.) + module.setDvConvergenceTolerance(np.linalg.norm(dv_N) / 1000.0) unitTestSim.AddModelToTask(unitTaskName, module) # Configure input messages @@ -317,7 +328,7 @@ def lambertValidatorTestFunction(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_er lambertProblemInMsgData.solverMethod = solverMethod lambertProblemInMsgData.r1_N = r1_BN_N lambertProblemInMsgData.r2_N = r3_BN_N - lambertProblemInMsgData.transferTime = tf-tm + lambertProblemInMsgData.transferTime = tf - tm lambertProblemInMsgData.mu = muBody lambertProblemInMsgData.numRevolutions = 0 lambertProblemInMsg = messaging.LambertProblemMsg().write(lambertProblemInMsgData) @@ -326,19 +337,23 @@ def lambertValidatorTestFunction(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_er lambertSolutionInMsgData.v1_N = v1_BN_N + dv_N lambertSolutionInMsgData.v2_N = v3_BN_N lambertSolutionInMsgData.valid = 1 - lambertSolutionInMsgData.v1Sol2_N = np.array([0., 0., 0.]) - lambertSolutionInMsgData.v2Sol2_N = np.array([0., 0., 0.]) + lambertSolutionInMsgData.v1Sol2_N = np.array([0.0, 0.0, 0.0]) + lambertSolutionInMsgData.v2Sol2_N = np.array([0.0, 0.0, 0.0]) lambertSolutionInMsgData.validSol2 = 0 - lambertSolutionInMsg = messaging.LambertSolutionMsg().write(lambertSolutionInMsgData) + lambertSolutionInMsg = messaging.LambertSolutionMsg().write( + lambertSolutionInMsgData + ) lambertPerformanceInMsgData = messaging.LambertPerformanceMsgPayload() - lambertPerformanceInMsgData.x = 0. + lambertPerformanceInMsgData.x = 0.0 lambertPerformanceInMsgData.numIter = numIter lambertPerformanceInMsgData.errX = errX - lambertPerformanceInMsgData.xSol2 = 0. + lambertPerformanceInMsgData.xSol2 = 0.0 lambertPerformanceInMsgData.numIterSol2 = 0 - lambertPerformanceInMsgData.errXSol2 = 0. - lambertPerformanceInMsg = messaging.LambertPerformanceMsg().write(lambertPerformanceInMsgData) + lambertPerformanceInMsgData.errXSol2 = 0.0 + lambertPerformanceInMsg = messaging.LambertPerformanceMsg().write( + lambertPerformanceInMsgData + ) # subscribe input messages to module module.navTransInMsg.subscribeTo(navTransInMsg) @@ -370,32 +385,39 @@ def lambertValidatorTestFunction(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_er lambertSolutionInMsgData.v1_N = v1_BN_N + dv_N * scaler[i] lambertSolutionInMsgData.v2_N = v3_BN_N lambertSolutionInMsgData.valid = 1 - lambertSolutionInMsgData.v1Sol2_N = np.array([0., 0., 0.]) - lambertSolutionInMsgData.v2Sol2_N = np.array([0., 0., 0.]) + lambertSolutionInMsgData.v1Sol2_N = np.array([0.0, 0.0, 0.0]) + lambertSolutionInMsgData.v2Sol2_N = np.array([0.0, 0.0, 0.0]) lambertSolutionInMsgData.validSol2 = 0 - lambertSolutionInMsg.write(lambertSolutionInMsgData, unitTestSim.TotalSim.CurrentNanos) + lambertSolutionInMsg.write( + lambertSolutionInMsgData, unitTestSim.TotalSim.CurrentNanos + ) unitTestSim.ConfigureStopTime(i * testProcessRate) unitTestSim.ExecuteSimulation() - initialStates = getInitialStates(r1_BN_N, v1_BN_N, dv_N * scaler[i], errStates, errDV) - violationsDistanceTarget, violationsOrbitRadius = countViolations(initialStates, muBody, tm, tf, r3_BN_N, - maxDistanceTarget, minOrbitRadius) + initialStates = getInitialStates( + r1_BN_N, v1_BN_N, dv_N * scaler[i], errStates, errDV + ) + violationsDistanceTarget, violationsOrbitRadius = countViolations( + initialStates, muBody, tm, tf, r3_BN_N, maxDistanceTarget, minOrbitRadius + ) # true values - if violationsDistanceTarget == 0 and \ - violationsOrbitRadius == 0 and \ - failedDvSolutionConvergenceTrue[i] == 0 and \ - numIter < 6 and \ - errX < 1.e-8: + if ( + violationsDistanceTarget == 0 + and violationsOrbitRadius == 0 + and failedDvSolutionConvergenceTrue[i] == 0 + and numIter < 6 + and errX < 1.0e-8 + ): dvTrue[i, :] = dv_N * scaler[i] burnStartTimeTrue[i] = macros.sec2nano(tm) else: - dvTrue[i, :] = np.array([0., 0., 0.]) - burnStartTimeTrue[i] = macros.sec2nano(0.) + dvTrue[i, :] = np.array([0.0, 0.0, 0.0]) + burnStartTimeTrue[i] = macros.sec2nano(0.0) if numIter >= 6: failedNumIterationsLambertTrue[i] = 1 - if errX >= 1.e-8: + if errX >= 1.0e-8: failedXToleranceLambertTrue[i] = 1 if violationsDistanceTarget != 0: failedDistanceTargetConstraintTrue[i] = 1 @@ -409,75 +431,93 @@ def lambertValidatorTestFunction(show_plots, p1_dv, p2_tm, p3_tf, p4_iter, p5_er failedNumIterationsLambert = lambertValidatorOutMsgRec.failedNumIterationsLambert failedXToleranceLambert = lambertValidatorOutMsgRec.failedXToleranceLambert failedDvSolutionConvergence = lambertValidatorOutMsgRec.failedDvSolutionConvergence - failedDistanceTargetConstraint = lambertValidatorOutMsgRec.failedDistanceTargetConstraint + failedDistanceTargetConstraint = ( + lambertValidatorOutMsgRec.failedDistanceTargetConstraint + ) failedOrbitRadiusConstraint = lambertValidatorOutMsgRec.failedOrbitRadiusConstraint - dvTheory = lambertValidatorOutMsgRec.dv_N # Delta-V that would be returned if all tests were passed + dvTheory = ( + lambertValidatorOutMsgRec.dv_N + ) # Delta-V that would be returned if all tests were passed # make sure module output data is correct - paramsString = ' for DV={}, maneuver time={}, final time={}, iterations={}, errorsX={}, accuracy={}'.format( - str(p1_dv), - str(p2_tm), - str(p3_tf), - str(p4_iter), - str(p5_errX), - str(accuracy)) - - np.testing.assert_allclose(dv, - dvTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: dv_N,' + paramsString), - verbose=True) - - np.testing.assert_allclose(burnStartTime, - burnStartTimeTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: burnStartTime,' + paramsString), - verbose=True) - - np.testing.assert_allclose(failedNumIterationsLambert, - failedNumIterationsLambertTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: failedNumIterationsLambert,' + paramsString), - verbose=True) - - np.testing.assert_allclose(failedXToleranceLambert, - failedXToleranceLambertTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: failedXToleranceLambert,' + paramsString), - verbose=True) - - np.testing.assert_allclose(failedDvSolutionConvergence, - failedDvSolutionConvergenceTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: failedDvSolutionConvergence,' + paramsString), - verbose=True) - - np.testing.assert_allclose(failedDistanceTargetConstraint, - failedDistanceTargetConstraintTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: failedDistanceTargetConstraint,' + paramsString), - verbose=True) - - np.testing.assert_allclose(failedOrbitRadiusConstraint, - failedOrbitRadiusConstraintTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: failedOrbitRadiusConstraint,' + paramsString), - verbose=True) - - np.testing.assert_allclose(dvTheory, - dvTheoryTrue, - rtol=0, - atol=accuracy, - err_msg=('Variable: dvTheory,' + paramsString), - verbose=True) + paramsString = " for DV={}, maneuver time={}, final time={}, iterations={}, errorsX={}, accuracy={}".format( + str(p1_dv), str(p2_tm), str(p3_tf), str(p4_iter), str(p5_errX), str(accuracy) + ) + + np.testing.assert_allclose( + dv, + dvTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: dv_N," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + burnStartTime, + burnStartTimeTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: burnStartTime," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + failedNumIterationsLambert, + failedNumIterationsLambertTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: failedNumIterationsLambert," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + failedXToleranceLambert, + failedXToleranceLambertTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: failedXToleranceLambert," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + failedDvSolutionConvergence, + failedDvSolutionConvergenceTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: failedDvSolutionConvergence," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + failedDistanceTargetConstraint, + failedDistanceTargetConstraintTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: failedDistanceTargetConstraint," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + failedOrbitRadiusConstraint, + failedOrbitRadiusConstraintTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: failedOrbitRadiusConstraint," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + dvTheory, + dvTheoryTrue, + rtol=0, + atol=accuracy, + err_msg=("Variable: dvTheory," + paramsString), + verbose=True, + ) if __name__ == "__main__": - test_lambertValidator(False, DVs[1], time_maneuver[0], time_final[1], iterations[0], errorsX[0], 1e-4) + test_lambertValidator( + False, DVs[1], time_maneuver[0], time_final[1], iterations[0], errorsX[0], 1e-4 + ) diff --git a/src/fswAlgorithms/orbitControl/smallBodyWaypointFeedback/_UnitTest/test_smallBodyWaypointFeedback.py b/src/fswAlgorithms/orbitControl/smallBodyWaypointFeedback/_UnitTest/test_smallBodyWaypointFeedback.py index aae1b1c1da..63df985568 100644 --- a/src/fswAlgorithms/orbitControl/smallBodyWaypointFeedback/_UnitTest/test_smallBodyWaypointFeedback.py +++ b/src/fswAlgorithms/orbitControl/smallBodyWaypointFeedback/_UnitTest/test_smallBodyWaypointFeedback.py @@ -32,6 +32,7 @@ # ,(1, 3) # ]) + def test_smallBodyWaypointFeedback(show_plots): r""" **Validation Test Description** @@ -73,14 +74,20 @@ def smallBodyWaypointFeedbackTestFunction1(): module.ModelTag = "smallBodyWaypointFeedback1" unitTestSim.AddModelToTask(unitTaskName, module) - module.A_sc = 1. # Surface area of the spacecraft, m^2 + module.A_sc = 1.0 # 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.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.x1_ref = [-2000.0, 0.0, 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.]) + 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, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + ) # Set the orbital parameters of the asteroid oeAsteroid = orbitalMotion.ClassicElements() @@ -90,11 +97,17 @@ def smallBodyWaypointFeedbackTestFunction1(): 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) + r_ON_N, v_ON_N = orbitalMotion.elem2rv( + orbitalMotion.MU_SUN * (1000.0**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 + r_BO_N = np.array( + [-2000.0, 1500.0, 1000.0] + ) # Position of the spacecraft relative to the body + v_BO_N = np.array( + [0.0, 0.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) @@ -129,13 +142,18 @@ def smallBodyWaypointFeedbackTestFunction1(): unitTestSim.AddModelToTask(unitTaskName, forceOutMsgRec) unitTestSim.InitializeSimulation() - unitTestSim.ConfigureStopTime(macros.sec2nano(0.)) + unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) unitTestSim.ExecuteSimulation() if np.linalg.norm(forceOutMsgRec.forceRequestBody) <= 1: testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed " - + "force output" + " unit test") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed " + + "force output" + + " unit test" + ) if testFailCount == 0: print("PASSED: " + module.ModelTag) @@ -144,6 +162,7 @@ def smallBodyWaypointFeedbackTestFunction1(): return [testFailCount, "".join(testMessages)] + def smallBodyWaypointFeedbackTestFunction2(): """This test checks that the force output is near zero when at the waypoint""" testFailCount = 0 @@ -161,14 +180,20 @@ def smallBodyWaypointFeedbackTestFunction2(): module.ModelTag = "smallBodyWaypointFeedback2" unitTestSim.AddModelToTask(unitTaskName, module) - module.A_sc = 1. # Surface area of the spacecraft, m^2 + module.A_sc = 1.0 # 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.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.x1_ref = [-2000.0, 0.0, 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.]) + 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, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + ) # Set the orbital parameters of the asteroid oeAsteroid = orbitalMotion.ClassicElements() @@ -178,11 +203,17 @@ def smallBodyWaypointFeedbackTestFunction2(): 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) + r_ON_N, v_ON_N = orbitalMotion.elem2rv( + orbitalMotion.MU_SUN * (1000.0**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_BO_H = np.array( + [-2000.0, 0.0, 0.0] + ) # Position of the spacecraft relative to the body + v_BO_H = np.array( + [0.0, 0.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) @@ -215,13 +246,18 @@ def smallBodyWaypointFeedbackTestFunction2(): unitTestSim.AddModelToTask(unitTaskName, forceOutMsgRec) unitTestSim.InitializeSimulation() - unitTestSim.ConfigureStopTime(macros.sec2nano(0.)) + unitTestSim.ConfigureStopTime(macros.sec2nano(0.0)) unitTestSim.ExecuteSimulation() if np.linalg.norm(forceOutMsgRec.forceRequestBody) >= 1e-8: testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed " - + "force output" + " unit test") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed " + + "force output" + + " unit test" + ) if testFailCount == 0: print("PASSED: " + module.ModelTag) diff --git a/src/fswAlgorithms/rwConfigData/_UnitTest/test_rwConfigData.py b/src/fswAlgorithms/rwConfigData/_UnitTest/test_rwConfigData.py index fc6c1b19f1..af314e0981 100644 --- a/src/fswAlgorithms/rwConfigData/_UnitTest/test_rwConfigData.py +++ b/src/fswAlgorithms/rwConfigData/_UnitTest/test_rwConfigData.py @@ -9,7 +9,9 @@ from Basilisk.fswAlgorithms import rwConfigData from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions def test_rwConfigData(): @@ -17,8 +19,9 @@ def test_rwConfigData(): [testResults, testMessage] = rwConfigDataTestFunction() assert testResults < 1, testMessage + def rwConfigDataTestFunction(): - """ Test the rwConfigData module """ + """Test the rwConfigData module""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -34,7 +37,9 @@ def rwConfigDataTestFunction(): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process # Construct the cssComm module module = rwConfigData.rwConfigData() @@ -51,7 +56,9 @@ def rwConfigDataTestFunction(): rwConfigElementList = list() for rw in range(numRW): rwConfigElementMsg = messaging.RWConfigElementMsgPayload() - rwConfigElementMsg.gsHat_B = gsHat_initial[rw] # Spin axis unit vector of the wheel in structure # [1, 0, 0] + rwConfigElementMsg.gsHat_B = gsHat_initial[ + rw + ] # Spin axis unit vector of the wheel in structure # [1, 0, 0] rwConfigElementMsg.Js = js_initial[rw] # Spin axis inertia of wheel [kgm2] rwConfigElementMsg.uMax = uMax_initial[rw] # maximum RW motor torque [Nm] @@ -83,24 +90,43 @@ def rwConfigDataTestFunction(): # Get the output from this simulation JsListLog = dataLog.JsList[:, :numRW] uMaxLog = dataLog.uMax[:, :numRW] - GsMatrix_B_Log = dataLog.GsMatrix_B[:, :(3*numRW)] + GsMatrix_B_Log = dataLog.GsMatrix_B[:, : (3 * numRW)] accuracy = 1e-6 # At each timestep, make sure the vehicleConfig values haven't changed from the initial values - testFailCount, testMessages = unitTestSupport.compareArrayND([js_initial]*2, JsListLog, accuracy, - "rwConfigData JsList", - 3, testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArrayND([uMax_initial]*2, uMaxLog, accuracy, - "rwConfigData uMax", - 3, testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArrayND([gsHat_initial.flatten()]*2, GsMatrix_B_Log, accuracy, - "rwConfigData GsMatrix_B", - 3*numRW, testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + [js_initial] * 2, + JsListLog, + accuracy, + "rwConfigData JsList", + 3, + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArrayND( + [uMax_initial] * 2, + uMaxLog, + accuracy, + "rwConfigData uMax", + 3, + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArrayND( + [gsHat_initial.flatten()] * 2, + GsMatrix_B_Log, + accuracy, + "rwConfigData GsMatrix_B", + 3 * numRW, + testFailCount, + testMessages, + ) if testFailCount == 0: print("PASSED: " + module.ModelTag) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + -if __name__ == '__main__': +if __name__ == "__main__": test_rwConfigData() diff --git a/src/fswAlgorithms/sensorInterfaces/CSSSensorData/_UnitTest/test_cssComm.py b/src/fswAlgorithms/sensorInterfaces/CSSSensorData/_UnitTest/test_cssComm.py index 7f452da9a2..92690d367f 100644 --- a/src/fswAlgorithms/sensorInterfaces/CSSSensorData/_UnitTest/test_cssComm.py +++ b/src/fswAlgorithms/sensorInterfaces/CSSSensorData/_UnitTest/test_cssComm.py @@ -12,27 +12,37 @@ from Basilisk.fswAlgorithms import cssComm from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -@pytest.mark.parametrize("numSensors, sensorData", [ - (4, [-100e-6, 200e-6, 600e-6, 300e-6, 200e-6]), # Five data inputs used despite four sensors to ensure all reset conditions are tested. - pytest.param(0, [-100e-6, 200e-6, 600e-6, 300e-6]), # Zero sensor number to ensure all reset conditions are tested - pytest.param(messaging.MAX_NUM_CSS_SENSORS+1, [200e-6]*messaging.MAX_NUM_CSS_SENSORS) # Indicate more sensor devices than is allowed. The output should be clipped to the allowed length -]) - +@pytest.mark.parametrize( + "numSensors, sensorData", + [ + ( + 4, + [-100e-6, 200e-6, 600e-6, 300e-6, 200e-6], + ), # Five data inputs used despite four sensors to ensure all reset conditions are tested. + pytest.param( + 0, [-100e-6, 200e-6, 600e-6, 300e-6] + ), # Zero sensor number to ensure all reset conditions are tested + pytest.param( + messaging.MAX_NUM_CSS_SENSORS + 1, [200e-6] * messaging.MAX_NUM_CSS_SENSORS + ), # Indicate more sensor devices than is allowed. The output should be clipped to the allowed length + ], +) def test_cssComm(numSensors, sensorData): """Module Unit Test""" [testResults, testMessage] = cssCommTestFunction(numSensors, sensorData) assert testResults < 1, testMessage - def cssCommTestFunction(numSensors, sensorData): - """ Test the cssComm module """ + """Test the cssComm module""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages unitTaskName = "unitTask" # arbitrary name (don't change) @@ -47,7 +57,9 @@ def cssCommTestFunction(numSensors, sensorData): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process # Construct the cssComm module module = cssComm.cssComm() @@ -55,13 +67,22 @@ def cssCommTestFunction(numSensors, sensorData): module.numSensors = numSensors module.maxSensorValue = 500e-6 - ChebyList = [-1.734963346951471e+06, 3.294117146099591e+06, - -2.816333294617512e+06, 2.163709942144332e+06, - -1.488025993860025e+06, 9.107359382775769e+05, - -4.919712500291216e+05, 2.318436583511218e+05, - -9.376105045529010e+04, 3.177536873430168e+04, - -8.704033370738143e+03, 1.816188108176300e+03, - -2.581556805090373e+02, 1.888418924282780e+01] + ChebyList = [ + -1.734963346951471e06, + 3.294117146099591e06, + -2.816333294617512e06, + 2.163709942144332e06, + -1.488025993860025e06, + 9.107359382775769e05, + -4.919712500291216e05, + 2.318436583511218e05, + -9.376105045529010e04, + 3.177536873430168e04, + -8.704033370738143e03, + 1.816188108176300e03, + -2.581556805090373e02, + 1.888418924282780e01, + ] module.chebyCount = len(ChebyList) module.kellyCheby = ChebyList @@ -92,39 +113,43 @@ def cssCommTestFunction(numSensors, sensorData): MAX_NUM_CSS_SENSORS = messaging.MAX_NUM_CSS_SENSORS outputData = dataLog.CosValue - trueCssList= [0]*MAX_NUM_CSS_SENSORS - if numSensors==4: + trueCssList = [0] * MAX_NUM_CSS_SENSORS + if numSensors == 4: trueCssList[0:4] = [0.0, 0.45791653042, 1.0, 0.615444781018] - if numSensors==MAX_NUM_CSS_SENSORS+1: - trueCssList = [0.45791653042]*32 + if numSensors == MAX_NUM_CSS_SENSORS + 1: + trueCssList = [0.45791653042] * 32 # Create the true array - trueCss = [ - trueCssList, - trueCssList - ] + trueCss = [trueCssList, trueCssList] accuracy = 1e-6 - testFailCount, testMessages = unitTestSupport.compareArrayND(trueCss, outputData, accuracy, "cosValues", - MAX_NUM_CSS_SENSORS, testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + trueCss, + outputData, + accuracy, + "cosValues", + MAX_NUM_CSS_SENSORS, + testFailCount, + testMessages, + ) # print out success message if no error were found - unitTestSupport.writeTeXSnippet('toleranceValue', str(accuracy), path) + unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) - snippentName = "passFail_"+str(numSensors) + snippentName = "passFail_" + str(numSensors) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] -if __name__ == '__main__': +if __name__ == "__main__": test_cssComm(4, [-100e-6, 200e-6, 600e-6, 300e-6, 200e-6]) diff --git a/src/fswAlgorithms/sensorInterfaces/TAMSensorData/_UnitTest/test_tamComm.py b/src/fswAlgorithms/sensorInterfaces/TAMSensorData/_UnitTest/test_tamComm.py index 60a3587c12..190e418e02 100644 --- a/src/fswAlgorithms/sensorInterfaces/TAMSensorData/_UnitTest/test_tamComm.py +++ b/src/fswAlgorithms/sensorInterfaces/TAMSensorData/_UnitTest/test_tamComm.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2019, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -30,7 +29,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation @@ -46,6 +45,7 @@ # @pytest.mark.xfail(conditionstring) # Provide a unique test method name, starting with 'test_'. + # update "module" in this function name to reflect the module name def test_module(show_plots): """ @@ -68,18 +68,19 @@ def test_module(show_plots): [testResults, testMessage] = tamCommTestFunction(show_plots) assert testResults < 1, testMessage + def tamCommTestFunction(show_plots): - """ Test the tamComm module """ - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + """Test the tamComm module""" + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -112,7 +113,7 @@ def tamCommTestFunction(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -123,17 +124,18 @@ def tamCommTestFunction(show_plots): # This pulls the actual data log from the simulation run. moduleOutput = dataLog.tam_B # set the filtered output truth states - trueVector = [ - [-1e-5, 2e-6, -3e-5], - [-1e-5, 2e-6, -3e-5], - [-1e-5, 2e-6, -3e-5] - ] + trueVector = [[-1e-5, 2e-6, -3e-5], [-1e-5, 2e-6, -3e-5], [-1e-5, 2e-6, -3e-5]] for i in range(len(trueVector)): trueVector[i] = np.dot(dcm3, trueVector[i]) - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "TAM Output Vector", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + moduleOutput, + accuracy, + "TAM Output Vector", + testFailCount, + testMessages, + ) # print out success message if no error were found if testFailCount == 0: @@ -144,7 +146,8 @@ def tamCommTestFunction(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + # # This statement below ensures that the unitTestScript can be run as a diff --git a/src/fswAlgorithms/sensorInterfaces/rateMsgConverter/_UnitTest/test_rateMsgConverter.py b/src/fswAlgorithms/sensorInterfaces/rateMsgConverter/_UnitTest/test_rateMsgConverter.py index 9199808abe..8e2f99780f 100755 --- a/src/fswAlgorithms/sensorInterfaces/rateMsgConverter/_UnitTest/test_rateMsgConverter.py +++ b/src/fswAlgorithms/sensorInterfaces/rateMsgConverter/_UnitTest/test_rateMsgConverter.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -28,7 +27,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation @@ -54,16 +53,16 @@ def test_module(show_plots): def rateMsgConvertFunction(show_plots): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -92,7 +91,7 @@ def rateMsgConvertFunction(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -104,38 +103,36 @@ def rateMsgConvertFunction(show_plots): # This pulls the actual data log from the simulation run. moduleOutput = dataLog.omega_BN_B # set the filtered output truth states - trueVector = [ - [-0.1, 0.2, -0.3], - [-0.1, 0.2, -0.3], - [-0.1, 0.2, -0.3] - ] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "Output Vector", - testFailCount, testMessages) + trueVector = [[-0.1, 0.2, -0.3], [-0.1, 0.2, -0.3], [-0.1, 0.2, -0.3]] + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, moduleOutput, accuracy, "Output Vector", testFailCount, testMessages + ) moduleOutput = dataLog.sigma_BN # set the filtered output truth states - trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "Output MRP Vector", - testFailCount, testMessages) + trueVector = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + moduleOutput, + accuracy, + "Output MRP Vector", + testFailCount, + testMessages, + ) moduleOutput = dataLog.vehSunPntBdy # set the filtered output truth states - trueVector = [ - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] - ] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "Output sun heading Vector", - testFailCount, testMessages) + trueVector = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + moduleOutput, + accuracy, + "Output sun heading Vector", + testFailCount, + testMessages, + ) # print out success message if no error were found if testFailCount == 0: @@ -145,7 +142,7 @@ def rateMsgConvertFunction(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # diff --git a/src/fswAlgorithms/sensorInterfaces/scanningInstrumentController/_UnitTest/test_scanningInstrumentController.py b/src/fswAlgorithms/sensorInterfaces/scanningInstrumentController/_UnitTest/test_scanningInstrumentController.py index 26d4582f3e..33a4bca006 100644 --- a/src/fswAlgorithms/sensorInterfaces/scanningInstrumentController/_UnitTest/test_scanningInstrumentController.py +++ b/src/fswAlgorithms/sensorInterfaces/scanningInstrumentController/_UnitTest/test_scanningInstrumentController.py @@ -29,40 +29,52 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) -#Test cases: -#1: attitude compliant, rate disabled, device status written, controller status of 1 -#2: attitude noncompliant, rate disabled, device status written, controller status of 1 -#3: attitude compliant, rate disabled, device status not written, no controller status -#4: attitude compliant, rate disabled, device status of 1, controller status of 0 -#5: attitude compliant, rate disabled, device status of 0, controller status of 1 -#6: attitude compliant, rate disabled, device status not written, controller status of 1 -#7: attitude compliant, rate disabled, rate noncompliant, device status not written, no +# Test cases: +# 1: attitude compliant, rate disabled, device status written, controller status of 1 +# 2: attitude noncompliant, rate disabled, device status written, controller status of 1 +# 3: attitude compliant, rate disabled, device status not written, no controller status +# 4: attitude compliant, rate disabled, device status of 1, controller status of 0 +# 5: attitude compliant, rate disabled, device status of 0, controller status of 1 +# 6: attitude compliant, rate disabled, device status not written, controller status of 1 +# 7: attitude compliant, rate disabled, rate noncompliant, device status not written, no # controller status -#8: attitude compliant, rate enabled, rate noncompliant, device status not written, no +# 8: attitude compliant, rate enabled, rate noncompliant, device status not written, no # controller status -#9: attitude compliant, rate enabled, rate compliant, device status not written, no +# 9: attitude compliant, rate enabled, rate compliant, device status not written, no # controller status -#10: attitude noncompliant, rate enabled, rate compliant -tests = [(0.1, 0.01, 0, 0, 0, 1, 1, [1, 1, 1]), - (0.1, 0.2, 0, 0, 0, 1, 1, [0, 0, 0]), - (0.1, 0.01, 0, 0, 0, None, None, [0, 0, 0]), - (0.1, 0.01, 0, 0, 0, 1, 0, [1, 1, 1]), - (0.1, 0.01, 0, 0, 0, 0, 1, [0, 0, 0]), - (0.1, 0.01, 0, 0, 0, None, 1, [1, 1, 1]), - (0.1, 0.01, 0, 0.01, 0.1, None, 1, [1, 1, 1]), - (0.1, 0.01, 1, 0.01, 0.1, None, 1, [0, 0, 0]), - (0.1, 0.01, 1, 0.01, 0.001, 1, None, [1, 1, 1]), - (0.1, 0.2, 1, 0.01, 0.001, 1, None, [0, 0, 0]) - ] - -@pytest.mark.parametrize('att_limit, att_mag, use_rate_limit,rate_limit,omega_mag' + - ',deviceStatus,controlStatus,expected_result', tests) -def test_scanningInstrumentController(att_limit, att_mag, use_rate_limit, rate_limit, - omega_mag, deviceStatus, controlStatus, - expected_result): +# 10: attitude noncompliant, rate enabled, rate compliant +tests = [ + (0.1, 0.01, 0, 0, 0, 1, 1, [1, 1, 1]), + (0.1, 0.2, 0, 0, 0, 1, 1, [0, 0, 0]), + (0.1, 0.01, 0, 0, 0, None, None, [0, 0, 0]), + (0.1, 0.01, 0, 0, 0, 1, 0, [1, 1, 1]), + (0.1, 0.01, 0, 0, 0, 0, 1, [0, 0, 0]), + (0.1, 0.01, 0, 0, 0, None, 1, [1, 1, 1]), + (0.1, 0.01, 0, 0.01, 0.1, None, 1, [1, 1, 1]), + (0.1, 0.01, 1, 0.01, 0.1, None, 1, [0, 0, 0]), + (0.1, 0.01, 1, 0.01, 0.001, 1, None, [1, 1, 1]), + (0.1, 0.2, 1, 0.01, 0.001, 1, None, [0, 0, 0]), +] + + +@pytest.mark.parametrize( + "att_limit, att_mag, use_rate_limit,rate_limit,omega_mag" + + ",deviceStatus,controlStatus,expected_result", + tests, +) +def test_scanningInstrumentController( + att_limit, + att_mag, + use_rate_limit, + rate_limit, + omega_mag, + deviceStatus, + controlStatus, + expected_result, +): r""" **Validation Test Description** @@ -87,16 +99,30 @@ def test_scanningInstrumentController(att_limit, att_mag, use_rate_limit, rate_l result. """ - module_results = scanningInstrumentControllerTestFunction(att_limit, - att_mag, use_rate_limit, rate_limit, omega_mag, - deviceStatus, controlStatus, expected_result) + module_results = scanningInstrumentControllerTestFunction( + att_limit, + att_mag, + use_rate_limit, + rate_limit, + omega_mag, + deviceStatus, + controlStatus, + expected_result, + ) np.testing.assert_array_equal(module_results, expected_result) -def scanningInstrumentControllerTestFunction(att_limit = 0.1, att_mag = 0.1, - use_rate_limit=1, rate_limit=0.01, omega_mag=0.001, - deviceStatus=None, controlStatus=None, expected_result=None): +def scanningInstrumentControllerTestFunction( + att_limit=0.1, + att_mag=0.1, + use_rate_limit=1, + rate_limit=0.01, + omega_mag=0.001, + deviceStatus=None, + controlStatus=None, + expected_result=None, +): """Test method""" unitTaskName = "unitTask" unitProcessName = "TestProcess" @@ -112,14 +138,14 @@ def scanningInstrumentControllerTestFunction(att_limit = 0.1, att_mag = 0.1, module.ModelTag = "scanningInstrumentControllerTag" unitTestSim.AddModelToTask(unitTaskName, module) - #Initializing the test module configuration data - module.useRateTolerance = use_rate_limit # rate limit enabled - module.rateErrTolerance = rate_limit # rate limit - module.attErrTolerance = att_limit # attitude error tolerance + # Initializing the test module configuration data + module.useRateTolerance = use_rate_limit # rate limit enabled + module.rateErrTolerance = rate_limit # rate limit + module.attErrTolerance = att_limit # attitude error tolerance # Configure blank module input messages accessInMsgData = messaging.AccessMsgPayload() - accessInMsgData.hasAccess = 1 # set access to true + accessInMsgData.hasAccess = 1 # set access to true accessInMsg = messaging.AccessMsg().write(accessInMsgData) attGuidInMsgData = messaging.AttGuidMsgPayload() diff --git a/src/fswAlgorithms/sensorInterfaces/simpleInstrumentController/_UnitTest/test_simpleInstrumentController.py b/src/fswAlgorithms/sensorInterfaces/simpleInstrumentController/_UnitTest/test_simpleInstrumentController.py index ec96e5cfca..c9c9c4924c 100644 --- a/src/fswAlgorithms/sensorInterfaces/simpleInstrumentController/_UnitTest/test_simpleInstrumentController.py +++ b/src/fswAlgorithms/sensorInterfaces/simpleInstrumentController/_UnitTest/test_simpleInstrumentController.py @@ -29,7 +29,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation @@ -48,16 +48,79 @@ # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ -tests = [(0, 0, 0, None, None, [1,0,0,1,0]), # rate disabled, device status not written, no controller status - (0, 0.01, 0.1, None, None, [1,0,0,1,0]), # rate disabled, rate noncompliant, device status not written, no controller status - (1, 0.01, 0.001, None, None, [1,0,0,1,0]), # rate enabled, rate compliant, device status not written, no controller status - (1, 0.01, 0.1, None, None, [0, 0, 0, 0, 0]), # rate enabled, rate noncompliant, device status not written, no controller status - (0, 0, 0, 1, 0, [1,0,0,1,0]), # rate disabled, device status of 1, controller status of 0 - (0, 0, 0, 0, 1, [0, 0, 0, 0, 0]), # rate disabled, device status of 0, controller status of 1 - (0, 0, 0, None, 1, [1,0,0,1,0]) # rate disabled, device status not written, controller status of 1 - ] -@pytest.mark.parametrize('use_rate_limit,rate_limit,omega_mag,deviceStatus,controlStatus,expected_result', tests) -def test_simple_instrument_controller(show_plots, use_rate_limit, rate_limit, omega_mag, deviceStatus, controlStatus, expected_result): +tests = [ + ( + 0, + 0, + 0, + None, + None, + [1, 0, 0, 1, 0], + ), # rate disabled, device status not written, no controller status + ( + 0, + 0.01, + 0.1, + None, + None, + [1, 0, 0, 1, 0], + ), # rate disabled, rate noncompliant, device status not written, no controller status + ( + 1, + 0.01, + 0.001, + None, + None, + [1, 0, 0, 1, 0], + ), # rate enabled, rate compliant, device status not written, no controller status + ( + 1, + 0.01, + 0.1, + None, + None, + [0, 0, 0, 0, 0], + ), # rate enabled, rate noncompliant, device status not written, no controller status + ( + 0, + 0, + 0, + 1, + 0, + [1, 0, 0, 1, 0], + ), # rate disabled, device status of 1, controller status of 0 + ( + 0, + 0, + 0, + 0, + 1, + [0, 0, 0, 0, 0], + ), # rate disabled, device status of 0, controller status of 1 + ( + 0, + 0, + 0, + None, + 1, + [1, 0, 0, 1, 0], + ), # rate disabled, device status not written, controller status of 1 +] + + +@pytest.mark.parametrize( + "use_rate_limit,rate_limit,omega_mag,deviceStatus,controlStatus,expected_result", + tests, +) +def test_simple_instrument_controller( + show_plots, + use_rate_limit, + rate_limit, + omega_mag, + deviceStatus, + controlStatus, + expected_result, +): r""" **Validation Test Description** @@ -79,14 +142,30 @@ def test_simple_instrument_controller(show_plots, use_rate_limit, rate_limit, om """ # each test method requires a single assert method to be called # pass on the testPlotFixture so that the main test function may set the DataStore attributes - [testResults, testMessage] = simpleInstrumentControllerTestFunction(show_plots, use_rate_limit, rate_limit, omega_mag, deviceStatus, controlStatus, expected_result) + [testResults, testMessage] = simpleInstrumentControllerTestFunction( + show_plots, + use_rate_limit, + rate_limit, + omega_mag, + deviceStatus, + controlStatus, + expected_result, + ) assert testResults < 1, testMessage -def simpleInstrumentControllerTestFunction(show_plots, use_rate_limit=1, rate_limit=0.01, omega_mag=0.001, deviceStatus=None, controlStatus=None, expected_result=None): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages +def simpleInstrumentControllerTestFunction( + show_plots, + use_rate_limit=1, + rate_limit=0.01, + omega_mag=0.001, + deviceStatus=None, + controlStatus=None, + expected_result=None, +): + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages unitTaskName = "unitTask" unitProcessName = "TestProcess" bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) @@ -101,15 +180,15 @@ def simpleInstrumentControllerTestFunction(show_plots, use_rate_limit=1, rate_li # Construct algorithm and associated C++ container module = simpleInstrumentController.simpleInstrumentController() - module.ModelTag = "simpleInstrumentController" # update python name of test module + module.ModelTag = "simpleInstrumentController" # update python name of test module # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - module.attErrTolerance = 0.1 # set the attitude error tolerance - module.rateErrTolerance = rate_limit # set the attitude rate error tolerance - module.useRateTolerance = use_rate_limit # enable attitude rate error tolerance + module.attErrTolerance = 0.1 # set the attitude error tolerance + module.rateErrTolerance = rate_limit # set the attitude rate error tolerance + module.useRateTolerance = use_rate_limit # enable attitude rate error tolerance # Create and write the ground location access message inputAccessMsgData = messaging.AccessMsgPayload() @@ -126,7 +205,9 @@ def simpleInstrumentControllerTestFunction(show_plots, use_rate_limit=1, rate_li if deviceStatus is not None: inputDeviceStatusMsgData = messaging.DeviceStatusMsgPayload() inputDeviceStatusMsgData.deviceStatus = deviceStatus - inputDeviceStatusMsg = messaging.DeviceStatusMsg().write(inputDeviceStatusMsgData) + inputDeviceStatusMsg = messaging.DeviceStatusMsg().write( + inputDeviceStatusMsgData + ) module.deviceStatusInMsg.subscribeTo(inputDeviceStatusMsg) # Set the controllerStatus variable @@ -148,38 +229,47 @@ def simpleInstrumentControllerTestFunction(show_plots, use_rate_limit=1, rate_li # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() # run the module again for an additional second - unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() # Now change the imaged variable back to 0 and run again for another image module.imaged = 0 - unitTestSim.ConfigureStopTime(macros.sec2nano(4.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(4.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - if not unitTestSupport.isArrayEqual(dataLog.deviceCmd, expected_result, len(expected_result), 1e-12): + if not unitTestSupport.isArrayEqual( + dataLog.deviceCmd, expected_result, len(expected_result), 1e-12 + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed dataVector" + " unit test at t=" + str(dataLog.times()[0]*macros.NANO2SEC) + "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed dataVector" + + " unit test at t=" + + str(dataLog.times()[0] * macros.NANO2SEC) + + "sec\n" + ) # Plots plt.close("all") # close all prior figures so we start with a clean slate plt.figure(1) plt.plot(dataLog.times() * macros.NANO2SEC, dataLog.deviceCmd) - plt.xlabel('Time [s]') - plt.ylabel('Device Status') - plt.suptitle('Device Status Over Time') + plt.xlabel("Time [s]") + plt.ylabel("Device Status") + plt.suptitle("Device Status Over Time") if show_plots: plt.show() # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -188,5 +278,5 @@ def simpleInstrumentControllerTestFunction(show_plots, use_rate_limit=1, rate_li # if __name__ == "__main__": simpleInstrumentControllerTestFunction( - True # show_plots + True # show_plots ) diff --git a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/_UnitTest/test_smallBodyNavEKF.py b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/_UnitTest/test_smallBodyNavEKF.py index 7f6a602c16..77d31e5360 100644 --- a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/_UnitTest/test_smallBodyNavEKF.py +++ b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/_UnitTest/test_smallBodyNavEKF.py @@ -60,23 +60,25 @@ def smallBodyNavEKFTestFunction(show_plots): unitTestSim.AddModelToTask(unitTaskName, module) # Set the filter parameters (sc area, mass, gravitational constants, etc.) - module.A_sc = 1. # Surface area of the spacecraft, m^2 - module.M_sc = 100. # Mass of the spacecraft, kg + module.A_sc = 1.0 # Surface area of the spacecraft, m^2 + module.M_sc = 100.0 # Mass of the spacecraft, kg module.mu_ast = 5.2 # Gravitational constant of the asteroid - module.Q = (0.1*np.identity(12)).tolist() # Process Noise - module.R = (0.1*np.identity(12)).tolist() # Measurement Noise + module.Q = (0.1 * np.identity(12)).tolist() # Process Noise + module.R = (0.1 * np.identity(12)).tolist() # Measurement Noise - bennu_radius = 1.355887692*orbitalMotion.AU*1000.0 # meters - bennu_velocity = np.sqrt(orbitalMotion.MU_SUN*(1000.**3)/bennu_radius) # m/s, assumes circular orbit + bennu_radius = 1.355887692 * orbitalMotion.AU * 1000.0 # meters + bennu_velocity = np.sqrt( + orbitalMotion.MU_SUN * (1000.0**3) / bennu_radius + ) # m/s, assumes circular orbit - x_0 = [2010., 1510., 1010., 0., 2., 0., 0.14, 0., 0., 0., 0., 0.] + x_0 = [2010.0, 1510.0, 1010.0, 0.0, 2.0, 0.0, 0.14, 0.0, 0.0, 0.0, 0.0, 0.0] module.x_hat_k = x_0 - module.P_k = (0.1*np.identity(12)).tolist() + module.P_k = (0.1 * np.identity(12)).tolist() # Configure blank module input messages navTransInMsgData = messaging.NavTransMsgPayload() - navTransInMsgData.r_BN_N = [bennu_radius + 1000., 1000., 1000.] - navTransInMsgData.v_BN_N = [0., bennu_velocity + 1., 0.] + navTransInMsgData.r_BN_N = [bennu_radius + 1000.0, 1000.0, 1000.0] + navTransInMsgData.v_BN_N = [0.0, bennu_velocity + 1.0, 0.0] navTransInMsg = messaging.NavTransMsg().write(navTransInMsgData) navAttInMsgData = messaging.NavAttMsgPayload() @@ -85,8 +87,8 @@ def smallBodyNavEKFTestFunction(show_plots): navAttInMsg = messaging.NavAttMsg().write(navAttInMsgData) asteroidEphemerisInMsgData = messaging.EphemerisMsgPayload() - asteroidEphemerisInMsgData.r_BdyZero_N = [bennu_radius, 0., 0.] - asteroidEphemerisInMsgData.v_BdyZero_N = [0., bennu_velocity, 0.] + asteroidEphemerisInMsgData.r_BdyZero_N = [bennu_radius, 0.0, 0.0] + asteroidEphemerisInMsgData.v_BdyZero_N = [0.0, bennu_velocity, 0.0] asteroidEphemerisInMsgData.sigma_BN = [0.1, 0.0, 0.0] asteroidEphemerisInMsgData.omega_BN_B = [0.0, 0.0, 0.0] asteroidEphemerisInMsg = messaging.EphemerisMsg().write(asteroidEphemerisInMsgData) @@ -119,61 +121,81 @@ def smallBodyNavEKFTestFunction(show_plots): unitTestSim.AddModelToTask(unitTaskName, asteroidEphemerisOutMsgRec) unitTestSim.InitializeSimulation() - unitTestSim.ConfigureStopTime(macros.sec2nano(10.)) + unitTestSim.ConfigureStopTime(macros.sec2nano(10.0)) unitTestSim.ExecuteSimulation() x_hat = smallBodyNavOutMsgRec.state x_hat_c_wrapped = smallBodyNavOutMsgRecC.state - true_x_hat = np.array([[1.33666664e+03, 1.18333330e+03, 1.00333330e+03, -4.77594532e-06, - 1.33332617, -6.10976335e-06, 1.13333333e-01, 0.00000000, - 0.00000000, 0.00000000, 0.00000000, 0.00000000]]) + true_x_hat = np.array( + [ + [ + 1.33666664e03, + 1.18333330e03, + 1.00333330e03, + -4.77594532e-06, + 1.33332617, + -6.10976335e-06, + 1.13333333e-01, + 0.00000000, + 0.00000000, + 0.00000000, + 0.00000000, + 0.00000000, + ] + ] + ) testFailCount, testMessages = unitTestSupport.compareArray( - true_x_hat, np.array([x_hat[-1,:]]), 0.1, "x_hat", - testFailCount, testMessages) + true_x_hat, np.array([x_hat[-1, :]]), 0.1, "x_hat", testFailCount, testMessages + ) testFailCount, testMessages = unitTestSupport.compareArray( - true_x_hat, np.array([x_hat_c_wrapped[-1,:]]), 0.1, "x_hat_c_wrapped", - testFailCount, testMessages) + true_x_hat, + np.array([x_hat_c_wrapped[-1, :]]), + 0.1, + "x_hat_c_wrapped", + testFailCount, + testMessages, + ) - plt.close('all') + plt.close("all") - plt.figure(1, figsize=(7, 5), dpi=80, facecolor='w', edgecolor='k') + plt.figure(1, figsize=(7, 5), dpi=80, facecolor="w", edgecolor="k") plt.ticklabel_format(useOffset=False) - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,0], label='x-pos') - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,1], label='y-pos') - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,2], label='z-pos') - plt.legend(loc='upper left') - plt.xlabel('Time (s)') - plt.ylabel('r_BO_O (m)') - plt.title('Estimated Relative Spacecraft Position') - - plt.figure(2, figsize=(7, 5), dpi=80, facecolor='w', edgecolor='k') - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,3], label='x-vel') - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,4], label='y-vel') - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,5], label='z-vel') - plt.legend(loc='upper left') - plt.xlabel('Time (s)') - plt.ylabel('v_BO_O (m/s)') - plt.title('Estimated Spacecraft Velocity') - - plt.figure(5, figsize=(7, 5), dpi=80, facecolor='w', edgecolor='k') - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,6], label='s1') - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,7], label='s2') - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,8], label='s3') - plt.legend(loc='upper left') - plt.xlabel('Time (s)') - plt.ylabel('sigma_AN (rad)') - plt.title('Estimated Asteroid Attitude') - - plt.figure(6, figsize=(7, 5), dpi=80, facecolor='w', edgecolor='k') - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,9], label='omega1') - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,10], label='omega2') - plt.plot(navTransOutMsgRec.times() * 1.0E-9, x_hat[:,11], label='omega3') - plt.legend(loc='upper left') - plt.xlabel('Time (s)') - plt.ylabel('omega_AN_A (rad/s)') - plt.title('Estimated Asteroid Rate') + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 0], label="x-pos") + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 1], label="y-pos") + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 2], label="z-pos") + plt.legend(loc="upper left") + plt.xlabel("Time (s)") + plt.ylabel("r_BO_O (m)") + plt.title("Estimated Relative Spacecraft Position") + + plt.figure(2, figsize=(7, 5), dpi=80, facecolor="w", edgecolor="k") + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 3], label="x-vel") + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 4], label="y-vel") + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 5], label="z-vel") + plt.legend(loc="upper left") + plt.xlabel("Time (s)") + plt.ylabel("v_BO_O (m/s)") + plt.title("Estimated Spacecraft Velocity") + + plt.figure(5, figsize=(7, 5), dpi=80, facecolor="w", edgecolor="k") + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 6], label="s1") + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 7], label="s2") + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 8], label="s3") + plt.legend(loc="upper left") + plt.xlabel("Time (s)") + plt.ylabel("sigma_AN (rad)") + plt.title("Estimated Asteroid Attitude") + + plt.figure(6, figsize=(7, 5), dpi=80, facecolor="w", edgecolor="k") + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 9], label="omega1") + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 10], label="omega2") + plt.plot(navTransOutMsgRec.times() * 1.0e-9, x_hat[:, 11], label="omega3") + plt.legend(loc="upper left") + plt.xlabel("Time (s)") + plt.ylabel("omega_AN_A (rad/s)") + plt.title("Estimated Asteroid Rate") if show_plots: plt.show() diff --git a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/_UnitTest/test_smallBodyNavUKF.py b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/_UnitTest/test_smallBodyNavUKF.py index 1394aa4708..2d86c6ab63 100644 --- a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/_UnitTest/test_smallBodyNavUKF.py +++ b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/_UnitTest/test_smallBodyNavUKF.py @@ -63,35 +63,45 @@ def smallBodyNavUKFTestFunction(show_plots): module.alpha = 0 # Filter hyperparameter module.beta = 2 # Filter hyperparameter module.kappa = 1e-3 # Filter hyperparameter - module.mu_ast = 17.2882449693*1e9 # Gravitational constant of the asteroid m^3/s^2 - module.P_proc = (0.1*np.identity(9)).tolist() # Process Noise - module.R_meas = (0.1*np.identity(3)).tolist() # Measurement Noise + module.mu_ast = ( + 17.2882449693 * 1e9 + ) # Gravitational constant of the asteroid m^3/s^2 + module.P_proc = (0.1 * np.identity(9)).tolist() # Process Noise + module.R_meas = (0.1 * np.identity(3)).tolist() # Measurement Noise vesta_radius = 2.3612 * orbitalMotion.AU * 1000 # meters - vesta_velocity = np.sqrt(orbitalMotion.MU_SUN*(1000.**3)/vesta_radius) # m/s, assumes circular orbit + vesta_velocity = np.sqrt( + orbitalMotion.MU_SUN * (1000.0**3) / vesta_radius + ) # m/s, assumes circular orbit - x_0 = [2010., 1510., 1010., 0., 2., 0., 0.14, 0., 0.] + x_0 = [2010.0, 1510.0, 1010.0, 0.0, 2.0, 0.0, 0.14, 0.0, 0.0] module.x_hat_k = x_0 - module.P_k = [[1000., 0., 0., 0., 0., 0., 0., 0., 0.], - [0., 1000., 0., 0., 0., 0., 0., 0., 0.], - [0., 0., 1000., 0., 0., 0., 0., 0., 0.], - [0., 0., 0., 1, 0., 0., 0., 0., 0.], - [0., 0., 0., 0., 1, 0., 0., 0., 0.], - [0., 0., 0., 0., 0., 1, 0., 0., 0.], - [0., 0., 0., 0., 0., 0., 1e-3, 0., 0.], - [0., 0., 0., 0., 0., 0., 0., 1e-3, 0.], - [0., 0., 0., 0., 0., 0., 0., 0., 1e-3]] - #module.P_k = P_k.tolist() + module.P_k = [ + [1000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 1, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 1, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3], + ] + # module.P_k = P_k.tolist() # Configure blank module input messages navTransInMsgData = messaging.NavTransMsgPayload() - navTransInMsgData.r_BN_N = [vesta_radius + 600. * 1000., -400. * 1000, 200. * 1000] - navTransInMsgData.v_BN_N = [0., vesta_velocity, 0.] + navTransInMsgData.r_BN_N = [ + vesta_radius + 600.0 * 1000.0, + -400.0 * 1000, + 200.0 * 1000, + ] + navTransInMsgData.v_BN_N = [0.0, vesta_velocity, 0.0] navTransInMsg = messaging.NavTransMsg().write(navTransInMsgData) asteroidEphemerisInMsgData = messaging.EphemerisMsgPayload() - asteroidEphemerisInMsgData.r_BdyZero_N = [vesta_radius, 0., 0.] - asteroidEphemerisInMsgData.v_BdyZero_N = [0., vesta_velocity, 0.] + asteroidEphemerisInMsgData.r_BdyZero_N = [vesta_radius, 0.0, 0.0] + asteroidEphemerisInMsgData.v_BdyZero_N = [0.0, vesta_velocity, 0.0] asteroidEphemerisInMsgData.sigma_BN = [0.0, 0.0, 0.0] asteroidEphemerisInMsgData.omega_BN_B = [0.0, 0.0, 0.0] asteroidEphemerisInMsg = messaging.EphemerisMsg().write(asteroidEphemerisInMsgData) @@ -118,50 +128,72 @@ def smallBodyNavUKFTestFunction(show_plots): # been considered and the spacecraft velocity in the small body # fixed frame is null, then the measured acceleration should correspond # to the Keplerian gravity with opposite sign - true_r = np.array([[600. * 1000, -400. * 1000, 200. * 1000]]) - true_v = np.array([[0., 0., 0.]]) - true_a = module.mu_ast * true_r / (np.linalg.norm(true_r))**3 + true_r = np.array([[600.0 * 1000, -400.0 * 1000, 200.0 * 1000]]) + true_v = np.array([[0.0, 0.0, 0.0]]) + true_a = module.mu_ast * true_r / (np.linalg.norm(true_r)) ** 3 true_x_hat = np.zeros(9) true_x_hat[0:3] = true_r true_x_hat[3:6] = true_v true_x_hat[6:9] = true_a testFailCount, testMessages = unitTestSupport.compareArrayRelative( - [true_x_hat], np.array([x_hat[-1,:]]), 0.01, "x_hat", - testFailCount, testMessages) + [true_x_hat], + np.array([x_hat[-1, :]]), + 0.01, + "x_hat", + testFailCount, + testMessages, + ) testFailCount, testMessages = unitTestSupport.compareArrayRelative( - [true_x_hat], np.array([x_hat_c_wrapped[-1,:]]), 0.01, "x_hat_c_wrapped", - testFailCount, testMessages) - - plt.close('all') - plt.figure(1, figsize=(7, 5), dpi=80, facecolor='w', edgecolor='k') + [true_x_hat], + np.array([x_hat_c_wrapped[-1, :]]), + 0.01, + "x_hat_c_wrapped", + testFailCount, + testMessages, + ) + + plt.close("all") + plt.figure(1, figsize=(7, 5), dpi=80, facecolor="w", edgecolor="k") plt.ticklabel_format(useOffset=False) - plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0E-9 / 60, x_hat[:,0] / 1000, label='x-pos') - plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0E-9 / 60, x_hat[:,1] / 1000, label='y-pos') - plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0E-9 / 60, x_hat[:,2] / 1000, label='z-pos') - plt.legend(loc='lower left') - plt.xlabel('Time (min)') - plt.ylabel('${}^{A}r_{BA}$ (km)') - plt.title('Estimated Relative Spacecraft Position') - - plt.figure(2, figsize=(7, 5), dpi=80, facecolor='w', edgecolor='k') - plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0E-9 / 60, x_hat[:,3], label='x-vel') - plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0E-9 / 60, x_hat[:,4], label='y-vel') - plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0E-9 / 60, x_hat[:,5], label='z-vel') - plt.legend(loc='upper right') - plt.xlabel('Time (min)') - plt.ylabel('${}^{A}v_{BA}$ (m/s)') - plt.title('Estimated Spacecraft Velocity') - - plt.figure(3, figsize=(7, 5), dpi=80, facecolor='w', edgecolor='k') - plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0E-9 / 60, x_hat[:,6], label='x-acc') - plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0E-9 / 60, x_hat[:,7], label='y-acc') - plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0E-9 / 60, x_hat[:,8], label='z-acc') - plt.legend(loc='lower right') - plt.xlabel('Time (min)') - plt.ylabel('${}^{A}a_{BA}$ (m/s^2)') - plt.title('Estimated Non-Keplerian Acceleration') + plt.plot( + smallBodyNavUKFOutMsgRec.times() * 1.0e-9 / 60, + x_hat[:, 0] / 1000, + label="x-pos", + ) + plt.plot( + smallBodyNavUKFOutMsgRec.times() * 1.0e-9 / 60, + x_hat[:, 1] / 1000, + label="y-pos", + ) + plt.plot( + smallBodyNavUKFOutMsgRec.times() * 1.0e-9 / 60, + x_hat[:, 2] / 1000, + label="z-pos", + ) + plt.legend(loc="lower left") + plt.xlabel("Time (min)") + plt.ylabel("${}^{A}r_{BA}$ (km)") + plt.title("Estimated Relative Spacecraft Position") + + plt.figure(2, figsize=(7, 5), dpi=80, facecolor="w", edgecolor="k") + plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0e-9 / 60, x_hat[:, 3], label="x-vel") + plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0e-9 / 60, x_hat[:, 4], label="y-vel") + plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0e-9 / 60, x_hat[:, 5], label="z-vel") + plt.legend(loc="upper right") + plt.xlabel("Time (min)") + plt.ylabel("${}^{A}v_{BA}$ (m/s)") + plt.title("Estimated Spacecraft Velocity") + + plt.figure(3, figsize=(7, 5), dpi=80, facecolor="w", edgecolor="k") + plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0e-9 / 60, x_hat[:, 6], label="x-acc") + plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0e-9 / 60, x_hat[:, 7], label="y-acc") + plt.plot(smallBodyNavUKFOutMsgRec.times() * 1.0e-9 / 60, x_hat[:, 8], label="z-acc") + plt.legend(loc="lower right") + plt.xlabel("Time (min)") + plt.ylabel("${}^{A}a_{BA}$ (m/s^2)") + plt.title("Estimated Non-Keplerian Acceleration") if show_plots: plt.show() diff --git a/src/fswAlgorithms/stateEstimation/thrustCMEstimation/_UnitTest/test_thrustCMEstimation.py b/src/fswAlgorithms/stateEstimation/thrustCMEstimation/_UnitTest/test_thrustCMEstimation.py index 4ffe7b1b8d..73b593fbce 100755 --- a/src/fswAlgorithms/stateEstimation/thrustCMEstimation/_UnitTest/test_thrustCMEstimation.py +++ b/src/fswAlgorithms/stateEstimation/thrustCMEstimation/_UnitTest/test_thrustCMEstimation.py @@ -61,18 +61,25 @@ def test_thrustCMEstimation(show_plots, dT, accuracy): def thrustCMEstimationTestFunction(show_plots, dT, accuracy): + r_CB_B = np.array([0, 0, 0]) # exact CM location + + r_TB_B = np.array( + [ + [6, 5, 4], # simulated thrust application point + [5, 4, 6], + [4, 6, 5], + [-6, 5, 4], + ] + ) - r_CB_B = np.array([0, 0, 0]) # exact CM location - - r_TB_B = np.array([[6, 5, 4], # simulated thrust application point - [5, 4, 6], - [4, 6, 5], - [-6, 5, 4]]) - - T_B = np.array([[1, 2, 3], # simulated thrust vector - [2, 3, 1], - [3, 1, 2], - [1, -2, 3]]) + T_B = np.array( + [ + [1, 2, 3], # simulated thrust vector + [2, 3, 1], + [3, 1, 2], + [1, -2, 3], + ] + ) unitTaskName = "unitTask" unitProcessName = "TestProcess" @@ -81,7 +88,7 @@ def thrustCMEstimationTestFunction(show_plots, dT, accuracy): unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(dT) # update process rate update time + testProcessRate = macros.sec2nano(dT) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -127,11 +134,15 @@ def thrustCMEstimationTestFunction(show_plots, dT, accuracy): unitTestSim.AddModelToTask(unitTaskName, cmEstimateLog) t = dT - R0 = np.array([[cmEstimation.R0[0][0], 0, 0], - [0, cmEstimation.R0[1][0], 0], - [0, 0, cmEstimation.R0[2][0]]]) + R0 = np.array( + [ + [cmEstimation.R0[0][0], 0, 0], + [0, cmEstimation.R0[1][0], 0], + [0, 0, cmEstimation.R0[2][0]], + ] + ) unitTestSim.InitializeSimulation() - unitTestSim.ConfigureStopTime(macros.sec2nano(t-0.1)) + unitTestSim.ConfigureStopTime(macros.sec2nano(t - 0.1)) for i in range(len(r_TB_B)): thrBConfig.timeTag = macros.sec2nano(t) thrBConfig.rThrust_B = r_TB_B[i] @@ -140,13 +151,13 @@ def thrustCMEstimationTestFunction(show_plots, dT, accuracy): thrConfigBMsg.write(thrBConfig) intTorque.timeTag = macros.sec2nano(t) - uMeasNoise = np.random.multivariate_normal([0,0,0], R0, size=1) + uMeasNoise = np.random.multivariate_normal([0, 0, 0], R0, size=1) intTorque.torqueRequestBody = -np.cross(r_TB_B[i], T_B[i]) + uMeasNoise[0] intFeedbackTorqueMsg.write(intTorque) t += dT unitTestSim.ExecuteSimulation() - unitTestSim.ConfigureStopTime(macros.sec2nano(t-0.1)) + unitTestSim.ConfigureStopTime(macros.sec2nano(t - 0.1)) # retrieve the logged data stateErr = cmEstimateLog.stateError @@ -156,17 +167,25 @@ def thrustCMEstimationTestFunction(show_plots, dT, accuracy): # check that post-fit residuals are smaller in magnitude that pre-fit residuals at each measurement for i in range(len(r_TB_B)): - np.testing.assert_array_less(np.linalg.norm(postFit[i]), np.linalg.norm(preFit[i]) + accuracy, verbose=True) + np.testing.assert_array_less( + np.linalg.norm(postFit[i]), + np.linalg.norm(preFit[i]) + accuracy, + verbose=True, + ) # check that components of post-fit residuals are within 3-sigma bounds of measurement covariance for i in range(len(r_TB_B)): for j in range(3): - np.testing.assert_array_less(postFit[i][j], 3*(R0[j][j])**0.5 + accuracy, verbose=True) + np.testing.assert_array_less( + postFit[i][j], 3 * (R0[j][j]) ** 0.5 + accuracy, verbose=True + ) # check that components of state errors are within 3-sigma bounds of state covariance for i in range(len(r_TB_B)): for j in range(3): - np.testing.assert_array_less(stateErr[i][j], 3*sigma[i][j] + accuracy, verbose=True) + np.testing.assert_array_less( + stateErr[i][j], 3 * sigma[i][j] + accuracy, verbose=True + ) return @@ -177,7 +196,7 @@ def thrustCMEstimationTestFunction(show_plots, dT, accuracy): # if __name__ == "__main__": test_thrustCMEstimation( - True, # show_plots - 10, # dTsim - 1e-12 # accuracy + True, # show_plots + 10, # dTsim + 1e-12, # accuracy ) diff --git a/src/fswAlgorithms/transDetermination/chebyPosEphem/_UnitTest/test_chebyPosEphem.py b/src/fswAlgorithms/transDetermination/chebyPosEphem/_UnitTest/test_chebyPosEphem.py index 999cd4bb1f..ce1da91b1e 100644 --- a/src/fswAlgorithms/transDetermination/chebyPosEphem/_UnitTest/test_chebyPosEphem.py +++ b/src/fswAlgorithms/transDetermination/chebyPosEphem/_UnitTest/test_chebyPosEphem.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -26,6 +25,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) from Basilisk import __path__ + bskPath = __path__[0] from Basilisk.utilities import SimulationBaseClass @@ -39,14 +39,13 @@ orbitPosAccuracy = 1.0 orbitVelAccuracy = 0.01 + # uncomment this line is 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() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("function", ["sineCosine" - , "earthOrbitFit" - ]) +@pytest.mark.parametrize("function", ["sineCosine", "earthOrbitFit"]) def test_chebyPosFitAllTest(show_plots, function): """Module Unit Test""" testFunction = globals().get(function) @@ -69,26 +68,30 @@ def sineCosine(show_plots): testMessages = [] # create empty list to store test log messages orbitRadius = 70000.0 - numCurvePoints = 365*3+1 - curveDurationDays = 365.0*3 - degChebCoeff =21 + numCurvePoints = 365 * 3 + 1 + curveDurationDays = 365.0 * 3 + degChebCoeff = 21 - angleSpace = numpy.linspace(-3*math.pi, 3*math.pi, numCurvePoints) + angleSpace = numpy.linspace(-3 * math.pi, 3 * math.pi, numCurvePoints) - cosineValues = numpy.cos(angleSpace)*orbitRadius - sineValues = numpy.sin(angleSpace)*orbitRadius + cosineValues = numpy.cos(angleSpace) * orbitRadius + sineValues = numpy.sin(angleSpace) * orbitRadius oopValues = numpy.sin(angleSpace) + orbitRadius - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/naif0012.tls') + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/naif0012.tls") et = pyswice.new_doubleArray(1) - timeStringMid = '2019 APR 1 12:12:12.0 (UTC)' + timeStringMid = "2019 APR 1 12:12:12.0 (UTC)" pyswice.str2et_c(timeStringMid, et) fitTimes = numpy.linspace(-1, 1, numCurvePoints) - chebCosCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, cosineValues, degChebCoeff) - chebSinCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, sineValues, degChebCoeff) + chebCosCoeff = numpy.polynomial.chebyshev.chebfit( + fitTimes, cosineValues, degChebCoeff + ) + chebSinCoeff = numpy.polynomial.chebyshev.chebfit( + fitTimes, sineValues, degChebCoeff + ) cheboopCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, oopValues, degChebCoeff) unitTaskName = "unitTask" # arbitrary name (don't change) @@ -99,7 +102,9 @@ def sineCosine(show_plots): FSWUnitTestProc = TotalSim.CreateNewProcess(unitProcessName) # create the dynamics task and specify the integration update time - FSWUnitTestProc.addTask(TotalSim.CreateNewTask(unitTaskName, macros.sec2nano(8640.0))) + FSWUnitTestProc.addTask( + TotalSim.CreateNewTask(unitTaskName, macros.sec2nano(8640.0)) + ) chebyFitModel = chebyPosEphem.chebyPosEphem() chebyFitModel.ModelTag = "chebyFitModel" @@ -110,14 +115,15 @@ def sineCosine(show_plots): totalList.extend(numpy.array(cheboopCoeff).tolist()) chebyFitModel.ephArray[0].posChebyCoeff = totalList - chebyFitModel.ephArray[0].nChebCoeff = degChebCoeff+1 + chebyFitModel.ephArray[0].nChebCoeff = degChebCoeff + 1 chebyFitModel.ephArray[0].ephemTimeMid = pyswice.doubleArray_getitem(et, 0) - chebyFitModel.ephArray[0].ephemTimeRad = curveDurationDays/2.0*86400.0 + chebyFitModel.ephArray[0].ephemTimeRad = curveDurationDays / 2.0 * 86400.0 clockCorrData = messaging.TDBVehicleClockCorrelationMsgPayload() clockCorrData.vehicleClockTime = 0.0 - clockCorrData.ephemerisTime = chebyFitModel.ephArray[0].ephemTimeMid - \ - chebyFitModel.ephArray[0].ephemTimeRad + clockCorrData.ephemerisTime = ( + chebyFitModel.ephArray[0].ephemTimeMid - chebyFitModel.ephArray[0].ephemTimeRad + ) clockInMsg = messaging.TDBVehicleClockCorrelationMsg().write(clockCorrData) chebyFitModel.clockCorrInMsg.subscribeTo(clockInMsg) @@ -127,65 +133,70 @@ def sineCosine(show_plots): TotalSim.AddModelToTask(unitTaskName, dataLog) TotalSim.InitializeSimulation() - TotalSim.ConfigureStopTime(int(curveDurationDays*86400.0*1.0E9)) + TotalSim.ConfigureStopTime(int(curveDurationDays * 86400.0 * 1.0e9)) TotalSim.ExecuteSimulation() posChebData = dataLog.r_BdyZero_N - angleSpaceFine = numpy.linspace(-3*math.pi, 3*math.pi, numCurvePoints*10-9) + angleSpaceFine = numpy.linspace(-3 * math.pi, 3 * math.pi, numCurvePoints * 10 - 9) - cosineValuesFine = numpy.cos(angleSpaceFine)*orbitRadius - sineValuesFine = numpy.sin(angleSpaceFine)*orbitRadius + cosineValuesFine = numpy.cos(angleSpaceFine) * orbitRadius + sineValuesFine = numpy.sin(angleSpaceFine) * orbitRadius oopValuesFine = numpy.sin(angleSpaceFine) + orbitRadius - maxErrVec = [max(abs(posChebData[:,0] - cosineValuesFine)), - max(abs(posChebData[:,1] - sineValuesFine)), - max(abs(posChebData[:,2] - oopValuesFine))] + maxErrVec = [ + max(abs(posChebData[:, 0] - cosineValuesFine)), + max(abs(posChebData[:, 1] - sineValuesFine)), + max(abs(posChebData[:, 2] - oopValuesFine)), + ] - print("Sine Wave error: " + str(max(maxErrVec))) + print("Sine Wave error: " + str(max(maxErrVec))) assert max(maxErrVec) < orbitPosAccuracy if testFailCount == 0: print("PASSED: " + " Sine and Cosine curve fit") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def earthOrbitFit(show_plots): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. - #__tracebackhide__ = True + # __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages - numCurvePoints = 365*3+1 - curveDurationSeconds = 3*5950.0 - degChebCoeff =23 + numCurvePoints = 365 * 3 + 1 + curveDurationSeconds = 3 * 5950.0 + degChebCoeff = 23 integFrame = "j2000" zeroBase = "Earth" dateSpice = "2015 February 10, 00:00:00.0 TDB" - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/naif0012.tls') + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/naif0012.tls") et = pyswice.new_doubleArray(1) pyswice.str2et_c(dateSpice, et) etStart = pyswice.doubleArray_getitem(et, 0) etEnd = etStart + curveDurationSeconds - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de430.bsp') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/naif0012.tls') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de-403-masses.tpc') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/pck00010.tpc') - pyswice.furnsh_c(path + '/hst_edited.bsp') + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/de430.bsp") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/naif0012.tls") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/de-403-masses.tpc") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/pck00010.tpc") + pyswice.furnsh_c(path + "/hst_edited.bsp") hubblePosList = [] hubbleVelList = [] timeHistory = numpy.linspace(etStart, etEnd, numCurvePoints) for timeVal in timeHistory: - stringCurrent = pyswice.et2utc_c(timeVal, 'C', 4, 1024, "Yo") - stateOut = spkRead('HUBBLE SPACE TELESCOPE', stringCurrent, integFrame, zeroBase) + stringCurrent = pyswice.et2utc_c(timeVal, "C", 4, 1024, "Yo") + stateOut = spkRead( + "HUBBLE SPACE TELESCOPE", stringCurrent, integFrame, zeroBase + ) hubblePosList.append(stateOut[0:3].tolist()) hubbleVelList.append(stateOut[3:6].tolist()) @@ -193,7 +204,9 @@ def earthOrbitFit(show_plots): hubbleVelList = numpy.array(hubbleVelList) fitTimes = numpy.linspace(-1, 1, numCurvePoints) - chebCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, hubblePosList, degChebCoeff) + chebCoeff = numpy.polynomial.chebyshev.chebfit( + fitTimes, hubblePosList, degChebCoeff + ) unitTaskName = "unitTask" # arbitrary name (don't change) unitProcessName = "TestProcess" # arbitrary name (don't change) @@ -203,25 +216,30 @@ def earthOrbitFit(show_plots): FSWUnitTestProc = TotalSim.CreateNewProcess(unitProcessName) # create the dynamics task and specify the integration update time - FSWUnitTestProc.addTask(TotalSim.CreateNewTask(unitTaskName, macros.sec2nano(curveDurationSeconds/(numCurvePoints-1)))) + FSWUnitTestProc.addTask( + TotalSim.CreateNewTask( + unitTaskName, macros.sec2nano(curveDurationSeconds / (numCurvePoints - 1)) + ) + ) chebyFitModel = chebyPosEphem.chebyPosEphem() chebyFitModel.ModelTag = "chebyFitModel" TotalSim.AddModelToTask(unitTaskName, chebyFitModel) - totalList = chebCoeff[:,0].tolist() - totalList.extend(chebCoeff[:,1].tolist()) - totalList.extend(chebCoeff[:,2].tolist()) + totalList = chebCoeff[:, 0].tolist() + totalList.extend(chebCoeff[:, 1].tolist()) + totalList.extend(chebCoeff[:, 2].tolist()) chebyFitModel.ephArray[0].posChebyCoeff = totalList - chebyFitModel.ephArray[0].nChebCoeff = degChebCoeff+1 - chebyFitModel.ephArray[0].ephemTimeMid = etStart + curveDurationSeconds/2.0 - chebyFitModel.ephArray[0].ephemTimeRad = curveDurationSeconds/2.0 + chebyFitModel.ephArray[0].nChebCoeff = degChebCoeff + 1 + chebyFitModel.ephArray[0].ephemTimeMid = etStart + curveDurationSeconds / 2.0 + chebyFitModel.ephArray[0].ephemTimeRad = curveDurationSeconds / 2.0 clockCorrData = messaging.TDBVehicleClockCorrelationMsgPayload() clockCorrData.vehicleClockTime = 0.0 - clockCorrData.ephemerisTime = chebyFitModel.ephArray[0].ephemTimeMid - \ - chebyFitModel.ephArray[0].ephemTimeRad + clockCorrData.ephemerisTime = ( + chebyFitModel.ephArray[0].ephemTimeMid - chebyFitModel.ephArray[0].ephemTimeRad + ) clockInMsg = messaging.TDBVehicleClockCorrelationMsg().write(clockCorrData) chebyFitModel.clockCorrInMsg.subscribeTo(clockInMsg) @@ -229,34 +247,44 @@ def earthOrbitFit(show_plots): TotalSim.AddModelToTask(unitTaskName, dataLog) TotalSim.InitializeSimulation() - TotalSim.ConfigureStopTime(int(curveDurationSeconds*1.0E9)) + TotalSim.ConfigureStopTime(int(curveDurationSeconds * 1.0e9)) TotalSim.ExecuteSimulation() posChebData = dataLog.r_BdyZero_N velChebData = dataLog.v_BdyZero_N - maxErrVec = [abs(max(posChebData[:,0] - hubblePosList[:,0])), - abs(max(posChebData[:,1] - hubblePosList[:,1])), - abs(max(posChebData[:,2] - hubblePosList[:,2]))] - maxVelErrVec = [abs(max(velChebData[:,0] - hubbleVelList[:,0])), - abs(max(velChebData[:,1] - hubbleVelList[:,1])), - abs(max(velChebData[:,2] - hubbleVelList[:,2]))] + maxErrVec = [ + abs(max(posChebData[:, 0] - hubblePosList[:, 0])), + abs(max(posChebData[:, 1] - hubblePosList[:, 1])), + abs(max(posChebData[:, 2] - hubblePosList[:, 2])), + ] + maxVelErrVec = [ + abs(max(velChebData[:, 0] - hubbleVelList[:, 0])), + abs(max(velChebData[:, 1] - hubbleVelList[:, 1])), + abs(max(velChebData[:, 2] - hubbleVelList[:, 2])), + ] print("Hubble Orbit Accuracy: " + str(max(maxErrVec))) print("Hubble Velocity Accuracy: " + str(max(maxVelErrVec))) assert (max(maxErrVec)) < orbitPosAccuracy assert (max(maxVelErrVec)) < orbitVelAccuracy plt.figure() - plt.plot(dataLog.times()*1.0E-9, velChebData[:,0], dataLog.times()*1.0E-9, hubbleVelList[:,0]) - - if(show_plots): + plt.plot( + dataLog.times() * 1.0e-9, + velChebData[:, 0], + dataLog.times() * 1.0e-9, + hubbleVelList[:, 0], + ) + + if show_plots: plt.show() - plt.close('all') + plt.close("all") if testFailCount == 0: print("PASSED: " + " Orbit curve fit") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": test_chebyPosFitAllTest(True) diff --git a/src/fswAlgorithms/transDetermination/dvAccumulation/_UnitTest/test_dvAccumulation.py b/src/fswAlgorithms/transDetermination/dvAccumulation/_UnitTest/test_dvAccumulation.py index 8a709798a4..dcb177302e 100644 --- a/src/fswAlgorithms/transDetermination/dvAccumulation/_UnitTest/test_dvAccumulation.py +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/_UnitTest/test_dvAccumulation.py @@ -19,23 +19,27 @@ def generateAccData(): - """ Returns a list of random AccPktDataFswMsg.""" + """Returns a list of random AccPktDataFswMsg.""" accPktList = list() for _ in range(120): accPacketData = messaging.AccPktDataMsgPayload() accPacketData.measTime = abs(int(random.normal(5e7, 1e7))) - accPacketData.accel_B = random.normal(0.1, 0.2, 3) # Acceleration in platform frame [m/s2] + accPacketData.accel_B = random.normal( + 0.1, 0.2, 3 + ) # Acceleration in platform frame [m/s2] accPktList.append(accPacketData) return accPktList + def test_dv_accumulation(): - """ Test dvAccumulation. """ + """Test dvAccumulation.""" [testResults, testMessage] = dvAccumulationTestFunction() assert testResults < 1, testMessage + def dvAccumulationTestFunction(): - """ Test the dvAccumulation module. Setup a simulation, """ + """Test the dvAccumulation module. Setup a simulation,""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -56,8 +60,12 @@ def dvAccumulationTestFunction(): invData.accPkts[i].measTime = invMeasTimes[i] # Run module quicksort function - dvAccumulation.dvAccumulation_QuickSort(randData.accPkts[0], 0, messaging.MAX_ACC_BUF_PKT - 1) - dvAccumulation.dvAccumulation_QuickSort(invData.accPkts[0], 0, messaging.MAX_ACC_BUF_PKT - 1) + dvAccumulation.dvAccumulation_QuickSort( + randData.accPkts[0], 0, messaging.MAX_ACC_BUF_PKT - 1 + ) + dvAccumulation.dvAccumulation_QuickSort( + invData.accPkts[0], 0, messaging.MAX_ACC_BUF_PKT - 1 + ) # Check that sorted packets properly randMeasTimes.sort() @@ -78,7 +86,9 @@ def dvAccumulationTestFunction(): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process # Construct the dvAccumulation module # Set the names for the input messages @@ -129,37 +139,48 @@ def dvAccumulationTestFunction(): # print(outputNavMsgData) # print(timeMsgData) - trueDVVector = [[4.82820079e-03, 7.81971465e-03, 2.29605663e-03], - [ 4.82820079e-03, 7.81971465e-03, 2.29605663e-03], - [ 4.82820079e-03, 7.81971465e-03, 2.29605663e-03], - [ 6.44596343e-03, 9.00203561e-03, 2.60580728e-03], - [ 6.44596343e-03, 9.00203561e-03, 2.60580728e-03]] - trueTime = np.array([7.2123026e+07, 7.2123026e+07, 7.2123026e+07, 7.6667436e+07, 7.6667436e+07]) * macros.NANO2SEC + trueDVVector = [ + [4.82820079e-03, 7.81971465e-03, 2.29605663e-03], + [4.82820079e-03, 7.81971465e-03, 2.29605663e-03], + [4.82820079e-03, 7.81971465e-03, 2.29605663e-03], + [6.44596343e-03, 9.00203561e-03, 2.60580728e-03], + [6.44596343e-03, 9.00203561e-03, 2.60580728e-03], + ] + trueTime = ( + np.array([7.2123026e07, 7.2123026e07, 7.2123026e07, 7.6667436e07, 7.6667436e07]) + * macros.NANO2SEC + ) accuracy = 1e-6 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) # At each timestep, make sure the vehicleConfig values haven't changed from the initial values - testFailCount, testMessages = unitTestSupport.compareArrayND(trueDVVector, outputNavMsgData, - accuracy, - "dvAccumulation output", - 2, testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArrayND([trueTime], [timeMsgData], - accuracy, "timeTag", 5, - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + trueDVVector, + outputNavMsgData, + accuracy, + "dvAccumulation output", + 2, + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArrayND( + [trueTime], [timeMsgData], accuracy, "timeTag", 5, testFailCount, testMessages + ) snippentName = "passFail" if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + -if __name__ == '__main__': +if __name__ == "__main__": test_dv_accumulation() diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/_UnitTest/test_ephemDifference.py b/src/fswAlgorithms/transDetermination/ephemDifference/_UnitTest/test_ephemDifference.py index 90130aa40c..fe92282be2 100644 --- a/src/fswAlgorithms/transDetermination/ephemDifference/_UnitTest/test_ephemDifference.py +++ b/src/fswAlgorithms/transDetermination/ephemDifference/_UnitTest/test_ephemDifference.py @@ -17,15 +17,16 @@ from Basilisk.utilities import astroFunctions from Basilisk.architecture import messaging -@pytest.mark.parametrize("ephBdyCount", [3, 0]) +@pytest.mark.parametrize("ephBdyCount", [3, 0]) def test_ephemDifference(ephBdyCount): - """ Test ephemDifference. """ + """Test ephemDifference.""" [testResults, testMessage] = ephemDifferenceTestFunction(ephBdyCount) assert testResults < 1, testMessage + def ephemDifferenceTestFunction(ephBdyCount): - """ Test the ephemDifference module. Setup a simulation, """ + """Test the ephemDifference module. Setup a simulation,""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -38,7 +39,9 @@ def ephemDifferenceTestFunction(ephBdyCount): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process ephemDiff = ephemDifference.ephemDifference() @@ -49,15 +52,21 @@ def ephemDifferenceTestFunction(ephBdyCount): unitTestSim.AddModelToTask(unitTaskName, ephemDiff) # Create the input message. - inputEphemBase = messaging.EphemerisMsgPayload() # The clock correlation message ? + inputEphemBase = messaging.EphemerisMsgPayload() # The clock correlation message ? # Get the Earth's position and velocity - position, velocity = astroFunctions.Earth_RV(astroFunctions.JulianDate([2018, 10, 16])) + position, velocity = astroFunctions.Earth_RV( + astroFunctions.JulianDate([2018, 10, 16]) + ) inputEphemBase.r_BdyZero_N = position inputEphemBase.v_BdyZero_N = velocity inputEphemBase.timeTag = 1234.0 ephBaseInMsg = messaging.EphemerisMsg().write(inputEphemBase) ephemDiff.ephBaseInMsg.subscribeTo(ephBaseInMsg) - functions = [astroFunctions.Mars_RV, astroFunctions.Jupiter_RV, astroFunctions.Saturn_RV] + functions = [ + astroFunctions.Mars_RV, + astroFunctions.Jupiter_RV, + astroFunctions.Saturn_RV, + ] changeBodyList = list() ephInMsgList = list() @@ -97,14 +106,17 @@ def ephemDifferenceTestFunction(ephBdyCount): unitTestSim.ExecuteSimulation() if ephBdyCount == 3: - trueRVector = [[69313607.6209608, -75620898.04028425, -5443274.17030424], - [-5.33462105e+08, -7.56888610e+08, 1.17556184e+07], - [9.94135029e+07, -1.54721593e+09, 1.65081472e+07]] - - trueVVector = [[15.04232523, -1.13359121, 0.47668898], - [23.2531093, -33.17628299, -0.22550391], - [21.02793499, -25.86425597, -0.38273815]] - + trueRVector = [ + [69313607.6209608, -75620898.04028425, -5443274.17030424], + [-5.33462105e08, -7.56888610e08, 1.17556184e07], + [9.94135029e07, -1.54721593e09, 1.65081472e07], + ] + + trueVVector = [ + [15.04232523, -1.13359121, 0.47668898], + [23.2531093, -33.17628299, -0.22550391], + [21.02793499, -25.86425597, -0.38273815], + ] posAcc = 1e1 velAcc = 1e-4 @@ -112,7 +124,6 @@ def ephemDifferenceTestFunction(ephBdyCount): unitTestSupport.writeTeXSnippet("toleranceValueVel", str(velAcc), path) for i in range(ephBdyCount): - outputData_R = dataLogList[i].r_BdyZero_N outputData_V = dataLogList[i].v_BdyZero_N timeTag = dataLogList[i].timeTag @@ -120,14 +131,24 @@ def ephemDifferenceTestFunction(ephBdyCount): # print(outputData_R) # At each timestep, make sure the vehicleConfig values haven't changed from the initial values - testFailCount, testMessages = unitTestSupport.compareArrayND([trueRVector[i]], outputData_R, - posAcc, - "ephemDifference position output body " + str(i), - 2, testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArrayND([trueVVector[i]], outputData_V, - velAcc, - "ephemDifference velocity output body " + str(i), - 2, testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + [trueRVector[i]], + outputData_R, + posAcc, + "ephemDifference position output body " + str(i), + 2, + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArrayND( + [trueVVector[i]], + outputData_V, + velAcc, + "ephemDifference velocity output body " + str(i), + 2, + testFailCount, + testMessages, + ) if timeTag[0] != 321.0: testFailCount += 1 testMessages.append("ephemDifference timeTag output body " + str(i)) @@ -138,17 +159,17 @@ def ephemDifferenceTestFunction(ephBdyCount): snippentName = "passFail" + str(ephBdyCount) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + ephemDiff.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + ephemDiff.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] -if __name__ == '__main__': +if __name__ == "__main__": test_ephemDifference(3) diff --git a/src/fswAlgorithms/transDetermination/ephemNavConverter/_UnitTest/test_ephemNavConverter.py b/src/fswAlgorithms/transDetermination/ephemNavConverter/_UnitTest/test_ephemNavConverter.py index abfeae7c3c..b6e1d9998d 100644 --- a/src/fswAlgorithms/transDetermination/ephemNavConverter/_UnitTest/test_ephemNavConverter.py +++ b/src/fswAlgorithms/transDetermination/ephemNavConverter/_UnitTest/test_ephemNavConverter.py @@ -15,13 +15,15 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) + def test_ephemNavConverter(): - """ Test ephemNavConverter. """ + """Test ephemNavConverter.""" [testResults, testMessage] = ephemNavConverterTestFunction() assert testResults < 1, testMessage + def ephemNavConverterTestFunction(): - """ Test the ephemNavConverter module. Setup a simulation """ + """Test the ephemNavConverter module. Setup a simulation""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -34,7 +36,9 @@ def ephemNavConverterTestFunction(): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process # Construct the ephemNavConverter module # Set the names for the input messages @@ -50,7 +54,9 @@ def ephemNavConverterTestFunction(): inputEphem = messaging.EphemerisMsgPayload() # Get the Earth's position and velocity - position, velocity = astroFunctions.Earth_RV(astroFunctions.JulianDate([2018, 10, 16])) + position, velocity = astroFunctions.Earth_RV( + astroFunctions.JulianDate([2018, 10, 16]) + ) inputEphem.r_BdyZero_N = position inputEphem.v_BdyZero_N = velocity inputEphem.timeTag = 1.0 # sec @@ -79,34 +85,47 @@ def ephemNavConverterTestFunction(): trueTime = [inputEphem.timeTag, inputEphem.timeTag] # At each timestep, make sure the vehicleConfig values haven't changed from the initial values - testFailCount, testMessages = unitTestSupport.compareArrayND(trueR, outputR, - posAcc, - "ephemNavConverter output Position", - 2, testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArrayND(trueV, outputV, - velAcc, - "ephemNavConverter output Velocity", - 2, testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareDoubleArray(trueTime, outputTime, - velAcc, - "ephemNavConverter output Time", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + trueR, + outputR, + posAcc, + "ephemNavConverter output Position", + 2, + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArrayND( + trueV, + outputV, + velAcc, + "ephemNavConverter output Velocity", + 2, + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareDoubleArray( + trueTime, + outputTime, + velAcc, + "ephemNavConverter output Time", + testFailCount, + testMessages, + ) # print out success message if no error were found snippentName = "passFail" if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + ephemNav.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + ephemNav.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] -if __name__ == '__main__': +if __name__ == "__main__": test_ephemNavConverter() diff --git a/src/fswAlgorithms/transDetermination/navAggregate/_UnitTest/test_navAggregate.py b/src/fswAlgorithms/transDetermination/navAggregate/_UnitTest/test_navAggregate.py index 661cf3241e..bc562dfb2c 100755 --- a/src/fswAlgorithms/transDetermination/navAggregate/_UnitTest/test_navAggregate.py +++ b/src/fswAlgorithms/transDetermination/navAggregate/_UnitTest/test_navAggregate.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -31,15 +30,10 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) - - - - - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import unitTestSupport @@ -47,6 +41,7 @@ from Basilisk.utilities import macros from Basilisk.architecture import messaging + # Uncomment this line is 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. @@ -54,53 +49,58 @@ # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("numAttNav, numTransNav", [ - (0, 0) - , (1, 1) - , (0, 1) - , (1, 0) - , (2, 2) - , (1, 2) - , (0, 2) - , (2, 1) - , (2, 0) - , (3, 3) - , (3, 2) - , (3, 1) - , (3, 0) - , (2, 3) - , (1, 3) - , (0, 3) - , (11, 11) - , (3, 11) - , (2, 11) - , (1, 11) - , (0, 11) - , (11, 3) - , (11, 2) - , (11, 1) - , (11, 0) -]) +@pytest.mark.parametrize( + "numAttNav, numTransNav", + [ + (0, 0), + (1, 1), + (0, 1), + (1, 0), + (2, 2), + (1, 2), + (0, 2), + (2, 1), + (2, 0), + (3, 3), + (3, 2), + (3, 1), + (3, 0), + (2, 3), + (1, 3), + (0, 3), + (11, 11), + (3, 11), + (2, 11), + (1, 11), + (0, 11), + (11, 3), + (11, 2), + (11, 1), + (11, 0), + ], +) # update "module" in this function name to reflect the module name def test_module(show_plots, numAttNav, numTransNav): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = navAggregateTestFunction(show_plots, numAttNav, numTransNav) + [testResults, testMessage] = navAggregateTestFunction( + show_plots, numAttNav, numTransNav + ) assert testResults < 1, testMessage def navAggregateTestFunction(show_plots, numAttNav, numTransNav): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -115,26 +115,26 @@ def navAggregateTestFunction(show_plots, numAttNav, numTransNav): navAtt1Msg = messaging.NavAttMsgPayload() navAtt1Msg.timeTag = 11.11 navAtt1Msg.sigma_BN = [0.1, 0.01, -0.1] - navAtt1Msg.omega_BN_B = [1., 1., -1.] + navAtt1Msg.omega_BN_B = [1.0, 1.0, -1.0] navAtt1Msg.vehSunPntBdy = [-0.1, 0.1, 0.1] navAtt1InMsg = messaging.NavAttMsg().write(navAtt1Msg) navAtt2Msg = messaging.NavAttMsgPayload() navAtt2Msg.timeTag = 22.22 navAtt2Msg.sigma_BN = [0.2, 0.02, -0.2] - navAtt2Msg.omega_BN_B = [2., 2., -2.] + navAtt2Msg.omega_BN_B = [2.0, 2.0, -2.0] navAtt2Msg.vehSunPntBdy = [-0.2, 0.2, 0.2] navAtt2InMsg = messaging.NavAttMsg().write(navAtt2Msg) navTrans1Msg = messaging.NavTransMsgPayload() navTrans1Msg.timeTag = 11.1 navTrans1Msg.r_BN_N = [1000.0, 100.0, -1000.0] - navTrans1Msg.v_BN_N = [1., 1., -1.] + navTrans1Msg.v_BN_N = [1.0, 1.0, -1.0] navTrans1Msg.vehAccumDV = [-10.1, 10.1, 10.1] navTrans1InMsg = messaging.NavTransMsg().write(navTrans1Msg) navTrans2Msg = messaging.NavTransMsgPayload() navTrans2Msg.timeTag = 22.2 navTrans2Msg.r_BN_N = [2000.0, 200.0, -2000.0] - navTrans2Msg.v_BN_N = [2., 2., -2.] + navTrans2Msg.v_BN_N = [2.0, 2.0, -2.0] navTrans2Msg.vehAccumDV = [-20.2, 20.2, 20.2] navTrans2InMsg = messaging.NavTransMsg().write(navTrans2Msg) @@ -145,11 +145,11 @@ def navAggregateTestFunction(show_plots, numAttNav, numTransNav): navTrans2 = navAggregate.AggregateTransInput() module.attMsgCount = numAttNav - if numAttNav == 3: # here the index asks to read from an empty (zero) message + if numAttNav == 3: # here the index asks to read from an empty (zero) message module.attMsgCount = 2 module.transMsgCount = numTransNav - if numTransNav == 3: # here the index asks to read from an empty (zero) message + if numTransNav == 3: # here the index asks to read from an empty (zero) message module.transMsgCount = 2 if numAttNav <= navAggregate.MAX_AGG_NAV_MSG: @@ -169,34 +169,62 @@ def navAggregateTestFunction(show_plots, numAttNav, numTransNav): for i in range(navAggregate.MAX_AGG_NAV_MSG): module.transMsgs[i].navTransInMsg.subscribeTo(navTrans1InMsg) - if numAttNav > 1: # always read from the last message counter + if numAttNav > 1: # always read from the last message counter module.attTimeIdx = numAttNav - 1 module.attIdx = numAttNav - 1 module.rateIdx = numAttNav - 1 module.sunIdx = numAttNav - 1 - if numTransNav > 1: # always read from the last message counter - module.transTimeIdx = numTransNav-1 - module.posIdx = numTransNav-1 - module.velIdx = numTransNav-1 - module.dvIdx = numTransNav-1 + if numTransNav > 1: # always read from the last message counter + module.transTimeIdx = numTransNav - 1 + module.posIdx = numTransNav - 1 + module.velIdx = numTransNav - 1 + module.dvIdx = numTransNav - 1 # write TeX snippets for the message values unitTestSupport.writeTeXSnippet("navAtt1Msg.timeTag", str(navAtt1Msg.timeTag), path) - unitTestSupport.writeTeXSnippet("navAtt1Msg.sigma_BN", str(navAtt1Msg.sigma_BN), path) - unitTestSupport.writeTeXSnippet("navAtt1Msg.omega_BN_B", str(navAtt1Msg.omega_BN_B), path) - unitTestSupport.writeTeXSnippet("navAtt1Msg.vehSunPntBdy", str(navAtt1Msg.vehSunPntBdy), path) + unitTestSupport.writeTeXSnippet( + "navAtt1Msg.sigma_BN", str(navAtt1Msg.sigma_BN), path + ) + unitTestSupport.writeTeXSnippet( + "navAtt1Msg.omega_BN_B", str(navAtt1Msg.omega_BN_B), path + ) + unitTestSupport.writeTeXSnippet( + "navAtt1Msg.vehSunPntBdy", str(navAtt1Msg.vehSunPntBdy), path + ) unitTestSupport.writeTeXSnippet("navAtt2Msg.timeTag", str(navAtt2Msg.timeTag), path) - unitTestSupport.writeTeXSnippet("navAtt2Msg.sigma_BN", str(navAtt2Msg.sigma_BN), path) - unitTestSupport.writeTeXSnippet("navAtt2Msg.omega_BN_B", str(navAtt2Msg.omega_BN_B), path) - unitTestSupport.writeTeXSnippet("navAtt2Msg.vehSunPntBdy", str(navAtt2Msg.vehSunPntBdy), path) - unitTestSupport.writeTeXSnippet("navTrans1Msg.timeTag", str(navTrans1Msg.timeTag), path) - unitTestSupport.writeTeXSnippet("navTrans1Msg.r_BN_N", str(navTrans1Msg.r_BN_N), path) - unitTestSupport.writeTeXSnippet("navTrans1Msg.v_BN_N", str(navTrans1Msg.v_BN_N), path) - unitTestSupport.writeTeXSnippet("navTrans1Msg.vehAccumDV", str(navTrans1Msg.vehAccumDV), path) - unitTestSupport.writeTeXSnippet("navTrans2Msg.timeTag", str(navTrans2Msg.timeTag), path) - unitTestSupport.writeTeXSnippet("navTrans2Msg.r_BN_N", str(navTrans2Msg.r_BN_N), path) - unitTestSupport.writeTeXSnippet("navTrans2Msg.v_BN_N", str(navTrans2Msg.v_BN_N), path) - unitTestSupport.writeTeXSnippet("navTrans2Msg.vehAccumDV", str(navTrans2Msg.vehAccumDV), path) + unitTestSupport.writeTeXSnippet( + "navAtt2Msg.sigma_BN", str(navAtt2Msg.sigma_BN), path + ) + unitTestSupport.writeTeXSnippet( + "navAtt2Msg.omega_BN_B", str(navAtt2Msg.omega_BN_B), path + ) + unitTestSupport.writeTeXSnippet( + "navAtt2Msg.vehSunPntBdy", str(navAtt2Msg.vehSunPntBdy), path + ) + unitTestSupport.writeTeXSnippet( + "navTrans1Msg.timeTag", str(navTrans1Msg.timeTag), path + ) + unitTestSupport.writeTeXSnippet( + "navTrans1Msg.r_BN_N", str(navTrans1Msg.r_BN_N), path + ) + unitTestSupport.writeTeXSnippet( + "navTrans1Msg.v_BN_N", str(navTrans1Msg.v_BN_N), path + ) + unitTestSupport.writeTeXSnippet( + "navTrans1Msg.vehAccumDV", str(navTrans1Msg.vehAccumDV), path + ) + unitTestSupport.writeTeXSnippet( + "navTrans2Msg.timeTag", str(navTrans2Msg.timeTag), path + ) + unitTestSupport.writeTeXSnippet( + "navTrans2Msg.r_BN_N", str(navTrans2Msg.r_BN_N), path + ) + unitTestSupport.writeTeXSnippet( + "navTrans2Msg.v_BN_N", str(navTrans2Msg.v_BN_N), path + ) + unitTestSupport.writeTeXSnippet( + "navTrans2Msg.vehAccumDV", str(navTrans2Msg.vehAccumDV), path + ) # Setup logging on the test module output message so that we get all the writes to it dataAttLog = module.navAttOutMsg.recorder() @@ -211,7 +239,7 @@ def navAggregateTestFunction(show_plots, numAttNav, numTransNav): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -229,28 +257,28 @@ def navAggregateTestFunction(show_plots, numAttNav, numTransNav): # set the filtered output truth states if numAttNav == 0 or numAttNav == 3: - trueAttTimeTag = [[0.0]]*3 - trueAttSigma = [[0., 0., 0.]]*3 - trueAttOmega = [[0., 0., 0.]]*3 - trueAttSunVector = [[0., 0., 0.]]*3 + trueAttTimeTag = [[0.0]] * 3 + trueAttSigma = [[0.0, 0.0, 0.0]] * 3 + trueAttOmega = [[0.0, 0.0, 0.0]] * 3 + trueAttSunVector = [[0.0, 0.0, 0.0]] * 3 if numTransNav == 0 or numTransNav == 3: - trueTransTimeTag = [[0.0]]*3 - trueTransPos = [[0.0, 0.0, 0.0]]*3 - trueTransVel = [[0.0, 0.0, 0.0]]*3 - trueTransAccum = [[0.0, 0.0, 0.0]]*3 + trueTransTimeTag = [[0.0]] * 3 + trueTransPos = [[0.0, 0.0, 0.0]] * 3 + trueTransVel = [[0.0, 0.0, 0.0]] * 3 + trueTransAccum = [[0.0, 0.0, 0.0]] * 3 if numAttNav == 1 or numAttNav == 11: - trueAttTimeTag = [[navAtt1Msg.timeTag]]*3 - trueAttSigma = [navAtt1Msg.sigma_BN]*3 - trueAttOmega = [navAtt1Msg.omega_BN_B]*3 - trueAttSunVector = [navAtt1Msg.vehSunPntBdy]*3 + trueAttTimeTag = [[navAtt1Msg.timeTag]] * 3 + trueAttSigma = [navAtt1Msg.sigma_BN] * 3 + trueAttOmega = [navAtt1Msg.omega_BN_B] * 3 + trueAttSunVector = [navAtt1Msg.vehSunPntBdy] * 3 if numTransNav == 1 or numTransNav == 11: - trueTransTimeTag = [[navTrans1Msg.timeTag]]*3 - trueTransPos = [navTrans1Msg.r_BN_N]*3 - trueTransVel = [navTrans1Msg.v_BN_N]*3 - trueTransAccum = [navTrans1Msg.vehAccumDV]*3 + trueTransTimeTag = [[navTrans1Msg.timeTag]] * 3 + trueTransPos = [navTrans1Msg.r_BN_N] * 3 + trueTransVel = [navTrans1Msg.v_BN_N] * 3 + trueTransAccum = [navTrans1Msg.vehAccumDV] * 3 if numAttNav == 2: trueAttTimeTag = [[navAtt2Msg.timeTag]] * 3 @@ -259,56 +287,78 @@ def navAggregateTestFunction(show_plots, numAttNav, numTransNav): trueAttSunVector = [navAtt2Msg.vehSunPntBdy] * 3 if numTransNav == 2: - trueTransTimeTag = [[navTrans2Msg.timeTag]]*3 - trueTransPos = [navTrans2Msg.r_BN_N]*3 - trueTransVel = [navTrans2Msg.v_BN_N]*3 - trueTransAccum = [navTrans2Msg.vehAccumDV]*3 + trueTransTimeTag = [[navTrans2Msg.timeTag]] * 3 + trueTransPos = [navTrans2Msg.r_BN_N] * 3 + trueTransVel = [navTrans2Msg.v_BN_N] * 3 + trueTransAccum = [navTrans2Msg.vehAccumDV] * 3 # compare the module results to the truth values accuracy = 1e-12 unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) # check if the module output matches the truth data - testFailCount, testMessages = unitTestSupport.compareArrayND(trueAttTimeTag, attTimeTag, - accuracy, "attTimeTag", 1, - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArray(trueAttSigma, attSigma, - accuracy, "sigma_BN", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArray(trueAttOmega, attOmega, - accuracy, "omega_BN_B", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArray(trueAttSunVector, attSunVector, - accuracy, "vehSunPntBdy", - testFailCount, testMessages) - - testFailCount, testMessages = unitTestSupport.compareArrayND(trueTransTimeTag, transTimeTag, - accuracy, "transTimeTag", 1, - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArray(trueTransPos, transPos, - accuracy, "sigma_BN", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArray(trueTransVel, transVel, - accuracy, "omega_BN_B", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArray(trueTransAccum, transAccum, - accuracy, "vehSunPntBdy", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + trueAttTimeTag, + attTimeTag, + accuracy, + "attTimeTag", + 1, + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArray( + trueAttSigma, attSigma, accuracy, "sigma_BN", testFailCount, testMessages + ) + testFailCount, testMessages = unitTestSupport.compareArray( + trueAttOmega, attOmega, accuracy, "omega_BN_B", testFailCount, testMessages + ) + testFailCount, testMessages = unitTestSupport.compareArray( + trueAttSunVector, + attSunVector, + accuracy, + "vehSunPntBdy", + testFailCount, + testMessages, + ) + + testFailCount, testMessages = unitTestSupport.compareArrayND( + trueTransTimeTag, + transTimeTag, + accuracy, + "transTimeTag", + 1, + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArray( + trueTransPos, transPos, accuracy, "sigma_BN", testFailCount, testMessages + ) + testFailCount, testMessages = unitTestSupport.compareArray( + trueTransVel, transVel, accuracy, "omega_BN_B", testFailCount, testMessages + ) + testFailCount, testMessages = unitTestSupport.compareArray( + trueTransAccum, + transAccum, + accuracy, + "vehSunPntBdy", + testFailCount, + testMessages, + ) if numAttNav == 11: if module.attMsgCount != navAggregate.MAX_AGG_NAV_MSG: testFailCount += 1 testMessages.append("FAILED numAttNav too large test") - if module.attTimeIdx != navAggregate.MAX_AGG_NAV_MSG-1: + if module.attTimeIdx != navAggregate.MAX_AGG_NAV_MSG - 1: testFailCount += 1 testMessages.append("FAILED attTimeIdx too large test") - if module.attIdx != navAggregate.MAX_AGG_NAV_MSG-1: + if module.attIdx != navAggregate.MAX_AGG_NAV_MSG - 1: testFailCount += 1 testMessages.append("FAILED attIdx too large test") - if module.rateIdx != navAggregate.MAX_AGG_NAV_MSG-1: + if module.rateIdx != navAggregate.MAX_AGG_NAV_MSG - 1: testFailCount += 1 testMessages.append("FAILED rateIdx too large test") - if module.sunIdx != navAggregate.MAX_AGG_NAV_MSG-1: + if module.sunIdx != navAggregate.MAX_AGG_NAV_MSG - 1: testFailCount += 1 testMessages.append("FAILED sunIdx too large test") @@ -316,29 +366,29 @@ def navAggregateTestFunction(show_plots, numAttNav, numTransNav): if module.transMsgCount != navAggregate.MAX_AGG_NAV_MSG: testFailCount += 1 testMessages.append("FAILED numTransNav too large test") - if module.posIdx != navAggregate.MAX_AGG_NAV_MSG-1: + if module.posIdx != navAggregate.MAX_AGG_NAV_MSG - 1: testFailCount += 1 testMessages.append("FAILED posIdx too large test") - if module.velIdx != navAggregate.MAX_AGG_NAV_MSG-1: + if module.velIdx != navAggregate.MAX_AGG_NAV_MSG - 1: testFailCount += 1 testMessages.append("FAILED velIdx too large test") - if module.dvIdx != navAggregate.MAX_AGG_NAV_MSG-1: + if module.dvIdx != navAggregate.MAX_AGG_NAV_MSG - 1: testFailCount += 1 testMessages.append("FAILED dvIdx too large test") # print out success message if no error were found snippentName = "passFail" + str(numAttNav) + str(numTransNav) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -347,7 +397,7 @@ def navAggregateTestFunction(show_plots, numAttNav, numTransNav): # if __name__ == "__main__": test_module( - False, - 2, # numAttNav - 2 # numTransNav - ) + False, + 2, # numAttNav + 2, # numTransNav + ) diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/_UnitTest/test_oeStateEphem.py b/src/fswAlgorithms/transDetermination/oeStateEphem/_UnitTest/test_oeStateEphem.py index 02118958c9..28a802be0a 100644 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/_UnitTest/test_oeStateEphem.py +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/_UnitTest/test_oeStateEphem.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -34,8 +33,9 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('fswAlgorithms') +splitPath = path.split("fswAlgorithms") from Basilisk import __path__ + bskPath = __path__[0] orbitPosAccuracy = 10000.0 @@ -44,15 +44,14 @@ unitTestSupport.writeTeXSnippet("toleranceVelValue", str(orbitVelAccuracy), path) -@pytest.mark.parametrize('validChebyCurveTime, anomFlag', [ - (True, 0), - (True, 1), - (True, -1), - (False, -1) -]) +@pytest.mark.parametrize( + "validChebyCurveTime, anomFlag", [(True, 0), (True, 1), (True, -1), (False, -1)] +) def test_chebyPosFitAllTest(show_plots, validChebyCurveTime, anomFlag): """Module Unit Test""" - [testResults, testMessage] = chebyPosFitAllTest(show_plots, validChebyCurveTime, anomFlag) + [testResults, testMessage] = chebyPosFitAllTest( + show_plots, validChebyCurveTime, anomFlag + ) assert testResults < 1, testMessage @@ -60,31 +59,31 @@ def chebyPosFitAllTest(show_plots, validChebyCurveTime, anomFlag): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. - #__tracebackhide__ = True + # __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages - numCurvePoints = 4*8640+1 - curveDurationSeconds = 4*86400 + numCurvePoints = 4 * 8640 + 1 + curveDurationSeconds = 4 * 86400 logPeriod = curveDurationSeconds // (numCurvePoints - 1) degChebCoeff = 14 integFrame = "j2000" zeroBase = "Earth" - centralBodyMu = 3.98574405096E14 + centralBodyMu = 3.98574405096e14 dateSpice = "2015 April 10, 00:00:00.0 TDB" - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/naif0012.tls') + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/naif0012.tls") et = pyswice.new_doubleArray(1) pyswice.str2et_c(dateSpice, et) etStart = pyswice.doubleArray_getitem(et, 0) etEnd = etStart + curveDurationSeconds - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de430.bsp') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/naif0012.tls') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de-403-masses.tpc') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/pck00010.tpc') - pyswice.furnsh_c(path + '/TDRSS.bsp') + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/de430.bsp") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/naif0012.tls") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/de-403-masses.tpc") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/pck00010.tpc") + pyswice.furnsh_c(path + "/TDRSS.bsp") tdrssPosList = [] tdrssVelList = [] @@ -101,10 +100,10 @@ def chebyPosFitAllTest(show_plots, validChebyCurveTime, anomFlag): anomCount = 0 for timeVal in timeHistory: - stringCurrent = pyswice.et2utc_c(timeVal, 'C', 4, 1024, "Yo") - stateOut = spkRead('-221', stringCurrent, integFrame, zeroBase) - position = stateOut[0:3]*1000.0 - velocity = stateOut[3:6]*1000.0 + stringCurrent = pyswice.et2utc_c(timeVal, "C", 4, 1024, "Yo") + stateOut = spkRead("-221", stringCurrent, integFrame, zeroBase) + position = stateOut[0:3] * 1000.0 + velocity = stateOut[3:6] * 1000.0 orbEl = orbitalMotion.rv2elem(centralBodyMu, position, velocity) tdrssPosList.append(position) tdrssVelList.append(velocity) @@ -114,12 +113,14 @@ def chebyPosFitAllTest(show_plots, validChebyCurveTime, anomFlag): OmegaArray.append(orbEl.Omega) omegaArray.append(orbEl.omega) if anomFlag == 1: - currentAnom = orbitalMotion.E2M(orbitalMotion.f2E(orbEl.f, orbEl.e), orbEl.e) + currentAnom = orbitalMotion.E2M( + orbitalMotion.f2E(orbEl.f, orbEl.e), orbEl.e + ) else: currentAnom = orbEl.f if currentAnom < anomPrev: anomCount += 1 - anomArray.append(2*math.pi*anomCount + currentAnom) + anomArray.append(2 * math.pi * anomCount + currentAnom) anomPrev = currentAnom tdrssPosList = numpy.array(tdrssPosList) @@ -129,9 +130,15 @@ def chebyPosFitAllTest(show_plots, validChebyCurveTime, anomFlag): chebRpCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, rpArray, degChebCoeff) chebEccCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, eccArray, degChebCoeff) chebIncCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, incArray, degChebCoeff) - chebOmegaCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, OmegaArray, degChebCoeff) - chebomegaCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, omegaArray, degChebCoeff) - chebAnomCoeff = numpy.polynomial.chebyshev.chebfit(fitTimes, anomArray, degChebCoeff) + chebOmegaCoeff = numpy.polynomial.chebyshev.chebfit( + fitTimes, OmegaArray, degChebCoeff + ) + chebomegaCoeff = numpy.polynomial.chebyshev.chebfit( + fitTimes, omegaArray, degChebCoeff + ) + chebAnomCoeff = numpy.polynomial.chebyshev.chebfit( + fitTimes, anomArray, degChebCoeff + ) unitTaskName = "unitTask" # arbitrary name (don't change) unitProcessName = "TestProcess" # arbitrary name (don't change) @@ -156,15 +163,17 @@ def chebyPosFitAllTest(show_plots, validChebyCurveTime, anomFlag): oeStateModel.ephArray[0].anomCoeff = chebAnomCoeff.tolist() oeStateModel.ephArray[0].RAANCoeff = chebOmegaCoeff.tolist() oeStateModel.ephArray[0].nChebCoeff = degChebCoeff + 1 - oeStateModel.ephArray[0].ephemTimeMid = etStart + curveDurationSeconds/2.0 - oeStateModel.ephArray[0].ephemTimeRad = curveDurationSeconds/2.0 + oeStateModel.ephArray[0].ephemTimeMid = etStart + curveDurationSeconds / 2.0 + oeStateModel.ephArray[0].ephemTimeRad = curveDurationSeconds / 2.0 if not (anomFlag == -1): oeStateModel.ephArray[0].anomalyFlag = anomFlag clockCorrData = messaging.TDBVehicleClockCorrelationMsgPayload() clockCorrData.vehicleClockTime = 0.0 - clockCorrData.ephemerisTime = oeStateModel.ephArray[0].ephemTimeMid - oeStateModel.ephArray[0].ephemTimeRad + clockCorrData.ephemerisTime = ( + oeStateModel.ephArray[0].ephemTimeMid - oeStateModel.ephArray[0].ephemTimeRad + ) clockInMsg = messaging.TDBVehicleClockCorrelationMsg().write(clockCorrData) oeStateModel.clockCorrInMsg.subscribeTo(clockInMsg) @@ -176,11 +185,11 @@ def chebyPosFitAllTest(show_plots, validChebyCurveTime, anomFlag): sim.InitializeSimulation() # increase the run time by one logging period so that the sim time is outside the # valid chebychev curve duration - sim.ConfigureStopTime(int((curveDurationSeconds + logPeriod) * 1.0E9)) + sim.ConfigureStopTime(int((curveDurationSeconds + logPeriod) * 1.0e9)) sim.ExecuteSimulation() else: sim.InitializeSimulation() - sim.ConfigureStopTime(int(curveDurationSeconds*1.0E9)) + sim.ConfigureStopTime(int(curveDurationSeconds * 1.0e9)) sim.ExecuteSimulation() posChebData = dataLog.r_BdyZero_N @@ -192,137 +201,173 @@ def chebyPosFitAllTest(show_plots, validChebyCurveTime, anomFlag): lastPos = posChebData[lastLogidx, 0:] - tdrssPosList[lastLogidx, :] if not numpy.array_equal(secondLastPos, lastPos): testFailCount += 1 - testMessages.append("FAILED: Expected Chebychev position to rail high or low " - + str(secondLastPos) - + " != " - + str(lastPos)) + testMessages.append( + "FAILED: Expected Chebychev position to rail high or low " + + str(secondLastPos) + + " != " + + str(lastPos) + ) secondLastVel = velChebData[lastLogidx + 1, 0:] - tdrssVelList[lastLogidx, :] lastVel = velChebData[lastLogidx, 0:] - tdrssVelList[lastLogidx, :] if not numpy.array_equal(secondLastVel, lastVel): testFailCount += 1 - testMessages.append("FAILED: Expected Chebychev velocity to rail high or low " - + str(secondLastVel) - + " != " - + str(lastVel)) + testMessages.append( + "FAILED: Expected Chebychev velocity to rail high or low " + + str(secondLastVel) + + " != " + + str(lastVel) + ) else: - maxErrVec = [abs(max(posChebData[:, 0] - tdrssPosList[:, 0])), - abs(max(posChebData[:, 1] - tdrssPosList[:, 1])), - abs(max(posChebData[:,2] - tdrssPosList[:, 2]))] - maxVelErrVec = [abs(max(velChebData[:, 0] - tdrssVelList[:, 0])), - abs(max(velChebData[:, 1] - tdrssVelList[:, 1])), - abs(max(velChebData[:, 2] - tdrssVelList[:, 2]))] + maxErrVec = [ + abs(max(posChebData[:, 0] - tdrssPosList[:, 0])), + abs(max(posChebData[:, 1] - tdrssPosList[:, 1])), + abs(max(posChebData[:, 2] - tdrssPosList[:, 2])), + ] + maxVelErrVec = [ + abs(max(velChebData[:, 0] - tdrssVelList[:, 0])), + abs(max(velChebData[:, 1] - tdrssVelList[:, 1])), + abs(max(velChebData[:, 2] - tdrssVelList[:, 2])), + ] if max(maxErrVec) >= orbitPosAccuracy: testFailCount += 1 - testMessages.append("FAILED: maxErrVec >= orbitPosAccuracy, TDRSS Orbit Accuracy: " - + str(max(maxErrVec))) + testMessages.append( + "FAILED: maxErrVec >= orbitPosAccuracy, TDRSS Orbit Accuracy: " + + str(max(maxErrVec)) + ) if max(maxVelErrVec) >= orbitVelAccuracy: testFailCount += 1 - testMessages.append("FAILED: maxVelErrVec >= orbitVelAccuracy, TDRSS Velocity Accuracy: " - + str(max(maxVelErrVec))) + testMessages.append( + "FAILED: maxVelErrVec >= orbitVelAccuracy, TDRSS Velocity Accuracy: " + + str(max(maxVelErrVec)) + ) plt.close("all") # plot the fitted and actual position coordinates plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") for idx in range(0, 3): - plt.plot(dataLog.times()*macros.NANO2HOUR, - posChebData[:, idx]/1000, - color=unitTestSupport.getLineColor(idx, 3), - linewidth=0.5, - label='$r_{fit,' + str(idx) + '}$') - plt.plot(dataLog.times()*macros.NANO2HOUR, - tdrssPosList[:, idx]/1000, - color=unitTestSupport.getLineColor(idx, 3), - linestyle='dashed', linewidth=2, - label='$r_{true,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [h]') - plt.ylabel('Inertial Position [km]') + plt.plot( + dataLog.times() * macros.NANO2HOUR, + posChebData[:, idx] / 1000, + color=unitTestSupport.getLineColor(idx, 3), + linewidth=0.5, + label="$r_{fit," + str(idx) + "}$", + ) + plt.plot( + dataLog.times() * macros.NANO2HOUR, + tdrssPosList[:, idx] / 1000, + color=unitTestSupport.getLineColor(idx, 3), + linestyle="dashed", + linewidth=2, + label="$r_{true," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [h]") + plt.ylabel("Inertial Position [km]") # plot the fitted and actual velocity coordinates plt.figure(2) for idx in range(0, 3): - plt.plot(dataLog.times()*macros.NANO2HOUR, - velChebData[:, idx]/1000, - color=unitTestSupport.getLineColor(idx, 3), - linewidth=0.5, - label='$v_{fit,' + str(idx) + '}$') - plt.plot(dataLog.times()*macros.NANO2HOUR, - tdrssVelList[:, idx]/1000, - color=unitTestSupport.getLineColor(idx, 3), - linestyle='dashed', linewidth=2, - label='$v_{true,' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [h]') - plt.ylabel('Velocity [km/s]') + plt.plot( + dataLog.times() * macros.NANO2HOUR, + velChebData[:, idx] / 1000, + color=unitTestSupport.getLineColor(idx, 3), + linewidth=0.5, + label="$v_{fit," + str(idx) + "}$", + ) + plt.plot( + dataLog.times() * macros.NANO2HOUR, + tdrssVelList[:, idx] / 1000, + color=unitTestSupport.getLineColor(idx, 3), + linestyle="dashed", + linewidth=2, + label="$v_{true," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [h]") + plt.ylabel("Velocity [km/s]") # plot the difference in position coordinates plt.figure(3) arrayLength = posChebData[:, 0].size - for idx in range(0,3): - plt.plot(dataLog.times() * macros.NANO2HOUR, - posChebData[:, idx] - tdrssPosList[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - linewidth=0.5, - label=r'$\Delta r_{' + str(idx) + '}$') - plt.plot(dataLog.times() * macros.NANO2HOUR, - orbitPosAccuracy*numpy.ones(arrayLength), - color='r', - linewidth=1) - plt.plot(dataLog.times() * macros.NANO2HOUR, - -orbitPosAccuracy * numpy.ones(arrayLength), - color='r', - linewidth=1) - plt.legend(loc='lower right') - plt.xlabel('Time [h]') - plt.ylabel('Position Difference [m]') + for idx in range(0, 3): + plt.plot( + dataLog.times() * macros.NANO2HOUR, + posChebData[:, idx] - tdrssPosList[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + linewidth=0.5, + label=r"$\Delta r_{" + str(idx) + "}$", + ) + plt.plot( + dataLog.times() * macros.NANO2HOUR, + orbitPosAccuracy * numpy.ones(arrayLength), + color="r", + linewidth=1, + ) + plt.plot( + dataLog.times() * macros.NANO2HOUR, + -orbitPosAccuracy * numpy.ones(arrayLength), + color="r", + linewidth=1, + ) + plt.legend(loc="lower right") + plt.xlabel("Time [h]") + plt.ylabel("Position Difference [m]") # plot the difference in velocity coordinates plt.figure(4) arrayLength = velChebData[:, 0].size - for idx in range(0,3): - plt.plot(dataLog.times() * macros.NANO2HOUR, - velChebData[:, idx] - tdrssVelList[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - linewidth=0.5, - label=r'$\Delta v_{' + str(idx) + '}$') - plt.plot(dataLog.times() * macros.NANO2HOUR, - orbitVelAccuracy*numpy.ones(arrayLength), - color='r', - linewidth=1) - plt.plot(dataLog.times() * macros.NANO2HOUR, - -orbitVelAccuracy * numpy.ones(arrayLength), - color='r', - linewidth=1) - plt.legend(loc='lower right') - plt.xlabel('Time [h]') - plt.ylabel('Velocity Difference [m/s]') + for idx in range(0, 3): + plt.plot( + dataLog.times() * macros.NANO2HOUR, + velChebData[:, idx] - tdrssVelList[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + linewidth=0.5, + label=r"$\Delta v_{" + str(idx) + "}$", + ) + plt.plot( + dataLog.times() * macros.NANO2HOUR, + orbitVelAccuracy * numpy.ones(arrayLength), + color="r", + linewidth=1, + ) + plt.plot( + dataLog.times() * macros.NANO2HOUR, + -orbitVelAccuracy * numpy.ones(arrayLength), + color="r", + linewidth=1, + ) + plt.legend(loc="lower right") + plt.xlabel("Time [h]") + plt.ylabel("Velocity Difference [m/s]") if show_plots: plt.show() - plt.close('all') + plt.close("all") snippentName = "passFail" + str(validChebyCurveTime) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + oeStateModel.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + oeStateModel.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": - chebyPosFitAllTest(True, # showPlots - True, # validChebyCurveTime - 1) # anomFlag + chebyPosFitAllTest( + True, # showPlots + True, # validChebyCurveTime + 1, + ) # anomFlag diff --git a/src/fswAlgorithms/vehicleConfigData/_UnitTest/test_vehicleConfigData.py b/src/fswAlgorithms/vehicleConfigData/_UnitTest/test_vehicleConfigData.py index 3889a07e2a..633d93fc2e 100644 --- a/src/fswAlgorithms/vehicleConfigData/_UnitTest/test_vehicleConfigData.py +++ b/src/fswAlgorithms/vehicleConfigData/_UnitTest/test_vehicleConfigData.py @@ -7,7 +7,9 @@ from Basilisk.fswAlgorithms import vehicleConfigData from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions def test_vehicleConfigData(): @@ -16,8 +18,9 @@ def test_vehicleConfigData(): assert testResults < 1, testMessage + def vehicleConfigDataTestFunction(): - """ Test the vehicleConfigData module """ + """Test the vehicleConfigData module""" testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -30,18 +33,18 @@ def vehicleConfigDataTestFunction(): # Create test thread testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) - testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Add a new task to the process + testProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, testProcessRate) + ) # Add a new task to the process # Construct the cssComm module module = vehicleConfigData.vehicleConfigData() # Populate the config - I = [1000., 0., 0., - 0., 800., 0., - 0., 0., 800.] + I = [1000.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 800.0] module.ISCPntB_B = I initialCoM = [1, 1, 1] module.CoM_B = initialCoM - mass = 300. + mass = 300.0 module.massSC = mass module.ModelTag = "vehicleConfigData" @@ -67,22 +70,40 @@ def vehicleConfigDataTestFunction(): accuracy = 1e-6 # At each timestep, make sure the vehicleConfig values haven't changed from the initial values - testFailCount, testMessages = unitTestSupport.compareArrayND([initialCoM for _ in range(len(CoMLog))], CoMLog, accuracy, - "VehicleConfigData CoM", - 3, testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArrayND([I for _ in range(len(Ilog))], Ilog, accuracy, - "VehicleConfigData I", - 3, testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareDoubleArray([mass for _ in range(len(MassLog))], MassLog, accuracy, - "VehicleConfigData Mass", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayND( + [initialCoM for _ in range(len(CoMLog))], + CoMLog, + accuracy, + "VehicleConfigData CoM", + 3, + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArrayND( + [I for _ in range(len(Ilog))], + Ilog, + accuracy, + "VehicleConfigData I", + 3, + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareDoubleArray( + [mass for _ in range(len(MassLog))], + MassLog, + accuracy, + "VehicleConfigData Mass", + testFailCount, + testMessages, + ) if testFailCount == 0: print("PASSED: " + module.ModelTag) else: print(testMessages) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + -if __name__ == '__main__': +if __name__ == "__main__": test_vehicleConfigData() diff --git a/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplate.py b/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplate.py index d47649d06f..fc925fae2f 100644 --- a/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplate.py +++ b/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplate.py @@ -30,11 +30,16 @@ import numpy as np from Basilisk.architecture import bskLogging from Basilisk.architecture import messaging # import the message definitions -from Basilisk.moduleTemplates import cModuleTemplate # import the module that is to be tested +from Basilisk.moduleTemplates import ( + cModuleTemplate, +) # import the module that is to be tested + # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -42,7 +47,9 @@ # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(conditionstring) # provide a unique test method name, starting with test_ -def test_module(show_plots): # update "module" in this function name to reflect the module name +def test_module( + show_plots, +): # update "module" in this function name to reflect the module name r""" **Validation Test Description** @@ -77,49 +84,49 @@ def test_module(show_plots): # update "module" in this function name to refl def fswModuleTestFunction(show_plots): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = cModuleTemplate.cModuleTemplate() - module.ModelTag = "cModuleTemplate" # update python name of test module + module.ModelTag = "cModuleTemplate" # update python name of test module # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - module.dummy = 1 # update module parameter with required values - module.dumVector = [1., 2., 3.] + module.dummy = 1 # update module parameter with required values + module.dumVector = [1.0, 2.0, 3.0] # Create input message and size it because the regular creator of that message # is not part of the test. - inputMessageData = messaging.CModuleTemplateMsgPayload() # Create a structure for the input message - inputMessageData.dataVector = [1.0, -0.5, 0.7] # Set up a list as a 3-vector + inputMessageData = ( + messaging.CModuleTemplateMsgPayload() + ) # Create a structure for the input message + inputMessageData.dataVector = [1.0, -0.5, 0.7] # Set up a list as a 3-vector inputMsg = messaging.CModuleTemplateMsg().write(inputMessageData) # Setup logging on the test module output message so that we get all the writes to it dataLog = module.dataOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) - variableName = "dummy" # name the module variable to be logged + variableName = "dummy" # name the module variable to be logged moduleLog = module.logger(variableName) unitTestSim.AddModelToTask(unitTaskName, moduleLog) # connect the message interfaces module.dataInMsg.subscribeTo(inputMsg) - # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() @@ -127,31 +134,32 @@ def fswModuleTestFunction(show_plots): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() # reset the module to test this functionality - module.Reset(1) # this module reset function needs a time input (in NanoSeconds) + module.Reset(1) # this module reset function needs a time input (in NanoSeconds) # run the module again for an additional 1.0 seconds - unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - # This pulls the actual data log from the simulation run. # Note that range(3) will provide [0, 1, 2] Those are the elements you get from the vector (all of them) - variableState = unitTestSupport.addTimeColumn(moduleLog.times(), getattr(moduleLog, variableName)) + variableState = unitTestSupport.addTimeColumn( + moduleLog.times(), getattr(moduleLog, variableName) + ) # set the filtered output truth states trueVector = [ - [2.0, -0.5, 0.7], - [3.0, -0.5, 0.7], - [4.0, -0.5, 0.7], - [2.0, -0.5, 0.7], - [3.0, -0.5, 0.7] - ] + [2.0, -0.5, 0.7], + [3.0, -0.5, 0.7], + [4.0, -0.5, 0.7], + [2.0, -0.5, 0.7], + [3.0, -0.5, 0.7], + ] # compare the module results to the truth values accuracy = 1e-12 @@ -159,24 +167,37 @@ def fswModuleTestFunction(show_plots): variableStateNoTime = np.transpose(variableState)[1] for i in range(0, len(trueVector)): # check a vector values - if not unitTestSupport.isArrayEqual(dataLog.dataVector[i], trueVector[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + dataLog.dataVector[i], trueVector[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed dataVector" + - " unit test at t=" + - str(dataLog.times()[i]*macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed dataVector" + + " unit test at t=" + + str(dataLog.times()[i] * macros.NANO2SEC) + + "sec\n" + ) # check a scalar double value - if not unitTestSupport.isDoubleEqual(variableStateNoTime[i], dummyTrue[i], accuracy): + if not unitTestSupport.isDoubleEqual( + variableStateNoTime[i], dummyTrue[i], accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed " + - variableName + " unit test at t=" + - str(variableState[i, 0]*macros.NANO2SEC) + - "sec\n") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed " + + variableName + + " unit test at t=" + + str(variableState[i, 0] * macros.NANO2SEC) + + "sec\n" + ) # Note that we can continue to step the simulation however we feel like. # Just because we stop and query data does not mean everything has to stop for good - unitTestSim.ConfigureStopTime(macros.sec2nano(2.6)) # run an additional 0.6 seconds + unitTestSim.ConfigureStopTime(macros.sec2nano(2.6)) # run an additional 0.6 seconds unitTestSim.ExecuteSimulation() # print out success message if no error were found @@ -190,25 +211,28 @@ def fswModuleTestFunction(show_plots): plt.close("all") # close all prior figures so we start with a clean slate plt.figure(1) plt.plot(variableState[:, 0] * macros.NANO2SEC, variableState[:, 1]) - plt.xlabel('Time [s]') - plt.ylabel('Variable Description [unit]') - plt.suptitle('Title of Sample Plot') + plt.xlabel("Time [s]") + plt.ylabel("Variable Description [unit]") + plt.suptitle("Title of Sample Plot") plt.figure(2) for idx in range(3): - plt.plot(dataLog.times() * macros.NANO2MIN, dataLog.dataVector[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$s_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Msg Output Vector States') + plt.plot( + dataLog.times() * macros.NANO2MIN, + dataLog.dataVector[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$s_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Msg Output Vector States") if show_plots: plt.show() # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -217,5 +241,5 @@ def fswModuleTestFunction(show_plots): # if __name__ == "__main__": fswModuleTestFunction( - True # show_plots + True # show_plots ) diff --git a/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplateParametrized.py b/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplateParametrized.py index 36ff3e3e5f..a58a5a766a 100755 --- a/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplateParametrized.py +++ b/src/moduleTemplates/cModuleTemplate/_UnitTest/test_cModuleTemplateParametrized.py @@ -32,22 +32,21 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) - - - - - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import matplotlib.pyplot as plt -from Basilisk.moduleTemplates import cModuleTemplate # import the module that is to be tested +from Basilisk.moduleTemplates import ( + cModuleTemplate, +) # import the module that is to be tested from Basilisk.utilities import macros -from Basilisk.architecture import messaging # import the message definitions +from Basilisk.architecture import messaging # import the message definitions from Basilisk.architecture import bskLogging @@ -61,11 +60,7 @@ # matters for the documentation in that it impacts the order in which the test arguments are shown. # The first parametrize arguments are shown last in the pytest argument list @pytest.mark.parametrize("accuracy", [1e-12]) -@pytest.mark.parametrize("param1, param2", [ - (1, 1) - ,(1, 3) - ,(2, 2) -]) +@pytest.mark.parametrize("param1, param2", [(1, 1), (1, 3), (2, 2)]) # update "module" in this function name to reflect the module name def test_module(show_plots, param1, param2, accuracy): @@ -117,41 +112,44 @@ def test_module(show_plots, param1, param2, accuracy): contained within this HTML ``pytest`` report. """ # each test method requires a single assert method to be called - [testResults, testMessage] = fswModuleTestFunction(show_plots, param1, param2, accuracy) + [testResults, testMessage] = fswModuleTestFunction( + show_plots, param1, param2, accuracy + ) assert testResults < 1, testMessage def fswModuleTestFunction(show_plots, param1, param2, accuracy): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container module = cModuleTemplate.cModuleTemplate() - module.ModelTag = "cModuleTemplate" # update python name of test module + module.ModelTag = "cModuleTemplate" # update python name of test module # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - module.dummy = 1 # update module parameter with required values - module.dumVector = [1., 2., 3.] + module.dummy = 1 # update module parameter with required values + module.dumVector = [1.0, 2.0, 3.0] # Create input message and size it because the regular creator of that message # is not part of the test. - inputMessageData = messaging.CModuleTemplateMsgPayload() # Create a structure for the input message - inputMessageData.dataVector = [param1, param2, 0.7] # Set up a list as a 3-vector + inputMessageData = ( + messaging.CModuleTemplateMsgPayload() + ) # Create a structure for the input message + inputMessageData.dataVector = [param1, param2, 0.7] # Set up a list as a 3-vector inputMsg = messaging.CModuleTemplateMsg().write(inputMessageData) module.dataInMsg.subscribeTo(inputMsg) @@ -159,7 +157,7 @@ def fswModuleTestFunction(show_plots, param1, param2, accuracy): dataLog = module.dataOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) - variableName = "dummy" # name the module variable to be logged + variableName = "dummy" # name the module variable to be logged moduleLog = module.logger(variableName) unitTestSim.AddModelToTask(unitTaskName, moduleLog) @@ -170,96 +168,120 @@ def fswModuleTestFunction(show_plots, param1, param2, accuracy): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() # reset the module to test this functionality - module.Reset(1) # this module reset function needs a time input (in NanoSeconds) + module.Reset(1) # this module reset function needs a time input (in NanoSeconds) # run the module again for an additional 1.0 seconds - unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - # This pulls the BSK module internal varialbe log from the simulation run. # Note, this should only be done for debugging as it is a slow process - variableState = unitTestSupport.addTimeColumn(moduleLog.times(), getattr(moduleLog, variableName)) + variableState = unitTestSupport.addTimeColumn( + moduleLog.times(), getattr(moduleLog, variableName) + ) # set the filtered output truth states trueVector = [] if param1 == 1: if param2 == 1: trueVector = [ - [2.0, 1.0, 0.7], - [3.0, 1.0, 0.7], - [4.0, 1.0, 0.7], - [2.0, 1.0, 0.7], - [3.0, 1.0, 0.7] - ] + [2.0, 1.0, 0.7], + [3.0, 1.0, 0.7], + [4.0, 1.0, 0.7], + [2.0, 1.0, 0.7], + [3.0, 1.0, 0.7], + ] else: if param2 == 3: trueVector = [ - [2.0, 3.0, 0.7], - [3.0, 3.0, 0.7], - [4.0, 3.0, 0.7], - [2.0, 3.0, 0.7], - [3.0, 3.0, 0.7] - ] + [2.0, 3.0, 0.7], + [3.0, 3.0, 0.7], + [4.0, 3.0, 0.7], + [2.0, 3.0, 0.7], + [3.0, 3.0, 0.7], + ] else: testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag - + " Module failed with unsupported input parameters") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed with unsupported input parameters" + ) else: if param1 == 2: trueVector = [ - [3.0, 2.0, 0.7], - [4.0, 2.0, 0.7], - [5.0, 2.0, 0.7], - [3.0, 2.0, 0.7], - [4.0, 2.0, 0.7] - ] + [3.0, 2.0, 0.7], + [4.0, 2.0, 0.7], + [5.0, 2.0, 0.7], + [3.0, 2.0, 0.7], + [4.0, 2.0, 0.7], + ] else: testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed with unsupported input parameters") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed with unsupported input parameters" + ) # compare the module results to the truth values dummyTrue = [1.0, 2.0, 3.0, 1.0, 2.0] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, dataLog.dataVector, - accuracy, "Output Vector", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + dataLog.dataVector, + accuracy, + "Output Vector", + testFailCount, + testMessages, + ) variableState = np.transpose(variableState)[1] - testFailCount, testMessages = unitTestSupport.compareDoubleArray(dummyTrue, variableState, - accuracy, "dummy parameter", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareDoubleArray( + dummyTrue, + variableState, + accuracy, + "dummy parameter", + testFailCount, + testMessages, + ) # Note that we can continue to step the simulation however we feel like. # Just because we stop and query data does not mean everything has to stop for good - unitTestSim.ConfigureStopTime(macros.sec2nano(0.6)) # run an additional 0.6 seconds + unitTestSim.ConfigureStopTime(macros.sec2nano(0.6)) # run an additional 0.6 seconds unitTestSim.ExecuteSimulation() # If the argument provided at commandline "--show_plots" evaluates as true, # plot all figures # plot a sample variable. - plt.close("all") # close all prior figures so we start with a clean slate + plt.close("all") # close all prior figures so we start with a clean slate plt.figure(1) - plt.plot(dataLog.times()*macros.NANO2SEC, variableState, - label='Case param1 = ' + str(param1) + ' and param2 = ' + str(param2)) - plt.legend(loc='upper left') - plt.xlabel('Time [s]') - plt.ylabel('Variable Description [unit]') - plt.suptitle('Title of Sample Plot') + plt.plot( + dataLog.times() * macros.NANO2SEC, + variableState, + label="Case param1 = " + str(param1) + " and param2 = " + str(param2), + ) + plt.legend(loc="upper left") + plt.xlabel("Time [s]") + plt.ylabel("Variable Description [unit]") + plt.suptitle("Title of Sample Plot") plt.figure(2) for idx in range(3): - plt.plot(dataLog.times() * macros.NANO2MIN, dataLog.dataVector[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$s_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Msg Output Vector States') + plt.plot( + dataLog.times() * macros.NANO2MIN, + dataLog.dataVector[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$s_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Msg Output Vector States") if show_plots: plt.show() @@ -270,7 +292,7 @@ def fswModuleTestFunction(show_plots, param1, param2, accuracy): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -278,9 +300,9 @@ def fswModuleTestFunction(show_plots, param1, param2, accuracy): # stand-along python script # if __name__ == "__main__": - test_module( # update "module" in function name - False, - 1, # param1 value - 1, # param2 value - 1e-12 # accuracy - ) + test_module( # update "module" in function name + False, + 1, # param1 value + 1, # param2 value + 1e-12, # accuracy + ) diff --git a/src/moduleTemplates/cppModuleTemplate/_UnitTest/test_cppModuleTemplateParametrized.py b/src/moduleTemplates/cppModuleTemplate/_UnitTest/test_cppModuleTemplateParametrized.py index 69cef36de6..89b2b8807a 100755 --- a/src/moduleTemplates/cppModuleTemplate/_UnitTest/test_cppModuleTemplateParametrized.py +++ b/src/moduleTemplates/cppModuleTemplate/_UnitTest/test_cppModuleTemplateParametrized.py @@ -32,22 +32,21 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) - - - - - # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import matplotlib.pyplot as plt -from Basilisk.moduleTemplates import cppModuleTemplate # import the module that is to be tested +from Basilisk.moduleTemplates import ( + cppModuleTemplate, +) # import the module that is to be tested from Basilisk.utilities import macros -from Basilisk.architecture import messaging # import the message definitions +from Basilisk.architecture import messaging # import the message definitions from Basilisk.architecture import bskLogging @@ -61,11 +60,7 @@ # matters for the documentation in that it impacts the order in which the test arguments are shown. # The first parametrize arguments are shown last in the pytest argument list @pytest.mark.parametrize("accuracy", [1e-12]) -@pytest.mark.parametrize("param1, param2", [ - (1, 1) - ,(1, 3) - ,(2, 2) -]) +@pytest.mark.parametrize("param1, param2", [(1, 1), (1, 3), (2, 2)]) # update "module" in this function name to reflect the module name def test_module(show_plots, param1, param2, accuracy): @@ -117,42 +112,45 @@ def test_module(show_plots, param1, param2, accuracy): contained within this HTML ``pytest`` report. """ # each test method requires a single assert method to be called - [testResults, testMessage] = cppModuleTestFunction(show_plots, param1, param2, accuracy) + [testResults, testMessage] = cppModuleTestFunction( + show_plots, param1, param2, accuracy + ) assert testResults < 1, testMessage def cppModuleTestFunction(show_plots, param1, param2, accuracy): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - # Construct algorithm and associated C++ container - module = cppModuleTemplate.CppModuleTemplate() # update with current values - module.ModelTag = "cppModuleTemplate" # update python name of test module + module = cppModuleTemplate.CppModuleTemplate() # update with current values + module.ModelTag = "cppModuleTemplate" # update python name of test module # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) # Initialize the test module configuration data - module.setDummy(1) # update module parameter with required values + module.setDummy(1) # update module parameter with required values with pytest.raises(bskLogging.BasiliskError): - module.setDumVector([1., -2., 3.]) + module.setDumVector([1.0, -2.0, 3.0]) # Create input message and size it because the regular creator of that message # is not part of the test. - inputMessageData = messaging.CModuleTemplateMsgPayload() # Create a structure for the input message - inputMessageData.dataVector = [param1, param2, 0.7] # Set up a list as a 3-vector + inputMessageData = ( + messaging.CModuleTemplateMsgPayload() + ) # Create a structure for the input message + inputMessageData.dataVector = [param1, param2, 0.7] # Set up a list as a 3-vector inputMsg = messaging.CModuleTemplateMsg().write(inputMessageData) module.dataInMsg.subscribeTo(inputMsg) @@ -160,7 +158,7 @@ def cppModuleTestFunction(show_plots, param1, param2, accuracy): dataLog = module.dataOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) - variableName = "dummy" # name the module variable to be logged + variableName = "dummy" # name the module variable to be logged moduleLog = module.logger(variableName) unitTestSim.AddModelToTask(unitTaskName, moduleLog) @@ -171,96 +169,120 @@ def cppModuleTestFunction(show_plots, param1, param2, accuracy): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() # reset the module to test this functionality - module.Reset(1) # this module reset function needs a time input (in NanoSeconds) + module.Reset(1) # this module reset function needs a time input (in NanoSeconds) # run the module again for an additional 1.0 seconds - unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(2.0)) # seconds to stop simulation unitTestSim.ExecuteSimulation() - # This pulls the BSK module internal varialbe log from the simulation run. # Note, this should only be done for debugging as it is a slow process - variableState = unitTestSupport.addTimeColumn(moduleLog.times(), getattr(moduleLog, variableName)) + variableState = unitTestSupport.addTimeColumn( + moduleLog.times(), getattr(moduleLog, variableName) + ) # set the filtered output truth states trueVector = [] if param1 == 1: if param2 == 1: trueVector = [ - [2.0, 1.0, 0.7], - [3.0, 1.0, 0.7], - [4.0, 1.0, 0.7], - [2.0, 1.0, 0.7], - [3.0, 1.0, 0.7] - ] + [2.0, 1.0, 0.7], + [3.0, 1.0, 0.7], + [4.0, 1.0, 0.7], + [2.0, 1.0, 0.7], + [3.0, 1.0, 0.7], + ] else: if param2 == 3: trueVector = [ - [2.0, 3.0, 0.7], - [3.0, 3.0, 0.7], - [4.0, 3.0, 0.7], - [2.0, 3.0, 0.7], - [3.0, 3.0, 0.7] - ] + [2.0, 3.0, 0.7], + [3.0, 3.0, 0.7], + [4.0, 3.0, 0.7], + [2.0, 3.0, 0.7], + [3.0, 3.0, 0.7], + ] else: testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag - + " Module failed with unsupported input parameters") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed with unsupported input parameters" + ) else: if param1 == 2: trueVector = [ - [3.0, 2.0, 0.7], - [4.0, 2.0, 0.7], - [5.0, 2.0, 0.7], - [3.0, 2.0, 0.7], - [4.0, 2.0, 0.7] - ] + [3.0, 2.0, 0.7], + [4.0, 2.0, 0.7], + [5.0, 2.0, 0.7], + [3.0, 2.0, 0.7], + [4.0, 2.0, 0.7], + ] else: testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed with unsupported input parameters") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed with unsupported input parameters" + ) # compare the module results to the truth values dummyTrue = [1.0, 2.0, 3.0, 1.0, 2.0] - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, dataLog.dataVector, - accuracy, "Output Vector", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, + dataLog.dataVector, + accuracy, + "Output Vector", + testFailCount, + testMessages, + ) variableState = np.transpose(variableState)[1] - testFailCount, testMessages = unitTestSupport.compareDoubleArray(dummyTrue, variableState, - accuracy, "dummy parameter", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareDoubleArray( + dummyTrue, + variableState, + accuracy, + "dummy parameter", + testFailCount, + testMessages, + ) # Note that we can continue to step the simulation however we feel like. # Just because we stop and query data does not mean everything has to stop for good - unitTestSim.ConfigureStopTime(macros.sec2nano(0.6)) # run an additional 0.6 seconds + unitTestSim.ConfigureStopTime(macros.sec2nano(0.6)) # run an additional 0.6 seconds unitTestSim.ExecuteSimulation() # If the argument provided at commandline "--show_plots" evaluates as true, # plot all figures # plot a sample variable. - plt.close("all") # close all prior figures so we start with a clean slate + plt.close("all") # close all prior figures so we start with a clean slate plt.figure(1) - plt.plot(dataLog.times()*macros.NANO2SEC, variableState, - label='Case param1 = ' + str(param1) + ' and param2 = ' + str(param2)) - plt.legend(loc='upper left') - plt.xlabel('Time [s]') - plt.ylabel('Variable Description [unit]') - plt.suptitle('Title of Sample Plot') + plt.plot( + dataLog.times() * macros.NANO2SEC, + variableState, + label="Case param1 = " + str(param1) + " and param2 = " + str(param2), + ) + plt.legend(loc="upper left") + plt.xlabel("Time [s]") + plt.ylabel("Variable Description [unit]") + plt.suptitle("Title of Sample Plot") plt.figure(2) for idx in range(3): - plt.plot(dataLog.times() * macros.NANO2MIN, dataLog.dataVector[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$s_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Msg Output Vector States') + plt.plot( + dataLog.times() * macros.NANO2MIN, + dataLog.dataVector[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$s_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Msg Output Vector States") if show_plots: plt.show() @@ -271,7 +293,7 @@ def cppModuleTestFunction(show_plots, param1, param2, accuracy): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -279,9 +301,9 @@ def cppModuleTestFunction(show_plots, param1, param2, accuracy): # stand-along python script # if __name__ == "__main__": - test_module( # update "module" in function name - False, - 1, # param1 value - 1, # param2 value - 1e-12 # accuracy - ) + test_module( # update "module" in function name + False, + 1, # param1 value + 1, # param2 value + 1e-12, # accuracy + ) diff --git a/src/reportconf.py b/src/reportconf.py index 769053bd15..b7a63145b2 100644 --- a/src/reportconf.py +++ b/src/reportconf.py @@ -30,42 +30,46 @@ import matplotlib.pyplot as plt import pytest -make_report = ('--report' in sys.argv) -report_dir = 'tests/report/' # relative to current folder -rel_fig_dir = 'assets/testFigs/' # relative to report/ +make_report = "--report" in sys.argv +report_dir = "tests/report/" # relative to current folder +rel_fig_dir = "assets/testFigs/" # relative to report/ testFigsDir = report_dir + rel_fig_dir # can assume this is run from basilisk/src/ if make_report: os.makedirs(testFigsDir) def get_test_name(item): # just get the name of the test from the item function - return str(item.function).split(' ')[1] + return str(item.function).split(" ")[1] def get_docstring(item): if item.function.__doc__: - return '' \ - + textwrap.dedent(str(item.function.__doc__)).replace('``', '') + '' + return ( + '' + + textwrap.dedent(str(item.function.__doc__)).replace("``", "") + + "" + ) else: - return ' ' \ - 'This test does not have a docstring
' + return ( + ' ' + "This test does not have a docstring
" + ) @pytest.hookimpl(hookwrapper=True) def pytest_runtest_makereport(item, call): """ - We (Basilisk) use this function to do two things: - 1) append the docstrings to the test log extras - 2) print test plots to the test log extras + We (Basilisk) use this function to do two things: + 1) append the docstrings to the test log extras + 2) print test plots to the test log extras - This is kept neat by inserting a table into the extras to separate the two things. + This is kept neat by inserting a table into the extras to separate the two things. """ - pytest_html = item.config.pluginmanager.getplugin('html') + pytest_html = item.config.pluginmanager.getplugin("html") outcome = yield if item.config.option.htmlpath: # don't save pictures etc. if not making a report - report = outcome.get_result() - extra = getattr(report, 'extra', []) + extra = getattr(report, "extra", []) # add the doc string extra.append(pytest_html.extras.html(get_docstring(item))) @@ -74,8 +78,10 @@ def pytest_runtest_makereport(item, call): dir_name = testFigsDir + get_test_name(item) i = 0 if len(plt.get_fignums()) > 0: - while i > -1: # this loops makes a numbered directory per run of the same test to avoid overwrite - dir_name_num = dir_name + '_' + str(i) + '/' + while ( + i > -1 + ): # this loops makes a numbered directory per run of the same test to avoid overwrite + dir_name_num = dir_name + "_" + str(i) + "/" if not os.path.exists(dir_name_num): os.makedirs(dir_name_num) break @@ -86,14 +92,25 @@ def pytest_runtest_makereport(item, call): for f in plt.get_fignums(): if not os.path.exists(dir_name_num): time.sleep(0.02) - filename = dir_name_num + 'figure_' + str(f) + '.svg' + filename = dir_name_num + "figure_" + str(f) + ".svg" plt.figure(f).savefig(filename, transparent=True) plt.close(f) # prevents saving same image multiple times - img_src = 'assets' + filename.split('assets')[1] # just want a relative (to report) path here - extra.append(pytest_html.extras.html('')) + img_src = ( + "assets" + filename.split("assets")[1] + ) # just want a relative (to report) path here + extra.append( + pytest_html.extras.html( + '' + ) + ) else: - extra.append(pytest_html.extras.html(' This test has no images.
')) + extra.append( + pytest_html.extras.html(" This test has no images.
") + ) extra.append(pytest_html.extras.html('
')) report.extra = extra diff --git a/src/simulation/deviceInterface/encoder/_UnitTest/test_encoder.py b/src/simulation/deviceInterface/encoder/_UnitTest/test_encoder.py index 06fc81e94a..3eb30a3418 100644 --- a/src/simulation/deviceInterface/encoder/_UnitTest/test_encoder.py +++ b/src/simulation/deviceInterface/encoder/_UnitTest/test_encoder.py @@ -32,25 +32,23 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.simulation import encoder # import the module that is to be tested -from Basilisk.architecture import messaging # import the message definitions +from Basilisk.simulation import encoder # import the module that is to be tested +from Basilisk.architecture import messaging # import the message definitions from Basilisk.utilities import macros from Basilisk.utilities import unitTestSupport - # Uncomment this line is 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(conditionstring) @pytest.mark.parametrize("accuracy", [1e-8]) - def test_encoder(show_plots, accuracy): r""" **Validation Test Description** @@ -123,7 +121,7 @@ def encoderTest(show_plots, accuracy): # wheelSpeedEncoder = encoder.Encoder() - wheelSpeedEncoder.ModelTag = 'rwSpeedsEncoder' + wheelSpeedEncoder.ModelTag = "rwSpeedsEncoder" wheelSpeedEncoder.clicksPerRotation = 2 wheelSpeedEncoder.numRW = numRW wheelSpeedEncoder.rwSpeedInMsg.subscribeTo(speedMsg) @@ -155,7 +153,7 @@ def encoderTest(show_plots, accuracy): numSteps += 1 # update the encoder to be OFF - wheelSpeedEncoder.rwSignalState = [encoder.SIGNAL_OFF]*numRW + wheelSpeedEncoder.rwSignalState = [encoder.SIGNAL_OFF] * numRW # run the sim unitTestSim.TotalSim.SingleStepProcesses() @@ -191,12 +189,16 @@ def encoderTest(show_plots, accuracy): # set the truth vectors # - trueWheelSpeedsEncoded = np.array([[100., 200., 300.], - [ 97.38937226, 197.92033718, 298.45130209], - [100.53096491, 201.06192983, 298.45130209], - [0., 0., 0.], - [499.51323192, 398.98226701, 298.45130209], - [499.51323192, 398.98226701, 298.45130209]]) + trueWheelSpeedsEncoded = np.array( + [ + [100.0, 200.0, 300.0], + [97.38937226, 197.92033718, 298.45130209], + [100.53096491, 201.06192983, 298.45130209], + [0.0, 0.0, 0.0], + [499.51323192, 398.98226701, 298.45130209], + [499.51323192, 398.98226701, 298.45130209], + ] + ) # # compare the module results to the true values @@ -205,7 +207,9 @@ def encoderTest(show_plots, accuracy): fail = 0 for i in range(numSteps): # check a vector values - if not unitTestSupport.isArrayEqual(wheelSpeedsEncoded[i, 0:3], trueWheelSpeedsEncoded[i, :], 3, accuracy): + if not unitTestSupport.isArrayEqual( + wheelSpeedsEncoded[i, 0:3], trueWheelSpeedsEncoded[i, :], 3, accuracy + ): fail += 1 if fail > 0: @@ -218,7 +222,7 @@ def encoderTest(show_plots, accuracy): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -227,5 +231,5 @@ def encoderTest(show_plots, accuracy): if __name__ == "__main__": test_encoder( False, # show_plots - 1e-8 # accuracy + 1e-8, # accuracy ) diff --git a/src/simulation/deviceInterface/hingedBodyLinearProfiler/_UnitTest/test_hingedBodyLinearProfiler.py b/src/simulation/deviceInterface/hingedBodyLinearProfiler/_UnitTest/test_hingedBodyLinearProfiler.py index cd748f119e..2ffd8fea59 100644 --- a/src/simulation/deviceInterface/hingedBodyLinearProfiler/_UnitTest/test_hingedBodyLinearProfiler.py +++ b/src/simulation/deviceInterface/hingedBodyLinearProfiler/_UnitTest/test_hingedBodyLinearProfiler.py @@ -27,10 +27,10 @@ from Basilisk.utilities import unitTestSupport -@pytest.mark.parametrize("startTime, endTime, startTheta, endTheta", [ - (macros.sec2nano(1), macros.sec2nano(2), 0, pi/180) -]) - +@pytest.mark.parametrize( + "startTime, endTime, startTheta, endTheta", + [(macros.sec2nano(1), macros.sec2nano(2), 0, pi / 180)], +) def test_hingedBodyLinearProfiler(show_plots, startTime, endTime, startTheta, endTheta): r""" **Validation Test Description** @@ -54,11 +54,15 @@ def test_hingedBodyLinearProfiler(show_plots, startTime, endTime, startTheta, en Before deployment, theta should be 0 and ``thetaDot`` 0. During deployment, ``thetaDot`` should be 1 degree per second, with theta varying linearly. After deployment, theta should be 1 degree and ``thetaDot`` 0. """ - [testResults, testMessage] = hingedBodyLinearProfilerTestFunction(show_plots, startTime, endTime, startTheta, endTheta) + [testResults, testMessage] = hingedBodyLinearProfilerTestFunction( + show_plots, startTime, endTime, startTheta, endTheta + ) assert testResults < 1, testMessage -def hingedBodyLinearProfilerTestFunction(show_plots, startTime, endTime, startTheta, endTheta): +def hingedBodyLinearProfilerTestFunction( + show_plots, startTime, endTime, startTheta, endTheta +): """Test method""" testFailCount = 0 testMessages = [] @@ -91,11 +95,20 @@ def hingedBodyLinearProfilerTestFunction(show_plots, startTime, endTime, startTh unitTestSim.ExecuteSimulation() # pull module data and make sure it is correct - trueTheta = np.array([0, 0, 0, pi/360, pi/180, pi/180, pi/180]); - trueThetaDot = np.array([0, 0, pi/180, pi/180, pi/180, 0, 0]) - - testFailCount, testMessages = unitTestSupport.compareVector(trueTheta, dataLog.theta, accuracy, "theta", testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(trueThetaDot, dataLog.thetaDot, accuracy, "thetaDot", testFailCount, testMessages) + trueTheta = np.array([0, 0, 0, pi / 360, pi / 180, pi / 180, pi / 180]) + trueThetaDot = np.array([0, 0, pi / 180, pi / 180, pi / 180, 0, 0]) + + testFailCount, testMessages = unitTestSupport.compareVector( + trueTheta, dataLog.theta, accuracy, "theta", testFailCount, testMessages + ) + testFailCount, testMessages = unitTestSupport.compareVector( + trueThetaDot, + dataLog.thetaDot, + accuracy, + "thetaDot", + testFailCount, + testMessages, + ) if testFailCount == 0: print("PASSED: " + module.ModelTag) @@ -106,4 +119,6 @@ def hingedBodyLinearProfilerTestFunction(show_plots, startTime, endTime, startTh if __name__ == "__main__": - test_hingedBodyLinearProfiler(False, macros.sec2nano(1), macros.sec2nano(2), 0, pi/180) + test_hingedBodyLinearProfiler( + False, macros.sec2nano(1), macros.sec2nano(2), 0, pi / 180 + ) diff --git a/src/simulation/deviceInterface/motorVoltageInterface/_UnitTest/test_motorVoltageInterface.py b/src/simulation/deviceInterface/motorVoltageInterface/_UnitTest/test_motorVoltageInterface.py index 6bbbca111f..efdc5919f3 100644 --- a/src/simulation/deviceInterface/motorVoltageInterface/_UnitTest/test_motorVoltageInterface.py +++ b/src/simulation/deviceInterface/motorVoltageInterface/_UnitTest/test_motorVoltageInterface.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -31,13 +30,17 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions -from Basilisk.simulation import motorVoltageInterface # import the module that is to be tested +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions +from Basilisk.simulation import ( + motorVoltageInterface, +) # import the module that is to be tested from Basilisk.utilities import macros from Basilisk.architecture import messaging @@ -45,6 +48,7 @@ def addTimeColumn(time, data): return np.transpose(np.vstack([[time], np.transpose(data)])) + # Uncomment this line is 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. @@ -52,11 +56,7 @@ def addTimeColumn(time, data): # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("voltage", [ - (5.0) - ,(-7.5) - ,(0.0) -]) +@pytest.mark.parametrize("voltage", [(5.0), (-7.5), (0.0)]) # update "module" in this function name to reflect the module name def test_module(show_plots, voltage): @@ -92,16 +92,16 @@ def test_module(show_plots, voltage): def run(show_plots, voltage): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -110,9 +110,11 @@ def run(show_plots, voltage): testModule.ModelTag = "motorVoltageInterface" # set module parameters(s) - testModule.setGains(np.array([1.32, 0.99, 1.31])) # [Nm/V] conversion gain - testModule.setScaleFactors(np.array([1.01, 1.00, 1.02])) # [ul] error scale factor - testModule.setBiases(np.array([0.01, 0.02, 0.04])) # [Nm] Torque bias from converter output + testModule.setGains(np.array([1.32, 0.99, 1.31])) # [Nm/V] conversion gain + testModule.setScaleFactors(np.array([1.01, 1.00, 1.02])) # [ul] error scale factor + testModule.setBiases( + np.array([0.01, 0.02, 0.04]) + ) # [Nm] Torque bias from converter output # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, testModule) @@ -120,7 +122,7 @@ def run(show_plots, voltage): # Create input message and size it because the regular creator of that message # is not part of the test. voltageData = messaging.ArrayMotorVoltageMsgPayload() - voltageData.voltage = [voltage, voltage+1.0, voltage+1.5] + voltageData.voltage = [voltage, voltage + 1.0, voltage + 1.5] voltageMsg = messaging.ArrayMotorVoltageMsg().write(voltageData) testModule.motorVoltageInMsg.subscribeTo(voltageMsg) @@ -135,61 +137,72 @@ def run(show_plots, voltage): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() - # This pulls the actual data log from the simulation run. moduleOutput = dataLog.motorTorque[:, :3] # set truth states - voltageTrue = np.array([1.0, 1.0, 1.0])*voltage + np.array([0.0, 1.0, 1.5]) + voltageTrue = np.array([1.0, 1.0, 1.0]) * voltage + np.array([0.0, 1.0, 1.5]) trueVector = [ - voltageTrue[0] * testModule.voltage2TorqueGain[0][0]*testModule.scaleFactor[0][0] + testModule.bias[0][0], - voltageTrue[1] * testModule.voltage2TorqueGain[1][0]*testModule.scaleFactor[1][0] + testModule.bias[1][0], - voltageTrue[2] * testModule.voltage2TorqueGain[2][0]*testModule.scaleFactor[2][0] + testModule.bias[2][0] + voltageTrue[0] + * testModule.voltage2TorqueGain[0][0] + * testModule.scaleFactor[0][0] + + testModule.bias[0][0], + voltageTrue[1] + * testModule.voltage2TorqueGain[1][0] + * testModule.scaleFactor[1][0] + + testModule.bias[1][0], + voltageTrue[2] + * testModule.voltage2TorqueGain[2][0] + * testModule.scaleFactor[2][0] + + testModule.bias[2][0], ] trueVector = np.array([trueVector, trueVector, trueVector]) # compare the module results to the truth values accuracy = 1e-12 - testFailCount, testMessages = unitTestSupport.compareArray(trueVector, moduleOutput, - accuracy, "Output Vector", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + trueVector, moduleOutput, accuracy, "Output Vector", testFailCount, testMessages + ) moduleOutput = addTimeColumn(dataLog.times(), moduleOutput) resultTable = moduleOutput resultTable[:, 0] = macros.NANO2SEC * resultTable[:, 0] diff = np.delete(moduleOutput, 0, 1) - trueVector - resultTable = np.insert(resultTable, list(range(2, 2 + len(diff.transpose()))), diff, axis=1) + resultTable = np.insert( + resultTable, list(range(2, 2 + len(diff.transpose()))), diff, axis=1 + ) tableName = "baseVoltage" + str(voltage) - tableHeaders = ["time [s]", "$u_{s,1}$ (Nm)", "Error", "$u_{s,2}$ (Nm)", "Error", "$u_{u,3}$ (Nm)", "Error"] - caption = 'RW motoor torque output for Base Voltaget = ' + str(voltage) + 'V.' - unitTestSupport.writeTableLaTeX( - tableName, - tableHeaders, - caption, - resultTable, - path) - + tableHeaders = [ + "time [s]", + "$u_{s,1}$ (Nm)", + "Error", + "$u_{s,2}$ (Nm)", + "Error", + "$u_{u,3}$ (Nm)", + "Error", + ] + caption = "RW motoor torque output for Base Voltaget = " + str(voltage) + "V." + unitTestSupport.writeTableLaTeX(tableName, tableHeaders, caption, resultTable, path) # print out success message if no error were found - snippetName = "passFail" + '{:1.1f}'.format(voltage) + snippetName = "passFail" + "{:1.1f}".format(voltage) if testFailCount == 0: colorText = "ForestGreen" print("PASSED: " + testModule.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: colorText = "Red" - passedText = r'\textcolor{' + colorText + '}{' + "FAILED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "FAILED" + "}" unitTestSupport.writeTeXSnippet(snippetName, passedText, path) - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -197,7 +210,7 @@ def run(show_plots, voltage): # stand-along python script # if __name__ == "__main__": - test_module( # update "module" in function name - False, - 5.0, # param1 value - ) + test_module( # update "module" in function name + False, + 5.0, # param1 value + ) diff --git a/src/simulation/deviceInterface/prescribedLinearTranslation/_UnitTest/test_prescribedLinearTranslation.py b/src/simulation/deviceInterface/prescribedLinearTranslation/_UnitTest/test_prescribedLinearTranslation.py index c1db67238a..622b1ad5af 100644 --- a/src/simulation/deviceInterface/prescribedLinearTranslation/_UnitTest/test_prescribedLinearTranslation.py +++ b/src/simulation/deviceInterface/prescribedLinearTranslation/_UnitTest/test_prescribedLinearTranslation.py @@ -38,7 +38,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) @@ -49,14 +49,16 @@ @pytest.mark.parametrize("transPosRef2", [0.5]) # [m] @pytest.mark.parametrize("transAccelMax", [0.01, 0.005]) # [m/s^2] @pytest.mark.parametrize("accuracy", [1e-8]) -def test_prescribedLinearTranslation(show_plots, - coastOptionBangDuration, - smoothingDuration, - transPosInit, - transPosRef1, - transPosRef2, - transAccelMax, - accuracy): +def test_prescribedLinearTranslation( + show_plots, + coastOptionBangDuration, + smoothingDuration, + transPosInit, + transPosRef1, + transPosRef2, + transAccelMax, + accuracy, +): r""" **Validation Test Description** @@ -122,8 +124,12 @@ def test_prescribedLinearTranslation(show_plots, linearTransRigidBodyMessageData = messaging.LinearTranslationRigidBodyMsgPayload() linearTransRigidBodyMessageData.rho = transPosRef1 # [m] linearTransRigidBodyMessageData.rhoDot = 0.0 # [m/s] - linearTransRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write(linearTransRigidBodyMessageData) - prescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTransRigidBodyMessage) + linearTransRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write( + linearTransRigidBodyMessageData + ) + prescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo( + linearTransRigidBodyMessage + ) # Log module data for module unit test validation prescribedStatesDataLog = prescribedTrans.prescribedTranslationOutMsg.recorder() @@ -139,8 +145,12 @@ def test_prescribedLinearTranslation(show_plots, linearTransRigidBodyMessageData = messaging.LinearTranslationRigidBodyMsgPayload() linearTransRigidBodyMessageData.rho = transPosRef2 # [m] linearTransRigidBodyMessageData.rhoDot = 0.0 # [m/s] - linearTransRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write(linearTransRigidBodyMessageData) - prescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTransRigidBodyMessage) + linearTransRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write( + linearTransRigidBodyMessageData + ) + prescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo( + linearTransRigidBodyMessage + ) # Execute the second translation segment unitTestSim.ConfigureStopTime(macros.sec2nano(2 * simTime)) @@ -158,14 +168,13 @@ def test_prescribedLinearTranslation(show_plots, transPosFinal2 = r_PM_M[-1].dot(transAxis_M) transPosFinalList = [transPosFinal1, transPosFinal2] # [m] transPosRefList = [transPosRef1, transPosRef2] # [m] - np.testing.assert_allclose(transPosRefList, - transPosFinalList, - atol=accuracy, - verbose=True) + np.testing.assert_allclose( + transPosRefList, transPosFinalList, atol=accuracy, verbose=True + ) # Unit test validation 2: Numerically check that the profiled accelerations, # velocities, and displacements are correct - if (smoothingDuration > 0.0): + if smoothingDuration > 0.0: transAccel = rPrimePrime_PM_M.dot(transAxis_M) transVel = rPrime_PM_M.dot(transAxis_M) transPos = r_PM_M.dot(transAxis_M) @@ -173,25 +182,33 @@ def test_prescribedLinearTranslation(show_plots, transVelNumerical = [] for i in range(len(timespan) - 1): # First order forward difference - transAccelNumerical.append((transVel[i+1] - transVel[i]) / testTimeStepSec) - transVelNumerical.append((transPos[i+1] - transPos[i]) / testTimeStepSec) - - np.testing.assert_allclose(transAccel[0:-1], - transAccelNumerical, - atol=1e-2, - verbose=True) - np.testing.assert_allclose(transVel[0:-1], - transVelNumerical, - atol=1e-2, - verbose=True) + transAccelNumerical.append( + (transVel[i + 1] - transVel[i]) / testTimeStepSec + ) + transVelNumerical.append((transPos[i + 1] - transPos[i]) / testTimeStepSec) + + np.testing.assert_allclose( + transAccel[0:-1], transAccelNumerical, atol=1e-2, verbose=True + ) + np.testing.assert_allclose( + transVel[0:-1], transVelNumerical, atol=1e-2, verbose=True + ) if show_plots: # Plot the difference between the numerical and profiled results plt.figure() plt.clf() - plt.plot(timespan[0:-1], transAccelNumerical - transAccel[0:-1], label=r'$\ddot{\rho}$') - plt.plot(timespan[0:-1], transVelNumerical - transVel[0:-1], label=r"$\dot{\rho}$") - plt.title(r'Profiled vs Numerical Difference', fontsize=14) - plt.legend(loc='upper right', prop={'size': 12}) + plt.plot( + timespan[0:-1], + transAccelNumerical - transAccel[0:-1], + label=r"$\ddot{\rho}$", + ) + plt.plot( + timespan[0:-1], + transVelNumerical - transVel[0:-1], + label=r"$\dot{\rho}$", + ) + plt.title(r"Profiled vs Numerical Difference", fontsize=14) + plt.legend(loc="upper right", prop={"size": 12}) plt.grid(True) if show_plots: @@ -203,33 +220,42 @@ def test_prescribedLinearTranslation(show_plots, plt.figure() plt.clf() plt.plot(timespan, r_PM_M.dot(transAxis_M), label=r"$l$") - plt.plot(timespan, transPosInitPlotting, '--', label=r'$\rho_{0}$') - plt.plot(timespan, transPosRef1Plotting, '--', label=r'$\rho_{Ref_1}$') - plt.plot(timespan, transPosRef2Plotting, '--', label=r'$\rho_{Ref_2}$') - plt.title(r'Profiled Translational Position $\rho_{\mathcal{P}/\mathcal{M}}$', fontsize=14) - plt.ylabel('(m)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='upper right', prop={'size': 12}) + plt.plot(timespan, transPosInitPlotting, "--", label=r"$\rho_{0}$") + plt.plot(timespan, transPosRef1Plotting, "--", label=r"$\rho_{Ref_1}$") + plt.plot(timespan, transPosRef2Plotting, "--", label=r"$\rho_{Ref_2}$") + plt.title( + r"Profiled Translational Position $\rho_{\mathcal{P}/\mathcal{M}}$", + fontsize=14, + ) + plt.ylabel("(m)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="upper right", prop={"size": 12}) plt.grid(True) # 1B. Plot transVel plt.figure() plt.clf() plt.plot(timespan, rPrime_PM_M.dot(transAxis_M), label=r"$\dot{\rho}$") - plt.title(r'Profiled Translational Velocity $\dot{\rho}_{\mathcal{P}/\mathcal{M}}$', fontsize=14) - plt.ylabel('(m/s)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='upper right', prop={'size': 12}) + plt.title( + r"Profiled Translational Velocity $\dot{\rho}_{\mathcal{P}/\mathcal{M}}$", + fontsize=14, + ) + plt.ylabel("(m/s)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="upper right", prop={"size": 12}) plt.grid(True) # 1C. Plot transAccel plt.figure() plt.clf() plt.plot(timespan, rPrimePrime_PM_M.dot(transAxis_M), label=r"$\ddot{\rho}$") - plt.title(r'Profiled Translational Acceleration $\ddot{\rho}_{\mathcal{P}/\mathcal{M}}$ ', fontsize=14) - plt.ylabel('(m/s$^2$)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='upper right', prop={'size': 12}) + plt.title( + r"Profiled Translational Acceleration $\ddot{\rho}_{\mathcal{P}/\mathcal{M}}$ ", + fontsize=14, + ) + plt.ylabel("(m/s$^2$)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="upper right", prop={"size": 12}) plt.grid(True) # 2. Plot the prescribed translational states @@ -238,44 +264,54 @@ def test_prescribedLinearTranslation(show_plots, transPosRef2Plotting = np.ones(len(timespan)) * transPosRef2 # [m] plt.figure() plt.clf() - plt.plot(timespan, r_PM_M[:, 0], label=r'$r_{1}$') - plt.plot(timespan, r_PM_M[:, 1], label=r'$r_{2}$') - plt.plot(timespan, r_PM_M[:, 2], label=r'$r_{3}$') - plt.plot(timespan, transPosRef1Plotting, '--', label=r'$\rho_{Ref_1}$') - plt.plot(timespan, transPosRef2Plotting, '--', label=r'$\rho_{Ref_2}$') - plt.title(r'${}^\mathcal{M} r_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(m)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='center left', prop={'size': 12}) + plt.plot(timespan, r_PM_M[:, 0], label=r"$r_{1}$") + plt.plot(timespan, r_PM_M[:, 1], label=r"$r_{2}$") + plt.plot(timespan, r_PM_M[:, 2], label=r"$r_{3}$") + plt.plot(timespan, transPosRef1Plotting, "--", label=r"$\rho_{Ref_1}$") + plt.plot(timespan, transPosRef2Plotting, "--", label=r"$\rho_{Ref_2}$") + plt.title( + r"${}^\mathcal{M} r_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory", + fontsize=14, + ) + plt.ylabel("(m)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="center left", prop={"size": 12}) plt.grid(True) # 2B. Plot rPrime_PM_P plt.figure() plt.clf() - plt.plot(timespan, rPrime_PM_M[:, 0], label='1') - plt.plot(timespan, rPrime_PM_M[:, 1], label='2') - plt.plot(timespan, rPrime_PM_M[:, 2], label='3') - plt.title(r'${}^\mathcal{M} r$Prime$_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(m/s)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='upper left', prop={'size': 12}) + plt.plot(timespan, rPrime_PM_M[:, 0], label="1") + plt.plot(timespan, rPrime_PM_M[:, 1], label="2") + plt.plot(timespan, rPrime_PM_M[:, 2], label="3") + plt.title( + r"${}^\mathcal{M} r$Prime$_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory", + fontsize=14, + ) + plt.ylabel("(m/s)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="upper left", prop={"size": 12}) plt.grid(True) # 2C. Plot rPrimePrime_PM_P plt.figure() plt.clf() - plt.plot(timespan, rPrimePrime_PM_M[:, 0], label='1') - plt.plot(timespan, rPrimePrime_PM_M[:, 1], label='2') - plt.plot(timespan, rPrimePrime_PM_M[:, 2], label='3') - plt.title(r'${}^\mathcal{M} r$PrimePrime$_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(m/s$^2$)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='upper left', prop={'size': 12}) + plt.plot(timespan, rPrimePrime_PM_M[:, 0], label="1") + plt.plot(timespan, rPrimePrime_PM_M[:, 1], label="2") + plt.plot(timespan, rPrimePrime_PM_M[:, 2], label="3") + plt.title( + r"${}^\mathcal{M} r$PrimePrime$_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory", + fontsize=14, + ) + plt.ylabel("(m/s$^2$)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="upper left", prop={"size": 12}) plt.grid(True) plt.show() plt.close("all") + if __name__ == "__main__": test_prescribedLinearTranslation( True, # show_plots @@ -285,5 +321,5 @@ def test_prescribedLinearTranslation(show_plots, -1.0, # [m] transPosRef1 0.5, # [m] transPosRef2 0.01, # [m/s^2] transAccelMax - 1e-8 # accuracy + 1e-8, # accuracy ) diff --git a/src/simulation/deviceInterface/prescribedRotation1DOF/_UnitTest/test_prescribedRotation1DOF.py b/src/simulation/deviceInterface/prescribedRotation1DOF/_UnitTest/test_prescribedRotation1DOF.py index 1089dc2cfe..aa4f8138a9 100755 --- a/src/simulation/deviceInterface/prescribedRotation1DOF/_UnitTest/test_prescribedRotation1DOF.py +++ b/src/simulation/deviceInterface/prescribedRotation1DOF/_UnitTest/test_prescribedRotation1DOF.py @@ -37,24 +37,29 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) + @pytest.mark.parametrize("coastOptionBangDuration", [0.0, 2.0]) # [s] @pytest.mark.parametrize("smoothingDuration", [0.0, 2.0]) # [s] @pytest.mark.parametrize("thetaInit", [0.0, macros.D2R * -5.0]) # [rad] @pytest.mark.parametrize("thetaRef1", [0.0, macros.D2R * -10.0]) # [rad] @pytest.mark.parametrize("thetaRef2", [macros.D2R * 5.0]) # [rad] -@pytest.mark.parametrize("thetaDDotMax", [macros.D2R * 0.05, macros.D2R * 0.1]) # [rad/s^2] +@pytest.mark.parametrize( + "thetaDDotMax", [macros.D2R * 0.05, macros.D2R * 0.1] +) # [rad/s^2] @pytest.mark.parametrize("accuracy", [1e-8]) -def test_prescribedRotation1DOF(show_plots, - coastOptionBangDuration, - smoothingDuration, - thetaInit, - thetaRef1, - thetaRef2, - thetaDDotMax, - accuracy): +def test_prescribedRotation1DOF( + show_plots, + coastOptionBangDuration, + smoothingDuration, + thetaInit, + thetaRef1, + thetaRef2, + thetaDDotMax, + accuracy, +): r""" **Validation Test Description** @@ -120,7 +125,9 @@ def test_prescribedRotation1DOF(show_plots, hingedRigidBodyMessageData = messaging.HingedRigidBodyMsgPayload() hingedRigidBodyMessageData.theta = thetaRef1 # [rad] hingedRigidBodyMessageData.thetaDot = 0.0 # [rad/s] - hingedRigidBodyMessage = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData) + hingedRigidBodyMessage = messaging.HingedRigidBodyMsg().write( + hingedRigidBodyMessageData + ) prescribedRot1DOF.spinningBodyInMsg.subscribeTo(hingedRigidBodyMessage) # Log module data for module unit test validation @@ -139,7 +146,9 @@ def test_prescribedRotation1DOF(show_plots, hingedRigidBodyMessageData = messaging.HingedRigidBodyMsgPayload() hingedRigidBodyMessageData.theta = thetaRef2 # [rad] hingedRigidBodyMessageData.thetaDot = 0.0 # [rad/s] - hingedRigidBodyMessage = messaging.HingedRigidBodyMsg().write(hingedRigidBodyMessageData) + hingedRigidBodyMessage = messaging.HingedRigidBodyMsg().write( + hingedRigidBodyMessageData + ) prescribedRot1DOF.spinningBodyInMsg.subscribeTo(hingedRigidBodyMessage) # Execute the second spinning body rotation @@ -149,7 +158,9 @@ def test_prescribedRotation1DOF(show_plots, # Extract logged data timespan = macros.NANO2SEC * scalarAngleDataLog.times() # [s] omega_PM_P = macros.R2D * prescribedRotStatesDataLog.omega_PM_P # [deg/s] - omegaPrime_PM_P = macros.R2D * prescribedRotStatesDataLog.omegaPrime_PM_P # [deg/s^2] + omegaPrime_PM_P = ( + macros.R2D * prescribedRotStatesDataLog.omegaPrime_PM_P + ) # [deg/s^2] sigma_PM = prescribedRotStatesDataLog.sigma_PM theta = macros.R2D * scalarAngleDataLog.theta # [deg] thetaDot = macros.R2D * scalarAngleDataLog.thetaDot # [deg/s] @@ -161,36 +172,41 @@ def test_prescribedRotation1DOF(show_plots, thetaFinal2 = theta[-1] thetaFinalList = [thetaFinal1, thetaFinal2] # [deg] thetaRefList = [macros.R2D * thetaRef1, macros.R2D * thetaRef2] # [deg] - np.testing.assert_allclose(thetaRefList, - thetaFinalList, - atol=accuracy, - verbose=True) + np.testing.assert_allclose( + thetaRefList, thetaFinalList, atol=accuracy, verbose=True + ) # Unit test validation 2: Numerically check that the profiled accelerations, angle rates, and angles are correct - if (smoothingDuration > 0.0): + if smoothingDuration > 0.0: thetaDDotNumerical = [] thetaDotNumerical = [] for i in range(len(timespan) - 1): # First order forward difference - thetaDDotNumerical.append((thetaDot[i+1] - thetaDot[i]) / testTimeStepSec) - thetaDotNumerical.append((theta[i+1] - theta[i]) / testTimeStepSec) - - np.testing.assert_allclose(thetaDDot[0:-1], - thetaDDotNumerical, - atol=1e-2, - verbose=True) - np.testing.assert_allclose(thetaDot[0:-1], - thetaDotNumerical, - atol=1e-2, - verbose=True) + thetaDDotNumerical.append((thetaDot[i + 1] - thetaDot[i]) / testTimeStepSec) + thetaDotNumerical.append((theta[i + 1] - theta[i]) / testTimeStepSec) + + np.testing.assert_allclose( + thetaDDot[0:-1], thetaDDotNumerical, atol=1e-2, verbose=True + ) + np.testing.assert_allclose( + thetaDot[0:-1], thetaDotNumerical, atol=1e-2, verbose=True + ) if show_plots: # Plot the difference between the numerical and profiled results plt.figure() plt.clf() - plt.plot(timespan[0:-1], thetaDDotNumerical - thetaDDot[0:-1], label=r'$\ddot{\theta}$') - plt.plot(timespan[0:-1], thetaDotNumerical - thetaDot[0:-1], label=r"$\dot{\theta}$") - plt.title(r'Profiled vs Numerical Difference', fontsize=14) - plt.legend(loc='upper right', prop={'size': 12}) + plt.plot( + timespan[0:-1], + thetaDDotNumerical - thetaDDot[0:-1], + label=r"$\ddot{\theta}$", + ) + plt.plot( + timespan[0:-1], + thetaDotNumerical - thetaDot[0:-1], + label=r"$\dot{\theta}$", + ) + plt.title(r"Profiled vs Numerical Difference", fontsize=14) + plt.legend(loc="upper right", prop={"size": 12}) plt.grid(True) if show_plots: @@ -202,78 +218,91 @@ def test_prescribedRotation1DOF(show_plots, plt.figure() plt.clf() plt.plot(timespan, theta, label=r"$\theta$") - plt.plot(timespan, thetaInitPlotting, '--', label=r'$\theta_{0}$') - plt.plot(timespan, thetaRef1Plotting, '--', label=r'$\theta_{Ref_1}$') - plt.plot(timespan, thetaRef2Plotting, '--', label=r'$\theta_{Ref_2}$') - plt.title(r'Profiled Angle $\theta_{\mathcal{P}/\mathcal{M}}$', fontsize=14) - plt.ylabel('(deg)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='upper right', prop={'size': 12}) + plt.plot(timespan, thetaInitPlotting, "--", label=r"$\theta_{0}$") + plt.plot(timespan, thetaRef1Plotting, "--", label=r"$\theta_{Ref_1}$") + plt.plot(timespan, thetaRef2Plotting, "--", label=r"$\theta_{Ref_2}$") + plt.title(r"Profiled Angle $\theta_{\mathcal{P}/\mathcal{M}}$", fontsize=14) + plt.ylabel("(deg)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="upper right", prop={"size": 12}) plt.grid(True) # 1B. Plot thetaDot plt.figure() plt.clf() plt.plot(timespan, thetaDot, label=r"$\dot{\theta}$") - plt.title(r'Profiled Angle Rate $\dot{\theta}_{\mathcal{P}/\mathcal{M}}$', fontsize=14) - plt.ylabel('(deg/s)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='upper right', prop={'size': 12}) + plt.title( + r"Profiled Angle Rate $\dot{\theta}_{\mathcal{P}/\mathcal{M}}$", fontsize=14 + ) + plt.ylabel("(deg/s)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="upper right", prop={"size": 12}) plt.grid(True) # 1C. Plot thetaDDot plt.figure() plt.clf() plt.plot(timespan, thetaDDot, label=r"$\ddot{\theta}$") - plt.title(r'Profiled Angular Acceleration $\ddot{\theta}_{\mathcal{P}/\mathcal{M}}$ ', fontsize=14) - plt.ylabel('(deg/s$^2$)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='upper right', prop={'size': 12}) + plt.title( + r"Profiled Angular Acceleration $\ddot{\theta}_{\mathcal{P}/\mathcal{M}}$ ", + fontsize=14, + ) + plt.ylabel("(deg/s$^2$)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="upper right", prop={"size": 12}) plt.grid(True) # 2. Plot the spinning body prescribed rotational states # 2A. Plot PRV angle from sigma_PM phi_PM = [] for i in range(len(timespan)): - phi_PM.append(macros.R2D * 4 * np.arctan(np.linalg.norm(sigma_PM[i, :]))) # [deg] + phi_PM.append( + macros.R2D * 4 * np.arctan(np.linalg.norm(sigma_PM[i, :])) + ) # [deg] plt.figure() plt.clf() plt.plot(timespan, phi_PM, label=r"$\Phi$") - plt.title(r'Profiled PRV Angle $\Phi_{\mathcal{P}/\mathcal{M}}$', fontsize=14) - plt.ylabel('(deg)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='center right', prop={'size': 14}) + plt.title(r"Profiled PRV Angle $\Phi_{\mathcal{P}/\mathcal{M}}$", fontsize=14) + plt.ylabel("(deg)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="center right", prop={"size": 14}) plt.grid(True) # 2B. Plot omega_PM_P plt.figure() plt.clf() - plt.plot(timespan, omega_PM_P[:, 0], label=r'$\omega_{1}$') - plt.plot(timespan, omega_PM_P[:, 1], label=r'$\omega_{2}$') - plt.plot(timespan, omega_PM_P[:, 2], label=r'$\omega_{3}$') - plt.title(r'Profiled Angular Velocity ${}^\mathcal{P} \omega_{\mathcal{P}/\mathcal{M}}$', fontsize=14) - plt.ylabel('(deg/s)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='upper right', prop={'size': 14}) + plt.plot(timespan, omega_PM_P[:, 0], label=r"$\omega_{1}$") + plt.plot(timespan, omega_PM_P[:, 1], label=r"$\omega_{2}$") + plt.plot(timespan, omega_PM_P[:, 2], label=r"$\omega_{3}$") + plt.title( + r"Profiled Angular Velocity ${}^\mathcal{P} \omega_{\mathcal{P}/\mathcal{M}}$", + fontsize=14, + ) + plt.ylabel("(deg/s)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="upper right", prop={"size": 14}) plt.grid(True) # 2C. Plot omegaPrime_PM_P plt.figure() plt.clf() - plt.plot(timespan, omegaPrime_PM_P[:, 0], label=r'1') - plt.plot(timespan, omegaPrime_PM_P[:, 1], label=r'2') - plt.plot(timespan, omegaPrime_PM_P[:, 2], label=r'3') - plt.title(r'Profiled Angular Acceleration ${}^\mathcal{P} \omega$Prime$_{\mathcal{P}/\mathcal{M}}$', - fontsize=14) - plt.ylabel('(deg/s$^2$)', fontsize=14) - plt.xlabel('Time (s)', fontsize=14) - plt.legend(loc='upper right', prop={'size': 14}) + plt.plot(timespan, omegaPrime_PM_P[:, 0], label=r"1") + plt.plot(timespan, omegaPrime_PM_P[:, 1], label=r"2") + plt.plot(timespan, omegaPrime_PM_P[:, 2], label=r"3") + plt.title( + r"Profiled Angular Acceleration ${}^\mathcal{P} \omega$Prime$_{\mathcal{P}/\mathcal{M}}$", + fontsize=14, + ) + plt.ylabel("(deg/s$^2$)", fontsize=14) + plt.xlabel("Time (s)", fontsize=14) + plt.legend(loc="upper right", prop={"size": 14}) plt.grid(True) plt.show() plt.close("all") + if __name__ == "__main__": test_prescribedRotation1DOF( True, # show_plots @@ -283,5 +312,5 @@ def test_prescribedRotation1DOF(show_plots, macros.D2R * -10.0, # [rad] thetaRef1 macros.D2R * 5.0, # [rad] thetaRef2 macros.D2R * 0.1, # [rad/s^2] thetaDDotMax - 1e-8 # accuracy + 1e-8, # accuracy ) diff --git a/src/simulation/deviceInterface/tempMeasurement/_UnitTest/test_tempMeasurement.py b/src/simulation/deviceInterface/tempMeasurement/_UnitTest/test_tempMeasurement.py index aabeaea251..7378fdb4aa 100644 --- a/src/simulation/deviceInterface/tempMeasurement/_UnitTest/test_tempMeasurement.py +++ b/src/simulation/deviceInterface/tempMeasurement/_UnitTest/test_tempMeasurement.py @@ -39,6 +39,7 @@ path = os.path.dirname(os.path.abspath(__file__)) + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. # Need RNGSeed 464374481 @@ -50,11 +51,12 @@ "TEMP_FAULT_STUCK_VALUE", "TEMP_FAULT_SPIKING", "BIASED", - "GAUSS_MARKOV" - ]) + "GAUSS_MARKOV", + ], +) # provide a unique test method name, starting with test_ def test_sensorThermalFault(tempFault): - '''This function is called by the py.test environment.''' + """This function is called by the py.test environment.""" # each test method requires a single assert method to be called [testResults, testMessage] = run(tempFault) assert testResults < 1, testMessage @@ -62,7 +64,6 @@ def test_sensorThermalFault(tempFault): def run(tempFault): - testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages @@ -74,7 +75,7 @@ def run(tempFault): unitTestSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.sec2nano(100.) + simulationTime = macros.sec2nano(100.0) # # create the simulation process @@ -95,21 +96,20 @@ def run(tempFault): # set the spacecraft message scStateMsgPayload = messaging.SCStatesMsgPayload() - scStateMsgPayload.r_BN_N = [6378*1000., 0., 0.] - scStateMsgPayload.sigma_BN = [0., 0., 0.] + scStateMsgPayload.r_BN_N = [6378 * 1000.0, 0.0, 0.0] + scStateMsgPayload.sigma_BN = [0.0, 0.0, 0.0] scStateMsg = messaging.SCStatesMsg().write(scStateMsgPayload) # set the sun message sunMsgPayload = messaging.SpicePlanetStateMsgPayload() - sunMsgPayload.PositionVector = [astroConstants.AU*1000., 0., 0.] + sunMsgPayload.PositionVector = [astroConstants.AU * 1000.0, 0.0, 0.0] sunMsg = messaging.SpicePlanetStateMsg().write(sunMsgPayload) - # # set up the truth value temperature modeling # sensorThermalModel = sensorThermal.SensorThermal() - sensorThermalModel.ModelTag = 'sensorThermalModel' + sensorThermalModel.ModelTag = "sensorThermalModel" sensorThermalModel.nHat_B = [0, 0, 1] sensorThermalModel.sensorArea = 1.0 # m^2 sensorThermalModel.sensorAbsorptivity = 0.25 @@ -151,7 +151,7 @@ def run(tempFault): elif tempFault == "TEMP_FAULT_STUCK_CURRENT": truthValue = -2.1722164230619447 unitTestSim.InitializeSimulation() - unitTestSim.ConfigureStopTime(round(simulationTime/2.0)) + unitTestSim.ConfigureStopTime(round(simulationTime / 2.0)) unitTestSim.ExecuteSimulation() tempMeasurementModel.faultState = tempMeasurement.TEMP_FAULT_STUCK_CURRENT unitTestSim.ConfigureStopTime(simulationTime) @@ -161,7 +161,7 @@ def run(tempFault): truthValue = 10.0 tempMeasurementModel.stuckValue = 10.0 unitTestSim.InitializeSimulation() - unitTestSim.ConfigureStopTime(round(simulationTime/2.0)) + unitTestSim.ConfigureStopTime(round(simulationTime / 2.0)) unitTestSim.ExecuteSimulation() tempMeasurementModel.faultState = tempMeasurement.TEMP_FAULT_STUCK_VALUE unitTestSim.ConfigureStopTime(simulationTime) @@ -172,27 +172,111 @@ def run(tempFault): unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(simulationTime) unitTestSim.ExecuteSimulation() - nominals = np.array([ 0., -0.04439869, -0.08875756, -0.13307667, - -0.17735608, -0.22159584, -0.265796, -0.30995662, -0.35407775, - -0.39815946, -0.44220178, -0.48620479, -0.53016852, -0.57409304, - -0.6179784, -0.66182465, -0.70563184, -0.74940004, -0.79312929, - -0.83681965, -0.88047117, -0.9240839, -0.9676579, -1.01119321, - -1.0546899, -1.09814802, -1.14156761, -1.18494873, -1.22829144, - -1.27159578, -1.31486181, -1.35808958, -1.40127914, -1.44443055, - -1.48754385, -1.5306191, -1.57365634, -1.61665564, -1.65961704, - -1.7025406, -1.74542636, -1.78827437, -1.83108469, -1.87385737, - -1.91659246, -1.95929001, -2.00195007, -2.04457269, -2.08715792, - -2.12970582, -2.17221642, -2.21468979, -2.25712597, -2.29952502, - -2.34188697, -2.38421189, -2.42649982, -2.46875082, -2.51096492, - -2.55314219, -2.59528266, -2.6373864, -2.67945344, -2.72148384, - -2.76347765, -2.80543491, -2.84735568, -2.88924, -2.93108792, - -2.9728995 , -3.01467477, -3.05641378, -3.0981166, -3.13978325, - -3.1814138, -3.22300829, -3.26456677, -3.30608928, -3.34757587, - -3.38902659, -3.43044149, -3.47182062, -3.51316402, -3.55447174, - -3.59574383, -3.63698033, -3.67818129, -3.71934677, -3.76047679, - -3.80157142, -3.8426307, -3.88365467, -3.92464338, -3.96559689, - -4.00651522, -4.04739844, -4.08824658, -4.1290597, -4.16983783, - -4.21058103, -4.25128934]) + nominals = np.array( + [ + 0.0, + -0.04439869, + -0.08875756, + -0.13307667, + -0.17735608, + -0.22159584, + -0.265796, + -0.30995662, + -0.35407775, + -0.39815946, + -0.44220178, + -0.48620479, + -0.53016852, + -0.57409304, + -0.6179784, + -0.66182465, + -0.70563184, + -0.74940004, + -0.79312929, + -0.83681965, + -0.88047117, + -0.9240839, + -0.9676579, + -1.01119321, + -1.0546899, + -1.09814802, + -1.14156761, + -1.18494873, + -1.22829144, + -1.27159578, + -1.31486181, + -1.35808958, + -1.40127914, + -1.44443055, + -1.48754385, + -1.5306191, + -1.57365634, + -1.61665564, + -1.65961704, + -1.7025406, + -1.74542636, + -1.78827437, + -1.83108469, + -1.87385737, + -1.91659246, + -1.95929001, + -2.00195007, + -2.04457269, + -2.08715792, + -2.12970582, + -2.17221642, + -2.21468979, + -2.25712597, + -2.29952502, + -2.34188697, + -2.38421189, + -2.42649982, + -2.46875082, + -2.51096492, + -2.55314219, + -2.59528266, + -2.6373864, + -2.67945344, + -2.72148384, + -2.76347765, + -2.80543491, + -2.84735568, + -2.88924, + -2.93108792, + -2.9728995, + -3.01467477, + -3.05641378, + -3.0981166, + -3.13978325, + -3.1814138, + -3.22300829, + -3.26456677, + -3.30608928, + -3.34757587, + -3.38902659, + -3.43044149, + -3.47182062, + -3.51316402, + -3.55447174, + -3.59574383, + -3.63698033, + -3.67818129, + -3.71934677, + -3.76047679, + -3.80157142, + -3.8426307, + -3.88365467, + -3.92464338, + -3.96559689, + -4.00651522, + -4.04739844, + -4.08824658, + -4.1290597, + -4.16983783, + -4.21058103, + -4.25128934, + ] + ) testValue = 0 sensorTemp = np.array(tempLog.temperature) @@ -203,7 +287,7 @@ def run(tempFault): else: if abs(sensorTemp[ii]) > 1e-6: testValue = testValue + 1 - testValue = testValue/len(nominals) + testValue = testValue / len(nominals) elif tempFault == "BIASED": biasVal = 1.0 tempMeasurementModel.senBias = biasVal @@ -217,23 +301,22 @@ def run(tempFault): else: NotImplementedError("Fault type specified does not exist.") - sensorTemp = np.array(tempLog.temperature) print(sensorTemp) - - # # compare the module results to the true values # if tempFault == "TEMP_FAULT_SPIKING": - if not unitTestSupport.isDoubleEqualRelative(testValue, truthValue, 5E-1): # only 101 values so need this to be relatively relaxed (within 50%) - testFailCount+= 1 - elif not unitTestSupport.isDoubleEqualRelative(sensorTemp[-1], truthValue, 1E-12): + if not unitTestSupport.isDoubleEqualRelative( + testValue, truthValue, 5e-1 + ): # only 101 values so need this to be relatively relaxed (within 50%) + testFailCount += 1 + elif not unitTestSupport.isDoubleEqualRelative(sensorTemp[-1], truthValue, 1e-12): testFailCount += 1 - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found @@ -248,6 +331,7 @@ def test_gauss_markov_properties(): [testResults, testMessage] = gauss_markov_test() assert testResults < 1, testMessage + def gauss_markov_test(): testFailCount = 0 testMessages = [] @@ -264,7 +348,7 @@ def gauss_markov_test(): # Set up the thermal sensor model sensorThermalModel = sensorThermal.SensorThermal() - sensorThermalModel.ModelTag = 'sensorThermalModel' + sensorThermalModel.ModelTag = "sensorThermalModel" sensorThermalModel.nHat_B = [0, 0, 1] sensorThermalModel.sensorArea = 1.0 sensorThermalModel.sensorAbsorptivity = 0.25 @@ -276,8 +360,12 @@ def gauss_markov_test(): # Set up required messages scStateMsg = messaging.SCStatesMsg().write(messaging.SCStatesMsgPayload()) - sunMsg = messaging.SpicePlanetStateMsg().write(messaging.SpicePlanetStateMsgPayload()) - deviceStatusMsg = messaging.DeviceStatusMsg().write(messaging.DeviceStatusMsgPayload()) + sunMsg = messaging.SpicePlanetStateMsg().write( + messaging.SpicePlanetStateMsgPayload() + ) + deviceStatusMsg = messaging.DeviceStatusMsg().write( + messaging.DeviceStatusMsgPayload() + ) sensorThermalModel.stateInMsg.subscribeTo(scStateMsg) sensorThermalModel.sunInMsg.subscribeTo(sunMsg) @@ -321,7 +409,7 @@ def gauss_markov_test(): print(f"Bound: {tempMeasurementModel.walkBounds}") # Test 1: Statistical Checks - countAllow = len(tempData) * 0.3/100. # Allow 0.3% violations + countAllow = len(tempData) * 0.3 / 100.0 # Allow 0.3% violations tempDiffCount = 0 for temp in tempData: if abs(temp - sensorThermalModel.T_0) > tempMeasurementModel.walkBounds: @@ -329,13 +417,18 @@ def gauss_markov_test(): if tempDiffCount > countAllow: testFailCount += 1 - testMessages.append(f"FAILED: Too many temperature errors ({tempDiffCount} > {countAllow})") + testMessages.append( + f"FAILED: Too many temperature errors ({tempDiffCount} > {countAllow})" + ) # Test 2: Error Bound Usage Check - similar to simpleNav approach sigmaThreshold = 0.8 hasLargeError = False for temp in tempData: - if abs(temp - sensorThermalModel.T_0) > tempMeasurementModel.walkBounds * sigmaThreshold: + if ( + abs(temp - sensorThermalModel.T_0) + > tempMeasurementModel.walkBounds * sigmaThreshold + ): hasLargeError = True break @@ -346,7 +439,7 @@ def gauss_markov_test(): if testFailCount == 0: print("PASSED: Gauss-Markov noise tests successful") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": diff --git a/src/simulation/dynamics/DynOutput/boreAngCalc/_UnitTest/test_bore_ang_calc.py b/src/simulation/dynamics/DynOutput/boreAngCalc/_UnitTest/test_bore_ang_calc.py index 2edaf4d55e..da262de090 100755 --- a/src/simulation/dynamics/DynOutput/boreAngCalc/_UnitTest/test_bore_ang_calc.py +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/_UnitTest/test_bore_ang_calc.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,7 +15,6 @@ # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - # # Bore Angle Calculation Test # @@ -40,56 +38,91 @@ path = os.path.dirname(os.path.abspath(__file__)) + class ResultsStore: def __init__(self): self.PassFail = [] + def texSnippet(self): for i in range(len(self.PassFail)): - snippetName = 'Result' + str(i) - if self.PassFail[i] == 'PASSED': - textColor = 'ForestGreen' - elif self.PassFail[i] == 'FAILED': - textColor = 'Red' - texSnippet = r'\textcolor{' + textColor + '}{'+ self.PassFail[i] + '}' + snippetName = "Result" + str(i) + if self.PassFail[i] == "PASSED": + textColor = "ForestGreen" + elif self.PassFail[i] == "FAILED": + textColor = "Red" + texSnippet = r"\textcolor{" + textColor + "}{" + self.PassFail[i] + "}" unitTestSupport.writeTeXSnippet(snippetName, texSnippet, path) + @pytest.fixture(scope="module") def testFixture(): listRes = ResultsStore() yield listRes listRes.texSnippet() + # uncomment this line is 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(True) # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("boresightLoc, eulerLoc", - [([1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3)], [0.0, 0.0, 0.0]), - ([-1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3)], [0.0, 0.0, 0.0]), - ([1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3)], [0.0, 0.0, 0.0]), - ([-1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3)], [0.0, 0.0, 0.0]), - ([1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3)], [0.0, 0.0, 0.0]), - ([-1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3)], [0.0, 0.0, 0.0]), - ([1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3)], [0.0, 0.0, 0.0]), - ([-1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3)], [0.0, 0.0, 0.0]), - ([0.0, 0.0, 1.0], [numpy.pi / 4, numpy.pi / 4, 0.0]), - ([0.0, 0.0, 1.0], [3 * numpy.pi / 4, numpy.pi / 4, 0.0]), - ([0.0, 0.0, 1.0], [5 * numpy.pi / 4, numpy.pi / 4, 0.0]), - ([0.0, 0.0, 1.0], [-numpy.pi / 4, numpy.pi / 4, 0.0]), - ([0.0, 0.0, 1.0], [numpy.pi / 4, -numpy.pi / 4, 0.0]), - ([0.0, 0.0, 1.0], [3 * numpy.pi / 4, -numpy.pi / 4, 0.0]), - ([0.0, 0.0, 1.0], [5 * numpy.pi / 4, -numpy.pi / 4, 0.0]), - ([0.0, 0.0, 1.0], [-numpy.pi / 4, -numpy.pi / 4, 0.0]), - ([1.0, 0.0, 0.0], [0.0, 0.0, 0.0])]) +@pytest.mark.parametrize( + "boresightLoc, eulerLoc", + [ + ( + [1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3)], + [0.0, 0.0, 0.0], + ), + ( + [-1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3)], + [0.0, 0.0, 0.0], + ), + ( + [1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3)], + [0.0, 0.0, 0.0], + ), + ( + [-1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3)], + [0.0, 0.0, 0.0], + ), + ( + [1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3)], + [0.0, 0.0, 0.0], + ), + ( + [-1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3)], + [0.0, 0.0, 0.0], + ), + ( + [1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3)], + [0.0, 0.0, 0.0], + ), + ( + [-1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3), -1.0 / numpy.sqrt(3)], + [0.0, 0.0, 0.0], + ), + ([0.0, 0.0, 1.0], [numpy.pi / 4, numpy.pi / 4, 0.0]), + ([0.0, 0.0, 1.0], [3 * numpy.pi / 4, numpy.pi / 4, 0.0]), + ([0.0, 0.0, 1.0], [5 * numpy.pi / 4, numpy.pi / 4, 0.0]), + ([0.0, 0.0, 1.0], [-numpy.pi / 4, numpy.pi / 4, 0.0]), + ([0.0, 0.0, 1.0], [numpy.pi / 4, -numpy.pi / 4, 0.0]), + ([0.0, 0.0, 1.0], [3 * numpy.pi / 4, -numpy.pi / 4, 0.0]), + ([0.0, 0.0, 1.0], [5 * numpy.pi / 4, -numpy.pi / 4, 0.0]), + ([0.0, 0.0, 1.0], [-numpy.pi / 4, -numpy.pi / 4, 0.0]), + ([1.0, 0.0, 0.0], [0.0, 0.0, 0.0]), + ], +) # # provide a unique test method name, starting with test_ def test_bore_ang_calc(testFixture, show_plots, boresightLoc, eulerLoc): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = bore_ang_calc_func(testFixture, show_plots, boresightLoc, eulerLoc) + [testResults, testMessage] = bore_ang_calc_func( + testFixture, show_plots, boresightLoc, eulerLoc + ) assert testResults < 1, testMessage + # Run unit test def bore_ang_calc_func(testFixture, show_plots, boresightLoc, eulerLoc): testFailCount = 0 # zero unit test result counter @@ -135,7 +168,7 @@ def bore_ang_calc_func(testFixture, show_plots, boresightLoc, eulerLoc): TotalSim.AddModelToTask(unitTaskName, BACObject) # # Configure simulation - TotalSim.ConfigureStopTime(int(1.0 * 1E9)) + TotalSim.ConfigureStopTime(int(1.0 * 1e9)) dataLog = BACObject.angOutMsg.recorder() TotalSim.AddModelToTask(unitTaskName, dataLog) @@ -158,21 +191,27 @@ def bore_ang_calc_func(testFixture, show_plots, boresightLoc, eulerLoc): dcm_BN = RigidBodyKinematics.MRP2C(stateMessage.sigma_BN) relPosVector = numpy.subtract(spiceMessage.PositionVector, stateMessage.r_BN_N) relVelVector = numpy.subtract(spiceMessage.VelocityVector, stateMessage.v_BN_N) - magRelVelVec = numpy.sqrt(relVelVector[0] ** 2 + relVelVector[1] ** 2 + relVelVector[2] ** 2) + magRelVelVec = numpy.sqrt( + relVelVector[0] ** 2 + relVelVector[1] ** 2 + relVelVector[2] ** 2 + ) if magRelVelVec == 0: secPointVector = numpy.zeros((1, 3)) magSecPtVec = 0 else: - secPointVector = numpy.cross(relPosVector, relVelVector) / numpy.linalg.norm(numpy.cross(relPosVector, - relVelVector)) + secPointVector = numpy.cross(relPosVector, relVelVector) / numpy.linalg.norm( + numpy.cross(relPosVector, relVelVector) + ) magSecPtVec = 1 primPointVector = relPosVector / numpy.linalg.norm(relPosVector) # r_p/b_N dcm_PoN = numpy.zeros((3, 3)) dcm_PoN[0, 0:2] = primPointVector[0:2] - magPrimPtVec = numpy.sqrt(primPointVector[0] ** 2 + primPointVector[1] ** 2 + primPointVector[2] ** 2) + magPrimPtVec = numpy.sqrt( + primPointVector[0] ** 2 + primPointVector[1] ** 2 + primPointVector[2] ** 2 + ) if magPrimPtVec != 0 and magSecPtVec != 0: dcm_PoN_2 = numpy.cross(primPointVector, secPointVector) / numpy.linalg.norm( - numpy.cross(primPointVector, secPointVector)) + numpy.cross(primPointVector, secPointVector) + ) for i in range(3): dcm_PoN[2, i] = dcm_PoN_2[i] dcm_PoN_1 = numpy.cross(dcm_PoN_2, primPointVector) @@ -193,16 +232,20 @@ def bore_ang_calc_func(testFixture, show_plots, boresightLoc, eulerLoc): r_B = numpy.dot(dcm_BN, stateMessage.r_BN_N) # BN * N = B # Set tolersnce - AllowTolerance = 1E-10 + AllowTolerance = 1e-10 boreVecPoint_final = [numpy.ndarray.tolist(boreVecPoint_1)] simBoreVecPt_final = [simBoreVecPt[0]] - testFailCount, testMessages = unitTestSupport.compareArray(boreVecPoint_final, simBoreVecPt_final, - AllowTolerance, - "Calculating the vector boreVec_Po.", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + boreVecPoint_final, + simBoreVecPt_final, + AllowTolerance, + "Calculating the vector boreVec_Po.", + testFailCount, + testMessages, + ) # Truth values - #boreVecPoint_1 = [0.0, 1.0, 0.0] + # boreVecPoint_1 = [0.0, 1.0, 0.0] baselinePoint = [1.0, 0.0, 0.0] baselinePoint = numpy.array(baselinePoint) @@ -221,19 +264,25 @@ def bore_ang_calc_func(testFixture, show_plots, boresightLoc, eulerLoc): boresightAzimuth = numpy.arctan2(boreVecPoint_1[2], boreVecPoint_1[1]) # Next Check - AllowTolerance = 1E-10 + AllowTolerance = 1e-10 simMiss_final = numpy.array(simMiss[-1]) - if (boresightMissAng - simMiss_final) > AllowTolerance: # Skip test days that are Sunday because of the end of a GPS week + if ( + (boresightMissAng - simMiss_final) > AllowTolerance + ): # Skip test days that are Sunday because of the end of a GPS week testFailCount += 1 testMessages.append( - "FAILED: Calculating the miss angle of the boresight failed with difference of: %(DiffVal)f \n" % \ - {"DiffVal": boresightMissAng - simMiss_final}) + "FAILED: Calculating the miss angle of the boresight failed with difference of: %(DiffVal)f \n" + % {"DiffVal": boresightMissAng - simMiss_final} + ) simAz_final = numpy.array(simAz[-1]) - if (boresightAzimuth - simAz_final) > AllowTolerance: # Skip test days that are Sunday because of the end of a GPS week + if ( + (boresightAzimuth - simAz_final) > AllowTolerance + ): # Skip test days that are Sunday because of the end of a GPS week testFailCount += 1 testMessages.append( - "FAILED: Calculating the azimuth angle of the boresight failed with difference of: %(DiffVal)f \n" % \ - {"DiffVal": boresightAzimuth - simAz_final}) + "FAILED: Calculating the azimuth angle of the boresight failed with difference of: %(DiffVal)f \n" + % {"DiffVal": boresightAzimuth - simAz_final} + ) # print out success message if no error were found if testFailCount == 0: @@ -245,11 +294,16 @@ def bore_ang_calc_func(testFixture, show_plots, boresightLoc, eulerLoc): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + # This statement below ensures that the unit test scrip can be run as a # stand-along python script # if __name__ == "__main__": - bore_ang_calc_func(ResultsStore(), False, # show_plots - [1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3)], [0.0, 0.0, 0.0]) + bore_ang_calc_func( + ResultsStore(), + False, # show_plots + [1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3), 1.0 / numpy.sqrt(3)], + [0.0, 0.0, 0.0], + ) diff --git a/src/simulation/dynamics/DynOutput/boreAngCalc/_UnitTest/test_bore_ang_calc_inertial_heading.py b/src/simulation/dynamics/DynOutput/boreAngCalc/_UnitTest/test_bore_ang_calc_inertial_heading.py index 0c3d74e54a..b5bc393b8b 100644 --- a/src/simulation/dynamics/DynOutput/boreAngCalc/_UnitTest/test_bore_ang_calc_inertial_heading.py +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/_UnitTest/test_bore_ang_calc_inertial_heading.py @@ -30,27 +30,38 @@ import pytest from Basilisk.architecture import messaging from Basilisk.simulation import boreAngCalc -from Basilisk.utilities import SimulationBaseClass, macros as mc, RigidBodyKinematics as rbk, unitTestSupport +from Basilisk.utilities import ( + SimulationBaseClass, + macros as mc, + RigidBodyKinematics as rbk, + unitTestSupport, +) path = os.path.dirname(os.path.abspath(__file__)) # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("inertialHeading, eulerLoc", - [([1.0, 0.0, 0.0], [0.0, 0.0, 0.0]), - ([0.0, 1.0, 0.0], [0.0, 0.0, 0.0]), - ([0.0, 0.0, 1.0], [0.0, 0.0, 0.0]), - ([1.0, 0.0, 0.0], [np.pi / 4, 0.0, - np.pi / 4]), - ([0.0, 1.0, 0.0], [np.pi / 4, 0.0, - np.pi / 4]), - ([0.0, 0.0, 1.0], [np.pi / 4, 0.0, - np.pi / 4]), - ([1 / np.sqrt(2), - 1 / np.sqrt(2), 0.0], [np.pi / 4, 0.0, - np.pi / 4]), - ([0.0, 1 / np.sqrt(2), 1 / np.sqrt(2)], [np.pi / 4, 0.0, - np.pi / 4]), - ([1 / np.sqrt(2), 0.0, - 1 / np.sqrt(2)], [np.pi / 4, 0.0, - np.pi / 4])]) +@pytest.mark.parametrize( + "inertialHeading, eulerLoc", + [ + ([1.0, 0.0, 0.0], [0.0, 0.0, 0.0]), + ([0.0, 1.0, 0.0], [0.0, 0.0, 0.0]), + ([0.0, 0.0, 1.0], [0.0, 0.0, 0.0]), + ([1.0, 0.0, 0.0], [np.pi / 4, 0.0, -np.pi / 4]), + ([0.0, 1.0, 0.0], [np.pi / 4, 0.0, -np.pi / 4]), + ([0.0, 0.0, 1.0], [np.pi / 4, 0.0, -np.pi / 4]), + ([1 / np.sqrt(2), -1 / np.sqrt(2), 0.0], [np.pi / 4, 0.0, -np.pi / 4]), + ([0.0, 1 / np.sqrt(2), 1 / np.sqrt(2)], [np.pi / 4, 0.0, -np.pi / 4]), + ([1 / np.sqrt(2), 0.0, -1 / np.sqrt(2)], [np.pi / 4, 0.0, -np.pi / 4]), + ], +) def test_bore_ang_calc_inertial_heading(show_plots, inertialHeading, eulerLoc): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = bore_ang_calc_inertial_heading_func(show_plots, inertialHeading, eulerLoc) + [testResults, testMessage] = bore_ang_calc_inertial_heading_func( + show_plots, inertialHeading, eulerLoc + ) assert testResults < 1, testMessage @@ -103,10 +114,12 @@ def bore_ang_calc_inertial_heading_func(show_plots, inertialHeading, eulerLoc): simMissAngle = dataLog.missAngle[0] # Compare the results - tol = 1E-10 + tol = 1e-10 if not unitTestSupport.isDoubleEqual(missAngle, simMissAngle, tol): testFailCount += 1 - testMessages.append("FAILED: Calculating the miss angle of the boresight failed \n") + testMessages.append( + "FAILED: Calculating the miss angle of the boresight failed \n" + ) # print out success message if no error were found if testFailCount == 0: @@ -114,13 +127,15 @@ def bore_ang_calc_inertial_heading_func(show_plots, inertialHeading, eulerLoc): else: print(testMessages) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # This statement below ensures that the unit test scrip can be run as a # stand-along python script # if __name__ == "__main__": - test_bore_ang_calc_inertial_heading(False, # show_plots - [1.0 / np.sqrt(3), 1.0 / np.sqrt(3), 1.0 / np.sqrt(3)], - [np.pi, 0.0, 0.0]) + test_bore_ang_calc_inertial_heading( + False, # show_plots + [1.0 / np.sqrt(3), 1.0 / np.sqrt(3), 1.0 / np.sqrt(3)], + [np.pi, 0.0, 0.0], + ) diff --git a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/Support/orb_elem_convert_support.py b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/Support/orb_elem_convert_support.py index df27ab4efc..d297e5819e 100644 --- a/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/Support/orb_elem_convert_support.py +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/_Documentation/Support/orb_elem_convert_support.py @@ -28,25 +28,21 @@ # - - - - - import math import matplotlib.pyplot as plt + # @cond DOXYGEN_IGNORE import numpy from Basilisk.utilities import macros as mc a = 10000000.0 e = 0.99 -i = 33.3*mc.D2R -AN = 48.2*mc.D2R -AP = 347.8*mc.D2R -f = 85.3*mc.D2R -mu = 0.3986004415E+15 +i = 33.3 * mc.D2R +AN = 48.2 * mc.D2R +AP = 347.8 * mc.D2R +f = 85.3 * mc.D2R +mu = 0.3986004415e15 ePlot = [] aInit = [] aFin = [] @@ -76,15 +72,33 @@ theta = AP + f # true latitude angle h = math.sqrt(mu * p) # orbit ang.momentum mag. rTruth = numpy.zeros(3) - rTruth[0] = r * (math.cos(AN) * math.cos(theta) - math.sin(AN) * math.sin(theta) * math.cos(i)) - rTruth[1] = r * (math.sin(AN) * math.cos(theta) + math.cos(AN) * math.sin(theta) * math.cos(i)) + rTruth[0] = r * ( + math.cos(AN) * math.cos(theta) + - math.sin(AN) * math.sin(theta) * math.cos(i) + ) + rTruth[1] = r * ( + math.sin(AN) * math.cos(theta) + + math.cos(AN) * math.sin(theta) * math.cos(i) + ) rTruth[2] = r * (math.sin(theta) * math.sin(i)) vTruth = numpy.zeros(3) - vTruth[0] = -mu / h * (math.cos(AN) * (math.sin(theta) + e * math.sin(AP)) + math.sin(AN) * (math.cos( - theta) + e * math.cos(AP)) * math.cos(i)) - vTruth[1] = -mu / h * (math.sin(AN) * (math.sin(theta) + e * math.sin(AP)) - math.cos(AN) * (math.cos( - theta) + e * math.cos(AP)) * math.cos(i)) + vTruth[0] = ( + -mu + / h + * ( + math.cos(AN) * (math.sin(theta) + e * math.sin(AP)) + + math.sin(AN) * (math.cos(theta) + e * math.cos(AP)) * math.cos(i) + ) + ) + vTruth[1] = ( + -mu + / h + * ( + math.sin(AN) * (math.sin(theta) + e * math.sin(AP)) + - math.cos(AN) * (math.cos(theta) + e * math.cos(AP)) * math.cos(i) + ) + ) vTruth[2] = -mu / h * (-(math.cos(theta) + e * math.cos(AP)) * math.sin(i)) ######### Calculate rv2elem ######### @@ -111,12 +125,12 @@ # compute semi - major axis alpha = 2.0 / r - v * v / mu - if (math.fabs(alpha) > epsConv): # elliptic or hyperbolic case + if math.fabs(alpha) > epsConv: # elliptic or hyperbolic case aO = 1.0 / alpha rApoap = p / (1.0 - eO) - else: # parabolic case + else: # parabolic case rp = p / 2.0 - aO = -rp # a is not defined for parabola, so -rp is returned instead + aO = -rp # a is not defined for parabola, so -rp is returned instead rApoap = -1.0 ePlot.append(e) aInit.append(a) @@ -125,6 +139,6 @@ aDiff = numpy.subtract(aInit, aFin) plt.figure() plt.plot(ePlot, aDiff) -plt.xlabel('Eccentricity') -plt.ylabel('Semimajor Axis Discrepancy (km)') +plt.xlabel("Eccentricity") +plt.ylabel("Semimajor Axis Discrepancy (km)") plt.show() diff --git a/src/simulation/dynamics/DynOutput/orbElemConvert/_UnitTest/test_orb_elem_convert.py b/src/simulation/dynamics/DynOutput/orbElemConvert/_UnitTest/test_orb_elem_convert.py index dc5d90768b..dd11116987 100644 --- a/src/simulation/dynamics/DynOutput/orbElemConvert/_UnitTest/test_orb_elem_convert.py +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/_UnitTest/test_orb_elem_convert.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -36,7 +35,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') +splitPath = path.split("simulation") from Basilisk.utilities import SimulationBaseClass from Basilisk.simulation import orbElemConvert @@ -46,109 +45,509 @@ from Basilisk.architecture import messaging from Basilisk.utilities.pythonVariableLogger import PythonVariableLogger + # Class in order to plot using data across the different parameterized scenarios class DataStore: def __init__(self): - self.Date = [] # replace these with appropriate containers for the data to be stored for plotting + self.Date = [] # replace these with appropriate containers for the data to be stored for plotting self.MarsPosErr = [] self.EarthPosErr = [] self.SunPosErr = [] -@pytest.mark.parametrize("a, e, i, AN, AP, f, mu, name", [ - # Inclined Elliptical Orbit Varying e - (10000000.0, 0.01, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 'IncEllip_e_1'), - (10000000.0, 0.10, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0), - (10000000.0, 0.25, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0), - (10000000.0, 0.50, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0), - (10000000.0, 0.75, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 'IncEllip_e_2'), - # Inclined Elliptical Orbit Varying a - (10000000.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncEllip_a_1'), - (100000.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (10000.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (1000.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (100.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (10.0, 0.50, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncEllip_a_2'), - - # Equatorial Elliptical Orbit Varying e - (10000000.0, 0.01, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquEllip_e_1'), - (10000000.0, 0.10, 0.0, 0.0, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0), - (10000000.0, 0.25, 0.0, 0.0, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0), - (10000000.0, 0.50, 0.0, 0.0, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 0), - (10000000.0, 0.75, 0.0, 0.0, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15, 'EquEllip_e_2'), - # Equatorial Elliptical Orbit Varying a - (10000000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquEllip_a_1'), # For i=0 => AN=0 - (100000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (10000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (1000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (100.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (10.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquEllip_a_2'), - - # Inclined Circular Orbit - (10000000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 'IncCirc_1'), - (1000000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (100000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (10000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (1000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (100.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (10.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 'IncCirc_2'), - - # Equatorial Circular Orbit - (10000000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 'EquCirc_1'), - (1000000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (100000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (10000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (1000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (100.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (10.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 'EquCirc_2'), - - # Inclined Parabolic Orbit - (-10.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncPara_1'), # For input of -a, - (-100.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), # must have e= 1.0 - (-1000.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), # or e >1.0 - (-10000.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-100000.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncPara_2'), - - # Equatorial Parabolic Orbit - (-10.0, 1.0, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquPara_1'), # For input of -a, - (-100.0, 1.0, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), # must have e= 1.0 - (-1000.0, 1.0, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), # or e >1.0 - (-10000.0, 1.0, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-100000.0, 1.0, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquPara_2'), - - # Inclined Hyperbolic Orbit varying a - (-10.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncHyp_a_1'), - (-100.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-1000.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-10000.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-100000.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncHyp_a_2'), - # Inclined Hyperbolic Orbit varying e - (-100000.0, 1.1, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncHyp_e_1'), - (-100000.0, 1.2, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-100000.0, 1.3, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-100000.0, 1.4, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-100000.0, 1.5, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'IncHyp_e_2'), - - # Equatorial Hyperbolic Orbit varying a - (-10.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquHyp_a_1'), - (-100.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-1000.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-10000.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-100000.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquHyp_a_2'), - # Equatorial Hyperbolic Orbit varying e - (-100000.0, 1.1, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquHyp_e_1'), - (-100000.0, 1.2, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-100000.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-100000.0, 1.4, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 0), - (-100000.0, 1.5, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15, 'EquHyp_e_2'), - - # # Approaching circular orbit - # (100000.0, 0.000001, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15), - - # These don't work - # (10000000.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15), # or e >1.0 - # (-10, 0.9, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15) -]) +@pytest.mark.parametrize( + "a, e, i, AN, AP, f, mu, name", + [ + # Inclined Elliptical Orbit Varying e + ( + 10000000.0, + 0.01, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncEllip_e_1", + ), + ( + 10000000.0, + 0.10, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 10000000.0, + 0.25, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 10000000.0, + 0.50, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 10000000.0, + 0.75, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncEllip_e_2", + ), + # Inclined Elliptical Orbit Varying a + ( + 10000000.0, + 0.50, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncEllip_a_1", + ), + ( + 100000.0, + 0.50, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 10000.0, + 0.50, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 1000.0, + 0.50, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 100.0, + 0.50, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 10.0, + 0.50, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncEllip_a_2", + ), + # Equatorial Elliptical Orbit Varying e + ( + 10000000.0, + 0.01, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "EquEllip_e_1", + ), + (10000000.0, 0.10, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + (10000000.0, 0.25, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + (10000000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + ( + 10000000.0, + 0.75, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "EquEllip_e_2", + ), + # Equatorial Elliptical Orbit Varying a + ( + 10000000.0, + 0.50, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "EquEllip_a_1", + ), # For i=0 => AN=0 + (100000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + (10000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + (1000.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + (100.0, 0.50, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + ( + 10.0, + 0.50, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "EquEllip_a_2", + ), + # Inclined Circular Orbit + ( + 10000000.0, + 0.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 0.0, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncCirc_1", + ), + ( + 1000000.0, + 0.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 0.0, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 100000.0, + 0.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 0.0, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 10000.0, + 0.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 0.0, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 1000.0, + 0.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 0.0, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 100.0, + 0.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 0.0, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + 10.0, + 0.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 0.0, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncCirc_2", + ), + # Equatorial Circular Orbit + (10000000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415e15, "EquCirc_1"), + (1000000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415e15, 0), + (100000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415e15, 0), + (10000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415e15, 0), + (1000.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415e15, 0), + (100.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415e15, 0), + (10.0, 0.0, 0.0, 0.0, 0.0, 85.3 * mc.D2R, 0.3986004415e15, "EquCirc_2"), + # Inclined Parabolic Orbit + ( + -10.0, + 1.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncPara_1", + ), # For input of -a, + ( + -100.0, + 1.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), # must have e= 1.0 + ( + -1000.0, + 1.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), # or e >1.0 + ( + -10000.0, + 1.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + -100000.0, + 1.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncPara_2", + ), + # Equatorial Parabolic Orbit + ( + -10.0, + 1.0, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "EquPara_1", + ), # For input of -a, + ( + -100.0, + 1.0, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), # must have e= 1.0 + ( + -1000.0, + 1.0, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), # or e >1.0 + (-10000.0, 1.0, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + ( + -100000.0, + 1.0, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "EquPara_2", + ), + # Inclined Hyperbolic Orbit varying a + ( + -10.0, + 1.3, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncHyp_a_1", + ), + ( + -100.0, + 1.3, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + -1000.0, + 1.3, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + -10000.0, + 1.3, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + -100000.0, + 1.3, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncHyp_a_2", + ), + # Inclined Hyperbolic Orbit varying e + ( + -100000.0, + 1.1, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncHyp_e_1", + ), + ( + -100000.0, + 1.2, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + -100000.0, + 1.3, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + -100000.0, + 1.4, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + ), + ( + -100000.0, + 1.5, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "IncHyp_e_2", + ), + # Equatorial Hyperbolic Orbit varying a + ( + -10.0, + 1.3, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "EquHyp_a_1", + ), + (-100.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + (-1000.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + (-10000.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + ( + -100000.0, + 1.3, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "EquHyp_a_2", + ), + # Equatorial Hyperbolic Orbit varying e + ( + -100000.0, + 1.1, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "EquHyp_e_1", + ), + (-100000.0, 1.2, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + (-100000.0, 1.3, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + (-100000.0, 1.4, 0.0, 0.0, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415e15, 0), + ( + -100000.0, + 1.5, + 0.0, + 0.0, + 347.8 * mc.D2R, + 85.3 * mc.D2R, + 0.3986004415e15, + "EquHyp_e_2", + ), + # # Approaching circular orbit + # (100000.0, 0.000001, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15), + # These don't work + # (10000000.0, 1.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 347.8 * mc.D2R, 85.3 * mc.D2R, 0.3986004415E+15), # or e >1.0 + # (-10, 0.9, 33.3*mc.D2R, 48.2*mc.D2R, 347.8*mc.D2R, 85.3*mc.D2R, 0.3986004415E+15) + ], +) # provide a unique test method name, starting with test_ def test_orb_elem_convert(a, e, i, AN, AP, f, mu, name, DispPlot=False): @@ -157,6 +556,7 @@ def test_orb_elem_convert(a, e, i, AN, AP, f, mu, name, DispPlot=False): [testResults, testMessage] = orbElem(a, e, i, AN, AP, f, mu, name, DispPlot) assert testResults < 1, testMessage + # Run unit test def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): # Elem2RV @@ -198,7 +598,7 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): ElemMessage.a = 0.0 ElemMessage.rPeriap = -a else: - ElemMessage.a = a # meters + ElemMessage.a = a # meters ElemMessage.e = e ElemMessage.i = i ElemMessage.Omega = AN @@ -213,7 +613,7 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): TotalSim.AddModelToTask(unitTaskName, dataLog) # Execute simulation - TotalSim.ConfigureStopTime(int(1E9)) + TotalSim.ConfigureStopTime(int(1e9)) TotalSim.InitializeSimulation() TotalSim.ExecuteSimulation() @@ -225,14 +625,18 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): rMsgPlanet = dataLog.PositionVector[-1] vMsgPlanet = dataLog.VelocityVector[-1] numpy.testing.assert_allclose( - rSim, rMsgPlanet, - rtol=epsDiff, verbose=True, - err_msg="FAILED: Planet Position Message" + rSim, + rMsgPlanet, + rtol=epsDiff, + verbose=True, + err_msg="FAILED: Planet Position Message", ) numpy.testing.assert_allclose( - vSim, vMsgPlanet, - rtol=epsDiff, verbose=True, - err_msg="FAILED: Planet Velocity Message" + vSim, + vMsgPlanet, + rtol=epsDiff, + verbose=True, + err_msg="FAILED: Planet Velocity Message", ) # Calculation of elem2rv @@ -262,27 +666,41 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): h = math.sqrt(mu * p) # orbit ang.momentum mag. rTruth = numpy.zeros(3) - rTruth[0] = r * (math.cos(AN) * math.cos(theta) - math.sin(AN) * math.sin(theta) * math.cos(i)) - rTruth[1] = r * (math.sin(AN) * math.cos(theta) + math.cos(AN) * math.sin(theta) * math.cos(i)) + rTruth[0] = r * ( + math.cos(AN) * math.cos(theta) + - math.sin(AN) * math.sin(theta) * math.cos(i) + ) + rTruth[1] = r * ( + math.sin(AN) * math.cos(theta) + + math.cos(AN) * math.sin(theta) * math.cos(i) + ) rTruth[2] = r * (math.sin(theta) * math.sin(i)) vTruth = numpy.zeros(3) - vTruth[0] = -mu / h * (math.cos(AN) * (math.sin(theta) + e * math.sin(AP)) + math.sin(AN) * (math.cos( - theta) + e * math.cos(AP)) * math.cos(i)) - vTruth[1] = -mu / h * (math.sin(AN) * (math.sin(theta) + e * math.sin(AP)) - math.cos(AN) * (math.cos( - theta) + e * math.cos(AP)) * math.cos(i)) + vTruth[0] = ( + -mu + / h + * ( + math.cos(AN) * (math.sin(theta) + e * math.sin(AP)) + + math.sin(AN) * (math.cos(theta) + e * math.cos(AP)) * math.cos(i) + ) + ) + vTruth[1] = ( + -mu + / h + * ( + math.sin(AN) * (math.sin(theta) + e * math.sin(AP)) + - math.cos(AN) * (math.cos(theta) + e * math.cos(AP)) * math.cos(i) + ) + ) vTruth[2] = -mu / h * (-(math.cos(theta) + e * math.cos(AP)) * math.sin(i)) # Position and Velocity Diff Checks numpy.testing.assert_allclose( - rSim, rTruth, - rtol=epsDiff, verbose=True, - err_msg="FAILED: Position Vector" + rSim, rTruth, rtol=epsDiff, verbose=True, err_msg="FAILED: Position Vector" ) numpy.testing.assert_allclose( - vSim, vTruth, - rtol=epsDiff, verbose=True, - err_msg="FAILED: Velocity Vector" + vSim, vTruth, rtol=epsDiff, verbose=True, err_msg="FAILED: Velocity Vector" ) ###### RV2ELEM ###### @@ -305,14 +723,16 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): TotalSim.AddModelToTask(unitTaskName, orb_elemObject) # Log Variables - elemLog = PythonVariableLogger({ - "a": lambda _: orb_elemObject.CurrentElem.a, - "e": lambda _: orb_elemObject.CurrentElem.e, - "i": lambda _: orb_elemObject.CurrentElem.i, - "Omega": lambda _: orb_elemObject.CurrentElem.Omega, - "omega": lambda _: orb_elemObject.CurrentElem.omega, - "f": lambda _: orb_elemObject.CurrentElem.f, - }) + elemLog = PythonVariableLogger( + { + "a": lambda _: orb_elemObject.CurrentElem.a, + "e": lambda _: orb_elemObject.CurrentElem.e, + "i": lambda _: orb_elemObject.CurrentElem.i, + "Omega": lambda _: orb_elemObject.CurrentElem.Omega, + "omega": lambda _: orb_elemObject.CurrentElem.omega, + "f": lambda _: orb_elemObject.CurrentElem.f, + } + ) TotalSim.AddModelToTask(unitTaskName, elemLog) orb_elemObjectLog = orb_elemObject.logger(["r_N", "v_N"]) @@ -337,7 +757,7 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): TotalSim.AddModelToTask(unitTaskName, dataElemLog) # Execute simulation - TotalSim.ConfigureStopTime(int(1E9)) + TotalSim.ConfigureStopTime(int(1e9)) TotalSim.InitializeSimulation() TotalSim.ExecuteSimulation() @@ -349,7 +769,14 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): fOut = elemLog.f[-1] # Element Diff Check - ElemDiff = [(a - aOut), (e - eOut), (i - iOut), (AN - ANOut), (AP - APOut), (f - fOut)] + ElemDiff = [ + (a - aOut), + (e - eOut), + (i - iOut), + (AN - ANOut), + (AP - APOut), + (f - fOut), + ] ElemDiffcsv = numpy.asarray(ElemDiff) for g in range(6): # check for angle roll over with 2*pi @@ -369,7 +796,14 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): APMsg = dataElemLog.omega[-1] fMsg = dataElemLog.f[-1] - ElemMsgDiff = [(aOut - aMsg), (eOut - eMsg), (iOut - iMsg), (ANOut - ANMsg), (APOut - APMsg), (fOut - fMsg)] + ElemMsgDiff = [ + (aOut - aMsg), + (eOut - eMsg), + (iOut - iMsg), + (ANOut - ANMsg), + (APOut - APMsg), + (fOut - fMsg), + ] for g in range(6): # check for angle roll over with 2*pi if g > 2: @@ -402,19 +836,19 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): # compute semi - major axis alpha = 2.0 / r - v * v / mu - if (math.fabs(alpha) > epsConv): # elliptic or hyperbolic case + if math.fabs(alpha) > epsConv: # elliptic or hyperbolic case aO = 1.0 / alpha - else: # parabolic case - aO = 0.0 # a is not defined for parabola, so -rp is returned instead + else: # parabolic case + aO = 0.0 # a is not defined for parabola, so -rp is returned instead # Calculate the inclination iO = math.acos(hVec[2] / h) # Calculate AP, AN, and True anomaly if eO >= 1e-11 and iO >= 1e-11: - # Case 1: Non - circular, inclined orbit + # Case 1: Non - circular, inclined orbit Omega = math.acos(nVec[0] / n) - if (nVec[1] < 0.0): + if nVec[1] < 0.0: Omega = 2.0 * math.pi - Omega omega = math.acos(numpy.dot(nVec, eVec) / n / eO) if eVec[2] < 0.0: @@ -423,8 +857,8 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): if numpy.dot(rTruth, vTruth) < 0.0: fO = 2.0 * math.pi - fO elif eO >= 1e-11 and iO < 1e-11: - # Case 2: Non - circular, equatorial orbit - # Equatorial orbit has no ascending node + # Case 2: Non - circular, equatorial orbit + # Equatorial orbit has no ascending node Omega = 0.0 # True longitude of periapsis, omegatilde_true omega = math.acos(eVec[0] / eO) @@ -434,9 +868,9 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): if numpy.dot(rTruth, vTruth) < 0.0: fO = 2.0 * math.pi - fO elif eO < 1e-11 and iO >= 1e-11: - # Case 3: Circular, inclined orbit + # Case 3: Circular, inclined orbit Omega = math.acos(nVec[0] / n) - if (nVec[1] < 0.0): + if nVec[1] < 0.0: Omega = 2.0 * math.pi - Omega omega = 0.0 # Argument of latitude, u = omega + f * / @@ -444,7 +878,7 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): if rTruth[2] < 0.0: fO = 2.0 * math.pi - fO elif eO < 1e-11 and iO < 1e-11: - # Case 4: Circular, equatorial orbit + # Case 4: Circular, equatorial orbit Omega = 0.0 omega = 0.0 # True longitude, lambda_true @@ -453,12 +887,19 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): fO = 2.0 * math.pi - fO else: print("Error: rv2elem couldn't identify orbit type.\n") - if (eO >= 1.0 and math.fabs(fO) > math.pi): + if eO >= 1.0 and math.fabs(fO) > math.pi: twopiSigned = math.copysign(2.0 * math.pi, fO) fO -= twopiSigned # Element Diff Check - ElemCalcDiff = [(aO - aOut), (eO - eOut), (iO - iOut), (Omega - ANOut), (omega - APOut), (fOut - fOut)] + ElemCalcDiff = [ + (aO - aOut), + (eO - eOut), + (iO - iOut), + (Omega - ANOut), + (omega - APOut), + (fOut - fOut), + ] ElemCalcDiffcsv = numpy.asarray(ElemCalcDiff) for g in range(6): # check for angle roll over with 2*pi @@ -473,59 +914,82 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): # create plot # txt = 'e = ' + str(e) + ' and a = ' + str(a) + 'km' - fact = (len(str(abs(a)))-3.0) - plt.close('all') + fact = len(str(abs(a))) - 3.0 + plt.close("all") - plt.figure(1,figsize=(7, 5), dpi=80, facecolor='w', edgecolor='k') + plt.figure(1, figsize=(7, 5), dpi=80, facecolor="w", edgecolor="k") # fig1.text(.5, .05, txt, ha='center') ax1 = plt.subplot(211) ax1.cla() index = numpy.arange(3) bar_width = 0.35 opacity = 0.8 - rects1 = ax1.bar(index, rSim, bar_width, alpha=opacity, color='b', label='Simulated Position') - rects2 = ax1.bar(index + bar_width, rTruth, bar_width, alpha=opacity, color='g', label='Calculated Position') - ax1.spines['left'].set_position('zero') - ax1.spines['right'].set_color('none') - ax1.spines['bottom'].set_position('zero') - ax1.spines['top'].set_color('none') + rects1 = ax1.bar( + index, rSim, bar_width, alpha=opacity, color="b", label="Simulated Position" + ) + rects2 = ax1.bar( + index + bar_width, + rTruth, + bar_width, + alpha=opacity, + color="g", + label="Calculated Position", + ) + ax1.spines["left"].set_position("zero") + ax1.spines["right"].set_color("none") + ax1.spines["bottom"].set_position("zero") + ax1.spines["top"].set_color("none") for xtick in ax1.get_xticklabels(): - xtick.set_bbox(dict(facecolor='white', edgecolor='None', alpha=0.5)) - ax1.xaxis.set_ticks_position('bottom') - ax1.yaxis.set_ticks_position('left') - plt.ylabel('Position (m)') - plt.xticks(index + bar_width, ('X', 'Y', 'Z')) - plt.legend(loc='lower right') + xtick.set_bbox(dict(facecolor="white", edgecolor="None", alpha=0.5)) + ax1.xaxis.set_ticks_position("bottom") + ax1.yaxis.set_ticks_position("left") + plt.ylabel("Position (m)") + plt.xticks(index + bar_width, ("X", "Y", "Z")) + plt.legend(loc="lower right") ax2 = plt.subplot(212) ax2.cla() - rects1 = ax2.bar(index, vSim, bar_width, alpha=opacity, color='b', label='Simulated Velocity') - rects2 = ax2.bar(index + bar_width, vTruth, bar_width, alpha=opacity, color='g', label='Calculated Velocity') - ax2.spines['left'].set_position('zero') - ax2.spines['right'].set_color('none') - ax2.spines['bottom'].set_position('zero') - ax2.spines['top'].set_color('none') + rects1 = ax2.bar( + index, vSim, bar_width, alpha=opacity, color="b", label="Simulated Velocity" + ) + rects2 = ax2.bar( + index + bar_width, + vTruth, + bar_width, + alpha=opacity, + color="g", + label="Calculated Velocity", + ) + ax2.spines["left"].set_position("zero") + ax2.spines["right"].set_color("none") + ax2.spines["bottom"].set_position("zero") + ax2.spines["top"].set_color("none") for xtick in ax2.get_xticklabels(): - xtick.set_bbox(dict(facecolor='white', edgecolor='None', alpha=0.5)) - ax2.xaxis.set_ticks_position('bottom') - ax2.yaxis.set_ticks_position('left') - plt.ylabel('Velocity (m/s)') - plt.xticks(index + bar_width, ('X', 'Y', 'Z')) - plt.legend(loc='lower right') + xtick.set_bbox(dict(facecolor="white", edgecolor="None", alpha=0.5)) + ax2.xaxis.set_ticks_position("bottom") + ax2.yaxis.set_ticks_position("left") + plt.ylabel("Velocity (m/s)") + plt.xticks(index + bar_width, ("X", "Y", "Z")) + plt.legend(loc="lower right") if name != 0: - unitTestSupport.writeFigureLaTeX(name, "$e = " + str(e) + "$ and $a = 10^" + str(int(fact)) + "$km", - plt, 'height=0.7\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + name, + "$e = " + str(e) + "$ and $a = 10^" + str(int(fact)) + "$km", + plt, + "height=0.7\\textwidth, keepaspectratio", + path, + ) if testFailCount2 == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" passFailMsg = "" # "Passed: " + name + "." - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" passFailMsg = "Failed: " + name + "." testMessages.append(passFailMsg) testMessages.append(" | ") - passedText = r'\textcolor{' + colorText + '}{' + "FAILED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "FAILED" + "}" # Write some snippets for AutoTex snippetName = name + "PassedText2" @@ -539,7 +1003,7 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): if DispPlot: plt.show() plt.close() - testFailCount = testFailCount1+testFailCount2 + testFailCount = testFailCount1 + testFailCount2 if testFailCount: print("Failed") @@ -547,8 +1011,18 @@ def orbElem(a, e, i, AN, AP, f, mu, name, DispPlot): else: print("PASSED") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": - orbElem(10000000.0, 0.0, 33.3 * mc.D2R, 48.2 * mc.D2R, 0.0, 85.3 * mc.D2R, 0.3986004415E+15, 0, True - ) + orbElem( + 10000000.0, + 0.0, + 33.3 * mc.D2R, + 48.2 * mc.D2R, + 0.0, + 85.3 * mc.D2R, + 0.3986004415e15, + 0, + True, + ) diff --git a/src/simulation/dynamics/ExtPulsedTorque/_UnitTest/test_extPulsedTorque.py b/src/simulation/dynamics/ExtPulsedTorque/_UnitTest/test_extPulsedTorque.py index 59912ba3ac..f34aa63cf4 100755 --- a/src/simulation/dynamics/ExtPulsedTorque/_UnitTest/test_extPulsedTorque.py +++ b/src/simulation/dynamics/ExtPulsedTorque/_UnitTest/test_extPulsedTorque.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -28,7 +27,9 @@ from Basilisk.simulation import ExtPulsedTorque from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -37,22 +38,18 @@ # @pytest.mark.xfail(True) -@pytest.mark.parametrize("offCount", [ - (3) - ,(0) -]) +@pytest.mark.parametrize("offCount", [(3), (0)]) # provide a unique test method name, starting with test_ def test_module(show_plots, offCount): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = run( - show_plots, offCount) + [testResults, testMessage] = run(show_plots, offCount) assert testResults < 1, testMessage def run(show_plots, offCount): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages unitTaskName = "unitTask" unitProcessName = "testProcess" @@ -73,7 +70,7 @@ def run(show_plots, offCount): testObject.readInputMessages() testObject.writeOutputMessages(0) - testObject.pulsedTorqueExternalPntB_B = [[-1], [1],[ -1]] + testObject.pulsedTorqueExternalPntB_B = [[-1], [1], [-1]] testObject.countOnPulse = 1 testObject.countOff = offCount @@ -96,60 +93,68 @@ def run(show_plots, offCount): DT = 0.1 testProcessRate = macros.sec2nano(DT) for tStop in range(1, 11): - scSim.ConfigureStopTime(macros.sec2nano(tStop*DT)) + scSim.ConfigureStopTime(macros.sec2nano(tStop * DT)) scSim.ExecuteSimulation() testObject.computeForceTorque(scSim.TotalSim.CurrentNanos, testProcessRate) scSim.TotalSim.SingleStepProcesses() # log the data - dataTorque = testObjectLog.torqueExternalPntB_B[1:,:] + dataTorque = testObjectLog.torqueExternalPntB_B[1:, :] np.set_printoptions(precision=16) # # set true position information # - if (offCount == 3): + if offCount == 3: trueTorque_B = [ - [0.0, 0.0, 0.0] - , [-1.0, 1.0, -1.0] - , [1.0, -1.0, 1.0] - , [0.0, 0.0, 0.0] - , [0.0, 0.0, 0.0] - , [0.0, 0.0, 0.0] - , [-1.0, 1.0, -1.0] - , [1.0, -1.0, 1.0] - , [0.0, 0.0, 0.0] - , [0.0, 0.0, 0.0] - , [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0], + [-1.0, 1.0, -1.0], + [1.0, -1.0, 1.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [-1.0, 1.0, -1.0], + [1.0, -1.0, 1.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], ] - if (offCount == 0): + if offCount == 0: trueTorque_B = [ - [0.0, 0.0, 0.0] - , [-1.0, 1.0, -1.0] - , [1.0, -1.0, 1.0] - , [-1.0, 1.0, -1.0] - , [1.0, -1.0, 1.0] - , [-1.0, 1.0, -1.0] - , [1.0, -1.0, 1.0] - , [-1.0, 1.0, -1.0] - , [1.0, -1.0, 1.0] - , [-1.0, 1.0, -1.0] - , [1.0, -1.0, 1.0] + [0.0, 0.0, 0.0], + [-1.0, 1.0, -1.0], + [1.0, -1.0, 1.0], + [-1.0, 1.0, -1.0], + [1.0, -1.0, 1.0], + [-1.0, 1.0, -1.0], + [1.0, -1.0, 1.0], + [-1.0, 1.0, -1.0], + [1.0, -1.0, 1.0], + [-1.0, 1.0, -1.0], + [1.0, -1.0, 1.0], ] # compare the module results to the truth values accuracy = 1e-12 - if (len(trueTorque_B) != len(dataTorque)): + if len(trueTorque_B) != len(dataTorque): testFailCount += 1 - testMessages.append("FAILED: ExtPulsedTorque failed torque unit test (unequal array sizes)\n") + testMessages.append( + "FAILED: ExtPulsedTorque failed torque unit test (unequal array sizes)\n" + ) else: - for i in range(0,len(trueTorque_B)): + for i in range(0, len(trueTorque_B)): # check a vector values - if not unitTestSupport.isArrayEqual(dataTorque[i],trueTorque_B[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + dataTorque[i], trueTorque_B[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: ExtPulsedTorque failed torque unit test at t=" + str(dataTorque[i,0]*macros.NANO2SEC) + "sec\n") + testMessages.append( + "FAILED: ExtPulsedTorque failed torque unit test at t=" + + str(dataTorque[i, 0] * macros.NANO2SEC) + + "sec\n" + ) # print out success message if no error were found if testFailCount == 0: @@ -157,13 +162,15 @@ def run(show_plots, offCount): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + # # This statement below ensures that the unit test scrip can be run as a # stand-along python script # if __name__ == "__main__": - test_module(False, # show_plots - 3 # offCount - ) + test_module( + False, # show_plots + 3, # offCount + ) diff --git a/src/simulation/dynamics/FuelTank/_UnitTest/test_mass_depletion.py b/src/simulation/dynamics/FuelTank/_UnitTest/test_mass_depletion.py index e56f9b3cd3..5bb3e98b53 100644 --- a/src/simulation/dynamics/FuelTank/_UnitTest/test_mass_depletion.py +++ b/src/simulation/dynamics/FuelTank/_UnitTest/test_mass_depletion.py @@ -34,8 +34,13 @@ path = os.path.dirname(os.path.abspath(filename)) -@pytest.mark.parametrize("thrusterConstructor", [thrusterDynamicEffector.ThrusterDynamicEffector, - thrusterStateEffector.ThrusterStateEffector]) +@pytest.mark.parametrize( + "thrusterConstructor", + [ + thrusterDynamicEffector.ThrusterDynamicEffector, + thrusterStateEffector.ThrusterStateEffector, + ], +) def test_massDepletionTest(show_plots, thrusterConstructor): """Module Unit Test""" # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -60,9 +65,9 @@ def test_massDepletionTest(show_plots, thrusterConstructor): # add thruster devices thFactory = simIncludeThruster.thrusterFactory() thFactory.create( - 'TEST_Thruster', + "TEST_Thruster", [1, 0, 0], # location in B-frame - [0, 1, 0] # direction in B-frame + [0, 1, 0], # direction in B-frame ) # create thruster object container and tie to spacecraft object @@ -94,10 +99,12 @@ def test_massDepletionTest(show_plots, thrusterConstructor): unitTestSim.earthGravBody = gravityEffector.GravBodyData() unitTestSim.earthGravBody.planetName = "earth_planet_data" - unitTestSim.earthGravBody.mu = 0.3986004415E+15 # meters + unitTestSim.earthGravBody.mu = 0.3986004415e15 # meters unitTestSim.earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([unitTestSim.earthGravBody]) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + [unitTestSim.earthGravBody] + ) dataLog = scObject.scStateOutMsg.recorder() fuelLog = unitTestSim.fuelTankStateEffector.fuelTankOutMsg.recorder() @@ -109,12 +116,22 @@ def test_massDepletionTest(show_plots, thrusterConstructor): scObject.hub.mHub = 750.0 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + 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.1], [0.2], [-0.3]] scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) unitTestSim.InitializeSimulation() @@ -125,9 +142,15 @@ def test_massDepletionTest(show_plots, thrusterConstructor): stopTime = 60.0 * 10.0 unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) unitTestSim.ExecuteSimulation() - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) thrust = thrLog.thrustForce_B thrustPercentage = thrLog.thrustFactor @@ -136,19 +159,37 @@ def test_massDepletionTest(show_plots, thrusterConstructor): plt.close("all") plt.figure(1) - plt.plot(orbAngMom_N[:, 0] * 1e-9, orbAngMom_N[:, 1] - orbAngMom_N[0, 1], orbAngMom_N[:, 0] * 1e-9, - orbAngMom_N[:, 2] - orbAngMom_N[0, 2], orbAngMom_N[:, 0] * 1e-9, orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) + plt.plot( + orbAngMom_N[:, 0] * 1e-9, + orbAngMom_N[:, 1] - orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * 1e-9, + orbAngMom_N[:, 2] - orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * 1e-9, + orbAngMom_N[:, 3] - orbAngMom_N[0, 3], + ) plt.title("Change in Orbital Angular Momentum") plt.figure(2) - plt.plot(rotAngMom_N[:, 0] * 1e-9, rotAngMom_N[:, 1] - rotAngMom_N[0, 1], rotAngMom_N[:, 0] * 1e-9, - rotAngMom_N[:, 2] - rotAngMom_N[0, 2], rotAngMom_N[:, 0] * 1e-9, rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + rotAngMom_N[:, 1] - rotAngMom_N[0, 1], + rotAngMom_N[:, 0] * 1e-9, + rotAngMom_N[:, 2] - rotAngMom_N[0, 2], + rotAngMom_N[:, 0] * 1e-9, + rotAngMom_N[:, 3] - rotAngMom_N[0, 3], + ) plt.title("Change in Rotational Angular Momentum") plt.figure(3) plt.plot(rotEnergy[:, 0] * 1e-9, rotEnergy[:, 1] - rotEnergy[0, 1]) plt.title("Change in Rotational Energy") plt.figure(4) - plt.plot(thrLog.times() * 1e-9, thrust[:, 0], thrLog.times() * 1e-9, thrust[:, 1], thrLog.times() * 1e-9, - thrust[:, 2]) + plt.plot( + thrLog.times() * 1e-9, + thrust[:, 0], + thrLog.times() * 1e-9, + thrust[:, 1], + thrLog.times() * 1e-9, + thrust[:, 2], + ) plt.xlim([0, 20]) plt.ylim([0, 1]) plt.title("Thrust") @@ -168,7 +209,7 @@ def test_massDepletionTest(show_plots, thrusterConstructor): if show_plots: plt.show() - plt.close('all') + plt.close("all") dataPos = posRef.getState() dataSigma = sigmaRef.getState() @@ -176,23 +217,41 @@ def test_massDepletionTest(show_plots, thrusterConstructor): dataSigma = [[dataSigma[0][0], dataSigma[1][0], dataSigma[2][0]]] if thrustersEffector.__class__.__name__ == "ThrusterDynamicEffector": - truePos = [[-6.7815933935338277e+06, 4.9468685979815889e+06, 5.4867416696776701e+06]] - trueSigma = [[1.4401781243854264e-01, -6.4168702021364002e-02, 3.0166086824900967e-01]] + truePos = [ + [-6.7815933935338277e06, 4.9468685979815889e06, 5.4867416696776701e06] + ] + trueSigma = [ + [1.4401781243854264e-01, -6.4168702021364002e-02, 3.0166086824900967e-01] + ] elif thrustersEffector.__class__.__name__ == "ThrusterStateEffector": truePos = [[-6781593.400948599, 4946868.619447934, 5486741.690842073]] trueSigma = [[0.14366625871003397, -0.06488330854626220, 0.3032637107362375]] for i in range(0, len(truePos)): - np.testing.assert_allclose(dataPos[i], truePos[i], rtol=1e-6, err_msg="Thruster position not equal") + np.testing.assert_allclose( + dataPos[i], truePos[i], rtol=1e-6, err_msg="Thruster position not equal" + ) for i in range(0, len(trueSigma)): # check a vector values - np.testing.assert_allclose(dataSigma[i], trueSigma[i], rtol=1e-4, err_msg="Thruster attitude not equal") + np.testing.assert_allclose( + dataSigma[i], trueSigma[i], rtol=1e-4, err_msg="Thruster attitude not equal" + ) # target value computed from MaxThrust / (EARTH_GRAV * steadyIsp) - np.testing.assert_allclose(fuelMassDot[100], -0.000403404216123, rtol=1e-3, - err_msg="Thruster mass depletion not ramped up") - np.testing.assert_allclose(fuelMassDot[-1],0, rtol=1e-12, err_msg="Thruster mass depletion not ramped down") + np.testing.assert_allclose( + fuelMassDot[100], + -0.000403404216123, + rtol=1e-3, + err_msg="Thruster mass depletion not ramped up", + ) + np.testing.assert_allclose( + fuelMassDot[-1], + 0, + rtol=1e-12, + err_msg="Thruster mass depletion not ramped down", + ) + if __name__ == "__main__": test_massDepletionTest(True, thrusterDynamicEffector.ThrusterDynamicEffector) diff --git a/src/simulation/dynamics/FuelTank/_UnitTest/test_tank_models.py b/src/simulation/dynamics/FuelTank/_UnitTest/test_tank_models.py index 6e5cf37bcf..7706bd621f 100644 --- a/src/simulation/dynamics/FuelTank/_UnitTest/test_tank_models.py +++ b/src/simulation/dynamics/FuelTank/_UnitTest/test_tank_models.py @@ -41,28 +41,16 @@ def test_tankModelConstantVolume(show_plots=False): true_ITankPntT_T = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], [100, 0, 0, 0, 100, 0, 0, 0, 100], - [50, 0, 0, 0, 50, 0, 0, 0, 50] + [50, 0, 0, 0, 50, 0, 0, 0, 50], ] true_IPrimeTankPntT_T = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], [-10, 0, 0, 0, -10, 0, 0, 0, -10], - [-10, 0, 0, 0, -10, 0, 0, 0, -10] - ] - true_r_TcT_T = [ - [1, 1, 1], - [1, 1, 1], - [1, 1, 1] - ] - true_rPrime_TcT_T = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, 0] - ] - true_rPPrime_TcT_T = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, 0] + [-10, 0, 0, 0, -10, 0, 0, 0, -10], ] + true_r_TcT_T = [[1, 1, 1], [1, 1, 1], [1, 1, 1]] + true_rPrime_TcT_T = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] + true_rPPrime_TcT_T = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] accuracy = 1e-8 for idx, trial in enumerate(trials): @@ -70,38 +58,48 @@ def test_tankModelConstantVolume(show_plots=False): model.computeTankPropDerivs(*trial) dataITank = model.ITankPntT_T dataITank = [dataITank[i][j] for i in range(3) for j in range(3)] - np.testing.assert_allclose(dataITank, - true_ITankPntT_T[idx], - rtol=accuracy, - err_msg="Constant volume tank inertia not equal") + np.testing.assert_allclose( + dataITank, + true_ITankPntT_T[idx], + rtol=accuracy, + err_msg="Constant volume tank inertia not equal", + ) dataIPrimeTank = model.IPrimeTankPntT_T dataIPrimeTank = [dataIPrimeTank[i][j] for i in range(3) for j in range(3)] - np.testing.assert_allclose(dataIPrimeTank, - true_IPrimeTankPntT_T[idx], - rtol=accuracy, - err_msg="Constant volume tank inertia derivatives not equal") + np.testing.assert_allclose( + dataIPrimeTank, + true_IPrimeTankPntT_T[idx], + rtol=accuracy, + err_msg="Constant volume tank inertia derivatives not equal", + ) dataR = model.r_TcT_T dataR = [dataR[i][0] for i in range(3)] - np.testing.assert_allclose(dataR, - true_r_TcT_T[idx], - rtol=accuracy, - err_msg="Constant volume tank tank center mass position not equal") + np.testing.assert_allclose( + dataR, + true_r_TcT_T[idx], + rtol=accuracy, + err_msg="Constant volume tank tank center mass position not equal", + ) dataRPrime = model.rPrime_TcT_T dataRPrime = [dataRPrime[i][0] for i in range(3)] - np.testing.assert_allclose(dataRPrime, - true_rPrime_TcT_T[idx], - rtol=accuracy, - err_msg="Constant volume tank tank center mass position derivative not equal") + np.testing.assert_allclose( + dataRPrime, + true_rPrime_TcT_T[idx], + rtol=accuracy, + err_msg="Constant volume tank tank center mass position derivative not equal", + ) dataRPPrime = model.rPPrime_TcT_T dataRPPrime = [dataRPPrime[i][0] for i in range(3)] - np.testing.assert_allclose(dataRPPrime, - true_rPPrime_TcT_T[idx], - rtol=accuracy, - err_msg="Constant volume tank tank center mass position second derivative not equal") + np.testing.assert_allclose( + dataRPPrime, + true_rPPrime_TcT_T[idx], + rtol=accuracy, + err_msg="Constant volume tank tank center mass position second derivative not equal", + ) def test_tankModelConstantDensity(show_plots=False): @@ -116,28 +114,36 @@ def test_tankModelConstantDensity(show_plots=False): true_ITankPntT_T = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], [100, 0, 0, 0, 100, 0, 0, 0, 100], - [31.498026247371826, 0, 0, 0, 31.498026247371826, 0, 0, 0, 31.498026247371826] + [31.498026247371826, 0, 0, 0, 31.498026247371826, 0, 0, 0, 31.498026247371826], ] true_IPrimeTankPntT_T = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], - [-16.666666666666668, 0, 0, 0, -16.666666666666668, 0, 0, 0, -16.666666666666668], - [-10.499342082457275, 0, 0, 0, -10.499342082457275, 0, 0, 0, -10.499342082457275] - ] - true_r_TcT_T = [ - [1, 1, 1], - [1, 1, 1], - [1, 1, 1] - ] - true_rPrime_TcT_T = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, 0] - ] - true_rPPrime_TcT_T = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, 0] + [ + -16.666666666666668, + 0, + 0, + 0, + -16.666666666666668, + 0, + 0, + 0, + -16.666666666666668, + ], + [ + -10.499342082457275, + 0, + 0, + 0, + -10.499342082457275, + 0, + 0, + 0, + -10.499342082457275, + ], ] + true_r_TcT_T = [[1, 1, 1], [1, 1, 1], [1, 1, 1]] + true_rPrime_TcT_T = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] + true_rPPrime_TcT_T = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] accuracy = 1e-8 for idx, trial in enumerate(trials): @@ -145,38 +151,48 @@ def test_tankModelConstantDensity(show_plots=False): model.computeTankPropDerivs(*trial) dataITank = model.ITankPntT_T dataITank = [dataITank[i][j] for i in range(3) for j in range(3)] - np.testing.assert_allclose(dataITank, - true_ITankPntT_T[idx], - rtol=accuracy, - err_msg="Constant density tank inertia not equal") + np.testing.assert_allclose( + dataITank, + true_ITankPntT_T[idx], + rtol=accuracy, + err_msg="Constant density tank inertia not equal", + ) dataIPrimeTank = model.IPrimeTankPntT_T dataIPrimeTank = [dataIPrimeTank[i][j] for i in range(3) for j in range(3)] - np.testing.assert_allclose(dataIPrimeTank, - true_IPrimeTankPntT_T[idx], - rtol=accuracy, - err_msg="Constant density tank inertia derivatives not equal") + np.testing.assert_allclose( + dataIPrimeTank, + true_IPrimeTankPntT_T[idx], + rtol=accuracy, + err_msg="Constant density tank inertia derivatives not equal", + ) dataR = model.r_TcT_T dataR = [dataR[i][0] for i in range(3)] - np.testing.assert_allclose(dataR, - true_r_TcT_T[idx], - rtol=accuracy, - err_msg="Constant density tank tank center mass position not equal") + np.testing.assert_allclose( + dataR, + true_r_TcT_T[idx], + rtol=accuracy, + err_msg="Constant density tank tank center mass position not equal", + ) dataRPrime = model.rPrime_TcT_T dataRPrime = [dataRPrime[i][0] for i in range(3)] - np.testing.assert_allclose(dataRPrime, - true_rPrime_TcT_T[idx], - rtol=accuracy, - err_msg="Constant density tank tank center mass position derivative not equal") + np.testing.assert_allclose( + dataRPrime, + true_rPrime_TcT_T[idx], + rtol=accuracy, + err_msg="Constant density tank tank center mass position derivative not equal", + ) dataRPPrime = model.rPPrime_TcT_T dataRPPrime = [dataRPPrime[i][0] for i in range(3)] - np.testing.assert_allclose(dataRPPrime, - true_rPPrime_TcT_T[idx], - rtol=accuracy, - err_msg="Constant density tank tank center mass position second derivative not equal") + np.testing.assert_allclose( + dataRPPrime, + true_rPPrime_TcT_T[idx], + rtol=accuracy, + err_msg="Constant density tank tank center mass position second derivative not equal", + ) def test_tankModelEmptying(show_plots=False): @@ -191,28 +207,16 @@ def test_tankModelEmptying(show_plots=False): true_ITankPntT_T = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], [100, 0, 0, 0, 100, 0, 0, 0, 100], - [50.0, 0, 0, 0, 50.0, 0, 0, 0, 50] + [50.0, 0, 0, 0, 50.0, 0, 0, 0, 50], ] true_IPrimeTankPntT_T = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0], - [-8.75, 0, 0, 0, -8.75, 0, 0, 0, -12.5] - ] - true_r_TcT_T = [ - [1, 1, 1 - 5.0], - [1, 1, 1], - [1, 1, 1.0 - 15.0 / 8.0] - ] - true_rPrime_TcT_T = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, -3.0 / 8.0] - ] - true_rPPrime_TcT_T = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, -17.0 / 30.0] + [-8.75, 0, 0, 0, -8.75, 0, 0, 0, -12.5], ] + true_r_TcT_T = [[1, 1, 1 - 5.0], [1, 1, 1], [1, 1, 1.0 - 15.0 / 8.0]] + true_rPrime_TcT_T = [[0, 0, 0], [0, 0, 0], [0, 0, -3.0 / 8.0]] + true_rPPrime_TcT_T = [[0, 0, 0], [0, 0, 0], [0, 0, -17.0 / 30.0]] accuracy = 1e-8 for idx, trial in enumerate(trials): @@ -220,38 +224,48 @@ def test_tankModelEmptying(show_plots=False): model.computeTankPropDerivs(*trial) dataITank = model.ITankPntT_T dataITank = [dataITank[i][j] for i in range(3) for j in range(3)] - np.testing.assert_allclose(dataITank, - true_ITankPntT_T[idx], - atol=accuracy, - err_msg="Emptying tank inertia not equal") + np.testing.assert_allclose( + dataITank, + true_ITankPntT_T[idx], + atol=accuracy, + err_msg="Emptying tank inertia not equal", + ) dataIPrimeTank = model.IPrimeTankPntT_T dataIPrimeTank = [dataIPrimeTank[i][j] for i in range(3) for j in range(3)] - np.testing.assert_allclose(dataIPrimeTank, - true_IPrimeTankPntT_T[idx], - rtol=accuracy, - err_msg="Emptying tank inertia derivative not equal") + np.testing.assert_allclose( + dataIPrimeTank, + true_IPrimeTankPntT_T[idx], + rtol=accuracy, + err_msg="Emptying tank inertia derivative not equal", + ) dataR = model.r_TcT_T dataR = [dataR[i][0] for i in range(3)] - np.testing.assert_allclose(dataR, - true_r_TcT_T[idx], - rtol=accuracy, - err_msg="Emptying tank center of mass position not equal") + np.testing.assert_allclose( + dataR, + true_r_TcT_T[idx], + rtol=accuracy, + err_msg="Emptying tank center of mass position not equal", + ) dataRPrime = model.rPrime_TcT_T dataRPrime = [dataRPrime[i][0] for i in range(3)] - np.testing.assert_allclose(dataRPrime, - true_rPrime_TcT_T[idx], - rtol=accuracy, - err_msg="Emptying tank center of mass position derivative not equal") + np.testing.assert_allclose( + dataRPrime, + true_rPrime_TcT_T[idx], + rtol=accuracy, + err_msg="Emptying tank center of mass position derivative not equal", + ) dataRPPrime = model.rPPrime_TcT_T dataRPPrime = [dataRPPrime[i][0] for i in range(3)] - np.testing.assert_allclose(dataRPPrime, - true_rPPrime_TcT_T[idx], - rtol=accuracy, - err_msg="Emptying tank center of mass position second derivative not equal") + np.testing.assert_allclose( + dataRPPrime, + true_rPPrime_TcT_T[idx], + rtol=accuracy, + err_msg="Emptying tank center of mass position second derivative not equal", + ) def test_tankModelUniformBurn(show_plots=False): @@ -261,34 +275,21 @@ def test_tankModelUniformBurn(show_plots=False): model.propMassInit = 10 model.r_TcT_TInit = [[1], [1], [1]] model.radiusTankInit = 5 - model.lengthTank = 5; - + model.lengthTank = 5 trials = [(0, 0), (10, -1), (5, -1)] # mFuel, mDotFuel true_ITankPntT_T = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], [83.33333333333334, 0, 0, 0, 83.33333333333334, 0, 0, 0, 125], - [41.66666666666667, 0, 0, 0, 41.66666666666667, 0, 0, 0, 62.5] + [41.66666666666667, 0, 0, 0, 41.66666666666667, 0, 0, 0, 62.5], ] true_IPrimeTankPntT_T = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], [-8.3333333333334, 0, 0, 0, -8.3333333333334, 0, 0, 0, -12.5], - [-8.3333333333334, 0, 0, 0, -8.3333333333334, 0, 0, 0, -12.5] - ] - true_r_TcT_T = [ - [1, 1, 1], - [1, 1, 1], - [1, 1, 1] - ] - true_rPrime_TcT_T = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, 0] - ] - true_rPPrime_TcT_T = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, 0] + [-8.3333333333334, 0, 0, 0, -8.3333333333334, 0, 0, 0, -12.5], ] + true_r_TcT_T = [[1, 1, 1], [1, 1, 1], [1, 1, 1]] + true_rPrime_TcT_T = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] + true_rPPrime_TcT_T = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] accuracy = 1e-8 for idx, trial in enumerate(trials): @@ -296,38 +297,48 @@ def test_tankModelUniformBurn(show_plots=False): model.computeTankPropDerivs(*trial) dataITank = model.ITankPntT_T dataITank = [dataITank[i][j] for i in range(3) for j in range(3)] - np.testing.assert_allclose(dataITank, - true_ITankPntT_T[idx], - rtol=accuracy, - err_msg="Tank uniform burn inertia not equal") + np.testing.assert_allclose( + dataITank, + true_ITankPntT_T[idx], + rtol=accuracy, + err_msg="Tank uniform burn inertia not equal", + ) dataIPrimeTank = model.IPrimeTankPntT_T dataIPrimeTank = [dataIPrimeTank[i][j] for i in range(3) for j in range(3)] - np.testing.assert_allclose(dataIPrimeTank, - true_IPrimeTankPntT_T[idx], - rtol=accuracy, - err_msg="Tank uniform burn inertia derivative not equal") + np.testing.assert_allclose( + dataIPrimeTank, + true_IPrimeTankPntT_T[idx], + rtol=accuracy, + err_msg="Tank uniform burn inertia derivative not equal", + ) dataR = model.r_TcT_T dataR = [dataR[i][0] for i in range(3)] - np.testing.assert_allclose(dataR, - true_r_TcT_T[idx], - rtol=accuracy, - err_msg="Tank uniform burn center of mass position not equal") + np.testing.assert_allclose( + dataR, + true_r_TcT_T[idx], + rtol=accuracy, + err_msg="Tank uniform burn center of mass position not equal", + ) dataRPrime = model.rPrime_TcT_T dataRPrime = [dataRPrime[i][0] for i in range(3)] - np.testing.assert_allclose(dataRPrime, - true_rPrime_TcT_T[idx], - rtol=accuracy, - err_msg="Tank uniform burn center of mass position derivative not equal") + np.testing.assert_allclose( + dataRPrime, + true_rPrime_TcT_T[idx], + rtol=accuracy, + err_msg="Tank uniform burn center of mass position derivative not equal", + ) dataRPPrime = model.rPPrime_TcT_T dataRPPrime = [dataRPPrime[i][0] for i in range(3)] - np.testing.assert_allclose(dataRPPrime, - true_rPPrime_TcT_T[idx], - rtol=accuracy, - err_msg="Tank uniform burn center of mass position second derivative not equal") + np.testing.assert_allclose( + dataRPPrime, + true_rPPrime_TcT_T[idx], + rtol=accuracy, + err_msg="Tank uniform burn center of mass position second derivative not equal", + ) def test_tankModelCentrifugalBurn(show_plots=False): @@ -343,28 +354,26 @@ def test_tankModelCentrifugalBurn(show_plots=False): true_ITankPntT_T = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], [83.33333333333334, 0, 0, 0, 83.33333333333334, 0, 0, 0, 125], - [57.291666666666671, 0, 0, 0, 57.291666666666671, 0, 0, 0, 93.75] + [57.291666666666671, 0, 0, 0, 57.291666666666671, 0, 0, 0, 93.75], ] true_IPrimeTankPntT_T = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], [-2.0833333333333335, 0, 0, 0, -2.0833333333333335, 0, 0, 0, 0.0], - [-8.3333333333333339, 0, 0, 0, -8.3333333333333339, 0, 0, 0, -12.500000000000002] - ] - true_r_TcT_T = [ - [1, 1, 1], - [1, 1, 1], - [1, 1, 1] - ] - true_rPrime_TcT_T = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, 0] - ] - true_rPPrime_TcT_T = [ - [0, 0, 0], - [0, 0, 0], - [0, 0, 0] + [ + -8.3333333333333339, + 0, + 0, + 0, + -8.3333333333333339, + 0, + 0, + 0, + -12.500000000000002, + ], ] + true_r_TcT_T = [[1, 1, 1], [1, 1, 1], [1, 1, 1]] + true_rPrime_TcT_T = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] + true_rPPrime_TcT_T = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] accuracy = 1e-8 for idx, trial in enumerate(trials): @@ -372,39 +381,49 @@ def test_tankModelCentrifugalBurn(show_plots=False): model.computeTankPropDerivs(*trial) dataITank = model.ITankPntT_T dataITank = [dataITank[i][j] for i in range(3) for j in range(3)] - np.testing.assert_allclose(dataITank, - true_ITankPntT_T[idx], - rtol=accuracy, - err_msg="Tank centrifugal burn inertia not equal") + np.testing.assert_allclose( + dataITank, + true_ITankPntT_T[idx], + rtol=accuracy, + err_msg="Tank centrifugal burn inertia not equal", + ) dataIPrimeTank = model.IPrimeTankPntT_T dataIPrimeTank = [dataIPrimeTank[i][j] for i in range(3) for j in range(3)] - np.testing.assert_allclose(dataIPrimeTank, - true_IPrimeTankPntT_T[idx], - rtol=accuracy, - err_msg="Tank centrifugal burn inertia derivative not equal") + np.testing.assert_allclose( + dataIPrimeTank, + true_IPrimeTankPntT_T[idx], + rtol=accuracy, + err_msg="Tank centrifugal burn inertia derivative not equal", + ) dataR = model.r_TcT_T dataR = [dataR[i][0] for i in range(3)] - np.testing.assert_allclose(dataR, - true_r_TcT_T[idx], - rtol=accuracy, - err_msg="Tank centrifugal burn center of mass position not equal") + np.testing.assert_allclose( + dataR, + true_r_TcT_T[idx], + rtol=accuracy, + err_msg="Tank centrifugal burn center of mass position not equal", + ) dataRPrime = model.rPrime_TcT_T dataRPrime = [dataRPrime[i][0] for i in range(3)] - np.testing.assert_allclose(dataRPrime, - true_rPrime_TcT_T[idx], - rtol=accuracy, - err_msg="Tank centrifugal burn center of mass position derivative not equal") + np.testing.assert_allclose( + dataRPrime, + true_rPrime_TcT_T[idx], + rtol=accuracy, + err_msg="Tank centrifugal burn center of mass position derivative not equal", + ) dataRPPrime = model.rPPrime_TcT_T dataRPPrime = [dataRPPrime[i][0] for i in range(3)] - np.testing.assert_allclose(dataRPPrime, - true_rPPrime_TcT_T[idx], - rtol=accuracy, - err_msg="Tank centrifugal burn center of mass position second derivative not equal") + np.testing.assert_allclose( + dataRPPrime, + true_rPPrime_TcT_T[idx], + rtol=accuracy, + err_msg="Tank centrifugal burn center of mass position second derivative not equal", + ) if __name__ == "__main__": diff --git a/src/simulation/dynamics/GravityGradientEffector/_UnitTest/test_gravityGradient.py b/src/simulation/dynamics/GravityGradientEffector/_UnitTest/test_gravityGradient.py index 3adb1ad2de..f7f29fa6e9 100644 --- a/src/simulation/dynamics/GravityGradientEffector/_UnitTest/test_gravityGradient.py +++ b/src/simulation/dynamics/GravityGradientEffector/_UnitTest/test_gravityGradient.py @@ -1,4 +1,3 @@ - # Copyright (c) 2020, Autonomous Vehicle Systems Lab, University of Colorado at Boulder # # Permission to use, copy, modify, and/or distribute this software for any @@ -30,8 +29,10 @@ import pytest from Basilisk import __path__ from Basilisk.simulation import GravityGradientEffector + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros @@ -49,12 +50,12 @@ # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(True, reason="Previously set sim parameters are not consistent with new formulation\n") + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. @pytest.mark.parametrize("planetCase", [0, 1, 2, 3]) @pytest.mark.parametrize("cmOffset", [[[0.1], [0.15], [-0.1]], [[0.0], [0.0], [0.0]]]) - # provide a unique test method name, starting with test_ def test_gravityGradientModule(show_plots, cmOffset, planetCase): r""" @@ -87,8 +88,7 @@ def test_gravityGradientModule(show_plots, cmOffset, planetCase): """ # each test method requires a single assert method to be called - [testResults, testMessage] = run( - show_plots, cmOffset, planetCase, 2.0) + [testResults, testMessage] = run(show_plots, cmOffset, planetCase, 2.0) assert testResults < 1, testMessage @@ -98,15 +98,15 @@ def truthGravityGradient(mu, rN, sigmaBN, hub): BN = RigidBodyKinematics.MRP2C(sigmaBN) rHatB = np.matmul(BN, rN) / r - ggTorque = 3*mu/r/r/r * np.cross(rHatB, np.matmul(I, rHatB)) + ggTorque = 3 * mu / r / r / r * np.cross(rHatB, np.matmul(I, rHatB)) return ggTorque + def run(show_plots, cmOffset, planetCase, simTime): """Call this routine directly to run the unit test.""" - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages # Create simulation variable names simTaskName = "simTask" @@ -134,9 +134,9 @@ def run(show_plots, cmOffset, planetCase, simTime): # for gravity acceleration calculations venus = gravFactory.createVenus() timeInitString = "2012 MAY 1 00:28:30.0" - gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/', - timeInitString, - epochInMsg=True) + gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", timeInitString, epochInMsg=True + ) scSim.AddModelToTask(simTaskName, gravFactory.spiceObject, -1) @@ -145,13 +145,17 @@ def run(show_plots, cmOffset, planetCase, simTime): earth.isCentralBody = False venus.isCentralBody = True mu = venus.mu - gravFactory.spiceObject.zeroBase = 'venus' # spacecraft states are logged relative to Earth for plotting + gravFactory.spiceObject.zeroBase = ( + "venus" # spacecraft states are logged relative to Earth for plotting + ) else: - gravFactory.spiceObject.zeroBase = 'earth' # spacecraft states are logged relative to Earth for plotting + gravFactory.spiceObject.zeroBase = ( + "earth" # spacecraft states are logged relative to Earth for plotting + ) # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rLEO = 7000. * 1000 # meters + rLEO = 7000.0 * 1000 # meters oe.a = rLEO oe.e = 0.0001 oe.i = 33.3 * macros.D2R @@ -159,15 +163,15 @@ def run(show_plots, cmOffset, planetCase, simTime): oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements - # with circular or equatorial orbit, some angles are arbitrary + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements + # with circular or equatorial orbit, some angles are arbitrary # setup basic spacecraft module scObject = spacecraft.Spacecraft() scObject.ModelTag = "bskTestSat" - IIC = [[500., 0., 0.] - , [0., 800., 0.] - , [0., 0., 350.]] + IIC = [[500.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 350.0]] scObject.hub.r_BcB_B = cmOffset scObject.hub.mHub = 100.0 # kg - spacecraft mass scObject.hub.IHubPntBc_B = IIC @@ -178,7 +182,9 @@ def run(show_plots, cmOffset, planetCase, simTime): scSim.AddModelToTask(simTaskName, scObject) - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # add gravity gradient effector ggEff = GravityGradientEffector.GravityGradientEffector() @@ -193,7 +199,9 @@ def run(show_plots, cmOffset, planetCase, simTime): # Setup data logging before the simulation is initialized # numDataPoints = 50 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) dataLogGG = ggEff.gravityGradientOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) @@ -228,30 +236,39 @@ def run(show_plots, cmOffset, planetCase, simTime): plt.close("all") # clears out plots from earlier test runs plt.figure(1) for idx in range(0, 3): - plt.plot(dataLog.times() * macros.NANO2MIN, attData[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'MRP Attitude $\sigma_{B/N}$') + plt.plot( + dataLog.times() * macros.NANO2MIN, + attData[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"MRP Attitude $\sigma_{B/N}$") plt.figure(2) for idx in range(0, 3): - plt.plot(dataLog.times() * macros.NANO2MIN, posData[:, idx]/1000, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$r_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'Inertial Position coordinates [km]') + plt.plot( + dataLog.times() * macros.NANO2MIN, + posData[:, idx] / 1000, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$r_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"Inertial Position coordinates [km]") plt.figure(3) for idx in range(0, 3): - plt.plot(dataLogGG.times() * macros.NANO2MIN, ggData[:, idx] , - color=unitTestSupport.getLineColor(idx, 3), - label=r'$r_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [min]') - plt.ylabel(r'GG Torque [Nm]') + plt.plot( + dataLogGG.times() * macros.NANO2MIN, + ggData[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$r_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [min]") + plt.ylabel(r"GG Torque [Nm]") plt.show() plt.close("all") @@ -260,11 +277,14 @@ def run(show_plots, cmOffset, planetCase, simTime): accuracy = 1e-10 for rV, sV, ggV in zip(posData, attData, ggData): ggTruth = truthGravityGradient(mu, rV[0:3], sV[0:3], scObject.hub) - testFailCount, testMessages = unitTestSupport.compareVector(ggV[0:3], - ggTruth, - accuracy, - "gravityGradientTorque_B", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareVector( + ggV[0:3], + ggTruth, + accuracy, + "gravityGradientTorque_B", + testFailCount, + testMessages, + ) print("Accuracy used: " + str(accuracy)) if testFailCount == 0: @@ -275,8 +295,12 @@ def run(show_plots, cmOffset, planetCase, simTime): return testFailCount, testMessages # close the plots being saved off to avoid over-writing old and new figures -if __name__ == '__main__': - run(True, # show_plots - [[0.0], [0.0], [0.0]], # cmOffset - 3, # planetCase - 3600) # simTime (seconds) + + +if __name__ == "__main__": + run( + True, # show_plots + [[0.0], [0.0], [0.0]], # cmOffset + 3, # planetCase + 3600, + ) # simTime (seconds) diff --git a/src/simulation/dynamics/HingedRigidBodies/_UnitTest/test_hingedRigidBodyStateEffector.py b/src/simulation/dynamics/HingedRigidBodies/_UnitTest/test_hingedRigidBodyStateEffector.py index 13d14cb34c..4d7cb0bcd3 100644 --- a/src/simulation/dynamics/HingedRigidBodies/_UnitTest/test_hingedRigidBodyStateEffector.py +++ b/src/simulation/dynamics/HingedRigidBodies/_UnitTest/test_hingedRigidBodyStateEffector.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -24,12 +23,13 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') - +splitPath = path.split("simulation") from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import matplotlib.pyplot as plt from Basilisk.simulation import spacecraft from Basilisk.simulation import hingedRigidBodyStateEffector @@ -47,6 +47,7 @@ # @pytest.mark.xfail() # need to update how the RW states are defined # provide a unique test method name, starting with test_ + @pytest.mark.parametrize("useScPlus", [True]) def test_hingedRigidBodyMotorTorque(show_plots, useScPlus): """Module Unit Test""" @@ -127,7 +128,11 @@ def hingedRigidBodyMotorTorque(show_plots, useScPlus): # Define mass properties of the rigid part of the spacecraft scObjectPrimary.hub.mHub = 750.0 scObjectPrimary.hub.r_BcB_B = [[0.0], [0.0], [1.0]] - scObjectPrimary.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] + scObjectPrimary.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 scObjectPrimary.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] @@ -158,9 +163,11 @@ def hingedRigidBodyMotorTorque(show_plots, useScPlus): if useScPlus: scLog = scObject.logger("totRotAngMomPntC_N") else: - scLog = pythonVariableLogger.PythonVariableLogger({ - "totRotAngMomPntC_N": lambda _: scObject.primaryCentralSpacecraft.totRotAngMomPntC_N - }) + scLog = pythonVariableLogger.PythonVariableLogger( + { + "totRotAngMomPntC_N": lambda _: scObject.primaryCentralSpacecraft.totRotAngMomPntC_N + } + ) unitTestSim.AddModelToTask(unitTaskName, scLog) unitTestSim.InitializeSimulation() @@ -189,7 +196,7 @@ def hingedRigidBodyMotorTorque(show_plots, useScPlus): # Get the last sigma and position dataPos = [rOut_CN_N[-1]] - truePos = [[0., 0., 0.]] + truePos = [[0.0, 0.0, 0.0]] initialRotAngMom_N = [[rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]]] @@ -199,46 +206,71 @@ def hingedRigidBodyMotorTorque(show_plots, useScPlus): plt.figure() plt.clf() - plt.plot(rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) , - rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) , - rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) ) - plt.xlabel('time (s)') - plt.ylabel('Ang. Momentum Difference') + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]), + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]), + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]), + ) + plt.xlabel("time (s)") + plt.ylabel("Ang. Momentum Difference") plt.figure() plt.clf() - plt.plot(dataLog.times() * 1e-9, vOut_CN_N[:, 0], - dataLog.times() * 1e-9, vOut_CN_N[:, 1], - dataLog.times() * 1e-9, vOut_CN_N[:, 2]) - plt.xlabel('time (s)') - plt.ylabel('m/s') + plt.plot( + dataLog.times() * 1e-9, + vOut_CN_N[:, 0], + dataLog.times() * 1e-9, + vOut_CN_N[:, 1], + dataLog.times() * 1e-9, + vOut_CN_N[:, 2], + ) + plt.xlabel("time (s)") + plt.ylabel("m/s") plt.figure() plt.clf() - plt.plot(dataLog.times() * macros.NANO2SEC, sigma_BN[:, 0], - color=unitTestSupport.getLineColor(0, 3), - label=r'$\sigma_{1}$') - plt.plot(dataLog.times() * macros.NANO2SEC, sigma_BN[:, 1], - color=unitTestSupport.getLineColor(1, 3), - label=r'$\sigma_{2}$') - plt.plot(dataLog.times() * macros.NANO2SEC, sigma_BN[:, 2], - color=unitTestSupport.getLineColor(2, 3), - label=r'$\sigma_{3}$') - plt.legend(loc='lower right') - plt.xlabel('time (s)') - plt.ylabel(r'MRP $\sigma_{B/N}$') + plt.plot( + dataLog.times() * macros.NANO2SEC, + sigma_BN[:, 0], + color=unitTestSupport.getLineColor(0, 3), + label=r"$\sigma_{1}$", + ) + plt.plot( + dataLog.times() * macros.NANO2SEC, + sigma_BN[:, 1], + color=unitTestSupport.getLineColor(1, 3), + label=r"$\sigma_{2}$", + ) + plt.plot( + dataLog.times() * macros.NANO2SEC, + sigma_BN[:, 2], + color=unitTestSupport.getLineColor(2, 3), + label=r"$\sigma_{3}$", + ) + plt.legend(loc="lower right") + plt.xlabel("time (s)") + plt.ylabel(r"MRP $\sigma_{B/N}$") plt.figure() plt.clf() - plt.plot(dataPanel1.times() * macros.NANO2SEC, theta1*macros.R2D, - color=unitTestSupport.getLineColor(0, 3), - label=r'$\theta_{1}$') - plt.plot(dataPanel2.times() * macros.NANO2SEC, theta2*macros.R2D, - color=unitTestSupport.getLineColor(1, 3), - label=r'$\theta_{2}$') - plt.legend(loc='lower right') - plt.xlabel('time (s)') - plt.ylabel('Hinge Angles [deg]') + plt.plot( + dataPanel1.times() * macros.NANO2SEC, + theta1 * macros.R2D, + color=unitTestSupport.getLineColor(0, 3), + label=r"$\theta_{1}$", + ) + plt.plot( + dataPanel2.times() * macros.NANO2SEC, + theta2 * macros.R2D, + color=unitTestSupport.getLineColor(1, 3), + label=r"$\theta_{2}$", + ) + plt.legend(loc="lower right") + plt.xlabel("time (s)") + plt.ylabel("Hinge Angles [deg]") if show_plots: plt.show() @@ -249,42 +281,62 @@ def hingedRigidBodyMotorTorque(show_plots, useScPlus): # check a vector values if not unitTestSupport.isArrayEqual(dataPos[i], truePos[i], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test failed position test") + testMessages.append( + "FAILED: Hinged Rigid Body integrated test failed position test" + ) finalRotAngMom = numpy.delete(finalRotAngMom, 0, axis=1) # remove time column for i in range(0, len(initialRotAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqual(finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: Hinged Rigid Body integrated test failed rotational angular momentum unit test") + "FAILED: Hinged Rigid Body integrated test failed rotational angular momentum unit test" + ) # check config log messages if not unitTestSupport.isArrayEqual(rB1N, [2.0, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test failed panel 1 r_BN_N config log test") + testMessages.append( + "FAILED: Hinged Rigid Body integrated test failed panel 1 r_BN_N config log test" + ) if not unitTestSupport.isArrayEqual(vB1N, [0.0, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test failed panel 1 v_BN_N config log test") + testMessages.append( + "FAILED: Hinged Rigid Body integrated test failed panel 1 v_BN_N config log test" + ) if not unitTestSupport.isArrayEqual(sB1N, [0.0, 0, 1.0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test failed panel 1 sigma_BN config log test") + testMessages.append( + "FAILED: Hinged Rigid Body integrated test failed panel 1 sigma_BN config log test" + ) if not unitTestSupport.isArrayEqual(oB1N, [0.0, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test failed panel 1 omega_BN_B config log test") + testMessages.append( + "FAILED: Hinged Rigid Body integrated test failed panel 1 omega_BN_B config log test" + ) if not unitTestSupport.isArrayEqual(rB2N, [-2.0, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test failed panel 2 r_BN_N config log test") + testMessages.append( + "FAILED: Hinged Rigid Body integrated test failed panel 2 r_BN_N config log test" + ) if not unitTestSupport.isArrayEqual(vB2N, [0.0, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test failed panel 2 v_BN_N config log test") + testMessages.append( + "FAILED: Hinged Rigid Body integrated test failed panel 2 v_BN_N config log test" + ) if not unitTestSupport.isArrayEqual(sB2N, [0.0, 0, 0.0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test failed panel 2 sigma_BN config log test") + testMessages.append( + "FAILED: Hinged Rigid Body integrated test failed panel 2 sigma_BN config log test" + ) if not unitTestSupport.isArrayEqual(oB2N, [0.0, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test failed panel 2 omega_BN_B config log test") - + testMessages.append( + "FAILED: Hinged Rigid Body integrated test failed panel 2 omega_BN_B config log test" + ) if testFailCount == 0: print("PASSED: " + " Hinged Rigid Body integrated test with motor torques") @@ -292,7 +344,7 @@ def hingedRigidBodyMotorTorque(show_plots, useScPlus): assert testFailCount < 1, testMessages # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def hingedRigidBodyLagrangVsBasilisk(show_plots): @@ -352,10 +404,10 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): # Define force and torque momentArm1_B = numpy.array([0.05, 0.0, 0.0]) force1_B = numpy.array([0.2, 0.7, 0.0]) - torque1_B = numpy.cross(momentArm1_B,force1_B) + torque1_B = numpy.cross(momentArm1_B, force1_B) momentArm2_B = numpy.array([-0.03, 0.0, 0.0]) force2_B = numpy.array([0.0, 1.0, 0.0]) - torque2_B = numpy.cross(momentArm2_B,force2_B) + torque2_B = numpy.cross(momentArm2_B, force2_B) # Add external force and torque extFTObject = extForceTorque.ExtForceTorque() @@ -368,7 +420,11 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): # Define mass properties of the rigid part of the spacecraft scObject.primaryCentralSpacecraft.hub.mHub = 750.0 scObject.primaryCentralSpacecraft.hub.r_BcB_B = [[0.0], [0.0], [0.0]] - scObject.primaryCentralSpacecraft.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] + scObject.primaryCentralSpacecraft.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.primaryCentralSpacecraft.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] @@ -384,10 +440,16 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): dataLog = scObject.primaryCentralSpacecraft.scStateOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) - stateLog = pythonVariableLogger.PythonVariableLogger({ - "theta1": lambda _: scObject.dynManager.getStateObject('spacecrafthingedRigidBodyTheta1').getState(), - "theta2": lambda _: scObject.dynManager.getStateObject('spacecrafthingedRigidBodyTheta2').getState(), - }) + stateLog = pythonVariableLogger.PythonVariableLogger( + { + "theta1": lambda _: scObject.dynManager.getStateObject( + "spacecrafthingedRigidBodyTheta1" + ).getState(), + "theta2": lambda _: scObject.dynManager.getStateObject( + "spacecrafthingedRigidBodyTheta2" + ).getState(), + } + ) unitTestSim.AddModelToTask(unitTaskName, stateLog) unitTestSim.InitializeSimulation() @@ -426,7 +488,7 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): rOut_BN_N = dataLog.r_BN_N sigmaOut_BN = dataLog.sigma_BN - thetaOut = 4.0*numpy.arctan(sigmaOut_BN[:,2]) + thetaOut = 4.0 * numpy.arctan(sigmaOut_BN[:, 2]) # Developing the lagrangian result # Define initial values @@ -436,8 +498,12 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): # Define variables for panel1 spacecraft.panel1.mass = unitTestSim.panel1.mass spacecraft.panel1.Inertia = unitTestSim.panel1.IPntS_S[1][1] - spacecraft.panel1.Rhinge = numpy.linalg.norm(numpy.asarray(unitTestSim.panel1.r_HB_B)) - spacecraft.panel1.beta = numpy.arctan2(unitTestSim.panel1.r_HB_B[1][0],unitTestSim.panel1.r_HB_B[0][0]) + spacecraft.panel1.Rhinge = numpy.linalg.norm( + numpy.asarray(unitTestSim.panel1.r_HB_B) + ) + spacecraft.panel1.beta = numpy.arctan2( + unitTestSim.panel1.r_HB_B[1][0], unitTestSim.panel1.r_HB_B[0][0] + ) spacecraft.panel1.thetaH = 0.0 spacecraft.panel1.d = unitTestSim.panel1.d spacecraft.panel1.k = unitTestSim.panel1.k @@ -445,27 +511,31 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): # Define variables for panel2 spacecraft.panel2.mass = unitTestSim.panel2.mass spacecraft.panel2.Inertia = unitTestSim.panel2.IPntS_S[1][1] - spacecraft.panel2.Rhinge = numpy.linalg.norm(numpy.asarray(unitTestSim.panel2.r_HB_B)) - spacecraft.panel2.beta = numpy.arctan2(unitTestSim.panel2.r_HB_B[1][0],unitTestSim.panel2.r_HB_B[0][0]) + spacecraft.panel2.Rhinge = numpy.linalg.norm( + numpy.asarray(unitTestSim.panel2.r_HB_B) + ) + spacecraft.panel2.beta = numpy.arctan2( + unitTestSim.panel2.r_HB_B[1][0], unitTestSim.panel2.r_HB_B[0][0] + ) spacecraft.panel2.thetaH = numpy.pi spacecraft.panel2.d = unitTestSim.panel2.d spacecraft.panel2.k = unitTestSim.panel2.k spacecraft.panel2.c = unitTestSim.panel2.c # Define initial conditions of the sim - time = numpy.arange(0.0,stopTime + stepSize,stepSize).flatten() + time = numpy.arange(0.0, stopTime + stepSize, stepSize).flatten() x0 = numpy.zeros(10) x0[3] = unitTestSim.panel1.thetaInit x0[4] = -unitTestSim.panel2.thetaInit - X = numpy.zeros((len(x0),len(time))) - X[:,0] = x0 - for j in range (1,(len(time))): - if time[j-1] < force1OffTime: + X = numpy.zeros((len(x0), len(time))) + X[:, 0] = x0 + for j in range(1, (len(time))): + if time[j - 1] < force1OffTime: spacecraft.xThrust_B = force1_B[0] spacecraft.yThrust_B = force1_B[1] spacecraft.Torque = torque1_B[2] - elif time[j-1] >= force2OnTime and time[j-1] < force2OffTime: + elif time[j - 1] >= force2OnTime and time[j - 1] < force2OffTime: spacecraft.xThrust_B = force2_B[0] spacecraft.yThrust_B = force2_B[1] spacecraft.Torque = torque2_B[2] @@ -473,16 +543,33 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): spacecraft.xThrust_B = 0.0 spacecraft.yThrust_B = 0.0 spacecraft.Torque = 0.0 - X[:, j] = rk4(planarFlexFunction, X[:, j-1], stepSize, time[j-1], spacecraft) + X[:, j] = rk4( + planarFlexFunction, X[:, j - 1], stepSize, time[j - 1], spacecraft + ) plt.figure() plt.clf() - plt.plot(time, X[0,:],'-b',label = "Lagrangian") - plt.plot(dataLog.times()*1e-9, (rOut_BN_N[:,0]-rOut_BN_N[:,0]),'-r',label = "Basilisk") - plt.plot([time[25], time[75], time[125], time[175]], [X[0,25], X[0,75], X[0,125], X[0,175],],'ok',label = "Test Points") - plt.xlabel('time (s)') - plt.ylabel('x position (m)') - plt.legend(loc ='upper left',numpoints = 1) + plt.plot(time, X[0, :], "-b", label="Lagrangian") + plt.plot( + dataLog.times() * 1e-9, + (rOut_BN_N[:, 0] - rOut_BN_N[:, 0]), + "-r", + label="Basilisk", + ) + plt.plot( + [time[25], time[75], time[125], time[175]], + [ + X[0, 25], + X[0, 75], + X[0, 125], + X[0, 175], + ], + "ok", + label="Test Points", + ) + plt.xlabel("time (s)") + plt.ylabel("x position (m)") + plt.legend(loc="upper left", numpoints=1) PlotName = "XPositionLagrangianVsBasilisk" PlotTitle = "X Position Lagrangian Vs Basilisk" format = r"width=0.8\textwidth" @@ -490,12 +577,27 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): plt.figure() plt.clf() - plt.plot(time, X[1,:],'-b',label = "Lagrangian") - plt.plot(dataLog.times()*1e-9, (rOut_BN_N[:,1]-rOut_BN_N[:,1]),'r',label = "Basilisk") - plt.plot([time[25], time[75], time[125], time[175]], [X[1,25], X[1,75], X[1,125], X[1,175],],'ok',label = "Test Points") - plt.xlabel('time (s)') - plt.ylabel('y position (m)') - plt.legend(loc ='upper left',numpoints = 1) + plt.plot(time, X[1, :], "-b", label="Lagrangian") + plt.plot( + dataLog.times() * 1e-9, + (rOut_BN_N[:, 1] - rOut_BN_N[:, 1]), + "r", + label="Basilisk", + ) + plt.plot( + [time[25], time[75], time[125], time[175]], + [ + X[1, 25], + X[1, 75], + X[1, 125], + X[1, 175], + ], + "ok", + label="Test Points", + ) + plt.xlabel("time (s)") + plt.ylabel("y position (m)") + plt.legend(loc="upper left", numpoints=1) PlotName = "YPositionLagrangianVsBasilisk" PlotTitle = "Y Position Lagrangian Vs Basilisk" format = r"width=0.8\textwidth" @@ -503,12 +605,22 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): plt.figure() plt.clf() - plt.plot(time, X[2,:],'-b',label = "Lagrangian") - plt.plot(dataLog.times()*1e-9, thetaOut,'-r',label = "Basilisk") - plt.plot([time[25], time[75], time[125], time[175]], [X[2,25], X[2,75], X[2,125], X[2,175],],'ok',label = "Test Points") - plt.xlabel('time (s)') - plt.ylabel('theta (rad)') - plt.legend(loc ='upper left',numpoints = 1) + plt.plot(time, X[2, :], "-b", label="Lagrangian") + plt.plot(dataLog.times() * 1e-9, thetaOut, "-r", label="Basilisk") + plt.plot( + [time[25], time[75], time[125], time[175]], + [ + X[2, 25], + X[2, 75], + X[2, 125], + X[2, 175], + ], + "ok", + label="Test Points", + ) + plt.xlabel("time (s)") + plt.ylabel("theta (rad)") + plt.legend(loc="upper left", numpoints=1) PlotName = "ThetaLagrangianVsBasilisk" PlotTitle = "Theta Lagrangian Vs Basilisk" format = r"width=0.8\textwidth" @@ -516,12 +628,22 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): plt.figure() plt.clf() - plt.plot(time, X[3,:],'-b',label = "Lagrangian") - plt.plot(theta1Out[:,0]*1e-9, theta1Out[:,1],'-r',label = "Basilisk") - plt.plot([time[25], time[75], time[125], time[175]], [X[3,25], X[3,75], X[3,125], X[3,175],],'ok',label = "Test Points") - plt.xlabel('time (s)') - plt.ylabel('theta 1 (rad)') - plt.legend(loc ='upper left',numpoints = 1) + plt.plot(time, X[3, :], "-b", label="Lagrangian") + plt.plot(theta1Out[:, 0] * 1e-9, theta1Out[:, 1], "-r", label="Basilisk") + plt.plot( + [time[25], time[75], time[125], time[175]], + [ + X[3, 25], + X[3, 75], + X[3, 125], + X[3, 175], + ], + "ok", + label="Test Points", + ) + plt.xlabel("time (s)") + plt.ylabel("theta 1 (rad)") + plt.legend(loc="upper left", numpoints=1) PlotName = "Theta1LagrangianVsBasilisk" PlotTitle = "Theta 1 Position Lagrangian Vs Basilisk" format = r"width=0.8\textwidth" @@ -529,12 +651,22 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): plt.figure() plt.clf() - plt.plot(time, -X[4,:],'-b',label = "Lagrangian") - plt.plot(theta2Out[:,0]*1e-9, theta2Out[:,1],'-r',label = "Basilisk") - plt.plot([time[25], time[75], time[125], time[175]], [-X[4,25], -X[4,75], -X[4,125], -X[4,175],],'ok',label = "Test Points") - plt.xlabel('time (s)') - plt.ylabel('theta 2 (rad)') - plt.legend(loc ='lower left',numpoints = 1) + plt.plot(time, -X[4, :], "-b", label="Lagrangian") + plt.plot(theta2Out[:, 0] * 1e-9, theta2Out[:, 1], "-r", label="Basilisk") + plt.plot( + [time[25], time[75], time[125], time[175]], + [ + -X[4, 25], + -X[4, 75], + -X[4, 125], + -X[4, 175], + ], + "ok", + label="Test Points", + ) + plt.xlabel("time (s)") + plt.ylabel("theta 2 (rad)") + plt.legend(loc="lower left", numpoints=1) PlotName = "Theta2LagrangianVsBasilisk" PlotTitle = "Theta 2 Lagrangian Vs Basilisk" format = r"width=0.8\textwidth" @@ -548,23 +680,32 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): timeList = [25, 75, 125, 175] for i in timeList: - if abs(X[0,i] - (rOut_BN_N[i,0]-rOut_BN_N[0,0])) > accuracy: - print(abs(X[0,i] - (rOut_BN_N[i,0]-rOut_BN_N[0,0]))) + if abs(X[0, i] - (rOut_BN_N[i, 0] - rOut_BN_N[0, 0])) > accuracy: + print(abs(X[0, i] - (rOut_BN_N[i, 0] - rOut_BN_N[0, 0]))) testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test Lagrangian vs. Basilisk failed x position comparison ") - if abs(X[1,i] - (rOut_BN_N[i,1]-rOut_BN_N[0,1])) > accuracy: + testMessages.append( + "FAILED: Hinged Rigid Body integrated test Lagrangian vs. Basilisk failed x position comparison " + ) + if abs(X[1, i] - (rOut_BN_N[i, 1] - rOut_BN_N[0, 1])) > accuracy: testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test Lagrangian vs. Basilisk failed y position comparison ") - if abs(X[2,i] - thetaOut[i]) > accuracy: + testMessages.append( + "FAILED: Hinged Rigid Body integrated test Lagrangian vs. Basilisk failed y position comparison " + ) + if abs(X[2, i] - thetaOut[i]) > accuracy: testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test Lagrangian vs. Basilisk failed theta comparison ") - if abs(X[3,i] - theta1Out[i,1]) > accuracy: + testMessages.append( + "FAILED: Hinged Rigid Body integrated test Lagrangian vs. Basilisk failed theta comparison " + ) + if abs(X[3, i] - theta1Out[i, 1]) > accuracy: testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test Lagrangian vs. Basilisk failed theta 1 comparison ") - if abs(-X[4,i] - theta2Out[i,1]) > accuracy: + testMessages.append( + "FAILED: Hinged Rigid Body integrated test Lagrangian vs. Basilisk failed theta 1 comparison " + ) + if abs(-X[4, i] - theta2Out[i, 1]) > accuracy: testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test Lagrangian vs. Basilisk failed theta 2 comparison ") - + testMessages.append( + "FAILED: Hinged Rigid Body integrated test Lagrangian vs. Basilisk failed theta 2 comparison " + ) if testFailCount == 0: print("PASSED: " + " Hinged Rigid Body Transient Integrated test") @@ -572,7 +713,7 @@ def hingedRigidBodyLagrangVsBasilisk(show_plots): assert testFailCount < 1, testMessages # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def planarFlexFunction(x, t, variables): @@ -610,59 +751,250 @@ def planarFlexFunction(x, t, variables): Torque = variables.Torque # Convert Tx_B and Ty_B to the inertial frame - dcm_BN = numpy.array([[numpy.cos(theta), numpy.sin(theta)], - [-numpy.sin(theta), numpy.cos(theta)]]) - Thrust_N = numpy.dot(dcm_BN.transpose(),numpy.array([[Tx_B],[Ty_B]])) - Tx = Thrust_N[0,0] - Ty = Thrust_N[1,0] - - matrixA = numpy.zeros((5,5)) + dcm_BN = numpy.array( + [[numpy.cos(theta), numpy.sin(theta)], [-numpy.sin(theta), numpy.cos(theta)]] + ) + Thrust_N = numpy.dot(dcm_BN.transpose(), numpy.array([[Tx_B], [Ty_B]])) + Tx = Thrust_N[0, 0] + Ty = Thrust_N[1, 0] + + matrixA = numpy.zeros((5, 5)) vectorB = numpy.zeros(5) # Populate X Translation Equation - matrixA[0,0] = 1.0 - matrixA[0,1] = 0.0 - matrixA[0,2] = -1/(mHub + mSP1 + mSP2)*(mSP1*Rhinge1*numpy.sin(beta1 + theta) + mSP2*Rhinge2*numpy.sin(beta2 + theta) + d1*mSP1*numpy.sin(thetaH1 + theta + theta1) + d2*mSP2*numpy.sin(thetaH2 + theta + theta2)) - matrixA[0,3] = -1/(mHub + mSP1 + mSP2)*(d1*mSP1*numpy.sin(thetaH1 + theta + theta1)) - matrixA[0,4] = -1/(mHub + mSP1 + mSP2)*(d2*mSP2*numpy.sin(thetaH2 + theta + theta2)) - vectorB[0] = 1/(mHub + mSP1 + mSP2)*(Tx + mSP1*Rhinge1*numpy.cos(beta1 + theta)*thetaDot**2 + mSP2*Rhinge2*numpy.cos(beta2 + theta)*thetaDot**2 + d1*mSP1*numpy.cos(thetaH1 + theta + theta1)*thetaDot**2 + d2*mSP2*numpy.cos(thetaH2 + theta + theta2)*thetaDot**2 - + 2*d1*mSP1*numpy.cos(thetaH1 + theta + theta1)*thetaDot*theta1Dot + d1*mSP1*numpy.cos(thetaH1 + theta + theta1)*theta1Dot**2 + 2*d2*mSP2*numpy.cos(thetaH2 + theta + theta2)*thetaDot*theta2Dot - + d2*mSP2*numpy.cos(thetaH2 + theta + theta2)*theta2Dot**2) + matrixA[0, 0] = 1.0 + matrixA[0, 1] = 0.0 + matrixA[0, 2] = ( + -1 + / (mHub + mSP1 + mSP2) + * ( + mSP1 * Rhinge1 * numpy.sin(beta1 + theta) + + mSP2 * Rhinge2 * numpy.sin(beta2 + theta) + + d1 * mSP1 * numpy.sin(thetaH1 + theta + theta1) + + d2 * mSP2 * numpy.sin(thetaH2 + theta + theta2) + ) + ) + matrixA[0, 3] = ( + -1 / (mHub + mSP1 + mSP2) * (d1 * mSP1 * numpy.sin(thetaH1 + theta + theta1)) + ) + matrixA[0, 4] = ( + -1 / (mHub + mSP1 + mSP2) * (d2 * mSP2 * numpy.sin(thetaH2 + theta + theta2)) + ) + vectorB[0] = ( + 1 + / (mHub + mSP1 + mSP2) + * ( + Tx + + mSP1 * Rhinge1 * numpy.cos(beta1 + theta) * thetaDot**2 + + mSP2 * Rhinge2 * numpy.cos(beta2 + theta) * thetaDot**2 + + d1 * mSP1 * numpy.cos(thetaH1 + theta + theta1) * thetaDot**2 + + d2 * mSP2 * numpy.cos(thetaH2 + theta + theta2) * thetaDot**2 + + 2 * d1 * mSP1 * numpy.cos(thetaH1 + theta + theta1) * thetaDot * theta1Dot + + d1 * mSP1 * numpy.cos(thetaH1 + theta + theta1) * theta1Dot**2 + + 2 * d2 * mSP2 * numpy.cos(thetaH2 + theta + theta2) * thetaDot * theta2Dot + + d2 * mSP2 * numpy.cos(thetaH2 + theta + theta2) * theta2Dot**2 + ) + ) # Populate Y Translation Equation - matrixA[1,0] = 0.0 - matrixA[1,1] = 1.0 - matrixA[1,2] = 1/(mHub + mSP1 + mSP2)*(mSP1*Rhinge1*numpy.cos(beta1 + theta) + mSP2*Rhinge2*numpy.cos(beta2 + theta) + d1*mSP1*numpy.cos(thetaH1 + theta + theta1) + d2*mSP2*numpy.cos(thetaH2 + theta + theta2)) - matrixA[1,3] = 1/(mHub + mSP1 + mSP2)*(d1*mSP1*numpy.cos(thetaH1 + theta + theta1)) - matrixA[1,4] = 1/(mHub + mSP1 + mSP2)*(d2*mSP2*numpy.cos(thetaH2 + theta + theta2)) - vectorB[1] = 1/(mHub + mSP1 + mSP2)*(Ty + mSP1*Rhinge1*numpy.sin(beta1 + theta)*thetaDot**2 + mSP2*Rhinge2*numpy.sin(beta2 + theta)*thetaDot**2 + d1*mSP1*numpy.sin(thetaH1 + theta + theta1)*thetaDot**2 + d2*mSP2*numpy.sin(thetaH2 + theta + theta2)*thetaDot**2 - + 2*d1*mSP1*numpy.sin(thetaH1 + theta + theta1)*thetaDot*theta1Dot + d1*mSP1*numpy.sin(thetaH1 + theta + theta1)*theta1Dot**2 + 2*d2*mSP2*numpy.sin(thetaH2 + theta + theta2)*thetaDot*theta2Dot - + d2*mSP2*numpy.sin(thetaH2 + theta + theta2)*theta2Dot**2) + matrixA[1, 0] = 0.0 + matrixA[1, 1] = 1.0 + matrixA[1, 2] = ( + 1 + / (mHub + mSP1 + mSP2) + * ( + mSP1 * Rhinge1 * numpy.cos(beta1 + theta) + + mSP2 * Rhinge2 * numpy.cos(beta2 + theta) + + d1 * mSP1 * numpy.cos(thetaH1 + theta + theta1) + + d2 * mSP2 * numpy.cos(thetaH2 + theta + theta2) + ) + ) + matrixA[1, 3] = ( + 1 / (mHub + mSP1 + mSP2) * (d1 * mSP1 * numpy.cos(thetaH1 + theta + theta1)) + ) + matrixA[1, 4] = ( + 1 / (mHub + mSP1 + mSP2) * (d2 * mSP2 * numpy.cos(thetaH2 + theta + theta2)) + ) + vectorB[1] = ( + 1 + / (mHub + mSP1 + mSP2) + * ( + Ty + + mSP1 * Rhinge1 * numpy.sin(beta1 + theta) * thetaDot**2 + + mSP2 * Rhinge2 * numpy.sin(beta2 + theta) * thetaDot**2 + + d1 * mSP1 * numpy.sin(thetaH1 + theta + theta1) * thetaDot**2 + + d2 * mSP2 * numpy.sin(thetaH2 + theta + theta2) * thetaDot**2 + + 2 * d1 * mSP1 * numpy.sin(thetaH1 + theta + theta1) * thetaDot * theta1Dot + + d1 * mSP1 * numpy.sin(thetaH1 + theta + theta1) * theta1Dot**2 + + 2 * d2 * mSP2 * numpy.sin(thetaH2 + theta + theta2) * thetaDot * theta2Dot + + d2 * mSP2 * numpy.sin(thetaH2 + theta + theta2) * theta2Dot**2 + ) + ) # Populate theta Equation - matrixA[2,0] = -1/(IHub + ISP1 + ISP2 + d1**2*mSP1 + d2**2*mSP2 + mSP1*Rhinge1**2 + mSP2*Rhinge2**2 + 2*d1*mSP1*Rhinge1*numpy.cos(beta1 - thetaH1 - theta1) + 2*d2*mSP2*Rhinge2*numpy.cos(beta2 - thetaH2 - theta2))*(mSP1*Rhinge1*numpy.sin(beta1 + theta) - + mSP2*Rhinge2*numpy.sin(beta2 + theta) + d1*mSP1*numpy.sin(thetaH1 + theta + theta1) + d2*mSP2*numpy.sin(thetaH2 + theta + theta2)) - matrixA[2,1] = 1/(IHub + ISP1 + ISP2 + d1**2*mSP1 + d2**2*mSP2 + mSP1*Rhinge1**2 + mSP2*Rhinge2**2 + 2*d1*mSP1*Rhinge1*numpy.cos(beta1 - thetaH1 - theta1) + 2*d2*mSP2*Rhinge2*numpy.cos(beta2 - thetaH2 - theta2))*(mSP1*Rhinge1*numpy.cos(beta1 + theta) - + mSP2*Rhinge2*numpy.cos(beta2 + theta) + d1*mSP1*numpy.cos(thetaH1 + theta + theta1) + d2*mSP2*numpy.cos(thetaH2 + theta + theta2)) - matrixA[2,2] = 1.0 - matrixA[2,3] = 1/(IHub + ISP1 + ISP2 + d1**2*mSP1 + d2**2*mSP2 + mSP1*Rhinge1**2 + mSP2*Rhinge2**2 + 2*d1*mSP1*Rhinge1*numpy.cos(beta1 - thetaH1 - theta1) + 2*d2*mSP2*Rhinge2*numpy.cos(beta2 - thetaH2 - theta2))*(ISP1 - + d1**2*mSP1 + d1*mSP1*Rhinge1*numpy.cos(beta1 - thetaH1 - theta1)) - matrixA[2,4] = 1/(IHub + ISP1 + ISP2 + d1**2*mSP1 + d2**2*mSP2 + mSP1*Rhinge1**2 + mSP2*Rhinge2**2 + 2*d1*mSP1*Rhinge1*numpy.cos(beta1 - thetaH1 - theta1) + 2*d2*mSP2*Rhinge2*numpy.cos(beta2 - thetaH2 - theta2))*(ISP2 - + d2**2*mSP2 + d2*mSP2*Rhinge2*numpy.cos(beta2 - thetaH2 - theta2)) - vectorB[2] = 1/(IHub + ISP1 + ISP2 + d1**2*mSP1 + d2**2*mSP2 + mSP1*Rhinge1**2 + mSP2*Rhinge2**2 + 2*d1*mSP1*Rhinge1*numpy.cos(beta1 - thetaH1 - theta1) + 2*d2*mSP2*Rhinge2*numpy.cos(beta2 - thetaH2 - theta2))*(Torque - - 2*d1*mSP1*Rhinge1*numpy.sin(beta1 - thetaH1 - theta1)*thetaDot*theta1Dot - d1*mSP1*Rhinge1*numpy.sin(beta1 - thetaH1 - theta1)*theta1Dot**2 - - 2*d2*mSP2*Rhinge2*numpy.sin(beta2 - thetaH2 - theta2)*thetaDot*theta2Dot - d2*mSP2*Rhinge2*numpy.sin(beta2 - thetaH2 - theta2)*theta2Dot**2) + matrixA[2, 0] = ( + -1 + / ( + IHub + + ISP1 + + ISP2 + + d1**2 * mSP1 + + d2**2 * mSP2 + + mSP1 * Rhinge1**2 + + mSP2 * Rhinge2**2 + + 2 * d1 * mSP1 * Rhinge1 * numpy.cos(beta1 - thetaH1 - theta1) + + 2 * d2 * mSP2 * Rhinge2 * numpy.cos(beta2 - thetaH2 - theta2) + ) + * ( + mSP1 * Rhinge1 * numpy.sin(beta1 + theta) + + mSP2 * Rhinge2 * numpy.sin(beta2 + theta) + + d1 * mSP1 * numpy.sin(thetaH1 + theta + theta1) + + d2 * mSP2 * numpy.sin(thetaH2 + theta + theta2) + ) + ) + matrixA[2, 1] = ( + 1 + / ( + IHub + + ISP1 + + ISP2 + + d1**2 * mSP1 + + d2**2 * mSP2 + + mSP1 * Rhinge1**2 + + mSP2 * Rhinge2**2 + + 2 * d1 * mSP1 * Rhinge1 * numpy.cos(beta1 - thetaH1 - theta1) + + 2 * d2 * mSP2 * Rhinge2 * numpy.cos(beta2 - thetaH2 - theta2) + ) + * ( + mSP1 * Rhinge1 * numpy.cos(beta1 + theta) + + mSP2 * Rhinge2 * numpy.cos(beta2 + theta) + + d1 * mSP1 * numpy.cos(thetaH1 + theta + theta1) + + d2 * mSP2 * numpy.cos(thetaH2 + theta + theta2) + ) + ) + matrixA[2, 2] = 1.0 + matrixA[2, 3] = ( + 1 + / ( + IHub + + ISP1 + + ISP2 + + d1**2 * mSP1 + + d2**2 * mSP2 + + mSP1 * Rhinge1**2 + + mSP2 * Rhinge2**2 + + 2 * d1 * mSP1 * Rhinge1 * numpy.cos(beta1 - thetaH1 - theta1) + + 2 * d2 * mSP2 * Rhinge2 * numpy.cos(beta2 - thetaH2 - theta2) + ) + * ( + ISP1 + + d1**2 * mSP1 + + d1 * mSP1 * Rhinge1 * numpy.cos(beta1 - thetaH1 - theta1) + ) + ) + matrixA[2, 4] = ( + 1 + / ( + IHub + + ISP1 + + ISP2 + + d1**2 * mSP1 + + d2**2 * mSP2 + + mSP1 * Rhinge1**2 + + mSP2 * Rhinge2**2 + + 2 * d1 * mSP1 * Rhinge1 * numpy.cos(beta1 - thetaH1 - theta1) + + 2 * d2 * mSP2 * Rhinge2 * numpy.cos(beta2 - thetaH2 - theta2) + ) + * ( + ISP2 + + d2**2 * mSP2 + + d2 * mSP2 * Rhinge2 * numpy.cos(beta2 - thetaH2 - theta2) + ) + ) + vectorB[2] = ( + 1 + / ( + IHub + + ISP1 + + ISP2 + + d1**2 * mSP1 + + d2**2 * mSP2 + + mSP1 * Rhinge1**2 + + mSP2 * Rhinge2**2 + + 2 * d1 * mSP1 * Rhinge1 * numpy.cos(beta1 - thetaH1 - theta1) + + 2 * d2 * mSP2 * Rhinge2 * numpy.cos(beta2 - thetaH2 - theta2) + ) + * ( + Torque + - 2 + * d1 + * mSP1 + * Rhinge1 + * numpy.sin(beta1 - thetaH1 - theta1) + * thetaDot + * theta1Dot + - d1 * mSP1 * Rhinge1 * numpy.sin(beta1 - thetaH1 - theta1) * theta1Dot**2 + - 2 + * d2 + * mSP2 + * Rhinge2 + * numpy.sin(beta2 - thetaH2 - theta2) + * thetaDot + * theta2Dot + - d2 * mSP2 * Rhinge2 * numpy.sin(beta2 - thetaH2 - theta2) * theta2Dot**2 + ) + ) # Populate theta1 Equation - matrixA[3,0] = -1/(ISP1 + d1**2*mSP1)*(d1*mSP1*numpy.sin(thetaH1 + theta + theta1)) - matrixA[3,1] = 1/(ISP1 + d1**2*mSP1)*(d1*mSP1*numpy.cos(thetaH1 + theta + theta1)) - matrixA[3,2] = 1/(ISP1 + d1**2*mSP1)*(ISP1 + d1**2*mSP1 + d1*mSP1*Rhinge1*numpy.cos(beta1 - thetaH1 - theta1)) - matrixA[3,3] = 1.0 - matrixA[3,4] = 0.0 - vectorB[3] = 1/(ISP1 + d1**2*mSP1)*(-k1*theta1 + d1*mSP1*Rhinge1*numpy.sin(beta1 - thetaH1 - theta1)*thetaDot**2 - c1*theta1Dot) + matrixA[3, 0] = ( + -1 / (ISP1 + d1**2 * mSP1) * (d1 * mSP1 * numpy.sin(thetaH1 + theta + theta1)) + ) + matrixA[3, 1] = ( + 1 / (ISP1 + d1**2 * mSP1) * (d1 * mSP1 * numpy.cos(thetaH1 + theta + theta1)) + ) + matrixA[3, 2] = ( + 1 + / (ISP1 + d1**2 * mSP1) + * ( + ISP1 + + d1**2 * mSP1 + + d1 * mSP1 * Rhinge1 * numpy.cos(beta1 - thetaH1 - theta1) + ) + ) + matrixA[3, 3] = 1.0 + matrixA[3, 4] = 0.0 + vectorB[3] = ( + 1 + / (ISP1 + d1**2 * mSP1) + * ( + -k1 * theta1 + + d1 * mSP1 * Rhinge1 * numpy.sin(beta1 - thetaH1 - theta1) * thetaDot**2 + - c1 * theta1Dot + ) + ) # Populate theta2 Equation - matrixA[4,0] = -1/(ISP2 + d2**2*mSP2)*(d2*mSP2*numpy.sin(thetaH2 + theta + theta2)) - matrixA[4,1] = 1/(ISP2 + d2**2*mSP2)*(d2*mSP2*numpy.cos(thetaH2 + theta + theta2)) - matrixA[4,2] = 1/(ISP2 + d2**2*mSP2)*(ISP2 + d2**2*mSP2 + d2*mSP2*Rhinge2*numpy.cos(beta2 - thetaH2 - theta2)) - matrixA[4,3] = 0.0 - matrixA[4,4] = 1.0 - vectorB[4] = 1/(ISP2 + d2**2*mSP2)*(-k2*theta2 + d2*mSP2*Rhinge2*numpy.sin(beta2 - thetaH2 - theta2)*thetaDot**2 - c2*theta2Dot) + matrixA[4, 0] = ( + -1 / (ISP2 + d2**2 * mSP2) * (d2 * mSP2 * numpy.sin(thetaH2 + theta + theta2)) + ) + matrixA[4, 1] = ( + 1 / (ISP2 + d2**2 * mSP2) * (d2 * mSP2 * numpy.cos(thetaH2 + theta + theta2)) + ) + matrixA[4, 2] = ( + 1 + / (ISP2 + d2**2 * mSP2) + * ( + ISP2 + + d2**2 * mSP2 + + d2 * mSP2 * Rhinge2 * numpy.cos(beta2 - thetaH2 - theta2) + ) + ) + matrixA[4, 3] = 0.0 + matrixA[4, 4] = 1.0 + vectorB[4] = ( + 1 + / (ISP2 + d2**2 * mSP2) + * ( + -k2 * theta2 + + d2 * mSP2 * Rhinge2 * numpy.sin(beta2 - thetaH2 - theta2) * thetaDot**2 + - c2 * theta2Dot + ) + ) Xdot = numpy.zeros(len(x)) # Populate Trivial derivatives @@ -672,18 +1004,18 @@ def planarFlexFunction(x, t, variables): Xdot[3] = theta1Dot Xdot[4] = theta2Dot # Calculate nontrivial derivatives - result = numpy.dot(numpy.linalg.inv(matrixA),vectorB) + result = numpy.dot(numpy.linalg.inv(matrixA), vectorB) Xdot[5:10] = result return Xdot def rk4(Fn, X, h, t, varargin): - k1 = h*Fn(X, t, varargin) - k2 = h*Fn(X+k1/2, t+h/2, varargin) - k3 = h*Fn(X+k2/2, t+h/2, varargin) - k4 = h*Fn(X+k3, t+h, varargin) - Z = X + (k1 + 2*k2 + 2*k3 + k4)/6.0 + k1 = h * Fn(X, t, varargin) + k2 = h * Fn(X + k1 / 2, t + h / 2, varargin) + k3 = h * Fn(X + k2 / 2, t + h / 2, varargin) + k4 = h * Fn(X + k3, t + h, varargin) + Z = X + (k1 + 2 * k2 + 2 * k3 + k4) / 6.0 return Z @@ -712,27 +1044,27 @@ class spacecraftClass: Torque = 0.0 -def newtonRapshon(funcAndDervi,guess,tolerance,variables): +def newtonRapshon(funcAndDervi, guess, tolerance, variables): xOld = guess - for i in range(1,101): + for i in range(1, 101): fx, fPrimex = funcAndDervi(xOld, variables) - xNew = xOld - fx/fPrimex + xNew = xOld - fx / fPrimex if abs(xNew - xOld) < tolerance: break xOld = xNew return xNew -def boxAndWingsFandFPrime(theta,variables): +def boxAndWingsFandFPrime(theta, variables): # Define variables F = variables.F mSC = variables.mSC k = variables.k mSP = variables.mSP d = variables.d - aSP = F/mSC - fX = k*theta + mSP*aSP*d*numpy.cos(theta) - fPrimeX = k - mSP*aSP*d*numpy.sin(theta) + aSP = F / mSC + fX = k * theta + mSP * aSP * d * numpy.cos(theta) + fPrimeX = k - mSP * aSP * d * numpy.sin(theta) return fX, fPrimeX @@ -743,5 +1075,6 @@ class boxAndWingParameters: mSP = 0 d = 0 + if __name__ == "__main__": hingedRigidBodyMotorTorque(True, True) diff --git a/src/simulation/dynamics/Integrators/_UnitTest/test_Integrators.py b/src/simulation/dynamics/Integrators/_UnitTest/test_Integrators.py index 4fe91d0520..1855f369ab 100755 --- a/src/simulation/dynamics/Integrators/_UnitTest/test_Integrators.py +++ b/src/simulation/dynamics/Integrators/_UnitTest/test_Integrators.py @@ -29,15 +29,19 @@ import matplotlib.pyplot as plt import numpy as np import pytest + # import simulation related support from Basilisk.simulation import spacecraft from Basilisk.simulation import svIntegrators + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # @cond DOXYGEN_IGNORE filename = inspect.getframeinfo(inspect.currentframe()).filename @@ -46,18 +50,21 @@ # @endcond + # uncomment this line is 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(True, reason="Scott's brain no-worky\n") # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("integratorCase", ["rk4", "rkf45", "rkf78", "euler", "rk2", "rk3", "bogackiShampine"]) +@pytest.mark.parametrize( + "integratorCase", + ["rk4", "rkf45", "rkf78", "euler", "rk2", "rk3", "bogackiShampine"], +) def test_scenarioIntegrators(show_plots, integratorCase): """This function is called by the py.test environment.""" # each test method requires a single assert method to be called - [testResults, testMessage] = run(True, - show_plots, integratorCase) + [testResults, testMessage] = run(True, show_plots, integratorCase) assert testResults < 1, testMessage @@ -79,7 +86,7 @@ def run(doUnitTests, show_plots, integratorCase): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(120.) + simulationTimeStep = macros.sec2nano(120.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # @@ -109,13 +116,9 @@ def run(doUnitTests, show_plots, integratorCase): elif integratorCase == "rk3": integratorObject = svIntegrators.svIntegratorRungeKutta( scObject, - a_coefficients=[ - [0, 0, 0], - [0.5, 0, 0], - [-1, 2, 0] - ], - b_coefficients=[1/6, 2/3, 1/6], - c_coefficients=[0, 0.5, 1] + a_coefficients=[[0, 0, 0], [0.5, 0, 0], [-1, 2, 0]], + b_coefficients=[1 / 6, 2 / 3, 1 / 6], + c_coefficients=[0, 0.5, 1], ) scObject.setIntegrator(integratorObject) elif integratorCase == "bogackiShampine": @@ -123,14 +126,14 @@ def run(doUnitTests, show_plots, integratorCase): scObject, largest_order=3, a_coefficients=[ - [0, 0, 0, 0], - [1/2, 0, 0, 0], - [0 , 3/4, 0, 0], - [2/9, 1/3, 4/9, 0] + [0, 0, 0, 0], + [1 / 2, 0, 0, 0], + [0, 3 / 4, 0, 0], + [2 / 9, 1 / 3, 4 / 9, 0], ], - b_coefficients=[7/24, 1/4, 1/3, 1/8], - b_star_coefficients=[2/9, 1/3, 4/9, 0], - c_coefficients=[0, 1/2, 3/4, 1] + b_coefficients=[7 / 24, 1 / 4, 1 / 3, 1 / 8], + b_star_coefficients=[2 / 9, 1 / 3, 4 / 9, 0], + c_coefficients=[0, 1 / 2, 3 / 4, 1], ) integratorObject.setRelativeTolerance(0) integratorObject.setAbsoluteTolerance(0.0001) @@ -147,14 +150,16 @@ def run(doUnitTests, show_plots, integratorCase): mu = earth.mu # attach gravity model to spacecraft - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # # setup orbit and simulation time # # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rLEO = 7000. * 1000 # meters + rLEO = 7000.0 * 1000 # meters oe.a = rLEO oe.e = 0.0001 oe.i = 33.3 * macros.D2R @@ -171,14 +176,16 @@ def run(doUnitTests, show_plots, integratorCase): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n simulationTime = macros.sec2nano(0.75 * P) # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) @@ -203,7 +210,7 @@ def run(doUnitTests, show_plots, integratorCase): # plot the results # np.set_printoptions(precision=16) - fileNameString = filename[len(path) + 6:-3] + fileNameString = filename[len(path) + 6 : -3] plt.close("all") # clears out plots from earlier test runs # draw orbit in perifocal frame @@ -215,7 +222,7 @@ def run(doUnitTests, show_plots, integratorCase): # draw the planet fig = plt.gcf() ax = fig.gca() - planetColor = '#008800' + planetColor = "#008800" planetRadius = earth.radEquator / 1000 ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor)) # draw the actual orbit @@ -226,23 +233,27 @@ def run(doUnitTests, show_plots, integratorCase): oeData = orbitalMotion.rv2elem(mu, posData[idx], velData[idx]) 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=unitTestSupport.getLineColor(labelStrings.index(integratorCase) + 1, len(labelStrings)) - , label=integratorCase - , linewidth=3.0 - ) + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + # , color=unitTestSupport.getLineColor(labelStrings.index(integratorCase) + 1, len(labelStrings)) + label=integratorCase, + 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('$i_e$ Cord. [km]') - plt.ylabel('$i_p$ Cord. [km]') - plt.legend(loc='lower right') + plt.plot( + rData * np.cos(fData) / 1000, + rData * np.sin(fData) / 1000, + "--", + color="#555555", + ) + plt.xlabel("$i_e$ Cord. [km]") + plt.ylabel("$i_p$ Cord. [km]") + plt.legend(loc="lower right") plt.grid() if doUnitTests: # only save off the figure if doing a unit test run # unitTestSupport.saveScenarioFigure( @@ -257,11 +268,12 @@ def run(doUnitTests, show_plots, integratorCase): "Illustration of the BSK integrated trajectories", plt, "", - path) + path, + ) if show_plots: plt.show() - plt.close('all') + plt.close("all") # # close the plots being saved off to avoid over-writing old and new figures # plt.close("all") @@ -278,73 +290,73 @@ def run(doUnitTests, show_plots, integratorCase): # setup truth data for unit test if integratorCase == "rk4": truePos = [ - [-2.8168016010234915e6, 5.248174846916147e6, 3.677157264677297e6] - , [-6.379381726549218e6, -1.4688565370540658e6, 2.4807857675497606e6] - , [-2.230094305694789e6, -6.410420020364709e6, -1.7146277675541767e6] - , [4.614900659014343e6, -3.60224207689023e6, -3.837022825958977e6] - , [5.879095186201691e6, 3.561495655367985e6, -1.3195821703218794e6] + [-2.8168016010234915e6, 5.248174846916147e6, 3.677157264677297e6], + [-6.379381726549218e6, -1.4688565370540658e6, 2.4807857675497606e6], + [-2.230094305694789e6, -6.410420020364709e6, -1.7146277675541767e6], + [4.614900659014343e6, -3.60224207689023e6, -3.837022825958977e6], + [5.879095186201691e6, 3.561495655367985e6, -1.3195821703218794e6], ] if integratorCase in {"rkf45", "rkf78", "bogackiShampine"}: - truePos = [[ 5879286.370258273, 3561242.50810664, -1319786.625981673]] - dataPosRed = dataPosRed[-1,:][np.newaxis] + truePos = [[5879286.370258273, 3561242.50810664, -1319786.625981673]] + dataPosRed = dataPosRed[-1, :][np.newaxis] if integratorCase == "euler": truePos = [ - [-2.8168016010234915e6, 5.248174846916147e6, 3.677157264677297e6] - , [-7.061548530211288e6, -1.4488790844105487e6, 2.823580168201031e6] - , [-4.831279689590867e6, -8.015202650472983e6, -1.1434851461593418e6] - , [719606.5825106134, -1.0537603309084207e7, -4.966060248346598e6] - , [6.431097055190775e6, -9.795566286964862e6, -7.438012269629238e6] + [-2.8168016010234915e6, 5.248174846916147e6, 3.677157264677297e6], + [-7.061548530211288e6, -1.4488790844105487e6, 2.823580168201031e6], + [-4.831279689590867e6, -8.015202650472983e6, -1.1434851461593418e6], + [719606.5825106134, -1.0537603309084207e7, -4.966060248346598e6], + [6.431097055190775e6, -9.795566286964862e6, -7.438012269629238e6], ] if integratorCase == "rk2": truePos = [ - [-2.8168016010234915e6, 5.248174846916147e6, 3.677157264677297e6] - , [-6.425636528569288e6, -1.466693214251768e6, 2.50438327358707e6] - , [-2.466642497083674e6, -6.509473992136429e6, -1.6421621818735446e6] - , [4.342561337924192e6, -4.1593822658140697e6, -3.947594705237753e6] - , [6.279757158711852e6, 2.8527385905952943e6, -1.8260959147806289e6] + [-2.8168016010234915e6, 5.248174846916147e6, 3.677157264677297e6], + [-6.425636528569288e6, -1.466693214251768e6, 2.50438327358707e6], + [-2.466642497083674e6, -6.509473992136429e6, -1.6421621818735446e6], + [4.342561337924192e6, -4.1593822658140697e6, -3.947594705237753e6], + [6.279757158711852e6, 2.8527385905952943e6, -1.8260959147806289e6], ] if integratorCase == "rk3": truePos = [ - [-2816801.601023492 , 5248174.846916147 , 3677157.2646772973], - [-6380014.419169294 , -1467304.5024778044, 2481775.1156921615], - [-2231193.424069216 , -6407257.277104054 , -1712704.800575082 ], - [ 4613563.820461048 , -3590442.718508557 , -3831202.081509318 ], - [ 5853794.060191272 , 3579538.889746918 , -1299292.6887013493] + [-2816801.601023492, 5248174.846916147, 3677157.2646772973], + [-6380014.419169294, -1467304.5024778044, 2481775.1156921615], + [-2231193.424069216, -6407257.277104054, -1712704.800575082], + [4613563.820461048, -3590442.718508557, -3831202.081509318], + [5853794.060191272, 3579538.889746918, -1299292.6887013493], ] # compare the results to the truth values accuracy = 1.0 # meters testFailCount, testMessages = unitTestSupport.compareArray( - truePos, dataPosRed, accuracy, "r_BN_N Vector", - testFailCount, testMessages) + truePos, dataPosRed, accuracy, "r_BN_N Vector", testFailCount, testMessages + ) # print out success message if no error were found if testFailCount == 0: print("PASSED ") passFailText = "PASSED" - colorText = 'ForestGreen' # color to write auto-documented "PASSED" message in in LATEX + colorText = "ForestGreen" # color to write auto-documented "PASSED" message in in LATEX snippetContent = "" else: print(testFailCount) print(testMessages) - passFailText = 'FAILED' - colorText = 'Red' # color to write auto-documented "FAILED" message in in LATEX + passFailText = "FAILED" + colorText = ( + "Red" # color to write auto-documented "FAILED" message in in LATEX + ) snippetContent = r"\begin{verbatim}" for message in testMessages: snippetContent += message snippetContent += r"\end{verbatim}" - snippetMsgName = fileNameString + 'Msg-' + integratorCase - unitTestSupport.writeTeXSnippet(snippetMsgName, snippetContent, - path) - snippetPassFailName = fileNameString + 'TestMsg-' + integratorCase - snippetContent = r'\textcolor{' + colorText + '}{' + passFailText + '}' - unitTestSupport.writeTeXSnippet(snippetPassFailName, snippetContent, - path) + snippetMsgName = fileNameString + "Msg-" + integratorCase + unitTestSupport.writeTeXSnippet(snippetMsgName, snippetContent, path) + snippetPassFailName = fileNameString + "TestMsg-" + integratorCase + snippetContent = r"\textcolor{" + colorText + "}{" + passFailText + "}" + unitTestSupport.writeTeXSnippet(snippetPassFailName, snippetContent, path) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -352,6 +364,8 @@ def run(doUnitTests, show_plots, integratorCase): # stand-along python script # if __name__ == "__main__": - run(True, # do unit tests + run( + True, # do unit tests True, # show_plots - 'bogackiShampine') # integrator case(0 - RK4, 1 - RKF45, 2 - Euler, 3 - RK2, 4 - RK3) + "bogackiShampine", + ) # integrator case(0 - RK4, 1 - RKF45, 2 - Euler, 3 - RK2, 4 - RK3) diff --git a/src/simulation/dynamics/LinearSpringMassDamper/_UnitTest/test_linearSpringMassDamper.py b/src/simulation/dynamics/LinearSpringMassDamper/_UnitTest/test_linearSpringMassDamper.py index 48a63ffc5c..ceac268dd6 100644 --- a/src/simulation/dynamics/LinearSpringMassDamper/_UnitTest/test_linearSpringMassDamper.py +++ b/src/simulation/dynamics/LinearSpringMassDamper/_UnitTest/test_linearSpringMassDamper.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -27,7 +26,9 @@ path = os.path.dirname(os.path.abspath(filename)) from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.simulation import spacecraft from Basilisk.simulation import linearSpringMassDamper from Basilisk.simulation import gravityEffector @@ -38,24 +39,29 @@ from Basilisk.utilities import simIncludeThruster from Basilisk.architecture import messaging -@pytest.mark.parametrize("useFlag, testCase", [ - (False,'NoGravity'), - (False,'Gravity'), - (False,'Damping'), - (False,'MassDepletion') -]) + +@pytest.mark.parametrize( + "useFlag, testCase", + [ + (False, "NoGravity"), + (False, "Gravity"), + (False, "Damping"), + (False, "MassDepletion"), + ], +) # uncomment this line is 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() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -def test_fuelSloshAllTest(show_plots,useFlag,testCase): +def test_fuelSloshAllTest(show_plots, useFlag, testCase): """Module Unit Test""" - [testResults, testMessage] = fuelSloshTest(show_plots,useFlag,testCase) + [testResults, testMessage] = fuelSloshTest(show_plots, useFlag, testCase) assert testResults < 1, testMessage -def fuelSloshTest(show_plots,useFlag,testCase): + +def fuelSloshTest(show_plots, useFlag, testCase): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. @@ -86,7 +92,11 @@ def fuelSloshTest(show_plots,useFlag,testCase): unitTestSim.particle1.k = 100.0 unitTestSim.particle1.c = 0.0 unitTestSim.particle1.r_PB_B = [[0.1], [0], [-0.1]] - unitTestSim.particle1.pHat_B = [[np.sqrt(3)/3], [np.sqrt(3)/3], [np.sqrt(3)/3]] + unitTestSim.particle1.pHat_B = [ + [np.sqrt(3) / 3], + [np.sqrt(3) / 3], + [np.sqrt(3) / 3], + ] unitTestSim.particle1.rhoInit = 0.05 unitTestSim.particle1.rhoDotInit = 0.0 unitTestSim.particle1.massInit = 10.0 @@ -95,7 +105,11 @@ def fuelSloshTest(show_plots,useFlag,testCase): unitTestSim.particle2.k = 100.0 unitTestSim.particle2.c = 0.0 unitTestSim.particle2.r_PB_B = [[0], [0], [0.1]] - unitTestSim.particle2.pHat_B = [[np.sqrt(3)/3], [-np.sqrt(3)/3], [-np.sqrt(3)/3]] + unitTestSim.particle2.pHat_B = [ + [np.sqrt(3) / 3], + [-np.sqrt(3) / 3], + [-np.sqrt(3) / 3], + ] unitTestSim.particle2.rhoInit = -0.025 unitTestSim.particle2.rhoDotInit = 0.0 unitTestSim.particle2.massInit = 20.0 @@ -104,32 +118,35 @@ def fuelSloshTest(show_plots,useFlag,testCase): unitTestSim.particle3.k = 100.0 unitTestSim.particle3.c = 0.0 unitTestSim.particle3.r_PB_B = [[-0.1], [0], [0.1]] - unitTestSim.particle3.pHat_B = [[-np.sqrt(3)/3], [-np.sqrt(3)/3], [np.sqrt(3)/3]] + unitTestSim.particle3.pHat_B = [ + [-np.sqrt(3) / 3], + [-np.sqrt(3) / 3], + [np.sqrt(3) / 3], + ] unitTestSim.particle3.rhoInit = -0.015 unitTestSim.particle3.rhoDotInit = 0.0 unitTestSim.particle3.massInit = 15.0 - if testCase == 'MassDepletion': + if testCase == "MassDepletion": thrusterCommandName = "acs_thruster_cmds" # add thruster devices thFactory = simIncludeThruster.thrusterFactory() - thFactory.create('MOOG_Monarc_445', - [1,0,0], # location in S frame - [0,1,0] # direction in S frame - ) + thFactory.create( + "MOOG_Monarc_445", + [1, 0, 0], # location in S frame + [0, 1, 0], # direction in S frame + ) # create thruster object container and tie to spacecraft object thrustersDynamicEffector = thrusterDynamicEffector.ThrusterDynamicEffector() - thFactory.addToSpacecraft("Thrusters", - thrustersDynamicEffector, - scObject) + thFactory.addToSpacecraft("Thrusters", thrustersDynamicEffector, scObject) unitTestSim.fuelTankStateEffector = fuelTank.FuelTank() tankModel = fuelTank.FuelTankModelConstantVolume() unitTestSim.fuelTankStateEffector.setTankModel(tankModel) tankModel.propMassInit = 40.0 - tankModel.r_TcT_TInit = [[0.0],[0.0],[0.0]] - unitTestSim.fuelTankStateEffector.r_TB_B = [[0.0],[0.0],[0.0]] + tankModel.r_TcT_TInit = [[0.0], [0.0], [0.0]] + unitTestSim.fuelTankStateEffector.r_TB_B = [[0.0], [0.0], [0.0]] tankModel.radiusTankInit = 46.0 / 2.0 / 3.2808399 / 12.0 # Add tank and thruster @@ -158,7 +175,7 @@ def fuelSloshTest(show_plots,useFlag,testCase): scObject.addStateEffector(unitTestSim.particle2) scObject.addStateEffector(unitTestSim.particle3) - if testCase == 'Damping': + if testCase == "Damping": unitTestSim.particle1.c = 15.0 unitTestSim.particle2.c = 17.0 unitTestSim.particle3.c = 11.0 @@ -170,31 +187,51 @@ def fuelSloshTest(show_plots,useFlag,testCase): scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] scObject.hub.r_CN_NInit = [[0.5], [0.4], [-0.7]] - scObject.hub.v_CN_NInit = [[0.1], [0.-5], [0.3]] + scObject.hub.v_CN_NInit = [[0.1], [0.0 - 5], [0.3]] scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] scObject.hub.omega_BN_BInit = [[0.1], [-0.1], [0.1]] - if testCase == 'Gravity': + if testCase == "Gravity": unitTestSim.earthGravBody = gravityEffector.GravBodyData() unitTestSim.earthGravBody.planetName = "earth_planet_data" - unitTestSim.earthGravBody.mu = 0.3986004415E+15 # meters! + unitTestSim.earthGravBody.mu = 0.3986004415e15 # meters! unitTestSim.earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([unitTestSim.earthGravBody]) - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + [unitTestSim.earthGravBody] + ) + scObject.hub.r_CN_NInit = [ + [-4020338.690396649], + [7490566.741852513], + [5248299.211589362], + ] + scObject.hub.v_CN_NInit = [ + [-5199.77710904224], + [-3436.681645356935], + [1041.576797498721], + ] dataLog = scObject.scStateOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) - scObjectLog = scObject.logger(["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) - if testCase == 'MassDepletion': - stateLog = pythonVariableLogger.PythonVariableLogger({ - "mass1": lambda _: scObject.dynManager.getStateObject('linearSpringMassDamperMass1').getState(), - "mass2": lambda _: scObject.dynManager.getStateObject('linearSpringMassDamperMass2').getState(), - "mass3": lambda _: scObject.dynManager.getStateObject('linearSpringMassDamperMass3').getState(), - }) + if testCase == "MassDepletion": + stateLog = pythonVariableLogger.PythonVariableLogger( + { + "mass1": lambda _: scObject.dynManager.getStateObject( + "linearSpringMassDamperMass1" + ).getState(), + "mass2": lambda _: scObject.dynManager.getStateObject( + "linearSpringMassDamperMass2" + ).getState(), + "mass3": lambda _: scObject.dynManager.getStateObject( + "linearSpringMassDamperMass3" + ).getState(), + } + ) unitTestSim.AddModelToTask(unitTaskName, stateLog) unitTestSim.InitializeSimulation() @@ -203,149 +240,219 @@ def fuelSloshTest(show_plots,useFlag,testCase): sigmaRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubSigma) stopTime = 2.5 - if testCase == 'MassDepletion': + if testCase == "MassDepletion": stopTime = 10.0 unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) unitTestSim.ExecuteSimulation() - if testCase == 'MassDepletion': + if testCase == "MassDepletion": fuelMass = dataTank.fuelMass fuelMassDot = dataTank.fuelMassDot mass1Out = unitTestSupport.addTimeColumn(stateLog.times(), stateLog.mass1) mass2Out = unitTestSupport.addTimeColumn(stateLog.times(), stateLog.mass2) mass3Out = unitTestSupport.addTimeColumn(stateLog.times(), stateLog.mass3) - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) - initialOrbAngMom_N = [ - [orbAngMom_N[0,1], orbAngMom_N[0,2], orbAngMom_N[0,3]] - ] + initialOrbAngMom_N = [[orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]]] - finalOrbAngMom = [ - [orbAngMom_N[-1,1], orbAngMom_N[-1,2], orbAngMom_N[-1,3]] - ] + finalOrbAngMom = [[orbAngMom_N[-1, 1], orbAngMom_N[-1, 2], orbAngMom_N[-1, 3]]] - initialRotAngMom_N = [ - [rotAngMom_N[0,1], rotAngMom_N[0,2], rotAngMom_N[0,3]] - ] + initialRotAngMom_N = [[rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]]] - finalRotAngMom = [ - [rotAngMom_N[-1,1], rotAngMom_N[-1,2], rotAngMom_N[-1,3]] - ] + finalRotAngMom = [[rotAngMom_N[-1, 1], rotAngMom_N[-1, 2], rotAngMom_N[-1, 3]]] - initialOrbEnergy = [ - [orbEnergy[0,1]] - ] + initialOrbEnergy = [[orbEnergy[0, 1]]] - finalOrbEnergy = [ - [orbEnergy[-1,1]] - ] + finalOrbEnergy = [[orbEnergy[-1, 1]]] - initialRotEnergy = [ - [rotEnergy[0,1]] - ] + initialRotEnergy = [[rotEnergy[0, 1]]] - finalRotEnergy = [ - [rotEnergy[-1,1]] - ] + finalRotEnergy = [[rotEnergy[-1, 1]]] - plt.close('all') - if testCase != 'MassDepletion': + plt.close("all") + if testCase != "MassDepletion": plt.figure() plt.clf() - plt.plot(orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,1] - orbAngMom_N[0,1])/orbAngMom_N[0,1], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,2] - orbAngMom_N[0,2])/orbAngMom_N[0,2], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,3] - orbAngMom_N[0,3])/orbAngMom_N[0,3]) + plt.plot( + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInOrbitalAngularMomentum" + testCase, "Change in Orbital Angular Momentum " + testCase, plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInOrbitalAngularMomentum" + testCase, + "Change in Orbital Angular Momentum " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(orbEnergy[:,0]*1e-9, (orbEnergy[:,1] - orbEnergy[0,1])/orbEnergy[0,1]) + plt.plot( + orbEnergy[:, 0] * 1e-9, + (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInOrbitalEnergy" + testCase, "Change in Orbital Energy " + testCase, plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInOrbitalEnergy" + testCase, + "Change in Orbital Energy " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,1] - rotAngMom_N[0,1])/rotAngMom_N[0,1], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,2] - rotAngMom_N[0,2])/rotAngMom_N[0,2], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,3] - rotAngMom_N[0,3])/rotAngMom_N[0,3]) + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) / rotAngMom_N[0, 1], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) / rotAngMom_N[0, 2], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) / rotAngMom_N[0, 3], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInRotationalAngularMomentum" + testCase, "Change in Rotational Angular Momentum " + testCase, plt, r"width=0.8\textwidth", path) - if testCase == 'Gravity' or testCase == 'NoGravity': + unitTestSupport.writeFigureLaTeX( + "ChangeInRotationalAngularMomentum" + testCase, + "Change in Rotational Angular Momentum " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) + if testCase == "Gravity" or testCase == "NoGravity": plt.figure() plt.clf() - plt.plot(rotEnergy[:,0]*1e-9, (rotEnergy[:,1] - rotEnergy[0,1])/rotEnergy[0,1]) + plt.plot( + rotEnergy[:, 0] * 1e-9, + (rotEnergy[:, 1] - rotEnergy[0, 1]) / rotEnergy[0, 1], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInRotationalEnergy" + testCase, "Change in Rotational Energy " + testCase, plt, r"width=0.8\textwidth", path) - if testCase == 'MassDepletion': + unitTestSupport.writeFigureLaTeX( + "ChangeInRotationalEnergy" + testCase, + "Change in Rotational Energy " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) + if testCase == "MassDepletion": plt.figure() - plt.plot(dataTank.times()*1e-9, fuelMass) + plt.plot(dataTank.times() * 1e-9, fuelMass) plt.title("Tank Fuel Mass") plt.figure() - plt.plot(dataTank.times()*1e-9, fuelMassDot) + plt.plot(dataTank.times() * 1e-9, fuelMassDot) plt.title("Tank Fuel Mass Dot") plt.figure() - plt.plot(mass1Out[:,0]*1e-9, mass1Out[:,1]) + plt.plot(mass1Out[:, 0] * 1e-9, mass1Out[:, 1]) plt.title("Fuel Particle 1 Mass") plt.figure() - plt.plot(mass2Out[:,0]*1e-9, mass2Out[:,1]) + plt.plot(mass2Out[:, 0] * 1e-9, mass2Out[:, 1]) plt.title("Fuel Particle 2 Mass") plt.figure() - plt.plot(mass3Out[:,0]*1e-9, mass3Out[:,1]) + plt.plot(mass3Out[:, 0] * 1e-9, mass3Out[:, 1]) plt.title("Fuel Particle 3 Mass") mDotFuel = -0.19392039093 - mDotParicle1True = mDotFuel*(10./85.) - mDotParicle2True = mDotFuel*(20./85.) - mDotParicle3True = mDotFuel*(15./85.) - mDotParicle1Data = (mass1Out[2,1] - mass1Out[1,1])/((mass1Out[2,0] - mass1Out[1,0])*1e-9) - mDotParicle2Data = (mass2Out[2,1] - mass2Out[1,1])/((mass2Out[2,0] - mass2Out[1,0])*1e-9) - mDotParicle3Data = (mass3Out[2,1] - mass3Out[1,1])/((mass3Out[2,0] - mass3Out[1,0])*1e-9) + mDotParicle1True = mDotFuel * (10.0 / 85.0) + mDotParicle2True = mDotFuel * (20.0 / 85.0) + mDotParicle3True = mDotFuel * (15.0 / 85.0) + mDotParicle1Data = (mass1Out[2, 1] - mass1Out[1, 1]) / ( + (mass1Out[2, 0] - mass1Out[1, 0]) * 1e-9 + ) + mDotParicle2Data = (mass2Out[2, 1] - mass2Out[1, 1]) / ( + (mass2Out[2, 0] - mass2Out[1, 0]) * 1e-9 + ) + mDotParicle3Data = (mass3Out[2, 1] - mass3Out[1, 1]) / ( + (mass3Out[2, 0] - mass3Out[1, 0]) * 1e-9 + ) if show_plots: plt.show() - plt.close('all') + plt.close("all") - if testCase != 'MassDepletion': + if testCase != "MassDepletion": accuracy = 1e-10 - for i in range(0,len(initialOrbAngMom_N)): + for i in range(0, len(initialOrbAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i],initialOrbAngMom_N[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Linear Spring Mass Damper unit test failed orbital angular momentum unit test") + testMessages.append( + "FAILED: Linear Spring Mass Damper unit test failed orbital angular momentum unit test" + ) - for i in range(0,len(initialRotAngMom_N)): + for i in range(0, len(initialRotAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotAngMom[i],initialRotAngMom_N[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Linear Spring Mass Damper unit test failed rotational angular momentum unit test") + testMessages.append( + "FAILED: Linear Spring Mass Damper unit test failed rotational angular momentum unit test" + ) - if testCase == 'Gravity' or testCase == 'NoGravity': - for i in range(0,len(initialRotEnergy)): + if testCase == "Gravity" or testCase == "NoGravity": + for i in range(0, len(initialRotEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotEnergy[i],initialRotEnergy[i],1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotEnergy[i], initialRotEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Linear Spring Mass Damper unit test failed rotational energy unit test") + testMessages.append( + "FAILED: Linear Spring Mass Damper unit test failed rotational energy unit test" + ) - for i in range(0,len(initialOrbEnergy)): + for i in range(0, len(initialOrbEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i],initialOrbEnergy[i],1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Linear Spring Mass Damper unit test failed orbital energy unit test") + testMessages.append( + "FAILED: Linear Spring Mass Damper unit test failed orbital energy unit test" + ) - if testCase == 'MassDepletion': + if testCase == "MassDepletion": accuracy = 1e-4 - if not unitTestSupport.isDoubleEqual(mDotParicle1Data,mDotParicle1True,accuracy): + if not unitTestSupport.isDoubleEqual( + mDotParicle1Data, mDotParicle1True, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Linear Spring Mass Damper unit test failed mass 1 dot test") - if not unitTestSupport.isDoubleEqual(mDotParicle2Data,mDotParicle2True,accuracy): + testMessages.append( + "FAILED: Linear Spring Mass Damper unit test failed mass 1 dot test" + ) + if not unitTestSupport.isDoubleEqual( + mDotParicle2Data, mDotParicle2True, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Linear Spring Mass Damper unit test failed mass 2 dot test") - if not unitTestSupport.isDoubleEqual(mDotParicle3Data,mDotParicle3True,accuracy): + testMessages.append( + "FAILED: Linear Spring Mass Damper unit test failed mass 2 dot test" + ) + if not unitTestSupport.isDoubleEqual( + mDotParicle3Data, mDotParicle3True, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Linear Spring Mass Damper unit test failed mass 3 dot test") + testMessages.append( + "FAILED: Linear Spring Mass Damper unit test failed mass 3 dot test" + ) if testFailCount == 0: print("PASSED: " + " Linear Spring Mass Damper Test") @@ -353,7 +460,8 @@ def fuelSloshTest(show_plots,useFlag,testCase): assert testFailCount < 1, testMessages # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": - fuelSloshTest(True,False,'Gravity') + fuelSloshTest(True, False, "Gravity") diff --git a/src/simulation/dynamics/MtbEffector/_UnitTest/test_MtbEffector.py b/src/simulation/dynamics/MtbEffector/_UnitTest/test_MtbEffector.py index cb9f54f80a..55e5f3b846 100644 --- a/src/simulation/dynamics/MtbEffector/_UnitTest/test_MtbEffector.py +++ b/src/simulation/dynamics/MtbEffector/_UnitTest/test_MtbEffector.py @@ -22,13 +22,19 @@ import matplotlib.pyplot as plt import numpy as np import pytest + # The path to the location of Basilisk # Used to get the location of supporting data. from Basilisk import __path__ from Basilisk.architecture import messaging from Basilisk.simulation import MtbEffector from Basilisk.simulation import spacecraft, magneticFieldWMM -from Basilisk.utilities import SimulationBaseClass, simIncludeGravBody, orbitalMotion, RigidBodyKinematics +from Basilisk.utilities import ( + SimulationBaseClass, + simIncludeGravBody, + orbitalMotion, + RigidBodyKinematics, +) from Basilisk.utilities import macros from Basilisk.utilities import unitTestSupport @@ -37,8 +43,7 @@ def truthMagneticTorque(bField_N, sigmaBN, mtbCmds, GtMatrix, numMTB, maxDipole): - - temp = np.asarray(GtMatrix[0:3*numMTB]) + temp = np.asarray(GtMatrix[0 : 3 * numMTB]) GtMatrix = np.reshape(temp, (3, numMTB)) bField_N = np.asarray(bField_N) BN = RigidBodyKinematics.MRP2C(sigmaBN) @@ -51,12 +56,12 @@ def truthMagneticTorque(bField_N, sigmaBN, mtbCmds, GtMatrix, numMTB, maxDipole) mtbCmds = np.asarray(mtbCmds[0:numMTB]) bTilde = np.asarray(RigidBodyKinematics.v3Tilde(bField_B)) - tauNet = - bTilde @ GtMatrix @ mtbCmds + tauNet = -bTilde @ GtMatrix @ mtbCmds return tauNet @pytest.mark.parametrize("accuracy", [1e-12]) -@pytest.mark.parametrize("maxDipole", [10., 0.1]) +@pytest.mark.parametrize("maxDipole", [10.0, 0.1]) def test_MtbEffector(show_plots, accuracy, maxDipole): r""" **Validation Test Description** @@ -76,20 +81,21 @@ def test_MtbEffector(show_plots, accuracy, maxDipole): Here discuss what variables and states are being checked. """ - [testResults, testMessage] = MtbEffectorTestFunction(show_plots, accuracy, maxDipole) + [testResults, testMessage] = MtbEffectorTestFunction( + show_plots, accuracy, maxDipole + ) assert testResults < 1, testMessage def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): """Call this routine directly to run the unit test.""" - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages # create simulation variable names simTaskName = "simTask" simProcessName = "simProcess" - # create a sim module as an empty container scSim = SimulationBaseClass.SimBaseClass() @@ -97,37 +103,37 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(1.) + simulationTimeStep = macros.sec2nano(1.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) - simTime = 3. + simTime = 3.0 simulationTime = macros.sec2nano(simTime) - # create Earth Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() earth = gravFactory.createEarth() earth.isCentralBody = True # ensure this is the central gravitational body mu = earth.mu - # initialize spacecraft object and set properties scObject = spacecraft.Spacecraft() scObject.ModelTag = "bskTestSat" - I = [10.5, 0., 0., - 0., 8., 0., - 0., 0., 6.75] + I = [10.5, 0.0, 0.0, 0.0, 8.0, 0.0, 0.0, 0.0, 6.75] scObject.hub.mHub = 10.0 # kg - spacecraft mass (arbitrary) - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM + scObject.hub.r_BcB_B = [ + [0.0], + [0.0], + [0.0], + ] # m - position vector of body-fixed point B relative to CM scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) oe = orbitalMotion.ClassicElements() - oe.a = 6778.14 * 1000. # meters + oe.a = 6778.14 * 1000.0 # meters oe.e = 0.0 - oe.i = 45. * macros.D2R - oe.Omega = 60. * macros.D2R - oe.omega = 0. * macros.D2R - oe.f = 0. * macros.D2R + oe.i = 45.0 * macros.D2R + oe.Omega = 60.0 * macros.D2R + oe.omega = 0.0 * macros.D2R + oe.f = 0.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m/s - v_CN_N @@ -136,39 +142,37 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): # add spacecraft object scSim.AddModelToTask(simTaskName, scObject) - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) - + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # add magnetic field module magModule = magneticFieldWMM.MagneticFieldWMM() magModule.ModelTag = "WMM" - magModule.dataPath = bskPath + '/supportData/MagneticField/' - epochMsg = unitTestSupport.timeStringToGregorianUTCMsg('2020 May 12, 00:00:0.0 (UTC)') + magModule.dataPath = bskPath + "/supportData/MagneticField/" + epochMsg = unitTestSupport.timeStringToGregorianUTCMsg( + "2020 May 12, 00:00:0.0 (UTC)" + ) magModule.epochInMsg.subscribeTo(epochMsg) - magModule.addSpacecraftToModel(scObject.scStateOutMsg) # this command can be repeated if multiple + magModule.addSpacecraftToModel( + scObject.scStateOutMsg + ) # this command can be repeated if multiple scSim.AddModelToTask(simTaskName, magModule) - # add magnetic torque bar effector mtbEff = MtbEffector.MtbEffector() mtbEff.ModelTag = "MtbEff" scObject.addDynamicEffector(mtbEff) scSim.AddModelToTask(simTaskName, mtbEff) - # mtbConfigData message mtbConfigParams = messaging.MTBArrayConfigMsgPayload() mtbConfigParams.numMTB = 3 # row major toque bar alignments - mtbConfigParams.GtMatrix_B = [ - 1., 0., 0., - 0., 1., 0., - 0., 0., 1. - ] - mtbConfigParams.maxMtbDipoles = [maxDipole]*4 + mtbConfigParams.GtMatrix_B = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + mtbConfigParams.maxMtbDipoles = [maxDipole] * 4 mtbParamsInMsg = messaging.MTBArrayConfigMsg().write(mtbConfigParams) - # MTBCmdMsgPayload, mtbCmdInMsg mtbCmdInMsgContainer = messaging.MTBCmdMsgPayload() mtbCmdInMsgContainer.mtbDipoleCmds = [0.2, 0.2, 0.2] @@ -179,10 +183,11 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): mtbEff.mtbParamsInMsg.subscribeTo(mtbParamsInMsg) mtbEff.magInMsg.subscribeTo(magModule.envOutMsgs[0]) - # Setup data logging before the simulation is initialized numDataPoints = 3600 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) dataLogMag = magModule.envOutMsgs[0].recorder(samplingTime) dataLogMTB = mtbEff.mtbOutMsg.recorder(samplingTime) @@ -191,7 +196,6 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): scSim.AddModelToTask(simTaskName, dataLogMag) scSim.AddModelToTask(simTaskName, dataLog) - # initialize Simulation scSim.InitializeSimulation() @@ -210,32 +214,41 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): plt.close("all") # clears out plots from earlier test runs plt.figure(1) for idx in range(0, 3): - plt.plot(dataLogMTB.times() * macros.NANO2SEC, mtbData[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\tau_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [s]') - plt.ylabel(r'MTB Net Torque $\tau_{B}$ [Nm]') + plt.plot( + dataLogMTB.times() * macros.NANO2SEC, + mtbData[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\tau_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [s]") + plt.ylabel(r"MTB Net Torque $\tau_{B}$ [Nm]") # plot magnetic field data in the Inertial frame plt.figure(2) for idx in range(3): - plt.plot(dataLogMag.times() * macros.NANO2SEC, dataMagField[:, idx] * 1e9, - color=unitTestSupport.getLineColor(idx, 3), - label=r'$B\_N_{' + str(idx) + '}$') - plt.legend(loc='lower right') - plt.xlabel('Time [s]') - plt.ylabel('Magnetic Field [nT]') + plt.plot( + dataLogMag.times() * macros.NANO2SEC, + dataMagField[:, idx] * 1e9, + color=unitTestSupport.getLineColor(idx, 3), + label=r"$B\_N_{" + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [s]") + plt.ylabel("Magnetic Field [nT]") # plot the Body relative to Inertial attitude plt.figure(3) for idx in range(0, 3): - plt.plot(dataLog.times() * macros.NANO2SEC, attData[:, idx], - color=unitTestSupport.getLineColor(idx, 3), - label=r'$\sigma_' + str(idx) + '$') - plt.legend(loc='lower right') - plt.xlabel('Time [s]') - plt.ylabel(r'MRP Attitude $\sigma_{B/N}$') + plt.plot( + dataLog.times() * macros.NANO2SEC, + attData[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r"$\sigma_" + str(idx) + "$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [s]") + plt.ylabel(r"MRP Attitude $\sigma_{B/N}$") plt.show() @@ -243,17 +256,18 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): # use mtbData[1:, :] since mtbData is the torque from prev iterations for sV, mtbTauV, bV in zip(attData[1:, :], mtbData[1:, :], dataMagField): - mtbTauTruth = truthMagneticTorque(bV, sV, mtbCmdInMsgContainer.mtbDipoleCmds, - mtbConfigParams.GtMatrix_B, - mtbConfigParams.numMTB, - maxDipole - ) - - testFailCount, testMessages = unitTestSupport.compareVector(mtbTauV, - mtbTauTruth, - accuracy, - "mtbTorque_B", - testFailCount, testMessages) + mtbTauTruth = truthMagneticTorque( + bV, + sV, + mtbCmdInMsgContainer.mtbDipoleCmds, + mtbConfigParams.GtMatrix_B, + mtbConfigParams.numMTB, + maxDipole, + ) + + testFailCount, testMessages = unitTestSupport.compareVector( + mtbTauV, mtbTauTruth, accuracy, "mtbTorque_B", testFailCount, testMessages + ) if testFailCount == 0: print("PASSED: Mtb Effector") @@ -261,9 +275,7 @@ def MtbEffectorTestFunction(show_plots, accuracy, maxDipole): print("Failed: Mtb Effector") return testFailCount, testMessages + + if __name__ == "__main__": - test_MtbEffector( - False, - 1e-12, - 0.1 - ) + test_MtbEffector(False, 1e-12, 0.1) diff --git a/src/simulation/dynamics/NHingedRigidBodies/_UnitTest/test_nHingedRigidBodyStateEffector.py b/src/simulation/dynamics/NHingedRigidBodies/_UnitTest/test_nHingedRigidBodyStateEffector.py index 0b883744e3..d4cef37816 100644 --- a/src/simulation/dynamics/NHingedRigidBodies/_UnitTest/test_nHingedRigidBodyStateEffector.py +++ b/src/simulation/dynamics/NHingedRigidBodies/_UnitTest/test_nHingedRigidBodyStateEffector.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -26,9 +25,9 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('SimCode') -sys.path.append(splitPath[0] + '/modules') -sys.path.append(splitPath[0] + '/PythonModules') +splitPath = path.split("SimCode") +sys.path.append(splitPath[0] + "/modules") +sys.path.append(splitPath[0] + "/PythonModules") from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import unitTestSupport @@ -39,10 +38,7 @@ from Basilisk.utilities import pythonVariableLogger -@pytest.mark.parametrize("testCase", [ - ('NoGravity'), - ('Gravity') -]) +@pytest.mark.parametrize("testCase", [("NoGravity"), ("Gravity")]) # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed # @pytest.mark.skipif(conditionstring) @@ -86,6 +82,7 @@ def test_nHingedRigidBodyAllTest(show_plots, testCase): [testResults, testMessage] = nHingedRigidBody(show_plots, testCase) assert testResults < 1, testMessage + def nHingedRigidBody(show_plots, testCase): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the @@ -110,8 +107,12 @@ def nHingedRigidBody(show_plots, testCase): testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - unitTestSim.effector1 = nHingedRigidBodyStateEffector.NHingedRigidBodyStateEffector() - unitTestSim.effector2 = nHingedRigidBodyStateEffector.NHingedRigidBodyStateEffector() + unitTestSim.effector1 = ( + nHingedRigidBodyStateEffector.NHingedRigidBodyStateEffector() + ) + unitTestSim.effector2 = ( + nHingedRigidBodyStateEffector.NHingedRigidBodyStateEffector() + ) unitTestSim.panel = nHingedRigidBodyStateEffector.HingedPanel() unitTestSim.effector1.r_HB_B = [[0.5], [0.0], [1.0]] @@ -126,7 +127,7 @@ def nHingedRigidBody(show_plots, testCase): unitTestSim.panel.d = 0.75 unitTestSim.panel.k = 500.0 unitTestSim.panel.c = 0.0 - unitTestSim.panel.thetaInit = 5*numpy.pi/180.0 + unitTestSim.panel.thetaInit = 5 * numpy.pi / 180.0 unitTestSim.panel.thetaDotInit = 0.0 unitTestSim.panel.theta_0 = 0.0 @@ -158,25 +159,43 @@ def nHingedRigidBody(show_plots, testCase): # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, scObject) - if testCase == 'Gravity': + if testCase == "Gravity": unitTestSim.earthGravBody = gravityEffector.GravBodyData() unitTestSim.earthGravBody.planetName = "earth_planet_data" - unitTestSim.earthGravBody.mu = 0.3986004415E+15 # meters! + unitTestSim.earthGravBody.mu = 0.3986004415e15 # meters! unitTestSim.earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([unitTestSim.earthGravBody]) - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + [unitTestSim.earthGravBody] + ) + scObject.hub.r_CN_NInit = [ + [-4020338.690396649], + [7490566.741852513], + [5248299.211589362], + ] + scObject.hub.v_CN_NInit = [ + [-5199.77710904224], + [-3436.681645356935], + [1041.576797498721], + ] dataLog = scObject.scStateOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) - scObjectLog = scObject.logger(["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) - stateLog = pythonVariableLogger.PythonVariableLogger({ - "theta1": lambda _: scObject.dynManager.getStateObject(f'nHingedRigidBody1Theta').getState(), - "theta2": lambda _: scObject.dynManager.getStateObject(f'nHingedRigidBody2Theta').getState(), - }) + stateLog = pythonVariableLogger.PythonVariableLogger( + { + "theta1": lambda _: scObject.dynManager.getStateObject( + f"nHingedRigidBody1Theta" + ).getState(), + "theta2": lambda _: scObject.dynManager.getStateObject( + f"nHingedRigidBody2Theta" + ).getState(), + } + ) unitTestSim.AddModelToTask(unitTaskName, stateLog) unitTestSim.InitializeSimulation() @@ -185,13 +204,25 @@ def nHingedRigidBody(show_plots, testCase): unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) unitTestSim.ExecuteSimulation() - nHingedRigidBody1ThetasOut = unitTestSupport.addTimeColumn(stateLog.times(), stateLog.theta1) - nHingedRigidBody2ThetasOut = unitTestSupport.addTimeColumn(stateLog.times(), stateLog.theta2) - - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) + nHingedRigidBody1ThetasOut = unitTestSupport.addTimeColumn( + stateLog.times(), stateLog.theta1 + ) + nHingedRigidBody2ThetasOut = unitTestSupport.addTimeColumn( + stateLog.times(), stateLog.theta2 + ) + + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) initialOrbAngMom_N = [[orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]]] @@ -213,70 +244,116 @@ def nHingedRigidBody(show_plots, testCase): plt.figure() plt.clf() - plt.plot(orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,1] - orbAngMom_N[0,1])/orbAngMom_N[0,1], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,2] - orbAngMom_N[0,2])/orbAngMom_N[0,2], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,3] - orbAngMom_N[0,3])/orbAngMom_N[0,3]) + plt.plot( + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") plt.suptitle("Change in Orbital Angular Momentum ") plt.figure() plt.clf() - plt.plot(orbEnergy[:,0]*1e-9, (orbEnergy[:,1] - orbEnergy[0,1])/orbEnergy[0,1]) + plt.plot( + orbEnergy[:, 0] * 1e-9, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1] + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") plt.suptitle("Change in Orbital Energy ") plt.figure() plt.clf() - plt.plot(rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,1] - rotAngMom_N[0,1])/rotAngMom_N[0,1], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,2] - rotAngMom_N[0,2])/rotAngMom_N[0,2], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,3] - rotAngMom_N[0,3])/rotAngMom_N[0,3]) + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) / rotAngMom_N[0, 1], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) / rotAngMom_N[0, 2], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) / rotAngMom_N[0, 3], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") plt.suptitle("Change in Rotational Angular Momentum") plt.figure() plt.clf() - plt.plot(rotEnergy[:,0]*1e-9, (rotEnergy[:,1] - rotEnergy[0,1])/rotEnergy[0,1]) + plt.plot( + rotEnergy[:, 0] * 1e-9, (rotEnergy[:, 1] - rotEnergy[0, 1]) / rotEnergy[0, 1] + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") plt.suptitle("Change in Rotational Energy") plt.figure() plt.clf() - plt.plot(nHingedRigidBody1ThetasOut[:,0]*1e-9, nHingedRigidBody1ThetasOut[:,1]*180/numpy.pi,'-b') - plt.xlabel('Time (s)') - plt.ylabel('Panel 1 Theta 1 (deg)') + plt.plot( + nHingedRigidBody1ThetasOut[:, 0] * 1e-9, + nHingedRigidBody1ThetasOut[:, 1] * 180 / numpy.pi, + "-b", + ) + plt.xlabel("Time (s)") + plt.ylabel("Panel 1 Theta 1 (deg)") plt.figure() plt.clf() - plt.plot(nHingedRigidBody1ThetasOut[:,0]*1e-9, nHingedRigidBody1ThetasOut[:,2]*180/numpy.pi,'-b') - plt.xlabel('Time (s)') - plt.ylabel('Panel 1 Theta 2 (deg)') + plt.plot( + nHingedRigidBody1ThetasOut[:, 0] * 1e-9, + nHingedRigidBody1ThetasOut[:, 2] * 180 / numpy.pi, + "-b", + ) + plt.xlabel("Time (s)") + plt.ylabel("Panel 1 Theta 2 (deg)") plt.figure() plt.clf() - plt.plot(nHingedRigidBody1ThetasOut[:,0]*1e-9, nHingedRigidBody1ThetasOut[:,3]*180/numpy.pi,'-b') - plt.xlabel('Time (s)') - plt.ylabel('Panel 1 Theta 3 (deg)') + plt.plot( + nHingedRigidBody1ThetasOut[:, 0] * 1e-9, + nHingedRigidBody1ThetasOut[:, 3] * 180 / numpy.pi, + "-b", + ) + plt.xlabel("Time (s)") + plt.ylabel("Panel 1 Theta 3 (deg)") plt.figure() plt.clf() - plt.plot(nHingedRigidBody1ThetasOut[:,0]*1e-9, nHingedRigidBody1ThetasOut[:,4]*180/numpy.pi,'-b') - plt.xlabel('Time (s)') - plt.ylabel('Panel 1 Theta 4 (deg)') + plt.plot( + nHingedRigidBody1ThetasOut[:, 0] * 1e-9, + nHingedRigidBody1ThetasOut[:, 4] * 180 / numpy.pi, + "-b", + ) + plt.xlabel("Time (s)") + plt.ylabel("Panel 1 Theta 4 (deg)") plt.figure() plt.clf() - plt.plot(nHingedRigidBody2ThetasOut[:,0]*1e-9, nHingedRigidBody2ThetasOut[:,1]*180/numpy.pi,'-b') - plt.xlabel('Time (s)') - plt.ylabel('Panel 2 Theta 1 (deg)') + plt.plot( + nHingedRigidBody2ThetasOut[:, 0] * 1e-9, + nHingedRigidBody2ThetasOut[:, 1] * 180 / numpy.pi, + "-b", + ) + plt.xlabel("Time (s)") + plt.ylabel("Panel 2 Theta 1 (deg)") plt.figure() plt.clf() - plt.plot(nHingedRigidBody2ThetasOut[:,0]*1e-9, nHingedRigidBody2ThetasOut[:,2]*180/numpy.pi,'-b') - plt.xlabel('Time (s)') - plt.ylabel('Panel 2 Theta 2 (deg)') + plt.plot( + nHingedRigidBody2ThetasOut[:, 0] * 1e-9, + nHingedRigidBody2ThetasOut[:, 2] * 180 / numpy.pi, + "-b", + ) + plt.xlabel("Time (s)") + plt.ylabel("Panel 2 Theta 2 (deg)") plt.figure() plt.clf() - plt.plot(nHingedRigidBody2ThetasOut[:,0]*1e-9, nHingedRigidBody2ThetasOut[:,3]*180/numpy.pi,'-b') - plt.xlabel('Time (s)') - plt.ylabel('Panel 2 Theta 3 (deg)') + plt.plot( + nHingedRigidBody2ThetasOut[:, 0] * 1e-9, + nHingedRigidBody2ThetasOut[:, 3] * 180 / numpy.pi, + "-b", + ) + plt.xlabel("Time (s)") + plt.ylabel("Panel 2 Theta 3 (deg)") if show_plots: plt.show() @@ -291,38 +368,53 @@ def nHingedRigidBody(show_plots, testCase): for i in range(0, len(initialOrbAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: N Hinged Rigid Body integrated test failed orbital angular momentum unit test") + testMessages.append( + "FAILED: N Hinged Rigid Body integrated test failed orbital angular momentum unit test" + ) for i in range(0, len(initialRotAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: N Hinged Rigid Body integrated test failed rotational angular momentum unit test") + "FAILED: N Hinged Rigid Body integrated test failed rotational angular momentum unit test" + ) for i in range(0, len(initialRotEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotEnergy[i], initialRotEnergy[i], 1, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotEnergy[i], initialRotEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: N Hinged Rigid Body integrated test failed rotational energy unit test") + testMessages.append( + "FAILED: N Hinged Rigid Body integrated test failed rotational energy unit test" + ) for i in range(0, len(initialOrbEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: N Hinged Rigid Body integrated test failed orbital energy unit test") + testMessages.append( + "FAILED: N Hinged Rigid Body integrated test failed orbital energy unit test" + ) if testFailCount == 0: print("PASSED: " + " N Hinged Rigid Body integrated test") print("Error tolerance for all tests was" + str(accuracy)) - assert testFailCount < 1, testMessages # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": - nHingedRigidBody(True, "Gravity") + nHingedRigidBody(True, "Gravity") diff --git a/src/simulation/dynamics/RadiationPressure/_UnitTest/Support/makeSphereXML.py b/src/simulation/dynamics/RadiationPressure/_UnitTest/Support/makeSphereXML.py index 814a130749..5c2c3b067b 100644 --- a/src/simulation/dynamics/RadiationPressure/_UnitTest/Support/makeSphereXML.py +++ b/src/simulation/dynamics/RadiationPressure/_UnitTest/Support/makeSphereXML.py @@ -34,20 +34,24 @@ # from mpl_toolkits.mplot3d import Axes3D # import matplotlib.pyplot as plt -class unitVectorXYZ(): + +class unitVectorXYZ: def __init__(self, x, y, z): self.x = x self.y = y self.z = z + numPoints = 64 -radius = 1. # radius of unit vector sphere -theta = np.linspace(0., 2.*np.pi, numPoints, endpoint=False) # angle about third axis -phi = np.linspace(-np.pi, np.pi, numPoints, endpoint=False) # elevation from x-y plane -xVec = np.zeros(len(theta)*len(phi)) # x component of unit vector -yVec = np.zeros(len(theta)*len(phi)) # y component of unit vector -zVec = np.zeros(len(theta)*len(phi)) # z component of unit vector -sHat_B = unitVectorXYZ(xVec,yVec,zVec) +radius = 1.0 # radius of unit vector sphere +theta = np.linspace( + 0.0, 2.0 * np.pi, numPoints, endpoint=False +) # angle about third axis +phi = np.linspace(-np.pi, np.pi, numPoints, endpoint=False) # elevation from x-y plane +xVec = np.zeros(len(theta) * len(phi)) # x component of unit vector +yVec = np.zeros(len(theta) * len(phi)) # y component of unit vector +zVec = np.zeros(len(theta) * len(phi)) # z component of unit vector +sHat_B = unitVectorXYZ(xVec, yVec, zVec) # fig = plt.figure() # ax = fig.add_subplot(111, projection='3d') @@ -58,66 +62,66 @@ def __init__(self, x, y, z): x = radius * np.cos(theta[i]) * np.sin(phi[j]) y = radius * np.sin(theta[i]) * np.sin(phi[j]) z = radius * np.cos(phi[j]) - sHat_B.x[numPoints*i+j] = x - sHat_B.y[numPoints*i+j] = y - sHat_B.z[numPoints*i+j] = z + sHat_B.x[numPoints * i + j] = x + sHat_B.y[numPoints * i + j] = y + sHat_B.z[numPoints * i + j] = z # ax.scatter(sHat_B.x[numPoints*i+j], sHat_B.y[numPoints*i+j], sHat_B.z[numPoints*i+j]) # plt.show() -lookupFile = open('cannonballLookup2.xml', 'w') +lookupFile = open("cannonballLookup2.xml", "w") -nl = '\n' -singleTab = ' ' -doubleTab = ' ' -tripleTab = ' ' +nl = "\n" +singleTab = " " +doubleTab = " " +tripleTab = " " header = '' -srp_values = '' -sHat_B_values = ' ' -lines =header,nl,srp_values,nl,sHat_B_values,nl +srp_values = "" +sHat_B_values = " " +lines = header, nl, srp_values, nl, sHat_B_values, nl lookupFile.writelines(lines) for i in range(len(sHat_B.x)): - top =' ' - value1 = ' ' + '{:1.16f}'.format(sHat_B.x[i]) + '' - value2 = ' ' + '{:1.16f}'.format(sHat_B.y[i]) + '' - value3 = ' ' + '{:1.16f}'.format(sHat_B.z[i]) + '' - bottom = ' ' - lines = top,nl,value1,nl,value2,nl,value3,nl,bottom,nl + top = ' ' + value1 = " " + "{:1.16f}".format(sHat_B.x[i]) + "" + value2 = " " + "{:1.16f}".format(sHat_B.y[i]) + "" + value3 = " " + "{:1.16f}".format(sHat_B.z[i]) + "" + bottom = " " + lines = top, nl, value1, nl, value2, nl, value3, nl, bottom, nl lookupFile.writelines(lines) -endSHat_B_values =' ' +endSHat_B_values = " " -startForce_B_values = ' ' +startForce_B_values = " " lines = endSHat_B_values, nl, startForce_B_values, nl lookupFile.writelines(lines) for i in range(len(sHat_B.x)): - top =' ' - value1 = ' ' + '{:1.16f}'.format(-sHat_B.x[i]) + '' - value2 = ' ' + '{:1.16f}'.format(-sHat_B.y[i]) + '' - value3 = ' ' + '{:1.16f}'.format(-sHat_B.z[i]) + '' - bottom = ' ' - lines = top,nl,value1,nl,value2,nl,value3,nl,bottom,nl + top = ' ' + value1 = " " + "{:1.16f}".format(-sHat_B.x[i]) + "" + value2 = " " + "{:1.16f}".format(-sHat_B.y[i]) + "" + value3 = " " + "{:1.16f}".format(-sHat_B.z[i]) + "" + bottom = " " + lines = top, nl, value1, nl, value2, nl, value3, nl, bottom, nl lookupFile.writelines(lines) -endForce_values =' ' +endForce_values = " " -startTorqueValues = ' ' +startTorqueValues = " " lines = endForce_values, nl, startTorqueValues, nl lookupFile.writelines(lines) -torqueValue = .0000000000000000 +torqueValue = 0.0000000000000000 for i in range(len(sHat_B.x)): - top =' ' - value1 = ' ' + '{:1.16f}'.format(torqueValue) + '' - value2 = ' ' + '{:1.16f}'.format(torqueValue) + '' - value3 = ' ' + '{:1.16f}'.format(torqueValue) + '' - bottom = ' ' - lines = top,nl,value1,nl,value2,nl,value3,nl,bottom,nl + top = ' ' + value1 = " " + "{:1.16f}".format(torqueValue) + "" + value2 = " " + "{:1.16f}".format(torqueValue) + "" + value3 = " " + "{:1.16f}".format(torqueValue) + "" + bottom = " " + lines = top, nl, value1, nl, value2, nl, value3, nl, bottom, nl lookupFile.writelines(lines) -endTorqueValues = ' ' -endFile = '' +endTorqueValues = " " +endFile = "" lines = endTorqueValues, nl, endFile lookupFile.writelines(lines) diff --git a/src/simulation/dynamics/RadiationPressure/_UnitTest/test_radiationPressure.py b/src/simulation/dynamics/RadiationPressure/_UnitTest/test_radiationPressure.py index 26abe46850..5ae53da0f9 100644 --- a/src/simulation/dynamics/RadiationPressure/_UnitTest/test_radiationPressure.py +++ b/src/simulation/dynamics/RadiationPressure/_UnitTest/test_radiationPressure.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,7 +15,6 @@ # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - # # RadiationPressure Unit Test # @@ -35,11 +33,10 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') - +splitPath = path.split("simulation") -#Import all of the modules that we are going to call in this simulation +# Import all of the modules that we are going to call in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import unitTestSupport from Basilisk.simulation import radiationPressure @@ -48,14 +45,18 @@ from Basilisk.simulation import spacecraft from Basilisk.architecture import messaging + # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(True) -@pytest.mark.parametrize("modelType, eclipseOn", [ - ("cannonball",False) - , ("lookup", False) - , ("lookup", True) - , ("cannonballLookup", False) -]) +@pytest.mark.parametrize( + "modelType, eclipseOn", + [ + ("cannonball", False), + ("lookup", False), + ("lookup", True), + ("cannonballLookup", False), + ], +) def test_unitRadiationPressure(show_plots, modelType, eclipseOn): """Module Unit Test""" [testResults, testMessage] = unitRadiationPressure(show_plots, modelType, eclipseOn) @@ -88,7 +89,6 @@ def unitRadiationPressure(show_plots, modelType, eclipseOn): scObject.ModelTag = "spacecraft" unitTestSim.AddModelToTask(testTaskName, scObject) - srpDynEffector = radiationPressure.RadiationPressure() srpDynEffector.ModelTag = "RadiationPressure" srpDynEffector2 = radiationPressure.RadiationPressure() @@ -117,11 +117,15 @@ def unitRadiationPressure(show_plots, modelType, eclipseOn): srpDynEffector.addTorqueLookupBEntry(handler.torqueBLookup[i, :]) srpDynEffector.addSHatLookupBEntry(handler.sHatBLookup[i, :]) srpDynEffector2.setUseCannonballModel() - srpDynEffector2.area = 182018.072141393 #set to give a force of 1N at 1AU to make spherical table generation easy + srpDynEffector2.area = 182018.072141393 # set to give a force of 1N at 1AU to make spherical table generation easy srpDynEffector2.coefficientReflection = 1.2 - r_N = [np.sin(np.pi/4.)*np.cos(np.pi/4.)*10.*om.AU*1000., np.sin(np.pi/4.)*np.sin(np.pi/4.)*10.*om.AU*1000., np.cos(np.pi/4.)*10.*om.AU*1000.] # [m] - sun_r_N = [0., 0., 0.] # [m] - sigma_BN = [0., 0., 0.] + r_N = [ + np.sin(np.pi / 4.0) * np.cos(np.pi / 4.0) * 10.0 * om.AU * 1000.0, + np.sin(np.pi / 4.0) * np.sin(np.pi / 4.0) * 10.0 * om.AU * 1000.0, + np.cos(np.pi / 4.0) * 10.0 * om.AU * 1000.0, + ] # [m] + sun_r_N = [0.0, 0.0, 0.0] # [m] + sigma_BN = [0.0, 0.0, 0.0] if eclipseOn: sunEclipseMsgData = messaging.EclipseMsgPayload() @@ -136,7 +140,6 @@ def unitRadiationPressure(show_plots, modelType, eclipseOn): scObject.hub.r_CN_NInit = r_N scObject.hub.sigma_BNInit = sigma_BN - sunSpiceMsg = messaging.SpicePlanetStateMsgPayload() sunSpiceMsg.PositionVector = sun_r_N sunMsg = messaging.SpicePlanetStateMsg().write(sunSpiceMsg) @@ -159,117 +162,187 @@ def unitRadiationPressure(show_plots, modelType, eclipseOn): srpDynEffector2.computeForceTorque(unitTestSim.TotalSim.CurrentNanos, testTaskRate) unitTestSim.TotalSim.SingleStepProcesses() - srpDataForce_B = unitTestSupport.addTimeColumn(srpDynEffectorLog[0].times(), srpDynEffectorLog[0].forceExternal_B) - srpDataForce_N = unitTestSupport.addTimeColumn(srpDynEffectorLog[0].times(), srpDynEffectorLog[0].forceExternal_N) - srpTorqueData = unitTestSupport.addTimeColumn(srpDynEffectorLog[0].times(), srpDynEffectorLog[0].torqueExternalPntB_B) - - srp2DataForce_B = unitTestSupport.addTimeColumn(srpDynEffectorLog[1].times(), srpDynEffectorLog[1].forceExternal_B) - srp2DataForce_N = unitTestSupport.addTimeColumn(srpDynEffectorLog[1].times(), srpDynEffectorLog[1].forceExternal_N) - srp2TorqueData = unitTestSupport.addTimeColumn(srpDynEffectorLog[1].times(), srpDynEffectorLog[1].torqueExternalPntB_B) - - errTol = 1E-12 + srpDataForce_B = unitTestSupport.addTimeColumn( + srpDynEffectorLog[0].times(), srpDynEffectorLog[0].forceExternal_B + ) + srpDataForce_N = unitTestSupport.addTimeColumn( + srpDynEffectorLog[0].times(), srpDynEffectorLog[0].forceExternal_N + ) + srpTorqueData = unitTestSupport.addTimeColumn( + srpDynEffectorLog[0].times(), srpDynEffectorLog[0].torqueExternalPntB_B + ) + + srp2DataForce_B = unitTestSupport.addTimeColumn( + srpDynEffectorLog[1].times(), srpDynEffectorLog[1].forceExternal_B + ) + srp2DataForce_N = unitTestSupport.addTimeColumn( + srpDynEffectorLog[1].times(), srpDynEffectorLog[1].forceExternal_N + ) + srp2TorqueData = unitTestSupport.addTimeColumn( + srpDynEffectorLog[1].times(), srpDynEffectorLog[1].torqueExternalPntB_B + ) + + errTol = 1e-12 if modelType == "cannonball": truthForceExternal_B = [0, 0, 0] - truthForceExternal_N = [-2.44694525395e-06, -1.94212316004e-05, -8.42121070088e-06] + truthForceExternal_N = [ + -2.44694525395e-06, + -1.94212316004e-05, + -8.42121070088e-06, + ] truthTorqueExternalPntB_B = [0, 0, 0] - testFailCount, testMessages = unitTestSupport.compareVector(truthForceExternal_B, - srpDataForce_B[1,1:], - errTol, - "Force_B", - testFailCount, - testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthForceExternal_N, - srpDataForce_N[1, 1:], - errTol, - "Force_N", - testFailCount, - testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthTorqueExternalPntB_B, - srpTorqueData[1, 1:], - errTol, - "Torque", - testFailCount, - testMessages) + testFailCount, testMessages = unitTestSupport.compareVector( + truthForceExternal_B, + srpDataForce_B[1, 1:], + errTol, + "Force_B", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthForceExternal_N, + srpDataForce_N[1, 1:], + errTol, + "Force_N", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthTorqueExternalPntB_B, + srpTorqueData[1, 1:], + errTol, + "Torque", + testFailCount, + testMessages, + ) if modelType == "lookup": - errTolTorque = errTol/100 - truthForceExternal_B = [0.26720220706099184E-04, - 0.13596079145805012E-04, 0.93948649829282319E-05] + errTolTorque = errTol / 100 + truthForceExternal_B = [ + 0.26720220706099184e-04, + -0.13596079145805012e-04, + 0.93948649829282319e-05, + ] truthForceExternal_N = [0, 0, 0] - truthTorqueExternalPntB_B = [-0.80492463017846114E-12, 0.50888380426172319E-12, 0.10249431804585393E-11] + truthTorqueExternalPntB_B = [ + -0.80492463017846114e-12, + 0.50888380426172319e-12, + 0.10249431804585393e-11, + ] if eclipseOn: - truthForceExternal_B = sunEclipseMsgData.shadowFactor*np.array(truthForceExternal_B) - truthTorqueExternalPntB_B = sunEclipseMsgData.shadowFactor * np.array(truthTorqueExternalPntB_B) - testFailCount, testMessages = unitTestSupport.compareVector(truthForceExternal_B, - srpDataForce_B[1, 1:], - errTol, - "Force_B", - testFailCount, - testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthForceExternal_N, - srpDataForce_N[1, 1:], - errTol, - "Force_N", - testFailCount, - testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthTorqueExternalPntB_B, - srpTorqueData[1, 1:], - errTolTorque, - "Torque", - testFailCount, - testMessages) + truthForceExternal_B = sunEclipseMsgData.shadowFactor * np.array( + truthForceExternal_B + ) + truthTorqueExternalPntB_B = sunEclipseMsgData.shadowFactor * np.array( + truthTorqueExternalPntB_B + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthForceExternal_B, + srpDataForce_B[1, 1:], + errTol, + "Force_B", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthForceExternal_N, + srpDataForce_N[1, 1:], + errTol, + "Force_N", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthTorqueExternalPntB_B, + srpTorqueData[1, 1:], + errTolTorque, + "Torque", + testFailCount, + testMessages, + ) if modelType == "cannonballLookup": - errTolTorque = errTol/100 - testFailCount, testMessages = unitTestSupport.compareVector(srp2DataForce_N[1, 1:], - srpDataForce_B[1, 1:], - errTol, - "Force_B", - testFailCount, - testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(srp2DataForce_B[1, 1:], - srpDataForce_N[1, 1:], - errTol, - "Force_N", - testFailCount, - testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(srp2TorqueData[1, 1:], - srpTorqueData[1, 1:], - errTolTorque, - "Torque", - testFailCount, - testMessages) - + errTolTorque = errTol / 100 + testFailCount, testMessages = unitTestSupport.compareVector( + srp2DataForce_N[1, 1:], + srpDataForce_B[1, 1:], + errTol, + "Force_B", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + srp2DataForce_B[1, 1:], + srpDataForce_N[1, 1:], + errTol, + "Force_N", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + srp2TorqueData[1, 1:], + srpTorqueData[1, 1:], + errTolTorque, + "Torque", + testFailCount, + testMessages, + ) if eclipseOn: - modelType = modelType + 'WithEclipse' #Do this so that the AutoTeX messages are clearly distinguishable. + modelType = ( + modelType + "WithEclipse" + ) # Do this so that the AutoTeX messages are clearly distinguishable. if testFailCount == 0: print("PASSED: " + modelType) passFailText = "PASSED" - colorText = 'ForestGreen' # color to write auto-documented "PASSED" message in in LATEX - snippetName = modelType + 'FailMsg' + colorText = ( + "ForestGreen" # color to write auto-documented "PASSED" message in in LATEX + ) + snippetName = modelType + "FailMsg" snippetContent = "" - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. else: - passFailText = 'FAILED' - colorText = 'Red' # color to write auto-documented "FAILED" message in in LATEX - snippetName = modelType + 'FailMsg' + passFailText = "FAILED" + colorText = "Red" # color to write auto-documented "FAILED" message in in LATEX + snippetName = modelType + "FailMsg" snippetContent = passFailText for message in testMessages: snippetContent += ". " + message snippetContent += "." - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) # write formatted LATEX string to file to be used by auto-documentation. - snippetName = modelType + 'PassFail' # name of file to be written for auto-documentation which specifies if this test was passed or failed. - snippetContent = r'\textcolor{' + colorText + '}{' + passFailText + '}' #write formatted LATEX string to file to be used by auto-documentation. - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) #write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. + snippetName = ( + modelType + "PassFail" + ) # name of file to be written for auto-documentation which specifies if this test was passed or failed. + snippetContent = ( + r"\textcolor{" + colorText + "}{" + passFailText + "}" + ) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. # write test accuracy to LATEX file for AutoTex - snippetName = modelType + 'Accuracy' - snippetContent = '{:1.1e}'.format(errTol)#write formatted LATEX string to file to be used by auto-documentation. - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) #write formatted LATEX string to file to be used by auto-documentation. - if modelType == 'lookupWithEclipse' or modelType == 'lookup' or modelType == 'cannonballLookup': - snippetName = modelType + 'TorqueAccuracy' - snippetContent = '{:1.1e}'.format(errTolTorque) # write formatted LATEX string to file to be used by auto-documentation. - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, - path) # write formatted LATEX string to file to be used by auto-documentation. + snippetName = modelType + "Accuracy" + snippetContent = "{:1.1e}".format( + errTol + ) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. + if ( + modelType == "lookupWithEclipse" + or modelType == "lookup" + or modelType == "cannonballLookup" + ): + snippetName = modelType + "TorqueAccuracy" + snippetContent = "{:1.1e}".format( + errTolTorque + ) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. if testFailCount: print(testMessages) @@ -278,7 +351,8 @@ def unitRadiationPressure(show_plots, modelType, eclipseOn): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": unitRadiationPressure(False, "cannonball", False) diff --git a/src/simulation/dynamics/RadiationPressure/_UnitTest/test_radiation_pressure_integrated.py b/src/simulation/dynamics/RadiationPressure/_UnitTest/test_radiation_pressure_integrated.py index 17aa608cad..0470d3714c 100644 --- a/src/simulation/dynamics/RadiationPressure/_UnitTest/test_radiation_pressure_integrated.py +++ b/src/simulation/dynamics/RadiationPressure/_UnitTest/test_radiation_pressure_integrated.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -32,8 +31,12 @@ bskPath = __path__[0] from Basilisk.simulation import spacecraft, radiationPressure -from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, - unitTestSupport) +from Basilisk.utilities import ( + SimulationBaseClass, + macros, + orbitalMotion, + unitTestSupport, +) from Basilisk.utilities.simIncludeGravBody import gravBodyFactory @@ -67,7 +70,9 @@ def radiationPressureIntegratedTest(show_plots): scObject.ModelTag = "spacecraftBody" sim.AddModelToTask(simTaskName, scObject) - srp = radiationPressure.RadiationPressure() # default model is the SRP_CANNONBALL_MODEL + srp = ( + radiationPressure.RadiationPressure() + ) # default model is the SRP_CANNONBALL_MODEL srp.area = 1.0 srp.coefficientReflection = 1.3 sim.AddModelToTask(simTaskName, srp, -1) @@ -79,18 +84,20 @@ def radiationPressureIntegratedTest(show_plots): planet.isCentralBody = True mu = planet.mu gravFactory.createSun() - spice_path = bskPath + '/supportData/EphemerisData/' - gravFactory.createSpiceInterface(spice_path, '2021 MAY 04 07:47:49.965 (UTC)') - gravFactory.spiceObject.zeroBase = 'Earth' + spice_path = bskPath + "/supportData/EphemerisData/" + gravFactory.createSpiceInterface(spice_path, "2021 MAY 04 07:47:49.965 (UTC)") + gravFactory.spiceObject.zeroBase = "Earth" sim.AddModelToTask(simTaskName, gravFactory.spiceObject, -1) srp.sunEphmInMsg.subscribeTo(gravFactory.spiceObject.planetStateOutMsgs[1]) # attach gravity model to spacecraft - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() - rGEO = 42000. * 1000 # meters + rGEO = 42000.0 * 1000 # meters oe.a = rGEO oe.e = 0.00001 oe.i = 0.0 * macros.D2R @@ -98,7 +105,9 @@ def radiationPressureIntegratedTest(show_plots): oe.omega = 347.8 * macros.D2R oe.f = 85.3 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements # with circular or equatorial orbit, some angles are arbitrary print(rN) @@ -110,7 +119,7 @@ def radiationPressureIntegratedTest(show_plots): # set the simulation time n = np.sqrt(mu / oe.a / oe.a / oe.a) - P = 2. * np.pi / n + P = 2.0 * np.pi / n simulationTime = macros.sec2nano(P) # Setup data logging before the simulation is initialized @@ -155,22 +164,31 @@ def radiationPressureIntegratedTest(show_plots): pos_rel_earth_parse = pos_rel_earth[::skipValue] # true position for un perturbed 2 body GEO orbit with cannonball SRP - true_pos = np.array([[-2.18197848e+07, 3.58872415e+07, 0.00000000e+00] - ,[-3.97753187e+07, 1.34888792e+07, -7.33231880e+01] - ,[-3.91389859e+07, -1.52401375e+07, -3.06322198e+02] - ,[-2.01838008e+07, -3.68366952e+07, -6.37764168e+02] - ,[ 8.21683806e+06, -4.11950440e+07, -9.13393204e+02] - ,[ 3.27532709e+07, -2.63024006e+07, -9.57828703e+02] - ,[ 4.19944648e+07, 9.02522873e+05, -6.78102461e+02] - ,[ 3.15828214e+07, 2.76842358e+07, -1.40473487e+02] - ,[ 6.38617052e+06, 4.15047581e+07, 4.29674085e+02] - ,[-2.18006914e+07, 3.58874726e+07, 7.40872311e+02]]) + true_pos = np.array( + [ + [-2.18197848e07, 3.58872415e07, 0.00000000e00], + [-3.97753187e07, 1.34888792e07, -7.33231880e01], + [-3.91389859e07, -1.52401375e07, -3.06322198e02], + [-2.01838008e07, -3.68366952e07, -6.37764168e02], + [8.21683806e06, -4.11950440e07, -9.13393204e02], + [3.27532709e07, -2.63024006e07, -9.57828703e02], + [4.19944648e07, 9.02522873e05, -6.78102461e02], + [3.15828214e07, 2.76842358e07, -1.40473487e02], + [6.38617052e06, 4.15047581e07, 4.29674085e02], + [-2.18006914e07, 3.58874726e07, 7.40872311e02], + ] + ) # compare the results to the truth values accuracy = 10.0 # meters testFailCount, testMessages = unitTestSupport.compareArray( - true_pos, pos_rel_earth_parse, accuracy, "r_BN_N Vector", - testFailCount, testMessages) + true_pos, + pos_rel_earth_parse, + accuracy, + "r_BN_N Vector", + testFailCount, + testMessages, + ) # print out success message if no error were found if testFailCount == 0: @@ -183,19 +201,22 @@ def radiationPressureIntegratedTest(show_plots): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") for idx in range(0, 3): - plt.plot(dataLog.times() * macros.NANO2SEC / P, pos_rel_earth[:, idx] / 1000., - color=unitTestSupport.getLineColor(idx, 3), - label='$r_{BN,' + str(idx) + '}$') - - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Inertial Position [km]') - plt.title('Position Relative To Earth') + plt.plot( + dataLog.times() * macros.NANO2SEC / P, + pos_rel_earth[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Inertial Position [km]") + plt.title("Position Relative To Earth") if show_plots: plt.show() - plt.close('all') + plt.close("all") figureList = {} fileName = os.path.basename(os.path.splitext(__file__)[0]) diff --git a/src/simulation/dynamics/RadiationPressure/parseSRPLookup.py b/src/simulation/dynamics/RadiationPressure/parseSRPLookup.py index b9543abd9e..46442d7fb1 100755 --- a/src/simulation/dynamics/RadiationPressure/parseSRPLookup.py +++ b/src/simulation/dynamics/RadiationPressure/parseSRPLookup.py @@ -23,6 +23,7 @@ class SRPLookupTableHandler: """Class to handle an SRP Lookup table""" + def __init__(self): self.sHatBLookup = np.zeros([1, 3]) self.forceBLookup = np.zeros([1, 3]) @@ -31,40 +32,40 @@ def __init__(self): def parseAndLoadXML(self, filePath): document = ElementTree.parse(filePath) - sHatBTree = document.find('sHatBValues') - forceBTree = document.find('forceBValues') - torqueBTree = document.find('torqueBValues') + sHatBTree = document.find("sHatBValues") + forceBTree = document.find("forceBValues") + torqueBTree = document.find("torqueBValues") self.sHatBLookup.resize([len(list(sHatBTree)), 3], refcheck=False) self.forceBLookup.resize([len(list(forceBTree)), 3], refcheck=False) self.torqueBLookup.resize([len(list(torqueBTree)), 3], refcheck=False) for node in list(sHatBTree): - idx = int(node.attrib['index']) + idx = int(node.attrib["index"]) for value in list(node): - if value.tag == 'value_1': + if value.tag == "value_1": self.sHatBLookup[idx, 0] = value.text - if value.tag == 'value_2': + if value.tag == "value_2": self.sHatBLookup[idx, 1] = value.text - if value.tag == 'value_3': + if value.tag == "value_3": self.sHatBLookup[idx, 2] = value.text for node in list(forceBTree): - idx = int(node.attrib['index']) + idx = int(node.attrib["index"]) for value in list(node): - if value.tag == 'value_1': + if value.tag == "value_1": self.forceBLookup[idx, 0] = value.text - if value.tag == 'value_2': + if value.tag == "value_2": self.forceBLookup[idx, 1] = value.text - if value.tag == 'value_3': + if value.tag == "value_3": self.forceBLookup[idx, 2] = value.text for node in list(torqueBTree): - idx = int(node.attrib['index']) + idx = int(node.attrib["index"]) for value in list(node): - if value.tag == 'value_1': + if value.tag == "value_1": self.torqueBLookup[idx, 0] = value.text - if value.tag == 'value_2': + if value.tag == "value_2": self.torqueBLookup[idx, 1] = value.text - if value.tag == 'value_3': + if value.tag == "value_3": self.torqueBLookup[idx, 2] = value.text diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_ThrusterDynamicsUnit.py b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_ThrusterDynamicsUnit.py index 88a6bbef9f..d5dd96ba21 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_ThrusterDynamicsUnit.py +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_ThrusterDynamicsUnit.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,7 +15,6 @@ # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - # # Thruster Unit Test # @@ -29,6 +27,7 @@ import inspect import math + # @cond DOXYGEN_IGNORE import os @@ -37,11 +36,11 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') +splitPath = path.split("simulation") # @endcond -#Import all of the modules that we are going to call in this simulation +# Import all of the modules that we are going to call in this simulation from Basilisk.utilities import unitTestSupport import matplotlib.pyplot as plt from Basilisk.utilities import SimulationBaseClass @@ -55,46 +54,71 @@ class ResultsStore: def __init__(self): self.PassFail = [] + def texSnippet(self): for i in range(len(self.PassFail)): - snippetName = 'Result' + str(i) - if self.PassFail[i] == 'PASSED': - textColor = 'ForestGreen' - elif self.PassFail[i] == 'FAILED': - textColor = 'Red' - texSnippet = r'\textcolor{' + textColor + '}{'+ self.PassFail[i] + '}' + snippetName = "Result" + str(i) + if self.PassFail[i] == "PASSED": + textColor = "ForestGreen" + elif self.PassFail[i] == "FAILED": + textColor = "Red" + texSnippet = r"\textcolor{" + textColor + "}{" + self.PassFail[i] + "}" unitTestSupport.writeTeXSnippet(snippetName, texSnippet, path) + @pytest.fixture(scope="module") def testFixture(): listRes = ResultsStore() yield listRes listRes.texSnippet() + def thrusterEffectorAllTests(show_plots): - [testResults, testMessage] = test_unitThrusters(show_plots) + [testResults, testMessage] = test_unitThrusters(show_plots) # Create function to run the simulation who's results will be compared to expected values def executeSimRun(simContainer, thrusterSet, simRate, totalTime): newStopTime = simContainer.TotalSim.CurrentNanos + totalTime - while(simContainer.TotalSim.CurrentNanos < newStopTime): + while simContainer.TotalSim.CurrentNanos < newStopTime: simContainer.ConfigureStopTime(simContainer.TotalSim.CurrentNanos + simRate) simContainer.ExecuteSimulation() timeStep = 1.0 # not explicity used in this test - thrusterSet.computeForceTorque(simContainer.TotalSim.CurrentNanos*macros.NANO2SEC, timeStep) - thrusterSet.computeForceTorque(simContainer.TotalSim.CurrentNanos*macros.NANO2SEC + simRate*macros.NANO2SEC/2.0, timeStep) - thrusterSet.computeForceTorque(simContainer.TotalSim.CurrentNanos * macros.NANO2SEC + simRate * macros.NANO2SEC / 2.0, timeStep) - thrusterSet.computeForceTorque(simContainer.TotalSim.CurrentNanos*macros.NANO2SEC + simRate*macros.NANO2SEC, timeStep) + thrusterSet.computeForceTorque( + simContainer.TotalSim.CurrentNanos * macros.NANO2SEC, timeStep + ) + thrusterSet.computeForceTorque( + simContainer.TotalSim.CurrentNanos * macros.NANO2SEC + + simRate * macros.NANO2SEC / 2.0, + timeStep, + ) + thrusterSet.computeForceTorque( + simContainer.TotalSim.CurrentNanos * macros.NANO2SEC + + simRate * macros.NANO2SEC / 2.0, + timeStep, + ) + thrusterSet.computeForceTorque( + simContainer.TotalSim.CurrentNanos * macros.NANO2SEC + + simRate * macros.NANO2SEC, + timeStep, + ) - thrusterSet.computeStateContribution(simContainer.TotalSim.CurrentNanos * macros.NANO2SEC) thrusterSet.computeStateContribution( - simContainer.TotalSim.CurrentNanos * macros.NANO2SEC + simRate * macros.NANO2SEC / 2.0) + simContainer.TotalSim.CurrentNanos * macros.NANO2SEC + ) thrusterSet.computeStateContribution( - simContainer.TotalSim.CurrentNanos * macros.NANO2SEC + simRate * macros.NANO2SEC / 2.0) + simContainer.TotalSim.CurrentNanos * macros.NANO2SEC + + simRate * macros.NANO2SEC / 2.0 + ) thrusterSet.computeStateContribution( - simContainer.TotalSim.CurrentNanos * macros.NANO2SEC + simRate * macros.NANO2SEC) + simContainer.TotalSim.CurrentNanos * macros.NANO2SEC + + simRate * macros.NANO2SEC / 2.0 + ) + thrusterSet.computeStateContribution( + simContainer.TotalSim.CurrentNanos * macros.NANO2SEC + + simRate * macros.NANO2SEC + ) def fixMDotData(mDotData): @@ -104,42 +128,267 @@ def fixMDotData(mDotData): is why we need to remove all zero rows at the beginning of mDotData but one. """ - firstNonZeroRow = np.nonzero(mDotData[:,1])[0][0] - return np.vstack([[0,0], mDotData[firstNonZeroRow:, :]]) + firstNonZeroRow = np.nonzero(mDotData[:, 1])[0][0] + return np.vstack([[0, 0], mDotData[firstNonZeroRow:, :]]) + # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(True) -@pytest.mark.parametrize("ramp, thrustNumber, duration, long_angle, lat_angle, location, rate, cutoff, rampDown, swirlTorque, blowDown", [ - ("OFF", 1, 5.0, 30., 15., [[1.125], [0.5], [2.0]], 1E8, "OFF", "OFF", 0.0, "OFF"), #Test random thrust config - ("OFF", 1, 0.1, 30., 15., [[1.125], [0.5], [2.0]], 1E8, "OFF", "OFF", 0.0, "OFF"), # Short fire test - ("OFF", 1, 0.1, 30., 15., [[1.125], [0.5], [2.0]], 1E6, "OFF", "OFF", 0.0, "OFF"), # Short fire test with higher test rate - ("OFF", 1, 5.0, 30., 15., [[1.125], [0.5], [2.0]], 1E7, "OFF", "OFF", 0.0, "OFF"), # rate test - ("OFF", 1, 5.0, 10., 35., [[1.125], [0.5], [2.0]], 1E8, "OFF", "OFF", 0.0, "OFF"), # angle test - ("OFF", 1, 5.0, 30., 15., [[1.], [1.5], [0.0]], 1E8, "OFF", "OFF", 0.0, "OFF"), # Position test - ("OFF", 2, 5.0, 30., 15., [[1.125], [0.5], [2.0]], 1E8, "OFF", "OFF", 0.0, "OFF"), # Number of thrusters test - ("ON", 1, 5.0, 30., 15., [[1.125], [0.5], [2.0]], 1E8, "OFF", "OFF", 0.0, "OFF"), # Basic ramp test - ("ON", 1, 0.5, 30., 15., [[1.125], [0.5], [2.0]], 1E8, "OFF", "OFF", 0.0, "OFF"), # Short ramp test - ("ON", 1, 5.0, 30., 15., [[1.125], [0.5], [2.0]], 1E7, "OFF", "OFF", 0.0, "OFF"), # rate ramp test - ("ON", 1, 5.0, 30., 15., [[1.125], [0.5], [2.0]], 1E8, "ON", "OFF", 0.0, "OFF"), # Cuttoff test - ("ON", 1, 5.0, 30., 15., [[1.125], [0.5], [2.0]], 1E8, "ON", "ON", 0.0, "OFF"), # Ramp down test - ("OFF", 1, 5.0, 30., 15., [[1.125], [0.5], [2.0]], 1E8, "OFF", "OFF", 2.0, "OFF"), # Simple swirl torque test - ("ON", 1, 5.0, 30., 15., [[1.125], [0.5], [2.0]], 1E8, "OFF", "OFF", 0.5, "OFF"), # Basic ramp with swirl torque test - ("OFF", 1, 5.0, 30., 15., [[1.125], [0.5], [2.0]], 1E8, "OFF", "OFF", 0.0, "ON"), # Blow down test - ]) - +@pytest.mark.parametrize( + "ramp, thrustNumber, duration, long_angle, lat_angle, location, rate, cutoff, rampDown, swirlTorque, blowDown", + [ + ( + "OFF", + 1, + 5.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e8, + "OFF", + "OFF", + 0.0, + "OFF", + ), # Test random thrust config + ( + "OFF", + 1, + 0.1, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e8, + "OFF", + "OFF", + 0.0, + "OFF", + ), # Short fire test + ( + "OFF", + 1, + 0.1, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e6, + "OFF", + "OFF", + 0.0, + "OFF", + ), # Short fire test with higher test rate + ( + "OFF", + 1, + 5.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e7, + "OFF", + "OFF", + 0.0, + "OFF", + ), # rate test + ( + "OFF", + 1, + 5.0, + 10.0, + 35.0, + [[1.125], [0.5], [2.0]], + 1e8, + "OFF", + "OFF", + 0.0, + "OFF", + ), # angle test + ( + "OFF", + 1, + 5.0, + 30.0, + 15.0, + [[1.0], [1.5], [0.0]], + 1e8, + "OFF", + "OFF", + 0.0, + "OFF", + ), # Position test + ( + "OFF", + 2, + 5.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e8, + "OFF", + "OFF", + 0.0, + "OFF", + ), # Number of thrusters test + ( + "ON", + 1, + 5.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e8, + "OFF", + "OFF", + 0.0, + "OFF", + ), # Basic ramp test + ( + "ON", + 1, + 0.5, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e8, + "OFF", + "OFF", + 0.0, + "OFF", + ), # Short ramp test + ( + "ON", + 1, + 5.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e7, + "OFF", + "OFF", + 0.0, + "OFF", + ), # rate ramp test + ( + "ON", + 1, + 5.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e8, + "ON", + "OFF", + 0.0, + "OFF", + ), # Cuttoff test + ( + "ON", + 1, + 5.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e8, + "ON", + "ON", + 0.0, + "OFF", + ), # Ramp down test + ( + "OFF", + 1, + 5.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e8, + "OFF", + "OFF", + 2.0, + "OFF", + ), # Simple swirl torque test + ( + "ON", + 1, + 5.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e8, + "OFF", + "OFF", + 0.5, + "OFF", + ), # Basic ramp with swirl torque test + ( + "OFF", + 1, + 5.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e8, + "OFF", + "OFF", + 0.0, + "ON", + ), # Blow down test + ], +) # provide a unique test method name, starting with test_ -def test_unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_angle, lat_angle, location, rate, cutoff, rampDown, swirlTorque, blowDown): +def test_unitThrusters( + testFixture, + show_plots, + ramp, + thrustNumber, + duration, + long_angle, + lat_angle, + location, + rate, + cutoff, + rampDown, + swirlTorque, + blowDown, +): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_angle, lat_angle, location, rate, cutoff, rampDown, swirlTorque, blowDown) + [testResults, testMessage] = unitThrusters( + testFixture, + show_plots, + ramp, + thrustNumber, + duration, + long_angle, + lat_angle, + location, + rate, + cutoff, + rampDown, + swirlTorque, + blowDown, + ) assert testResults < 1, testMessage # Run the test -def unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_angle, lat_angle, location, rate, cutoff, rampDown, swirlTorque, blowDown): +def unitThrusters( + testFixture, + show_plots, + ramp, + thrustNumber, + duration, + long_angle, + lat_angle, + location, + rate, + cutoff, + rampDown, + swirlTorque, + blowDown, +): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. @@ -147,8 +396,8 @@ def unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_an 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) + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) # Constants g = 9.80665 @@ -171,38 +420,72 @@ def unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_an isp_blow_down_coeff = [] # Create thruster characteristic parameters (position, angle thrust, ISP, time of thrust) - angledeg_long = long_angle # Parametrized angle of thrust + angledeg_long = long_angle # Parametrized angle of thrust angledeg_lat = lat_angle - anglerad_long = angledeg_long * math.pi/180.0 + anglerad_long = angledeg_long * math.pi / 180.0 anglerad_lat = angledeg_lat * math.pi / 180.0 thruster1 = thrusterDynamicEffector.THRSimConfig() - thruster1.thrLoc_B = location # Parametrized location for thruster - thruster1.thrDir_B = [[math.cos(anglerad_long)*math.cos(anglerad_lat)], [math.sin(anglerad_long)*math.cos(anglerad_lat)], [math.sin(anglerad_lat)]] + thruster1.thrLoc_B = location # Parametrized location for thruster + thruster1.thrDir_B = [ + [math.cos(anglerad_long) * math.cos(anglerad_lat)], + [math.sin(anglerad_long) * math.cos(anglerad_lat)], + [math.sin(anglerad_lat)], + ] thruster1.MaxThrust = 1.0 thruster1.steadyIsp = Isp thruster1.MinOnTime = 0.006 thruster1.MaxSwirlTorque = swirlTorque - for thr_coeff in thrust_blow_down_coeff: thruster1.thrBlowDownCoeff.push_back(thr_coeff) - for isp_coeff in isp_blow_down_coeff: thruster1.ispBlowDownCoeff.push_back(isp_coeff) + for thr_coeff in thrust_blow_down_coeff: + thruster1.thrBlowDownCoeff.push_back(thr_coeff) + for isp_coeff in isp_blow_down_coeff: + thruster1.ispBlowDownCoeff.push_back(isp_coeff) thrusterSet.addThruster(thruster1) - loc1 = np.array([thruster1.thrLoc_B[0][0],thruster1.thrLoc_B[1][0],thruster1.thrLoc_B[2][0]]) - dir1 = np.array([thruster1.thrDir_B[0][0], thruster1.thrDir_B[1][0], thruster1.thrDir_B[2][0]]) + loc1 = np.array( + [thruster1.thrLoc_B[0][0], thruster1.thrLoc_B[1][0], thruster1.thrLoc_B[2][0]] + ) + dir1 = np.array( + [thruster1.thrDir_B[0][0], thruster1.thrDir_B[1][0], thruster1.thrDir_B[2][0]] + ) - if thrustNumber==2: + if thrustNumber == 2: thruster2 = thrusterDynamicEffector.THRSimConfig() - thruster2.thrLoc_B =np.array([[1.], [0.0], [0.0]]).reshape([3,1]) - thruster2.thrDir_B = np.array([[math.cos(anglerad_long+math.pi/4.)*math.cos(anglerad_lat-math.pi/4.)], [math.sin(anglerad_long+math.pi/4.)*math.cos(anglerad_lat-math.pi/4.)], [math.sin(anglerad_lat-math.pi/4.)]]).reshape([3,1]) + thruster2.thrLoc_B = np.array([[1.0], [0.0], [0.0]]).reshape([3, 1]) + thruster2.thrDir_B = np.array( + [ + [ + math.cos(anglerad_long + math.pi / 4.0) + * math.cos(anglerad_lat - math.pi / 4.0) + ], + [ + math.sin(anglerad_long + math.pi / 4.0) + * math.cos(anglerad_lat - math.pi / 4.0) + ], + [math.sin(anglerad_lat - math.pi / 4.0)], + ] + ).reshape([3, 1]) thruster2.MaxThrust = 1.0 thruster2.steadyIsp = 226.7 thruster2.MinOnTime = 0.006 thrusterSet.addThruster(thruster2) - loc2 = np.array([thruster2.thrLoc_B[0][0],thruster2.thrLoc_B[1][0],thruster2.thrLoc_B[2][0]]) - dir2 = np.array([thruster2.thrDir_B[0][0], thruster2.thrDir_B[1][0], thruster2.thrDir_B[2][0]]) + loc2 = np.array( + [ + thruster2.thrLoc_B[0][0], + thruster2.thrLoc_B[1][0], + thruster2.thrLoc_B[2][0], + ] + ) + dir2 = np.array( + [ + thruster2.thrDir_B[0][0], + thruster2.thrDir_B[1][0], + thruster2.thrDir_B[2][0], + ] + ) # Create a Simulation - testRate = int(rate) # Parametrized rate of test + testRate = int(rate) # Parametrized rate of test TotalSim = SimulationBaseClass.SimBaseClass() # Create a new process for the unit test task and add the module to tasking @@ -217,149 +500,299 @@ def unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_an # TotalSim.AddModelToTask(unitTaskName, TotalSim.scObject) # Define the start of the thrust and it's duration - sparetime = 3.*1./macros.NANO2SEC - thrStartTime=sparetime - thrDurationTime=duration*1./macros.NANO2SEC # Parametrized thrust duration - - #Configure a single thruster firing, create a message for it - thrusterSetLog = thrusterSet.logger(["forceExternal_B", "torqueExternalPntB_B", "mDotTotal"]) + sparetime = 3.0 * 1.0 / macros.NANO2SEC + thrStartTime = sparetime + thrDurationTime = duration * 1.0 / macros.NANO2SEC # Parametrized thrust duration + + # Configure a single thruster firing, create a message for it + thrusterSetLog = thrusterSet.logger( + ["forceExternal_B", "torqueExternalPntB_B", "mDotTotal"] + ) TotalSim.AddModelToTask(unitTaskName, thrusterSetLog) ThrustMessage = messaging.THRArrayOnTimeCmdMsgPayload() - if thrustNumber==1: - ThrustMessage.OnTimeRequest = [0.] - if thrustNumber==2: - ThrustMessage.OnTimeRequest = [0., 0.] + if thrustNumber == 1: + ThrustMessage.OnTimeRequest = [0.0] + if thrustNumber == 2: + ThrustMessage.OnTimeRequest = [0.0, 0.0] thrCmdMsg = messaging.THRArrayOnTimeCmdMsg().write(ThrustMessage) thrusterSet.cmdsInMsg.subscribeTo(thrCmdMsg) TotalSim.InitializeSimulation() - #Configure the hub and link states - TotalSim.newManager.createProperty("r_BN_N", [[0], [0], [0]]) # manually create the property + # Configure the hub and link states + TotalSim.newManager.createProperty( + "r_BN_N", [[0], [0], [0]] + ) # manually create the property TotalSim.scObject.hub.registerStates(TotalSim.newManager) # assign state engine names of parent rigid body thrusterSet.setStateNameOfSigma(TotalSim.scObject.hub.nameOfHubSigma) thrusterSet.setStateNameOfOmega(TotalSim.scObject.hub.nameOfHubOmega) - thrusterSet.setPropName_inertialPosition(TotalSim.scObject.gravField.inertialPositionPropName) + thrusterSet.setPropName_inertialPosition( + TotalSim.scObject.gravField.inertialPositionPropName + ) thrusterSet.linkInStates(TotalSim.newManager) plt.close("all") if ramp == "OFF": # Run the simulation executeSimRun(TotalSim, thrusterSet, testRate, int(thrStartTime)) - if thrustNumber==1: - ThrustMessage.OnTimeRequest = [thrDurationTime*macros.NANO2SEC] - if thrustNumber==2: - ThrustMessage.OnTimeRequest = [thrDurationTime * macros.NANO2SEC, thrDurationTime * macros.NANO2SEC] - thrCmdMsg.write(ThrustMessage, TotalSim.TotalSim.CurrentNanos+testRate) - executeSimRun(TotalSim, thrusterSet, testRate, int(thrDurationTime+sparetime)) + if thrustNumber == 1: + ThrustMessage.OnTimeRequest = [thrDurationTime * macros.NANO2SEC] + if thrustNumber == 2: + ThrustMessage.OnTimeRequest = [ + thrDurationTime * macros.NANO2SEC, + thrDurationTime * macros.NANO2SEC, + ] + thrCmdMsg.write(ThrustMessage, TotalSim.TotalSim.CurrentNanos + testRate) + executeSimRun(TotalSim, thrusterSet, testRate, int(thrDurationTime + sparetime)) # Gather the Force and Torque results - thrForce = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.forceExternal_B) - thrTorque = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.torqueExternalPntB_B) - mDotData = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.mDotTotal) + thrForce = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.forceExternal_B + ) + thrTorque = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.torqueExternalPntB_B + ) + mDotData = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.mDotTotal + ) mDotData = fixMDotData(mDotData) # Auto Generate LaTex Figures format = r"width=0.8\textwidth" - snippetName = ("Snippet" + str(thrustNumber) + "Thrusters_" + str(int(duration)) + "s_" + str(int(long_angle)) + - "deg_" + "Loc" + str(int(loc1[2])) + "_Rate" + str(int(1./(testRate*macros.NANO2SEC))) + - "_Swirl" + str(int(swirlTorque)) + "_BlowDown" + blowDown) - if thrustNumber==1: - texSnippet = ("The thruster is set at " + str(int(long_angle)) + r"$^\circ$ off the x-axis " + - str(int(lat_angle)) + r"$^\circ$ off the z-axis, in the position $\bm r = \left(" + - str(loc1[0]) + "," + str(loc1[1]) + "," + str(loc1[2]) + - r"\right)$. The test is launched using " + str(thrustNumber) + " thruster, for " + - str(duration) + " seconds. The test rate is " + str(int(1./(testRate*macros.NANO2SEC))) + - " steps per second. Swirl torque is set to " + str(int(swirlTorque)) + - " Newton meters and blow down effects are " + blowDown + ".") - if thrustNumber==2: - texSnippet = ("The first thruster is set at " + str(int(long_angle)) + r"$^\circ$ off the x-axis " + - str(int(lat_angle)) + r"$^\circ$ off the z-axis, in the position $\bm r = \left(" + - str(loc1[0]) + "," + str(loc1[1]) + "," + str(loc1[2]) + - r"\right)$. The second thruster is set at " + str(int(long_angle+45)) + - r"$^\circ$ off the x-axis " + str(int(lat_angle+45)) + - r"$^\circ$ off the z-axis, in the position $\bm r = \left(" + str(loc2[0]) + "," + - str(loc2[1]) + "," + str(loc2[2]) + r"\right)$. The test uses these " + str(thrustNumber) + - " thrusters for " + str(duration) + " seconds. The test rate is " + - str(int(1. / (testRate * macros.NANO2SEC))) + " steps per second. Swirl torque is set to " + - str(int(swirlTorque)) + " Newton meters and blow down effects are " + blowDown + ".") + snippetName = ( + "Snippet" + + str(thrustNumber) + + "Thrusters_" + + str(int(duration)) + + "s_" + + str(int(long_angle)) + + "deg_" + + "Loc" + + str(int(loc1[2])) + + "_Rate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + "_Swirl" + + str(int(swirlTorque)) + + "_BlowDown" + + blowDown + ) + if thrustNumber == 1: + texSnippet = ( + "The thruster is set at " + + str(int(long_angle)) + + r"$^\circ$ off the x-axis " + + str(int(lat_angle)) + + r"$^\circ$ off the z-axis, in the position $\bm r = \left(" + + str(loc1[0]) + + "," + + str(loc1[1]) + + "," + + str(loc1[2]) + + r"\right)$. The test is launched using " + + str(thrustNumber) + + " thruster, for " + + str(duration) + + " seconds. The test rate is " + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + " steps per second. Swirl torque is set to " + + str(int(swirlTorque)) + + " Newton meters and blow down effects are " + + blowDown + + "." + ) + if thrustNumber == 2: + texSnippet = ( + "The first thruster is set at " + + str(int(long_angle)) + + r"$^\circ$ off the x-axis " + + str(int(lat_angle)) + + r"$^\circ$ off the z-axis, in the position $\bm r = \left(" + + str(loc1[0]) + + "," + + str(loc1[1]) + + "," + + str(loc1[2]) + + r"\right)$. The second thruster is set at " + + str(int(long_angle + 45)) + + r"$^\circ$ off the x-axis " + + str(int(lat_angle + 45)) + + r"$^\circ$ off the z-axis, in the position $\bm r = \left(" + + str(loc2[0]) + + "," + + str(loc2[1]) + + "," + + str(loc2[2]) + + r"\right)$. The test uses these " + + str(thrustNumber) + + " thrusters for " + + str(duration) + + " seconds. The test rate is " + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + " steps per second. Swirl torque is set to " + + str(int(swirlTorque)) + + " Newton meters and blow down effects are " + + blowDown + + "." + ) unitTestSupport.writeTeXSnippet(snippetName, texSnippet, path) - PlotName = ("Force_" + str(thrustNumber) + "Thrusters_" + str(int(duration)) + "s_" + str(int(long_angle)) + - "deg_" + "Loc" + str(int(location[2][0])) + "_Rate" + str(int(1./(testRate*macros.NANO2SEC))) + - "_Swirl" + str(int(swirlTorque)) + "_BlowDown" + blowDown) - PlotTitle = ("Force on Y with " + str(thrustNumber) + " thrusters, for " + str(int(duration)) + " sec at " + - str(int(long_angle)) + " deg " + "Rate" + str(int(1./(testRate*macros.NANO2SEC))) + ", Swirl" + - str(int(swirlTorque)) + "Nm, BlowDown" + blowDown) + PlotName = ( + "Force_" + + str(thrustNumber) + + "Thrusters_" + + str(int(duration)) + + "s_" + + str(int(long_angle)) + + "deg_" + + "Loc" + + str(int(location[2][0])) + + "_Rate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + "_Swirl" + + str(int(swirlTorque)) + + "_BlowDown" + + blowDown + ) + PlotTitle = ( + "Force on Y with " + + str(thrustNumber) + + " thrusters, for " + + str(int(duration)) + + " sec at " + + str(int(long_angle)) + + " deg " + + "Rate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + ", Swirl" + + str(int(swirlTorque)) + + "Nm, BlowDown" + + blowDown + ) plt.close("all") plt.figure(1) plt.clf() - plt.plot(thrForce[:,0]*macros.NANO2SEC, thrForce[:,2]) - plt.xlabel('Time(s)') - plt.ylabel('Thrust Factor (N)') - plt.ylim(-0.2,1) + plt.plot(thrForce[:, 0] * macros.NANO2SEC, thrForce[:, 2]) + plt.xlabel("Time(s)") + plt.ylabel("Thrust Factor (N)") + plt.ylim(-0.2, 1) unitTestSupport.writeFigureLaTeX(PlotName, PlotTitle, plt, format, path) - if show_plots==True: + if show_plots == True: plt.show() - plt.close('all') - - PlotName = ("Torque_" + str(thrustNumber) + "Thrusters_" + str(int(duration)) + "s_" + str(int(long_angle)) + - "deg_" + "Loc" + str(int(location[2][0])) + "_Rate" + str(int(1./(testRate*macros.NANO2SEC))) + - "_Swirl" + str(int(swirlTorque)) + "_BlowDown" + blowDown) - PlotTitle = ("Torque on X with " + str(thrustNumber) + " thrusters, for " + str(int(duration)) + " sec at " + - str(int(long_angle)) + " deg " + "Rate" + str(int(1./(testRate*macros.NANO2SEC))) + ", Swirl" + - str(int(swirlTorque)) + "Nm, BlowDown" + blowDown) + plt.close("all") + + PlotName = ( + "Torque_" + + str(thrustNumber) + + "Thrusters_" + + str(int(duration)) + + "s_" + + str(int(long_angle)) + + "deg_" + + "Loc" + + str(int(location[2][0])) + + "_Rate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + "_Swirl" + + str(int(swirlTorque)) + + "_BlowDown" + + blowDown + ) + PlotTitle = ( + "Torque on X with " + + str(thrustNumber) + + " thrusters, for " + + str(int(duration)) + + " sec at " + + str(int(long_angle)) + + " deg " + + "Rate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + ", Swirl" + + str(int(swirlTorque)) + + "Nm, BlowDown" + + blowDown + ) plt.figure(11) plt.clf() - plt.plot(thrForce[:,0]*macros.NANO2SEC, thrTorque[:,1]) - plt.xlabel('Time(s)') - plt.ylabel('Thrust Torque (Nm)') + plt.plot(thrForce[:, 0] * macros.NANO2SEC, thrTorque[:, 1]) + plt.xlabel("Time(s)") + plt.ylabel("Thrust Torque (Nm)") plt.ylim(-1.5, 2) unitTestSupport.writeFigureLaTeX(PlotName, PlotTitle, plt, format, path) - if show_plots==True: + if show_plots == True: plt.show() - plt.close('all') - - PlotName = (str(thrustNumber) + "Thrusters_" + str(int(duration)) + "s_" + str(int(long_angle)) + "deg_" + - "Loc" + str(int(location[2][0])) + "_Rate" + str(int(1./(testRate*macros.NANO2SEC))) + - "_Swirl" + str(int(swirlTorque)) + "_BlowDown" + blowDown) - PlotTitle = ("All Forces and Torques " + str(thrustNumber) + " thrusters, for " + str(int(duration)) + - " sec at " + str(int(long_angle)) + " deg " + "Rate" + str(int(1./(testRate*macros.NANO2SEC))) + - ", Swirl" + str(int(swirlTorque)) + "Nm, BlowDown" + blowDown) + plt.close("all") + + PlotName = ( + str(thrustNumber) + + "Thrusters_" + + str(int(duration)) + + "s_" + + str(int(long_angle)) + + "deg_" + + "Loc" + + str(int(location[2][0])) + + "_Rate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + "_Swirl" + + str(int(swirlTorque)) + + "_BlowDown" + + blowDown + ) + PlotTitle = ( + "All Forces and Torques " + + str(thrustNumber) + + " thrusters, for " + + str(int(duration)) + + " sec at " + + str(int(long_angle)) + + " deg " + + "Rate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + ", Swirl" + + str(int(swirlTorque)) + + "Nm, BlowDown" + + blowDown + ) plt.figure(22) plt.clf() - plt.plot(thrForce[:,0]*1.0E-9, thrForce[:,1], 'b', label='x Force') - plt.plot(thrTorque[:,0]*1.0E-9, thrTorque[:,1], 'b--', label='x Torque') - plt.plot(thrForce[:,0]*1.0E-9, thrForce[:,2], 'g', label='y Force') - plt.plot(thrTorque[:,0]*1.0E-9, thrTorque[:,2], 'g--', label='y Torque') - plt.plot(thrForce[:,0]*1.0E-9, thrForce[:,3], 'r', label = 'z Force') - plt.plot(thrTorque[:,0]*1.0E-9, thrTorque[:,3], 'r--', label='z Torque') - plt.legend(loc='upper right') - plt.xlabel('Time(s)') + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 1], "b", label="x Force") + plt.plot(thrTorque[:, 0] * 1.0e-9, thrTorque[:, 1], "b--", label="x Torque") + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 2], "g", label="y Force") + plt.plot(thrTorque[:, 0] * 1.0e-9, thrTorque[:, 2], "g--", label="y Torque") + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 3], "r", label="z Force") + plt.plot(thrTorque[:, 0] * 1.0e-9, thrTorque[:, 3], "r--", label="z Torque") + plt.legend(loc="upper right") + plt.xlabel("Time(s)") plt.ylim(-1.5, 2) - plt.legend(loc='upper right') + plt.legend(loc="upper right") unitTestSupport.writeFigureLaTeX(PlotName, PlotTitle, plt, format, path) - if show_plots==True: + if show_plots == True: plt.show() - plt.close('all') + plt.close("all") # Create expected Force to test against thrForce - expMDot = np.zeros([np.shape(np.array(mDotData))[0],1]) + expMDot = np.zeros([np.shape(np.array(mDotData))[0], 1]) mDotData = np.delete(mDotData, 0, axis=1) for i in range(np.shape(np.array(mDotData))[0]): - if (i > 0 and i < int(round((thrDurationTime) / testRate)) + 1): + if i > 0 and i < int(round((thrDurationTime) / testRate)) + 1: expMDot[i, 0] = thrustNumber / (g * Isp) - expectedpoints=np.zeros([3,np.shape(thrForce)[0]]) - for i in range(np.shape(thrForce)[0]):# Thrust fires 2 times steps after the pause of sim and restart - if (i>int(round(thrStartTime/ testRate)) + 1 and i int(round(thrStartTime / testRate)) + 1 + and i < int(round((thrStartTime + thrDurationTime) / testRate)) + 2 + ): if thrustNumber == 1: expectedpoints[0:3, i] = dir1 * fuel_ratio else: @@ -367,40 +800,55 @@ def unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_an # Modify expected values for comparison and define errorTolerance TruthForce = np.transpose(expectedpoints) - ErrTolerance = 10E-9 + ErrTolerance = 10e-9 # Compare Force values thrForce = np.delete(thrForce, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthForce, thrForce, ErrTolerance, "Force", testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + TruthForce, thrForce, ErrTolerance, "Force", testFailCount, testMessages + ) for i in range(0, len(np.array(mDotData))): - if not unitTestSupport.isArrayEqual(np.array(mDotData)[i,:], expMDot[i,:], 1, ErrTolerance): - testFailCount+=1 - testMessages.append('M dot failure') + if not unitTestSupport.isArrayEqual( + np.array(mDotData)[i, :], expMDot[i, :], 1, ErrTolerance + ): + testFailCount += 1 + testMessages.append("M dot failure") # Create expected Torque to test against thrTorque expectedpointstor = np.zeros([3, np.shape(thrTorque)[0]]) - for i in range(np.shape(thrForce)[0]): # Thrust fires 2 times steps after the pause of sim and restart - if (i>int(round(thrStartTime/ testRate)) + 1 and i int(round(thrStartTime / testRate)) + 1 + and i < int(round((thrStartTime + thrDurationTime) / testRate)) + 2 + ): if thrustNumber == 1: - expectedpointstor[0:3, i] = (np.cross(loc1, dir1) + swirlTorque * dir1) * fuel_ratio + expectedpointstor[0:3, i] = ( + np.cross(loc1, dir1) + swirlTorque * dir1 + ) * fuel_ratio else: - expectedpointstor[0:3, i] = (np.cross(loc1, dir1) + swirlTorque * dir1 + np.cross(loc2, dir2)) * fuel_ratio + expectedpointstor[0:3, i] = ( + np.cross(loc1, dir1) + swirlTorque * dir1 + np.cross(loc2, dir2) + ) * fuel_ratio # Define errorTolerance TruthTorque = np.transpose(expectedpointstor) - ErrTolerance = 10E-9 + ErrTolerance = 10e-9 # Compare Torque values # Compare Force values thrTorque = np.delete(thrTorque, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthTorque, thrTorque, ErrTolerance, "Torque", testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + TruthTorque, thrTorque, ErrTolerance, "Torque", testFailCount, testMessages + ) if ramp == "ON": format = r"width=0.8\textwidth" rampsteps = 10 - sparetime = 3.*1/macros.NANO2SEC - thrStartTime = sparetime - 1.*1/macros.NANO2SEC + sparetime = 3.0 * 1 / macros.NANO2SEC + thrStartTime = sparetime - 1.0 * 1 / macros.NANO2SEC # Setup thruster ramp on and ramp off configuration rampOnList = [] @@ -408,7 +856,7 @@ def unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_an # Note that this ramp is totally linear and ramps up 30 ms using 30 steps for i in range(rampsteps): newElement = thrusterDynamicEffector.THRTimePair() - newElement.TimeDelta = (i + 1.) * 0.1 + newElement.TimeDelta = (i + 1.0) * 0.1 newElement.ThrustFactor = (i + 1.0) / 10.0 newElement.IspFactor = (i + 1.0) / 10.0 rampOnList.append(newElement) @@ -419,240 +867,650 @@ def unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_an rampOffList.append(newElement) # Set up the ramps - thruster1.ThrusterOnRamp = \ - thrusterDynamicEffector.ThrusterTimeVector(rampOnList) - thruster1.ThrusterOffRamp = \ - thrusterDynamicEffector.ThrusterTimeVector(rampOffList) + thruster1.ThrusterOnRamp = thrusterDynamicEffector.ThrusterTimeVector( + rampOnList + ) + thruster1.ThrusterOffRamp = thrusterDynamicEffector.ThrusterTimeVector( + rampOffList + ) if rampDown == "OFF": if cutoff == "OFF": # Execute a new firing that will use the thruster ramps executeSimRun(TotalSim, thrusterSet, testRate, int(thrStartTime)) - ThrustMessage.OnTimeRequest = [thrDurationTime*macros.NANO2SEC] - thrCmdMsg.write(ThrustMessage, TotalSim.TotalSim.CurrentNanos+testRate) - executeSimRun(TotalSim, thrusterSet, testRate, int(thrDurationTime+sparetime)) + ThrustMessage.OnTimeRequest = [thrDurationTime * macros.NANO2SEC] + thrCmdMsg.write( + ThrustMessage, TotalSim.TotalSim.CurrentNanos + testRate + ) + executeSimRun( + TotalSim, thrusterSet, testRate, int(thrDurationTime + sparetime) + ) # Extract log variables and plot the results - thrForce = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.forceExternal_B) - thrTorque = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.torqueExternalPntB_B) - mDotData = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.mDotTotal) + thrForce = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.forceExternal_B + ) + thrTorque = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.torqueExternalPntB_B + ) + mDotData = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.mDotTotal + ) mDotData = fixMDotData(mDotData) - snippetName = ("Snippet" + "Ramp_" + str(rampsteps) + "steps_" + str(int(duration)) + "s" + "_Cutoff" + - cutoff + "_Rate" + str(int(1. / (testRate * macros.NANO2SEC))) + "_Cutoff" + cutoff + - "_Swirl" + str(int(swirlTorque)) + "_BlowDown" + blowDown) - texSnippet = ("We test the ramped thrust with " + str(rampsteps) + - " incremental steps. The single thruster is set at the default " + str(int(long_angle)) + - r"$^\circ$ off the x-axis " + str(int(lat_angle)) + - r"$^\circ$ off the z-axis, at $\bm r = \left(" + str(loc1[0]) + "," + str(loc1[1]) + "," + - str(loc1[2]) + r"\right)$. The thrust is set for " + str(duration) + - " seconds with a test rate of " + str(int(1. / (testRate * macros.NANO2SEC))) + - " steps per second. The Cutoff test is " + cutoff + ", swirl torque is set to " + - str(int(swirlTorque)) + " Newton meters, and blow down effects are " + blowDown + ".") + snippetName = ( + "Snippet" + + "Ramp_" + + str(rampsteps) + + "steps_" + + str(int(duration)) + + "s" + + "_Cutoff" + + cutoff + + "_Rate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + "_Cutoff" + + cutoff + + "_Swirl" + + str(int(swirlTorque)) + + "_BlowDown" + + blowDown + ) + texSnippet = ( + "We test the ramped thrust with " + + str(rampsteps) + + " incremental steps. The single thruster is set at the default " + + str(int(long_angle)) + + r"$^\circ$ off the x-axis " + + str(int(lat_angle)) + + r"$^\circ$ off the z-axis, at $\bm r = \left(" + + str(loc1[0]) + + "," + + str(loc1[1]) + + "," + + str(loc1[2]) + + r"\right)$. The thrust is set for " + + str(duration) + + " seconds with a test rate of " + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + " steps per second. The Cutoff test is " + + cutoff + + ", swirl torque is set to " + + str(int(swirlTorque)) + + " Newton meters, and blow down effects are " + + blowDown + + "." + ) unitTestSupport.writeTeXSnippet(snippetName, texSnippet, path) - PlotName = ("Ramp_" + str(rampsteps) + "steps_Cutoff" + cutoff + "_" + str(int(duration)) + "s" + - "_testRate" + str(int(1. / (testRate * macros.NANO2SEC))) + "_Swirl" + str(int(swirlTorque)) - + "_BlowDown" + blowDown) - PlotTitle = ("All Forces and Torques with " + str(rampsteps) + " step Ramp, thrust for " + - str(int(duration)) + "s. Cutoff " + cutoff+", testRate " + - str(int(1. / (testRate * macros.NANO2SEC))) + ", swirlTorque " + str(int(swirlTorque)) + - ", blowDown " + blowDown) + PlotName = ( + "Ramp_" + + str(rampsteps) + + "steps_Cutoff" + + cutoff + + "_" + + str(int(duration)) + + "s" + + "_testRate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + "_Swirl" + + str(int(swirlTorque)) + + "_BlowDown" + + blowDown + ) + PlotTitle = ( + "All Forces and Torques with " + + str(rampsteps) + + " step Ramp, thrust for " + + str(int(duration)) + + "s. Cutoff " + + cutoff + + ", testRate " + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + ", swirlTorque " + + str(int(swirlTorque)) + + ", blowDown " + + blowDown + ) plt.figure(22) plt.clf() - plt.plot(thrForce[:, 0] * 1.0E-9, thrForce[:, 1], 'b', label='x Force') - plt.plot(thrTorque[:, 0] * 1.0E-9, thrTorque[:, 1], 'b--', label='x Torque') - plt.plot(thrForce[:, 0] * 1.0E-9, thrForce[:, 2], 'g', label='y Force') - plt.plot(thrTorque[:, 0] * 1.0E-9, thrTorque[:, 2], 'g--', label='y Torque') - plt.plot(thrForce[:, 0] * 1.0E-9, thrForce[:, 3], 'r', label='z Force') - plt.plot(thrTorque[:, 0] * 1.0E-9, thrTorque[:, 3], 'r--', label='z Torque') - plt.legend(loc='upper right') - plt.xlabel('Time(s)') + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 1], "b", label="x Force") + plt.plot( + thrTorque[:, 0] * 1.0e-9, thrTorque[:, 1], "b--", label="x Torque" + ) + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 2], "g", label="y Force") + plt.plot( + thrTorque[:, 0] * 1.0e-9, thrTorque[:, 2], "g--", label="y Torque" + ) + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 3], "r", label="z Force") + plt.plot( + thrTorque[:, 0] * 1.0e-9, thrTorque[:, 3], "r--", label="z Torque" + ) + plt.legend(loc="upper right") + plt.xlabel("Time(s)") plt.ylim(-1.5, 2) - plt.legend(loc='upper left') + plt.legend(loc="upper left") unitTestSupport.writeFigureLaTeX(PlotName, PlotTitle, plt, format, path) if show_plots == True: plt.show() - plt.close('all') + plt.close("all") # Create expected Force to test against thrForce expectedpoints = np.zeros([3, np.shape(thrForce)[0]]) - RampFunction= np.zeros([np.shape(thrForce)[0]]) - ramplength = 1. - if ramplength < thrDurationTime*macros.NANO2SEC: + RampFunction = np.zeros([np.shape(thrForce)[0]]) + ramplength = 1.0 + if ramplength < thrDurationTime * macros.NANO2SEC: for i in range(np.shape(thrForce)[0]): - if i int(round(thrStartTime / testRate)) + 1 + and i + < int( + round( + (thrStartTime + ramplength * 1.0 / macros.NANO2SEC) + / testRate + ) + ) + + 2 + ): # ramp up + RampFunction[i] = ( + i - int(round(thrStartTime / testRate)) - 2 + 1.0 + ) * (macros.NANO2SEC * testRate) + if ( + i + > int( + round( + (thrStartTime + ramplength * 1.0 / macros.NANO2SEC) + / testRate + ) + ) + + 1 + and i + < int(round((thrStartTime + thrDurationTime) / testRate)) + + 2 + ): + RampFunction[i] = 1.0 + if ( + i + > int(round((thrStartTime + thrDurationTime) / testRate)) + + 1 + and i + < int( + round( + ( + thrStartTime + + thrDurationTime + + ramplength * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 2 + ): + RampFunction[i] = 1.0 - ( + i + - int( + round((thrStartTime + thrDurationTime) / testRate) + ) + - 2 + + 1.0 + ) * (macros.NANO2SEC * testRate) + if ( + i + > int( + round( + ( + thrStartTime + + thrDurationTime + + ramplength * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 1 + ): RampFunction[i] = 0.0 - if (i > int(round(thrStartTime / testRate)) + 1 and i < int(round((thrStartTime + ramplength*1.0/macros.NANO2SEC)/ testRate)) + 2) : #ramp up - RampFunction[i] = (i-int(round(thrStartTime / testRate)) - 2 + 1.0) * (macros.NANO2SEC*testRate) - if (i > int(round((thrStartTime + ramplength*1.0/macros.NANO2SEC)/ testRate)) + 1 and i < int(round((thrStartTime + thrDurationTime) / testRate)) + 2): - RampFunction[i]=1.0 - if (i > int(round((thrStartTime + thrDurationTime) / testRate)) + 1 and i < int(round((thrStartTime + thrDurationTime+ ramplength*1.0/macros.NANO2SEC) / testRate)) + 2): - RampFunction[i] = 1.0 - (i - int(round((thrStartTime + thrDurationTime) / testRate))-2 + 1.0) *(macros.NANO2SEC*testRate) - if (i > int(round((thrStartTime + thrDurationTime+ ramplength*1.0/macros.NANO2SEC) / testRate)) + 1): - RampFunction[i] = 0. else: for i in range(np.shape(thrForce)[0]): - if i int(round(thrStartTime / testRate)) + 1 + and i + < int(round((thrStartTime + thrDurationTime) / testRate)) + + 2 + ): # ramp up + RampFunction[i] = ( + i - int(round(thrStartTime / testRate)) - 2 + 1.0 + ) * (macros.NANO2SEC * testRate) + if ( + i + > int(round((thrStartTime + thrDurationTime) / testRate)) + + 1 + and i + < int( + round((thrStartTime + 2 * thrDurationTime) / testRate) + ) + + 2 + ): + RampFunction[i] = RampFunction[ + int(round((thrStartTime + thrDurationTime) / testRate)) + + 1 + ] - ( + i + - int( + round((thrStartTime + thrDurationTime) / testRate) + ) + - 2 + + 1.0 + ) * (macros.NANO2SEC * testRate) + if ( + i + > int( + round( + ( + thrStartTime + + thrDurationTime + + ramplength * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 1 + ): RampFunction[i] = 0.0 - if (i > int(round(thrStartTime / testRate)) + 1 and i < int(round((thrStartTime + thrDurationTime)/ testRate)) + 2) : #ramp up - RampFunction[i] = (i-int(round(thrStartTime / testRate)) - 2 + 1.0) * (macros.NANO2SEC*testRate) - if (i > int(round((thrStartTime + thrDurationTime) / testRate)) + 1 and i < int(round((thrStartTime + 2*thrDurationTime) / testRate)) + 2): - RampFunction[i] = RampFunction[int(round((thrStartTime + thrDurationTime) / testRate)) + 1] - (i - int(round((thrStartTime + thrDurationTime) / testRate))-2 + 1.0) *(macros.NANO2SEC*testRate) - if (i > int(round((thrStartTime + thrDurationTime+ ramplength*1.0/macros.NANO2SEC) / testRate)) + 1): - RampFunction[i] = 0. - # Create expected Force to test against thrForce expMDot = np.zeros([np.shape(np.array(mDotData))[0], 1]) - for i in range(np.shape(RampFunction)[0]- (int(round(thrStartTime/testRate))+1)): - if (i>0 and RampFunction[i + int(round(thrStartTime/testRate))+1]!=0.): + for i in range( + np.shape(RampFunction)[0] + - (int(round(thrStartTime / testRate)) + 1) + ): + if ( + i > 0 + and RampFunction[i + int(round(thrStartTime / testRate)) + 1] + != 0.0 + ): expMDot[i, 0] = thrustNumber / (g * Isp) - for i in range(np.shape(thrForce)[0]): # Thrust fires 2 times steps after the pause of sim and restart - if (i > int(round(thrStartTime / testRate)) + 1 and i < int(round((thrStartTime + thrDurationTime+ ramplength*1.0/macros.NANO2SEC) / testRate)) + 2): - expectedpoints[0:3, i] = dir1*RampFunction[i] + for i in range( + np.shape(thrForce)[0] + ): # Thrust fires 2 times steps after the pause of sim and restart + if ( + i > int(round(thrStartTime / testRate)) + 1 + and i + < int( + round( + ( + thrStartTime + + thrDurationTime + + ramplength * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 2 + ): + expectedpoints[0:3, i] = dir1 * RampFunction[i] # Modify expected values for comparison and define errorTolerance TruthForce = np.transpose(expectedpoints) - ErrTolerance = 10E-9 + ErrTolerance = 10e-9 # Compare Force values and M-dot thrForce = np.delete(thrForce, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthForce, thrForce, ErrTolerance, "Force", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + TruthForce, + thrForce, + ErrTolerance, + "Force", + testFailCount, + testMessages, + ) mDotData = np.delete(mDotData, 0, axis=1) # remove time column for i in range(0, len(np.array(mDotData))): - if not unitTestSupport.isArrayEqual(np.array(mDotData)[i, :], expMDot[i, :], 1, ErrTolerance): + if not unitTestSupport.isArrayEqual( + np.array(mDotData)[i, :], expMDot[i, :], 1, ErrTolerance + ): testFailCount += 1 - testMessages.append('M dot failure') + testMessages.append("M dot failure") # Create expected Torque to test against thrTorque expectedpointstor = np.zeros([3, np.shape(thrTorque)[0]]) - for i in range(np.shape(thrForce)[0]): # Thrust fires 2 times steps after the pause of sim and restart - if (i > int(round(thrStartTime / testRate)) + 1 and i < int(round((thrStartTime + thrDurationTime+ ramplength*1.0/macros.NANO2SEC) / testRate)) + 2): - expectedpointstor[0:3, i] = (np.cross(loc1,dir1) + swirlTorque * dir1) * RampFunction[i] + for i in range( + np.shape(thrForce)[0] + ): # Thrust fires 2 times steps after the pause of sim and restart + if ( + i > int(round(thrStartTime / testRate)) + 1 + and i + < int( + round( + ( + thrStartTime + + thrDurationTime + + ramplength * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 2 + ): + expectedpointstor[0:3, i] = ( + np.cross(loc1, dir1) + swirlTorque * dir1 + ) * RampFunction[i] # Define errorTolerance TruthTorque = np.transpose(expectedpointstor) - ErrTolerance = 10E-9 + ErrTolerance = 10e-9 # Compare Torque values # Compare Force values thrTorque = np.delete(thrTorque, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthTorque, thrTorque, ErrTolerance, - "Torque", testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + TruthTorque, + thrTorque, + ErrTolerance, + "Torque", + testFailCount, + testMessages, + ) if cutoff == "ON": COtime = 0.2 COrestart = 0.3 executeSimRun(TotalSim, thrusterSet, testRate, int(thrStartTime)) - ThrustMessage.OnTimeRequest = [COtime * 10.] - thrCmdMsg.write(ThrustMessage, TotalSim.TotalSim.CurrentNanos+testRate) - executeSimRun(TotalSim, thrusterSet, testRate, int(COtime * 1.0 / macros.NANO2SEC)) + ThrustMessage.OnTimeRequest = [COtime * 10.0] + thrCmdMsg.write( + ThrustMessage, TotalSim.TotalSim.CurrentNanos + testRate + ) + executeSimRun( + TotalSim, thrusterSet, testRate, int(COtime * 1.0 / macros.NANO2SEC) + ) ThrustMessage.OnTimeRequest = [COrestart] - thrCmdMsg.write(ThrustMessage, TotalSim.TotalSim.CurrentNanos+testRate) - executeSimRun(TotalSim, thrusterSet, testRate, int(COrestart * 1.0 / macros.NANO2SEC + sparetime)) + thrCmdMsg.write( + ThrustMessage, TotalSim.TotalSim.CurrentNanos + testRate + ) + executeSimRun( + TotalSim, + thrusterSet, + testRate, + int(COrestart * 1.0 / macros.NANO2SEC + sparetime), + ) # Extract log variables and plot the results - thrForce = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.forceExternal_B) - thrTorque = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.torqueExternalPntB_B) - mDotData = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.mDotTotal) + thrForce = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.forceExternal_B + ) + thrTorque = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.torqueExternalPntB_B + ) + mDotData = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.mDotTotal + ) mDotData = fixMDotData(mDotData) - PlotName = ("Ramp_" + str(rampsteps) + "steps_Cutoff" + cutoff + "_" + str(int(duration)) + "s" + - "_testRate" + str(int(1. / (testRate * macros.NANO2SEC))) + "_Swirl" + str(int(swirlTorque)) + - "_BlowDown" + blowDown) - PlotTitle = ("All Forces and Torques, with a " + str(rampsteps) + " step Ramp, thrust for " + - str(int(duration)) + "s. Cutoff " + cutoff + ", testRate" + - str(int(1. / (testRate * macros.NANO2SEC))) + ", swirlTorque " + str(int(swirlTorque)) + - ", blowDown " + blowDown) - - snippetName = ("Snippet" + "Ramp_" + str(rampsteps) + "steps_Cutoff" + cutoff + "_Rate" + - str(int(1. / (testRate * macros.NANO2SEC))) + "_Cutoff" + cutoff + "_Swirl" + - str(int(swirlTorque)) + "_BlowDown" + blowDown) - texSnippet = ("We test the ramped thrust with " + str(rampsteps) + - " incremental steps. The single thruster is set at the default " + str(int(long_angle)) + - r"$^\circ$ off the x-axis " + str(int(lat_angle)) + - r"$^\circ$ off the z-axis, at $\bm r = \left(" + str(loc1[0]) + "," + str(loc1[1]) + "," + - str(loc1[2]) + r"\right)$. The thrust is set for " + str(duration) + - " seconds with a test rate of " + str(int(1. / (testRate * macros.NANO2SEC))) + - " steps per second. The Cutoff test is " + cutoff + ", swirl torque is set to " + - str(int(swirlTorque)) + " Newton meters, and blow down effects are " + blowDown + ".") + PlotName = ( + "Ramp_" + + str(rampsteps) + + "steps_Cutoff" + + cutoff + + "_" + + str(int(duration)) + + "s" + + "_testRate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + "_Swirl" + + str(int(swirlTorque)) + + "_BlowDown" + + blowDown + ) + PlotTitle = ( + "All Forces and Torques, with a " + + str(rampsteps) + + " step Ramp, thrust for " + + str(int(duration)) + + "s. Cutoff " + + cutoff + + ", testRate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + ", swirlTorque " + + str(int(swirlTorque)) + + ", blowDown " + + blowDown + ) + + snippetName = ( + "Snippet" + + "Ramp_" + + str(rampsteps) + + "steps_Cutoff" + + cutoff + + "_Rate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + "_Cutoff" + + cutoff + + "_Swirl" + + str(int(swirlTorque)) + + "_BlowDown" + + blowDown + ) + texSnippet = ( + "We test the ramped thrust with " + + str(rampsteps) + + " incremental steps. The single thruster is set at the default " + + str(int(long_angle)) + + r"$^\circ$ off the x-axis " + + str(int(lat_angle)) + + r"$^\circ$ off the z-axis, at $\bm r = \left(" + + str(loc1[0]) + + "," + + str(loc1[1]) + + "," + + str(loc1[2]) + + r"\right)$. The thrust is set for " + + str(duration) + + " seconds with a test rate of " + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + " steps per second. The Cutoff test is " + + cutoff + + ", swirl torque is set to " + + str(int(swirlTorque)) + + " Newton meters, and blow down effects are " + + blowDown + + "." + ) unitTestSupport.writeTeXSnippet(snippetName, texSnippet, path) plt.figure(55) plt.clf() - plt.plot(thrForce[:, 0] * 1.0E-9, thrForce[:, 1], 'b', label='x Force') - plt.plot(thrTorque[:, 0] * 1.0E-9, thrTorque[:, 1], 'b--', label='x Torque') - plt.plot(thrForce[:, 0] * 1.0E-9, thrForce[:, 2], 'g', label='y Force') - plt.plot(thrTorque[:, 0] * 1.0E-9, thrTorque[:, 2], 'g--', label='y Torque') - plt.plot(thrForce[:, 0] * 1.0E-9, thrForce[:, 3], 'r', label='z Force') - plt.plot(thrTorque[:, 0] * 1.0E-9, thrTorque[:, 3], 'r--', label='z Torque') - plt.legend(loc='upper right') - plt.xlabel('Time(s)') + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 1], "b", label="x Force") + plt.plot( + thrTorque[:, 0] * 1.0e-9, thrTorque[:, 1], "b--", label="x Torque" + ) + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 2], "g", label="y Force") + plt.plot( + thrTorque[:, 0] * 1.0e-9, thrTorque[:, 2], "g--", label="y Torque" + ) + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 3], "r", label="z Force") + plt.plot( + thrTorque[:, 0] * 1.0e-9, thrTorque[:, 3], "r--", label="z Torque" + ) + plt.legend(loc="upper right") + plt.xlabel("Time(s)") plt.ylim(-1.5, 2) - plt.legend(loc='upper left') + plt.legend(loc="upper left") unitTestSupport.writeFigureLaTeX(PlotName, PlotTitle, plt, format, path) if show_plots == True: plt.show() - plt.close('all') + plt.close("all") # Create expected Force to test against thrForce expectedpoints = np.zeros([3, np.shape(thrForce)[0]]) - RampFunction= np.zeros([np.shape(thrForce)[0]]) + RampFunction = np.zeros([np.shape(thrForce)[0]]) ramplength = 0.5 for i in range(np.shape(thrForce)[0]): - if i int(round(thrStartTime / testRate)) + 1 + and i + < int( + round( + (thrStartTime + ramplength * 1.0 / macros.NANO2SEC) + / testRate + ) + ) + + 2 + ): # ramp up + RampFunction[i] = ( + i - int(round(thrStartTime / testRate)) - 2 + 1.0 + ) * (macros.NANO2SEC * testRate) + if ( + i + > int( + round( + (thrStartTime + ramplength * 1.0 / macros.NANO2SEC) + / testRate + ) + ) + + 1 + and i + < int( + round( + (thrStartTime + 2 * ramplength * 1.0 / macros.NANO2SEC) + / testRate + ) + ) + + 2 + ): + RampFunction[i] = RampFunction[ + int( + round( + (thrStartTime + ramplength * 1.0 / macros.NANO2SEC) + / testRate + ) + ) + + 1 + ] - ( + i + - int( + round( + (thrStartTime + ramplength * 1.0 / macros.NANO2SEC) + / testRate + ) + ) + - 2 + + 1.0 + ) * (macros.NANO2SEC * testRate) + if ( + i + > int( + round( + (thrStartTime + 2 * ramplength * 1.0 / macros.NANO2SEC) + / testRate + ) + ) + + 1 + ): RampFunction[i] = 0.0 - if (i > int(round(thrStartTime / testRate)) + 1 and i < int(round((thrStartTime + ramplength*1.0/macros.NANO2SEC)/ testRate)) + 2) : #ramp up - RampFunction[i] = (i-int(round(thrStartTime / testRate)) - 2 + 1.0) * (macros.NANO2SEC*testRate) - if (i > int(round((thrStartTime + ramplength*1.0/macros.NANO2SEC) / testRate)) + 1 and i < int(round((thrStartTime + 2*ramplength*1.0/macros.NANO2SEC) / testRate)) + 2): - RampFunction[i] = RampFunction[int(round((thrStartTime + ramplength*1.0/macros.NANO2SEC) / testRate)) + 1] - (i - int(round((thrStartTime + ramplength*1.0/macros.NANO2SEC) / testRate))-2 + 1.0) *(macros.NANO2SEC*testRate) - if (i > int(round((thrStartTime + 2*ramplength*1.0/macros.NANO2SEC) / testRate)) + 1): - RampFunction[i] = 0. # Create expected Force to test against thrForce expMDot = np.zeros([np.shape(np.array(mDotData))[0], 1]) - for i in range(np.shape(RampFunction)[0]- (int(round(thrStartTime/testRate))+1)): - if (i>0 and RampFunction[i + int(round(thrStartTime/testRate))+1]!=0.): + for i in range( + np.shape(RampFunction)[0] + - (int(round(thrStartTime / testRate)) + 1) + ): + if ( + i > 0 + and RampFunction[i + int(round(thrStartTime / testRate)) + 1] + != 0.0 + ): expMDot[i, 0] = thrustNumber / (g * Isp) - for i in range(np.shape(thrForce)[0]):# Thrust fires 2 times steps after the pause of sim and restart - if (i > int(round(thrStartTime / testRate)) + 1 and i < int(round((thrStartTime + thrDurationTime+ ramplength*1.0/macros.NANO2SEC) / testRate)) + 2): - expectedpoints[0:3, i] = dir1*RampFunction[i] + for i in range( + np.shape(thrForce)[0] + ): # Thrust fires 2 times steps after the pause of sim and restart + if ( + i > int(round(thrStartTime / testRate)) + 1 + and i + < int( + round( + ( + thrStartTime + + thrDurationTime + + ramplength * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 2 + ): + expectedpoints[0:3, i] = dir1 * RampFunction[i] # Modify expected values for comparison and define errorTolerance TruthForce = np.transpose(expectedpoints) - ErrTolerance = 10E-9 + ErrTolerance = 10e-9 # Compare Force values thrForce = np.delete(thrForce, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthForce, thrForce, ErrTolerance, "Force", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + TruthForce, + thrForce, + ErrTolerance, + "Force", + testFailCount, + testMessages, + ) mDotData = np.delete(mDotData, 0, axis=1) # remove time column for i in range(0, len(np.array(mDotData))): - if not unitTestSupport.isArrayEqual(np.array(mDotData)[i, :], expMDot[i, :], 1, ErrTolerance): + if not unitTestSupport.isArrayEqual( + np.array(mDotData)[i, :], expMDot[i, :], 1, ErrTolerance + ): testFailCount += 1 - testMessages.append('M dot failure') + testMessages.append("M dot failure") # Create expected Torque to test against thrTorque expectedpointstor = np.zeros([3, np.shape(thrTorque)[0]]) - for i in range(np.shape(thrForce)[0]): # Thrust fires 2 times steps after the pause of sim and restart - if (i > int(round(thrStartTime / testRate)) + 1 and i < int(round((thrStartTime + thrDurationTime+ ramplength*1.0/macros.NANO2SEC) / testRate)) + 2): - expectedpointstor[0:3, i] = (np.cross(loc1,dir1) + swirlTorque * dir1) * RampFunction[i] + for i in range( + np.shape(thrForce)[0] + ): # Thrust fires 2 times steps after the pause of sim and restart + if ( + i > int(round(thrStartTime / testRate)) + 1 + and i + < int( + round( + ( + thrStartTime + + thrDurationTime + + ramplength * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 2 + ): + expectedpointstor[0:3, i] = ( + np.cross(loc1, dir1) + swirlTorque * dir1 + ) * RampFunction[i] # Define errorTolerance TruthTorque = np.transpose(expectedpointstor) - ErrTolerance = 10E-9 + ErrTolerance = 10e-9 # Compare Torque values # Compare Force values thrTorque = np.delete(thrTorque, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthTorque, thrTorque, ErrTolerance, - "Torque", testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + TruthTorque, + thrTorque, + ErrTolerance, + "Torque", + testFailCount, + testMessages, + ) if rampDown == "ON": RDrestart = 0.2 @@ -661,82 +1519,334 @@ def unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_an executeSimRun(TotalSim, thrusterSet, testRate, int(thrStartTime)) ThrustMessage.OnTimeRequest = [RDstart] - thrCmdMsg.write(ThrustMessage, TotalSim.TotalSim.CurrentNanos+testRate) - executeSimRun(TotalSim, thrusterSet, testRate, int((RDstart+ RDrestart) * 1.0 / macros.NANO2SEC)) + thrCmdMsg.write(ThrustMessage, TotalSim.TotalSim.CurrentNanos + testRate) + executeSimRun( + TotalSim, + thrusterSet, + testRate, + int((RDstart + RDrestart) * 1.0 / macros.NANO2SEC), + ) ThrustMessage.OnTimeRequest = [RDlength] - thrCmdMsg.write(ThrustMessage, TotalSim.TotalSim.CurrentNanos+testRate) - executeSimRun(TotalSim, thrusterSet, testRate, int(RDlength * 1.0 / macros.NANO2SEC + sparetime)) + thrCmdMsg.write(ThrustMessage, TotalSim.TotalSim.CurrentNanos + testRate) + executeSimRun( + TotalSim, + thrusterSet, + testRate, + int(RDlength * 1.0 / macros.NANO2SEC + sparetime), + ) # Extract log variables and plot the results - thrForce = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.forceExternal_B) - thrTorque = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.torqueExternalPntB_B) - mDotData = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.mDotTotal) + thrForce = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.forceExternal_B + ) + thrTorque = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.torqueExternalPntB_B + ) + mDotData = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.mDotTotal + ) mDotData = fixMDotData(mDotData) - PlotName = ("Ramp_" + str(rampsteps) + "steps_Cutoff" + cutoff + "rampDown" + rampDown+"_testRate" + - str(int(1. / (testRate * macros.NANO2SEC))) + "_Swirl" + str(int(swirlTorque)) + "_BlowDown" + - blowDown) - PlotTitle = ("All Forces and Torques, with a " + str(rampsteps) + " step Ramp, Cutoff " + cutoff + - ", RampDown" + rampDown + " testRate" + str(int(1. / (testRate * macros.NANO2SEC))) + - ", swirlTorque " + str(int(swirlTorque)) + ", blowDown " + blowDown) - - snippetName = ("Snippet" + "Ramp_" + str(rampsteps) + "steps_Cutoff" + cutoff + "_Rate" + - str(int(1. / (testRate * macros.NANO2SEC))) + "rampDown" + rampDown + "_Swirl" + - str(int(swirlTorque)) + "_BlowDown" + blowDown) - texSnippet = ("We test the ramped thrust with " + str(rampsteps) + - " incremental steps. The single thruster is set at the default " + str(int(long_angle)) + - r"$^\circ$ off the x-axis " + str(int(lat_angle)) + - r"$^\circ$ off the z-axis, at $\bm r = \left(" + str(loc1[0]) + "," + str(loc1[1]) + "," + - str(loc1[2]) + r"\right)$. The thrust is set for " + str(RDstart) + - " seconds initially with a test rate of " + str(int(1. / (testRate * macros.NANO2SEC))) + - " steps per second. The Cutoff test is " + cutoff + ", the RampDown test is " + rampDown + - ", swirl torque is set to " + str(int(swirlTorque)) + " Newton meters, and blow down effects are " - + blowDown + ".") + PlotName = ( + "Ramp_" + + str(rampsteps) + + "steps_Cutoff" + + cutoff + + "rampDown" + + rampDown + + "_testRate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + "_Swirl" + + str(int(swirlTorque)) + + "_BlowDown" + + blowDown + ) + PlotTitle = ( + "All Forces and Torques, with a " + + str(rampsteps) + + " step Ramp, Cutoff " + + cutoff + + ", RampDown" + + rampDown + + " testRate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + ", swirlTorque " + + str(int(swirlTorque)) + + ", blowDown " + + blowDown + ) + + snippetName = ( + "Snippet" + + "Ramp_" + + str(rampsteps) + + "steps_Cutoff" + + cutoff + + "_Rate" + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + "rampDown" + + rampDown + + "_Swirl" + + str(int(swirlTorque)) + + "_BlowDown" + + blowDown + ) + texSnippet = ( + "We test the ramped thrust with " + + str(rampsteps) + + " incremental steps. The single thruster is set at the default " + + str(int(long_angle)) + + r"$^\circ$ off the x-axis " + + str(int(lat_angle)) + + r"$^\circ$ off the z-axis, at $\bm r = \left(" + + str(loc1[0]) + + "," + + str(loc1[1]) + + "," + + str(loc1[2]) + + r"\right)$. The thrust is set for " + + str(RDstart) + + " seconds initially with a test rate of " + + str(int(1.0 / (testRate * macros.NANO2SEC))) + + " steps per second. The Cutoff test is " + + cutoff + + ", the RampDown test is " + + rampDown + + ", swirl torque is set to " + + str(int(swirlTorque)) + + " Newton meters, and blow down effects are " + + blowDown + + "." + ) unitTestSupport.writeTeXSnippet(snippetName, texSnippet, path) plt.figure(55) plt.clf() - plt.plot(thrForce[:, 0] * 1.0E-9, thrForce[:, 1], 'b', label='x Force') - plt.plot(thrTorque[:, 0] * 1.0E-9, thrTorque[:, 1], 'b--', label='x Torque') - plt.plot(thrForce[:, 0] * 1.0E-9, thrForce[:, 2], 'g', label='y Force') - plt.plot(thrTorque[:, 0] * 1.0E-9, thrTorque[:, 2], 'g--', label='y Torque') - plt.plot(thrForce[:, 0] * 1.0E-9, thrForce[:, 3], 'r', label='z Force') - plt.plot(thrTorque[:, 0] * 1.0E-9, thrTorque[:, 3], 'r--', label='z Torque') - plt.legend(loc='upper right') - plt.xlabel('Time(s)') + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 1], "b", label="x Force") + plt.plot(thrTorque[:, 0] * 1.0e-9, thrTorque[:, 1], "b--", label="x Torque") + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 2], "g", label="y Force") + plt.plot(thrTorque[:, 0] * 1.0e-9, thrTorque[:, 2], "g--", label="y Torque") + plt.plot(thrForce[:, 0] * 1.0e-9, thrForce[:, 3], "r", label="z Force") + plt.plot(thrTorque[:, 0] * 1.0e-9, thrTorque[:, 3], "r--", label="z Torque") + plt.legend(loc="upper right") + plt.xlabel("Time(s)") plt.ylim(-1.5, 2) - plt.legend(loc='upper left') + plt.legend(loc="upper left") unitTestSupport.writeFigureLaTeX(PlotName, PlotTitle, plt, format, path) if show_plots == True: plt.show() - plt.close('all') + plt.close("all") # Create expected Force to test against thrForce expectedpoints = np.zeros([3, np.shape(thrForce)[0]]) RampFunction = np.zeros([np.shape(thrForce)[0]]) - ramplength = 1. + ramplength = 1.0 for i in range(np.shape(thrForce)[0]): if i < int(round(thrStartTime / testRate)) + 2: RampFunction[i] = 0.0 - if (i > int(round(thrStartTime / testRate)) + 1 and i < int(round((thrStartTime + RDstart * 1.0 / macros.NANO2SEC) / testRate)) + 2): # ramp up - RampFunction[i] = (i - int(round(thrStartTime / testRate)) - 2 + 1.0) * (macros.NANO2SEC * testRate) - if (i > int(round((thrStartTime + RDstart * 1.0 / macros.NANO2SEC) / testRate)) + 1 and i < int(round((thrStartTime + (RDstart+RDrestart) * 1.0 / macros.NANO2SEC) / testRate)) + 2): - RampFunction[i] = RampFunction[int(round((thrStartTime + RDstart * 1.0 / macros.NANO2SEC) / testRate)) + 1] - (i - int(round((thrStartTime + (RDstart) * 1.0 / macros.NANO2SEC) / testRate)) - 2 + 1.0) * (macros.NANO2SEC * testRate) - if (i > int(round((thrStartTime + (RDstart+RDrestart) * 1.0 / macros.NANO2SEC) / testRate)) + 1 and i < int(round((thrStartTime + (RDstart+RDrestart+(1. -RDstart+RDrestart)) * 1.0 / macros.NANO2SEC) / testRate)) + 2): # ramp up - RampFunction[i] = RampFunction[int(round((thrStartTime + (RDstart+RDrestart) * 1.0 / macros.NANO2SEC) / testRate)) + 1]+ (i - int(round((thrStartTime+ (RDstart+RDrestart)* 1.0 / macros.NANO2SEC) / testRate)) - 2 + 1.0) * (macros.NANO2SEC * testRate) - if (i > int(round((thrStartTime + (RDstart+RDrestart+(1. -RDstart+RDrestart)) * 1.0 / macros.NANO2SEC) / testRate)) + 1 and i < int(round((thrStartTime + (RDstart+RDrestart + RDlength) * 1.0 / macros.NANO2SEC) / testRate)) + 2): + if ( + i > int(round(thrStartTime / testRate)) + 1 + and i + < int( + round( + (thrStartTime + RDstart * 1.0 / macros.NANO2SEC) / testRate + ) + ) + + 2 + ): # ramp up + RampFunction[i] = ( + i - int(round(thrStartTime / testRate)) - 2 + 1.0 + ) * (macros.NANO2SEC * testRate) + if ( + i + > int( + round( + (thrStartTime + RDstart * 1.0 / macros.NANO2SEC) / testRate + ) + ) + + 1 + and i + < int( + round( + ( + thrStartTime + + (RDstart + RDrestart) * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 2 + ): + RampFunction[i] = RampFunction[ + int( + round( + (thrStartTime + RDstart * 1.0 / macros.NANO2SEC) + / testRate + ) + ) + + 1 + ] - ( + i + - int( + round( + (thrStartTime + (RDstart) * 1.0 / macros.NANO2SEC) + / testRate + ) + ) + - 2 + + 1.0 + ) * (macros.NANO2SEC * testRate) + if ( + i + > int( + round( + ( + thrStartTime + + (RDstart + RDrestart) * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 1 + and i + < int( + round( + ( + thrStartTime + + (RDstart + RDrestart + (1.0 - RDstart + RDrestart)) + * 1.0 + / macros.NANO2SEC + ) + / testRate + ) + ) + + 2 + ): # ramp up + RampFunction[i] = RampFunction[ + int( + round( + ( + thrStartTime + + (RDstart + RDrestart) * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 1 + ] + ( + i + - int( + round( + ( + thrStartTime + + (RDstart + RDrestart) * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + - 2 + + 1.0 + ) * (macros.NANO2SEC * testRate) + if ( + i + > int( + round( + ( + thrStartTime + + (RDstart + RDrestart + (1.0 - RDstart + RDrestart)) + * 1.0 + / macros.NANO2SEC + ) + / testRate + ) + ) + + 1 + and i + < int( + round( + ( + thrStartTime + + (RDstart + RDrestart + RDlength) + * 1.0 + / macros.NANO2SEC + ) + / testRate + ) + ) + + 2 + ): RampFunction[i] = 1.0 - if (i > int(round((thrStartTime + (RDstart+RDrestart + RDlength) * 1.0 / macros.NANO2SEC) / testRate)) + 1 and i < int(round((thrStartTime + (RDstart+RDrestart + RDlength+1) * 1.0 / macros.NANO2SEC) / testRate)) + 2): - RampFunction[i] = 1.0 - (i - int(round((thrStartTime + (RDstart+RDrestart + RDlength) * 1.0 / macros.NANO2SEC) / testRate)) - 2 + 1.0) * (macros.NANO2SEC * testRate) - if (i > int(round((thrStartTime + (RDstart+RDrestart + RDlength+ ramplength) * 1.0 / macros.NANO2SEC) / testRate)) + 1): - RampFunction[i] = 0. + if ( + i + > int( + round( + ( + thrStartTime + + (RDstart + RDrestart + RDlength) + * 1.0 + / macros.NANO2SEC + ) + / testRate + ) + ) + + 1 + and i + < int( + round( + ( + thrStartTime + + (RDstart + RDrestart + RDlength + 1) + * 1.0 + / macros.NANO2SEC + ) + / testRate + ) + ) + + 2 + ): + RampFunction[i] = 1.0 - ( + i + - int( + round( + ( + thrStartTime + + (RDstart + RDrestart + RDlength) + * 1.0 + / macros.NANO2SEC + ) + / testRate + ) + ) + - 2 + + 1.0 + ) * (macros.NANO2SEC * testRate) + if ( + i + > int( + round( + ( + thrStartTime + + (RDstart + RDrestart + RDlength + ramplength) + * 1.0 + / macros.NANO2SEC + ) + / testRate + ) + ) + + 1 + ): + RampFunction[i] = 0.0 # Create expected Force to test against thrForce expMDot = np.zeros([np.shape(np.array(mDotData))[0], 1]) - for i in range(1,np.shape(RampFunction)[0] - (int(round(thrStartTime / testRate))+1)): - if (RampFunction[i + int(round(thrStartTime / testRate))+1] != 0.): + for i in range( + 1, np.shape(RampFunction)[0] - (int(round(thrStartTime / testRate)) + 1) + ): + if RampFunction[i + int(round(thrStartTime / testRate)) + 1] != 0.0: expMDot[i, 0] = thrustNumber / (g * Isp) - expMDot[i+1, 0] = thrustNumber / (g * Isp) # The way the last ramp is set up, we need another mdot value + expMDot[i + 1, 0] = thrustNumber / ( + g * Isp + ) # The way the last ramp is set up, we need another mdot value PlotName = "Ramp_function" PlotTitle = "Example of ramp function" @@ -744,53 +1854,90 @@ def unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_an plt.figure(11) plt.clf() plt.plot(thrForce[:, 0] * macros.NANO2SEC, RampFunction) - plt.xlabel('Time(s)') - plt.ylabel('Ramp(-)') + plt.xlabel("Time(s)") + plt.ylabel("Ramp(-)") plt.ylim(-1.5, 2) unitTestSupport.writeFigureLaTeX(PlotName, PlotTitle, plt, format, path) if show_plots == True: plt.show() - plt.close('all') - - for i in range(np.shape(thrForce)[0]): # Thrust fires 2 times steps after the pause of sim and restart - if (i > int(round(thrStartTime / testRate)) + 1 and i < int( - round((thrStartTime + thrDurationTime + ramplength * 1.0 / macros.NANO2SEC) / testRate)) + 2): - expectedpoints[0:3, i] = dir1*RampFunction[i] - + plt.close("all") + + for i in range( + np.shape(thrForce)[0] + ): # Thrust fires 2 times steps after the pause of sim and restart + if ( + i > int(round(thrStartTime / testRate)) + 1 + and i + < int( + round( + ( + thrStartTime + + thrDurationTime + + ramplength * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 2 + ): + expectedpoints[0:3, i] = dir1 * RampFunction[i] # Modify expected values for comparison and define errorTolerance TruthForce = np.transpose(expectedpoints) - ErrTolerance = 10E-9 + ErrTolerance = 10e-9 # Compare Force values thrForce = np.delete(thrForce, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthForce, thrForce, ErrTolerance, "Force", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + TruthForce, thrForce, ErrTolerance, "Force", testFailCount, testMessages + ) mDotData = np.delete(mDotData, 0, axis=1) # remove time column for i in range(0, len(np.array(mDotData))): - if not unitTestSupport.isArrayEqual(np.array(mDotData)[i, :], expMDot[i, :], 1, ErrTolerance): + if not unitTestSupport.isArrayEqual( + np.array(mDotData)[i, :], expMDot[i, :], 1, ErrTolerance + ): testFailCount += 1 - testMessages.append('M dot failure') + testMessages.append("M dot failure") # Create expected Torque to test against thrTorque expectedpointstor = np.zeros([3, np.shape(thrTorque)[0]]) - for i in range(np.shape(thrForce)[0]): # Thrust fires 2 times steps after the pause of sim and restart - if (i > int(round(thrStartTime / testRate)) + 1 and i < int( - round((thrStartTime + thrDurationTime + ramplength * 1.0 / macros.NANO2SEC) / testRate)) + 2): - expectedpointstor[0:3, i] = (np.cross(loc1, dir1) + swirlTorque * dir1) * RampFunction[i] - + for i in range( + np.shape(thrForce)[0] + ): # Thrust fires 2 times steps after the pause of sim and restart + if ( + i > int(round(thrStartTime / testRate)) + 1 + and i + < int( + round( + ( + thrStartTime + + thrDurationTime + + ramplength * 1.0 / macros.NANO2SEC + ) + / testRate + ) + ) + + 2 + ): + expectedpointstor[0:3, i] = ( + np.cross(loc1, dir1) + swirlTorque * dir1 + ) * RampFunction[i] # Define errorTolerance TruthTorque = np.transpose(expectedpointstor) - ErrTolerance = 10E-9 + ErrTolerance = 10e-9 # Compare Torque values # Compare Force values thrTorque = np.delete(thrTorque, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthTorque, thrTorque, ErrTolerance, - "Torque", testFailCount, testMessages) - - + testFailCount, testMessages = unitTestSupport.compareArray( + TruthTorque, + thrTorque, + ErrTolerance, + "Torque", + testFailCount, + testMessages, + ) if testFailCount == 0: print("PASSED") @@ -801,7 +1948,22 @@ def unitThrusters(testFixture, show_plots, ramp, thrustNumber, duration, long_an # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": - unitThrusters(ResultsStore(), True, "OFF", 1, 5.0, 30., 15.,[[1.125], [0.5], [2.0]], 1E8, "OFF", "OFF", 0.0, "ON") + unitThrusters( + ResultsStore(), + True, + "OFF", + 1, + 5.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 1e8, + "OFF", + "OFF", + 0.0, + "ON", + ) diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py index 44242298f4..d317ddc968 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_dynamics_attached_body.py @@ -23,9 +23,14 @@ 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, RigidBodyKinematics as rbk +splitPath = path.split("simulation") + +from Basilisk.utilities import ( + SimulationBaseClass, + unitTestSupport, + macros, + RigidBodyKinematics as rbk, +) from Basilisk.simulation import spacecraft, thrusterDynamicEffector from Basilisk.architecture import messaging from Basilisk.architecture import sysModel @@ -40,9 +45,17 @@ def thrusterEffectorAllTests(show_plots): # @pytest.mark.xfail(True) -@pytest.mark.parametrize("long_angle, lat_angle, location, rate", [ - (30., 15., [[1.125], [0.5], [2.0]], macros.sec2nano(0.01)), # 1 thruster, thrust on -]) +@pytest.mark.parametrize( + "long_angle, lat_angle, location, rate", + [ + ( + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + macros.sec2nano(0.01), + ), # 1 thruster, thrust on + ], +) # provide a unique test method name, starting with test_ def test_unitThrusters(show_plots, long_angle, lat_angle, location, rate): r""" @@ -58,7 +71,9 @@ def test_unitThrusters(show_plots, long_angle, lat_angle, location, rate): everything matches accordingly. """ # each test method requires a single assert method to be called - [testResults, testMessage] = unitThrusters(show_plots, long_angle, lat_angle, location, rate) + [testResults, testMessage] = unitThrusters( + show_plots, long_angle, lat_angle, location, rate + ) assert testResults < 1, testMessage @@ -95,8 +110,16 @@ def unitThrusters(show_plots, long_angle, lat_angle, location, rate): scObject.hub.mHub = 750.0 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + 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.1], [0.2], [-0.3]] scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] @@ -115,8 +138,11 @@ def unitThrusters(show_plots, long_angle, lat_angle, location, rate): lat_angle_rad = lat_angle_deg * math.pi / 180.0 thruster = thrusterDynamicEffector.THRSimConfig() thruster.thrLoc_B = location # Parametrized location for thruster - thruster.thrDir_B = [[math.cos(long_angle_rad) * math.cos(lat_angle_rad)], - [math.sin(long_angle_rad) * math.cos(lat_angle_rad)], [math.sin(lat_angle_rad)]] + thruster.thrDir_B = [ + [math.cos(long_angle_rad) * math.cos(lat_angle_rad)], + [math.sin(long_angle_rad) * math.cos(lat_angle_rad)], + [math.sin(lat_angle_rad)], + ] thruster.MaxThrust = 10.0 thruster.steadyIsp = 226.7 thruster.MinOnTime = 0.006 @@ -137,8 +163,12 @@ def unitThrusters(show_plots, long_angle, lat_angle, location, rate): thrusterSet.addThruster(thruster, pyModule.bodyOutMsg) # Define the location and direction with respect to the platform - loc = np.array([thruster.thrLoc_B[0][0], thruster.thrLoc_B[1][0], thruster.thrLoc_B[2][0]]) - dir = np.array([thruster.thrDir_B[0][0], thruster.thrDir_B[1][0], thruster.thrDir_B[2][0]]) + loc = np.array( + [thruster.thrLoc_B[0][0], thruster.thrLoc_B[1][0], thruster.thrLoc_B[2][0]] + ) + dir = np.array( + [thruster.thrDir_B[0][0], thruster.thrDir_B[1][0], thruster.thrDir_B[2][0]] + ) # Update the direction and location of the thruster to the hub dir = dcm_BF.dot(dir) @@ -180,8 +210,12 @@ def unitThrusters(show_plots, long_angle, lat_angle, location, rate): TotalSim.ExecuteSimulation() # Gather the Force, Torque and Mass Rate results - thrForce = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.forceExternal_B) - thrTorque = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.torqueExternalPntB_B) + thrForce = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.forceExternal_B + ) + thrTorque = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.torqueExternalPntB_B + ) # Generate the truth data (force, torque and mass rate) expectedThrustData = np.zeros([3, np.shape(thrForce)[0]]) @@ -200,17 +234,19 @@ def unitThrusters(show_plots, long_angle, lat_angle, location, rate): # Modify expected values for comparison and define errorTolerance TruthForce = np.transpose(expectedThrustData) TruthTorque = np.transpose(expectedTorqueData) - ErrTolerance = 1E-3 + ErrTolerance = 1e-3 # Compare Force values thrForce = np.delete(thrForce, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthForce, thrForce, ErrTolerance, "Force", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + TruthForce, thrForce, ErrTolerance, "Force", testFailCount, testMessages + ) # Compare Torque values thrTorque = np.delete(thrTorque, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthTorque, thrTorque, ErrTolerance, "Torque", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + TruthTorque, thrTorque, ErrTolerance, "Torque", testFailCount, testMessages + ) if testFailCount == 0: print("PASSED") @@ -219,7 +255,7 @@ def unitThrusters(show_plots, long_angle, lat_angle, location, rate): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] class attachedBodyModule(sysModel.SysModel): @@ -269,4 +305,4 @@ def writeOutputMsg(self, CurrentSimNanos): if __name__ == "__main__": - unitThrusters(False, 30., 15., [[1.125], [0.5], [2.0]], macros.sec2nano(0.01)) + unitThrusters(False, 30.0, 15.0, [[1.125], [0.5], [2.0]], macros.sec2nano(0.01)) diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_integrated_sim.py b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_integrated_sim.py index 8c12e3c5c6..3db8225b51 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_integrated_sim.py +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/_UnitTest/test_thruster_integrated_sim.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -23,7 +22,9 @@ from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import simIncludeThruster -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import numpy @@ -36,6 +37,7 @@ def test_thrusterIntegratedTest(show_plots): [testResults, testMessage] = thrusterIntegratedTest(show_plots) assert testResults < 1, testMessage + def thrusterIntegratedTest(show_plots): """Module Unit Test""" # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -63,26 +65,32 @@ def thrusterIntegratedTest(show_plots): # add thruster devices thFactory = simIncludeThruster.thrusterFactory() TH1 = thFactory.create( - 'MOOG_Monarc_1', - [1,0,0], # location in B-frame - [0,1,0], # thruster force direction in B-frame - thrBlowDownCoeff=[0, 0, 0.9], # polynomial coefficients for fuel mass to thrust blow down model in descending order - ispBlowDownCoeff=[0, 0, 227.5] # polynomial coefficients for fuel mass to Isp blow down model in descending order + "MOOG_Monarc_1", + [1, 0, 0], # location in B-frame + [0, 1, 0], # thruster force direction in B-frame + thrBlowDownCoeff=[ + 0, + 0, + 0.9, + ], # polynomial coefficients for fuel mass to thrust blow down model in descending order + ispBlowDownCoeff=[ + 0, + 0, + 227.5, + ], # polynomial coefficients for fuel mass to Isp blow down model in descending order ) # create thruster object container and tie to spacecraft object thrustersDynamicEffector = thrusterDynamicEffector.ThrusterDynamicEffector() - thFactory.addToSpacecraft("Thrusters", - thrustersDynamicEffector, - scObject) + thFactory.addToSpacecraft("Thrusters", thrustersDynamicEffector, scObject) # create tank object container unitTestSim.fuelTankStateEffector = fuelTank.FuelTank() tankModel = fuelTank.FuelTankModelConstantVolume() unitTestSim.fuelTankStateEffector.setTankModel(tankModel) tankModel.propMassInit = 40.0 - tankModel.r_TcT_TInit = [[0.0],[0.0],[0.0]] - unitTestSim.fuelTankStateEffector.r_TB_B = [[0.0],[0.0],[0.0]] + tankModel.r_TcT_TInit = [[0.0], [0.0], [0.0]] + unitTestSim.fuelTankStateEffector.r_TB_B = [[0.0], [0.0], [0.0]] tankModel.radiusTankInit = 46.0 / 2.0 / 3.2808399 / 12.0 unitTestSim.fuelTankStateEffector.addThrusterSet(thrustersDynamicEffector) @@ -103,17 +111,27 @@ def thrusterIntegratedTest(show_plots): unitTestSim.earthGravBody = gravityEffector.GravBodyData() unitTestSim.earthGravBody.planetName = "earth_planet_data" - unitTestSim.earthGravBody.mu = 0.3986004415E+15 # meters! + unitTestSim.earthGravBody.mu = 0.3986004415e15 # meters! unitTestSim.earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([unitTestSim.earthGravBody]) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + [unitTestSim.earthGravBody] + ) # Define initial conditions of the spacecraft scObject.hub.mHub = 750.0 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + 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.1], [0.2], [-0.3]] scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] @@ -123,8 +141,7 @@ def thrusterIntegratedTest(show_plots): posRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubPosition) sigmaRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubSigma) - - stopTime = 60.0*10.0 + stopTime = 60.0 * 10.0 unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) unitTestSim.ExecuteSimulation() @@ -133,9 +150,13 @@ def thrusterIntegratedTest(show_plots): dataPos = [dataPos[0][0], dataPos[1][0], dataPos[2][0]] dataSigma = [dataSigma[0][0], dataSigma[1][0], dataSigma[2][0]] - truePos = [-6.7815933935338277e+06, 4.9468685979815889e+06, 5.4867416696776701e+06] + truePos = [-6.7815933935338277e06, 4.9468685979815889e06, 5.4867416696776701e06] - trueSigma = [1.4401781243854264e-01, -6.4168702021364002e-02, 3.0166086824900967e-01] + trueSigma = [ + 1.4401781243854264e-01, + -6.4168702021364002e-02, + 3.0166086824900967e-01, + ] accuracy = 1e-8 numpy.testing.assert_allclose(dataPos, truePos, rtol=accuracy, verbose=True) @@ -144,7 +165,12 @@ def thrusterIntegratedTest(show_plots): numpy.testing.assert_allclose(dataSigma, trueSigma, rtol=accuracy, verbose=True) accuracy = 1e-9 - numpy.testing.assert_allclose(thrustersDynamicEffector.fuelMass, fuelLog.fuelMass[-1], rtol=accuracy, verbose=True) + numpy.testing.assert_allclose( + thrustersDynamicEffector.fuelMass, + fuelLog.fuelMass[-1], + rtol=accuracy, + verbose=True, + ) if testFailCount == 0: print("PASSED: " + " Thruster Integrated Sim Test") @@ -153,7 +179,8 @@ def thrusterIntegratedTest(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": test_thrusterIntegratedTest(False) diff --git a/src/simulation/dynamics/Thrusters/thrusterStateEffector/_UnitTest/test_ThrusterStateEffectorUnit.py b/src/simulation/dynamics/Thrusters/thrusterStateEffector/_UnitTest/test_ThrusterStateEffectorUnit.py index 1055701dd1..141d81ed8f 100644 --- a/src/simulation/dynamics/Thrusters/thrusterStateEffector/_UnitTest/test_ThrusterStateEffectorUnit.py +++ b/src/simulation/dynamics/Thrusters/thrusterStateEffector/_UnitTest/test_ThrusterStateEffectorUnit.py @@ -23,9 +23,14 @@ 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, RigidBodyKinematics as rbk +splitPath = path.split("simulation") + +from Basilisk.utilities import ( + SimulationBaseClass, + unitTestSupport, + macros, + RigidBodyKinematics as rbk, +) from Basilisk.simulation import spacecraft, thrusterStateEffector from Basilisk.architecture import messaging from Basilisk.architecture import sysModel @@ -38,12 +43,12 @@ def __init__(self): def texSnippet(self): for i in range(len(self.PassFail)): - snippetName = 'Result' + str(i) - if self.PassFail[i] == 'PASSED': - textColor = 'ForestGreen' - elif self.PassFail[i] == 'FAILED': - textColor = 'Red' - texSnippet = r'\textcolor{' + textColor + '}{' + self.PassFail[i] + '}' + snippetName = "Result" + str(i) + if self.PassFail[i] == "PASSED": + textColor = "ForestGreen" + elif self.PassFail[i] == "FAILED": + textColor = "Red" + texSnippet = r"\textcolor{" + textColor + "}{" + self.PassFail[i] + "}" unitTestSupport.writeTeXSnippet(snippetName, texSnippet, path) @@ -57,22 +62,118 @@ def testFixture(): def thrusterEffectorAllTests(show_plots): [testResults, testMessage] = test_unitThrusters(show_plots) + # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(True) -@pytest.mark.parametrize("thrustNumber, initialConditions, duration, long_angle, lat_angle, location, swirlTorque, rate, attachBody", [ - (1, 0., 2.0, 30., 15., [[1.125], [0.5], [2.0]], 0.0, macros.sec2nano(0.01), "OFF"), # 1 thruster, thrust on - (1, 1., 0.0, 30., 15., [[1.125], [0.5], [2.0]], 0.0, macros.sec2nano(0.01), "OFF"), # 1 thruster, thrust off - (1, 0., 2.0, 60., -15., [[-1.125], [0.5], [-2.0]], 0.0, macros.sec2nano(0.01), "OFF"), # 1 thruster, thrust on, different orientation and location - (1, 1., 0.0, 60., -15., [[-1.125], [0.5], [-2.0]], 0.0, macros.sec2nano(0.01), "OFF"), # 1 thruster, thrust off, different orientation and location - (2, 0., 2.0, 30., 15., [[1.125], [0.5], [2.0]], 0.0, macros.sec2nano(0.01), "OFF"), # 2 thrusters, thrust on - (2, 1., 0.0, 30., 15., [[1.125], [0.5], [2.0]], 0.0, macros.sec2nano(0.01), "OFF"), # 2 thrusters, thrust off - (2, 0., 2.0, 30., 15., [[1.125], [0.5], [2.0]], 2.0, macros.sec2nano(0.01), "OFF"), # 2 thrusters, thrust on, swirl torque - (2, 0., 2.0, 30., 15., [[1.125], [0.5], [2.0]], 0.0, macros.sec2nano(0.01), "ON") # 2 thrusters, attached body -]) +@pytest.mark.parametrize( + "thrustNumber, initialConditions, duration, long_angle, lat_angle, location, swirlTorque, rate, attachBody", + [ + ( + 1, + 0.0, + 2.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 0.0, + macros.sec2nano(0.01), + "OFF", + ), # 1 thruster, thrust on + ( + 1, + 1.0, + 0.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 0.0, + macros.sec2nano(0.01), + "OFF", + ), # 1 thruster, thrust off + ( + 1, + 0.0, + 2.0, + 60.0, + -15.0, + [[-1.125], [0.5], [-2.0]], + 0.0, + macros.sec2nano(0.01), + "OFF", + ), # 1 thruster, thrust on, different orientation and location + ( + 1, + 1.0, + 0.0, + 60.0, + -15.0, + [[-1.125], [0.5], [-2.0]], + 0.0, + macros.sec2nano(0.01), + "OFF", + ), # 1 thruster, thrust off, different orientation and location + ( + 2, + 0.0, + 2.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 0.0, + macros.sec2nano(0.01), + "OFF", + ), # 2 thrusters, thrust on + ( + 2, + 1.0, + 0.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 0.0, + macros.sec2nano(0.01), + "OFF", + ), # 2 thrusters, thrust off + ( + 2, + 0.0, + 2.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 2.0, + macros.sec2nano(0.01), + "OFF", + ), # 2 thrusters, thrust on, swirl torque + ( + 2, + 0.0, + 2.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 0.0, + macros.sec2nano(0.01), + "ON", + ), # 2 thrusters, attached body + ], +) # provide a unique test method name, starting with test_ -def test_unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, duration, long_angle, lat_angle, location, swirlTorque, rate, attachBody): +def test_unitThrusters( + testFixture, + show_plots, + thrustNumber, + initialConditions, + duration, + long_angle, + lat_angle, + location, + swirlTorque, + rate, + attachBody, +): r""" **Validation Test Description** @@ -124,13 +225,36 @@ def test_unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, All these variables are compared to the true values from the closed-form expressions given in :ref:`thrusterStateEffector`. """ # each test method requires a single assert method to be called - [testResults, testMessage] = unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, duration, long_angle, - lat_angle, location, swirlTorque, rate, attachBody) + [testResults, testMessage] = unitThrusters( + testFixture, + show_plots, + thrustNumber, + initialConditions, + duration, + long_angle, + lat_angle, + location, + swirlTorque, + rate, + attachBody, + ) assert testResults < 1, testMessage # Run the test -def unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, duration, long_angle, lat_angle, location, swirlTorque, rate, attachBody): +def unitThrusters( + testFixture, + show_plots, + thrustNumber, + initialConditions, + duration, + long_angle, + lat_angle, + location, + swirlTorque, + rate, + attachBody, +): __tracebackhide__ = True testFailCount = 0 # zero unit test result counter @@ -164,8 +288,16 @@ def unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, dura scObject.hub.mHub = 750.0 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + 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.1], [0.2], [-0.3]] scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] @@ -184,8 +316,11 @@ def unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, dura lat_angle_rad = lat_angle_deg * math.pi / 180.0 thruster1 = thrusterStateEffector.THRSimConfig() thruster1.thrLoc_B = location # Parametrized location for thruster - thruster1.thrDir_B = [[math.cos(long_angle_rad) * math.cos(lat_angle_rad)], - [math.sin(long_angle_rad) * math.cos(lat_angle_rad)], [math.sin(lat_angle_rad)]] + thruster1.thrDir_B = [ + [math.cos(long_angle_rad) * math.cos(lat_angle_rad)], + [math.sin(long_angle_rad) * math.cos(lat_angle_rad)], + [math.sin(lat_angle_rad)], + ] thruster1.MaxThrust = 10.0 thruster1.steadyIsp = 226.7 thruster1.MinOnTime = 0.006 @@ -193,24 +328,49 @@ def unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, dura thruster1.MaxSwirlTorque = swirlTorque thrusterSet.addThruster(thruster1) - loc1 = np.array([thruster1.thrLoc_B[0][0], thruster1.thrLoc_B[1][0], thruster1.thrLoc_B[2][0]]) - dir1 = np.array([thruster1.thrDir_B[0][0], thruster1.thrDir_B[1][0], thruster1.thrDir_B[2][0]]) + loc1 = np.array( + [thruster1.thrLoc_B[0][0], thruster1.thrLoc_B[1][0], thruster1.thrLoc_B[2][0]] + ) + dir1 = np.array( + [thruster1.thrDir_B[0][0], thruster1.thrDir_B[1][0], thruster1.thrDir_B[2][0]] + ) # Create thruster characteristic parameters for thruster 2 if thrustNumber == 2: thruster2 = thrusterStateEffector.THRSimConfig() - thruster2.thrLoc_B = np.array([[1.], [0.0], [0.0]]).reshape([3, 1]) + thruster2.thrLoc_B = np.array([[1.0], [0.0], [0.0]]).reshape([3, 1]) thruster2.thrDir_B = np.array( - [[math.cos(long_angle_rad + math.pi / 4.) * math.cos(lat_angle_rad - math.pi / 4.)], - [math.sin(long_angle_rad + math.pi / 4.) * math.cos(lat_angle_rad - math.pi / 4.)], - [math.sin(lat_angle_rad - math.pi / 4.)]]).reshape([3, 1]) + [ + [ + math.cos(long_angle_rad + math.pi / 4.0) + * math.cos(lat_angle_rad - math.pi / 4.0) + ], + [ + math.sin(long_angle_rad + math.pi / 4.0) + * math.cos(lat_angle_rad - math.pi / 4.0) + ], + [math.sin(lat_angle_rad - math.pi / 4.0)], + ] + ).reshape([3, 1]) thruster2.MaxThrust = 20.0 thruster2.steadyIsp = 226.7 thruster2.MinOnTime = 0.006 thruster2.cutoffFrequency = 2 - loc2 = np.array([thruster2.thrLoc_B[0][0], thruster2.thrLoc_B[1][0], thruster2.thrLoc_B[2][0]]) - dir2 = np.array([thruster2.thrDir_B[0][0], thruster2.thrDir_B[1][0], thruster2.thrDir_B[2][0]]) + loc2 = np.array( + [ + thruster2.thrLoc_B[0][0], + thruster2.thrLoc_B[1][0], + thruster2.thrLoc_B[2][0], + ] + ) + dir2 = np.array( + [ + thruster2.thrDir_B[0][0], + thruster2.thrDir_B[1][0], + thruster2.thrDir_B[2][0], + ] + ) if attachBody == "ON": # Set up the dcm and location @@ -252,7 +412,9 @@ def unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, dura thrDurationTime = macros.sec2nano(2.0) # Log variables of interest - thrusterSetLog = thrusterSet.logger(["forceOnBody_B", "torqueOnBodyPntB_B", "mDotTotal"]) + thrusterSetLog = thrusterSet.logger( + ["forceOnBody_B", "torqueOnBodyPntB_B", "mDotTotal"] + ) TotalSim.AddModelToTask(unitTaskName2, thrusterSetLog) # Configure a single thruster firing, create a message for it @@ -260,7 +422,7 @@ def unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, dura if thrustNumber == 1: ThrustMessage.OnTimeRequest = [duration] if thrustNumber == 2: - ThrustMessage.OnTimeRequest = [duration, 2.] + ThrustMessage.OnTimeRequest = [duration, 2.0] thrCmdMsg = messaging.THRArrayOnTimeCmdMsg().write(ThrustMessage) thrusterSet.cmdsInMsg.subscribeTo(thrCmdMsg) @@ -278,15 +440,21 @@ def unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, dura dataThrustFactor = dataRec.thrustFactor plt.figure(1) plt.plot(dataRec.times() * macros.NANO2SEC, dataThrustFactor) - plt.xlabel('Time [s]') - plt.ylabel('Thrust Factor') + plt.xlabel("Time [s]") + plt.ylabel("Thrust Factor") if show_plots: plt.show() # Gather the Force, Torque and Mass Rate results - thrForce = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.forceOnBody_B) - thrTorque = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.torqueOnBodyPntB_B) - mDot = unitTestSupport.addTimeColumn(thrusterSetLog.times(), thrusterSetLog.mDotTotal) + thrForce = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.forceOnBody_B + ) + thrTorque = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.torqueOnBodyPntB_B + ) + mDot = unitTestSupport.addTimeColumn( + thrusterSetLog.times(), thrusterSetLog.mDotTotal + ) # Save the time vector timeSec = dataRec.times() * macros.NANO2SEC @@ -298,58 +466,93 @@ def unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, dura for i in range(np.shape(thrForce)[0]): if thrustNumber == 1: # Compute the thrust force - if duration == 0.: - thrustFactor1 = initialConditions * np.exp(- thruster1.cutoffFrequency * timeSec[i]) + if duration == 0.0: + thrustFactor1 = initialConditions * np.exp( + -thruster1.cutoffFrequency * timeSec[i] + ) force1 = thrustFactor1 * thruster1.MaxThrust * dir1 expectedThrustData[0:3, i] = force1 else: - thrustFactor1 = (1.0 + (initialConditions - 1.0) * np.exp(- thruster1.cutoffFrequency * timeSec[i])) + thrustFactor1 = 1.0 + (initialConditions - 1.0) * np.exp( + -thruster1.cutoffFrequency * timeSec[i] + ) force1 = thrustFactor1 * thruster1.MaxThrust * dir1 expectedThrustData[0:3, i] = force1 # Compute the torque - expectedTorqueData[0:3, i] = np.cross(loc1, force1) + thrustFactor1 * swirlTorque * dir1 + expectedTorqueData[0:3, i] = ( + np.cross(loc1, force1) + thrustFactor1 * swirlTorque * dir1 + ) # Compute the mass flow rate expectedMDot[0, i] = thruster1.MaxThrust * thrustFactor1 / (g * Isp) else: # Compute the thrust force - if duration == 0.: - thrustFactor1 = initialConditions * np.exp(- thruster1.cutoffFrequency * timeSec[i]) - thrustFactor2 = (1.0 - np.exp(- thruster2.cutoffFrequency * timeSec[i])) + if duration == 0.0: + thrustFactor1 = initialConditions * np.exp( + -thruster1.cutoffFrequency * timeSec[i] + ) + thrustFactor2 = 1.0 - np.exp(-thruster2.cutoffFrequency * timeSec[i]) force1 = thrustFactor1 * thruster1.MaxThrust * dir1 force2 = thrustFactor2 * thruster2.MaxThrust * dir2 expectedThrustData[0:3, i] = force1 + force2 else: - thrustFactor1 = (1.0 + (initialConditions - 1.0) * np.exp(- thruster1.cutoffFrequency * timeSec[i])) - thrustFactor2 = (1.0 - np.exp(- thruster2.cutoffFrequency * timeSec[i])) + thrustFactor1 = 1.0 + (initialConditions - 1.0) * np.exp( + -thruster1.cutoffFrequency * timeSec[i] + ) + thrustFactor2 = 1.0 - np.exp(-thruster2.cutoffFrequency * timeSec[i]) force1 = thrustFactor1 * thruster1.MaxThrust * dir1 force2 = thrustFactor2 * thruster2.MaxThrust * dir2 expectedThrustData[0:3, i] = force1 + force2 # Compute the torque - expectedTorqueData[0:3, i] = np.cross(loc1, force1) + thrustFactor1 * swirlTorque * dir1 + np.cross(loc2, force2) + expectedTorqueData[0:3, i] = ( + np.cross(loc1, force1) + + thrustFactor1 * swirlTorque * dir1 + + np.cross(loc2, force2) + ) # Compute the mass flow rate - expectedMDot[0, i] = (thruster1.MaxThrust * thrustFactor1 + thruster2.MaxThrust * thrustFactor2) / (g * Isp) + expectedMDot[0, i] = ( + thruster1.MaxThrust * thrustFactor1 + + thruster2.MaxThrust * thrustFactor2 + ) / (g * Isp) # Modify expected values for comparison and define errorTolerance TruthForce = np.transpose(expectedThrustData) TruthTorque = np.transpose(expectedTorqueData) TruthMDot = np.transpose(expectedMDot) - ErrTolerance = 1E-3 + ErrTolerance = 1e-3 # Compare Force values (exclude first element because of python process priority) thrForce = np.delete(thrForce, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthForce[1:, :], thrForce[1:, :], ErrTolerance, "Force", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + TruthForce[1:, :], + thrForce[1:, :], + ErrTolerance, + "Force", + testFailCount, + testMessages, + ) # Compare Torque values (exclude first element because of python process priority) thrTorque = np.delete(thrTorque, 0, axis=1) # remove time column - testFailCount, testMessages = unitTestSupport.compareArray(TruthTorque[1:, :], thrTorque[1:, :], ErrTolerance, "Torque", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + TruthTorque[1:, :], + thrTorque[1:, :], + ErrTolerance, + "Torque", + testFailCount, + testMessages, + ) # Compare mass flow rate values mDot = np.delete(mDot, 0, axis=1) - ErrTolerance = 1E-6 - testFailCount, testMessages = unitTestSupport.compareArray(np.transpose(TruthMDot), np.transpose(mDot), ErrTolerance, "MDot", - testFailCount, testMessages) + ErrTolerance = 1e-6 + testFailCount, testMessages = unitTestSupport.compareArray( + np.transpose(TruthMDot), + np.transpose(mDot), + ErrTolerance, + "MDot", + testFailCount, + testMessages, + ) if testFailCount == 0: print("PASSED") @@ -360,7 +563,7 @@ def unitThrusters(testFixture, show_plots, thrustNumber, initialConditions, dura # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] class attachedBodyModule(sysModel.SysModel): @@ -410,4 +613,16 @@ def writeOutputMsg(self, CurrentSimNanos): if __name__ == "__main__": - unitThrusters(ResultsStore(), False, 2, 0., 2.0, 30., 15., [[1.125], [0.5], [2.0]], 0.0, macros.sec2nano(0.01), "ON") + unitThrusters( + ResultsStore(), + False, + 2, + 0.0, + 2.0, + 30.0, + 15.0, + [[1.125], [0.5], [2.0]], + 0.0, + macros.sec2nano(0.01), + "ON", + ) diff --git a/src/simulation/dynamics/VSCMGs/_UnitTest/test_VSCMGStateEffector_ConfigureVSCMGRequests.py b/src/simulation/dynamics/VSCMGs/_UnitTest/test_VSCMGStateEffector_ConfigureVSCMGRequests.py index e539cfa478..efab047de5 100755 --- a/src/simulation/dynamics/VSCMGs/_UnitTest/test_VSCMGStateEffector_ConfigureVSCMGRequests.py +++ b/src/simulation/dynamics/VSCMGs/_UnitTest/test_VSCMGStateEffector_ConfigureVSCMGRequests.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -31,68 +30,72 @@ # methods -def listStack(vec,simStopTime,unitProcRate): +def listStack(vec, simStopTime, unitProcRate): # returns a list duplicated the number of times needed to be consistent with module output - return [vec] * int(simStopTime/(float(unitProcRate)/float(macros.sec2nano(1)))) + return [vec] * int(simStopTime / (float(unitProcRate) / float(macros.sec2nano(1)))) + -def writeNewVSCMGCmds(self,u_s_cmd,u_g_cmd,numVSCMG): +def writeNewVSCMGCmds(self, u_s_cmd, u_g_cmd, numVSCMG): NewVSCMGCmdsVec = messaging.VSCMGCmdMsgPayloadVector(numVSCMG) cmds = messaging.VSCMGCmdMsgPayload() - for i in range(0,numVSCMG): + for i in range(0, numVSCMG): cmds.u_s_cmd = u_s_cmd[i] cmds.u_g_cmd = u_g_cmd[i] NewVSCMGCmdsVec[i] = cmds # set the data self.newVSCMGCmds = NewVSCMGCmdsVec # set in module + def defaultVSCMG(VSCMG): - VSCMG.rGB_B = [[0.],[0.],[0.]] - VSCMG.gsHat0_B = [[1.],[0.],[0.]] - VSCMG.gtHat0_B = [[1.],[0.],[0.]] - VSCMG.ggHat_B = [[1.],[0.],[0.]] - VSCMG.w2Hat0_B = [[0.],[1.],[0.]] - VSCMG.w3Hat0_B = [[0.],[0.],[1.]] - VSCMG.theta = 0. - VSCMG.u_s_current = 0. - VSCMG.u_s_max = 0. - VSCMG.u_s_min = 0. - VSCMG.u_s_f = 0. - VSCMG.u_g_current = 0. - VSCMG.u_g_max = 0. - VSCMG.u_g_min = 0. - VSCMG.u_g_f = 0. - VSCMG.Omega = 0. - VSCMG.gamma = 0. - VSCMG.gammaDot = 0. - VSCMG.Omega_max = 1000. + VSCMG.rGB_B = [[0.0], [0.0], [0.0]] + VSCMG.gsHat0_B = [[1.0], [0.0], [0.0]] + VSCMG.gtHat0_B = [[1.0], [0.0], [0.0]] + VSCMG.ggHat_B = [[1.0], [0.0], [0.0]] + VSCMG.w2Hat0_B = [[0.0], [1.0], [0.0]] + VSCMG.w3Hat0_B = [[0.0], [0.0], [1.0]] + VSCMG.theta = 0.0 + VSCMG.u_s_current = 0.0 + VSCMG.u_s_max = 0.0 + VSCMG.u_s_min = 0.0 + VSCMG.u_s_f = 0.0 + VSCMG.u_g_current = 0.0 + VSCMG.u_g_max = 0.0 + VSCMG.u_g_min = 0.0 + VSCMG.u_g_f = 0.0 + VSCMG.Omega = 0.0 + VSCMG.gamma = 0.0 + VSCMG.gammaDot = 0.0 + VSCMG.Omega_max = 1000.0 VSCMG.gammaDot_max = -1 - VSCMG.IW1 = 0. - VSCMG.IW2 = 0. - VSCMG.IW3 = 0. - VSCMG.U_s = 0. - VSCMG.U_d = 0. - VSCMG.massW = 0. - VSCMG.massG = 0. - VSCMG.wheelLinearFrictionRatio = 0. + VSCMG.IW1 = 0.0 + VSCMG.IW2 = 0.0 + VSCMG.IW3 = 0.0 + VSCMG.U_s = 0.0 + VSCMG.U_d = 0.0 + VSCMG.massW = 0.0 + VSCMG.massG = 0.0 + VSCMG.wheelLinearFrictionRatio = 0.0 VSCMG.VSCMGModel = 0 return + def asEigen(v): out = [] - for i in range(0,len(v)): + for i in range(0, len(v)): out.append([v[i]]) return out + # uncomment this line is 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 + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("useFlag, testCase", [ - (False,'saturation'), - (False,'minimum'), - (False,'friction') -]) +@pytest.mark.parametrize( + "useFlag, testCase", + [(False, "saturation"), (False, "minimum"), (False, "friction")], +) # provide a unique test method name, starting with test_ def test_unitSimVSCMG(show_plots, useFlag, testCase): @@ -114,48 +117,48 @@ def unitSimVSCMG(show_plots, useFlag, testCase): numVSCMG = 2 VSCMGs = [] - for i in range(0,numVSCMG): + for i in range(0, numVSCMG): msg = messaging.VSCMGConfigMsgPayload() defaultVSCMG(msg) VSCMGs.append(msg) expOut = dict() # expected output - if testCase == 'basic': + if testCase == "basic": pass - elif testCase == 'saturation': - VSCMGs[0].u_s_max = 1. - VSCMGs[1].u_s_max = 2. - VSCMGs[0].u_g_max = 1. - VSCMGs[1].u_g_max = 2. - u_s_cmd = [-1.2,1.5] - u_g_cmd = [-1.2,1.5] - writeNewVSCMGCmds(VSCMG,u_s_cmd,u_g_cmd,len(VSCMGs)) - - expOut['u_s_current'] = [-1.,1.5] - - elif testCase == 'minimum': - VSCMGs[0].u_s_min = .1 - VSCMGs[1].u_s_min = .0 - VSCMGs[0].u_g_min = .1 - VSCMGs[1].u_g_min = .0 - u_s_cmd = [-.09,0.0001] - u_g_cmd = [-.09,0.0001] - writeNewVSCMGCmds(VSCMG,u_s_cmd,u_g_cmd,len(VSCMGs)) - - expOut['u_s_current'] = [0.,0.0001] - - elif testCase == 'friction': - u_s_f = [0.1,0.] - u_g_f = [0.1,0.] - Omega = [-20.,0.] - Omega_max = [100.,0.] - gammaDot = [-20.,0.] - gammaDot_max = [100.,0.] - wheelLinearFrictionRatio = [0.1,0.] - gimbalLinearFrictionRatio = [0.1,0.] - for i in range(0,numVSCMG): + elif testCase == "saturation": + VSCMGs[0].u_s_max = 1.0 + VSCMGs[1].u_s_max = 2.0 + VSCMGs[0].u_g_max = 1.0 + VSCMGs[1].u_g_max = 2.0 + u_s_cmd = [-1.2, 1.5] + u_g_cmd = [-1.2, 1.5] + writeNewVSCMGCmds(VSCMG, u_s_cmd, u_g_cmd, len(VSCMGs)) + + expOut["u_s_current"] = [-1.0, 1.5] + + elif testCase == "minimum": + VSCMGs[0].u_s_min = 0.1 + VSCMGs[1].u_s_min = 0.0 + VSCMGs[0].u_g_min = 0.1 + VSCMGs[1].u_g_min = 0.0 + u_s_cmd = [-0.09, 0.0001] + u_g_cmd = [-0.09, 0.0001] + writeNewVSCMGCmds(VSCMG, u_s_cmd, u_g_cmd, len(VSCMGs)) + + expOut["u_s_current"] = [0.0, 0.0001] + + elif testCase == "friction": + u_s_f = [0.1, 0.0] + u_g_f = [0.1, 0.0] + Omega = [-20.0, 0.0] + Omega_max = [100.0, 0.0] + gammaDot = [-20.0, 0.0] + gammaDot_max = [100.0, 0.0] + wheelLinearFrictionRatio = [0.1, 0.0] + gimbalLinearFrictionRatio = [0.1, 0.0] + for i in range(0, numVSCMG): VSCMGs[i].u_s_f = u_s_f[i] VSCMGs[i].Omega = Omega[i] VSCMGs[i].Omega_max = Omega_max[i] @@ -164,25 +167,25 @@ def unitSimVSCMG(show_plots, useFlag, testCase): VSCMGs[i].gammaDot = gammaDot[i] VSCMGs[i].gammaDot_max = gammaDot_max[i] VSCMGs[i].gimbalLinearFrictionRatio = gimbalLinearFrictionRatio[i] - u_s_cmd = [-1.,0.] - u_g_cmd = [-1.,0.] - writeNewVSCMGCmds(VSCMG,u_s_cmd,u_g_cmd,len(VSCMGs)) + u_s_cmd = [-1.0, 0.0] + u_g_cmd = [-1.0, 0.0] + writeNewVSCMGCmds(VSCMG, u_s_cmd, u_g_cmd, len(VSCMGs)) - expOut['u_s_current'] = np.asarray(u_s_cmd) + np.asarray(u_s_f) + expOut["u_s_current"] = np.asarray(u_s_cmd) + np.asarray(u_s_f) else: - raise Exception('invalid test case') + raise Exception("invalid test case") - for i in range(0,len(VSCMGs)): + for i in range(0, len(VSCMGs)): VSCMG.AddVSCMG(VSCMGs[i]) - VSCMG.ConfigureVSCMGRequests(0.) + VSCMG.ConfigureVSCMGRequests(0.0) - if not 'accuracy' in vars(): + if not "accuracy" in vars(): accuracy = 1e-10 for outputName in list(expOut.keys()): - for i in range(0,numVSCMG): + for i in range(0, numVSCMG): if expOut[outputName][i] != getattr(VSCMG.VSCMGData[i], outputName): print("expected: " + str(expOut[outputName][i])) print("got :" + str(getattr(VSCMG.VSCMGData[i], outputName))) @@ -193,8 +196,9 @@ def unitSimVSCMG(show_plots, useFlag, testCase): if testFail: testFailCount += 1 - testMessages.append("FAILED: " + VSCMG.ModelTag + " Module failed " + - outputName + " unit test") + testMessages.append( + "FAILED: " + VSCMG.ModelTag + " Module failed " + outputName + " unit test" + ) np.set_printoptions(precision=16) @@ -206,14 +210,14 @@ def unitSimVSCMG(show_plots, useFlag, testCase): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # This statement below ensures that the unit test script can be run as a # stand-along python script if __name__ == "__main__": test_unitSimVSCMG( - False, # show_plots - False, # useFlag - 'friction' # testCase + False, # show_plots + False, # useFlag + "friction", # testCase ) diff --git a/src/simulation/dynamics/VSCMGs/_UnitTest/test_VSCMGStateEffector_integrated.py b/src/simulation/dynamics/VSCMGs/_UnitTest/test_VSCMGStateEffector_integrated.py index 2a80d3aff5..d8aed47c1e 100644 --- a/src/simulation/dynamics/VSCMGs/_UnitTest/test_VSCMGStateEffector_integrated.py +++ b/src/simulation/dynamics/VSCMGs/_UnitTest/test_VSCMGStateEffector_integrated.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -27,7 +26,9 @@ path = os.path.dirname(os.path.abspath(filename)) from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import matplotlib as mpl import matplotlib.pyplot as plt from Basilisk.simulation import spacecraft @@ -36,33 +37,33 @@ from Basilisk.simulation import vscmgStateEffector from Basilisk.architecture import messaging -mpl.rc("figure", figsize=(5.75,4)) +mpl.rc("figure", figsize=(5.75, 4)) def defaultVSCMG(): VSCMG = messaging.VSCMGConfigMsgPayload() - VSCMG.rGB_B = [[0.],[0.],[0.]] - VSCMG.gsHat0_B = [[0.],[0.],[0.]] - VSCMG.gtHat0_B = [[0.],[0.],[0.]] - VSCMG.ggHat_B = [[0.],[0.],[0.]] + VSCMG.rGB_B = [[0.0], [0.0], [0.0]] + VSCMG.gsHat0_B = [[0.0], [0.0], [0.0]] + VSCMG.gtHat0_B = [[0.0], [0.0], [0.0]] + VSCMG.ggHat_B = [[0.0], [0.0], [0.0]] VSCMG.u_s_max = -1 VSCMG.u_s_min = -1 - VSCMG.u_s_f = 0. + VSCMG.u_s_f = 0.0 VSCMG.wheelLinearFrictionRatio = -1 - VSCMG.u_g_current = 0. + VSCMG.u_g_current = 0.0 VSCMG.u_g_max = -1 VSCMG.u_g_min = -1 - VSCMG.u_g_f = 0. + VSCMG.u_g_f = 0.0 VSCMG.gimbalLinearFrictionRatio = -1 - VSCMG.Omega = 0. - VSCMG.gamma = 0. - VSCMG.gammaDot = 0. - VSCMG.Omega_max = 6000. * macros.RPM + VSCMG.Omega = 0.0 + VSCMG.gamma = 0.0 + VSCMG.gammaDot = 0.0 + VSCMG.Omega_max = 6000.0 * macros.RPM VSCMG.gammaDot_max = -1 - VSCMG.IW1 = 100./VSCMG.Omega_max # 0.159154943092 - VSCMG.IW2 = 0.5*VSCMG.IW1 # 0.0795774715459 - VSCMG.IW3 = 0.5*VSCMG.IW1 # 0.0795774715459 + VSCMG.IW1 = 100.0 / VSCMG.Omega_max # 0.159154943092 + VSCMG.IW2 = 0.5 * VSCMG.IW1 # 0.0795774715459 + VSCMG.IW3 = 0.5 * VSCMG.IW1 # 0.0795774715459 VSCMG.IG1 = 0.1 VSCMG.IG2 = 0.2 VSCMG.IG3 = 0.3 @@ -70,55 +71,61 @@ def defaultVSCMG(): VSCMG.U_d = 1.54e-06 * 1e4 VSCMG.l = 0.01 VSCMG.L = 0.1 - VSCMG.rGcG_G = [[0.0001],[-0.02],[0.1]] - VSCMG.massW = 6. - VSCMG.massG = 6. + VSCMG.rGcG_G = [[0.0001], [-0.02], [0.1]] + VSCMG.massW = 6.0 + VSCMG.massG = 6.0 VSCMG.VSCMGModel = 0 return VSCMG -def computeFFT(y,dt): - Fs = 1.0/dt # sampling rate - Ts = dt # sampling interval - n = len(y) # length of the signal + +def computeFFT(y, dt): + Fs = 1.0 / dt # sampling rate + Ts = dt # sampling interval + n = len(y) # length of the signal k = np.arange(n) - T = n/Fs - frq = k/T # two sides frequency range - frq = frq[list(range(n//2))] # one side frequency range - Y = np.fft.fft(y)/n # fft computing and normalization - Y = Y[list(range(n//2))] + T = n / Fs + frq = k / T # two sides frequency range + frq = frq[list(range(n // 2))] # one side frequency range + Y = np.fft.fft(y) / n # fft computing and normalization + Y = Y[list(range(n // 2))] Y = abs(Y) - return [frq,Y] + return [frq, Y] + -def findPeaks(Y,maxfind): - peakIdxs = np.r_[True, Y[1:] > Y[:-1]] & np.r_[Y[:-1] > Y[1:], True] - peakIdxs[0] = False - peakIdxs[-1] = False +def findPeaks(Y, maxfind): + peakIdxs = np.r_[True, Y[1:] > Y[:-1]] & np.r_[Y[:-1] > Y[1:], True] + peakIdxs[0] = False + peakIdxs[-1] = False - peakIdxs = np.array(np.where(peakIdxs==True))[0] - threshold = np.sort(Y[peakIdxs])[-maxfind] - peakIdxs = peakIdxs[np.where(Y[peakIdxs] >= threshold)[0]] + peakIdxs = np.array(np.where(peakIdxs == True))[0] + threshold = np.sort(Y[peakIdxs])[-maxfind] + peakIdxs = peakIdxs[np.where(Y[peakIdxs] >= threshold)[0]] - return peakIdxs + return peakIdxs -@pytest.mark.parametrize("useFlag, testCase", [ - (False,'BalancedWheels'), - (False,'JitterSimple'), - (False,'JitterFullyCoupled'), - (False,'JitterFullyCoupledGravity'), -]) +@pytest.mark.parametrize( + "useFlag, testCase", + [ + (False, "BalancedWheels"), + (False, "JitterSimple"), + (False, "JitterFullyCoupled"), + (False, "JitterFullyCoupledGravity"), + ], +) # uncomment this line is 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() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -def test_VSCMGIntegratedTest(show_plots,useFlag,testCase): +def test_VSCMGIntegratedTest(show_plots, useFlag, testCase): """Module Unit Test""" - [testResults, testMessage] = VSCMGIntegratedTest(show_plots,useFlag,testCase) + [testResults, testMessage] = VSCMGIntegratedTest(show_plots, useFlag, testCase) assert testResults < 1, testMessage -def VSCMGIntegratedTest(show_plots,useFlag,testCase): + +def VSCMGIntegratedTest(show_plots, useFlag, testCase): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. @@ -137,12 +144,12 @@ def VSCMGIntegratedTest(show_plots,useFlag,testCase): unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - if testCase == 'JitterFullyCoupled' or testCase == 'JitterFullyCoupledGravity': + if testCase == "JitterFullyCoupled" or testCase == "JitterFullyCoupledGravity": dt = 0.00001 duration = 0.01 else: dt = 0.001 - duration = 1. + duration = 1.0 testProcessRate = macros.sec2nano(dt) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -150,47 +157,51 @@ def VSCMGIntegratedTest(show_plots,useFlag,testCase): # add RW devices VSCMGs = [] - ang = 54.75 * np.pi/180 + ang = 54.75 * np.pi / 180 VSCMGs.append(defaultVSCMG()) VSCMGs[0].gsHat0_B = [[1.0], [0.0], [0.0]] VSCMGs[0].gtHat0_B = [[0.0], [1.0], [0.0]] VSCMGs[0].ggHat_B = [[0.0], [0.0], [1.0]] VSCMGs[0].Omega = 2000 * macros.RPM - VSCMGs[0].gamma = 0. + VSCMGs[0].gamma = 0.0 VSCMGs[0].gammaDot = 0.06 VSCMGs[0].rGB_B = [[0.1], [0.002], [-0.02]] VSCMGs.append(defaultVSCMG()) VSCMGs[1].gsHat0_B = [[0.0], [1.0], [0.0]] VSCMGs[1].ggHat_B = [[math.cos(ang)], [0.0], [math.sin(ang)]] - VSCMGs[1].gtHat0_B = np.cross(np.array([math.cos(ang), 0.0, math.sin(ang)]),np.array([0.0, 1.0, 0.0])) - VSCMGs[1].Omega = 350 * macros.RPM - VSCMGs[1].gamma = 0. + VSCMGs[1].gtHat0_B = np.cross( + np.array([math.cos(ang), 0.0, math.sin(ang)]), np.array([0.0, 1.0, 0.0]) + ) + VSCMGs[1].Omega = 350 * macros.RPM + VSCMGs[1].gamma = 0.0 VSCMGs[1].gammaDot = 0.011 VSCMGs[1].rGB_B = [[0.0], [-0.05], [0.0]] VSCMGs.append(defaultVSCMG()) VSCMGs[2].gsHat0_B = [[0.0], [-1.0], [0.0]] VSCMGs[2].ggHat_B = [[-math.cos(ang)], [0.0], [math.sin(ang)]] - VSCMGs[2].gtHat0_B = np.cross(np.array([-math.cos(ang), 0.0, math.sin(ang)]),np.array([0.0, -1.0, 0.0])) + VSCMGs[2].gtHat0_B = np.cross( + np.array([-math.cos(ang), 0.0, math.sin(ang)]), np.array([0.0, -1.0, 0.0]) + ) VSCMGs[2].Omega = -900 * macros.RPM - VSCMGs[2].gamma = 0. + VSCMGs[2].gamma = 0.0 VSCMGs[2].gammaDot = -0.003 VSCMGs[2].rGB_B = [[-0.1], [0.05], [0.05]] - if testCase == 'BalancedWheels': + if testCase == "BalancedWheels": VSCMGModel = 0 - elif testCase == 'JitterSimple': + elif testCase == "JitterSimple": VSCMGModel = 1 - elif testCase == 'JitterFullyCoupled' or testCase == 'JitterFullyCoupledGravity': + elif testCase == "JitterFullyCoupled" or testCase == "JitterFullyCoupledGravity": VSCMGModel = 2 for VSCMG in VSCMGs: VSCMG.VSCMGModel = VSCMGModel - if testCase == 'JitterFullyCoupled': - VSCMGs = [VSCMGs[0],VSCMGs[2]] + if testCase == "JitterFullyCoupled": + VSCMGs = [VSCMGs[0], VSCMGs[2]] N = len(VSCMGs) @@ -203,12 +214,12 @@ def VSCMGIntegratedTest(show_plots,useFlag,testCase): # set RW torque command cmdArray = messaging.VSCMGArrayTorqueMsgPayload() - if testCase == 'BalancedWheels' or testCase == 'JitterFullyCoupled': + if testCase == "BalancedWheels" or testCase == "JitterFullyCoupled": cmdArray.wheelTorque = [0.0, 0.0, 0.0] # [Nm] cmdArray.gimbalTorque = [0.0, 0.0, 0.0] # [Nm] else: - cmdArray.wheelTorque = [0.001, 0.005, -0.009] # [Nm] - cmdArray.gimbalTorque = [0.008, -0.0015, -0.006] # [Nm] + cmdArray.wheelTorque = [0.001, 0.005, -0.009] # [Nm] + cmdArray.gimbalTorque = [0.008, -0.0015, -0.006] # [Nm] cmdMsg = messaging.VSCMGArrayTorqueMsg().write(cmdArray) rwStateEffector.cmdsInMsg.subscribeTo(cmdMsg) @@ -216,24 +227,34 @@ def VSCMGIntegratedTest(show_plots,useFlag,testCase): unitTestSim.AddModelToTask(unitTaskName, rwStateEffector) unitTestSim.AddModelToTask(unitTaskName, scObject) - if testCase != 'JitterFullyCoupled': + if testCase != "JitterFullyCoupled": unitTestSim.earthGravBody = gravityEffector.GravBodyData() unitTestSim.earthGravBody.planetName = "earth_planet_data" - unitTestSim.earthGravBody.mu = 0.3986004415E+15 # meters! + unitTestSim.earthGravBody.mu = 0.3986004415e15 # meters! unitTestSim.earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([unitTestSim.earthGravBody]) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + [unitTestSim.earthGravBody] + ) scObject.hub.mHub = 750.0 scObject.hub.r_BcB_B = [[-0.0002], [0.0001], [0.1]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] - if testCase == 'JitterFullyCoupled': - scObject.hub.r_CN_NInit = [[0.1], [-0.2], [0.3]] - scObject.hub.v_CN_NInit = [[-0.4], [0.5], [-0.8]] + if testCase == "JitterFullyCoupled": + scObject.hub.r_CN_NInit = [[0.1], [-0.2], [0.3]] + scObject.hub.v_CN_NInit = [[-0.4], [0.5], [-0.8]] else: - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + 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.08], [0.01], [0.0]] @@ -243,7 +264,9 @@ def VSCMGIntegratedTest(show_plots,useFlag,testCase): unitTestSim.AddModelToTask(unitTaskName, dataLog) unitTestSim.AddModelToTask(unitTaskName, speedLog) - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) unitTestSim.ConfigureStopTime(macros.sec2nano(duration)) @@ -255,10 +278,18 @@ def VSCMGIntegratedTest(show_plots,useFlag,testCase): unitTestSim.ExecuteSimulation() - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) wheelSpeeds = speedLog.wheelSpeeds gimbalAngles = speedLog.gimbalAngles @@ -271,245 +302,283 @@ def VSCMGIntegratedTest(show_plots,useFlag,testCase): dataPos = [[dataPos[0][0], dataPos[1][0], dataPos[2][0]]] dataSigma = [[dataSigma[0][0], dataSigma[1][0], dataSigma[2][0]]] - if testCase == 'BalancedWheels': - truePos = [ - [-4025537.664298894, 7487128.570444949, 5249339.643828076] - ] + if testCase == "BalancedWheels": + truePos = [[-4025537.664298894, 7487128.570444949, 5249339.643828076]] - trueSigma = [ - [0.0185829763256, 0.00212563436704, -0.00118728497031] - ] + trueSigma = [[0.0185829763256, 0.00212563436704, -0.00118728497031]] - elif testCase == 'JitterSimple': - truePos = [ - [-4025537.659558947, 7487128.570662447, 5249339.653774626] - ] + elif testCase == "JitterSimple": + truePos = [[-4025537.659558947, 7487128.570662447, 5249339.653774626]] trueSigma = [ [0.018774477186285467, 0.0018376842577357564, -0.00023633044221463834] ] - elif testCase == 'JitterFullyCoupled': - truePos = [ - [0.0970572658434, -0.195562924079, 0.191874379545] - ] - - trueSigma = [ - [0.000201909373901, 2.9217809421e-05, 4.00231302121e-06] - ] - elif testCase == 'JitterFullyCoupledGravity': - truePos = [ - [-4020390.68802, 7490532.37502, 5248309.52745] - ] - - trueSigma = [ - [0.000201662012765, 2.92123940252e-05, 4.15606551702e-06] - ] + elif testCase == "JitterFullyCoupled": + truePos = [[0.0970572658434, -0.195562924079, 0.191874379545]] + trueSigma = [[0.000201909373901, 2.9217809421e-05, 4.00231302121e-06]] + elif testCase == "JitterFullyCoupledGravity": + truePos = [[-4020390.68802, 7490532.37502, 5248309.52745]] + trueSigma = [[0.000201662012765, 2.92123940252e-05, 4.15606551702e-06]] - initialOrbAngMom_N = [ - [orbAngMom_N[0,1], orbAngMom_N[0,2], orbAngMom_N[0,3]] - ] + initialOrbAngMom_N = [[orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]]] - finalOrbAngMom = [ - [orbAngMom_N[-1,1], orbAngMom_N[-1,2], orbAngMom_N[-1,3]] - ] + finalOrbAngMom = [[orbAngMom_N[-1, 1], orbAngMom_N[-1, 2], orbAngMom_N[-1, 3]]] - initialRotAngMom_N = [ - [rotAngMom_N[0,1], rotAngMom_N[0,2], rotAngMom_N[0,3]] - ] + initialRotAngMom_N = [[rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]]] - finalRotAngMom = [ - [rotAngMom_N[-1,1], rotAngMom_N[-1,2], rotAngMom_N[-1,3]] - ] + finalRotAngMom = [[rotAngMom_N[-1, 1], rotAngMom_N[-1, 2], rotAngMom_N[-1, 3]]] - initialOrbEnergy = [ - [orbEnergy[0,1]] - ] + initialOrbEnergy = [[orbEnergy[0, 1]]] - finalOrbEnergy = [ - [orbEnergy[-1,1]] - ] + finalOrbEnergy = [[orbEnergy[-1, 1]]] - initialRotEnergy = [ - [rotEnergy[0,1]] - ] + initialRotEnergy = [[rotEnergy[0, 1]]] - finalRotEnergy = [ - [rotEnergy[-1,1]] - ] + finalRotEnergy = [[rotEnergy[-1, 1]]] plt.close("all") plt.figure() - plt.plot(orbAngMom_N[:,0]*1e-9, orbAngMom_N[:,1] - orbAngMom_N[0,1], orbAngMom_N[:,0]*1e-9, orbAngMom_N[:,2] - orbAngMom_N[0,2], orbAngMom_N[:,0]*1e-9, orbAngMom_N[:,3] - orbAngMom_N[0,3]) + plt.plot( + orbAngMom_N[:, 0] * 1e-9, + orbAngMom_N[:, 1] - orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * 1e-9, + orbAngMom_N[:, 2] - orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * 1e-9, + orbAngMom_N[:, 3] - orbAngMom_N[0, 3], + ) plt.title("Change in Orbital Angular Momentum") - unitTestSupport.writeFigureLaTeX("ChangeInOrbitalAngularMomentum" + testCase, - "Change in Orbital Angular Momentum " + testCase, plt, "width=0.80\\textwidth", - path) + unitTestSupport.writeFigureLaTeX( + "ChangeInOrbitalAngularMomentum" + testCase, + "Change in Orbital Angular Momentum " + testCase, + plt, + "width=0.80\\textwidth", + path, + ) plt.figure() - plt.plot(rotAngMom_N[:,0]*1e-9, rotAngMom_N[:,1] - rotAngMom_N[0,1], rotAngMom_N[:,0]*1e-9, rotAngMom_N[:,2] - rotAngMom_N[0,2], rotAngMom_N[:,0]*1e-9, rotAngMom_N[:,3] - rotAngMom_N[0,3]) + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + rotAngMom_N[:, 1] - rotAngMom_N[0, 1], + rotAngMom_N[:, 0] * 1e-9, + rotAngMom_N[:, 2] - rotAngMom_N[0, 2], + rotAngMom_N[:, 0] * 1e-9, + rotAngMom_N[:, 3] - rotAngMom_N[0, 3], + ) plt.title("Change in Rotational Angular Momentum") - unitTestSupport.writeFigureLaTeX("ChangeInOrbitalEnergy" + testCase, "Change in Orbital Energy " + testCase, plt, - "width=0.80\\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInOrbitalEnergy" + testCase, + "Change in Orbital Energy " + testCase, + plt, + "width=0.80\\textwidth", + path, + ) plt.figure() - plt.plot(orbEnergy[:,0]*1e-9, orbEnergy[:,1] - orbEnergy[0,1]) + plt.plot(orbEnergy[:, 0] * 1e-9, orbEnergy[:, 1] - orbEnergy[0, 1]) plt.title("Change in Orbital Energy") - unitTestSupport.writeFigureLaTeX("ChangeInRotationalAngularMomentum" + testCase, - "Change in Rotational Angular Momentum " + testCase, plt, "width=0.80\\textwidth", - path) + unitTestSupport.writeFigureLaTeX( + "ChangeInRotationalAngularMomentum" + testCase, + "Change in Rotational Angular Momentum " + testCase, + plt, + "width=0.80\\textwidth", + path, + ) plt.figure() - plt.plot(rotEnergy[:,0]*1e-9, rotEnergy[:,1] - rotEnergy[0,1]) + plt.plot(rotEnergy[:, 0] * 1e-9, rotEnergy[:, 1] - rotEnergy[0, 1]) plt.title("Change in Rotational Energy") - unitTestSupport.writeFigureLaTeX("ChangeInRotationalEnergy" + testCase, "Change in Rotational Energy " + testCase, - plt, "width=0.80\\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInRotationalEnergy" + testCase, + "Change in Rotational Energy " + testCase, + plt, + "width=0.80\\textwidth", + path, + ) plt.figure() - for i in range(0,N): - plt.subplot(4,1,i+1) - plt.plot(speedLog.times()*1.0E-9, wheelSpeeds[:,i] / (2.0 * math.pi) * 60, label='RWA' + str(i)) - plt.xlabel('Time (s)') - plt.ylabel(r'RW' + str(i) + r' $\Omega$ (RPM)') + for i in range(0, N): + plt.subplot(4, 1, i + 1) + plt.plot( + speedLog.times() * 1.0e-9, + wheelSpeeds[:, i] / (2.0 * math.pi) * 60, + label="RWA" + str(i), + ) + plt.xlabel("Time (s)") + plt.ylabel(r"RW" + str(i) + r" $\Omega$ (RPM)") plt.figure() - for i in range(0,N): - plt.subplot(4,1,i+1) - plt.plot(speedLog.times()*1.0E-9, gimbalAngles[:,i], label=str(i)) - plt.xlabel('Time (s)') - plt.ylabel(r'$\gamma_'+str(i)+'$ (rad)') + for i in range(0, N): + plt.subplot(4, 1, i + 1) + plt.plot(speedLog.times() * 1.0e-9, gimbalAngles[:, i], label=str(i)) + plt.xlabel("Time (s)") + plt.ylabel(r"$\gamma_" + str(i) + "$ (rad)") plt.figure() - for i in range(0,N): - plt.subplot(4,1,i+1) - plt.plot(speedLog.times()*1.0E-9, gimbalRates[:,i] * 180/np.pi, label=str(i)) - plt.xlabel('Time (s)') - plt.ylabel(r'$\dot{\gamma}_'+str(i)+'$ (d/s)') + for i in range(0, N): + plt.subplot(4, 1, i + 1) + plt.plot( + speedLog.times() * 1.0e-9, gimbalRates[:, i] * 180 / np.pi, label=str(i) + ) + plt.xlabel("Time (s)") + plt.ylabel(r"$\dot{\gamma}_" + str(i) + "$ (d/s)") plt.figure() - for i in range(0,N): - plt.subplot(4,1,i+1) - plt.plot(dataLog.times()*1.0E-9, sigmaData[:,i], label='MRP' + str(i)) - plt.xlabel('Time (s)') - plt.ylabel(r'MRP b' + str(i)) + for i in range(0, N): + plt.subplot(4, 1, i + 1) + plt.plot(dataLog.times() * 1.0e-9, sigmaData[:, i], label="MRP" + str(i)) + plt.xlabel("Time (s)") + plt.ylabel(r"MRP b" + str(i)) plt.figure() - for i in range(0,N): - plt.subplot(4,1,i+1) - plt.plot(dataLog.times()*1.0E-9, omegaData[:,i] * 180/math.pi, label='omega' + str(i)) - plt.xlabel('Time (s)') - plt.ylabel(r'b' + str(i) + r' $\omega$ (d/s)') - - if testCase != 'BalancedWheels' and testCase != 'JitterFullyCoupledGravity': - istart = int(.01/dt) - sigmaDataCut = sigmaData#sigmaData[istart:,:] - thetaData = np.empty([len(sigmaDataCut[:,0]),2]) - thetaData[:,0] = sigmaDataCut[:,0] - for i in range(0,len(thetaData[:,0])): - thetaData[i,1] = 4*np.arctan(np.linalg.norm(sigmaDataCut[i,1:])) - - if testCase == 'JitterSimple': + for i in range(0, N): + plt.subplot(4, 1, i + 1) + plt.plot( + dataLog.times() * 1.0e-9, + omegaData[:, i] * 180 / math.pi, + label="omega" + str(i), + ) + plt.xlabel("Time (s)") + plt.ylabel(r"b" + str(i) + r" $\omega$ (d/s)") + + if testCase != "BalancedWheels" and testCase != "JitterFullyCoupledGravity": + istart = int(0.01 / dt) + sigmaDataCut = sigmaData # sigmaData[istart:,:] + thetaData = np.empty([len(sigmaDataCut[:, 0]), 2]) + thetaData[:, 0] = sigmaDataCut[:, 0] + for i in range(0, len(thetaData[:, 0])): + thetaData[i, 1] = 4 * np.arctan(np.linalg.norm(sigmaDataCut[i, 1:])) + + if testCase == "JitterSimple": fitOrd = 11 else: fitOrd = 9 - thetaFit = np.empty([len(sigmaDataCut[:,0]),2]) - thetaFit[:,0] = thetaData[:,0] - p = np.polyfit(thetaData[:,0]*1e-9,thetaData[:,1],fitOrd) - thetaFit[:,1] = np.polyval(p,thetaFit[:,0]*1e-9) + thetaFit = np.empty([len(sigmaDataCut[:, 0]), 2]) + thetaFit[:, 0] = thetaData[:, 0] + p = np.polyfit(thetaData[:, 0] * 1e-9, thetaData[:, 1], fitOrd) + thetaFit[:, 1] = np.polyval(p, thetaFit[:, 0] * 1e-9) plt.figure() - plt.plot(thetaData[:,0]*1e-9, thetaData[:,1]-thetaFit[:,1]) + plt.plot(thetaData[:, 0] * 1e-9, thetaData[:, 1] - thetaFit[:, 1]) plt.title("Principle Angle Fit") - plt.xlabel('Time (s)') - plt.ylabel(r'$\theta$ (deg)') - - [frq,Y] = computeFFT(thetaData[:,1]-thetaFit[:,1],dt) - peakIdxs = findPeaks(Y,N) - wheelSpeeds_data = np.array(frq[peakIdxs])*60. - wheelSpeeds_true = np.sort(abs(np.array([VSCMG.Omega/macros.RPM for VSCMG in VSCMGs]))) - - fig, ax = plt.subplots(2,1) - ax[0].plot(thetaFit[:,0]*1e-9,thetaData[:,1]-thetaFit[:,1]) - ax[0].set_xlabel('Time') - ax[0].set_ylabel('Amplitude') - ax[1].plot(frq,abs(Y),'r') - ax[1].set_xlabel('Freq (Hz)') - ax[1].set_ylabel('Magnitude') - ax[1].plot(frq[peakIdxs],Y[peakIdxs],'bo') - plt.xlim((0,VSCMGs[0].Omega_max/macros.RPM/60.)) + plt.xlabel("Time (s)") + plt.ylabel(r"$\theta$ (deg)") + + [frq, Y] = computeFFT(thetaData[:, 1] - thetaFit[:, 1], dt) + peakIdxs = findPeaks(Y, N) + wheelSpeeds_data = np.array(frq[peakIdxs]) * 60.0 + wheelSpeeds_true = np.sort( + abs(np.array([VSCMG.Omega / macros.RPM for VSCMG in VSCMGs])) + ) + + fig, ax = plt.subplots(2, 1) + ax[0].plot(thetaFit[:, 0] * 1e-9, thetaData[:, 1] - thetaFit[:, 1]) + ax[0].set_xlabel("Time") + ax[0].set_ylabel("Amplitude") + ax[1].plot(frq, abs(Y), "r") + ax[1].set_xlabel("Freq (Hz)") + ax[1].set_ylabel("Magnitude") + ax[1].plot(frq[peakIdxs], Y[peakIdxs], "bo") + plt.xlim((0, VSCMGs[0].Omega_max / macros.RPM / 60.0)) plt.figure() - plt.plot(thetaData[:,0]*1e-9, thetaData[:,1]) + plt.plot(thetaData[:, 0] * 1e-9, thetaData[:, 1]) plt.title("Principle Angle") - plt.xlabel('Time (s)') - plt.ylabel(r'$\theta$ (deg)') + plt.xlabel("Time (s)") + plt.ylabel(r"$\theta$ (deg)") if show_plots == True: plt.show() - plt.close('all') + plt.close("all") accuracy = 1e-7 - for i in range(0,len(truePos)): + for i in range(0, len(truePos)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(dataPos[i],truePos[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + dataPos[i], truePos[i], 3, accuracy + ): testFailCount += 1 testMessages.append("FAILED: VSCMG Integrated Test failed pos unit test") - for i in range(0,len(trueSigma)): + for i in range(0, len(trueSigma)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(dataSigma[i],trueSigma[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + dataSigma[i], trueSigma[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: VSCMG Integrated Test failed attitude unit test") + testMessages.append( + "FAILED: VSCMG Integrated Test failed attitude unit test" + ) - if testCase == 'JitterSimple': + if testCase == "JitterSimple": for i in range(N): # check a vector values - if not abs(wheelSpeeds_data[i] - wheelSpeeds_true[i]) / wheelSpeeds_true[i] < .09: + if ( + not abs(wheelSpeeds_data[i] - wheelSpeeds_true[i]) / wheelSpeeds_true[i] + < 0.09 + ): testFailCount += 1 - testMessages.append("FAILED: VSCMG Integrated Test failed jitter unit test") + testMessages.append( + "FAILED: VSCMG Integrated Test failed jitter unit test" + ) accuracy = 1e-10 - if testCase == 'BalancedWheels' or testCase == 'JitterFullyCoupled': - - for i in range(0,len(initialOrbAngMom_N)): + if testCase == "BalancedWheels" or testCase == "JitterFullyCoupled": + for i in range(0, len(initialOrbAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i],initialOrbAngMom_N[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: VSCMG Integrated Test failed orbital angular momentum unit test") + testMessages.append( + "FAILED: VSCMG Integrated Test failed orbital angular momentum unit test" + ) - for i in range(0,len(initialRotAngMom_N)): + for i in range(0, len(initialRotAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotAngMom[i],initialRotAngMom_N[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: VSCMG Integrated Test failed rotational angular momentum unit test") + testMessages.append( + "FAILED: VSCMG Integrated Test failed rotational angular momentum unit test" + ) for i in range(0, len(initialOrbEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: VSCMG Integrated Test failed orbital energy unit test") + testMessages.append( + "FAILED: VSCMG Integrated Test failed orbital energy unit test" + ) for i in range(0, len(initialRotEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotEnergy[i], initialRotEnergy[i], 1, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotEnergy[i], initialRotEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: VSCMG Integrated Test failed rot energy unit test") + testMessages.append( + "FAILED: VSCMG Integrated Test failed rot energy unit test" + ) # print out success message if no errors were found - if testFailCount == 0: + if testFailCount == 0: print("PASSED ") - colorText = 'ForestGreen' - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + colorText = "ForestGreen" + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" # Write some snippets for AutoTex - snippetName = testCase + 'PassFail' + snippetName = testCase + "PassFail" unitTestSupport.writeTeXSnippet(snippetName, passedText, path) elif testFailCount > 0: - colorText = 'Red' - passedText = r'\textcolor{' + colorText + '}{' + "FAILED" + '}' + colorText = "Red" + passedText = r"\textcolor{" + colorText + "}{" + "FAILED" + "}" # Write some snippets for AutoTex - snippetName = testCase + 'PassFail' + snippetName = testCase + "PassFail" unitTestSupport.writeTeXSnippet(snippetName, passedText, path) if testFailCount == 0: @@ -521,7 +590,8 @@ def VSCMGIntegratedTest(show_plots,useFlag,testCase): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": - VSCMGIntegratedTest(False,False,'JitterFullyCoupled') + VSCMGIntegratedTest(False, False, "JitterFullyCoupled") diff --git a/src/simulation/dynamics/constraintEffector/_UnitTest/test_ConstraintDynamicEffectorUnit.py b/src/simulation/dynamics/constraintEffector/_UnitTest/test_ConstraintDynamicEffectorUnit.py index 075b740589..d47c46fd52 100644 --- a/src/simulation/dynamics/constraintEffector/_UnitTest/test_ConstraintDynamicEffectorUnit.py +++ b/src/simulation/dynamics/constraintEffector/_UnitTest/test_ConstraintDynamicEffectorUnit.py @@ -27,15 +27,33 @@ import matplotlib.pyplot as plt import numpy as np -from Basilisk.utilities import SimulationBaseClass, unitTestSupport, orbitalMotion, macros, RigidBodyKinematics -from Basilisk.simulation import spacecraft, constraintDynamicEffector, gravityEffector, svIntegrators +from Basilisk.utilities import ( + SimulationBaseClass, + unitTestSupport, + orbitalMotion, + macros, + RigidBodyKinematics, +) +from Basilisk.simulation import ( + spacecraft, + constraintDynamicEffector, + gravityEffector, + svIntegrators, +) # 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() -@pytest.mark.parametrize("function", ["constraintEffectorOrbitalConservation", "constraintEffectorRotationalConservation"]) + +@pytest.mark.parametrize( + "function", + [ + "constraintEffectorOrbitalConservation", + "constraintEffectorRotationalConservation", + ], +) def test_constraintEffector(show_plots, function): r"""Module Unit Test **Validation Test Description** @@ -78,7 +96,6 @@ def test_constraintEffector(show_plots, function): def constraintEffectorOrbitalConservation(show_plots): - # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() @@ -104,15 +121,23 @@ def constraintEffectorOrbitalConservation(show_plots): # Define mass properties of the rigid hub of both spacecraft scObject1.hub.mHub = 750.0 scObject1.hub.r_BcB_B = [[0.0], [0.0], [1.0]] - scObject1.hub.IHubPntBc_B = [[600.0, 0.0, 0.0], [0.0, 600.0, 0.0], [0.0, 0.0, 600.0]] + scObject1.hub.IHubPntBc_B = [ + [600.0, 0.0, 0.0], + [0.0, 600.0, 0.0], + [0.0, 0.0, 600.0], + ] scObject2.hub.mHub = 750.0 scObject2.hub.r_BcB_B = [[0.0], [0.0], [1.0]] - scObject2.hub.IHubPntBc_B = [[600.0, 0.0, 0.0], [0.0, 600.0, 0.0], [0.0, 0.0, 600.0]] + scObject2.hub.IHubPntBc_B = [ + [600.0, 0.0, 0.0], + [0.0, 600.0, 0.0], + [0.0, 0.0, 600.0], + ] # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # [meters^3/s^2] + earthGravBody.mu = 0.3986004415e15 # [meters^3/s^2] earthGravBody.isCentralBody = True scObject1.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) scObject2.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -127,27 +152,29 @@ def constraintEffectorOrbitalConservation(show_plots): r_B2N_N_0, rDot_B2N_N = orbitalMotion.elem2rv(earthGravBody.mu, oe) # With initial attitudes at zero (B1, B2, and N frames all initially aligned) - dir = r_B2N_N_0/np.linalg.norm(r_B2N_N_0) + dir = r_B2N_N_0 / np.linalg.norm(r_B2N_N_0) l = 0.1 - COMoffset = 0.1 # distance from COM to where the arm connects to the spacecraft hub, same for both spacecraft [meters] - r_P1B1_B1 = np.dot(dir,COMoffset) - r_P2B2_B2 = np.dot(-dir,COMoffset) - r_P2P1_B1Init = np.dot(dir,l) + COMoffset = 0.1 # distance from COM to where the arm connects to the spacecraft hub, same for both spacecraft [meters] + r_P1B1_B1 = np.dot(dir, COMoffset) + r_P2B2_B2 = np.dot(-dir, COMoffset) + r_P2P1_B1Init = np.dot(dir, l) r_B1N_N_0 = r_B2N_N_0 + r_P2B2_B2 - r_P2P1_B1Init - r_P1B1_B1 rDot_B1N_N = rDot_B2N_N # Compute rotational states # let C be the frame at the combined COM of the two vehicles - r_CN_N = (r_B1N_N_0 * scObject1.hub.mHub + r_B2N_N_0 * scObject2.hub.mHub) / (scObject1.hub.mHub + scObject2.hub.mHub) + r_CN_N = (r_B1N_N_0 * scObject1.hub.mHub + r_B2N_N_0 * scObject2.hub.mHub) / ( + scObject1.hub.mHub + scObject2.hub.mHub + ) r_B1C_N = r_B1N_N_0 - r_CN_N r_B2C_N = r_B2N_N_0 - r_CN_N # compute relative velocity due to spin and COM offset - target_spin = [0.01,0.01,0.01] + target_spin = [0.01, 0.01, 0.01] omega_CN_N = np.array(target_spin) omega_B1N_B1_0 = omega_CN_N omega_B2N_B2_0 = omega_CN_N - dv_B1C_N = np.cross(omega_CN_N,r_B1C_N) - dv_B2C_N = np.cross(omega_CN_N,r_B2C_N) + dv_B1C_N = np.cross(omega_CN_N, r_B1C_N) + dv_B2C_N = np.cross(omega_CN_N, r_B2C_N) rDot_B1N_N_0 = rDot_B1N_N + dv_B1C_N rDot_B2N_N_0 = rDot_B2N_N + dv_B2C_N @@ -166,7 +193,7 @@ def constraintEffectorOrbitalConservation(show_plots): constraintEffector.setR_P1B1_B1(r_P1B1_B1) constraintEffector.setR_P2B2_B2(r_P2B2_B2) constraintEffector.setR_P2P1_B1Init(r_P2P1_B1Init) - constraintEffector.setAlpha(1E3) + constraintEffector.setAlpha(1e3) constraintEffector.setBeta(1e3) # Add constraints to both spacecraft scObject1.addDynamicEffector(constraintEffector) @@ -186,8 +213,8 @@ def constraintEffectorOrbitalConservation(show_plots): # Log energy and momentum variables conservationData1 = scObject1.logger(["totOrbAngMomPntN_N", "totOrbEnergy"]) conservationData2 = scObject2.logger(["totOrbAngMomPntN_N", "totOrbEnergy"]) - unitTestSim.AddModelToTask(unitTaskName,conservationData1) - unitTestSim.AddModelToTask(unitTaskName,conservationData2) + unitTestSim.AddModelToTask(unitTaskName, conservationData1) + unitTestSim.AddModelToTask(unitTaskName, conservationData2) # Initialize the simulation unitTestSim.InitializeSimulation() @@ -217,12 +244,14 @@ def constraintEffectorOrbitalConservation(show_plots): r_P2B2_B1 = np.empty(r_B1N_N_hist.shape) sigma_B2B1 = np.empty(sigma_B1N_hist.shape) for i in range(orbAngMom1_N.shape[0]): - dcm_B1N = RigidBodyKinematics.MRP2C(sigma_B1N_hist[i,:]) - r_B1N_B1[i,:] = dcm_B1N@r_B1N_N_hist[i,:] - r_B2N_B1[i,:] = dcm_B1N@r_B2N_N_hist[i,:] - dcm_NB2 = np.transpose(RigidBodyKinematics.MRP2C(sigma_B2N_hist[i,:])) - r_P2B2_B1[i,:] = dcm_B1N@dcm_NB2@r_P2B2_B2 - sigma_B2B1[i,:] = RigidBodyKinematics.subMRP(sigma_B2N_hist[i,:],sigma_B1N_hist[i,:]) + dcm_B1N = RigidBodyKinematics.MRP2C(sigma_B1N_hist[i, :]) + r_B1N_B1[i, :] = dcm_B1N @ r_B1N_N_hist[i, :] + r_B2N_B1[i, :] = dcm_B1N @ r_B2N_N_hist[i, :] + dcm_NB2 = np.transpose(RigidBodyKinematics.MRP2C(sigma_B2N_hist[i, :])) + r_P2B2_B1[i, :] = dcm_B1N @ dcm_NB2 @ r_P2B2_B2 + sigma_B2B1[i, :] = RigidBodyKinematics.subMRP( + sigma_B2N_hist[i, :], sigma_B1N_hist[i, :] + ) psi_B1 = r_B1N_B1 + r_P1B1_B1 + r_P2P1_B1Init - (r_B2N_B1 + r_P2B2_B1) # Compute conservation quantities @@ -235,58 +264,87 @@ def constraintEffectorOrbitalConservation(show_plots): plt.clf() for i in range(3): plt.semilogy(constraintTimeData, np.abs(psi_B1[:, i])) - plt.semilogy(constraintTimeData, np.linalg.norm(psi_B1,axis=1)) - plt.legend([r'$\psi_1$',r'$\psi_2$',r'$\psi_3$',r'$\psi$ magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'variation from fixed position: $\psi$ (meters)') - plt.title('Direction Constraint Violation Components') + plt.semilogy(constraintTimeData, np.linalg.norm(psi_B1, axis=1)) + plt.legend([r"$\psi_1$", r"$\psi_2$", r"$\psi_3$", r"$\psi$ magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"variation from fixed position: $\psi$ (meters)") + plt.title("Direction Constraint Violation Components") plt.figure() plt.clf() for i in range(3): - plt.semilogy(constraintTimeData, np.abs(4*np.arctan(sigma_B2B1[:, i]) * macros.R2D)) - plt.semilogy(constraintTimeData, np.linalg.norm(4*np.arctan(sigma_B2B1) * macros.R2D,axis=1)) - plt.legend([r'$\phi_1$',r'$\phi_2$',r'$\phi_3$',r'$\phi$ magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'relative attitude angle: $\phi$ (deg)') - plt.title('Attitude Constraint Violation Components') + plt.semilogy( + constraintTimeData, np.abs(4 * np.arctan(sigma_B2B1[:, i]) * macros.R2D) + ) + plt.semilogy( + constraintTimeData, + np.linalg.norm(4 * np.arctan(sigma_B2B1) * macros.R2D, axis=1), + ) + plt.legend([r"$\phi_1$", r"$\phi_2$", r"$\phi_3$", r"$\phi$ magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"relative attitude angle: $\phi$ (deg)") + plt.title("Attitude Constraint Violation Components") plt.figure() plt.clf() - plt.plot(conservationTimeData, (combinedOrbAngMom[:,0] - combinedOrbAngMom[0,0])/combinedOrbAngMom[0,0], - conservationTimeData, (combinedOrbAngMom[:,1] - combinedOrbAngMom[0,1])/combinedOrbAngMom[0,1], - conservationTimeData, (combinedOrbAngMom[:,2] - combinedOrbAngMom[0,2])/combinedOrbAngMom[0,2]) - plt.xlabel('time (seconds)') - plt.ylabel('Relative Difference') - plt.title('Combined Orbital Angular Momentum') + plt.plot( + conservationTimeData, + (combinedOrbAngMom[:, 0] - combinedOrbAngMom[0, 0]) / combinedOrbAngMom[0, 0], + conservationTimeData, + (combinedOrbAngMom[:, 1] - combinedOrbAngMom[0, 1]) / combinedOrbAngMom[0, 1], + conservationTimeData, + (combinedOrbAngMom[:, 2] - combinedOrbAngMom[0, 2]) / combinedOrbAngMom[0, 2], + ) + plt.xlabel("time (seconds)") + plt.ylabel("Relative Difference") + plt.title("Combined Orbital Angular Momentum") plt.figure() plt.clf() - plt.plot(conservationTimeData, (combinedOrbEnergy - combinedOrbEnergy[0])/combinedOrbEnergy[0]) - plt.xlabel('time (seconds)') - plt.ylabel('Relative Difference') - plt.title('Combined Orbital Energy') + plt.plot( + conservationTimeData, + (combinedOrbEnergy - combinedOrbEnergy[0]) / combinedOrbEnergy[0], + ) + plt.xlabel("time (seconds)") + plt.ylabel("Relative Difference") + plt.title("Combined Orbital Energy") if show_plots: plt.show() plt.close("all") # Testing setup - dirCnstAccuracy = 1E-7 - attCnstAccuracy = 1E-4 - accuracy = 1E-12 - np.testing.assert_allclose(psi_B1, 0, atol=dirCnstAccuracy, - err_msg='direction constraint component magnitude exceeded in orbital conservation test') - np.testing.assert_allclose(sigma_B2B1, 0, atol=attCnstAccuracy, - err_msg='attitude constraint component magnitude exceeded in orbital conservation test') + dirCnstAccuracy = 1e-7 + attCnstAccuracy = 1e-4 + accuracy = 1e-12 + np.testing.assert_allclose( + psi_B1, + 0, + atol=dirCnstAccuracy, + err_msg="direction constraint component magnitude exceeded in orbital conservation test", + ) + np.testing.assert_allclose( + sigma_B2B1, + 0, + atol=attCnstAccuracy, + err_msg="attitude constraint component magnitude exceeded in orbital conservation test", + ) for i in range(3): - np.testing.assert_allclose(orbAngMom1_N[:,i]+orbAngMom2_N[:,i], orbAngMom1_N[0,i]+orbAngMom2_N[0,i], atol=accuracy, - err_msg='orbital angular momentum difference component magnitude exceeded') - np.testing.assert_allclose(orbEnergy1+orbEnergy2, orbEnergy1[0]+orbEnergy2[0], atol=accuracy, - err_msg='orbital energy difference magnitude exceeded') + np.testing.assert_allclose( + orbAngMom1_N[:, i] + orbAngMom2_N[:, i], + orbAngMom1_N[0, i] + orbAngMom2_N[0, i], + atol=accuracy, + err_msg="orbital angular momentum difference component magnitude exceeded", + ) + np.testing.assert_allclose( + orbEnergy1 + orbEnergy2, + orbEnergy1[0] + orbEnergy2[0], + atol=accuracy, + err_msg="orbital energy difference magnitude exceeded", + ) -def constraintEffectorRotationalConservation(show_plots): +def constraintEffectorRotationalConservation(show_plots): # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() @@ -312,35 +370,45 @@ def constraintEffectorRotationalConservation(show_plots): # Define mass properties of the rigid hub of both spacecraft scObject1.hub.mHub = 750.0 scObject1.hub.r_BcB_B = [[0.0], [0.0], [1.0]] - scObject1.hub.IHubPntBc_B = [[600.0, 0.0, 0.0], [0.0, 600.0, 0.0], [0.0, 0.0, 600.0]] + scObject1.hub.IHubPntBc_B = [ + [600.0, 0.0, 0.0], + [0.0, 600.0, 0.0], + [0.0, 0.0, 600.0], + ] scObject2.hub.mHub = 750.0 scObject2.hub.r_BcB_B = [[0.0], [0.0], [1.0]] - scObject2.hub.IHubPntBc_B = [[600.0, 0.0, 0.0], [0.0, 600.0, 0.0], [0.0, 0.0, 600.0]] + scObject2.hub.IHubPntBc_B = [ + [600.0, 0.0, 0.0], + [0.0, 600.0, 0.0], + [0.0, 0.0, 600.0], + ] # With initial attitudes at zero (B1, B2, and N frames all initially aligned) - r_B2N_N_0 = np.array([1,1,1]) - rDot_B2N_N = np.array([1,1,1]) - dir = r_B2N_N_0/np.linalg.norm(r_B2N_N_0) + r_B2N_N_0 = np.array([1, 1, 1]) + rDot_B2N_N = np.array([1, 1, 1]) + dir = r_B2N_N_0 / np.linalg.norm(r_B2N_N_0) l = 0.1 - COMoffset = 0.1 # distance from COM to where the arm connects to the spacecraft hub, same for both spacecraft [meters] - r_P1B1_B1 = np.dot(dir,COMoffset) - r_P2B2_B2 = np.dot(-dir,COMoffset) - r_P2P1_B1Init = np.dot(dir,l) + COMoffset = 0.1 # distance from COM to where the arm connects to the spacecraft hub, same for both spacecraft [meters] + r_P1B1_B1 = np.dot(dir, COMoffset) + r_P2B2_B2 = np.dot(-dir, COMoffset) + r_P2P1_B1Init = np.dot(dir, l) r_B1N_N_0 = r_B2N_N_0 + r_P2B2_B2 - r_P2P1_B1Init - r_P1B1_B1 rDot_B1N_N = rDot_B2N_N # Compute rotational states # let C be the frame at the combined COM of the two vehicles - r_CN_N = (r_B1N_N_0 * scObject1.hub.mHub + r_B2N_N_0 * scObject2.hub.mHub) / (scObject1.hub.mHub + scObject2.hub.mHub) + r_CN_N = (r_B1N_N_0 * scObject1.hub.mHub + r_B2N_N_0 * scObject2.hub.mHub) / ( + scObject1.hub.mHub + scObject2.hub.mHub + ) r_B1C_N = r_B1N_N_0 - r_CN_N r_B2C_N = r_B2N_N_0 - r_CN_N # compute relative velocity due to spin and COM offset - target_spin = [0.01,0.01,0.01] + target_spin = [0.01, 0.01, 0.01] omega_CN_N = np.array(target_spin) omega_B1N_B1_0 = omega_CN_N omega_B2N_B2_0 = omega_CN_N - dv_B1C_N = np.cross(omega_CN_N,r_B1C_N) - dv_B2C_N = np.cross(omega_CN_N,r_B2C_N) + dv_B1C_N = np.cross(omega_CN_N, r_B1C_N) + dv_B2C_N = np.cross(omega_CN_N, r_B2C_N) rDot_B1N_N_0 = rDot_B1N_N + dv_B1C_N rDot_B2N_N_0 = rDot_B2N_N + dv_B2C_N @@ -359,7 +427,7 @@ def constraintEffectorRotationalConservation(show_plots): constraintEffector.setR_P1B1_B1(r_P1B1_B1) constraintEffector.setR_P2B2_B2(r_P2B2_B2) constraintEffector.setR_P2P1_B1Init(r_P2P1_B1Init) - constraintEffector.setAlpha(1E3) + constraintEffector.setAlpha(1e3) constraintEffector.setBeta(1e3) # Add constraints to both spacecraft scObject1.addDynamicEffector(constraintEffector) @@ -379,8 +447,8 @@ def constraintEffectorRotationalConservation(show_plots): # Log energy and momentum variables conservationData1 = scObject1.logger(["totRotAngMomPntC_N", "totRotEnergy"]) conservationData2 = scObject2.logger(["totRotAngMomPntC_N", "totRotEnergy"]) - unitTestSim.AddModelToTask(unitTaskName,conservationData1) - unitTestSim.AddModelToTask(unitTaskName,conservationData2) + unitTestSim.AddModelToTask(unitTaskName, conservationData1) + unitTestSim.AddModelToTask(unitTaskName, conservationData2) # Initialize the simulation unitTestSim.InitializeSimulation() @@ -413,26 +481,40 @@ def constraintEffectorRotationalConservation(show_plots): r_B2N_B1 = np.empty(r_B2N_N_hist.shape) r_P2B2_B1 = np.empty(r_B1N_N_hist.shape) for i in range(rotAngMom1PntC1_N.shape[0]): - dcm_B1N = RigidBodyKinematics.MRP2C(sigma_B1N_hist[i,:]) - r_B1N_B1[i,:] = dcm_B1N@r_B1N_N_hist[i,:] - r_B2N_B1[i,:] = dcm_B1N@r_B2N_N_hist[i,:] - dcm_NB2 = np.transpose(RigidBodyKinematics.MRP2C(sigma_B2N_hist[i,:])) - r_P2B2_B1[i,:] = dcm_B1N@dcm_NB2@r_P2B2_B2 + dcm_B1N = RigidBodyKinematics.MRP2C(sigma_B1N_hist[i, :]) + r_B1N_B1[i, :] = dcm_B1N @ r_B1N_N_hist[i, :] + r_B2N_B1[i, :] = dcm_B1N @ r_B2N_N_hist[i, :] + dcm_NB2 = np.transpose(RigidBodyKinematics.MRP2C(sigma_B2N_hist[i, :])) + r_P2B2_B1[i, :] = dcm_B1N @ dcm_NB2 @ r_P2B2_B2 psi_B1 = r_B1N_B1 + r_P1B1_B1 + r_P2P1_B1Init - (r_B2N_B1 + r_P2B2_B1) - sigma_B2B1 = sigma_B2N_hist-sigma_B1N_hist + sigma_B2B1 = sigma_B2N_hist - sigma_B1N_hist # Compute conservation quantities - r_CN_N_hist = (r_B1N_N_hist * scObject1.hub.mHub + r_B2N_N_hist * scObject2.hub.mHub)/(scObject1.hub.mHub+scObject2.hub.mHub) + r_CN_N_hist = ( + r_B1N_N_hist * scObject1.hub.mHub + r_B2N_N_hist * scObject2.hub.mHub + ) / (scObject1.hub.mHub + scObject2.hub.mHub) r_B1C_N_hist = r_B1N_N_hist - r_CN_N_hist r_B2C_N_hist = r_B2N_N_hist - r_CN_N_hist - rdot_CN_N_hist = (rdot_B1N_N_hist * scObject1.hub.mHub + rdot_B2N_N_hist * scObject2.hub.mHub)/(scObject1.hub.mHub+scObject2.hub.mHub) + rdot_CN_N_hist = ( + rdot_B1N_N_hist * scObject1.hub.mHub + rdot_B2N_N_hist * scObject2.hub.mHub + ) / (scObject1.hub.mHub + scObject2.hub.mHub) rdot_B1C_N_hist = rdot_B1N_N_hist - rdot_CN_N_hist rdot_B2C_N_hist = rdot_B2N_N_hist - rdot_CN_N_hist rotAngMom1PntCT_N = np.empty(rotAngMom1PntC1_N.shape) rotAngMom2PntCT_N = np.empty(rotAngMom2PntC2_N.shape) for i1 in range(rotAngMom1PntC1_N.shape[0]): - rotAngMom1PntCT_N[i1,:] = np.cross(r_CN_N_hist[i1,:],scObject1.hub.mHub*rdot_B1C_N_hist[i1,:]) + RigidBodyKinematics.MRP2C(sigma_B1N_hist[i1,:]).transpose()@(scObject1.hub.IHubPntBc_B@omega_B1N_B1_hist[i1,:]) + scObject1.hub.mHub*np.cross(r_B1C_N_hist[i1,:],rdot_B1C_N_hist[i1,:]) - rotAngMom2PntCT_N[i1,:] = np.cross(r_CN_N_hist[i1,:],scObject2.hub.mHub*rdot_B2C_N_hist[i1,:]) + RigidBodyKinematics.MRP2C(sigma_B2N_hist[i1,:]).transpose()@(scObject2.hub.IHubPntBc_B@omega_B2N_B2_hist[i1,:]) + scObject2.hub.mHub*np.cross(r_B2C_N_hist[i1,:],rdot_B2C_N_hist[i1,:]) + rotAngMom1PntCT_N[i1, :] = ( + np.cross(r_CN_N_hist[i1, :], scObject1.hub.mHub * rdot_B1C_N_hist[i1, :]) + + RigidBodyKinematics.MRP2C(sigma_B1N_hist[i1, :]).transpose() + @ (scObject1.hub.IHubPntBc_B @ omega_B1N_B1_hist[i1, :]) + + scObject1.hub.mHub * np.cross(r_B1C_N_hist[i1, :], rdot_B1C_N_hist[i1, :]) + ) + rotAngMom2PntCT_N[i1, :] = ( + np.cross(r_CN_N_hist[i1, :], scObject2.hub.mHub * rdot_B2C_N_hist[i1, :]) + + RigidBodyKinematics.MRP2C(sigma_B2N_hist[i1, :]).transpose() + @ (scObject2.hub.IHubPntBc_B @ omega_B2N_B2_hist[i1, :]) + + scObject2.hub.mHub * np.cross(r_B2C_N_hist[i1, :], rdot_B2C_N_hist[i1, :]) + ) combinedRotAngMom = rotAngMom1PntCT_N + rotAngMom2PntCT_N combinedRotEnergy = rotEnergy1 + rotEnergy2 @@ -442,53 +524,83 @@ def constraintEffectorRotationalConservation(show_plots): plt.clf() for i in range(3): plt.semilogy(constraintTimeData, np.abs(psi_B1[:, i])) - plt.semilogy(constraintTimeData, np.linalg.norm(psi_B1,axis=1)) - plt.legend([r'$\psi_1$',r'$\psi_2$',r'$\psi_3$',r'$\psi$ magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'variation from fixed position: $\psi$ (meters)') - plt.title('Direction Constraint Violation Components') + plt.semilogy(constraintTimeData, np.linalg.norm(psi_B1, axis=1)) + plt.legend([r"$\psi_1$", r"$\psi_2$", r"$\psi_3$", r"$\psi$ magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"variation from fixed position: $\psi$ (meters)") + plt.title("Direction Constraint Violation Components") plt.figure() plt.clf() for i in range(3): - plt.semilogy(constraintTimeData, np.abs(4*np.arctan(sigma_B2B1[:, i]) * macros.R2D)) - plt.semilogy(constraintTimeData, np.linalg.norm(4*np.arctan(sigma_B2B1) * macros.R2D,axis=1)) - plt.legend([r'$\phi_1$',r'$\phi_2$',r'$\phi_3$',r'$\phi$ magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'relative attitude angle: $\phi$ (deg)') - plt.title('Attitude Constraint Violation Components') + plt.semilogy( + constraintTimeData, np.abs(4 * np.arctan(sigma_B2B1[:, i]) * macros.R2D) + ) + plt.semilogy( + constraintTimeData, + np.linalg.norm(4 * np.arctan(sigma_B2B1) * macros.R2D, axis=1), + ) + plt.legend([r"$\phi_1$", r"$\phi_2$", r"$\phi_3$", r"$\phi$ magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"relative attitude angle: $\phi$ (deg)") + plt.title("Attitude Constraint Violation Components") plt.figure() plt.clf() - plt.plot(conservationTimeData, (combinedRotAngMom[:,0] - combinedRotAngMom[0,0])/combinedRotAngMom[0,0], - conservationTimeData, (combinedRotAngMom[:,1] - combinedRotAngMom[0,1])/combinedRotAngMom[0,1], - conservationTimeData, (combinedRotAngMom[:,2] - combinedRotAngMom[0,2])/combinedRotAngMom[0,2]) - plt.xlabel('time (seconds)') - plt.ylabel('Relative Difference') - plt.title('Combined Rotational Angular Momentum') + plt.plot( + conservationTimeData, + (combinedRotAngMom[:, 0] - combinedRotAngMom[0, 0]) / combinedRotAngMom[0, 0], + conservationTimeData, + (combinedRotAngMom[:, 1] - combinedRotAngMom[0, 1]) / combinedRotAngMom[0, 1], + conservationTimeData, + (combinedRotAngMom[:, 2] - combinedRotAngMom[0, 2]) / combinedRotAngMom[0, 2], + ) + plt.xlabel("time (seconds)") + plt.ylabel("Relative Difference") + plt.title("Combined Rotational Angular Momentum") plt.figure() plt.clf() - plt.plot(conservationTimeData, (combinedRotEnergy - combinedRotEnergy[0])/combinedRotEnergy[0]) - plt.xlabel('time (seconds)') - plt.ylabel('Relative Difference') - plt.title('Combined Rotational Energy') + plt.plot( + conservationTimeData, + (combinedRotEnergy - combinedRotEnergy[0]) / combinedRotEnergy[0], + ) + plt.xlabel("time (seconds)") + plt.ylabel("Relative Difference") + plt.title("Combined Rotational Energy") if show_plots: plt.show() plt.close("all") # Testing setup - accuracy = 1E-12 - np.testing.assert_allclose(psi_B1, 0, atol=accuracy, - err_msg='direction constraint component magnitude exceeded in rotational conservation test') - np.testing.assert_allclose(sigma_B2B1, 0, atol=accuracy, - err_msg='attitude constraint component magnitude exceeded in rotational conservation test') + accuracy = 1e-12 + np.testing.assert_allclose( + psi_B1, + 0, + atol=accuracy, + err_msg="direction constraint component magnitude exceeded in rotational conservation test", + ) + np.testing.assert_allclose( + sigma_B2B1, + 0, + atol=accuracy, + err_msg="attitude constraint component magnitude exceeded in rotational conservation test", + ) for i in range(3): - np.testing.assert_allclose(rotAngMom1PntCT_N[:,i]+rotAngMom2PntCT_N[:,i], rotAngMom1PntCT_N[0,i]+rotAngMom2PntCT_N[0,i], atol=accuracy, - err_msg='rotational angular momentum difference component magnitude exceeded') - np.testing.assert_allclose(rotEnergy1+rotEnergy2, rotEnergy1[0]+rotEnergy2[0], atol=accuracy, - err_msg='rotational energy difference magnitude exceeded') + np.testing.assert_allclose( + rotAngMom1PntCT_N[:, i] + rotAngMom2PntCT_N[:, i], + rotAngMom1PntCT_N[0, i] + rotAngMom2PntCT_N[0, i], + atol=accuracy, + err_msg="rotational angular momentum difference component magnitude exceeded", + ) + np.testing.assert_allclose( + rotEnergy1 + rotEnergy2, + rotEnergy1[0] + rotEnergy2[0], + atol=accuracy, + err_msg="rotational energy difference magnitude exceeded", + ) + if __name__ == "__main__": # constraintEffectorOrbitalConservation(True) diff --git a/src/simulation/dynamics/constraintEffector/_UnitTest/test_InOutMessageFiltering.py b/src/simulation/dynamics/constraintEffector/_UnitTest/test_InOutMessageFiltering.py index 486816f90b..9378ac07c5 100644 --- a/src/simulation/dynamics/constraintEffector/_UnitTest/test_InOutMessageFiltering.py +++ b/src/simulation/dynamics/constraintEffector/_UnitTest/test_InOutMessageFiltering.py @@ -28,8 +28,19 @@ import numpy as np from contextlib import nullcontext -from Basilisk.utilities import SimulationBaseClass, unitTestSupport, orbitalMotion, macros, RigidBodyKinematics -from Basilisk.simulation import spacecraft, constraintDynamicEffector, gravityEffector, svIntegrators +from Basilisk.utilities import ( + SimulationBaseClass, + unitTestSupport, + orbitalMotion, + macros, + RigidBodyKinematics, +) +from Basilisk.simulation import ( + spacecraft, + constraintDynamicEffector, + gravityEffector, + svIntegrators, +) from Basilisk.architecture import messaging from Basilisk.architecture.bskLogging import BasiliskError @@ -38,14 +49,18 @@ # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail() -@pytest.mark.parametrize("CutOffFreq,useConstEffector",[ - (0.1,0), #Constraint Dynamic effector not connected - (0.1,1), #Constraint Dynamic effector connected - (0.1,-1), #Constraint Dynamic effector default state (connected) - (-1,1), # Negative Cut off frequency test - (0,1)]) #Zero cut off frequency test -def test_constraintEffectorAllCases(show_plots,CutOffFreq,useConstEffector): +@pytest.mark.parametrize( + "CutOffFreq,useConstEffector", + [ + (0.1, 0), # Constraint Dynamic effector not connected + (0.1, 1), # Constraint Dynamic effector connected + (0.1, -1), # Constraint Dynamic effector default state (connected) + (-1, 1), # Negative Cut off frequency test + (0, 1), + ], +) # Zero cut off frequency test +def test_constraintEffectorAllCases(show_plots, CutOffFreq, useConstEffector): r"""Module Unit Test **Validation Test Description** @@ -98,35 +113,45 @@ def run_test(show_plots, CutOffFreq, useConstEffector): # Define mass properties of the rigid hub of both spacecraft scObject1.hub.mHub = 750.0 scObject1.hub.r_BcB_B = [[0.0], [0.0], [1.0]] - scObject1.hub.IHubPntBc_B = [[600.0, 0.0, 0.0], [0.0, 600.0, 0.0], [0.0, 0.0, 600.0]] + scObject1.hub.IHubPntBc_B = [ + [600.0, 0.0, 0.0], + [0.0, 600.0, 0.0], + [0.0, 0.0, 600.0], + ] scObject2.hub.mHub = 750.0 scObject2.hub.r_BcB_B = [[0.0], [0.0], [1.0]] - scObject2.hub.IHubPntBc_B = [[600.0, 0.0, 0.0], [0.0, 600.0, 0.0], [0.0, 0.0, 600.0]] + scObject2.hub.IHubPntBc_B = [ + [600.0, 0.0, 0.0], + [0.0, 600.0, 0.0], + [0.0, 0.0, 600.0], + ] # With initial attitudes at zero (B1, B2, and N frames all initially aligned) - r_B2N_N_0 = np.array([1,1,1]) - rDot_B2N_N = np.array([1,1,1]) - dir = r_B2N_N_0/np.linalg.norm(r_B2N_N_0) + r_B2N_N_0 = np.array([1, 1, 1]) + rDot_B2N_N = np.array([1, 1, 1]) + dir = r_B2N_N_0 / np.linalg.norm(r_B2N_N_0) l = 0.1 - COMoffset = 0.1 # distance from COM to where the arm connects to the spacecraft hub, same for both spacecraft [meters] - r_P1B1_B1 = np.dot(dir,COMoffset) - r_P2B2_B2 = np.dot(-dir,COMoffset) - r_P2P1_B1Init = np.dot(dir,l) + COMoffset = 0.1 # distance from COM to where the arm connects to the spacecraft hub, same for both spacecraft [meters] + r_P1B1_B1 = np.dot(dir, COMoffset) + r_P2B2_B2 = np.dot(-dir, COMoffset) + r_P2P1_B1Init = np.dot(dir, l) r_B1N_N_0 = r_B2N_N_0 + r_P2B2_B2 - r_P2P1_B1Init - r_P1B1_B1 rDot_B1N_N = rDot_B2N_N # Compute rotational states # let C be the frame at the combined COM of the two vehicles - r_CN_N = (r_B1N_N_0 * scObject1.hub.mHub + r_B2N_N_0 * scObject2.hub.mHub) / (scObject1.hub.mHub + scObject2.hub.mHub) + r_CN_N = (r_B1N_N_0 * scObject1.hub.mHub + r_B2N_N_0 * scObject2.hub.mHub) / ( + scObject1.hub.mHub + scObject2.hub.mHub + ) r_B1C_N = r_B1N_N_0 - r_CN_N r_B2C_N = r_B2N_N_0 - r_CN_N # compute relative velocity due to spin and COM offset - target_spin = [0.01,0.01,0.01] + target_spin = [0.01, 0.01, 0.01] omega_CN_N = np.array(target_spin) omega_B1N_B1_0 = omega_CN_N omega_B2N_B2_0 = omega_CN_N - dv_B1C_N = np.cross(omega_CN_N,r_B1C_N) - dv_B2C_N = np.cross(omega_CN_N,r_B2C_N) + dv_B1C_N = np.cross(omega_CN_N, r_B1C_N) + dv_B2C_N = np.cross(omega_CN_N, r_B2C_N) rDot_B1N_N_0 = rDot_B1N_N + dv_B1C_N rDot_B2N_N_0 = rDot_B2N_N + dv_B2C_N @@ -138,10 +163,10 @@ def run_test(show_plots, CutOffFreq, useConstEffector): scObject2.hub.v_CN_NInit = rDot_B2N_N_0 scObject2.hub.omega_BN_BInit = omega_B2N_B2_0 - alpha = 1E3 - beta = 1E3 - k_d = alpha*alpha - c_d = 2*beta + alpha = 1e3 + beta = 1e3 + k_d = alpha * alpha + c_d = 2 * beta wc = CutOffFreq h = 1.0 k = 0.7 @@ -155,7 +180,7 @@ def run_test(show_plots, CutOffFreq, useConstEffector): constraintEffector.setR_P2P1_B1Init(r_P2P1_B1Init) constraintEffector.setAlpha(alpha) constraintEffector.setBeta(beta) - constraintEffector.setFilter_Data(wc,h,k) + constraintEffector.setFilter_Data(wc, h, k) if useConstEffector != -1: effectorStatusMsgPayload = messaging.DeviceStatusMsgPayload() @@ -172,10 +197,9 @@ def run_test(show_plots, CutOffFreq, useConstEffector): unitTestSim.AddModelToTask(unitTaskName, scObject2) unitTestSim.AddModelToTask(unitTaskName, constraintEffector) - - if useConstEffector==1: + if useConstEffector == 1: print("Constraint effector is connected") - elif useConstEffector==0: + elif useConstEffector == 0: print("Constraint effector is not connected") else: print("Default behaviour expected") @@ -224,9 +248,8 @@ def run_test(show_plots, CutOffFreq, useConstEffector): final_psi_compare = np.zeros(r_B1N_N_hist.shape) sigma_B2B1 = np.zeros(r_B1N_N_hist.shape) - if useConstEffector!=0: - - # Compute constraint violations + if useConstEffector != 0: + # Compute constraint violations check_psi_N = np.empty(r_B1N_N_hist.shape) psiPrime_N = np.empty(r_B1N_N_hist.shape) check_FcN = np.empty(r_B1N_N_hist.shape) @@ -237,41 +260,60 @@ def run_test(show_plots, CutOffFreq, useConstEffector): check_filtered_LB2 = np.zeros(r_B1N_N_hist.shape[0]) for i in range(r_B1N_N_hist.shape[0]): - dcm_NB1 = np.transpose(RigidBodyKinematics.MRP2C(sigma_B1N_hist[i,:])) - dcm_B1N = RigidBodyKinematics.MRP2C(sigma_B1N_hist[i,:]) - dcm_B2N = RigidBodyKinematics.MRP2C(sigma_B2N_hist[i,:]) - dcm_NB2 = np.transpose(RigidBodyKinematics.MRP2C(sigma_B2N_hist[i,:])) - r_P2P1_N = dcm_NB2@r_P2B2_B2+r_B2N_N_hist[i,:]-dcm_NB1@r_P1B1_B1-r_B1N_N_hist[i,:] - sigma_B2B1[i,:] = RigidBodyKinematics.C2MRP(dcm_B2N@dcm_NB1) - rDot_P1B1_B1 = np.cross(omega_B1N_B1_hist[i,:],r_P1B1_B1) - rDot_P2B2_B2 = np.cross(omega_B2N_B2_hist[i,:],r_P2B2_B2) - rDot_P1N_N = dcm_NB1@rDot_P1B1_B1+rdot_B1N_N_hist[i,:] - rDot_P2N_N = dcm_NB2@rDot_P2B2_B2+rdot_B2N_N_hist[i,:] - rDot_P2P1_N = rDot_P2N_N-rDot_P1N_N - check_psi_N[i,:] = r_P2P1_N - dcm_NB1@r_P2P1_B1Init - omega_B1N_N = dcm_NB1@omega_B1N_B1_hist[i,:] - psiPrime_N[i,:] = rDot_P2P1_N - np.cross(omega_B1N_N,r_P2P1_N) - check_FcN[i,:] = k_d*check_psi_N[i,:]+c_d*psiPrime_N[i,:] - - omega_B1N_B2 = dcm_B2N@omega_B1N_N - omega_B2B1_B2 = omega_B2N_B2_hist[i,:]-omega_B1N_B2 - Fc_B1 = dcm_B1N@check_FcN[i,:] - L_B1_len = np.cross(r_P1B1_B1,Fc_B1) - Fc_B2 = dcm_B2N@check_FcN[i,:] - L_B2_len = -np.cross(r_P2B2_B2,Fc_B2) - dcm_B1B2 = dcm_B1N@dcm_NB2 - L_B2_att = -k_d*sigma_B2B1[i,:]-c_d*0.25*RigidBodyKinematics.BmatMRP(sigma_B2B1[i,:])@omega_B2B1_B2 - L_B1_att = -dcm_B1B2@L_B2_att - check_LB2[i,:] = L_B2_len+L_B2_att - check_LB1[i,:] = L_B1_len+L_B1_att - - final_psi_compare = np.linalg.norm(psi_N_hist[-1,:]-check_psi_N[-1,:]) - final_FcN_compare = np.linalg.norm(Fc_N_hist[-1,:]-check_FcN[-1,:]) - final_L_B1_compare = np.linalg.norm(L_B1_hist[-1,:]-check_LB1[-1,:]) - final_L_B2_compare = np.linalg.norm(L_B2_hist[-1,:]-check_LB2[-1,:]) - - num_coeffs = np.array([np.power(wc * h, 2), 2 * np.power(wc * h, 2), np.power(wc * h, 2)]) - denom_coeffs = np.array([-4 + 4 * k * h - np.power(wc * h, 2),8 - 2 * np.power(wc * h, 2),4 + 4 * k * h + np.power(wc * h, 2)]) + dcm_NB1 = np.transpose(RigidBodyKinematics.MRP2C(sigma_B1N_hist[i, :])) + dcm_B1N = RigidBodyKinematics.MRP2C(sigma_B1N_hist[i, :]) + dcm_B2N = RigidBodyKinematics.MRP2C(sigma_B2N_hist[i, :]) + dcm_NB2 = np.transpose(RigidBodyKinematics.MRP2C(sigma_B2N_hist[i, :])) + r_P2P1_N = ( + dcm_NB2 @ r_P2B2_B2 + + r_B2N_N_hist[i, :] + - dcm_NB1 @ r_P1B1_B1 + - r_B1N_N_hist[i, :] + ) + sigma_B2B1[i, :] = RigidBodyKinematics.C2MRP(dcm_B2N @ dcm_NB1) + rDot_P1B1_B1 = np.cross(omega_B1N_B1_hist[i, :], r_P1B1_B1) + rDot_P2B2_B2 = np.cross(omega_B2N_B2_hist[i, :], r_P2B2_B2) + rDot_P1N_N = dcm_NB1 @ rDot_P1B1_B1 + rdot_B1N_N_hist[i, :] + rDot_P2N_N = dcm_NB2 @ rDot_P2B2_B2 + rdot_B2N_N_hist[i, :] + rDot_P2P1_N = rDot_P2N_N - rDot_P1N_N + check_psi_N[i, :] = r_P2P1_N - dcm_NB1 @ r_P2P1_B1Init + omega_B1N_N = dcm_NB1 @ omega_B1N_B1_hist[i, :] + psiPrime_N[i, :] = rDot_P2P1_N - np.cross(omega_B1N_N, r_P2P1_N) + check_FcN[i, :] = k_d * check_psi_N[i, :] + c_d * psiPrime_N[i, :] + + omega_B1N_B2 = dcm_B2N @ omega_B1N_N + omega_B2B1_B2 = omega_B2N_B2_hist[i, :] - omega_B1N_B2 + Fc_B1 = dcm_B1N @ check_FcN[i, :] + L_B1_len = np.cross(r_P1B1_B1, Fc_B1) + Fc_B2 = dcm_B2N @ check_FcN[i, :] + L_B2_len = -np.cross(r_P2B2_B2, Fc_B2) + dcm_B1B2 = dcm_B1N @ dcm_NB2 + L_B2_att = ( + -k_d * sigma_B2B1[i, :] + - c_d + * 0.25 + * RigidBodyKinematics.BmatMRP(sigma_B2B1[i, :]) + @ omega_B2B1_B2 + ) + L_B1_att = -dcm_B1B2 @ L_B2_att + check_LB2[i, :] = L_B2_len + L_B2_att + check_LB1[i, :] = L_B1_len + L_B1_att + + final_psi_compare = np.linalg.norm(psi_N_hist[-1, :] - check_psi_N[-1, :]) + final_FcN_compare = np.linalg.norm(Fc_N_hist[-1, :] - check_FcN[-1, :]) + final_L_B1_compare = np.linalg.norm(L_B1_hist[-1, :] - check_LB1[-1, :]) + final_L_B2_compare = np.linalg.norm(L_B2_hist[-1, :] - check_LB2[-1, :]) + + num_coeffs = np.array( + [np.power(wc * h, 2), 2 * np.power(wc * h, 2), np.power(wc * h, 2)] + ) + denom_coeffs = np.array( + [ + -4 + 4 * k * h - np.power(wc * h, 2), + 8 - 2 * np.power(wc * h, 2), + 4 + 4 * k * h + np.power(wc * h, 2), + ] + ) # Calculations a = denom_coeffs[1] / denom_coeffs[2] @@ -284,14 +326,32 @@ def run_test(show_plots, CutOffFreq, useConstEffector): check_filtered_LB1[0:2] = T1_filtered_hist[0:2] check_filtered_LB2[0:2] = T2_filtered_hist[0:2] - for i in range(2,r_B1N_N_hist.shape[0]): - check_filtered_FcN[i] = a*check_filtered_FcN[i-1]+b*check_filtered_FcN[i-2]+c*np.linalg.norm(Fc_N_hist[i,:])+d*np.linalg.norm(Fc_N_hist[i-1,:])+e*np.linalg.norm(Fc_N_hist[i-2,:]) - check_filtered_LB1[i] = a*check_filtered_LB1[i-1]+b*check_filtered_LB1[i-2]+c*np.linalg.norm(L_B1_hist[i,:])+d*np.linalg.norm(L_B1_hist[i-1,:])+e*np.linalg.norm(L_B1_hist[i-2,:]) - check_filtered_LB2[i] = a*check_filtered_LB2[i-1]+b*check_filtered_LB2[i-2]+c*np.linalg.norm(L_B2_hist[i,:])+d*np.linalg.norm(L_B2_hist[i-1,:])+e*np.linalg.norm(L_B2_hist[i-2,:]) - - final_filtered_FcN_compare = F_filtered_hist[-1]-check_filtered_FcN[-1] - final_filtered_LB1_compare = T1_filtered_hist[-1]-check_filtered_LB1[-1] - final_filtered_LB2_compare = T2_filtered_hist[-1]-check_filtered_LB2[-1] + for i in range(2, r_B1N_N_hist.shape[0]): + check_filtered_FcN[i] = ( + a * check_filtered_FcN[i - 1] + + b * check_filtered_FcN[i - 2] + + c * np.linalg.norm(Fc_N_hist[i, :]) + + d * np.linalg.norm(Fc_N_hist[i - 1, :]) + + e * np.linalg.norm(Fc_N_hist[i - 2, :]) + ) + check_filtered_LB1[i] = ( + a * check_filtered_LB1[i - 1] + + b * check_filtered_LB1[i - 2] + + c * np.linalg.norm(L_B1_hist[i, :]) + + d * np.linalg.norm(L_B1_hist[i - 1, :]) + + e * np.linalg.norm(L_B1_hist[i - 2, :]) + ) + check_filtered_LB2[i] = ( + a * check_filtered_LB2[i - 1] + + b * check_filtered_LB2[i - 2] + + c * np.linalg.norm(L_B2_hist[i, :]) + + d * np.linalg.norm(L_B2_hist[i - 1, :]) + + e * np.linalg.norm(L_B2_hist[i - 2, :]) + ) + + final_filtered_FcN_compare = F_filtered_hist[-1] - check_filtered_FcN[-1] + final_filtered_LB1_compare = T1_filtered_hist[-1] - check_filtered_LB1[-1] + final_filtered_LB2_compare = T2_filtered_hist[-1] - check_filtered_LB2[-1] # Plotting plt.close("all") @@ -299,97 +359,136 @@ def run_test(show_plots, CutOffFreq, useConstEffector): plt.clf() for i in range(3): plt.semilogy(constraintTimeData, np.abs(psi_N_hist[:, i])) - plt.semilogy(constraintTimeData, np.linalg.norm(psi_N_hist,axis=1)) - plt.legend([r'$\psi_1$',r'$\psi_2$',r'$\psi_3$',r'$\psi$ magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'variation from fixed position: $\psi$ (meters)') - plt.title('Direction Constraint Violation Components') + plt.semilogy(constraintTimeData, np.linalg.norm(psi_N_hist, axis=1)) + plt.legend([r"$\psi_1$", r"$\psi_2$", r"$\psi_3$", r"$\psi$ magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"variation from fixed position: $\psi$ (meters)") + plt.title("Direction Constraint Violation Components") plt.figure() plt.clf() for i in range(3): - plt.semilogy(constraintTimeData, np.abs(4*np.arctan(sigma_B2B1[:, i]) * macros.R2D)) - plt.semilogy(constraintTimeData, np.linalg.norm(4*np.arctan(sigma_B2B1) * macros.R2D,axis=1)) - plt.legend([r'$\phi_1$',r'$\phi_2$',r'$\phi_3$',r'$\phi$ magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'relative attitude angle: $\phi$ (deg)') - plt.title('Attitude Constraint Violation Components') + plt.semilogy( + constraintTimeData, np.abs(4 * np.arctan(sigma_B2B1[:, i]) * macros.R2D) + ) + plt.semilogy( + constraintTimeData, + np.linalg.norm(4 * np.arctan(sigma_B2B1) * macros.R2D, axis=1), + ) + plt.legend([r"$\phi_1$", r"$\phi_2$", r"$\phi_3$", r"$\phi$ magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"relative attitude angle: $\phi$ (deg)") + plt.title("Attitude Constraint Violation Components") plt.figure() plt.clf() for i in range(3): plt.semilogy(constraintTimeData, np.abs(Fc_N_hist[:, i])) - plt.semilogy(constraintTimeData, np.linalg.norm(Fc_N_hist,axis=1)) - plt.legend([r'$FcN_1$',r'$FcN_2$',r'$FcN_3$',r'$FcN$ magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'Constraint force: $FcN$ (N)') - plt.title('Constraint Force') + plt.semilogy(constraintTimeData, np.linalg.norm(Fc_N_hist, axis=1)) + plt.legend([r"$FcN_1$", r"$FcN_2$", r"$FcN_3$", r"$FcN$ magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"Constraint force: $FcN$ (N)") + plt.title("Constraint Force") plt.figure() plt.clf() plt.semilogy(constraintTimeData, F_filtered_hist) - plt.semilogy(constraintTimeData, np.linalg.norm(Fc_N_hist,axis=1)) - plt.legend([r'F_filtered magnitude',r'F_unfiltered magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'Force(N)') - plt.title('Comparison between Filtered and Unifiltered Constraint Force') + plt.semilogy(constraintTimeData, np.linalg.norm(Fc_N_hist, axis=1)) + plt.legend([r"F_filtered magnitude", r"F_unfiltered magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"Force(N)") + plt.title("Comparison between Filtered and Unifiltered Constraint Force") plt.figure() plt.clf() plt.semilogy(constraintTimeData, T2_filtered_hist) - plt.semilogy(constraintTimeData, np.linalg.norm(L_B2_hist,axis=1)) - plt.legend([r'T1_filtered magnitude',r'T1_unfiltered magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'Torque(N.m)') - plt.title('Comparison between Filtered and Unifiltered Constraint Torque on s/c 2') + plt.semilogy(constraintTimeData, np.linalg.norm(L_B2_hist, axis=1)) + plt.legend([r"T1_filtered magnitude", r"T1_unfiltered magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"Torque(N.m)") + plt.title("Comparison between Filtered and Unifiltered Constraint Torque on s/c 2") plt.figure() plt.clf() plt.semilogy(constraintTimeData, T1_filtered_hist) - plt.semilogy(constraintTimeData, np.linalg.norm(L_B1_hist,axis=1)) - plt.legend([r'T2_filtered magnitude',r'T2_unfiltered magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'Torque(N.m)') - plt.title('Comparison between Filtered and Unifiltered Constraint Torque on s/c 1') + plt.semilogy(constraintTimeData, np.linalg.norm(L_B1_hist, axis=1)) + plt.legend([r"T2_filtered magnitude", r"T2_unfiltered magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"Torque(N.m)") + plt.title("Comparison between Filtered and Unifiltered Constraint Torque on s/c 1") plt.figure() plt.clf() for i in range(3): plt.semilogy(constraintTimeData, np.abs(psi_N_hist[:, i])) - plt.semilogy(constraintTimeData, np.linalg.norm(psi_N_hist,axis=1)) - plt.legend([r'$\psi_1$',r'$\psi_2$',r'$\psi_3$',r'$\psi$ magnitude']) - plt.xlabel('time (seconds)') - plt.ylabel(r'variation from fixed position: $\psi$ (meters)') - plt.title('Direction Constraint Violation Components in Inertial frame') + plt.semilogy(constraintTimeData, np.linalg.norm(psi_N_hist, axis=1)) + plt.legend([r"$\psi_1$", r"$\psi_2$", r"$\psi_3$", r"$\psi$ magnitude"]) + plt.xlabel("time (seconds)") + plt.ylabel(r"variation from fixed position: $\psi$ (meters)") + plt.title("Direction Constraint Violation Components in Inertial frame") if show_plots: plt.show() plt.close("all") - accuracy = 1E-08 - np.testing.assert_allclose(final_psi_compare,0,atol = accuracy, err_msg = 'direction constraint output message norm is incorrect') - np.testing.assert_allclose(final_FcN_compare,0,atol = accuracy, err_msg = 'constraint force output message norm is incorrect') - np.testing.assert_allclose(final_L_B1_compare,0,atol = accuracy, err_msg = 'constraint torque on s/c 1 output message norm is incorrect') - np.testing.assert_allclose(final_L_B2_compare,0,atol = accuracy, err_msg = 'constraint torque on s/c 2 output message norm is incorrect') - np.testing.assert_allclose(final_filtered_FcN_compare,0,atol = accuracy, err_msg = 'filtered constraint force output message norm is incorrect') - np.testing.assert_allclose(final_filtered_LB1_compare,0,atol = accuracy, err_msg = 'filtered constraint torque on s/c 1 output message norm is incorrect') - np.testing.assert_allclose(final_filtered_LB2_compare,0,atol = accuracy, err_msg = 'filtered constraint torque on s/c 2 output message norm is incorrect') + accuracy = 1e-08 + np.testing.assert_allclose( + final_psi_compare, + 0, + atol=accuracy, + err_msg="direction constraint output message norm is incorrect", + ) + np.testing.assert_allclose( + final_FcN_compare, + 0, + atol=accuracy, + err_msg="constraint force output message norm is incorrect", + ) + np.testing.assert_allclose( + final_L_B1_compare, + 0, + atol=accuracy, + err_msg="constraint torque on s/c 1 output message norm is incorrect", + ) + np.testing.assert_allclose( + final_L_B2_compare, + 0, + atol=accuracy, + err_msg="constraint torque on s/c 2 output message norm is incorrect", + ) + np.testing.assert_allclose( + final_filtered_FcN_compare, + 0, + atol=accuracy, + err_msg="filtered constraint force output message norm is incorrect", + ) + np.testing.assert_allclose( + final_filtered_LB1_compare, + 0, + atol=accuracy, + err_msg="filtered constraint torque on s/c 1 output message norm is incorrect", + ) + np.testing.assert_allclose( + final_filtered_LB2_compare, + 0, + atol=accuracy, + err_msg="filtered constraint torque on s/c 2 output message norm is incorrect", + ) if useConstEffector == 0: - assert np.linalg.norm(Fc_N_hist[-1,:])==0,"deviceStatus 0 test case failed" + assert np.linalg.norm(Fc_N_hist[-1, :]) == 0, "deviceStatus 0 test case failed" elif useConstEffector == -1: - assert np.linalg.norm(Fc_N_hist[-1,:])>0,"deviceStatus -1 test case failed" + assert np.linalg.norm(Fc_N_hist[-1, :]) > 0, "deviceStatus -1 test case failed" else: - assert np.linalg.norm(Fc_N_hist[-1,:])>0,"deviceStatus 1 test case failed" - - + assert np.linalg.norm(Fc_N_hist[-1, :]) > 0, "deviceStatus 1 test case failed" if wc == 0.1 and useConstEffector == 1: - assert F_filtered_hist[-1]>0,"positive cut off frequency test case failed" + assert F_filtered_hist[-1] > 0, "positive cut off frequency test case failed" elif wc == 0 and useConstEffector == 1: - assert F_filtered_hist[-1]==0,"zero cut off frequency test case failed" + assert F_filtered_hist[-1] == 0, "zero cut off frequency test case failed" elif wc == -1 and useConstEffector == 1: - assert F_filtered_hist[-1]==0,"negative cut off frequency test case failed" + assert F_filtered_hist[-1] == 0, "negative cut off frequency test case failed" + if __name__ == "__main__": - test_constraintEffectorAllCases(True,0.1,-1) + test_constraintEffectorAllCases(True, 0.1, -1) diff --git a/src/simulation/dynamics/dragEffector/_UnitTest/test_atmoDrag.py b/src/simulation/dynamics/dragEffector/_UnitTest/test_atmoDrag.py index 038383512d..68023b1bb0 100644 --- a/src/simulation/dynamics/dragEffector/_UnitTest/test_atmoDrag.py +++ b/src/simulation/dynamics/dragEffector/_UnitTest/test_atmoDrag.py @@ -1,4 +1,3 @@ - # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder # # Permission to use, copy, modify, and/or distribute this software for any @@ -28,11 +27,14 @@ import matplotlib.pyplot as plt import numpy as np + # print dir(exponentialAtmosphere) from Basilisk.simulation import dragDynamicEffector from Basilisk.simulation import exponentialAtmosphere, simpleNav + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros @@ -52,6 +54,7 @@ # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. + # provide a unique test method name, starting with test_ def test_scenarioDragOrbit(): """This function is called by the py.test environment.""" @@ -63,41 +66,41 @@ def test_scenarioDragOrbit(): showVal = False testResults = [] testMessage = [] - [leoResults, leoMessages] = run( - showVal, orb1, earthCase) - #[gtoResults, gtoMessages] = run( + [leoResults, leoMessages] = run(showVal, orb1, earthCase) + # [gtoResults, gtoMessages] = run( # showVal, orb2, earthCase) - #[lmoResults, lmoMessages] = run( - #showVal, orb1, marsCase) - #[mtoResults, mtoMessages] = run( - #showVal, orb2, marsCase) + # [lmoResults, lmoMessages] = run( + # showVal, orb1, marsCase) + # [mtoResults, mtoMessages] = run( + # showVal, orb2, marsCase) - testResults = leoResults#+gtoResults#+lmoResults+mtoResults + testResults = leoResults # +gtoResults#+lmoResults+mtoResults testMessage.append(leoMessages) - #testMessage.append(gtoMessages) + # testMessage.append(gtoMessages) ##testMessage.append(lmoMessages) - #testMessage.append(mtoMessages) + # testMessage.append(mtoMessages) assert testResults < 1, testMessage + def expAtmoComp(alt, baseDens, scaleHeight): - dens = baseDens * math.exp(-alt/scaleHeight) + dens = baseDens * math.exp(-alt / scaleHeight) return dens + def cannonballDragComp(dragCoeff, dens, area, vel, att): dragDir_N = -vel / np.linalg.norm(vel) dcm_BN = RigidBodyKinematics.MRP2C(att) dragDir_B = dcm_BN.dot(dragDir_N) - dragForce = 0.5 * dragCoeff * dens * area * np.linalg.norm(vel)**2.0 * dragDir_B + dragForce = 0.5 * dragCoeff * dens * area * np.linalg.norm(vel) ** 2.0 * dragDir_B return dragForce def run(show_plots, orbitCase, planetCase): """Call this routine directly to run the tutorial scenario.""" - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages # Create simulation variable names simTaskName = "simTask" @@ -113,8 +116,6 @@ def run(show_plots, orbitCase, planetCase): simulationTimeStep = macros.sec2nano(1.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) - - # Initialize new atmosphere and drag model, add them to task newAtmo = exponentialAtmosphere.ExponentialAtmosphere() atmoTaskName = "atmosphere" @@ -129,7 +130,7 @@ def run(show_plots, orbitCase, planetCase): dragEffectorTaskName = "drag" dragEffector.coreParams.projectedArea = projArea dragEffector.coreParams.dragCoeff = dragCoeff - dragEffector.coreParams.comOffset = [1., 0., 0.] + dragEffector.coreParams.comOffset = [1.0, 0.0, 0.0] dynProcess.addTask(scSim.CreateNewTask(atmoTaskName, simulationTimeStep)) dynProcess.addTask(scSim.CreateNewTask(dragEffectorTaskName, simulationTimeStep)) @@ -162,17 +163,19 @@ def run(show_plots, orbitCase, planetCase): planet = gravFactory.createEarth() elif planetCase == "Mars": planet = gravFactory.createMars() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu # attach gravity model to spacecraft - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # # setup orbit and simulation time oe = orbitalMotion.ClassicElements() if planetCase == "Earth": - r_eq = 6371*1000.0 + r_eq = 6371 * 1000.0 refBaseDens = 1.217 refScaleHeight = 8500.0 @@ -183,10 +186,10 @@ def run(show_plots, orbitCase, planetCase): else: return 1, "Test failed- did not initialize planets." if orbitCase == "LPO": - orbAltMin = 300.0*1000.0 + orbAltMin = 300.0 * 1000.0 orbAltMax = orbAltMin elif orbitCase == "LTO": - orbAltMin = 300*1000.0 + orbAltMin = 300 * 1000.0 orbAltMax = 800.0 * 1000.0 newAtmo.planetRadius = r_eq @@ -195,30 +198,34 @@ def run(show_plots, orbitCase, planetCase): rMin = r_eq + orbAltMin rMax = r_eq + orbAltMax - oe.a = (rMin+rMax)/2.0 - oe.e = 1.0 - rMin/oe.a - oe.i = 0.0*macros.D2R + oe.a = (rMin + rMax) / 2.0 + oe.e = 1.0 - rMin / oe.a + oe.i = 0.0 * macros.D2R - oe.Omega = 0.0*macros.D2R - oe.omega = 0.0*macros.D2R - oe.f = 0.0*macros.D2R + oe.Omega = 0.0 * macros.D2R + oe.omega = 0.0 * macros.D2R + oe.f = 0.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements - # with circular or equatorial orbit, some angles are - # arbitrary + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements + # with circular or equatorial orbit, some angles are + # arbitrary # set the simulation time - n = np.sqrt(mu/oe.a/oe.a/oe.a) - P = 2.*np.pi/n + n = np.sqrt(mu / oe.a / oe.a / oe.a) + P = 2.0 * np.pi / n - simulationTime = macros.sec2nano(1*P) + simulationTime = macros.sec2nano(1 * P) # # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) dataNewAtmoLog = newAtmo.envOutMsgs[0].recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) @@ -258,26 +265,31 @@ def run(show_plots, orbitCase, planetCase): endInd = dragForce.shape[0] - refDragForce = np.zeros([endInd,3]) - refDensData = np.zeros([endInd,1]) + refDragForce = np.zeros([endInd, 3]) + refDensData = np.zeros([endInd, 1]) accuracy = 1e-13 # print planetCase # print orbitCase - for ind in range(0, endInd-1): + for ind in range(0, endInd - 1): # print "Position data:", posData[ind,1:] # print "Velocity data:", velData[ind,1:] # print "Density data:", densData[ind,1] - refDragForce[ind] = cannonballDragComp(dragCoeff,densData[ind],projArea,velData[ind], attData[ind]) + refDragForce[ind] = cannonballDragComp( + dragCoeff, densData[ind], projArea, velData[ind], attData[ind] + ) # print "Reference drag data:", refDragForce[ind,:] # print "Drag Data:", dragForce[ind,:] # print "" # check a vector values - for ind in range(1,endInd-1): - if not unitTestSupport.isArrayEqual(dragForce[ind,:], refDragForce[ind],3,accuracy): + for ind in range(1, endInd - 1): + if not unitTestSupport.isArrayEqual( + dragForce[ind, :], refDragForce[ind], 3, accuracy + ): testFailCount += 1 testMessages.append( "FAILED: DragEffector failed force unit test with a value difference of " - + str(np.linalg.norm(dragForce[ind,:]-refDragForce[ind]))) + + str(np.linalg.norm(dragForce[ind, :] - refDragForce[ind])) + ) # # plot the results @@ -289,74 +301,82 @@ def run(show_plots, orbitCase, planetCase): plt.figure(1) fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') - for idx in range(0,3): - plt.plot(dataLog.times()*macros.NANO2SEC/P, posData[:, idx]/1000., - color=unitTestSupport.getLineColor(idx,3), - label='$r_{BN,'+str(idx)+'}$') - plt.legend(loc='lower right') - plt.xlabel('Time [orbits]') - plt.ylabel('Inertial Position [km]') + ax.ticklabel_format(useOffset=False, style="plain") + for idx in range(0, 3): + plt.plot( + dataLog.times() * macros.NANO2SEC / P, + posData[:, idx] / 1000.0, + color=unitTestSupport.getLineColor(idx, 3), + label="$r_{BN," + str(idx) + "}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Inertial Position [km]") # draw orbit in perifocal frame - b = oe.a*np.sqrt(1-oe.e*oe.e) - p = oe.a*(1-oe.e*oe.e) - plt.figure(2,figsize=tuple(np.array((1.0, b/oe.a))*4.75),dpi=100) - plt.axis(np.array([-oe.rApoap, oe.rPeriap, -b, b])/1000*1.25) + b = oe.a * np.sqrt(1 - oe.e * oe.e) + p = oe.a * (1 - oe.e * oe.e) + plt.figure(2, figsize=tuple(np.array((1.0, b / oe.a)) * 4.75), dpi=100) + plt.axis(np.array([-oe.rApoap, oe.rPeriap, -b, b]) / 1000 * 1.25) # draw the planet fig = plt.gcf() ax = fig.gca() - planetColor= '#008800' - planetRadius = planet.radEquator/1000 + 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(posData)): - oeData = orbitalMotion.rv2elem(mu,posData[idx,0:3],velData[idx,0:3]) + rData = [] + fData = [] + for idx in range(0, len(posData)): + oeData = orbitalMotion.rv2elem(mu, posData[idx, 0:3], velData[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 - ) + 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) + 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('$i_e$ Cord. [km]') - plt.ylabel('$i_p$ Cord. [km]') + 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("$i_e$ Cord. [km]") + plt.ylabel("$i_p$ Cord. [km]") plt.grid() plt.figure() fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='plain') + ax.ticklabel_format(useOffset=False, style="plain") smaData = [] for idx in range(0, len(posData)): oeData = orbitalMotion.rv2elem(mu, posData[idx, 0:3], velData[idx, 0:3]) - smaData.append(oeData.a/1000.) - plt.plot(posData[:, 0]*macros.NANO2SEC/P, smaData - ,color='#aa0000', - ) - plt.xlabel('Time [orbits]') - plt.ylabel('SMA [km]') - + smaData.append(oeData.a / 1000.0) + plt.plot( + posData[:, 0] * macros.NANO2SEC / P, + smaData, + color="#aa0000", + ) + plt.xlabel("Time [orbits]") + plt.ylabel("SMA [km]") plt.figure() fig = plt.gcf() ax = fig.gca() - ax.ticklabel_format(useOffset=False, style='sci') - plt.plot( dataNewAtmoLog.times()*macros.NANO2SEC, densData) - plt.title('Density Data vs. Time') - plt.xlabel('Time') - plt.ylabel('Density in kg/m^3') + ax.ticklabel_format(useOffset=False, style="sci") + plt.plot(dataNewAtmoLog.times() * macros.NANO2SEC, densData) + plt.title("Density Data vs. Time") + plt.xlabel("Time") + plt.ylabel("Density in kg/m^3") plt.show() plt.close("all") @@ -369,5 +389,7 @@ def run(show_plots, orbitCase, planetCase): return testFailCount, testMessages # close the plots being saved off to avoid over-writing old and new figures -if __name__ == '__main__': - run(True,"LPO","Earth") + + +if __name__ == "__main__": + run(True, "LPO", "Earth") diff --git a/src/simulation/dynamics/dualHingedRigidBodies/_UnitTest/test_dualhingedRigidBodyStateEffector.py b/src/simulation/dynamics/dualHingedRigidBodies/_UnitTest/test_dualhingedRigidBodyStateEffector.py index ddf5b8aeae..0d07d8602b 100644 --- a/src/simulation/dynamics/dualHingedRigidBodies/_UnitTest/test_dualhingedRigidBodyStateEffector.py +++ b/src/simulation/dynamics/dualHingedRigidBodies/_UnitTest/test_dualhingedRigidBodyStateEffector.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -25,7 +24,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') +splitPath = path.split("simulation") from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import unitTestSupport @@ -38,10 +37,10 @@ from Basilisk.architecture import messaging from Basilisk.utilities import deprecated -@pytest.mark.parametrize("useFlag, testCase", [ - (False, 'NoGravity'), - (False, 'Gravity') -]) + +@pytest.mark.parametrize( + "useFlag, testCase", [(False, "NoGravity"), (False, "Gravity")] +) # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed # @pytest.mark.skipif(conditionstring) @@ -53,6 +52,7 @@ def test_dualHingedRigidBody(show_plots, useFlag, testCase): [testResults, testMessage] = dualHingedRigidBodyTest(show_plots, useFlag, testCase) assert testResults < 1, testMessage + def dualHingedRigidBodyTest(show_plots, useFlag, testCase): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the @@ -76,13 +76,21 @@ def dualHingedRigidBodyTest(show_plots, useFlag, testCase): testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - unitTestSim.panel1 = dualHingedRigidBodyStateEffector.DualHingedRigidBodyStateEffector() - unitTestSim.panel2 = dualHingedRigidBodyStateEffector.DualHingedRigidBodyStateEffector() + unitTestSim.panel1 = ( + dualHingedRigidBodyStateEffector.DualHingedRigidBodyStateEffector() + ) + unitTestSim.panel2 = ( + dualHingedRigidBodyStateEffector.DualHingedRigidBodyStateEffector() + ) # Define Variable for panel 1 unitTestSim.panel1.ModelTag = "panel1" unitTestSim.panel1.mass1 = 50.0 - unitTestSim.panel1.IPntS1_S1 = [[50.0, 0.0, 0.0], [0.0, 25.0, 0.0], [0.0, 0.0, 25.0]] + unitTestSim.panel1.IPntS1_S1 = [ + [50.0, 0.0, 0.0], + [0.0, 25.0, 0.0], + [0.0, 0.0, 25.0], + ] unitTestSim.panel1.d1 = 0.75 unitTestSim.panel1.l1 = 1.5 unitTestSim.panel1.k1 = 100.0 @@ -90,11 +98,15 @@ def dualHingedRigidBodyTest(show_plots, useFlag, testCase): unitTestSim.panel1.r_H1B_B = [[0.5], [0.0], [1.0]] unitTestSim.panel1.dcm_H1B = [[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 1.0]] unitTestSim.panel1.mass2 = 50.0 - unitTestSim.panel1.IPntS2_S2 = [[50.0, 0.0, 0.0], [0.0, 25.0, 0.0], [0.0, 0.0, 25.0]] + unitTestSim.panel1.IPntS2_S2 = [ + [50.0, 0.0, 0.0], + [0.0, 25.0, 0.0], + [0.0, 0.0, 25.0], + ] unitTestSim.panel1.d2 = 0.75 unitTestSim.panel1.k2 = 100.0 unitTestSim.panel1.c2 = 0.0 - unitTestSim.panel1.theta1Init = 5*numpy.pi/180.0 + unitTestSim.panel1.theta1Init = 5 * numpy.pi / 180.0 unitTestSim.panel1.theta1DotInit = 0.0 unitTestSim.panel1.theta2Init = 0.0 unitTestSim.panel1.theta2DotInit = 0.0 @@ -102,7 +114,11 @@ def dualHingedRigidBodyTest(show_plots, useFlag, testCase): # Define Variables for panel 2 unitTestSim.panel2.ModelTag = "panel2" unitTestSim.panel2.mass1 = 50.0 - unitTestSim.panel2.IPntS1_S1 = [[50.0, 0.0, 0.0], [0.0, 25.0, 0.0], [0.0, 0.0, 25.0]] + unitTestSim.panel2.IPntS1_S1 = [ + [50.0, 0.0, 0.0], + [0.0, 25.0, 0.0], + [0.0, 0.0, 25.0], + ] unitTestSim.panel2.d1 = 0.75 unitTestSim.panel2.l1 = 1.5 unitTestSim.panel2.k1 = 100.0 @@ -110,11 +126,15 @@ def dualHingedRigidBodyTest(show_plots, useFlag, testCase): unitTestSim.panel2.r_H1B_B = [[-0.5], [0.0], [1.0]] unitTestSim.panel2.dcm_H1B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] unitTestSim.panel2.mass2 = 50.0 - unitTestSim.panel2.IPntS2_S2 = [[50.0, 0.0, 0.0], [0.0, 25.0, 0.0], [0.0, 0.0, 25.0]] + unitTestSim.panel2.IPntS2_S2 = [ + [50.0, 0.0, 0.0], + [0.0, 25.0, 0.0], + [0.0, 0.0, 25.0], + ] unitTestSim.panel2.d2 = 0.75 unitTestSim.panel2.k2 = 100.0 unitTestSim.panel2.c2 = 0.0 - unitTestSim.panel2.theta1Init = 5*numpy.pi/180.0 + unitTestSim.panel2.theta1Init = 5 * numpy.pi / 180.0 unitTestSim.panel2.theta1DotInit = 0.0 unitTestSim.panel2.theta2Init = 0.0 unitTestSim.panel2.theta2DotInit = 0.0 @@ -137,20 +157,32 @@ def dualHingedRigidBodyTest(show_plots, useFlag, testCase): # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, scObject) - if testCase == 'Gravity': + if testCase == "Gravity": unitTestSim.earthGravBody = gravityEffector.GravBodyData() unitTestSim.earthGravBody.planetName = "earth_planet_data" - unitTestSim.earthGravBody.mu = 0.3986004415E+15 # meters! + unitTestSim.earthGravBody.mu = 0.3986004415e15 # meters! unitTestSim.earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([unitTestSim.earthGravBody]) - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + [unitTestSim.earthGravBody] + ) + scObject.hub.r_CN_NInit = [ + [-4020338.690396649], + [7490566.741852513], + [5248299.211589362], + ] + scObject.hub.v_CN_NInit = [ + [-5199.77710904224], + [-3436.681645356935], + [1041.576797498721], + ] dataLog = scObject.scStateOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) # Add energy and momentum s to log - scObjectLog = scObject.logger(["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) unitTestSim.InitializeSimulation() @@ -159,96 +191,146 @@ def dualHingedRigidBodyTest(show_plots, useFlag, testCase): unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) unitTestSim.ExecuteSimulation() - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) - initialOrbAngMom_N = [ - [orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]] - ] + initialOrbAngMom_N = [[orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]]] - finalOrbAngMom = [ - [orbAngMom_N[-1, 1], orbAngMom_N[-1, 2], orbAngMom_N[-1, 3]] - ] + finalOrbAngMom = [[orbAngMom_N[-1, 1], orbAngMom_N[-1, 2], orbAngMom_N[-1, 3]]] - initialRotAngMom_N = [ - [rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]] - ] + initialRotAngMom_N = [[rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]]] - finalRotAngMom = [ - [rotAngMom_N[-1, 1], rotAngMom_N[-1, 2], rotAngMom_N[-1, 3]] - ] + finalRotAngMom = [[rotAngMom_N[-1, 1], rotAngMom_N[-1, 2], rotAngMom_N[-1, 3]]] - initialOrbEnergy = [ - [orbEnergy[0, 1]] - ] + initialOrbEnergy = [[orbEnergy[0, 1]]] - finalOrbEnergy = [ - [orbEnergy[-1, 1]] - ] + finalOrbEnergy = [[orbEnergy[-1, 1]]] - initialRotEnergy = [ - [rotEnergy[int(len(rotEnergy)/2)+1, 1]] - ] + initialRotEnergy = [[rotEnergy[int(len(rotEnergy) / 2) + 1, 1]]] - finalRotEnergy = [ - [rotEnergy[-1, 1]] - ] + finalRotEnergy = [[rotEnergy[-1, 1]]] - plt.close('all') + plt.close("all") plt.figure() plt.clf() - plt.plot(orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,1] - orbAngMom_N[0,1])/orbAngMom_N[0,1], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,2] - orbAngMom_N[0,2])/orbAngMom_N[0,2], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,3] - orbAngMom_N[0,3])/orbAngMom_N[0,3]) + plt.plot( + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInOrbitalAngularMomentum" + testCase, "Change in Orbital Angular Momentum " + testCase, plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInOrbitalAngularMomentum" + testCase, + "Change in Orbital Angular Momentum " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(orbEnergy[:,0]*1e-9, (orbEnergy[:,1] - orbEnergy[0,1])/orbEnergy[0,1]) + plt.plot( + orbEnergy[:, 0] * 1e-9, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1] + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInOrbitalEnergy" + testCase, "Change in Orbital Energy " + testCase, plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInOrbitalEnergy" + testCase, + "Change in Orbital Energy " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,1] - rotAngMom_N[0,1])/rotAngMom_N[0,1], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,2] - rotAngMom_N[0,2])/rotAngMom_N[0,2], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,3] - rotAngMom_N[0,3])/rotAngMom_N[0,3]) + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) / rotAngMom_N[0, 1], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) / rotAngMom_N[0, 2], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) / rotAngMom_N[0, 3], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInRotationalAngularMomentum" + testCase, "Change in Rotational Angular Momentum " + testCase, plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInRotationalAngularMomentum" + testCase, + "Change in Rotational Angular Momentum " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(rotEnergy[:,0]*1e-9, (rotEnergy[:,1] - rotEnergy[0,1])/rotEnergy[0,1]) + plt.plot( + rotEnergy[:, 0] * 1e-9, (rotEnergy[:, 1] - rotEnergy[0, 1]) / rotEnergy[0, 1] + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInRotationalEnergy" + testCase, "Change in Rotational Energy " + testCase, plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInRotationalEnergy" + testCase, + "Change in Rotational Energy " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) if show_plots: plt.show() plt.close("all") accuracy = 1e-10 - for i in range(0,len(initialOrbAngMom_N)): + for i in range(0, len(initialOrbAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i],initialOrbAngMom_N[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body Integrated Test failed orbital angular momentum unit test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body Integrated Test failed orbital angular momentum unit test" + ) - for i in range(0,len(initialRotAngMom_N)): + for i in range(0, len(initialRotAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotAngMom[i],initialRotAngMom_N[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body Integrated Test failed rotational angular momentum unit test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body Integrated Test failed rotational angular momentum unit test" + ) - for i in range(0,len(initialOrbEnergy)): + for i in range(0, len(initialOrbEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i],initialOrbEnergy[i],1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body Integrated Test failed orbital energy unit test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body Integrated Test failed orbital energy unit test" + ) - for i in range(0,len(initialRotEnergy)): + for i in range(0, len(initialRotEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotEnergy[i],initialRotEnergy[i],1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotEnergy[i], initialRotEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body Integrated Test failed rotational energy unit test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body Integrated Test failed rotational energy unit test" + ) if testFailCount == 0: print("PASSED: " + " Dual Hinged Rigid Body Test") @@ -257,7 +339,7 @@ def dualHingedRigidBodyTest(show_plots, useFlag, testCase): print(testMessages) # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] @pytest.mark.parametrize("useScPlus", [True]) @@ -296,13 +378,21 @@ def dualHingedRigidBodyMotorTorque(show_plots, useScPlus): testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) - unitTestSim.panel1 = dualHingedRigidBodyStateEffector.DualHingedRigidBodyStateEffector() - unitTestSim.panel2 = dualHingedRigidBodyStateEffector.DualHingedRigidBodyStateEffector() + unitTestSim.panel1 = ( + dualHingedRigidBodyStateEffector.DualHingedRigidBodyStateEffector() + ) + unitTestSim.panel2 = ( + dualHingedRigidBodyStateEffector.DualHingedRigidBodyStateEffector() + ) # Define Variable for panel 1 unitTestSim.panel1.ModelTag = "panel1" unitTestSim.panel1.mass1 = 50.0 - unitTestSim.panel1.IPntS1_S1 = [[50.0, 0.0, 0.0], [0.0, 25.0, 0.0], [0.0, 0.0, 25.0]] + unitTestSim.panel1.IPntS1_S1 = [ + [50.0, 0.0, 0.0], + [0.0, 25.0, 0.0], + [0.0, 0.0, 25.0], + ] unitTestSim.panel1.d1 = 0.75 unitTestSim.panel1.l1 = 1.5 unitTestSim.panel1.k1 = 0.0 @@ -310,11 +400,15 @@ def dualHingedRigidBodyMotorTorque(show_plots, useScPlus): unitTestSim.panel1.r_H1B_B = [[0.5], [0.0], [1.0]] unitTestSim.panel1.dcm_H1B = [[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 1.0]] unitTestSim.panel1.mass2 = 50.0 - unitTestSim.panel1.IPntS2_S2 = [[50.0, 0.0, 0.0], [0.0, 25.0, 0.0], [0.0, 0.0, 25.0]] + unitTestSim.panel1.IPntS2_S2 = [ + [50.0, 0.0, 0.0], + [0.0, 25.0, 0.0], + [0.0, 0.0, 25.0], + ] unitTestSim.panel1.d2 = 0.75 unitTestSim.panel1.k2 = 100.0 unitTestSim.panel1.c2 = 0.0 - unitTestSim.panel1.theta1Init = 0*numpy.pi/180.0 + unitTestSim.panel1.theta1Init = 0 * numpy.pi / 180.0 unitTestSim.panel1.theta1DotInit = 0.0 unitTestSim.panel1.theta2Init = 0.0 unitTestSim.panel1.theta2DotInit = 0.0 @@ -328,7 +422,11 @@ def dualHingedRigidBodyMotorTorque(show_plots, useScPlus): # Define Variables for panel 2 unitTestSim.panel2.ModelTag = "panel2" unitTestSim.panel2.mass1 = 50.0 - unitTestSim.panel2.IPntS1_S1 = [[50.0, 0.0, 0.0], [0.0, 25.0, 0.0], [0.0, 0.0, 25.0]] + unitTestSim.panel2.IPntS1_S1 = [ + [50.0, 0.0, 0.0], + [0.0, 25.0, 0.0], + [0.0, 0.0, 25.0], + ] unitTestSim.panel2.d1 = 0.75 unitTestSim.panel2.l1 = 1.5 unitTestSim.panel2.k1 = 0.0 @@ -338,7 +436,11 @@ def dualHingedRigidBodyMotorTorque(show_plots, useScPlus): unitTestSim.panel2.nameOfTheta1State = "dualHingedRigidBody2Theta1" unitTestSim.panel2.nameOfTheta1DotState = "dualHingedRigidBody2ThetaDot1" unitTestSim.panel2.mass2 = 50.0 - unitTestSim.panel2.IPntS2_S2 = [[50.0, 0.0, 0.0], [0.0, 25.0, 0.0], [0.0, 0.0, 25.0]] + unitTestSim.panel2.IPntS2_S2 = [ + [50.0, 0.0, 0.0], + [0.0, 25.0, 0.0], + [0.0, 0.0, 25.0], + ] unitTestSim.panel2.d2 = 0.75 unitTestSim.panel2.k2 = 0.0 unitTestSim.panel2.c2 = 0.0 @@ -360,7 +462,11 @@ def dualHingedRigidBodyMotorTorque(show_plots, useScPlus): # Define mass properties of the rigid part of the spacecraft scObjectPrimary.hub.mHub = 750.0 scObjectPrimary.hub.r_BcB_B = [[0.0], [0.0], [1.0]] - scObjectPrimary.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] + scObjectPrimary.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 scObjectPrimary.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] @@ -391,9 +497,11 @@ def dualHingedRigidBodyMotorTorque(show_plots, useScPlus): if useScPlus: scLog = scObject.logger("totRotAngMomPntC_N") else: - scLog = pythonVariableLogger.PythonVariableLogger({ - "totRotAngMomPntC_N": lambda _: scObject.primaryCentralSpacecraft.totRotAngMomPntC_N - }) + scLog = pythonVariableLogger.PythonVariableLogger( + { + "totRotAngMomPntC_N": lambda _: scObject.primaryCentralSpacecraft.totRotAngMomPntC_N + } + ) unitTestSim.AddModelToTask(unitTaskName, scLog) unitTestSim.InitializeSimulation() @@ -423,7 +531,7 @@ def dualHingedRigidBodyMotorTorque(show_plots, useScPlus): # Get the last sigma and position dataPos = [rOut_CN_N[-1]] - truePos = [[0., 0., 0.]] + truePos = [[0.0, 0.0, 0.0]] initialRotAngMom_N = [[rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]]] @@ -433,51 +541,83 @@ def dualHingedRigidBodyMotorTorque(show_plots, useScPlus): plt.figure() plt.clf() - plt.plot(rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) , - rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) , - rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) ) - plt.xlabel('time (s)') - plt.ylabel('Ang. Momentum Difference') + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]), + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]), + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]), + ) + plt.xlabel("time (s)") + plt.ylabel("Ang. Momentum Difference") plt.figure() plt.clf() - plt.plot(dataLog.times() * 1e-9, vOut_CN_N[:, 0], dataLog.times() * 1e-9, vOut_CN_N[:, 1], dataLog.times() * 1e-9, - vOut_CN_N[:, 2]) - plt.xlabel('time (s)') - plt.ylabel('m/s') + plt.plot( + dataLog.times() * 1e-9, + vOut_CN_N[:, 0], + dataLog.times() * 1e-9, + vOut_CN_N[:, 1], + dataLog.times() * 1e-9, + vOut_CN_N[:, 2], + ) + plt.xlabel("time (s)") + plt.ylabel("m/s") plt.figure() plt.clf() - plt.plot(dataLog.times() * macros.NANO2SEC, sigma_BN[:, 0], - color=unitTestSupport.getLineColor(0, 3), - label=r'$\sigma_{1}$') - plt.plot(dataLog.times() * macros.NANO2SEC, sigma_BN[:, 1], - color=unitTestSupport.getLineColor(1, 3), - label=r'$\sigma_{2}$') - plt.plot(dataLog.times() * macros.NANO2SEC, sigma_BN[:, 2], - color=unitTestSupport.getLineColor(2, 3), - label=r'$\sigma_{3}$') - plt.legend(loc='lower right') - plt.xlabel('time (s)') - plt.ylabel(r'MRP $\sigma_{B/N}$') + plt.plot( + dataLog.times() * macros.NANO2SEC, + sigma_BN[:, 0], + color=unitTestSupport.getLineColor(0, 3), + label=r"$\sigma_{1}$", + ) + plt.plot( + dataLog.times() * macros.NANO2SEC, + sigma_BN[:, 1], + color=unitTestSupport.getLineColor(1, 3), + label=r"$\sigma_{2}$", + ) + plt.plot( + dataLog.times() * macros.NANO2SEC, + sigma_BN[:, 2], + color=unitTestSupport.getLineColor(2, 3), + label=r"$\sigma_{3}$", + ) + plt.legend(loc="lower right") + plt.xlabel("time (s)") + plt.ylabel(r"MRP $\sigma_{B/N}$") plt.figure() plt.clf() - plt.plot(dataPanel10Log.times() * macros.NANO2SEC, thetaP1A1*macros.R2D, - color=unitTestSupport.getLineColor(0, 4), - label=r'Panel 1 $\theta_{1}$') - plt.plot(dataPanel10Log.times() * macros.NANO2SEC, thetaP1A2*macros.R2D, - color=unitTestSupport.getLineColor(1, 4), - label=r'Panel 1 $\theta_{2}$') - plt.plot(dataPanel10Log.times() * macros.NANO2SEC, thetaP2A1 * macros.R2D, - color=unitTestSupport.getLineColor(2, 4), - label=r'Panel 2 $\theta_{1}$') - plt.plot(dataPanel10Log.times() * macros.NANO2SEC, thetaP2A2 * macros.R2D, - color=unitTestSupport.getLineColor(3, 4), - label=r'Panel 2 $\theta_{2}$') - plt.legend(loc='lower right') - plt.xlabel('time (s)') - plt.ylabel('Hinge Angles [deg]') + plt.plot( + dataPanel10Log.times() * macros.NANO2SEC, + thetaP1A1 * macros.R2D, + color=unitTestSupport.getLineColor(0, 4), + label=r"Panel 1 $\theta_{1}$", + ) + plt.plot( + dataPanel10Log.times() * macros.NANO2SEC, + thetaP1A2 * macros.R2D, + color=unitTestSupport.getLineColor(1, 4), + label=r"Panel 1 $\theta_{2}$", + ) + plt.plot( + dataPanel10Log.times() * macros.NANO2SEC, + thetaP2A1 * macros.R2D, + color=unitTestSupport.getLineColor(2, 4), + label=r"Panel 2 $\theta_{1}$", + ) + plt.plot( + dataPanel10Log.times() * macros.NANO2SEC, + thetaP2A2 * macros.R2D, + color=unitTestSupport.getLineColor(3, 4), + label=r"Panel 2 $\theta_{2}$", + ) + plt.legend(loc="lower right") + plt.xlabel("time (s)") + plt.ylabel("Hinge Angles [deg]") if show_plots: plt.show() @@ -488,51 +628,70 @@ def dualHingedRigidBodyMotorTorque(show_plots, useScPlus): # check a vector values if not unitTestSupport.isArrayEqual(dataPos[i], truePos[i], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Hinged Rigid Body integrated test failed position test") + testMessages.append( + "FAILED: Hinged Rigid Body integrated test failed position test" + ) for i in range(0, len(initialRotAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqual(finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: Hinged Rigid Body integrated test failed rotational angular momentum unit test") + "FAILED: Hinged Rigid Body integrated test failed rotational angular momentum unit test" + ) # check config log messages if not unitTestSupport.isArrayEqual(rB1N, [1.25, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body integrated test failed panel 1 r_S1N_N config log test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body integrated test failed panel 1 r_S1N_N config log test" + ) if not unitTestSupport.isArrayEqual(vB1N, [0.0, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body integrated test failed panel 1 v_S1N_N config log test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body integrated test failed panel 1 v_S1N_N config log test" + ) if not unitTestSupport.isArrayEqual(sB1N, [0.0, 0, 1.0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body integrated test failed panel 1 sigma_S1N config log test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body integrated test failed panel 1 sigma_S1N config log test" + ) if not unitTestSupport.isArrayEqual(oB1N, [0.0, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body integrated test failed panel 1 omega_S1N_B config log test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body integrated test failed panel 1 omega_S1N_B config log test" + ) if not unitTestSupport.isArrayEqual(rB2N, [-2.75, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body integrated test failed panel 2 r_S2N_N config log test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body integrated test failed panel 2 r_S2N_N config log test" + ) if not unitTestSupport.isArrayEqual(vB2N, [0.0, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body integrated test failed panel 2 v_S2N_N config log test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body integrated test failed panel 2 v_S2N_N config log test" + ) if not unitTestSupport.isArrayEqual(sB2N, [0.0, 0, 0.0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body integrated test failed panel 2 sigma_S2N config log test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body integrated test failed panel 2 sigma_S2N config log test" + ) if not unitTestSupport.isArrayEqual(oB2N, [0.0, 0, 0], 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: Dual Hinged Rigid Body integrated test failed panel 2 omega_S2N_B config log test") + testMessages.append( + "FAILED: Dual Hinged Rigid Body integrated test failed panel 2 omega_S2N_B config log test" + ) if testFailCount == 0: print("PASSED: " + " Dual Hinged Rigid Body integrated test with motor torques") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] - - + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": - dualHingedRigidBodyTest(True, False, 'NoGravity') + dualHingedRigidBodyTest(True, False, "NoGravity") # dualHingedRigidBodyMotorTorque(True, True) diff --git a/src/simulation/dynamics/extForceTorque/_UnitTest/test_extForceTorque.py b/src/simulation/dynamics/extForceTorque/_UnitTest/test_extForceTorque.py index 40a68f5df1..058e16bc2d 100755 --- a/src/simulation/dynamics/extForceTorque/_UnitTest/test_extForceTorque.py +++ b/src/simulation/dynamics/extForceTorque/_UnitTest/test_extForceTorque.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -30,7 +29,9 @@ from Basilisk.simulation import extForceTorque from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed # @pytest.mark.skipif(conditionstring) @@ -47,23 +48,24 @@ for i in range(0, 4): for j in range(0, 4): for k in range(0, 4): - caseList.append([i,j,k]) -@pytest.mark.parametrize("torqueInput, forceNInput, forceBInput", caseList) + caseList.append([i, j, k]) + +@pytest.mark.parametrize("torqueInput, forceNInput, forceBInput", caseList) # provide a unique test method name, starting with test_ def test_unitDynamicsModes(show_plots, torqueInput, forceNInput, forceBInput): """Module Unit Test""" # each test method requires a single assert method to be called [testResults, testMessage] = unitDynamicsModesTestFunction( - show_plots, torqueInput, forceNInput, forceBInput) + show_plots, torqueInput, forceNInput, forceBInput + ) assert testResults < 1, testMessage - def unitDynamicsModesTestFunction(show_plots, torqueInput, forceNInput, forceBInput): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages unitTaskName = "unitTask" unitProcessName = "testProcess" @@ -80,25 +82,25 @@ def unitDynamicsModesTestFunction(show_plots, torqueInput, forceNInput, forceBIn extFTObject = extForceTorque.ExtForceTorque() extFTObject.ModelTag = "externalDisturbance" - if torqueInput==1 or torqueInput==3: - extFTObject.extTorquePntB_B = [[-1], [1],[ -1]] - if torqueInput==2 or torqueInput==3: + if torqueInput == 1 or torqueInput == 3: + extFTObject.extTorquePntB_B = [[-1], [1], [-1]] + if torqueInput == 2 or torqueInput == 3: msgData = messaging.CmdTorqueBodyMsgPayload() msgData.torqueRequestBody = [-1.0, 1.0, -1.0] cmdTorqueMsg = messaging.CmdTorqueBodyMsg().write(msgData) extFTObject.cmdTorqueInMsg.subscribeTo(cmdTorqueMsg) - if forceNInput==1 or forceNInput==3: - extFTObject.extForce_N = [[-10.], [-5.], [5.]] - if forceNInput==2 or forceNInput==3: + if forceNInput == 1 or forceNInput == 3: + extFTObject.extForce_N = [[-10.0], [-5.0], [5.0]] + if forceNInput == 2 or forceNInput == 3: msgData = messaging.CmdForceInertialMsgPayload() msgData.forceRequestInertial = [-10.0, -5.0, 5.0] cmdForceInertialMsg = messaging.CmdForceInertialMsg().write(msgData) extFTObject.cmdForceInertialInMsg.subscribeTo(cmdForceInertialMsg) - if forceBInput==1 or forceBInput==3: - extFTObject.extForce_B = [[10.], [20.], [30.]] - if forceBInput==2 or forceBInput==3: + if forceBInput == 1 or forceBInput == 3: + extFTObject.extForce_B = [[10.0], [20.0], [30.0]] + if forceBInput == 2 or forceBInput == 3: msgData = messaging.CmdForceBodyMsgPayload() msgData.forceRequestBody = [10.0, 20.0, 30.0] cmdForceBodyMsg = messaging.CmdForceBodyMsg().write(msgData) @@ -109,7 +111,9 @@ def unitDynamicsModesTestFunction(show_plots, torqueInput, forceNInput, forceBIn # # Setup data logging # - extFTObjectLog = extFTObject.logger(["forceExternal_N", "forceExternal_B", "torqueExternalPntB_B"]) + extFTObjectLog = extFTObject.logger( + ["forceExternal_N", "forceExternal_B", "torqueExternalPntB_B"] + ) scSim.AddModelToTask(unitTaskName, extFTObjectLog) # @@ -138,76 +142,81 @@ def unitDynamicsModesTestFunction(show_plots, torqueInput, forceNInput, forceBIn # if torqueInput == 3: - trueTorque_B = [ - [-2., 2., -2.] - ] - elif torqueInput>0: - trueTorque_B = [ - [-1., 1., -1.] - ] + trueTorque_B = [[-2.0, 2.0, -2.0]] + elif torqueInput > 0: + trueTorque_B = [[-1.0, 1.0, -1.0]] else: - trueTorque_B = [ - [0, 0, 0] - ] + trueTorque_B = [[0, 0, 0]] if forceBInput == 3: - trueForceB = [ - [20, 40, 60] - ] - elif forceBInput>0: - trueForceB = [ - [10, 20, 30] - ] + trueForceB = [[20, 40, 60]] + elif forceBInput > 0: + trueForceB = [[10, 20, 30]] else: - trueForceB = [ - [0, 0, 0] - ] + trueForceB = [[0, 0, 0]] if forceNInput == 3: - trueForceN = [ - [-20., -10., 10.] - ] + trueForceN = [[-20.0, -10.0, 10.0]] elif forceNInput > 0: - trueForceN = [ - [-10., -5., 5.] - ] + trueForceN = [[-10.0, -5.0, 5.0]] else: - trueForceN = [ - [0, 0, 0] - ] + trueForceN = [[0, 0, 0]] # compare the module results to the truth values accuracy = 1e-12 - if (len(trueTorque_B) != len(dataTorque)): + if len(trueTorque_B) != len(dataTorque): testFailCount += 1 - testMessages.append("FAILED: ExtForceTorque failed torque unit test (unequal array sizes)\n") + testMessages.append( + "FAILED: ExtForceTorque failed torque unit test (unequal array sizes)\n" + ) else: - for i in range(0,len(trueTorque_B)): + for i in range(0, len(trueTorque_B)): # check a vector values - if not unitTestSupport.isArrayEqual(dataTorque[i],trueTorque_B[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + dataTorque[i], trueTorque_B[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: ExtForceTorque failed torque unit test at t=" + str(dataTorque[i,0]*macros.NANO2SEC) + "sec\n") + testMessages.append( + "FAILED: ExtForceTorque failed torque unit test at t=" + + str(dataTorque[i, 0] * macros.NANO2SEC) + + "sec\n" + ) - if (len(trueForceN) != len(dataForceN)): + if len(trueForceN) != len(dataForceN): testFailCount += 1 - testMessages.append("FAILED: ExtForceTorque failed force_N unit test (unequal array sizes)\n") + testMessages.append( + "FAILED: ExtForceTorque failed force_N unit test (unequal array sizes)\n" + ) else: - for i in range(0,len(trueForceN)): + for i in range(0, len(trueForceN)): # check a vector values - if not unitTestSupport.isArrayEqual(dataForceN[i],trueForceN[i],3,accuracy): + if not unitTestSupport.isArrayEqual( + dataForceN[i], trueForceN[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: ExtForceTorque failed force_N unit test at t=" + str(dataForceN[i,0]*macros.NANO2SEC) + "sec\n") + testMessages.append( + "FAILED: ExtForceTorque failed force_N unit test at t=" + + str(dataForceN[i, 0] * macros.NANO2SEC) + + "sec\n" + ) - if (len(trueForceB) != len(dataForceB)): + if len(trueForceB) != len(dataForceB): testFailCount += 1 - testMessages.append("FAILED: ExtForceTorque failed force_B unit test (unequal array sizes)\n") + testMessages.append( + "FAILED: ExtForceTorque failed force_B unit test (unequal array sizes)\n" + ) else: for i in range(0, len(trueForceB)): # check a vector values - if not unitTestSupport.isArrayEqual(dataForceB[i], trueForceB[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + dataForceB[i], trueForceB[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: ExtForceTorque failed force_B unit test at t="+str( - dataForceB[i, 0]*macros.NANO2SEC)+"sec\n") + testMessages.append( + "FAILED: ExtForceTorque failed force_B unit test at t=" + + str(dataForceB[i, 0] * macros.NANO2SEC) + + "sec\n" + ) # print out success message if no error were found if testFailCount == 0: @@ -215,15 +224,17 @@ def unitDynamicsModesTestFunction(show_plots, torqueInput, forceNInput, forceBIn # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + # # This statement below ensures that the unit test scrip can be run as a # stand-along python script # if __name__ == "__main__": - test_unitDynamicsModes(False, # show_plots - 1, # torqueInput - 0, # forceNInput - 0 # forceBInput - ) + test_unitDynamicsModes( + False, # show_plots + 1, # torqueInput + 0, # forceNInput + 0, # forceBInput + ) diff --git a/src/simulation/dynamics/extForceTorque/_UnitTest/test_extForceTorqueIntegrated.py b/src/simulation/dynamics/extForceTorque/_UnitTest/test_extForceTorqueIntegrated.py index 1ad2a25e75..4a6c62d10d 100644 --- a/src/simulation/dynamics/extForceTorque/_UnitTest/test_extForceTorqueIntegrated.py +++ b/src/simulation/dynamics/extForceTorque/_UnitTest/test_extForceTorqueIntegrated.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -22,7 +21,9 @@ from Basilisk.simulation import spacecraft from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions # uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed @@ -31,9 +32,10 @@ # @pytest.mark.xfail() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("function", ["extForceBodyAndTorque" - , "extForceInertialAndTorque" - ]) + +@pytest.mark.parametrize( + "function", ["extForceBodyAndTorque", "extForceInertialAndTorque"] +) def test_ForceBodyAndTorqueAllTest(show_plots, function): """Module Unit Test""" testFunction = globals().get(function) @@ -75,17 +77,27 @@ def extForceBodyAndTorque(): unitTestSim.earthGravBody = gravityEffector.GravBodyData() unitTestSim.earthGravBody.planetName = "earth_planet_data" - unitTestSim.earthGravBody.mu = 0.3986004415E+15 # meters! + unitTestSim.earthGravBody.mu = 0.3986004415e15 # meters! unitTestSim.earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([unitTestSim.earthGravBody]) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + [unitTestSim.earthGravBody] + ) # Define initial conditions scObject.hub.mHub = 750.0 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + 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.1], [0.2], [-0.3]] scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] @@ -101,7 +113,7 @@ def extForceBodyAndTorque(): posRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubPosition) sigmaRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubSigma) - stopTime = 60.0*10.0 + stopTime = 60.0 * 10.0 unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) unitTestSim.ExecuteSimulation() @@ -110,26 +122,30 @@ def extForceBodyAndTorque(): dataPos = [[dataPos[0][0], dataPos[1][0], dataPos[2][0]]] dataSigma = [[dataSigma[0][0], dataSigma[1][0], dataSigma[2][0]]] - truePos = [ - [-6.78136423e+06, 4.94628599e+06, 5.48655395e+06] - ] + truePos = [[-6.78136423e06, 4.94628599e06, 5.48655395e06]] - trueSigma = [ - [4.91025978e-01, -4.21586707e-01, 3.61459503e-01] - ] + trueSigma = [[4.91025978e-01, -4.21586707e-01, 3.61459503e-01]] accuracy = 1e-8 - for i in range(0,len(truePos)): + for i in range(0, len(truePos)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(dataPos[i],truePos[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + dataPos[i], truePos[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: External Body Force and Torque failed pos unit test") + testMessages.append( + "FAILED: External Body Force and Torque failed pos unit test" + ) - for i in range(0,len(trueSigma)): + for i in range(0, len(trueSigma)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(dataSigma[i],trueSigma[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + dataSigma[i], trueSigma[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: External Body Force and Torque failed attitude unit test") + testMessages.append( + "FAILED: External Body Force and Torque failed attitude unit test" + ) if testFailCount == 0: print("PASSED: " + " External Body Force and Torque Inegrated Sim Test") @@ -138,7 +154,8 @@ def extForceBodyAndTorque(): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def extForceInertialAndTorque(): # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -168,17 +185,27 @@ def extForceInertialAndTorque(): unitTestSim.earthGravBody = gravityEffector.GravBodyData() unitTestSim.earthGravBody.planetName = "earth_planet_data" - unitTestSim.earthGravBody.mu = 0.3986004415E+15 # meters! + unitTestSim.earthGravBody.mu = 0.3986004415e15 # meters! unitTestSim.earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([unitTestSim.earthGravBody]) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + [unitTestSim.earthGravBody] + ) # Define initial conditions of the spacecraft scObject.hub.mHub = 750.0 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + 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.1], [0.2], [-0.3]] scObject.hub.omega_BN_BInit = [[0.001], [-0.01], [0.03]] @@ -194,7 +221,7 @@ def extForceInertialAndTorque(): posRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubPosition) sigmaRef = scObject.dynManager.getStateObject(scObject.hub.nameOfHubSigma) - stopTime = 60.0*10.0 + stopTime = 60.0 * 10.0 unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) unitTestSim.ExecuteSimulation() @@ -203,26 +230,30 @@ def extForceInertialAndTorque(): dataPos = [[dataPos[0][0], dataPos[1][0], dataPos[2][0]]] dataSigma = [[dataSigma[0][0], dataSigma[1][0], dataSigma[2][0]]] - truePos = [ - [-6.78183900e+06, 4.94674963e+06, 5.48686274e+06] - ] + truePos = [[-6.78183900e06, 4.94674963e06, 5.48686274e06]] - trueSigma = [ - [4.91025978e-01, -4.21586707e-01, 3.61459503e-01] - ] + trueSigma = [[4.91025978e-01, -4.21586707e-01, 3.61459503e-01]] accuracy = 1e-8 - for i in range(0,len(truePos)): + for i in range(0, len(truePos)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(dataPos[i],truePos[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + dataPos[i], truePos[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: External Inertial Force and Torque failed pos unit test") + testMessages.append( + "FAILED: External Inertial Force and Torque failed pos unit test" + ) - for i in range(0,len(trueSigma)): + for i in range(0, len(trueSigma)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(dataSigma[i],trueSigma[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + dataSigma[i], trueSigma[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: External Inertial Force and Torque failed attitude unit test") + testMessages.append( + "FAILED: External Inertial Force and Torque failed attitude unit test" + ) if testFailCount == 0: print("PASSED: " + " External Inertial Force and Torque Inegrated Sim Test") @@ -231,7 +262,8 @@ def extForceInertialAndTorque(): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": extForceBodyAndTorque() diff --git a/src/simulation/dynamics/facetDragEffector/_UnitTest/test_unitFacetDrag.py b/src/simulation/dynamics/facetDragEffector/_UnitTest/test_unitFacetDrag.py index 90408c142d..680c434887 100644 --- a/src/simulation/dynamics/facetDragEffector/_UnitTest/test_unitFacetDrag.py +++ b/src/simulation/dynamics/facetDragEffector/_UnitTest/test_unitFacetDrag.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016-2018, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,7 +15,6 @@ # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - # # # @@ -34,7 +32,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) @@ -53,24 +51,55 @@ from Basilisk.utilities import simIncludeGravBody -test_drag = [([1.0, 1.0], np.array([2.0, 2.0]), [np.array([1, 0, 0]), np.array([0, 1, 0])], [np.array([0.1, 0, 0]), np.array([0, 0.1, 0])]), - ([1.0, 1.0], np.array([2.0, 2.0]), [np.array([1, 0, 0]), np.array([0, 1, 0])], [np.array([0.3, 0, 0]), np.array([0, 0.3, 0])]), - ([1.0, 2.0], np.array([2.0, 4.0]), [np.array([1, 0, 0]), np.array([0, 1, 0])], [np.array([0.1, 0, 0]), np.array([0, 0.1, 0])]), - ([1.0, 1.0], np.array([2.0, 2.0]), [np.array([1, 0, 0]), np.array([0, 1, 0])], [np.array([0.1, 0, 0]), np.array([0, 0, 0.1])]), - ([1.0, 1.0], np.array([2.0, 2.0]), [np.array([1, 0, 0]), np.array([0, 0, 1])], [np.array([0.1, 0, 0]), np.array([0, 0, 0.1])]), - ([1.0, 1.0], np.array([2.0, 2.0]), [np.array([0, 0, -1]), np.array([0, -1, 0])], [np.array([0, 0, 0.1]), np.array([0, 0.1, 0])]), +test_drag = [ + ( + [1.0, 1.0], + np.array([2.0, 2.0]), + [np.array([1, 0, 0]), np.array([0, 1, 0])], + [np.array([0.1, 0, 0]), np.array([0, 0.1, 0])], + ), + ( + [1.0, 1.0], + np.array([2.0, 2.0]), + [np.array([1, 0, 0]), np.array([0, 1, 0])], + [np.array([0.3, 0, 0]), np.array([0, 0.3, 0])], + ), + ( + [1.0, 2.0], + np.array([2.0, 4.0]), + [np.array([1, 0, 0]), np.array([0, 1, 0])], + [np.array([0.1, 0, 0]), np.array([0, 0.1, 0])], + ), + ( + [1.0, 1.0], + np.array([2.0, 2.0]), + [np.array([1, 0, 0]), np.array([0, 1, 0])], + [np.array([0.1, 0, 0]), np.array([0, 0, 0.1])], + ), + ( + [1.0, 1.0], + np.array([2.0, 2.0]), + [np.array([1, 0, 0]), np.array([0, 0, 1])], + [np.array([0.1, 0, 0]), np.array([0, 0, 0.1])], + ), + ( + [1.0, 1.0], + np.array([2.0, 2.0]), + [np.array([0, 0, -1]), np.array([0, -1, 0])], + [np.array([0, 0, 0.1]), np.array([0, 0.1, 0])], + ), ] + @pytest.mark.parametrize("scAreas, scCoeff, B_normals, B_locations", test_drag) def test_DragCalculation(scAreas, scCoeff, B_normals, B_locations): - ## Simulation initialization simTaskName = "simTask" simProcessName = "simProcess" scSim = SimulationBaseClass.SimBaseClass() dynProcess = scSim.CreateNewProcess(simProcessName) - simulationTimeStep = macros.sec2nano(5.) + simulationTimeStep = macros.sec2nano(5.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # initialize spacecraft object and set properties @@ -89,7 +118,7 @@ def test_DragCalculation(scAreas, scCoeff, B_normals, B_locations): scObject.addDynamicEffector(newDrag) try: - for i in range(0,len(scAreas)): + for i in range(0, len(scAreas)): newDrag.addFacet(scAreas[i], scCoeff[i], B_normals[i], B_locations[i]) except: pytest.fail("ERROR: FacetDrag unit test failed while setting facet parameters.") @@ -98,16 +127,18 @@ def test_DragCalculation(scAreas, scCoeff, B_normals, B_locations): gravFactory = simIncludeGravBody.gravBodyFactory() planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu # attach gravity model to spacecraft - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # # setup orbit and simulation time oe = orbitalMotion.ClassicElements() - r_eq = 6371*1000.0 + r_eq = 6371 * 1000.0 refBaseDens = 1.217 refScaleHeight = 8500.0 @@ -116,15 +147,15 @@ def test_DragCalculation(scAreas, scCoeff, B_normals, B_locations): newAtmo.scaleHeight = refScaleHeight newAtmo.planetRadius = r_eq - rN = np.array([r_eq+200.0e3,0,0]) - vN = np.array([0,7.788e3,0]) - sig_BN = np.array([0,0,0]) + rN = np.array([r_eq + 200.0e3, 0, 0]) + vN = np.array([0, 7.788e3, 0]) + sig_BN = np.array([0, 0, 0]) # initialize Spacecraft States with the initialization variables scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m - v_CN_N scObject.hub.sigma_BNInit = sig_BN - simulationTime = macros.sec2nano(5.) + simulationTime = macros.sec2nano(5.0) # # Setup data logging before the simulation is initialized # @@ -154,8 +185,12 @@ def test_DragCalculation(scAreas, scCoeff, B_normals, B_locations): scSim.ExecuteSimulation() # Retrieve logged data - dragDataForce_B = unitTestSupport.addTimeColumn(newDragLog.times(), newDragLog.forceExternal_B) - dragTorqueData = unitTestSupport.addTimeColumn(newDragLog.times(), newDragLog.torqueExternalPntB_B) + dragDataForce_B = unitTestSupport.addTimeColumn( + newDragLog.times(), newDragLog.forceExternal_B + ) + dragTorqueData = unitTestSupport.addTimeColumn( + newDragLog.times(), newDragLog.torqueExternalPntB_B + ) posData = dataLog.r_BN_N velData = dataLog.v_BN_N attData = dataLog.sigma_BN @@ -170,39 +205,68 @@ def checkFacetDragForce(dens, area, coeff, facet_dir, sigma_BN, inertial_vel): if projArea > 0: drag_force = -0.5 * dens * projArea * coeff * vMag**2.0 * v_hat_B else: - drag_force = np.zeros([3,]) + drag_force = np.zeros( + [ + 3, + ] + ) return drag_force - # Compare to expected values - test_val_force = np.zeros([3,]) - test_val_torque = np.zeros([3,]) + test_val_force = np.zeros( + [ + 3, + ] + ) + test_val_torque = np.zeros( + [ + 3, + ] + ) for i in range(len(scAreas)): - val_force_i = checkFacetDragForce(densData[i], scAreas[i], scCoeff[i], B_normals[i], attData[1], velData[1]) + val_force_i = checkFacetDragForce( + densData[i], scAreas[i], scCoeff[i], B_normals[i], attData[1], velData[1] + ) test_val_force += val_force_i test_val_torque += np.cross(B_locations[i], val_force_i) assert len(densData) > 0, "FAILED: ExpAtmo failed to pull any logged data" - np.testing.assert_allclose(dragDataForce_B[1,1:4], test_val_force, atol = 1e-06) - np.testing.assert_allclose(dragTorqueData[1,1:4], test_val_torque, atol = 1e-06) - - -test_shadow = [([1.0, 1.0], np.array([2.0, 2.0]), [np.array([0, 0, -1]), np.array([0, -1, 0])], [np.array([0, 0, 0.1]), np.array([0, 0.1, 0])]), - ([1.0, 1.0], np.array([2.0, 4.0]), [np.array([0, 0, -1]), np.array([0, -1, 0])], [np.array([0, 0, 0.1]), np.array([0, 0.1, 0])]), - ([1.0, 1.0], np.array([2.0, 2.0]), [np.array([0, 0, -1]), np.array([0, -1, 0])], [np.array([0, 0, 0.4]), np.array([0, 0.4, 0])]), + np.testing.assert_allclose(dragDataForce_B[1, 1:4], test_val_force, atol=1e-06) + np.testing.assert_allclose(dragTorqueData[1, 1:4], test_val_torque, atol=1e-06) + + +test_shadow = [ + ( + [1.0, 1.0], + np.array([2.0, 2.0]), + [np.array([0, 0, -1]), np.array([0, -1, 0])], + [np.array([0, 0, 0.1]), np.array([0, 0.1, 0])], + ), + ( + [1.0, 1.0], + np.array([2.0, 4.0]), + [np.array([0, 0, -1]), np.array([0, -1, 0])], + [np.array([0, 0, 0.1]), np.array([0, 0.1, 0])], + ), + ( + [1.0, 1.0], + np.array([2.0, 2.0]), + [np.array([0, 0, -1]), np.array([0, -1, 0])], + [np.array([0, 0, 0.4]), np.array([0, 0.4, 0])], + ), ] + @pytest.mark.parametrize("scAreas, scCoeff, B_normals, B_locations", test_shadow) def test_ShadowCalculation(scAreas, scCoeff, B_normals, B_locations): - ## Simulation initialization simTaskName = "simTask" simProcessName = "simProcess" scSim = SimulationBaseClass.SimBaseClass() dynProcess = scSim.CreateNewProcess(simProcessName) - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # initialize spacecraft object and set properties @@ -224,8 +288,10 @@ def test_ShadowCalculation(scAreas, scCoeff, B_normals, B_locations): scObject.addDynamicEffector(newDrag) try: - for ind in range(0,len(scAreas)): - newDrag.addFacet(scAreas[ind], scCoeff[ind], B_normals[ind], B_locations[ind]) + for ind in range(0, len(scAreas)): + newDrag.addFacet( + scAreas[ind], scCoeff[ind], B_normals[ind], B_locations[ind] + ) except: pytest.fail("ERROR: FacetDrag unit test failed while setting facet parameters.") @@ -233,16 +299,18 @@ def test_ShadowCalculation(scAreas, scCoeff, B_normals, B_locations): gravFactory = simIncludeGravBody.gravBodyFactory() planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu # attach gravity model to spacecraft - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # # setup orbit and simulation time oe = orbitalMotion.ClassicElements() - r_eq = 6371*1000.0 + r_eq = 6371 * 1000.0 refBaseDens = 1.217 refScaleHeight = 8500.0 @@ -251,16 +319,16 @@ def test_ShadowCalculation(scAreas, scCoeff, B_normals, B_locations): newAtmo.scaleHeight = refScaleHeight newAtmo.planetRadius = r_eq - rN = np.array([r_eq+200.0e3,0,0]) - vN = np.array([0,7.788e3,0]) - sig_BN = np.array([0,0,0]) + rN = np.array([r_eq + 200.0e3, 0, 0]) + vN = np.array([0, 7.788e3, 0]) + sig_BN = np.array([0, 0, 0]) # initialize Spacecraft States with the initialization variables scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m - v_CN_N scObject.hub.sigma_BNInit = sig_BN - simulationTime = macros.sec2nano(10.) + simulationTime = macros.sec2nano(10.0) # # Setup data logging before the simulation is initialized # @@ -290,8 +358,12 @@ def test_ShadowCalculation(scAreas, scCoeff, B_normals, B_locations): scSim.ExecuteSimulation() # Retrieve logged data - dragDataForce_B = unitTestSupport.addTimeColumn(newDragLog.times(), newDragLog.forceExternal_B) - dragTorqueData = unitTestSupport.addTimeColumn(newDragLog.times(), newDragLog.torqueExternalPntB_B) + dragDataForce_B = unitTestSupport.addTimeColumn( + newDragLog.times(), newDragLog.forceExternal_B + ) + dragTorqueData = unitTestSupport.addTimeColumn( + newDragLog.times(), newDragLog.torqueExternalPntB_B + ) posData = dataLog.r_BN_N velData = dataLog.v_BN_N attData = dataLog.sigma_BN @@ -299,11 +371,12 @@ def test_ShadowCalculation(scAreas, scCoeff, B_normals, B_locations): np.set_printoptions(precision=16) assert len(densData) > 0, "FAILED: ExpAtmo failed to pull any logged data" - for ind in range(1,len(densData)): - np.testing.assert_allclose(dragDataForce_B[ind,1:4], [0, 0, 0], atol = 1e-11) - np.testing.assert_allclose(dragTorqueData[ind,1:4], [0, 0, 0], atol = 1e-11) + for ind in range(1, len(densData)): + np.testing.assert_allclose(dragDataForce_B[ind, 1:4], [0, 0, 0], atol=1e-11) + np.testing.assert_allclose(dragTorqueData[ind, 1:4], [0, 0, 0], atol=1e-11) + -if __name__=="__main__": +if __name__ == "__main__": scAreas = [1.0, 1.0] scCoeff = np.array([2.0, 2.0]) B_normals = [np.array([0, 0, -1]), np.array([0, -1, 0])] diff --git a/src/simulation/dynamics/facetSRPDynamicEffector/_UnitTest/test_unitFacetSRPDynamicEffector.py b/src/simulation/dynamics/facetSRPDynamicEffector/_UnitTest/test_unitFacetSRPDynamicEffector.py index d3d2988fc7..82f599d4ec 100644 --- a/src/simulation/dynamics/facetSRPDynamicEffector/_UnitTest/test_unitFacetSRPDynamicEffector.py +++ b/src/simulation/dynamics/facetSRPDynamicEffector/_UnitTest/test_unitFacetSRPDynamicEffector.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -41,9 +40,16 @@ from Basilisk.simulation import spacecraft from Basilisk.architecture import messaging + # Vary the articulated facet initial angles -@pytest.mark.parametrize("facetRotAngle1", [macros.D2R * -10.4, macros.D2R * 45.2, macros.D2R * 90.0, macros.D2R * 180.0]) -@pytest.mark.parametrize("facetRotAngle2", [macros.D2R * -28.0, macros.D2R * 45.2, macros.D2R * -90.0, macros.D2R * 180.0]) +@pytest.mark.parametrize( + "facetRotAngle1", + [macros.D2R * -10.4, macros.D2R * 45.2, macros.D2R * 90.0, macros.D2R * 180.0], +) +@pytest.mark.parametrize( + "facetRotAngle2", + [macros.D2R * -28.0, macros.D2R * 45.2, macros.D2R * -90.0, macros.D2R * 180.0], +) def test_facetSRPDynamicEffector(show_plots, facetRotAngle1, facetRotAngle2): r""" **Verification Test Description** @@ -81,7 +87,7 @@ def test_facetSRPDynamicEffector(show_plots, facetRotAngle1, facetRotAngle2): sunStateMsg.PositionVector = [0.0, 0.0, 0.0] sunStateMsg.VelocityVector = [0.0, 0.0, 0.0] sunMsg = messaging.SpicePlanetStateMsg().write(sunStateMsg) - gravFactory.gravBodies['sun'].planetBodyInMsg.subscribeTo(sunMsg) + gravFactory.gravBodies["sun"].planetBodyInMsg.subscribeTo(sunMsg) # Create the spacecraft object and set the spacecraft orbit scObject = spacecraft.Spacecraft() @@ -105,12 +111,16 @@ def test_facetSRPDynamicEffector(show_plots, facetRotAngle1, facetRotAngle2): facetRotAngle1MessageData = messaging.HingedRigidBodyMsgPayload() facetRotAngle1MessageData.theta = facetRotAngle1 # [rad] facetRotAngle1MessageData.thetaDot = 0.0 # [rad] - facetRotAngle1Message = messaging.HingedRigidBodyMsg().write(facetRotAngle1MessageData) + facetRotAngle1Message = messaging.HingedRigidBodyMsg().write( + facetRotAngle1MessageData + ) facetRotAngle2MessageData = messaging.HingedRigidBodyMsgPayload() facetRotAngle2MessageData.theta = facetRotAngle2 # [rad] facetRotAngle2MessageData.thetaDot = 0.0 # [rad] - facetRotAngle2Message = messaging.HingedRigidBodyMsg().write(facetRotAngle2MessageData) + facetRotAngle2Message = messaging.HingedRigidBodyMsg().write( + facetRotAngle2MessageData + ) # Create an instance of the facetSRPDynamicEffector module to be tested srpEffector = facetSRPDynamicEffector.FacetSRPDynamicEffector() @@ -131,7 +141,18 @@ def test_facetSRPDynamicEffector(show_plots, facetRotAngle1, facetRotAngle2): # Define facet areas area1 = 1.5 * 1.5 area2 = np.pi * (0.5 * 7.5) * (0.5 * 7.5) - facetAreaList = [area1, area1, area1, area1, area1, area1, area2, area2, area2, area2] + facetAreaList = [ + area1, + area1, + area1, + area1, + area1, + area1, + area2, + area2, + area2, + area2, + ] # Define the initial facet attitudes relative to B frame prv_F01B = (macros.D2R * -90.0) * np.array([0.0, 0.0, 1.0]) @@ -144,71 +165,85 @@ def test_facetSRPDynamicEffector(show_plots, facetRotAngle1, facetRotAngle2): prv_F08B = (macros.D2R * 180.0) * np.array([0.0, 0.0, 1.0]) prv_F09B = (macros.D2R * 0.0) * np.array([0.0, 0.0, 1.0]) prv_F010B = (macros.D2R * 180.0) * np.array([0.0, 0.0, 1.0]) - facetDcm_F0BList = [rbk.PRV2C(prv_F01B), - rbk.PRV2C(prv_F02B), - rbk.PRV2C(prv_F03B), - rbk.PRV2C(prv_F04B), - rbk.PRV2C(prv_F05B), - rbk.PRV2C(prv_F06B), - rbk.PRV2C(prv_F07B), - rbk.PRV2C(prv_F08B), - rbk.PRV2C(prv_F09B), - rbk.PRV2C(prv_F010B)] + facetDcm_F0BList = [ + rbk.PRV2C(prv_F01B), + rbk.PRV2C(prv_F02B), + rbk.PRV2C(prv_F03B), + rbk.PRV2C(prv_F04B), + rbk.PRV2C(prv_F05B), + rbk.PRV2C(prv_F06B), + rbk.PRV2C(prv_F07B), + rbk.PRV2C(prv_F08B), + rbk.PRV2C(prv_F09B), + rbk.PRV2C(prv_F010B), + ] # Define the facet normal vectors in F frame components - facetNHat_FList = [np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0]), - np.array([0.0, 1.0, 0.0])] + facetNHat_FList = [ + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + np.array([0.0, 1.0, 0.0]), + ] # Define facet articulation axes in F frame components - facetRotHat_FList = [np.array([0.0, 0.0, 0.0]), - np.array([0.0, 0.0, 0.0]), - np.array([0.0, 0.0, 0.0]), - np.array([0.0, 0.0, 0.0]), - np.array([0.0, 0.0, 0.0]), - np.array([0.0, 0.0, 0.0]), - np.array([1.0, 0.0, 0.0]), - np.array([-1.0, 0.0, 0.0]), - np.array([-1.0, 0.0, 0.0]), - np.array([1.0, 0.0, 0.0])] + facetRotHat_FList = [ + np.array([0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0]), + np.array([1.0, 0.0, 0.0]), + np.array([-1.0, 0.0, 0.0]), + np.array([-1.0, 0.0, 0.0]), + np.array([1.0, 0.0, 0.0]), + ] # Define facet center of pressure locations relative to point B - facetR_CopB_BList = [np.array([0.75, 0.0, 0.0]), - np.array([0.0, 0.75, 0.0]), - np.array([-0.75, 0.0, 0.0]), - np.array([0.0, -0.75, 0.0]), - np.array([0.0, 0.0, 0.75]), - np.array([0.0, 0.0, -0.75]), - np.array([4.5, 0.0, 0.75]), - np.array([4.5, 0.0, 0.75]), - np.array([-4.5, 0.0, 0.75]), - np.array([-4.5, 0.0, 0.75])] + facetR_CopB_BList = [ + np.array([0.75, 0.0, 0.0]), + np.array([0.0, 0.75, 0.0]), + np.array([-0.75, 0.0, 0.0]), + np.array([0.0, -0.75, 0.0]), + np.array([0.0, 0.0, 0.75]), + np.array([0.0, 0.0, -0.75]), + np.array([4.5, 0.0, 0.75]), + np.array([4.5, 0.0, 0.75]), + np.array([-4.5, 0.0, 0.75]), + np.array([-4.5, 0.0, 0.75]), + ] # Define facet optical coefficients facetDiffuseCoeffList = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - facetSpecularCoeffList = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) + facetSpecularCoeffList = np.array( + [0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9] + ) # Populate the srpEffector spacecraft geometry structure with the facet information for i in range(numFacets): - srpEffector.addFacet(facetAreaList[i], - facetDcm_F0BList[i], - facetNHat_FList[i], - facetRotHat_FList[i], - facetR_CopB_BList[i], - facetDiffuseCoeffList[i], - facetSpecularCoeffList[i]) + srpEffector.addFacet( + facetAreaList[i], + facetDcm_F0BList[i], + facetNHat_FList[i], + facetRotHat_FList[i], + facetR_CopB_BList[i], + facetDiffuseCoeffList[i], + facetSpecularCoeffList[i], + ) # Set up data logging scPosDataLog = scObject.scStateOutMsg.recorder() - sunPosDataLog = gravFactory.gravBodies['sun'].planetBodyInMsg.recorder() - srpDataLog = srpEffector.logger(["forceExternal_B", "torqueExternalPntB_B"], simulationTimeStep) + sunPosDataLog = gravFactory.gravBodies["sun"].planetBodyInMsg.recorder() + srpDataLog = srpEffector.logger( + ["forceExternal_B", "torqueExternalPntB_B"], simulationTimeStep + ) unitTestSim.AddModelToTask(unitTaskName, scPosDataLog) unitTestSim.AddModelToTask(unitTaskName, sunPosDataLog) unitTestSim.AddModelToTask(unitTaskName, srpDataLog) @@ -230,34 +265,40 @@ def test_facetSRPDynamicEffector(show_plots, facetRotAngle1, facetRotAngle2): # Plot the spacecraft inertial position plt.figure() plt.clf() - plt.plot(timespan, r_BN_N[:, 0], label=r'$r_{\mathcal{B}/\mathcal{N}} \cdot \hat{n}_1$') - plt.plot(timespan, r_BN_N[:, 1], label=r'$r_{\mathcal{B}/\mathcal{N}} \cdot \hat{n}_2$') - plt.plot(timespan, r_BN_N[:, 2], label=r'$r_{\mathcal{B}/\mathcal{N}} \cdot \hat{n}_3$') + plt.plot( + timespan, r_BN_N[:, 0], label=r"$r_{\mathcal{B}/\mathcal{N}} \cdot \hat{n}_1$" + ) + plt.plot( + timespan, r_BN_N[:, 1], label=r"$r_{\mathcal{B}/\mathcal{N}} \cdot \hat{n}_2$" + ) + plt.plot( + timespan, r_BN_N[:, 2], label=r"$r_{\mathcal{B}/\mathcal{N}} \cdot \hat{n}_3$" + ) plt.title("Spacecraft Inertial Position Components") - plt.xlabel(r'Time (s)') - plt.ylabel(r'${}^N r_{\mathcal{B}/\mathcal{N}}$ (m)') + plt.xlabel(r"Time (s)") + plt.ylabel(r"${}^N r_{\mathcal{B}/\mathcal{N}}$ (m)") plt.legend() # Plot SRP force plt.figure() plt.clf() - plt.plot(timespan, srpForce_BSim[:, 0], label=r'$F_{SRP} \cdot \hat{b}_1$') - plt.plot(timespan, srpForce_BSim[:, 1], label=r'$F_{SRP} \cdot \hat{b}_2$') - plt.plot(timespan, srpForce_BSim[:, 2], label=r'$F_{SRP} \cdot \hat{b}_3$') + plt.plot(timespan, srpForce_BSim[:, 0], label=r"$F_{SRP} \cdot \hat{b}_1$") + plt.plot(timespan, srpForce_BSim[:, 1], label=r"$F_{SRP} \cdot \hat{b}_2$") + plt.plot(timespan, srpForce_BSim[:, 2], label=r"$F_{SRP} \cdot \hat{b}_3$") plt.title("SRP Force Components") - plt.xlabel('Time (s)') - plt.ylabel(r'${}^B F_{SRP}$ (N)') + plt.xlabel("Time (s)") + plt.ylabel(r"${}^B F_{SRP}$ (N)") plt.legend() # Plot SRP torque plt.figure() plt.clf() - plt.plot(timespan, srpTorque_BSim[:, 0], label=r'$L_{SRP} \cdot \hat{b}_1$') - plt.plot(timespan, srpTorque_BSim[:, 1], label=r'$L_{SRP} \cdot \hat{b}_2$') - plt.plot(timespan, srpTorque_BSim[:, 2], label=r'$L_{SRP} \cdot \hat{b}_3$') + plt.plot(timespan, srpTorque_BSim[:, 0], label=r"$L_{SRP} \cdot \hat{b}_1$") + plt.plot(timespan, srpTorque_BSim[:, 1], label=r"$L_{SRP} \cdot \hat{b}_2$") + plt.plot(timespan, srpTorque_BSim[:, 2], label=r"$L_{SRP} \cdot \hat{b}_3$") plt.title("SRP Torque Components") - plt.xlabel('Time (s)') - plt.ylabel(r'${}^B L_{SRP}$ (Nm)') + plt.xlabel("Time (s)") + plt.ylabel(r"${}^B L_{SRP}$ (Nm)") plt.legend() if show_plots: @@ -265,48 +306,59 @@ def test_facetSRPDynamicEffector(show_plots, facetRotAngle1, facetRotAngle2): plt.close("all") # Verify the results by comparing the last srp force and torque simulation values with the calculated truth values - srpForce_BTruth = np.zeros([3,]) - srpTorque_BTruth = np.zeros([3,]) + srpForce_BTruth = np.zeros( + [ + 3, + ] + ) + srpTorque_BTruth = np.zeros( + [ + 3, + ] + ) for i in range(len(facetAreaList)): - srpForce_BFacet, srpTorque_BFacet = computeFacetSRPForceTorque(i, - facetRotAngle1, - facetRotAngle2, - facetAreaList[i], - facetDcm_F0BList[i], - facetNHat_FList[i], - facetRotHat_FList[i], - facetR_CopB_BList[i], - facetDiffuseCoeffList[i], - facetSpecularCoeffList[i], - sigma_BN[-1], - r_BN_N[-1], - r_SN_N[-1]) + srpForce_BFacet, srpTorque_BFacet = computeFacetSRPForceTorque( + i, + facetRotAngle1, + facetRotAngle2, + facetAreaList[i], + facetDcm_F0BList[i], + facetNHat_FList[i], + facetRotHat_FList[i], + facetR_CopB_BList[i], + facetDiffuseCoeffList[i], + facetSpecularCoeffList[i], + sigma_BN[-1], + r_BN_N[-1], + r_SN_N[-1], + ) srpForce_BTruth += srpForce_BFacet srpTorque_BTruth += srpTorque_BFacet for idx in range(3): - np.testing.assert_allclose(srpForce_BSim[-1, idx], - srpForce_BTruth[idx], - atol=1e-12, - verbose=True) - np.testing.assert_allclose(srpTorque_BSim[-1, idx], - srpTorque_BTruth[idx], - atol=1e-12, - verbose=True) - -def computeFacetSRPForceTorque(index, - facetRotAngle1, - facetRotAngle2, - facetArea, - facetDcm_F0B, - facetNHat_F, - facetRotHat_F, - facetR_CopB_B, - facetDiffuseCoeff, - facetSpecularCoeff, - sigma_BN, - r_BN_N, - r_SN_N): + np.testing.assert_allclose( + srpForce_BSim[-1, idx], srpForce_BTruth[idx], atol=1e-12, verbose=True + ) + np.testing.assert_allclose( + srpTorque_BSim[-1, idx], srpTorque_BTruth[idx], atol=1e-12, verbose=True + ) + + +def computeFacetSRPForceTorque( + index, + facetRotAngle1, + facetRotAngle2, + facetArea, + facetDcm_F0B, + facetNHat_F, + facetRotHat_F, + facetR_CopB_B, + facetDiffuseCoeff, + facetSpecularCoeff, + sigma_BN, + r_BN_N, + r_SN_N, +): # Define required constants speedLight = 299792458.0 # [m/s] Speed of light AstU = 149597870700.0 # [m] Astronomical unit @@ -323,10 +375,10 @@ def computeFacetSRPForceTorque(index, # Rotate the articulated facet normal vector dcm_FF0 = np.eye(3) - if (index == 6 or index == 7): + if index == 6 or index == 7: prv_FF0 = facetRotAngle1 * facetRotHat_F dcm_FF0 = rbk.PRV2C(prv_FF0) - if (index == 8 or index == 9): + if index == 8 or index == 9: prv_FF0 = facetRotAngle2 * facetRotHat_F dcm_FF0 = rbk.PRV2C(prv_FF0) @@ -344,15 +396,33 @@ def computeFacetSRPForceTorque(index, # Compute the SRP force acting on the facet if projArea > 0: - srpForce_BTruth = -SRPPressure * projArea * ((1-facetSpecularCoeff) * sHat + 2 * ( (facetDiffuseCoeff / 3) + facetSpecularCoeff * cosTheta) * facetNHat_B) + srpForce_BTruth = ( + -SRPPressure + * projArea + * ( + (1 - facetSpecularCoeff) * sHat + + 2 + * ((facetDiffuseCoeff / 3) + facetSpecularCoeff * cosTheta) + * facetNHat_B + ) + ) srpTorque_BTruth = np.cross(facetR_CopB_B, srpForce_BTruth) else: - srpForce_BTruth = np.zeros([3,]) - srpTorque_BTruth = np.zeros([3,]) + srpForce_BTruth = np.zeros( + [ + 3, + ] + ) + srpTorque_BTruth = np.zeros( + [ + 3, + ] + ) return srpForce_BTruth, srpTorque_BTruth -if __name__=="__main__": + +if __name__ == "__main__": test_facetSRPDynamicEffector( True, # show plots macros.D2R * -10.0, # [rad] facetRotAngle1 diff --git a/src/simulation/dynamics/gravityEffector/_UnitTest/test_gravityDynEffector.py b/src/simulation/dynamics/gravityEffector/_UnitTest/test_gravityDynEffector.py index b3a0262df5..bc42b896a8 100644 --- a/src/simulation/dynamics/gravityEffector/_UnitTest/test_gravityDynEffector.py +++ b/src/simulation/dynamics/gravityEffector/_UnitTest/test_gravityDynEffector.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -26,10 +25,13 @@ path = os.path.dirname(os.path.abspath(filename)) from Basilisk import __path__ + bskPath = __path__[0] from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.utilities import macros from Basilisk.simulation import gravityEffector from Basilisk.simulation import spiceInterface @@ -41,43 +43,48 @@ from Basilisk.simulation.gravityEffector import loadGravFromFileToList from Basilisk.architecture.bskLogging import BasiliskError -#script to check spherical harmonics calcs out to 20th degree -#Uses coefficient from Vallado tables D-1 +# script to check spherical harmonics calcs out to 20th degree +# Uses coefficient from Vallado tables D-1 + def computeGravityTo20(positionVector): - #This code follows the formulation in Vallado, page 521, second edition and uses data from UTexas CSR for - #gravitation harmonics parameters - #Written 201780807 by Scott Carnahan - #AVS Lab | CU Boulder + # This code follows the formulation in Vallado, page 521, second edition and uses data from UTexas CSR for + # gravitation harmonics parameters + # Written 201780807 by Scott Carnahan + # AVS Lab | CU Boulder - #INPUTS - #positionVector - [x,y,z] coordinates list of spacecraft in [m] in earth body frame so that lat, long can be calculated + # INPUTS + # positionVector - [x,y,z] coordinates list of spacecraft in [m] in earth body frame so that lat, long can be calculated def legendres(degree, alpha): - P = np.zeros((degree+1,degree+1)) - P[0,0] = 1 - P[1,0] = alpha - cosPhi = np.sqrt(1-alpha**2) - P[1,1] = cosPhi - - for l in range(2,degree+1): - for m in range(0,l+1): + P = np.zeros((degree + 1, degree + 1)) + P[0, 0] = 1 + P[1, 0] = alpha + cosPhi = np.sqrt(1 - alpha**2) + P[1, 1] = cosPhi + + for l in range(2, degree + 1): + for m in range(0, l + 1): if m == 0 and l >= 2: - P[l,m] = ((2*l-1)*alpha*P[l-1,0]-(l-1)*P[l-2,0]) / l + P[l, m] = ( + (2 * l - 1) * alpha * P[l - 1, 0] - (l - 1) * P[l - 2, 0] + ) / l elif m != 0 and m < l: - P[l, m] = (P[l-2, m]+(2*l-1)*cosPhi*P[l-1,m-1]) + P[l, m] = P[l - 2, m] + (2 * l - 1) * cosPhi * P[l - 1, m - 1] elif m == l and l != 0: - P[l,m] = (2*l-1)*cosPhi*P[l-1,m-1] + P[l, m] = (2 * l - 1) * cosPhi * P[l - 1, m - 1] else: - print(l,", ", m) + print(l, ", ", m) return P maxDegree = 20 - cList = np.zeros(maxDegree+2) - sList = np.zeros(maxDegree+2) - muEarth = 0. - radEarth = 0. - [cList, sList, muEarth, radEarth] = loadGravFromFileToList(path + '/GGM03S.txt', maxDegree+2) + cList = np.zeros(maxDegree + 2) + sList = np.zeros(maxDegree + 2) + muEarth = 0.0 + radEarth = 0.0 + [cList, sList, muEarth, radEarth] = loadGravFromFileToList( + path + "/GGM03S.txt", maxDegree + 2 + ) r = np.linalg.norm(positionVector) rHat = positionVector / r @@ -89,56 +96,77 @@ def legendres(degree, alpha): rK = positionVector[2] rIJ = np.sqrt(rI**2 + rJ**2) - if rIJ != 0.: - phi = math.atan(rK / rIJ) #latitude in radians + if rIJ != 0.0: + phi = math.atan(rK / rIJ) # latitude in radians else: - phi = math.copysign(np.pi/2., rK) - if rI != 0.: - lambdaSat = math.atan(rJ / rI) #longitude in radians + phi = math.copysign(np.pi / 2.0, rK) + if rI != 0.0: + lambdaSat = math.atan(rJ / rI) # longitude in radians else: - lambdaSat = math.copysign(np.pi/2., rJ) + lambdaSat = math.copysign(np.pi / 2.0, rJ) - P = legendres(maxDegree+1,np.sin(phi)) + P = legendres(maxDegree + 1, np.sin(phi)) - dUdr = 0. - dUdphi = 0. - dUdlambda = 0. + dUdr = 0.0 + dUdphi = 0.0 + dUdlambda = 0.0 - for l in range(0, maxDegree+1): - for m in range(0,l+1): + for l in range(0, maxDegree + 1): + for m in range(0, l + 1): if m == 0: k = 1 else: k = 2 - num = math.factorial(l+m) - den = math.factorial(l-m)*k*(2*l+1) - PI = np.sqrt(float(num)/float(den)) + num = math.factorial(l + m) + den = math.factorial(l - m) * k * (2 * l + 1) + PI = np.sqrt(float(num) / float(den)) cList[l][m] = cList[l][m] / PI sList[l][m] = sList[l][m] / PI - for l in range(2,maxDegree+1): #can only do for max degree minus 1 - for m in range(0,l+1): - dUdr = dUdr + (((radEarth/r)**l)*(l+1)*P[l,m]) * (cList[l][m]*np.cos(m*lambdaSat)+sList[l][m]*np.sin(m*lambdaSat)) - dUdphi = dUdphi + (((radEarth/r)**l)*P[l,m+1] - m*np.tan(phi)*P[l,m]) * (cList[l][m]*np.cos(m*lambdaSat) + sList[l][m]*np.sin(m*lambdaSat)) - dUdlambda = dUdlambda + (((radEarth/r)**l)*m*P[l,m]) * (sList[l][m]*np.cos(m*lambdaSat) - cList[l][m]*np.sin(m*lambdaSat)) + for l in range(2, maxDegree + 1): # can only do for max degree minus 1 + for m in range(0, l + 1): + dUdr = dUdr + (((radEarth / r) ** l) * (l + 1) * P[l, m]) * ( + cList[l][m] * np.cos(m * lambdaSat) + + sList[l][m] * np.sin(m * lambdaSat) + ) + dUdphi = dUdphi + ( + ((radEarth / r) ** l) * P[l, m + 1] - m * np.tan(phi) * P[l, m] + ) * ( + cList[l][m] * np.cos(m * lambdaSat) + + sList[l][m] * np.sin(m * lambdaSat) + ) + dUdlambda = dUdlambda + (((radEarth / r) ** l) * m * P[l, m]) * ( + sList[l][m] * np.cos(m * lambdaSat) + - cList[l][m] * np.sin(m * lambdaSat) + ) dUdr = -muEarth * dUdr / r**2 dUdphi = muEarth * dUdphi / r dUdlambda = muEarth * dUdlambda / r - - if rI != 0. and rJ != 0.: - accelerationI = (dUdr/r - rK*dUdphi/(r**2)/((rI**2+rJ**2)**0.5))*rI - (dUdlambda/(rI**2+rJ**2))*rJ + grav0[0] - accelerationJ = (dUdr/r - rK*dUdphi/(r**2)/((rI**2+rJ**2)**0.5))*rJ + (dUdlambda/(rI**2+rJ**2))*rI + grav0[1] + if rI != 0.0 and rJ != 0.0: + accelerationI = ( + (dUdr / r - rK * dUdphi / (r**2) / ((rI**2 + rJ**2) ** 0.5)) * rI + - (dUdlambda / (rI**2 + rJ**2)) * rJ + + grav0[0] + ) + accelerationJ = ( + (dUdr / r - rK * dUdphi / (r**2) / ((rI**2 + rJ**2) ** 0.5)) * rJ + + (dUdlambda / (rI**2 + rJ**2)) * rI + + grav0[1] + ) else: - accelerationI = dUdr/r + grav0[0] - accelerationJ = dUdr/r + grav0[1] - accelerationK = (dUdr/r)*rK + (((rI**2+rJ**2)**0.5)*dUdphi/(r**2)) + grav0[2] + accelerationI = dUdr / r + grav0[0] + accelerationJ = dUdr / r + grav0[1] + accelerationK = ( + (dUdr / r) * rK + (((rI**2 + rJ**2) ** 0.5) * dUdphi / (r**2)) + grav0[2] + ) accelerationVector = [accelerationI, accelerationJ, accelerationK] return accelerationVector + # uncomment this line is 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 @@ -155,6 +183,7 @@ def test_gravityEffectorAllTest(show_plots): [testResults, testMessage] = multiBodyGravity(show_plots) assert testResults < 1, testMessage + def independentSphericalHarmonics(show_plots): testCase = "independentCheck" # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -167,52 +196,65 @@ def independentSphericalHarmonics(show_plots): spherHarm = gravityEffector.SphericalHarmonics() - gravityEffector.loadGravFromFile(path + '/GGM03S.txt', spherHarm, 20) - gravCheck = computeGravityTo20([15000., 10000., 6378.1363E3]) + gravityEffector.loadGravFromFile(path + "/GGM03S.txt", spherHarm, 20) + gravCheck = computeGravityTo20([15000.0, 10000.0, 6378.1363e3]) spherHarm.initializeParameters() - gravOut = spherHarm.computeField([[15000.0], [10000.0], [(6378.1363) * 1.0E3]], 20, True) + gravOut = spherHarm.computeField( + [[15000.0], [10000.0], [(6378.1363) * 1.0e3]], 20, True + ) gravOutMag = np.linalg.norm(gravOut) gravCheckMag = np.linalg.norm(gravCheck) accuracy = 1e-12 - relative = (gravCheckMag-gravOutMag)/gravCheckMag + relative = (gravCheckMag - gravOutMag) / gravCheckMag if abs(relative) > accuracy: testFailCount += 1 testMessages.append("Failed independent spherical harmonics check") - snippetName = testCase + 'Accuracy' - snippetContent = '{:1.1e}'.format(accuracy) # write formatted LATEX string to file to be used by auto-documentation. - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, - path) # write formatted LATEX string to file to be used by auto-documentation. + snippetName = testCase + "Accuracy" + snippetContent = "{:1.1e}".format( + accuracy + ) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. if testFailCount == 0: - passFailText = 'PASSED' + passFailText = "PASSED" print("PASSED: " + testCase) - colorText = 'ForestGreen' # color to write auto-documented "PASSED" message in in LATEX. - snippetName = testCase + 'FailMsg' + colorText = "ForestGreen" # color to write auto-documented "PASSED" message in in LATEX. + snippetName = testCase + "FailMsg" snippetContent = "" - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, - path) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. else: - passFailText = 'FAILED' - colorText = 'Red' # color to write auto-documented "FAILED" message in in LATEX - snippetName = testCase + 'FailMsg' + passFailText = "FAILED" + colorText = "Red" # color to write auto-documented "FAILED" message in in LATEX + snippetName = testCase + "FailMsg" snippetContent = passFailText for message in testMessages: snippetContent += ". " + message snippetContent += "." - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, - path) # write formatted LATEX string to file to be used by auto-documentation. - snippetName = testCase + 'PassFail' # name of file to be written for auto-documentation which specifies if this test was passed or failed. - snippetContent = r'\textcolor{' + colorText + '}{' + passFailText + '}' # write formatted LATEX string to file to be used by auto-documentation. - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, - path) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. + snippetName = ( + testCase + "PassFail" + ) # name of file to be written for auto-documentation which specifies if this test was passed or failed. + snippetContent = ( + r"\textcolor{" + colorText + "}{" + passFailText + "}" + ) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def sphericalHarmonics(show_plots): - testCase = 'sphericalHarmonics' + testCase = "sphericalHarmonics" # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. @@ -233,14 +275,13 @@ def sphericalHarmonics(show_plots): if spherHarm.cBar[i][j] != testHarm[i][j]: vecCheckSuccess = False - - if(vecCheckSuccess != True): + if vecCheckSuccess != True: testFailCount += 1 testMessages.append("2D vector not input appropriately to spherical harmonics") - gravityEffector.loadGravFromFile(path + '/GGM03S.txt', spherHarm, 20) + gravityEffector.loadGravFromFile(path + "/GGM03S.txt", spherHarm, 20) spherHarm.initializeParameters() - gravOut = spherHarm.computeField([[0.0], [0.0], [(6378.1363)*1.0E3]], 20, True) + gravOut = spherHarm.computeField([[0.0], [0.0], [(6378.1363) * 1.0e3]], 20, True) gravMag = np.linalg.norm(np.array(gravOut)) accuracy = 0.1 @@ -248,43 +289,58 @@ def sphericalHarmonics(show_plots): if gravMag > (gravExpected + accuracy) or gravMag < (gravExpected - accuracy): testFailCount += 1 testMessages.append("Gravity magnitude not within allowable tolerance") - snippetName = testCase + 'Accuracy' - snippetContent = '{:1.1e}'.format(accuracy) # write formatted LATEX string to file to be used by auto-documentation. - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) #write formatted LATEX string to file to be used by auto-documentation. + snippetName = testCase + "Accuracy" + snippetContent = "{:1.1e}".format( + accuracy + ) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. try: - spherHarm.computeField([[0.0], [0.0], [(6378.1363)*1.0E3]], 100, True) + spherHarm.computeField([[0.0], [0.0], [(6378.1363) * 1.0e3]], 100, True) testFailCount += 1 testMessages.append("Gravity ceiling not enforced correctly") except BasiliskError: - pass # Great! We threw an error + pass # Great! We threw an error if testFailCount == 0: - passFailText = 'PASSED' + passFailText = "PASSED" print("PASSED: " + " Spherical Harmonics") - colorText = 'ForestGreen' # color to write auto-documented "PASSED" message in in LATEX. - snippetName = testCase + 'FailMsg' + colorText = "ForestGreen" # color to write auto-documented "PASSED" message in in LATEX. + snippetName = testCase + "FailMsg" snippetContent = "" - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. else: - passFailText = 'FAILED' - colorText = 'Red' # color to write auto-documented "FAILED" message in in LATEX - snippetName = testCase + 'FailMsg' + passFailText = "FAILED" + colorText = "Red" # color to write auto-documented "FAILED" message in in LATEX + snippetName = testCase + "FailMsg" snippetContent = passFailText for message in testMessages: snippetContent += ". " + message snippetContent += "." - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) # write formatted LATEX string to file to be used by auto-documentation. - snippetName = testCase + 'PassFail' # name of file to be written for auto-documentation which specifies if this test was passed or failed. - snippetContent = r'\textcolor{' + colorText + '}{' + passFailText + '}' #write formatted LATEX string to file to be used by auto-documentation. - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) #write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. + snippetName = ( + testCase + "PassFail" + ) # name of file to be written for auto-documentation which specifies if this test was passed or failed. + snippetContent = ( + r"\textcolor{" + colorText + "}{" + passFailText + "}" + ) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def singleGravityBody(show_plots): - testCase = 'singleBody' + testCase = "singleBody" # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. @@ -309,8 +365,10 @@ def singleGravityBody(show_plots): SpiceObject = spiceInterface.SpiceInterface() SpiceObject.ModelTag = "SpiceInterfaceData" - SpiceObject.SPICEDataPath = bskPath + '/supportData/EphemerisData/' - SpiceObject.addPlanetNames(spiceInterface.StringVector(["earth", "mars barycenter", "sun"])) + SpiceObject.SPICEDataPath = bskPath + "/supportData/EphemerisData/" + SpiceObject.addPlanetNames( + spiceInterface.StringVector(["earth", "mars barycenter", "sun"]) + ) SpiceObject.UTCCalInit = DateSpice TotalSim.AddModelToTask(unitTaskName, SpiceObject) SpiceObject.UTCCalInit = "1994 JAN 26 00:02:00.184" @@ -318,7 +376,7 @@ def singleGravityBody(show_plots): gravBody1 = gravityEffector.GravBodyData() gravBody1.planetName = "earth_planet_data" gravBody1.isCentralBody = False - gravBody1.useSphericalHarmonicsGravityModel(path + '/GGM03S.txt', 60) + gravBody1.useSphericalHarmonicsGravityModel(path + "/GGM03S.txt", 60) gravBody1.planetBodyInMsg.subscribeTo(SpiceObject.planetStateOutMsgs[0]) # Use the python spice utility to load in spacecraft SPICE ephemeris data @@ -326,11 +384,11 @@ def singleGravityBody(show_plots): # separate from the earlier SPICE setup that was loaded to BSK. This is why # all required SPICE libraries must be included when setting up and loading # SPICE kernals in Python. - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de430.bsp') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/naif0012.tls') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de-403-masses.tpc') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/pck00010.tpc') - pyswice.furnsh_c(path + '/hst_edited.bsp') + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/de430.bsp") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/naif0012.tls") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/de-403-masses.tpc") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/pck00010.tpc") + pyswice.furnsh_c(path + "/hst_edited.bsp") SpiceObject.UTCCalInit = "2012 MAY 1 00:02:00.184" stringCurrent = SpiceObject.UTCCalInit @@ -347,71 +405,93 @@ def singleGravityBody(show_plots): gravBody1.registerProperties(newManager) SpiceObject.UpdateState(0) - for i in range(2*3600): - stateOut = spkRead('HUBBLE SPACE TELESCOPE', stringCurrent, 'J2000', 'EARTH') - etPrev =etCurr - 2.0 - stringPrev = pyswice.et2utc_c(etPrev, 'C', 4, 1024, "Yo") - statePrev = spkRead('HUBBLE SPACE TELESCOPE', stringPrev, 'J2000', 'EARTH') - etNext =etCurr + 2.0 - stringNext = pyswice.et2utc_c(etNext, 'C', 4, 1024, "Yo") - stateNext = spkRead('HUBBLE SPACE TELESCOPE', stringNext, 'J2000', 'EARTH') - gravVec = (stateNext[3:6] - statePrev[3:6])/(etNext - etPrev) + for i in range(2 * 3600): + stateOut = spkRead("HUBBLE SPACE TELESCOPE", stringCurrent, "J2000", "EARTH") + etPrev = etCurr - 2.0 + stringPrev = pyswice.et2utc_c(etPrev, "C", 4, 1024, "Yo") + statePrev = spkRead("HUBBLE SPACE TELESCOPE", stringPrev, "J2000", "EARTH") + etNext = etCurr + 2.0 + stringNext = pyswice.et2utc_c(etNext, "C", 4, 1024, "Yo") + stateNext = spkRead("HUBBLE SPACE TELESCOPE", stringNext, "J2000", "EARTH") + gravVec = (stateNext[3:6] - statePrev[3:6]) / (etNext - etPrev) normVec.append(np.linalg.norm(stateOut[0:3])) - stateOut*=1000.0 - SpiceObject.J2000Current = etCurr;SpiceObject.UpdateState(0) + stateOut *= 1000.0 + SpiceObject.J2000Current = etCurr + SpiceObject.UpdateState(0) gravBody1.loadEphemeris() - gravOut = gravBody1.computeGravityInertial(stateOut[0:3].reshape(3,1).tolist(), 0) - gravErrNorm.append(np.linalg.norm(gravVec*1000.0 - np.array(gravOut).reshape(3))/ - np.linalg.norm(gravVec*1000.0)) + gravOut = gravBody1.computeGravityInertial( + stateOut[0:3].reshape(3, 1).tolist(), 0 + ) + gravErrNorm.append( + np.linalg.norm(gravVec * 1000.0 - np.array(gravOut).reshape(3)) + / np.linalg.norm(gravVec * 1000.0) + ) pyswice.str2et_c(stringCurrent, et) etCurr = pyswice.doubleArray_getitem(et, 0) - etCurr += dt; - stringCurrent = pyswice.et2utc_c(etCurr, 'C', 4, 1024, "Yo") + etCurr += dt + stringCurrent = pyswice.et2utc_c(etCurr, "C", 4, 1024, "Yo") accuracy = 1.0e-4 for gravErr in gravErrNorm: if gravErr > accuracy: testFailCount += 1 - testMessages.append("Gravity numerical error too high for kernel comparison") + testMessages.append( + "Gravity numerical error too high for kernel comparison" + ) break - snippetName = testCase + 'Accuracy' - snippetContent = '{:1.1e}'.format(accuracy) # write formatted LATEX string to file to be used by auto-documentation. - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) #write formatted LATEX string to file to be used by auto-documentation. - - pyswice.unload_c(bskPath + '/supportData/EphemerisData/de430.bsp') - pyswice.unload_c(bskPath + '/supportData/EphemerisData/naif0012.tls') - pyswice.unload_c(bskPath + '/supportData/EphemerisData/de-403-masses.tpc') - pyswice.unload_c(bskPath + '/supportData/EphemerisData/pck00010.tpc') - pyswice.unload_c(path + '/hst_edited.bsp') - + snippetName = testCase + "Accuracy" + snippetContent = "{:1.1e}".format( + accuracy + ) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. + + pyswice.unload_c(bskPath + "/supportData/EphemerisData/de430.bsp") + pyswice.unload_c(bskPath + "/supportData/EphemerisData/naif0012.tls") + pyswice.unload_c(bskPath + "/supportData/EphemerisData/de-403-masses.tpc") + pyswice.unload_c(bskPath + "/supportData/EphemerisData/pck00010.tpc") + pyswice.unload_c(path + "/hst_edited.bsp") if testFailCount == 0: - passFailText = 'PASSED' + passFailText = "PASSED" print("PASSED: " + "Single-body with Spherical Harmonics") - colorText = 'ForestGreen' # color to write auto-documented "PASSED" message in in LATEX - snippetName = testCase + 'FailMsg' + colorText = ( + "ForestGreen" # color to write auto-documented "PASSED" message in in LATEX + ) + snippetName = testCase + "FailMsg" snippetContent = "" - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. else: - passFailText = 'FAILED' - colorText = 'Red' # color to write auto-documented "FAILED" message in in LATEX - snippetName = testCase + 'FailMsg' + passFailText = "FAILED" + colorText = "Red" # color to write auto-documented "FAILED" message in in LATEX + snippetName = testCase + "FailMsg" snippetContent = passFailText for message in testMessages: snippetContent += ". " + message snippetContent += "." - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) # write formatted LATEX string to file to be used by auto-documentation. - snippetName = testCase + 'PassFail' # name of file to be written for auto-documentation which specifies if this test was passed or failed. - snippetContent = r'\textcolor{' + colorText + '}{' + passFailText + '}' #write formatted LATEX string to file to be used by auto-documentation. - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) #write formatted LATEX string to file to be used by auto-documentation. - + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. + snippetName = ( + testCase + "PassFail" + ) # name of file to be written for auto-documentation which specifies if this test was passed or failed. + snippetContent = ( + r"\textcolor{" + colorText + "}{" + passFailText + "}" + ) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def register(manager): """ @@ -423,7 +503,7 @@ def register(manager): positionName = "hubPosition" stateDim = [3, 1] posState = manager.registerState(stateDim[0], stateDim[1], positionName) - posVelSig = [[0.], [0.], [0.]] + posVelSig = [[0.0], [0.0], [0.0]] posState.setState(posVelSig) velocityName = "hubVelocity" stateDim = [3, 1] @@ -439,8 +519,9 @@ def register(manager): return + def multiBodyGravity(show_plots): - testCase = 'multiBody' #for AutoTeX stuff + testCase = "multiBody" # for AutoTeX stuff # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. @@ -459,54 +540,59 @@ def multiBodyGravity(show_plots): # DynUnitTestProc = multiSim.CreateNewProcess(unitProcessName) # # create the dynamics task and specify the integration update time - DynUnitTestProc.addTask(multiSim.CreateNewTask(unitTaskName, macros.sec2nano(1000.0))) - - #Create dynParamManager to feed fake spacecraft data to so that the gravityEffector can access it. - #This places the spacecraft at the coordinate frame origin so that planets can be placed around it. - #velocity and attitude are just set to zero. - #center of mass and time are set to zero. + DynUnitTestProc.addTask( + multiSim.CreateNewTask(unitTaskName, macros.sec2nano(1000.0)) + ) + + # Create dynParamManager to feed fake spacecraft data to so that the gravityEffector can access it. + # This places the spacecraft at the coordinate frame origin so that planets can be placed around it. + # velocity and attitude are just set to zero. + # center of mass and time are set to zero. newManager = stateArchitecture.DynParamManager() register(newManager) - - #Create a message struct to place gravBody1 where it is wanted + # Create a message struct to place gravBody1 where it is wanted localPlanetEditor = messaging.SpicePlanetStateMsgPayload() - localPlanetEditor.PositionVector = [om.AU/10., 0., 0.] - localPlanetEditor.VelocityVector = [0., 0., 0.] + localPlanetEditor.PositionVector = [om.AU / 10.0, 0.0, 0.0] + localPlanetEditor.VelocityVector = [0.0, 0.0, 0.0] localPlanetEditor.J20002Pfix = np.identity(3) - #Grav Body 1 is twice the size of the other two + # Grav Body 1 is twice the size of the other two gravBody1 = gravityEffector.GravBodyData() gravBody1.planetName = "gravBody1_planet_data" - gravBody1.mu = 1000000. - gravBody1.radEquator = 6500. + gravBody1.mu = 1000000.0 + gravBody1.radEquator = 6500.0 gravBody1.isCentralBody = False gravBody1.localPlanet = localPlanetEditor - #This is the gravityEffector which will actually compute the gravitational acceleration + # This is the gravityEffector which will actually compute the gravitational acceleration allGrav = gravityEffector.GravityEffector() allGrav.gravBodies = gravityEffector.GravBodyVector([gravBody1]) allGrav.linkInStates(newManager) allGrav.registerProperties(newManager) allGrav.Reset(0) multiSim.AddModelToTask(unitTaskName, allGrav) - posVelSig = [[0.], [0.], [0.]] - allGrav.computeGravityField(posVelSig, posVelSig) #compute acceleration only considering the first body. - step1 = newManager.getPropertyReference(allGrav.vehicleGravityPropName) #retrieve total gravitational acceleration in inertial frame - - #Create a message struct to place gravBody2&3 where they are wanted. - localPlanetEditor.PositionVector = [-om.AU/10., 0., 0.] - localPlanetEditor.VelocityVector = [0., 0., 0.] + posVelSig = [[0.0], [0.0], [0.0]] + allGrav.computeGravityField( + posVelSig, posVelSig + ) # compute acceleration only considering the first body. + step1 = newManager.getPropertyReference( + allGrav.vehicleGravityPropName + ) # retrieve total gravitational acceleration in inertial frame + + # Create a message struct to place gravBody2&3 where they are wanted. + localPlanetEditor.PositionVector = [-om.AU / 10.0, 0.0, 0.0] + localPlanetEditor.VelocityVector = [0.0, 0.0, 0.0] - #grav Body 2 and 3 are coincident with each other, half the mass of gravBody1 and are in the opposite direction of gravBody1 + # grav Body 2 and 3 are coincident with each other, half the mass of gravBody1 and are in the opposite direction of gravBody1 gravBody2 = gravityEffector.GravBodyData() gravBody2.planetName = "gravBody2_planet_data" - gravBody2.mu = gravBody1.mu/2. - gravBody2.radEquator = 6500. + gravBody2.mu = gravBody1.mu / 2.0 + gravBody2.radEquator = 6500.0 gravBody2.isCentralBody = False gravBody2.localPlanet = localPlanetEditor - #This is the gravityEffector which will actually compute the gravitational acceleration + # This is the gravityEffector which will actually compute the gravitational acceleration newManager = stateArchitecture.DynParamManager() register(newManager) allGrav2 = gravityEffector.GravityEffector() @@ -515,66 +601,102 @@ def multiBodyGravity(show_plots): allGrav2.registerProperties(newManager) allGrav2.Reset(0) multiSim.AddModelToTask(unitTaskName, allGrav2) - allGrav2.computeGravityField(posVelSig, posVelSig) #compute acceleration considering the first and second bodies. - step2 = newManager.getPropertyReference(allGrav2.vehicleGravityPropName) #retrieve total gravitational acceleration in inertial frame + allGrav2.computeGravityField( + posVelSig, posVelSig + ) # compute acceleration considering the first and second bodies. + step2 = newManager.getPropertyReference( + allGrav2.vehicleGravityPropName + ) # retrieve total gravitational acceleration in inertial frame # grav Body 2 and 3 are coincident with each other, half the mass of gravBody1 and are in the opposite direction of gravBody1 gravBody3 = gravityEffector.GravBodyData() gravBody3.planetName = "gravBody3_planet_data" gravBody3.mu = gravBody2.mu - gravBody3.radEquator = 6500. + gravBody3.radEquator = 6500.0 gravBody3.isCentralBody = False gravBody3.localPlanet = localPlanetEditor - #This is the gravityEffector which will actually compute the gravitational acceleration + # This is the gravityEffector which will actually compute the gravitational acceleration newManager = stateArchitecture.DynParamManager() register(newManager) allGrav3 = gravityEffector.GravityEffector() - allGrav3.gravBodies = gravityEffector.GravBodyVector([gravBody1, gravBody2, gravBody3]) + allGrav3.gravBodies = gravityEffector.GravBodyVector( + [gravBody1, gravBody2, gravBody3] + ) allGrav3.linkInStates(newManager) allGrav3.registerProperties(newManager) allGrav3.Reset(0) multiSim.AddModelToTask(unitTaskName, allGrav3) - allGrav3.computeGravityField(posVelSig, posVelSig) #comput acceleration considering all three bodies - step3 = newManager.getPropertyReference(allGrav3.vehicleGravityPropName) #retrieve total gravitational acceleration in inertial frame - - step3 = [0., step3[0][0], step3[1][0], step3[2][0]] #add a first (time) column to use isArrayZero - - #Test results for accuracy + allGrav3.computeGravityField( + posVelSig, posVelSig + ) # comput acceleration considering all three bodies + step3 = newManager.getPropertyReference( + allGrav3.vehicleGravityPropName + ) # retrieve total gravitational acceleration in inertial frame + + step3 = [ + 0.0, + step3[0][0], + step3[1][0], + step3[2][0], + ] # add a first (time) column to use isArrayZero + + # Test results for accuracy accuracy = 1e-12 - snippetName = testCase + 'Accuracy' - snippetContent = '{:1.1e}'.format(accuracy) # write formatted LATEX string to file to be used by auto-documentation. - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) #write formatted LATEX string to file to be used by auto-documentation. - if not unitTestSupport.isDoubleEqualRelative(step2[0][0]/step1[0][0], 0.5, accuracy): #if the second grav body doesn't cancel exactly half of the first body's acceleration. + snippetName = testCase + "Accuracy" + snippetContent = "{:1.1e}".format( + accuracy + ) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. + if not unitTestSupport.isDoubleEqualRelative( + step2[0][0] / step1[0][0], 0.5, accuracy + ): # if the second grav body doesn't cancel exactly half of the first body's acceleration. testFailCount += 1 passFailText = "Step 2 was not half of step 1" testMessages.append(passFailText) - elif not unitTestSupport.isArrayZero(step3, 3, accuracy): #if the net acceleration is not now 0. + elif not unitTestSupport.isArrayZero( + step3, 3, accuracy + ): # if the net acceleration is not now 0. testFailCount += 1 passFailText = "Step 3 did not cause gravity to return to 0" testMessages.append(passFailText) - #Record test results to LaTeX + # Record test results to LaTeX if testFailCount == 0: - passFailText = 'PASSED' + passFailText = "PASSED" print("PASSED: " + " Multi-Body") - colorText = 'ForestGreen' # color to write auto-documented "PASSED" message in in LATEX - snippetName = testCase + 'FailMsg' + colorText = ( + "ForestGreen" # color to write auto-documented "PASSED" message in in LATEX + ) + snippetName = testCase + "FailMsg" snippetContent = "" - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. else: - passFailText = 'FAILED' - colorText = 'Red' # color to write auto-documented "FAILED" message in in LATEX - snippetName = testCase + 'FailMsg' + passFailText = "FAILED" + colorText = "Red" # color to write auto-documented "FAILED" message in in LATEX + snippetName = testCase + "FailMsg" snippetContent = passFailText for message in testMessages: snippetContent += ". " + message snippetContent += "." - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) # write formatted LATEX string to file to be used by auto-documentation. - snippetName = testCase + 'PassFail' # name of file to be written for auto-documentation which specifies if this test was passed or failed. - snippetContent = r'\textcolor{' + colorText + '}{' + passFailText + '}' #write formatted LATEX string to file to be used by auto-documentation. - unitTestSupport.writeTeXSnippet(snippetName, snippetContent, path) #write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. + snippetName = ( + testCase + "PassFail" + ) # name of file to be written for auto-documentation which specifies if this test was passed or failed. + snippetContent = ( + r"\textcolor{" + colorText + "}{" + passFailText + "}" + ) # write formatted LATEX string to file to be used by auto-documentation. + unitTestSupport.writeTeXSnippet( + snippetName, snippetContent, path + ) # write formatted LATEX string to file to be used by auto-documentation. + + return [testFailCount, "".join(testMessages)] - return [testFailCount, ''.join(testMessages)] if __name__ == "__main__": # test_gravityEffectorAllTest(False) diff --git a/src/simulation/dynamics/gravityEffector/_UnitTest/test_gravitySpacecraft.py b/src/simulation/dynamics/gravityEffector/_UnitTest/test_gravitySpacecraft.py index 9f43b12f0c..bf07878b90 100644 --- a/src/simulation/dynamics/gravityEffector/_UnitTest/test_gravitySpacecraft.py +++ b/src/simulation/dynamics/gravityEffector/_UnitTest/test_gravitySpacecraft.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -25,6 +24,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) from Basilisk import __path__ + bskPath = __path__[0] from Basilisk.utilities import macros @@ -39,15 +39,15 @@ from Basilisk.utilities import simIncludeGravBody import pytest + # uncomment this line is 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() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("function", ["singleGravityBody" - , "multiBodyGravity" - , "polyGravityBody" - ]) +@pytest.mark.parametrize( + "function", ["singleGravityBody", "multiBodyGravity", "polyGravityBody"] +) def test_gravityEffectorAllTest(show_plots, function): """Module Unit Test""" testFunction = globals().get(function) @@ -81,21 +81,28 @@ def singleGravityBody(show_plots): DynUnitTestProc = unitTestSim.CreateNewProcess(unitProcessName) # create the dynamics task and specify the integration update time - DynUnitTestProc.addTask(unitTestSim.CreateNewTask(unitTaskName, macros.sec2nano(10.0))) - + DynUnitTestProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, macros.sec2nano(10.0)) + ) # setup Gravity Bodies gravFactory = simIncludeGravBody.gravBodyFactory() - gravBodies = gravFactory.createBodies(['earth', 'sun', 'moon', 'jupiter barycenter']) - gravBodies['earth'].isCentralBody = True - gravBodies['earth'].useSphericalHarmonicsGravityModel(path + '/../_UnitTest/GGM03S.txt' - , 40 - ) + gravBodies = gravFactory.createBodies( + ["earth", "sun", "moon", "jupiter barycenter"] + ) + gravBodies["earth"].isCentralBody = True + gravBodies["earth"].useSphericalHarmonicsGravityModel( + path + "/../_UnitTest/GGM03S.txt", 40 + ) stringCurrent = "2016 MAY 1 00:32:30.0" - gravFactory.createSpiceInterface(bskPath +'/supportData/EphemerisData/', stringCurrent) - gravFactory.spiceObject.zeroBase = 'Earth' + gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", stringCurrent + ) + gravFactory.spiceObject.zeroBase = "Earth" - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) unitTestSim.AddModelToTask(unitTaskName, gravFactory.spiceObject, 10) @@ -104,23 +111,23 @@ def singleGravityBody(show_plots): # separate from the earlier SPICE setup that was loaded to BSK. This is why # all required SPICE libraries must be included when setting up and loading # SPICE kernels in Python. - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de430.bsp') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/naif0012.tls') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de-403-masses.tpc') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/pck00010.tpc') - pyswice.furnsh_c(path + '/../_UnitTest/hst_edited.bsp') + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/de430.bsp") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/naif0012.tls") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/de-403-masses.tpc") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/pck00010.tpc") + pyswice.furnsh_c(path + "/../_UnitTest/hst_edited.bsp") unitTestSim.AddModelToTask(unitTaskName, scObject, 9) - stateOut = spkRead('HUBBLE SPACE TELESCOPE', stringCurrent, 'J2000', 'EARTH') + stateOut = spkRead("HUBBLE SPACE TELESCOPE", stringCurrent, "J2000", "EARTH") scObject.hub.mHub = 100 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] - scObject.hub.r_CN_NInit = (1000.0*stateOut[0:3].reshape(3,1)).tolist() - velStart = 1000.0*stateOut[3:6] - scObject.hub.v_CN_NInit = (velStart.reshape(3,1)).tolist() + scObject.hub.r_CN_NInit = (1000.0 * stateOut[0:3].reshape(3, 1)).tolist() + velStart = 1000.0 * stateOut[3:6] + scObject.hub.v_CN_NInit = (velStart.reshape(3, 1)).tolist() scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] scObject.hub.omega_BN_BInit = [[0.001], [-0.002], [0.003]] @@ -139,53 +146,63 @@ def singleGravityBody(show_plots): posArray = [] velArray = [] posError = [] - while(currentTime < totalTime): + while currentTime < totalTime: unitTestSim.ConfigureStopTime(macros.sec2nano(currentTime + dt)) unitTestSim.ExecuteSimulation() - stateOut = spkRead('HUBBLE SPACE TELESCOPE', gravFactory.spiceObject.getCurrentTimeString(), 'J2000', 'EARTH') + stateOut = spkRead( + "HUBBLE SPACE TELESCOPE", + gravFactory.spiceObject.getCurrentTimeString(), + "J2000", + "EARTH", + ) posCurr = posRef.getState() posCurr = [y for x in posCurr for y in x] posArray.append(posCurr) velCurr = velRef.getState() velCurr = [y for x in velCurr for y in x] velArray.append(velCurr) - posDiff = numpy.array(posCurr) - stateOut[0:3]*1000.0 - posRow = [unitTestSim.TotalSim.CurrentNanos*1.0E-9] + posDiff = numpy.array(posCurr) - stateOut[0:3] * 1000.0 + posRow = [unitTestSim.TotalSim.CurrentNanos * 1.0e-9] posRow.extend(posDiff.tolist()) posError.append(posRow) assert numpy.linalg.norm(posDiff) < 1000.0 currentTime += dt - stateOut = spkRead('HUBBLE SPACE TELESCOPE', gravFactory.spiceObject.getCurrentTimeString(), 'J2000', 'EARTH') + stateOut = spkRead( + "HUBBLE SPACE TELESCOPE", + gravFactory.spiceObject.getCurrentTimeString(), + "J2000", + "EARTH", + ) posArray = numpy.array(posArray) posError = numpy.array(posError) gravFactory.unloadSpiceKernels() - pyswice.unload_c(bskPath + '/supportData/EphemerisData/de430.bsp') - pyswice.unload_c(bskPath + '/supportData/EphemerisData/naif0012.tls') - pyswice.unload_c(bskPath + '/supportData/EphemerisData/de-403-masses.tpc') - pyswice.unload_c(bskPath + '/supportData/EphemerisData/pck00010.tpc') - pyswice.unload_c(path + '/../_UnitTest/hst_edited.bsp') + pyswice.unload_c(bskPath + "/supportData/EphemerisData/de430.bsp") + pyswice.unload_c(bskPath + "/supportData/EphemerisData/naif0012.tls") + pyswice.unload_c(bskPath + "/supportData/EphemerisData/de-403-masses.tpc") + pyswice.unload_c(bskPath + "/supportData/EphemerisData/pck00010.tpc") + pyswice.unload_c(path + "/../_UnitTest/hst_edited.bsp") - print(numpy.max(abs(posError[:,1:4]))) + print(numpy.max(abs(posError[:, 1:4]))) if show_plots: plt.close("all") plt.figure() plt.plot(posError[:, 0], posError[:, 1:4]) - plt.xlabel('Time (s)') - plt.ylabel('Position Difference (m)') + plt.xlabel("Time (s)") + plt.ylabel("Position Difference (m)") plt.show() plt.close("all") - if testFailCount == 0: print("PASSED: " + " Single body with spherical harmonics") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def multiBodyGravity(show_plots): # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -208,18 +225,26 @@ def multiBodyGravity(show_plots): DynUnitTestProc = unitTestSim.CreateNewProcess(unitProcessName) # create the dynamics task and specify the integration update time - DynUnitTestProc.addTask(unitTestSim.CreateNewTask(unitTaskName, macros.sec2nano(5.0))) + DynUnitTestProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, macros.sec2nano(5.0)) + ) # setup Gravity Bodies gravFactory = simIncludeGravBody.gravBodyFactory() - gravBodies = gravFactory.createBodies(['earth', 'mars barycenter', 'sun', 'moon', 'jupiter barycenter']) - gravBodies['sun'].isCentralBody = True + gravBodies = gravFactory.createBodies( + ["earth", "mars barycenter", "sun", "moon", "jupiter barycenter"] + ) + gravBodies["sun"].isCentralBody = True stringCurrent = "2008 September 19, 04:00:00.0" - gravFactory.createSpiceInterface(bskPath +'/supportData/EphemerisData/', stringCurrent) - gravFactory.spiceObject.zeroBase = 'Earth' + gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", stringCurrent + ) + gravFactory.spiceObject.zeroBase = "Earth" - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) unitTestSim.AddModelToTask(unitTaskName, gravFactory.spiceObject, 10) @@ -228,22 +253,22 @@ def multiBodyGravity(show_plots): # separate from the earlier SPICE setup that was loaded to BSK. This is why # all required SPICE libraries must be included when setting up and loading # SPICE kernels in Python. - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de430.bsp') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/naif0012.tls') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/de-403-masses.tpc') - pyswice.furnsh_c(bskPath + '/supportData/EphemerisData/pck00010.tpc') - pyswice.furnsh_c(path + '/../_UnitTest/nh_pred_od077.bsp') + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/de430.bsp") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/naif0012.tls") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/de-403-masses.tpc") + pyswice.furnsh_c(bskPath + "/supportData/EphemerisData/pck00010.tpc") + pyswice.furnsh_c(path + "/../_UnitTest/nh_pred_od077.bsp") unitTestSim.AddModelToTask(unitTaskName, scObject, 9) - stateOut = spkRead('NEW HORIZONS', stringCurrent, 'J2000', 'SUN') + stateOut = spkRead("NEW HORIZONS", stringCurrent, "J2000", "SUN") scObject.hub.mHub = 100 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] - scObject.hub.r_CN_NInit = (1000.0*stateOut[0:3].reshape(3,1)).tolist() - velStart = 1000.0*stateOut[3:6] - scObject.hub.v_CN_NInit = (velStart.reshape(3,1)).tolist() + scObject.hub.r_CN_NInit = (1000.0 * stateOut[0:3].reshape(3, 1)).tolist() + velStart = 1000.0 * stateOut[3:6] + scObject.hub.v_CN_NInit = (velStart.reshape(3, 1)).tolist() scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] scObject.hub.omega_BN_BInit = [[0.001], [-0.002], [0.003]] @@ -262,53 +287,57 @@ def multiBodyGravity(show_plots): while currentTime < totalTime: unitTestSim.ConfigureStopTime(macros.sec2nano(currentTime + dt)) unitTestSim.ExecuteSimulation() - timeString = pyswice.et2utc_c(gravFactory.spiceObject.J2000Current, 'C', 4, 1024, "Yo") - stateOut = spkRead('NEW HORIZONS', timeString, 'J2000', 'SUN') + timeString = pyswice.et2utc_c( + gravFactory.spiceObject.J2000Current, "C", 4, 1024, "Yo" + ) + stateOut = spkRead("NEW HORIZONS", timeString, "J2000", "SUN") posCurr = posRef.getState() posCurr = [y for x in posCurr for y in x] posArray.append(posCurr) velCurr = velRef.getState() velCurr = [y for x in velCurr for y in x] velArray.append(velCurr) - posDiff = numpy.array(posCurr) - stateOut[0:3]*1000.0 - posRow = [unitTestSim.TotalSim.CurrentNanos*1.0E-9] + posDiff = numpy.array(posCurr) - stateOut[0:3] * 1000.0 + posRow = [unitTestSim.TotalSim.CurrentNanos * 1.0e-9] posRow.extend(posDiff.tolist()) posError.append(posRow) assert numpy.linalg.norm(posDiff) < 1000.0 - if currentTime > 0.0 + dt/2.0: - posJump = stateOut[0:3]*1000.0 - numpy.array(posPrevious) + if currentTime > 0.0 + dt / 2.0: + posJump = stateOut[0:3] * 1000.0 - numpy.array(posPrevious) posInc.append(posJump.tolist()) - posPrevious = stateOut[0:3]*1000.0 + posPrevious = stateOut[0:3] * 1000.0 currentTime += dt - stateOut = spkRead('NEW HORIZONS', gravFactory.spiceObject.getCurrentTimeString(), 'J2000', 'SUN') + stateOut = spkRead( + "NEW HORIZONS", gravFactory.spiceObject.getCurrentTimeString(), "J2000", "SUN" + ) posArray = numpy.array(posArray) posError = numpy.array(posError) posInc = numpy.array(posInc) gravFactory.unloadSpiceKernels() - pyswice.unload_c(bskPath + '/supportData/EphemerisData/de430.bsp') - pyswice.unload_c(bskPath + '/supportData/EphemerisData/naif0012.tls') - pyswice.unload_c(bskPath + '/supportData/EphemerisData/de-403-masses.tpc') - pyswice.unload_c(bskPath + '/supportData/EphemerisData/pck00010.tpc') - pyswice.unload_c(path + '/../_UnitTest/nh_pred_od077.bsp') + pyswice.unload_c(bskPath + "/supportData/EphemerisData/de430.bsp") + pyswice.unload_c(bskPath + "/supportData/EphemerisData/naif0012.tls") + pyswice.unload_c(bskPath + "/supportData/EphemerisData/de-403-masses.tpc") + pyswice.unload_c(bskPath + "/supportData/EphemerisData/pck00010.tpc") + pyswice.unload_c(path + "/../_UnitTest/nh_pred_od077.bsp") plt.close("all") plt.figure() - plt.plot(posError[:,0], posError[:,1:4]) - plt.xlabel('Time (s)') - plt.ylabel('Position Difference (m)') + plt.plot(posError[:, 0], posError[:, 1:4]) + plt.xlabel("Time (s)") + plt.ylabel("Position Difference (m)") - if(show_plots): + if show_plots: plt.show() - plt.close('all') + plt.close("all") if testFailCount == 0: print("PASSED: " + " multi-point source bodies") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def polyGravityBody(show_plots): @@ -322,10 +351,10 @@ def polyGravityBody(show_plots): testMessages = [] # create empty list to store test log messages # Obtain validation data (simulation with tight integration tolerances in MATLAB) - valData = numpy.genfromtxt(path + '/../_UnitTest/polyTestData.csv', delimiter=',') - tVal = numpy.array(valData[:,0]) - posVal = numpy.array(valData[:,1:4]) - velVal = numpy.array(valData[:,4:7]) + valData = numpy.genfromtxt(path + "/../_UnitTest/polyTestData.csv", delimiter=",") + tVal = numpy.array(valData[:, 0]) + posVal = numpy.array(valData[:, 1:4]) + velVal = numpy.array(valData[:, 4:7]) # Create a sim module as an empty container unitTaskName = "unitTask" # arbitrary name (don't change) @@ -337,16 +366,18 @@ def polyGravityBody(show_plots): DynUnitTestProc = unitTestSim.CreateNewProcess(unitProcessName) # create the dynamics task and specify the integration update time intTime = 30.0 - DynUnitTestProc.addTask(unitTestSim.CreateNewTask(unitTaskName, macros.sec2nano(intTime))) + DynUnitTestProc.addTask( + unitTestSim.CreateNewTask(unitTaskName, macros.sec2nano(intTime)) + ) # specify orbit of polyhedral body oePolyBody = planetEphemeris.ClassicElements() oePolyBody.a = 2.3612 * orbitalMotion.AU * 1000 oePolyBody.e = 0 - oePolyBody.i = 0*macros.D2R - oePolyBody.Omega = 0*macros.D2R - oePolyBody.omega = 0*macros.D2R - oePolyBody.f = 0*macros.D2R + oePolyBody.i = 0 * macros.D2R + oePolyBody.Omega = 0 * macros.D2R + oePolyBody.omega = 0 * macros.D2R + oePolyBody.f = 0 * macros.D2R raPolyBody = 0 * macros.D2R decPolyBody = 90 * macros.D2R @@ -355,7 +386,7 @@ def polyGravityBody(show_plots): # setup celestial object ephemeris module polyBodyEphem = planetEphemeris.PlanetEphemeris() - polyBodyEphem.ModelTag = 'erosEphemeris' + polyBodyEphem.ModelTag = "erosEphemeris" polyBodyEphem.setPlanetNames(planetEphemeris.StringVector(["eros"])) # specify celestial objects orbit @@ -365,14 +396,16 @@ def polyGravityBody(show_plots): polyBodyEphem.rightAscension = planetEphemeris.DoubleVector([raPolyBody]) polyBodyEphem.declination = planetEphemeris.DoubleVector([decPolyBody]) polyBodyEphem.lst0 = planetEphemeris.DoubleVector([lst0PolyBody]) - polyBodyEphem.rotRate = planetEphemeris.DoubleVector([360 * macros.D2R / rotPeriodPolyBody]) + polyBodyEphem.rotRate = planetEphemeris.DoubleVector( + [360 * macros.D2R / rotPeriodPolyBody] + ) # setup polyhedral gravity body mu = 4.46275472004 * 1e5 gravFactory = simIncludeGravBody.gravBodyFactory() - polyBody = gravFactory.createCustomGravObject('eros', mu=mu) + polyBody = gravFactory.createCustomGravObject("eros", mu=mu) polyBody.isCentralBody = True - polyBody.usePolyhedralGravityModel(path + '/../_UnitTest/EROS856Vert1708Fac.txt') + polyBody.usePolyhedralGravityModel(path + "/../_UnitTest/EROS856Vert1708Fac.txt") polyBody.planetBodyInMsg.subscribeTo(polyBodyEphem.planetOutMsgs[0]) # create an ephemeris converter @@ -383,12 +416,14 @@ def polyGravityBody(show_plots): # create spacecraft and attach polyhedral body scObject = spacecraft.Spacecraft() scObject.ModelTag = "spacecraft" - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # set initial conditions for spacecraft - angvelPolyBody = np.array([0,0,360 * macros.D2R / rotPeriodPolyBody]) - posInit = posVal[0,0:3] - velInit = velVal[0,0:3] + np.cross(angvelPolyBody, posInit) + angvelPolyBody = np.array([0, 0, 360 * macros.D2R / rotPeriodPolyBody]) + posInit = posVal[0, 0:3] + velInit = velVal[0, 0:3] + np.cross(angvelPolyBody, posInit) scObject.hub.r_CN_NInit = posInit.tolist() scObject.hub.v_CN_NInit = velInit.tolist() @@ -397,11 +432,13 @@ def polyGravityBody(show_plots): unitTestSim.AddModelToTask(unitTaskName, polyBodyEphemConverter, ModelPriority=9) unitTestSim.AddModelToTask(unitTaskName, scObject, ModelPriority=8) - totalTime = 24*3600 + totalTime = 24 * 3600 samplingTime = 300 scRec = scObject.scStateOutMsg.recorder(macros.sec2nano(samplingTime)) - polyBodyRec = polyBodyEphemConverter.ephemOutMsgs[0].recorder(macros.sec2nano(samplingTime)) + polyBodyRec = polyBodyEphemConverter.ephemOutMsgs[0].recorder( + macros.sec2nano(samplingTime) + ) unitTestSim.AddModelToTask(unitTaskName, scRec) unitTestSim.AddModelToTask(unitTaskName, polyBodyRec) @@ -423,29 +460,29 @@ def polyGravityBody(show_plots): R_AN = RigidBodyKinematics.MRP2C(sigma_AN[ii][0:3]) # rotate position and velocity - posArray[ii,0:3] = R_AN.dot(numpy.subtract(r_BN_N[ii],r_AN_N[ii])) + posArray[ii, 0:3] = R_AN.dot(numpy.subtract(r_BN_N[ii], r_AN_N[ii])) # compute error in position and assert max error - posError = numpy.linalg.norm(posArray - posVal,axis=1) + posError = numpy.linalg.norm(posArray - posVal, axis=1) assert max(posError) < 10 print(max(posError)) plt.close("all") plt.figure() plt.plot(tVal, posArray - posVal) - plt.xlabel('Time (s)') - plt.ylabel('Position Difference (m)') + plt.xlabel("Time (s)") + plt.ylabel("Position Difference (m)") - if(show_plots): + if show_plots: plt.show() - plt.close('all') + plt.close("all") if testFailCount == 0: print("PASSED: " + " Single body with polyhedral shape") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": diff --git a/src/simulation/dynamics/gravityEffector/gravCoeffOps.py b/src/simulation/dynamics/gravityEffector/gravCoeffOps.py index f0a4828acd..0bdc7537b0 100755 --- a/src/simulation/dynamics/gravityEffector/gravCoeffOps.py +++ b/src/simulation/dynamics/gravityEffector/gravCoeffOps.py @@ -19,12 +19,10 @@ from Basilisk import __path__ -def loadGravFromFile( - fileName: str, - spherHarm: "SphericalHarmonicsGravityModel", - maxDeg: int = 2 - ): +def loadGravFromFile( + fileName: str, spherHarm: "SphericalHarmonicsGravityModel", maxDeg: int = 2 +): [clmList, slmList, mu, radEquator] = loadGravFromFileToList(fileName, maxDeg=2) spherHarm.muBody = mu @@ -33,9 +31,10 @@ def loadGravFromFile( spherHarm.sBar = slmList spherHarm.maxDeg = maxDeg + def loadGravFromFileToList(fileName: str, maxDeg: int = 2): - with open(fileName, 'r') as csvfile: - gravReader = csv.reader(csvfile, delimiter=',') + with open(fileName, "r") as csvfile: + gravReader = csv.reader(csvfile, delimiter=",") firstRow = next(gravReader) clmList = [] slmList = [] @@ -50,29 +49,36 @@ def loadGravFromFileToList(fileName: str, maxDeg: int = 2): refLong = float(firstRow[6]) refLat = float(firstRow[7]) except Exception as ex: - raise ValueError("File is not in the expected JPL format for " - "spherical Harmonics", ex) + raise ValueError( + "File is not in the expected JPL format for spherical Harmonics", ex + ) if maxDegreeFile < maxDeg or maxOrderFile < maxDeg: - raise ValueError(f"Requested using Spherical Harmonics of degree {maxDeg}" - f", but file '{fileName}' has maximum degree/order of" - f"{min(maxDegreeFile, maxOrderFile)}") + raise ValueError( + f"Requested using Spherical Harmonics of degree {maxDeg}" + f", but file '{fileName}' has maximum degree/order of" + f"{min(maxDegreeFile, maxOrderFile)}" + ) if not coefficientsNormalized: - raise ValueError("Coefficients in given file are not normalized. This is " - "not currently supported in Basilisk.") + raise ValueError( + "Coefficients in given file are not normalized. This is " + "not currently supported in Basilisk." + ) if refLong != 0 or refLat != 0: - raise ValueError("Coefficients in given file use a reference longitude" - " or latitude that is not zero. This is not currently " - "supported in Basilisk.") + raise ValueError( + "Coefficients in given file use a reference longitude" + " or latitude that is not zero. This is not currently " + "supported in Basilisk." + ) clmRow = [] slmRow = [] currDeg = 0 for gravRow in gravReader: while int(gravRow[0]) > currDeg: - if (len(clmRow) < currDeg + 1): + if len(clmRow) < currDeg + 1: clmRow.extend([0.0] * (currDeg + 1 - len(clmRow))) slmRow.extend([0.0] * (currDeg + 1 - len(slmRow))) clmList.append(clmRow) @@ -91,17 +97,20 @@ def loadPolyFromFile(fileName: str, poly: "PolyhedralGravityModel"): poly.xyzVertex = vertList poly.orderFacet = faceList + def loadPolyFromFileToList(fileName: str): with open(fileName) as polyFile: - if fileName.endswith('.tab'): + if fileName.endswith(".tab"): try: - nVertex, nFacet = [int(x) for x in next(polyFile).split()] # read first line - fileType = 'gaskell' + nVertex, nFacet = [ + int(x) for x in next(polyFile).split() + ] # read first line + fileType = "gaskell" except: polyFile.seek(0) - fileType = 'pds3' + fileType = "pds3" - if fileType == 'gaskell': + if fileType == "gaskell": vertList = [] faceList = [] @@ -113,12 +122,20 @@ def loadPolyFromFileToList(fileName: str): arrtemp.append(float(x)) if contLines < nVertex: - vertList.append([float(arrtemp[1]*1e3),float(arrtemp[2]*1e3),float(arrtemp[3]*1e3)]) + vertList.append( + [ + float(arrtemp[1] * 1e3), + float(arrtemp[2] * 1e3), + float(arrtemp[3] * 1e3), + ] + ) else: - faceList.append([int(arrtemp[1]),int(arrtemp[2]),int(arrtemp[3])]) + faceList.append( + [int(arrtemp[1]), int(arrtemp[2]), int(arrtemp[3])] + ) contLines += 1 - elif fileType == 'pds3': + elif fileType == "pds3": nVertex = 0 nFacet = 0 vertList = [] @@ -126,13 +143,25 @@ def loadPolyFromFileToList(fileName: str): for line in polyFile: arrtemp = line.split() if arrtemp: - if arrtemp[0] == 'v': + if arrtemp[0] == "v": nVertex += 1 - vertList.append([float(arrtemp[1])*1e3, float(arrtemp[2])*1e3, float(arrtemp[3])*1e3]) - elif arrtemp[0] == 'f': + vertList.append( + [ + float(arrtemp[1]) * 1e3, + float(arrtemp[2]) * 1e3, + float(arrtemp[3]) * 1e3, + ] + ) + elif arrtemp[0] == "f": nFacet += 1 - faceList.append([int(arrtemp[1])+1, int(arrtemp[2])+1, int(arrtemp[3])+1]) - elif fileName.endswith('.obj'): + faceList.append( + [ + int(arrtemp[1]) + 1, + int(arrtemp[2]) + 1, + int(arrtemp[3]) + 1, + ] + ) + elif fileName.endswith(".obj"): nVertex = 0 nFacet = 0 vertList = [] @@ -140,14 +169,24 @@ def loadPolyFromFileToList(fileName: str): for line in polyFile: arrtemp = line.split() if arrtemp: - if arrtemp[0] == 'v': + if arrtemp[0] == "v": nVertex += 1 - vertList.append([float(arrtemp[1])*1e3, float(arrtemp[2])*1e3, float(arrtemp[3])*1e3]) - elif arrtemp[0] == 'f': + vertList.append( + [ + float(arrtemp[1]) * 1e3, + float(arrtemp[2]) * 1e3, + float(arrtemp[3]) * 1e3, + ] + ) + elif arrtemp[0] == "f": nFacet += 1 - faceList.append([int(arrtemp[1]), int(arrtemp[2]), int(arrtemp[3])]) - elif fileName.endswith('.txt'): - nVertex, nFacet = [int(x) for x in next(polyFile).split()] # read first line + faceList.append( + [int(arrtemp[1]), int(arrtemp[2]), int(arrtemp[3])] + ) + elif fileName.endswith(".txt"): + nVertex, nFacet = [ + int(x) for x in next(polyFile).split() + ] # read first line vertList = [] faceList = [] @@ -159,13 +198,21 @@ def loadPolyFromFileToList(fileName: str): arrtemp.append(float(x)) if contLines < nVertex: - vertList.append([float(arrtemp[0]*1e3),float(arrtemp[1]*1e3),float(arrtemp[2]*1e3)]) + vertList.append( + [ + float(arrtemp[0] * 1e3), + float(arrtemp[1] * 1e3), + float(arrtemp[2] * 1e3), + ] + ) else: - faceList.append([int(arrtemp[0]),int(arrtemp[1]),int(arrtemp[2])]) + faceList.append([int(arrtemp[0]), int(arrtemp[1]), int(arrtemp[2])]) contLines += 1 else: - raise ValueError("Unrecognized file extension. Valid extensions are " - "'.tab', '.obj', and '.txt'") + raise ValueError( + "Unrecognized file extension. Valid extensions are " + "'.tab', '.obj', and '.txt'" + ) return [vertList, faceList, nVertex, nFacet] diff --git a/src/simulation/dynamics/hingedRigidBodyMotor/_UnitTest/test_hingedRigidBodyMotor.py b/src/simulation/dynamics/hingedRigidBodyMotor/_UnitTest/test_hingedRigidBodyMotor.py index a8e259bd69..e677a9840b 100644 --- a/src/simulation/dynamics/hingedRigidBodyMotor/_UnitTest/test_hingedRigidBodyMotor.py +++ b/src/simulation/dynamics/hingedRigidBodyMotor/_UnitTest/test_hingedRigidBodyMotor.py @@ -26,13 +26,15 @@ @pytest.mark.parametrize("accuracy", [1e-12]) -@pytest.mark.parametrize("K", [5,10]) -@pytest.mark.parametrize("P", [1,2]) -@pytest.mark.parametrize("sensedTheta, sensedThetaDot, refTheta, refThetaDot", [ - (1,.1,1.2,.2), - (1,.1,.8,-.1) -]) -def test_hingedRigidBodyMotor(show_plots, K, P, sensedTheta, sensedThetaDot, refTheta, refThetaDot, accuracy): +@pytest.mark.parametrize("K", [5, 10]) +@pytest.mark.parametrize("P", [1, 2]) +@pytest.mark.parametrize( + "sensedTheta, sensedThetaDot, refTheta, refThetaDot", + [(1, 0.1, 1.2, 0.2), (1, 0.1, 0.8, -0.1)], +) +def test_hingedRigidBodyMotor( + show_plots, K, P, sensedTheta, sensedThetaDot, refTheta, refThetaDot, accuracy +): r""" **Validation Test Description** @@ -54,11 +56,15 @@ def test_hingedRigidBodyMotor(show_plots, K, P, sensedTheta, sensedThetaDot, ref K and P are varied (note both must be set to positive values). The sensed hinged rigid body state is held constant while the reference is also varied to check positive and negative deltas. """ - [testResults, testMessage] = hingedRigidBodyMotorTestFunction(show_plots, K, P, sensedTheta, sensedThetaDot, refTheta, refThetaDot, accuracy) + [testResults, testMessage] = hingedRigidBodyMotorTestFunction( + show_plots, K, P, sensedTheta, sensedThetaDot, refTheta, refThetaDot, accuracy + ) assert testResults < 1, testMessage -def hingedRigidBodyMotorTestFunction(show_plots, K, P, sensedTheta, sensedThetaDot, refTheta, refThetaDot, accuracy): +def hingedRigidBodyMotorTestFunction( + show_plots, K, P, sensedTheta, sensedThetaDot, refTheta, refThetaDot, accuracy +): """Test method""" testFailCount = 0 testMessages = [] @@ -79,12 +85,16 @@ def hingedRigidBodyMotorTestFunction(show_plots, K, P, sensedTheta, sensedThetaD hingedBodyStateSensedInMsgData = messaging.HingedRigidBodyMsgPayload() hingedBodyStateSensedInMsgData.theta = sensedTheta hingedBodyStateSensedInMsgData.thetaDot = sensedThetaDot - hingedBodyStateSensedInMsg = messaging.HingedRigidBodyMsg().write(hingedBodyStateSensedInMsgData) + hingedBodyStateSensedInMsg = messaging.HingedRigidBodyMsg().write( + hingedBodyStateSensedInMsgData + ) hingedBodyStateReferenceInMsgData = messaging.HingedRigidBodyMsgPayload() hingedBodyStateReferenceInMsgData.theta = refTheta hingedBodyStateReferenceInMsgData.thetaDot = refThetaDot - hingedBodyStateReferenceInMsg = messaging.HingedRigidBodyMsg().write(hingedBodyStateReferenceInMsgData) + hingedBodyStateReferenceInMsg = messaging.HingedRigidBodyMsg().write( + hingedBodyStateReferenceInMsgData + ) # subscribe input messages to module module.hingedBodyStateSensedInMsg.subscribeTo(hingedBodyStateSensedInMsg) @@ -102,10 +112,12 @@ def hingedRigidBodyMotorTestFunction(show_plots, K, P, sensedTheta, sensedThetaD unitTestSim.ExecuteSimulation() # pull module data and make sure it is correct - trueTorque = -K*(sensedTheta-refTheta)-P*(sensedThetaDot-refThetaDot) - torqueEqualTest = unitTestSupport.isDoubleEqualRelative(trueTorque, dataLog.motorTorque[-1][0], accuracy) + trueTorque = -K * (sensedTheta - refTheta) - P * (sensedThetaDot - refThetaDot) + torqueEqualTest = unitTestSupport.isDoubleEqualRelative( + trueTorque, dataLog.motorTorque[-1][0], accuracy + ) if not torqueEqualTest: - testFailCount += 1; + testFailCount += 1 testMessages.append("Failed motor torque.") if testFailCount == 0: print("PASSED: " + module.ModelTag) @@ -116,4 +128,6 @@ def hingedRigidBodyMotorTestFunction(show_plots, K, P, sensedTheta, sensedThetaD if __name__ == "__main__": - test_hingedRigidBodyMotor(False, 5, 1, 1, .1, 1.2, .2, 1e-12) # first test case above + test_hingedRigidBodyMotor( + False, 5, 1, 1, 0.1, 1.2, 0.2, 1e-12 + ) # first test case above diff --git a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesNDOF/_UnitTest/test_linearTranslationNDOFStateEffector.py b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesNDOF/_UnitTest/test_linearTranslationNDOFStateEffector.py index da5bb9a308..05c54f7032 100644 --- a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesNDOF/_UnitTest/test_linearTranslationNDOFStateEffector.py +++ b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesNDOF/_UnitTest/test_linearTranslationNDOFStateEffector.py @@ -34,10 +34,14 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') +splitPath = path.split("simulation") from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros -from Basilisk.simulation import spacecraft, linearTranslationNDOFStateEffector, gravityEffector +from Basilisk.simulation import ( + spacecraft, + linearTranslationNDOFStateEffector, + gravityEffector, +) from Basilisk.architecture import messaging @@ -47,9 +51,15 @@ # @pytest.mark.xfail() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("function", ["translatingBodyNoInput" - , "translatingBodyLockAxis" - , "translatingBodyCommandedForce"]) + +@pytest.mark.parametrize( + "function", + [ + "translatingBodyNoInput", + "translatingBodyLockAxis", + "translatingBodyCommandedForce", + ], +) def test_translatingBody(show_plots, function): r""" **Validation Test Description** @@ -78,6 +88,7 @@ def test_translatingBody(show_plots, function): testFunction(show_plots) + def translatingBodyNoInput(show_plots): r""" This test does not use any input messages or lock flags, so the links are free to move. @@ -97,22 +108,36 @@ def translatingBodyNoInput(show_plots): testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Create four translating rigid bodies - translatingBodyEffector = linearTranslationNDOFStateEffector.linearTranslationNDOFStateEffector() + translatingBodyEffector = ( + linearTranslationNDOFStateEffector.linearTranslationNDOFStateEffector() + ) translatingBodyEffector.ModelTag = "translatingBodyEffector" # define properties translatingBody1 = linearTranslationNDOFStateEffector.translatingBody() translatingBody1.setMass(np.random.uniform(5.0, 50.0)) - translatingBody1.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody1.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody1.setDCM_FP([[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]]) - translatingBody1.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody1.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody1.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody1.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody1.setFHat_P([[3.0 / 5.0], [4.0 / 5.0], [0.0]]) translatingBody1.setRhoInit(np.random.uniform(-5.0, 10.0)) translatingBody1.setRhoDotInit(0.05) @@ -121,16 +146,28 @@ def translatingBodyNoInput(show_plots): translatingBody2 = linearTranslationNDOFStateEffector.translatingBody() translatingBody2.setMass(np.random.uniform(5.0, 50.0)) - translatingBody2.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody2.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody2.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - translatingBody2.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody2.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody2.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody2.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody2.setFHat_P([[3.0 / 5.0], [4.0 / 5.0], [0.0]]) translatingBody2.setRhoInit(np.random.uniform(-5.0, 5.0)) translatingBody2.setRhoDotInit(0.05) @@ -139,16 +176,28 @@ def translatingBodyNoInput(show_plots): translatingBody3 = linearTranslationNDOFStateEffector.translatingBody() translatingBody3.setMass(np.random.uniform(5.0, 50.0)) - translatingBody3.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody3.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody3.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - translatingBody3.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody3.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody3.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody3.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody3.setFHat_P([[3.0 / 5.0], [4.0 / 5.0], [0.0]]) translatingBody3.setRhoInit(np.random.uniform(-5.0, 5.0)) translatingBody3.setRhoDotInit(0.05) @@ -157,16 +206,28 @@ def translatingBodyNoInput(show_plots): translatingBody4 = linearTranslationNDOFStateEffector.translatingBody() translatingBody4.setMass(np.random.uniform(5.0, 50.0)) - translatingBody4.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody4.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody4.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - translatingBody4.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody4.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody4.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody4.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody4.setFHat_P([[0.0], [0.0], [1.0]]) translatingBody4.setRhoInit(np.random.uniform(-5.0, 5.0)) translatingBody4.setRhoDotInit(0.05) @@ -182,8 +243,16 @@ def translatingBodyNoInput(show_plots): 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.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]] @@ -194,7 +263,7 @@ def translatingBodyNoInput(show_plots): # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -206,7 +275,9 @@ def translatingBodyNoInput(show_plots): unitTestSim.InitializeSimulation() # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Add states to log @@ -253,55 +324,65 @@ def translatingBodyNoInput(show_plots): 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.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.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.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.xlabel("time (s)") + plt.ylabel("Relative Difference") + plt.title("Rotational Energy") plt.figure() plt.clf() - plt.plot(rho1Data.times() * 1e-9, rho1, label=r'$\rho_1$') - plt.plot(rho2Data.times() * 1e-9, rho2, label=r'$\rho_2$') - plt.plot(rho3Data.times() * 1e-9, rho3, label=r'$\rho_3$') - plt.plot(rho4Data.times() * 1e-9, rho4, label=r'$\rho_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Displacement') + plt.plot(rho1Data.times() * 1e-9, rho1, label=r"$\rho_1$") + plt.plot(rho2Data.times() * 1e-9, rho2, label=r"$\rho_2$") + plt.plot(rho3Data.times() * 1e-9, rho3, label=r"$\rho_3$") + plt.plot(rho4Data.times() * 1e-9, rho4, label=r"$\rho_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Displacement") plt.figure() plt.clf() - plt.plot(rho1Data.times() * 1e-9, rho1Dot, label=r'$\dot{\rho}_1$') - plt.plot(rho2Data.times() * 1e-9, rho2Dot, label=r'$\dot{\rho}_2$') - plt.plot(rho3Data.times() * 1e-9, rho3Dot, label=r'$\dot{\rho}_3$') - plt.plot(rho4Data.times() * 1e-9, rho4Dot, label=r'$\dot{\rho}_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Displacement Rate') + plt.plot(rho1Data.times() * 1e-9, rho1Dot, label=r"$\dot{\rho}_1$") + plt.plot(rho2Data.times() * 1e-9, rho2Dot, label=r"$\dot{\rho}_2$") + plt.plot(rho3Data.times() * 1e-9, rho3Dot, label=r"$\dot{\rho}_3$") + plt.plot(rho4Data.times() * 1e-9, rho4Dot, label=r"$\dot{\rho}_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Displacement Rate") if show_plots: plt.show() @@ -310,14 +391,31 @@ def translatingBodyNoInput(show_plots): # 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.") + 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.") + 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.", + ) def translatingBodyLockAxis(show_plots): @@ -339,22 +437,36 @@ def translatingBodyLockAxis(show_plots): testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Create four translating rigid bodies - translatingBodyEffector = linearTranslationNDOFStateEffector.linearTranslationNDOFStateEffector() + translatingBodyEffector = ( + linearTranslationNDOFStateEffector.linearTranslationNDOFStateEffector() + ) translatingBodyEffector.ModelTag = "translatingBodyEffector" # define properties translatingBody1 = linearTranslationNDOFStateEffector.translatingBody() translatingBody1.setMass(np.random.uniform(5.0, 50.0)) - translatingBody1.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody1.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody1.setDCM_FP([[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]]) - translatingBody1.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody1.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody1.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody1.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody1.setFHat_P([[3.0 / 5.0], [4.0 / 5.0], [0.0]]) translatingBody1.setRhoInit(np.random.uniform(-5.0, 10.0)) translatingBody1.setRhoDotInit(0.05) @@ -363,16 +475,28 @@ def translatingBodyLockAxis(show_plots): translatingBody2 = linearTranslationNDOFStateEffector.translatingBody() translatingBody2.setMass(np.random.uniform(5.0, 50.0)) - translatingBody2.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody2.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody2.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - translatingBody2.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody2.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody2.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody2.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody2.setFHat_P([[3.0 / 5.0], [4.0 / 5.0], [0.0]]) translatingBody2.setRhoInit(np.random.uniform(-5.0, 5.0)) translatingBody2.setRhoDotInit(0.05) @@ -381,16 +505,28 @@ def translatingBodyLockAxis(show_plots): translatingBody3 = linearTranslationNDOFStateEffector.translatingBody() translatingBody3.setMass(np.random.uniform(5.0, 50.0)) - translatingBody3.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody3.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody3.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - translatingBody3.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody3.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody3.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody3.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody3.setFHat_P([[3.0 / 5.0], [4.0 / 5.0], [0.0]]) translatingBody3.setRhoInit(np.random.uniform(-5.0, 5.0)) translatingBody3.setRhoDotInit(0.05) @@ -399,16 +535,28 @@ def translatingBodyLockAxis(show_plots): translatingBody4 = linearTranslationNDOFStateEffector.translatingBody() translatingBody4.setMass(np.random.uniform(5.0, 50.0)) - translatingBody4.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody4.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody4.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - translatingBody4.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody4.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody4.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody4.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody4.setFHat_P([[0.0], [0.0], [1.0]]) translatingBody4.setRhoInit(np.random.uniform(-5.0, 5.0)) translatingBody4.setRhoDotInit(0.05) @@ -424,8 +572,16 @@ def translatingBodyLockAxis(show_plots): 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.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]] @@ -436,7 +592,7 @@ def translatingBodyLockAxis(show_plots): # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -454,7 +610,9 @@ def translatingBodyLockAxis(show_plots): unitTestSim.InitializeSimulation() # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Add states to log @@ -501,55 +659,65 @@ def translatingBodyLockAxis(show_plots): 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.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.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.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.xlabel("time (s)") + plt.ylabel("Relative Difference") + plt.title("Rotational Energy") plt.figure() plt.clf() - plt.plot(rho1Data.times() * 1e-9, rho1, label=r'$\rho_1$') - plt.plot(rho2Data.times() * 1e-9, rho2, label=r'$\rho_2$') - plt.plot(rho3Data.times() * 1e-9, rho3, label=r'$\rho_3$') - plt.plot(rho4Data.times() * 1e-9, rho4, label=r'$\rho_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Displacement') + plt.plot(rho1Data.times() * 1e-9, rho1, label=r"$\rho_1$") + plt.plot(rho2Data.times() * 1e-9, rho2, label=r"$\rho_2$") + plt.plot(rho3Data.times() * 1e-9, rho3, label=r"$\rho_3$") + plt.plot(rho4Data.times() * 1e-9, rho4, label=r"$\rho_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Displacement") plt.figure() plt.clf() - plt.plot(rho1Data.times() * 1e-9, rho1Dot, label=r'$\dot{\rho}_1$') - plt.plot(rho2Data.times() * 1e-9, rho2Dot, label=r'$\dot{\rho}_2$') - plt.plot(rho3Data.times() * 1e-9, rho3Dot, label=r'$\dot{\rho}_3$') - plt.plot(rho4Data.times() * 1e-9, rho4Dot, label=r'$\dot{\rho}_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Displacement Rate') + plt.plot(rho1Data.times() * 1e-9, rho1Dot, label=r"$\dot{\rho}_1$") + plt.plot(rho2Data.times() * 1e-9, rho2Dot, label=r"$\dot{\rho}_2$") + plt.plot(rho3Data.times() * 1e-9, rho3Dot, label=r"$\dot{\rho}_3$") + plt.plot(rho4Data.times() * 1e-9, rho4Dot, label=r"$\dot{\rho}_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Displacement Rate") if show_plots: plt.show() @@ -558,15 +726,31 @@ def translatingBodyLockAxis(show_plots): # 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.") + 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.") + 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.", + ) def translatingBodyCommandedForce(show_plots): @@ -588,22 +772,36 @@ def translatingBodyCommandedForce(show_plots): testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Create four translating rigid bodies - translatingBodyEffector = linearTranslationNDOFStateEffector.linearTranslationNDOFStateEffector() + translatingBodyEffector = ( + linearTranslationNDOFStateEffector.linearTranslationNDOFStateEffector() + ) translatingBodyEffector.ModelTag = "translatingBodyEffector" # define properties translatingBody1 = linearTranslationNDOFStateEffector.translatingBody() translatingBody1.setMass(np.random.uniform(5.0, 50.0)) - translatingBody1.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody1.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody1.setDCM_FP([[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]]) - translatingBody1.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody1.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody1.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody1.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody1.setFHat_P([[3.0 / 5.0], [4.0 / 5.0], [0.0]]) translatingBody1.setRhoInit(np.random.uniform(-5.0, 10.0)) translatingBody1.setRhoDotInit(0.05) @@ -612,16 +810,28 @@ def translatingBodyCommandedForce(show_plots): translatingBody2 = linearTranslationNDOFStateEffector.translatingBody() translatingBody2.setMass(np.random.uniform(5.0, 50.0)) - translatingBody2.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody2.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody2.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - translatingBody2.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody2.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody2.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody2.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody2.setFHat_P([[3.0 / 5.0], [4.0 / 5.0], [0.0]]) translatingBody2.setRhoInit(np.random.uniform(-5.0, 5.0)) translatingBody2.setRhoDotInit(0.05) @@ -630,16 +840,28 @@ def translatingBodyCommandedForce(show_plots): translatingBody3 = linearTranslationNDOFStateEffector.translatingBody() translatingBody3.setMass(np.random.uniform(5.0, 50.0)) - translatingBody3.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody3.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody3.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - translatingBody3.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody3.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody3.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody3.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody3.setFHat_P([[3.0 / 5.0], [4.0 / 5.0], [0.0]]) translatingBody3.setRhoInit(np.random.uniform(-5.0, 5.0)) translatingBody3.setRhoDotInit(0.05) @@ -648,16 +870,28 @@ def translatingBodyCommandedForce(show_plots): translatingBody4 = linearTranslationNDOFStateEffector.translatingBody() translatingBody4.setMass(np.random.uniform(5.0, 50.0)) - translatingBody4.setIPntFc_F([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + translatingBody4.setIPntFc_F( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) translatingBody4.setDCM_FP([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - translatingBody4.setR_FcF_F([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - translatingBody4.setR_F0P_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + translatingBody4.setR_FcF_F( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + translatingBody4.setR_F0P_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) translatingBody4.setFHat_P([[0.0], [0.0], [1.0]]) translatingBody4.setRhoInit(np.random.uniform(-5.0, 5.0)) translatingBody4.setRhoDotInit(0.05) @@ -673,8 +907,16 @@ def translatingBodyCommandedForce(show_plots): 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.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]] @@ -685,7 +927,7 @@ def translatingBodyCommandedForce(show_plots): # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -703,7 +945,9 @@ def translatingBodyCommandedForce(show_plots): unitTestSim.InitializeSimulation() # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Add states to log @@ -749,48 +993,58 @@ def translatingBodyCommandedForce(show_plots): 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.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.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.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(rho1Data.times() * 1e-9, rho1, label=r'$\rho_1$') - plt.plot(rho2Data.times() * 1e-9, rho2, label=r'$\rho_2$') - plt.plot(rho3Data.times() * 1e-9, rho3, label=r'$\rho_3$') - plt.plot(rho4Data.times() * 1e-9, rho4, label=r'$\rho_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Displacement') + plt.plot(rho1Data.times() * 1e-9, rho1, label=r"$\rho_1$") + plt.plot(rho2Data.times() * 1e-9, rho2, label=r"$\rho_2$") + plt.plot(rho3Data.times() * 1e-9, rho3, label=r"$\rho_3$") + plt.plot(rho4Data.times() * 1e-9, rho4, label=r"$\rho_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Displacement") plt.figure() plt.clf() - plt.plot(rho1Data.times() * 1e-9, rho1Dot, label=r'$\dot{\rho}_1$') - plt.plot(rho2Data.times() * 1e-9, rho2Dot, label=r'$\dot{\rho}_2$') - plt.plot(rho3Data.times() * 1e-9, rho3Dot, label=r'$\dot{\rho}_3$') - plt.plot(rho4Data.times() * 1e-9, rho4Dot, label=r'$\dot{\rho}_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Displacement Rate') + plt.plot(rho1Data.times() * 1e-9, rho1Dot, label=r"$\dot{\rho}_1$") + plt.plot(rho2Data.times() * 1e-9, rho2Dot, label=r"$\dot{\rho}_2$") + plt.plot(rho3Data.times() * 1e-9, rho3Dot, label=r"$\dot{\rho}_3$") + plt.plot(rho4Data.times() * 1e-9, rho4Dot, label=r"$\dot{\rho}_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Displacement Rate") if show_plots: plt.show() @@ -799,13 +1053,25 @@ def translatingBodyCommandedForce(show_plots): # Testing setup accuracy = 1e-12 - np.testing.assert_allclose(finalOrbEnergy, initialOrbEnergy, rtol=accuracy, - err_msg="Orbital energy is not constant.") + 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.") + 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__": diff --git a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/_UnitTest/test_linearTranslationOneDOFStateEffector.py b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/_UnitTest/test_linearTranslationOneDOFStateEffector.py index 08b792e6bd..ea7a1ec78e 100644 --- a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/_UnitTest/test_linearTranslationOneDOFStateEffector.py +++ b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/_UnitTest/test_linearTranslationOneDOFStateEffector.py @@ -31,10 +31,14 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') +splitPath = path.split("simulation") from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros -from Basilisk.simulation import spacecraft, linearTranslationOneDOFStateEffector, gravityEffector +from Basilisk.simulation import ( + spacecraft, + linearTranslationOneDOFStateEffector, + gravityEffector, +) from Basilisk.architecture import messaging @@ -46,11 +50,15 @@ # tests are paramterized by four functions -@pytest.mark.parametrize("function", ["translatingBodyNoInput" - , "translatingBodyLockFlag" - , "translatingBodyCommandedForce" - , "translatingBodyRhoReference" - ]) +@pytest.mark.parametrize( + "function", + [ + "translatingBodyNoInput", + "translatingBodyLockFlag", + "translatingBodyCommandedForce", + "translatingBodyRhoReference", + ], +) def test_translatingBody(show_plots, function): r""" **Validation Test Description** @@ -113,13 +121,23 @@ def translatingBodyNoInput(show_plots): 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.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() + translatingBody = ( + linearTranslationOneDOFStateEffector.linearTranslationOneDOFStateEffector() + ) # Define properties of translating body mass = 20.0 @@ -128,12 +146,8 @@ def translatingBodyNoInput(show_plots): 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]] + 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 @@ -161,7 +175,7 @@ def translatingBodyNoInput(show_plots): # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -170,7 +184,9 @@ def translatingBodyNoInput(show_plots): unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Initialize the simulation @@ -208,47 +224,57 @@ def translatingBodyNoInput(show_plots): 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.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.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.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.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.xlabel("time (s)") + plt.ylabel("rho") plt.figure() plt.clf() plt.plot(timeSec, rhoDot) - plt.xlabel('time (s)') - plt.ylabel('rhoDot') + plt.xlabel("time (s)") + plt.ylabel("rhoDot") if show_plots: plt.show() @@ -257,11 +283,31 @@ def translatingBodyNoInput(show_plots): # 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.") + 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.") + 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 @@ -292,13 +338,23 @@ def translatingBodyLockFlag(show_plots): 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.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() + translatingBody = ( + linearTranslationOneDOFStateEffector.linearTranslationOneDOFStateEffector() + ) # Define properties of translating body mass = 20.0 @@ -307,12 +363,8 @@ def translatingBodyLockFlag(show_plots): 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]] + 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 @@ -346,7 +398,7 @@ def translatingBodyLockFlag(show_plots): # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -355,7 +407,9 @@ def translatingBodyLockFlag(show_plots): unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Initialize the simulation @@ -393,47 +447,57 @@ def translatingBodyLockFlag(show_plots): 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.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.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.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.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.xlabel("time (s)") + plt.ylabel("rho") plt.figure() plt.clf() plt.plot(timeSec, rhoDot) - plt.xlabel('time (s)') - plt.ylabel('rhoDot') + plt.xlabel("time (s)") + plt.ylabel("rhoDot") if show_plots: plt.show() @@ -442,11 +506,31 @@ def translatingBodyLockFlag(show_plots): # 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.") + 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.") + 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 @@ -477,13 +561,23 @@ def translatingBodyCommandedForce(show_plots, cmdForce): 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.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() + translatingBody = ( + linearTranslationOneDOFStateEffector.linearTranslationOneDOFStateEffector() + ) # Define properties of translating body mass = 20.0 @@ -492,12 +586,8 @@ def translatingBodyCommandedForce(show_plots, cmdForce): 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]] + 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 @@ -531,7 +621,7 @@ def translatingBodyCommandedForce(show_plots, cmdForce): # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -540,7 +630,9 @@ def translatingBodyCommandedForce(show_plots, cmdForce): unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Initialize the simulation @@ -575,40 +667,50 @@ def translatingBodyCommandedForce(show_plots, cmdForce): 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.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.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.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.xlabel("time (s)") + plt.ylabel("rho") plt.figure() plt.clf() plt.plot(timeSec, rhoDot) - plt.xlabel('time (s)') - plt.ylabel('rhoDot') + plt.xlabel("time (s)") + plt.ylabel("rhoDot") if show_plots: plt.show() @@ -617,10 +719,25 @@ def translatingBodyCommandedForce(show_plots, cmdForce): # Testing setup accuracy = 1e-12 - np.testing.assert_allclose(finalOrbEnergy, initialOrbEnergy, rtol=accuracy, err_msg="Orbital energy is not constant.") + 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.") + 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 @@ -651,13 +768,23 @@ def translatingBodyRhoReference(show_plots, rhoRef): 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.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() + translatingBody = ( + linearTranslationOneDOFStateEffector.linearTranslationOneDOFStateEffector() + ) # Define properties of translating body mass = 20.0 @@ -666,12 +793,8 @@ def translatingBodyRhoReference(show_plots, rhoRef): 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]] + 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 @@ -706,7 +829,7 @@ def translatingBodyRhoReference(show_plots, rhoRef): # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -715,7 +838,9 @@ def translatingBodyRhoReference(show_plots, rhoRef): unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Initialize the simulation @@ -751,40 +876,50 @@ def translatingBodyRhoReference(show_plots, rhoRef): 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.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.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.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.xlabel("time (s)") + plt.ylabel("rho") plt.figure() plt.clf() plt.plot(timeSec, rhoDot) - plt.xlabel('time (s)') - plt.ylabel('rhoDot') + plt.xlabel("time (s)") + plt.ylabel("rhoDot") if show_plots: plt.show() @@ -793,11 +928,31 @@ def translatingBodyRhoReference(show_plots, rhoRef): # 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.") + 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.") + 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__": diff --git a/src/simulation/dynamics/msmForceTorque/_UnitTest/test_msmForceTorque.py b/src/simulation/dynamics/msmForceTorque/_UnitTest/test_msmForceTorque.py index 7f110be973..d70e491bfd 100644 --- a/src/simulation/dynamics/msmForceTorque/_UnitTest/test_msmForceTorque.py +++ b/src/simulation/dynamics/msmForceTorque/_UnitTest/test_msmForceTorque.py @@ -69,50 +69,52 @@ def msmForceTorqueTestFunction(show_plots, accuracy): # Configure space object state and voltage input messages sc0StateInMsgsData = messaging.SCStatesMsgPayload() - sc0StateInMsgsData.r_BN_N = [10., 2., 3.] + sc0StateInMsgsData.r_BN_N = [10.0, 2.0, 3.0] sc0StateInMsgsData.sigma_BN = [0.1, 0.2, 0.3] sc0StateInMsg = messaging.SCStatesMsg().write(sc0StateInMsgsData) sc1StateInMsgsData = messaging.SCStatesMsgPayload() - sc1StateInMsgsData.r_BN_N = [-10., -2., 3.] + sc1StateInMsgsData.r_BN_N = [-10.0, -2.0, 3.0] sc1StateInMsgsData.sigma_BN = [-0.1, 0.2, 0.3] sc1StateInMsg = messaging.SCStatesMsg().write(sc1StateInMsgsData) sc2StateInMsgsData = messaging.SCStatesMsgPayload() - sc2StateInMsgsData.r_BN_N = [1., 1., 0.] + sc2StateInMsgsData.r_BN_N = [1.0, 1.0, 0.0] sc2StateInMsgsData.sigma_BN = [0.1, 0.2, -0.3] sc2StateInMsg = messaging.SCStatesMsg().write(sc2StateInMsgsData) volt0InMsgData = messaging.VoltMsgPayload() - volt0InMsgData.voltage = 30000. + volt0InMsgData.voltage = 30000.0 volt0InMsg = messaging.VoltMsg().write(volt0InMsgData) volt1InMsgData = messaging.VoltMsgPayload() - volt1InMsgData.voltage = -10000. + volt1InMsgData.voltage = -10000.0 volt1InMsg = messaging.VoltMsg().write(volt1InMsgData) volt2InMsgData = messaging.VoltMsgPayload() - volt2InMsgData.voltage = 20000. + volt2InMsgData.voltage = 20000.0 volt2InMsg = messaging.VoltMsg().write(volt2InMsgData) # create a list of sphere body-fixed locations and associated radii - spPosList = [ - [1., 2., 3.] - , [4., 5., 6.] - , [14., 5., 6.] - ] - rList = [1., 2., 1.5] + spPosList = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [14.0, 5.0, 6.0]] + rList = [1.0, 2.0, 1.5] # add spacecraft to state - module.addSpacecraftToModel(sc0StateInMsg - , messaging.DoubleVector(rList[:-1]) - , unitTestSupport.npList2EigenXdVector(spPosList[:-1])) - module.addSpacecraftToModel(sc1StateInMsg - , messaging.DoubleVector(rList) - , unitTestSupport.npList2EigenXdVector(spPosList)) - module.addSpacecraftToModel(sc2StateInMsg - , messaging.DoubleVector(rList[:-1]) - , unitTestSupport.npList2EigenXdVector(spPosList[:-1])) + module.addSpacecraftToModel( + sc0StateInMsg, + messaging.DoubleVector(rList[:-1]), + unitTestSupport.npList2EigenXdVector(spPosList[:-1]), + ) + module.addSpacecraftToModel( + sc1StateInMsg, + messaging.DoubleVector(rList), + unitTestSupport.npList2EigenXdVector(spPosList), + ) + module.addSpacecraftToModel( + sc2StateInMsg, + messaging.DoubleVector(rList[:-1]), + unitTestSupport.npList2EigenXdVector(spPosList[:-1]), + ) # subscribe input messages to module module.voltInMsgs[0].subscribeTo(volt0InMsg) @@ -124,39 +126,51 @@ def msmForceTorqueTestFunction(show_plots, accuracy): # set truth force and torque values fTruth = [ - [6.48179e-05, 0.00147205, 0.000924806] - , [0.00107182, -0.000240543, 0.000110224] - , [-0.00113664, -0.00123151, -0.00103503] + [6.48179e-05, 0.00147205, 0.000924806], + [0.00107182, -0.000240543, 0.000110224], + [-0.00113664, -0.00123151, -0.00103503], ] tauTruth = [ - [-0.00268192, 0.00295288, -0.000603687] - , [0.00688387, -0.00209438, -0.00647544] - , [0.00581629, 0.007876, -0.00986612] + [-0.00268192, 0.00295288, -0.000603687], + [0.00688387, -0.00209438, -0.00647544], + [0.00581629, 0.007876, -0.00986612], ] chargeTruth = [ - [1.99932e-6, 5.73861e-6] - , [-1.06715e-6, -2.51072e-6, -1.94044e-6] - , [1.30148e-6, 3.23131e-6] + [1.99932e-6, 5.73861e-6], + [-1.06715e-6, -2.51072e-6, -1.94044e-6], + [1.30148e-6, 3.23131e-6], ] # pull module data and make sure it is correct for i in range(3): f = module.eForceOutMsgs[i].read().forceRequestInertial - testFailCount, testMessages = \ - unitTestSupport.compareDoubleArrayRelative(f, fTruth[i], - accuracy, "sc" + str(i) + " force test", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareDoubleArrayRelative( + f, + fTruth[i], + accuracy, + "sc" + str(i) + " force test", + testFailCount, + testMessages, + ) tau = module.eTorqueOutMsgs[i].read().torqueRequestBody - testFailCount, testMessages = \ - unitTestSupport.compareDoubleArrayRelative(tau, tauTruth[i], - accuracy, "sc" + str(i) + " torque test", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareDoubleArrayRelative( + tau, + tauTruth[i], + accuracy, + "sc" + str(i) + " torque test", + testFailCount, + testMessages, + ) charge = unitTestSupport.columnToRowList(module.chargeMsmOutMsgs[i].read().q) - testFailCount, testMessages = \ - unitTestSupport.compareListRelative(charge, chargeTruth[i], - accuracy, "sc" + str(i) + " charge test", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareListRelative( + charge, + chargeTruth[i], + accuracy, + "sc" + str(i) + " charge test", + testFailCount, + testMessages, + ) if testFailCount == 0: print("PASSED: " + module.ModelTag) diff --git a/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py b/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py index b127befb5a..7c9faed8e3 100644 --- a/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py +++ b/src/simulation/dynamics/prescribedMotion/_UnitTest/test_PrescribedMotionStateEffector.py @@ -43,19 +43,22 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') +splitPath = path.split("simulation") + +matplotlib.rc("xtick", labelsize=16) +matplotlib.rc("ytick", labelsize=16) -matplotlib.rc('xtick', labelsize=16) -matplotlib.rc('ytick', labelsize=16) # Vary the simulation parameters for pytest @pytest.mark.parametrize("rotTest", [True, False]) -@pytest.mark.parametrize("thetaInit", [0, np.pi/18]) -@pytest.mark.parametrize("theta_Ref", [np.pi/36]) +@pytest.mark.parametrize("thetaInit", [0, np.pi / 18]) +@pytest.mark.parametrize("theta_Ref", [np.pi / 36]) @pytest.mark.parametrize("posInit", [0, 0.2]) @pytest.mark.parametrize("posRef", [0.1]) @pytest.mark.parametrize("accuracy", [1e-8]) -def test_PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy): +def test_PrescribedMotionTestFunction( + show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy +): r""" **Validation Test Description** @@ -99,12 +102,16 @@ def test_PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, dynamics. """ - [testResults, testMessage] = PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy) + [testResults, testMessage] = PrescribedMotionTestFunction( + show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy + ) assert testResults < 1, testMessage -def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy): +def PrescribedMotionTestFunction( + show_plots, rotTest, thetaInit, theta_Ref, posInit, posRef, accuracy +): """Call this routine directly to run the unit test.""" __tracebackhide__ = True @@ -132,8 +139,16 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI 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 inertial hub 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.r_CN_NInit = [ + [-4020338.690396649], + [7490566.741852513], + [5248299.211589362], + ] + scObject.hub.v_CN_NInit = [ + [-5199.77710904224], + [-3436.681645356935], + [1041.576797498721], + ] scObject.hub.omega_BN_BInit = np.array([0.0, 0.0, 0.0]) scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] @@ -151,14 +166,14 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI sigma_PM = rbk.PRV2MRP(prvInit_PM) platform.mass = 100.0 - platform.IPntPc_P= [[50.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] + platform.IPntPc_P = [[50.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] platform.r_MB_B = [0.0, 0.0, 0.0] - platform.r_PcP_P= [0.0, 0.0, 0.0] + platform.r_PcP_P = [0.0, 0.0, 0.0] platform.r_PM_M = r_PM_M platform.rPrime_PM_M = np.array([0.0, 0.0, 0.0]) platform.rPrimePrime_PM_M = np.array([0.0, 0.0, 0.0]) - platform.omega_PM_P= np.array([0.0, 0.0, 0.0]) - platform.omegaPrime_PM_P= np.array([0.0, 0.0, 0.0]) + platform.omega_PM_P = np.array([0.0, 0.0, 0.0]) + platform.omegaPrime_PM_P = np.array([0.0, 0.0, 0.0]) platform.sigma_PM = sigma_PM platform.omega_MB_B = [0.0, 0.0, 0.0] platform.omegaPrime_MB_B = [0.0, 0.0, 0.0] @@ -172,7 +187,6 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI unitTestSim.AddModelToTask(unitTaskName, platform) if rotTest: - # ** ** ** ** ** ROTATIONAL 1 DOF INTEGRATED TEST: ** ** ** ** ** # Create an instance of the prescribedRotation1DOF module to be tested @@ -193,20 +207,26 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI SpinningBodyMessageData = messaging.HingedRigidBodyMsgPayload() SpinningBodyMessageData.theta = theta_Ref SpinningBodyMessageData.thetaDot = thetaDot_Ref - SpinningBodyMessage = messaging.HingedRigidBodyMsg().write(SpinningBodyMessageData) + SpinningBodyMessage = messaging.HingedRigidBodyMsg().write( + SpinningBodyMessageData + ) PrescribedRot1DOF.spinningBodyInMsg.subscribeTo(SpinningBodyMessage) - platform.prescribedRotationInMsg.subscribeTo(PrescribedRot1DOF.prescribedRotationOutMsg) + platform.prescribedRotationInMsg.subscribeTo( + PrescribedRot1DOF.prescribedRotationOutMsg + ) # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 + earthGravBody.mu = 0.3986004415e15 earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Add other states to log @@ -219,22 +239,33 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI unitTestSim.InitializeSimulation() # Set the simulation time - simTime = np.sqrt(((0.5 * np.abs(theta_Ref - thetaInit)) * 8) / accelMax) + 3 * testIncrement + simTime = ( + np.sqrt(((0.5 * np.abs(theta_Ref - thetaInit)) * 8) / accelMax) + + 3 * testIncrement + ) unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) # Begin the simulation unitTestSim.ExecuteSimulation() # Extract the logged data - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) omega_BN_B = scStateData.omega_BN_B r_BN_N = scStateData.r_BN_N sigma_BN = scStateData.sigma_BN - omega_PM_P= prescribedRotStateData.omega_PM_P - omegaPrime_PM_P= prescribedRotStateData.omegaPrime_PM_P + omega_PM_P = prescribedRotStateData.omega_PM_P + omegaPrime_PM_P = prescribedRotStateData.omegaPrime_PM_P sigma_PM = prescribedRotStateData.sigma_PM timespan = prescribedRotStateData.times() thetaDot_Final = np.linalg.norm(omega_PM_P[-1, :]) @@ -253,7 +284,9 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI n = len(timespan) theta_PM = [] for i in range(n): - theta_PM.append((180 / np.pi) * (4 * np.arctan(np.linalg.norm(sigma_PM[i, :])))) + theta_PM.append( + (180 / np.pi) * (4 * np.arctan(np.linalg.norm(sigma_PM[i, :]))) + ) plt.close("all") @@ -262,101 +295,169 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI thetaInit_plotting = np.ones(len(timespan)) * thetaInit plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, theta_PM, label=r'$\Phi$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * theta_Ref_plotting, '--', label=r'$\Phi_{Ref}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * thetaInit_plotting, '--', label=r'$\Phi_{0}$') - plt.title(r'$\Phi_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(deg)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='center right', prop={'size': 16}) + plt.plot(timespan * macros.NANO2SEC, theta_PM, label=r"$\Phi$") + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * theta_Ref_plotting, + "--", + label=r"$\Phi_{Ref}$", + ) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * thetaInit_plotting, + "--", + label=r"$\Phi_{0}$", + ) + plt.title(r"$\Phi_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory", fontsize=14) + plt.ylabel("(deg)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) + plt.legend(loc="center right", prop={"size": 16}) # Plot omega_PM_P plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_PM_P[:, 0], label=r'$\omega_{1}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_PM_P[:, 1], label=r'$\omega_{2}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_PM_P[:, 2], label=r'$\omega_{3}$') - plt.title(r'${}^\mathcal{P} \omega_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(deg/s)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='upper right', prop={'size': 16}) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * omega_PM_P[:, 0], + label=r"$\omega_{1}$", + ) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * omega_PM_P[:, 1], + label=r"$\omega_{2}$", + ) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * omega_PM_P[:, 2], + label=r"$\omega_{3}$", + ) + plt.title( + r"${}^\mathcal{P} \omega_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory", + fontsize=14, + ) + plt.ylabel("(deg/s)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) + plt.legend(loc="upper right", prop={"size": 16}) # Plotting omegaPrime_PM_P plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_PM_P[:, 0], label='1') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_PM_P[:, 1], label='2') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_PM_P[:, 2], label='3') - plt.title(r'${}^\mathcal{P} \omega Prime_{\mathcal{P}/\mathcal{M}}$ Profiled Angular Acceleration', fontsize=14) - plt.ylabel(r'(deg/$s^2$)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='upper right', prop={'size': 16}) + plt.plot( + timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_PM_P[:, 0], label="1" + ) + plt.plot( + timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_PM_P[:, 1], label="2" + ) + plt.plot( + timespan * macros.NANO2SEC, (180 / np.pi) * omegaPrime_PM_P[:, 2], label="3" + ) + plt.title( + r"${}^\mathcal{P} \omega Prime_{\mathcal{P}/\mathcal{M}}$ Profiled Angular Acceleration", + fontsize=14, + ) + plt.ylabel(r"(deg/$s^2$)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) + plt.legend(loc="upper right", prop={"size": 16}) # Plot r_BN_N plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 0], label=r'$r_{1}$') - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 1], label=r'$r_{2}$') - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 2], label=r'$r_{3}$') - plt.title(r'${}^\mathcal{N} r_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial Trajectory', fontsize=14) - plt.ylabel('(m)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='center left', prop={'size': 16}) + plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 0], label=r"$r_{1}$") + plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 1], label=r"$r_{2}$") + plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 2], label=r"$r_{3}$") + plt.title( + r"${}^\mathcal{N} r_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial Trajectory", + fontsize=14, + ) + plt.ylabel("(m)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) + plt.legend(loc="center left", prop={"size": 16}) # Plot sigma_BN plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 0], label=r'$\sigma_{1}$') - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 1], label=r'$\sigma_{2}$') - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 2], label=r'$\sigma_{3}$') - plt.title(r'$\sigma_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial MRP Attitude', fontsize=14) - plt.ylabel('', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='center left', prop={'size': 16}) + plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 0], label=r"$\sigma_{1}$") + plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 1], label=r"$\sigma_{2}$") + plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 2], label=r"$\sigma_{3}$") + plt.title( + r"$\sigma_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial MRP Attitude", + fontsize=14, + ) + plt.ylabel("", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) + plt.legend(loc="center left", prop={"size": 16}) # Plot omega_BN_B plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 0], label=r'$\omega_{1}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 1], label=r'$\omega_{2}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 2], label=r'$\omega_{3}$') - plt.title(r'Spacecraft Hub Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$', fontsize=14) - plt.xlabel('Time (s)', fontsize=16) - plt.ylabel('(deg/s)', fontsize=16) - plt.legend(loc='lower right', prop={'size': 16}) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * omega_BN_B[:, 0], + label=r"$\omega_{1}$", + ) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * omega_BN_B[:, 1], + label=r"$\omega_{2}$", + ) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * omega_BN_B[:, 2], + label=r"$\omega_{3}$", + ) + plt.title( + r"Spacecraft Hub Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$", + fontsize=14, + ) + plt.xlabel("Time (s)", fontsize=16) + plt.ylabel("(deg/s)", fontsize=16) + plt.legend(loc="lower right", prop={"size": 16}) # Plotting: Conservation quantities plt.figure() plt.clf() - plt.plot(orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], - orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], - orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3]) - plt.title('Orbital Angular Momentum Relative Difference', fontsize=14) - plt.ylabel('(Nms)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) + plt.plot( + orbAngMom_N[:, 0] * macros.NANO2SEC, + (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * macros.NANO2SEC, + (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * macros.NANO2SEC, + (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3], + ) + plt.title("Orbital Angular Momentum Relative Difference", fontsize=14) + plt.ylabel("(Nms)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) plt.figure() plt.clf() - plt.plot(orbEnergy[:, 0] * macros.NANO2SEC, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1]) - plt.title('Orbital Energy Relative Difference', fontsize=14) - plt.ylabel('Energy (J)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) + plt.plot( + orbEnergy[:, 0] * macros.NANO2SEC, + (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1], + ) + plt.title("Orbital Energy Relative Difference", fontsize=14) + plt.ylabel("Energy (J)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) plt.figure() plt.clf() - plt.plot(rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]), - rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]), - rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 3] - rotAngMom_N[0, 3])) - plt.title('Rotational Angular Momentum Difference', fontsize=14) - plt.ylabel('(Nms)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) + plt.plot( + rotAngMom_N[:, 0] * macros.NANO2SEC, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]), + rotAngMom_N[:, 0] * macros.NANO2SEC, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]), + rotAngMom_N[:, 0] * macros.NANO2SEC, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]), + ) + plt.title("Rotational Angular Momentum Difference", fontsize=14) + plt.ylabel("(Nms)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) plt.figure() plt.clf() plt.plot(rotEnergy[:, 0] * macros.NANO2SEC, (rotEnergy[:, 1] - rotEnergy[0, 1])) - plt.title('Total Energy Difference', fontsize=14) - plt.ylabel('Energy (J)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) + plt.title("Total Energy Difference", fontsize=14) + plt.ylabel("Energy (J)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) if show_plots: plt.show() @@ -368,38 +469,55 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI finalOrbEnergy = np.delete(finalOrbEnergy, 0, axis=1) # remove the time column for i in range(0, len(initialOrbAngMom_N)): - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: Prescribed Motion integrated test failed orbital angular momentum unit test") + "FAILED: Prescribed Motion integrated test failed orbital angular momentum unit test" + ) for i in range(0, len(initialRotAngMom_N)): - if not unitTestSupport.isArrayEqual(finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: Prescribed Motion integrated test failed rotational angular momentum unit test") + "FAILED: Prescribed Motion integrated test failed rotational angular momentum unit test" + ) for i in range(0, len(initialOrbEnergy)): - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Prescribed Motion integrated test failed orbital energy unit test") + testMessages.append( + "FAILED: Prescribed Motion integrated test failed orbital energy unit test" + ) # Check to ensure the initial angle rate converged to the reference angle rate if not unitTestSupport.isDoubleEqual(thetaDot_Final, thetaDot_Ref, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + PrescribedRot1DOF.ModelTag + "thetaDot_Final and thetaDot_Ref do not match") + testMessages.append( + "FAILED: " + + PrescribedRot1DOF.ModelTag + + "thetaDot_Final and thetaDot_Ref do not match" + ) # Check to ensure the initial angle converged to the reference angle if not unitTestSupport.isDoubleEqual(theta_PM_Pinal, theta_Ref, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + PrescribedRot1DOF.ModelTag + "theta_PM_Pinal and theta_Ref do not match") + testMessages.append( + "FAILED: " + + PrescribedRot1DOF.ModelTag + + "theta_PM_Pinal and theta_Ref do not match" + ) # testMessages.append("theta_PM_Pinal: " + str(theta_PM_Pinal) + " theta_Ref: " + str(theta_Ref)) if testFailCount == 0: print("PASSED: " + "prescribedMotion and prescribedRot1DOF integrated test") else: - # ** ** ** ** ** TRANSLATIONAL INTEGRATED TEST ** ** ** ** ** # Create an instance of the prescribedLinearTranslation module to be tested @@ -417,23 +535,35 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI # Create the prescribedTrans input message velRef = 0.0 # [m/s] - linearTranslationRigidBodyMessageData = messaging.LinearTranslationRigidBodyMsgPayload() + linearTranslationRigidBodyMessageData = ( + messaging.LinearTranslationRigidBodyMsgPayload() + ) linearTranslationRigidBodyMessageData.rho = posRef linearTranslationRigidBodyMessageData.rhoDot = velRef - linearTranslationRigidBodyMessage = messaging.LinearTranslationRigidBodyMsg().write(linearTranslationRigidBodyMessageData) - PrescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo(linearTranslationRigidBodyMessage) - - platform.prescribedTranslationInMsg.subscribeTo(PrescribedTrans.prescribedTranslationOutMsg) + linearTranslationRigidBodyMessage = ( + messaging.LinearTranslationRigidBodyMsg().write( + linearTranslationRigidBodyMessageData + ) + ) + PrescribedTrans.linearTranslationRigidBodyInMsg.subscribeTo( + linearTranslationRigidBodyMessage + ) + + platform.prescribedTranslationInMsg.subscribeTo( + PrescribedTrans.prescribedTranslationOutMsg + ) # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 + earthGravBody.mu = 0.3986004415e15 earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Add other states to log @@ -446,17 +576,28 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI unitTestSim.InitializeSimulation() # Set the simulation time - simTime = np.sqrt(((0.5 * np.abs(posRef - posInit)) * 8) / accelMax) + 3 * testIncrement + simTime = ( + np.sqrt(((0.5 * np.abs(posRef - posInit)) * 8) / accelMax) + + 3 * testIncrement + ) unitTestSim.ConfigureStopTime(macros.sec2nano(simTime)) # Begin the simulation unitTestSim.ExecuteSimulation() # Extract the logged data - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) r_BN_N = scStateData.r_BN_N sigma_BN = scStateData.sigma_BN omega_BN_B = scStateData.omega_BN_B @@ -485,104 +626,159 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, r_PM_M[:, 0], label=r'$r_{1}$') - plt.plot(timespan * macros.NANO2SEC, r_PM_M[:, 1], label=r'$r_{2}$') - plt.plot(timespan * macros.NANO2SEC, r_PM_M[:, 2], label=r'$r_{3}$') - plt.plot(timespan * macros.NANO2SEC, r_PM_M_1_Ref, '--', label=r'$r_{1 Ref}$') - plt.plot(timespan * macros.NANO2SEC, r_PM_M_2_Ref, '--', label=r'$r_{2 Ref}$') - plt.plot(timespan * macros.NANO2SEC, r_PM_M_3_Ref, '--', label=r'$r_{3 Ref}$') - plt.title(r'${}^\mathcal{M} r_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(m)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='center left', prop={'size': 16}) + plt.plot(timespan * macros.NANO2SEC, r_PM_M[:, 0], label=r"$r_{1}$") + plt.plot(timespan * macros.NANO2SEC, r_PM_M[:, 1], label=r"$r_{2}$") + plt.plot(timespan * macros.NANO2SEC, r_PM_M[:, 2], label=r"$r_{3}$") + plt.plot(timespan * macros.NANO2SEC, r_PM_M_1_Ref, "--", label=r"$r_{1 Ref}$") + plt.plot(timespan * macros.NANO2SEC, r_PM_M_2_Ref, "--", label=r"$r_{2 Ref}$") + plt.plot(timespan * macros.NANO2SEC, r_PM_M_3_Ref, "--", label=r"$r_{3 Ref}$") + plt.title( + r"${}^\mathcal{M} r_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory", + fontsize=14, + ) + plt.ylabel("(m)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) + plt.legend(loc="center left", prop={"size": 16}) # Plot rPrime_PM_P plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, rPrime_PM_M[:, 0], label='1') - plt.plot(timespan * macros.NANO2SEC, rPrime_PM_M[:, 1], label='2') - plt.plot(timespan * macros.NANO2SEC, rPrime_PM_M[:, 2], label='3') - plt.title(r'${}^\mathcal{M} rPrime_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory', fontsize=14) - plt.ylabel('(m/s)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='upper left', prop={'size': 16}) + plt.plot(timespan * macros.NANO2SEC, rPrime_PM_M[:, 0], label="1") + plt.plot(timespan * macros.NANO2SEC, rPrime_PM_M[:, 1], label="2") + plt.plot(timespan * macros.NANO2SEC, rPrime_PM_M[:, 2], label="3") + plt.title( + r"${}^\mathcal{M} rPrime_{\mathcal{P}/\mathcal{M}}$ Profiled Trajectory", + fontsize=14, + ) + plt.ylabel("(m/s)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) + plt.legend(loc="upper left", prop={"size": 16}) # Plotting rPrimePrime_PM_M plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * rPrimePrime_PM_M[:, 0], label='1') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * rPrimePrime_PM_M[:, 1], label='2') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * rPrimePrime_PM_M[:, 2], label='3') - plt.title(r'${}^\mathcal{M} rPrimePrime_{\mathcal{P}/\mathcal{M}}$ Profiled Acceleration', fontsize=14) - plt.ylabel(r'(m/s$^2$)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='lower left', prop={'size': 16}) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * rPrimePrime_PM_M[:, 0], + label="1", + ) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * rPrimePrime_PM_M[:, 1], + label="2", + ) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * rPrimePrime_PM_M[:, 2], + label="3", + ) + plt.title( + r"${}^\mathcal{M} rPrimePrime_{\mathcal{P}/\mathcal{M}}$ Profiled Acceleration", + fontsize=14, + ) + plt.ylabel(r"(m/s$^2$)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) + plt.legend(loc="lower left", prop={"size": 16}) # Plot r_BN_N plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 0], label=r'$r_{1}$') - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 1], label=r'$r_{2}$') - plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 2], label=r'$r_{3}$') - plt.title(r'${}^\mathcal{N} r_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial Trajectory', fontsize=14) - plt.ylabel('(m)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='center left', prop={'size': 16}) + plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 0], label=r"$r_{1}$") + plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 1], label=r"$r_{2}$") + plt.plot(timespan * macros.NANO2SEC, r_BN_N[:, 2], label=r"$r_{3}$") + plt.title( + r"${}^\mathcal{N} r_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial Trajectory", + fontsize=14, + ) + plt.ylabel("(m)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) + plt.legend(loc="center left", prop={"size": 16}) # Plot sigma_BN plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 0], label=r'$\sigma_{1}$') - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 1], label=r'$\sigma_{2}$') - plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 2], label=r'$\sigma_{3}$') - plt.title(r'$\sigma_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial MRP Attitude', fontsize=14) - plt.ylabel('', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='lower left', prop={'size': 16}) + plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 0], label=r"$\sigma_{1}$") + plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 1], label=r"$\sigma_{2}$") + plt.plot(timespan * macros.NANO2SEC, sigma_BN[:, 2], label=r"$\sigma_{3}$") + plt.title( + r"$\sigma_{\mathcal{B}/\mathcal{N}}$ Spacecraft Inertial MRP Attitude", + fontsize=14, + ) + plt.ylabel("", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) + plt.legend(loc="lower left", prop={"size": 16}) # Plot omega_BN_B plt.figure() plt.clf() - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 0], label=r'$\omega_{1}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 1], label=r'$\omega_{2}$') - plt.plot(timespan * macros.NANO2SEC, (180 / np.pi) * omega_BN_B[:, 2], label=r'$\omega_{3}$') - plt.title(r'Spacecraft Hub Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$', fontsize=14) - plt.ylabel('(deg/s)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) - plt.legend(loc='lower left', prop={'size': 16}) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * omega_BN_B[:, 0], + label=r"$\omega_{1}$", + ) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * omega_BN_B[:, 1], + label=r"$\omega_{2}$", + ) + plt.plot( + timespan * macros.NANO2SEC, + (180 / np.pi) * omega_BN_B[:, 2], + label=r"$\omega_{3}$", + ) + plt.title( + r"Spacecraft Hub Angular Velocity ${}^\mathcal{B} \omega_{\mathcal{B}/\mathcal{N}}$", + fontsize=14, + ) + plt.ylabel("(deg/s)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) + plt.legend(loc="lower left", prop={"size": 16}) # Plotting: Conservation quantities plt.figure() plt.clf() - plt.plot(orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], - orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], - orbAngMom_N[:, 0] * macros.NANO2SEC, (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3]) - plt.title('Orbital Angular Momentum Relative Difference', fontsize=14) - plt.ylabel('(Nms)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) + plt.plot( + orbAngMom_N[:, 0] * macros.NANO2SEC, + (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * macros.NANO2SEC, + (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * macros.NANO2SEC, + (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3], + ) + plt.title("Orbital Angular Momentum Relative Difference", fontsize=14) + plt.ylabel("(Nms)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) plt.figure() plt.clf() - plt.plot(orbEnergy[:, 0] * macros.NANO2SEC, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1]) - plt.title('Orbital Energy Relative Difference', fontsize=14) - plt.ylabel('Energy (J)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) + plt.plot( + orbEnergy[:, 0] * macros.NANO2SEC, + (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1], + ) + plt.title("Orbital Energy Relative Difference", fontsize=14) + plt.ylabel("Energy (J)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) plt.figure() plt.clf() - plt.plot(rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]), - rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]), - rotAngMom_N[:, 0] * macros.NANO2SEC, (rotAngMom_N[:, 3] - rotAngMom_N[0, 3])) - plt.title('Rotational Angular Momentum Difference', fontsize=14) - plt.ylabel('(Nms)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) + plt.plot( + rotAngMom_N[:, 0] * macros.NANO2SEC, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]), + rotAngMom_N[:, 0] * macros.NANO2SEC, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]), + rotAngMom_N[:, 0] * macros.NANO2SEC, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]), + ) + plt.title("Rotational Angular Momentum Difference", fontsize=14) + plt.ylabel("(Nms)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) plt.figure() plt.clf() plt.plot(rotEnergy[:, 0] * macros.NANO2SEC, (rotEnergy[:, 1] - rotEnergy[0, 1])) - plt.title('Total Energy Difference', fontsize=14) - plt.ylabel('Energy (J)', fontsize=16) - plt.xlabel('Time (s)', fontsize=16) + plt.title("Total Energy Difference", fontsize=14) + plt.ylabel("Energy (J)", fontsize=16) + plt.xlabel("Time (s)", fontsize=16) if show_plots: plt.show() @@ -594,40 +790,71 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI finalOrbEnergy = np.delete(finalOrbEnergy, 0, axis=1) # remove the time column for i in range(0, len(initialOrbAngMom_N)): - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: Prescribed Motion integrated test failed orbital angular momentum unit test") + "FAILED: Prescribed Motion integrated test failed orbital angular momentum unit test" + ) for i in range(0, len(initialRotAngMom_N)): - if not unitTestSupport.isArrayEqual(finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqual( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: Prescribed Motion integrated test failed rotational angular momentum unit test") + "FAILED: Prescribed Motion integrated test failed rotational angular momentum unit test" + ) for i in range(0, len(initialOrbEnergy)): - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Prescribed Motion integrated test failed orbital energy unit test") + testMessages.append( + "FAILED: Prescribed Motion integrated test failed orbital energy unit test" + ) # Check to ensure the initial velocity converged to the reference velocity rPrime_PM_M_Ref = np.array([0.0, 0.0, 0.0]) - if not unitTestSupport.isArrayEqual(rPrime_PM_M_Final, rPrime_PM_M_Ref, 3, accuracy): + if not unitTestSupport.isArrayEqual( + rPrime_PM_M_Final, rPrime_PM_M_Ref, 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: " + PrescribedTrans.ModelTag + "rPrime_PM_M_Final and rPrime_PM_M_Ref do not match") - testMessages.append("rPrime_PM_M_Final: " + str(rPrime_PM_M_Final) + " rPrime_PM_M_Ref: " + str(rPrime_PM_M_Ref)) + testMessages.append( + "FAILED: " + + PrescribedTrans.ModelTag + + "rPrime_PM_M_Final and rPrime_PM_M_Ref do not match" + ) + testMessages.append( + "rPrime_PM_M_Final: " + + str(rPrime_PM_M_Final) + + " rPrime_PM_M_Ref: " + + str(rPrime_PM_M_Ref) + ) # Check to ensure the initial position converged to the reference position r_PM_M_Ref = np.array([posRef, 0.0, 0.0]) if not unitTestSupport.isArrayEqual(r_PM_M_Final, r_PM_M_Ref, 3, accuracy): testFailCount += 1 - testMessages.append("FAILED: " + PrescribedTrans.ModelTag + "r_PM_M_Final and r_PM_M_Ref do not match") - testMessages.append("r_PM_M_Final: " + str(r_PM_M_Final) + " r_PM_M_Ref: " + str(r_PM_M_Ref)) + testMessages.append( + "FAILED: " + + PrescribedTrans.ModelTag + + "r_PM_M_Final and r_PM_M_Ref do not match" + ) + testMessages.append( + "r_PM_M_Final: " + str(r_PM_M_Final) + " r_PM_M_Ref: " + str(r_PM_M_Ref) + ) if testFailCount == 0: - print("PASSED: " + "prescribedMotion and prescribedLinearTranslation integrated test") + print( + "PASSED: " + + "prescribedMotion and prescribedLinearTranslation integrated test" + ) + + return [testFailCount, "".join(testMessages)] - return [testFailCount, ''.join(testMessages)] # # This statement below ensures that the unitTestScript can be run as a @@ -635,12 +862,12 @@ def PrescribedMotionTestFunction(show_plots, rotTest, thetaInit, theta_Ref, posI # if __name__ == "__main__": PrescribedMotionTestFunction( - True, # show_plots - False, # rotTest, (True: prescribedRot1DOF integrated test, - # False: prescribedTrans integrated test) - 0.0, # thetaInit [rad] - np.pi / 12, # theta_Ref [rad] - 0.0, # posInit [m] - 0.5, # posRef [m] - 1e-8 # accuracy - ) + True, # show_plots + False, # rotTest, (True: prescribedRot1DOF integrated test, + # False: prescribedTrans integrated test) + 0.0, # thetaInit [rad] + np.pi / 12, # theta_Ref [rad] + 0.0, # posInit [m] + 0.5, # posRef [m] + 1e-8, # accuracy + ) diff --git a/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelMemoryLeak.py b/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelMemoryLeak.py index d83f7c2d9c..3d14b9029d 100644 --- a/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelMemoryLeak.py +++ b/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelMemoryLeak.py @@ -11,6 +11,7 @@ import psutil import os + def getMemoryUsage(): """Get memory usage of current process in MB""" process = psutil.Process(os.getpid()) @@ -18,6 +19,7 @@ def getMemoryUsage(): memory_info = process.memory_info() return memory_info.rss / 1024 / 1024 # RSS in MB + def create_and_run_simulation(): """Create and run a simulation with RW setup""" # Create simulation variable names @@ -35,10 +37,12 @@ def create_and_run_simulation(): # Create 3 reaction wheels for i in range(3): - rwFactory.create('Honeywell_HR16', - [1, 0, 0] if i == 0 else [0, 1, 0] if i == 1 else [0, 0, 1], - Omega=500., - maxMomentum=50.) + rwFactory.create( + "Honeywell_HR16", + [1, 0, 0] if i == 0 else [0, 1, 0] if i == 1 else [0, 0, 1], + Omega=500.0, + maxMomentum=50.0, + ) # Create RW state effector rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector() @@ -67,7 +71,7 @@ def create_and_run_simulation(): # Clear message subscriptions for msg in rwStateEffector.rwOutMsgs: - if hasattr(msg, 'unsubscribeAll'): + if hasattr(msg, "unsubscribeAll"): msg.unsubscribeAll() # Clear references in reverse order @@ -79,10 +83,14 @@ def create_and_run_simulation(): gc.collect() -@pytest.mark.parametrize("num_iterations,max_allowed_growth", [ - (25, 3.0), # Reduced from 50 to 25 iterations - (50, 3.0), # Reduced from 100 to 50 iterations -]) + +@pytest.mark.parametrize( + "num_iterations,max_allowed_growth", + [ + (25, 3.0), # Reduced from 50 to 25 iterations + (50, 3.0), # Reduced from 100 to 50 iterations + ], +) @pytest.mark.flaky(retries=3, delay=0) def test_rw_memory_leak(num_iterations, max_allowed_growth): """Test for memory leaks in reaction wheel implementation""" @@ -99,21 +107,30 @@ def test_rw_memory_leak(num_iterations, max_allowed_growth): memory_measurements.append(current_memory) if (i + 1) % 10 == 0: - print(f"Iteration {i+1}/{num_iterations}, Memory: {current_memory:.2f} MB") + print( + f"Iteration {i + 1}/{num_iterations}, Memory: {current_memory:.2f} MB" + ) print(f"Delta from start: {current_memory - initial_memory:.2f} MB") # Calculate memory statistics memory_growth = memory_measurements[-1] - initial_memory - memory_trend = np.polyfit(range(len(memory_measurements)), memory_measurements, 1)[0] + memory_trend = np.polyfit(range(len(memory_measurements)), memory_measurements, 1)[ + 0 + ] # More detailed failure messages if memory_growth >= max_allowed_growth: - pytest.fail(f"Memory growth ({memory_growth:.2f} MB) exceeds maximum allowed " - f"({max_allowed_growth:.2f} MB)\nTrend: {memory_trend:.4f} MB/iteration") + pytest.fail( + f"Memory growth ({memory_growth:.2f} MB) exceeds maximum allowed " + f"({max_allowed_growth:.2f} MB)\nTrend: {memory_trend:.4f} MB/iteration" + ) if memory_trend >= 0.05: - pytest.fail(f"Memory growth trend ({memory_trend:.4f} MB/iteration) indicates " - f"potential leak\nTotal growth: {memory_growth:.2f} MB") + pytest.fail( + f"Memory growth trend ({memory_trend:.4f} MB/iteration) indicates " + f"potential leak\nTotal growth: {memory_growth:.2f} MB" + ) + if __name__ == "__main__": test_rw_memory_leak(50, 3.0) diff --git a/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_ConfigureRWRequests.py b/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_ConfigureRWRequests.py index 02ad1e497a..75f0e09c21 100755 --- a/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_ConfigureRWRequests.py +++ b/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_ConfigureRWRequests.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -39,9 +38,9 @@ # methods -def listStack(vec,simStopTime,unitProcRate): +def listStack(vec, simStopTime, unitProcRate): # returns a list duplicated the number of times needed to be consistent with module output - return [vec] * int(simStopTime/(float(unitProcRate)/float(macros.sec2nano(1)))) + return [vec] * int(simStopTime / (float(unitProcRate) / float(macros.sec2nano(1)))) def writeNewRWCmds(self, u_cmd, numRW): @@ -57,10 +56,10 @@ def writeNewRWCmds(self, u_cmd, numRW): def defaultReactionWheel(): RW = reactionWheelStateEffector.RWConfigPayload() - RW.rWB_B = [[0.], [0.], [0.]] - RW.gsHat_B = [[1.], [0.], [0.]] - RW.w2Hat0_B = [[0.], [1.], [0.]] - RW.w3Hat0_B = [[0.], [0.], [1.]] + RW.rWB_B = [[0.0], [0.0], [0.0]] + RW.gsHat_B = [[1.0], [0.0], [0.0]] + RW.w2Hat0_B = [[0.0], [1.0], [0.0]] + RW.w3Hat0_B = [[0.0], [0.0], [1.0]] RW.RWModel = reactionWheelStateEffector.BalancedWheels return RW @@ -71,6 +70,7 @@ def asEigen(v): out.append([v[i]]) return out + # uncomment this line is 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 @@ -78,13 +78,15 @@ def asEigen(v): # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("useFlag, testCase", [ - (False, 'saturation'), - (False, 'minimum'), - (False, 'speedSaturation'), - (False, 'powerSaturation') -]) - +@pytest.mark.parametrize( + "useFlag, testCase", + [ + (False, "saturation"), + (False, "minimum"), + (False, "speedSaturation"), + (False, "powerSaturation"), + ], +) # provide a unique test method name, starting with test_ def test_unitSimReactionWheel(show_plots, useFlag, testCase): @@ -112,62 +114,62 @@ def unitSimReactionWheel(show_plots, useFlag, testCase): expOut = dict() # expected output print(testCase) - if testCase == 'basic': + if testCase == "basic": pass - elif testCase == 'saturation': + elif testCase == "saturation": RWs.append(defaultReactionWheel()) - RWs[0].u_max = 1. - RWs[1].u_max = 2. - RWs[2].u_max = 2. + RWs[0].u_max = 1.0 + RWs[1].u_max = 2.0 + RWs[2].u_max = 2.0 u_cmd = [-1.2, 1.5, 2.5] writeNewRWCmds(ReactionWheel, u_cmd, len(RWs)) - expOut['u_current'] = [-1., 1.5, 2.] + expOut["u_current"] = [-1.0, 1.5, 2.0] - elif testCase == 'minimum': - RWs[0].u_min = .1 - RWs[1].u_min = .0 - u_cmd = [-.09, 0.0001] + elif testCase == "minimum": + RWs[0].u_min = 0.1 + RWs[1].u_min = 0.0 + u_cmd = [-0.09, 0.0001] writeNewRWCmds(ReactionWheel, u_cmd, len(RWs)) - expOut['u_current'] = [0., 0.0001] + expOut["u_current"] = [0.0, 0.0001] - elif testCase == 'speedSaturation': + elif testCase == "speedSaturation": RWs.append(defaultReactionWheel()) - RWs[0].Omega_max = 50. - RWs[1].Omega_max = 50. - RWs[2].Omega_max = 50. - RWs[0].Omega = 49. - RWs[1].Omega = 51. - RWs[2].Omega = -52. + RWs[0].Omega_max = 50.0 + RWs[1].Omega_max = 50.0 + RWs[2].Omega_max = 50.0 + RWs[0].Omega = 49.0 + RWs[1].Omega = 51.0 + RWs[2].Omega = -52.0 u_cmd = [1.5, 1.5, 1.5] writeNewRWCmds(ReactionWheel, u_cmd, len(RWs)) - expOut['u_current'] = [1.5, 0.0, 1.5] + expOut["u_current"] = [1.5, 0.0, 1.5] - elif testCase == 'powerSaturation': + elif testCase == "powerSaturation": RWs.append(defaultReactionWheel()) - RWs[0].P_max = 1. - RWs[1].P_max = 1. - RWs[2].P_max = 1. - RWs[0].Omega = 50. - RWs[1].Omega = 50. - RWs[2].Omega = 50. + RWs[0].P_max = 1.0 + RWs[1].P_max = 1.0 + RWs[2].P_max = 1.0 + RWs[0].Omega = 50.0 + RWs[1].Omega = 50.0 + RWs[2].Omega = 50.0 u_cmd = [0.01, -0.04, 0.04] writeNewRWCmds(ReactionWheel, u_cmd, len(RWs)) - expOut['u_current'] = [0.01, -0.02, 0.02] + expOut["u_current"] = [0.01, -0.02, 0.02] else: - raise Exception('invalid test case') + raise Exception("invalid test case") for i in range(0, len(RWs)): ReactionWheel.addReactionWheel(RWs[i]) - ReactionWheel.ConfigureRWRequests(0.) + ReactionWheel.ConfigureRWRequests(0.0) - if 'accuracy' not in vars(): + if "accuracy" not in vars(): accuracy = 1e-10 for outputName in list(expOut.keys()): @@ -182,27 +184,32 @@ def unitSimReactionWheel(show_plots, useFlag, testCase): if testFail: testFailCount += 1 - testMessages.append("FAILED: " + ReactionWheel.ModelTag + " Module failed " + - outputName + " unit test") + testMessages.append( + "FAILED: " + + ReactionWheel.ModelTag + + " Module failed " + + outputName + + " unit test" + ) np.set_printoptions(precision=16) # print out success message if no errors were found if testFailCount == 0: print("PASSED ") - colorText = 'ForestGreen' - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + colorText = "ForestGreen" + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' - passedText = r'\textcolor{' + colorText + '}{' + "FAILED" + '}' + colorText = "Red" + passedText = r"\textcolor{" + colorText + "}{" + "FAILED" + "}" # Write some snippets for AutoTex - snippetName = testCase + 'PassFail' + snippetName = testCase + "PassFail" unitTestSupport.writeTeXSnippet(snippetName, passedText, path) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # This statement below ensures that the unit test script can be run as a @@ -211,5 +218,5 @@ def unitSimReactionWheel(show_plots, useFlag, testCase): test_unitSimReactionWheel( False, # show_plots False, # useFlag - 'speedSaturation' # testCase + "speedSaturation", # testCase ) diff --git a/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_RWUpdate.py b/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_RWUpdate.py index 03effde941..4b603b03d1 100644 --- a/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_RWUpdate.py +++ b/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_RWUpdate.py @@ -32,10 +32,12 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') +splitPath = path.split("simulation") from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.simulation import spacecraft from Basilisk.utilities import macros from Basilisk.utilities import simIncludeRW @@ -72,8 +74,8 @@ def test_RWUpdate(show_plots, accuracy): [testResults, testMessage] = RWUpdateTest(show_plots, accuracy) assert testResults < 1, testMessage -def RWUpdateTest(show_plots, accuracy): +def RWUpdateTest(show_plots, accuracy): testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages @@ -85,13 +87,13 @@ def RWUpdateTest(show_plots, accuracy): unitTestSim = SimulationBaseClass.SimBaseClass() # set the simulation time variable used later on - simulationTime = macros.sec2nano(1.) + simulationTime = macros.sec2nano(1.0) # # create the simulation process # - testProcessRate = macros.sec2nano(1.) # update process rate update time + testProcessRate = macros.sec2nano(1.0) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -114,24 +116,27 @@ def RWUpdateTest(show_plots, accuracy): varRWModel = messaging.BalancedWheels # create the reaction wheels - RW1 = rwFactory.create('Honeywell_HR16' - , [1, 0, 0] # gsHat_B - , Omega=500. # RPM - , maxMomentum=50. - , RWModel=varRWModel - ) - RW2 = rwFactory.create('Honeywell_HR16' - , [0, 1, 0] # gsHat_B - , Omega=500. # RPM - , maxMomentum=50. - , RWModel=varRWModel - ) - RW3 = rwFactory.create('Honeywell_HR16' - , [0, 0, 1] # gsHat_B - , Omega=500. # RPM - , maxMomentum=50. - , RWModel=varRWModel - ) + RW1 = rwFactory.create( + "Honeywell_HR16", + [1, 0, 0], # gsHat_B + Omega=500.0, # RPM + maxMomentum=50.0, + RWModel=varRWModel, + ) + RW2 = rwFactory.create( + "Honeywell_HR16", + [0, 1, 0], # gsHat_B + Omega=500.0, # RPM + maxMomentum=50.0, + RWModel=varRWModel, + ) + RW3 = rwFactory.create( + "Honeywell_HR16", + [0, 0, 1], # gsHat_B + Omega=500.0, # RPM + maxMomentum=50.0, + RWModel=varRWModel, + ) numRW = rwFactory.getNumOfDevices() # create RW object container and tie to spacecraft object @@ -144,7 +149,7 @@ def RWUpdateTest(show_plots, accuracy): cmdArray.motorTorque = [0.4, 0.1, -0.5] # [Nm] cmdMsg = messaging.ArrayMotorTorqueMsg().write(cmdArray) rwStateEffector.rwMotorCmdInMsg.subscribeTo(cmdMsg) - trueTorque = [[0.4, 0., -0.5]] + trueTorque = [[0.4, 0.0, -0.5]] # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, rwStateEffector) @@ -184,7 +189,7 @@ def RWUpdateTest(show_plots, accuracy): numTests += 1 # expected output - trueTorque.append([0.4, 0., -0.5]) + trueTorque.append([0.4, 0.0, -0.5]) # # Second test @@ -199,7 +204,7 @@ def RWUpdateTest(show_plots, accuracy): RW3.u_min = 0.05 # reconfigure a simulation stop time and re-execute the simulation run - unitTestSim.ConfigureStopTime(2*simulationTime) + unitTestSim.ConfigureStopTime(2 * simulationTime) unitTestSim.ExecuteSimulation() numTests += 1 @@ -219,7 +224,7 @@ def RWUpdateTest(show_plots, accuracy): RW3.Omega = 2 # reconfigure a simulation stop time and re-execute the simulation run - unitTestSim.ConfigureStopTime(3*simulationTime) + unitTestSim.ConfigureStopTime(3 * simulationTime) unitTestSim.ExecuteSimulation() numTests += 1 @@ -234,20 +239,20 @@ def RWUpdateTest(show_plots, accuracy): RW1.P_max = -1 RW2.P_max = -1 RW3.P_max = -1 - RW1.Omega = 100*macros.RPM - RW2.Omega = -200*macros.RPM - RW3.Omega = 50*macros.RPM - RW1.Omega_max = 100*macros.RPM - RW2.Omega_max = 100*macros.RPM - RW3.Omega_max = 100*macros.RPM + RW1.Omega = 100 * macros.RPM + RW2.Omega = -200 * macros.RPM + RW3.Omega = 50 * macros.RPM + RW1.Omega_max = 100 * macros.RPM + RW2.Omega_max = 100 * macros.RPM + RW3.Omega_max = 100 * macros.RPM # reconfigure a simulation stop time and re-execute the simulation run - unitTestSim.ConfigureStopTime(4*simulationTime) + unitTestSim.ConfigureStopTime(4 * simulationTime) unitTestSim.ExecuteSimulation() numTests += 1 # expected output - trueTorque.append([0., 0.1, -0.3]) + trueTorque.append([0.0, 0.1, -0.3]) # # retrieve the logged data @@ -263,27 +268,32 @@ def RWUpdateTest(show_plots, accuracy): # # do the comparison - for i in range(numTests+1): + for i in range(numTests + 1): # check a vector values - if not unitTestSupport.isArrayEqual(dataRW[:, i], trueTorque[i], numRW, accuracy): + if not unitTestSupport.isArrayEqual( + dataRW[:, i], trueTorque[i], numRW, accuracy + ): testFailCount += 1 if i == 0: testMessages.append("FAILED: Reaction Wheel Update Test failed setup") else: - testMessages.append("FAILED: Reaction Wheel Update Test failed test " + str(i) + "\n") + testMessages.append( + "FAILED: Reaction Wheel Update Test failed test " + str(i) + "\n" + ) if not testFailCount: print("PASSED") # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + # # Run this unitTest as a standalone python script # if __name__ == "__main__": test_RWUpdate( - False # show_plots - , 1e-8 # accuracy + False, # show_plots + 1e-8, # accuracy ) diff --git a/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_integrated.py b/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_integrated.py index f72e7da59f..a12b25bae7 100644 --- a/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_integrated.py +++ b/src/simulation/dynamics/reactionWheels/_UnitTest/test_reactionWheelStateEffector_integrated.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -24,10 +23,12 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') +splitPath = path.split("simulation") from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import matplotlib as mpl import matplotlib.pyplot as plt from Basilisk.simulation import spacecraft @@ -39,26 +40,33 @@ mpl.rc("figure", figsize=(5.75, 4)) -@pytest.mark.parametrize("useFlag, testCase", [ - (False,'BalancedWheels'), - (False,'JitterSimple'), - (False,'JitterFullyCoupled'), - (False, 'BOE'), - (False, 'FrictionSpinDown'), - (False, 'FrictionSpinUp') -]) + +@pytest.mark.parametrize( + "useFlag, testCase", + [ + (False, "BalancedWheels"), + (False, "JitterSimple"), + (False, "JitterFullyCoupled"), + (False, "BOE"), + (False, "FrictionSpinDown"), + (False, "FrictionSpinUp"), + ], +) # uncomment this line is 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() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -def test_reactionWheelIntegratedTest(show_plots,useFlag,testCase): +def test_reactionWheelIntegratedTest(show_plots, useFlag, testCase): """Module Unit Test""" - [testResults, testMessage] = reactionWheelIntegratedTest(show_plots,useFlag,testCase) + [testResults, testMessage] = reactionWheelIntegratedTest( + show_plots, useFlag, testCase + ) assert testResults < 1, testMessage -def reactionWheelIntegratedTest(show_plots,useFlag,testCase): + +def reactionWheelIntegratedTest(show_plots, useFlag, testCase): # The __tracebackhide__ setting influences pytest showing of tracebacks: # the mrp_steering_tracking() function will not be shown unless the # --fulltrace command line option is specified. @@ -78,11 +86,11 @@ def reactionWheelIntegratedTest(show_plots,useFlag,testCase): # Create test thread stepSize = 0.0001 - if testCase == 'BOE': + if testCase == "BOE": stepSize = 0.1 - if testCase == 'FrictionSpinDown' or testCase == 'FrictionSpinUp': + if testCase == "FrictionSpinDown" or testCase == "FrictionSpinUp": stepSize = 0.01 - if testCase == 'JitterFullyCoupled': + if testCase == "JitterFullyCoupled": stepSize = 0.00001 testProcessRate = macros.sec2nano(stepSize) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) @@ -91,86 +99,105 @@ def reactionWheelIntegratedTest(show_plots,useFlag,testCase): # add RW devices # The clearRWSetup() is critical if the script is to run multiple times rwFactory = simIncludeRW.rwFactory() - varMaxMomentum = 100. # Nms - - if testCase == 'BalancedWheels' or testCase == 'BOE' or testCase == 'FrictionSpinDown' or testCase == 'FrictionSpinUp': + varMaxMomentum = 100.0 # Nms + + if ( + testCase == "BalancedWheels" + or testCase == "BOE" + or testCase == "FrictionSpinDown" + or testCase == "FrictionSpinUp" + ): varRWModel = reactionWheelStateEffector.BalancedWheels - elif testCase == 'JitterSimple': + elif testCase == "JitterSimple": varRWModel = reactionWheelStateEffector.JitterSimple - elif testCase == 'JitterFullyCoupled': + elif testCase == "JitterFullyCoupled": varRWModel = reactionWheelStateEffector.JitterFullyCoupled - if testCase == 'BalancedWheels' or testCase == 'JitterSimple' or testCase == 'JitterFullyCoupled': + if ( + testCase == "BalancedWheels" + or testCase == "JitterSimple" + or testCase == "JitterFullyCoupled" + ): rwFactory.create( - 'Honeywell_HR16' - ,[1,0,0] # gsHat_B - ,Omega = 500. # RPM - ,rWB_B = [0.1,0.,0.] # m - ,maxMomentum = varMaxMomentum - ,RWModel= varRWModel - ,fCoulomb=10 - ,fStatic=5 - ,cViscous=3 - ,useRWfriction=True - ) - assert rwFactory.rwList["RW1"].fCoulomb == 10, "wasn't able to set custom fCoulomb value" - assert rwFactory.rwList["RW1"].fStatic == 5, "wasn't able to set custom fStatic value" - assert rwFactory.rwList["RW1"].cViscous == 3, "wasn't able to set custom cViscous value" + "Honeywell_HR16", + [1, 0, 0], # gsHat_B + Omega=500.0, # RPM + rWB_B=[0.1, 0.0, 0.0], # m + maxMomentum=varMaxMomentum, + RWModel=varRWModel, + fCoulomb=10, + fStatic=5, + cViscous=3, + useRWfriction=True, + ) + assert rwFactory.rwList["RW1"].fCoulomb == 10, ( + "wasn't able to set custom fCoulomb value" + ) + assert rwFactory.rwList["RW1"].fStatic == 5, ( + "wasn't able to set custom fStatic value" + ) + assert rwFactory.rwList["RW1"].cViscous == 3, ( + "wasn't able to set custom cViscous value" + ) # reset RW values to continue the test rwFactory.rwList["RW1"].fCoulomb = 0.0 rwFactory.rwList["RW1"].fStatic = 0.0 rwFactory.rwList["RW1"].cViscous = 0.0 rwFactory.create( - 'Honeywell_HR16', - [0,1,0] # gsHat_B - ,Omega = 200. # RPM - ,rWB_B = [0.,0.1,0.] # m - ,maxMomentum = varMaxMomentum - ,RWModel= varRWModel - ) + "Honeywell_HR16", + [0, 1, 0], # gsHat_B + Omega=200.0, # RPM + rWB_B=[0.0, 0.1, 0.0], # m + maxMomentum=varMaxMomentum, + RWModel=varRWModel, + ) rwFactory.create( - 'Honeywell_HR16' - ,[0,0,1] # gsHat_B - ,Omega = -150. # RPM - ,rWB_B = [0.,0.,0.1] # m - ,maxMomentum = varMaxMomentum - ,RWModel= varRWModel - ) - if testCase == 'BOE' or testCase == 'FrictionSpinDown' or testCase == 'FrictionSpinUp': - initialWheelSpeed = 100. + "Honeywell_HR16", + [0, 0, 1], # gsHat_B + Omega=-150.0, # RPM + rWB_B=[0.0, 0.0, 0.1], # m + maxMomentum=varMaxMomentum, + RWModel=varRWModel, + ) + if ( + testCase == "BOE" + or testCase == "FrictionSpinDown" + or testCase == "FrictionSpinUp" + ): + initialWheelSpeed = 100.0 rwCopy1 = rwFactory.create( - 'Honeywell_HR16' - ,[0,0,1] # gsHat_B - ,Omega = initialWheelSpeed # RPM - ,rWB_B = [0.0,0.,0.] # m - ,maxMomentum = varMaxMomentum - ,RWModel= varRWModel - ) - if testCase == 'FrictionSpinDown' or testCase == 'FrictionSpinUp': + "Honeywell_HR16", + [0, 0, 1], # gsHat_B + Omega=initialWheelSpeed, # RPM + rWB_B=[0.0, 0.0, 0.0], # m + maxMomentum=varMaxMomentum, + RWModel=varRWModel, + ) + if testCase == "FrictionSpinDown" or testCase == "FrictionSpinUp": rwCopy1.fCoulomb = 0.03 rwCopy1.fStatic = 0.06 rwCopy1.betaStatic = 0.15 rwCopy1.cViscous = 0.001 rwCopy1.omegaLimitCycle = 0.001 - rwCopy1.Omega = 15. - rwCopy1.gsHat_B = [[np.sqrt(3)/3], [np.sqrt(3)/3], [np.sqrt(3)/3]] - rwCopy1.rWB_B = [[0.5],[-0.5],[0.5]] + rwCopy1.Omega = 15.0 + rwCopy1.gsHat_B = [[np.sqrt(3) / 3], [np.sqrt(3) / 3], [np.sqrt(3) / 3]] + rwCopy1.rWB_B = [[0.5], [-0.5], [0.5]] rwCopy2 = rwFactory.create( - 'Honeywell_HR16' - ,[np.sqrt(3)/3,np.sqrt(3)/3,np.sqrt(3)/3] # gsHat_B - ,Omega = -initialWheelSpeed # RPM - ,rWB_B = [-0.5,0.5,-0.5] # m - ,maxMomentum = varMaxMomentum - ,RWModel= varRWModel - ) + "Honeywell_HR16", + [np.sqrt(3) / 3, np.sqrt(3) / 3, np.sqrt(3) / 3], # gsHat_B + Omega=-initialWheelSpeed, # RPM + rWB_B=[-0.5, 0.5, -0.5], # m + maxMomentum=varMaxMomentum, + RWModel=varRWModel, + ) rwCopy2.fCoulomb = 0.03 rwCopy2.fStatic = 0.06 rwCopy2.betaStatic = 0.15 rwCopy2.cViscous = 0.001 rwCopy2.omegaLimitCycle = 0.001 - rwCopy2.Omega = -15. - if testCase == 'FrictionSpinUp': + rwCopy2.Omega = -15.0 + if testCase == "FrictionSpinUp": rwCopy1.Omega = 0.0 rwCopy2.Omega = 0.0 @@ -185,11 +212,15 @@ def reactionWheelIntegratedTest(show_plots,useFlag,testCase): # set RW torque command cmdArray = messaging.ArrayMotorTorqueMsgPayload() - if testCase == 'BalancedWheels' or testCase == 'JitterSimple' or testCase == 'JitterFullyCoupled': - cmdArray.motorTorque = [0.20, 0.10, -0.50] # [Nm] - if testCase == 'BOE' or testCase == 'FrictionSpinDown': - cmdArray.motorTorque = [0.0] # [Nm] - if testCase == 'FrictionSpinUp': + if ( + testCase == "BalancedWheels" + or testCase == "JitterSimple" + or testCase == "JitterFullyCoupled" + ): + cmdArray.motorTorque = [0.20, 0.10, -0.50] # [Nm] + if testCase == "BOE" or testCase == "FrictionSpinDown": + cmdArray.motorTorque = [0.0] # [Nm] + if testCase == "FrictionSpinUp": cmdArray.motorTorque = [0.1, -0.1] cmdMsg = messaging.ArrayMotorTorqueMsg().write(cmdArray) rwStateEffector.rwMotorCmdInMsg.subscribeTo(cmdMsg) @@ -198,13 +229,19 @@ def reactionWheelIntegratedTest(show_plots,useFlag,testCase): unitTestSim.AddModelToTask(unitTaskName, rwStateEffector) unitTestSim.AddModelToTask(unitTaskName, scObject) - if testCase == 'BalancedWheels' or testCase == 'JitterSimple' or testCase == 'JitterFullyCoupled': + if ( + testCase == "BalancedWheels" + or testCase == "JitterSimple" + or testCase == "JitterFullyCoupled" + ): unitTestSim.earthGravBody = gravityEffector.GravBodyData() unitTestSim.earthGravBody.planetName = "earth_planet_data" - unitTestSim.earthGravBody.mu = 0.3986004415E+15 # meters! + unitTestSim.earthGravBody.mu = 0.3986004415e15 # meters! unitTestSim.earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([unitTestSim.earthGravBody]) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + [unitTestSim.earthGravBody] + ) # log data scDataLog = scObject.scStateOutMsg.recorder() @@ -212,25 +249,48 @@ def reactionWheelIntegratedTest(show_plots,useFlag,testCase): unitTestSim.AddModelToTask(unitTaskName, scDataLog) unitTestSim.AddModelToTask(unitTaskName, speedDataLog) - # Define initial conditions of the sim - if testCase == 'BalancedWheels' or testCase == 'JitterSimple' or testCase == 'JitterFullyCoupled': + if ( + testCase == "BalancedWheels" + or testCase == "JitterSimple" + or testCase == "JitterFullyCoupled" + ): scObject.hub.r_BcB_B = [[-0.0002], [0.0001], [0.1]] - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + scObject.hub.r_CN_NInit = [ + [-4020338.690396649], + [7490566.741852513], + [5248299.211589362], + ] + scObject.hub.v_CN_NInit = [ + [-5199.77710904224], + [-3436.681645356935], + [1041.576797498721], + ] scObject.hub.omega_BN_BInit = [[0.08], [0.01], [0.0]] scObject.hub.mHub = 750.0 - scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] + scObject.hub.IHubPntBc_B = [ + [900.0, 0.0, 0.0], + [0.0, 800.0, 0.0], + [0.0, 0.0, 600.0], + ] scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] - if testCase == 'BOE' or testCase == 'FrictionSpinDown' or testCase == 'FrictionSpinUp': - wheelSpeedMax = 6000.0*macros.RPM - wheelJs = varMaxMomentum/wheelSpeedMax + if ( + testCase == "BOE" + or testCase == "FrictionSpinDown" + or testCase == "FrictionSpinUp" + ): + wheelSpeedMax = 6000.0 * macros.RPM + wheelJs = varMaxMomentum / wheelSpeedMax scObject.hub.mHub = 5.0 I1Hub = 2.0 - scObject.hub.IHubPntBc_B = [[2., 0.0, 0.0], [0.0, 2., 0.0], [0.0, 0.0, I1Hub + wheelJs]] + scObject.hub.IHubPntBc_B = [ + [2.0, 0.0, 0.0], + [0.0, 2.0, 0.0], + [0.0, 0.0, I1Hub + wheelJs], + ] scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.35]] - if testCase == 'FrictionSpinDown' or testCase == 'FrictionSpinUp': + if testCase == "FrictionSpinDown" or testCase == "FrictionSpinUp": scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] rw0DataLog = rwStateEffector.rwOutMsgs[0].recorder() rw1DataLog = rwStateEffector.rwOutMsgs[1].recorder() @@ -239,24 +299,30 @@ def reactionWheelIntegratedTest(show_plots,useFlag,testCase): scObject.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] scObject.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) unitTestSim.InitializeSimulation() stopTime = 1.0 - if testCase == 'BOE': + if testCase == "BOE": stopTime = 10.0 - if testCase == 'FrictionSpinDown' or testCase == 'FrictionSpinUp': + if testCase == "FrictionSpinDown" or testCase == "FrictionSpinUp": stopTime = 100.0 - if testCase == 'JitterFullyCoupled': + if testCase == "JitterFullyCoupled": stopTime = 0.1 - unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime/2)) + unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime / 2)) unitTestSim.ExecuteSimulation() - if testCase == 'BalancedWheels' or testCase == 'JitterSimple' or testCase == 'JitterFullyCoupled': - cmdArray.motorTorque = [0.0, 0.0, 0.0] # [Nm] - if testCase == 'BOE': + if ( + testCase == "BalancedWheels" + or testCase == "JitterSimple" + or testCase == "JitterFullyCoupled" + ): + cmdArray.motorTorque = [0.0, 0.0, 0.0] # [Nm] + if testCase == "BOE": motorTorque = 0.2 cmdArray.motorTorque = [motorTorque] cmdMsg.write(cmdArray) @@ -264,18 +330,30 @@ def reactionWheelIntegratedTest(show_plots,useFlag,testCase): unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) unitTestSim.ExecuteSimulation() - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) posData = scDataLog.r_BN_N sigmaData = scDataLog.sigma_BN omegaData = scDataLog.omega_BN_B - if testCase == 'BOE' or testCase == 'FrictionSpinDown' or testCase == 'FrictionSpinUp': + if ( + testCase == "BOE" + or testCase == "FrictionSpinDown" + or testCase == "FrictionSpinUp" + ): wheelSpeeds = speedDataLog.wheelSpeeds[:, 0] - if testCase == 'BOE': - thetaOut = 4.0*np.arctan(sigmaData[:, 2]) + if testCase == "BOE": + thetaOut = 4.0 * np.arctan(sigmaData[:, 2]) # Find BOE calculations timeBOE = np.array([2.0, 4.0, 6.0, 8.0, 10.0]) timeTorqueOn = 5.0 @@ -284,14 +362,22 @@ def reactionWheelIntegratedTest(show_plots,useFlag,testCase): wheelSpeedBOE = np.zeros(5) for i in range(5): if timeBOE[i] > timeTorqueOn: - omegaBOE[i] = scObject.hub.omega_BN_BInit[2][0] - motorTorque/I1Hub*(timeBOE[i]-timeTorqueOn) - thetaBOE[i] = scObject.hub.omega_BN_BInit[2][0]*(timeBOE[i]-timeTorqueOn) - 0.5*motorTorque/I1Hub*(timeBOE[i]-timeTorqueOn)**2 + scObject.hub.omega_BN_BInit[2][0]*(timeTorqueOn) - wheelSpeedBOE[i] = initialWheelSpeed*macros.RPM + (I1Hub + wheelJs)*motorTorque/(I1Hub*wheelJs)*(timeBOE[i]-timeTorqueOn) + omegaBOE[i] = scObject.hub.omega_BN_BInit[2][ + 0 + ] - motorTorque / I1Hub * (timeBOE[i] - timeTorqueOn) + thetaBOE[i] = ( + scObject.hub.omega_BN_BInit[2][0] * (timeBOE[i] - timeTorqueOn) + - 0.5 * motorTorque / I1Hub * (timeBOE[i] - timeTorqueOn) ** 2 + + scObject.hub.omega_BN_BInit[2][0] * (timeTorqueOn) + ) + wheelSpeedBOE[i] = initialWheelSpeed * macros.RPM + ( + I1Hub + wheelJs + ) * motorTorque / (I1Hub * wheelJs) * (timeBOE[i] - timeTorqueOn) else: omegaBOE[i] = scObject.hub.omega_BN_BInit[2][0] - wheelSpeedBOE[i] = initialWheelSpeed*macros.RPM - thetaBOE[i] = scObject.hub.omega_BN_BInit[2][0]*(timeBOE[i]) - if testCase == 'FrictionSpinDown' or testCase == 'FrictionSpinUp': + wheelSpeedBOE[i] = initialWheelSpeed * macros.RPM + thetaBOE[i] = scObject.hub.omega_BN_BInit[2][0] * (timeBOE[i]) + if testCase == "FrictionSpinDown" or testCase == "FrictionSpinUp": wheelSpeedBeforeInteg1 = rw0DataLog.Omega wheelSpeedBeforeInteg2 = rw1DataLog.Omega frictionTorque1 = rw0DataLog.frictionTorque @@ -300,251 +386,357 @@ def reactionWheelIntegratedTest(show_plots,useFlag,testCase): dataPos = posData[-1] dataSigma = sigmaData[-1] - if testCase == 'BalancedWheels': - truePos = [ - [-4.02553766e+06, 7.48712857e+06, 5.24933964e+06] - ] + if testCase == "BalancedWheels": + truePos = [[-4.02553766e06, 7.48712857e06, 5.24933964e06]] - trueSigma = [ - [1.99853994e-02, 2.45647716e-03, 8.45356279e-06] - ] + trueSigma = [[1.99853994e-02, 2.45647716e-03, 8.45356279e-06]] - elif testCase == 'JitterSimple': - truePos = [ - [-4.02553766e+06, 7.48712857e+06, 5.24933964e+06] - ] + elif testCase == "JitterSimple": + truePos = [[-4.02553766e06, 7.48712857e06, 5.24933964e06]] - trueSigma = [ - [1.98964221e-02, 2.24474932e-03, -5.66618270e-05] - ] + trueSigma = [[1.98964221e-02, 2.24474932e-03, -5.66618270e-05]] - elif testCase == 'JitterFullyCoupled': - truePos = [ - [-4.02085866e+06, 7.49022306e+06, 5.24840326e+06] - ] + elif testCase == "JitterFullyCoupled": + truePos = [[-4.02085866e06, 7.49022306e06, 5.24840326e06]] - trueSigma = [ - [1.98708924e-03, 2.26086385e-04, -1.60335529e-05] - ] + trueSigma = [[1.98708924e-03, 2.26086385e-04, -1.60335529e-05]] - initialOrbAngMom_N = [ - [orbAngMom_N[0,1], orbAngMom_N[0,2], orbAngMom_N[0,3]] - ] + initialOrbAngMom_N = [[orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]]] - finalOrbAngMom = [ - [orbAngMom_N[-1,1], orbAngMom_N[-1,2], orbAngMom_N[-1,3]] - ] + finalOrbAngMom = [[orbAngMom_N[-1, 1], orbAngMom_N[-1, 2], orbAngMom_N[-1, 3]]] - initialRotAngMom_N = [ - [rotAngMom_N[0,1], rotAngMom_N[0,2], rotAngMom_N[0,3]] - ] + initialRotAngMom_N = [[rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]]] - finalRotAngMom = [ - [rotAngMom_N[-1,1], rotAngMom_N[-1,2], rotAngMom_N[-1,3]] - ] + finalRotAngMom = [[rotAngMom_N[-1, 1], rotAngMom_N[-1, 2], rotAngMom_N[-1, 3]]] - initialOrbEnergy = [ - [orbEnergy[0,1]] - ] + initialOrbEnergy = [[orbEnergy[0, 1]]] - finalOrbEnergy = [ - [orbEnergy[-1,1]] - ] + finalOrbEnergy = [[orbEnergy[-1, 1]]] - initialRotEnergy = [ - [rotEnergy[int(len(rotEnergy)/2)+1,1]] - ] + initialRotEnergy = [[rotEnergy[int(len(rotEnergy) / 2) + 1, 1]]] - finalRotEnergy = [ - [rotEnergy[-1,1]] - ] + finalRotEnergy = [[rotEnergy[-1, 1]]] plt.close("all") - if testCase == 'BalancedWheels' or testCase == 'JitterFullyCoupled': + if testCase == "BalancedWheels" or testCase == "JitterFullyCoupled": plt.figure() plt.clf() - plt.plot(orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,1] - orbAngMom_N[0,1])/orbAngMom_N[0,1], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,2] - orbAngMom_N[0,2])/orbAngMom_N[0,2], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,3] - orbAngMom_N[0,3])/orbAngMom_N[0,3]) + plt.plot( + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInOrbitalAngularMomentum" + testCase, "Change in Orbital Angular Momentum " + testCase, plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInOrbitalAngularMomentum" + testCase, + "Change in Orbital Angular Momentum " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(orbEnergy[:,0]*1e-9, (orbEnergy[:,1] - orbEnergy[0,1])/orbEnergy[0,1]) + plt.plot( + orbEnergy[:, 0] * 1e-9, + (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInOrbitalEnergy" + testCase, "Change in Orbital Energy " + testCase, plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInOrbitalEnergy" + testCase, + "Change in Orbital Energy " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,1] - rotAngMom_N[0,1])/rotAngMom_N[0,1], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,2] - rotAngMom_N[0,2])/rotAngMom_N[0,2], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,3] - rotAngMom_N[0,3])/rotAngMom_N[0,3]) + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) / rotAngMom_N[0, 1], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) / rotAngMom_N[0, 2], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) / rotAngMom_N[0, 3], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInRotationalAngularMomentum" + testCase, "Change in Rotational Angular Momentum " + testCase, plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInRotationalAngularMomentum" + testCase, + "Change in Rotational Angular Momentum " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(rotEnergy[int(len(rotEnergy)/2)+1:,0]*1e-9, (rotEnergy[int(len(rotEnergy)/2)+1:,1] - rotEnergy[int(len(rotEnergy)/2)+1,1])/rotEnergy[int(len(rotEnergy)/2)+1,1]) + plt.plot( + rotEnergy[int(len(rotEnergy) / 2) + 1 :, 0] * 1e-9, + ( + rotEnergy[int(len(rotEnergy) / 2) + 1 :, 1] + - rotEnergy[int(len(rotEnergy) / 2) + 1, 1] + ) + / rotEnergy[int(len(rotEnergy) / 2) + 1, 1], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("ChangeInRotationalEnergy" + testCase, "Change in Rotational Energy " + testCase, plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ChangeInRotationalEnergy" + testCase, + "Change in Rotational Energy " + testCase, + plt, + r"width=0.8\textwidth", + path, + ) if show_plots: plt.show() - plt.close('all') + plt.close("all") - if testCase == 'BOE': + if testCase == "BOE": plt.figure() plt.clf() - plt.plot(scDataLog.times()*1e-9, thetaOut, label = 'Basilisk') - plt.plot(timeBOE, thetaBOE, 'ro', label='BOE') - plt.legend(loc='upper left', numpoints=1) + plt.plot(scDataLog.times() * 1e-9, thetaOut, label="Basilisk") + plt.plot(timeBOE, thetaBOE, "ro", label="BOE") + plt.legend(loc="upper left", numpoints=1) plt.xlabel("Time (s)") plt.ylabel("Theta (rad)") - unitTestSupport.writeFigureLaTeX("ReactionWheelBOETheta", "Reaction Wheel BOE Theta", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ReactionWheelBOETheta", + "Reaction Wheel BOE Theta", + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(scDataLog.times()*1e-9, omegaData[:,2], label = 'Basilisk') - plt.plot(timeBOE, omegaBOE, 'ro', label='BOE') - plt.legend(loc='upper right', numpoints=1) + plt.plot(scDataLog.times() * 1e-9, omegaData[:, 2], label="Basilisk") + plt.plot(timeBOE, omegaBOE, "ro", label="BOE") + plt.legend(loc="upper right", numpoints=1) plt.xlabel("Time (s)") plt.ylabel("Body Rate (rad/s)") - unitTestSupport.writeFigureLaTeX("ReactionWheelBOEBodyRate", "Reaction Wheel BOE Body Rate", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ReactionWheelBOEBodyRate", + "Reaction Wheel BOE Body Rate", + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(scDataLog.times()*1e-9, wheelSpeeds, label = 'Basilisk') - plt.plot(timeBOE, wheelSpeedBOE, 'ro', label='BOE') - plt.legend(loc ='upper left', numpoints=1) + plt.plot(scDataLog.times() * 1e-9, wheelSpeeds, label="Basilisk") + plt.plot(timeBOE, wheelSpeedBOE, "ro", label="BOE") + plt.legend(loc="upper left", numpoints=1) plt.xlabel("Time (s)") plt.ylabel("Wheel Speed (rad/s)") - unitTestSupport.writeFigureLaTeX("ReactionWheelBOERWRate", "Reaction Wheel BOE RW Rate", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ReactionWheelBOERWRate", + "Reaction Wheel BOE RW Rate", + plt, + r"width=0.8\textwidth", + path, + ) if show_plots: plt.show() - plt.close('all') + plt.close("all") - if testCase == 'FrictionSpinDown' or testCase == 'FrictionSpinUp': + if testCase == "FrictionSpinDown" or testCase == "FrictionSpinUp": plt.figure() plt.clf() - plt.plot(scDataLog.times()*1e-9, omegaData[:,2], label='Basilisk') + plt.plot(scDataLog.times() * 1e-9, omegaData[:, 2], label="Basilisk") plt.xlabel("Time (s)") plt.ylabel("Body Rate (rad/s)") - unitTestSupport.writeFigureLaTeX("ReactionWheel" + testCase + "TestBodyRates", "Reaction Wheel " + testCase + " Test Body Rates", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ReactionWheel" + testCase + "TestBodyRates", + "Reaction Wheel " + testCase + " Test Body Rates", + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(speedDataLog.times()*1e-9, wheelSpeeds, label = 'RW 1 Wheel Speed') - plt.plot(speedDataLog.times()*1e-9, wheelSpeeds, label = 'RW 2 Wheel Speed') - plt.legend(loc='upper right') + plt.plot(speedDataLog.times() * 1e-9, wheelSpeeds, label="RW 1 Wheel Speed") + plt.plot(speedDataLog.times() * 1e-9, wheelSpeeds, label="RW 2 Wheel Speed") + plt.legend(loc="upper right") plt.xlabel("Time (s)") plt.ylabel("Wheel Speed (rad/s)") - unitTestSupport.writeFigureLaTeX("ReactionWheel" + testCase + "TestWheelSpeed", "Reaction Wheel " + testCase + " Test Wheel Speed", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ReactionWheel" + testCase + "TestWheelSpeed", + "Reaction Wheel " + testCase + " Test Wheel Speed", + plt, + r"width=0.8\textwidth", + path, + ) print(wheelSpeedBeforeInteg1) print(frictionTorque1) plt.figure() plt.clf() - plt.plot(wheelSpeedBeforeInteg1, frictionTorque1, label = 'RW 1 Friction Torque') - plt.plot(wheelSpeedBeforeInteg2, frictionTorque2, label = 'RW 2 Friction Torque') - plt.legend(loc='upper right') + plt.plot(wheelSpeedBeforeInteg1, frictionTorque1, label="RW 1 Friction Torque") + plt.plot(wheelSpeedBeforeInteg2, frictionTorque2, label="RW 2 Friction Torque") + plt.legend(loc="upper right") plt.xlabel("Wheel Speed (rad/s)") plt.ylabel("Friction Torque (N-m)") axes = plt.gca() plt.xlim([-15, 15]) - unitTestSupport.writeFigureLaTeX("ReactionWheel" + testCase + "TestFrictionTorque", "Reaction Wheel " + testCase + " Test Friction Torque", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "ReactionWheel" + testCase + "TestFrictionTorque", + "Reaction Wheel " + testCase + " Test Friction Torque", + plt, + r"width=0.8\textwidth", + path, + ) if show_plots: plt.show() - plt.close('all') + plt.close("all") accuracy = 1e-7 - if testCase == 'BalancedWheels' or testCase == 'JitterSimple' or testCase == 'JitterFullyCoupled': - for i in range(0,len(truePos)): + if ( + testCase == "BalancedWheels" + or testCase == "JitterSimple" + or testCase == "JitterFullyCoupled" + ): + for i in range(0, len(truePos)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(dataPos,truePos[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + dataPos, truePos[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Reaction Wheel Integrated Test failed pos unit test") + testMessages.append( + "FAILED: Reaction Wheel Integrated Test failed pos unit test" + ) - for i in range(0,len(trueSigma)): + for i in range(0, len(trueSigma)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(dataSigma,trueSigma[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + dataSigma, trueSigma[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Reaction Wheel Integrated Test failed attitude unit test") + testMessages.append( + "FAILED: Reaction Wheel Integrated Test failed attitude unit test" + ) accuracy = 1e-10 - if testCase == 'BalancedWheels' or testCase == 'JitterFullyCoupled': - for i in range(0,len(initialOrbAngMom_N)): + if testCase == "BalancedWheels" or testCase == "JitterFullyCoupled": + for i in range(0, len(initialOrbAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i],initialOrbAngMom_N[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Reaction Wheel Integrated Test failed orbital angular momentum unit test") + testMessages.append( + "FAILED: Reaction Wheel Integrated Test failed orbital angular momentum unit test" + ) - for i in range(0,len(initialRotAngMom_N)): + for i in range(0, len(initialRotAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotAngMom[i],initialRotAngMom_N[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Reaction Wheel Integrated Test failed rotational angular momentum unit test") + testMessages.append( + "FAILED: Reaction Wheel Integrated Test failed rotational angular momentum unit test" + ) - for i in range(0,len(initialOrbEnergy)): + for i in range(0, len(initialOrbEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i],initialOrbEnergy[i],1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Reaction Wheel Integrated Test failed orbital energy unit test") + testMessages.append( + "FAILED: Reaction Wheel Integrated Test failed orbital energy unit test" + ) - for i in range(0,len(initialRotEnergy)): + for i in range(0, len(initialRotEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotEnergy[i],initialRotEnergy[i],1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotEnergy[i], initialRotEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Reaction Wheel Integrated Test failed rotational energy unit test") + testMessages.append( + "FAILED: Reaction Wheel Integrated Test failed rotational energy unit test" + ) accuracy = 1e-8 - if testCase == 'BOE': + if testCase == "BOE": for i in range(5): - if abs((omegaBOE[i] - omegaData[int(timeBOE[i]/stepSize),2])/omegaBOE[i]) > accuracy: + if ( + abs( + (omegaBOE[i] - omegaData[int(timeBOE[i] / stepSize), 2]) + / omegaBOE[i] + ) + > accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Reaction Wheel Integrated Test failed BOE body rates unit test") - if abs((thetaBOE[i] - thetaOut[int(timeBOE[i]/stepSize)])/thetaBOE[i]) > accuracy: + testMessages.append( + "FAILED: Reaction Wheel Integrated Test failed BOE body rates unit test" + ) + if ( + abs((thetaBOE[i] - thetaOut[int(timeBOE[i] / stepSize)]) / thetaBOE[i]) + > accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Reaction Wheel Integrated Test failed BOE theta unit test") - if abs((wheelSpeedBOE[i] - wheelSpeeds[int(timeBOE[i]/stepSize)])/wheelSpeedBOE[i]) > accuracy: + testMessages.append( + "FAILED: Reaction Wheel Integrated Test failed BOE theta unit test" + ) + if ( + abs( + (wheelSpeedBOE[i] - wheelSpeeds[int(timeBOE[i] / stepSize)]) + / wheelSpeedBOE[i] + ) + > accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Reaction Wheel Integrated Test failed BOE wheel speed unit test") + testMessages.append( + "FAILED: Reaction Wheel Integrated Test failed BOE wheel speed unit test" + ) if testFailCount == 0: print("PASSED: " + " Reaction Wheel Integrated Sim " + testCase) # print out success message if no errors were found - if testCase == 'JitterSimple' and testFailCount == 0: + if testCase == "JitterSimple" and testFailCount == 0: print("PASSED ") - colorText = 'ForestGreen' - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + colorText = "ForestGreen" + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" # Write some snippets for AutoTex - snippetName = testCase + 'PassFail' + snippetName = testCase + "PassFail" unitTestSupport.writeTeXSnippet(snippetName, passedText, path) - elif testCase == 'JitterSimple' and testFailCount > 0: - colorText = 'Red' - passedText = r'\textcolor{' + colorText + '}{' + "FAILED" + '}' + elif testCase == "JitterSimple" and testFailCount > 0: + colorText = "Red" + passedText = r"\textcolor{" + colorText + "}{" + "FAILED" + "}" # Write some snippets for AutoTex - snippetName = testCase + 'PassFail' + snippetName = testCase + "PassFail" unitTestSupport.writeTeXSnippet(snippetName, passedText, path) assert testFailCount < 1, testMessages # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def test_setJs(show_plots): """Module Unit Test of settign Js value""" rwFactory = simIncludeRW.rwFactory() RW = rwFactory.create( - 'custom' - , [1, 0, 0] # gsHat_B - , rWB_B=[0.1, 0., 0.] # m - , u_max= 0.01 # N - , Js=0.1 # kg*m^2 + "custom", + [1, 0, 0], # gsHat_B + rWB_B=[0.1, 0.0, 0.0], # m + u_max=0.01, # N + Js=0.1, # kg*m^2 ) assert RW.Js == 0.1, "Setting Js through RW factory class create function failed" if __name__ == "__main__": - reactionWheelIntegratedTest(True,True,'BalancedWheels') + reactionWheelIntegratedTest(True, True, "BalancedWheels") # reactionWheelIntegratedTest(True,True,'BalancedWheels') # test_setJs(False) diff --git a/src/simulation/dynamics/spacecraft/_UnitTest/test_spacecraft.py b/src/simulation/dynamics/spacecraft/_UnitTest/test_spacecraft.py index fa9285761d..16b1d4eb34 100644 --- a/src/simulation/dynamics/spacecraft/_UnitTest/test_spacecraft.py +++ b/src/simulation/dynamics/spacecraft/_UnitTest/test_spacecraft.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -27,7 +26,9 @@ path = os.path.dirname(os.path.abspath(filename)) from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions import matplotlib.pyplot as plt from Basilisk.simulation import spacecraft from Basilisk.utilities import macros @@ -38,9 +39,11 @@ from Basilisk.simulation import GravityGradientEffector from Basilisk.architecture import messaging + def addTimeColumn(time, data): return numpy.transpose(numpy.vstack([[time], numpy.transpose(data)])) + # uncomment this line is 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 @@ -48,15 +51,19 @@ def addTimeColumn(time, data): # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("function", ["SCTranslation" - , "SCTransAndRotation" - , "SCRotation" - , "SCTransBOE" - , "SCPointBVsPointC" - , "scOptionalRef" - , "scAccumDV" - , "scAccumDVExtForce" - ]) +@pytest.mark.parametrize( + "function", + [ + "SCTranslation", + "SCTransAndRotation", + "SCRotation", + "SCTransBOE", + "SCPointBVsPointC", + "scOptionalRef", + "scAccumDV", + "scAccumDVExtForce", + ], +) def test_spacecraftAllTest(show_plots, function): """Module Unit Test""" func = globals().get(function) @@ -103,99 +110,143 @@ def SCTranslation(show_plots): unitTestSim.earthGravBody = gravityEffector.GravBodyData() unitTestSim.earthGravBody.planetName = "earth_planet_data" - unitTestSim.earthGravBody.mu = 0.3986004415E+15 # meters! + unitTestSim.earthGravBody.mu = 0.3986004415e15 # meters! unitTestSim.earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([unitTestSim.earthGravBody]) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + [unitTestSim.earthGravBody] + ) dataLog = scObject.scStateOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) # Define initial conditions of the spacecraft scObject.hub.mHub = 100 - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + scObject.hub.r_CN_NInit = [ + [-4020338.690396649], + [7490566.741852513], + [5248299.211589362], + ] + scObject.hub.v_CN_NInit = [ + [-5199.77710904224], + [-3436.681645356935], + [1041.576797498721], + ] scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totOrbEnergy"]) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) unitTestSim.InitializeSimulation() accuracy = 1e-3 - if not unitTestSupport.isArrayEqual(scObject.scStateOutMsg.read().r_BN_N, - [item for sublist in scObject.hub.r_CN_NInit for item in sublist], - 3, accuracy): + if not unitTestSupport.isArrayEqual( + scObject.scStateOutMsg.read().r_BN_N, + [item for sublist in scObject.hub.r_CN_NInit for item in sublist], + 3, + accuracy, + ): testFailCount += 1 - testMessages.append("FAILED: SCHub Translation test failed init pos msg unit test") - if not unitTestSupport.isArrayEqual(scObject.scStateOutMsg.read().v_BN_N, - [item for sublist in scObject.hub.v_CN_NInit for item in sublist], - 3, accuracy): + testMessages.append( + "FAILED: SCHub Translation test failed init pos msg unit test" + ) + if not unitTestSupport.isArrayEqual( + scObject.scStateOutMsg.read().v_BN_N, + [item for sublist in scObject.hub.v_CN_NInit for item in sublist], + 3, + accuracy, + ): testFailCount += 1 - testMessages.append("FAILED: SCHub Translation test failed init pos msg unit test") - + testMessages.append( + "FAILED: SCHub Translation test failed init pos msg unit test" + ) stopTime = 10.0 unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) unitTestSim.ExecuteSimulation() - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) plt.close("all") plt.figure() plt.clf() - plt.plot(orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,1] - orbAngMom_N[0,1])/orbAngMom_N[0,1], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,2] - orbAngMom_N[0,2])/orbAngMom_N[0,2], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,3] - orbAngMom_N[0,3])/orbAngMom_N[0,3]) + plt.plot( + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("scPlusChangeInOrbitalAngularMomentumTranslationOnly", "Change in Orbital Angular Momentum Translation Only", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "scPlusChangeInOrbitalAngularMomentumTranslationOnly", + "Change in Orbital Angular Momentum Translation Only", + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(orbEnergy[:,0]*1e-9, (orbEnergy[:,1] - orbEnergy[0,1])/orbEnergy[0,1]) + plt.plot( + orbEnergy[:, 0] * 1e-9, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1] + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("scPlusChangeInOrbitalEnergyTranslationOnly", "Change in Orbital Energy Translation Only", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "scPlusChangeInOrbitalEnergyTranslationOnly", + "Change in Orbital Energy Translation Only", + plt, + r"width=0.8\textwidth", + path, + ) if show_plots: plt.show() - plt.close('all') + plt.close("all") moduleOutput = dataLog.r_BN_N - truePos = [ - [-4072255.7737936215, 7456050.4649078, 5258610.029627514] - ] + truePos = [[-4072255.7737936215, 7456050.4649078, 5258610.029627514]] - initialOrbAngMom_N = [ - [orbAngMom_N[0,1], orbAngMom_N[0,2], orbAngMom_N[0,3]] - ] + initialOrbAngMom_N = [[orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]]] - finalOrbAngMom = [ - [orbAngMom_N[-1,1], orbAngMom_N[-1,2], orbAngMom_N[-1,3]] - ] + finalOrbAngMom = [[orbAngMom_N[-1, 1], orbAngMom_N[-1, 2], orbAngMom_N[-1, 3]]] - initialOrbEnergy = [ - [orbEnergy[0,1]] - ] + initialOrbEnergy = [[orbEnergy[0, 1]]] - finalOrbEnergy = [ - [orbEnergy[-1,1]] - ] + finalOrbEnergy = [[orbEnergy[-1, 1]]] accuracy = 1e-10 - for i in range(0,len(truePos)): + for i in range(0, len(truePos)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(moduleOutput[-1,:],truePos[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + moduleOutput[-1, :], truePos[i], 3, accuracy + ): testFailCount += 1 testMessages.append("FAILED: SCHub Translation test failed pos unit test") - for i in range(0,len(initialOrbAngMom_N)): + for i in range(0, len(initialOrbAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i],initialOrbAngMom_N[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: SCHub Translation test failed orbital angular momentum unit test") + testMessages.append( + "FAILED: SCHub Translation test failed orbital angular momentum unit test" + ) - for i in range(0,len(initialOrbEnergy)): + for i in range(0, len(initialOrbEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i],initialOrbEnergy[i],1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: SCHub Translation test failed orbital energy unit test") + testMessages.append( + "FAILED: SCHub Translation test failed orbital energy unit test" + ) if testFailCount == 0: print("PASSED: " + " SCHub Translation Integrated Sim Test") @@ -204,7 +255,8 @@ def SCTranslation(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def SCTransAndRotation(show_plots): """Module Unit Test""" @@ -235,10 +287,12 @@ def SCTransAndRotation(show_plots): unitTestSim.earthGravBody = gravityEffector.GravBodyData() unitTestSim.earthGravBody.planetName = "earth_planet_data" - unitTestSim.earthGravBody.mu = 0.3986004415E+15 # meters! + unitTestSim.earthGravBody.mu = 0.3986004415e15 # meters! unitTestSim.earthGravBody.isCentralBody = True - scObject.gravField.gravBodies = spacecraft.GravBodyVector([unitTestSim.earthGravBody]) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + [unitTestSim.earthGravBody] + ) dataLog = scObject.scStateOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, dataLog) @@ -247,12 +301,22 @@ def SCTransAndRotation(show_plots): scObject.hub.mHub = 100 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[500, 0.0, 0.0], [0.0, 200, 0.0], [0.0, 0.0, 300]] - scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] - scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] + 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.5], [-0.4], [0.7]] - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) unitTestSim.InitializeSimulation() @@ -261,120 +325,174 @@ def SCTransAndRotation(show_plots): unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) unitTestSim.ExecuteSimulation() - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) r_BN_NOutput = dataLog.r_BN_N sigma_BNOutput = dataLog.sigma_BN - truePos = [ - [-4072255.7737936215, 7456050.4649078, 5258610.029627514] - ] + truePos = [[-4072255.7737936215, 7456050.4649078, 5258610.029627514]] - trueSigma = [ - [3.73034285e-01, -2.39564413e-03, 2.08570797e-01] - ] + trueSigma = [[3.73034285e-01, -2.39564413e-03, 2.08570797e-01]] - initialOrbAngMom_N = [ - [orbAngMom_N[0,1], orbAngMom_N[0,2], orbAngMom_N[0,3]] - ] + initialOrbAngMom_N = [[orbAngMom_N[0, 1], orbAngMom_N[0, 2], orbAngMom_N[0, 3]]] - finalOrbAngMom = [ - [orbAngMom_N[-1,1], orbAngMom_N[-1,2], orbAngMom_N[-1,3]] - ] + finalOrbAngMom = [[orbAngMom_N[-1, 1], orbAngMom_N[-1, 2], orbAngMom_N[-1, 3]]] - initialRotAngMom_N = [ - [rotAngMom_N[0,1], rotAngMom_N[0,2], rotAngMom_N[0,3]] - ] + initialRotAngMom_N = [[rotAngMom_N[0, 1], rotAngMom_N[0, 2], rotAngMom_N[0, 3]]] - finalRotAngMom = [ - [rotAngMom_N[-1,1], rotAngMom_N[-1,2], rotAngMom_N[-1,3]] - ] + finalRotAngMom = [[rotAngMom_N[-1, 1], rotAngMom_N[-1, 2], rotAngMom_N[-1, 3]]] - initialOrbEnergy = [ - [orbEnergy[0,1]] - ] + initialOrbEnergy = [[orbEnergy[0, 1]]] - finalOrbEnergy = [ - [orbEnergy[-1,1]] - ] + finalOrbEnergy = [[orbEnergy[-1, 1]]] - initialRotEnergy = [ - [rotEnergy[0,1]] - ] + initialRotEnergy = [[rotEnergy[0, 1]]] - finalRotEnergy = [ - [rotEnergy[-1,1]] - ] + finalRotEnergy = [[rotEnergy[-1, 1]]] - plt.close('all') + plt.close("all") plt.figure() plt.clf() - plt.plot(orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,1] - orbAngMom_N[0,1])/orbAngMom_N[0,1], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,2] - orbAngMom_N[0,2])/orbAngMom_N[0,2], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,3] - orbAngMom_N[0,3])/orbAngMom_N[0,3]) + plt.plot( + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("scPlusChangeInOrbitalAngularMomentumTranslationAndRotation", "Change in Orbital Angular Momentum Translation And Rotation", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "scPlusChangeInOrbitalAngularMomentumTranslationAndRotation", + "Change in Orbital Angular Momentum Translation And Rotation", + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(orbEnergy[:,0]*1e-9, (orbEnergy[:,1] - orbEnergy[0,1])/orbEnergy[0,1]) + plt.plot( + orbEnergy[:, 0] * 1e-9, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1] + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("scPlusChangeInOrbitalEnergyTranslationAndRotation", "Change in Orbital Energy Translation And Rotation", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "scPlusChangeInOrbitalEnergyTranslationAndRotation", + "Change in Orbital Energy Translation And Rotation", + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,1] - rotAngMom_N[0,1])/rotAngMom_N[0,1], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,2] - rotAngMom_N[0,2])/rotAngMom_N[0,2], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,3] - rotAngMom_N[0,3])/rotAngMom_N[0,3]) + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) / rotAngMom_N[0, 1], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) / rotAngMom_N[0, 2], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) / rotAngMom_N[0, 3], + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("scPlusChangeInRotationalAngularMomentumTranslationAndRotation", "Change in Rotational Angular Momentum Translation And Rotation", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "scPlusChangeInRotationalAngularMomentumTranslationAndRotation", + "Change in Rotational Angular Momentum Translation And Rotation", + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(rotEnergy[:,0]*1e-9, (rotEnergy[:,1] - rotEnergy[0,1])/rotEnergy[0,1]) + plt.plot( + rotEnergy[:, 0] * 1e-9, (rotEnergy[:, 1] - rotEnergy[0, 1]) / rotEnergy[0, 1] + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("scPlusChangeInRotationalEnergyTranslationAndRotation", "Change in Rotational Energy Translation And Rotation", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "scPlusChangeInRotationalEnergyTranslationAndRotation", + "Change in Rotational Energy Translation And Rotation", + plt, + r"width=0.8\textwidth", + path, + ) if show_plots: plt.show() - plt.close('all') + plt.close("all") accuracy = 1e-8 - for i in range(0,len(truePos)): + for i in range(0, len(truePos)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(r_BN_NOutput[-1,:],truePos[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + r_BN_NOutput[-1, :], truePos[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Translation and Rotation Integrated test failed pos unit test") + testMessages.append( + "FAILED: Spacecraft Translation and Rotation Integrated test failed pos unit test" + ) - for i in range(0,len(trueSigma)): + for i in range(0, len(trueSigma)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(sigma_BNOutput[-1,:],trueSigma[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + sigma_BNOutput[-1, :], trueSigma[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Translation and Rotation Integrated test failed attitude unit test") + testMessages.append( + "FAILED: Spacecraft Translation and Rotation Integrated test failed attitude unit test" + ) accuracy = 1e-10 - for i in range(0,len(initialOrbAngMom_N)): + for i in range(0, len(initialOrbAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i],initialOrbAngMom_N[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Translation and Rotation Integrated test failed orbital angular momentum unit test") + testMessages.append( + "FAILED: Spacecraft Translation and Rotation Integrated test failed orbital angular momentum unit test" + ) - for i in range(0,len(initialRotAngMom_N)): + for i in range(0, len(initialRotAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotAngMom[i],initialRotAngMom_N[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Translation and Rotation Integrated test failed rotational angular momentum unit test") + testMessages.append( + "FAILED: Spacecraft Translation and Rotation Integrated test failed rotational angular momentum unit test" + ) - for i in range(0,len(initialRotEnergy)): + for i in range(0, len(initialRotEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotEnergy[i],initialRotEnergy[i],1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotEnergy[i], initialRotEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Translation and Rotation Integrated test failed rotational energy unit test") + testMessages.append( + "FAILED: Spacecraft Translation and Rotation Integrated test failed rotational energy unit test" + ) - for i in range(0,len(initialOrbEnergy)): + for i in range(0, len(initialOrbEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i],initialOrbEnergy[i],1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Translation and Rotation Integrated test failed orbital energy unit test") + testMessages.append( + "FAILED: Spacecraft Translation and Rotation Integrated test failed orbital energy unit test" + ) if testFailCount == 0: print("PASSED: " + " Spacecraft Translation and Rotation Integrated Sim Test") @@ -383,7 +501,8 @@ def SCTransAndRotation(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def SCRotation(show_plots): """Module Unit Test""" @@ -421,24 +540,27 @@ def SCRotation(show_plots): scObject.hub.omega_BN_BInit = [[0.5], [-0.4], [0.7]] # BOE for rotational dynamics - h = numpy.dot(numpy.asarray(scObject.hub.IHubPntBc_B),numpy.asarray(scObject.hub.omega_BN_BInit).flatten()) + h = numpy.dot( + numpy.asarray(scObject.hub.IHubPntBc_B), + numpy.asarray(scObject.hub.omega_BN_BInit).flatten(), + ) H = numpy.linalg.norm(h) - n3_B = -h/H + n3_B = -h / H # Find DCM n2_B = numpy.zeros(3) n2_B[1] = 0.1 - n2_B[0] = -n2_B[1]*n3_B[1]/n3_B[0] - n2_B = n2_B/numpy.linalg.norm(n2_B) - n1_B = numpy.cross(n2_B,n3_B) - n1_B = n1_B/(numpy.linalg.norm(n1_B)) - dcm_BN = numpy.zeros([3,3]) - dcm_BN[:,0] = n1_B - dcm_BN[:,1] = n2_B - dcm_BN[:,2] = n3_B - h3_N = numpy.array([0,0,-H]) - h3_B = numpy.dot(dcm_BN,h3_N) - h3_Ncheck = numpy.dot(dcm_BN.transpose(),h3_B) + n2_B[0] = -n2_B[1] * n3_B[1] / n3_B[0] + n2_B = n2_B / numpy.linalg.norm(n2_B) + n1_B = numpy.cross(n2_B, n3_B) + n1_B = n1_B / (numpy.linalg.norm(n1_B)) + dcm_BN = numpy.zeros([3, 3]) + dcm_BN[:, 0] = n1_B + dcm_BN[:, 1] = n2_B + dcm_BN[:, 2] = n3_B + h3_N = numpy.array([0, 0, -H]) + h3_B = numpy.dot(dcm_BN, h3_N) + h3_Ncheck = numpy.dot(dcm_BN.transpose(), h3_B) sigmaCalc = RigidBodyKinematics.C2MRP(dcm_BN) scObject.hub.sigma_BNInit = [[sigmaCalc[0]], [sigmaCalc[1]], [sigmaCalc[2]]] @@ -451,58 +573,55 @@ def SCRotation(show_plots): unitTestSim.ConfigureStopTime(macros.sec2nano(stopTime)) unitTestSim.ExecuteSimulation() - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) rotAngMomMag = numpy.zeros(len(rotAngMom_N)) - for i in range(0,len(rotAngMom_N)): - rotAngMomMag[i] = numpy.linalg.norm(numpy.asarray(rotAngMom_N[i,1:4])) + for i in range(0, len(rotAngMom_N)): + rotAngMomMag[i] = numpy.linalg.norm(numpy.asarray(rotAngMom_N[i, 1:4])) - trueSigma = [ - [5.72693314e-01, 5.10734375e-01, -3.07377611e-01] - ] + trueSigma = [[5.72693314e-01, 5.10734375e-01, -3.07377611e-01]] - initialRotAngMom_N = [ - [numpy.linalg.norm(numpy.asarray(rotAngMom_N[0,1:4]))] - ] + initialRotAngMom_N = [[numpy.linalg.norm(numpy.asarray(rotAngMom_N[0, 1:4]))]] - finalRotAngMom = [ - [numpy.linalg.norm(numpy.asarray(rotAngMom_N[-1,1:4]))] - ] + finalRotAngMom = [[numpy.linalg.norm(numpy.asarray(rotAngMom_N[-1, 1:4]))]] - initialRotEnergy = [ - [rotEnergy[0,1]] - ] + initialRotEnergy = [[rotEnergy[0, 1]]] - finalRotEnergy = [ - [rotEnergy[-1,1]] - ] + finalRotEnergy = [[rotEnergy[-1, 1]]] moduleOutput = addTimeColumn(dataLog.times(), dataLog.sigma_BN) omega_BNOutput = addTimeColumn(dataLog.times(), dataLog.omega_BN_B) check = 0 - for i in range(0,len(moduleOutput)): - if check == 0 and moduleOutput[i+1,2] < moduleOutput[i,2]: + for i in range(0, len(moduleOutput)): + if check == 0 and moduleOutput[i + 1, 2] < moduleOutput[i, 2]: check = 1 - if check == 1 and moduleOutput[i+1,2] > moduleOutput[i,2]: + if check == 1 and moduleOutput[i + 1, 2] > moduleOutput[i, 2]: check = 2 - index = i+1 + index = i + 1 break - sigmaBeforeSwitch = moduleOutput[index-1,1:4] - sigmaBeforeBefore = moduleOutput[index-2,1:4] - sigmaAfterSwitch = moduleOutput[index,:] - deltaT = (moduleOutput[index-1,0] - moduleOutput[index-2,0])*1e-9 - yPrime = (sigmaBeforeSwitch - sigmaBeforeBefore)/deltaT - sigmaGhost = sigmaBeforeSwitch + yPrime*deltaT - sigmaAfterAnalytical = - sigmaGhost/numpy.dot(numpy.linalg.norm(numpy.asarray(sigmaGhost)),numpy.linalg.norm(numpy.asarray(sigmaGhost))) + sigmaBeforeSwitch = moduleOutput[index - 1, 1:4] + sigmaBeforeBefore = moduleOutput[index - 2, 1:4] + sigmaAfterSwitch = moduleOutput[index, :] + deltaT = (moduleOutput[index - 1, 0] - moduleOutput[index - 2, 0]) * 1e-9 + yPrime = (sigmaBeforeSwitch - sigmaBeforeBefore) / deltaT + sigmaGhost = sigmaBeforeSwitch + yPrime * deltaT + sigmaAfterAnalytical = -sigmaGhost / numpy.dot( + numpy.linalg.norm(numpy.asarray(sigmaGhost)), + numpy.linalg.norm(numpy.asarray(sigmaGhost)), + ) timeArray = numpy.zeros(5) - sigmaArray = numpy.zeros([3,5]) - omegaAnalyticalArray = numpy.zeros([3,5]) - omegaArray = numpy.zeros([4,5]) + sigmaArray = numpy.zeros([3, 5]) + omegaAnalyticalArray = numpy.zeros([3, 5]) + omegaArray = numpy.zeros([4, 5]) for i in range(0, 5): - idx = int(stopTime/timeStep*(i+1)/5) + idx = int(stopTime / timeStep * (i + 1) / 5) timeArray[i] = moduleOutput[idx, 0] sigmaArray[:, i] = moduleOutput[idx, 1:4] sigma = sigmaArray[:, i] @@ -510,96 +629,215 @@ def SCRotation(show_plots): sigma1 = sigma[0] sigma2 = sigma[1] sigma3 = sigma[2] - omegaArray[:,i] = omega_BNOutput[idx, :] - omegaAnalyticalArray[0,i] = -H/(1 + sigmaNorm**2)**2*(8*sigma1*sigma3 - 4*sigma2*(1 - sigmaNorm**2))/scObject.hub.IHubPntBc_B[0][0] - omegaAnalyticalArray[1,i] = -H/(1 + sigmaNorm**2)**2*(8*sigma2*sigma3 + 4*sigma1*(1 - sigmaNorm**2))/scObject.hub.IHubPntBc_B[1][1] - omegaAnalyticalArray[2,i] = -H/(1 + sigmaNorm**2)**2*(4*(-sigma1**2 - sigma2**2 + sigma3**2) + (1 - sigmaNorm**2)**2)/scObject.hub.IHubPntBc_B[2][2] - - plt.close("all") # clear out earlier figures + omegaArray[:, i] = omega_BNOutput[idx, :] + omegaAnalyticalArray[0, i] = ( + -H + / (1 + sigmaNorm**2) ** 2 + * (8 * sigma1 * sigma3 - 4 * sigma2 * (1 - sigmaNorm**2)) + / scObject.hub.IHubPntBc_B[0][0] + ) + omegaAnalyticalArray[1, i] = ( + -H + / (1 + sigmaNorm**2) ** 2 + * (8 * sigma2 * sigma3 + 4 * sigma1 * (1 - sigmaNorm**2)) + / scObject.hub.IHubPntBc_B[1][1] + ) + omegaAnalyticalArray[2, i] = ( + -H + / (1 + sigmaNorm**2) ** 2 + * (4 * (-(sigma1**2) - sigma2**2 + sigma3**2) + (1 - sigmaNorm**2) ** 2) + / scObject.hub.IHubPntBc_B[2][2] + ) + + plt.close("all") # clear out earlier figures plt.figure() plt.clf() - plt.plot(moduleOutput[:,0]*1e-9, moduleOutput[:,1], moduleOutput[:,0]*1e-9, moduleOutput[:,2], moduleOutput[:,0]*1e-9, moduleOutput[:,3]) - plt.plot(moduleOutput[index,0]*1e-9, moduleOutput[index,1],'bo') - plt.plot(moduleOutput[index,0]*1e-9, sigmaGhost[0],'yo') - plt.plot(moduleOutput[index-1,0]*1e-9, moduleOutput[index-1,1],'bo') + plt.plot( + moduleOutput[:, 0] * 1e-9, + moduleOutput[:, 1], + moduleOutput[:, 0] * 1e-9, + moduleOutput[:, 2], + moduleOutput[:, 0] * 1e-9, + moduleOutput[:, 3], + ) + plt.plot(moduleOutput[index, 0] * 1e-9, moduleOutput[index, 1], "bo") + plt.plot(moduleOutput[index, 0] * 1e-9, sigmaGhost[0], "yo") + plt.plot(moduleOutput[index - 1, 0] * 1e-9, moduleOutput[index - 1, 1], "bo") plt.xlabel("Time (s)") plt.ylabel("MRPs") - unitTestSupport.writeFigureLaTeX("scPlusMRPs", "Attitude of Spacecraft in MRPs", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "scPlusMRPs", + "Attitude of Spacecraft in MRPs", + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(moduleOutput[index - 3: index + 3,0]*1e-9, moduleOutput[index - 3: index + 3,1],"b") - plt.plot(moduleOutput[index-1,0]*1e-9, moduleOutput[index-1,1],'bo', label = "Basilisk " + r"$\sigma_{1,t-1}$") - plt.plot(moduleOutput[index,0]*1e-9, moduleOutput[index,1],'ro', label = "Basilisk " + r"$\sigma_{1,t}$") - plt.plot(moduleOutput[index,0]*1e-9, sigmaGhost[0],'ko', label = "Basilisk " + r"$\sigma_{1,0}$") - plt.plot([moduleOutput[index-1,0]*1e-9, moduleOutput[index,0]*1e-9], [moduleOutput[index-1,1], sigmaGhost[0]],'--k') + plt.plot( + moduleOutput[index - 3 : index + 3, 0] * 1e-9, + moduleOutput[index - 3 : index + 3, 1], + "b", + ) + plt.plot( + moduleOutput[index - 1, 0] * 1e-9, + moduleOutput[index - 1, 1], + "bo", + label="Basilisk " + r"$\sigma_{1,t-1}$", + ) + plt.plot( + moduleOutput[index, 0] * 1e-9, + moduleOutput[index, 1], + "ro", + label="Basilisk " + r"$\sigma_{1,t}$", + ) + plt.plot( + moduleOutput[index, 0] * 1e-9, + sigmaGhost[0], + "ko", + label="Basilisk " + r"$\sigma_{1,0}$", + ) + plt.plot( + [moduleOutput[index - 1, 0] * 1e-9, moduleOutput[index, 0] * 1e-9], + [moduleOutput[index - 1, 1], sigmaGhost[0]], + "--k", + ) axes = plt.gca() - axes.set_ylim([-0.5,0.5]) - plt.legend(loc ='upper right',numpoints = 1) + axes.set_ylim([-0.5, 0.5]) + plt.legend(loc="upper right", numpoints=1) plt.xlabel("Time (s)") plt.ylabel("MRPs") - unitTestSupport.writeFigureLaTeX("scPlusMRPSwitching", "MRP Switching", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "scPlusMRPSwitching", "MRP Switching", plt, r"width=0.8\textwidth", path + ) plt.figure() plt.clf() - plt.plot(rotAngMom_N[:,0]*1e-9, (rotAngMomMag - rotAngMomMag[0])/rotAngMomMag[0]) + plt.plot( + rotAngMom_N[:, 0] * 1e-9, (rotAngMomMag - rotAngMomMag[0]) / rotAngMomMag[0] + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("scPlusChangeInRotationalAngularMomentumRotationOnly", "Change in Rotational Angular Momentum Rotation Only", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "scPlusChangeInRotationalAngularMomentumRotationOnly", + "Change in Rotational Angular Momentum Rotation Only", + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(rotEnergy[:,0]*1e-9, (rotEnergy[:,1] - rotEnergy[0,1])/rotEnergy[0,1]) + plt.plot( + rotEnergy[:, 0] * 1e-9, (rotEnergy[:, 1] - rotEnergy[0, 1]) / rotEnergy[0, 1] + ) plt.xlabel("Time (s)") plt.ylabel("Relative Difference") - unitTestSupport.writeFigureLaTeX("scPlusChangeInRotationalEnergyRotationOnly", "Change in Rotational Energy Rotation Only", plt, r"width=0.8\textwidth", path) + unitTestSupport.writeFigureLaTeX( + "scPlusChangeInRotationalEnergyRotationOnly", + "Change in Rotational Energy Rotation Only", + plt, + r"width=0.8\textwidth", + path, + ) plt.figure() plt.clf() - plt.plot(omega_BNOutput[:,0]*1e-9,omega_BNOutput[:,1],label = r"$\omega_1$" + " Basilisk") - plt.plot(omega_BNOutput[:,0]*1e-9,omega_BNOutput[:,2],label = r"$\omega_2$" + " Basilisk") - plt.plot(omega_BNOutput[:,0]*1e-9,omega_BNOutput[:,3], label = r"$\omega_3$" + " Basilisk") - plt.plot(timeArray*1e-9,omegaAnalyticalArray[0,:],'bo', label = r"$\omega_1$" + " BOE") - plt.plot(timeArray*1e-9,omegaAnalyticalArray[1,:],'go', label = r"$\omega_2$" + " BOE") - plt.plot(timeArray*1e-9,omegaAnalyticalArray[2,:],'ro', label = r"$\omega_3$" + " BOE") + plt.plot( + omega_BNOutput[:, 0] * 1e-9, + omega_BNOutput[:, 1], + label=r"$\omega_1$" + " Basilisk", + ) + plt.plot( + omega_BNOutput[:, 0] * 1e-9, + omega_BNOutput[:, 2], + label=r"$\omega_2$" + " Basilisk", + ) + plt.plot( + omega_BNOutput[:, 0] * 1e-9, + omega_BNOutput[:, 3], + label=r"$\omega_3$" + " Basilisk", + ) + plt.plot( + timeArray * 1e-9, omegaAnalyticalArray[0, :], "bo", label=r"$\omega_1$" + " BOE" + ) + plt.plot( + timeArray * 1e-9, omegaAnalyticalArray[1, :], "go", label=r"$\omega_2$" + " BOE" + ) + plt.plot( + timeArray * 1e-9, omegaAnalyticalArray[2, :], "ro", label=r"$\omega_3$" + " BOE" + ) plt.xlabel("Time (s)") plt.ylabel("Angular Velocity (rad/s)") - plt.legend(loc ='lower right',numpoints = 1, prop = {'size': 6.5}) - unitTestSupport.writeFigureLaTeX("scPlusBasiliskVsBOECalcForRotation", "Basilisk Vs BOE Calc For Rotation", plt, r"width=0.8\textwidth", path) + plt.legend(loc="lower right", numpoints=1, prop={"size": 6.5}) + unitTestSupport.writeFigureLaTeX( + "scPlusBasiliskVsBOECalcForRotation", + "Basilisk Vs BOE Calc For Rotation", + plt, + r"width=0.8\textwidth", + path, + ) if show_plots: plt.show() plt.close("all") moduleOutput = dataLog.sigma_BN accuracy = 1e-8 - for i in range(0,len(trueSigma)): + for i in range(0, len(trueSigma)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(moduleOutput[-1],trueSigma[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + moduleOutput[-1], trueSigma[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Translation and Rotation Integrated test failed attitude unit test") + testMessages.append( + "FAILED: Spacecraft Translation and Rotation Integrated test failed attitude unit test" + ) accuracy = 1e-10 - for i in range(0,len(initialRotAngMom_N)): + for i in range(0, len(initialRotAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotAngMom[i],initialRotAngMom_N[i],1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotAngMom[i], initialRotAngMom_N[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Translation and Rotation Integrated test failed rotational angular momentum unit test") + testMessages.append( + "FAILED: Spacecraft Translation and Rotation Integrated test failed rotational angular momentum unit test" + ) - for i in range(0,len(initialRotEnergy)): + for i in range(0, len(initialRotEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotEnergy[i],initialRotEnergy[i],1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotEnergy[i], initialRotEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Rotation Integrated test failed rotational energy unit test") + testMessages.append( + "FAILED: Spacecraft Rotation Integrated test failed rotational energy unit test" + ) omegaArray = (numpy.delete(omegaArray, 0, 0)).transpose() omegaAnalyticalArray = omegaAnalyticalArray.transpose() - for i in range(0,len(omegaAnalyticalArray)): + for i in range(0, len(omegaAnalyticalArray)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(omegaArray[i],omegaAnalyticalArray[i],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + omegaArray[i], omegaAnalyticalArray[i], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Rotation Integrated test Rotational BOE unit test") + testMessages.append( + "FAILED: Spacecraft Rotation Integrated test Rotational BOE unit test" + ) accuracy = 1e-5 - if not unitTestSupport.isArrayEqualRelative(numpy.delete(sigmaAfterSwitch, 0,), sigmaAfterAnalytical,1,accuracy): + if not unitTestSupport.isArrayEqualRelative( + numpy.delete( + sigmaAfterSwitch, + 0, + ), + sigmaAfterAnalytical, + 1, + accuracy, + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Rotation Integrated test failed MRP Switching unit test") + testMessages.append( + "FAILED: Spacecraft Rotation Integrated test failed MRP Switching unit test" + ) if testFailCount == 0: print("PASSED: " + "Spacecraft Rotation Integrated test") @@ -608,7 +846,8 @@ def SCRotation(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def SCTransBOE(show_plots): """Module Unit Test""" @@ -639,11 +878,11 @@ def SCTransBOE(show_plots): unitTestSim.AddModelToTask(unitTaskName, scObject) # Define conditions for the forces and times - F1 = 3. - F2 = -7. - t1 = 3. - t2 = 6. - t3 = 10. + F1 = 3.0 + F2 = -7.0 + t1 = 3.0 + t2 = 6.0 + t3 = 10.0 # Add external force and torque extFTObject = extForceTorque.ExtForceTorque() @@ -690,31 +929,39 @@ def SCTransBOE(show_plots): v_BN_NOutput = addTimeColumn(dataLog.times(), dataLog.v_BN_N) # BOE calcs - a1 = F1/scObject.hub.mHub - a2 = F2/scObject.hub.mHub - v1 = a1*t1 + a1 = F1 / scObject.hub.mHub + a2 = F2 / scObject.hub.mHub + v1 = a1 * t1 v2 = v1 - v3 = v2 + a2*(t3-t2) - x1 = 0.5*v1*t1 - x2 = x1 + v2*(t2-t1) - t0 = t2 - v2/a2 - x3 = x2 + 0.5*v2*(t0-t2) + 0.5*v3*(t3-t0) + v3 = v2 + a2 * (t3 - t2) + x1 = 0.5 * v1 * t1 + x2 = x1 + v2 * (t2 - t1) + t0 = t2 - v2 / a2 + x3 = x2 + 0.5 * v2 * (t0 - t2) + 0.5 * v3 * (t3 - t0) # truth and Basilisk truthV = [v1, v2, v3] truthX = [x1, x2, x3] - basiliskV = [v_BN_NOutput[int(t1/timeStep), 1], v_BN_NOutput[int(t2/timeStep), 1], v_BN_NOutput[int(t3/timeStep), 1]] - basiliskX = [r_BN_NOutput[int(t1/timeStep), 1], r_BN_NOutput[int(t2/timeStep), 1], r_BN_NOutput[int(t3/timeStep), 1]] + basiliskV = [ + v_BN_NOutput[int(t1 / timeStep), 1], + v_BN_NOutput[int(t2 / timeStep), 1], + v_BN_NOutput[int(t3 / timeStep), 1], + ] + basiliskX = [ + r_BN_NOutput[int(t1 / timeStep), 1], + r_BN_NOutput[int(t2 / timeStep), 1], + r_BN_NOutput[int(t3 / timeStep), 1], + ] - plt.close('all') + plt.close("all") plt.figure() plt.clf() - plt.plot(r_BN_NOutput[:,0]*1e-9, r_BN_NOutput[:,1],'-b',label = "Basilisk") - plt.plot([t1, t2, t3], [x1, x2, x3],'ro',markersize = 6.5,label = "BOE") - plt.xlabel('time (s)') - plt.ylabel('X (m)') - plt.legend(loc ='upper left',numpoints = 1) + plt.plot(r_BN_NOutput[:, 0] * 1e-9, r_BN_NOutput[:, 1], "-b", label="Basilisk") + plt.plot([t1, t2, t3], [x1, x2, x3], "ro", markersize=6.5, label="BOE") + plt.xlabel("time (s)") + plt.ylabel("X (m)") + plt.legend(loc="upper left", numpoints=1) PlotName = "scPlusTranslationPositionBOE" PlotTitle = "Translation Position BOE" format = r"width=0.8\textwidth" @@ -722,31 +969,35 @@ def SCTransBOE(show_plots): plt.figure() plt.clf() - plt.plot(v_BN_NOutput[:,0]*1e-9, v_BN_NOutput[:,1],'-b',label = "Basilisk") - plt.plot([t1, t2, t3], [v1, v2, v3],'ro',markersize = 6.5,label = "BOE") - plt.xlabel('time (s)') - plt.ylabel('X velocity (m/s)') - plt.legend(loc ='lower left',numpoints = 1) + plt.plot(v_BN_NOutput[:, 0] * 1e-9, v_BN_NOutput[:, 1], "-b", label="Basilisk") + plt.plot([t1, t2, t3], [v1, v2, v3], "ro", markersize=6.5, label="BOE") + plt.xlabel("time (s)") + plt.ylabel("X velocity (m/s)") + plt.legend(loc="lower left", numpoints=1) PlotName = "scPlusTranslationVelocityBOE" PlotTitle = "Translation Velocity BOE" format = r"width=0.8\textwidth" unitTestSupport.writeFigureLaTeX(PlotName, PlotTitle, plt, format, path) if show_plots: plt.show() - plt.close('all') + plt.close("all") accuracy = 1e-10 - for i in range(0,3): + for i in range(0, 3): # check a vector values - if abs((truthX[i] - basiliskX[i])/truthX[i]) > accuracy: + if abs((truthX[i] - basiliskX[i]) / truthX[i]) > accuracy: testFailCount += 1 - testMessages.append("FAILED: Spacecraft Translation BOE Integrated test failed pos unit test") + testMessages.append( + "FAILED: Spacecraft Translation BOE Integrated test failed pos unit test" + ) - for i in range(0,3): + for i in range(0, 3): # check a vector values - if abs((truthV[i] - basiliskV[i])/truthV[i]) > accuracy: + if abs((truthV[i] - basiliskV[i]) / truthV[i]) > accuracy: testFailCount += 1 - testMessages.append("FAILED: Spacecraft Translation BOE Integrated test failed velocity unit test") + testMessages.append( + "FAILED: Spacecraft Translation BOE Integrated test failed velocity unit test" + ) if testFailCount == 0: print("PASSED: " + " Spacecraft Translation BOE Integrated Sim Test") @@ -755,7 +1006,8 @@ def SCTransBOE(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def SCPointBVsPointC(show_plots): """Module Unit Test""" @@ -787,12 +1039,16 @@ def SCPointBVsPointC(show_plots): # Define location of force rFBc_B = numpy.array([0.3, -0.7, 0.4]) force_B = numpy.array([0.5, 0.6, -0.2]) - torquePntC_B = numpy.cross(rFBc_B,force_B) + torquePntC_B = numpy.cross(rFBc_B, force_B) # Add external force and torque extFTObject = extForceTorque.ExtForceTorque() extFTObject.ModelTag = "externalDisturbance" - extFTObject.extTorquePntB_B = [[torquePntC_B[0]], [torquePntC_B[1]], [torquePntC_B[2]]] + extFTObject.extTorquePntB_B = [ + [torquePntC_B[0]], + [torquePntC_B[1]], + [torquePntC_B[2]], + ] extFTObject.extForce_B = [[force_B[0]], [force_B[1]], [force_B[2]]] scObject.addDynamicEffector(extFTObject) unitTestSim.AddModelToTask(unitTaskName, extFTObject) @@ -804,8 +1060,8 @@ def SCPointBVsPointC(show_plots): scObject.hub.mHub = 100 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[500, 0.0, 0.0], [0.0, 200, 0.0], [0.0, 0.0, 300]] - scObject.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] - scObject.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] + scObject.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] + scObject.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] scObject.hub.omega_BN_BInit = [[0.5], [-0.4], [0.7]] @@ -840,12 +1096,16 @@ def SCPointBVsPointC(show_plots): # Define location of force rBcB_B = numpy.array([0.4, 0.5, 0.2]) rFB_B = rBcB_B + rFBc_B - torquePntB_B = numpy.cross(rFB_B,force_B) + torquePntB_B = numpy.cross(rFB_B, force_B) # Add external force and torque extFTObject = extForceTorque.ExtForceTorque() extFTObject.ModelTag = "externalDisturbance" - extFTObject.extTorquePntB_B = [[torquePntB_B[0]], [torquePntB_B[1]], [torquePntB_B[2]]] + extFTObject.extTorquePntB_B = [ + [torquePntB_B[0]], + [torquePntB_B[1]], + [torquePntB_B[2]], + ] extFTObject.extForce_B = [[force_B[0]], [force_B[1]], [force_B[2]]] scObject.addDynamicEffector(extFTObject) unitTestSim.AddModelToTask(unitTaskName, extFTObject) @@ -857,8 +1117,8 @@ def SCPointBVsPointC(show_plots): scObject.hub.mHub = 100 scObject.hub.r_BcB_B = [[rBcB_B[0]], [rBcB_B[1]], [rBcB_B[2]]] scObject.hub.IHubPntBc_B = [[500, 0.0, 0.0], [0.0, 200, 0.0], [0.0, 0.0, 300]] - scObject.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] - scObject.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] + scObject.hub.r_CN_NInit = [[0.0], [0.0], [0.0]] + scObject.hub.v_CN_NInit = [[0.0], [0.0], [0.0]] scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] scObject.hub.omega_BN_BInit = [[0.5], [-0.4], [0.7]] @@ -873,13 +1133,39 @@ def SCPointBVsPointC(show_plots): plt.figure() plt.clf() - plt.plot(r_CN_NOutput1[:,0]*1e-9, r_CN_NOutput1[:,1], 'k', label = 'Torque About Point C', linewidth=3.0) - plt.plot(r_CN_NOutput1[:,0]*1e-9,r_CN_NOutput1[:,2], 'k', r_CN_NOutput1[:,0]*1e-9, r_CN_NOutput1[:,3], 'k', linewidth=3.0) - plt.plot(r_CN_NOutput2[:,0]*1e-9, r_CN_NOutput2[:,1], '--c', label = 'Torque About Point B') - plt.plot(r_CN_NOutput2[:,0]*1e-9,r_CN_NOutput2[:,2], '--c', r_CN_NOutput2[:,0]*1e-9, r_CN_NOutput1[:,3], '--c') - plt.xlabel('Time (s)') - plt.ylabel('Inertial Position (m)') - plt.legend(loc ='upper left', handlelength=3.5) + plt.plot( + r_CN_NOutput1[:, 0] * 1e-9, + r_CN_NOutput1[:, 1], + "k", + label="Torque About Point C", + linewidth=3.0, + ) + plt.plot( + r_CN_NOutput1[:, 0] * 1e-9, + r_CN_NOutput1[:, 2], + "k", + r_CN_NOutput1[:, 0] * 1e-9, + r_CN_NOutput1[:, 3], + "k", + linewidth=3.0, + ) + plt.plot( + r_CN_NOutput2[:, 0] * 1e-9, + r_CN_NOutput2[:, 1], + "--c", + label="Torque About Point B", + ) + plt.plot( + r_CN_NOutput2[:, 0] * 1e-9, + r_CN_NOutput2[:, 2], + "--c", + r_CN_NOutput2[:, 0] * 1e-9, + r_CN_NOutput1[:, 3], + "--c", + ) + plt.xlabel("Time (s)") + plt.ylabel("Inertial Position (m)") + plt.legend(loc="upper left", handlelength=3.5) PlotName = "scPlusPointBVsPointCTranslation" PlotTitle = "PointB Vs PointC Translation" format = r"width=0.8\textwidth" @@ -887,29 +1173,63 @@ def SCPointBVsPointC(show_plots): plt.figure() plt.clf() - plt.plot(sigma_BNOutput1[:,0]*1e-9, sigma_BNOutput1[:,1], 'k', label = 'Torque About Point C', linewidth=3.0) - plt.plot(sigma_BNOutput1[:,0]*1e-9, sigma_BNOutput1[:,2], 'k', sigma_BNOutput1[:,0]*1e-9, sigma_BNOutput1[:,3], 'k', linewidth=3.0) - plt.plot(sigma_BNOutput2[:,0]*1e-9, sigma_BNOutput2[:,1], '--c', label = 'Torque About Point B') - plt.plot(sigma_BNOutput2[:,0]*1e-9, sigma_BNOutput2[:,2], '--c', sigma_BNOutput2[:,0]*1e-9, sigma_BNOutput2[:,3], '--c') - plt.xlabel('Time (s)') - plt.ylabel('MRPs') - plt.legend(loc ='upper right', handlelength=3.5) + plt.plot( + sigma_BNOutput1[:, 0] * 1e-9, + sigma_BNOutput1[:, 1], + "k", + label="Torque About Point C", + linewidth=3.0, + ) + plt.plot( + sigma_BNOutput1[:, 0] * 1e-9, + sigma_BNOutput1[:, 2], + "k", + sigma_BNOutput1[:, 0] * 1e-9, + sigma_BNOutput1[:, 3], + "k", + linewidth=3.0, + ) + plt.plot( + sigma_BNOutput2[:, 0] * 1e-9, + sigma_BNOutput2[:, 1], + "--c", + label="Torque About Point B", + ) + plt.plot( + sigma_BNOutput2[:, 0] * 1e-9, + sigma_BNOutput2[:, 2], + "--c", + sigma_BNOutput2[:, 0] * 1e-9, + sigma_BNOutput2[:, 3], + "--c", + ) + plt.xlabel("Time (s)") + plt.ylabel("MRPs") + plt.legend(loc="upper right", handlelength=3.5) PlotName = "scPlusPointBVsPointCAttitude" PlotTitle = "PointB Vs PointC Attitude" format = r"width=0.8\textwidth" unitTestSupport.writeFigureLaTeX(PlotName, PlotTitle, plt, format, path) if show_plots: plt.show() - plt.close('all') + plt.close("all") accuracy = 1e-8 - if not unitTestSupport.isArrayEqualRelative(r_CN_NOutput1[-1,1:4],r_CN_NOutput2[-1,1:4],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + r_CN_NOutput1[-1, 1:4], r_CN_NOutput2[-1, 1:4], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Point B Vs Point C test failed pos unit test") + testMessages.append( + "FAILED: Spacecraft Point B Vs Point C test failed pos unit test" + ) - if not unitTestSupport.isArrayEqualRelative(sigma_BNOutput1[-1,1:4],sigma_BNOutput2[-1,1:4],3,accuracy): + if not unitTestSupport.isArrayEqualRelative( + sigma_BNOutput1[-1, 1:4], sigma_BNOutput2[-1, 1:4], 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Point B Vs Point C test failed attitude unit test") + testMessages.append( + "FAILED: Spacecraft Point B Vs Point C test failed attitude unit test" + ) if testFailCount == 0: print("PASSED: " + " Spacecraft Point B Vs Point C Integrated Sim Test") @@ -918,7 +1238,8 @@ def SCPointBVsPointC(show_plots): # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + @pytest.mark.parametrize("accuracy", [1e-3]) def scOptionalRef(show_plots, accuracy): @@ -956,7 +1277,9 @@ def scOptionalRef(show_plots, accuracy): gravFactory = simIncludeGravBody.gravBodyFactory() earth = gravFactory.createEarth() earth.isCentralBody = True # ensure this is the central gravitational body - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # add gravity gradient effector ggEff = GravityGradientEffector.GravityGradientEffector() @@ -969,10 +1292,10 @@ def scOptionalRef(show_plots, accuracy): scObject.hub.mHub = 100 scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] scObject.hub.IHubPntBc_B = [[500, 0.0, 0.0], [0.0, 200, 0.0], [0.0, 0.0, 300]] - scObject.hub.r_CN_NInit = [[7000000.0], [0.0], [0.0]] - scObject.hub.v_CN_NInit = [[7000.0], [0.0], [0.0]] + scObject.hub.r_CN_NInit = [[7000000.0], [0.0], [0.0]] + scObject.hub.v_CN_NInit = [[7000.0], [0.0], [0.0]] scObject.hub.sigma_BNInit = [[0.5], [0.4], [0.3]] - scObject.hub.sigma_BNInit = [[0.], [0.], [1.0]] + scObject.hub.sigma_BNInit = [[0.0], [0.0], [1.0]] scObject.hub.omega_BN_BInit = [[0.5], [-0.4], [0.7]] # write attitude reference message @@ -1000,23 +1323,23 @@ def scOptionalRef(show_plots, accuracy): r_RN_Out = dataLog.r_BN_N v_RN_Out = dataLog.v_BN_N - trueSigma = [attRef.sigma_RN]*3 - trueOmega = [[-0.0001, -0.0002, 0.0003]]*3 - truer_RN_N = [transRef.r_RN_N]*3 - truev_RN_N = [transRef.v_RN_N]*3 - - testFailCount, testMessages = unitTestSupport.compareArray(trueSigma, sigmaOut, - accuracy, "sigma_BN", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArray(trueOmega, omegaOut, - accuracy, "omega_BN_B", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArray(truer_RN_N, r_RN_Out, - accuracy, "r_RN_N", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArray(truev_RN_N, v_RN_Out, - accuracy, "v_RN_N", - testFailCount, testMessages) + trueSigma = [attRef.sigma_RN] * 3 + trueOmega = [[-0.0001, -0.0002, 0.0003]] * 3 + truer_RN_N = [transRef.r_RN_N] * 3 + truev_RN_N = [transRef.v_RN_N] * 3 + + testFailCount, testMessages = unitTestSupport.compareArray( + trueSigma, sigmaOut, accuracy, "sigma_BN", testFailCount, testMessages + ) + testFailCount, testMessages = unitTestSupport.compareArray( + trueOmega, omegaOut, accuracy, "omega_BN_B", testFailCount, testMessages + ) + testFailCount, testMessages = unitTestSupport.compareArray( + truer_RN_N, r_RN_Out, accuracy, "r_RN_N", testFailCount, testMessages + ) + testFailCount, testMessages = unitTestSupport.compareArray( + truev_RN_N, v_RN_Out, accuracy, "v_RN_N", testFailCount, testMessages + ) if testFailCount == 0: print("PASSED: scPlus setting optional reference state input message") @@ -1025,7 +1348,8 @@ def scOptionalRef(show_plots, accuracy): assert testFailCount < 1, testMessages - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def scAccumDV(): """Module Unit Test""" @@ -1062,16 +1386,18 @@ def scAccumDV(): gravFactory = simIncludeGravBody.gravBodyFactory() earth = gravFactory.createEarth() earth.isCentralBody = True # ensure this is the central gravitational body - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # Define initial conditions of the spacecraft scObject.hub.mHub = 100 scObject.hub.r_BcB_B = [[0.0], [100.0], [0.0]] scObject.hub.IHubPntBc_B = [[500, 0.0, 0.0], [0.0, 200, 0.0], [0.0, 0.0, 300]] - scObject.hub.r_CN_NInit = [[-7000000.0], [0.0], [0.0]] - scObject.hub.v_CN_NInit = [[0.0], [7000.0], [0.0]] + scObject.hub.r_CN_NInit = [[-7000000.0], [0.0], [0.0]] + scObject.hub.v_CN_NInit = [[0.0], [7000.0], [0.0]] scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] - scObject.hub.omega_BN_BInit = [[0.0], [0.0], [numpy.pi/180]] + scObject.hub.omega_BN_BInit = [[0.0], [0.0], [numpy.pi / 180]] unitTestSim.InitializeSimulation() @@ -1086,28 +1412,44 @@ def scAccumDV(): accuracy = 1e-10 truth_dataAccumDV_CN_B = [0.0, 0.0, 0.0] truth_dataAccumDV_CN_N = [0.0, 0.0, 0.0] - v_r = numpy.cross(numpy.array(scObject.hub.omega_BN_BInit).T, -numpy.array(scObject.hub.r_BcB_B).T)[0] + v_r = numpy.cross( + numpy.array(scObject.hub.omega_BN_BInit).T, -numpy.array(scObject.hub.r_BcB_B).T + )[0] truth_dataAccumDV_BN_B = numpy.zeros(3) - for i in range(len(dataLog.times())-1): - if not unitTestSupport.isArrayEqual(dataAccumDV_CN_B[i+1],truth_dataAccumDV_CN_B,3,accuracy): + for i in range(len(dataLog.times()) - 1): + if not unitTestSupport.isArrayEqual( + dataAccumDV_CN_B[i + 1], truth_dataAccumDV_CN_B, 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Point C Accumulated DV test failed pos unit test") - - truth_dataAccumDV_BN_B += numpy.matmul(RigidBodyKinematics.MRP2C(dataLog.sigma_BN[i+1]), - numpy.matmul(RigidBodyKinematics.MRP2C(dataLog.sigma_BN[i+1]).T,v_r) - - numpy.matmul(RigidBodyKinematics.MRP2C(dataLog.sigma_BN[i]).T,v_r)) - if not unitTestSupport.isArrayEqual(dataAccumDV_BN_B[i+1],truth_dataAccumDV_BN_B,3,accuracy): + testMessages.append( + "FAILED: Spacecraft Point C Accumulated DV test failed pos unit test" + ) + + truth_dataAccumDV_BN_B += numpy.matmul( + RigidBodyKinematics.MRP2C(dataLog.sigma_BN[i + 1]), + numpy.matmul(RigidBodyKinematics.MRP2C(dataLog.sigma_BN[i + 1]).T, v_r) + - numpy.matmul(RigidBodyKinematics.MRP2C(dataLog.sigma_BN[i]).T, v_r), + ) + if not unitTestSupport.isArrayEqual( + dataAccumDV_BN_B[i + 1], truth_dataAccumDV_BN_B, 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Point B Accumulated DV test failed pos unit test") + testMessages.append( + "FAILED: Spacecraft Point B Accumulated DV test failed pos unit test" + ) - if not unitTestSupport.isArrayEqual(dataAccumDV_CN_N[i+1],truth_dataAccumDV_CN_N,3,accuracy): + if not unitTestSupport.isArrayEqual( + dataAccumDV_CN_N[i + 1], truth_dataAccumDV_CN_N, 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Point C Accumulated DV in inertial frame test failed pos unit test") + testMessages.append( + "FAILED: Spacecraft Point C Accumulated DV in inertial frame test failed pos unit test" + ) if testFailCount == 0: print("PASSED: Spacecraft Accumulated DV tests with offset CoM") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] def scAccumDVExtForce(): @@ -1164,14 +1506,18 @@ def scAccumDVExtForce(): accuracy = 1e-10 for i in range(len(dataLog.times())): truth_dataAccumDV_CN_N = extForce * timeArraySec[i] / scObject.hub.mHub - if not unitTestSupport.isArrayEqual(dataAccumDV_CN_N[i], truth_dataAccumDV_CN_N, 3, accuracy): + if not unitTestSupport.isArrayEqual( + dataAccumDV_CN_N[i], truth_dataAccumDV_CN_N, 3, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spacecraft Point C Accumulated DV with external force test failed unit test") + testMessages.append( + "FAILED: Spacecraft Point C Accumulated DV with external force test failed unit test" + ) if testFailCount == 0: print("PASSED: Spacecraft Accumulated DV tests with offset CoM") - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": diff --git a/src/simulation/dynamics/sphericalPendulum/_UnitTest/test_sphericalPendulum.py b/src/simulation/dynamics/sphericalPendulum/_UnitTest/test_sphericalPendulum.py index ec6788e857..d0ba99044d 100644 --- a/src/simulation/dynamics/sphericalPendulum/_UnitTest/test_sphericalPendulum.py +++ b/src/simulation/dynamics/sphericalPendulum/_UnitTest/test_sphericalPendulum.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,8 +15,6 @@ # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - - import inspect import os @@ -29,7 +26,9 @@ path = os.path.dirname(os.path.abspath(filename)) from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.simulation import spacecraft from Basilisk.simulation import sphericalPendulum from Basilisk.utilities import simIncludeGravBody @@ -41,11 +40,8 @@ from Basilisk.utilities import simIncludeThruster from Basilisk.architecture import messaging -@pytest.mark.parametrize("useFlag, testCase", [ - (False, 1), - (False, 2), - (False,3) -]) + +@pytest.mark.parametrize("useFlag, testCase", [(False, 1), (False, 2), (False, 3)]) # provide a unique test method name, starting with test_ def test_scenarioSphericalPendulum(show_plots, useFlag, testCase): """This function is called by the py.test environment.""" @@ -53,12 +49,13 @@ def test_scenarioSphericalPendulum(show_plots, useFlag, testCase): [testResults, testMessage] = sphericalPendulumTest(show_plots, useFlag, testCase) assert testResults < 1, testMessage -def sphericalPendulumTest(show_plots, useFlag,testCase): + +def sphericalPendulumTest(show_plots, useFlag, testCase): """Call this routine directly to run the test scenario.""" - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages - if testCase == 1 or testCase == 3: + if testCase == 1 or testCase == 3: timeStep = 0.01 if testCase == 2: timeStep = 0.001 @@ -66,9 +63,9 @@ def sphericalPendulumTest(show_plots, useFlag,testCase): simTaskName = "simTask" simProcessName = "simProcess" # create simulation - scSim=SimulationBaseClass.SimBaseClass() + scSim = SimulationBaseClass.SimBaseClass() # close possible other simulation - #crete a dynamical process + # crete a dynamical process dynProcess = scSim.CreateNewProcess(simProcessName) simulationTimeStep = macros.sec2nano(timeStep) # add task to the dynamical process @@ -84,47 +81,57 @@ def sphericalPendulumTest(show_plots, useFlag,testCase): scSim.pendulum1 = sphericalPendulum.SphericalPendulum() # Define Variables for pendulum 1 scSim.pendulum1.pendulumRadius = 0.3 # m/s - scSim.pendulum1.d = [[0.1], [0.1], [0.1]] # m - scSim.pendulum1.D = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # N*s/m - scSim.pendulum1.phiDotInit = 0.01 # rad/s - scSim.pendulum1.thetaDotInit = 0.05 # rad/s - scSim.pendulum1.massInit = 20.0 # kg - scSim.pendulum1.pHat_01=[[np.sqrt(2)/2], [0] , [np.sqrt(2)/2]] # first unit vector of the Pendulum frame - scSim.pendulum1.pHat_02=[[0],[1],[0]] # second unit vector of the Pendulum frame - scSim.pendulum1.pHat_03=[[-np.sqrt(2)/2],[0],[np.sqrt(2)/2]] # third unit vector of the Pendulum frame + scSim.pendulum1.d = [[0.1], [0.1], [0.1]] # m + scSim.pendulum1.D = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # N*s/m + scSim.pendulum1.phiDotInit = 0.01 # rad/s + scSim.pendulum1.thetaDotInit = 0.05 # rad/s + scSim.pendulum1.massInit = 20.0 # kg + scSim.pendulum1.pHat_01 = [ + [np.sqrt(2) / 2], + [0], + [np.sqrt(2) / 2], + ] # first unit vector of the Pendulum frame + scSim.pendulum1.pHat_02 = [ + [0], + [1], + [0], + ] # second unit vector of the Pendulum frame + scSim.pendulum1.pHat_03 = [ + [-np.sqrt(2) / 2], + [0], + [np.sqrt(2) / 2], + ] # third unit vector of the Pendulum frame # Pendulum 2 scSim.pendulum2 = sphericalPendulum.SphericalPendulum() # Define Variables for pendulum 2 scSim.pendulum2.pendulumRadius = 0.4 # m/s - scSim.pendulum2.d = [[0.1], [0.1], [0.1]] # m - scSim.pendulum2.D = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # N*s/m - scSim.pendulum2.phiDotInit = 0.1 # rad/s - scSim.pendulum2.thetaDotInit = 0.5 # rad/s - scSim.pendulum2.massInit =40.0 # kg + scSim.pendulum2.d = [[0.1], [0.1], [0.1]] # m + scSim.pendulum2.D = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # N*s/m + scSim.pendulum2.phiDotInit = 0.1 # rad/s + scSim.pendulum2.thetaDotInit = 0.5 # rad/s + scSim.pendulum2.massInit = 40.0 # kg # Pendulum frame same as Body frame if testCase == 3: # add thruster devices thFactory = simIncludeThruster.thrusterFactory() - thFactory.create('MOOG_Monarc_445', - [1,0,0], # location in S frame - [0,1,0] # direction in S frame - ) - + thFactory.create( + "MOOG_Monarc_445", + [1, 0, 0], # location in S frame + [0, 1, 0], # direction in S frame + ) # create thruster object container and tie to spacecraft object thrustersDynamicEffector = thrusterDynamicEffector.ThrusterDynamicEffector() - thFactory.addToSpacecraft("Thrusters", - thrustersDynamicEffector, - scObject) + thFactory.addToSpacecraft("Thrusters", thrustersDynamicEffector, scObject) scSim.fuelTankStateEffector = fuelTank.FuelTank() tankModel = fuelTank.FuelTankModelConstantVolume() scSim.fuelTankStateEffector.setTankModel(tankModel) tankModel.propMassInit = 40.0 - tankModel.r_TcT_TInit = [[0.0],[0.0],[0.0]] - scSim.fuelTankStateEffector.r_TB_B = [[0.0],[0.0],[0.0]] + tankModel.r_TcT_TInit = [[0.0], [0.0], [0.0]] + scSim.fuelTankStateEffector.r_TB_B = [[0.0], [0.0], [0.0]] tankModel.radiusTankInit = 46.0 / 2.0 / 3.2808399 / 12.0 # Add tank and thruster @@ -149,11 +156,15 @@ def sphericalPendulumTest(show_plots, useFlag,testCase): scSim.fuelTankStateEffector.pushFuelSloshParticle(scSim.pendulum2) # define hub properties - scObject.hub.mHub = 1500 # kg - scObject.hub.r_BcB_B = [[1.0], [0.5], [0.1]] # m - scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] # kg*m^2 - scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] # rad - scObject.hub.omega_BN_BInit = [[1.0], [0.5], [0.1]] # rad/s + scObject.hub.mHub = 1500 # kg + scObject.hub.r_BcB_B = [[1.0], [0.5], [0.1]] # m + scObject.hub.IHubPntBc_B = [ + [900.0, 0.0, 0.0], + [0.0, 800.0, 0.0], + [0.0, 0.0, 600.0], + ] # kg*m^2 + scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] # rad + scObject.hub.omega_BN_BInit = [[1.0], [0.5], [0.1]] # rad/s # Add fuel slosh to spacecraft scObject.addStateEffector(scSim.pendulum1) @@ -168,16 +179,18 @@ def sphericalPendulumTest(show_plots, useFlag,testCase): mu = planet.mu # attach gravity to the spacecraft - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # initialize orbital elements oe = orbitalMotion.ClassicElements() - oe.a=6700.0*1000 - oe.e=0.01 - oe.omega=100.0*macros.D2R - oe.Omega=100.0*macros.D2R - oe.i=30.0*macros.D2R - oe.f=0.0 + oe.a = 6700.0 * 1000 + oe.e = 0.01 + oe.omega = 100.0 * macros.D2R + oe.Omega = 100.0 * macros.D2R + oe.i = 30.0 * macros.D2R + oe.f = 0.0 # convert them in position and velocity rN, vN = orbitalMotion.elem2rv(mu, oe) @@ -191,7 +204,9 @@ def sphericalPendulumTest(show_plots, useFlag,testCase): # Setup data logging before the simulation is initialized # numDataPoints = 100 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) @@ -200,14 +215,22 @@ def sphericalPendulumTest(show_plots, useFlag,testCase): # If the routine InitializeSimulationAndDiscover() is run instead of InitializeSimulation(), # then the all messages are auto-discovered that are shared across different BSK threads. # - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"] + ) scSim.AddModelToTask(simTaskName, scObjectLog) if testCase == 3: - stateLog = pythonVariableLogger.PythonVariableLogger({ - "sphericalPendulumMass1": lambda _: scObject.dynManager.getStateObject(scSim.pendulum1.nameOfMassState).getState(), - "sphericalPendulumMass2": lambda _: scObject.dynManager.getStateObject(scSim.pendulum2.nameOfMassState).getState(), - }) + stateLog = pythonVariableLogger.PythonVariableLogger( + { + "sphericalPendulumMass1": lambda _: scObject.dynManager.getStateObject( + scSim.pendulum1.nameOfMassState + ).getState(), + "sphericalPendulumMass2": lambda _: scObject.dynManager.getStateObject( + scSim.pendulum2.nameOfMassState + ).getState(), + } + ) scSim.AddModelToTask(simTaskName, stateLog) scSim.InitializeSimulation() @@ -221,15 +244,26 @@ def sphericalPendulumTest(show_plots, useFlag,testCase): if testCase == 3: fuelMass = fuelLog.fuelMass fuelMassDot = fuelLog.fuelMassDot - mass1Out = unitTestSupport.addTimeColumn(stateLog.times(), stateLog.sphericalPendulumMass1) - mass2Out = unitTestSupport.addTimeColumn(stateLog.times(), stateLog.sphericalPendulumMass2) - + mass1Out = unitTestSupport.addTimeColumn( + stateLog.times(), stateLog.sphericalPendulumMass1 + ) + mass2Out = unitTestSupport.addTimeColumn( + stateLog.times(), stateLog.sphericalPendulumMass2 + ) # request energy and momentum - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) if timeStep == 0.01: testCaseName = "OneHundredth" @@ -239,87 +273,197 @@ def sphericalPendulumTest(show_plots, useFlag,testCase): plt.close("all") # clears out plots from earlier test runs if testCase != 3: - plt.figure(1,figsize=(5,4)) - plt.plot(orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,1] - orbAngMom_N[0,1])/orbAngMom_N[0,1], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,2] - orbAngMom_N[0,2])/orbAngMom_N[0,2], orbAngMom_N[:,0]*1e-9, (orbAngMom_N[:,3] - orbAngMom_N[0,3])/orbAngMom_N[0,3]) - plt.xlabel('Time (s)') - plt.ylabel('Relative Orbital Angular Momentum Variation') - unitTestSupport.writeFigureLaTeX("ChangeInOrbitalAngularMomentum" + testCaseName, "Change in Orbital Angular Momentum " + testCaseName, plt, r"width=0.8\textwidth", path) - - plt.figure(2,figsize=(5,4)) - plt.plot(orbEnergy[:,0]*1e-9, (orbEnergy[:,1] - orbEnergy[0,1])/orbEnergy[0,1]) - plt.xlabel('Time (s)') - plt.ylabel('Relative Orbital Energy Variation') - unitTestSupport.writeFigureLaTeX("ChangeInOrbitalEnergy" + testCaseName, "Change in Orbital Energy " + testCaseName, plt, r"width=0.8\textwidth", path) - - plt.figure(3,figsize=(5,4)) - plt.plot(rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,1] - rotAngMom_N[0,1])/rotAngMom_N[0,1], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,2] - rotAngMom_N[0,2])/rotAngMom_N[0,2], rotAngMom_N[:,0]*1e-9, (rotAngMom_N[:,3] - rotAngMom_N[0,3])/rotAngMom_N[0,3]) - plt.xlabel('Time (s)') - plt.ylabel('Relative Rotational Angular Momentum Variation') - unitTestSupport.writeFigureLaTeX("ChangeInRotationalAngularMomentum" + testCaseName, "Change in Rotational Angular Momentum " + testCaseName, plt, r"width=0.8\textwidth", path) - - plt.figure(4,figsize=(5,4)) - plt.plot(rotEnergy[:,0]*1e-9, (rotEnergy[:,1] - rotEnergy[0,1])/rotEnergy[0,1]) - plt.xlabel('Time (s)') - plt.ylabel('Relative Rotational Energy Variation') - unitTestSupport.writeFigureLaTeX("ChangeInRotationalEnergy" + testCaseName, "Change in Rotational Energy " + testCaseName, plt, r"width=0.8\textwidth", path) + plt.figure(1, figsize=(5, 4)) + plt.plot( + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3], + ) + plt.xlabel("Time (s)") + plt.ylabel("Relative Orbital Angular Momentum Variation") + unitTestSupport.writeFigureLaTeX( + "ChangeInOrbitalAngularMomentum" + testCaseName, + "Change in Orbital Angular Momentum " + testCaseName, + plt, + r"width=0.8\textwidth", + path, + ) + + plt.figure(2, figsize=(5, 4)) + plt.plot( + orbEnergy[:, 0] * 1e-9, + (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1], + ) + plt.xlabel("Time (s)") + plt.ylabel("Relative Orbital Energy Variation") + unitTestSupport.writeFigureLaTeX( + "ChangeInOrbitalEnergy" + testCaseName, + "Change in Orbital Energy " + testCaseName, + plt, + r"width=0.8\textwidth", + path, + ) + + plt.figure(3, figsize=(5, 4)) + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) / rotAngMom_N[0, 1], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) / rotAngMom_N[0, 2], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) / rotAngMom_N[0, 3], + ) + plt.xlabel("Time (s)") + plt.ylabel("Relative Rotational Angular Momentum Variation") + unitTestSupport.writeFigureLaTeX( + "ChangeInRotationalAngularMomentum" + testCaseName, + "Change in Rotational Angular Momentum " + testCaseName, + plt, + r"width=0.8\textwidth", + path, + ) + + plt.figure(4, figsize=(5, 4)) + plt.plot( + rotEnergy[:, 0] * 1e-9, + (rotEnergy[:, 1] - rotEnergy[0, 1]) / rotEnergy[0, 1], + ) + plt.xlabel("Time (s)") + plt.ylabel("Relative Rotational Energy Variation") + unitTestSupport.writeFigureLaTeX( + "ChangeInRotationalEnergy" + testCaseName, + "Change in Rotational Energy " + testCaseName, + plt, + r"width=0.8\textwidth", + path, + ) if testCase == 3: plt.figure() - plt.plot(fuelLog.times()*1e-9, fuelMass) + plt.plot(fuelLog.times() * 1e-9, fuelMass) plt.title("Tank Fuel Mass") plt.figure() - plt.plot(fuelLog.times()*1e-9, fuelMassDot) + plt.plot(fuelLog.times() * 1e-9, fuelMassDot) plt.title("Tank Fuel Mass Dot") plt.figure() - plt.plot(mass1Out[:,0]*1e-9, mass1Out[:,1]) + plt.plot(mass1Out[:, 0] * 1e-9, mass1Out[:, 1]) plt.title("Fuel Particle 1 Mass") plt.figure() - plt.plot(mass2Out[:,0]*1e-9, mass2Out[:,1]) + plt.plot(mass2Out[:, 0] * 1e-9, mass2Out[:, 1]) plt.title("Fuel Particle 2 Mass") mDotFuel = -0.19392039093 - mDotParicle1True = mDotFuel*(20./100.) - mDotParicle2True = mDotFuel*(40./100.) - mDotParicle1Data = [0,(mass1Out[2,1] - mass1Out[1,1])/((mass1Out[2,0] - mass1Out[1,0])*1e-9)] - mDotParicle2Data = [0,(mass2Out[2,1] - mass2Out[1,1])/((mass2Out[2,0] - mass2Out[1,0])*1e-9)] + mDotParicle1True = mDotFuel * (20.0 / 100.0) + mDotParicle2True = mDotFuel * (40.0 / 100.0) + mDotParicle1Data = [ + 0, + (mass1Out[2, 1] - mass1Out[1, 1]) + / ((mass1Out[2, 0] - mass1Out[1, 0]) * 1e-9), + ] + mDotParicle2Data = [ + 0, + (mass2Out[2, 1] - mass2Out[1, 1]) + / ((mass2Out[2, 0] - mass2Out[1, 0]) * 1e-9), + ] if show_plots: plt.show() - plt.close('all') + plt.close("all") if testCase != 3: accuracy = 1e-8 - for k in range(len((rotAngMom_N[:,1]))): - if abs((rotAngMom_N[k,1] - rotAngMom_N[0,1])/rotAngMom_N[0,1])>accuracy: + for k in range(len((rotAngMom_N[:, 1]))): + if ( + abs((rotAngMom_N[k, 1] - rotAngMom_N[0, 1]) / rotAngMom_N[0, 1]) + > accuracy + ): testFailCount += 1 - testMessages.append("FAILED: SphericalPendulum does not conserve Rotational Angular Momentum around x axes (timeStep={}s)".format(timeStep)) - if abs((rotAngMom_N[k,2] - rotAngMom_N[0,2])/rotAngMom_N[0,2])>accuracy: + testMessages.append( + "FAILED: SphericalPendulum does not conserve Rotational Angular Momentum around x axes (timeStep={}s)".format( + timeStep + ) + ) + if ( + abs((rotAngMom_N[k, 2] - rotAngMom_N[0, 2]) / rotAngMom_N[0, 2]) + > accuracy + ): testFailCount += 1 - testMessages.append("FAILED: SphericalPendulum does not conserve Rotational Angular Momentum around y axes (timeStep={}s)".format(timeStep)) - if abs((rotAngMom_N[k,3] - rotAngMom_N[0,3])/rotAngMom_N[0,3])>accuracy: + testMessages.append( + "FAILED: SphericalPendulum does not conserve Rotational Angular Momentum around y axes (timeStep={}s)".format( + timeStep + ) + ) + if ( + abs((rotAngMom_N[k, 3] - rotAngMom_N[0, 3]) / rotAngMom_N[0, 3]) + > accuracy + ): testFailCount += 1 - testMessages.append("FAILED: SphericalPendulum does not conserve Rotational Angular Momentum around z axes (timeStep={}s)".format(timeStep)) - if abs((orbAngMom_N[k,1] - orbAngMom_N[0,1])/orbAngMom_N[0,1])>accuracy: + testMessages.append( + "FAILED: SphericalPendulum does not conserve Rotational Angular Momentum around z axes (timeStep={}s)".format( + timeStep + ) + ) + if ( + abs((orbAngMom_N[k, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1]) + > accuracy + ): testFailCount += 1 - testMessages.append("FAILED: SphericalPendulum does not conserve Orbital Angular Momentum around x axes (timeStep={}s)".format(timeStep)) - if abs((orbAngMom_N[k,2] - orbAngMom_N[0,2])/orbAngMom_N[0,2])>accuracy: + testMessages.append( + "FAILED: SphericalPendulum does not conserve Orbital Angular Momentum around x axes (timeStep={}s)".format( + timeStep + ) + ) + if ( + abs((orbAngMom_N[k, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2]) + > accuracy + ): testFailCount += 1 - testMessages.append("FAILED: SphericalPendulum does not conserve Orbital Angular Momentum around y axes (timeStep={}s)".format(timeStep)) - if abs((orbAngMom_N[k,3] - orbAngMom_N[0,3])/orbAngMom_N[0,3])>accuracy: + testMessages.append( + "FAILED: SphericalPendulum does not conserve Orbital Angular Momentum around y axes (timeStep={}s)".format( + timeStep + ) + ) + if ( + abs((orbAngMom_N[k, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3]) + > accuracy + ): testFailCount += 1 - testMessages.append("FAILED: SphericalPendulum does not conserve Orbital Angular Momentum around z axes (timeStep={}s)".format(timeStep)) - if abs((rotEnergy[k,1] - rotEnergy[0,1])/rotEnergy[0,1])>accuracy: + testMessages.append( + "FAILED: SphericalPendulum does not conserve Orbital Angular Momentum around z axes (timeStep={}s)".format( + timeStep + ) + ) + if abs((rotEnergy[k, 1] - rotEnergy[0, 1]) / rotEnergy[0, 1]) > accuracy: testFailCount += 1 - testMessages.append("FAILED: SphericalPendulum does not conserve Rotational Energy (timeStep={}s)".format(timeStep)) - if abs((orbEnergy[k,1] - orbEnergy[0,1])/orbEnergy[0,1])>accuracy: + testMessages.append( + "FAILED: SphericalPendulum does not conserve Rotational Energy (timeStep={}s)".format( + timeStep + ) + ) + if abs((orbEnergy[k, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1]) > accuracy: testFailCount += 1 - testMessages.append("FAILED: SphericalPendulum does not conserve Orbital Energy (timeStep={}s)".format(timeStep)) + testMessages.append( + "FAILED: SphericalPendulum does not conserve Orbital Energy (timeStep={}s)".format( + timeStep + ) + ) if testCase == 3: accuracy = 1e-4 - if not unitTestSupport.isDoubleEqual(mDotParicle1Data[1],mDotParicle1True,accuracy): + if not unitTestSupport.isDoubleEqual( + mDotParicle1Data[1], mDotParicle1True, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Linear Spring Mass Damper unit test failed mass 1 dot test") - if not unitTestSupport.isDoubleEqual(mDotParicle2Data[1],mDotParicle2True,accuracy): + testMessages.append( + "FAILED: Linear Spring Mass Damper unit test failed mass 1 dot test" + ) + if not unitTestSupport.isDoubleEqual( + mDotParicle2Data[1], mDotParicle2True, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Linear Spring Mass Damper unit test failed mass 2 dot test") + testMessages.append( + "FAILED: Linear Spring Mass Damper unit test failed mass 2 dot test" + ) if testFailCount == 0: print("PASSED ") @@ -327,10 +471,12 @@ def sphericalPendulumTest(show_plots, useFlag,testCase): print(testFailCount) print(testMessages) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + if __name__ == "__main__": - sphericalPendulumTest(True, # showplots - False, # useFlag - 3, # testCase - ) + sphericalPendulumTest( + True, # showplots + False, # useFlag + 3, # testCase + ) diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/_UnitTest/test_spinningBodyNDOFStateEffector.py b/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/_UnitTest/test_spinningBodyNDOFStateEffector.py index bd074f3437..ac3a806a7b 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/_UnitTest/test_spinningBodyNDOFStateEffector.py +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/_UnitTest/test_spinningBodyNDOFStateEffector.py @@ -30,10 +30,14 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') +splitPath = path.split("simulation") from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros -from Basilisk.simulation import spacecraft, spinningBodyNDOFStateEffector, gravityEffector +from Basilisk.simulation import ( + spacecraft, + spinningBodyNDOFStateEffector, + gravityEffector, +) from Basilisk.architecture import messaging @@ -43,9 +47,11 @@ # @pytest.mark.xfail() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("function", ["spinningBodyNoInput" - , "spinningBodyLockAxis" - , "spinningBodyCommandedTorque"]) + +@pytest.mark.parametrize( + "function", + ["spinningBodyNoInput", "spinningBodyLockAxis", "spinningBodyCommandedTorque"], +) def test_spinningBody(show_plots, function): r""" **Validation Test Description** @@ -102,16 +108,28 @@ def spinningBodyNoInput(show_plots): # Define properties of spinning bodies spinningBody1 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody1.setMass(np.random.uniform(5.0, 50.0)) - spinningBody1.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + spinningBody1.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) spinningBody1.setDCM_S0P([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 1.0]]) - spinningBody1.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody1.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + spinningBody1.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody1.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) spinningBody1.setSHat_S([[0], [0], [1]]) spinningBody1.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody1.setThetaDotInit(0.0 * macros.D2R) @@ -120,16 +138,28 @@ def spinningBodyNoInput(show_plots): spinningBody2 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody2.setMass(np.random.uniform(5.0, 50.0)) - spinningBody2.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) - spinningBody2.setDCM_S0P([[0.0, -1.0, 0.0], [0.0, .0, -1.0], [1.0, 0.0, 0.0]]) - spinningBody2.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody2.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + spinningBody2.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) + spinningBody2.setDCM_S0P([[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]]) + spinningBody2.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody2.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) spinningBody2.setSHat_S([[0], [-1], [0]]) spinningBody2.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody2.setThetaDotInit(0.0 * macros.D2R) @@ -138,17 +168,29 @@ def spinningBodyNoInput(show_plots): spinningBody3 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody3.setMass(np.random.uniform(5.0, 50.0)) - spinningBody3.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + spinningBody3.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) spinningBody3.setDCM_S0P([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]]) - spinningBody3.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody3.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody3.setSHat_S([[np.sqrt(1/2)], [np.sqrt(1/2)], [0]]) + spinningBody3.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody3.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody3.setSHat_S([[np.sqrt(1 / 2)], [np.sqrt(1 / 2)], [0]]) spinningBody3.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody3.setThetaDotInit(0.0 * macros.D2R) spinningBody3.setK(np.random.random()) @@ -156,17 +198,29 @@ def spinningBodyNoInput(show_plots): spinningBody4 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody4.setMass(np.random.uniform(5.0, 50.0)) - spinningBody4.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) - spinningBody4.setDCM_S0P([[0.0, 1.0, 0.0], [0.0, .0, 1.0], [1.0, 0.0, 0.0]]) - spinningBody4.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody4.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody4.setSHat_S([[np.sqrt(1/2)], [-np.sqrt(1/2)], [0]]) + spinningBody4.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) + spinningBody4.setDCM_S0P([[0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]]) + spinningBody4.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody4.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody4.setSHat_S([[np.sqrt(1 / 2)], [-np.sqrt(1 / 2)], [0]]) spinningBody4.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody4.setThetaDotInit(0.0 * macros.D2R) spinningBody4.setK(np.random.random()) @@ -181,8 +235,16 @@ def spinningBodyNoInput(show_plots): 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.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]] @@ -193,7 +255,7 @@ def spinningBodyNoInput(show_plots): # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -202,7 +264,9 @@ def spinningBodyNoInput(show_plots): unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totRotEnergy", "totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N"]) + scObjectLog = scObject.logger( + ["totRotEnergy", "totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Add states to log @@ -252,12 +316,17 @@ def spinningBodyNoInput(show_plots): plt.close("all") plt.figure() ax = plt.axes() - 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)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Orbital Angular Momentum', fontsize='22') + 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)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Orbital Angular Momentum", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -265,21 +334,26 @@ def spinningBodyNoInput(show_plots): plt.figure() ax = plt.axes() plt.plot(timeSec, (orbEnergy - initialOrbEnergy) / initialOrbEnergy) - plt.xlabel('time (s)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Orbital Energy', fontsize='22') + plt.xlabel("time (s)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Orbital Energy", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) plt.figure() ax = plt.axes() - 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)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Rotational Angular Momentum', fontsize='22') + 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)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Rotational Angular Momentum", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -287,32 +361,32 @@ def spinningBodyNoInput(show_plots): plt.figure() ax = plt.axes() plt.plot(timeSec, (rotEnergy - initialRotEnergy) / initialRotEnergy) - plt.xlabel('time (s)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Rotational Energy', fontsize='22') + plt.xlabel("time (s)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Rotational Energy", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) plt.figure() plt.clf() - plt.plot(timeSec, theta1, label=r'$\theta_1$') - plt.plot(timeSec, theta2, label=r'$\theta_2$') - plt.plot(timeSec, theta3, label=r'$\theta_3$') - plt.plot(timeSec, theta4, label=r'$\theta_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Angle') + plt.plot(timeSec, theta1, label=r"$\theta_1$") + plt.plot(timeSec, theta2, label=r"$\theta_2$") + plt.plot(timeSec, theta3, label=r"$\theta_3$") + plt.plot(timeSec, theta4, label=r"$\theta_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Angle") plt.figure() plt.clf() - plt.plot(timeSec, theta1Dot, label=r'$\dot{\theta}_1$') - plt.plot(timeSec, theta2Dot, label=r'$\dot{\theta}_2$') - plt.plot(timeSec, theta3Dot, label=r'$\dot{\theta}_3$') - plt.plot(timeSec, theta4Dot, label=r'$\dot{\theta}_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Angle Rate') + plt.plot(timeSec, theta1Dot, label=r"$\dot{\theta}_1$") + plt.plot(timeSec, theta2Dot, label=r"$\dot{\theta}_2$") + plt.plot(timeSec, theta3Dot, label=r"$\dot{\theta}_3$") + plt.plot(timeSec, theta4Dot, label=r"$\dot{\theta}_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Angle Rate") if show_plots: plt.show() @@ -355,16 +429,28 @@ def spinningBodyLockAxis(show_plots): # Define properties of spinning bodies spinningBody1 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody1.setMass(np.random.uniform(5.0, 50.0)) - spinningBody1.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + spinningBody1.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) spinningBody1.setDCM_S0P([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 1.0]]) - spinningBody1.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody1.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + spinningBody1.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody1.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) spinningBody1.setSHat_S([[0], [0], [1]]) spinningBody1.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody1.setThetaDotInit(np.random.uniform(-1.0, 1.0) * macros.D2R) @@ -373,16 +459,28 @@ def spinningBodyLockAxis(show_plots): spinningBody2 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody2.setMass(np.random.uniform(5.0, 50.0)) - spinningBody2.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) - spinningBody2.setDCM_S0P([[0.0, -1.0, 0.0], [0.0, .0, -1.0], [1.0, 0.0, 0.0]]) - spinningBody2.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody2.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + spinningBody2.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) + spinningBody2.setDCM_S0P([[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]]) + spinningBody2.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody2.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) spinningBody2.setSHat_S([[0], [-1], [0]]) spinningBody2.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody2.setThetaDotInit(np.random.uniform(-1.0, 1.0) * macros.D2R) @@ -391,17 +489,29 @@ def spinningBodyLockAxis(show_plots): spinningBody3 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody3.setMass(np.random.uniform(5.0, 50.0)) - spinningBody3.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + spinningBody3.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) spinningBody3.setDCM_S0P([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]]) - spinningBody3.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody3.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody3.setSHat_S([[np.sqrt(1/2)], [np.sqrt(1/2)], [0]]) + spinningBody3.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody3.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody3.setSHat_S([[np.sqrt(1 / 2)], [np.sqrt(1 / 2)], [0]]) spinningBody3.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody3.setThetaDotInit(np.random.uniform(-1.0, 1.0) * macros.D2R) spinningBody3.setK(np.random.random()) @@ -409,17 +519,29 @@ def spinningBodyLockAxis(show_plots): spinningBody4 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody4.setMass(np.random.uniform(5.0, 50.0)) - spinningBody4.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) - spinningBody4.setDCM_S0P([[0.0, 1.0, 0.0], [0.0, .0, 1.0], [1.0, 0.0, 0.0]]) - spinningBody4.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody4.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody4.setSHat_S([[np.sqrt(1/2)], [-np.sqrt(1/2)], [0]]) + spinningBody4.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) + spinningBody4.setDCM_S0P([[0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]]) + spinningBody4.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody4.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody4.setSHat_S([[np.sqrt(1 / 2)], [-np.sqrt(1 / 2)], [0]]) spinningBody4.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody4.setThetaDotInit(np.random.uniform(-1.0, 1.0) * macros.D2R) spinningBody4.setK(np.random.random()) @@ -434,8 +556,16 @@ def spinningBodyLockAxis(show_plots): 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.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]] @@ -452,7 +582,7 @@ def spinningBodyLockAxis(show_plots): # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -461,7 +591,9 @@ def spinningBodyLockAxis(show_plots): unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totRotEnergy", "totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N"]) + scObjectLog = scObject.logger( + ["totRotEnergy", "totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Add states to log @@ -511,12 +643,17 @@ def spinningBodyLockAxis(show_plots): plt.close("all") plt.figure() ax = plt.axes() - 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)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Orbital Angular Momentum', fontsize='22') + 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)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Orbital Angular Momentum", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -524,21 +661,26 @@ def spinningBodyLockAxis(show_plots): plt.figure() ax = plt.axes() plt.plot(timeSec, (orbEnergy - initialOrbEnergy) / initialOrbEnergy) - plt.xlabel('time (s)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Orbital Energy', fontsize='22') + plt.xlabel("time (s)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Orbital Energy", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) plt.figure() ax = plt.axes() - 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)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Rotational Angular Momentum', fontsize='22') + 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)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Rotational Angular Momentum", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -546,32 +688,32 @@ def spinningBodyLockAxis(show_plots): plt.figure() ax = plt.axes() plt.plot(timeSec, (rotEnergy - initialRotEnergy) / initialRotEnergy) - plt.xlabel('time (s)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Rotational Energy', fontsize='22') + plt.xlabel("time (s)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Rotational Energy", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) plt.figure() plt.clf() - plt.plot(timeSec, theta1, label=r'$\theta_1$') - plt.plot(timeSec, theta2, label=r'$\theta_2$') - plt.plot(timeSec, theta3, label=r'$\theta_3$') - plt.plot(timeSec, theta4, label=r'$\theta_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Angle') + plt.plot(timeSec, theta1, label=r"$\theta_1$") + plt.plot(timeSec, theta2, label=r"$\theta_2$") + plt.plot(timeSec, theta3, label=r"$\theta_3$") + plt.plot(timeSec, theta4, label=r"$\theta_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Angle") plt.figure() plt.clf() - plt.plot(timeSec, theta1Dot, label=r'$\dot{\theta}_1$') - plt.plot(timeSec, theta2Dot, label=r'$\dot{\theta}_2$') - plt.plot(timeSec, theta3Dot, label=r'$\dot{\theta}_3$') - plt.plot(timeSec, theta4Dot, label=r'$\dot{\theta}_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Angle Rate') + plt.plot(timeSec, theta1Dot, label=r"$\dot{\theta}_1$") + plt.plot(timeSec, theta2Dot, label=r"$\dot{\theta}_2$") + plt.plot(timeSec, theta3Dot, label=r"$\dot{\theta}_3$") + plt.plot(timeSec, theta4Dot, label=r"$\dot{\theta}_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Angle Rate") if show_plots: plt.show() @@ -614,16 +756,28 @@ def spinningBodyCommandedTorque(show_plots): # Define properties of spinning bodies spinningBody1 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody1.setMass(np.random.uniform(5.0, 50.0)) - spinningBody1.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + spinningBody1.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) spinningBody1.setDCM_S0P([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 1.0]]) - spinningBody1.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody1.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + spinningBody1.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody1.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) spinningBody1.setSHat_S([[0], [0], [1]]) spinningBody1.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody1.setThetaDotInit(np.random.uniform(-1.0, 1.0) * macros.D2R) @@ -632,16 +786,28 @@ def spinningBodyCommandedTorque(show_plots): spinningBody2 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody2.setMass(np.random.uniform(5.0, 50.0)) - spinningBody2.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) - spinningBody2.setDCM_S0P([[0.0, -1.0, 0.0], [0.0, .0, -1.0], [1.0, 0.0, 0.0]]) - spinningBody2.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody2.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) + spinningBody2.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) + spinningBody2.setDCM_S0P([[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]]) + spinningBody2.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody2.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) spinningBody2.setSHat_S([[0], [-1], [0]]) spinningBody2.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody2.setThetaDotInit(np.random.uniform(-1.0, 1.0) * macros.D2R) @@ -650,17 +816,29 @@ def spinningBodyCommandedTorque(show_plots): spinningBody3 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody3.setMass(np.random.uniform(5.0, 50.0)) - spinningBody3.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) + spinningBody3.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) spinningBody3.setDCM_S0P([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]]) - spinningBody3.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody3.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody3.setSHat_S([[np.sqrt(1/2)], [np.sqrt(1/2)], [0]]) + spinningBody3.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody3.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody3.setSHat_S([[np.sqrt(1 / 2)], [np.sqrt(1 / 2)], [0]]) spinningBody3.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody3.setThetaDotInit(np.random.uniform(-1.0, 1.0) * macros.D2R) spinningBody3.setK(np.random.random()) @@ -668,17 +846,29 @@ def spinningBodyCommandedTorque(show_plots): spinningBody4 = spinningBodyNDOFStateEffector.SpinningBody() spinningBody4.setMass(np.random.uniform(5.0, 50.0)) - spinningBody4.setISPntSc_S([[np.random.uniform(5.0, 100.0), 0.0, 0.0], - [0.0, np.random.uniform(5.0, 100.0), 0.0], - [0.0, 0.0, np.random.uniform(5.0, 100.0)]]) - spinningBody4.setDCM_S0P([[0.0, 1.0, 0.0], [0.0, .0, 1.0], [1.0, 0.0, 0.0]]) - spinningBody4.setR_ScS_S([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody4.setR_SP_P([[np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)], - [np.random.uniform(-1.0, 1.0)]]) - spinningBody4.setSHat_S([[np.sqrt(1/2)], [-np.sqrt(1/2)], [0]]) + spinningBody4.setISPntSc_S( + [ + [np.random.uniform(5.0, 100.0), 0.0, 0.0], + [0.0, np.random.uniform(5.0, 100.0), 0.0], + [0.0, 0.0, np.random.uniform(5.0, 100.0)], + ] + ) + spinningBody4.setDCM_S0P([[0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]]) + spinningBody4.setR_ScS_S( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody4.setR_SP_P( + [ + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + [np.random.uniform(-1.0, 1.0)], + ] + ) + spinningBody4.setSHat_S([[np.sqrt(1 / 2)], [-np.sqrt(1 / 2)], [0]]) spinningBody4.setThetaInit(np.random.uniform(-10.0, 10.0) * macros.D2R) spinningBody4.setThetaDotInit(np.random.uniform(-1.0, 1.0) * macros.D2R) spinningBody4.setK(np.random.random()) @@ -693,8 +883,16 @@ def spinningBodyCommandedTorque(show_plots): 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.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]] @@ -711,7 +909,7 @@ def spinningBodyCommandedTorque(show_plots): # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -720,7 +918,9 @@ def spinningBodyCommandedTorque(show_plots): unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totRotEnergy", "totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N"]) + scObjectLog = scObject.logger( + ["totRotEnergy", "totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Add states to log @@ -769,12 +969,17 @@ def spinningBodyCommandedTorque(show_plots): plt.close("all") plt.figure() ax = plt.axes() - 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)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Orbital Angular Momentum', fontsize='22') + 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)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Orbital Angular Momentum", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -782,21 +987,26 @@ def spinningBodyCommandedTorque(show_plots): plt.figure() ax = plt.axes() plt.plot(timeSec, (orbEnergy - initialOrbEnergy) / initialOrbEnergy) - plt.xlabel('time (s)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Orbital Energy', fontsize='22') + plt.xlabel("time (s)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Orbital Energy", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) plt.figure() ax = plt.axes() - 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)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Rotational Angular Momentum', fontsize='22') + 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)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Rotational Angular Momentum", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) @@ -804,32 +1014,32 @@ def spinningBodyCommandedTorque(show_plots): plt.figure() ax = plt.axes() plt.plot(timeSec, (rotEnergy - initialRotEnergy) / initialRotEnergy) - plt.xlabel('time (s)', fontsize='18') - plt.ylabel('Relative Difference', fontsize='18') - plt.title('Rotational Energy', fontsize='22') + plt.xlabel("time (s)", fontsize="18") + plt.ylabel("Relative Difference", fontsize="18") + plt.title("Rotational Energy", fontsize="22") plt.xticks(fontsize=14) plt.yticks(fontsize=14) ax.yaxis.offsetText.set_fontsize(14) plt.figure() plt.clf() - plt.plot(timeSec, theta1, label=r'$\theta_1$') - plt.plot(timeSec, theta2, label=r'$\theta_2$') - plt.plot(timeSec, theta3, label=r'$\theta_3$') - plt.plot(timeSec, theta4, label=r'$\theta_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Angle') + plt.plot(timeSec, theta1, label=r"$\theta_1$") + plt.plot(timeSec, theta2, label=r"$\theta_2$") + plt.plot(timeSec, theta3, label=r"$\theta_3$") + plt.plot(timeSec, theta4, label=r"$\theta_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Angle") plt.figure() plt.clf() - plt.plot(timeSec, theta1Dot, label=r'$\dot{\theta}_1$') - plt.plot(timeSec, theta2Dot, label=r'$\dot{\theta}_2$') - plt.plot(timeSec, theta3Dot, label=r'$\dot{\theta}_3$') - plt.plot(timeSec, theta4Dot, label=r'$\dot{\theta}_4$') - plt.legend(loc='best') - plt.xlabel('time (s)') - plt.ylabel('Angle Rate') + plt.plot(timeSec, theta1Dot, label=r"$\dot{\theta}_1$") + plt.plot(timeSec, theta2Dot, label=r"$\dot{\theta}_2$") + plt.plot(timeSec, theta3Dot, label=r"$\dot{\theta}_3$") + plt.plot(timeSec, theta4Dot, label=r"$\dot{\theta}_4$") + plt.legend(loc="best") + plt.xlabel("time (s)") + plt.ylabel("Angle Rate") if show_plots: plt.show() diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py index bafe34b9da..b272cd14e8 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py @@ -31,10 +31,14 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') +splitPath = path.split("simulation") from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros -from Basilisk.simulation import spacecraft, spinningBodyOneDOFStateEffector, gravityEffector +from Basilisk.simulation import ( + spacecraft, + spinningBodyOneDOFStateEffector, + gravityEffector, +) from Basilisk.architecture import messaging @@ -43,12 +47,15 @@ # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("cmdTorque, lock, thetaRef", [ - (0.0, False, 0.0) - , (0.0, True, 0.0) - , (1.0, False, 0.0) - , (0.0, False, 20.0 * macros.D2R) -]) +@pytest.mark.parametrize( + "cmdTorque, lock, thetaRef", + [ + (0.0, False, 0.0), + (0.0, True, 0.0), + (1.0, False, 0.0), + (0.0, False, 20.0 * macros.D2R), + ], +) def test_spinningBody(show_plots, cmdTorque, lock, thetaRef): r""" **Validation Test Description** @@ -101,8 +108,16 @@ def spinningBody(show_plots, cmdTorque, lock, thetaRef): 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.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]] @@ -112,7 +127,7 @@ def spinningBody(show_plots, cmdTorque, lock, thetaRef): # Define properties of spinning body spinningBody.mass = 50.0 spinningBody.IPntSc_S = [[50.0, 0.0, 0.0], [0.0, 30.0, 0.0], [0.0, 0.0, 40.0]] - spinningBody.dcm_S0B = [[0.0, -1.0, 0.0], [0.0, .0, -1.0], [1.0, 0.0, 0.0]] + spinningBody.dcm_S0B = [[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]] spinningBody.r_ScS_S = [[1.0], [0.0], [-1.0]] spinningBody.r_SB_B = [[0.5], [-1.5], [-0.5]] spinningBody.sHat_S = [[0], [-1], [0]] @@ -155,7 +170,7 @@ def spinningBody(show_plots, cmdTorque, lock, thetaRef): # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -164,7 +179,9 @@ def spinningBody(show_plots, cmdTorque, lock, thetaRef): unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Initialize the simulation @@ -180,10 +197,18 @@ def spinningBody(show_plots, cmdTorque, lock, thetaRef): unitTestSim.ExecuteSimulation() # Extract the logged variables - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) theta = thetaData.theta thetaDot = thetaData.thetaDot @@ -201,47 +226,61 @@ def spinningBody(show_plots, cmdTorque, lock, thetaRef): plt.close("all") plt.figure() plt.clf() - plt.plot(orbAngMom_N[:, 0] * 1e-9, (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], - orbAngMom_N[:, 0] * 1e-9, (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], - orbAngMom_N[:, 0] * 1e-9, (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3]) - plt.xlabel('time (s)') - plt.ylabel('Relative Difference') - plt.title('Orbital Angular Momentum') + plt.plot( + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3], + ) + plt.xlabel("time (s)") + plt.ylabel("Relative Difference") + plt.title("Orbital Angular Momentum") plt.figure() plt.clf() - plt.plot(orbEnergy[:, 0] * 1e-9, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1]) - plt.xlabel('time (s)') - plt.ylabel('Relative Difference') - plt.title('Orbital Energy') + plt.plot( + orbEnergy[:, 0] * 1e-9, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1] + ) + plt.xlabel("time (s)") + plt.ylabel("Relative Difference") + plt.title("Orbital Energy") plt.figure() plt.clf() - plt.plot(rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) / rotAngMom_N[0, 1], - rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) / rotAngMom_N[0, 2], - rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) / rotAngMom_N[0, 3]) - plt.xlabel('time (s)') - plt.ylabel('Relative Difference') - plt.title('Rotational Angular Momentum') + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) / rotAngMom_N[0, 1], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) / rotAngMom_N[0, 2], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) / rotAngMom_N[0, 3], + ) + plt.xlabel("time (s)") + plt.ylabel("Relative Difference") + plt.title("Rotational Angular Momentum") plt.figure() plt.clf() - plt.plot(rotEnergy[:, 0] * 1e-9, (rotEnergy[:, 1] - rotEnergy[0, 1]) / rotEnergy[0, 1]) - plt.xlabel('time (s)') - plt.ylabel('Relative Difference') - plt.title('Rotational Energy') + plt.plot( + rotEnergy[:, 0] * 1e-9, (rotEnergy[:, 1] - rotEnergy[0, 1]) / rotEnergy[0, 1] + ) + plt.xlabel("time (s)") + plt.ylabel("Relative Difference") + plt.title("Rotational Energy") plt.figure() plt.clf() plt.plot(thetaData.times() * 1e-9, theta) - plt.xlabel('time (s)') - plt.ylabel('theta') + plt.xlabel("time (s)") + plt.ylabel("theta") plt.figure() plt.clf() plt.plot(thetaData.times() * 1e-9, thetaDot) - plt.xlabel('time (s)') - plt.ylabel('thetaDot') + plt.xlabel("time (s)") + plt.ylabel("thetaDot") if show_plots: plt.show() @@ -256,36 +295,52 @@ def spinningBody(show_plots, cmdTorque, lock, thetaRef): for i in range(0, len(initialOrbAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: Spinning Body integrated test failed orbital angular momentum unit test") + "FAILED: Spinning Body integrated test failed orbital angular momentum unit test" + ) for i in range(0, len(initialRotAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: Spinning Body integrated test failed rotational angular momentum unit test") + "FAILED: Spinning Body integrated test failed rotational angular momentum unit test" + ) # Only check rotational energy if no torques and no damping are applied if cmdTorque == 0.0 and thetaRef == 0.0: for i in range(0, len(initialRotEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotEnergy[i], initialRotEnergy[i], 1, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotEnergy[i], initialRotEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spinning Body integrated test failed rotational energy unit test") + testMessages.append( + "FAILED: Spinning Body integrated test failed rotational energy unit test" + ) for i in range(0, len(initialOrbEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spinning Body integrated test failed orbital energy unit test") + testMessages.append( + "FAILED: Spinning Body integrated test failed orbital energy unit test" + ) if thetaRef != 0.0: if not unitTestSupport.isDoubleEqual(theta[-1], thetaRef, 0.01): testFailCount += 1 - testMessages.append("FAILED: Spinning Body integrated test failed angle convergence unit test") + testMessages.append( + "FAILED: Spinning Body integrated test failed angle convergence unit test" + ) if testFailCount == 0: print("PASSED: " + " Spinning Body gravity integrated test") @@ -293,7 +348,7 @@ def spinningBody(show_plots, cmdTorque, lock, thetaRef): assert testFailCount < 1, testMessages # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/_UnitTest/test_spinningBodyTwoDOFStateEffector.py b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/_UnitTest/test_spinningBodyTwoDOFStateEffector.py index cd80abc153..497075181b 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/_UnitTest/test_spinningBodyTwoDOFStateEffector.py +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/_UnitTest/test_spinningBodyTwoDOFStateEffector.py @@ -30,10 +30,14 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -splitPath = path.split('simulation') +splitPath = path.split("simulation") from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros -from Basilisk.simulation import spacecraft, spinningBodyTwoDOFStateEffector, gravityEffector +from Basilisk.simulation import ( + spacecraft, + spinningBodyTwoDOFStateEffector, + gravityEffector, +) from Basilisk.architecture import messaging @@ -43,16 +47,22 @@ # @pytest.mark.xfail() # need to update how the RW states are defined # provide a unique test method name, starting with test_ -@pytest.mark.parametrize("cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, theta2Ref", [ - (0.0, False, 0.0, 0.0, False, 0.0) - , (0.0, True, 0.0, 0.0, False, 0.0) - , (0.0, False, 0.0, 0.0, True, 0.0) - , (0.0, True, 0.0, 0.0, True, 0.0) - , (1.0, False, 0.0, -2.0, False, 0.0) - , (0.0, False, 10.0 * macros.D2R, 0.0, False, -5.0 * macros.D2R) - , (0.0, False, -5.0 * macros.D2R, 0.0, False, 10.0 * macros.D2R) -]) -def test_spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, theta2Ref): + +@pytest.mark.parametrize( + "cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, theta2Ref", + [ + (0.0, False, 0.0, 0.0, False, 0.0), + (0.0, True, 0.0, 0.0, False, 0.0), + (0.0, False, 0.0, 0.0, True, 0.0), + (0.0, True, 0.0, 0.0, True, 0.0), + (1.0, False, 0.0, -2.0, False, 0.0), + (0.0, False, 10.0 * macros.D2R, 0.0, False, -5.0 * macros.D2R), + (0.0, False, -5.0 * macros.D2R, 0.0, False, 10.0 * macros.D2R), + ], +) +def test_spinningBody( + show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, theta2Ref +): r""" **Validation Test Description** @@ -73,11 +83,15 @@ def test_spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock against their initial values. """ - [testResults, testMessage] = spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, theta2Ref) + [testResults, testMessage] = spinningBody( + show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, theta2Ref + ) assert testResults < 1, testMessage -def spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, theta2Ref): +def spinningBody( + show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, theta2Ref +): __tracebackhide__ = True testFailCount = 0 # zero unit test result counter @@ -106,7 +120,7 @@ def spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, th spinningBody.IS1PntSc1_S1 = [[100.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]] spinningBody.IS2PntSc2_S2 = [[50.0, 0.0, 0.0], [0.0, 30.0, 0.0], [0.0, 0.0, 40.0]] spinningBody.dcm_S10B = [[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 1.0]] - spinningBody.dcm_S20S1 = [[0.0, -1.0, 0.0], [0.0, .0, -1.0], [1.0, 0.0, 0.0]] + spinningBody.dcm_S20S1 = [[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]] spinningBody.r_Sc1S1_S1 = [[2.0], [-0.5], [0.0]] spinningBody.r_Sc2S2_S2 = [[1.0], [0.0], [-1.0]] spinningBody.r_S1B_B = [[-2.0], [0.5], [-1.0]] @@ -169,8 +183,16 @@ def spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, th 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.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.01], [-0.01], [0.01]] @@ -181,7 +203,7 @@ def spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, th # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" - earthGravBody.mu = 0.3986004415E+15 # meters! + earthGravBody.mu = 0.3986004415e15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) @@ -190,7 +212,9 @@ def spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, th unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log - scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) + scObjectLog = scObject.logger( + ["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"] + ) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Initialize the simulation @@ -203,15 +227,23 @@ def spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, th unitTestSim.AddModelToTask(unitTaskName, theta2Data) # Setup and run the simulation - stopTime = 25000*testProcessRate + stopTime = 25000 * testProcessRate unitTestSim.ConfigureStopTime(stopTime) unitTestSim.ExecuteSimulation() # Extract the logged variables - orbAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N) - rotAngMom_N = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotAngMomPntC_N) - rotEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totRotEnergy) - orbEnergy = unitTestSupport.addTimeColumn(scObjectLog.times(), scObjectLog.totOrbEnergy) + orbAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbAngMomPntN_N + ) + rotAngMom_N = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotAngMomPntC_N + ) + rotEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totRotEnergy + ) + orbEnergy = unitTestSupport.addTimeColumn( + scObjectLog.times(), scObjectLog.totOrbEnergy + ) theta1 = theta1Data.theta theta1Dot = theta1Data.thetaDot theta2 = theta2Data.theta @@ -231,59 +263,73 @@ def spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, th plt.close("all") plt.figure() plt.clf() - plt.plot(orbAngMom_N[:, 0] * 1e-9, (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], - orbAngMom_N[:, 0] * 1e-9, (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], - orbAngMom_N[:, 0] * 1e-9, (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3]) - plt.xlabel('time (s)') - plt.ylabel('Relative Difference') - plt.title('Orbital Angular Momentum') + plt.plot( + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 1] - orbAngMom_N[0, 1]) / orbAngMom_N[0, 1], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 2] - orbAngMom_N[0, 2]) / orbAngMom_N[0, 2], + orbAngMom_N[:, 0] * 1e-9, + (orbAngMom_N[:, 3] - orbAngMom_N[0, 3]) / orbAngMom_N[0, 3], + ) + plt.xlabel("time (s)") + plt.ylabel("Relative Difference") + plt.title("Orbital Angular Momentum") plt.figure() plt.clf() - plt.plot(orbEnergy[:, 0] * 1e-9, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1]) - plt.xlabel('time (s)') - plt.ylabel('Relative Difference') - plt.title('Orbital Energy') + plt.plot( + orbEnergy[:, 0] * 1e-9, (orbEnergy[:, 1] - orbEnergy[0, 1]) / orbEnergy[0, 1] + ) + plt.xlabel("time (s)") + plt.ylabel("Relative Difference") + plt.title("Orbital Energy") plt.figure() plt.clf() - plt.plot(rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) / rotAngMom_N[0, 1], - rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) / rotAngMom_N[0, 2], - rotAngMom_N[:, 0] * 1e-9, (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) / rotAngMom_N[0, 3]) - plt.xlabel('time (s)') - plt.ylabel('Relative Difference') - plt.title('Rotational Angular Momentum') + plt.plot( + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 1] - rotAngMom_N[0, 1]) / rotAngMom_N[0, 1], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 2] - rotAngMom_N[0, 2]) / rotAngMom_N[0, 2], + rotAngMom_N[:, 0] * 1e-9, + (rotAngMom_N[:, 3] - rotAngMom_N[0, 3]) / rotAngMom_N[0, 3], + ) + plt.xlabel("time (s)") + plt.ylabel("Relative Difference") + plt.title("Rotational Angular Momentum") plt.figure() plt.clf() - plt.plot(rotEnergy[:, 0] * 1e-9, (rotEnergy[:, 1] - rotEnergy[0, 1]) / rotEnergy[0, 1]) - plt.xlabel('time (s)') - plt.ylabel('Relative Difference') - plt.title('Rotational Energy') + plt.plot( + rotEnergy[:, 0] * 1e-9, (rotEnergy[:, 1] - rotEnergy[0, 1]) / rotEnergy[0, 1] + ) + plt.xlabel("time (s)") + plt.ylabel("Relative Difference") + plt.title("Rotational Energy") plt.figure() plt.clf() plt.plot(theta1Data.times() * 1e-9, theta1) - plt.xlabel('time (s)') - plt.ylabel('theta1') + plt.xlabel("time (s)") + plt.ylabel("theta1") plt.figure() plt.clf() plt.plot(theta1Data.times() * 1e-9, theta1Dot) - plt.xlabel('time (s)') - plt.ylabel('theta1Dot') + plt.xlabel("time (s)") + plt.ylabel("theta1Dot") plt.figure() plt.clf() plt.plot(theta2Data.times() * 1e-9, theta2) - plt.xlabel('time (s)') - plt.ylabel('theta2') + plt.xlabel("time (s)") + plt.ylabel("theta2") plt.figure() plt.clf() plt.plot(theta2Data.times() * 1e-9, theta2Dot) - plt.xlabel('time (s)') - plt.ylabel('theta2Dot') + plt.xlabel("time (s)") + plt.ylabel("theta2Dot") if show_plots: plt.show() @@ -298,39 +344,57 @@ def spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, th for i in range(0, len(initialOrbAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbAngMom[i], initialOrbAngMom_N[i], 3, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: Spinning Body integrated test failed orbital angular momentum unit test") + "FAILED: Spinning Body integrated test failed orbital angular momentum unit test" + ) for i in range(0, len(initialRotAngMom_N)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotAngMom[i], initialRotAngMom_N[i], 3, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: Spinning Body integrated test failed rotational angular momentum unit test") + "FAILED: Spinning Body integrated test failed rotational angular momentum unit test" + ) if cmdTorque1 == 0 and cmdTorque2 == 0 and theta1Ref == 0.0 and theta2Ref == 0.0: for i in range(0, len(initialRotEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalRotEnergy[i], initialRotEnergy[i], 1, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalRotEnergy[i], initialRotEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spinning Body integrated test failed rotational energy unit test") + testMessages.append( + "FAILED: Spinning Body integrated test failed rotational energy unit test" + ) for i in range(0, len(initialOrbEnergy)): # check a vector values - if not unitTestSupport.isArrayEqualRelative(finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy): + if not unitTestSupport.isArrayEqualRelative( + finalOrbEnergy[i], initialOrbEnergy[i], 1, accuracy + ): testFailCount += 1 - testMessages.append("FAILED: Spinning Body integrated test failed orbital energy unit test") + testMessages.append( + "FAILED: Spinning Body integrated test failed orbital energy unit test" + ) if theta1Ref != 0.0 or theta2Ref != 0.0: if not unitTestSupport.isDoubleEqual(theta1[-1], theta1Ref, 0.01): testFailCount += 1 - testMessages.append("FAILED: Spinning Body integrated test failed angle 1 convergence unit test") + testMessages.append( + "FAILED: Spinning Body integrated test failed angle 1 convergence unit test" + ) if not unitTestSupport.isDoubleEqual(theta2[-1], theta2Ref, 0.01): testFailCount += 1 - testMessages.append("FAILED: Spinning Body integrated test failed angle 2 convergence unit test") + testMessages.append( + "FAILED: Spinning Body integrated test failed angle 2 convergence unit test" + ) if testFailCount == 0: print("PASSED: " + " Spinning Body gravity integrated test") @@ -338,7 +402,7 @@ def spinningBody(show_plots, cmdTorque1, lock1, theta1Ref, cmdTorque2, lock2, th assert testFailCount < 1, testMessages # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] if __name__ == "__main__": diff --git a/src/simulation/dynamics/stateArchitecture/_UnitTest/test_stateArchitecture.py b/src/simulation/dynamics/stateArchitecture/_UnitTest/test_stateArchitecture.py index fb5fa9832d..f2f9ffc52b 100644 --- a/src/simulation/dynamics/stateArchitecture/_UnitTest/test_stateArchitecture.py +++ b/src/simulation/dynamics/stateArchitecture/_UnitTest/test_stateArchitecture.py @@ -27,6 +27,7 @@ # @pytest.mark.xfail() # need to update how the RW states are defined # provide a unique test method name, starting with test_ + def stateData(show_plots): """Module Unit Test""" # The __tracebackhide__ setting influences pytest showing of tracebacks: @@ -44,19 +45,19 @@ def stateData(show_plots): predictedDerivative = [[0.0], [0.0]] - if(newState.getRowSize() != len(stateUse)): + if newState.getRowSize() != len(stateUse): testFailCount += 1 testMessages.append("State row sized incorrectly") - if(newState.getColumnSize() != len(stateUse[0])): + if newState.getColumnSize() != len(stateUse[0]): testFailCount += 1 testMessages.append("State column sized incorrectly") - if(newState.getName() != stateName): + if newState.getName() != stateName: testFailCount += 1 testMessages.append("State name incorrect") - if(newState.getState() != stateUse): + if newState.getState() != stateUse: testFailCount += 1 testMessages.append("State equality check failure.") - if(newState.getStateDeriv() != predictedDerivative): + if newState.getStateDeriv() != predictedDerivative: testFailCount += 1 testMessages.append("State derivative zero check failure.") @@ -64,15 +65,17 @@ def stateData(show_plots): newState.setDerivative(derivativeInc) newState.propagateState(0.1) - predictedDerivativeNum = numpy.array(predictedDerivative) + numpy.array(derivativeInc) + predictedDerivativeNum = numpy.array(predictedDerivative) + numpy.array( + derivativeInc + ) obsDerivativeNum = numpy.array(newState.getStateDeriv()) - if(obsDerivativeNum.tolist() != predictedDerivativeNum.tolist()): + if obsDerivativeNum.tolist() != predictedDerivativeNum.tolist(): testFailCount += 1 testMessages.append("State derivative update check failure.") stateUpdateNum = numpy.array(newState.getState()) - predUpStateNum = numpy.array(stateUse) + predictedDerivativeNum*0.1 - if(stateUpdateNum.tolist() != stateUpdateNum.tolist()): + predUpStateNum = numpy.array(stateUse) + predictedDerivativeNum * 0.1 + if stateUpdateNum.tolist() != stateUpdateNum.tolist(): testFailCount += 1 testMessages.append("State propagation update check failure.") @@ -81,23 +84,23 @@ def stateData(show_plots): priorState *= scaleFactor newState.scaleState(scaleFactor) stateUpdateNum = numpy.array(newState.getState()) - if(stateUpdateNum.tolist() != priorState.tolist()): + if stateUpdateNum.tolist() != priorState.tolist(): testFailCount += 1 testMessages.append("State scaling update check failure.") dummyState = stateArchitecture.StateData("dummy", newState.getState()) dummyState.addState(newState) - if(dummyState.getState() != (2.0*stateUpdateNum).tolist()): + if dummyState.getState() != (2.0 * stateUpdateNum).tolist(): testFailCount += 1 testMessages.append("Plus operator failed on StateData") - if testFailCount == 0: print("PASSED: " + " State data") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def stateProperties(show_plots): """Module Unit Test""" @@ -128,7 +131,7 @@ def stateProperties(show_plots): testFailCount += 1 testMessages.append("Set and property reference matching failed.") - newGravList = [[0.0], [9.81*2], [-0.1]] + newGravList = [[0.0], [9.81 * 2], [-0.1]] newManager.createProperty(gravName, newGravList) propRef = newManager.getPropertyReference(gravName) if propRef != newGravList: @@ -137,8 +140,8 @@ def stateProperties(show_plots): try: wrongGravList = [[0.0], [9.81], [-0.1]] - newManager.setPropertyValue(gravName+"Scott", wrongGravList) - propRef = newManager.getPropertyReference(gravName+"Scott") + newManager.setPropertyValue(gravName + "Scott", wrongGravList) + propRef = newManager.getPropertyReference(gravName + "Scott") testFailCount += 1 testMessages.append("Set and property reference matching failed.") except BasiliskError: @@ -152,12 +155,12 @@ def stateProperties(show_plots): testFailCount += 1 testMessages.append("1x1 Eigen property creation failed.") - if testFailCount == 0: print("PASSED: " + " State properties") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def stateArchitectureTest(show_plots): """Module Unit Test""" @@ -191,20 +194,22 @@ def stateArchitectureTest(show_plots): testFailCount += 1 testMessages.append("Failed to return proper state name for velocity") - - if(newManager.registerState(stateDim[0], stateDim[1], positionName).getName() != positionName): + if ( + newManager.registerState(stateDim[0], stateDim[1], positionName).getName() + != positionName + ): testFailCount += 1 testMessages.append("Failed to return proper state name in overload of call") try: - newManager.registerState(stateDim[0], stateDim[1]+2, positionName) + newManager.registerState(stateDim[0], stateDim[1] + 2, positionName) testFailCount += 1 except BasiliskError: pass positionStateLookup = newManager.getStateObject("Array1_flex") - if(positionStateLookup.getName() != flexName): + if positionStateLookup.getName() != flexName: testFailCount += 1 testMessages.append("State lookup for solar array flex failed") @@ -215,18 +220,18 @@ def stateArchitectureTest(show_plots): vectorComposite = newManager.getStateVector() vectorComposite.addStates(vectorComposite) vectorComposite.scaleStates(vectorFactor) - numpyOutput = (numpy.array(vecStart) + numpy.array(vecStart))*vectorFactor + numpyOutput = (numpy.array(vecStart) + numpy.array(vecStart)) * vectorFactor newManager.updateStateVector(vectorComposite) - if(velState.getState() != numpyOutput.tolist()): + if velState.getState() != numpyOutput.tolist(): testFailCount += 1 testMessages.append("Velocity state update via state-manager failed") dt = 1.0 posState.setDerivative(vecStart) newManager.propagateStateVector(dt) - numpyOutput += numpy.array(vecStart)*dt - if(posState.getState() != numpyOutput.tolist()): + numpyOutput += numpy.array(vecStart) * dt + if posState.getState() != numpyOutput.tolist(): testFailCount += 1 testMessages.append("Position state propagation via state-manager failed") @@ -234,7 +239,8 @@ def stateArchitectureTest(show_plots): print("PASSED: " + " State manager") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def EigenConversions(show_plots): """Module Unit Test""" @@ -250,10 +256,10 @@ def EigenConversions(show_plots): outputArray = sim_model.new_doubleArray(3) stateArchitecture.eigenVector3d2CArray(inputArray, outputArray) - flatList = [y for x in inputArray for y in x] + flatList = [y for x in inputArray for y in x] for i in range(len(flatList)): - if(flatList[i] != sim_model.doubleArray_getitem(outputArray, i)): + if flatList[i] != sim_model.doubleArray_getitem(outputArray, i): testFailCount += 1 testMessages.append("3-vector conversion failed") @@ -261,10 +267,10 @@ def EigenConversions(show_plots): outputArray = sim_model.new_doubleArray(9) stateArchitecture.eigenMatrix3d2CArray(inputArray, outputArray) - flatList = [y for x in inputArray for y in x] + flatList = [y for x in inputArray for y in x] for i in range(len(flatList)): - if(flatList[i] != sim_model.doubleArray_getitem(outputArray, i)): + if flatList[i] != sim_model.doubleArray_getitem(outputArray, i): print(sim_model.doubleArray_getitem(outputArray, i)) testFailCount += 1 testMessages.append("3x3 matrix conversion failed") @@ -273,10 +279,10 @@ def EigenConversions(show_plots): outputArray = sim_model.new_doubleArray(12) stateArchitecture.eigenMatrixXd2CArray(inputArray, outputArray) - flatList = [y for x in inputArray for y in x] + flatList = [y for x in inputArray for y in x] for i in range(len(flatList)): - if(flatList[i] != sim_model.doubleArray_getitem(outputArray, i)): + if flatList[i] != sim_model.doubleArray_getitem(outputArray, i): print(sim_model.doubleArray_getitem(outputArray, i)) testFailCount += 1 testMessages.append("3x4 matrix conversion failed") @@ -285,15 +291,18 @@ def EigenConversions(show_plots): print("PASSED: " + " Eigen Conversions") # return fail count and join into a single string all messages in the list # testMessage - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + FUNCTIONS = (stateData, stateArchitectureTest, stateProperties, EigenConversions) + @pytest.mark.parametrize("function", FUNCTIONS) def test_stateArchitectureAllTests(show_plots, function): """Module Unit Test""" [testResults, testMessage] = function(show_plots) assert testResults < 1, testMessage + if __name__ == "__main__": pytest.main([__file__, "--tb=native"]) diff --git a/src/simulation/environment/ExponentialAtmosphere/_UnitTest/test_integratedExponentialAtmosphere.py b/src/simulation/environment/ExponentialAtmosphere/_UnitTest/test_integratedExponentialAtmosphere.py index 2564dda871..42de7f9d91 100644 --- a/src/simulation/environment/ExponentialAtmosphere/_UnitTest/test_integratedExponentialAtmosphere.py +++ b/src/simulation/environment/ExponentialAtmosphere/_UnitTest/test_integratedExponentialAtmosphere.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,7 +15,6 @@ # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - import inspect import math import os @@ -28,7 +26,9 @@ # import general simulation support files from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion @@ -60,11 +60,11 @@ def test_unitExponentialAtmosphere(): snippetName = "passFail" testSum = sum(testResults) if testSum == 0: - colorText = 'ForestGreen' - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + colorText = "ForestGreen" + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + colorText = "Red" + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippetName, passedText, path) if testSum == 0: @@ -72,6 +72,7 @@ def test_unitExponentialAtmosphere(): assert testSum < 1, testMessage + def AddSpacecraftToModel(atmoModel): testFailCount = 0 testMessages = [] @@ -89,25 +90,28 @@ def AddSpacecraftToModel(atmoModel): if len(atmoModel.scStateInMsgs) != 2: testFailCount += 1 testMessages.append( - "FAILED: ExponentialAtmosphere does not have enough input message names.") + "FAILED: ExponentialAtmosphere does not have enough input message names." + ) if len(atmoModel.envOutMsgs) != 2: testFailCount += 1 testMessages.append( - "FAILED: ExponentialAtmosphere does not have enough output message names.") + "FAILED: ExponentialAtmosphere does not have enough output message names." + ) return testFailCount, testMessages + ## Test specific atmospheric model performance + def TestExponentialAtmosphere(): testFailCount = 0 testMessages = [] def expAtmoComp(alt, baseDens, scaleHeight): - density = baseDens * math.exp(-alt/scaleHeight) + density = baseDens * math.exp(-alt / scaleHeight) return density - # Create simulation variable names simTaskName = "simTask" simProcessName = "simProcess" @@ -119,7 +123,7 @@ def expAtmoComp(alt, baseDens, scaleHeight): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # Initialize new atmosphere and drag model, add them to task @@ -140,16 +144,18 @@ def expAtmoComp(alt, baseDens, scaleHeight): planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu # attach gravity model to spacecraft - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # # setup orbit and simulation time oe = orbitalMotion.ClassicElements() - r_eq = 6371*1000.0 + r_eq = 6371 * 1000.0 refBaseDens = 1.217 refScaleHeight = 8500.0 @@ -158,16 +164,17 @@ def expAtmoComp(alt, baseDens, scaleHeight): newAtmo.scaleHeight = refScaleHeight newAtmo.planetRadius = r_eq - - oe.a = r_eq + 300.*1000 + oe.a = r_eq + 300.0 * 1000 oe.e = 0.0 - oe.i = 0.0*macros.D2R + oe.i = 0.0 * macros.D2R - oe.Omega = 0.0*macros.D2R - oe.omega = 0.0*macros.D2R - oe.f = 0.0*macros.D2R + oe.Omega = 0.0 * macros.D2R + oe.omega = 0.0 * macros.D2R + oe.f = 0.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements # with circular or equatorial orbit, some angles are # arbitrary @@ -177,18 +184,19 @@ def expAtmoComp(alt, baseDens, scaleHeight): scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m - v_CN_N - # set the simulation time - n = np.sqrt(mu/oe.a/oe.a/oe.a) - P = 2.*np.pi/n + n = np.sqrt(mu / oe.a / oe.a / oe.a) + P = 2.0 * np.pi / n - simulationTime = macros.sec2nano(0.5*P) + simulationTime = macros.sec2nano(0.5 * P) # # Setup data logging before the simulation is initialized # numDataPoints = 10 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = scObject.scStateOutMsg.recorder(samplingTime) denLog = newAtmo.envOutMsgs[0].recorder(samplingTime) @@ -221,22 +229,29 @@ def expAtmoComp(alt, baseDens, scaleHeight): unitTestSupport.writeTeXSnippet("toleranceValue", str(accuracy), path) if len(densData) > 0: - for ind in range(0,len(densData)): + for ind in range(0, len(densData)): dist = np.linalg.norm(posData[ind]) alt = dist - newAtmo.planetRadius trueDensity = expAtmoComp(alt, refBaseDens, refScaleHeight) # check a vector values - if not unitTestSupport.isDoubleEqualRelative(densData[ind], trueDensity, accuracy): + if not unitTestSupport.isDoubleEqualRelative( + densData[ind], trueDensity, accuracy + ): testFailCount += 1 testMessages.append( - "FAILED: ExpAtmo failed density unit test at t=" + str(densData[ind, 0] * macros.NANO2SEC) + "sec with a value difference of "+str(densData[ind,1]-trueDensity)) + "FAILED: ExpAtmo failed density unit test at t=" + + str(densData[ind, 0] * macros.NANO2SEC) + + "sec with a value difference of " + + str(densData[ind, 1] - trueDensity) + ) else: testFailCount += 1 testMessages.append("FAILED: ExpAtmo failed to pull any logged data") return testFailCount, testMessages -if __name__=='__main__': + +if __name__ == "__main__": # TestExponentialAtmosphere() test_unitExponentialAtmosphere() diff --git a/src/simulation/environment/ExponentialAtmosphere/_UnitTest/test_unitTestExponentialAtmosphere.py b/src/simulation/environment/ExponentialAtmosphere/_UnitTest/test_unitTestExponentialAtmosphere.py index 544e3717b1..cdb7952c93 100755 --- a/src/simulation/environment/ExponentialAtmosphere/_UnitTest/test_unitTestExponentialAtmosphere.py +++ b/src/simulation/environment/ExponentialAtmosphere/_UnitTest/test_unitTestExponentialAtmosphere.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -32,12 +31,14 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.simulation import exponentialAtmosphere from Basilisk.architecture import messaging from Basilisk.utilities import macros @@ -52,31 +53,32 @@ # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("useDefault", [ True, False]) -@pytest.mark.parametrize("useMinReach", [ True, False]) -@pytest.mark.parametrize("useMaxReach", [ True, False]) -@pytest.mark.parametrize("usePlanetEphemeris", [ True, False]) - +@pytest.mark.parametrize("useDefault", [True, False]) +@pytest.mark.parametrize("useMinReach", [True, False]) +@pytest.mark.parametrize("useMaxReach", [True, False]) +@pytest.mark.parametrize("usePlanetEphemeris", [True, False]) # update "module" in this function name to reflect the module name def test_module(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris) + [testResults, testMessage] = run( + show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris + ) assert testResults < 1, testMessage def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -96,14 +98,14 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): minReach = -1.0 if useMinReach: - minReach = 200*1000.0 # meters + minReach = 200 * 1000.0 # meters testModule.envMinReach = minReach - testModule.planetRadius = 6378136.6 #meters + testModule.planetRadius = 6378136.6 # meters maxReach = -1.0 if useMaxReach: - maxReach = 200*1000.0 # meters + maxReach = 200 * 1000.0 # meters testModule.envMaxReach = maxReach - testModule.planetRadius = 6378136.6 + testModule.planetRadius = 6378136.6 planetPosition = [0.0, 0.0, 0.0] if usePlanetEphemeris: planetStateMsg = messaging.SpicePlanetStateMsgPayload() @@ -120,7 +122,7 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): # # setup orbit and simulation time oe = orbitalMotion.ClassicElements() - mu = 0.3986004415E+15 # meters^3/s^2 + mu = 0.3986004415e15 # meters^3/s^2 oe.a = r0 oe.e = 0.0 oe.i = 45.0 * macros.D2R @@ -132,11 +134,15 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): r1N, v1N = orbitalMotion.elem2rv(mu, oe) # create the input messages - sc0StateMsg = messaging.SCStatesMsgPayload() # Create a structure for the input message + sc0StateMsg = ( + messaging.SCStatesMsgPayload() + ) # Create a structure for the input message sc0StateMsg.r_BN_N = np.array(r0N) + np.array(planetPosition) sc0InMsg = messaging.SCStatesMsg().write(sc0StateMsg) - sc1StateMsg = messaging.SCStatesMsgPayload() # Create a structure for the input message + sc1StateMsg = ( + messaging.SCStatesMsgPayload() + ) # Create a structure for the input message sc1StateMsg.r_BN_N = np.array(r1N) + np.array(planetPosition) sc1InMsg = messaging.SCStatesMsg().write(sc1StateMsg) @@ -157,7 +163,7 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -167,7 +173,7 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): dens1Data = dataLog1.neutralDensity def expAtmoComp(alt, baseDens, scaleHeight, minReach, maxReach): - density = baseDens * math.exp(-alt/scaleHeight) + density = baseDens * math.exp(-alt / scaleHeight) if alt < minReach: density = 0.0 if alt > maxReach and maxReach > 0: @@ -185,40 +191,66 @@ def expAtmoComp(alt, baseDens, scaleHeight, minReach, maxReach): trueDensity = expAtmoComp(alt, refBaseDens, refScaleHeight, minReach, maxReach) if trueDensity != 0: testFailCount, testMessages = unitTestSupport.compareDoubleArrayRelative( - [trueDensity]*3, dens0Data, accuracy, "density sc0", - testFailCount, testMessages) + [trueDensity] * 3, + dens0Data, + accuracy, + "density sc0", + testFailCount, + testMessages, + ) else: testFailCount, testMessages = unitTestSupport.compareDoubleArray( - [trueDensity] * 3, dens0Data, accuracy, "density sc0", - testFailCount, testMessages) + [trueDensity] * 3, + dens0Data, + accuracy, + "density sc0", + testFailCount, + testMessages, + ) # check spacecraft 1 neutral density results alt = r1 - refPlanetRadius trueDensity = expAtmoComp(alt, refBaseDens, refScaleHeight, minReach, maxReach) if trueDensity != 0: testFailCount, testMessages = unitTestSupport.compareDoubleArrayRelative( - [trueDensity]*3, dens1Data, accuracy, "density sc1", - testFailCount, testMessages) + [trueDensity] * 3, + dens1Data, + accuracy, + "density sc1", + testFailCount, + testMessages, + ) else: testFailCount, testMessages = unitTestSupport.compareDoubleArray( - [trueDensity] * 3, dens1Data, accuracy, "density sc1", - testFailCount, testMessages) + [trueDensity] * 3, + dens1Data, + accuracy, + "density sc1", + testFailCount, + testMessages, + ) # print out success or failure message - snippentName = "unitTestPassFail" + str(useDefault) + str(useMinReach) + str(useMaxReach) + str(usePlanetEphemeris) + snippentName = ( + "unitTestPassFail" + + str(useDefault) + + str(useMinReach) + + str(useMaxReach) + + str(usePlanetEphemeris) + ) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + testModule.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + testModule.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -226,10 +258,10 @@ def expAtmoComp(alt, baseDens, scaleHeight, minReach, maxReach): # stand-along python script # if __name__ == "__main__": - test_module( # update "module" in function name - False, # showplots - False, # useDefault - False, # useMinReach - True, # useMaxReach - False # usePlanetEphemeris - ) + test_module( # update "module" in function name + False, # showplots + False, # useDefault + False, # useMinReach + True, # useMaxReach + False, # usePlanetEphemeris + ) diff --git a/src/simulation/environment/MsisAtmosphere/_UnitTest/test_msiseAtmosphere.py b/src/simulation/environment/MsisAtmosphere/_UnitTest/test_msiseAtmosphere.py index b0cac9f11c..8e6043f73b 100644 --- a/src/simulation/environment/MsisAtmosphere/_UnitTest/test_msiseAtmosphere.py +++ b/src/simulation/environment/MsisAtmosphere/_UnitTest/test_msiseAtmosphere.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016-2017, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -16,7 +15,6 @@ # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. - # # Basilisk Scenario Script and Integrated Test # @@ -32,18 +30,22 @@ import pytest from Basilisk.architecture import messaging from Basilisk.simulation import msisAtmosphere + # import simulation related support from Basilisk.simulation import spacecraft + # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion from Basilisk.utilities import simIncludeGravBody -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) @@ -52,12 +54,12 @@ # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(True, reason="Previously set sim parameters are not consistent with new formulation\n") + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. @pytest.mark.parametrize("orbitType", ["LPO", "LTO"]) @pytest.mark.parametrize("setEpoch", ["Default", "Direct", "Msg"]) - # provide a unique test method name, starting with test_ def test_scenarioMsisAtmosphereOrbit(show_plots, orbitType, setEpoch): """This function is called by the py.test environment.""" @@ -71,8 +73,8 @@ def test_scenarioMsisAtmosphereOrbit(show_plots, orbitType, setEpoch): def run(show_plots, orbitCase, setEpoch): """Call this routine directly to run the script.""" - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages # # From here on there scenario python code is found. Above this line the code is to setup a @@ -90,7 +92,7 @@ def run(show_plots, orbitCase, setEpoch): dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the integration update time - simulationTimeStep = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(10.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # Initialize new atmosphere and drag model, add them to task @@ -99,7 +101,9 @@ def run(show_plots, orbitCase, setEpoch): newAtmo.ModelTag = "MsisAtmo" if setEpoch == "Msg": - epochMsg = unitTestSupport.timeStringToGregorianUTCMsg('2019 Jan 01 00:00:00.00 (UTC)') + epochMsg = unitTestSupport.timeStringToGregorianUTCMsg( + "2019 Jan 01 00:00:00.00 (UTC)" + ) newAtmo.epochInMsg.subscribeTo(epochMsg) # setting epoch day of year info deliberately to a false value. The epoch msg info should be used @@ -122,51 +126,73 @@ def run(show_plots, orbitCase, setEpoch): newAtmo.addSpacecraftToModel(scObject.scStateOutMsg) planet = gravFactory.createEarth() - planet.isCentralBody = True # ensure this is the central gravitational body + planet.isCentralBody = True # ensure this is the central gravitational body mu = planet.mu # attach gravity model to spacecraft - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # setup orbit and simulation time oe = orbitalMotion.ClassicElements() r_eq = planet.radEquator if orbitCase == "LPO": - orbAltMin = 100.0*1000.0 + orbAltMin = 100.0 * 1000.0 orbAltMax = orbAltMin elif orbitCase == "LTO": - orbAltMin = 100.*1000.0 + orbAltMin = 100.0 * 1000.0 orbAltMax = 100.0 * 1000.0 rMin = r_eq + orbAltMin rMax = r_eq + orbAltMax - oe.a = (rMin+rMax)/2.0 - oe.e = 1.0 - rMin/oe.a - oe.i = 0.0*macros.D2R + oe.a = (rMin + rMax) / 2.0 + oe.e = 1.0 - rMin / oe.a + oe.i = 0.0 * macros.D2R - oe.Omega = 0.0*macros.D2R - oe.omega = 0.0*macros.D2R - oe.f = 0.0*macros.D2R + oe.Omega = 0.0 * macros.D2R + oe.omega = 0.0 * macros.D2R + oe.f = 0.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) - oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements - # with circular or equatorial orbit, some angles are - # arbitrary + oe = orbitalMotion.rv2elem( + mu, rN, vN + ) # this stores consistent initial orbit elements + # with circular or equatorial orbit, some angles are + # arbitrary # set the simulation time - n = np.sqrt(mu/oe.a/oe.a/oe.a) - P = 2.*np.pi/n + n = np.sqrt(mu / oe.a / oe.a / oe.a) + P = 2.0 * np.pi / n - simulationTime = macros.sec2nano(0.002*P) + simulationTime = macros.sec2nano(0.002 * P) # # Setup data logging before the simulation is initialized # sw_msg_names = [ - "ap_24_0", "ap_3_0", "ap_3_-3", "ap_3_-6", "ap_3_-9", - "ap_3_-12", "ap_3_-15", "ap_3_-18", "ap_3_-21", "ap_3_-24", - "ap_3_-27", "ap_3_-30", "ap_3_-33", "ap_3_-36", "ap_3_-39", - "ap_3_-42", "ap_3_-45", "ap_3_-48", "ap_3_-51", "ap_3_-54", - "ap_3_-57", "f107_1944_0", "f107_24_-24" + "ap_24_0", + "ap_3_0", + "ap_3_-3", + "ap_3_-6", + "ap_3_-9", + "ap_3_-12", + "ap_3_-15", + "ap_3_-18", + "ap_3_-21", + "ap_3_-24", + "ap_3_-27", + "ap_3_-30", + "ap_3_-33", + "ap_3_-36", + "ap_3_-39", + "ap_3_-42", + "ap_3_-45", + "ap_3_-48", + "ap_3_-51", + "ap_3_-54", + "ap_3_-57", + "f107_1944_0", + "f107_24_-24", ] swMsgList = [] @@ -210,7 +236,7 @@ def run(show_plots, orbitCase, setEpoch): # Compare to expected values - refAtmoData = np.loadtxt(path + '/truthOutputs.txt') + refAtmoData = np.loadtxt(path + "/truthOutputs.txt") accuracy = 1e-8 @@ -219,33 +245,41 @@ def run(show_plots, orbitCase, setEpoch): # Test atmospheric density calculation; note that refAtmoData is in g/cm^3, # and must be adjusted by a factor of 1e-3 to match kg/m^3 print(densData[-1]) - print(refAtmoData[5]*1000) - if np.testing.assert_allclose(densData[-1], refAtmoData[5]*1000., atol=accuracy): + print(refAtmoData[5] * 1000) + if np.testing.assert_allclose(densData[-1], refAtmoData[5] * 1000.0, atol=accuracy): testFailCount += 1 - testMessages.append("FAILED: NRLMSISE-00 failed density unit test with a value difference of "+str(densData[0]-refAtmoData[5]*1000)) + testMessages.append( + "FAILED: NRLMSISE-00 failed density unit test with a value difference of " + + str(densData[0] - refAtmoData[5] * 1000) + ) print(tempData[-1]) print(refAtmoData[-1]) if np.testing.assert_allclose(tempData[-1], refAtmoData[-1], atol=accuracy): testFailCount += 1 testMessages.append( - "FAILED: NRLMSISE-00 failed temperature unit test with a value difference of "+str(tempData[-1]-refAtmoData[-1])) + "FAILED: NRLMSISE-00 failed temperature unit test with a value difference of " + + str(tempData[-1] - refAtmoData[-1]) + ) snippentName = "unitTestPassFail" + str(orbitCase) + str(setEpoch) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + newAtmo.ModelTag) - passedText = '\\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = "\\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + newAtmo.ModelTag) - passedText = '\\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = "\\textcolor{" + colorText + "}{" + "Failed" + "}" print(testMessages) unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + -if __name__ == '__main__': - run(True, - "LPO", # orbitCase - "Msg") # setEpoch +if __name__ == "__main__": + run( + True, + "LPO", # orbitCase + "Msg", + ) # setEpoch diff --git a/src/simulation/environment/TabularAtmosphere/_UnitTest/test_unitTestTabularAtmosphere.py b/src/simulation/environment/TabularAtmosphere/_UnitTest/test_unitTestTabularAtmosphere.py index f400e9e18f..a90516cab0 100644 --- a/src/simulation/environment/TabularAtmosphere/_UnitTest/test_unitTestTabularAtmosphere.py +++ b/src/simulation/environment/TabularAtmosphere/_UnitTest/test_unitTestTabularAtmosphere.py @@ -42,11 +42,13 @@ from Basilisk.utilities import orbitalMotion from Basilisk.utilities.readAtmTable import readAtmTable -@pytest.mark.parametrize("accuracy", [1e-12]) -@pytest.mark.parametrize("altitude", [42.0, 33.33333, 10000.0, -10.0]) # exact, interpolate, above, below -@pytest.mark.parametrize("useMinReach", [ True, False]) -@pytest.mark.parametrize("useMaxReach", [ True, False]) +@pytest.mark.parametrize("accuracy", [1e-12]) +@pytest.mark.parametrize( + "altitude", [42.0, 33.33333, 10000.0, -10.0] +) # exact, interpolate, above, below +@pytest.mark.parametrize("useMinReach", [True, False]) +@pytest.mark.parametrize("useMaxReach", [True, False]) def test_tabularAtmosphere(altitude, accuracy, useMinReach, useMaxReach): r""" **Validation Test Description** @@ -82,31 +84,34 @@ def test_tabularAtmosphere(altitude, accuracy, useMinReach, useMaxReach): """ # each test method requires a single assert method to be called - [testResults, testMessage] = tabularAtmosphereTestFunction(altitude, accuracy, useMinReach, useMaxReach) + [testResults, testMessage] = tabularAtmosphereTestFunction( + altitude, accuracy, useMinReach, useMaxReach + ) assert testResults < 1, testMessage + def tabularAtmosphereTestFunction(altitude, accuracy, useMinReach, useMaxReach): - testFailCount = 0 # zero unit test result counter - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread - testProcessRate = macros.sec2nano(0.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container - module = tabularAtmosphere.TabularAtmosphere() # update with current values - module.ModelTag = "tabularAtmosphere" # update python name of test module + module = tabularAtmosphere.TabularAtmosphere() # update with current values + module.ModelTag = "tabularAtmosphere" # update python name of test module # define constants & load data r_eq = 6378136.6 - filename = bskPath + '/supportData/AtmosphereData/EarthGRAMNominal.txt' - altList, rhoList, tempList = readAtmTable(filename,'EarthGRAM') + filename = bskPath + "/supportData/AtmosphereData/EarthGRAMNominal.txt" + altList, rhoList, tempList = readAtmTable(filename, "EarthGRAM") # assign constants & ref. data to module module.planetRadius = r_eq @@ -132,7 +137,7 @@ def tabularAtmosphereTestFunction(altitude, accuracy, useMinReach, useMaxReach): # setup orbit and simulation time r0 = r_eq + (altitude * 1000.0) # meters oe = orbitalMotion.ClassicElements() - mu = 0.3986004415E+15 # meters^3/s^2 + mu = 0.3986004415e15 # meters^3/s^2 oe.a = r0 oe.e = 0.0 oe.i = 45.0 * macros.D2R @@ -142,7 +147,9 @@ def tabularAtmosphereTestFunction(altitude, accuracy, useMinReach, useMaxReach): r0N, v0N = orbitalMotion.elem2rv(mu, oe) # create the input messages - scStateMsg = messaging.SCStatesMsgPayload() # Create a structure for the input message + scStateMsg = ( + messaging.SCStatesMsgPayload() + ) # Create a structure for the input message scStateMsg.r_BN_N = np.array(r0N) scInMsg = messaging.SCStatesMsg().write(scStateMsg) @@ -160,7 +167,7 @@ def tabularAtmosphereTestFunction(altitude, accuracy, useMinReach, useMaxReach): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -180,37 +187,45 @@ def tabAtmoComp(val, xList, yList): else: for i, x in enumerate(xList): if x >= val: - x0 = xList[i-1] - y0 = yList[i-1] + x0 = xList[i - 1] + y0 = yList[i - 1] y1 = yList[i] - m = (y1 - y0)/(x - x0) + m = (y1 - y0) / (x - x0) out = y0 + (val - x0) * m return out # compute truth values trueDensity = tabAtmoComp(altitude * 1000, altList, rhoList) - print('\nmodule density: {0:.6e}'.format(densData[0])) - print('true density: {0:.6e}'.format(trueDensity)) + print("\nmodule density: {0:.6e}".format(densData[0])) + print("true density: {0:.6e}".format(trueDensity)) trueTemp = tabAtmoComp(altitude * 1000, altList, tempList) - print('\nmodule temperature: {0:.6e}'.format(tempData[0])) - print('true temperature: {0:.6e}\n'.format(trueTemp)) + print("\nmodule temperature: {0:.6e}".format(tempData[0])) + print("true temperature: {0:.6e}\n".format(trueTemp)) # compare truth values to module results if trueDensity != 0: - testFailCount = not unitTestSupport.isDoubleEqualRelative(densData[0], trueDensity, accuracy) + testFailCount = not unitTestSupport.isDoubleEqualRelative( + densData[0], trueDensity, accuracy + ) else: - testFailCount = not unitTestSupport.isDoubleEqual(densData[0], trueDensity, accuracy) + testFailCount = not unitTestSupport.isDoubleEqual( + densData[0], trueDensity, accuracy + ) if testFailCount == 0: testMessage = "density computed correctly" else: testMessage = "density computed incorrectly" # compare truth values to module results for temperature - if trueTemp != 0 : # needs checking - testFailCount = not unitTestSupport.isDoubleEqualRelative(tempData[0], trueTemp, accuracy) + if trueTemp != 0: # needs checking + testFailCount = not unitTestSupport.isDoubleEqualRelative( + tempData[0], trueTemp, accuracy + ) else: - testFailCount = not unitTestSupport.isDoubleEqual(tempData[0], trueTemp, accuracy) + testFailCount = not unitTestSupport.isDoubleEqual( + tempData[0], trueTemp, accuracy + ) if testFailCount == 0: testMessage += " and temperature computed correctly" else: @@ -229,8 +244,8 @@ def tabAtmoComp(val, xList, yList): # if __name__ == "__main__": test_tabularAtmosphere( - 10000.0, # altitude - 1e-12, # accuracy - True, - True - ) + 10000.0, # altitude + 1e-12, # accuracy + True, + True, + ) diff --git a/src/simulation/environment/albedo/_UnitTest/test_albedo.py b/src/simulation/environment/albedo/_UnitTest/test_albedo.py index 588a68f74b..ef1aa34599 100644 --- a/src/simulation/environment/albedo/_UnitTest/test_albedo.py +++ b/src/simulation/environment/albedo/_UnitTest/test_albedo.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2020, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -41,12 +40,14 @@ path = os.path.dirname(os.path.abspath(__file__)) + # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(True) -@pytest.mark.parametrize("planetCase", ['earth', 'mars']) -@pytest.mark.parametrize("modelType", ['ALBEDO_AVG_IMPLICIT', 'ALBEDO_AVG_EXPLICIT', 'ALBEDO_DATA']) +@pytest.mark.parametrize("planetCase", ["earth", "mars"]) +@pytest.mark.parametrize( + "modelType", ["ALBEDO_AVG_IMPLICIT", "ALBEDO_AVG_EXPLICIT", "ALBEDO_DATA"] +) @pytest.mark.parametrize("useEclipse", [True, False]) - def test_unitAlbedo(show_plots, planetCase, modelType, useEclipse): """ **Validation Test Description** @@ -71,7 +72,9 @@ def test_unitAlbedo(show_plots, planetCase, modelType, useEclipse): """ # each test method requires a single assert method to be called - [testResults, testMessage] = unitAlbedo(show_plots, planetCase, modelType, useEclipse) + [testResults, testMessage] = unitAlbedo( + show_plots, planetCase, modelType, useEclipse + ) assert testResults < 1, testMessage @@ -95,9 +98,9 @@ def unitAlbedo(show_plots, planetCase, modelType, useEclipse): # Albedo A1 albModule = albedo.Albedo() albModule.ModelTag = "Albedo_0" - if modelType == 'ALBEDO_DATA': + if modelType == "ALBEDO_DATA": dataPath = os.path.abspath(bskPath + "/supportData/AlbedoData/") - if planetCase == 'earth': + if planetCase == "earth": fileName = "Earth_ALB_2018_CERES_All_10x10.csv" else: fileName = "Mars_ALB_TES_10x10.csv" @@ -106,8 +109,10 @@ def unitAlbedo(show_plots, planetCase, modelType, useEclipse): ALB_avg = 0.5 numLat = 200 numLon = 400 - if modelType == 'ALBEDO_AVG_EXPLICIT': - albModule.addPlanetandAlbedoAverageModel(planetInMsg, ALB_avg, numLat, numLon) + if modelType == "ALBEDO_AVG_EXPLICIT": + albModule.addPlanetandAlbedoAverageModel( + planetInMsg, ALB_avg, numLat, numLon + ) else: albModule.addPlanetandAlbedoAverageModel(planetInMsg) @@ -118,15 +123,15 @@ def unitAlbedo(show_plots, planetCase, modelType, useEclipse): # Create dummy planet message planetPositionMsg = messaging.SpicePlanetStateMsgPayload() - planetPositionMsg.PositionVector = [0., 0., 0.] + planetPositionMsg.PositionVector = [0.0, 0.0, 0.0] gravFactory = simIncludeGravBody.gravBodyFactory() - if planetCase == 'earth': + if planetCase == "earth": planet = gravFactory.createEarth() - sunPositionMsg.PositionVector = [-om.AU * 1000., 0.0, 0.0] - elif planetCase == 'mars': + sunPositionMsg.PositionVector = [-om.AU * 1000.0, 0.0, 0.0] + elif planetCase == "mars": planet = gravFactory.createMars() - sunPositionMsg.PositionVector = [-1.5 * om.AU * 1000., 0.0, 0.0] + sunPositionMsg.PositionVector = [-1.5 * om.AU * 1000.0, 0.0, 0.0] planetPositionMsg.PlanetName = planetCase planetPositionMsg.J20002Pfix = np.identity(3) @@ -135,15 +140,15 @@ def unitAlbedo(show_plots, planetCase, modelType, useEclipse): # Create dummy spacecraft message scStateMsg = messaging.SCStatesMsgPayload() rSC = req + 6000 * 1000 # meters - alpha = 71. * macros.D2R + alpha = 71.0 * macros.D2R scStateMsg.r_BN_N = np.dot(rSC, [np.cos(alpha), np.sin(alpha), 0.0]) - scStateMsg.sigma_BN = [0., 0., 0.] + scStateMsg.sigma_BN = [0.0, 0.0, 0.0] # Albedo instrument configuration config1 = albedo.instConfig_t() - config1.fov = 80. * macros.D2R + config1.fov = 80.0 * macros.D2R config1.nHat_B = np.array([-np.cos(alpha), -np.sin(alpha), 0.0]) - config1.r_IB_B = np.array([0., 0., 0.]) + config1.r_IB_B = np.array([0.0, 0.0, 0.0]) albModule.addInstrumentConfig(config1) sunInMsg = messaging.SpicePlanetStateMsg().write(sunPositionMsg) @@ -166,15 +171,15 @@ def unitAlbedo(show_plots, planetCase, modelType, useEclipse): unitTestSim.TotalSim.SingleStepProcesses() # This pulls the actual data log from the simulation run. dataAlb0 = dataLog.albedoAtInstrument - errTol = 1E-12 - if planetCase == 'earth': - if modelType == 'ALBEDO_DATA': + errTol = 1e-12 + if planetCase == "earth": + if modelType == "ALBEDO_DATA": if useEclipse: truthAlb = 0.0022055492477917 else: truthAlb = 0.0022055492477917 else: - if modelType == 'ALBEDO_AVG_EXPLICIT': + if modelType == "ALBEDO_AVG_EXPLICIT": if useEclipse: truthAlb = 0.0041742091531996 else: @@ -185,13 +190,13 @@ def unitAlbedo(show_plots, planetCase, modelType, useEclipse): else: truthAlb = 0.002421222716229847 else: - if modelType == 'ALBEDO_DATA': + if modelType == "ALBEDO_DATA": if useEclipse: truthAlb = 0.0014001432717662 else: truthAlb = 0.0014001432717662 else: - if modelType == 'ALBEDO_AVG_EXPLICIT': + if modelType == "ALBEDO_AVG_EXPLICIT": if useEclipse: truthAlb = 0.0035681407388827 else: @@ -206,12 +211,15 @@ def unitAlbedo(show_plots, planetCase, modelType, useEclipse): testFailCount += 1 # print out success or failure message if testFailCount == 0: - print("PASSED: " + albModule.ModelTag) + print("PASSED: " + albModule.ModelTag) else: - print("Failed: " + albModule.ModelTag) - print("This test uses a relative accuracy value of " + str(errTol * 100) + " percent") + print("Failed: " + albModule.ModelTag) + print( + "This test uses a relative accuracy value of " + str(errTol * 100) + " percent" + ) + + return [testFailCount, "".join(testMessages)] - return [testFailCount, ''.join(testMessages)] def test_albedo_invalid_file(tmp_path): """Verify that Albedo model returns gracefully when file cannot be loaded. @@ -243,13 +251,16 @@ def test_albedo_invalid_file(tmp_path): albModule.spacecraftStateInMsg.subscribeTo(scInMsg) with pytest.raises(BasiliskError): - albModule.addPlanetandAlbedoDataModel(planetInMsg, str(tmp_path), "does_not_exist.file") + albModule.addPlanetandAlbedoDataModel( + planetInMsg, str(tmp_path), "does_not_exist.file" + ) albModule.Reset(0) # the fact that we got here without segfaulting means the test # passed assert True + if __name__ == "__main__": # unitAlbedo(False, 'earth', 'ALBEDO_AVG_EXPLICIT', True) - unitAlbedo(False, 'mars', 'ALBEDO_AVG_IMPLICIT', False) + unitAlbedo(False, "mars", "ALBEDO_AVG_IMPLICIT", False) diff --git a/src/simulation/environment/dentonFluxModel/_UnitTest/test_dentonFluxModel.py b/src/simulation/environment/dentonFluxModel/_UnitTest/test_dentonFluxModel.py index ca192bed75..544f9d8a84 100644 --- a/src/simulation/environment/dentonFluxModel/_UnitTest/test_dentonFluxModel.py +++ b/src/simulation/environment/dentonFluxModel/_UnitTest/test_dentonFluxModel.py @@ -40,23 +40,28 @@ from Basilisk.utilities import macros from Basilisk.simulation import dentonFluxModel -Kps = ['0o', '4-', '5+'] +Kps = ["0o", "4-", "5+"] LTs = [0.00, 14.73] -z_offsets = [0., 3500e3] -r_EN_Ns = np.array([[0., 0., 0.], [400e3, 300e3, -200e3]]) +z_offsets = [0.0, 3500e3] +r_EN_Ns = np.array([[0.0, 0.0, 0.0], [400e3, 300e3, -200e3]]) + @pytest.mark.parametrize("accuracy", [1e2]) -@pytest.mark.parametrize("param1_Kp, param2_LT, param3_z, param4_r_EN", [ - (Kps[0], LTs[0], z_offsets[0], r_EN_Ns[0]), - (Kps[1], LTs[1], z_offsets[1], r_EN_Ns[0]), - (Kps[1], LTs[1], z_offsets[0], r_EN_Ns[1]), - (Kps[1], LTs[1], z_offsets[1], r_EN_Ns[1]), - (Kps[1], LTs[0], z_offsets[1], r_EN_Ns[1]), - (Kps[2], LTs[1], z_offsets[1], r_EN_Ns[1]), - (Kps[2], LTs[0], z_offsets[1], r_EN_Ns[1]), -]) - -def test_dentonFluxModel(show_plots, param1_Kp, param2_LT, param3_z, param4_r_EN, accuracy): +@pytest.mark.parametrize( + "param1_Kp, param2_LT, param3_z, param4_r_EN", + [ + (Kps[0], LTs[0], z_offsets[0], r_EN_Ns[0]), + (Kps[1], LTs[1], z_offsets[1], r_EN_Ns[0]), + (Kps[1], LTs[1], z_offsets[0], r_EN_Ns[1]), + (Kps[1], LTs[1], z_offsets[1], r_EN_Ns[1]), + (Kps[1], LTs[0], z_offsets[1], r_EN_Ns[1]), + (Kps[2], LTs[1], z_offsets[1], r_EN_Ns[1]), + (Kps[2], LTs[0], z_offsets[1], r_EN_Ns[1]), + ], +) +def test_dentonFluxModel( + show_plots, param1_Kp, param2_LT, param3_z, param4_r_EN, accuracy +): r""" **Validation Test Description** @@ -77,11 +82,14 @@ def test_dentonFluxModel(show_plots, param1_Kp, param2_LT, param3_z, param4_r_EN The electron and ion energies are compared to make sure the flux data is computed for the same energy. The main part of the unitTest is to compare the electron and ion flux. """ - dentonFluxModelTestFunction(show_plots, param1_Kp, param2_LT, param3_z, param4_r_EN, - accuracy) + dentonFluxModelTestFunction( + show_plots, param1_Kp, param2_LT, param3_z, param4_r_EN, accuracy + ) -def dentonFluxModelTestFunction(show_plots, param1_Kp, param2_LT, param3_z, param4_r_EN, accuracy): +def dentonFluxModelTestFunction( + show_plots, param1_Kp, param2_LT, param3_z, param4_r_EN, accuracy +): """Test method""" unitTaskName = "unitTask" unitProcessName = "TestProcess" @@ -97,17 +105,19 @@ def dentonFluxModelTestFunction(show_plots, param1_Kp, param2_LT, param3_z, para module.ModelTag = "dentonFluxModule" module.kpIndex = param1_Kp module.numOutputEnergies = 6 - module.dataPath = bskPath + '/supportData/DentonGEO/' + module.dataPath = bskPath + "/supportData/DentonGEO/" unitTestSim.AddModelToTask(unitTaskName, module) # Set up position vectors (param3_z is used to offset S/C and sun from equatorial plane) LT = param2_LT - angle = LT * 360./24. * np.pi/180 - np.pi + angle = LT * 360.0 / 24.0 * np.pi / 180 - np.pi orbitRadius = 42000 * 1e3 # GEO orbit - r_BE_N = np.array([orbitRadius * math.cos(angle), orbitRadius * math.sin(angle), param3_z]) - r_SE_N = np.array([149000000000.0, 0., -2.73*param3_z]) + r_BE_N = np.array( + [orbitRadius * math.cos(angle), orbitRadius * math.sin(angle), param3_z] + ) + r_SE_N = np.array([149000000000.0, 0.0, -2.73 * param3_z]) r_EN_N = param4_r_EN r_BN_N = r_BE_N + r_EN_N r_SN_N = r_SE_N + r_EN_N @@ -146,65 +156,75 @@ def dentonFluxModelTestFunction(show_plots, param1_Kp, param2_LT, param3_z, para # convert Kp index to Kp index counter (between 0 and 27) kpMain = param1_Kp[0] # main Kp index, between 0 and 9 kpSub = param1_Kp[1] # sub Kp index, either '-', 'o', or '+' - if kpSub == '-': - kpIndexCounter = 3*int(kpMain) - 1 - elif kpSub == 'o': - kpIndexCounter = 3*int(kpMain) - elif kpSub == '+': - kpIndexCounter = 3*int(kpMain) + 1 + if kpSub == "-": + kpIndexCounter = 3 * int(kpMain) - 1 + elif kpSub == "o": + kpIndexCounter = 3 * int(kpMain) + elif kpSub == "+": + kpIndexCounter = 3 * int(kpMain) + 1 # load true data from corresponding support file (note that Python indexing starts at 0 and Fortran indexing # starts at 1, relevant for Kp index counter) - filename = 'FluxData_' + str(kpIndexCounter+1) + '_' + str("%.2f" % param2_LT) + '.txt' - filepath = path + '/Support/' + filename + filename = ( + "FluxData_" + str(kpIndexCounter + 1) + "_" + str("%.2f" % param2_LT) + ".txt" + ) + filepath = path + "/Support/" + filename trueEnergyData = np.array([0.0] * messaging.MAX_PLASMA_FLUX_SIZE) trueElectronFluxData = np.array([0.0] * messaging.MAX_PLASMA_FLUX_SIZE) trueIonFluxData = np.array([0.0] * messaging.MAX_PLASMA_FLUX_SIZE) - with open(filepath, 'r') as file: + with open(filepath, "r") as file: rows = np.loadtxt(file, delimiter=",", unpack=False) # true flux data provided by Denton is in Units of [cm^-2 s^-1 sr^-2 eV^-1], but DentonFluxModel converts it to # [m^-2 s^-1 sr^-2 eV^-1]. Need to multiply by 1e4 - trueEnergyData[0:module.numOutputEnergies] = rows[0] - trueElectronFluxData[0:module.numOutputEnergies] = 10.**(rows[1]) * 1e4 - trueIonFluxData[0:module.numOutputEnergies] = 10.**(rows[2]) * 1e4 + trueEnergyData[0 : module.numOutputEnergies] = rows[0] + trueElectronFluxData[0 : module.numOutputEnergies] = 10.0 ** (rows[1]) * 1e4 + trueIonFluxData[0 : module.numOutputEnergies] = 10.0 ** (rows[2]) * 1e4 # make sure module output data is correct - paramsString = ' for Kp-Index={}, LT={}, accuracy={}'.format( - str(param1_Kp), - str(param2_LT), - str(accuracy)) - - np.testing.assert_allclose(energyData, - trueEnergyData, - rtol=0, - atol=accuracy, - err_msg=('Variable: energyData,' + paramsString), - verbose=True) - - np.testing.assert_allclose(electronFluxData, - trueElectronFluxData, - rtol=0, - atol=accuracy, - err_msg=('Variable: electronFluxData,' + paramsString), - verbose=True) - - np.testing.assert_allclose(ionFluxData, - trueIonFluxData, - rtol=0, - atol=accuracy, - err_msg=('Variable: ionFluxData,' + paramsString), - verbose=True) + paramsString = " for Kp-Index={}, LT={}, accuracy={}".format( + str(param1_Kp), str(param2_LT), str(accuracy) + ) + + np.testing.assert_allclose( + energyData, + trueEnergyData, + rtol=0, + atol=accuracy, + err_msg=("Variable: energyData," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + electronFluxData, + trueElectronFluxData, + rtol=0, + atol=accuracy, + err_msg=("Variable: electronFluxData," + paramsString), + verbose=True, + ) + + np.testing.assert_allclose( + ionFluxData, + trueIonFluxData, + rtol=0, + atol=accuracy, + err_msg=("Variable: ionFluxData," + paramsString), + verbose=True, + ) plt.figure(1) fig = plt.gcf() ax = fig.gca() - plt.semilogy(energyData[0:module.numOutputEnergies], electronFluxData[0:module.numOutputEnergies]) - plt.xlabel('Energy [eV]') - plt.ylabel('Electron Flux [e$^{-}$ cm$^{-2}$ s$^{-1}$ str$^{-1}$ eV$^{-1}$]') + plt.semilogy( + energyData[0 : module.numOutputEnergies], + electronFluxData[0 : module.numOutputEnergies], + ) + plt.xlabel("Energy [eV]") + plt.ylabel("Electron Flux [e$^{-}$ cm$^{-2}$ s$^{-1}$ str$^{-1}$ eV$^{-1}$]") if show_plots: plt.show() if __name__ == "__main__": - test_dentonFluxModel(False, '4-', LTs[1], z_offsets[1], r_EN_Ns[1], 1e2) + test_dentonFluxModel(False, "4-", LTs[1], z_offsets[1], r_EN_Ns[1], 1e2) diff --git a/src/simulation/environment/eclipse/_UnitTest/test_eclipse.py b/src/simulation/environment/eclipse/_UnitTest/test_eclipse.py index 047aa4d340..21a9683c39 100755 --- a/src/simulation/environment/eclipse/_UnitTest/test_eclipse.py +++ b/src/simulation/environment/eclipse/_UnitTest/test_eclipse.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -44,11 +43,22 @@ path = os.path.dirname(os.path.abspath(__file__)) + # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(True) -@pytest.mark.parametrize("eclipseCondition, planet", [ -("partial", "earth"), ("full", "earth"), ("none", "earth"), ("annular", "earth"), -("partial", "mars"), ("full", "mars"), ("none", "mars"), ("annular", "mars")]) +@pytest.mark.parametrize( + "eclipseCondition, planet", + [ + ("partial", "earth"), + ("full", "earth"), + ("none", "earth"), + ("annular", "earth"), + ("partial", "mars"), + ("full", "mars"), + ("none", "mars"), + ("annular", "mars"), + ], +) def test_unitEclipse(show_plots, eclipseCondition, planet): """ **Test Description and Success Criteria** @@ -154,22 +164,25 @@ def unitEclipse(show_plots, eclipseCondition, planet): earth.isCentralBody = True elif planet == "mars": mars.isCentralBody = True - scObject_0.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject_0.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # setup Spice interface for some solar system bodies - timeInitString = '2021 MAY 04 07:47:48.965 (UTC)' - gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/' - , timeInitString - # earth and mars must come first as with gravBodies - , spicePlanetNames=["earth", "mars barycenter", "sun", "venus"] - ) + timeInitString = "2021 MAY 04 07:47:48.965 (UTC)" + gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", + timeInitString, + # earth and mars must come first as with gravBodies + spicePlanetNames=["earth", "mars barycenter", "sun", "venus"], + ) if planet == "earth": if eclipseCondition == "full": gravFactory.spiceObject.zeroBase = "earth" # set up spacecraft 0 position and velocity for full eclipse oe = orbitalMotion.ClassicElements() - r_0 = (500 + orbitalMotion.REQ_EARTH) # km + r_0 = 500 + orbitalMotion.REQ_EARTH # km oe.a = r_0 oe.e = 0.00001 oe.i = 5.0 * macros.D2R @@ -183,7 +196,7 @@ def unitEclipse(show_plots, eclipseCondition, planet): gravFactory.spiceObject.zeroBase = "earth" # set up spacecraft 0 position and velocity for full eclipse oe = orbitalMotion.ClassicElements() - r_0 = (500 + orbitalMotion.REQ_EARTH) # km + r_0 = 500 + orbitalMotion.REQ_EARTH # km oe.a = r_0 oe.e = 0.00001 oe.i = 5.0 * macros.D2R @@ -207,15 +220,27 @@ def unitEclipse(show_plots, eclipseCondition, planet): scObject_0.hub.v_CN_NInit = v_N_0 * 1000 # convert to meters elif eclipseCondition == "annular": gravFactory.spiceObject.zeroBase = "earth" - scObject_0.hub.r_CN_NInit = [-326716535628.942, -287302983139.247, -124542549301.050] + scObject_0.hub.r_CN_NInit = [ + -326716535628.942, + -287302983139.247, + -124542549301.050, + ] elif planet == "mars": if eclipseCondition == "full": gravFactory.spiceObject.zeroBase = "mars barycenter" - scObject_0.hub.r_CN_NInit = [-2930233.55919119, 2567609.100747609, 41384.23366372246] # meters + scObject_0.hub.r_CN_NInit = [ + -2930233.55919119, + 2567609.100747609, + 41384.23366372246, + ] # meters elif eclipseCondition == "partial": gravFactory.spiceObject.zeroBase = "mars barycenter" - scObject_0.hub.r_CN_NInit = [-6050166.454829555, 2813822.447404055, 571725.5651779658] # meters + scObject_0.hub.r_CN_NInit = [ + -6050166.454829555, + 2813822.447404055, + 571725.5651779658, + ] # meters elif eclipseCondition == "none": oe = orbitalMotion.ClassicElements() r_0 = 9959991.68982 # km @@ -230,16 +255,28 @@ def unitEclipse(show_plots, eclipseCondition, planet): scObject_0.hub.v_CN_NInit = v_N_0 * 1000 # convert to meters elif eclipseCondition == "annular": gravFactory.spiceObject.zeroBase = "mars barycenter" - scObject_0.hub.r_CN_NInit = [-427424601171.464, 541312532797.400, 259820030623.064] # meters + scObject_0.hub.r_CN_NInit = [ + -427424601171.464, + 541312532797.400, + 259820030623.064, + ] # meters unitTestSim.AddModelToTask(testTaskName, gravFactory.spiceObject, -1) eclipseObject = eclipse.Eclipse() eclipseObject.addSpacecraftToModel(scObject_0.scStateOutMsg) - eclipseObject.addPlanetToModel(gravFactory.spiceObject.planetStateOutMsgs[3]) # venus - eclipseObject.addPlanetToModel(gravFactory.spiceObject.planetStateOutMsgs[1]) # mars - eclipseObject.addPlanetToModel(gravFactory.spiceObject.planetStateOutMsgs[0]) # earth - eclipseObject.sunInMsg.subscribeTo(gravFactory.spiceObject.planetStateOutMsgs[2]) # sun + eclipseObject.addPlanetToModel( + gravFactory.spiceObject.planetStateOutMsgs[3] + ) # venus + eclipseObject.addPlanetToModel( + gravFactory.spiceObject.planetStateOutMsgs[1] + ) # mars + eclipseObject.addPlanetToModel( + gravFactory.spiceObject.planetStateOutMsgs[0] + ) # earth + eclipseObject.sunInMsg.subscribeTo( + gravFactory.spiceObject.planetStateOutMsgs[2] + ) # sun unitTestSim.AddModelToTask(testTaskName, eclipseObject) @@ -254,52 +291,84 @@ def unitEclipse(show_plots, eclipseCondition, planet): eclipseData_0 = dataLog.shadowFactor # Obtain body position vectors to check with MATLAB - errTol = 1E-12 + errTol = 1e-12 if planet == "earth": if eclipseCondition == "partial": truthShadowFactor = 0.62310760206735027 - if not unitTestSupport.isDoubleEqual(eclipseData_0[-1], truthShadowFactor, errTol): + if not unitTestSupport.isDoubleEqual( + eclipseData_0[-1], truthShadowFactor, errTol + ): testFailCount += 1 - testMessages.append("Shadow Factor failed for Earth partial eclipse condition") + testMessages.append( + "Shadow Factor failed for Earth partial eclipse condition" + ) elif eclipseCondition == "full": truthShadowFactor = 0.0 - if not unitTestSupport.isDoubleEqual(eclipseData_0[-1], truthShadowFactor, errTol): + if not unitTestSupport.isDoubleEqual( + eclipseData_0[-1], truthShadowFactor, errTol + ): testFailCount += 1 - testMessages.append("Shadow Factor failed for Earth full eclipse condition") + testMessages.append( + "Shadow Factor failed for Earth full eclipse condition" + ) elif eclipseCondition == "none": truthShadowFactor = 1.0 - if not unitTestSupport.isDoubleEqual(eclipseData_0[-1], truthShadowFactor, errTol): + if not unitTestSupport.isDoubleEqual( + eclipseData_0[-1], truthShadowFactor, errTol + ): testFailCount += 1 - testMessages.append("Shadow Factor failed for Earth none eclipse condition") + testMessages.append( + "Shadow Factor failed for Earth none eclipse condition" + ) elif eclipseCondition == "annular": truthShadowFactor = 1.497253388113018e-04 - if not unitTestSupport.isDoubleEqual(eclipseData_0[-1], truthShadowFactor, errTol): + if not unitTestSupport.isDoubleEqual( + eclipseData_0[-1], truthShadowFactor, errTol + ): testFailCount += 1 - testMessages.append("Shadow Factor failed for Earth annular eclipse condition") + testMessages.append( + "Shadow Factor failed for Earth annular eclipse condition" + ) elif planet == "mars": if eclipseCondition == "partial": truthShadowFactor = 0.18745025055615416 - if not unitTestSupport.isDoubleEqual(eclipseData_0[-1], truthShadowFactor, errTol): + if not unitTestSupport.isDoubleEqual( + eclipseData_0[-1], truthShadowFactor, errTol + ): testFailCount += 1 - testMessages.append("Shadow Factor failed for Mars partial eclipse condition") + testMessages.append( + "Shadow Factor failed for Mars partial eclipse condition" + ) elif eclipseCondition == "full": truthShadowFactor = 0.0 - if not unitTestSupport.isDoubleEqual(eclipseData_0[-1], truthShadowFactor, errTol): + if not unitTestSupport.isDoubleEqual( + eclipseData_0[-1], truthShadowFactor, errTol + ): testFailCount += 1 - testMessages.append("Shadow Factor failed for Mars full eclipse condition") + testMessages.append( + "Shadow Factor failed for Mars full eclipse condition" + ) elif eclipseCondition == "none": truthShadowFactor = 1.0 - if not unitTestSupport.isDoubleEqual(eclipseData_0[-1], truthShadowFactor, errTol): + if not unitTestSupport.isDoubleEqual( + eclipseData_0[-1], truthShadowFactor, errTol + ): testFailCount += 1 - testMessages.append("Shadow Factor failed for Mars none eclipse condition") + testMessages.append( + "Shadow Factor failed for Mars none eclipse condition" + ) elif eclipseCondition == "annular": truthShadowFactor = 4.245137380531894e-05 - if not unitTestSupport.isDoubleEqual(eclipseData_0[-1], truthShadowFactor, errTol): + if not unitTestSupport.isDoubleEqual( + eclipseData_0[-1], truthShadowFactor, errTol + ): testFailCount += 1 - testMessages.append("Shadow Factor failed for Mars annular eclipse condition") + testMessages.append( + "Shadow Factor failed for Mars annular eclipse condition" + ) if testFailCount == 0: print("PASSED: " + planet + "-" + eclipseCondition) @@ -308,14 +377,15 @@ def unitEclipse(show_plots, eclipseCondition, planet): else: print(testMessages) - print('The error tolerance for all tests is ' + str(errTol)) + print("The error tolerance for all tests is " + str(errTol)) # # unload the SPICE libraries that were loaded by the spiceObject earlier # gravFactory.unloadSpiceKernels() - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def unitEclipseCustom(show_plots): __tracebackhide__ = True @@ -340,18 +410,22 @@ def unitEclipseCustom(show_plots): # setup Gravity Bodies gravFactory = simIncludeGravBody.gravBodyFactory() mu_bennu = 4.892 - custom = gravFactory.createCustomGravObject("custom", mu_bennu) # creates a custom grav object (bennu) - scObject_0.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + custom = gravFactory.createCustomGravObject( + "custom", mu_bennu + ) # creates a custom grav object (bennu) + scObject_0.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) # Create the ephemeris data for the bodies # setup celestial object ephemeris module gravBodyEphem = planetEphemeris.PlanetEphemeris() - gravBodyEphem.ModelTag = 'planetEphemeris' + gravBodyEphem.ModelTag = "planetEphemeris" gravBodyEphem.setPlanetNames(planetEphemeris.StringVector(["custom"])) # Specify bennu orbit oeAsteroid = planetEphemeris.ClassicElements() - oeAsteroid.a = 1.1259 * orbitalMotion.AU * 1000. # m + oeAsteroid.a = 1.1259 * orbitalMotion.AU * 1000.0 # m oeAsteroid.e = 0.20373 oeAsteroid.i = 6.0343 * macros.D2R oeAsteroid.Omega = 2.01820 * macros.D2R @@ -375,8 +449,8 @@ def unitEclipseCustom(show_plots): eclipseObject = eclipse.Eclipse() eclipseObject.addSpacecraftToModel(scObject_0.scStateOutMsg) eclipseObject.addPlanetToModel(gravBodyEphem.planetOutMsgs[0]) # custom - eclipseObject.sunInMsg.subscribeTo(sunPlanetStateMsg) # sun - eclipseObject.rEqCustom = 282. # m + eclipseObject.sunInMsg.subscribeTo(sunPlanetStateMsg) # sun + eclipseObject.rEqCustom = 282.0 # m unitTestSim.AddModelToTask(testTaskName, eclipseObject) @@ -391,7 +465,7 @@ def unitEclipseCustom(show_plots): eclipseData_0 = dataLog.shadowFactor # Obtain body position vectors to check with MATLAB - errTol = 1E-12 + errTol = 1e-12 truthShadowFactor = 0.0 if not unitTestSupport.isDoubleEqual(eclipseData_0[-1], truthShadowFactor, errTol): testFailCount += 1 @@ -404,9 +478,10 @@ def unitEclipseCustom(show_plots): else: print(testMessages) - print('The error tolerance for all tests is ' + str(errTol)) + print("The error tolerance for all tests is " + str(errTol)) + + return [testFailCount, "".join(testMessages)] - return [testFailCount, ''.join(testMessages)] if __name__ == "__main__": unitEclipse(False, "annular", "mars") diff --git a/src/simulation/environment/ephemerisConverter/_UnitTest/test_ephemconvert.py b/src/simulation/environment/ephemerisConverter/_UnitTest/test_ephemconvert.py index 11ffa572ea..11fc23808c 100755 --- a/src/simulation/environment/ephemerisConverter/_UnitTest/test_ephemconvert.py +++ b/src/simulation/environment/ephemerisConverter/_UnitTest/test_ephemconvert.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -54,9 +53,9 @@ def unitephemeris_converter(show_plots): # Create a sim module as an empty container sim = SimulationBaseClass.SimBaseClass() - simulationTime = macros.sec2nano(30.) + simulationTime = macros.sec2nano(30.0) numDataPoints = 600 - samplingTime = simulationTime // (numDataPoints-1) + samplingTime = simulationTime // (numDataPoints - 1) DynUnitTestProc = sim.CreateNewProcess(unitProcessName) # create the dynamics task and specify the integration update time DynUnitTestProc.addTask(sim.CreateNewTask(unitTaskName, samplingTime)) @@ -67,14 +66,14 @@ def unitephemeris_converter(show_plots): # Initialize the spice module spiceObject = spiceInterface.SpiceInterface() spiceObject.ModelTag = "SpiceInterfaceData" - spiceObject.SPICEDataPath = bskPath + '/supportData/EphemerisData/' + spiceObject.SPICEDataPath = bskPath + "/supportData/EphemerisData/" spiceObject.addPlanetNames(spiceInterface.StringVector(planets)) spiceObject.UTCCalInit = "2015 February 10, 00:00:00.0 TDB" sim.AddModelToTask(unitTaskName, spiceObject) # Initialize the ephemeris module ephemObject = ephemerisConverter.EphemerisConverter() - ephemObject.ModelTag = 'EphemData' + ephemObject.ModelTag = "EphemData" ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[0]) # earth ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[1]) # mars ephemObject.addSpiceInputMsg(spiceObject.planetStateOutMsgs[2]) # sun @@ -104,13 +103,13 @@ def unitephemeris_converter(show_plots): spicePlanetDCM_PN = dataSpiceLog[i].J20002Pfix spicePlanetDCM_PN_dot = dataSpiceLog[i].J20002Pfix_dot for j in range(0, numDataPoints): - dcm_PN = spicePlanetDCM_PN[j,:] - dcm_PN_dot = spicePlanetDCM_PN_dot[j,:] - sigma_BN[i,j,0:3] = RigidBodyKinematics.C2MRP(dcm_PN) + dcm_PN = spicePlanetDCM_PN[j, :] + dcm_PN_dot = spicePlanetDCM_PN_dot[j, :] + sigma_BN[i, j, 0:3] = RigidBodyKinematics.C2MRP(dcm_PN) omega_BN_B_tilde = -np.matmul(dcm_PN_dot, dcm_PN.T) - omega_BN_B[i,j,0] = omega_BN_B_tilde[2,1] - omega_BN_B[i,j,1] = omega_BN_B_tilde[0,2] - omega_BN_B[i,j,2] = omega_BN_B_tilde[1,0] + omega_BN_B[i, j, 0] = omega_BN_B_tilde[2, 1] + omega_BN_B[i, j, 1] = omega_BN_B_tilde[0, 2] + omega_BN_B[i, j, 2] = omega_BN_B_tilde[1, 0] # Get the position, velocities, attitude, attitude rate, and time for the message before and after the copy accuracy = 1e-12 @@ -121,10 +120,38 @@ def unitephemeris_converter(show_plots): spicePlanetVelData = dataSpiceLog[i].VelocityVector ephemPlanetAttData = dataEphemLog[i].sigma_BN ephemePlanetAngVelData = dataEphemLog[i].omega_BN_B - testFailCount, testMessages = unitTestSupport.compareArrayRelative(spicePlanetPosData[:,0:3], ephemPlanetPosData, accuracy, "Position", testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArrayRelative(spicePlanetVelData[:,0:3], ephemPlanetVelData, accuracy, "Velocity", testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArrayRelative(sigma_BN[i,:,:], ephemPlanetAttData, accuracy, "Attitude", testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArray(omega_BN_B[i,:], ephemePlanetAngVelData, accuracy, "Angular Velocity", testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArrayRelative( + spicePlanetPosData[:, 0:3], + ephemPlanetPosData, + accuracy, + "Position", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArrayRelative( + spicePlanetVelData[:, 0:3], + ephemPlanetVelData, + accuracy, + "Velocity", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArrayRelative( + sigma_BN[i, :, :], + ephemPlanetAttData, + accuracy, + "Attitude", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArray( + omega_BN_B[i, :], + ephemePlanetAngVelData, + accuracy, + "Angular Velocity", + testFailCount, + testMessages, + ) # print out success message if no error were found if testFailCount == 0: @@ -134,7 +161,7 @@ def unitephemeris_converter(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # This statement below ensures that the unit test scrip can be run as a diff --git a/src/simulation/environment/groundLocation/_UnitTest/test_unitGroundLocation.py b/src/simulation/environment/groundLocation/_UnitTest/test_unitGroundLocation.py index c79a52d162..fc9832cce4 100644 --- a/src/simulation/environment/groundLocation/_UnitTest/test_unitGroundLocation.py +++ b/src/simulation/environment/groundLocation/_UnitTest/test_unitGroundLocation.py @@ -33,7 +33,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) bskPath = __path__[0] @@ -54,33 +54,38 @@ def test_range(show_plots): simProcessName = "simProcess" scSim = SimulationBaseClass.SimBaseClass() dynProcess = scSim.CreateNewProcess(simProcessName) - simulationTime = macros.sec2nano(10.) - simulationTimeStep = macros.sec2nano(1.) + simulationTime = macros.sec2nano(10.0) + simulationTimeStep = macros.sec2nano(1.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # Initialize new atmosphere and drag model, add them to task groundTarget = groundLocation.GroundLocation() groundTarget.ModelTag = "groundTarget" - groundTarget.planetRadius = orbitalMotion.REQ_EARTH * 1000. - groundTarget.maximumRange = 100e3 # meters - groundTarget.minimumElevation = np.radians(80.) - groundTarget.specifyLocation(np.radians(0.), np.radians(0.), 0.) + groundTarget.planetRadius = orbitalMotion.REQ_EARTH * 1000.0 + groundTarget.maximumRange = 100e3 # meters + groundTarget.minimumElevation = np.radians(80.0) + groundTarget.specifyLocation(np.radians(0.0), np.radians(0.0), 0.0) scSim.AddModelToTask(simTaskName, groundTarget) # Write out mock planet rotation, spacecraft position messages sc1_message = messaging.SCStatesMsgPayload() - sc1_message.r_BN_N = [orbitalMotion.REQ_EARTH*1e3 + 100e3, 0, 0] # SC1 is in range + sc1_message.r_BN_N = [ + orbitalMotion.REQ_EARTH * 1e3 + 100e3, + 0, + 0, + ] # SC1 is in range sc1Msg = messaging.SCStatesMsg().write(sc1_message) sc2_message = messaging.SCStatesMsgPayload() # SC2 is placed inside/outside the visibility cone for the ground station - sc2_message.r_BN_N = [orbitalMotion.REQ_EARTH*1e3 + 101e3,0, 0] + sc2_message.r_BN_N = [orbitalMotion.REQ_EARTH * 1e3 + 101e3, 0, 0] sc2Msg = messaging.SCStatesMsg().write(sc2_message) sc3_message = messaging.SCStatesMsgPayload() # SC3 is inside the altitude limit, but outside the visibility cone - sc3_message.r_BN_N = rbk.euler3(np.radians(11.)).dot(np.array([100e3, 0, 0])) + np.array( - [orbitalMotion.REQ_EARTH * 1e3, 0, 0]) + sc3_message.r_BN_N = rbk.euler3(np.radians(11.0)).dot( + np.array([100e3, 0, 0]) + ) + np.array([orbitalMotion.REQ_EARTH * 1e3, 0, 0]) sc3Msg = messaging.SCStatesMsg().write(sc3_message) groundTarget.addSpacecraftToModel(sc1Msg) @@ -89,7 +94,9 @@ def test_range(show_plots): # Log the access indicator numDataPoints = 2 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog0 = groundTarget.accessOutMsgs[0].recorder(samplingTime) dataLog1 = groundTarget.accessOutMsgs[1].recorder(samplingTime) dataLog2 = groundTarget.accessOutMsgs[2].recorder(samplingTime) @@ -118,18 +125,18 @@ def test_range(show_plots): # Compare to expected values accuracy = 1e-8 ref_ranges = [100e3, 101e3, 100e3] - ref_elevation = [np.radians(90.), np.radians(90.), np.radians(79.)] + ref_elevation = [np.radians(90.0), np.radians(90.0), np.radians(79.0)] ref_access = [1, 0, 0] test_ranges = [sc1_slant[1], sc2_slant[1], sc3_slant[1]] - test_elevation = [sc1_elevation[1],sc2_elevation[1],sc3_elevation[1]] - test_access = [sc1_access[1],sc2_access[1],sc3_access[1]] + test_elevation = [sc1_elevation[1], sc2_elevation[1], sc3_elevation[1]] + test_access = [sc1_access[1], sc2_access[1], sc3_access[1]] range_worked = test_ranges == pytest.approx(ref_ranges, accuracy) elevation_worked = test_elevation == pytest.approx(ref_elevation, accuracy) access_worked = test_access == pytest.approx(ref_access, abs=1e-16) - assert (range_worked and elevation_worked and access_worked) + assert range_worked and elevation_worked and access_worked def test_rotation(show_plots): @@ -141,39 +148,43 @@ def test_rotation(show_plots): :return: """ - simTime = 1. + simTime = 1.0 simTaskName = "simTask" simProcessName = "simProcess" scSim = SimulationBaseClass.SimBaseClass() dynProcess = scSim.CreateNewProcess(simProcessName) simulationTime = macros.sec2nano(simTime) - simulationTimeStep = macros.sec2nano(1.) + simulationTimeStep = macros.sec2nano(1.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # Initialize new atmosphere and drag model, add them to task groundTarget = groundLocation.GroundLocation() groundTarget.ModelTag = "groundTarget" - groundTarget.planetRadius = orbitalMotion.REQ_EARTH * 1000. - groundTarget.maximumRange = 200e3 # meters - groundTarget.minimumElevation = np.radians(10.) - groundTarget.specifyLocation(np.radians(0.), np.radians(10.), 0.) + groundTarget.planetRadius = orbitalMotion.REQ_EARTH * 1000.0 + groundTarget.maximumRange = 200e3 # meters + groundTarget.minimumElevation = np.radians(10.0) + groundTarget.specifyLocation(np.radians(0.0), np.radians(10.0), 0.0) scSim.AddModelToTask(simTaskName, groundTarget) # Write out mock planet rotation, spacecraft position messages sc1_message = messaging.SCStatesMsgPayload() - sc1_message.r_BN_N = np.array([orbitalMotion.REQ_EARTH*1e3 + 90e3, 0, 0]) # SC1 is in range + sc1_message.r_BN_N = np.array( + [orbitalMotion.REQ_EARTH * 1e3 + 90e3, 0, 0] + ) # SC1 is in range scMsg = messaging.SCStatesMsg().write(sc1_message) groundTarget.addSpacecraftToModel(scMsg) planet_message = messaging.SpicePlanetStateMsgPayload() - planet_message.J20002Pfix = rbk.euler3(np.radians(-10.)).tolist() + planet_message.J20002Pfix = rbk.euler3(np.radians(-10.0)).tolist() planetMsg = messaging.SpicePlanetStateMsg().write(planet_message) groundTarget.planetInMsg.subscribeTo(planetMsg) # Log the access indicator numDataPoints = 2 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = groundTarget.accessOutMsgs[0].recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) @@ -191,7 +202,7 @@ def test_rotation(show_plots): # Compare to expected values accuracy = 1e-8 ref_ranges = [90e3] - ref_elevation = [np.radians(90.)] + ref_elevation = [np.radians(90.0)] ref_access = [1] test_ranges = [sc1_slant[1]] @@ -202,7 +213,8 @@ def test_rotation(show_plots): elevation_worked = test_elevation == pytest.approx(ref_elevation, accuracy) access_worked = test_access == pytest.approx(ref_access, abs=1e-16) - assert (range_worked and elevation_worked and access_worked) + assert range_worked and elevation_worked and access_worked + def test_AzElR_rates(): """ @@ -223,24 +235,26 @@ def test_AzElR_rates(): planet = gravFactory.createEarth() mu = planet.mu planet.isCentralBody = True - timeInitString = '2021 MAY 04 06:47:48.965 (UTC)' - gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/' - , timeInitString - ) - gravFactory.spiceObject.zeroBase = 'Earth' + timeInitString = "2021 MAY 04 06:47:48.965 (UTC)" + gravFactory.createSpiceInterface( + bskPath + "/supportData/EphemerisData/", timeInitString + ) + gravFactory.spiceObject.zeroBase = "Earth" scSim.AddModelToTask(simTaskName, gravFactory.spiceObject, -1) scObject = spacecraft.Spacecraft() - scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scObject.gravField.gravBodies = spacecraft.GravBodyVector( + list(gravFactory.gravBodies.values()) + ) scSim.AddModelToTask(simTaskName, scObject) oe = orbitalMotion.ClassicElements() - r_sc = planet.radEquator + 100*1E3 + r_sc = planet.radEquator + 100 * 1e3 oe.a = r_sc oe.e = 0.00001 - oe.i = 53.0*macros.D2R - oe.Omega = 115.0*macros.D2R - oe.omega = 5.0*macros.D2R - oe.f = 75.*macros.D2R + oe.i = 53.0 * macros.D2R + oe.Omega = 115.0 * macros.D2R + oe.omega = 5.0 * macros.D2R + oe.f = 75.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) scObject.hub.r_CN_NInit = rN scObject.hub.v_CN_NInit = vN @@ -249,13 +263,15 @@ def test_AzElR_rates(): groundStation.planetRadius = planet.radEquator groundStation.specifyLocation(np.radians(40.009971), np.radians(-105.243895), 1624) groundStation.planetInMsg.subscribeTo(gravFactory.spiceObject.planetStateOutMsgs[0]) - groundStation.minimumElevation = np.radians(60.) + groundStation.minimumElevation = np.radians(60.0) groundStation.addSpacecraftToModel(scObject.scStateOutMsg) scSim.AddModelToTask(simTaskName, groundStation) # Log the Az,El,R and rates info numDataPoints = 2 - samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + samplingTime = unitTestSupport.samplingTime( + simulationTime, simulationTimeStep, numDataPoints + ) dataLog = groundStation.accessOutMsgs[0].recorder(samplingTime) scSim.AddModelToTask(simTaskName, dataLog) @@ -273,15 +289,15 @@ def test_AzElR_rates(): sc_az_rate = dataLog.az_dot # Euler integration - sc_Euler_range = sc_range[0] + sc_range_rate[0]*dt - sc_Euler_elev = sc_elevation[0] + sc_el_rate[0]*dt - sc_Euler_azimuth = sc_azimuth[0] + sc_az_rate[0]*dt + sc_Euler_range = sc_range[0] + sc_range_rate[0] * dt + sc_Euler_elev = sc_elevation[0] + sc_el_rate[0] * dt + sc_Euler_azimuth = sc_azimuth[0] + sc_az_rate[0] * dt range_rate_worked = sc_range[1] == pytest.approx(sc_Euler_range, rel=1e-5) el_rate_worked = sc_elevation[1] == pytest.approx(sc_Euler_elev, rel=1e-5) az_rate_worked = sc_azimuth[1] == pytest.approx(sc_Euler_azimuth, rel=1e-5) - assert (range_rate_worked and el_rate_worked and az_rate_worked) + assert range_rate_worked and el_rate_worked and az_rate_worked def plot_geometry(groundLocation, scLocations, minimumElevation): @@ -295,30 +311,40 @@ def plot_geometry(groundLocation, scLocations, minimumElevation): :return: """ fig = plt.figure() - ax = fig.add_subplot(projection='3d') + ax = fig.add_subplot(projection="3d") # draw sphere - u, v = np.mgrid[0:2 * np.pi:20j, 0:np.pi:20j] - x = orbitalMotion.REQ_EARTH*1000 * np.cos(u) * np.sin(v) - y = orbitalMotion.REQ_EARTH*1000 *np.sin(u) * np.sin(v) - z = orbitalMotion.REQ_EARTH*1000 *np.cos(v) + u, v = np.mgrid[0 : 2 * np.pi : 20j, 0 : np.pi : 20j] + x = orbitalMotion.REQ_EARTH * 1000 * np.cos(u) * np.sin(v) + y = orbitalMotion.REQ_EARTH * 1000 * np.sin(u) * np.sin(v) + z = orbitalMotion.REQ_EARTH * 1000 * np.cos(v) ax.plot_wireframe(x, y, z, color="g") # draw a point0 - ax.scatter(groundLocation[0],groundLocation[1],groundLocation[2], color="r", s=100) + ax.scatter( + groundLocation[0], groundLocation[1], groundLocation[2], color="r", s=100 + ) # draw a vector for location in scLocations: - ax.scatter(location[0],location[1],location[2],color='k',s=100) - - ax.quiver(groundLocation[0],groundLocation[1],groundLocation[2], - location[0],location[1],location[2], length=1.0, normalize=True) - #ax.add_artist(a) + ax.scatter(location[0], location[1], location[2], color="k", s=100) + + ax.quiver( + groundLocation[0], + groundLocation[1], + groundLocation[2], + location[0], + location[1], + location[2], + length=1.0, + normalize=True, + ) + # ax.add_artist(a) plt.show() -if __name__ == '__main__': +if __name__ == "__main__": test_rotation(False) # test_range(True) diff --git a/src/simulation/environment/groundMapping/_UnitTest/test_groundMapping.py b/src/simulation/environment/groundMapping/_UnitTest/test_groundMapping.py index 091a3b8b05..50b4738f51 100644 --- a/src/simulation/environment/groundMapping/_UnitTest/test_groundMapping.py +++ b/src/simulation/environment/groundMapping/_UnitTest/test_groundMapping.py @@ -28,8 +28,8 @@ import pytest -@pytest.mark.parametrize("maxRange", [1e-12, 0.001, -1.0]) +@pytest.mark.parametrize("maxRange", [1e-12, 0.001, -1.0]) def test_groundMapping(maxRange): r""" This test checks two points to determine if they are accessible for mapping or not. One point should be mapped, @@ -60,21 +60,21 @@ def groundMappingTestFunction(maxRange): # Configure blank module input messages planetInMsgData = messaging.SpicePlanetStateMsgPayload() - planetInMsgData.J20002Pfix = [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]] - planetInMsgData.PositionVector = [0., 0., 0.] + planetInMsgData.J20002Pfix = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] + planetInMsgData.PositionVector = [0.0, 0.0, 0.0] planetInMsg = messaging.SpicePlanetStateMsg().write(planetInMsgData) scStateInMsgData = messaging.SCStatesMsgPayload() - scStateInMsgData.r_BN_N = [0., -1., 0.] - scStateInMsgData.sigma_BN = [0., 0., 0.] + scStateInMsgData.r_BN_N = [0.0, -1.0, 0.0] + scStateInMsgData.sigma_BN = [0.0, 0.0, 0.0] scStateInMsg = messaging.SCStatesMsg().write(scStateInMsgData) # Create the initial imaging target groundMap = groundMapping.GroundMapping() groundMap.ModelTag = "groundMapping" - groundMap.addPointToModel([0., -0.1, 0.]) - groundMap.addPointToModel([0., 0., math.tan(np.radians(22.5))+0.1]) - groundMap.minimumElevation = np.radians(45.) + groundMap.addPointToModel([0.0, -0.1, 0.0]) + groundMap.addPointToModel([0.0, 0.0, math.tan(np.radians(22.5)) + 0.1]) + groundMap.minimumElevation = np.radians(45.0) if maxRange > 0.0: groundMap.maximumRange = maxRange groundMap.cameraPos_B = [0, 0, 0] @@ -107,7 +107,7 @@ def groundMappingTestFunction(maxRange): map_access[idx] = 1 # If the first target is not mapped, failure - if not map_access[0] and (maxRange > 1.0 or maxRange < 0.0) : + if not map_access[0] and (maxRange > 1.0 or maxRange < 0.0): testFailCount += 1 # If the second target is mapped, failure diff --git a/src/simulation/environment/magneticFieldCenteredDipole/_UnitTest/test_magneticFieldCenteredDipole.py b/src/simulation/environment/magneticFieldCenteredDipole/_UnitTest/test_magneticFieldCenteredDipole.py index f5bcb5a1a7..3314274c7c 100755 --- a/src/simulation/environment/magneticFieldCenteredDipole/_UnitTest/test_magneticFieldCenteredDipole.py +++ b/src/simulation/environment/magneticFieldCenteredDipole/_UnitTest/test_magneticFieldCenteredDipole.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -31,12 +30,14 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.simulation import magneticFieldCenteredDipole from Basilisk.architecture import messaging from Basilisk.utilities import macros @@ -51,31 +52,32 @@ # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("useDefault", [ True, False]) -@pytest.mark.parametrize("useMinReach", [ True, False]) -@pytest.mark.parametrize("useMaxReach", [ True, False]) -@pytest.mark.parametrize("usePlanetEphemeris", [ True, False]) - +@pytest.mark.parametrize("useDefault", [True, False]) +@pytest.mark.parametrize("useMinReach", [True, False]) +@pytest.mark.parametrize("useMaxReach", [True, False]) +@pytest.mark.parametrize("usePlanetEphemeris", [True, False]) # update "module" in this function name to reflect the module name def test_module(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris) + [testResults, testMessage] = run( + show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris + ) assert testResults < 1, testMessage def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) @@ -84,10 +86,10 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): testModule.ModelTag = "CenteredDipole" if useDefault: - refg10 = 0.0 # Tesla - refg11 = 0.0 # Tesla - refh11 = 0.0 # Tesla - refPlanetRadius = 0.0 # meters + refg10 = 0.0 # Tesla + refg11 = 0.0 # Tesla + refh11 = 0.0 # Tesla + refPlanetRadius = 0.0 # meters else: simSetPlanetEnvironment.centeredDipoleMagField(testModule, "earth") refg10 = testModule.g10 @@ -97,11 +99,11 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): minReach = -1.0 if useMinReach: - minReach = (orbitalMotion.REQ_EARTH+300.)*1000.0 # meters + minReach = (orbitalMotion.REQ_EARTH + 300.0) * 1000.0 # meters testModule.envMinReach = minReach maxReach = -1.0 if useMaxReach: - maxReach = (orbitalMotion.REQ_EARTH+100.) # meters + maxReach = orbitalMotion.REQ_EARTH + 100.0 # meters testModule.envMaxReach = maxReach planetPosition = np.array([0.0, 0.0, 0.0]) refPlanetDCM = np.array(((1, 0, 0), (0, 1, 0), (0, 0, 1))) @@ -114,7 +116,6 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): planetMsg = messaging.SpicePlanetStateMsg().write(planetStateMsg) testModule.planetPosInMsg.subscribeTo(planetMsg) - # add spacecraft to environment model sc0StateMsg = messaging.SCStatesMsg() sc1StateMsg = messaging.SCStatesMsg() @@ -129,7 +130,7 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): # # setup orbit and simulation time oe = orbitalMotion.ClassicElements() - mu = 0.3986004415E+15 # meters^3/s^2 + mu = 0.3986004415e15 # meters^3/s^2 oe.a = r0 oe.e = 0.0 oe.i = 45.0 * macros.D2R @@ -140,13 +141,16 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): oe.a = r1 r1N, v1N = orbitalMotion.elem2rv(mu, oe) - # create the input messages - sc0StateMsgData = messaging.SCStatesMsgPayload() # Create a structure for the input message + sc0StateMsgData = ( + messaging.SCStatesMsgPayload() + ) # Create a structure for the input message sc0StateMsgData.r_BN_N = np.array(r0N) + np.array(planetPosition) sc0StateMsg.write(sc0StateMsgData) - sc1StateMsgData = messaging.SCStatesMsgPayload() # Create a structure for the input message + sc1StateMsgData = ( + messaging.SCStatesMsgPayload() + ) # Create a structure for the input message sc1StateMsgData.r_BN_N = np.array(r1N) + np.array(planetPosition) sc1StateMsg.write(sc1StateMsgData) @@ -163,7 +167,7 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -175,56 +179,84 @@ def run(show_plots, useDefault, useMinReach, useMaxReach, usePlanetEphemeris): def centeredDipole(pos_N, X, refPlanetRadius, refPlanetDCM, minReach, maxReach): radius = np.linalg.norm(pos_N) planetPos_E = refPlanetDCM.dot(pos_N) - rHat_E = planetPos_E/radius + rHat_E = planetPos_E / radius - magField_E = (refPlanetRadius/radius)**3 * (3*rHat_E*np.dot(rHat_E, X)-X) + magField_E = (refPlanetRadius / radius) ** 3 * ( + 3 * rHat_E * np.dot(rHat_E, X) - X + ) - magField_N = [((refPlanetDCM.transpose()).dot(magField_E)).tolist()]*3 + magField_N = [((refPlanetDCM.transpose()).dot(magField_E)).tolist()] * 3 if radius < minReach: - magField_N = [[0.0, 0.0, 0.0]]*3 + magField_N = [[0.0, 0.0, 0.0]] * 3 if radius > maxReach and maxReach > 0: - magField_N = [[0.0, 0.0, 0.0]]*3 + magField_N = [[0.0, 0.0, 0.0]] * 3 return magField_N - # compare the module results to the truth values accuracy = 1e-5 unitTestSupport.writeTeXSnippet("unitTestToleranceValue", str(accuracy), path) - # check the exponential atmosphere results # # check spacecraft 0 neutral density results if len(mag0Data) > 0: - trueMagField = centeredDipole(r0N, np.array([refg11, refh11, refg10]), refPlanetRadius, refPlanetDCM, minReach, maxReach) + trueMagField = centeredDipole( + r0N, + np.array([refg11, refh11, refg10]), + refPlanetRadius, + refPlanetDCM, + minReach, + maxReach, + ) testFailCount, testMessages = unitTestSupport.compareArrayRelative( - trueMagField, mag0Data, accuracy, "SC0 mag vector", - testFailCount, testMessages) + trueMagField, + mag0Data, + accuracy, + "SC0 mag vector", + testFailCount, + testMessages, + ) if len(mag1Data) > 0: - trueMagField = centeredDipole(r1N, np.array([refg11, refh11, refg10]), refPlanetRadius, refPlanetDCM, minReach, maxReach) + trueMagField = centeredDipole( + r1N, + np.array([refg11, refh11, refg10]), + refPlanetRadius, + refPlanetDCM, + minReach, + maxReach, + ) testFailCount, testMessages = unitTestSupport.compareArrayRelative( - trueMagField, mag1Data, accuracy, "SC1 mag vector", - testFailCount, testMessages) - + trueMagField, + mag1Data, + accuracy, + "SC1 mag vector", + testFailCount, + testMessages, + ) # print out success or failure message - snippentName = "unitTestPassFail" + str(useDefault) + str(useMinReach) + str(useMaxReach) + str(usePlanetEphemeris) + snippentName = ( + "unitTestPassFail" + + str(useDefault) + + str(useMinReach) + + str(useMaxReach) + + str(usePlanetEphemeris) + ) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + testModule.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + testModule.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -232,10 +264,10 @@ def centeredDipole(pos_N, X, refPlanetRadius, refPlanetDCM, minReach, maxReach): # stand-along python script # if __name__ == "__main__": - test_module( # update "module" in function name - False, # showplots - False, # useDefault - False, # useMinReach - False, # useMaxReach - True # usePlanetEphemeris - ) + test_module( # update "module" in function name + False, # showplots + False, # useDefault + False, # useMinReach + False, # useMaxReach + True, # usePlanetEphemeris + ) diff --git a/src/simulation/environment/magneticFieldWMM/_UnitTest/test_magneticFieldWMM.py b/src/simulation/environment/magneticFieldWMM/_UnitTest/test_magneticFieldWMM.py index fcc2f8eb35..90a0857f39 100755 --- a/src/simulation/environment/magneticFieldWMM/_UnitTest/test_magneticFieldWMM.py +++ b/src/simulation/environment/magneticFieldWMM/_UnitTest/test_magneticFieldWMM.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -35,11 +34,13 @@ from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskPath = path.split('src')[0] +bskPath = path.split("src")[0] # Uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed. @@ -49,58 +50,101 @@ # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("decimalYear, Height, Lat, Lon, BxTrue, ByTrue, BzTrue", [ - (2020, 0, 80, 0, 6414.5, -153.0, 54169.0) - , (2020, 0, 0, 120, 39624.3, 109.9, -10932.5) - , (2020, 0, -80, 240, 5801.8, 15571.1, -51959.6) - , (2020, 100, 80, 0, 6114.2, -191.5, 52011.7) - , (2020, 100, 0, 120, 37636.7, 104.9, -10474.8) - , (2020, 100, -80, 240, 5613.9, 14614.0, -49483.9) - , (2022.5, 0, 80, 0, 6374.2, -6.9, 54274.1) - , (2022.5, 0, 0, 120, 39684.7, -42.2, -10809.5) - , (2022.5, 0, -80, 240, 5877.0, 15575.7, -51734.1) - , (2022.5, 100, 80, 0, 6076.7, -51.7, 52107.6) - , (2022.5, 100, 0, 120, 37694.0, -35.3, -10362.0) - , (2022.5, 100, -80, 240, 5683.3, 14617.4, -49273.2) -]) -@pytest.mark.parametrize("useDefault, useMsg", [ - (False, False) - , (False, True) - , (True, True) -]) +@pytest.mark.parametrize( + "decimalYear, Height, Lat, Lon, BxTrue, ByTrue, BzTrue", + [ + (2020, 0, 80, 0, 6414.5, -153.0, 54169.0), + (2020, 0, 0, 120, 39624.3, 109.9, -10932.5), + (2020, 0, -80, 240, 5801.8, 15571.1, -51959.6), + (2020, 100, 80, 0, 6114.2, -191.5, 52011.7), + (2020, 100, 0, 120, 37636.7, 104.9, -10474.8), + (2020, 100, -80, 240, 5613.9, 14614.0, -49483.9), + (2022.5, 0, 80, 0, 6374.2, -6.9, 54274.1), + (2022.5, 0, 0, 120, 39684.7, -42.2, -10809.5), + (2022.5, 0, -80, 240, 5877.0, 15575.7, -51734.1), + (2022.5, 100, 80, 0, 6076.7, -51.7, 52107.6), + (2022.5, 100, 0, 120, 37694.0, -35.3, -10362.0), + (2022.5, 100, -80, 240, 5683.3, 14617.4, -49273.2), + ], +) +@pytest.mark.parametrize( + "useDefault, useMsg", [(False, False), (False, True), (True, True)] +) @pytest.mark.parametrize("useMinReach", [True, False]) @pytest.mark.parametrize("useMaxReach", [True, False]) @pytest.mark.parametrize("usePlanetEphemeris", [True, False]) @pytest.mark.parametrize("accuracy", [0.1]) # update "module" in this function name to reflect the module name -def test_module(show_plots, decimalYear, Height, Lat, Lon, BxTrue, ByTrue, BzTrue, - useDefault, useMsg, useMinReach, useMaxReach, usePlanetEphemeris, accuracy): +def test_module( + show_plots, + decimalYear, + Height, + Lat, + Lon, + BxTrue, + ByTrue, + BzTrue, + useDefault, + useMsg, + useMinReach, + useMaxReach, + usePlanetEphemeris, + accuracy, +): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = run(show_plots, decimalYear, Height, Lat, Lon, BxTrue, ByTrue, BzTrue, - useDefault, useMsg, useMinReach, useMaxReach, usePlanetEphemeris, accuracy) + [testResults, testMessage] = run( + show_plots, + decimalYear, + Height, + Lat, + Lon, + BxTrue, + ByTrue, + BzTrue, + useDefault, + useMsg, + useMinReach, + useMaxReach, + usePlanetEphemeris, + accuracy, + ) assert testResults < 1, testMessage -def run(show_plots, decimalYear, Height, Lat, Lon, BxTrue, ByTrue, BzTrue, useDefault, useMsg, useMinReach, - useMaxReach, usePlanetEphemeris, accuracy): - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) +def run( + show_plots, + decimalYear, + Height, + Lat, + Lon, + BxTrue, + ByTrue, + BzTrue, + useDefault, + useMsg, + useMinReach, + useMaxReach, + usePlanetEphemeris, + accuracy, +): + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container testModule = magneticFieldWMM.MagneticFieldWMM() testModule.ModelTag = "WMM" - testModule.dataPath = bskPath + '/supportData/MagneticField/' + testModule.dataPath = bskPath + "/supportData/MagneticField/" if not useDefault: testModule.epochDateFractionalYear = decimalYear @@ -122,11 +166,11 @@ def run(show_plots, decimalYear, Height, Lat, Lon, BxTrue, ByTrue, BzTrue, useDe minReach = -1.0 if useMinReach: - minReach = (orbitalMotion.REQ_EARTH+200.)*1000.0 # meters + minReach = (orbitalMotion.REQ_EARTH + 200.0) * 1000.0 # meters testModule.envMinReach = minReach maxReach = -1.0 if useMaxReach: - maxReach = (orbitalMotion.REQ_EARTH-200.)*1000.0 # meters + maxReach = (orbitalMotion.REQ_EARTH - 200.0) * 1000.0 # meters testModule.envMaxReach = maxReach planetPosition = np.array([0.0, 0.0, 0.0]) refPlanetDCM = np.array(((1, 0, 0), (0, 1, 0), (0, 0, 1))) @@ -151,15 +195,22 @@ def run(show_plots, decimalYear, Height, Lat, Lon, BxTrue, ByTrue, BzTrue, useDe r0 = (orbitalMotion.REQ_EARTH + Height) * 1000.0 # meters phi = Lat * macros.D2R long = Lon * macros.D2R - r0P = np.array([np.cos(phi)*np.cos(long),np.cos(phi)*np.sin(long),np.sin(phi)])*r0 - r0N = np.dot(refPlanetDCM.transpose(),r0P) + r0P = ( + np.array([np.cos(phi) * np.cos(long), np.cos(phi) * np.sin(long), np.sin(phi)]) + * r0 + ) + r0N = np.dot(refPlanetDCM.transpose(), r0P) # create the input messages - sc0StateMsgData = messaging.SCStatesMsgPayload() # Create a structure for the input message + sc0StateMsgData = ( + messaging.SCStatesMsgPayload() + ) # Create a structure for the input message sc0StateMsgData.r_BN_N = np.array(r0N) + np.array(planetPosition) sc0StateMsg.write(sc0StateMsgData) - sc1StateMsgData = messaging.SCStatesMsgPayload() # Create a structure for the input message + sc1StateMsgData = ( + messaging.SCStatesMsgPayload() + ) # Create a structure for the input message sc1StateMsgData.r_BN_N = np.array(r0N) + np.array(planetPosition) sc1StateMsg.write(sc1StateMsgData) @@ -175,15 +226,15 @@ def run(show_plots, decimalYear, Height, Lat, Lon, BxTrue, ByTrue, BzTrue, useDe unitTestSim.TotalSim.SingleStepProcesses() # This pulls the actual data log from the simulation run and converts to nano-Tesla - mag0Data = dataLog0.magField_N*1e9 - mag1Data = dataLog1.magField_N*1e9 + mag0Data = dataLog0.magField_N * 1e9 + mag1Data = dataLog1.magField_N * 1e9 def wmmInertial(pos_N, Bx, By, Bz, phi, long, refPlanetDCM, minReach, maxReach): radius = np.linalg.norm(pos_N) B_M = np.array([Bx, By, Bz]) - M2 = rbk.euler2(phi + np.pi/2.0) + M2 = rbk.euler2(phi + np.pi / 2.0) M3 = rbk.euler3(-long) - PM = np.dot(M3,M2) + PM = np.dot(M3, M2) NM = np.dot(refPlanetDCM.transpose(), PM) magField_N = [np.dot(NM, B_M).tolist()] @@ -200,29 +251,47 @@ def wmmInertial(pos_N, Bx, By, Bz, phi, long, refPlanetDCM, minReach, maxReach): # # check spacecraft 0 neutral density results if len(mag0Data) > 0: - trueMagField = wmmInertial(r0N, BxTrue, ByTrue, BzTrue, phi, long, refPlanetDCM, minReach, maxReach) + trueMagField = wmmInertial( + r0N, BxTrue, ByTrue, BzTrue, phi, long, refPlanetDCM, minReach, maxReach + ) testFailCount, testMessages = unitTestSupport.compareArray( - trueMagField, mag0Data, accuracy, "SC0 mag vector", - testFailCount, testMessages) + trueMagField, + mag0Data, + accuracy, + "SC0 mag vector", + testFailCount, + testMessages, + ) if len(mag1Data) > 0: testFailCount, testMessages = unitTestSupport.compareArrayRelative( - trueMagField, mag1Data, accuracy, "SC1 mag vector", - testFailCount, testMessages) + trueMagField, + mag1Data, + accuracy, + "SC1 mag vector", + testFailCount, + testMessages, + ) # print out success or failure message - snippentName = "unitTestPassFail" + str(useDefault) + str(useMinReach) + str(useMaxReach) + str(usePlanetEphemeris) + snippentName = ( + "unitTestPassFail" + + str(useDefault) + + str(useMinReach) + + str(useMaxReach) + + str(usePlanetEphemeris) + ) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + testModule.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + testModule.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -230,19 +299,19 @@ def wmmInertial(pos_N, Bx, By, Bz, phi, long, refPlanetDCM, minReach, maxReach): # stand-along python script # if __name__ == "__main__": - test_module( # update "module" in function name - False, # showplots - 2020, # decimalYear - 0, # Height (km) - 80, # latitude (deg) - 0, # longitude (deg) - 6570.4, # BxTrue (nT) - -146.3, # ByTrue (nT) - 54606.0, # BzTrue (nT) - True, # useDefault - False, # useMsg - False, # useMinReach - False, # useMaxReach - False, # usePlanetEphemeris - 0.1 # accuracy - ) + test_module( # update "module" in function name + False, # showplots + 2020, # decimalYear + 0, # Height (km) + 80, # latitude (deg) + 0, # longitude (deg) + 6570.4, # BxTrue (nT) + -146.3, # ByTrue (nT) + 54606.0, # BzTrue (nT) + True, # useDefault + False, # useMsg + False, # useMinReach + False, # useMaxReach + False, # usePlanetEphemeris + 0.1, # accuracy + ) diff --git a/src/simulation/environment/planetEphemeris/_UnitTest/test_planetEphemeris.py b/src/simulation/environment/planetEphemeris/_UnitTest/test_planetEphemeris.py index 729b91abd8..1cc03ffdb9 100755 --- a/src/simulation/environment/planetEphemeris/_UnitTest/test_planetEphemeris.py +++ b/src/simulation/environment/planetEphemeris/_UnitTest/test_planetEphemeris.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -31,14 +30,16 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) # Import all of the modules that we are going to be called in this simulation from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import orbitalMotion from Basilisk.utilities import RigidBodyKinematics as rbk -from Basilisk.utilities import unitTestSupport # general support file with common unit test functions +from Basilisk.utilities import ( + unitTestSupport, +) # general support file with common unit test functions from Basilisk.simulation import planetEphemeris from Basilisk.utilities import macros from Basilisk.architecture import bskLogging @@ -52,42 +53,44 @@ # Provide a unique test method name, starting with 'test_'. # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("setRAN", [ True, False]) -@pytest.mark.parametrize("setDEC", [ True, False]) -@pytest.mark.parametrize("setLST", [ True, False]) -@pytest.mark.parametrize("setRate", [ True, False]) - +@pytest.mark.parametrize("setRAN", [True, False]) +@pytest.mark.parametrize("setDEC", [True, False]) +@pytest.mark.parametrize("setLST", [True, False]) +@pytest.mark.parametrize("setRate", [True, False]) # update "module" in this function name to reflect the module name def test_module(show_plots, setRAN, setDEC, setLST, setRate): """Module Unit Test""" # each test method requires a single assert method to be called - expect_error = any([setRAN, setDEC, setLST, setRate]) and not all([setRAN, setDEC, setLST, setRate]) + expect_error = any([setRAN, setDEC, setLST, setRate]) and not all( + [setRAN, setDEC, setLST, setRate] + ) with pytest.raises(BasiliskError) if expect_error else nullcontext(): - [testResults, testMessage] = planetEphemerisTest(show_plots, setRAN, setDEC, setLST, setRate) + [testResults, testMessage] = planetEphemerisTest( + show_plots, setRAN, setDEC, setLST, setRate + ) assert testResults < 1, testMessage def planetEphemerisTest(show_plots, setRAN, setDEC, setLST, setRate): bskLogging.setDefaultLogLevel(bskLogging.BSK_SILENT) - testFailCount = 0 # zero unit test result counter - testMessages = [] # create empty array to store test log messages - unitTaskName = "unitTask" # arbitrary name (don't change) - unitProcessName = "TestProcess" # arbitrary name (don't change) - + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array 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.5) # update process rate update time + testProcessRate = macros.sec2nano(0.5) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # Construct algorithm and associated C++ container module = planetEphemeris.PlanetEphemeris() - module.ModelTag = 'planetEphemeris' + module.ModelTag = "planetEphemeris" # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, module) @@ -98,44 +101,44 @@ def planetEphemerisTest(show_plots, setRAN, setDEC, setLST, setRate): # set gravitational constant of the sun - mu = orbitalMotion.MU_SUN*1000.*1000.*1000 # m^3/s^2 + mu = orbitalMotion.MU_SUN * 1000.0 * 1000.0 * 1000 # m^3/s^2 # setup planet ephemeris states oeEarth = planetEphemeris.ClassicElements() - oeEarth.a = orbitalMotion.SMA_EARTH*1000 # meters + oeEarth.a = orbitalMotion.SMA_EARTH * 1000 # meters oeEarth.e = 0.001 - oeEarth.i = 10.0*macros.D2R - oeEarth.Omega = 30.0*macros.D2R - oeEarth.omega = 20.0*macros.D2R - oeEarth.f = 90.0*macros.D2R + oeEarth.i = 10.0 * macros.D2R + oeEarth.Omega = 30.0 * macros.D2R + oeEarth.omega = 20.0 * macros.D2R + oeEarth.f = 90.0 * macros.D2R oeVenus = planetEphemeris.ClassicElements() - oeVenus.a = orbitalMotion.SMA_VENUS*1000 # meters + oeVenus.a = orbitalMotion.SMA_VENUS * 1000 # meters oeVenus.e = 0.001 - oeVenus.i = 5.0*macros.D2R - oeVenus.Omega = 110.0*macros.D2R - oeVenus.omega = 220.0*macros.D2R - oeVenus.f = 180.0*macros.D2R + oeVenus.i = 5.0 * macros.D2R + oeVenus.Omega = 110.0 * macros.D2R + oeVenus.omega = 220.0 * macros.D2R + oeVenus.f = 180.0 * macros.D2R module.planetElements = planetEphemeris.classicElementVector([oeEarth, oeVenus]) evalAttitude = 1 if setRAN: # setup planet local right ascension angle at epoch - RANlist = [0.*macros.D2R, 272.76*macros.D2R] + RANlist = [0.0 * macros.D2R, 272.76 * macros.D2R] module.rightAscension = planetEphemeris.DoubleVector(RANlist) else: evalAttitude = 0 if setDEC: # setup planet local declination angle at epoch - DEClist = [90.*macros.D2R, 67.16*macros.D2R] + DEClist = [90.0 * macros.D2R, 67.16 * macros.D2R] module.declination = planetEphemeris.DoubleVector(DEClist) else: evalAttitude = 0 if setLST: # setup planet local sidereal time at epoch - lstList = [10.*macros.D2R, 30.*macros.D2R] + lstList = [10.0 * macros.D2R, 30.0 * macros.D2R] module.lst0 = planetEphemeris.DoubleVector(lstList) else: evalAttitude = 0 @@ -160,7 +163,7 @@ def planetEphemerisTest(show_plots, setRAN, setDEC, setLST, setRate): # NOTE: the total simulation time may be longer than this value. The # simulation is stopped at the next logging event on or after the # simulation end time. - unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation + unitTestSim.ConfigureStopTime(macros.sec2nano(1.0)) # seconds to stop simulation # Begin the simulation time run set above unitTestSim.ExecuteSimulation() @@ -185,12 +188,20 @@ def planetEphemerisTest(show_plots, setRAN, setDEC, setLST, setRate): if planet != FinalPlanetMessage.PlanetName: testFailCount += 1 - testMessages.append("FAILED: planetEphemeris() didn't set the desired plane name " + planet) + testMessages.append( + "FAILED: planetEphemeris() didn't set the desired plane name " + planet + ) # check that the time information is correct timeTrue = [0.0, 0.5, 1.0] testFailCount, testMessages = unitTestSupport.compareDoubleArray( - timeTrue, J2000Current, accuracy, "J2000Current", testFailCount, testMessages) + timeTrue, + J2000Current, + accuracy, + "J2000Current", + testFailCount, + testMessages, + ) # check that the position and velocity vectors are correct if planet == "earth": @@ -203,19 +214,29 @@ def planetEphemerisTest(show_plots, setRAN, setDEC, setLST, setRate): rTrue = [] vTrue = [] for time in timeTrue: - Mt = M0 + np.sqrt(mu/oe.a/oe.a/oe.a)*time + Mt = M0 + np.sqrt(mu / oe.a / oe.a / oe.a) * time Et = orbitalMotion.M2E(Mt, oe.e) oe.f = orbitalMotion.E2f(Et, oe.e) rv, vv = orbitalMotion.elem2rv(mu, oe) rTrue.append(rv) vTrue.append(vv) - testFailCount, testMessages = unitTestSupport.compareArray(rTrue, PositionVector, - accuracy, "Position Vector", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArray(vTrue, VelocityVector, - accuracy, "Velocity Vector", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareArray( + rTrue, + PositionVector, + accuracy, + "Position Vector", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareArray( + vTrue, + VelocityVector, + accuracy, + "Velocity Vector", + testFailCount, + testMessages, + ) # check if the planet DCM and DCM rate is correct dcmTrue = [] @@ -227,45 +248,57 @@ def planetEphemerisTest(show_plots, setRAN, setDEC, setLST, setRate): omega_NP_P = np.array([0.0, 0.0, -omegaList[c]]) tilde = rbk.v3Tilde(omega_NP_P) for time in timeTrue: - lst = lst0 + omegaList[c]*time - DCM = rbk.euler3232C([RAN, np.pi/2.0 - DEC, lst]) + lst = lst0 + omegaList[c] * time + DCM = rbk.euler3232C([RAN, np.pi / 2.0 - DEC, lst]) dcmTrue.append(DCM) dDCMdt = np.matmul(tilde, DCM) dcmRateTrue.append(dDCMdt) else: for time in timeTrue: dcmTrue.append(np.identity(3)) - dcmRateTrue.append([0.0]*9) - - testFailCount, testMessages = unitTestSupport.compareArrayND(dcmTrue, J20002Pfix, - accuracy, "DCM", 9, - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareArrayND(dcmRateTrue, J20002Pfix_dot, - 1e-10, "DCM Rate", 9, - testFailCount, testMessages) + dcmRateTrue.append([0.0] * 9) + + testFailCount, testMessages = unitTestSupport.compareArrayND( + dcmTrue, J20002Pfix, accuracy, "DCM", 9, testFailCount, testMessages + ) + testFailCount, testMessages = unitTestSupport.compareArrayND( + dcmRateTrue, + J20002Pfix_dot, + 1e-10, + "DCM Rate", + 9, + testFailCount, + testMessages, + ) # check if the orientation evaluation flag is set correctly flagTrue = [evalAttitude] * 3 testFailCount, testMessages = unitTestSupport.compareDoubleArray( - flagTrue, computeOrient, accuracy, "computeOrient", testFailCount, testMessages) + flagTrue, + computeOrient, + accuracy, + "computeOrient", + testFailCount, + testMessages, + ) - c = c+1 + c = c + 1 # print out success message if no error were found snippentName = "passFail" + str(setRAN) + str(setDEC) + str(setLST) + str(setRate) if testFailCount == 0: - colorText = 'ForestGreen' + colorText = "ForestGreen" print("PASSED: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "PASSED" + "}" else: - colorText = 'Red' + colorText = "Red" print("Failed: " + module.ModelTag) - passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}' + passedText = r"\textcolor{" + colorText + "}{" + "Failed" + "}" unitTestSupport.writeTeXSnippet(snippentName, passedText, path) # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # @@ -274,9 +307,9 @@ def planetEphemerisTest(show_plots, setRAN, setDEC, setLST, setRate): # if __name__ == "__main__": test_module( - False, # show plots flag - True, # setRAN - True, # setDEC - True, # setLST - True # setRate + False, # show plots flag + True, # setRAN + True, # setDEC + True, # setLST + True, # setRate ) diff --git a/src/simulation/environment/solarFlux/_UnitTest/test_solarFlux.py b/src/simulation/environment/solarFlux/_UnitTest/test_solarFlux.py index 1795657b7e..7516fbe7a2 100755 --- a/src/simulation/environment/solarFlux/_UnitTest/test_solarFlux.py +++ b/src/simulation/environment/solarFlux/_UnitTest/test_solarFlux.py @@ -25,7 +25,10 @@ from Basilisk.utilities import orbitalMotion as om -@pytest.mark.parametrize("positionFactor, shadowFactor, eclipseMsgName, relTol", [(np.sqrt(2), 0.5, "eclipse_data_0", 1e-8), (np.sqrt(2), 0.5, "", 1e-8)]) +@pytest.mark.parametrize( + "positionFactor, shadowFactor, eclipseMsgName, relTol", + [(np.sqrt(2), 0.5, "eclipse_data_0", 1e-8), (np.sqrt(2), 0.5, "", 1e-8)], +) def test_solarFlux(show_plots, positionFactor, shadowFactor, eclipseMsgName, relTol): """ **Test Description** @@ -50,11 +53,11 @@ def test_solarFlux(show_plots, positionFactor, shadowFactor, eclipseMsgName, rel proc.addTask(task) sunPositionMessage = messaging.SpicePlanetStateMsgPayload() - sunPositionMessage.PositionVector = [0., 0., 0.] + sunPositionMessage.PositionVector = [0.0, 0.0, 0.0] sunMsg = messaging.SpicePlanetStateMsg().write(sunPositionMessage) scPositionMessage = messaging.SCStatesMsgPayload() - scPositionMessage.r_BN_N = [0., 0., om.AU*1000] + scPositionMessage.r_BN_N = [0.0, 0.0, om.AU * 1000] scMsg = messaging.SCStatesMsg().write(scPositionMessage) eclipseMessage = messaging.EclipseMsgPayload() @@ -74,13 +77,15 @@ def test_solarFlux(show_plots, positionFactor, shadowFactor, eclipseMsgName, rel sim.TotalSim.SingleStepProcesses() fluxOutEarth = dataLog.flux - scPositionMessage.r_BN_N = [0., 0., positionFactor * om.AU*1000] + scPositionMessage.r_BN_N = [0.0, 0.0, positionFactor * om.AU * 1000] scMsg.write(scPositionMessage) sim.TotalSim.SingleStepProcesses() fluxOutFurther = dataLog.flux - assert fluxOutFurther[1] == pytest.approx(fluxOutEarth[0] / shadowFactor / (positionFactor**2) * shadowFactor, rel=relTol) + assert fluxOutFurther[1] == pytest.approx( + fluxOutEarth[0] / shadowFactor / (positionFactor**2) * shadowFactor, rel=relTol + ) if __name__ == "__main__": diff --git a/src/simulation/environment/spacecraftLocation/_UnitTest/test_unitSpacecraftLocation.py b/src/simulation/environment/spacecraftLocation/_UnitTest/test_unitSpacecraftLocation.py index 477fbdfac8..aef3f86de9 100644 --- a/src/simulation/environment/spacecraftLocation/_UnitTest/test_unitSpacecraftLocation.py +++ b/src/simulation/environment/spacecraftLocation/_UnitTest/test_unitSpacecraftLocation.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016-2017, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -30,15 +29,18 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) -bskName = 'Basilisk' +bskName = "Basilisk" splitPath = path.split(bskName) + @pytest.mark.parametrize("defaultPolarRadius", [False, True]) @pytest.mark.parametrize("defaultPlanet", [False, True]) @pytest.mark.parametrize("latitude", [55, 65, 115]) -@pytest.mark.parametrize("maxRange", [-1, 3000.*1000]) +@pytest.mark.parametrize("maxRange", [-1, 3000.0 * 1000]) @pytest.mark.parametrize("cone", [0, 1, -1]) -def test_spacecraftLocation(show_plots, defaultPolarRadius, defaultPlanet, latitude, maxRange, cone): +def test_spacecraftLocation( + show_plots, defaultPolarRadius, defaultPlanet, latitude, maxRange, cone +): """ Tests whether spacecraftLocation: @@ -51,12 +53,13 @@ def test_spacecraftLocation(show_plots, defaultPolarRadius, defaultPlanet, latit """ # each test method requires a single assert method to be called - [testResults, testMessage] = run(show_plots, defaultPolarRadius, defaultPlanet, latitude, maxRange, cone) + [testResults, testMessage] = run( + show_plots, defaultPolarRadius, defaultPlanet, latitude, maxRange, cone + ) assert testResults < 1, testMessage def run(showplots, defaultPolarRadius, defaultPlanet, latitude, maxRange, cone): - testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -64,20 +67,20 @@ def run(showplots, defaultPolarRadius, defaultPlanet, latitude, maxRange, cone): simProcessName = "simProcess" scSim = SimulationBaseClass.SimBaseClass() dynProcess = scSim.CreateNewProcess(simProcessName) - simulationTimeStep = macros.sec2nano(1.) + simulationTimeStep = macros.sec2nano(1.0) dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) # Initialize new atmosphere and drag model, add them to task module = spacecraftLocation.SpacecraftLocation() module.ModelTag = "scLocation" - module.rEquator = orbitalMotion.REQ_EARTH * 1000. + module.rEquator = orbitalMotion.REQ_EARTH * 1000.0 if not defaultPolarRadius: - module.rPolar = orbitalMotion.REQ_EARTH * 1000. * 0.5 + module.rPolar = orbitalMotion.REQ_EARTH * 1000.0 * 0.5 if maxRange: module.maximumRange = maxRange if cone != 0: module.aHat_B = [0, 0, cone] - module.theta = 80. * macros.D2R + module.theta = 80.0 * macros.D2R scSim.AddModelToTask(simTaskName, module) @@ -85,16 +88,16 @@ def run(showplots, defaultPolarRadius, defaultPlanet, latitude, maxRange, cone): planetPos = np.array([0.0, 0.0, 0.0]) if not defaultPlanet: planet_message = messaging.SpicePlanetStateMsgPayload() - planet_message.J20002Pfix = rbk.euler3(np.radians(-90.)).tolist() + planet_message.J20002Pfix = rbk.euler3(np.radians(-90.0)).tolist() planetPos = np.array([orbitalMotion.AU * 1000, 0.0, 0.0]) planet_message.PositionVector = planetPos planetMsg = messaging.SpicePlanetStateMsg().write(planet_message) module.planetInMsg.subscribeTo(planetMsg) # create primary spacecraft state message - alt = 1000. * 1000 # meters + alt = 1000.0 * 1000 # meters scMsgData = messaging.SCStatesMsgPayload() - r = orbitalMotion.REQ_EARTH * 1000. + alt + r = orbitalMotion.REQ_EARTH * 1000.0 + alt scMsgData.r_BN_N = planetPos + np.array([r, 0.0, 0.0]) scMsgData.sigma_BN = [1.0, 0.0, 0.0] scMsg = messaging.SCStatesMsg().write(scMsgData) @@ -102,7 +105,9 @@ def run(showplots, defaultPolarRadius, defaultPlanet, latitude, maxRange, cone): sc1MsgData = messaging.SCStatesMsgPayload() angle = np.radians(latitude) - sc1MsgData.r_BN_N = planetPos + np.array([r * np.cos(angle), 0.0, r * np.sin(angle)]) + sc1MsgData.r_BN_N = planetPos + np.array( + [r * np.cos(angle), 0.0, r * np.sin(angle)] + ) sc1Msg = messaging.SCStatesMsg().write(sc1MsgData) module.addSpacecraftToModel(sc1Msg) @@ -112,7 +117,9 @@ def run(showplots, defaultPolarRadius, defaultPlanet, latitude, maxRange, cone): # check polar planet radius default behavior if defaultPolarRadius and module.rPolar < 0: testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed default polar radius check.") + testMessages.append( + "FAILED: " + module.ModelTag + " Module failed default polar radius check." + ) scSim.TotalSim.SingleStepProcesses() @@ -128,34 +135,49 @@ def run(showplots, defaultPolarRadius, defaultPlanet, latitude, maxRange, cone): if accessMsg.hasAccess != trueAccess: testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed access test.") + testMessages.append( + "FAILED: " + module.ModelTag + " Module failed access test." + ) if accessMsg.slantRange <= 1e-6: testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed positive slant range test.") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed positive slant range test." + ) if (latitude == 65 and defaultPolarRadius) or latitude == 115: # should not have access if accessMsg.hasAccess != 0: testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed negative have access test.") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed negative have access test." + ) if np.abs(accessMsg.slantRange) > 1e-6: testFailCount += 1 - testMessages.append("FAILED: " + module.ModelTag + " Module failed negative slant range test.") + testMessages.append( + "FAILED: " + + module.ModelTag + + " Module failed negative slant range test." + ) if testFailCount == 0: print("PASSED: " + module.ModelTag) else: print(testMessages) - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] -if __name__ == '__main__': - run(False - , False # defaultPolarRadius - , True # defaultPlanet - , 55 # true latitude angle (deg) - , -7000.*1000 # max range - , 1 # cone case, 0-> no cone, 1 -> [0, 1, 0], -1 -> [0, -1, 0] - ) +if __name__ == "__main__": + run( + False, + False, # defaultPolarRadius + True, # defaultPlanet + 55, # true latitude angle (deg) + -7000.0 * 1000, # max range + 1, # cone case, 0-> no cone, 1 -> [0, 1, 0], -1 -> [0, -1, 0] + ) diff --git a/src/simulation/environment/spiceInterface/_UnitTest/test_unitSpice.py b/src/simulation/environment/spiceInterface/_UnitTest/test_unitSpice.py index 6c7b2315b5..73dde7e30d 100755 --- a/src/simulation/environment/spiceInterface/_UnitTest/test_unitSpice.py +++ b/src/simulation/environment/spiceInterface/_UnitTest/test_unitSpice.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -34,6 +33,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) from Basilisk import __path__ + bskPath = __path__[0] @@ -57,34 +57,34 @@ def __init__(self): def plotData(self): fig1 = plt.figure(1) rect = fig1.patch - rect.set_facecolor('white') + rect.set_facecolor("white") plt.xticks(numpy.arange(len(self.Date)), self.Date) - plt.plot(self.MarsPosErr, 'r', label='Mars') - plt.plot(self.EarthPosErr, 'bs', label='Earth') - plt.plot(self.SunPosErr, 'yo', label='Sun') + plt.plot(self.MarsPosErr, "r", label="Mars") + plt.plot(self.EarthPosErr, "bs", label="Earth") + plt.plot(self.SunPosErr, "yo", label="Sun") - plt.rc('font', size=50) - plt.legend(loc='upper left') + plt.rc("font", size=50) + plt.legend(loc="upper left") fig1.autofmt_xdate() - plt.xlabel('Date of test') - plt.ylabel('Position Error [m]') + plt.xlabel("Date of test") + plt.ylabel("Position Error [m]") plt.show() def giveData(self): plt.figure(1) plt.close(1) - fig1 = plt.figure(1, figsize=(7, 5), dpi=80, facecolor='w', edgecolor='k') + fig1 = plt.figure(1, figsize=(7, 5), dpi=80, facecolor="w", edgecolor="k") plt.xticks(numpy.arange(len(self.Date)), self.Date) - plt.plot(self.MarsPosErr, 'r', label='Mars') - plt.plot(self.EarthPosErr, 'b', label='Earth') - plt.plot(self.SunPosErr, 'y', label='Sun') + plt.plot(self.MarsPosErr, "r", label="Mars") + plt.plot(self.EarthPosErr, "b", label="Earth") + plt.plot(self.SunPosErr, "y", label="Sun") - plt.legend(loc='upper left') + plt.legend(loc="upper left") fig1.autofmt_xdate() - plt.xlabel('Date of test') - plt.ylabel('Position Error [m]') + plt.xlabel("Date of test") + plt.ylabel("Position Error [m]") return plt @@ -96,11 +96,29 @@ def testPlottingFixture(show_plots): yield dataStore plt = dataStore.giveData() - unitTestSupport.writeFigureLaTeX('EphemMars', 'Ephemeris Error on Mars', plt, 'height=0.7\\textwidth, keepaspectratio', path) - plt.ylim(0, 2E-2) - unitTestSupport.writeFigureLaTeX('EphemEarth', 'Ephemeris Error on Earth', plt, 'height=0.7\\textwidth, keepaspectratio', path) - plt.ylim(0, 5E-6) - unitTestSupport.writeFigureLaTeX('EphemSun', 'Ephemeris Error on Sun', plt, 'height=0.7\\textwidth, keepaspectratio', path) + unitTestSupport.writeFigureLaTeX( + "EphemMars", + "Ephemeris Error on Mars", + plt, + "height=0.7\\textwidth, keepaspectratio", + path, + ) + plt.ylim(0, 2e-2) + unitTestSupport.writeFigureLaTeX( + "EphemEarth", + "Ephemeris Error on Earth", + plt, + "height=0.7\\textwidth, keepaspectratio", + path, + ) + plt.ylim(0, 5e-6) + unitTestSupport.writeFigureLaTeX( + "EphemSun", + "Ephemeris Error on Sun", + plt, + "height=0.7\\textwidth, keepaspectratio", + path, + ) if show_plots: dataStore.plotData() @@ -110,49 +128,244 @@ def testPlottingFixture(show_plots): # uncomment this line if this test has an expected failure, adjust message as needed # @pytest.mark.xfail(True) + # The following 'parametrize' function decorator provides the parameters and expected results for each # of the multiple test runs for this test. -@pytest.mark.parametrize("DateSpice, DatePlot , MarsTruthPos , EarthTruthPos, SunTruthPos, useMsg", [ - ("2015 February 10, 00:00:00.0 TDB", "02/10/15" ,[2.049283795042291E+08, 4.654550957513031E+07, 1.580778617009296E+07] ,[-1.137790671899544E+08, 8.569008401822130E+07, 3.712507705247846E+07],[4.480338216752146E+05, -7.947764237588293E+04, -5.745748832696378E+04], False), - ("2015 February 20, 00:00:00.0 TDB", "02/20/15" ,[ 1.997780190793433E+08, 6.636509140613769E+07, 2.503734390917690E+07], [-1.286636391244711E+08, 6.611156946652703E+07, 2.863736192793162E+07],[4.535258225415821E+05, -7.180260790174162E+04, -5.428151563919459E+04], False), - ("2015 April 10, 00:00:00.0 TDB", "04/10/15" ,[1.468463828343225E+08, 1.502909016357645E+08, 6.495986693265321E+07],[-1.406808744055921E+08, -4.614996219219251E+07, -2.003047222725225E+07],[4.786772771370058E+05, -3.283487082146838E+04, -3.809690999538498E+04], False), - ("2015 April 20, 00:00:00.0 TDB", "04/20/15" ,[1.313859463489376E+08, 1.637422864185919E+08, 7.154683454681394E+07], [-1.304372112275012E+08, -6.769916175223185E+07, -2.937179538983412E+07],[4.834188130324496E+05, -2.461799304268214E+04, -3.467083541217462E+04], False), - ("2015 June 10, 00:00:00.0 TDB", "06/10/15" , [3.777078276008123E+07, 2.080046702252371E+08, 9.437503998071109E+07],[-2.946784585692780E+07, -1.365779215199542E+08, -5.923299652516938E+07],[5.052070933145228E+05, 1.837038638578682E+04, -1.665575509449521E+04], False), - ("2015 June 20, 00:00:00.0 TDB", "06/20/15" , [1.778745850655047E+07, 2.116712756672253E+08, 9.659608128589308E+07], [-4.338053580692466E+06, -1.393743714818930E+08, -6.044500073550620E+07],[5.090137386469169E+05, 2.694272566092012E+04, -1.304862690440150E+04], False), - ("2015 August 10, 00:00:00.0 TDB", "08/10/15" , [-8.302866300591002E+07, 2.053384243312354E+08, 9.641192179982041E+07],[1.111973130587749E+08, -9.507060674145325E+07, -4.123957889640842E+07],[5.263951240980130E+05, 7.113788303899391E+04, 5.601415938789949E+03], False), - ("2015 August 20, 00:00:00.0 TDB", "08/20/15" , [-1.015602120938274E+08, 1.994877113707506E+08, 9.422840996376510E+07], [1.267319535735967E+08, -7.664279872369213E+07, -3.325170492059625E+07],[5.294368386862668E+05, 7.989635630604146E+04, 9.307380417148834E+03], False), - ("2015 October 10, 00:00:00.0 TDB", "10/10/15" , [-1.828944464171771E+08, 1.498117608528323E+08, 7.363786404040858E+07],[1.440636517249971E+08, 3.827074461651713E+07, 1.656440704503509E+07],[5.433268959250135E+05, 1.251717566539142E+05, 2.849999378507032E+04], False), - ("2015 October 20, 00:00:00.0 TDB", "10/20/15" , [-1.955685835661002E+08, 1.367530082668284E+08, 6.799006120628108E+07],[1.343849818314204E+08, 6.019977403127116E+07, 2.607024615553362E+07],[5.457339294611338E+05, 1.342148834831663E+05, 3.234185290692726E+04], False), - ("2015 December 10, 00:00:00.0 TDB", "12/10/15" , [-2.392022492203017E+08, 5.847873056287902E+07, 3.326431285934674E+07],[3.277145427626925E+07, 1.320956053465003E+08, 5.723804900679157E+07],[5.559607335969181E+05, 1.813263443761486E+05, 5.242991145066972E+04], False), - ("2015 December 20, 00:00:00.0 TDB", "12/20/15" , [-2.432532635321551E+08, 4.156075241419497E+07, 2.561357000250468E+07], [6.866134677340530E+06, 1.351080593806486E+08, 5.854460725992832E+07],[5.575179227151767E+05, 1.907337298309725E+05, 5.645553733620559E+04], False), - ("2016 February 10, 00:00:00.0 TDB", "02/10/16" , [-2.385499316808322E+08, -4.818711325555268E+07, -1.567983931044450E+07],[-1.132504603146183E+08, 8.647183658089657E+07, 3.746046979137553E+07],[5.630027736619673E+05, 2.402590276126245E+05, 7.772804647512530E+04], False), - ("2016 February 20, 00:00:00.0 TDB", "02/20/16" , [-2.326189626865872E+08, -6.493600278878165E+07,-2.352250994262638E+07], [-1.282197228963156E+08, 6.695033443521750E+07, 2.899822684259482E+07],[5.635403728358555E+05, 2.498445036007692E+05, 8.185973180152237E+04], False), - ("2016 April 10, 00:00:00.0 TDB", "04/10/16" , [-1.795928090776869E+08, -1.395102817786797E+08, -5.916067235603344E+07],[-1.399773606371217E+08, -4.749126225182984E+07, -2.061331784067504E+07],[5.639699901756521E+05, 2.977755140828988E+05, 1.025609223621660E+05], False), - ("2016 April 20, 00:00:00.0 TDB", "04/20/16" , [-1.646488222290798E+08, -1.517361128528306E+08, -6.517204439842616E+07], [-1.294310199316289E+08, -6.890085351639988E+07, -2.989515576444890E+07],[5.636348418836893E+05, 3.073681895822543E+05, 1.067117713233756E+05], False), - ("2016 June 10, 00:00:00.0 TDB", "06/10/16" , [-7.085675304355654E+07, -1.933788289169757E+08, -8.680583098365544E+07],[-2.756178030784301E+07, -1.365892814324490E+08, -5.923888961370525E+07],[5.597493293268962E+05, 3.563312003229167E+05, 1.279448896916322E+05], False), - ("2016 June 20, 00:00:00.0 TDB", "06/20/16" , [-4.994076874358515E+07, -1.967336617729592E+08, -8.890950206156498E+07], [-2.413995783152328E+06, -1.390828611356292E+08, -6.032062925759617E+07],[5.585605124166313E+05, 3.659402953599973E+05, 1.321213212542782E+05], False), - ("2016 August 10, 00:00:00.0 TDB", "08/10/16" , [5.965895856067932E+07, -1.851251207911916E+08, -8.654489416392270E+07],[1.124873641861596E+08, -9.344410235679612E+07, -4.053621796412993E+07],[5.501477436262188E+05, 4.149525682073312E+05, 1.534983234437988E+05], False), - ("2016 August 20, 00:00:00.0 TDB", "08/20/16" , [8.021899957229958E+07, -1.770523551156136E+08, -8.339737954004113E+07] , [1.277516713236801E+08, -7.484361731649198E+07, -3.247232896320245E+07],[5.480148786547601E+05, 4.245199573757614E+05, 1.576874582857746E+05], False), - ("2016 October 10, 00:00:00.0 TDB", "10/10/16" , [1.674991232418756E+08, -1.090427183510760E+08, -5.456038490399034E+07],[ 1.434719792031415E+08, 4.029387436854774E+07, 1.744057129058395E+07],[5.348794087050703E+05, 4.727986075510736E+05, 1.788947974297997E+05], False), - ("2016 October 20, 00:00:00.0 TDB", "10/20/16" , [1.797014954219412E+08, -9.133803506487390E+07, -4.676932170648168E+07] , [ 1.334883617893556E+08, 6.209917895944476E+07, 2.689423661839254E+07],[5.318931290166953E+05, 4.821605725626923E+05, 1.830201949404063E+05], False), - ("2016 December 10, 00:00:00.0 TDB", "12/10/16" , [2.087370835407280E+08, 1.035903131428964E+07, -9.084001492689754E+05] ,[3.082308903715757E+07, 1.328026978502711E+08, 5.754559376449955E+07],[5.147792796720199E+05, 5.293951386498961E+05, 2.038816823549219E+05], False), - ("2016 December 20, 00:00:00.0 TDB", "12/20/16" , [2.075411186038260E+08, 3.092173198610598E+07, 8.555251204561792E+06], [4.885421269793877E+06, 1.355125217382336E+08, 5.872015978003684E+07],[5.110736843893399E+05, 5.385860060393942E+05, 2.079481168492821E+05], True) -]) - - +@pytest.mark.parametrize( + "DateSpice, DatePlot , MarsTruthPos , EarthTruthPos, SunTruthPos, useMsg", + [ + ( + "2015 February 10, 00:00:00.0 TDB", + "02/10/15", + [2.049283795042291e08, 4.654550957513031e07, 1.580778617009296e07], + [-1.137790671899544e08, 8.569008401822130e07, 3.712507705247846e07], + [4.480338216752146e05, -7.947764237588293e04, -5.745748832696378e04], + False, + ), + ( + "2015 February 20, 00:00:00.0 TDB", + "02/20/15", + [1.997780190793433e08, 6.636509140613769e07, 2.503734390917690e07], + [-1.286636391244711e08, 6.611156946652703e07, 2.863736192793162e07], + [4.535258225415821e05, -7.180260790174162e04, -5.428151563919459e04], + False, + ), + ( + "2015 April 10, 00:00:00.0 TDB", + "04/10/15", + [1.468463828343225e08, 1.502909016357645e08, 6.495986693265321e07], + [-1.406808744055921e08, -4.614996219219251e07, -2.003047222725225e07], + [4.786772771370058e05, -3.283487082146838e04, -3.809690999538498e04], + False, + ), + ( + "2015 April 20, 00:00:00.0 TDB", + "04/20/15", + [1.313859463489376e08, 1.637422864185919e08, 7.154683454681394e07], + [-1.304372112275012e08, -6.769916175223185e07, -2.937179538983412e07], + [4.834188130324496e05, -2.461799304268214e04, -3.467083541217462e04], + False, + ), + ( + "2015 June 10, 00:00:00.0 TDB", + "06/10/15", + [3.777078276008123e07, 2.080046702252371e08, 9.437503998071109e07], + [-2.946784585692780e07, -1.365779215199542e08, -5.923299652516938e07], + [5.052070933145228e05, 1.837038638578682e04, -1.665575509449521e04], + False, + ), + ( + "2015 June 20, 00:00:00.0 TDB", + "06/20/15", + [1.778745850655047e07, 2.116712756672253e08, 9.659608128589308e07], + [-4.338053580692466e06, -1.393743714818930e08, -6.044500073550620e07], + [5.090137386469169e05, 2.694272566092012e04, -1.304862690440150e04], + False, + ), + ( + "2015 August 10, 00:00:00.0 TDB", + "08/10/15", + [-8.302866300591002e07, 2.053384243312354e08, 9.641192179982041e07], + [1.111973130587749e08, -9.507060674145325e07, -4.123957889640842e07], + [5.263951240980130e05, 7.113788303899391e04, 5.601415938789949e03], + False, + ), + ( + "2015 August 20, 00:00:00.0 TDB", + "08/20/15", + [-1.015602120938274e08, 1.994877113707506e08, 9.422840996376510e07], + [1.267319535735967e08, -7.664279872369213e07, -3.325170492059625e07], + [5.294368386862668e05, 7.989635630604146e04, 9.307380417148834e03], + False, + ), + ( + "2015 October 10, 00:00:00.0 TDB", + "10/10/15", + [-1.828944464171771e08, 1.498117608528323e08, 7.363786404040858e07], + [1.440636517249971e08, 3.827074461651713e07, 1.656440704503509e07], + [5.433268959250135e05, 1.251717566539142e05, 2.849999378507032e04], + False, + ), + ( + "2015 October 20, 00:00:00.0 TDB", + "10/20/15", + [-1.955685835661002e08, 1.367530082668284e08, 6.799006120628108e07], + [1.343849818314204e08, 6.019977403127116e07, 2.607024615553362e07], + [5.457339294611338e05, 1.342148834831663e05, 3.234185290692726e04], + False, + ), + ( + "2015 December 10, 00:00:00.0 TDB", + "12/10/15", + [-2.392022492203017e08, 5.847873056287902e07, 3.326431285934674e07], + [3.277145427626925e07, 1.320956053465003e08, 5.723804900679157e07], + [5.559607335969181e05, 1.813263443761486e05, 5.242991145066972e04], + False, + ), + ( + "2015 December 20, 00:00:00.0 TDB", + "12/20/15", + [-2.432532635321551e08, 4.156075241419497e07, 2.561357000250468e07], + [6.866134677340530e06, 1.351080593806486e08, 5.854460725992832e07], + [5.575179227151767e05, 1.907337298309725e05, 5.645553733620559e04], + False, + ), + ( + "2016 February 10, 00:00:00.0 TDB", + "02/10/16", + [-2.385499316808322e08, -4.818711325555268e07, -1.567983931044450e07], + [-1.132504603146183e08, 8.647183658089657e07, 3.746046979137553e07], + [5.630027736619673e05, 2.402590276126245e05, 7.772804647512530e04], + False, + ), + ( + "2016 February 20, 00:00:00.0 TDB", + "02/20/16", + [-2.326189626865872e08, -6.493600278878165e07, -2.352250994262638e07], + [-1.282197228963156e08, 6.695033443521750e07, 2.899822684259482e07], + [5.635403728358555e05, 2.498445036007692e05, 8.185973180152237e04], + False, + ), + ( + "2016 April 10, 00:00:00.0 TDB", + "04/10/16", + [-1.795928090776869e08, -1.395102817786797e08, -5.916067235603344e07], + [-1.399773606371217e08, -4.749126225182984e07, -2.061331784067504e07], + [5.639699901756521e05, 2.977755140828988e05, 1.025609223621660e05], + False, + ), + ( + "2016 April 20, 00:00:00.0 TDB", + "04/20/16", + [-1.646488222290798e08, -1.517361128528306e08, -6.517204439842616e07], + [-1.294310199316289e08, -6.890085351639988e07, -2.989515576444890e07], + [5.636348418836893e05, 3.073681895822543e05, 1.067117713233756e05], + False, + ), + ( + "2016 June 10, 00:00:00.0 TDB", + "06/10/16", + [-7.085675304355654e07, -1.933788289169757e08, -8.680583098365544e07], + [-2.756178030784301e07, -1.365892814324490e08, -5.923888961370525e07], + [5.597493293268962e05, 3.563312003229167e05, 1.279448896916322e05], + False, + ), + ( + "2016 June 20, 00:00:00.0 TDB", + "06/20/16", + [-4.994076874358515e07, -1.967336617729592e08, -8.890950206156498e07], + [-2.413995783152328e06, -1.390828611356292e08, -6.032062925759617e07], + [5.585605124166313e05, 3.659402953599973e05, 1.321213212542782e05], + False, + ), + ( + "2016 August 10, 00:00:00.0 TDB", + "08/10/16", + [5.965895856067932e07, -1.851251207911916e08, -8.654489416392270e07], + [1.124873641861596e08, -9.344410235679612e07, -4.053621796412993e07], + [5.501477436262188e05, 4.149525682073312e05, 1.534983234437988e05], + False, + ), + ( + "2016 August 20, 00:00:00.0 TDB", + "08/20/16", + [8.021899957229958e07, -1.770523551156136e08, -8.339737954004113e07], + [1.277516713236801e08, -7.484361731649198e07, -3.247232896320245e07], + [5.480148786547601e05, 4.245199573757614e05, 1.576874582857746e05], + False, + ), + ( + "2016 October 10, 00:00:00.0 TDB", + "10/10/16", + [1.674991232418756e08, -1.090427183510760e08, -5.456038490399034e07], + [1.434719792031415e08, 4.029387436854774e07, 1.744057129058395e07], + [5.348794087050703e05, 4.727986075510736e05, 1.788947974297997e05], + False, + ), + ( + "2016 October 20, 00:00:00.0 TDB", + "10/20/16", + [1.797014954219412e08, -9.133803506487390e07, -4.676932170648168e07], + [1.334883617893556e08, 6.209917895944476e07, 2.689423661839254e07], + [5.318931290166953e05, 4.821605725626923e05, 1.830201949404063e05], + False, + ), + ( + "2016 December 10, 00:00:00.0 TDB", + "12/10/16", + [2.087370835407280e08, 1.035903131428964e07, -9.084001492689754e05], + [3.082308903715757e07, 1.328026978502711e08, 5.754559376449955e07], + [5.147792796720199e05, 5.293951386498961e05, 2.038816823549219e05], + False, + ), + ( + "2016 December 20, 00:00:00.0 TDB", + "12/20/16", + [2.075411186038260e08, 3.092173198610598e07, 8.555251204561792e06], + [4.885421269793877e06, 1.355125217382336e08, 5.872015978003684e07], + [5.110736843893399e05, 5.385860060393942e05, 2.079481168492821e05], + True, + ), + ], +) # provide a unique test method name, starting with test_ -def test_unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos, EarthTruthPos, - SunTruthPos, useMsg): +def test_unitSpice( + testPlottingFixture, + show_plots, + DateSpice, + DatePlot, + MarsTruthPos, + EarthTruthPos, + SunTruthPos, + useMsg, +): """Module Unit Test""" # each test method requires a single assert method to be called - [testResults, testMessage] = unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos, - EarthTruthPos, SunTruthPos, useMsg) + [testResults, testMessage] = unitSpice( + testPlottingFixture, + show_plots, + DateSpice, + DatePlot, + MarsTruthPos, + EarthTruthPos, + SunTruthPos, + useMsg, + ) assert testResults < 1, testMessage # Run unit test -def unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos, EarthTruthPos, SunTruthPos, useMsg): +def unitSpice( + testPlottingFixture, + show_plots, + DateSpice, + DatePlot, + MarsTruthPos, + EarthTruthPos, + SunTruthPos, + useMsg, +): testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages @@ -170,16 +383,16 @@ def unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos # Initialize the spice modules that we are using. SpiceObject = spiceInterface.SpiceInterface() SpiceObject.ModelTag = "SpiceInterfaceData" - SpiceObject.SPICEDataPath = bskPath + '/supportData/EphemerisData/' + SpiceObject.SPICEDataPath = bskPath + "/supportData/EphemerisData/" planetNames = ["earth", "mars barycenter", "sun"] SpiceObject.addPlanetNames(spiceInterface.StringVector(planetNames)) SpiceObject.UTCCalInit = DateSpice - if useMsg: # in this case check that the planet frame names can be set as well + if useMsg: # in this case check that the planet frame names can be set as well planetFrames = [] for planet in planetNames: planetFrames.append("IAU_" + planet) - planetFrames[1] = "" # testing that default IAU values are used here + planetFrames[1] = "" # testing that default IAU values are used here SpiceObject.planetFrames = spiceInterface.StringVector(planetFrames) TotalSim.AddModelToTask(unitTaskName, SpiceObject) @@ -192,33 +405,39 @@ def unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos # epoch message over-rules any info set in this variable. SpiceObject.UTCCalInit = "1990 February 10, 00:00:00.0 TDB" - spiceObjectLog = SpiceObject.logger(["GPSSeconds", "J2000Current", "julianDateCurrent", "GPSWeek"]) + spiceObjectLog = SpiceObject.logger( + ["GPSSeconds", "J2000Current", "julianDateCurrent", "GPSWeek"] + ) TotalSim.AddModelToTask(unitTaskName, spiceObjectLog) # Configure simulation - TotalSim.ConfigureStopTime(int(60.0 * 1E9)) + TotalSim.ConfigureStopTime(int(60.0 * 1e9)) # Execute simulation TotalSim.InitializeSimulation() TotalSim.ExecuteSimulation() # Get the logged variables (GPS seconds, Julian Date) - DataGPSSec = unitTestSupport.addTimeColumn(spiceObjectLog.times(), spiceObjectLog.GPSSeconds) - DataJD = unitTestSupport.addTimeColumn(spiceObjectLog.times(), spiceObjectLog.julianDateCurrent) + DataGPSSec = unitTestSupport.addTimeColumn( + spiceObjectLog.times(), spiceObjectLog.GPSSeconds + ) + DataJD = unitTestSupport.addTimeColumn( + spiceObjectLog.times(), spiceObjectLog.julianDateCurrent + ) # Get parametrized date from DatePlot year = "".join(("20", DatePlot[6:8])) - date = datetime.datetime( int(year), int(DatePlot[0:2]), int(DatePlot[3:5])) + date = datetime.datetime(int(year), int(DatePlot[0:2]), int(DatePlot[3:5])) testPlottingFixture.Date = DatePlot[0:8] # Get the GPS time date2 = datetime.datetime(1980, 0o1, 6) # Start of GPS time - timeDiff = date-date2 # Time in days since GPS time - GPSTimeSeconds = timeDiff.days*86400 + timeDiff = date - date2 # Time in days since GPS time + GPSTimeSeconds = timeDiff.days * 86400 # Get the Julian day - JulianStartDate = date.toordinal()+1721424.5 + JulianStartDate = date.toordinal() + 1721424.5 # # Begin testing module results to truth values @@ -227,27 +446,35 @@ def unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos # Truth values # For Time delta check GPSRow = DataGPSSec[0, :] - InitDiff = GPSRow[1] - GPSRow[0] * 1.0E-9 + InitDiff = GPSRow[1] - GPSRow[0] * 1.0e-9 i = 1 # Compare the GPS seconds values - AllowTolerance = 1E-6 + AllowTolerance = 1e-6 while i < DataGPSSec.shape[0]: - if date.isoweekday() == 7: # Skip test on Sundays, because it's the end of a GPS week (seconds go to zero) + if ( + date.isoweekday() == 7 + ): # Skip test on Sundays, because it's the end of a GPS week (seconds go to zero) i += 1 else: GPSRow = DataGPSSec[i, :] - CurrDiff = GPSRow[1] - GPSRow[0] * 1.0E-9 + CurrDiff = GPSRow[1] - GPSRow[0] * 1.0e-9 if abs(CurrDiff - InitDiff) > AllowTolerance: testFailCount += 1 - testMessages.append("FAILED: Time delta check failed with difference of: %(DiffVal)f \n"% \ - {"DiffVal": CurrDiff - InitDiff}) + testMessages.append( + "FAILED: Time delta check failed with difference of: %(DiffVal)f \n" + % {"DiffVal": CurrDiff - InitDiff} + ) i += 1 # Truth values # For absolute GPS time check - if date > datetime.datetime(2015, 0o6, 30): # Taking into account the extra leap second added on 6/30/2015 - GPSEndTime = GPSTimeSeconds + 17 + 60.0 - 68.184 # 17 GPS skip seconds passed that date + if date > datetime.datetime( + 2015, 0o6, 30 + ): # Taking into account the extra leap second added on 6/30/2015 + GPSEndTime = ( + GPSTimeSeconds + 17 + 60.0 - 68.184 + ) # 17 GPS skip seconds passed that date else: GPSEndTime = GPSTimeSeconds + 16 + 60.0 - 67.184 # 16 GPS skip seconds before @@ -256,18 +483,22 @@ def unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos GPSSecDiff = abs(GPSRow[1] - GPSSecondAssumed) # TestResults['GPSAbsTimeCheck'] = True - AllowTolerance = 1E-4 + AllowTolerance = 1e-4 if useMsg: - AllowTolerance = 2E-2 + AllowTolerance = 2e-2 # Skip test days that are Sunday because of the end of a GPS week if date.isoweekday() != 7 and GPSSecDiff > AllowTolerance: testFailCount += 1 - testMessages.append("FAILED: Absolute GPS time check failed with difference of: %(DiffVal)f \n" % \ - {"DiffVal": GPSSecDiff}) + testMessages.append( + "FAILED: Absolute GPS time check failed with difference of: %(DiffVal)f \n" + % {"DiffVal": GPSSecDiff} + ) # Truth values # For absolute Julian date time check - if date>datetime.datetime(2015, 0o6, 30): # Taking into account the extra leap second added on 6/30/2015 + if date > datetime.datetime( + 2015, 0o6, 30 + ): # Taking into account the extra leap second added on 6/30/2015 JDEndTime = JulianStartDate + 0.0006944440 - 68.184 / 86400 else: JDEndTime = JulianStartDate + 0.0006944440 - 67.184 / 86400 @@ -277,8 +508,10 @@ def unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos JDTimeErrorAllow = 0.1 / (24.0 * 3600.0) if abs(JDEndSim - JDEndTime) > JDTimeErrorAllow: testFailCount += 1 - testMessages.append("FAILED: Absolute Julian Date time check failed with difference of: %(DiffVal)f \n" % \ - {"DiffVal": abs(JDEndSim - JDEndTime)}) + testMessages.append( + "FAILED: Absolute Julian Date time check failed with difference of: %(DiffVal)f \n" + % {"DiffVal": abs(JDEndSim - JDEndTime)} + ) # Truth values # For Mars position check @@ -300,10 +533,12 @@ def unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos PosErrTolerance = 250 if useMsg: PosErrTolerance = 1000 - if (PosDiffNorm > PosErrTolerance): + if PosDiffNorm > PosErrTolerance: testFailCount += 1 - testMessages.append("FAILED: Mars position check failed with difference of: %(DiffVal)f \n" % \ - {"DiffVal": PosDiffNorm}) + testMessages.append( + "FAILED: Mars position check failed with difference of: %(DiffVal)f \n" + % {"DiffVal": PosDiffNorm} + ) # Truth values # For Earth position check @@ -322,16 +557,18 @@ def unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos testPlottingFixture.EarthPosErr = PosDiffNorm # TestResults['EarthPosCheck'] = True - if (PosDiffNorm > PosErrTolerance): + if PosDiffNorm > PosErrTolerance: testFailCount += 1 - testMessages.append("FAILED: Earth position check failed with difference of: %(DiffVal)f \n" % \ - {"DiffVal": PosDiffNorm}) + testMessages.append( + "FAILED: Earth position check failed with difference of: %(DiffVal)f \n" + % {"DiffVal": PosDiffNorm} + ) # Truth Sun position values SunPosEnd = numpy.array(SunTruthPos) SunPosEnd = SunPosEnd * 1000.0 - #Simulated Sun position values + # Simulated Sun position values SunPosVec = SpiceObject.planetStateOutMsgs[2].read().PositionVector # Compare Sun position values @@ -343,13 +580,16 @@ def unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos testPlottingFixture.SunPosErr = PosDiffNorm # Test Sun position values - if (PosDiffNorm > PosErrTolerance): + if PosDiffNorm > PosErrTolerance: testFailCount += 1 - testMessages.append("FAILED: Sun position check failed with difference of: %(DiffVal)f \n" % \ - {"DiffVal": PosDiffNorm}) - - if date == datetime.datetime(2016,12,20): # Only test the false files on the last test - + testMessages.append( + "FAILED: Sun position check failed with difference of: %(DiffVal)f \n" + % {"DiffVal": PosDiffNorm} + ) + + if date == datetime.datetime( + 2016, 12, 20 + ): # Only test the false files on the last test # Test non existing directory SpiceObject.SPICEDataPath = "ADirectoryThatDoesntreallyexist" SpiceObject.SPICELoaded = False @@ -374,7 +614,8 @@ def unitSpice(testPlottingFixture, show_plots, DateSpice, DatePlot, MarsTruthPos # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] + def test_59_999999_second_epoch(): """ @@ -396,7 +637,7 @@ def test_59_999999_second_epoch(): # Initialize the spice modules that we are using. SpiceObject = spiceInterface.SpiceInterface() SpiceObject.ModelTag = "SpiceInterfaceData" - SpiceObject.SPICEDataPath = bskPath + '/supportData/EphemerisData/' + SpiceObject.SPICEDataPath = bskPath + "/supportData/EphemerisData/" TotalSim.AddModelToTask(unitTaskName, SpiceObject) @@ -404,26 +645,27 @@ def test_59_999999_second_epoch(): SpiceObject.epochInMsg.subscribeTo(epochMsg) # Configure simulation - TotalSim.ConfigureStopTime(int(60.0 * 1E9)) + TotalSim.ConfigureStopTime(int(60.0 * 1e9)) # Execute simulation TotalSim.InitializeSimulation() TotalSim.ExecuteSimulation() print(SpiceObject.UTCCalInit) - assert(SpiceObject.UTCCalInit[-15:] == "59.999999 (UTC)") + assert SpiceObject.UTCCalInit[-15:] == "59.999999 (UTC)" + # This statement below ensures that the unit test scrip can be run as a # stand-along python script # if __name__ == "__main__": test_unitSpice( - testPlottingFixture, - True, # show_plots - "2015 February 10, 00:00:00.00 TDB", - "02/10/15", - [2.049283795042291E+08, 4.654550957513031E+07, 1.580778617009296E+07], - [-1.137790671899544E+08, 8.569008401822130E+07, 3.712507705247846E+07], - [4.480338216752146E+05, -7.947764237588293E+04, -5.745748832696378E+04], - True # useMsg - ) + testPlottingFixture, + True, # show_plots + "2015 February 10, 00:00:00.00 TDB", + "02/10/15", + [2.049283795042291e08, 4.654550957513031e07, 1.580778617009296e07], + [-1.137790671899544e08, 8.569008401822130e07, 3.712507705247846e07], + [4.480338216752146e05, -7.947764237588293e04, -5.745748832696378e04], + True, # useMsg + ) diff --git a/src/simulation/environment/spiceInterface/_UnitTest/test_unitSpiceSpacecraft.py b/src/simulation/environment/spiceInterface/_UnitTest/test_unitSpiceSpacecraft.py index 34aef5098d..db12347f35 100755 --- a/src/simulation/environment/spiceInterface/_UnitTest/test_unitSpiceSpacecraft.py +++ b/src/simulation/environment/spiceInterface/_UnitTest/test_unitSpiceSpacecraft.py @@ -1,4 +1,3 @@ - # ISC License # # Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado at Boulder @@ -34,6 +33,7 @@ filename = inspect.getframeinfo(inspect.currentframe()).filename path = os.path.dirname(os.path.abspath(filename)) from Basilisk import __path__ + bskPath = __path__[0] from Basilisk.utilities import unitTestSupport @@ -41,6 +41,7 @@ from Basilisk.simulation import spiceInterface from Basilisk.utilities import macros + # provide a unique test method name, starting with test_ def test_unitSpiceSc(show_plots): """Module Unit Test""" @@ -69,12 +70,14 @@ def unitSpiceSc(show_plots): # Initialize the spice modules that we are using. spiceObject = spiceInterface.SpiceInterface() spiceObject.ModelTag = "SpiceInterfaceData" - spiceObject.SPICEDataPath = bskPath + '/supportData/EphemerisData/' + spiceObject.SPICEDataPath = bskPath + "/supportData/EphemerisData/" scNames = ["HUBBLE SPACE TELESCOPE"] spiceObject.addSpacecraftNames(spiceInterface.StringVector(scNames)) spiceObject.UTCCalInit = dateSpice spiceObject.zeroBase = "earth" - spiceObject.loadSpiceKernel("hst_edited.bsp", bskPath + '/supportData/EphemerisData/') + spiceObject.loadSpiceKernel( + "hst_edited.bsp", bskPath + "/supportData/EphemerisData/" + ) TotalSim.AddModelToTask(unitTaskName, spiceObject) @@ -86,67 +89,116 @@ def unitSpiceSc(show_plots): TotalSim.ExecuteSimulation() # unload spice kernel - spiceObject.unloadSpiceKernel("hst_edited.bsp", bskPath + '/supportData/EphemerisData/') + spiceObject.unloadSpiceKernel( + "hst_edited.bsp", bskPath + "/supportData/EphemerisData/" + ) # set truth - truthPosition = np.array([-5855529.540348052, 1986110.860522791, -3116764.7117067943]) - truthVelocity = np.array([-1848.9038338503085, -7268.515626753905, -1155.3578832725618]) - truthAtt = np.array([0., 0., 0.]) - truthZero = np.array([0., 0., 0.]) + truthPosition = np.array( + [-5855529.540348052, 1986110.860522791, -3116764.7117067943] + ) + truthVelocity = np.array( + [-1848.9038338503085, -7268.515626753905, -1155.3578832725618] + ) + truthAtt = np.array([0.0, 0.0, 0.0]) + truthZero = np.array([0.0, 0.0, 0.0]) scStateMsg = spiceObject.scStateOutMsgs[0].read() # print(scStateMsg.r_BN_N) # print(scStateMsg.v_BN_N) # print(scStateMsg.sigma_BN) accuracy = 0.01 - testFailCount, testMessages = unitTestSupport.compareVector(truthPosition, - scStateMsg.r_BN_N, - accuracy, "scState-r_BN_N", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthPosition, - scStateMsg.r_CN_N, - accuracy, "scState-r_CN_N", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthVelocity, - scStateMsg.v_BN_N, - accuracy, "scState-v_BN_N", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthVelocity, - scStateMsg.v_CN_N, - accuracy, "scState-v_CN_N", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthAtt, - scStateMsg.sigma_BN, - accuracy, "scState-sigma_BN", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareVector( + truthPosition, + scStateMsg.r_BN_N, + accuracy, + "scState-r_BN_N", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthPosition, + scStateMsg.r_CN_N, + accuracy, + "scState-r_CN_N", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthVelocity, + scStateMsg.v_BN_N, + accuracy, + "scState-v_BN_N", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthVelocity, + scStateMsg.v_CN_N, + accuracy, + "scState-v_CN_N", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthAtt, + scStateMsg.sigma_BN, + accuracy, + "scState-sigma_BN", + testFailCount, + testMessages, + ) attStateMsg = spiceObject.attRefStateOutMsgs[0].read() - testFailCount, testMessages = unitTestSupport.compareVector(truthAtt, - attStateMsg.sigma_RN, - accuracy, "scState-sigma_RN", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthZero, - attStateMsg.omega_RN_N, - accuracy, "scState-omega_RN_N", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthZero, - attStateMsg.domega_RN_N, - accuracy, "scState-domega_RN_N", - testFailCount, testMessages) + testFailCount, testMessages = unitTestSupport.compareVector( + truthAtt, + attStateMsg.sigma_RN, + accuracy, + "scState-sigma_RN", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthZero, + attStateMsg.omega_RN_N, + accuracy, + "scState-omega_RN_N", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthZero, + attStateMsg.domega_RN_N, + accuracy, + "scState-domega_RN_N", + testFailCount, + testMessages, + ) transStateMsg = spiceObject.transRefStateOutMsgs[0].read() - testFailCount, testMessages = unitTestSupport.compareVector(truthPosition, - transStateMsg.r_RN_N, - accuracy, "scState-r_RN_N", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthVelocity, - transStateMsg.v_RN_N, - accuracy, "scState-v_RN_N", - testFailCount, testMessages) - testFailCount, testMessages = unitTestSupport.compareVector(truthZero, - transStateMsg.a_RN_N, - accuracy, "scState-a_RN_N", - testFailCount, testMessages) - + testFailCount, testMessages = unitTestSupport.compareVector( + truthPosition, + transStateMsg.r_RN_N, + accuracy, + "scState-r_RN_N", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthVelocity, + transStateMsg.v_RN_N, + accuracy, + "scState-v_RN_N", + testFailCount, + testMessages, + ) + testFailCount, testMessages = unitTestSupport.compareVector( + truthZero, + transStateMsg.a_RN_N, + accuracy, + "scState-a_RN_N", + testFailCount, + testMessages, + ) # print out success message if no error were found if testFailCount == 0: @@ -154,7 +206,7 @@ def unitSpiceSc(show_plots): # each test method requires a single assert method to be called # this check below just makes sure no sub-test failures were found - return [testFailCount, ''.join(testMessages)] + return [testFailCount, "".join(testMessages)] # This statement below ensures that the unit test scrip can be run as a @@ -162,5 +214,5 @@ def unitSpiceSc(show_plots): # if __name__ == "__main__": test_unitSpiceSc( - False # show_plots - ) + False # show_plots + ) diff --git a/src/simulation/mujocoDynamics/MJSystemCoM/_UnitTest/test_MJSystemCoM.py b/src/simulation/mujocoDynamics/MJSystemCoM/_UnitTest/test_MJSystemCoM.py index c05a146cf3..f0abd35ff4 100644 --- a/src/simulation/mujocoDynamics/MJSystemCoM/_UnitTest/test_MJSystemCoM.py +++ b/src/simulation/mujocoDynamics/MJSystemCoM/_UnitTest/test_MJSystemCoM.py @@ -26,6 +26,7 @@ from Basilisk.utilities import unitTestSupport from Basilisk.architecture import messaging from Basilisk.utilities import macros + try: from Basilisk.simulation import mujoco from Basilisk.simulation import MJSystemCoM @@ -37,9 +38,10 @@ # Common parameters used in both XML and truth calc HUB_MASS = 50.0 ARM_MASS = 10.0 -SX, SY, SZ = 0.5, 0.3, 0.2 # hub half-sizes (m) → face centers at ±SX -L = 2.0 # arm length (m) -THETA_DEG = 30.0 # arm yaw angle for "angled" case +SX, SY, SZ = 0.5, 0.3, 0.2 # hub half-sizes (m) → face centers at ±SX +L = 2.0 # arm length (m) +THETA_DEG = 30.0 # arm yaw angle for "angled" case + def _xml_single(): return f""" @@ -54,6 +56,7 @@ def _xml_single(): """ + def _xml_straight(): # Arms go straight ±X from the face centers. # Place each arm body at the face center; set its inertial COM at L/2 along arm direction. @@ -69,14 +72,14 @@ def _xml_straight(): - + - + @@ -84,6 +87,7 @@ def _xml_straight(): """ + def _xml_angled(): # Arms leave ±X faces at yaw = ±θ outward (both tilt toward +Y). # Right arm direction u_plus = [cosθ, sinθ, 0]. @@ -100,20 +104,21 @@ def _xml_angled(): - - + + - - + + """ + def _write_temp_xml(xml_text: str, basename: str) -> str: tmpdir = tempfile.mkdtemp(prefix="mjcom_") path = os.path.join(tmpdir, f"{basename}.xml") @@ -121,6 +126,7 @@ def _write_temp_xml(xml_text: str, basename: str) -> str: f.write(xml_text) return path + def _expected_com(model: str, r_root, v_root): r_root = np.asarray(r_root, dtype=float) v_root = np.asarray(v_root, dtype=float) @@ -142,12 +148,12 @@ def _expected_com(model: str, r_root, v_root): # COM locations of the welded arm bodies in the hub frame: # right arm COM = face center + (L/2)[c, s, 0] - r_plus = np.array([ SX, 0.0, 0.0]) + np.array([(L/2)*c, (L/2)*s, 0.0]) + r_plus = np.array([SX, 0.0, 0.0]) + np.array([(L / 2) * c, (L / 2) * s, 0.0]) # left arm COM = (-SX,0,0) + (L/2)[-c, s, 0] - r_minus= np.array([-SX, 0.0, 0.0]) + np.array([-(L/2)*c, (L/2)*s, 0.0]) + r_minus = np.array([-SX, 0.0, 0.0]) + np.array([-(L / 2) * c, (L / 2) * s, 0.0]) - M = HUB_MASS + 2*ARM_MASS - r_off = (HUB_MASS*np.zeros(3) + ARM_MASS*r_plus + ARM_MASS*r_minus) / M + M = HUB_MASS + 2 * ARM_MASS + r_off = (HUB_MASS * np.zeros(3) + ARM_MASS * r_plus + ARM_MASS * r_minus) / M rC = r_root + r_off vC = v_root @@ -157,16 +163,17 @@ def _expected_com(model: str, r_root, v_root): return rC, vC + @pytest.mark.skipif(not couldImportMujoco, reason="Compiled Basilisk without --mujoco") @pytest.mark.parametrize("model", ["single", "straight", "angled"]) @pytest.mark.parametrize("moving", [True, False]) @pytest.mark.parametrize("displaced", [True, False]) - def test_MJSystemCoM(show_plots, model, moving, displaced): """Module Unit Test""" [testResults, testMessage] = MJSystemCoMTest(show_plots, model, moving, displaced) assert testResults < 1, testMessage + def MJSystemCoMTest(show_plots, model, moving, displaced): r""" **Validation Test Description** @@ -203,8 +210,12 @@ def MJSystemCoMTest(show_plots, model, moving, displaced): root_name = "hub" # Root initial pose - r0 = np.array([0.0, 0.0, 0.0]) if not displaced else np.array([1234.5, -678.9, 42.0]) # m - v0 = np.array([0.0, 0.0, 0.0]) if not moving else np.array([0.12, -0.07, 0.03]) # m/s + r0 = ( + np.array([0.0, 0.0, 0.0]) if not displaced else np.array([1234.5, -678.9, 42.0]) + ) # m + v0 = ( + np.array([0.0, 0.0, 0.0]) if not moving else np.array([0.12, -0.07, 0.03]) + ) # m/s unitTestSim = SimulationBaseClass.SimBaseClass() testProcessRate = macros.sec2nano(0.1) @@ -238,10 +249,10 @@ def MJSystemCoMTest(show_plots, model, moving, displaced): unitTestSim.ExecuteSimulation() # pull module data and make sure it is correct - r_CN_N_module = comStatesOutMsgRec.r_CN_N[-1,:] - v_CN_N_module = comStatesOutMsgRec.v_CN_N[-1,:] - r_CN_N_module_c = comStatesOutMsgCRec.r_CN_N[-1,:] - v_CN_N_module_c = comStatesOutMsgCRec.v_CN_N[-1,:] + r_CN_N_module = comStatesOutMsgRec.r_CN_N[-1, :] + v_CN_N_module = comStatesOutMsgRec.v_CN_N[-1, :] + r_CN_N_module_c = comStatesOutMsgCRec.r_CN_N[-1, :] + v_CN_N_module_c = comStatesOutMsgCRec.v_CN_N[-1, :] # compute the truth data r_CN_N_truth0, v_CN_N_truth0 = _expected_com(model, r0, v0) @@ -251,16 +262,40 @@ def MJSystemCoMTest(show_plots, model, moving, displaced): # Compare acc = 1e-12 testFailCount, testMessages = unitTestSupport.compareArrayND( - [r_CN_N_truth], [r_CN_N_module], acc, "CoM_position", 3, testFailCount, testMessages + [r_CN_N_truth], + [r_CN_N_module], + acc, + "CoM_position", + 3, + testFailCount, + testMessages, ) testFailCount, testMessages = unitTestSupport.compareArrayND( - [v_CN_N_truth], [v_CN_N_module], acc, "CoM_velocity", 3, testFailCount, testMessages + [v_CN_N_truth], + [v_CN_N_module], + acc, + "CoM_velocity", + 3, + testFailCount, + testMessages, ) testFailCount, testMessages = unitTestSupport.compareArrayND( - [r_CN_N_module], [r_CN_N_module_c], acc, "cMsgPosition", 3, testFailCount, testMessages + [r_CN_N_module], + [r_CN_N_module_c], + acc, + "cMsgPosition", + 3, + testFailCount, + testMessages, ) testFailCount, testMessages = unitTestSupport.compareArrayND( - [v_CN_N_module], [v_CN_N_module_c], acc, "cMsgVelocity", 3, testFailCount, testMessages + [v_CN_N_module], + [v_CN_N_module_c], + acc, + "cMsgVelocity", + 3, + testFailCount, + testMessages, ) if testFailCount == 0: diff --git a/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py b/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py index e5360d3dec..64ccd4a256 100644 --- a/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py +++ b/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py @@ -66,7 +66,7 @@ def test_pointMass(showPlots): """ # Orbital parameters for the body - mu = 0.3986004415e15 # m**3/s**2 + mu = 0.3986004415e15 # m**3/s**2 oe = orbitalMotion.ClassicElements() rLEO = 7000.0 * 1000 # meters @@ -80,7 +80,7 @@ def test_pointMass(showPlots): rN, vN = orbitalMotion.elem2rv(mu, oe) oe = orbitalMotion.rv2elem(mu, rN, vN) - period = 2 * np.pi * np.sqrt(oe.a**3 / mu) # s + period = 2 * np.pi * np.sqrt(oe.a**3 / mu) # s tf = 2 * period # Because we use an adaptive integrator we can set the @@ -128,7 +128,7 @@ def test_pointMass(showPlots): # Add random attitude and attitude rate, which should have no impact body.setAttitude(rbk.euler1232MRP([np.pi / 2, np.pi / 6, np.pi / 4])) - body.setAttitudeRate([0.3, 0.1, 0.2]) # rad/s + body.setAttitudeRate([0.3, 0.1, 0.2]) # rad/s # Run sim scSim.ConfigureStopTime(macros.sec2nano(tf)) @@ -211,7 +211,7 @@ def test_dumbbell(showPlots, initialAngularRate): """ # Initial orbital parameters of the body: circular equatorial LEO orbit - mu = 0.3986004415e15 # m**3/s**2 + mu = 0.3986004415e15 # m**3/s**2 oe = orbitalMotion.ClassicElements() rLEO = 7000.0 * 1000 # meters @@ -225,7 +225,7 @@ def test_dumbbell(showPlots, initialAngularRate): rN, vN = orbitalMotion.elem2rv(mu, oe) oe = orbitalMotion.rv2elem(mu, rN, vN) - period = 2 * np.pi * np.sqrt(oe.a**3 / mu) # s + period = 2 * np.pi * np.sqrt(oe.a**3 / mu) # s # Integrate for an orbit, record 50 points through orbit tf = period * 1 @@ -285,7 +285,9 @@ def test_dumbbell(showPlots, initialAngularRate): mainBody.setPosition(rN) mainBody.setVelocity(vN) mainBody.setAttitude(rbk.euler1232MRP([0, 0, 0])) - mainBody.setAttitudeRate([0, 0, 2 * np.pi / period if initialAngularRate else 0]) # rad/s + mainBody.setAttitudeRate( + [0, 0, 2 * np.pi / period if initialAngularRate else 0] + ) # rad/s # Run sim scSim.ExecuteSimulation() @@ -452,8 +454,8 @@ def test_gps(showPlots: bool, useSphericalHarmonics: bool, useThirdBodies: bool) # initial date, simulation time, and time step utcCalInit = "2022 NOV 14 00:01:10" - tf = 1 * 3600 # s - dt = 1 # s + tf = 1 * 3600 # s + dt = 1 # s # load trajectory from SPICE spiceSatelliteState = loadSatelliteTrajectory(utcCalInit, tf, dt) @@ -533,8 +535,8 @@ def test_gps(showPlots: bool, useSphericalHarmonics: bool, useThirdBodies: bool) scSim.ConfigureStopTime(macros.sec2nano(tf)) # Initialize the body to the same position and velocity as the SPICE result - body.setPosition(spiceSatelliteState[0, :3] * 1000) # m - body.setVelocity(spiceSatelliteState[0, 3:] * 1000) # m + body.setPosition(spiceSatelliteState[0, :3] * 1000) # m + body.setVelocity(spiceSatelliteState[0, 3:] * 1000) # m # Run sim scSim.ExecuteSimulation() @@ -606,11 +608,11 @@ def test_mujocoVsSpacecraft( # Initial time, final time, and time step # We run for 24hr only if we want to plot results utcCalInit = "2022 NOV 14 00:01:10" - dt = 1 # s - tf = 24 * 3600 if showPlots else 10 # s + dt = 1 # s + tf = 24 * 3600 if showPlots else 10 # s # initial state of the body - mu = 0.3986004415e15 # m**3/s**2 + mu = 0.3986004415e15 # m**3/s**2 oe = orbitalMotion.ClassicElements() rLEO = 7000.0 * 1000 # meters @@ -643,7 +645,7 @@ def wrap(): tic = time.time() result = fn() toc = time.time() - print(f"{fn.__name__} took {toc-tic} seconds") + print(f"{fn.__name__} took {toc - tic} seconds") return result return wrap @@ -682,11 +684,15 @@ def spacecraftSim(): ) # initialize spacecraft parameters - scObject.hub.mHub = 100 # kg - scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - scObject.hub.IHubPntBc_B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] # kg*m**2 - scObject.hub.r_CN_NInit = rN # m - scObject.hub.v_CN_NInit = vN # m/s + scObject.hub.mHub = 100 # kg + scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m + scObject.hub.IHubPntBc_B = [ + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0], + ] # kg*m**2 + scObject.hub.r_CN_NInit = rN # m + scObject.hub.v_CN_NInit = vN # m/s # Create recorder scStateRecorder = scObject.scStateOutMsg.recorder() @@ -757,7 +763,6 @@ def mujocoSim(): bodyStateRecorder = mujocoSim() if showPlots: - t = bodyStateRecorder.times() * macros.NANO2SEC diff = np.linalg.norm(scStateRecorder.r_BN_N - bodyStateRecorder.r_BN_N, axis=1) diff --git a/src/simulation/mujocoDynamics/PIDControllers/_UnitTest/test_PIDControllers.py b/src/simulation/mujocoDynamics/PIDControllers/_UnitTest/test_PIDControllers.py index 1c30b0cd4f..f8ec990c1a 100644 --- a/src/simulation/mujocoDynamics/PIDControllers/_UnitTest/test_PIDControllers.py +++ b/src/simulation/mujocoDynamics/PIDControllers/_UnitTest/test_PIDControllers.py @@ -16,6 +16,7 @@ except: couldImportMujoco = False + @pytest.mark.skipif(not couldImportMujoco, reason="Compiled Basilisk without --mujoco") def test_jointPIDController(showPlots: bool = False): """ @@ -23,10 +24,10 @@ def test_jointPIDController(showPlots: bool = False): Verifies that the controller computes the correct output for given input messages. Plots joint angle, velocity, and control torque if plot=True. """ - dt = .1 + dt = 0.1 tf = 50 - desiredPosition = np.pi/2 + desiredPosition = np.pi / 2 desiredVelocity = desiredPosition / tf scSim = SimulationBaseClass.SimBaseClass() @@ -35,7 +36,7 @@ def test_jointPIDController(showPlots: bool = False): # Create the MJScene from a simple cannonball body scene = mujoco.MJScene( -r""" + r"""