diff --git a/.clang-format b/.clang-format index 9c1e9f3917..1d90b3c9f0 100644 --- a/.clang-format +++ b/.clang-format @@ -3,4 +3,5 @@ BasedOnStyle: Mozilla ColumnLimit: '120' IndentWidth: '4' TabWidth: '4' +SortIncludes: false ... 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/.gitignore b/.gitignore index 2b717ce721..1c85a9ee3a 100755 --- a/.gitignore +++ b/.gitignore @@ -62,3 +62,9 @@ venv/ build/ !.github/actions/build/ !.github/actions/build/** + +# Ruff cache +.ruff_cache/ + +# UV +uv.lock diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 8443f3b395..69348bcc71 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,12 +1,21 @@ 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 + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v20.1.0 + hooks: + - id: clang-format + files: \.(c|cc|cpp|cxx|h|hpp|ih|i)$ 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/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/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/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/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..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): @@ -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 @@ -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 449de3f504..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" @@ -140,4 +154,3 @@ def InitAllDynObjects(self): self.SetSimpleNavObject() self.SetReactionWheelDynEffector() self.SetExternalForceTorqueObject() - 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 c3ed7ffe4b..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): """ @@ -183,7 +215,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) @@ -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 9971b9509f..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) @@ -296,4 +371,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/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 f421d9ac32..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) @@ -190,5 +245,3 @@ def run(show_plots): if __name__ == "__main__": run(False) - - 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/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/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 15a4aa6849..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 @@ -115,28 +118,36 @@ 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)], - [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,35 +174,35 @@ 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 - tabAtmo.altList = tabularAtmosphere.DoubleVector(altList) + tabAtmo.altList = tabularAtmosphere.DoubleVector(altList) tabAtmo.rhoList = tabularAtmosphere.DoubleVector(rhoList) tabAtmo.tempList = tabularAtmosphere.DoubleVector(tempList) # 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)) @@ -217,7 +228,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) @@ -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,7 +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 c5d56bb8fd..e9b8863d59 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. @@ -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 @@ -163,10 +175,10 @@ 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.k = 200. - panel1.c = 20. - panel1.r_HB_B = [[-.5], [0.0], [-1.0]] + panel1.d = 1.5 + 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 @@ -175,10 +187,10 @@ 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.k = 200. - panel2.c = 20. - panel2.r_HB_B = [[.5], [0.0], [-1.0]] + panel2.d = 1.5 + 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 @@ -228,7 +244,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 +252,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) @@ -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 e1b0c06648..7c739f0662 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:: @@ -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,32 +82,31 @@ 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() 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() @@ -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) \ No newline at end of file + 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 19ab8de7a9..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,34 +313,41 @@ 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) + ["totOrbEnergy", "totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totRotEnergy"], + 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) @@ -436,10 +450,10 @@ 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)') + + 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/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/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 b51a90d290..620d994d3e 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 @@ -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 dcd71d6b78..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,17 +131,23 @@ # 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: - + 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 +155,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 @@ -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,10 +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/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/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/libs/mujoco/test_package/src/main.cpp b/libs/mujoco/test_package/src/main.cpp index 9547c64bc1..d42e6705c1 100644 --- a/libs/mujoco/test_package/src/main.cpp +++ b/libs/mujoco/test_package/src/main.cpp @@ -1,7 +1,9 @@ #include // Just checking that we can actually run a mujoco function -int main(int argc, char** argv) { - auto res = mju_min(0, 1); - return res == 0 ? 0 : 1; +int +main(int argc, char** argv) +{ + auto res = mju_min(0, 1); + return res == 0 ? 0 : 1; } 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 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/_GeneralModuleFiles/sys_model.cpp b/src/architecture/_GeneralModuleFiles/sys_model.cpp index c203975926..9138b658c4 100644 --- a/src/architecture/_GeneralModuleFiles/sys_model.cpp +++ b/src/architecture/_GeneralModuleFiles/sys_model.cpp @@ -21,11 +21,13 @@ #include "architecture/utilities/moduleIdGenerator/moduleIdGenerator.h" SysModel::SysModel() - : moduleID(ModuleIdGenerator::GetInstance()->checkoutModuleID()) -{} + : moduleID(ModuleIdGenerator::GetInstance()->checkoutModuleID()) +{ +} -SysModel::SysModel(const SysModel &obj) - : ModelTag{obj.ModelTag}, - RNGSeed{obj.RNGSeed}, - moduleID{ModuleIdGenerator::GetInstance()->checkoutModuleID()} -{} +SysModel::SysModel(const SysModel& obj) + : ModelTag{ obj.ModelTag } + , RNGSeed{ obj.RNGSeed } + , moduleID{ ModuleIdGenerator::GetInstance()->checkoutModuleID() } +{ +} diff --git a/src/architecture/_GeneralModuleFiles/sys_model.h b/src/architecture/_GeneralModuleFiles/sys_model.h index a42fe43d74..c71b88a93c 100644 --- a/src/architecture/_GeneralModuleFiles/sys_model.h +++ b/src/architecture/_GeneralModuleFiles/sys_model.h @@ -27,7 +27,7 @@ /*! @brief Simulation System Model Class */ class SysModel { -public: + public: SysModel(); /** @@ -38,21 +38,21 @@ class SysModel * * @param obj The SysModel object to copy data from. */ - SysModel(const SysModel &obj); + SysModel(const SysModel& obj); - virtual ~SysModel(){}; + virtual ~SysModel() {}; /** Initializes the module, create messages */ - virtual void SelfInit(){}; + virtual void SelfInit() {}; /** ??? */ - virtual void IntegratedInit(){}; + virtual void IntegratedInit() {}; /** Reads incoming messages, performs module actions, writes output messages */ - virtual void UpdateState(uint64_t CurrentSimNanos){}; + virtual void UpdateState(uint64_t CurrentSimNanos) {}; /** Called at simulation initialization, resets module to specified time */ - virtual void Reset(uint64_t CurrentSimNanos){}; + virtual void Reset(uint64_t CurrentSimNanos) {}; std::string ModelTag = ""; //!< Basilisk module tag name uint64_t CallCounts = 0; //!< Counts on the model being called @@ -60,5 +60,4 @@ class SysModel int64_t moduleID; //!< Module ID for this module (handed out by module_id_generator) }; - #endif /* _SYS_MODEL_H_ */ diff --git a/src/architecture/_GeneralModuleFiles/sys_model_task.cpp b/src/architecture/_GeneralModuleFiles/sys_model_task.cpp old mode 100755 new mode 100644 index 5c1f1e7722..2b8ea39c9f --- a/src/architecture/_GeneralModuleFiles/sys_model_task.cpp +++ b/src/architecture/_GeneralModuleFiles/sys_model_task.cpp @@ -25,36 +25,37 @@ @param FirstStartTime The amount of time in nanoseconds to hold a task dormant before starting. After this time the task is executed at integer amounts of InputPeriod again */ -SysModelTask::SysModelTask(uint64_t InputPeriod, uint64_t FirstStartTime) : - NextStartTime(FirstStartTime), TaskPeriod(InputPeriod), FirstTaskTime(FirstStartTime) +SysModelTask::SysModelTask(uint64_t InputPeriod, uint64_t FirstStartTime) + : NextStartTime(FirstStartTime) + , TaskPeriod(InputPeriod) + , FirstTaskTime(FirstStartTime) { this->NextPickupTime = this->NextStartTime + this->TaskPeriod; } /*! This method self-initializes all of the models that have been added to the Task. */ -void SysModelTask::SelfInitTaskList() const +void +SysModelTask::SelfInitTaskList() const { //! - Loop over all models and do the self init for each - for(auto const& modelPair : this->TaskModels) - { + for (auto const& modelPair : this->TaskModels) { SysModel* NonIt = modelPair.ModelPtr; NonIt->SelfInit(); } } - /*! This method resets all of the models that have been added to the Task at the CurrentSimTime. See sys_model_task.h for related method ResetTask() @param CurrentSimTime The time to start at after reset */ -void SysModelTask::ResetTaskList(uint64_t CurrentSimTime) +void +SysModelTask::ResetTaskList(uint64_t CurrentSimTime) { - for (auto const& modelPair : this->TaskModels) - { - modelPair.ModelPtr->Reset(CurrentSimTime); - } - this->NextStartTime = CurrentSimTime; + for (auto const& modelPair : this->TaskModels) { + modelPair.ModelPtr->Reset(CurrentSimTime); + } + this->NextStartTime = CurrentSimTime; this->NextPickupTime = this->NextStartTime + this->TaskPeriod; } @@ -62,12 +63,12 @@ void SysModelTask::ResetTaskList(uint64_t CurrentSimTime) Then, it sets its NextStartTime appropriately. @param CurrentSimNanos The current simulation time in [ns] */ -void SysModelTask::ExecuteTaskList(uint64_t CurrentSimNanos) +void +SysModelTask::ExecuteTaskList(uint64_t CurrentSimNanos) { //! - Loop over all of the models in the simulation and call their UpdateState - for(auto ModelPair = this->TaskModels.begin(); (ModelPair != this->TaskModels.end() && this->taskActive); - ModelPair++) - { + for (auto ModelPair = this->TaskModels.begin(); (ModelPair != this->TaskModels.end() && this->taskActive); + ModelPair++) { SysModel* NonIt = (ModelPair->ModelPtr); NonIt->UpdateState(CurrentSimNanos); NonIt->CallCounts += 1; @@ -81,21 +82,19 @@ void SysModelTask::ExecuteTaskList(uint64_t CurrentSimNanos) @param NewModel The new model that we are adding to the Task @param Priority The selected priority of the model being added (highest goes first) */ -void SysModelTask::AddNewObject(SysModel *NewModel, int32_t Priority) +void +SysModelTask::AddNewObject(SysModel* NewModel, int32_t Priority) { ModelPriorityPair LocalPair; //! - Set the local pair with the requested priority and mode LocalPair.CurrentModelPriority = Priority; LocalPair.ModelPtr = NewModel; -// SystemMessaging::GetInstance()->addModuleToProcess(NewModel->moduleID, -// parentProc); + // SystemMessaging::GetInstance()->addModuleToProcess(NewModel->moduleID, + // parentProc); //! - Loop through the ModelPair vector and if Priority is higher than next, insert - for(auto ModelPair = this->TaskModels.begin(); ModelPair != this->TaskModels.end(); - ModelPair++) - { - if(Priority > ModelPair->CurrentModelPriority) - { + for (auto ModelPair = this->TaskModels.begin(); ModelPair != this->TaskModels.end(); ModelPair++) { + if (Priority > ModelPair->CurrentModelPriority) { this->TaskModels.insert(ModelPair, LocalPair); return; } @@ -109,24 +108,21 @@ void SysModelTask::AddNewObject(SysModel *NewModel, int32_t Priority) was specified at task creation. @param newPeriod The period that the task should run at going forward */ -void SysModelTask::updatePeriod(uint64_t newPeriod) +void +SysModelTask::updatePeriod(uint64_t newPeriod) { //! - If the requested time is above the min time, set the next time based on the previous time plus the new period - if(this->NextStartTime > this->TaskPeriod) - { - uint64_t newStartTime = (this->NextStartTime/newPeriod)*newPeriod; - if(newStartTime <= (this->NextStartTime - this->TaskPeriod)) - { + if (this->NextStartTime > this->TaskPeriod) { + uint64_t newStartTime = (this->NextStartTime / newPeriod) * newPeriod; + if (newStartTime <= (this->NextStartTime - this->TaskPeriod)) { newStartTime += newPeriod; } this->NextStartTime = newStartTime; } //! - Otherwise, we just should keep the original requested first call time for the task - else - { + else { this->NextStartTime = this->FirstTaskTime; } //! - Change the period of the task so that future calls will be based on the new period this->TaskPeriod = newPeriod; - } diff --git a/src/architecture/_GeneralModuleFiles/sys_model_task.h b/src/architecture/_GeneralModuleFiles/sys_model_task.h old mode 100755 new mode 100644 index 4d43c6818a..bc1319f824 --- a/src/architecture/_GeneralModuleFiles/sys_model_task.h +++ b/src/architecture/_GeneralModuleFiles/sys_model_task.h @@ -26,39 +26,44 @@ #include //! Structure used to pair a model and its requested priority -typedef struct { - int32_t CurrentModelPriority; //!< The current model priority. Higher goes first - SysModel *ModelPtr; //!< The model associated with this priority -}ModelPriorityPair; +typedef struct +{ + int32_t CurrentModelPriority; //!< The current model priority. Higher goes first + SysModel* ModelPtr; //!< The model associated with this priority +} ModelPriorityPair; //! Class used to group a set of models into one "Task" of execution class SysModelTask { -public: - SysModelTask()=default; - explicit SysModelTask(uint64_t InputPeriod, uint64_t FirstStartTime=0); //!< class method - ~SysModelTask()=default; - void AddNewObject(SysModel *NewModel, int32_t Priority = -1); + public: + SysModelTask() = default; + explicit SysModelTask(uint64_t InputPeriod, uint64_t FirstStartTime = 0); //!< class method + ~SysModelTask() = default; + void AddNewObject(SysModel* NewModel, int32_t Priority = -1); void SelfInitTaskList() const; - //void CrossInitTaskList(); + // void CrossInitTaskList(); void ExecuteTaskList(uint64_t CurrentSimTime); - void ResetTaskList(uint64_t CurrentSimTime); - void ResetTask() {this->NextStartTime = this->FirstTaskTime;} //!< Resets the task - void enableTask() {this->taskActive = true;} //!< Enables the task. Great comment huh? - void disableTask() {this->taskActive = false;} //!< Disables the task. I know. + void ResetTaskList(uint64_t CurrentSimTime); + void ResetTask() { this->NextStartTime = this->FirstTaskTime; } //!< Resets the task + void enableTask() { this->taskActive = true; } //!< Enables the task. Great comment huh? + void disableTask() { this->taskActive = false; } //!< Disables the task. I know. void updatePeriod(uint64_t newPeriod); - void updateParentProc(std::string const& parent) {this->parentProc = parent;} //!< Allows the system to move task to a different process + void updateParentProc(std::string const& parent) + { + this->parentProc = parent; + } //!< Allows the system to move task to a different process - std::vector TaskModels{}; //!< -- Array that has pointers to all task sysModels - std::string TaskName{}; //!< -- Identifier for Task - std::string parentProc; //!< -- Process that calls this task - uint64_t NextStartTime=0; //!< [ns] Next time to start task - uint64_t NextPickupTime=0; //!< [ns] Next time read Task outputs - uint64_t TaskPeriod=100; //!< [ns] Cycle rate for Task - uint64_t FirstTaskTime=0; //!< [ns] Time to start Task for first time. After this time the normal periodic updates resume. - bool taskActive=true; //!< -- Flag indicating whether the Task has been disabled - BSKLogger bskLogger; //!< -- BSK Logging + std::vector TaskModels{}; //!< -- Array that has pointers to all task sysModels + std::string TaskName{}; //!< -- Identifier for Task + std::string parentProc; //!< -- Process that calls this task + uint64_t NextStartTime = 0; //!< [ns] Next time to start task + uint64_t NextPickupTime = 0; //!< [ns] Next time read Task outputs + uint64_t TaskPeriod = 100; //!< [ns] Cycle rate for Task + uint64_t FirstTaskTime = + 0; //!< [ns] Time to start Task for first time. After this time the normal periodic updates resume. + bool taskActive = true; //!< -- Flag indicating whether the Task has been disabled + BSKLogger bskLogger; //!< -- BSK Logging }; #endif /* _SysModelTask_H_ */ diff --git a/src/architecture/alg_contain/alg_contain.cpp b/src/architecture/alg_contain/alg_contain.cpp old mode 100755 new mode 100644 index 736db36424..bcfa29e6b8 --- a/src/architecture/alg_contain/alg_contain.cpp +++ b/src/architecture/alg_contain/alg_contain.cpp @@ -23,19 +23,20 @@ AlgContain::AlgContain() AlgSelfInit = NULL; AlgUpdate = NULL; DataPtr = NULL; - AlgReset = NULL; + AlgReset = NULL; CallCounts = 0; return; } -AlgContain::AlgContain(void *DataIn, void(*UpPtr) (void*, uint64_t, uint64_t), +AlgContain::AlgContain(void* DataIn, + void (*UpPtr)(void*, uint64_t, uint64_t), void (*SelfPtr)(void*, uint64_t), - void (*ResetPtr) (void*, uint64_t, uint64_t)) + void (*ResetPtr)(void*, uint64_t, uint64_t)) { DataPtr = DataIn; AlgSelfInit = SelfPtr; AlgUpdate = UpPtr; - AlgReset = ResetPtr; + AlgReset = ResetPtr; } AlgContain::~AlgContain() @@ -43,27 +44,26 @@ AlgContain::~AlgContain() return; } -void AlgContain::SelfInit() +void +AlgContain::SelfInit() { - if(AlgSelfInit != NULL) - { - AlgSelfInit(DataPtr, (uint32_t) moduleID); + if (AlgSelfInit != NULL) { + AlgSelfInit(DataPtr, (uint32_t)moduleID); } } - -void AlgContain::UpdateState(uint64_t CurrentSimNanos) +void +AlgContain::UpdateState(uint64_t CurrentSimNanos) { - if(AlgUpdate != NULL) - { - AlgUpdate(DataPtr, CurrentSimNanos, (uint32_t) moduleID); + if (AlgUpdate != NULL) { + AlgUpdate(DataPtr, CurrentSimNanos, (uint32_t)moduleID); } } -void AlgContain::Reset(uint64_t CurrentSimNanos) +void +AlgContain::Reset(uint64_t CurrentSimNanos) { - if (AlgReset != NULL) - { - AlgReset(DataPtr, CurrentSimNanos, (uint32_t) moduleID); - } + if (AlgReset != NULL) { + AlgReset(DataPtr, CurrentSimNanos, (uint32_t)moduleID); + } } diff --git a/src/architecture/alg_contain/alg_contain.h b/src/architecture/alg_contain/alg_contain.h old mode 100755 new mode 100644 index 85e193930e..54c9e77658 --- a/src/architecture/alg_contain/alg_contain.h +++ b/src/architecture/alg_contain/alg_contain.h @@ -17,44 +17,43 @@ */ - #ifndef ALG_CONTAIN_H #define ALG_CONTAIN_H #include "architecture/_GeneralModuleFiles/sys_model.h" #include "architecture/utilities/bskLogging.h" - typedef void (*AlgPtr)(void*, uint64_t); typedef void (*AlgUpdatePtr)(void*, uint64_t, uint64_t); /*! @brief AlgContain Class */ -class AlgContain: public SysModel { -public: +class AlgContain : public SysModel +{ + public: AlgContain(); ~AlgContain(); - 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 - void UseReset(void(*LocPtr)(void*, uint64_t, uint64_t)) { AlgReset = LocPtr; } //!< method - void SelfInit(); //!< method - void UpdateState(uint64_t CurrentSimNanos); //!< method - void Reset(uint64_t CurrentSimNanos); //!< method - 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 - AlgUpdatePtr AlgReset; //!< class variable - AlgUpdatePtr AlgUpdate; //!< class variable - BSKLogger bskLogger; //!< -- BSK Logging + 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 + void UseReset(void (*LocPtr)(void*, uint64_t, uint64_t)) { AlgReset = LocPtr; } //!< method + void SelfInit(); //!< method + void UpdateState(uint64_t CurrentSimNanos); //!< method + void Reset(uint64_t CurrentSimNanos); //!< method + 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 + AlgUpdatePtr AlgReset; //!< class variable + AlgUpdatePtr AlgUpdate; //!< class variable + BSKLogger bskLogger; //!< -- BSK Logging }; - #endif diff --git a/src/architecture/messaging/_UnitTest/test_CMsgTimeWritten.py b/src/architecture/messaging/_UnitTest/test_CMsgTimeWritten.py index 725a545cab..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 @@ -72,4 +74,3 @@ def test_CMsgTimeWritten(): if __name__ == "__main__": CMsgTimeWritten() - 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/messaging.h b/src/architecture/messaging/messaging.h index 600ad67bfb..3fa4a2621f 100644 --- a/src/architecture/messaging/messaging.h +++ b/src/architecture/messaging/messaging.h @@ -33,168 +33,194 @@ class Recorder; /*! Read functors have read-only access to messages*/ template -class ReadFunctor{ -private: - messageType* payloadPointer; //!< -- pointer to the incoming msg data - MsgHeader *headerPointer; //!< -- pointer to the incoming msg header - bool initialized; //!< -- flag indicating if the input message is connect to another message - -public: +class ReadFunctor +{ + private: + messageType* payloadPointer; //!< -- pointer to the incoming msg data + MsgHeader* headerPointer; //!< -- pointer to the incoming msg header + bool initialized; //!< -- flag indicating if the input message is connect to another message + + public: //!< -- BSK Logging - BSKLogger bskLogger; //!< -- bsk logging instance - messageType zeroMsgPayload ={}; //!< -- zero'd copy of the message payload type - + BSKLogger bskLogger; //!< -- bsk logging instance + messageType zeroMsgPayload = {}; //!< -- zero'd copy of the message payload type //! constructor - ReadFunctor() : initialized(false) {}; + ReadFunctor() + : initialized(false) {}; //! constructor - ReadFunctor(messageType* payloadPtr, MsgHeader *headerPtr) : payloadPointer(payloadPtr), headerPointer(headerPtr), initialized(true){}; + ReadFunctor(messageType* payloadPtr, MsgHeader* headerPtr) + : payloadPointer(payloadPtr) + , headerPointer(headerPtr) + , initialized(true) {}; //! constructor - const messageType& operator()(){ + const messageType& operator()() + { if (!this->initialized) { messageType var; - bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are trying to read an un-connected message of type %s\nThis program is about to self destruct.", typeid(var).name()); + bskLogger.bskLog(BSK_ERROR, + "In C++ read functor, you are trying to read an un-connected message of type %s\nThis " + "program is about to self destruct.", + typeid(var).name()); } return *this->payloadPointer; - }; //! check if this msg has been connected to - bool isLinked(){return this->initialized;}; // something that can be checked so that uninitialized messages aren't read. + bool isLinked() + { + return this->initialized; + }; // something that can be checked so that uninitialized messages aren't read. //! check if the message has been ever written to - bool isWritten(){ + bool isWritten() + { if (this->initialized) { return this->headerPointer->isWritten; } else { messageType var; - bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are checking if an unconnected msg of type %s is written.", typeid(var).name()); + bskLogger.bskLog(BSK_ERROR, + "In C++ read functor, you are checking if an unconnected msg of type %s is written.", + typeid(var).name()); return false; } }; //! return the time at which the message was written - uint64_t timeWritten(){ + uint64_t timeWritten() + { if (!this->initialized) { messageType var; - bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are requesting the write time of an unconnected msg of type %s.", typeid(var).name()); + bskLogger.bskLog(BSK_ERROR, + "In C++ read functor, you are requesting the write time of an unconnected msg of type %s.", + typeid(var).name()); return 0; } return this->headerPointer->timeWritten; }; //! return the moduleID of who wrote wrote the message - int64_t moduleID(){ + int64_t moduleID() + { if (!this->initialized) { messageType var; - bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are requesting moduleID of an unconnected msg of type %s.", typeid(var).name()); + bskLogger.bskLog(BSK_ERROR, + "In C++ read functor, you are requesting moduleID of an unconnected msg of type %s.", + typeid(var).name()); return 0; } if (!this->headerPointer->isWritten) { messageType var; - bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are requesting moduleID of an unwritten msg of type %s.", typeid(var).name()); + bskLogger.bskLog(BSK_ERROR, + "In C++ read functor, you are requesting moduleID of an unwritten msg of type %s.", + typeid(var).name()); return 0; } return this->headerPointer->moduleID; }; //! subscribe to a C message - void subscribeToC(void* source){ + void subscribeToC(void* source) + { // this method works by knowing that the first member of a C message is the header. - this->headerPointer = (MsgHeader*) source; + this->headerPointer = (MsgHeader*)source; // advance the address to connect to C-wrapped message payload // this assumes the header memory is aligned with 0 additional padding MsgHeader* pt = this->headerPointer; - this->payloadPointer = (messageType *) (++pt); - + this->payloadPointer = (messageType*)(++pt); // set flag that this input message is connected to another message - this->initialized = true; // set input message as linked - this->headerPointer->isLinked = 1; // set source output message as linked + this->initialized = true; // set input message as linked + this->headerPointer->isLinked = 1; // set source output message as linked }; //! Subscribe to the message located at the sourceAddr in memory void subscribeToAddr(uint64_t sourceAddr) { - //!Cast a pointer at the sourceAddr and call the regular subscribe - Message *source = - reinterpret_cast *> (sourceAddr); - subscribeTo(source); + //! Cast a pointer at the sourceAddr and call the regular subscribe + Message* source = reinterpret_cast*>(sourceAddr); + subscribeTo(source); } //! Subscribe to the C message located at the sourceAddr in memory void subscribeToCAddr(uint64_t sourceAddr) { //! Cast a void* pointer and call the regular C-subscribe method - void *source = reinterpret_cast (sourceAddr); + void* source = reinterpret_cast(sourceAddr); subscribeToC(source); } //! Subscribe to a C++ message - void subscribeTo(Message *source){ + void subscribeTo(Message* source) + { *this = source->addSubscriber(); this->initialized = true; }; //! Unsubscribe to the connected message, noop if no message was connected - void unsubscribe(){ + void unsubscribe() + { this->payloadPointer = nullptr; this->headerPointer = nullptr; this->initialized = false; } //! Check if self has been subscribed to a C message - uint8_t isSubscribedToC(void *source){ + uint8_t isSubscribedToC(void* source) + { - int8_t firstCheck = (this->headerPointer == (MsgHeader*) source); + int8_t firstCheck = (this->headerPointer == (MsgHeader*)source); MsgHeader* pt = this->headerPointer; - int8_t secondCheck = (this->payloadPointer == (messageType *) (++pt)); + int8_t secondCheck = (this->payloadPointer == (messageType*)(++pt)); return (this->initialized && firstCheck && secondCheck); - }; //! Check if self has been subscribed to a Cpp message - uint8_t isSubscribedTo(Message *source){ + uint8_t isSubscribedTo(Message* source) + { - MsgHeader *dummyMsgPtr; + MsgHeader* dummyMsgPtr; int8_t firstCheck = (this->payloadPointer == source->getMsgPointers(&(dummyMsgPtr))); int8_t secondCheck = (this->headerPointer == dummyMsgPtr); - return (this->initialized && firstCheck && secondCheck ); - + return (this->initialized && firstCheck && secondCheck); }; //! Check if self has been subscribed to the message at sourceAddr uint8_t isSubscribedToAddr(uint64_t sourceAddr) { - //!Cast a pointer at the sourceAddr and call the regular is-subscribe - Message *source = reinterpret_cast *> (sourceAddr); - return(isSubscribedTo(source)); + //! Cast a pointer at the sourceAddr and call the regular is-subscribe + Message* source = reinterpret_cast*>(sourceAddr); + return (isSubscribedTo(source)); } //! Check if self has been subscribed to the message at sourceAddr uint8_t isSubscribedToCAddr(uint64_t sourceAddr) { - //!Cast a void* pointer at the sourceAddr and call the regular is-subscribe - void *source = reinterpret_cast (sourceAddr); - return(isSubscribedToC(source)); + //! Cast a void* pointer at the sourceAddr and call the regular is-subscribe + void* source = reinterpret_cast(sourceAddr); + return (isSubscribedToC(source)); } //! Recorder method description - Recorder recorder(uint64_t timeDiff = 0){return Recorder(this, timeDiff);} + Recorder recorder(uint64_t timeDiff = 0) { return Recorder(this, timeDiff); } }; /*! Write Functor */ template -class WriteFunctor{ -private: - messageType* payloadPointer; //!< pointer to the message payload - MsgHeader* headerPointer; //!< pointer to the message header -public: +class WriteFunctor +{ + private: + messageType* payloadPointer; //!< pointer to the message payload + MsgHeader* headerPointer; //!< pointer to the message header + public: //! write functor constructor - WriteFunctor(){}; + WriteFunctor() {}; //! write functor constructor - WriteFunctor(messageType* payloadPointer, MsgHeader *headerPointer) : payloadPointer(payloadPointer), headerPointer(headerPointer){}; + WriteFunctor(messageType* payloadPointer, MsgHeader* headerPointer) + : payloadPointer(payloadPointer) + , headerPointer(headerPointer) {}; //! write functor constructor - void operator()(messageType *payload, int64_t moduleID, uint64_t callTime){ + void operator()(messageType* payload, int64_t moduleID, uint64_t callTime) + { *this->payloadPointer = *payload; this->headerPointer->isWritten = 1; this->headerPointer->timeWritten = callTime; @@ -210,12 +236,13 @@ class Recorder; * base class template for bsk messages */ template -class Message{ -private: - messageType payload = {}; //!< struct defining message payload, zero'd on creation - MsgHeader header = {}; //!< struct defining the message header, zero'd on creation - ReadFunctor read = ReadFunctor(&payload, &header); //!< read functor instance -public: +class Message +{ + private: + messageType payload = {}; //!< struct defining message payload, zero'd on creation + MsgHeader header = {}; //!< struct defining the message header, zero'd on creation + ReadFunctor read = ReadFunctor(&payload, &header); //!< read functor instance + public: //! write functor to this message WriteFunctor write = WriteFunctor(&payload, &header); //! -- request read rights. returns reference to class ``read`` variable @@ -223,67 +250,77 @@ class Message{ //! -- request write rights. WriteFunctor addAuthor(); //! for plain ole c modules - messageType* subscribeRaw(MsgHeader **msgPtr); + messageType* subscribeRaw(MsgHeader** msgPtr); //! for plain ole c modules - messageType* getMsgPointers(MsgHeader **msgPtr); + messageType* getMsgPointers(MsgHeader** msgPtr); //! Recorder object - Recorder recorder(uint64_t timeDiff = 0){return Recorder(this, timeDiff);} + Recorder recorder(uint64_t timeDiff = 0) { return Recorder(this, timeDiff); } - messageType zeroMsgPayload = {}; //!< zero'd copy of the message payload structure + messageType zeroMsgPayload = {}; //!< zero'd copy of the message payload structure //! check if this msg has been connected to - bool isLinked(){return this->header.isLinked;}; + bool isLinked() { return this->header.isLinked; }; //! Return the memory size of the payload, be careful about dynamically sized things - uint64_t getPayloadSize() {return sizeof(messageType);}; + uint64_t getPayloadSize() { return sizeof(messageType); }; }; - template -ReadFunctor Message::addSubscriber(){ +ReadFunctor +Message::addSubscriber() +{ this->header.isLinked = 1; return this->read; } template -WriteFunctor Message::addAuthor(){ +WriteFunctor +Message::addAuthor() +{ return this->write; } template -messageType* Message::subscribeRaw(MsgHeader **msgPtr){ +messageType* +Message::subscribeRaw(MsgHeader** msgPtr) +{ *msgPtr = &this->header; this->header.isLinked = 1; return &this->payload; } template -messageType* Message::getMsgPointers(MsgHeader **msgPtr){ +messageType* +Message::getMsgPointers(MsgHeader** msgPtr) +{ *msgPtr = &this->header; return &this->payload; } /*! Keep a time history of messages accessible to users from python */ template -class Recorder : public SysModel{ -public: - Recorder(){}; +class Recorder : public SysModel +{ + public: + Recorder() {}; //! -- Use this to record cpp messages - Recorder(Message* message, uint64_t timeDiff = 0){ + Recorder(Message* message, uint64_t timeDiff = 0) + { this->timeInterval = timeDiff; this->readMessage = message->addSubscriber(); this->ModelTag = "Rec:" + findMsgName(std::string(typeid(*message).name())); } //! -- Use this to record C messages - Recorder(void* message, uint64_t timeDiff = 0){ + Recorder(void* message, uint64_t timeDiff = 0) + { this->timeInterval = timeDiff; - MsgHeader* msgPt = (MsgHeader *) message; - MsgHeader *pt = msgPt; + MsgHeader* msgPt = (MsgHeader*)message; + MsgHeader* pt = msgPt; messageType* payloadPointer; - payloadPointer = (messageType *) (++pt); + payloadPointer = (messageType*)(++pt); this->readMessage = ReadFunctor(payloadPointer, msgPt); this->ModelTag = "Rec:"; @@ -292,28 +329,35 @@ class Recorder : public SysModel{ this->ModelTag += findMsgName(msgName); } //! -- Use this to keep track of what someone is reading - Recorder(ReadFunctor* messageReader, uint64_t timeDiff = 0){ + Recorder(ReadFunctor* messageReader, uint64_t timeDiff = 0) + { this->timeInterval = timeDiff; this->readMessage = *messageReader; if (!messageReader->isLinked()) { messageType var; - bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are requesting to record an un-connected input message of type %s.", typeid(var).name()); + bskLogger.bskLog( + BSK_ERROR, + "In C++ read functor, you are requesting to record an un-connected input message of type %s.", + typeid(var).name()); } this->ModelTag = "Rec:" + findMsgName(std::string(typeid(*messageReader).name())); } - ~Recorder(){}; + ~Recorder() {}; //! -- self initialization - void SelfInit(){}; + void SelfInit() {}; //! -- cross initialization - void IntegratedInit(){}; + void IntegratedInit() {}; //! -- Read and record the message - void UpdateState(uint64_t CurrentSimNanos){ + void UpdateState(uint64_t CurrentSimNanos) + { if (CurrentSimNanos >= this->nextUpdateTime) { // Log warning if message is invalid but don't change behavior if (!this->readMessage.isLinked() || !this->readMessage.isWritten()) { messageType var; - bskLogger.bskLog(BSK_WARNING, "Recording message of type %s that is not properly initialized or written", typeid(var).name()); + bskLogger.bskLog(BSK_WARNING, + "Recording message of type %s that is not properly initialized or written", + typeid(var).name()); } // Record the message @@ -324,63 +368,64 @@ class Recorder : public SysModel{ } }; //! Reset method - void Reset(uint64_t CurrentSimNanos){ - this->msgRecord.clear(); //!< -- Can only reset to 0 for now + void Reset(uint64_t CurrentSimNanos) + { + this->msgRecord.clear(); //!< -- Can only reset to 0 for now this->msgRecordTimes.clear(); this->msgWrittenTimes.clear(); this->nextUpdateTime = CurrentSimNanos; }; //! time recorded method - std::vector& times(){return this->msgRecordTimes;} + std::vector& times() { return this->msgRecordTimes; } //! time written method - std::vector& timesWritten(){return this->msgWrittenTimes;} + std::vector& timesWritten() { return this->msgWrittenTimes; } //! record method - std::vector& record(){return this->msgRecord;}; + std::vector& record() { return this->msgRecord; }; //! size of the record so far - size_t size(){return this->record().size();} + size_t size() { return this->record().size(); } //! determine message name - std::string findMsgName(std::string msgName) { + std::string findMsgName(std::string msgName) + { size_t locMsg = msgName.find("Payload"); if (locMsg != std::string::npos) { msgName.erase(locMsg, std::string::npos); } locMsg = msgName.find("Message"); if (locMsg != std::string::npos) { - msgName.replace(locMsg, 7, ""); + msgName.replace(locMsg, 7, ""); } - for (int c = 0; c<10; c++) { + for (int c = 0; c < 10; c++) { locMsg = msgName.rfind(std::to_string(c)); if (locMsg != std::string::npos) { - msgName.erase(0, locMsg+1); + msgName.erase(0, locMsg + 1); } } return msgName; }; //! clear the recorded messages, i.e. purge the history - void clear(){ + void clear() + { this->msgRecord.clear(); this->msgRecordTimes.clear(); this->msgWrittenTimes.clear(); }; - BSKLogger bskLogger; //!< -- BSK Logging + BSKLogger bskLogger; //!< -- BSK Logging //! method to update the minimum time interval before recording the next message - void updateTimeInterval(uint64_t timeDiff) { - this->timeInterval = timeDiff; - }; + void updateTimeInterval(uint64_t timeDiff) { this->timeInterval = timeDiff; }; -private: - std::vector msgRecord; //!< vector of recorded messages - std::vector msgRecordTimes; //!< vector of times at which messages are recorded - std::vector msgWrittenTimes; //!< vector of times at which messages are written - uint64_t nextUpdateTime = 0; //!< [ns] earliest time at which the msg is recorded again - uint64_t timeInterval; //!< [ns] recording time intervale + private: + std::vector msgRecord; //!< vector of recorded messages + std::vector msgRecordTimes; //!< vector of times at which messages are recorded + std::vector msgWrittenTimes; //!< vector of times at which messages are written + uint64_t nextUpdateTime = 0; //!< [ns] earliest time at which the msg is recorded again + uint64_t timeInterval; //!< [ns] recording time intervale -private: - ReadFunctor readMessage; //!< method description + private: + ReadFunctor readMessage; //!< method description }; #endif 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/messaging/msgHeader.h b/src/architecture/messaging/msgHeader.h index 8dcfafac34..58de10d51c 100644 --- a/src/architecture/messaging/msgHeader.h +++ b/src/architecture/messaging/msgHeader.h @@ -20,11 +20,13 @@ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF #define msgHeader_h /*! @brief message system 2 header information structure */ -typedef struct { - int64_t isLinked; //!< flag if the message has is connected to another message - int64_t isWritten; //!< flag if the message conntent has ever been written - uint64_t timeWritten; //!< [ns] time the message was written - int64_t moduleID; //!< ID of the module who wrote the message, negative value for Python module, non-negative for C/C++ modules -}MsgHeader; +typedef struct +{ + int64_t isLinked; //!< flag if the message has is connected to another message + int64_t isWritten; //!< flag if the message conntent has ever been written + uint64_t timeWritten; //!< [ns] time the message was written + int64_t moduleID; //!< ID of the module who wrote the message, negative value for Python module, non-negative for + //!< C/C++ modules +} MsgHeader; #endif /* msgHeader_h */ diff --git a/src/architecture/msgPayloadDefC/AccDataMsgPayload.h b/src/architecture/msgPayloadDefC/AccDataMsgPayload.h old mode 100755 new mode 100644 index 76f6b2b662..fdff92654d --- a/src/architecture/msgPayloadDefC/AccDataMsgPayload.h +++ b/src/architecture/msgPayloadDefC/AccDataMsgPayload.h @@ -20,17 +20,14 @@ #ifndef _ACC_PKT_DATA_MESSAGE_H #define _ACC_PKT_DATA_MESSAGE_H - #define MAX_ACC_BUF_PKT 120 #include "AccPktDataMsgPayload.h" - /*! @brief Structure used to define accelerometer package data */ -typedef struct { +typedef struct +{ AccPktDataMsgPayload accPkts[MAX_ACC_BUF_PKT]; //!< [-] Accelerometer buffer read in -}AccDataMsgPayload; - - +} AccDataMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/AccPktDataMsgPayload.h b/src/architecture/msgPayloadDefC/AccPktDataMsgPayload.h old mode 100755 new mode 100644 index 056a57055f..d62dbce344 --- a/src/architecture/msgPayloadDefC/AccPktDataMsgPayload.h +++ b/src/architecture/msgPayloadDefC/AccPktDataMsgPayload.h @@ -21,13 +21,12 @@ #ifndef _ACC_DATA_MESSAGE_H #define _ACC_DATA_MESSAGE_H - /*! @brief Structure used to define accelerometer package data */ -typedef struct { - uint64_t measTime; //!< [Tick] Measurement time for accel - double gyro_B[3]; //!< [r/s] Angular rate measurement from gyro - double accel_B[3]; //!< [m/s2] Acceleration in platform frame -}AccPktDataMsgPayload; - +typedef struct +{ + uint64_t measTime; //!< [Tick] Measurement time for accel + double gyro_B[3]; //!< [r/s] Angular rate measurement from gyro + double accel_B[3]; //!< [m/s2] Acceleration in platform frame +} AccPktDataMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/AccessMsgPayload.h b/src/architecture/msgPayloadDefC/AccessMsgPayload.h index f8b2305e15..29590ec03a 100644 --- a/src/architecture/msgPayloadDefC/AccessMsgPayload.h +++ b/src/architecture/msgPayloadDefC/AccessMsgPayload.h @@ -23,20 +23,20 @@ /*! @brief Message that defines access to spacecraft from a groundLocation, providing access, range, and elevation with * respect to a ground location. */ -typedef struct { - uint64_t hasAccess;//!< [-] 1 when the writer has access to a spacecraft; 0 otherwise. - double slantRange;//!< [m] Range from a location to the spacecraft. - double elevation;//!< [rad] Elevation angle for a given spacecraft. - double azimuth; //!< [rad] Azimuth angle for a spacecraft. - double range_dot; //!< [m/s] Range rate of a given spacecraft relative to a location in the SEZ rotating frame. - double el_dot; //!< [rad/s] Elevation angle rate for a given spacecraft in the SEZ rotating frame. - double az_dot; //!< [rad/s] Azimuth angle rate for a given spacecraft in the SEZ rotating frame. - double r_BL_L[3]; //!<[m] Spacecraft position relative to the groundLocation in the SEZ frame. - double v_BL_L[3]; //!<[m/s] SEZ relative time derivative of r_BL vector in SEZ vector components. - uint64_t hasIllumination;//!< [-] 1 when illumination constraints are met; 0 otherwise. +typedef struct +{ + uint64_t hasAccess; //!< [-] 1 when the writer has access to a spacecraft; 0 otherwise. + double slantRange; //!< [m] Range from a location to the spacecraft. + double elevation; //!< [rad] Elevation angle for a given spacecraft. + double azimuth; //!< [rad] Azimuth angle for a spacecraft. + double range_dot; //!< [m/s] Range rate of a given spacecraft relative to a location in the SEZ rotating frame. + double el_dot; //!< [rad/s] Elevation angle rate for a given spacecraft in the SEZ rotating frame. + double az_dot; //!< [rad/s] Azimuth angle rate for a given spacecraft in the SEZ rotating frame. + double r_BL_L[3]; //!<[m] Spacecraft position relative to the groundLocation in the SEZ frame. + double v_BL_L[3]; //!<[m/s] SEZ relative time derivative of r_BL vector in SEZ vector components. + uint64_t hasIllumination; //!< [-] 1 when illumination constraints are met; 0 otherwise. double sunIncidenceAngle; //!<[rad] Angle between bore-sight and Sun vector - double scViewAngle; //!<[rad] Angle between bore-sight and deputy SC vector -}AccessMsgPayload; - + double scViewAngle; //!<[rad] Angle between bore-sight and deputy SC vector +} AccessMsgPayload; #endif /* accessSimMsg.h */ diff --git a/src/architecture/msgPayloadDefC/AlbedoMsgPayload.h b/src/architecture/msgPayloadDefC/AlbedoMsgPayload.h index 5cfe9858d8..1c3809f562 100644 --- a/src/architecture/msgPayloadDefC/AlbedoMsgPayload.h +++ b/src/architecture/msgPayloadDefC/AlbedoMsgPayload.h @@ -21,14 +21,15 @@ #define albedoSimMsg_H /*! albedo message definition */ -typedef struct { +typedef struct +{ // Maximum albedo acting on the instrument - // considering the instrument's position - double albedoAtInstrumentMax; //!< [-] Max albedo flux ratio at instrument - double AfluxAtInstrumentMax; //!< [W/m^2] Max albedo flux at instrument + // 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 // considering the unit normal and fov of the instrument in addition to the position - double albedoAtInstrument; //!< [-] Albedo flux ratio at instrument - double AfluxAtInstrument; //!< [W/m^2] Albedo flux at instrument -}AlbedoMsgPayload; + double albedoAtInstrument; //!< [-] Albedo flux ratio at instrument + double AfluxAtInstrument; //!< [W/m^2] Albedo flux at instrument +} AlbedoMsgPayload; #endif /* albedoSimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/ArrayEffectorLockMsgPayload.h b/src/architecture/msgPayloadDefC/ArrayEffectorLockMsgPayload.h old mode 100755 new mode 100644 index 33e5223aa4..16e451cf27 --- a/src/architecture/msgPayloadDefC/ArrayEffectorLockMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ArrayEffectorLockMsgPayload.h @@ -20,15 +20,12 @@ #ifndef ARRAY_EFFECTOR_LOCK_H #define ARRAY_EFFECTOR_LOCK_H - #include "architecture/utilities/macroDefinitions.h" - - /*! @brief Structure used to define the output definition for vehicle effectors*/ -typedef struct { - int effectorLockFlag[MAX_EFF_CNT]; //!< effector lock flag; 0 : do not lock; 1 : lock -}ArrayEffectorLockMsgPayload; - +typedef struct +{ + int effectorLockFlag[MAX_EFF_CNT]; //!< effector lock flag; 0 : do not lock; 1 : lock +} ArrayEffectorLockMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/ArrayMotorForceMsgPayload.h b/src/architecture/msgPayloadDefC/ArrayMotorForceMsgPayload.h index 60d244d3d1..e8e9ab8024 100644 --- a/src/architecture/msgPayloadDefC/ArrayMotorForceMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ArrayMotorForceMsgPayload.h @@ -20,15 +20,12 @@ #ifndef ARRAY_MOTOR_FORCE_H #define ARRAY_MOTOR_FORCE_H - #include "architecture/utilities/macroDefinitions.h" - - /*! @brief Structure used to define the output definition for vehicle effectors*/ -typedef struct { - double motorForce[MAX_EFF_CNT]; //!< [N] motor force array -}ArrayMotorForceMsgPayload; - +typedef struct +{ + double motorForce[MAX_EFF_CNT]; //!< [N] motor force array +} ArrayMotorForceMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/ArrayMotorTorqueMsgPayload.h b/src/architecture/msgPayloadDefC/ArrayMotorTorqueMsgPayload.h old mode 100755 new mode 100644 index 4af9645e08..92864c3069 --- a/src/architecture/msgPayloadDefC/ArrayMotorTorqueMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ArrayMotorTorqueMsgPayload.h @@ -20,15 +20,12 @@ #ifndef ARRAY_MOTOR_TORQUE_H #define ARRAY_MOTOR_TORQUE_H - #include "architecture/utilities/macroDefinitions.h" - - /*! @brief Structure used to define the output definition for vehicle effectors*/ -typedef struct { - double motorTorque[MAX_EFF_CNT]; //!< [Nm] motor torque array -}ArrayMotorTorqueMsgPayload; - +typedef struct +{ + double motorTorque[MAX_EFF_CNT]; //!< [Nm] motor torque array +} ArrayMotorTorqueMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/ArrayMotorVoltageMsgPayload.h b/src/architecture/msgPayloadDefC/ArrayMotorVoltageMsgPayload.h index 812eabcd87..165f22ab86 100644 --- a/src/architecture/msgPayloadDefC/ArrayMotorVoltageMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ArrayMotorVoltageMsgPayload.h @@ -20,15 +20,12 @@ #ifndef SIM_RW_VOLTAGE_INPUT_H #define SIM_RW_VOLTAGE_INPUT_H - #include "architecture/utilities/macroDefinitions.h" - - /*! @brief Structure used to define the message format of the motor voltage input */ -typedef struct { +typedef struct +{ double voltage[MAX_EFF_CNT]; //!< [V] Motor voltage input value -}ArrayMotorVoltageMsgPayload; - +} ArrayMotorVoltageMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/AtmoPropsMsgPayload.h b/src/architecture/msgPayloadDefC/AtmoPropsMsgPayload.h index 3f4cd8e120..0f17f2025a 100644 --- a/src/architecture/msgPayloadDefC/AtmoPropsMsgPayload.h +++ b/src/architecture/msgPayloadDefC/AtmoPropsMsgPayload.h @@ -21,8 +21,9 @@ #define AtmoPropsSimMsg_H /*! atmospheric property message definition */ -typedef struct { - double neutralDensity; //!< kg/m^3 Local neutral particle density - double localTemp; //!< K Local avg particle temperature -}AtmoPropsMsgPayload; +typedef struct +{ + double neutralDensity; //!< kg/m^3 Local neutral particle density + double localTemp; //!< K Local avg particle temperature +} AtmoPropsMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/AttGuidMsgPayload.h b/src/architecture/msgPayloadDefC/AttGuidMsgPayload.h old mode 100755 new mode 100644 index 5021dd2220..a98926b652 --- a/src/architecture/msgPayloadDefC/AttGuidMsgPayload.h +++ b/src/architecture/msgPayloadDefC/AttGuidMsgPayload.h @@ -20,15 +20,14 @@ #ifndef ATT_GUID_MESSAGE_H #define ATT_GUID_MESSAGE_H - - /*! @brief Structure used to define the output definition for attitude guidance*/ -typedef struct { - double sigma_BR[3]; //!< Current attitude error estimate (MRPs) of B relative to R*/ - double omega_BR_B[3]; //!< [r/s] Current body error estimate of B relateive to R in B frame compoonents */ - double omega_RN_B[3]; //!< [r/s] Reference frame rate vector of the of R relative to N in B frame components */ - double domega_RN_B[3]; //!< [r/s2] Reference frame inertial body acceleration of R relative to N in B frame components */ -}AttGuidMsgPayload; - +typedef struct +{ + double sigma_BR[3]; //!< Current attitude error estimate (MRPs) of B relative to R*/ + double omega_BR_B[3]; //!< [r/s] Current body error estimate of B relateive to R in B frame compoonents */ + double omega_RN_B[3]; //!< [r/s] Reference frame rate vector of the of R relative to N in B frame components */ + double + domega_RN_B[3]; //!< [r/s2] Reference frame inertial body acceleration of R relative to N in B frame components */ +} AttGuidMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/AttRefMsgPayload.h b/src/architecture/msgPayloadDefC/AttRefMsgPayload.h old mode 100755 new mode 100644 index cf8d86c328..0352c155c9 --- a/src/architecture/msgPayloadDefC/AttRefMsgPayload.h +++ b/src/architecture/msgPayloadDefC/AttRefMsgPayload.h @@ -20,14 +20,12 @@ #ifndef ATT_REF_MESSAGE_H #define ATT_REF_MESSAGE_H - - /*! @brief Structure used to define the output definition for attitude reference generation */ -typedef struct { - double sigma_RN[3]; //!< MRP Reference attitude of R relative to N - double omega_RN_N[3]; //!< [r/s] Reference frame rate vector of the of R relative to N in N frame components - double domega_RN_N[3]; //!< [r/s2] Reference frame inertial acceleration of R relative to N in N frame components -}AttRefMsgPayload; - +typedef struct +{ + double sigma_RN[3]; //!< MRP Reference attitude of R relative to N + double omega_RN_N[3]; //!< [r/s] Reference frame rate vector of the of R relative to N in N frame components + double domega_RN_N[3]; //!< [r/s2] Reference frame inertial acceleration of R relative to N in N frame components +} AttRefMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/AttStateMsgPayload.h b/src/architecture/msgPayloadDefC/AttStateMsgPayload.h old mode 100755 new mode 100644 index 7e949acad0..c0b8bb3b0d --- a/src/architecture/msgPayloadDefC/AttStateMsgPayload.h +++ b/src/architecture/msgPayloadDefC/AttStateMsgPayload.h @@ -20,13 +20,13 @@ #ifndef ATT_STATE_MESSAGE_H #define ATT_STATE_MESSAGE_H - - /*! @brief Structure used to define the output euler set for attitude reference generation */ -typedef struct { - double state[3]; //!< [] 3D attitude orientation coordinate set The units depend on the attitude coordinate chosen and can be either radians (i.e. Euler angles) or dimensionless (i.e. MRP, quaternions, etc.) - double rate[3]; //!< [] 3D attitude rate coordinate set. These rate coordinates can be either omega (in rad/sec) or attitude coordiante rates with appropriate units -}AttStateMsgPayload; - +typedef struct +{ + double state[3]; //!< [] 3D attitude orientation coordinate set The units depend on the attitude coordinate chosen + //!< and can be either radians (i.e. Euler angles) or dimensionless (i.e. MRP, quaternions, etc.) + double rate[3]; //!< [] 3D attitude rate coordinate set. These rate coordinates can be either omega (in rad/sec) + //!< or attitude coordiante rates with appropriate units +} AttStateMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/BodyHeadingMsgPayload.h b/src/architecture/msgPayloadDefC/BodyHeadingMsgPayload.h index fdfb7b0689..fa1ee7854d 100644 --- a/src/architecture/msgPayloadDefC/BodyHeadingMsgPayload.h +++ b/src/architecture/msgPayloadDefC/BodyHeadingMsgPayload.h @@ -20,7 +20,6 @@ #ifndef bodyHeadingSimMsg_h #define bodyHeadingSimMsg_h - //!@brief Planet heading message definition. /*! Many modules in Basilisk utilize the spacecraft body frame heading to something. For instance, a spacecraft may want to point at a point on earth, the sun, another planet @@ -28,9 +27,9 @@ in being agnostic of the thing being pointed to while not including separate information about attitude and rates that are not necessarily desired */ -typedef struct { - double rHat_XB_B[3]; //!< [] unit heading vector to any thing "X" in the spacecraft, "B", body frame -}BodyHeadingMsgPayload; - +typedef struct +{ + double rHat_XB_B[3]; //!< [] unit heading vector to any thing "X" in the spacecraft, "B", body frame +} BodyHeadingMsgPayload; #endif /* bodyHeadingSimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/BoreAngleMsgPayload.h b/src/architecture/msgPayloadDefC/BoreAngleMsgPayload.h old mode 100755 new mode 100644 index b308e852f0..18aeef1986 --- a/src/architecture/msgPayloadDefC/BoreAngleMsgPayload.h +++ b/src/architecture/msgPayloadDefC/BoreAngleMsgPayload.h @@ -20,13 +20,11 @@ #ifndef SIM_BORE_ANGLE_H #define SIM_BORE_ANGLE_H - /*! @brief Structure used to compute a angle between boresight and body */ -typedef struct { - double azimuth; //!< [r] the location angle to put the miss in a quadrant - double missAngle; //!< [r] the angular distance between the boresight and body -}BoreAngleMsgPayload; - - +typedef struct +{ + double azimuth; //!< [r] the location angle to put the miss in a quadrant + double missAngle; //!< [r] the angular distance between the boresight and body +} BoreAngleMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/CMEstDataMsgPayload.h b/src/architecture/msgPayloadDefC/CMEstDataMsgPayload.h old mode 100755 new mode 100644 index 92ebbdeeac..2d591e210d --- a/src/architecture/msgPayloadDefC/CMEstDataMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CMEstDataMsgPayload.h @@ -21,15 +21,14 @@ #define _CM_EST_DATA_MESSAGE_H /*! @brief Structure used to define CM estimation output data */ -typedef struct { - double attError; //!< [-] attitude convergence error - double state[3]; //!< [m] estimated cm location - double stateError[3]; //!< [m] errpr w.r.t. truth - double covariance[3]; //!< [m^2] CM estimated covariance - double preFitRes[3]; //!< [Nm] pre-fit torque residuals - double postFitRes[3]; //!< [Nm] post-fit torque residuals -}CMEstDataMsgPayload; - - +typedef struct +{ + double attError; //!< [-] attitude convergence error + double state[3]; //!< [m] estimated cm location + double stateError[3]; //!< [m] errpr w.r.t. truth + double covariance[3]; //!< [m^2] CM estimated covariance + double preFitRes[3]; //!< [Nm] pre-fit torque residuals + double postFitRes[3]; //!< [Nm] post-fit torque residuals +} CMEstDataMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/CModuleTemplateMsgPayload.h b/src/architecture/msgPayloadDefC/CModuleTemplateMsgPayload.h old mode 100755 new mode 100644 index 237be41b24..4107140715 --- a/src/architecture/msgPayloadDefC/CModuleTemplateMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CModuleTemplateMsgPayload.h @@ -20,12 +20,11 @@ #ifndef _FSW_MODULE_TEMPLATE_OUT_H_ #define _FSW_MODULE_TEMPLATE_OUT_H_ - /*! @brief Structure used to define the output of the sub-module. This is the same output message that is used by all sub-modules in the module folder. */ -typedef struct { - double dataVector[3]; //!< [units] sample message vector -}CModuleTemplateMsgPayload; - +typedef struct +{ + double dataVector[3]; //!< [units] sample message vector +} CModuleTemplateMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/CSSArraySensorMsgPayload.h b/src/architecture/msgPayloadDefC/CSSArraySensorMsgPayload.h old mode 100755 new mode 100644 index 0060cba3e1..a06be821aa --- a/src/architecture/msgPayloadDefC/CSSArraySensorMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CSSArraySensorMsgPayload.h @@ -22,11 +22,13 @@ #include "architecture/utilities/macroDefinitions.h" - -/*! @brief Output structure for CSS array or constellation interface. Each element contains the raw measurement which should be a cosine value nominally */ -typedef struct { - double timeTag; //!< [s] Current vehicle time-tag associated with measurements - double CosValue[MAX_NUM_CSS_SENSORS]; //!< Current measured CSS value (ideally a cosine value) for the constellation of CSS sensors -}CSSArraySensorMsgPayload; +/*! @brief Output structure for CSS array or constellation interface. Each element contains the raw measurement which + * should be a cosine value nominally */ +typedef struct +{ + double timeTag; //!< [s] Current vehicle time-tag associated with measurements + double CosValue[MAX_NUM_CSS_SENSORS]; //!< Current measured CSS value (ideally a cosine value) for the constellation + //!< of CSS sensors +} CSSArraySensorMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/CSSConfigMsgPayload.h b/src/architecture/msgPayloadDefC/CSSConfigMsgPayload.h index 20e4ed48d4..ab9a5500b8 100644 --- a/src/architecture/msgPayloadDefC/CSSConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CSSConfigMsgPayload.h @@ -24,14 +24,12 @@ #include "architecture/msgPayloadDefC/CSSUnitConfigMsgPayload.h" #include "architecture/utilities/macroDefinitions.h" - - /*! @brief Structure used to contain the configuration information for each sun sensor*/ -typedef struct { - uint32_t nCSS; //!< [-] Number of coarse sun sensors in cluster - CSSUnitConfigMsgPayload cssVals[MAX_NUM_CSS_SENSORS]; //!< [-] constellation of CSS elements -}CSSConfigMsgPayload; - +typedef struct +{ + uint32_t nCSS; //!< [-] Number of coarse sun sensors in cluster + CSSUnitConfigMsgPayload cssVals[MAX_NUM_CSS_SENSORS]; //!< [-] constellation of CSS elements +} CSSConfigMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/CSSRawDataMsgPayload.h b/src/architecture/msgPayloadDefC/CSSRawDataMsgPayload.h old mode 100755 new mode 100644 index 1754c66956..ce9792af0a --- a/src/architecture/msgPayloadDefC/CSSRawDataMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CSSRawDataMsgPayload.h @@ -20,12 +20,10 @@ #ifndef SIM_CSS_RAW_DATA_MESSAGE_H #define SIM_CSS_RAW_DATA_MESSAGE_H - //!@brief CSS raw data output message definition. -typedef struct { - double OutputData; //!< CSS measurement output -}CSSRawDataMsgPayload; - - +typedef struct +{ + double OutputData; //!< CSS measurement output +} CSSRawDataMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/CSSUnitConfigMsgPayload.h b/src/architecture/msgPayloadDefC/CSSUnitConfigMsgPayload.h index 0548709072..579b71e3db 100644 --- a/src/architecture/msgPayloadDefC/CSSUnitConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CSSUnitConfigMsgPayload.h @@ -20,15 +20,14 @@ #ifndef CSS_UNIT_MESSAGE_H #define CSS_UNIT_MESSAGE_H - - - /*! @brief Structure used to contain the configuration information for 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 -}CSSUnitConfigMsgPayload; - +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 +} CSSUnitConfigMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/CameraConfigMsgPayload.h b/src/architecture/msgPayloadDefC/CameraConfigMsgPayload.h old mode 100755 new mode 100644 index d8ca6c2b78..d1a0d230cf --- a/src/architecture/msgPayloadDefC/CameraConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CameraConfigMsgPayload.h @@ -25,25 +25,39 @@ #include "architecture/utilities/macroDefinitions.h" -typedef struct { - int64_t cameraID; //!< [-] ID of the camera that took the snapshot*/ - int isOn; //!< The camera is taking images at rendering rate if 1, 0 if not*/ - char parentName[MAX_STRING_LENGTH]; //!< [-] Name of the parent body to which the camera should be attached - double fieldOfView; //!< [rad] Camera Field of View, edge-to-edge along camera y-axis */ - int resolution[2]; //!< [-] Camera resolution, width/height in pixels (pixelWidth/pixelHeight in Unity) in pixels*/ - uint64_t renderRate; //!< [ns] Frame time interval at which to capture images in units of nanosecond */ - double cameraPos_B[3]; //!< [m] Camera position in body frame */ - double sigma_CB[3]; //!< [-] MRP defining the orientation of the camera frame relative to the body frame */ +typedef struct +{ + int64_t cameraID; //!< [-] ID of the camera that took the snapshot*/ + int isOn; //!< The camera is taking images at rendering rate if 1, 0 if not*/ + char parentName[MAX_STRING_LENGTH]; //!< [-] Name of the parent body to which the camera should be attached + double fieldOfView; //!< [rad] Camera Field of View, edge-to-edge along camera y-axis */ + int resolution[2]; //!< [-] Camera resolution, width/height in pixels (pixelWidth/pixelHeight in Unity) in pixels*/ + uint64_t renderRate; //!< [ns] Frame time interval at which to capture images in units of nanosecond */ + double cameraPos_B[3]; //!< [m] Camera position in body frame */ + double sigma_CB[3]; //!< [-] MRP defining the orientation of the camera frame relative to the body frame */ char skyBox[MAX_STRING_LENGTH]; //!< string containing the star field preference - int postProcessingOn; //!< (Optional) Enable post-processing of camera image. Value of 0 (protobuffer default) to use viz default which is off, -1 for false, 1 for true - double ppFocusDistance; //!< (Optional) Distance to the point of focus, minimum value of 0.1, Value of 0 to turn off this parameter entirely. - double ppAperture; //!< (Optional) Ratio of the aperture (known as f-stop or f-number). The smaller the value is, the shallower the depth of field is. Valid Setting Range: 0.05 to 32. Value of 0 to turn off this parameter entirely. - double ppFocalLength; //!< [m] (Optional) Valid setting range: 0.001m to 0.3m. Value of 0 to turn off this parameter entirely. - int ppMaxBlurSize; //!< (Optional) Convolution kernel size of the bokeh filter, which determines the maximum radius of bokeh. It also affects the performance (the larger the kernel is, the longer the GPU time is required). Depth textures Value of 1 for Small, 2 for Medium, 3 for Large, 4 for Extra Large. Value of 0 to turn off this parameter entirely. - int updateCameraParameters; //!< If true, commands camera to update Instrument Camera to current message's parameters - int renderMode; //!< (Optional) Value of 0 to render visual image (default), value of 1 to render depth buffer to image - double depthMapClippingPlanes[2]; //!< (Optional) [m] Set the bounds of rendered depth map by setting the near and far clipping planes when in renderMode=1 (depthMap mode). Default values of 0.1 and 100. - int showHUDElementsInImage; //!< (Optional) Value of 0 (protobuffer default) to use viz default, -1 for false, 1 for true -}CameraConfigMsgPayload; + int postProcessingOn; //!< (Optional) Enable post-processing of camera image. Value of 0 (protobuffer default) to + //!< use viz default which is off, -1 for false, 1 for true + double ppFocusDistance; //!< (Optional) Distance to the point of focus, minimum value of 0.1, Value of 0 to turn off + //!< this parameter entirely. + double ppAperture; //!< (Optional) Ratio of the aperture (known as f-stop or f-number). The smaller the value is, + //!< the shallower the depth of field is. Valid Setting Range: 0.05 to 32. Value of 0 to turn + //!< off this parameter entirely. + double ppFocalLength; //!< [m] (Optional) Valid setting range: 0.001m to 0.3m. Value of 0 to turn off this parameter + //!< entirely. + int ppMaxBlurSize; //!< (Optional) Convolution kernel size of the bokeh filter, which determines the maximum radius + //!< of bokeh. It also affects the performance (the larger the kernel is, the longer the GPU time + //!< is required). Depth textures Value of 1 for Small, 2 for Medium, 3 for Large, 4 for Extra + //!< Large. Value of 0 to turn off this parameter entirely. + int + updateCameraParameters; //!< If true, commands camera to update Instrument Camera to current message's parameters + int renderMode; //!< (Optional) Value of 0 to render visual image (default), value of 1 to render depth buffer to + //!< image + double depthMapClippingPlanes[2]; //!< (Optional) [m] Set the bounds of rendered depth map by setting the near and + //!< far clipping planes when in renderMode=1 (depthMap mode). Default values of + //!< 0.1 and 100. + int showHUDElementsInImage; //!< (Optional) Value of 0 (protobuffer default) to use viz default, -1 for false, 1 for + //!< true +} CameraConfigMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/CameraImageMsgPayload.h b/src/architecture/msgPayloadDefC/CameraImageMsgPayload.h old mode 100755 new mode 100644 index 6204320e69..1180faaf60 --- a/src/architecture/msgPayloadDefC/CameraImageMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CameraImageMsgPayload.h @@ -27,14 +27,14 @@ #include "architecture/utilities/macroDefinitions.h" /*! @brief Structure used to define the output definition for attitude guidance*/ -typedef struct { - uint64_t timeTag; //!< --[ns] Current vehicle time-tag associated with measurements*/ - int valid; //!< -- A valid image is present for 1, 0 if not*/ +typedef struct +{ + uint64_t timeTag; //!< --[ns] Current vehicle time-tag associated with measurements*/ + int valid; //!< -- A valid image is present for 1, 0 if not*/ int64_t cameraID; //!< -- [-] ID of the camera that took the snapshot*/ void* imagePointer; //!< -- Pointer to the image int32_t imageBufferLength; //!< -- Length of the buffer for recasting - int8_t imageType; //!< -- Number of channels in each pixel, RGB = 3, RGBA = 4 -}CameraImageMsgPayload; - + int8_t imageType; //!< -- Number of channels in each pixel, RGB = 3, RGBA = 4 +} CameraImageMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/ClassicElementsMsgPayload.h b/src/architecture/msgPayloadDefC/ClassicElementsMsgPayload.h old mode 100755 new mode 100644 index ce2448a679..f3ce690b40 --- a/src/architecture/msgPayloadDefC/ClassicElementsMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ClassicElementsMsgPayload.h @@ -20,22 +20,21 @@ #ifndef _CLASSIC_ELEMENTS_H #define _CLASSIC_ELEMENTS_H - /*! @brief Structure used to define classic orbit elements */ -typedef struct { - double a; //!< object semi-major axis - double e; //!< Eccentricity of the orbit - double i; //!< inclination of the orbital plane - double Omega; //!< Right ascension of the ascending node - double omega; //!< Argument of periapsis of the orbit - double f; //!< True anomaly of the orbit - double rmag; //!< Magnitude of the position vector (extra) - double alpha; //!< Inverted semi-major axis (extra) - double rPeriap; //!< Radius of periapsis (extra) - double rApoap; //!< Radius if apoapsis (extra) +typedef struct +{ + double a; //!< object semi-major axis + double e; //!< Eccentricity of the orbit + double i; //!< inclination of the orbital plane + double Omega; //!< Right ascension of the ascending node + double omega; //!< Argument of periapsis of the orbit + double f; //!< True anomaly of the orbit + double rmag; //!< Magnitude of the position vector (extra) + double alpha; //!< Inverted semi-major axis (extra) + double rPeriap; //!< Radius of periapsis (extra) + double rApoap; //!< Radius if apoapsis (extra) } ClassicElementsMsgPayload; typedef ClassicElementsMsgPayload classicElements; - #endif diff --git a/src/architecture/msgPayloadDefC/CmdForceBodyMsgPayload.h b/src/architecture/msgPayloadDefC/CmdForceBodyMsgPayload.h old mode 100755 new mode 100644 index 040ab821df..13005124ce --- a/src/architecture/msgPayloadDefC/CmdForceBodyMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CmdForceBodyMsgPayload.h @@ -20,11 +20,10 @@ #ifndef _CMD_FORCE_BODY_MESSAGE_ #define _CMD_FORCE_BODY_MESSAGE_ - /*! @brief Message used to define the vehicle control force vector in Body frame components*/ -typedef struct { - double forceRequestBody[3]; //!< [N] Control force requested -}CmdForceBodyMsgPayload; - +typedef struct +{ + double forceRequestBody[3]; //!< [N] Control force requested +} CmdForceBodyMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/CmdForceInertialMsgPayload.h b/src/architecture/msgPayloadDefC/CmdForceInertialMsgPayload.h old mode 100755 new mode 100644 index 923a132b05..b42f1aa243 --- a/src/architecture/msgPayloadDefC/CmdForceInertialMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CmdForceInertialMsgPayload.h @@ -21,9 +21,9 @@ #define _CMD_FORCE_INERTIAL_MESSAGE_ /*! @brief Message used to define the vehicle control force vector in Inertial frame components*/ -typedef struct { - double forceRequestInertial[3]; //!< [N] control force request -}CmdForceInertialMsgPayload; - +typedef struct +{ + double forceRequestInertial[3]; //!< [N] control force request +} CmdForceInertialMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/CmdTorqueBodyMsgPayload.h b/src/architecture/msgPayloadDefC/CmdTorqueBodyMsgPayload.h old mode 100755 new mode 100644 index 72d965b1a0..2792117953 --- a/src/architecture/msgPayloadDefC/CmdTorqueBodyMsgPayload.h +++ b/src/architecture/msgPayloadDefC/CmdTorqueBodyMsgPayload.h @@ -20,11 +20,10 @@ #ifndef _CMD_TORQUE_BODY_MESSAGE_ #define _CMD_TORQUE_BODY_MESSAGE_ - /*! @brief Message used to define the vehicle control torque vector in Body frame components*/ -typedef struct { - double torqueRequestBody[3]; //!< [Nm] Control torque requested -}CmdTorqueBodyMsgPayload; - +typedef struct +{ + double torqueRequestBody[3]; //!< [Nm] Control torque requested +} CmdTorqueBodyMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/ColorMsgPayload.h b/src/architecture/msgPayloadDefC/ColorMsgPayload.h old mode 100755 new mode 100644 index d840ae46bb..ec229daf88 --- a/src/architecture/msgPayloadDefC/ColorMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ColorMsgPayload.h @@ -20,13 +20,10 @@ #ifndef _COLOR_MESSAGE_H #define _COLOR_MESSAGE_H - - /*! @brief Structure used to define RGBA color */ -typedef struct { +typedef struct +{ int colorRGBA[4]; //!< [-] 0-255 Color values in RGBA format -}ColorMsgPayload; - - +} ColorMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/ConstDynEffectorMsgPayload.h b/src/architecture/msgPayloadDefC/ConstDynEffectorMsgPayload.h index 107bbf0d62..71600db697 100644 --- a/src/architecture/msgPayloadDefC/ConstDynEffectorMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ConstDynEffectorMsgPayload.h @@ -21,14 +21,15 @@ #define CONSTRAINT_DYN_EFFECTOR_MSG_H /*! @brief Structure used to Constraint Dynamic Effector output message */ -typedef struct { - double Fc_N[3]; //!< [N] Constraint force applied in Inertial frame - double L1_B1[3]; //!< [N.m] Constraint torque applied on s/c 1 in B1 frame - double L2_B2[3]; //!< [N.m] Constraint torque applied on s/c 2 in B2 frame - double psi_N[3]; //!< [m] Direction constraint violation +typedef struct +{ + double Fc_N[3]; //!< [N] Constraint force applied in Inertial frame + double L1_B1[3]; //!< [N.m] Constraint torque applied on s/c 1 in B1 frame + double L2_B2[3]; //!< [N.m] Constraint torque applied on s/c 2 in B2 frame + double psi_N[3]; //!< [m] Direction constraint violation double Fc_mag_filtered; //!< [N] Magnitude of filtered constraint force applied in Inertial frame double L1_mag_filtered; //!< [N.m] Magnitude of filtered constraint torque on s/c 1 applied in B1 frame double L2_mag_filtered; //!< [N.m] Magnitude of filtered constraint torque on s/c 2 applied in B2 frame -}ConstDynEffectorMsgPayload; +} ConstDynEffectorMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/DataNodeUsageMsgPayload.h b/src/architecture/msgPayloadDefC/DataNodeUsageMsgPayload.h index 187735eb5e..8db9f1ab67 100644 --- a/src/architecture/msgPayloadDefC/DataNodeUsageMsgPayload.h +++ b/src/architecture/msgPayloadDefC/DataNodeUsageMsgPayload.h @@ -20,12 +20,13 @@ #ifndef BASILISK_DATANODEUSAGESIMMSG_H #define BASILISK_DATANODEUSAGESIMMSG_H - /*! @brief Message for reporting the science or telemetry data produced or consumed by a module.*/ -typedef struct{ - //std::string dataName; //!< Data name - char dataName[128]; //!< data name - double baudRate; //!< [bits/s] Data usage by the message writer; positive for data generators, negative for data sinks -}DataNodeUsageMsgPayload; - -#endif //BASILISK_DATANODEUSAGESIMMSG_H +typedef struct +{ + // std::string dataName; //!< Data name + char dataName[128]; //!< data name + double + baudRate; //!< [bits/s] Data usage by the message writer; positive for data generators, negative for data sinks +} DataNodeUsageMsgPayload; + +#endif // BASILISK_DATANODEUSAGESIMMSG_H diff --git a/src/architecture/msgPayloadDefC/DesiredVelocityMsgPayload.h b/src/architecture/msgPayloadDefC/DesiredVelocityMsgPayload.h old mode 100755 new mode 100644 index c6c02494d7..a148df99a5 --- a/src/architecture/msgPayloadDefC/DesiredVelocityMsgPayload.h +++ b/src/architecture/msgPayloadDefC/DesiredVelocityMsgPayload.h @@ -20,11 +20,11 @@ #ifndef DESIRED_VELOCITY_MESSAGE_H #define DESIRED_VELOCITY_MESSAGE_H - /*! @brief Structure used to define a desired velocity for a maneuver in the inertial frame */ -typedef struct { +typedef struct +{ double vDesired_N[3]; //!< [m/s] Desired velocity in inertial frame N - double maneuverTime; //!< [s] Time of maneuver -}DesiredVelocityMsgPayload; + double maneuverTime; //!< [s] Time of maneuver +} DesiredVelocityMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/DeviceCmdMsgPayload.h b/src/architecture/msgPayloadDefC/DeviceCmdMsgPayload.h index d202024f03..ba9cc75e88 100644 --- a/src/architecture/msgPayloadDefC/DeviceCmdMsgPayload.h +++ b/src/architecture/msgPayloadDefC/DeviceCmdMsgPayload.h @@ -21,8 +21,9 @@ #define BASILISK_DEVICECMDMSGPAYLOAD_H //! @brief Device command message used to change the state of instruments. -typedef struct{ - uint64_t deviceCmd; //!< device command; 0 is off, >0 commands other states -}DeviceCmdMsgPayload; +typedef struct +{ + uint64_t deviceCmd; //!< device command; 0 is off, >0 commands other states +} DeviceCmdMsgPayload; -#endif //BASILISK_DEVICECMDMSGPAYLOAD_H +#endif // BASILISK_DEVICECMDMSGPAYLOAD_H diff --git a/src/architecture/msgPayloadDefC/DeviceStatusMsgPayload.h b/src/architecture/msgPayloadDefC/DeviceStatusMsgPayload.h index 74244d3dc0..4cd6be88f4 100644 --- a/src/architecture/msgPayloadDefC/DeviceStatusMsgPayload.h +++ b/src/architecture/msgPayloadDefC/DeviceStatusMsgPayload.h @@ -20,12 +20,16 @@ #ifndef BASILISK_DEVICESTATUSMSGPAYLOAD_H #define BASILISK_DEVICESTATUSMSGPAYLOAD_H - -enum deviceState {On = 1, Off = 0}; +enum deviceState +{ + On = 1, + Off = 0 +}; //! @brief Power node command message used to change the state of power modules. -typedef struct{ - enum deviceState deviceStatus; //!< device status indicator; 0 is off, 1 is on -}DeviceStatusMsgPayload; +typedef struct +{ + enum deviceState deviceStatus; //!< device status indicator; 0 is off, 1 is on +} DeviceStatusMsgPayload; -#endif //BASILISK_DEVICESTATUSMSGPAYLOAD_H +#endif // BASILISK_DEVICESTATUSMSGPAYLOAD_H diff --git a/src/architecture/msgPayloadDefC/DipoleRequestBodyMsgPayload.h b/src/architecture/msgPayloadDefC/DipoleRequestBodyMsgPayload.h index b23b6f73e8..6ca65658c3 100644 --- a/src/architecture/msgPayloadDefC/DipoleRequestBodyMsgPayload.h +++ b/src/architecture/msgPayloadDefC/DipoleRequestBodyMsgPayload.h @@ -20,12 +20,11 @@ #ifndef DIPOLE_REQUEST_BODY_MSG_H #define DIPOLE_REQUEST_BODY_MSG_H - /*! @brief Structure used to define the output of the sub-module. This is the same output message that is used by all sub-modules in the module folder. */ -typedef struct { - double dipole_B[3]; //!< [A-m2] desired net magnetic torque bar dipole in the Body frame. -}DipoleRequestBodyMsgPayload; - +typedef struct +{ + double dipole_B[3]; //!< [A-m2] desired net magnetic torque bar dipole in the Body frame. +} DipoleRequestBodyMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/DvBurnCmdMsgPayload.h b/src/architecture/msgPayloadDefC/DvBurnCmdMsgPayload.h old mode 100755 new mode 100644 index 9f24bb4807..4f8c81959c --- a/src/architecture/msgPayloadDefC/DvBurnCmdMsgPayload.h +++ b/src/architecture/msgPayloadDefC/DvBurnCmdMsgPayload.h @@ -21,15 +21,13 @@ #ifndef DV_BURN_CMD_MESSAGE_H #define DV_BURN_CMD_MESSAGE_H - - /*! @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 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 -}DvBurnCmdMsgPayload; - +typedef struct +{ + 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 +} DvBurnCmdMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/DvExecutionDataMsgPayload.h b/src/architecture/msgPayloadDefC/DvExecutionDataMsgPayload.h old mode 100755 new mode 100644 index bfad1c8690..bdbe93f74b --- a/src/architecture/msgPayloadDefC/DvExecutionDataMsgPayload.h +++ b/src/architecture/msgPayloadDefC/DvExecutionDataMsgPayload.h @@ -22,14 +22,11 @@ #include "stdint.h" - /*! @brief DV executation data structure */ -typedef struct { - uint32_t burnExecuting; /*!< [-] Flag indicating whether burn is executing*/ - uint32_t burnComplete; /*!< [-] Flag indicating whether the burn is complete */ -}DvExecutionDataMsgPayload; - - - +typedef struct +{ + uint32_t burnExecuting; /*!< [-] Flag indicating whether burn is executing*/ + uint32_t burnComplete; /*!< [-] Flag indicating whether the burn is complete */ +} DvExecutionDataMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/EclipseMsgPayload.h b/src/architecture/msgPayloadDefC/EclipseMsgPayload.h index 63ea633406..75dea72bd1 100644 --- a/src/architecture/msgPayloadDefC/EclipseMsgPayload.h +++ b/src/architecture/msgPayloadDefC/EclipseMsgPayload.h @@ -20,11 +20,10 @@ #ifndef eclipseSimMsg_h #define eclipseSimMsg_h - //!@brief Eclipse shadow factor message definition. -typedef struct { - double shadowFactor; //!< Proportion of illumination due to eclipse. 0 = fully shadowed, 1 = fully illuminated. -}EclipseMsgPayload; - +typedef struct +{ + double shadowFactor; //!< Proportion of illumination due to eclipse. 0 = fully shadowed, 1 = fully illuminated. +} EclipseMsgPayload; #endif /* eclipseSimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/EphemerisMsgPayload.h b/src/architecture/msgPayloadDefC/EphemerisMsgPayload.h old mode 100755 new mode 100644 index 88ad83775a..2fa14f77e6 --- a/src/architecture/msgPayloadDefC/EphemerisMsgPayload.h +++ b/src/architecture/msgPayloadDefC/EphemerisMsgPayload.h @@ -20,15 +20,14 @@ #ifndef EPHEMERIS_OUTPUT_H #define EPHEMERIS_OUTPUT_H - /*! @brief Message structure used to write ephemeris states out to other modules*/ -typedef struct { - double r_BdyZero_N[3]; //!< [m] Position of orbital body - double v_BdyZero_N[3]; //!< [m/s] Velocity of orbital body - double sigma_BN[3]; //!< MRP attitude of the orbital body fixed frame relative to inertial - double omega_BN_B[3]; //!< [r/s] angular velocity of the orbital body relative to inertial - double timeTag; //!< [s] vehicle Time-tag for state -}EphemerisMsgPayload; - +typedef struct +{ + double r_BdyZero_N[3]; //!< [m] Position of orbital body + double v_BdyZero_N[3]; //!< [m/s] Velocity of orbital body + double sigma_BN[3]; //!< MRP attitude of the orbital body fixed frame relative to inertial + double omega_BN_B[3]; //!< [r/s] angular velocity of the orbital body relative to inertial + double timeTag; //!< [s] vehicle Time-tag for state +} EphemerisMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/EpochMsgPayload.h b/src/architecture/msgPayloadDefC/EpochMsgPayload.h index baf72b46dd..d1efa87aa3 100644 --- a/src/architecture/msgPayloadDefC/EpochMsgPayload.h +++ b/src/architecture/msgPayloadDefC/EpochMsgPayload.h @@ -20,16 +20,15 @@ #ifndef epochSimMsg_h #define epochSimMsg_h - /*! @brief Structure used to define the the epoch date and time message */ -typedef struct { - int year; //!< year, integer - int month; //!< month, integer - int day; //!< day, integer - int hours; //!< hours, integer - int minutes; //!< minutes, integer - double seconds; //!< seconds, double -}EpochMsgPayload; - +typedef struct +{ + int year; //!< year, integer + int month; //!< month, integer + int day; //!< day, integer + int hours; //!< hours, integer + int minutes; //!< minutes, integer + double seconds; //!< seconds, double +} EpochMsgPayload; #endif /* epochSimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/ForceAtSiteMsgPayload.h b/src/architecture/msgPayloadDefC/ForceAtSiteMsgPayload.h index 2388208e28..dc0e0ce0d0 100644 --- a/src/architecture/msgPayloadDefC/ForceAtSiteMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ForceAtSiteMsgPayload.h @@ -25,8 +25,9 @@ * Sites are locations fixed on a body (in the context of MJScene) with an * associated reference frame. This message represents a force that * could be applied at this site and expressed on the site's reference frame. -*/ -typedef struct { + */ +typedef struct +{ double force_S[3]; //!< [N] Force in Newtons } ForceAtSiteMsgPayload; diff --git a/src/architecture/msgPayloadDefC/FuelTankMsgPayload.h b/src/architecture/msgPayloadDefC/FuelTankMsgPayload.h index 29702d0c6e..c5cae33272 100644 --- a/src/architecture/msgPayloadDefC/FuelTankMsgPayload.h +++ b/src/architecture/msgPayloadDefC/FuelTankMsgPayload.h @@ -20,13 +20,12 @@ #ifndef fuelTankSimMsg_h #define fuelTankSimMsg_h - /*! @brief Structure used to define the individual fuel tank mass property message*/ -typedef struct { - double fuelMass; //!< [kg], current fuel mass - double fuelMassDot; //!< [kg/s], current rate of change of fuel mass - double maxFuelMass; //!< [kg], maximum fuel mass capacity -}FuelTankMsgPayload; - +typedef struct +{ + double fuelMass; //!< [kg], current fuel mass + double fuelMassDot; //!< [kg/s], current rate of change of fuel mass + double maxFuelMass; //!< [kg], maximum fuel mass capacity +} FuelTankMsgPayload; #endif /* fuelTankSimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/GravityGradientMsgPayload.h b/src/architecture/msgPayloadDefC/GravityGradientMsgPayload.h index e289ba93df..5c8c15564f 100644 --- a/src/architecture/msgPayloadDefC/GravityGradientMsgPayload.h +++ b/src/architecture/msgPayloadDefC/GravityGradientMsgPayload.h @@ -20,10 +20,10 @@ #ifndef gravityGradientSimMsg_H #define gravityGradientSimMsg_H - /*! gravity gradient torque message definition */ -typedef struct { - double gravityGradientTorque_B[3]; //!< [Nm] Gravity Gradient torque in body frame components -}GravityGradientMsgPayload; +typedef struct +{ + double gravityGradientTorque_B[3]; //!< [Nm] Gravity Gradient torque in body frame components +} GravityGradientMsgPayload; -#endif //BASILISK_DATANODEUSAGESIMMSG_H +#endif // BASILISK_DATANODEUSAGESIMMSG_H diff --git a/src/architecture/msgPayloadDefC/GroundStateMsgPayload.h b/src/architecture/msgPayloadDefC/GroundStateMsgPayload.h index 424f64bed4..d9831bea9f 100644 --- a/src/architecture/msgPayloadDefC/GroundStateMsgPayload.h +++ b/src/architecture/msgPayloadDefC/GroundStateMsgPayload.h @@ -17,17 +17,15 @@ */ - #ifndef BASILISK_GROUNDPOSITIONSIMMSG_H #define BASILISK_GROUNDPOSITIONSIMMSG_H - /*! @brief Message that defines the inertial location of a groundLocation at the current time. */ -typedef struct { +typedef struct +{ double r_LN_N[3]; //!< Position vector of the location w.r.t. the inertial origin in the inertial frame double r_LP_N[3]; //!< Position vector of the location with respect to the planet center in the inertial frame -}GroundStateMsgPayload; - +} GroundStateMsgPayload; -#endif //BASILISK_GROUNDPOSITIONSIMMSG_H +#endif // BASILISK_GROUNDPOSITIONSIMMSG_H diff --git a/src/architecture/msgPayloadDefC/HeadingFilterMsgPayload.h b/src/architecture/msgPayloadDefC/HeadingFilterMsgPayload.h old mode 100755 new mode 100644 index 67c8690afb..877f713314 --- a/src/architecture/msgPayloadDefC/HeadingFilterMsgPayload.h +++ b/src/architecture/msgPayloadDefC/HeadingFilterMsgPayload.h @@ -26,15 +26,14 @@ /*! @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 covar[HEAD_N_STATES_SWITCH*HEAD_N_STATES_SWITCH]; /*!< [-] Current covariance of the filter */ - double state[HEAD_N_STATES_SWITCH]; /*!< [-] Current estimated state of the filter */ - double stateError[HEAD_N_STATES_SWITCH]; /*!< [-] Current deviation of the state from the reference state */ - double postFitRes[3]; /*!< [-] PostFit Residuals */ - -}HeadingFilterMsgPayload; - - +typedef struct +{ + double timeTag; /*!< [s] Current time of validity for output */ + double covar[HEAD_N_STATES_SWITCH * HEAD_N_STATES_SWITCH]; /*!< [-] Current covariance of the filter */ + double state[HEAD_N_STATES_SWITCH]; /*!< [-] Current estimated state of the filter */ + double stateError[HEAD_N_STATES_SWITCH]; /*!< [-] Current deviation of the state from the reference state */ + double postFitRes[3]; /*!< [-] PostFit Residuals */ + +} HeadingFilterMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/HillRelStateMsgPayload.h b/src/architecture/msgPayloadDefC/HillRelStateMsgPayload.h old mode 100755 new mode 100644 index 4ce28c680c..1d1fab08e1 --- a/src/architecture/msgPayloadDefC/HillRelStateMsgPayload.h +++ b/src/architecture/msgPayloadDefC/HillRelStateMsgPayload.h @@ -20,13 +20,11 @@ #ifndef HILL_NAV_MESSAGE_H #define HILL_NAV_MESSAGE_H - - /*! @brief Structure used to define the output definition for attitude reference generation */ -typedef struct { - double r_DC_H[3]; //!< [m] Relative position of the deputy to the chief in Hill-frame components - double v_DC_H[3]; //!< [m/s] Relative velocity of the deputy to the chief in Hill-frame components -}HillRelStateMsgPayload; - +typedef struct +{ + double r_DC_H[3]; //!< [m] Relative position of the deputy to the chief in Hill-frame components + double v_DC_H[3]; //!< [m/s] Relative velocity of the deputy to the chief in Hill-frame components +} HillRelStateMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h b/src/architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h index 53f78bedd1..c215735f8c 100644 --- a/src/architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h +++ b/src/architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h @@ -20,12 +20,11 @@ #ifndef hingedRigidBodySimMsg_h #define hingedRigidBodySimMsg_h - /*! @brief Structure used to define the individual Hinged Rigid Body data message*/ -typedef struct { - double theta; //!< [rad], panel angular displacement - double thetaDot; //!< [rad/s], panel angular displacement rate -}HingedRigidBodyMsgPayload; - +typedef struct +{ + double theta; //!< [rad], panel angular displacement + double thetaDot; //!< [rad/s], panel angular displacement rate +} HingedRigidBodyMsgPayload; #endif /* hingedRigidBodySimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/IMUSensorBodyMsgPayload.h b/src/architecture/msgPayloadDefC/IMUSensorBodyMsgPayload.h old mode 100755 new mode 100644 index 42e11e6d4d..53dfad5bf2 --- a/src/architecture/msgPayloadDefC/IMUSensorBodyMsgPayload.h +++ b/src/architecture/msgPayloadDefC/IMUSensorBodyMsgPayload.h @@ -20,14 +20,13 @@ #ifndef _IMU_SENSOR_BODY_MESSAGE_H #define _IMU_SENSOR_BODY_MESSAGE_H - /*! @brief Output structure for IMU structure in vehicle body frame*/ -typedef struct { - double DVFrameBody[3]; //!< m/s Accumulated DVs in body - double AccelBody[3]; //!< m/s2 Apparent acceleration of the body - double DRFrameBody[3]; //!< r Accumulated DRs in body - double AngVelBody[3]; //!< r/s Angular velocity in platform body -}IMUSensorBodyMsgPayload; - +typedef struct +{ + double DVFrameBody[3]; //!< m/s Accumulated DVs in body + double AccelBody[3]; //!< m/s2 Apparent acceleration of the body + double DRFrameBody[3]; //!< r Accumulated DRs in body + double AngVelBody[3]; //!< r/s Angular velocity in platform body +} IMUSensorBodyMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h b/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h old mode 100755 new mode 100644 index fc0b1e19e9..dfcb24ac22 --- a/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h +++ b/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h @@ -20,14 +20,13 @@ #ifndef _IMU_SENSOR_MESSAG_H #define _IMU_SENSOR_MESSAG_H - //! @brief Simulated IMU Sensor output message definition. -typedef struct{ - double DVFramePlatform[3]; //!< m/s Accumulated DVs in platform - double AccelPlatform[3]; //!< m/s2 Apparent acceleration of the platform - double DRFramePlatform[3]; //!< r Accumulated DRs in platform - double AngVelPlatform[3]; //!< r/s Angular velocity in platform frame -}IMUSensorMsgPayload; - +typedef struct +{ + double DVFramePlatform[3]; //!< m/s Accumulated DVs in platform + double AccelPlatform[3]; //!< m/s2 Apparent acceleration of the platform + double DRFramePlatform[3]; //!< r Accumulated DRs in platform + double AngVelPlatform[3]; //!< r/s Angular velocity in platform frame +} IMUSensorMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/InertialFilterMsgPayload.h b/src/architecture/msgPayloadDefC/InertialFilterMsgPayload.h old mode 100755 new mode 100644 index 3811835e13..3a669efa6d --- a/src/architecture/msgPayloadDefC/InertialFilterMsgPayload.h +++ b/src/architecture/msgPayloadDefC/InertialFilterMsgPayload.h @@ -20,19 +20,17 @@ #ifndef INERTIAL_FILTER_MESSAGE_H #define INERTIAL_FILTER_MESSAGE_H - - #define AKF_N_STATES 6 #define MAX_N_ATT_STATES 4 /*! @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 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 -}InertialFilterMsgPayload; - +typedef struct +{ + 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 +} InertialFilterMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/InertialHeadingMsgPayload.h b/src/architecture/msgPayloadDefC/InertialHeadingMsgPayload.h index 0e052872c8..3f2e25f022 100644 --- a/src/architecture/msgPayloadDefC/InertialHeadingMsgPayload.h +++ b/src/architecture/msgPayloadDefC/InertialHeadingMsgPayload.h @@ -20,14 +20,13 @@ #ifndef inertialHeadingSimMsg_h #define inertialHeadingSimMsg_h - //!@brief Inertial heading message definition. /*! This message contains the unit direction vector of a certain direction expressed in inertial frame coordinates. */ -typedef struct { - double rHat_XN_N[3]; //!< [] unit heading vector to any thing "X" in space, "N", inertial frame -}InertialHeadingMsgPayload; - +typedef struct +{ + double rHat_XN_N[3]; //!< [] unit heading vector to any thing "X" in space, "N", inertial frame +} InertialHeadingMsgPayload; #endif /* inertialHeadingSimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/LambertPerformanceMsgPayload.h b/src/architecture/msgPayloadDefC/LambertPerformanceMsgPayload.h old mode 100755 new mode 100644 index 3638cac515..4888ddc529 --- a/src/architecture/msgPayloadDefC/LambertPerformanceMsgPayload.h +++ b/src/architecture/msgPayloadDefC/LambertPerformanceMsgPayload.h @@ -20,15 +20,15 @@ #ifndef LAMBERT_PERFORMANCE_MESSAGE_H #define LAMBERT_PERFORMANCE_MESSAGE_H - /*! @brief Structure used to define extra output of the Lambert problem solution */ -typedef struct { - double x; //!< [-] solution for free variable (iteration variable) - int numIter; //!< [-] number of root-finder iterations to find x - double errX; //!< [-] difference in x between last and second-to-last iteration - double xSol2; //!< [-] second solution for free variable (iteration variable) - int numIterSol2; //!< [-] number of root-finder iterations to find x_sol2 - double errXSol2; //!< [-] difference in x_sol2 between last and second-to-last iteration -}LambertPerformanceMsgPayload; +typedef struct +{ + double x; //!< [-] solution for free variable (iteration variable) + int numIter; //!< [-] number of root-finder iterations to find x + double errX; //!< [-] difference in x between last and second-to-last iteration + double xSol2; //!< [-] second solution for free variable (iteration variable) + int numIterSol2; //!< [-] number of root-finder iterations to find x_sol2 + double errXSol2; //!< [-] difference in x_sol2 between last and second-to-last iteration +} LambertPerformanceMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/LambertProblemMsgPayload.h b/src/architecture/msgPayloadDefC/LambertProblemMsgPayload.h old mode 100755 new mode 100644 index 6f88e77088..90f6b7646e --- a/src/architecture/msgPayloadDefC/LambertProblemMsgPayload.h +++ b/src/architecture/msgPayloadDefC/LambertProblemMsgPayload.h @@ -20,19 +20,21 @@ #ifndef LAMBERT_PROBLEM_MESSAGE_H #define LAMBERT_PROBLEM_MESSAGE_H -typedef enum { +typedef enum +{ IZZO, GOODING } SolverMethod; /*! @brief Structure used to define the input for Lambert problem */ -typedef struct { - SolverMethod solverMethod; //!< [-] lambert solver algorithm (GOODING or IZZO) - double r1_N[3]; //!< [m] position vector at t0 - double r2_N[3]; //!< [m] position vector at t1 - double transferTime; //!< [s] time of flight between r1_N and r2_N (t1-t0) - double mu; //!< [m^3 s^-2] gravitational parameter of body - int numRevolutions; //!< [-] number of revolutions to be completed (completed orbits) -}LambertProblemMsgPayload; +typedef struct +{ + SolverMethod solverMethod; //!< [-] lambert solver algorithm (GOODING or IZZO) + double r1_N[3]; //!< [m] position vector at t0 + double r2_N[3]; //!< [m] position vector at t1 + double transferTime; //!< [s] time of flight between r1_N and r2_N (t1-t0) + double mu; //!< [m^3 s^-2] gravitational parameter of body + int numRevolutions; //!< [-] number of revolutions to be completed (completed orbits) +} LambertProblemMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/LambertSolutionMsgPayload.h b/src/architecture/msgPayloadDefC/LambertSolutionMsgPayload.h old mode 100755 new mode 100644 index 3563d06e7f..fb98b1df88 --- a/src/architecture/msgPayloadDefC/LambertSolutionMsgPayload.h +++ b/src/architecture/msgPayloadDefC/LambertSolutionMsgPayload.h @@ -20,15 +20,15 @@ #ifndef LAMBERT_SOLUTION_MESSAGE_H #define LAMBERT_SOLUTION_MESSAGE_H - /*! @brief Structure used to define the output of the Lambert problem solution */ -typedef struct { - double v1_N[3]; //!< [m/s] velocity solution at t1 - double v2_N[3]; //!< [m/s] velocity solution at t2 - int valid; //!< [-] valid solution if 1, not if 0 - double v1Sol2_N[3]; //!< [m/s] second velocity solution at t1 (for multi-revolution solutions) - double v2Sol2_N[3]; //!< [m/s] second velocity solution at t2 (for multi-revolution solutions) - int validSol2; //!< [-] valid second solution if 1, not if 0 -}LambertSolutionMsgPayload; +typedef struct +{ + double v1_N[3]; //!< [m/s] velocity solution at t1 + double v2_N[3]; //!< [m/s] velocity solution at t2 + int valid; //!< [-] valid solution if 1, not if 0 + double v1Sol2_N[3]; //!< [m/s] second velocity solution at t1 (for multi-revolution solutions) + double v2Sol2_N[3]; //!< [m/s] second velocity solution at t2 (for multi-revolution solutions) + int validSol2; //!< [-] valid second solution if 1, not if 0 +} LambertSolutionMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/LambertValidatorMsgPayload.h b/src/architecture/msgPayloadDefC/LambertValidatorMsgPayload.h old mode 100755 new mode 100644 index 50784c8da6..8e50df5285 --- a/src/architecture/msgPayloadDefC/LambertValidatorMsgPayload.h +++ b/src/architecture/msgPayloadDefC/LambertValidatorMsgPayload.h @@ -20,22 +20,22 @@ #ifndef LAMBERT_VALIDATOR_MESSAGE_H #define LAMBERT_VALIDATOR_MESSAGE_H - /*! @brief Structure used to log results of lambert validator, such as constraint violations. Specifies why lambertValidator failed to write a non-zero DV command message */ -typedef struct { - int failedValidLambert; //!< [-] no valid lambert solution if 1, otherwise 0 - int failedNumIterationsLambert; //!< [-] Lambert solver root finder iterations for x too high if 1, otherwise 0 - int failedXToleranceLambert; //!< [-] Lambert solver root finder for x not converged if 1, otherwise 0 - int failedXSolutionConvergence; //!< [-] x solution too different from previous time step if 1, otherwise 0 +typedef struct +{ + int failedValidLambert; //!< [-] no valid lambert solution if 1, otherwise 0 + int failedNumIterationsLambert; //!< [-] Lambert solver root finder iterations for x too high if 1, otherwise 0 + int failedXToleranceLambert; //!< [-] Lambert solver root finder for x not converged if 1, otherwise 0 + int failedXSolutionConvergence; //!< [-] x solution too different from previous time step if 1, otherwise 0 int failedDvSolutionConvergence; //!< [-] Delta-V solution too different from previous time step if 1, otherwise 0 int failedDistanceTargetConstraint; //!< [-] violated the maximum distance from target constraint if 1, otherwise 0 - int failedOrbitRadiusConstraint; //!< [-] violated the minimum orbit radius constraint if 1, otherwise 0 - double xSolutionDifference; //!< [-] difference in solution for free variable (iteration variable) to previous time + int failedOrbitRadiusConstraint; //!< [-] violated the minimum orbit radius constraint if 1, otherwise 0 + double xSolutionDifference; //!< [-] difference in solution for free variable (iteration variable) to previous time double dvSolutionDifference; //!< [m/s] difference in Delta-V solution magnitude to previous time step int violationsDistanceTarget; //!< [-] number of violations of the maximum distance from target constraint - int violationsOrbitRadius; //!< [-] number of violations of the minimum orbit radius constraint + int violationsOrbitRadius; //!< [-] number of violations of the minimum orbit radius constraint double dv_N[3]; //!< [m/s] Delta-V vector that would be commanded at maneuver time if all checks passed -}LambertValidatorMsgPayload; +} LambertValidatorMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/LandmarkMsgPayload.h b/src/architecture/msgPayloadDefC/LandmarkMsgPayload.h index 1f06b05bb5..e2b58ad340 100644 --- a/src/architecture/msgPayloadDefC/LandmarkMsgPayload.h +++ b/src/architecture/msgPayloadDefC/LandmarkMsgPayload.h @@ -23,10 +23,10 @@ /*! @brief Message that defines visibility of a landmark by the on-board camera providing the landmark pixel using the pinhole camera model */ -typedef struct { - int isVisible;//!< [-] 1 when the landmark is visible by the camera, 0 otherwise. - int pL[2]; //!< [-] landmark pixel as view from pinhole camera -}LandmarkMsgPayload; - +typedef struct +{ + int isVisible; //!< [-] 1 when the landmark is visible by the camera, 0 otherwise. + int pL[2]; //!< [-] landmark pixel as view from pinhole camera +} LandmarkMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/LinearTranslationRigidBodyMsgPayload.h b/src/architecture/msgPayloadDefC/LinearTranslationRigidBodyMsgPayload.h index 26a6abf8b4..7f4e89d646 100644 --- a/src/architecture/msgPayloadDefC/LinearTranslationRigidBodyMsgPayload.h +++ b/src/architecture/msgPayloadDefC/LinearTranslationRigidBodyMsgPayload.h @@ -20,12 +20,11 @@ #ifndef linearTranslationRigidBodySimMsg_h #define linearTranslationRigidBodySimMsg_h - - /*! @brief Structure used to define the translating rigid body data message*/ -typedef struct { - double rho; //!< [m], body linear displacement - double rhoDot; //!< [m/s], body linear displacement rate -}LinearTranslationRigidBodyMsgPayload; - +/*! @brief Structure used to define the translating rigid body data message*/ +typedef struct +{ + double rho; //!< [m], body linear displacement + double rhoDot; //!< [m/s], body linear displacement rate +} LinearTranslationRigidBodyMsgPayload; #endif /* linearTranslationRigidBodySimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/MTBArrayConfigMsgPayload.h b/src/architecture/msgPayloadDefC/MTBArrayConfigMsgPayload.h index 02f494e757..ee93969619 100644 --- a/src/architecture/msgPayloadDefC/MTBArrayConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/MTBArrayConfigMsgPayload.h @@ -23,12 +23,12 @@ #include "architecture/utilities/macroDefinitions.h" /*! @brief magnetic torque bar array configuration msg */ -typedef struct{ - int numMTB; //!< [-] number of magnetic torque bars on the spacecraft - double GtMatrix_B[3*MAX_EFF_CNT]; //!< [-] magnetic torque bar alignment matrix in Body frame components, must be provided in row-major format +typedef struct +{ + int numMTB; //!< [-] number of magnetic torque bars on the spacecraft + double GtMatrix_B[3 * MAX_EFF_CNT]; //!< [-] magnetic torque bar alignment matrix in Body frame components, must be + //!< provided in row-major format double maxMtbDipoles[MAX_EFF_CNT]; //!< [A-m2] maximum commandable dipole for each magnetic torque bar -}MTBArrayConfigMsgPayload; - +} MTBArrayConfigMsgPayload; #endif - diff --git a/src/architecture/msgPayloadDefC/MTBCmdMsgPayload.h b/src/architecture/msgPayloadDefC/MTBCmdMsgPayload.h index 9adcea0b85..fd1f2068bc 100644 --- a/src/architecture/msgPayloadDefC/MTBCmdMsgPayload.h +++ b/src/architecture/msgPayloadDefC/MTBCmdMsgPayload.h @@ -23,9 +23,9 @@ #include "architecture/utilities/macroDefinitions.h" /*! @brief Message for magnetic torque bar dipole commands. */ -typedef struct { - double mtbDipoleCmds[MAX_EFF_CNT]; //!< [A-m2] magnetic torque bar dipole cmds -}MTBCmdMsgPayload; - +typedef struct +{ + double mtbDipoleCmds[MAX_EFF_CNT]; //!< [A-m2] magnetic torque bar dipole cmds +} MTBCmdMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/MTBMsgPayload.h b/src/architecture/msgPayloadDefC/MTBMsgPayload.h index f39f8b47f9..999645faf0 100644 --- a/src/architecture/msgPayloadDefC/MTBMsgPayload.h +++ b/src/architecture/msgPayloadDefC/MTBMsgPayload.h @@ -21,8 +21,9 @@ #define MTB_MSG_H /*! gravity gradient torque message definition */ -typedef struct { - double mtbNetTorque_B[3]; //!< [Nm] net torque contribution of all magnetic torque bars in Body frame components -}MTBMsgPayload; +typedef struct +{ + double mtbNetTorque_B[3]; //!< [Nm] net torque contribution of all magnetic torque bars in Body frame components +} MTBMsgPayload; -#endif //BASILISK_DATANODEUSAGESIMMSG_H +#endif // BASILISK_DATANODEUSAGESIMMSG_H diff --git a/src/architecture/msgPayloadDefC/MagneticFieldMsgPayload.h b/src/architecture/msgPayloadDefC/MagneticFieldMsgPayload.h index eec08bd004..e987bed9ff 100644 --- a/src/architecture/msgPayloadDefC/MagneticFieldMsgPayload.h +++ b/src/architecture/msgPayloadDefC/MagneticFieldMsgPayload.h @@ -21,7 +21,8 @@ #define magneticFieldSimMsg_H /*! magnetic field message definition */ -typedef struct { - double magField_N[3]; //!< [Tesla] Local magnetic field -}MagneticFieldMsgPayload; +typedef struct +{ + double magField_N[3]; //!< [Tesla] Local magnetic field +} MagneticFieldMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/NavAttMsgPayload.h b/src/architecture/msgPayloadDefC/NavAttMsgPayload.h old mode 100755 new mode 100644 index 47368f3376..d53cf7abe4 --- a/src/architecture/msgPayloadDefC/NavAttMsgPayload.h +++ b/src/architecture/msgPayloadDefC/NavAttMsgPayload.h @@ -21,12 +21,13 @@ #define NAV_ATT_MESSAGE_H /*! @brief Structure used to define the output definition for attitude guidance*/ -typedef struct { - double timeTag; //!< [s] Current vehicle time-tag associated with measurements*/ - double sigma_BN[3]; //!< Current spacecraft attitude (MRPs) of body relative to inertial */ - double omega_BN_B[3]; //!< [r/s] Current spacecraft angular velocity vector of body frame B relative to inertial frame N, in B frame components - double vehSunPntBdy[3]; //!< Current sun pointing vector in body frame -}NavAttMsgPayload; - +typedef struct +{ + double timeTag; //!< [s] Current vehicle time-tag associated with measurements*/ + double sigma_BN[3]; //!< Current spacecraft attitude (MRPs) of body relative to inertial */ + double omega_BN_B[3]; //!< [r/s] Current spacecraft angular velocity vector of body frame B relative to inertial + //!< frame N, in B frame components + double vehSunPntBdy[3]; //!< Current sun pointing vector in body frame +} NavAttMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/NavTransMsgPayload.h b/src/architecture/msgPayloadDefC/NavTransMsgPayload.h old mode 100755 new mode 100644 index 7ea38596f4..94c82bbcc3 --- a/src/architecture/msgPayloadDefC/NavTransMsgPayload.h +++ b/src/architecture/msgPayloadDefC/NavTransMsgPayload.h @@ -20,14 +20,13 @@ #ifndef NAV_TRANS_MESSAGE_H #define NAV_TRANS_MESSAGE_H - /*! @brief Structure used to define the output definition for translatoin guidance*/ -typedef struct { - double timeTag; //!< [s] Current vehicle time-tag associated with measurements*/ - double r_BN_N[3]; //!< [m] Current inertial spacecraft position vector in inertial frame N components - double v_BN_N[3]; //!< [m/s] Current inertial velocity of the spacecraft in inertial frame N components - double vehAccumDV[3]; //!< [m/s] Total accumulated delta-velocity for s/c -}NavTransMsgPayload; - +typedef struct +{ + double timeTag; //!< [s] Current vehicle time-tag associated with measurements*/ + double r_BN_N[3]; //!< [m] Current inertial spacecraft position vector in inertial frame N components + double v_BN_N[3]; //!< [m/s] Current inertial velocity of the spacecraft in inertial frame N components + double vehAccumDV[3]; //!< [m/s] Total accumulated delta-velocity for s/c +} NavTransMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/OpNavCirclesMsgPayload.h b/src/architecture/msgPayloadDefC/OpNavCirclesMsgPayload.h old mode 100755 new mode 100644 index 3f146ecd83..1fdb0f7c1a --- a/src/architecture/msgPayloadDefC/OpNavCirclesMsgPayload.h +++ b/src/architecture/msgPayloadDefC/OpNavCirclesMsgPayload.h @@ -24,15 +24,16 @@ #include "architecture/utilities/macroDefinitions.h" -typedef struct { - uint64_t timeTag; //!< --[ns] Current vehicle time-tag associated with measurements - int valid; //!< -- Valid measurement if 1, not if 0 - int64_t cameraID; //!< -- [-] ID of the camera that took the snapshot +typedef struct +{ + uint64_t timeTag; //!< --[ns] Current vehicle time-tag associated with measurements + int valid; //!< -- Valid measurement if 1, not if 0 + int64_t cameraID; //!< -- [-] ID of the camera that took the snapshot double planetIds[MAX_CIRCLE_NUM]; //!< -- [-] Ids for identified celestial bodies - double circlesCenters[2*MAX_CIRCLE_NUM]; //!< -- [-] Center x, y in pixels of the circles - double circlesRadii[MAX_CIRCLE_NUM]; //!< -- [-] Radius rho in pixels of the circles - double uncertainty[3*3]; //!< -- [-] Uncertainty about the image processing results for x, y, rho (center and radius) for main circle -}OpNavCirclesMsgPayload; - + double circlesCenters[2 * MAX_CIRCLE_NUM]; //!< -- [-] Center x, y in pixels of the circles + double circlesRadii[MAX_CIRCLE_NUM]; //!< -- [-] Radius rho in pixels of the circles + double uncertainty[3 * 3]; //!< -- [-] Uncertainty about the image processing results for x, y, rho (center and + //!< radius) for main circle +} OpNavCirclesMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/OpNavFilterMsgPayload.h b/src/architecture/msgPayloadDefC/OpNavFilterMsgPayload.h old mode 100755 new mode 100644 index 6bc51342ed..80184df4ad --- a/src/architecture/msgPayloadDefC/OpNavFilterMsgPayload.h +++ b/src/architecture/msgPayloadDefC/OpNavFilterMsgPayload.h @@ -20,20 +20,19 @@ #ifndef OPNAV_FILTER_MESSAGE_H #define OPNAV_FILTER_MESSAGE_H - #define ODUKF_N_STATES 6 #define ODUKF_N_MEAS 3 /*! @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 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 - double postFitRes[ODUKF_N_MEAS]; //!< [-] PostFit Residuals - int numObs; //!< [-] Valid observation count for this frame -}OpNavFilterMsgPayload; - +typedef struct +{ + 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 + double postFitRes[ODUKF_N_MEAS]; //!< [-] PostFit Residuals + int numObs; //!< [-] Valid observation count for this frame +} OpNavFilterMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/OpNavLimbMsgPayload.h b/src/architecture/msgPayloadDefC/OpNavLimbMsgPayload.h old mode 100755 new mode 100644 index 9290cc0167..4ca5146053 --- a/src/architecture/msgPayloadDefC/OpNavLimbMsgPayload.h +++ b/src/architecture/msgPayloadDefC/OpNavLimbMsgPayload.h @@ -22,17 +22,15 @@ #include "architecture/utilities/macroDefinitions.h" - /*! @brief Structure used to define the message containing planet limb data for opNav*/ -typedef struct { - double timeTag; //!< --[s] Current vehicle time-tag associated with measurements - int valid; //!< -- Valid measurement if 1, not if 0 - int32_t numLimbPoints; //!< -- [-] Number of limb points found - int64_t cameraID; //!< -- [-] ID of the camera that took the snapshot - double planetIds; //!< -- [-] ID for identified celestial body - double limbPoints[2*MAX_LIMB_PNTS]; //!< -- [-] (x, y) in pixels of the limb points -}OpNavLimbMsgPayload; - +typedef struct +{ + double timeTag; //!< --[s] Current vehicle time-tag associated with measurements + int valid; //!< -- Valid measurement if 1, not if 0 + int32_t numLimbPoints; //!< -- [-] Number of limb points found + int64_t cameraID; //!< -- [-] ID of the camera that took the snapshot + double planetIds; //!< -- [-] ID for identified celestial body + double limbPoints[2 * MAX_LIMB_PNTS]; //!< -- [-] (x, y) in pixels of the limb points +} OpNavLimbMsgPayload; #endif - diff --git a/src/architecture/msgPayloadDefC/OpNavMsgPayload.h b/src/architecture/msgPayloadDefC/OpNavMsgPayload.h old mode 100755 new mode 100644 index ad186cd68e..3847ae7d09 --- a/src/architecture/msgPayloadDefC/OpNavMsgPayload.h +++ b/src/architecture/msgPayloadDefC/OpNavMsgPayload.h @@ -22,20 +22,18 @@ /*! @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 - int valid; //!< Valid measurement if 1, invalid if 0 - double covar_N[3*3]; //!< [m^2] Current covariance of the filter - double covar_B[3*3]; //!< [m^2] Current covariance of the filter - double covar_C[3*3]; //!< [m^2] Current covariance of the filter - double r_BN_N[3]; //!< [m] Current estimated state of the filter - double r_BN_B[3]; //!< [m] Current estimated state of the filter - double r_BN_C[3]; //!< [m] Current estimated state of the filter - int planetID; //!< [-] Planet being navigated, Earth=1, Mars=2, Jupiter=3 - int faultDetected; //!< [-] Bool if a fault is detected -}OpNavMsgPayload; - - - +typedef struct +{ + double timeTag; //!< [s] Current time of validity for output + int valid; //!< Valid measurement if 1, invalid if 0 + double covar_N[3 * 3]; //!< [m^2] Current covariance of the filter + double covar_B[3 * 3]; //!< [m^2] Current covariance of the filter + double covar_C[3 * 3]; //!< [m^2] Current covariance of the filter + double r_BN_N[3]; //!< [m] Current estimated state of the filter + double r_BN_B[3]; //!< [m] Current estimated state of the filter + double r_BN_C[3]; //!< [m] Current estimated state of the filter + int planetID; //!< [-] Planet being navigated, Earth=1, Mars=2, Jupiter=3 + int faultDetected; //!< [-] Bool if a fault is detected +} OpNavMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/PixelLineFilterMsgPayload.h b/src/architecture/msgPayloadDefC/PixelLineFilterMsgPayload.h old mode 100755 new mode 100644 index 429eaad999..be4030f7ca --- a/src/architecture/msgPayloadDefC/PixelLineFilterMsgPayload.h +++ b/src/architecture/msgPayloadDefC/PixelLineFilterMsgPayload.h @@ -20,21 +20,20 @@ #ifndef PIXLINE_FILTER_MESSAGE_H #define PIXLINE_FILTER_MESSAGE_H - #define PIXLINE_N_STATES 9 #define PIXLINE_DYN_STATES 6 #define PIXLINE_N_MEAS 6 /*! @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 covar[PIXLINE_N_STATES*PIXLINE_N_STATES]; //!< [-] Current covariance of the filter - double state[PIXLINE_N_STATES]; //!< [-] Current estimated state of the filter - double stateError[PIXLINE_N_STATES]; //!< [-] Current deviation of the state from the reference state - double postFitRes[PIXLINE_N_MEAS]; //!< [-] PostFit Residuals - int numObs; //!< [-] Valid observation count for this frame -}PixelLineFilterMsgPayload; - +typedef struct +{ + double timeTag; //!< [s] Current time of validity for output + double covar[PIXLINE_N_STATES * PIXLINE_N_STATES]; //!< [-] Current covariance of the filter + double state[PIXLINE_N_STATES]; //!< [-] Current estimated state of the filter + double stateError[PIXLINE_N_STATES]; //!< [-] Current deviation of the state from the reference state + double postFitRes[PIXLINE_N_MEAS]; //!< [-] PostFit Residuals + int numObs; //!< [-] Valid observation count for this frame +} PixelLineFilterMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/PlasmaFluxMsgPayload.h b/src/architecture/msgPayloadDefC/PlasmaFluxMsgPayload.h old mode 100755 new mode 100644 index aea011c8b2..0e1d987d23 --- a/src/architecture/msgPayloadDefC/PlasmaFluxMsgPayload.h +++ b/src/architecture/msgPayloadDefC/PlasmaFluxMsgPayload.h @@ -22,12 +22,12 @@ #define MAX_PLASMA_FLUX_SIZE 50 - //!@brief space weather ambient plasma flux message definition. -typedef struct { - double meanElectronFlux[MAX_PLASMA_FLUX_SIZE]; //!< [m^-2 s^-1 sr^-2 eV^-1] differential flux - double meanIonFlux[MAX_PLASMA_FLUX_SIZE]; //!< [m^-2 s^-1 sr^-2 eV^-1] differential flux - double energies[MAX_PLASMA_FLUX_SIZE]; //!< [eV] -}PlasmaFluxMsgPayload; +typedef struct +{ + double meanElectronFlux[MAX_PLASMA_FLUX_SIZE]; //!< [m^-2 s^-1 sr^-2 eV^-1] differential flux + double meanIonFlux[MAX_PLASMA_FLUX_SIZE]; //!< [m^-2 s^-1 sr^-2 eV^-1] differential flux + double energies[MAX_PLASMA_FLUX_SIZE]; //!< [eV] +} PlasmaFluxMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/PowerNodeStatusMsgPayload.h b/src/architecture/msgPayloadDefC/PowerNodeStatusMsgPayload.h index 4009b5dde4..bc191b8676 100644 --- a/src/architecture/msgPayloadDefC/PowerNodeStatusMsgPayload.h +++ b/src/architecture/msgPayloadDefC/PowerNodeStatusMsgPayload.h @@ -19,13 +19,10 @@ #ifndef BASILISK_POWERNODESTATUSMSG_H #define BASILISK_POWERNODESTATUSMSG_H - - //! @brief Power node command message used to change the state of power modules. -typedef struct{ - uint64_t powerStatus; //!< Power status indicator; 0 is off, 1 is on, additional values -}PowerNodeStatusMsgPayload; - - +typedef struct +{ + uint64_t powerStatus; //!< Power status indicator; 0 is off, 1 is on, additional values +} PowerNodeStatusMsgPayload; -#endif //BASILISK_POWERNODESTATUSMSG_H +#endif // BASILISK_POWERNODESTATUSMSG_H diff --git a/src/architecture/msgPayloadDefC/PowerNodeUsageMsgPayload.h b/src/architecture/msgPayloadDefC/PowerNodeUsageMsgPayload.h index 9fcff6226f..cd6166d1af 100644 --- a/src/architecture/msgPayloadDefC/PowerNodeUsageMsgPayload.h +++ b/src/architecture/msgPayloadDefC/PowerNodeUsageMsgPayload.h @@ -20,10 +20,9 @@ #ifndef BASILISK_POWERNODEUSAGESIMMSG_H #define BASILISK_POWERNODEUSAGESIMMSG_H - - /*! @brief Message for reporting the power consumed produced or consumed by a module.*/ -typedef struct{ +typedef struct +{ double netPower; //!< [W] Power usage by the message writer; positive for sources, negative for sinks -}PowerNodeUsageMsgPayload; -#endif //BASILISK_POWERNODEUSAGESIMMSG_H +} PowerNodeUsageMsgPayload; +#endif // BASILISK_POWERNODEUSAGESIMMSG_H diff --git a/src/architecture/msgPayloadDefC/PowerStorageFaultMsgPayload.h b/src/architecture/msgPayloadDefC/PowerStorageFaultMsgPayload.h index 48e3af3a6e..78d924c69f 100644 --- a/src/architecture/msgPayloadDefC/PowerStorageFaultMsgPayload.h +++ b/src/architecture/msgPayloadDefC/PowerStorageFaultMsgPayload.h @@ -20,11 +20,10 @@ #ifndef BASILISK_POWERSTORAGEFAULTSIMMSG_H #define BASILISK_POWERSTORAGEFAULTSIMMSG_H - - /*! @brief Message to store current battery stored charge, maximum charge, and received power.*/ -typedef struct{ +typedef struct +{ double faultCapacityRatio; //!< Fault capacity ratio (faulted capacity / nominal capacity) -}PowerStorageFaultMsgPayload; +} PowerStorageFaultMsgPayload; -#endif //BASILISK_POWERSTORAGEFAULTSIMMSG_H +#endif // BASILISK_POWERSTORAGEFAULTSIMMSG_H diff --git a/src/architecture/msgPayloadDefC/PowerStorageStatusMsgPayload.h b/src/architecture/msgPayloadDefC/PowerStorageStatusMsgPayload.h index cfd4cd9985..e1597e2c4d 100644 --- a/src/architecture/msgPayloadDefC/PowerStorageStatusMsgPayload.h +++ b/src/architecture/msgPayloadDefC/PowerStorageStatusMsgPayload.h @@ -20,13 +20,12 @@ #ifndef BASILISK_POWERSTORAGESTATUSSIMMSG_H #define BASILISK_POWERSTORAGESTATUSSIMMSG_H - - /*! @brief Message to store current battery stored charge, maximum charge, and received power.*/ -typedef struct{ - double storageLevel; //!< [W-s] Battery stored charge in Watt-hours. +typedef struct +{ + double storageLevel; //!< [W-s] Battery stored charge in Watt-hours. double storageCapacity; //!< [W-s] Maximum battery storage capacity. double currentNetPower; //!< [W] Current net power received/drained from the battery. -}PowerStorageStatusMsgPayload; +} PowerStorageStatusMsgPayload; -#endif //BASILISK_POWERSTORAGESTATUSSIMMSG_H +#endif // BASILISK_POWERSTORAGESTATUSSIMMSG_H diff --git a/src/architecture/msgPayloadDefC/PrescribedMotionMsgPayload.h b/src/architecture/msgPayloadDefC/PrescribedMotionMsgPayload.h index a1bd307980..4e471c54a8 100644 --- a/src/architecture/msgPayloadDefC/PrescribedMotionMsgPayload.h +++ b/src/architecture/msgPayloadDefC/PrescribedMotionMsgPayload.h @@ -20,16 +20,15 @@ #ifndef prescribedMotionSimMsg_h #define prescribedMotionSimMsg_h - - /*! @brief Structure used to define the individual prescribed motion state effector data message*/ -typedef struct { - double r_PM_M[3]; //!< [m] position vector from the M frame origin to the P frame origin in M frame components - double rPrime_PM_M[3]; //!< [m/s] B frame time derivative of r_PM_M - double rPrimePrime_PM_M[3]; //!< [m/s^2] B frame time derivative of rPrime_PM_M - double omega_PM_P[3]; //!< [rad/s] Angular velocity of the P frame wrt the M frame in P frame components - double omegaPrime_PM_P[3]; //!< [rad/s^2] B frame time derivative of omega_PM_P - double sigma_PM[3]; //!< MRP attitude parameters for the P frame relative to the M frame -}PrescribedMotionMsgPayload; - +/*! @brief Structure used to define the individual prescribed motion state effector data message*/ +typedef struct +{ + double r_PM_M[3]; //!< [m] position vector from the M frame origin to the P frame origin in M frame components + double rPrime_PM_M[3]; //!< [m/s] B frame time derivative of r_PM_M + double rPrimePrime_PM_M[3]; //!< [m/s^2] B frame time derivative of rPrime_PM_M + double omega_PM_P[3]; //!< [rad/s] Angular velocity of the P frame wrt the M frame in P frame components + double omegaPrime_PM_P[3]; //!< [rad/s^2] B frame time derivative of omega_PM_P + double sigma_PM[3]; //!< MRP attitude parameters for the P frame relative to the M frame +} PrescribedMotionMsgPayload; #endif /* prescribedMotionSimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h b/src/architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h index 84c095cc85..0961cfaacd 100644 --- a/src/architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h +++ b/src/architecture/msgPayloadDefC/PrescribedRotationMsgPayload.h @@ -20,13 +20,12 @@ #ifndef prescribedRotationSimMsg_h #define prescribedRotationSimMsg_h - - /*! @brief Structure used to define the prescribed motion state effector rotational state data message */ -typedef struct { - double omega_PM_P[3]; //!< [rad/s] Angular velocity of the P frame wrt the M frame in P frame components - double omegaPrime_PM_P[3]; //!< [rad/s^2] B/M frame time derivative of omega_PM_P - double sigma_PM[3]; //!< MRP attitude parameters for the P frame relative to the M frame -}PrescribedRotationMsgPayload; - +/*! @brief Structure used to define the prescribed motion state effector rotational state data message */ +typedef struct +{ + double omega_PM_P[3]; //!< [rad/s] Angular velocity of the P frame wrt the M frame in P frame components + double omegaPrime_PM_P[3]; //!< [rad/s^2] B/M frame time derivative of omega_PM_P + double sigma_PM[3]; //!< MRP attitude parameters for the P frame relative to the M frame +} PrescribedRotationMsgPayload; #endif /* prescribedRotationSimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/PrescribedTranslationMsgPayload.h b/src/architecture/msgPayloadDefC/PrescribedTranslationMsgPayload.h index c4a3d2fe20..b7daddc18d 100644 --- a/src/architecture/msgPayloadDefC/PrescribedTranslationMsgPayload.h +++ b/src/architecture/msgPayloadDefC/PrescribedTranslationMsgPayload.h @@ -20,13 +20,13 @@ #ifndef prescribedTranslationSimMsg_h #define prescribedTranslationSimMsg_h - - /*! @brief Structure used to define the prescribed motion state effector translational state data message */ -typedef struct { - double r_PM_M[3]; //!< [m] Position vector from the M frame origin to the P frame origin expressed in M frame components - double rPrime_PM_M[3]; //!< [m/s] B/M frame time derivative of r_PM_M - double rPrimePrime_PM_M[3]; //!< [m/s^2] B/M frame time derivative of rPrime_PM_M -}PrescribedTranslationMsgPayload; - +/*! @brief Structure used to define the prescribed motion state effector translational state data message */ +typedef struct +{ + double + r_PM_M[3]; //!< [m] Position vector from the M frame origin to the P frame origin expressed in M frame components + double rPrime_PM_M[3]; //!< [m/s] B/M frame time derivative of r_PM_M + double rPrimePrime_PM_M[3]; //!< [m/s^2] B/M frame time derivative of rPrime_PM_M +} PrescribedTranslationMsgPayload; #endif /* prescribedTranslationSimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/PyBatteryMsgPayload.h b/src/architecture/msgPayloadDefC/PyBatteryMsgPayload.h old mode 100755 new mode 100644 index 818af2d279..b02ae9f7ac --- a/src/architecture/msgPayloadDefC/PyBatteryMsgPayload.h +++ b/src/architecture/msgPayloadDefC/PyBatteryMsgPayload.h @@ -23,20 +23,19 @@ #include #include - //! @brief Container for all battery output data /*! This structure contains all data output by the python battery module*/ -typedef struct { - double stateOfCharge; //!< [%] Battery state of charge as %-full - double stateOfChargeAh; //!< [Ah] Battery state of charge un-scaled - double solarArrayTemperature; //!< [K] Temperature of solar arrays - double busVoltage; //!< [V] Bus voltage - double batteryCurrent; //!< [A] Total current flowing through battery - double solarArrayCurrent; //!< [A] Current sent to battery from solar arrays - double batteryEMF; //!< [V] Electro-motive force of battery at this current form look-up table - double batteryESR; //!< [Ohms] Equivalent Series Resistance of battery from look-up table - double batteryVoltage; //!< [V] total voltage across battery -}PyBatteryMsgPayload; - +typedef struct +{ + double stateOfCharge; //!< [%] Battery state of charge as %-full + double stateOfChargeAh; //!< [Ah] Battery state of charge un-scaled + double solarArrayTemperature; //!< [K] Temperature of solar arrays + double busVoltage; //!< [V] Bus voltage + double batteryCurrent; //!< [A] Total current flowing through battery + double solarArrayCurrent; //!< [A] Current sent to battery from solar arrays + double batteryEMF; //!< [V] Electro-motive force of battery at this current form look-up table + double batteryESR; //!< [Ohms] Equivalent Series Resistance of battery from look-up table + double batteryVoltage; //!< [V] total voltage across battery +} PyBatteryMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h b/src/architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h old mode 100755 new mode 100644 index c5e0c440db..1da5779e5b --- a/src/architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h @@ -24,12 +24,12 @@ #include "architecture/utilities/macroDefinitions.h" /*! @brief RW array configuration FSW msg */ -typedef struct{ - double GsMatrix_B[3*MAX_EFF_CNT]; //!< [-] The RW spin axis matrix in body frame components +typedef struct +{ + 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 + int numRW; //!< [-] The number of reaction wheels available on vehicle double uMax[MAX_EFF_CNT]; //!< [Nm] The maximum RW motor torque -}RWArrayConfigMsgPayload; - +} RWArrayConfigMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/RWAvailabilityMsgPayload.h b/src/architecture/msgPayloadDefC/RWAvailabilityMsgPayload.h old mode 100755 new mode 100644 index 14120f9f44..4c6396155f --- a/src/architecture/msgPayloadDefC/RWAvailabilityMsgPayload.h +++ b/src/architecture/msgPayloadDefC/RWAvailabilityMsgPayload.h @@ -24,11 +24,10 @@ #include "fswAlgorithms/fswUtilities/fswDefinitions.h" #include "architecture/utilities/macroDefinitions.h" - /*! @brief Array with availability of RW */ -typedef struct { - FSWdeviceAvailability wheelAvailability[MAX_EFF_CNT]; //!< The current state of the wheel -}RWAvailabilityMsgPayload; - +typedef struct +{ + FSWdeviceAvailability wheelAvailability[MAX_EFF_CNT]; //!< The current state of the wheel +} RWAvailabilityMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/RWCmdMsgPayload.h b/src/architecture/msgPayloadDefC/RWCmdMsgPayload.h old mode 100755 new mode 100644 index 0870344af4..8711f5f1ce --- a/src/architecture/msgPayloadDefC/RWCmdMsgPayload.h +++ b/src/architecture/msgPayloadDefC/RWCmdMsgPayload.h @@ -20,13 +20,10 @@ #ifndef SIM_RW_CMD_H #define SIM_RW_CMD_H - - /*! @brief Structure used to define the individual RW motor torque command message*/ -typedef struct { +typedef struct +{ double u_cmd; //!< [Nm], torque command for individual RW -}RWCmdMsgPayload; - - +} RWCmdMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/RWConfigElementMsgPayload.h b/src/architecture/msgPayloadDefC/RWConfigElementMsgPayload.h old mode 100755 new mode 100644 index ad3efdaff5..b3fb2a1563 --- a/src/architecture/msgPayloadDefC/RWConfigElementMsgPayload.h +++ b/src/architecture/msgPayloadDefC/RWConfigElementMsgPayload.h @@ -20,14 +20,12 @@ #ifndef _RW_CONFIG_ELEMENT_MESSAGE_H #define _RW_CONFIG_ELEMENT_MESSAGE_H - - /*! @brief Message used to define a single FSW RW configuration message */ -typedef struct { - double gsHat_B[3]; //!< [-] Spin axis unit vector of the wheel in structure - double Js; //!< [kgm2] Spin axis inertia of the wheel - double uMax; //!< [Nm] maximum RW motor torque -}RWConfigElementMsgPayload; - +typedef struct +{ + double gsHat_B[3]; //!< [-] Spin axis unit vector of the wheel in structure + double Js; //!< [kgm2] Spin axis inertia of the wheel + double uMax; //!< [Nm] maximum RW motor torque +} RWConfigElementMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/RWConfigLogMsgPayload.h b/src/architecture/msgPayloadDefC/RWConfigLogMsgPayload.h old mode 100755 new mode 100644 index c7eca5bbb9..30c4b09dcb --- a/src/architecture/msgPayloadDefC/RWConfigLogMsgPayload.h +++ b/src/architecture/msgPayloadDefC/RWConfigLogMsgPayload.h @@ -22,9 +22,9 @@ #include "simulation/dynamics/reactionWheels/reactionWheelSupport.h" - /*! @brief Structure used to define the individual RW configuration data message*/ -typedef struct { +typedef struct +{ double rWB_B[3]; //!< [m], position vector of the RW relative to the spacecraft body frame double gsHat_B[3]; //!< [-] spin axis unit vector in body frame double w2Hat0_B[3]; //!< [-] initial torque axis unit vector in body frame @@ -38,7 +38,7 @@ typedef struct { double U_s; //!< [kg-m], static imbalance double U_d; //!< [kg-m^2], dynamic imbalance double d; //!< [m], wheel center of mass offset from wheel frame origin - double J13; //!< [kg-m^2], x-z inertia of wheel about wheel center in wheel frame (imbalance) + double J13; //!< [kg-m^2], x-z inertia of wheel about wheel center in wheel frame (imbalance) double u_current; //!< [N-m], current motor torque double frictionTorque; //!< [N-m], friction Torque double u_max; //!< [N-m], Max torque @@ -48,8 +48,6 @@ typedef struct { double P_max; //!< [N-m/s], maximum wheel power double linearFrictionRatio; //!< [%] ratio relative to max speed value up to which the friction behaves linearly RWModels RWModel; //!< [-], Type of imbalance model to use -}RWConfigLogMsgPayload; - - +} RWConfigLogMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/RWConstellationMsgPayload.h b/src/architecture/msgPayloadDefC/RWConstellationMsgPayload.h old mode 100755 new mode 100644 index 74376a69cf..8a147cdff9 --- a/src/architecture/msgPayloadDefC/RWConstellationMsgPayload.h +++ b/src/architecture/msgPayloadDefC/RWConstellationMsgPayload.h @@ -23,13 +23,11 @@ #include "architecture/utilities/macroDefinitions.h" #include "RWConfigElementMsgPayload.h" - - /*! @brief Message used to define an array of RW FSW configurations */ -typedef struct { - int numRW; //!< [-] number of RWs - RWConfigElementMsgPayload reactionWheels[MAX_EFF_CNT]; //!< [-] array of the reaction wheels -}RWConstellationMsgPayload; - +typedef struct +{ + int numRW; //!< [-] number of RWs + RWConfigElementMsgPayload reactionWheels[MAX_EFF_CNT]; //!< [-] array of the reaction wheels +} RWConstellationMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/RWSpeedMsgPayload.h b/src/architecture/msgPayloadDefC/RWSpeedMsgPayload.h old mode 100755 new mode 100644 index 07eb87aa9d..19f5553f41 --- a/src/architecture/msgPayloadDefC/RWSpeedMsgPayload.h +++ b/src/architecture/msgPayloadDefC/RWSpeedMsgPayload.h @@ -20,16 +20,13 @@ #ifndef RW_SPEED_MESSAGE_STRUCT_H #define RW_SPEED_MESSAGE_STRUCT_H - - #include "architecture/utilities/macroDefinitions.h" - /*! @brief Structure used to define the output definition for reaction wheel speeds*/ -typedef struct { - double wheelSpeeds[MAX_EFF_CNT]; //!< r/s The current angular velocities of the RW wheel - double wheelThetas[MAX_EFF_CNT]; //!< rad The current angle of the RW if jitter is enabled -}RWSpeedMsgPayload; - +typedef struct +{ + double wheelSpeeds[MAX_EFF_CNT]; //!< r/s The current angular velocities of the RW wheel + double wheelThetas[MAX_EFF_CNT]; //!< rad The current angle of the RW if jitter is enabled +} RWSpeedMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/RateCmdMsgPayload.h b/src/architecture/msgPayloadDefC/RateCmdMsgPayload.h old mode 100755 new mode 100644 index 5b12176a40..a8cb7b58ea --- a/src/architecture/msgPayloadDefC/RateCmdMsgPayload.h +++ b/src/architecture/msgPayloadDefC/RateCmdMsgPayload.h @@ -20,13 +20,11 @@ #ifndef _RATE_STEERING_MSG_H #define _RATE_STEERING_MSG_H - - /*! @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 omegap_BastR_B[3]; //!< [r/s^2] Body-frame derivative of omega_BastR_B -}RateCmdMsgPayload; - +typedef struct +{ + 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; #endif diff --git a/src/architecture/msgPayloadDefC/RealTimeFactorMsgPayload.h b/src/architecture/msgPayloadDefC/RealTimeFactorMsgPayload.h old mode 100755 new mode 100644 index b9e3109386..e946936c88 --- a/src/architecture/msgPayloadDefC/RealTimeFactorMsgPayload.h +++ b/src/architecture/msgPayloadDefC/RealTimeFactorMsgPayload.h @@ -20,11 +20,10 @@ #ifndef REAL_TIME_FACTOR_H #define REAL_TIME_FACTOR_H - //! @brief Container for sim real time speed up/down factor. -typedef struct { - double speedFactor; //!< -- factor of real time at which the sim should run -}RealTimeFactorMsgPayload; - +typedef struct +{ + double speedFactor; //!< -- factor of real time at which the sim should run +} RealTimeFactorMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/ReconfigBurnArrayInfoMsgPayload.h b/src/architecture/msgPayloadDefC/ReconfigBurnArrayInfoMsgPayload.h index ce44a4ba5b..6c157192bf 100644 --- a/src/architecture/msgPayloadDefC/ReconfigBurnArrayInfoMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ReconfigBurnArrayInfoMsgPayload.h @@ -24,10 +24,10 @@ #include "ReconfigBurnInfoMsgPayload.h" - //! @brief Container for the orbit reconfiguration burn information. -typedef struct { +//! @brief Container for the orbit reconfiguration burn information. +typedef struct +{ ReconfigBurnInfoMsgPayload burnArray[MAX_BURN_CNT]; //!< array of burn info messages -}ReconfigBurnArrayInfoMsgPayload; +} 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..3d3ed2af6d 100644 --- a/src/architecture/msgPayloadDefC/ReconfigBurnInfoMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ReconfigBurnInfoMsgPayload.h @@ -21,12 +21,12 @@ #define RECONFIG_BURN_INFO_H //! @brief Container for the orbit reconfiguration burn information. -typedef struct { - int flag; //!< 0:not scheduled yet, 1:not burned yet, 2:already burned, 3:skipped (combined with another burn) - double t; //!< [s] simulation time of the scheduled burn since module reset +typedef struct +{ + int flag; //!< 0:not scheduled yet, 1:not burned yet, 2:already burned, 3:skipped (combined with another burn) + double t; //!< [s] simulation time of the scheduled burn since module reset double thrustOnTime; //!< thrust on duration time [s] double sigma_RN[3]; //!< target attitude -}ReconfigBurnInfoMsgPayload; +} ReconfigBurnInfoMsgPayload; - -#endif \ No newline at end of file +#endif diff --git a/src/architecture/msgPayloadDefC/SCEnergyMomentumMsgPayload.h b/src/architecture/msgPayloadDefC/SCEnergyMomentumMsgPayload.h old mode 100755 new mode 100644 index cb5f8ec0dd..838eacaf32 --- a/src/architecture/msgPayloadDefC/SCEnergyMomentumMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SCEnergyMomentumMsgPayload.h @@ -20,18 +20,14 @@ #ifndef SC_ENERGY_MOMENTUM_MESSAGE_H #define SC_ENERGY_MOMENTUM_MESSAGE_H - - /*! @brief This structure is used in the messaging system to communicate what the state of the vehicle is currently.*/ -typedef struct { - double spacecraftOrbEnergy; //!< [J] Total orbital kinetic energy - double spacecraftRotEnergy; //!< [J] Total rotational energy - double spacecraftOrbAngMomPntN_N[3]; //!< [kg m^2/s] Total orbital angular momentum about N in N frame components - double spacecraftRotAngMomPntC_N[3]; //!< [kg m^2/s] Total rotational angular momentum about C in N frame components -}SCEnergyMomentumMsgPayload; - - - +typedef struct +{ + double spacecraftOrbEnergy; //!< [J] Total orbital kinetic energy + double spacecraftRotEnergy; //!< [J] Total rotational energy + double spacecraftOrbAngMomPntN_N[3]; //!< [kg m^2/s] Total orbital angular momentum about N in N frame components + double spacecraftRotAngMomPntC_N[3]; //!< [kg m^2/s] Total rotational angular momentum about C in N frame components +} SCEnergyMomentumMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/SCMassPropsMsgPayload.h b/src/architecture/msgPayloadDefC/SCMassPropsMsgPayload.h old mode 100755 new mode 100644 index 480468efce..87648ddd6f --- a/src/architecture/msgPayloadDefC/SCMassPropsMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SCMassPropsMsgPayload.h @@ -20,14 +20,13 @@ #ifndef SC_MASS_PROPS_MESSAGE_H #define SC_MASS_PROPS_MESSAGE_H - /*! @brief This structure is used in the messaging system to communicate what the mass properties of the vehicle are currently.*/ -typedef struct { - double massSC; //!< kg Current spacecraft mass - double c_B[3]; //!< m Center of mass of spacecraft with respect to point B - double ISC_PntB_B[3][3]; //!< kgm2 Inertia tensor of spacecraft (relative to body) -}SCMassPropsMsgPayload; - +typedef struct +{ + double massSC; //!< kg Current spacecraft mass + double c_B[3]; //!< m Center of mass of spacecraft with respect to point B + double ISC_PntB_B[3][3]; //!< kgm2 Inertia tensor of spacecraft (relative to body) +} SCMassPropsMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/SCStatesMsgPayload.h b/src/architecture/msgPayloadDefC/SCStatesMsgPayload.h old mode 100755 new mode 100644 index 4d4005b274..ea7a5177a4 --- a/src/architecture/msgPayloadDefC/SCStatesMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SCStatesMsgPayload.h @@ -20,24 +20,22 @@ #ifndef SC_STATE_MESSAGE_H #define SC_STATE_MESSAGE_H - /*! @brief This structure is used in the messaging system to communicate what the state of the vehicle is currently.*/ -typedef struct { - double r_BN_N[3]; //!< m Current position vector (inertial) - double v_BN_N[3]; //!< m/s Current velocity vector (inertial) - double r_CN_N[3]; //!< m Current position of CoM vector (inertial) - double v_CN_N[3]; //!< m/s Current velocity of CoM vector (inertial) - double sigma_BN[3]; //!< -- Current MRPs (inertial) - double omega_BN_B[3]; //!< r/s Current angular velocity - double omegaDot_BN_B[3]; //!< r/s/s Current angular acceleration - double TotalAccumDVBdy[3]; //!< m/s Accumulated DV of center of mass in body frame coordinates - double TotalAccumDV_BN_B[3]; //!< m/s Accumulated DV of body frame in body frame coordinates - double TotalAccumDV_CN_N[3]; //!< m/s Accumulated DV of center of mass in inertial frame coordinates - double nonConservativeAccelpntB_B[3];//!< m/s/s Current Spacecraft non-conservative body frame accel - uint64_t MRPSwitchCount; //!< -- Number of times that MRPs have switched -}SCStatesMsgPayload; - - +typedef struct +{ + double r_BN_N[3]; //!< m Current position vector (inertial) + double v_BN_N[3]; //!< m/s Current velocity vector (inertial) + double r_CN_N[3]; //!< m Current position of CoM vector (inertial) + double v_CN_N[3]; //!< m/s Current velocity of CoM vector (inertial) + double sigma_BN[3]; //!< -- Current MRPs (inertial) + double omega_BN_B[3]; //!< r/s Current angular velocity + double omegaDot_BN_B[3]; //!< r/s/s Current angular acceleration + double TotalAccumDVBdy[3]; //!< m/s Accumulated DV of center of mass in body frame coordinates + double TotalAccumDV_BN_B[3]; //!< m/s Accumulated DV of body frame in body frame coordinates + double TotalAccumDV_CN_N[3]; //!< m/s Accumulated DV of center of mass in inertial frame coordinates + double nonConservativeAccelpntB_B[3]; //!< m/s/s Current Spacecraft non-conservative body frame accel + uint64_t MRPSwitchCount; //!< -- Number of times that MRPs have switched +} SCStatesMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/STAttMsgPayload.h b/src/architecture/msgPayloadDefC/STAttMsgPayload.h old mode 100755 new mode 100644 index ca3bfbc5e7..8276e5401f --- a/src/architecture/msgPayloadDefC/STAttMsgPayload.h +++ b/src/architecture/msgPayloadDefC/STAttMsgPayload.h @@ -20,12 +20,11 @@ #ifndef ST_ATTITUDE_MESSAGE_H #define ST_ATTITUDE_MESSAGE_H - /*! @brief Output structure for ST attitude measurement in vehicle body frame*/ -typedef struct { - uint64_t timeTag; //!< [ns] Vehicle time code associated with measurement - double MRP_BdyInrtl[3]; //!< [-] MRP estimate of inertial to body transformation -}STAttMsgPayload; - +typedef struct +{ + uint64_t timeTag; //!< [ns] Vehicle time code associated with measurement + double MRP_BdyInrtl[3]; //!< [-] MRP estimate of inertial to body transformation +} STAttMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/STSensorMsgPayload.h b/src/architecture/msgPayloadDefC/STSensorMsgPayload.h old mode 100755 new mode 100644 index f3c4fd73e9..9b3d7833df --- a/src/architecture/msgPayloadDefC/STSensorMsgPayload.h +++ b/src/architecture/msgPayloadDefC/STSensorMsgPayload.h @@ -21,12 +21,11 @@ #define _ST_HW_OUTPUT_ #include - /*! @brief Output structure for ST structure in vehicle body frame*/ -typedef struct { - uint64_t timeTag; //!< [ns] Time tag placed on the output state - double qInrtl2Case[4]; //!< [-] Quaternion to go from the inertial to case -}STSensorMsgPayload; - +typedef struct +{ + uint64_t timeTag; //!< [ns] Time tag placed on the output state + double qInrtl2Case[4]; //!< [-] Quaternion to go from the inertial to case +} STSensorMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/ScalarJointStateMsgPayload.h b/src/architecture/msgPayloadDefC/ScalarJointStateMsgPayload.h index 84be3fd54a..140da75d46 100644 --- a/src/architecture/msgPayloadDefC/ScalarJointStateMsgPayload.h +++ b/src/architecture/msgPayloadDefC/ScalarJointStateMsgPayload.h @@ -29,7 +29,8 @@ * This message can be used to represent the current state of a joint, or * to indicate the desired state when constraining the state of a joint. */ -typedef struct { +typedef struct +{ double state; //!< [rad] or [m] State of the joint } ScalarJointStateMsgPayload; diff --git a/src/architecture/msgPayloadDefC/SingleActuatorMsgPayload.h b/src/architecture/msgPayloadDefC/SingleActuatorMsgPayload.h index 539c76d49b..ac6440f854 100644 --- a/src/architecture/msgPayloadDefC/SingleActuatorMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SingleActuatorMsgPayload.h @@ -35,7 +35,8 @@ * and the `input` in this message defines the magnitude of both. Other actuators * in MuJoCo are supported and controled with this message. */ -typedef struct { +typedef struct +{ double input; //!< [-] Real valued input } SingleActuatorMsgPayload; diff --git a/src/architecture/msgPayloadDefC/SmallBodyNavMsgPayload.h b/src/architecture/msgPayloadDefC/SmallBodyNavMsgPayload.h index 4b0405f5b2..37dbd1659d 100644 --- a/src/architecture/msgPayloadDefC/SmallBodyNavMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SmallBodyNavMsgPayload.h @@ -23,9 +23,10 @@ #define SMALL_BODY_NAV_N_STATES 12 //! @brief Full states and covariances associated with spacecraft navigation about a small body -typedef struct{ - double state[SMALL_BODY_NAV_N_STATES]; //!< Current state estimate from the filter - double covar[SMALL_BODY_NAV_N_STATES][SMALL_BODY_NAV_N_STATES]; //!< Current covariance of the filter -}SmallBodyNavMsgPayload; +typedef struct +{ + double state[SMALL_BODY_NAV_N_STATES]; //!< Current state estimate from the filter + double covar[SMALL_BODY_NAV_N_STATES][SMALL_BODY_NAV_N_STATES]; //!< Current covariance of the filter +} SmallBodyNavMsgPayload; -#endif //AVS_SMALLBODYNAVMSGPAYLOAD_H +#endif // AVS_SMALLBODYNAVMSGPAYLOAD_H diff --git a/src/architecture/msgPayloadDefC/SmallBodyNavUKFMsgPayload.h b/src/architecture/msgPayloadDefC/SmallBodyNavUKFMsgPayload.h index ef0e1f7d6c..9e755c1631 100644 --- a/src/architecture/msgPayloadDefC/SmallBodyNavUKFMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SmallBodyNavUKFMsgPayload.h @@ -22,10 +22,10 @@ /*! @brief Structure used to define the output of the sub-module. This is the same output message that is used by all sub-modules in the module folder. */ -typedef struct{ - double state[9]; //!< [units] state - double covar[9][9]; //!< [units] covariance -}SmallBodyNavUKFMsgPayload; - +typedef struct +{ + double state[9]; //!< [units] state + double covar[9][9]; //!< [units] covariance +} SmallBodyNavUKFMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/SolarFluxMsgPayload.h b/src/architecture/msgPayloadDefC/SolarFluxMsgPayload.h index b146ee826d..9bb193bb9e 100644 --- a/src/architecture/msgPayloadDefC/SolarFluxMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SolarFluxMsgPayload.h @@ -20,11 +20,10 @@ #ifndef solarFluxSimMsg_h #define solarFluxSimMsg_h - //!@brief Solar flux message definition. -typedef struct { - double flux; //!< [W/m2] at s/c position -}SolarFluxMsgPayload; - +typedef struct +{ + double flux; //!< [W/m2] at s/c position +} SolarFluxMsgPayload; #endif /* solarFluxSimMsg_h */ diff --git a/src/architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h b/src/architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h old mode 100755 new mode 100644 index 9c2c476dc1..d339a639f7 --- a/src/architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h @@ -20,19 +20,18 @@ #ifndef SpicePlanetState_MESSAGE_H #define SpicePlanetState_MESSAGE_H - #define MAX_BODY_NAME_LENGTH 64 //! @brief The SPICE planet statate structure is the struct used to ouput planetary body states to the messaging system -typedef struct { - double J2000Current; //!< s Time of validity for the planet state - double PositionVector[3]; //!< m True position of the planet for the time - double VelocityVector[3]; //!< m/s True velocity of the planet for the time - double J20002Pfix[3][3]; //!< (-) Orientation matrix of planet-fixed relative to inertial - double J20002Pfix_dot[3][3]; //!< (-) Derivative of the orientation matrix of planet-fixed relative to inertial - int computeOrient; //!< (-) Flag indicating whether the reference should be computed - char PlanetName[MAX_BODY_NAME_LENGTH]; //!< -- Name of the planet for the state -}SpicePlanetStateMsgPayload; - +typedef struct +{ + double J2000Current; //!< s Time of validity for the planet state + double PositionVector[3]; //!< m True position of the planet for the time + double VelocityVector[3]; //!< m/s True velocity of the planet for the time + double J20002Pfix[3][3]; //!< (-) Orientation matrix of planet-fixed relative to inertial + double J20002Pfix_dot[3][3]; //!< (-) Derivative of the orientation matrix of planet-fixed relative to inertial + int computeOrient; //!< (-) Flag indicating whether the reference should be computed + char PlanetName[MAX_BODY_NAME_LENGTH]; //!< -- Name of the planet for the state +} SpicePlanetStateMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/SpiceTimeMsgPayload.h b/src/architecture/msgPayloadDefC/SpiceTimeMsgPayload.h old mode 100755 new mode 100644 index 6060b9323e..91608dc595 --- a/src/architecture/msgPayloadDefC/SpiceTimeMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SpiceTimeMsgPayload.h @@ -20,16 +20,14 @@ #ifndef SPICE_TIME_MESSAGE_H #define SPICE_TIME_MESSAGE_H - - //! The SPICE time output structure outputs time information to the rest of the system -typedef struct { - double J2000Current; //!< s Current J2000 elapsed time - double JulianDateCurrent; //!< d Current JulianDate - double GPSSeconds; //!< s Current GPS seconds - uint16_t GPSWeek; //!< -- Current GPS week value - uint64_t GPSRollovers; //!< -- Count on the number of GPS rollovers -}SpiceTimeMsgPayload; - +typedef struct +{ + double J2000Current; //!< s Current J2000 elapsed time + double JulianDateCurrent; //!< d Current JulianDate + double GPSSeconds; //!< s Current GPS seconds + uint16_t GPSWeek; //!< -- Current GPS week value + uint64_t GPSRollovers; //!< -- Count on the number of GPS rollovers +} SpiceTimeMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/SunlineFilterMsgPayload.h b/src/architecture/msgPayloadDefC/SunlineFilterMsgPayload.h old mode 100755 new mode 100644 index 160ca2228c..92c00fd002 --- a/src/architecture/msgPayloadDefC/SunlineFilterMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SunlineFilterMsgPayload.h @@ -20,7 +20,6 @@ #ifndef SUNLINE_FILTER_MESSAGE_H #define SUNLINE_FILTER_MESSAGE_H - #define SKF_N_STATES 6 #define SKF_N_STATES_SWITCH 6 #define EKF_N_STATES_SWITCH 5 @@ -29,14 +28,14 @@ /*! @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 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 - double postFitRes[MAX_N_CSS_MEAS]; //!< [-] PostFit Residuals - int numObs; //!< [-] Valid observation count for this frame -}SunlineFilterMsgPayload; - +typedef struct +{ + 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 + double postFitRes[MAX_N_CSS_MEAS]; //!< [-] PostFit Residuals + int numObs; //!< [-] Valid observation count for this frame +} SunlineFilterMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/SwDataMsgPayload.h b/src/architecture/msgPayloadDefC/SwDataMsgPayload.h index ff520bdb76..d9890c23ef 100644 --- a/src/architecture/msgPayloadDefC/SwDataMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SwDataMsgPayload.h @@ -21,9 +21,9 @@ #define BASILISK_SWDATASIMMSG_H /*! SW data message definition */ -typedef struct { - double dataValue; //!< kg/m^3 Local neutral particle density -}SwDataMsgPayload; +typedef struct +{ + double dataValue; //!< kg/m^3 Local neutral particle density +} SwDataMsgPayload; - -#endif //BASILISK_SWDATASIMMSG_H +#endif // BASILISK_SWDATASIMMSG_H diff --git a/src/architecture/msgPayloadDefC/SynchClockMsgPayload.h b/src/architecture/msgPayloadDefC/SynchClockMsgPayload.h old mode 100755 new mode 100644 index 8ffbcdba4f..bb9ee3cdce --- a/src/architecture/msgPayloadDefC/SynchClockMsgPayload.h +++ b/src/architecture/msgPayloadDefC/SynchClockMsgPayload.h @@ -20,14 +20,12 @@ #ifndef SYNC_CLOCK_MESSAGE_H #define SYNC_CLOCK_MESSAGE_H - //! @brief Output diagnostic structure used for analyzing how the synch is performing. -typedef struct { - double initTimeDelta; //!< s Time remaining in synch frame on arrival - double finalTimeDelta; //!< s Time remaining in synch frame on departure - uint64_t overrunCounter; //!< (-) Indicator of how many times we've missed the synch frame -}SynchClockMsgPayload; - - +typedef struct +{ + double initTimeDelta; //!< s Time remaining in synch frame on arrival + double finalTimeDelta; //!< s Time remaining in synch frame on departure + uint64_t overrunCounter; //!< (-) Indicator of how many times we've missed the synch frame +} SynchClockMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/TAMSensorBodyMsgPayload.h b/src/architecture/msgPayloadDefC/TAMSensorBodyMsgPayload.h index ff1456e7fd..803df15048 100644 --- a/src/architecture/msgPayloadDefC/TAMSensorBodyMsgPayload.h +++ b/src/architecture/msgPayloadDefC/TAMSensorBodyMsgPayload.h @@ -20,11 +20,10 @@ #ifndef _TAM_SENSOR_BODY_MESSAGE_H #define _TAM_SENSOR_BODY_MESSAGE_H - /*! @brief Output structure for TAM measurements*/ -typedef struct { - double tam_B[3]; //!< [Tesla] TAM measurements in vehicle body frame -}TAMSensorBodyMsgPayload; - +typedef struct +{ + double tam_B[3]; //!< [Tesla] TAM measurements in vehicle body frame +} TAMSensorBodyMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/TAMSensorMsgPayload.h b/src/architecture/msgPayloadDefC/TAMSensorMsgPayload.h index 7a695947d0..8d6fb744cf 100644 --- a/src/architecture/msgPayloadDefC/TAMSensorMsgPayload.h +++ b/src/architecture/msgPayloadDefC/TAMSensorMsgPayload.h @@ -20,11 +20,10 @@ #ifndef _TAM_SENSOR_MESSAGE_H #define _TAM_SENSOR_MESSAGE_H - //! @brief Simulated TAM Sensor output message definition. -typedef struct { - double tam_S[3]; //!< [T] Magnetic field measurements in sensor frame -}TAMSensorMsgPayload; - +typedef struct +{ + double tam_S[3]; //!< [T] Magnetic field measurements in sensor frame +} TAMSensorMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/TDBVehicleClockCorrelationMsgPayload.h b/src/architecture/msgPayloadDefC/TDBVehicleClockCorrelationMsgPayload.h old mode 100755 new mode 100644 index ae192dd51a..cf1f5c38a3 --- a/src/architecture/msgPayloadDefC/TDBVehicleClockCorrelationMsgPayload.h +++ b/src/architecture/msgPayloadDefC/TDBVehicleClockCorrelationMsgPayload.h @@ -20,13 +20,13 @@ #ifndef _EPHEMERIS_INTERFACE_DATA_H_ #define _EPHEMERIS_INTERFACE_DATA_H_ - /*! @brief time correlation factor structure used to take vehicle time and convert it over to ephemeris time (TDB) */ -typedef struct { - double ephemerisTime; //!< [s] Ephemeris time associated with the vehicle time - double vehicleClockTime; //!< [s] Vehicle time code converted over to seconds -}TDBVehicleClockCorrelationMsgPayload; +typedef struct +{ + double ephemerisTime; //!< [s] Ephemeris time associated with the vehicle time + double vehicleClockTime; //!< [s] Vehicle time code converted over to seconds +} TDBVehicleClockCorrelationMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/THRArrayCmdForceMsgPayload.h b/src/architecture/msgPayloadDefC/THRArrayCmdForceMsgPayload.h old mode 100755 new mode 100644 index 626962de93..a46e1fe4e4 --- a/src/architecture/msgPayloadDefC/THRArrayCmdForceMsgPayload.h +++ b/src/architecture/msgPayloadDefC/THRArrayCmdForceMsgPayload.h @@ -22,12 +22,10 @@ #include "architecture/utilities/macroDefinitions.h" - - /*! @brief Message used to define a vector of thruster force commands */ -typedef struct { - double thrForce[MAX_EFF_CNT]; //!< [N] array of thruster force values -}THRArrayCmdForceMsgPayload; - +typedef struct +{ + double thrForce[MAX_EFF_CNT]; //!< [N] array of thruster force values +} THRArrayCmdForceMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/THRArrayConfigMsgPayload.h b/src/architecture/msgPayloadDefC/THRArrayConfigMsgPayload.h old mode 100755 new mode 100644 index 33d0a322d2..34d1e8e9ca --- a/src/architecture/msgPayloadDefC/THRArrayConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/THRArrayConfigMsgPayload.h @@ -23,14 +23,11 @@ #include "architecture/utilities/macroDefinitions.h" #include "THRConfigMsgPayload.h" - - /*! @brief FSW message definition containing the thruster cluster information */ -typedef struct { - int numThrusters; //!< [-] number of thrusters - THRConfigMsgPayload thrusters[MAX_EFF_CNT]; //!< [-] array of thruster configuration information -}THRArrayConfigMsgPayload; - - +typedef struct +{ + int numThrusters; //!< [-] number of thrusters + THRConfigMsgPayload thrusters[MAX_EFF_CNT]; //!< [-] array of thruster configuration information +} THRArrayConfigMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/THRArrayOnTimeCmdMsgPayload.h b/src/architecture/msgPayloadDefC/THRArrayOnTimeCmdMsgPayload.h old mode 100755 new mode 100644 index dd1c18464e..99e03e4453 --- a/src/architecture/msgPayloadDefC/THRArrayOnTimeCmdMsgPayload.h +++ b/src/architecture/msgPayloadDefC/THRArrayOnTimeCmdMsgPayload.h @@ -22,11 +22,10 @@ #include "architecture/utilities/macroDefinitions.h" - /*! @brief Structure used to define the output definition for vehicle effectors*/ -typedef struct { - double OnTimeRequest[MAX_EFF_CNT]; //!< - Control request fraction array -}THRArrayOnTimeCmdMsgPayload; - +typedef struct +{ + double OnTimeRequest[MAX_EFF_CNT]; //!< - Control request fraction array +} THRArrayOnTimeCmdMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/THRConfigMsgPayload.h b/src/architecture/msgPayloadDefC/THRConfigMsgPayload.h old mode 100755 new mode 100644 index f280b5a83a..0facb0d0c6 --- a/src/architecture/msgPayloadDefC/THRConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/THRConfigMsgPayload.h @@ -20,15 +20,13 @@ #ifndef FSW_THR_CONFIG_MESSAGE_H #define FSW_THR_CONFIG_MESSAGE_H - - /*! @brief Single Thruster configuration message */ -typedef struct { - double rThrust_B[3]; //!< [m] Location of the thruster in the spacecraft - double tHatThrust_B[3]; //!< [-] Unit vector of the thrust direction - double maxThrust; //!< [N] Max thrust - double swirlTorque; //!< [Nm] Swirl torque -}THRConfigMsgPayload; - +typedef struct +{ + double rThrust_B[3]; //!< [m] Location of the thruster in the spacecraft + double tHatThrust_B[3]; //!< [-] Unit vector of the thrust direction + double maxThrust; //!< [N] Max thrust + double swirlTorque; //!< [Nm] Swirl torque +} THRConfigMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/TemperatureMsgPayload.h b/src/architecture/msgPayloadDefC/TemperatureMsgPayload.h index 6a19ec8981..418956627d 100644 --- a/src/architecture/msgPayloadDefC/TemperatureMsgPayload.h +++ b/src/architecture/msgPayloadDefC/TemperatureMsgPayload.h @@ -20,13 +20,10 @@ #ifndef TEMPERATURE_H #define TEMPERATURE_H +/*! @brief Message for reporting the power consumed produced or consumed by a module.*/ +typedef struct +{ + double temperature; //!< [Celsius] current temperature +} TemperatureMsgPayload; - - /*! @brief Message for reporting the power consumed produced or consumed by a module.*/ -typedef struct { - double temperature; //!< [Celsius] current temperature -}TemperatureMsgPayload; - - -#endif //TEMPERATURE_H - +#endif // TEMPERATURE_H diff --git a/src/architecture/msgPayloadDefC/TorqueAtSiteMsgPayload.h b/src/architecture/msgPayloadDefC/TorqueAtSiteMsgPayload.h index d505eafa74..52e6c35a26 100644 --- a/src/architecture/msgPayloadDefC/TorqueAtSiteMsgPayload.h +++ b/src/architecture/msgPayloadDefC/TorqueAtSiteMsgPayload.h @@ -25,8 +25,9 @@ * Sites are locations fixed on a body (in the context of MJScene) with an * associated reference frame. This message represents a torque that * could be applied at this site and expressed on the site's reference frame. -*/ -typedef struct { + */ +typedef struct +{ double torque_S[3]; //!< [Nm] Torque in Newton*Meter } TorqueAtSiteMsgPayload; diff --git a/src/architecture/msgPayloadDefC/TransRefMsgPayload.h b/src/architecture/msgPayloadDefC/TransRefMsgPayload.h old mode 100755 new mode 100644 index b1058bfea4..dfcdc81b07 --- a/src/architecture/msgPayloadDefC/TransRefMsgPayload.h +++ b/src/architecture/msgPayloadDefC/TransRefMsgPayload.h @@ -20,14 +20,12 @@ #ifndef TRANS_REF_MESSAGE_H #define TRANS_REF_MESSAGE_H - - /*! @brief Structure used to define the output definition for attitude reference generation */ -typedef struct { - double r_RN_N[3]; //!< [m] spacecraft reference position vector in inertial frame components - double v_RN_N[3]; //!< [m/s] spacecraft reference inertial velocity vector in N frame components - double a_RN_N[3]; //!< [m/s^2] spacecraft reference acceleration vector in N frame components -}TransRefMsgPayload; - +typedef struct +{ + double r_RN_N[3]; //!< [m] spacecraft reference position vector in inertial frame components + double v_RN_N[3]; //!< [m/s] spacecraft reference inertial velocity vector in N frame components + double a_RN_N[3]; //!< [m/s^2] spacecraft reference acceleration vector in N frame components +} TransRefMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/TypesTestMsgPayload.h b/src/architecture/msgPayloadDefC/TypesTestMsgPayload.h index 7897362db3..e2dfac7dc4 100644 --- a/src/architecture/msgPayloadDefC/TypesTestMsgPayload.h +++ b/src/architecture/msgPayloadDefC/TypesTestMsgPayload.h @@ -24,58 +24,60 @@ #define TYPES_TEST_ARRAY_SIZE 3 -/*! @brief Structure used to test the handling of various built-in types in message payloads across SWIG C/Python bridge */ -typedef struct { +/*! @brief Structure used to test the handling of various built-in types in message payloads across SWIG C/Python bridge + */ +typedef struct +{ // Scalars - int8_t i8Test; //!< Test variable of int8_t - uint8_t ui8Test; //!< Test variable of uint8_t - int16_t i16Test; //!< Test variable of int16_t - uint16_t ui16Test; //!< Test variable of uint16_t - int32_t i32Test; //!< Test variable of int32_t - uint32_t ui32Test; //!< Test variable of uint32_t - int64_t i64Test; //!< Test variable of int64_t - uint64_t ui64Test; //!< Test variable of uint64_t - float f32Test; //!< Test variable of float - double f64Test; //!< Test variable of double - uint8_t uint8Test; //!< Test variable of uint8_t (alias) + int8_t i8Test; //!< Test variable of int8_t + uint8_t ui8Test; //!< Test variable of uint8_t + int16_t i16Test; //!< Test variable of int16_t + uint16_t ui16Test; //!< Test variable of uint16_t + int32_t i32Test; //!< Test variable of int32_t + uint32_t ui32Test; //!< Test variable of uint32_t + int64_t i64Test; //!< Test variable of int64_t + uint64_t ui64Test; //!< Test variable of uint64_t + float f32Test; //!< Test variable of float + double f64Test; //!< Test variable of double + uint8_t uint8Test; //!< Test variable of uint8_t (alias) unsigned char ucharTest; //!< Test variable of unsigned char - int8_t int8Test; //!< Test variable of int8_t (alias) + int8_t int8Test; //!< Test variable of int8_t (alias) signed char scharTest; //!< Test variable of signed char - bool boolTest; //!< Test variable of bool + bool boolTest; //!< Test variable of bool // 1D Arrays, integer - int16_t i16TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of int16_t - uint16_t ui16TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of uint16_t - int32_t i32TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of int32_t - uint32_t ui32TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of uint32_t - int64_t i64TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of int64_t - uint64_t ui64TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of uint64_t - uint8_t uint8TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Array test of uint8_t + int16_t i16TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of int16_t + uint16_t ui16TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of uint16_t + int32_t i32TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of int32_t + uint32_t ui32TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of uint32_t + int64_t i64TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of int64_t + uint64_t ui64TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of uint64_t + uint8_t uint8TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Array test of uint8_t unsigned char ucharTestArray[TYPES_TEST_ARRAY_SIZE]; //!< Array test of unsigned char - int8_t int8TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Array test of int8_t + int8_t int8TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Array test of int8_t signed char scharTestArray[TYPES_TEST_ARRAY_SIZE]; //!< Array test of signed char - bool boolTestArray[TYPES_TEST_ARRAY_SIZE]; //!< Array test of bool + bool boolTestArray[TYPES_TEST_ARRAY_SIZE]; //!< Array test of bool // 2D Arrays, integer - int16_t i16TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of int16_t - uint16_t ui16TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of uint16_t - int32_t i32TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of int32_t - uint32_t ui32TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of uint32_t - int64_t i64TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of int64_t - uint64_t ui64TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of uint64_t - uint8_t uint8TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< 2D Array test of uint8_t + int16_t i16TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of int16_t + uint16_t ui16TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of uint16_t + int32_t i32TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of int32_t + uint32_t ui32TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of uint32_t + int64_t i64TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of int64_t + uint64_t ui64TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of uint64_t + uint8_t uint8TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< 2D Array test of uint8_t unsigned char ucharTestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< 2D Array test of unsigned char - int8_t int8TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< 2D Array test of int8_t + int8_t int8TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< 2D Array test of int8_t signed char scharTestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< 2D Array test of signed char - bool boolTestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< 2D Array test of bool + bool boolTestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< 2D Array test of bool // 1D Arrays, floating point - float f32TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of float - double f64TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of double + float f32TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of float + double f64TestArray[TYPES_TEST_ARRAY_SIZE]; //!< Test variable of array of double // 2D Arrays, floating point - float f32TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of float - double f64TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of double + float f32TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of float + double f64TestArray2[TYPES_TEST_ARRAY_SIZE][TYPES_TEST_ARRAY_SIZE]; //!< Test variable of 2D array of double } TypesTestMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/VSCMGArrayConfigMsgPayload.h b/src/architecture/msgPayloadDefC/VSCMGArrayConfigMsgPayload.h index a3940a04c2..530e1218fd 100644 --- a/src/architecture/msgPayloadDefC/VSCMGArrayConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/VSCMGArrayConfigMsgPayload.h @@ -24,19 +24,19 @@ #include "architecture/utilities/macroDefinitions.h" /*! @brief VSCMG array configuration FSW msg */ -typedef struct{ - double Gs0Matrix_B[3*MAX_EFF_CNT]; //!< [-] The initial VSCMGs first axis matrix in body frame components - double Gt0Matrix_B[3*MAX_EFF_CNT]; //!< [-] The initial VSCMGs second axis matrix in body frame components - double GgMatrix_B[3*MAX_EFF_CNT]; //!< [-] The initial VSCMGs third axis matrix in body frame components - double JsList[MAX_EFF_CNT]; //!< [kgm2] The first axis inertia for VSCMGs - double JtList[MAX_EFF_CNT]; //!< [kgm2] The second axis inertia for VSCMGs - double JgList[MAX_EFF_CNT]; //!< [kgm2] The third axis inertia for VSCMGs - double IwsList[MAX_EFF_CNT]; //!< [kgm2] The wheel spin axis inertia for VSCMGs - double Omega0List[MAX_EFF_CNT]; //!< [rad/s] The initial wheel speeds for VSCMGs - double gamma0List[MAX_EFF_CNT]; //!< [rad] The initial gimbal angles for VSCMGs - double gammaDot0List[MAX_EFF_CNT]; //!< [rad/s] The initial gimbal rates for VSCMGs - int numVSCMG; //!< [-] The number of VSCMGs available on vehicle -}VSCMGArrayConfigMsgPayload; - +typedef struct +{ + double Gs0Matrix_B[3 * MAX_EFF_CNT]; //!< [-] The initial VSCMGs first axis matrix in body frame components + double Gt0Matrix_B[3 * MAX_EFF_CNT]; //!< [-] The initial VSCMGs second axis matrix in body frame components + double GgMatrix_B[3 * MAX_EFF_CNT]; //!< [-] The initial VSCMGs third axis matrix in body frame components + double JsList[MAX_EFF_CNT]; //!< [kgm2] The first axis inertia for VSCMGs + double JtList[MAX_EFF_CNT]; //!< [kgm2] The second axis inertia for VSCMGs + double JgList[MAX_EFF_CNT]; //!< [kgm2] The third axis inertia for VSCMGs + double IwsList[MAX_EFF_CNT]; //!< [kgm2] The wheel spin axis inertia for VSCMGs + double Omega0List[MAX_EFF_CNT]; //!< [rad/s] The initial wheel speeds for VSCMGs + double gamma0List[MAX_EFF_CNT]; //!< [rad] The initial gimbal angles for VSCMGs + double gammaDot0List[MAX_EFF_CNT]; //!< [rad/s] The initial gimbal rates for VSCMGs + int numVSCMG; //!< [-] The number of VSCMGs available on vehicle +} VSCMGArrayConfigMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/VSCMGArrayTorqueMsgPayload.h b/src/architecture/msgPayloadDefC/VSCMGArrayTorqueMsgPayload.h old mode 100755 new mode 100644 index 64f0f3e7ad..e57cd6722d --- a/src/architecture/msgPayloadDefC/VSCMGArrayTorqueMsgPayload.h +++ b/src/architecture/msgPayloadDefC/VSCMGArrayTorqueMsgPayload.h @@ -22,15 +22,11 @@ #include "architecture/utilities/macroDefinitions.h" - /*! @brief Structure used to define the output definition for vehicle effectors*/ -typedef struct { - double wheelTorque[MAX_EFF_CNT]; //!< [N-m] VSCMG wheel torque array - double gimbalTorque[MAX_EFF_CNT]; //!< [N-m] VSCMG gimbal torque array -}VSCMGArrayTorqueMsgPayload; - - - - +typedef struct +{ + double wheelTorque[MAX_EFF_CNT]; //!< [N-m] VSCMG wheel torque array + double gimbalTorque[MAX_EFF_CNT]; //!< [N-m] VSCMG gimbal torque array +} VSCMGArrayTorqueMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/VSCMGCmdMsgPayload.h b/src/architecture/msgPayloadDefC/VSCMGCmdMsgPayload.h old mode 100755 new mode 100644 index a38987e4d9..14406bec39 --- a/src/architecture/msgPayloadDefC/VSCMGCmdMsgPayload.h +++ b/src/architecture/msgPayloadDefC/VSCMGCmdMsgPayload.h @@ -20,14 +20,11 @@ #ifndef SIM_VSCMG_CMD_H #define SIM_VSCMG_CMD_H - - /*! @brief Structure used to define the individual RW motor torque command message*/ -typedef struct { +typedef struct +{ double u_s_cmd; //!< [Nm] torque command for individual wheel - double u_g_cmd; //!< [Nm] torque command for individual gimbal -}VSCMGCmdMsgPayload; - - + double u_g_cmd; //!< [Nm] torque command for individual gimbal +} VSCMGCmdMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/VSCMGConfigElementMsgPayload.h b/src/architecture/msgPayloadDefC/VSCMGConfigElementMsgPayload.h index 86da2d6215..31438d1dd5 100644 --- a/src/architecture/msgPayloadDefC/VSCMGConfigElementMsgPayload.h +++ b/src/architecture/msgPayloadDefC/VSCMGConfigElementMsgPayload.h @@ -24,18 +24,18 @@ #include "architecture/utilities/macroDefinitions.h" /*! @brief VSCMG array configuration FSW msg */ -typedef struct{ - double gsHat0_B[3]; //!< [-] The initial VSCMG first axis matrix in body frame components - double gtHat0_B[3]; //!< [-] The initial VSCMG second axis matrix in body frame components - double ggHat_B[3]; //!< [-] The VSCMG third axis matrix in body frame components - double Js; //!< [kgm2] The first axis inertia for VSCMG - double Jt; //!< [kgm2] The second axis inertia for VSCMG - double Jg; //!< [kgm2] The third axis inertia for VSCMG - double Iws; //!< [kgm2] The wheel spin axis inertia for VSCMG - double Omega0; //!< [rad/s] The initial wheel speed for VSCMG - double gamma0; //!< [rad] The initial gimbal angle for VSCMG - double gammaDot0; //!< [rad/s] The initial gimbal rate for VSCMG -}VSCMGConfigElementMsgPayload; - +typedef struct +{ + double gsHat0_B[3]; //!< [-] The initial VSCMG first axis matrix in body frame components + double gtHat0_B[3]; //!< [-] The initial VSCMG second axis matrix in body frame components + double ggHat_B[3]; //!< [-] The VSCMG third axis matrix in body frame components + double Js; //!< [kgm2] The first axis inertia for VSCMG + double Jt; //!< [kgm2] The second axis inertia for VSCMG + double Jg; //!< [kgm2] The third axis inertia for VSCMG + double Iws; //!< [kgm2] The wheel spin axis inertia for VSCMG + double Omega0; //!< [rad/s] The initial wheel speed for VSCMG + double gamma0; //!< [rad] The initial gimbal angle for VSCMG + double gammaDot0; //!< [rad/s] The initial gimbal rate for VSCMG +} VSCMGConfigElementMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/VSCMGRefStatesMsgPayload.h b/src/architecture/msgPayloadDefC/VSCMGRefStatesMsgPayload.h index 450cda536f..5c537ebbfe 100644 --- a/src/architecture/msgPayloadDefC/VSCMGRefStatesMsgPayload.h +++ b/src/architecture/msgPayloadDefC/VSCMGRefStatesMsgPayload.h @@ -23,7 +23,8 @@ #include "architecture/utilities/macroDefinitions.h" /*! @brief Structure used to define the desired VSCMG RW accelerations and gimbal rates */ -typedef struct { +typedef struct +{ double wheelAccels[MAX_EFF_CNT]; //!< [rad/s^2] The desired wheel accelerations for the VSCMGs double gimbalRates[MAX_EFF_CNT]; //!< [rad/s] The desired gimbal rates for the VSCMGs } VSCMGRefStatesMsgPayload; diff --git a/src/architecture/msgPayloadDefC/VSCMGSpeedMsgPayload.h b/src/architecture/msgPayloadDefC/VSCMGSpeedMsgPayload.h old mode 100755 new mode 100644 index f1b7d8770b..9e539ae8c2 --- a/src/architecture/msgPayloadDefC/VSCMGSpeedMsgPayload.h +++ b/src/architecture/msgPayloadDefC/VSCMGSpeedMsgPayload.h @@ -22,14 +22,12 @@ #include "architecture/utilities/macroDefinitions.h" - /*! @brief Structure used to define the output definition for VSCMG speeds*/ -typedef struct { - double wheelSpeeds[MAX_EFF_CNT]; //!< r/s The current angular velocities of the VSCMG wheel - double gimbalAngles[MAX_EFF_CNT]; //!< r The current angles of the VSCMG gimbal - double gimbalRates[MAX_EFF_CNT]; //!< r/s The current angular velocities of the VSCMG gimbal -}VSCMGSpeedMsgPayload; - - +typedef struct +{ + double wheelSpeeds[MAX_EFF_CNT]; //!< r/s The current angular velocities of the VSCMG wheel + double gimbalAngles[MAX_EFF_CNT]; //!< r The current angles of the VSCMG gimbal + double gimbalRates[MAX_EFF_CNT]; //!< r/s The current angular velocities of the VSCMG gimbal +} VSCMGSpeedMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/VehicleConfigMsgPayload.h b/src/architecture/msgPayloadDefC/VehicleConfigMsgPayload.h old mode 100755 new mode 100644 index 6f1f8106af..7d990976ad --- a/src/architecture/msgPayloadDefC/VehicleConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefC/VehicleConfigMsgPayload.h @@ -22,14 +22,13 @@ #include - /*! @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 CoM_B[3]; //!< [m] Center of mass of spacecraft in body - double massSC; //!< [kg] Spacecraft mass - uint32_t CurrentADCSState; //!< [-] Current ADCS state for subsystem -}VehicleConfigMsgPayload; - +typedef struct +{ + 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 +} VehicleConfigMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefC/VoltMsgPayload.h b/src/architecture/msgPayloadDefC/VoltMsgPayload.h old mode 100755 new mode 100644 index 7c001fb7bd..e0582236ea --- a/src/architecture/msgPayloadDefC/VoltMsgPayload.h +++ b/src/architecture/msgPayloadDefC/VoltMsgPayload.h @@ -21,9 +21,9 @@ #define VOLT_MESSAGE_H /*! @brief Structure used to define the output definition for attitude guidance*/ -typedef struct { - double voltage; //!< [V] space object potential -}VoltMsgPayload; - +typedef struct +{ + double voltage; //!< [V] space object potential +} VoltMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefCpp/CSSConfigLogMsgPayload.h b/src/architecture/msgPayloadDefCpp/CSSConfigLogMsgPayload.h index e645898ced..fd456792ab 100644 --- a/src/architecture/msgPayloadDefCpp/CSSConfigLogMsgPayload.h +++ b/src/architecture/msgPayloadDefCpp/CSSConfigLogMsgPayload.h @@ -20,24 +20,22 @@ #ifndef CSSCONFIGLOGSIMMSG_H #define CSSCONFIGLOGSIMMSG_H - //!@brief CSS configuration message log message /*! This message is the outpout of each CSS device to log all the configuration and measurement states. */ typedef struct -//@cond DOXYGEN_IGNORE -CSSConfigLogMsgPayload + //@cond DOXYGEN_IGNORE + CSSConfigLogMsgPayload //@endcond { - double r_B[3] = {0}; //!< [m] sensor position vector in the spacecraft, "B", body frame - double nHat_B[3]; //!< [] sensor unit direction vector in the spacecraft, "B", body frame - double fov; //!< [rad] field of view (boresight to edge) - double signal; //!< [] current sensor signal - double maxSignal; //!< [] maximum sensor signal, -1 means this value is not set - double minSignal; //!< [] maximum sensor signal, -1 means this value is not set - int CSSGroupID = 0; //!< [] Group ID if the CSS is part of a cluster -}CSSConfigLogMsgPayload; - + double r_B[3] = { 0 }; //!< [m] sensor position vector in the spacecraft, "B", body frame + double nHat_B[3]; //!< [] sensor unit direction vector in the spacecraft, "B", body frame + double fov; //!< [rad] field of view (boresight to edge) + double signal; //!< [] current sensor signal + double maxSignal; //!< [] maximum sensor signal, -1 means this value is not set + double minSignal; //!< [] maximum sensor signal, -1 means this value is not set + int CSSGroupID = 0; //!< [] Group ID if the CSS is part of a cluster +} CSSConfigLogMsgPayload; #endif /* CSSCONFIGLOGSIMMSG_H */ diff --git a/src/architecture/msgPayloadDefCpp/ChargeMsmMsgPayload.h b/src/architecture/msgPayloadDefCpp/ChargeMsmMsgPayload.h old mode 100755 new mode 100644 index fecd73fd39..fbc0d6aba8 --- a/src/architecture/msgPayloadDefCpp/ChargeMsmMsgPayload.h +++ b/src/architecture/msgPayloadDefCpp/ChargeMsmMsgPayload.h @@ -22,16 +22,13 @@ #include - /*! @brief Structure used to define the MSM spacecraft sphere charge value message */ typedef struct -//@cond DOXYGEN_IGNORE -ChargeMsmMsgPayload + //@cond DOXYGEN_IGNORE + ChargeMsmMsgPayload //@endcond { - Eigen::VectorXd q; //!< [C], charge of each MSM sphere -}ChargeMsmMsgPayload; - - + Eigen::VectorXd q; //!< [C], charge of each MSM sphere +} ChargeMsmMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefCpp/DataStorageStatusMsgPayload.h b/src/architecture/msgPayloadDefCpp/DataStorageStatusMsgPayload.h index 20b69848e2..aa6fee1d2c 100644 --- a/src/architecture/msgPayloadDefCpp/DataStorageStatusMsgPayload.h +++ b/src/architecture/msgPayloadDefCpp/DataStorageStatusMsgPayload.h @@ -22,18 +22,17 @@ #ifndef BASILISK_DATASTORAGESTATUSSIMMSG_H #define BASILISK_DATASTORAGESTATUSSIMMSG_H - /*! @brief Message to store current storage unit stored data, storage capacity, and received data.*/ typedef struct -//@cond DOXYGEN_IGNORE -DataStorageStatusMsgPayload + //@cond DOXYGEN_IGNORE + DataStorageStatusMsgPayload //@endcond { - double storageLevel; //!< [b] Storage unit stored data in bits. + double storageLevel; //!< [b] Storage unit stored data in bits. double storageCapacity; //!< [b] Maximum data storage unit capacity. - double currentNetBaud; //!< [baud] Current data written to or removed from the storage unit net power. - std::vector storedDataName; //!< [] vector of data name strings - std::vector storedData; //!< [] vector of stored data amount for each data name group -}DataStorageStatusMsgPayload; + double currentNetBaud; //!< [baud] Current data written to or removed from the storage unit net power. + std::vector storedDataName; //!< [] vector of data name strings + std::vector storedData; //!< [] vector of stored data amount for each data name group +} DataStorageStatusMsgPayload; -#endif //BASILISK_DATASTORAGESTATUSSIMMSG_H +#endif // BASILISK_DATASTORAGESTATUSSIMMSG_H diff --git a/src/architecture/msgPayloadDefCpp/MJSceneStateMsgPayload.h b/src/architecture/msgPayloadDefCpp/MJSceneStateMsgPayload.h index 7d889270f6..ea9316170b 100644 --- a/src/architecture/msgPayloadDefCpp/MJSceneStateMsgPayload.h +++ b/src/architecture/msgPayloadDefCpp/MJSceneStateMsgPayload.h @@ -24,8 +24,8 @@ /** Use to represent the state of an MJScene in generalized coordinates */ typedef struct -//@cond DOXYGEN_IGNORE -MJSceneStateMsgPayload + //@cond DOXYGEN_IGNORE + MJSceneStateMsgPayload //@endcond { Eigen::VectorXd qpos; //!< [-] Position of the scene in generalized coordinates diff --git a/src/architecture/msgPayloadDefCpp/THROutputMsgPayload.h b/src/architecture/msgPayloadDefCpp/THROutputMsgPayload.h old mode 100755 new mode 100644 index 3947921371..7b8caf3612 --- a/src/architecture/msgPayloadDefCpp/THROutputMsgPayload.h +++ b/src/architecture/msgPayloadDefCpp/THROutputMsgPayload.h @@ -20,25 +20,22 @@ #ifndef SIM_THRUSTER_OUTPUT_MSG_H #define SIM_THRUSTER_OUTPUT_MSG_H - - /*! This structure is used in the messaging system to communicate what the state of the vehicle is currently.*/ typedef struct -//@cond DOXYGEN_IGNORE -THROutputMsgPayload + //@cond DOXYGEN_IGNORE + THROutputMsgPayload //@endcond { - double maxThrust; //!< [N] Steady state thrust of thruster - double thrustFactor; //!< Current thrust percentage due to ramp up/down - double thrustBlowDownFactor; //!< Current thrust percentage due to tank blow down - double ispBlowDownFactor; //!< Current isp percentage due to tank blow down - double thrustForce = 0; //!< [N] Thrust force magnitude - double thrustForce_B[3] = {0}; //!< [N] Thrust force vector in body frame components - double thrustTorquePntB_B[3] = {0}; //!< [N-m] Thrust torque about point B in body frame components - double thrusterLocation[3] = {0}; //!< [m] Current position vector (inertial) - double thrusterDirection[3] = {0}; //!< Unit vector of thruster pointing -}THROutputMsgPayload; - + double maxThrust; //!< [N] Steady state thrust of thruster + double thrustFactor; //!< Current thrust percentage due to ramp up/down + double thrustBlowDownFactor; //!< Current thrust percentage due to tank blow down + double ispBlowDownFactor; //!< Current isp percentage due to tank blow down + double thrustForce = 0; //!< [N] Thrust force magnitude + double thrustForce_B[3] = { 0 }; //!< [N] Thrust force vector in body frame components + double thrustTorquePntB_B[3] = { 0 }; //!< [N-m] Thrust torque about point B in body frame components + double thrusterLocation[3] = { 0 }; //!< [m] Current position vector (inertial) + double thrusterDirection[3] = { 0 }; //!< Unit vector of thruster pointing +} THROutputMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h b/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h old mode 100755 new mode 100644 index 6907747f3f..e4bee9a03c --- a/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h @@ -23,103 +23,113 @@ #include #include - /*! @brief enumeration definiting the types of VSCMG modes */ -enum VSCMGModels { vscmgBalancedWheels, vscmgJitterSimple, vscmgJitterFullyCoupled }; - +enum VSCMGModels +{ + vscmgBalancedWheels, + vscmgJitterSimple, + vscmgJitterFullyCoupled +}; /*! @brief Structure used to define the individual VSCMG configuration data message*/ typedef struct -//@cond DOXYGEN_IGNORE -VSCMGConfigMsgPayload + //@cond DOXYGEN_IGNORE + VSCMGConfigMsgPayload //@endcond { - VSCMGModels VSCMGModel; //!< [-], type of imbalance model to use - Eigen::Vector3d rGB_B; //!< [m], vector pointing from body frame B origin to VSCMG frame G origin in B frame components - Eigen::Vector3d gsHat0_B; //!< [-] first axis of the G frame in B frame components (G frame for gamma=0)] - Eigen::Vector3d gsHat_B; //!< [-] first axis of the G frame in B frame components - Eigen::Vector3d gtHat0_B; //!< [-] second axis of the G frame in B frame components (G frame for gamma=0)] - Eigen::Vector3d gtHat_B; //!< [-] second axis of the G frame in B frame components - Eigen::Vector3d ggHat_B; //!< [-] third axis of the G frame in B frame components - Eigen::Vector3d w2Hat0_B; //!< [-] second axis of the W frame in B frame components (W frame for theta=0) - Eigen::Vector3d w2Hat_B; //!< [-] second axis of the W frame in B frame components - Eigen::Vector3d w3Hat0_B; //!< [-] third axis of the W frame in B frame components (W frame for theta=0) - Eigen::Vector3d w3Hat_B; //!< [-] third axis of the W frame in B frame components - double massV; //!< [kg] mass of the VSCMG system - double massG; //!< [kg] mass of the gimbal - double massW; //!< [kg] mass of the wheel - double theta; //!< [rad] wheel angle - double Omega; //!< [rad/s] wheel speed - double gamma; //!< [rad] gimbal angle - double gammaDot; //!< [rad/s] gimbal rate - double IW1; //!< [kg-m^2] inertia of the wheel about the wheel spin axis in W frame components - double IW2; //!< [kg-m^2] inertia of the wheel about the wheel second axis in W frame components - double IW3; //!< [kg-m^2] inertia of the wheel about the wheel third axis in W frame components - double IW13; //!< [kg-m^2] inertia coupling between wheel first and third axes in W frame components - double IG1; //!< [kg-m^2] inertia of the gimbal about the gimbal first axis in G frame components - double IG2; //!< [kg-m^2] inertia of the gimbal about the gimbal second axis in G frame components - double IG3; //!< [kg-m^2] inertia of the gimbal about the gimbal third axis in G frame components - double IG12; //!< [kg-m^2] inertia coupling between gimbal first and second axes in G frame components - double IG13; //!< [kg-m^2] inertia coupling between gimbal first and third axes in G frame components - double IG23; //!< [kg-m^2] inertia coupling between gimbal second and third axes in G frame components - double IV1; //!< [kg-m^2] inertia of the VSCMG about the VSCMG first axis in G frame components - double IV2; //!< [kg-m^2] inertia of the VSCMG about the VSCMG second axis in G frame components - double IV3; //!< [kg-m^2] inertia of the VSCMG about the VSCMG third axis in G frame components - double rhoG; //!< [-] ratio of the gimbal mass to the VSCMG mass - double rhoW; //!< [-] ratio of the wheel mass to the VSCMG mass - double U_s; //!< [kg-m] static imbalance - double U_d; //!< [kg-m^2] dynamic imbalance - double d; //!< [m] offset of point Wc (center of mass of the wheel) from wheel frame W origin - double l; //!< [m] offset of wheel third axis from gimbal third axis - double L; //!< [m] offset of the wheel first axis from the gimbal first axis - double u_s_current; //!< [N-m] current wheel motor torque - double u_s_max = -1.0; //!< [N-m] Max wheel torque - double u_s_min; //!< [N-m] Min wheel torque - double u_s_f; //!< [N-m] Coulomb friction wheel torque magnitude - double Omega_max = -1.0; //!< [rad/s] max wheel speed - double wheelLinearFrictionRatio;//!< [%] ratio relative to max wheel speed value up to which the friction behaves linearly - double u_g_current; //!< [N-m] current gimbal motor torque - double u_g_max = -1.0; //!< [N-m] Max gimbal torque - double u_g_min; //!< [N-m] Min gimbal torque - double u_g_f; //!< [N-m] Coulomb friction gimbal torque magnitude - double gammaDot_max; //!< [rad/s] max gimbal rate - double gimbalLinearFrictionRatio;//!< [%] ratio relative to max gimbal rate value up to which the friction behaves linearly - - Eigen::Matrix3d IGPntGc_B; //!< [kg-m^2] inertia of the gimbal about point Gc (center of mass of the gimbal) in B frame components - Eigen::Matrix3d IWPntWc_B; //!< [kg-m^2] inertia of the wheel about point Wc (center of mass of the wheel) in B frame components - Eigen::Matrix3d IPrimeGPntGc_B; //!< [kg-m^2] body frame time derivative of IGPntGc_B - Eigen::Matrix3d IPrimeWPntWc_B; //!< [kg-m^2] body frame time derivative of IWPntWc_B - Eigen::Vector3d rGcG_G; //!< [m] vector pointing from VSCMG frame G origin to point Gc (center of mass of the gimbal) in G frame components - Eigen::Vector3d rGcG_B; //!< [m] vector pointing from VSCMG frame G origin to point Gc (center of mass of the gimbal) in B frame components - Eigen::Vector3d rGcB_B; //!< [m] vector pointing from body frame B origin to point Gc (center of mass of the gimbal) in B frame components - Eigen::Vector3d rWcB_B; //!< [m] vector pointing from body frame B origin to point Wc (center of mass of the wheel) in B frame components - Eigen::Vector3d rWcG_B; //!< [m] vector pointing from VSCMG frame G origin to point Wc (center of mass of the wheel) in B frame components - Eigen::Matrix3d rTildeGcB_B; //!< [m] tilde matrix of rGcB_B - Eigen::Matrix3d rTildeWcB_B; //!< [m] tilde matrix of rWcB_B - Eigen::Vector3d rPrimeGcB_B; //!< [m/s] body frame time derivative of rGcB_B - Eigen::Vector3d rPrimeWcB_B; //!< [m/s] body frame time derivative of rWcB_B - Eigen::Matrix3d rPrimeTildeGcB_B; //!< [m/s] tilde matrix of body frame time derivative of rGcB_B - Eigen::Matrix3d rPrimeTildeWcB_B; //!< [m/s] tilde matrix of body frame time derivative of rWcB_B - - Eigen::Vector3d aOmega; //!< [1/m] parameter used in coupled jitter back substitution - Eigen::Vector3d bOmega; //!< [-] parameter used in coupled jitter back substitution - double cOmega; //!< [-] parameter used in coupled jitter back substitution - double dOmega; //!< [rad/s^2] parameter used in coupled jitter back substitution - double eOmega; //!< [kg-m^2] parameter used in coupled jitter back substitution - Eigen::Vector3d agamma; //!< [1/m] parameter used in coupled jitter back substitution - Eigen::Vector3d bgamma; //!< [-] parameter used in coupled jitter back substitution - double cgamma; //!< [-] parameter used in coupled jitter back substitution - double dgamma; //!< [rad/s^2] parameter used in coupled jitter back substitution - double egamma; //!< [kg-m^2] parameter used in coupled jitter back substitution - Eigen::Vector3d p; //!< [1/m] parameter used in coupled jitter back substitution - Eigen::Vector3d q; //!< [-] parameter used in coupled jitter back substitution - double s; //!< [rad/s^2] parameter used in coupled jitter back substitution - - double gravityTorqueWheel_s; //!< [N-m] gravitational torque on the wheel about wheel spin axis - double gravityTorqueGimbal_g; //!< [N-m] gravitational torque on the gimbal about gimbal third axis -}VSCMGConfigMsgPayload; - - - + VSCMGModels VSCMGModel; //!< [-], type of imbalance model to use + Eigen::Vector3d + rGB_B; //!< [m], vector pointing from body frame B origin to VSCMG frame G origin in B frame components + Eigen::Vector3d gsHat0_B; //!< [-] first axis of the G frame in B frame components (G frame for gamma=0)] + Eigen::Vector3d gsHat_B; //!< [-] first axis of the G frame in B frame components + Eigen::Vector3d gtHat0_B; //!< [-] second axis of the G frame in B frame components (G frame for gamma=0)] + Eigen::Vector3d gtHat_B; //!< [-] second axis of the G frame in B frame components + Eigen::Vector3d ggHat_B; //!< [-] third axis of the G frame in B frame components + Eigen::Vector3d w2Hat0_B; //!< [-] second axis of the W frame in B frame components (W frame for theta=0) + Eigen::Vector3d w2Hat_B; //!< [-] second axis of the W frame in B frame components + Eigen::Vector3d w3Hat0_B; //!< [-] third axis of the W frame in B frame components (W frame for theta=0) + Eigen::Vector3d w3Hat_B; //!< [-] third axis of the W frame in B frame components + double massV; //!< [kg] mass of the VSCMG system + double massG; //!< [kg] mass of the gimbal + double massW; //!< [kg] mass of the wheel + double theta; //!< [rad] wheel angle + double Omega; //!< [rad/s] wheel speed + double gamma; //!< [rad] gimbal angle + double gammaDot; //!< [rad/s] gimbal rate + double IW1; //!< [kg-m^2] inertia of the wheel about the wheel spin axis in W frame components + double IW2; //!< [kg-m^2] inertia of the wheel about the wheel second axis in W frame components + double IW3; //!< [kg-m^2] inertia of the wheel about the wheel third axis in W frame components + double IW13; //!< [kg-m^2] inertia coupling between wheel first and third axes in W frame components + double IG1; //!< [kg-m^2] inertia of the gimbal about the gimbal first axis in G frame components + double IG2; //!< [kg-m^2] inertia of the gimbal about the gimbal second axis in G frame components + double IG3; //!< [kg-m^2] inertia of the gimbal about the gimbal third axis in G frame components + double IG12; //!< [kg-m^2] inertia coupling between gimbal first and second axes in G frame components + double IG13; //!< [kg-m^2] inertia coupling between gimbal first and third axes in G frame components + double IG23; //!< [kg-m^2] inertia coupling between gimbal second and third axes in G frame components + double IV1; //!< [kg-m^2] inertia of the VSCMG about the VSCMG first axis in G frame components + double IV2; //!< [kg-m^2] inertia of the VSCMG about the VSCMG second axis in G frame components + double IV3; //!< [kg-m^2] inertia of the VSCMG about the VSCMG third axis in G frame components + double rhoG; //!< [-] ratio of the gimbal mass to the VSCMG mass + double rhoW; //!< [-] ratio of the wheel mass to the VSCMG mass + double U_s; //!< [kg-m] static imbalance + double U_d; //!< [kg-m^2] dynamic imbalance + double d; //!< [m] offset of point Wc (center of mass of the wheel) from wheel frame W origin + double l; //!< [m] offset of wheel third axis from gimbal third axis + double L; //!< [m] offset of the wheel first axis from the gimbal first axis + double u_s_current; //!< [N-m] current wheel motor torque + double u_s_max = -1.0; //!< [N-m] Max wheel torque + double u_s_min; //!< [N-m] Min wheel torque + double u_s_f; //!< [N-m] Coulomb friction wheel torque magnitude + double Omega_max = -1.0; //!< [rad/s] max wheel speed + double wheelLinearFrictionRatio; //!< [%] ratio relative to max wheel speed value up to which the friction behaves + //!< linearly + double u_g_current; //!< [N-m] current gimbal motor torque + double u_g_max = -1.0; //!< [N-m] Max gimbal torque + double u_g_min; //!< [N-m] Min gimbal torque + double u_g_f; //!< [N-m] Coulomb friction gimbal torque magnitude + double gammaDot_max; //!< [rad/s] max gimbal rate + double gimbalLinearFrictionRatio; //!< [%] ratio relative to max gimbal rate value up to which the friction behaves + //!< linearly + + Eigen::Matrix3d + IGPntGc_B; //!< [kg-m^2] inertia of the gimbal about point Gc (center of mass of the gimbal) in B frame components + Eigen::Matrix3d + IWPntWc_B; //!< [kg-m^2] inertia of the wheel about point Wc (center of mass of the wheel) in B frame components + Eigen::Matrix3d IPrimeGPntGc_B; //!< [kg-m^2] body frame time derivative of IGPntGc_B + Eigen::Matrix3d IPrimeWPntWc_B; //!< [kg-m^2] body frame time derivative of IWPntWc_B + Eigen::Vector3d rGcG_G; //!< [m] vector pointing from VSCMG frame G origin to point Gc (center of mass of the + //!< gimbal) in G frame components + Eigen::Vector3d rGcG_B; //!< [m] vector pointing from VSCMG frame G origin to point Gc (center of mass of the + //!< gimbal) in B frame components + Eigen::Vector3d rGcB_B; //!< [m] vector pointing from body frame B origin to point Gc (center of mass of the gimbal) + //!< in B frame components + Eigen::Vector3d rWcB_B; //!< [m] vector pointing from body frame B origin to point Wc (center of mass of the wheel) + //!< in B frame components + Eigen::Vector3d rWcG_B; //!< [m] vector pointing from VSCMG frame G origin to point Wc (center of mass of the wheel) + //!< in B frame components + Eigen::Matrix3d rTildeGcB_B; //!< [m] tilde matrix of rGcB_B + Eigen::Matrix3d rTildeWcB_B; //!< [m] tilde matrix of rWcB_B + Eigen::Vector3d rPrimeGcB_B; //!< [m/s] body frame time derivative of rGcB_B + Eigen::Vector3d rPrimeWcB_B; //!< [m/s] body frame time derivative of rWcB_B + Eigen::Matrix3d rPrimeTildeGcB_B; //!< [m/s] tilde matrix of body frame time derivative of rGcB_B + Eigen::Matrix3d rPrimeTildeWcB_B; //!< [m/s] tilde matrix of body frame time derivative of rWcB_B + + Eigen::Vector3d aOmega; //!< [1/m] parameter used in coupled jitter back substitution + Eigen::Vector3d bOmega; //!< [-] parameter used in coupled jitter back substitution + double cOmega; //!< [-] parameter used in coupled jitter back substitution + double dOmega; //!< [rad/s^2] parameter used in coupled jitter back substitution + double eOmega; //!< [kg-m^2] parameter used in coupled jitter back substitution + Eigen::Vector3d agamma; //!< [1/m] parameter used in coupled jitter back substitution + Eigen::Vector3d bgamma; //!< [-] parameter used in coupled jitter back substitution + double cgamma; //!< [-] parameter used in coupled jitter back substitution + double dgamma; //!< [rad/s^2] parameter used in coupled jitter back substitution + double egamma; //!< [kg-m^2] parameter used in coupled jitter back substitution + Eigen::Vector3d p; //!< [1/m] parameter used in coupled jitter back substitution + Eigen::Vector3d q; //!< [-] parameter used in coupled jitter back substitution + double s; //!< [rad/s^2] parameter used in coupled jitter back substitution + + double gravityTorqueWheel_s; //!< [N-m] gravitational torque on the wheel about wheel spin axis + double gravityTorqueGimbal_g; //!< [N-m] gravitational torque on the gimbal about gimbal third axis +} VSCMGConfigMsgPayload; #endif diff --git a/src/architecture/msgPayloadDefCpp/VizUserInputMsgPayload.h b/src/architecture/msgPayloadDefCpp/VizUserInputMsgPayload.h index 5a61bf8915..8a2fb2652e 100644 --- a/src/architecture/msgPayloadDefCpp/VizUserInputMsgPayload.h +++ b/src/architecture/msgPayloadDefCpp/VizUserInputMsgPayload.h @@ -25,27 +25,27 @@ /*! @brief Structure used to contain a single VizEventDialog panel response from Vizard */ typedef struct -//@cond DOXYGEN_IGNORE -VizEventReply + //@cond DOXYGEN_IGNORE + VizEventReply //@endcond { - std::string eventHandlerID; //!< Name provided when setting up the VizEventDialog object - std::string reply; //!< Option selection - bool eventHandlerDestroyed; //!< Was the panel closed and destroyed? + std::string eventHandlerID; //!< Name provided when setting up the VizEventDialog object + std::string reply; //!< Option selection + bool eventHandlerDestroyed; //!< Was the panel closed and destroyed? -}VizEventReply; +} VizEventReply; /*! @brief Structure containing all keyboard inputs and EventReply objects recorded since the message was last read */ typedef struct -//@cond DOXYGEN_IGNORE -VizUserInputMsgPayload + //@cond DOXYGEN_IGNORE + VizUserInputMsgPayload //@endcond { - int frameNumber; //!< Vizard frame number - std::string keyboardInput; //!< String containing all keyboard inputs since last update. + int frameNumber; //!< Vizard frame number + std::string keyboardInput; //!< String containing all keyboard inputs since last update. std::vector vizEventReplies; //!< Contains all panel inputs since last update -}VizUserInputMsgPayload; +} VizUserInputMsgPayload; #endif diff --git a/src/architecture/system_model/_UnitTest/test_PySysModel.py b/src/architecture/system_model/_UnitTest/test_PySysModel.py index d4c7362893..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""" @@ -92,7 +98,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 +122,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"): @@ -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() \ No newline at end of file + test_ErrorPySysModel() diff --git a/src/architecture/system_model/sim_model.cpp b/src/architecture/system_model/sim_model.cpp old mode 100755 new mode 100644 index 53e3a1d7b0..da3afafe75 --- a/src/architecture/system_model/sim_model.cpp +++ b/src/architecture/system_model/sim_model.cpp @@ -20,31 +20,28 @@ #include "sim_model.h" #include -void activateNewThread(void *threadData) +void +activateNewThread(void* threadData) { - auto *theThread = static_cast (threadData); + auto* theThread = static_cast(threadData); - //std::cout << "Starting thread yes" << std::endl; + // std::cout << "Starting thread yes" << std::endl; try { theThread->postInit(); - while(theThread->threadValid()) - { + while (theThread->threadValid()) { theThread->lockThread(); - if(theThread->selfInitNow){ + if (theThread->selfInitNow) { theThread->selfInitProcesses(); theThread->selfInitNow = false; - } - else if(theThread->crossInitNow){ + } else if (theThread->crossInitNow) { theThread->crossInitProcesses(); theThread->crossInitNow = false; - } - else if(theThread->resetNow){ + } else if (theThread->resetNow) { theThread->resetProcesses(); theThread->resetNow = false; - } - else{ + } else { theThread->StepUntilStop(); } theThread->unlockParent(); @@ -54,20 +51,21 @@ void activateNewThread(void *threadData) theThread->threadException = std::current_exception(); theThread->unlockParent(); // Make sure to unlock so main thread doesn't hang } - } -SimThreadExecution::SimThreadExecution(uint64_t threadIdent, uint64_t currentSimNanos) : - currentThreadNanos(currentSimNanos), threadID(threadIdent) +SimThreadExecution::SimThreadExecution(uint64_t threadIdent, uint64_t currentSimNanos) + : currentThreadNanos(currentSimNanos) + , threadID(threadIdent) { - } /*! This method provides a synchronization mechanism for the "child" thread ensuring that it can be held at a fixed point after it finishes the execution of a given frame until it is released by the "parent" thread. */ -void SimThreadExecution::lockThread() { +void +SimThreadExecution::lockThread() +{ this->selfThreadLock.acquire(); } @@ -75,7 +73,9 @@ void SimThreadExecution::lockThread() { the parent and all other threads in the system can be forced to wait at a known time until this thread has finished its execution for that time. */ -void SimThreadExecution::lockParent() { +void +SimThreadExecution::lockParent() +{ this->parentThreadLock.acquire(); } @@ -83,7 +83,9 @@ void SimThreadExecution::lockParent() { child thread for a single frame's execution. It is intended to only be called from the parent thread. */ -void SimThreadExecution::unlockThread() { +void +SimThreadExecution::unlockThread() +{ this->selfThreadLock.release(); } @@ -92,7 +94,9 @@ void SimThreadExecution::unlockThread() { parent and all of its other children have to wait for this child to finish its execution. */ -void SimThreadExecution::unlockParent() { +void +SimThreadExecution::unlockParent() +{ this->parentThreadLock.release(); } @@ -101,43 +105,37 @@ void SimThreadExecution::unlockParent() { processes are triggered @param stopPri The priority level below which the sim won't go */ -void SimThreadExecution::SingleStepProcesses(int64_t stopPri) +void +SimThreadExecution::SingleStepProcesses(int64_t stopPri) { - uint64_t nextCallTime = ~((uint64_t) 0); - std::vector::iterator it = this->processList.begin(); + uint64_t nextCallTime = ~((uint64_t)0); + std::vector::iterator it = this->processList.begin(); this->CurrentNanos = this->NextTaskTime; - while(it!= this->processList.end() && this->threadValid()) - { - SysProcess *localProc = (*it); - if(localProc->processEnabled()) - { - while(localProc->nextTaskTime < this->CurrentNanos || - (localProc->nextTaskTime == this->CurrentNanos && - localProc->processPriority >= stopPri)) - { + while (it != this->processList.end() && this->threadValid()) { + SysProcess* localProc = (*it); + if (localProc->processEnabled()) { + while (localProc->nextTaskTime < this->CurrentNanos || + (localProc->nextTaskTime == this->CurrentNanos && localProc->processPriority >= stopPri)) { localProc->singleStepNextTask(this->CurrentNanos); } - if(localProc->getNextTime() < nextCallTime) - { + if (localProc->getNextTime() < nextCallTime) { nextCallTime = localProc->getNextTime(); this->nextProcPriority = localProc->processPriority; - } - else if(localProc->getNextTime() == nextCallTime && - localProc->processPriority > this->nextProcPriority) - { + } else if (localProc->getNextTime() == nextCallTime && + localProc->processPriority > this->nextProcPriority) { this->nextProcPriority = localProc->processPriority; } } it++; } - this->NextTaskTime = nextCallTime != ~((uint64_t) 0) ? nextCallTime : this->CurrentNanos; - + this->NextTaskTime = nextCallTime != ~((uint64_t)0) ? nextCallTime : this->CurrentNanos; } /*! This method steps the simulation until the specified stop time and stop priority have been reached. */ -void SimThreadExecution::StepUntilStop() +void +SimThreadExecution::StepUntilStop() { /*! - Note that we have to step until both the time is greater and the next Task's start time is in the future. If the NextTaskTime is less than @@ -145,9 +143,9 @@ void SimThreadExecution::StepUntilStop() (that's less than all process priorities, so it will run through the next process)*/ int64_t inPri = stopThreadNanos == this->NextTaskTime ? stopThreadPriority : -1; - while(this->threadValid() && (this->NextTaskTime < stopThreadNanos || (this->NextTaskTime == stopThreadNanos && - this->nextProcPriority >= stopThreadPriority)) ) - { + while (this->threadValid() && + (this->NextTaskTime < stopThreadNanos || + (this->NextTaskTime == stopThreadNanos && this->nextProcPriority >= stopThreadPriority))) { this->SingleStepProcesses(inPri); inPri = stopThreadNanos == this->NextTaskTime ? stopThreadPriority : -1; } @@ -157,12 +155,13 @@ void SimThreadExecution::StepUntilStop() sharing process between different threads is handled. TODO: Make this method move messages safely between threads */ -void SimThreadExecution::moveProcessMessages() const { -// for(auto const& process : this->processList) -// { -// process->routeInterfaces(this->CurrentNanos); -// } - +void +SimThreadExecution::moveProcessMessages() const +{ + // for(auto const& process : this->processList) + // { + // process->routeInterfaces(this->CurrentNanos); + // } } /*! Once threads are released for execution, this method ensures that they finish @@ -170,10 +169,11 @@ void SimThreadExecution::moveProcessMessages() const { activities. It's very similar to the locking process, but provides different functionality. */ -void SimThreadExecution::waitOnInit() { +void +SimThreadExecution::waitOnInit() +{ std::unique_lock lck(this->initReadyLock); - while(!this->threadActive()) - { + while (!this->threadActive()) { (this)->initHoldVar.wait(lck); } } @@ -182,7 +182,9 @@ void SimThreadExecution::waitOnInit() { they have cleared their construction phase and are ready to go through initialization. */ -void SimThreadExecution::postInit() { +void +SimThreadExecution::postInit() +{ std::unique_lock lck(this->initReadyLock); this->threadReady(); this->initHoldVar.notify_one(); @@ -192,9 +194,10 @@ void SimThreadExecution::postInit() { and processes and initialize them serially. Note that other threads can also be initializing their systems simultaneously. */ -void SimThreadExecution::selfInitProcesses() const { - for(auto const& process : this->processList) - { +void +SimThreadExecution::selfInitProcesses() const +{ + for (auto const& process : this->processList) { process->selfInitProcess(); } } @@ -202,24 +205,27 @@ void SimThreadExecution::selfInitProcesses() const { /*! This method is vestigial and should probably be removed once MT message movement has been completed. */ -void SimThreadExecution::crossInitProcesses() const { -// for(auto const& process : this->processList) -// { -// process->crossInitProcess(); -// } +void +SimThreadExecution::crossInitProcesses() const +{ + // for(auto const& process : this->processList) + // { + // process->crossInitProcess(); + // } } /*! This method allows the "child" thread to reset both its timing/scheduling, as well as all of its allocated tasks/modules when commanded. This is always called during init, but can be called during runtime as well. */ -void SimThreadExecution::resetProcesses() { +void +SimThreadExecution::resetProcesses() +{ this->currentThreadNanos = 0; this->CurrentNanos = 0; this->NextTaskTime = 0; this->threadException = nullptr; // Clear any stored exception - for(auto const& process : this->processList) - { + for (auto const& process : this->processList) { process->resetProcess(this->currentThreadNanos); } } @@ -228,7 +234,9 @@ void SimThreadExecution::resetProcesses() { thread. It allows the user to put specific processes onto specific threads if that is desired. */ -void SimThreadExecution::addNewProcess(SysProcess* newProc) { +void +SimThreadExecution::addNewProcess(SysProcess* newProc) +{ processList.push_back(newProc); newProc->setProcessControlStatus(true); } @@ -237,8 +245,8 @@ void SimThreadExecution::addNewProcess(SysProcess* newProc) { */ SimModel::SimModel() { - //Default to single-threaded runtime - auto *newThread = new SimThreadExecution(0, 0); + // Default to single-threaded runtime + auto* newThread = new SimThreadExecution(0, 0); this->threadList.push_back(newThread); } @@ -253,42 +261,39 @@ SimModel::~SimModel() @param SimStopTime Nanoseconds to step the simulation for @param stopPri The priority level below which the sim won't go */ -void SimModel::StepUntilStop(uint64_t SimStopTime, int64_t stopPri) +void +SimModel::StepUntilStop(uint64_t SimStopTime, int64_t stopPri) { std::cout << std::flush; - for(auto const* simThread : this->threadList) - { + for (auto const* simThread : this->threadList) { simThread->moveProcessMessages(); } - for(auto const& simThread : this->threadList) - { + for (auto const& simThread : this->threadList) { simThread->stopThreadNanos = SimStopTime; simThread->stopThreadPriority = stopPri; - if(simThread->procCount() > 0) { + if (simThread->procCount() > 0) { simThread->unlockThread(); } } - this->NextTaskTime = (uint64_t) ~0; - this->CurrentNanos = (uint64_t) ~0; - for(auto const& simThread : this->threadList) - { - if(simThread->procCount() > 0) { + this->NextTaskTime = (uint64_t)~0; + this->CurrentNanos = (uint64_t)~0; + for (auto const& simThread : this->threadList) { + if (simThread->procCount() > 0) { simThread->lockParent(); // Check if any thread had an exception and re-throw it - if(simThread->threadException) { + if (simThread->threadException) { std::rethrow_exception(simThread->threadException); } - this->NextTaskTime = simThread->NextTaskTime < this->NextTaskTime ? - simThread->NextTaskTime : this->NextTaskTime; - this->CurrentNanos = simThread->CurrentNanos < this->CurrentNanos ? - simThread->CurrentNanos : this->CurrentNanos; + this->NextTaskTime = + simThread->NextTaskTime < this->NextTaskTime ? simThread->NextTaskTime : this->NextTaskTime; + this->CurrentNanos = + simThread->CurrentNanos < this->CurrentNanos ? simThread->CurrentNanos : this->CurrentNanos; } } } - /*! This method allows the user to attach a process to the simulation for execution. Note that the priority level of the process determines what order it gets called in: higher priorities are called before lower @@ -296,12 +301,11 @@ void SimModel::StepUntilStop(uint64_t SimStopTime, int64_t stopPri) @param newProc the new process to be added */ -void SimModel::addNewProcess(SysProcess *newProc) +void +SimModel::addNewProcess(SysProcess* newProc) { - for(auto it = this->processList.begin(); it != this->processList.end(); it++) - { - if(newProc->processPriority > (*it)->processPriority) - { + for (auto it = this->processList.begin(); it != this->processList.end(); it++) { + if (newProc->processPriority > (*it)->processPriority) { this->processList.insert(it, newProc); return; } @@ -313,45 +317,42 @@ void SimModel::addNewProcess(SysProcess *newProc) * all of the tasks within each process, and all of the models within * each task and self-inits them. */ -void SimModel::selfInitSimulation() +void +SimModel::selfInitSimulation() { - for(auto const& simThread : this->threadList) - { + for (auto const& simThread : this->threadList) { simThread->selfInitNow = true; simThread->unlockThread(); } - for(auto const& simThread : this->threadList) { + for (auto const& simThread : this->threadList) { simThread->lockParent(); // Check if any thread had an exception and re-throw it - if(simThread->threadException) { + if (simThread->threadException) { std::rethrow_exception(simThread->threadException); } } this->NextTaskTime = 0; this->CurrentNanos = 0; - } /*! This method goes through all of the processes in the simulation, * all of the tasks within each process, and all of the models within * each task and resets them. */ -void SimModel::resetInitSimulation() const +void +SimModel::resetInitSimulation() const { - - for(auto const& simThread : this->threadList) - { + for (auto const& simThread : this->threadList) { simThread->resetNow = true; simThread->unlockThread(); } - for(auto const& simThread : this->threadList) - { + for (auto const& simThread : this->threadList) { simThread->lockParent(); // Check if any thread had an exception and re-throw it - if(simThread->threadException) { + if (simThread->threadException) { std::rethrow_exception(simThread->threadException); } } @@ -363,36 +364,30 @@ void SimModel::resetInitSimulation() const @param stopPri The priority level below which the sim won't go */ -void SimModel::SingleStepProcesses(int64_t stopPri) +void +SimModel::SingleStepProcesses(int64_t stopPri) { - uint64_t nextCallTime = ~((uint64_t) 0); + uint64_t nextCallTime = ~((uint64_t)0); auto it = this->processList.begin(); this->CurrentNanos = this->NextTaskTime; - while(it!= this->processList.end()) - { - if(SysProcess *localProc = (*it); localProc->processEnabled()) - { - while(localProc->nextTaskTime < this->CurrentNanos || - (localProc->nextTaskTime == this->CurrentNanos && - localProc->processPriority >= stopPri)) - { + while (it != this->processList.end()) { + if (SysProcess* localProc = (*it); localProc->processEnabled()) { + while (localProc->nextTaskTime < this->CurrentNanos || + (localProc->nextTaskTime == this->CurrentNanos && localProc->processPriority >= stopPri)) { localProc->singleStepNextTask(this->CurrentNanos); } - if(localProc->getNextTime() < nextCallTime) - { + if (localProc->getNextTime() < nextCallTime) { nextCallTime = localProc->getNextTime(); this->nextProcPriority = localProc->processPriority; - } - else if(localProc->getNextTime() == nextCallTime && - localProc->processPriority > this->nextProcPriority) - { + } else if (localProc->getNextTime() == nextCallTime && + localProc->processPriority > this->nextProcPriority) { this->nextProcPriority = localProc->processPriority; } } it++; } - this->NextTaskTime = nextCallTime != ~((uint64_t) 0) ? nextCallTime : this->CurrentNanos; + this->NextTaskTime = nextCallTime != ~((uint64_t)0) ? nextCallTime : this->CurrentNanos; //! - If a message has been added to logger, link the message IDs } @@ -400,17 +395,16 @@ void SimModel::SingleStepProcesses(int64_t stopPri) * tasks back to the initial call times. It clears all message logs. However, * it does not clear all message buffers and does not reset individual models. */ -void SimModel::ResetSimulation() +void +SimModel::ResetSimulation() { //! - Iterate through model list and call the Task model initializer - for(auto const& process : this->processList) - { + for (auto const& process : this->processList) { process->reInitProcess(); } this->NextTaskTime = 0; this->CurrentNanos = 0; - for(auto const& simThread : this->threadList) - { + for (auto const& simThread : this->threadList) { simThread->NextTaskTime = 0; simThread->CurrentNanos = 0; } @@ -421,18 +415,17 @@ void SimModel::ResetSimulation() threads need to be reallocated. Otherwise it is basically a no-op. */ -void SimModel::clearProcsFromThreads() const { +void +SimModel::clearProcsFromThreads() const +{ - for(auto const& simThread : this->threadList) - { + for (auto const& simThread : this->threadList) { simThread->clearProcessList(); } //! - Iterate through model list and call the Task model initializer - for(auto const& process : this->processList) - { + for (auto const& process : this->processList) { process->setProcessControlStatus(false); } - } /*! This method provides an easy mechanism for allowing the user to change the @@ -441,18 +434,17 @@ void SimModel::clearProcsFromThreads() const { any existing thread data, and then allocates fresh threads for the runtime. @param threadCount number of threads */ -void SimModel::resetThreads(uint64_t threadCount) +void +SimModel::resetThreads(uint64_t threadCount) { this->clearProcsFromThreads(); this->deleteThreads(); this->threadList.clear(); - for(uint64_t i=0; ithreadList.push_back(newThread); } - } /*! This method walks through all of the child threads that have been created in @@ -460,12 +452,13 @@ void SimModel::resetThreads(uint64_t threadCount) memory that has been allocated to them in the architecture. It just ensures clean shutdown of any existing runtime stuff. */ -void SimModel::deleteThreads() { - for(auto const& simThread : this->threadList) - { +void +SimModel::deleteThreads() +{ + for (auto const& simThread : this->threadList) { simThread->killThread(); simThread->unlockThread(); - if(simThread->threadContext && simThread->threadContext->joinable()) { + if (simThread->threadContext && simThread->threadContext->joinable()) { simThread->threadContext->join(); delete simThread->threadContext; } @@ -480,35 +473,31 @@ void SimModel::deleteThreads() { and pops all of the processes onto those threads in a round-robin fashion. */ -void SimModel::assignRemainingProcs() { +void +SimModel::assignRemainingProcs() +{ - std::vector::iterator it; + std::vector::iterator it; std::vector::iterator thrIt; - for(it=this->processList.begin(), thrIt=threadList.begin(); it!= this->processList.end(); it++, thrIt++) - { - if(thrIt == threadList.end()) - { + for (it = this->processList.begin(), thrIt = threadList.begin(); it != this->processList.end(); it++, thrIt++) { + if (thrIt == threadList.end()) { thrIt = threadList.begin(); } - if((*it)->getProcessControlStatus()) { - thrIt--; //Didn't get a thread to add, so roll back - } - else - { + if ((*it)->getProcessControlStatus()) { + thrIt--; // Didn't get a thread to add, so roll back + } else { (*thrIt)->addNewProcess((*it)); } } - for(auto const& simThread : this->threadList) - { - it=this->processList.begin(); + for (auto const& simThread : this->threadList) { + it = this->processList.begin(); simThread->nextProcPriority = (*it)->processPriority; simThread->NextTaskTime = 0; simThread->CurrentNanos = 0; - //simThread->lockThread(); + // simThread->lockThread(); simThread->threadContext = new std::thread(activateNewThread, simThread); } - for(auto const& simThread : this->threadList) - { + for (auto const& simThread : this->threadList) { simThread->waitOnInit(); } } @@ -522,9 +511,10 @@ void SimModel::assignRemainingProcs() { @param threadSel The thread index in the thread-pool that the process gets added to */ -void SimModel::addProcessToThread(SysProcess *newProc, uint64_t threadSel) +void +SimModel::addProcessToThread(SysProcess* newProc, uint64_t threadSel) { std::vector::iterator thrIt; - thrIt=threadList.begin() + threadSel; + thrIt = threadList.begin() + threadSel; (*thrIt)->addNewProcess(newProc); } diff --git a/src/architecture/system_model/sim_model.h b/src/architecture/system_model/sim_model.h old mode 100755 new mode 100644 index 92f2134f04..0b18dc5767 --- a/src/architecture/system_model/sim_model.h +++ b/src/architecture/system_model/sim_model.h @@ -30,85 +30,96 @@ #include #include -//! This class handles the management of a given "thread" of execution and provides the main mechanism for running concurrent jobs inside BSK +//! This class handles the management of a given "thread" of execution and provides the main mechanism for running +//! concurrent jobs inside BSK class SimThreadExecution { -public: - SimThreadExecution()=default; - explicit SimThreadExecution(uint64_t threadIdent, uint64_t currentSimNanos=0); //!< Constructor for a given sim thread - ~SimThreadExecution()=default; //!< Destructor for given sim thread - void updateNewStopTime(uint64_t newStopNanos) {stopThreadNanos = newStopNanos;} //!< Method to update a new simulation stop time - void clearProcessList() {processList.clear();} //!< clear the process list + public: + SimThreadExecution() = default; + explicit SimThreadExecution(uint64_t threadIdent, + uint64_t currentSimNanos = 0); //!< Constructor for a given sim thread + ~SimThreadExecution() = default; //!< Destructor for given sim thread + void updateNewStopTime(uint64_t newStopNanos) + { + stopThreadNanos = newStopNanos; + } //!< Method to update a new simulation stop time + void clearProcessList() { processList.clear(); } //!< clear the process list void selfInitProcesses() const; void crossInitProcesses() const; void resetProcesses(); void addNewProcess(SysProcess* newProc); - uint64_t procCount() const {return processList.size();} //!< Gets the current "thread-count" in the system - bool threadActive() const {return this->threadRunning;} //!< Tells the caller if the thread is currently allocated processes and is in execution - void threadReady() {this->threadRunning=true;} //!< Allows the system to put the thread into a running state + uint64_t procCount() const { return processList.size(); } //!< Gets the current "thread-count" in the system + bool threadActive() const + { + return this->threadRunning; + } //!< Tells the caller if the thread is currently allocated processes and is in execution + void threadReady() { this->threadRunning = true; } //!< Allows the system to put the thread into a running state void waitOnInit(); void postInit(); - bool threadValid() const {return !this->terminateThread;} //!< Determines if the thread is currently usable or if it has been requested to shutdown - void killThread() {this->terminateThread=true;} //!< Politely asks the thread to no longer be alive. + bool threadValid() const + { + return !this->terminateThread; + } //!< Determines if the thread is currently usable or if it has been requested to shutdown + void killThread() { this->terminateThread = true; } //!< Politely asks the thread to no longer be alive. void lockThread(); void unlockThread(); void lockParent(); void unlockParent(); - void StepUntilStop(); //!< Step simulation until stop time uint64_t reached - void SingleStepProcesses(int64_t stopPri=-1); //!< Step only the next Task in the simulation + void StepUntilStop(); //!< Step simulation until stop time uint64_t reached + void SingleStepProcesses(int64_t stopPri = -1); //!< Step only the next Task in the simulation void moveProcessMessages() const; - uint64_t CurrentNanos=0; //!< [ns] Current sim time - uint64_t NextTaskTime=0; //!< [ns] time for the next Task - uint64_t currentThreadNanos=0; //!< Current simulation time available at thread - uint64_t stopThreadNanos=0; //!< Current stop conditions for the thread - int64_t stopThreadPriority=-1; //!< Current stop priority for thread - uint64_t threadID=0; //!< Identifier for thread - std::thread *threadContext=nullptr; //!< std::thread data for concurrent execution - int64_t nextProcPriority=-1; //!< [-] Priority level for the next process - bool selfInitNow{}; //!< Flag requesting self init - bool crossInitNow{}; //!< Flag requesting cross-init - bool resetNow{}; //!< Flag requesting that the thread execute reset - std::exception_ptr threadException = nullptr; //!< Exception pointer for thread errors - //! -private: - bool threadRunning{}; //!< Flag that will allow for easy concurrent locking - bool terminateThread{}; //!< Flag that indicates that it is time to take thread down - BSKSemaphore parentThreadLock; //!< Lock that ensures parent thread won't proceed - BSKSemaphore selfThreadLock; //!< Lock that ensures this thread only reaches allowed time - std::vector processList; //!< List of processes associated with thread - std::mutex initReadyLock; //!< Lock function to ensure runtime locks are configured - std::condition_variable initHoldVar; //!< Conditional variable used to prevent race conditions + uint64_t CurrentNanos = 0; //!< [ns] Current sim time + uint64_t NextTaskTime = 0; //!< [ns] time for the next Task + uint64_t currentThreadNanos = 0; //!< Current simulation time available at thread + uint64_t stopThreadNanos = 0; //!< Current stop conditions for the thread + int64_t stopThreadPriority = -1; //!< Current stop priority for thread + uint64_t threadID = 0; //!< Identifier for thread + std::thread* threadContext = nullptr; //!< std::thread data for concurrent execution + int64_t nextProcPriority = -1; //!< [-] Priority level for the next process + bool selfInitNow{}; //!< Flag requesting self init + bool crossInitNow{}; //!< Flag requesting cross-init + bool resetNow{}; //!< Flag requesting that the thread execute reset + std::exception_ptr threadException = nullptr; //!< Exception pointer for thread errors + //! + private: + bool threadRunning{}; //!< Flag that will allow for easy concurrent locking + bool terminateThread{}; //!< Flag that indicates that it is time to take thread down + BSKSemaphore parentThreadLock; //!< Lock that ensures parent thread won't proceed + BSKSemaphore selfThreadLock; //!< Lock that ensures this thread only reaches allowed time + std::vector processList; //!< List of processes associated with thread + std::mutex initReadyLock; //!< Lock function to ensure runtime locks are configured + std::condition_variable initHoldVar; //!< Conditional variable used to prevent race conditions }; //! The top-level container for an entire simulation class SimModel { -public: + public: SimModel(); //!< The SimModel constructor - ~SimModel(); //!< SimModel destructorS + ~SimModel(); //!< SimModel destructorS - void selfInitSimulation(); //!< Method to initialize all added Tasks - void resetInitSimulation() const; //!< Method to reset all added tasks - void StepUntilStop(uint64_t SimStopTime, int64_t stopPri); //!< Step simulation until stop time uint64_t reached - void SingleStepProcesses(int64_t stopPri=-1); //!< Step only the next Task in the simulation - void addNewProcess(SysProcess *newProc); - void addProcessToThread(SysProcess *newProc, uint64_t threadSel); - void ResetSimulation(); //!< Reset simulation back to zero + void selfInitSimulation(); //!< Method to initialize all added Tasks + void resetInitSimulation() const; //!< Method to reset all added tasks + void StepUntilStop(uint64_t SimStopTime, int64_t stopPri); //!< Step simulation until stop time uint64_t reached + void SingleStepProcesses(int64_t stopPri = -1); //!< Step only the next Task in the simulation + void addNewProcess(SysProcess* newProc); + void addProcessToThread(SysProcess* newProc, uint64_t threadSel); + void ResetSimulation(); //!< Reset simulation back to zero void clearProcsFromThreads() const; void resetThreads(uint64_t threadCount); void deleteThreads(); void assignRemainingProcs(); - uint64_t getThreadCount() const {return threadList.size();} //!< returns the number of threads used + uint64_t getThreadCount() const { return threadList.size(); } //!< returns the number of threads used - BSKLogger bskLogger; //!< -- BSK Logging + BSKLogger bskLogger; //!< -- BSK Logging - std::vector processList; //!< -- List of processes we've created - std::vector threadList{}; //!< -- Array of threads that we're running on - std::string SimulationName; //!< -- Identifier for Sim - uint64_t CurrentNanos=0; //!< [ns] Current sim time - uint64_t NextTaskTime=0; //!< [ns] time for the next Task - int64_t nextProcPriority=-1; //!< [-] Priority level for the next process + std::vector processList; //!< -- List of processes we've created + std::vector threadList{}; //!< -- Array of threads that we're running on + std::string SimulationName; //!< -- Identifier for Sim + uint64_t CurrentNanos = 0; //!< [ns] Current sim time + uint64_t NextTaskTime = 0; //!< [ns] time for the next Task + int64_t nextProcPriority = -1; //!< [-] Priority level for the next process }; #endif /* _SimModel_H_ */ diff --git a/src/architecture/system_model/sys_process.cpp b/src/architecture/system_model/sys_process.cpp old mode 100755 new mode 100644 index c64001ec53..0b3bd6d678 --- a/src/architecture/system_model/sys_process.cpp +++ b/src/architecture/system_model/sys_process.cpp @@ -24,7 +24,8 @@ /*! Make a process AND attach a storage bucket with the provided name. Give the process the same name. */ -SysProcess::SysProcess(std::string name) : SysProcess() +SysProcess::SysProcess(std::string name) + : SysProcess() { this->processName = std::move(name); } @@ -32,30 +33,28 @@ SysProcess::SysProcess(std::string name) : SysProcess() /*! This method sets the nextTaskTime = 0 and calls SelfInitTaskList() for * all process tasks. */ -void SysProcess::selfInitProcess() +void +SysProcess::selfInitProcess() { this->nextTaskTime = 0; //! - Iterate through model list and call the Task model self-initializer - for(auto const& process : this->processTasks) - { - SysModelTask *localTask = process.TaskPtr; + for (auto const& process : this->processTasks) { + SysModelTask* localTask = process.TaskPtr; localTask->SelfInitTaskList(); } } - - /*! This method resets each task and associated model-set inside the process ensuring that all parameters go back to their default state. @param currentTime Current simulation time in ns that reset is occurring at */ -void SysProcess::resetProcess(uint64_t currentTime) +void +SysProcess::resetProcess(uint64_t currentTime) { - for(auto const& process : this->processTasks) - { - SysModelTask *localTask = process.TaskPtr; + for (auto const& process : this->processTasks) { + SysModelTask* localTask = process.TaskPtr; localTask->ResetTaskList(currentTime); //! Time of reset. Models that utilize currentTime will start at this. } this->nextTaskTime = currentTime; @@ -64,19 +63,18 @@ void SysProcess::resetProcess(uint64_t currentTime) /*! This method does two things: 1) resets the next task time for * all process tasks to the first task time. 2) clears the process list * and then adds everything back into the process with the correct priority. -*/ -void SysProcess::reInitProcess() + */ +void +SysProcess::reInitProcess() { - for(auto const& task : this->processTasks) - { - SysModelTask *localTask = task.TaskPtr; + for (auto const& task : this->processTasks) { + SysModelTask* localTask = task.TaskPtr; localTask->ResetTask(); } std::vector taskPtrs = this->processTasks; this->processTasks.clear(); - for(auto const& task : taskPtrs) - { + for (auto const& task : taskPtrs) { this->addNewTask(task.TaskPtr, task.taskPriority); } } @@ -84,43 +82,38 @@ void SysProcess::reInitProcess() /*! This method steps the next task up to currentNanos * unless it isn't supposed to run yet. */ -void SysProcess::singleStepNextTask(uint64_t currentNanos) +void +SysProcess::singleStepNextTask(uint64_t currentNanos) { //! - Check to make sure that there are models to be called. - if(this->processTasks.begin() == this->processTasks.end()) - { + if (this->processTasks.begin() == this->processTasks.end()) { bskLogger.bskLog(BSK_WARNING, "Received a step command on sim that has no active Tasks."); return; } auto fireIt = this->processTasks.begin(); //! - If the requested time does not meet our next start time, just return - for(auto it=this->processTasks.begin(); it!=this->processTasks.end(); it++) - { - if(it->NextTaskStart < fireIt->NextTaskStart || - (it->NextTaskStart==fireIt->NextTaskStart && it->taskPriority > fireIt->taskPriority)) - { + for (auto it = this->processTasks.begin(); it != this->processTasks.end(); it++) { + if (it->NextTaskStart < fireIt->NextTaskStart || + (it->NextTaskStart == fireIt->NextTaskStart && it->taskPriority > fireIt->taskPriority)) { fireIt = it; } } - if(fireIt->NextTaskStart > currentNanos) - { + if (fireIt->NextTaskStart > currentNanos) { this->nextTaskTime = fireIt->NextTaskStart; return; } //! - Call the next scheduled model, and set the time to its start - SysModelTask *localTask = fireIt->TaskPtr; + SysModelTask* localTask = fireIt->TaskPtr; localTask->ExecuteTaskList(currentNanos); fireIt->NextTaskStart = localTask->NextStartTime; //! - Figure out when we are going to be called next for scheduling purposes - fireIt=this->processTasks.begin(); + fireIt = this->processTasks.begin(); //! - If the requested time does not meet our next start time, just return - for(auto it=this->processTasks.begin(); it!=this->processTasks.end(); it++) - { - if(it->NextTaskStart < fireIt->NextTaskStart || - (it->NextTaskStart==fireIt->NextTaskStart && it->taskPriority > fireIt->taskPriority)) - { + for (auto it = this->processTasks.begin(); it != this->processTasks.end(); it++) { + if (it->NextTaskStart < fireIt->NextTaskStart || + (it->NextTaskStart == fireIt->NextTaskStart && it->taskPriority > fireIt->taskPriority)) { fireIt = it; } } @@ -132,7 +125,8 @@ void SysProcess::singleStepNextTask(uint64_t currentNanos) @param newTask The new task that we are adding to the list @param taskPriority The selected priority of the task being added */ -void SysProcess::addNewTask(SysModelTask *newTask, int32_t taskPriority) +void +SysProcess::addNewTask(SysModelTask* newTask, int32_t taskPriority) { ModelScheduleEntry localEntry; localEntry.TaskPtr = newTask; @@ -150,16 +144,14 @@ void SysProcess::addNewTask(SysModelTask *newTask, int32_t taskPriority) the simulation will faithfully schedule it. @param taskCall Pointer to a struct that contains start time and task handle. */ -void SysProcess::scheduleTask(const ModelScheduleEntry& taskCall) +void +SysProcess::scheduleTask(const ModelScheduleEntry& taskCall) { //! - Iteratre through all of the task models to find correct place - for(auto it = this->processTasks.begin(); it != this->processTasks.end(); it++) - { + for (auto it = this->processTasks.begin(); it != this->processTasks.end(); it++) { //! - If the next Task starts after new Task, pop it on just prior - if(it->NextTaskStart > taskCall.NextTaskStart || - (it->NextTaskStart == taskCall.NextTaskStart && - taskCall.taskPriority > it->taskPriority)) - { + if (it->NextTaskStart > taskCall.NextTaskStart || + (it->NextTaskStart == taskCall.NextTaskStart && taskCall.taskPriority > it->taskPriority)) { this->processTasks.insert(it, taskCall); return; } @@ -172,11 +164,11 @@ void SysProcess::scheduleTask(const ModelScheduleEntry& taskCall) a process' tasks. It is handy for a FSW scheme where you have tons of tasks and you are really only turning one on at a time. */ -void SysProcess::disableAllTasks() const +void +SysProcess::disableAllTasks() const { //! - Iterate through all of the tasks to disable them - for(auto const& scheduleEntry : this->processTasks) - { + for (auto const& scheduleEntry : this->processTasks) { scheduleEntry.TaskPtr->disableTask(); } } @@ -184,32 +176,34 @@ void SysProcess::disableAllTasks() const a processes tasks. It is handy for a process that starts out almost entirely inhibited but you want to turn it all on at once. */ -void SysProcess::enableAllTasks() const +void +SysProcess::enableAllTasks() const { //! - Iterate through all of the task models to disable them - for(auto const& scheduleEntry : this->processTasks) - { + for (auto const& scheduleEntry : this->processTasks) { scheduleEntry.TaskPtr->enableTask(); } } /*! This method updates a specified task's period once it locates that task in the list. It will warn the user if a task is not found. - @param taskName The name of the task you want to change period of - @param newPeriod the new number of nanoseconds you want between calls + @param taskName The name of the task you want to change period of + @param newPeriod the new number of nanoseconds you want between calls */ -void SysProcess::changeTaskPeriod(std::string const& taskName, uint64_t newPeriod) +void +SysProcess::changeTaskPeriod(std::string const& taskName, uint64_t newPeriod) { - //! - Iteratre through all of the task models to disable them - for (ModelScheduleEntry &scheduleEntry : this->processTasks) - { - if (scheduleEntry.TaskPtr->TaskName == taskName) - { + //! - Iteratre through all of the task models to disable them + for (ModelScheduleEntry& scheduleEntry : this->processTasks) { + if (scheduleEntry.TaskPtr->TaskName == taskName) { scheduleEntry.TaskPtr->updatePeriod(newPeriod); scheduleEntry.NextTaskStart = scheduleEntry.TaskPtr->NextStartTime; scheduleEntry.TaskUpdatePeriod = scheduleEntry.TaskPtr->TaskPeriod; - return; - } - } - bskLogger.bskLog(BSK_WARNING, "You attempted to change the period of task: %s I couldn't find that in process: %s", taskName.c_str(), this->processName.c_str()); + return; + } + } + bskLogger.bskLog(BSK_WARNING, + "You attempted to change the period of task: %s I couldn't find that in process: %s", + taskName.c_str(), + this->processName.c_str()); } diff --git a/src/architecture/system_model/sys_process.h b/src/architecture/system_model/sys_process.h old mode 100755 new mode 100644 index 54794d0335..9ea5c5545e --- a/src/architecture/system_model/sys_process.h +++ b/src/architecture/system_model/sys_process.h @@ -26,49 +26,56 @@ #include //! Structure that contains the information needed to call a Task -typedef struct { - uint64_t NextTaskStart; /*!< Time to call Task next*/ - uint64_t TaskUpdatePeriod; /*!< Period of update for Task*/ - int32_t taskPriority; /*!< [-] Priority level for the task*/ - SysModelTask *TaskPtr; /*!< Handle to the Task that needs to be called*/ -}ModelScheduleEntry; +typedef struct +{ + uint64_t NextTaskStart; /*!< Time to call Task next*/ + uint64_t TaskUpdatePeriod; /*!< Period of update for Task*/ + int32_t taskPriority; /*!< [-] Priority level for the task*/ + SysModelTask* TaskPtr; /*!< Handle to the Task that needs to be called*/ +} ModelScheduleEntry; //! Class used to group a set of tasks into one process (task group) of execution class SysProcess { -public: - SysProcess()=default; + public: + SysProcess() = default; explicit SysProcess(std::string name); //!< class method - ~SysProcess()=default; - void addNewTask(SysModelTask *newTask, int32_t taskPriority = -1); //!< class method - void selfInitProcess(); //!< class method - void resetProcess(uint64_t currentTime); //!< class method - void reInitProcess(); //!< class method - void enableProcess() {this->processActive = true;} //!< class method - void disableProcess() {this->processActive = false;} //!< class method - void scheduleTask(const ModelScheduleEntry & taskCall); //!< class method - void setProcessName(std::string const& newName){this->processName = newName;} //!< class method - std::string const getProcessName() { return(this->processName);} //!< class method - uint64_t getNextTime() { return(this->nextTaskTime);} //!< class method - void singleStepNextTask(uint64_t currentNanos); //!< class method - bool processEnabled() const {return this->processActive;} //!< class method - void changeTaskPeriod(const std::string& taskName, uint64_t newPeriod); //!< class method - void setPriority(int64_t newPriority) {this->processPriority = newPriority;} //!< class method - void disableAllTasks() const; //!< class method - void enableAllTasks() const; //!< class method - bool getProcessControlStatus() const {return this->processOnThread;} //!< Allows caller to see if this process is parented by a thread - void setProcessControlStatus(bool processTaken) {processOnThread = processTaken;} //!< Provides a mechanism to say that this process is allocated to a thread + ~SysProcess() = default; + void addNewTask(SysModelTask* newTask, int32_t taskPriority = -1); //!< class method + void selfInitProcess(); //!< class method + void resetProcess(uint64_t currentTime); //!< class method + void reInitProcess(); //!< class method + void enableProcess() { this->processActive = true; } //!< class method + void disableProcess() { this->processActive = false; } //!< class method + void scheduleTask(const ModelScheduleEntry& taskCall); //!< class method + void setProcessName(std::string const& newName) { this->processName = newName; } //!< class method + std::string const getProcessName() { return (this->processName); } //!< class method + uint64_t getNextTime() { return (this->nextTaskTime); } //!< class method + void singleStepNextTask(uint64_t currentNanos); //!< class method + bool processEnabled() const { return this->processActive; } //!< class method + void changeTaskPeriod(const std::string& taskName, uint64_t newPeriod); //!< class method + void setPriority(int64_t newPriority) { this->processPriority = newPriority; } //!< class method + void disableAllTasks() const; //!< class method + void enableAllTasks() const; //!< class method + bool getProcessControlStatus() const + { + return this->processOnThread; + } //!< Allows caller to see if this process is parented by a thread + void setProcessControlStatus(bool processTaken) + { + processOnThread = processTaken; + } //!< Provides a mechanism to say that this process is allocated to a thread -public: - std::vector processTasks; //!< -- Array that has pointers to all process tasks - uint64_t nextTaskTime=0; //!< [ns] time for the next Task - uint64_t prevRouteTime=0; //!< [ns] Time that interfaces were previously routed - std::string processName{}; //!< -- Identifier for process - bool processActive{}; //!< -- Flag indicating whether the Process is active - bool processOnThread{}; //!< -- Flag indicating that the process has been added to a thread for execution - int64_t processPriority=-1; //!< [-] Priority level for process (higher first) - BSKLogger bskLogger; //!< -- BSK Logging + public: + std::vector processTasks; //!< -- Array that has pointers to all process tasks + uint64_t nextTaskTime = 0; //!< [ns] time for the next Task + uint64_t prevRouteTime = 0; //!< [ns] Time that interfaces were previously routed + std::string processName{}; //!< -- Identifier for process + bool processActive{}; //!< -- Flag indicating whether the Process is active + bool processOnThread{}; //!< -- Flag indicating that the process has been added to a thread for execution + int64_t processPriority = -1; //!< [-] Priority level for process (higher first) + BSKLogger bskLogger; //!< -- BSK Logging }; #endif /* _SysProcess_H_ */ diff --git a/src/architecture/utilities/BSpline.cpp b/src/architecture/utilities/BSpline.cpp index 5638da43f3..1c82148a55 100644 --- a/src/architecture/utilities/BSpline.cpp +++ b/src/architecture/utilities/BSpline.cpp @@ -42,12 +42,13 @@ InputDataSet::InputDataSet(Eigen::VectorXd X1, Eigen::VectorXd X2, Eigen::Vector this->AvgXDot_flag = false; this->W_flag = false; - uint64_t N1 = (uint64_t) X1.size(); - uint64_t N2 = (uint64_t) X2.size(); - uint64_t N3 = (uint64_t) X3.size(); + uint64_t N1 = (uint64_t)X1.size(); + uint64_t N2 = (uint64_t)X2.size(); + uint64_t N3 = (uint64_t)X3.size(); if ((N1 != N2) || (N1 != N3) || (N2 != N3)) { - std::cout << "Error in BSpline.InputDataSet: \n the Input coordinate vectors X1, X2, X3 have different sizes. \n"; + std::cout + << "Error in BSpline.InputDataSet: \n the Input coordinate vectors X1, X2, X3 have different sizes. \n"; } return; @@ -60,25 +61,69 @@ InputDataSet::~InputDataSet() } /*! Set the first derivative of the starting point (optional) */ -void InputDataSet::setXDot_0(Eigen::Vector3d XDot_0) {this->XDot_0 = XDot_0; this->XDot_0_flag = true; return;} +void +InputDataSet::setXDot_0(Eigen::Vector3d XDot_0) +{ + this->XDot_0 = XDot_0; + this->XDot_0_flag = true; + return; +} /*! Set the first derivative of the last point (optional) */ -void InputDataSet::setXDot_N(Eigen::Vector3d XDot_N) {this->XDot_N = XDot_N; this->XDot_N_flag = true; return;} +void +InputDataSet::setXDot_N(Eigen::Vector3d XDot_N) +{ + this->XDot_N = XDot_N; + this->XDot_N_flag = true; + return; +} /*! Set the second derivative of the starting point (optional) */ -void InputDataSet::setXDDot_0(Eigen::Vector3d XDDot_0) {this->XDDot_0 = XDDot_0; this->XDDot_0_flag = true; return;} +void +InputDataSet::setXDDot_0(Eigen::Vector3d XDDot_0) +{ + this->XDDot_0 = XDDot_0; + this->XDDot_0_flag = true; + return; +} /*! Set the second derivative of the last point (optional) */ -void InputDataSet::setXDDot_N(Eigen::Vector3d XDDot_N) {this->XDDot_N = XDDot_N; this->XDDot_N_flag = true; return;} +void +InputDataSet::setXDDot_N(Eigen::Vector3d XDDot_N) +{ + this->XDDot_N = XDDot_N; + this->XDDot_N_flag = true; + return; +} /*! Set the time tags for each waypoint (optional). Cannot be imposed together with avg velocity norm below */ -void InputDataSet::setT(Eigen::VectorXd T) {this->T = T; this->T_flag = true; this->AvgXDot_flag = false; return;} +void +InputDataSet::setT(Eigen::VectorXd T) +{ + this->T = T; + this->T_flag = true; + this->AvgXDot_flag = false; + return; +} /*! Set the average velocity norm (optional). Cannot be imposed together with time tag vector above */ -void InputDataSet::setAvgXDot(double AvgXDot) {this->AvgXDot = AvgXDot; this->AvgXDot_flag = true; this->T_flag = false; return;} +void +InputDataSet::setAvgXDot(double AvgXDot) +{ + this->AvgXDot = AvgXDot; + this->AvgXDot_flag = true; + this->T_flag = false; + return; +} /*! Set the weights for each waypoint. Weights are used in the LS approximation */ -void InputDataSet::setW(Eigen::VectorXd W) {this->W = W; this->W_flag = true; return;} +void +InputDataSet::setW(Eigen::VectorXd W) +{ + this->W = W; + this->W_flag = true; + return; +} /*! This constructor initializes an Output structure for BSpline interpolation */ OutputDataSet::OutputDataSet() @@ -93,15 +138,16 @@ OutputDataSet::~OutputDataSet() } /*! This method returns x, xDot and xDDot at the desired input time T */ -void OutputDataSet::getData(double T, double x[3], double xDot[3], double xDDot[3]) +void +OutputDataSet::getData(double T, double x[3], double xDot[3], double xDDot[3]) { - int N = (int) this->T.size() - 1; + int N = (int)this->T.size() - 1; double Ttot = this->T[N]; // if T < Ttot calculalte the values if (T <= Ttot) { double t = T / Ttot; - int Q = (int) this->C1.size(); + int Q = (int)this->C1.size(); Eigen::VectorXd NN(Q), NN1(Q), NN2(Q); basisFunction(t, this->U, Q, this->P, &NN[0], &NN1[0], &NN2[0]); x[0] = NN.dot(this->C1); @@ -110,9 +156,9 @@ void OutputDataSet::getData(double T, double x[3], double xDot[3], double xDDot[ xDot[0] = NN1.dot(this->C1) / Ttot; xDot[1] = NN1.dot(this->C2) / Ttot; xDot[2] = NN1.dot(this->C3) / Ttot; - xDDot[0] = NN2.dot(this->C1) / pow(Ttot,2); - xDDot[1] = NN2.dot(this->C2) / pow(Ttot,2); - xDDot[2] = NN2.dot(this->C3) / pow(Ttot,2); + xDDot[0] = NN2.dot(this->C1) / pow(Ttot, 2); + xDDot[1] = NN2.dot(this->C2) / pow(Ttot, 2); + xDDot[2] = NN2.dot(this->C3) / pow(Ttot, 2); } // if t > Ttot return final value with zero derivatives else { @@ -130,56 +176,57 @@ void OutputDataSet::getData(double T, double x[3], double xDot[3], double xDDot[ /*! This method returns single coordinates of x, xDot and xDDot at the desired input time T. */ /*! It is designed to be accessible from Python */ -double OutputDataSet::getStates(double T, int derivative, int index) +double +OutputDataSet::getStates(double T, int derivative, int index) { - int N = (int) this->T.size()-1; + int N = (int)this->T.size() - 1; double Ttot = this->T[N]; // if T < Ttot calculalte the values if (T <= Ttot) { double t = T / Ttot; - int Q = (int) this->C1.size(); + int Q = (int)this->C1.size(); Eigen::VectorXd NN(Q), NN1(Q), NN2(Q); basisFunction(t, this->U, Q, this->P, &NN[0], &NN1[0], &NN2[0]); switch (derivative) { - case 0 : - switch(index) { - case 0 : + case 0: + switch (index) { + case 0: return NN.dot(this->C1); - case 1 : + case 1: return NN.dot(this->C2); - case 2 : + case 2: return NN.dot(this->C3); - default : + default: std::cout << "Error in Output.getStates: invalid index \n"; return 1000; } - case 1 : - switch(index) { - case 0 : + case 1: + switch (index) { + case 0: return NN1.dot(this->C1) / Ttot; - case 1 : + case 1: return NN1.dot(this->C2) / Ttot; - case 2 : + case 2: return NN1.dot(this->C3) / Ttot; - default : + default: std::cout << "Error in Output.getStates: invalid index \n"; return 1000; } - case 2 : - switch(index) { - case 0 : - return NN2.dot(this->C1) / pow(Ttot,2); - case 1 : - return NN2.dot(this->C2) / pow(Ttot,2); - case 2 : - return NN2.dot(this->C3) / pow(Ttot,2); - default : + case 2: + switch (index) { + case 0: + return NN2.dot(this->C1) / pow(Ttot, 2); + case 1: + return NN2.dot(this->C2) / pow(Ttot, 2); + case 2: + return NN2.dot(this->C3) / pow(Ttot, 2); + default: std::cout << "Error in Output.getStates: invalid index \n"; return 1000; } - default : + default: std::cout << "Error in Output.getStates: invalid derivative \n"; return 1000; } @@ -187,72 +234,76 @@ double OutputDataSet::getStates(double T, int derivative, int index) // if t > Ttot return final value with zero derivatives else { switch (derivative) { - case 0 : - switch(index) { - case 0 : + case 0: + switch (index) { + case 0: return this->X1[N]; - case 1 : + case 1: return this->X2[N]; - case 2 : + case 2: return this->X3[N]; - default : + default: std::cout << "Error in Output.getStates: invalid index \n"; return 1000; } - case 1 : - switch(index) { - case 0 : + case 1: + switch (index) { + case 0: return 0; - case 1 : + case 1: return 0; - case 2 : + case 2: return 0; - default : + default: std::cout << "Error in Output.getStates: invalid index \n"; return 1000; } - case 2 : - switch(index) { - case 0 : + case 2: + switch (index) { + case 0: return 0; - case 1 : + case 1: return 0; - case 2 : + case 2: return 0; - default : + default: std::cout << "Error in Output.getStates: invalid index \n"; return 1000; } - default : + default: std::cout << "Error in Output.getStates: invalid derivative \n"; return 1000; } } } -/*! This function takes the Input structure, performs the BSpline interpolation and outputs the result into Output structure */ -void interpolate(InputDataSet Input, int Num, int P, OutputDataSet *Output) +/*! This function takes the Input structure, performs the BSpline interpolation and outputs the result into Output + * structure */ +void +interpolate(InputDataSet Input, int Num, int P, OutputDataSet* Output) { Output->P = P; // N = number of waypoints - 1 - int N = (int) Input.X1.size() - 1; + int N = (int)Input.X1.size() - 1; - // T = time tags; if not specified, it is computed from a cartesian distance assuming a constant velocity norm on average - Eigen::VectorXd T(N+1); + // T = time tags; if not specified, it is computed from a cartesian distance assuming a constant velocity norm on + // average + Eigen::VectorXd T(N + 1); double S = 0; if (Input.T_flag == true) { T = Input.T; - } - else { + } else { T[0] = 0; - for (int n = 1; n < N+1; n++) { - T[n] = T[n-1] + pow( (pow(Input.X1[n]-Input.X1[n-1], 2) + pow(Input.X2[n]-Input.X2[n-1], 2) + pow(Input.X3[n]-Input.X3[n-1], 2)), 0.5 ); - S += T[n] - T[n-1]; + for (int n = 1; n < N + 1; n++) { + T[n] = T[n - 1] + pow((pow(Input.X1[n] - Input.X1[n - 1], 2) + pow(Input.X2[n] - Input.X2[n - 1], 2) + + pow(Input.X3[n] - Input.X3[n - 1], 2)), + 0.5); + S += T[n] - T[n - 1]; } } if (Input.AvgXDot_flag == true) { - for (int n = 0; n < N+1; n++) { + for (int n = 0; n < N + 1; n++) { T[n] = T[n] / T[N] * S / Input.AvgXDot; } } @@ -260,111 +311,119 @@ void interpolate(InputDataSet Input, int Num, int P, OutputDataSet *Output) double Ttot = T[N]; // build uk vector: normalized waypoint time tags - Eigen::VectorXd uk(N+1); - for (int n = 0; n < N+1; n++) { + Eigen::VectorXd uk(N + 1); + for (int n = 0; n < N + 1; n++) { uk[n] = T[n] / Ttot; } // K = number of endpoint derivatives int K = 0; - if (Input.XDot_0_flag == true) {K += 1;} - if (Input.XDot_N_flag == true) {K += 1;} - if (Input.XDDot_0_flag == true) {K += 1;} - if (Input.XDDot_N_flag == true) {K += 1;} + if (Input.XDot_0_flag == true) { + K += 1; + } + if (Input.XDot_N_flag == true) { + K += 1; + } + if (Input.XDDot_0_flag == true) { + K += 1; + } + if (Input.XDDot_N_flag == true) { + K += 1; + } // The maximum polynomial order is N + K. If a higher order is requested, print a BSK_ERROR if (P > N + K) { - std::cout << "Error in BSpline.interpolate: \n the desired polynomial order P is too high. Mass matrix A will be singular. \n" ; + std::cout << "Error in BSpline.interpolate: \n the desired polynomial order P is too high. Mass matrix A will " + "be singular. \n"; } int M = N + P + K + 1; // build knot vector U of size M + 1 - Eigen::VectorXd U(M+1); + Eigen::VectorXd U(M + 1); double u; - for (int p = 0; p < P+1; p++) { + for (int p = 0; p < P + 1; p++) { U[p] = 0; } - for (int j = 0; j < M-2*P-1; j++) { + for (int j = 0; j < M - 2 * P - 1; j++) { u = 0.0; - for (int i = j; i < j+P; i++) { + for (int i = j; i < j + P; i++) { if (i >= uk.size()) { u += uk[N] / P; - } - else { + } else { u += uk[i] / P; } - U[P+j+1] = u; + U[P + j + 1] = u; } } - for (int p = 0; p < P+1; p++) { - U[M-P+p] = 1; + for (int p = 0; p < P + 1; p++) { + U[M - P + p] = 1; } // build stiffness matrix A of size (N+K+1)x(N+K+1) - Eigen::MatrixXd A(N+K+1,N+K+1); + Eigen::MatrixXd A(N + K + 1, N + K + 1); // build vectors Q1, Q2, Q3 (left hand side of linear system) - Eigen::VectorXd Q1(N+K+1), Q2(N+K+1), Q3(N+K+1); + Eigen::VectorXd Q1(N + K + 1), Q2(N + K + 1), Q3(N + K + 1); // populate A with zeros - for (int n = 0; n < N+K+1; n++) { - for (int m = 0; m < N+K+1; m++) { - A(n,m) = 0; + for (int n = 0; n < N + K + 1; n++) { + for (int m = 0; m < N + K + 1; m++) { + A(n, m) = 0; } } int n = -1; // constrain first derivative at starting point if (Input.XDot_0_flag == true) { n += 1; - A(n,0) = -1; - A(n,1) = 1; - Q1[n] = U[P+1] / P * Input.XDot_0[0] * Ttot; - Q2[n] = U[P+1] / P * Input.XDot_0[1] * Ttot; - Q3[n] = U[P+1] / P * Input.XDot_0[2] * Ttot; + A(n, 0) = -1; + A(n, 1) = 1; + Q1[n] = U[P + 1] / P * Input.XDot_0[0] * Ttot; + Q2[n] = U[P + 1] / P * Input.XDot_0[1] * Ttot; + Q3[n] = U[P + 1] / P * Input.XDot_0[2] * Ttot; } // constrain second derivative at starting point if (Input.XDDot_0_flag == true) { n += 1; - A(n,0) = U[P+2]; - A(n,1) = -(U[P+1] + U[P+2]); - A(n,2) = U[P+1]; - Q1[n] = ( pow(U[P+1],2) * U[P+2] / (P*(P-1)) ) * Input.XDDot_0[0] * pow(Ttot,2); - Q2[n] = ( pow(U[P+1],2) * U[P+2] / (P*(P-1)) ) * Input.XDDot_0[1] * pow(Ttot,2); - Q3[n] = ( pow(U[P+1],2) * U[P+2] / (P*(P-1)) ) * Input.XDDot_0[2] * pow(Ttot,2); + A(n, 0) = U[P + 2]; + A(n, 1) = -(U[P + 1] + U[P + 2]); + A(n, 2) = U[P + 1]; + Q1[n] = (pow(U[P + 1], 2) * U[P + 2] / (P * (P - 1))) * Input.XDDot_0[0] * pow(Ttot, 2); + Q2[n] = (pow(U[P + 1], 2) * U[P + 2] / (P * (P - 1))) * Input.XDDot_0[1] * pow(Ttot, 2); + Q3[n] = (pow(U[P + 1], 2) * U[P + 2] / (P * (P - 1))) * Input.XDDot_0[2] * pow(Ttot, 2); } n += 1; int m = -1; int n0 = n; - Eigen::VectorXd NN(N+K+1), NN1(N+K+1), NN2(N+K+1); + Eigen::VectorXd NN(N + K + 1), NN1(N + K + 1), NN2(N + K + 1); // constrain waypoints - for (n = n0; n < N+n0+1; n++) { + for (n = n0; n < N + n0 + 1; n++) { m += 1; - basisFunction(uk[m], U, N+K+1, P, &NN[0], &NN1[0], &NN2[0]); - for (int b = 0; b < N+K+1; b++) { - A(n,b) = NN[b]; + basisFunction(uk[m], U, N + K + 1, P, &NN[0], &NN1[0], &NN2[0]); + for (int b = 0; b < N + K + 1; b++) { + A(n, b) = NN[b]; } Q1[n] = Input.X1[m]; Q2[n] = Input.X2[m]; Q3[n] = Input.X3[m]; } - n = N+n0; + n = N + n0; // constrain second derivative at final point if (Input.XDDot_N_flag == true) { n += 1; - A(n,N+K-2) = 1 - U[M-P-1]; - A(n,N+K-1) = -(2 - U[M-P-1] - U[M-P-2]); - A(n,N+K) = 1 - U[M-P-2]; - Q1[n] = ( pow((1-U[M-P-1]),2) * (1-U[M-P-2]) / (P*(P-1)) ) * Input.XDDot_N[0] * pow(Ttot,2); - Q2[n] = ( pow((1-U[M-P-1]),2) * (1-U[M-P-2]) / (P*(P-1)) ) * Input.XDDot_N[1] * pow(Ttot,2); - Q3[n] = ( pow((1-U[M-P-1]),2) * (1-U[M-P-2]) / (P*(P-1)) ) * Input.XDDot_N[2] * pow(Ttot,2); + A(n, N + K - 2) = 1 - U[M - P - 1]; + A(n, N + K - 1) = -(2 - U[M - P - 1] - U[M - P - 2]); + A(n, N + K) = 1 - U[M - P - 2]; + Q1[n] = (pow((1 - U[M - P - 1]), 2) * (1 - U[M - P - 2]) / (P * (P - 1))) * Input.XDDot_N[0] * pow(Ttot, 2); + Q2[n] = (pow((1 - U[M - P - 1]), 2) * (1 - U[M - P - 2]) / (P * (P - 1))) * Input.XDDot_N[1] * pow(Ttot, 2); + Q3[n] = (pow((1 - U[M - P - 1]), 2) * (1 - U[M - P - 2]) / (P * (P - 1))) * Input.XDDot_N[2] * pow(Ttot, 2); } // constrain first derivative at final point if (Input.XDot_N_flag == true) { n += 1; - A(n,N+K-1) = -1; - A(n,N+K) = 1; - Q1[n] = (1-U[M-P-1]) / P * Input.XDot_N[0] * Ttot; - Q2[n] = (1-U[M-P-1]) / P * Input.XDot_N[1] * Ttot; - Q3[n] = (1-U[M-P-1]) / P * Input.XDot_N[2] * Ttot; + A(n, N + K - 1) = -1; + A(n, N + K) = 1; + Q1[n] = (1 - U[M - P - 1]) / P * Input.XDot_N[0] * Ttot; + Q2[n] = (1 - U[M - P - 1]) / P * Input.XDot_N[1] * Ttot; + Q3[n] = (1 - U[M - P - 1]) / P * Input.XDot_N[2] * Ttot; } // solve linear systems @@ -392,46 +451,50 @@ void interpolate(InputDataSet Input, int Num, int P, OutputDataSet *Output) Output->XDD2.resize(Num); Output->XDD3.resize(Num); for (int i = 0; i < Num; i++) { - basisFunction(t, U, N+K+1, P, &NN[0], &NN1[0], &NN2[0]); + basisFunction(t, U, N + K + 1, P, &NN[0], &NN1[0], &NN2[0]); Output->T[i] = t * Ttot; Output->X1[i] = NN.dot(C1); Output->X2[i] = NN.dot(C2); Output->X3[i] = NN.dot(C3); - Output->XD1[i] = NN1.dot(C1) / Ttot; - Output->XD2[i] = NN1.dot(C2) / Ttot; - Output->XD3[i] = NN1.dot(C3) / Ttot; - Output->XDD1[i] = NN2.dot(C1) / pow(Ttot,2); - Output->XDD2[i] = NN2.dot(C2) / pow(Ttot,2); - Output->XDD3[i] = NN2.dot(C3) / pow(Ttot,2); + Output->XD1[i] = NN1.dot(C1) / Ttot; + Output->XD2[i] = NN1.dot(C2) / Ttot; + Output->XD3[i] = NN1.dot(C3) / Ttot; + Output->XDD1[i] = NN2.dot(C1) / pow(Ttot, 2); + Output->XDD2[i] = NN2.dot(C2) / pow(Ttot, 2); + Output->XDD3[i] = NN2.dot(C3) / pow(Ttot, 2); t += dt; } return; } -/*! This function takes the Input structure, performs the BSpline LS approximation and outputs the result into Output structure */ -void approximate(InputDataSet Input, int Num, int Q, int P, OutputDataSet *Output) +/*! This function takes the Input structure, performs the BSpline LS approximation and outputs the result into Output + * structure */ +void +approximate(InputDataSet Input, int Num, int Q, int P, OutputDataSet* Output) { Output->P = P; // N = number of waypoints - 1 - int N = (int) Input.X1.size() - 1; + int N = (int)Input.X1.size() - 1; - // T = time tags; if not specified, it is computed from a cartesian distance assuming a constant velocity norm on average - Eigen::VectorXd T(N+1); + // T = time tags; if not specified, it is computed from a cartesian distance assuming a constant velocity norm on + // average + Eigen::VectorXd T(N + 1); double S = 0; if (Input.T_flag == true) { T = Input.T; - } - else { + } else { T[0] = 0; - for (int n = 1; n < N+1; n++) { - T[n] = T[n-1] + pow( (pow(Input.X1[n]-Input.X1[n-1], 2) + pow(Input.X2[n]-Input.X2[n-1], 2) + pow(Input.X3[n]-Input.X3[n-1], 2)), 0.5 ); - S += T[n] - T[n-1]; + for (int n = 1; n < N + 1; n++) { + T[n] = T[n - 1] + pow((pow(Input.X1[n] - Input.X1[n - 1], 2) + pow(Input.X2[n] - Input.X2[n - 1], 2) + + pow(Input.X3[n] - Input.X3[n - 1], 2)), + 0.5); + S += T[n] - T[n - 1]; } } if (Input.AvgXDot_flag == true) { - for (int n = 0; n < N+1; n++) { + for (int n = 0; n < N + 1; n++) { T[n] = T[n] / T[N] * S / Input.AvgXDot; } } @@ -439,62 +502,71 @@ void approximate(InputDataSet Input, int Num, int Q, int P, OutputDataSet *Outpu double Ttot = T[N]; // build uk vector: normalized waypoint time tags - Eigen::VectorXd uk(N+1); - for (int n = 0; n < N+1; n++) { + Eigen::VectorXd uk(N + 1); + for (int n = 0; n < N + 1; n++) { uk[n] = T[n] / Ttot; } // The maximum polynomial order is N + K. If a higher order is requested, print a BSK_ERROR if (P > Q) { - std::cout << "Error in BSpline.approximate: \n the desired polynomial order P can't be higher than the number of control points Q. \n" ; + std::cout << "Error in BSpline.approximate: \n the desired polynomial order P can't be higher than the number " + "of control points Q. \n"; } // build knot vector U of size Q + P + 2 - Eigen::VectorXd U(Q+P+2); + Eigen::VectorXd U(Q + P + 2); double d, alpha; int i; - d = ((double)(N+1)) / ((double)abs(Q-P+1)); - for (int p = 0; p < P+1; p++) { + d = ((double)(N + 1)) / ((double)abs(Q - P + 1)); + for (int p = 0; p < P + 1; p++) { U[p] = 0; } - for (int j = 1; j < Q-P+1; j++) { - i = int(j*d); - alpha = j*d - i; - U[P+j] = (1-alpha)*uk[i-1] + alpha*uk[i]; + for (int j = 1; j < Q - P + 1; j++) { + i = int(j * d); + alpha = j * d - i; + U[P + j] = (1 - alpha) * uk[i - 1] + alpha * uk[i]; } - for (int p = 0; p < P+1; p++) { - U[Q+p+1] = 1; + for (int p = 0; p < P + 1; p++) { + U[Q + p + 1] = 1; } // K = number of endpoint derivatives int K = 0; - if (Input.XDot_0_flag == true) {K += 1;} - if (Input.XDot_N_flag == true) {K += 1;} - if (Input.XDDot_0_flag == true) {K += 1;} - if (Input.XDDot_N_flag == true) {K += 1;} + if (Input.XDot_0_flag == true) { + K += 1; + } + if (Input.XDot_N_flag == true) { + K += 1; + } + if (Input.XDDot_0_flag == true) { + K += 1; + } + if (Input.XDDot_N_flag == true) { + K += 1; + } // build stiffness matrix MD of size (K+2)x(K+2) - Eigen::MatrixXd MD(K+2,K+2); + Eigen::MatrixXd MD(K + 2, K + 2); // build vectors T1, T2, T3 (left hand side of linear system) - Eigen::VectorXd T1(K+2), T2(K+2), T3(K+2); + Eigen::VectorXd T1(K + 2), T2(K + 2), T3(K + 2); // populate MD with zeros - for (int n = 0; n < K+2; n++) { - for (int m = 0; m < K+2; m++) { - MD(n,m) = 0; + for (int n = 0; n < K + 2; n++) { + for (int m = 0; m < K + 2; m++) { + MD(n, m) = 0; } } - Eigen::VectorXd NN(Q+1), NN1(Q+1), NN2(Q+1); - basisFunction(uk[0], U, Q+1, P, &NN[0], &NN1[0], &NN2[0]); + Eigen::VectorXd NN(Q + 1), NN1(Q + 1), NN2(Q + 1); + basisFunction(uk[0], U, Q + 1, P, &NN[0], &NN1[0], &NN2[0]); int n = 0; - MD(0,0) = NN[0]; + MD(0, 0) = NN[0]; T1[0] = Input.X1[0]; T2[0] = Input.X2[0]; T3[0] = Input.X3[0]; // constrain first derivative at starting point if (Input.XDot_0_flag == true) { n += 1; - MD(n,0) = NN1[0]; - MD(n,1) = NN1[1]; + MD(n, 0) = NN1[0]; + MD(n, 1) = NN1[1]; T1[n] = Input.XDot_0[0] * Ttot; T2[n] = Input.XDot_0[1] * Ttot; T3[n] = Input.XDot_0[2] * Ttot; @@ -502,35 +574,35 @@ void approximate(InputDataSet Input, int Num, int Q, int P, OutputDataSet *Outpu // constrain second derivative at starting point if (Input.XDDot_0_flag == true) { n += 1; - MD(n,0) = NN2[0]; - MD(n,1) = NN2[1]; - MD(n,2) = NN2[2]; - T1[n] = Input.XDDot_0[0] * pow(Ttot,2); - T2[n] = Input.XDDot_0[1] * pow(Ttot,2); - T3[n] = Input.XDDot_0[2] * pow(Ttot,2); - } - basisFunction(uk[N], U, Q+1, P, &NN[0], &NN1[0], &NN2[0]); + MD(n, 0) = NN2[0]; + MD(n, 1) = NN2[1]; + MD(n, 2) = NN2[2]; + T1[n] = Input.XDDot_0[0] * pow(Ttot, 2); + T2[n] = Input.XDDot_0[1] * pow(Ttot, 2); + T3[n] = Input.XDDot_0[2] * pow(Ttot, 2); + } + basisFunction(uk[N], U, Q + 1, P, &NN[0], &NN1[0], &NN2[0]); // constrain second derivative at ending point if (Input.XDDot_N_flag == true) { n += 1; - MD(n,K-1) = NN2[Q-2]; - MD(n,K) = NN2[Q-1]; - MD(n,K+1) = NN2[Q]; - T1[K-1] = Input.XDDot_N[0] * pow(Ttot,2); - T2[K] = Input.XDDot_N[1] * pow(Ttot,2); - T3[K+1] = Input.XDDot_N[2] * pow(Ttot,2); + MD(n, K - 1) = NN2[Q - 2]; + MD(n, K) = NN2[Q - 1]; + MD(n, K + 1) = NN2[Q]; + T1[K - 1] = Input.XDDot_N[0] * pow(Ttot, 2); + T2[K] = Input.XDDot_N[1] * pow(Ttot, 2); + T3[K + 1] = Input.XDDot_N[2] * pow(Ttot, 2); } // constrain first derivative at ending point if (Input.XDot_N_flag == true) { n += 1; - MD(n,K) = NN1[Q-1]; - MD(n,K+1) = NN1[Q]; + MD(n, K) = NN1[Q - 1]; + MD(n, K + 1) = NN1[Q]; T1[n] = Input.XDot_N[0] * Ttot; T2[n] = Input.XDot_N[1] * Ttot; T3[n] = Input.XDot_N[2] * Ttot; } n += 1; - MD(n,K+1) = NN[Q]; + MD(n, K + 1) = NN[Q]; T1[n] = Input.X1[N]; T2[n] = Input.X2[N]; T3[n] = Input.X3[N]; @@ -542,72 +614,74 @@ void approximate(InputDataSet Input, int Num, int Q, int P, OutputDataSet *Outpu Eigen::VectorXd C3_1 = B * T3; // populate Rk vectors with the base points for LS minimization - Eigen::VectorXd Rk1(N-1), Rk2(N-1), Rk3(N-1); + Eigen::VectorXd Rk1(N - 1), Rk2(N - 1), Rk3(N - 1); for (int n = 1; n < N; n++) { - basisFunction(uk[n], U, Q+1, P, &NN[0], &NN1[0], &NN2[0]); - Rk1[n-1] = Input.X1[n] - NN[0]*C1_1[0] - NN[Q]*C1_1[K+1]; - Rk2[n-1] = Input.X2[n] - NN[0]*C2_1[0] - NN[Q]*C2_1[K+1]; - Rk3[n-1] = Input.X3[n] - NN[0]*C3_1[0] - NN[Q]*C3_1[K+1]; + basisFunction(uk[n], U, Q + 1, P, &NN[0], &NN1[0], &NN2[0]); + Rk1[n - 1] = Input.X1[n] - NN[0] * C1_1[0] - NN[Q] * C1_1[K + 1]; + Rk2[n - 1] = Input.X2[n] - NN[0] * C2_1[0] - NN[Q] * C2_1[K + 1]; + Rk3[n - 1] = Input.X3[n] - NN[0] * C3_1[0] - NN[Q] * C3_1[K + 1]; if (Input.XDot_0_flag == true) { - Rk1[n-1] -= NN[1]*C1_1[1]; - Rk2[n-1] -= NN[1]*C2_1[1]; - Rk3[n-1] -= NN[1]*C3_1[1]; + Rk1[n - 1] -= NN[1] * C1_1[1]; + Rk2[n - 1] -= NN[1] * C2_1[1]; + Rk3[n - 1] -= NN[1] * C3_1[1]; } if (Input.XDDot_0_flag == true) { - Rk1[n-1] -= NN[2]*C1_1[2]; - Rk2[n-1] -= NN[2]*C2_1[2]; - Rk3[n-1] -= NN[2]*C3_1[2]; + Rk1[n - 1] -= NN[2] * C1_1[2]; + Rk2[n - 1] -= NN[2] * C2_1[2]; + Rk3[n - 1] -= NN[2] * C3_1[2]; } if (Input.XDDot_N_flag == true) { - Rk1[n-1] -= NN[Q-2]*C1_1[K-1]; - Rk2[n-1] -= NN[Q-2]*C2_1[K-1]; - Rk3[n-1] -= NN[Q-2]*C3_1[K-1]; + Rk1[n - 1] -= NN[Q - 2] * C1_1[K - 1]; + Rk2[n - 1] -= NN[Q - 2] * C2_1[K - 1]; + Rk3[n - 1] -= NN[Q - 2] * C3_1[K - 1]; } if (Input.XDot_N_flag == true) { - Rk1[n-1] -= NN[Q-1]*C1_1[K]; - Rk2[n-1] -= NN[Q-1]*C2_1[K]; - Rk3[n-1] -= NN[Q-1]*C3_1[K]; + Rk1[n - 1] -= NN[Q - 1] * C1_1[K]; + Rk2[n - 1] -= NN[Q - 1] * C2_1[K]; + Rk3[n - 1] -= NN[Q - 1] * C3_1[K]; } } // populate LS matrix ND - Eigen::MatrixXd ND(N-1,Q-K-1); - for (int n = 0; n < N-1; n++) { - basisFunction(uk[1+n], U, Q+1, P, &NN[0], &NN1[0], &NN2[0]); + Eigen::MatrixXd ND(N - 1, Q - K - 1); + for (int n = 0; n < N - 1; n++) { + basisFunction(uk[1 + n], U, Q + 1, P, &NN[0], &NN1[0], &NN2[0]); int k = 1; - if (Input.XDot_0_flag == true) {k += 1;} - if (Input.XDDot_0_flag == true) {k += 1;} - for (int b = 0; b < Q-K-1; b++) { - ND(n,b) = NN[k+b]; + if (Input.XDot_0_flag == true) { + k += 1; + } + if (Input.XDDot_0_flag == true) { + k += 1; + } + for (int b = 0; b < Q - K - 1; b++) { + ND(n, b) = NN[k + b]; } } // populate weight matrix W - Eigen::MatrixXd W(N-1,N-1); - for (int n = 0; n < N-1; n++) { - for (int m = 0; m < N-1; m++) { + Eigen::MatrixXd W(N - 1, N - 1); + for (int n = 0; n < N - 1; n++) { + for (int m = 0; m < N - 1; m++) { if (n == m) { if (Input.W_flag) { - W(n,m) = Input.W[n+1]; - } - else { - W(n,m) = 1; + W(n, m) = Input.W[n + 1]; + } else { + W(n, m) = 1; } - } - else { - W(n,m) = 0; + } else { + W(n, m) = 0; } } } - Eigen::VectorXd R1(Q-K-1), R2(Q-K-1), R3(Q-K-1); + Eigen::VectorXd R1(Q - K - 1), R2(Q - K - 1), R3(Q - K - 1); B = ND.transpose() * W; R1 = B * Rk1; R2 = B * Rk2; R3 = B * Rk3; // compute LS values R for the control points - Eigen::MatrixXd NWN(Q-K-1,Q-K-1), NWN_inv(Q-K-1,Q-K-1); + Eigen::MatrixXd NWN(Q - K - 1, Q - K - 1), NWN_inv(Q - K - 1, Q - K - 1); NWN = B * ND; NWN_inv = NWN.inverse(); Eigen::VectorXd C1_2 = NWN_inv * R1; @@ -615,30 +689,44 @@ void approximate(InputDataSet Input, int Num, int Q, int P, OutputDataSet *Outpu Eigen::VectorXd C3_2 = NWN_inv * R3; // build control point vectors C - Eigen::VectorXd C1(Q+1), C2(Q+1), C3(Q+1); + Eigen::VectorXd C1(Q + 1), C2(Q + 1), C3(Q + 1); n = 0; - C1[n] = C1_1[n]; C2[n] = C2_1[n]; C3[n] = C3_1[n]; + C1[n] = C1_1[n]; + C2[n] = C2_1[n]; + C3[n] = C3_1[n]; if (Input.XDot_0_flag == true) { n += 1; - C1[n] = C1_1[n]; C2[n] = C2_1[n]; C3[n] = C3_1[n]; + C1[n] = C1_1[n]; + C2[n] = C2_1[n]; + C3[n] = C3_1[n]; } if (Input.XDDot_0_flag == true) { n += 1; - C1[n] = C1_1[n]; C2[n] = C2_1[n]; C3[n] = C3_1[n]; + C1[n] = C1_1[n]; + C2[n] = C2_1[n]; + C3[n] = C3_1[n]; } - for (int q = 0; q < Q-K-1; q++) { - C1[n+q+1] = C1_2[q]; C2[n+q+1] = C2_2[q]; C3[n+q+1] = C3_2[q]; + for (int q = 0; q < Q - K - 1; q++) { + C1[n + q + 1] = C1_2[q]; + C2[n + q + 1] = C2_2[q]; + C3[n + q + 1] = C3_2[q]; } if (Input.XDDot_N_flag == true) { n += 1; - C1[Q-K-1+n] = C1_1[n]; C2[Q-K-1+n] = C2_1[n]; C3[Q-K-1+n] = C3_1[n]; + C1[Q - K - 1 + n] = C1_1[n]; + C2[Q - K - 1 + n] = C2_1[n]; + C3[Q - K - 1 + n] = C3_1[n]; } if (Input.XDot_N_flag == true) { n += 1; - C1[Q-K-1+n] = C1_1[n]; C2[Q-K-1+n] = C2_1[n]; C3[Q-K-1+n] = C3_1[n]; + C1[Q - K - 1 + n] = C1_1[n]; + C2[Q - K - 1 + n] = C2_1[n]; + C3[Q - K - 1 + n] = C3_1[n]; } n += 1; - C1[Q-K-1+n] = C1_1[n]; C2[Q-K-1+n] = C2_1[n]; C3[Q-K-1+n] = C3_1[n]; + C1[Q - K - 1 + n] = C1_1[n]; + C2[Q - K - 1 + n] = C2_1[n]; + C3[Q - K - 1 + n] = C3_1[n]; Output->U = U; Output->C1 = C1; @@ -659,66 +747,68 @@ void approximate(InputDataSet Input, int Num, int Q, int P, OutputDataSet *Outpu Output->XDD2.resize(Num); Output->XDD3.resize(Num); for (int i = 0; i < Num; i++) { - basisFunction(t, U, Q+1, P, &NN[0], &NN1[0], &NN2[0]); + basisFunction(t, U, Q + 1, P, &NN[0], &NN1[0], &NN2[0]); Output->T[i] = t * Ttot; Output->X1[i] = NN.dot(C1); Output->X2[i] = NN.dot(C2); Output->X3[i] = NN.dot(C3); - Output->XD1[i] = NN1.dot(C1) / Ttot; - Output->XD2[i] = NN1.dot(C2) / Ttot; - Output->XD3[i] = NN1.dot(C3) / Ttot; - Output->XDD1[i] = NN2.dot(C1) / pow(Ttot,2); - Output->XDD2[i] = NN2.dot(C2) / pow(Ttot,2); - Output->XDD3[i] = NN2.dot(C3) / pow(Ttot,2); + Output->XD1[i] = NN1.dot(C1) / Ttot; + Output->XD2[i] = NN1.dot(C2) / Ttot; + Output->XD3[i] = NN1.dot(C3) / Ttot; + Output->XDD1[i] = NN2.dot(C1) / pow(Ttot, 2); + Output->XDD2[i] = NN2.dot(C2) / pow(Ttot, 2); + Output->XDD3[i] = NN2.dot(C3) / pow(Ttot, 2); t += dt; } return; } -/*! This function calculates the basis functions NN of order P, and derivatives NN1, NN2, for a given time t and knot vector U */ -void basisFunction(double t, Eigen::VectorXd U, int I, int P, double *NN, double *NN1, double *NN2) +/*! This function calculates the basis functions NN of order P, and derivatives NN1, NN2, for a given time t and knot + * vector U */ +void +basisFunction(double t, Eigen::VectorXd U, int I, int P, double* NN, double* NN1, double* NN2) { - Eigen::MatrixXd N(I, P+1); - Eigen::MatrixXd N1(I, P+1); - Eigen::MatrixXd N2(I, P+1); + Eigen::MatrixXd N(I, P + 1); + Eigen::MatrixXd N1(I, P + 1); + Eigen::MatrixXd N2(I, P + 1); /* populate matrices with zeros */ for (int i = 0; i < I; i++) { - for (int p = 0; p < P+1; p++) { - N(i,p) = 0; - N1(i,p) = 0; - N2(i,p) = 0; + for (int p = 0; p < P + 1; p++) { + N(i, p) = 0; + N1(i, p) = 0; + N2(i, p) = 0; } } /* zero order */ for (int i = 0; i < I; i++) { - if ( (t >= U(i)) && (t < U(i+1)) ) { - N(i,0) = 1; + if ((t >= U(i)) && (t < U(i + 1))) { + N(i, 0) = 1; } } - if (abs(t-1.0) < 1e-5) { - N(I-1,0) = 1; + if (abs(t - 1.0) < 1e-5) { + N(I - 1, 0) = 1; } /* higher order - De Boor formula */ - for (int p = 1; p < P+1; p++) { + for (int p = 1; p < P + 1; p++) { for (int i = 0; i < I; i++) { - if (U[i+p]-U[i] != 0) { - N(i,p) += (t-U[i]) / (U[i+p]-U[i]) * N(i,p-1); - N1(i,p) += p / (U[i+p]-U[i]) * N(i,p-1); - N2(i,p) += p / (U[i+p]-U[i]) * N1(i,p-1); + if (U[i + p] - U[i] != 0) { + N(i, p) += (t - U[i]) / (U[i + p] - U[i]) * N(i, p - 1); + N1(i, p) += p / (U[i + p] - U[i]) * N(i, p - 1); + N2(i, p) += p / (U[i + p] - U[i]) * N1(i, p - 1); } - if (U[i+p+1]-U[i+1] != 0) { - N(i,p) += (U[i+p+1]-t) / (U[i+p+1]-U[i+1]) * N(i+1,p-1); - N1(i,p) -= p / (U[i+p+1]-U[i+1]) * N(i+1,p-1); - N2(i,p) -= p / (U[i+p+1]-U[i+1]) * N1(i+1,p-1); + if (U[i + p + 1] - U[i + 1] != 0) { + N(i, p) += (U[i + p + 1] - t) / (U[i + p + 1] - U[i + 1]) * N(i + 1, p - 1); + N1(i, p) -= p / (U[i + p + 1] - U[i + 1]) * N(i + 1, p - 1); + N2(i, p) -= p / (U[i + p + 1] - U[i + 1]) * N1(i + 1, p - 1); } } } // output result for (int i = 0; i < I; i++) { - *(NN+i) = N(i,P); - *(NN1+i) = N1(i,P); - *(NN2+i) = N2(i,P); + *(NN + i) = N(i, P); + *(NN1 + i) = N1(i, P); + *(NN2 + i) = N2(i, P); } return; diff --git a/src/architecture/utilities/BSpline.h b/src/architecture/utilities/BSpline.h index 6b86e6e394..0e4276760c 100644 --- a/src/architecture/utilities/BSpline.h +++ b/src/architecture/utilities/BSpline.h @@ -17,17 +17,16 @@ */ - #include #include "architecture/utilities/avsEigenSupport.h" #include "architecture/utilities/macroDefinitions.h" - //! @brief The InputDataSet class contains the information about the points that must be interpolated. //! It is used as a data structure to intialize the inputs that are passed to the interpolating function. -class InputDataSet { -public: +class InputDataSet +{ + public: InputDataSet(); InputDataSet(Eigen::VectorXd X1, Eigen::VectorXd X2, Eigen::VectorXd X3); ~InputDataSet(); @@ -39,55 +38,59 @@ 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 - Eigen::VectorXd X1; //!< coordinate #1 of the waypoints - Eigen::VectorXd X2; //!< coordinate #2 of the waypoints - Eigen::VectorXd X3; //!< coordinate #3 of the waypoints - Eigen::Vector3d XDot_0; //!< 3D vector containing the first derivative at starting point - Eigen::Vector3d XDot_N; //!< 3D vector containing the first derivative at final point - Eigen::Vector3d XDDot_0; //!< 3D vector containing the second derivative at starting point - Eigen::Vector3d XDDot_N; //!< 3D vector containing the second derivative at final point - bool T_flag; //!< indicates that time tags have been specified; if true, AvgXDot_flag is false - bool AvgXDot_flag; //!< indicates that avg velocity norm has been specified; if true, T_flag is false - bool W_flag; //!< indicates that weight vector has been specified - bool XDot_0_flag; //!< indicates that first derivative at starting point has been specified - bool XDot_N_flag; //!< indicates that first derivative at final point has been specified - bool XDDot_0_flag; //!< indicates that second derivative at starting point has been specified - bool XDDot_N_flag; //!< indicates that second derivative at final point has been specified + + 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 + Eigen::VectorXd X1; //!< coordinate #1 of the waypoints + Eigen::VectorXd X2; //!< coordinate #2 of the waypoints + Eigen::VectorXd X3; //!< coordinate #3 of the waypoints + Eigen::Vector3d XDot_0; //!< 3D vector containing the first derivative at starting point + Eigen::Vector3d XDot_N; //!< 3D vector containing the first derivative at final point + Eigen::Vector3d XDDot_0; //!< 3D vector containing the second derivative at starting point + Eigen::Vector3d XDDot_N; //!< 3D vector containing the second derivative at final point + bool T_flag; //!< indicates that time tags have been specified; if true, AvgXDot_flag is false + bool AvgXDot_flag; //!< indicates that avg velocity norm has been specified; if true, T_flag is false + bool W_flag; //!< indicates that weight vector has been specified + bool XDot_0_flag; //!< indicates that first derivative at starting point has been specified + bool XDot_N_flag; //!< indicates that first derivative at final point has been specified + bool XDDot_0_flag; //!< indicates that second derivative at starting point has been specified + 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: +class OutputDataSet +{ + public: 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 - Eigen::VectorXd X3; //!< coordinate #3 of the interpolated trajectory - Eigen::VectorXd XD1; //!< first derivative of coordinate #1 of the interpolated trajectory - Eigen::VectorXd XD2; //!< first derivative of coordinate #2 of the interpolated trajectory - Eigen::VectorXd XD3; //!< first derivative of coordinate #3 of the interpolated trajectory - Eigen::VectorXd XDD1; //!< second derivative of coordinate #1 of the interpolated trajectory - Eigen::VectorXd XDD2; //!< second derivative of coordinate #2 of the interpolated trajectory - Eigen::VectorXd XDD3; //!< second derivative of coordinate #3 of the interpolated trajectory + 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 + Eigen::VectorXd X3; //!< coordinate #3 of the interpolated trajectory + Eigen::VectorXd XD1; //!< first derivative of coordinate #1 of the interpolated trajectory + Eigen::VectorXd XD2; //!< first derivative of coordinate #2 of the interpolated trajectory + Eigen::VectorXd XD3; //!< first derivative of coordinate #3 of the interpolated trajectory + Eigen::VectorXd XDD1; //!< second derivative of coordinate #1 of the interpolated trajectory + Eigen::VectorXd XDD2; //!< second derivative of coordinate #2 of the interpolated trajectory + Eigen::VectorXd XDD3; //!< second derivative of coordinate #3 of the interpolated trajectory - int P; //!< polynomial degree of the BSpline - Eigen::VectorXd U; //!< knot vector of the BSpline - Eigen::VectorXd C1; //!< coordinate #1 of the control points - Eigen::VectorXd C2; //!< coordinate #2 of the control points - Eigen::VectorXd C3; //!< coordinate #3 of the control points + int P; //!< polynomial degree of the BSpline + Eigen::VectorXd U; //!< knot vector of the BSpline + Eigen::VectorXd C1; //!< coordinate #1 of the control points + Eigen::VectorXd C2; //!< coordinate #2 of the control points + Eigen::VectorXd C3; //!< coordinate #3 of the control points }; -void interpolate(InputDataSet Input, int Num, int P, OutputDataSet *Output); +void +interpolate(InputDataSet Input, int Num, int P, OutputDataSet* Output); -void approximate(InputDataSet Input, int Num, int Q, 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/astroConstants.h b/src/architecture/utilities/astroConstants.h index 62fba553b0..7e44c4e69d 100644 --- a/src/architecture/utilities/astroConstants.h +++ b/src/architecture/utilities/astroConstants.h @@ -23,43 +23,42 @@ #include #ifndef G_UNIVERSIAL -#define G_UNIVERSIAL 6.67259e-20 /* universial Gravitational Constant, units are in km^3/s^2/kg */ +#define G_UNIVERSIAL 6.67259e-20 /* universial Gravitational Constant, units are in km^3/s^2/kg */ #endif #ifndef AU -#define AU 149597870.693 /* astronomical unit in units of kilometers */ +#define AU 149597870.693 /* astronomical unit in units of kilometers */ #endif #ifndef AU2KM -#define AU2KM 149597870.693 /* convert astronomical unit to kilometers */ +#define AU2KM 149597870.693 /* convert astronomical unit to kilometers */ #endif #ifndef SPEED_LIGHT -#define SPEED_LIGHT 299792458 /* [m/s] convert astronomical unit to kilometers */ +#define SPEED_LIGHT 299792458 /* [m/s] convert astronomical unit to kilometers */ #endif #ifndef SOLAR_FLUX_EARTH -#define SOLAR_FLUX_EARTH 1372.5398 /* [W/m^2] solar flux at earth */ +#define SOLAR_FLUX_EARTH 1372.5398 /* [W/m^2] solar flux at earth */ #endif - /* common conversions */ #ifndef MPI -#define MPI 3.141592653589793 +#define MPI 3.141592653589793 #endif #ifndef MPI_2 -#define MPI_2 1.5707963267948966 +#define MPI_2 1.5707963267948966 #endif #ifndef M_E -#define M_E 2.718281828459045 +#define M_E 2.718281828459045 #endif #ifndef D2R -#define D2R (MPI/180.) +#define D2R (MPI / 180.) #endif #ifndef R2D -#define R2D 180./MPI +#define R2D 180. / MPI #endif #ifndef NAN -#define NAN sqrt((float)-1) +#define NAN sqrt((float)-1) #endif #ifndef RPM -#define RPM (2.*MPI/60.) +#define RPM (2. * MPI / 60.) #endif #ifndef SEC2DAY #define SEC2DAY 1.0 / (60.0 * 60.0 * 24.0); @@ -72,113 +71,113 @@ /* Gravitational Constants mu = G*m, where m is the planet of the attracting planet. All units are km^3/s^2. * Values are obtained from SPICE kernels in http://naif.jpl.nasa.gov/pub/naif/generic_kernels/ */ -#define MU_SUN 132712440023.310 -#define MU_MERCURY 22032.080 -#define MU_VENUS 324858.599 -#define MU_EARTH 398600.436 -#define MU_MOON 4902.799 -#define MU_MARS 42828.314 -#define MU_JUPITER 126712767.881 -#define MU_SATURN 37940626.068 -#define MU_URANUS 5794559.128 -#define MU_NEPTUNE 6836534.065 -#define MU_PLUTO 983.055 +#define MU_SUN 132712440023.310 +#define MU_MERCURY 22032.080 +#define MU_VENUS 324858.599 +#define MU_EARTH 398600.436 +#define MU_MOON 4902.799 +#define MU_MARS 42828.314 +#define MU_JUPITER 126712767.881 +#define MU_SATURN 37940626.068 +#define MU_URANUS 5794559.128 +#define MU_NEPTUNE 6836534.065 +#define MU_PLUTO 983.055 /* planet information for major solar system bodies. Units are in km. * data taken from http://nssdc.gsfc.nasa.gov/planetary/planets.html */ /* Sun */ -#define REQ_SUN 695000. /* km */ +#define REQ_SUN 695000. /* km */ /* Mercury */ -#define REQ_MERCURY 2439.7 /* km */ -#define J2_MERCURY 60.0e-6 -#define SMA_MERCURY 0.38709893*AU -#define I_MERCURY 7.00487*D2R -#define E_MERCURY 0.20563069 -#define OMEGA_MERCURY (2*MPI/1407.6/3600.) /* Mercury's planetary rotation rate, rad/sec */ +#define REQ_MERCURY 2439.7 /* km */ +#define J2_MERCURY 60.0e-6 +#define SMA_MERCURY 0.38709893 * AU +#define I_MERCURY 7.00487 * D2R +#define E_MERCURY 0.20563069 +#define OMEGA_MERCURY (2 * MPI / 1407.6 / 3600.) /* Mercury's planetary rotation rate, rad/sec */ /* Venus */ -#define REQ_VENUS 6051.8 /* km */ -#define J2_VENUS 4.458e-6 -#define SMA_VENUS 0.72333199*AU -#define I_VENUS 3.39471*D2R -#define E_VENUS 0.00677323 -#define OMEGA_VENUS (2*MPI/(-5832.6)/3600.) /* Venus' planetary rotation rate, rad/sec */ +#define REQ_VENUS 6051.8 /* km */ +#define J2_VENUS 4.458e-6 +#define SMA_VENUS 0.72333199 * AU +#define I_VENUS 3.39471 * D2R +#define E_VENUS 0.00677323 +#define OMEGA_VENUS (2 * MPI / (-5832.6) / 3600.) /* Venus' planetary rotation rate, rad/sec */ /* Earth */ -#define REQ_EARTH 6378.1366 /* km, from SPICE */ -#define RP_EARTH 6356.7519 /* km, from SPICE */ -#define J2_EARTH 1082.616e-6 -#define J3_EARTH -2.53881e-6 -#define J4_EARTH -1.65597e-6 -#define J5_EARTH -0.15e-6 -#define J6_EARTH 0.57e-6 -#define SMA_EARTH 1.00000011*AU -#define I_EARTH 0.00005*D2R -#define E_EARTH 0.01671022 -#define OMEGA_EARTH (2*MPI/23.9345/3600.) /* Earth's planetary rotation rate, rad/sec */ +#define REQ_EARTH 6378.1366 /* km, from SPICE */ +#define RP_EARTH 6356.7519 /* km, from SPICE */ +#define J2_EARTH 1082.616e-6 +#define J3_EARTH -2.53881e-6 +#define J4_EARTH -1.65597e-6 +#define J5_EARTH -0.15e-6 +#define J6_EARTH 0.57e-6 +#define SMA_EARTH 1.00000011 * AU +#define I_EARTH 0.00005 * D2R +#define E_EARTH 0.01671022 +#define OMEGA_EARTH (2 * MPI / 23.9345 / 3600.) /* Earth's planetary rotation rate, rad/sec */ /* Moon */ -#define REQ_MOON 1737.4 -#define J2_MOON 202.7e-6 -#define SMA_MOON 0.3844e6 -#define E_MOON 0.0549 -#define OMEGA_MOON (2*MPI/655.728/3600.) /* Moon's rotation rate, rad/sec */ +#define REQ_MOON 1737.4 +#define J2_MOON 202.7e-6 +#define SMA_MOON 0.3844e6 +#define E_MOON 0.0549 +#define OMEGA_MOON (2 * MPI / 655.728 / 3600.) /* Moon's rotation rate, rad/sec */ /* Mars */ -#define REQ_MARS 3396.19 /* km */ -#define RP_MARS 3376.2 /* km */ -#define J2_MARS 1960.45e-6 -#define SMA_MARS 1.52366231*AU -#define I_MARS 1.85061*D2R -#define E_MARS 0.09341233 -#define OMEGA_MARS (2*MPI/24.6229/3600.) /* Mars' polar rotation rate, rad/sec */ +#define REQ_MARS 3396.19 /* km */ +#define RP_MARS 3376.2 /* km */ +#define J2_MARS 1960.45e-6 +#define SMA_MARS 1.52366231 * AU +#define I_MARS 1.85061 * D2R +#define E_MARS 0.09341233 +#define OMEGA_MARS (2 * MPI / 24.6229 / 3600.) /* Mars' polar rotation rate, rad/sec */ /* Phobos */ -#define REQ_PHOBOS 11.2 /* km */ +#define REQ_PHOBOS 11.2 /* km */ /* Deimos */ -#define REQ_DEIMOS 6.1 /* km */ +#define REQ_DEIMOS 6.1 /* km */ /* Jupiter */ -#define REQ_JUPITER 71492. -#define J2_JUPITER 14736.e-6 -#define SMA_JUPITER 5.20336301*AU -#define I_JUPITER 1.30530*D2R -#define E_JUPITER 0.04839266 -#define OMEGA_JUPITER (2*MPI/9.9250/3600.) /* Jupiter's polar rotation rate, rad/sec */ +#define REQ_JUPITER 71492. +#define J2_JUPITER 14736.e-6 +#define SMA_JUPITER 5.20336301 * AU +#define I_JUPITER 1.30530 * D2R +#define E_JUPITER 0.04839266 +#define OMEGA_JUPITER (2 * MPI / 9.9250 / 3600.) /* Jupiter's polar rotation rate, rad/sec */ /* Saturn */ -#define REQ_SATURN 60268. -#define J2_SATURN 16298.e-6 -#define SMA_SATURN 9.53707032*AU -#define I_SATURN 2.48446*D2R -#define E_SATURN 0.05415060 -#define OMEGA_SATURN (2*MPI/10.656/3600.) /* Saturn's polar rotation rate, rad/sec */ +#define REQ_SATURN 60268. +#define J2_SATURN 16298.e-6 +#define SMA_SATURN 9.53707032 * AU +#define I_SATURN 2.48446 * D2R +#define E_SATURN 0.05415060 +#define OMEGA_SATURN (2 * MPI / 10.656 / 3600.) /* Saturn's polar rotation rate, rad/sec */ /* Uranus */ -#define REQ_URANUS 25559. -#define J2_URANUS 3343.43e-6 -#define SMA_URANUS 19.19126393*AU -#define I_URANUS 0.76986*D2R -#define E_URANUS 0.04716771 -#define OMEGA_URANUS (2*MPI/(-17.24)/3600.) /* Uranus' polar rotation rate, rad/sec */ +#define REQ_URANUS 25559. +#define J2_URANUS 3343.43e-6 +#define SMA_URANUS 19.19126393 * AU +#define I_URANUS 0.76986 * D2R +#define E_URANUS 0.04716771 +#define OMEGA_URANUS (2 * MPI / (-17.24) / 3600.) /* Uranus' polar rotation rate, rad/sec */ /* Neptune */ -#define REQ_NEPTUNE 24746. -#define J2_NEPTUNE 3411.e-6 -#define SMA_NEPTUNE 30.06896348*AU -#define I_NEPTUNE 1.76917*D2R -#define E_NEPTUNE 0.00858587 -#define OMEGA_NEPTUNE (2*MPI/16.11/3600.) /* Neptune's polar rotation rate, rad/sec */ +#define REQ_NEPTUNE 24746. +#define J2_NEPTUNE 3411.e-6 +#define SMA_NEPTUNE 30.06896348 * AU +#define I_NEPTUNE 1.76917 * D2R +#define E_NEPTUNE 0.00858587 +#define OMEGA_NEPTUNE (2 * MPI / 16.11 / 3600.) /* Neptune's polar rotation rate, rad/sec */ /* Pluto */ -#define REQ_PLUTO 1137. -#define SMA_PLUTO 39.48168677*AU -#define I_PLUTO 17.14175*D2R -#define E_PLUTO 0.24880766 -#define OMEGA_PLUTO (2*MPI/(-153.2928)/3600.) /* Pluto's polar rotation rate, rad/sec */ +#define REQ_PLUTO 1137. +#define SMA_PLUTO 39.48168677 * AU +#define I_PLUTO 17.14175 * D2R +#define E_PLUTO 0.24880766 +#define OMEGA_PLUTO (2 * MPI / (-153.2928) / 3600.) /* Pluto's polar rotation rate, rad/sec */ #endif diff --git a/src/architecture/utilities/avsEigenMRP.h b/src/architecture/utilities/avsEigenMRP.h index 32aa0f1280..aa8f682218 100644 --- a/src/architecture/utilities/avsEigenMRP.h +++ b/src/architecture/utilities/avsEigenMRP.h @@ -19,823 +19,876 @@ /// \cond DO_NOT_DOCUMENT - #ifndef EIGEN_MRP_H #define EIGEN_MRP_H -//#include -//#include -//#include -//#include -//#include "Eigen/src/Geometry/Quaternion.h" +// #include +// #include +// #include +// #include +// #include "Eigen/src/Geometry/Quaternion.h" namespace Eigen { - template class MRP; +template +class MRP; + +/*************************************************************************** + * Definition of MRPBase + * The implementation is at the end of the file + ***************************************************************************/ + +namespace internal { +template +struct MRPbase_assign_impl; +} + +/** + * \class MRPBase + * \brief Base class for MRP expressions + * \tparam Derived derived type (CRTP) + * \sa class MRP + */ +template +class MRPBase : public RotationBase +{ + typedef RotationBase Base; + + public: + using Base::operator*; + using Base::derived; + + typedef typename internal::traits::Scalar Scalar; //!< variable + typedef typename NumTraits::Real RealScalar; //!< variable + typedef typename internal::traits::Coefficients Coefficients; //!< variable + enum + { + Flags = Eigen::internal::traits::Flags + }; + // typedef typename Matrix Coefficients; + /** the type of a 3D vector */ + typedef Matrix Vector3; + /** the equivalent rotation matrix type */ + typedef Matrix Matrix3; + /** the equivalent angle-axis type */ + typedef AngleAxis AngleAxisType; + + /** default constructor */ + MRPBase() = default; + /** copy constructor */ + MRPBase(const MRPBase& /*rhs*/) = default; + + /** \returns the \c x coefficient */ + inline Scalar x() const { return this->derived().coeffs().coeff(0); } + /** \returns the \c y coefficient */ + inline Scalar y() const { return this->derived().coeffs().coeff(1); } + /** \returns the \c z coefficient */ + inline Scalar z() const { return this->derived().coeffs().coeff(2); } + + /** \returns a reference to the \c x coefficient */ + inline Scalar& x() { return this->derived().coeffs().coeffRef(0); } + /** \returns a reference to the \c y coefficient */ + inline Scalar& y() { return this->derived().coeffs().coeffRef(1); } + /** \returns a reference to the \c z coefficient */ + inline Scalar& z() { return this->derived().coeffs().coeffRef(2); } + + /** \returns a read-only vector expression of the imaginary part (x,y,z) */ + inline const VectorBlock vec() const { return coeffs().template head<3>(); } + + /** \returns a vector expression of the imaginary part (x,y,z) */ + inline VectorBlock vec() { return coeffs().template head<3>(); } + + /** \returns a read-only vector expression of the coefficients (x,y,z) */ + inline const typename internal::traits::Coefficients& coeffs() const { return derived().coeffs(); } + + /** \returns a vector expression of the coefficients (x,y,z) */ + inline typename internal::traits::Coefficients& coeffs() { return derived().coeffs(); } + + EIGEN_STRONG_INLINE MRPBase& operator=(const MRPBase& other); //!< method + template + EIGEN_STRONG_INLINE Derived& operator=(const MRPBase& other); //!< method - - /*************************************************************************** - * Definition of MRPBase - * The implementation is at the end of the file - ***************************************************************************/ + // disabled this copy operator as it is giving very strange compilation errors when compiling + // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's + // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase + // we didn't have to add, in addition to templated operator=, such a non-templated copy operator. + // Derived& operator=(const MRPBase& other) + // { return operator=(other); } - namespace internal { - template - struct MRPbase_assign_impl; - } + Derived& operator=(const AngleAxisType& aa); + template + Derived& operator=(const MatrixBase& m); //!< method - /** - * \class MRPBase - * \brief Base class for MRP expressions - * \tparam Derived derived type (CRTP) - * \sa class MRP + /** \returns a MRP representing an identity mapping or zero rotation + * \sa MatrixBase::Identity() */ - template - class MRPBase : public RotationBase - { - typedef RotationBase Base; - public: - using Base::operator*; - using Base::derived; - - typedef typename internal::traits::Scalar Scalar; //!< variable - typedef typename NumTraits::Real RealScalar; //!< variable - typedef typename internal::traits::Coefficients Coefficients; //!< variable - enum { - Flags = Eigen::internal::traits::Flags - }; - - // typedef typename Matrix Coefficients; - /** the type of a 3D vector */ - typedef Matrix Vector3; - /** the equivalent rotation matrix type */ - typedef Matrix Matrix3; - /** the equivalent angle-axis type */ - typedef AngleAxis AngleAxisType; - - /** default constructor */ - MRPBase() = default; - /** copy constructor */ - MRPBase(const MRPBase &/*rhs*/) = default; - - /** \returns the \c x coefficient */ - inline Scalar x() const { return this->derived().coeffs().coeff(0); } - /** \returns the \c y coefficient */ - inline Scalar y() const { return this->derived().coeffs().coeff(1); } - /** \returns the \c z coefficient */ - inline Scalar z() const { return this->derived().coeffs().coeff(2); } - - /** \returns a reference to the \c x coefficient */ - inline Scalar& x() { return this->derived().coeffs().coeffRef(0); } - /** \returns a reference to the \c y coefficient */ - inline Scalar& y() { return this->derived().coeffs().coeffRef(1); } - /** \returns a reference to the \c z coefficient */ - inline Scalar& z() { return this->derived().coeffs().coeffRef(2); } - - /** \returns a read-only vector expression of the imaginary part (x,y,z) */ - inline const VectorBlock vec() const { return coeffs().template head<3>(); } - - /** \returns a vector expression of the imaginary part (x,y,z) */ - inline VectorBlock vec() { return coeffs().template head<3>(); } - - /** \returns a read-only vector expression of the coefficients (x,y,z) */ - inline const typename internal::traits::Coefficients& coeffs() const { return derived().coeffs(); } - - /** \returns a vector expression of the coefficients (x,y,z) */ - inline typename internal::traits::Coefficients& coeffs() { return derived().coeffs(); } - - EIGEN_STRONG_INLINE MRPBase& operator=(const MRPBase& other); //!< method - template EIGEN_STRONG_INLINE Derived& operator=(const MRPBase& other); //!< method - - // disabled this copy operator as it is giving very strange compilation errors when compiling - // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's - // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase - // we didn't have to add, in addition to templated operator=, such a non-templated copy operator. - // Derived& operator=(const MRPBase& other) - // { return operator=(other); } - - Derived& operator=(const AngleAxisType& aa); - template Derived& operator=(const MatrixBase& m); //!< method - - /** \returns a MRP representing an identity mapping or zero rotation - * \sa MatrixBase::Identity() - */ - static inline MRP Identity() { return MRP(Scalar(0), Scalar(0), Scalar(0)); } - - /** \sa MRPBase::Identity(), MatrixBase::setIdentity() - */ - inline MRPBase& setIdentity() { coeffs() << Scalar(0), Scalar(0), Scalar(0); return *this; } - - /** \returns the squared norm of the MRP's coefficients - * \sa MRPBase::norm(), MatrixBase::squaredNorm() - */ - inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } - - /** \returns the norm of the MRP's coefficients - * \sa MRPBase::squaredNorm(), MatrixBase::norm() - */ - inline Scalar norm() const { return coeffs().norm(); } - - /** Normalizes the MRP \c *this - * \sa normalized(), MatrixBase::normalize() */ - inline void normalize() { coeffs().normalize(); } - /** \returns a normalized copy of \c *this - * \sa normalize(), MatrixBase::normalized() */ - inline MRP normalized() const { return MRP(coeffs().normalized()); } - - /** \returns the dot product of \c *this and \a other - * Geometrically speaking, the dot product of two unit MRPs - * corresponds to the cosine of half the angle between the two rotations. - * \sa angularDistance() - */ - template inline Scalar dot(const MRPBase& other) const { return coeffs().dot(other.coeffs()); } - - template Scalar angularDistance(const MRPBase& other) const; //!< method - - /** \returns an equivalent 3x3 rotation matrix */ - Matrix3 toRotationMatrix() const; - - /** \returns the MRP which transform \a a into \a b through a rotation */ - template - Derived& setFromTwoVectors(const MatrixBase& a, const MatrixBase& b); - - template EIGEN_STRONG_INLINE MRP operator* (const MRPBase& q) const; //!< method - template EIGEN_STRONG_INLINE Derived& operator*= (const MRPBase& q); - template EIGEN_STRONG_INLINE Derived& operator+= (const MRPBase& q); - template EIGEN_STRONG_INLINE Derived& operator-= (const MRPBase& q); - - /** \returns the MRP describing the shadow set */ - MRP shadow() const; - - /** \returns 3x3 [B] matrix for the MRP differential kinematic equations */ - Matrix3 Bmat() const; - - /** \returns the MRP describing the inverse rotation */ - MRP inverse() const; - - /** \returns the conjugated MRP */ - MRP conjugate() const; - - // template MRP slerp(const Scalar& t, const MRPBase& other) const; - - /** \returns \c true if \c *this is approximately equal to \a other, within the precision - * determined by \a prec. - * - * \sa MatrixBase::isApprox() */ - template - bool isApprox(const MRPBase& other, const RealScalar& prec = NumTraits::dummy_precision()) const - { return coeffs().isApprox(other.coeffs(), prec); } - - /** return the result vector of \a v through the rotation*/ - EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; - - /** \returns \c *this with scalar type casted to \a NewScalarType - * - * Note that if \a NewScalarType is equal to the current scalar type of \c *this - * then this function smartly returns a const reference to \c *this. - */ - template - inline typename internal::cast_return_type >::type cast() const - { - return typename internal::cast_return_type >::type(derived()); - } - - }; - - /*************************************************************************** - * Definition/implementation of MRP - ***************************************************************************/ + static inline MRP Identity() { return MRP(Scalar(0), Scalar(0), Scalar(0)); } - /** - * - * \class MRP - * - * \brief The MRP class used to represent 3D orientations and rotations - * - * \tparam _Scalar the scalar type, i.e., the type of the coefficients - * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. - * - * This class represents a MRP \f$ (x,y,z) \f$ that is a convenient representation of - * orientations and rotations of objects in three dimensions. Compared to other representations - * like Euler angles or 3x3 matrices, MRPs offer the following advantages: - * \li \b compact storage (3 scalars) - * \li \b efficient to compose (28 flops), - * - * The following two typedefs are provided for convenience: - * \li \c MRPf for \c float - * \li \c MRPd for \c double - * - * \warning Operations interpreting the MRP as rotation have undefined behavior if the MRP is not normalized. - * - * \sa class AngleAxis, class Transform + /** \sa MRPBase::Identity(), MatrixBase::setIdentity() */ - - namespace internal { - template - /*! structure definition */ - struct traits > - { - /** struct definition */ - typedef MRP<_Scalar,_Options> PlainObject; - /** struct definition */ - typedef _Scalar Scalar; - /** struct definition */ - typedef Matrix<_Scalar,3,1,_Options> Coefficients; - enum{ - IsAligned = internal::traits::Flags & PacketAccessBit, - Flags = IsAligned ? (PacketAccessBit | LvalueBit) : LvalueBit - }; - }; - } - - template - class MRP : public MRPBase > + inline MRPBase& setIdentity() { - typedef MRPBase > Base; - enum { IsAligned = internal::traits::IsAligned }; - - public: - typedef _Scalar Scalar; //!< variable - - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MRP) - using Base::operator*=; - - typedef typename internal::traits::Coefficients Coefficients; //!< variable - typedef typename Base::AngleAxisType AngleAxisType; //!< variable - - /** Default constructor leaving the MRP uninitialized. */ - inline MRP() {} + coeffs() << Scalar(0), Scalar(0), Scalar(0); + return *this; + } - /** Constructs and initializes the MRP \f$ (x,y,z) \f$ from - * its four coefficients \a x, \a y and \a z. - */ - inline MRP(const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z){} + /** \returns the squared norm of the MRP's coefficients + * \sa MRPBase::norm(), MatrixBase::squaredNorm() + */ + inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } - /** Constructs and initialize a MRP from the array data */ - inline MRP(const Scalar* data) : m_coeffs(data) {} + /** \returns the norm of the MRP's coefficients + * \sa MRPBase::squaredNorm(), MatrixBase::norm() + */ + inline Scalar norm() const { return coeffs().norm(); } + + /** Normalizes the MRP \c *this + * \sa normalized(), MatrixBase::normalize() */ + inline void normalize() { coeffs().normalize(); } + /** \returns a normalized copy of \c *this + * \sa normalize(), MatrixBase::normalized() */ + inline MRP normalized() const { return MRP(coeffs().normalized()); } + + /** \returns the dot product of \c *this and \a other + * Geometrically speaking, the dot product of two unit MRPs + * corresponds to the cosine of half the angle between the two rotations. + * \sa angularDistance() + */ + template + inline Scalar dot(const MRPBase& other) const + { + return coeffs().dot(other.coeffs()); + } - /** Copy constructor */ - template EIGEN_STRONG_INLINE MRP(const MRPBase& other) { this->Base::operator=(other); } + template + Scalar angularDistance(const MRPBase& other) const; //!< method - /** Constructs and initializes a MRP from the angle-axis \a aa */ - explicit inline MRP(const AngleAxisType& aa) { *this = aa; } + /** \returns an equivalent 3x3 rotation matrix */ + Matrix3 toRotationMatrix() const; - /** Constructs and initializes a MRP from either: - * - a rotation matrix expression, - * - a 3D vector expression representing MRP coefficients. - */ - template - explicit inline MRP(const MatrixBase& other) { *this = other; } + /** \returns the MRP which transform \a a into \a b through a rotation */ + template + Derived& setFromTwoVectors(const MatrixBase& a, const MatrixBase& b); - /** Explicit copy constructor with scalar conversion */ - // template - // explicit inline MRP(const MRP& other) - // { m_coeffs = other.coeffs().template cast(); } + template + EIGEN_STRONG_INLINE MRP operator*(const MRPBase& q) const; //!< method + template + EIGEN_STRONG_INLINE Derived& operator*=(const MRPBase& q); + template + EIGEN_STRONG_INLINE Derived& operator+=(const MRPBase& q); + template + EIGEN_STRONG_INLINE Derived& operator-=(const MRPBase& q); - template - static MRP FromTwoVectors(const MatrixBase& a, const MatrixBase& b); + /** \returns the MRP describing the shadow set */ + MRP shadow() const; - inline Coefficients& coeffs() { return m_coeffs;} //!< method - inline const Coefficients& coeffs() const { return m_coeffs;} //!< method + /** \returns 3x3 [B] matrix for the MRP differential kinematic equations */ + Matrix3 Bmat() const; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(IsAligned)) + /** \returns the MRP describing the inverse rotation */ + MRP inverse() const; - protected: - Coefficients m_coeffs; //!< variable + /** \returns the conjugated MRP */ + MRP conjugate() const; -#ifndef EIGEN_PARSED_BY_DOXYGEN - /** - Check template parameters - */ - static EIGEN_STRONG_INLINE void _check_template_params() //!< method - { - EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options, - INVALID_MATRIX_TEMPLATE_PARAMETERS) - } -#endif - }; + // template MRP slerp(const Scalar& t, const MRPBase& other) const; - /** - * single precision MRP type */ - typedef MRP MRPf; - /** - * double precision MRP type */ - typedef MRP MRPd; - - /*************************************************************************** - * Specialization of Map> - ***************************************************************************/ - - namespace internal { - /** struct definition */ - template - /** struct definition */ - struct traits, _Options> > : traits > - { - /** struct definition */ - typedef Map, _Options> Coefficients; - }; + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + template + bool isApprox(const MRPBase& other, + const RealScalar& prec = NumTraits::dummy_precision()) const + { + return coeffs().isApprox(other.coeffs(), prec); } - namespace internal { - template - /** struct definition */ - struct traits, _Options> > : traits > - { - /** struct definition */ - typedef Map, _Options> Coefficients; - /** struct definition */ - typedef traits > TraitsBase; - enum { - Flags = TraitsBase::Flags & ~LvalueBit - }; - }; - } + /** return the result vector of \a v through the rotation*/ + EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; - /** - * \brief MRP expression mapping a constant memory buffer + /** \returns \c *this with scalar type casted to \a NewScalarType * - * \tparam _Scalar the type of the MRP coefficients - * \tparam _Options see class Map - * - * This is a specialization of class Map for MRP. This class allows to view - * a 3 scalar memory buffer as an Eigen's MRP object. - * - * \sa class Map, class MRP, class MRPBase + * Note that if \a NewScalarType is equal to the current scalar type of \c *this + * then this function smartly returns a const reference to \c *this. */ - template - class Map, _Options > - : public MRPBase, _Options> > + template + inline typename internal::cast_return_type>::type cast() const { - typedef MRPBase, _Options> > Base; - - public: - typedef _Scalar Scalar; //!< variable - typedef typename internal::traits::Coefficients Coefficients; //!< variable - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) - using Base::operator*=; - - /** Constructs a Mapped MRP object from the pointer \a coeffs - * - * The pointer \a coeffs must reference the three coefficients of MRP in the following order: - * \code *coeffs == {x, y, z} \endcode - * - * If the template parameter _Options is set to Aligned, then the pointer coeffs must be aligned. */ - EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {} - - inline const Coefficients& coeffs() const { return m_coeffs;} //!< method - - protected: - const Coefficients m_coeffs; //!< variable - }; + return typename internal::cast_return_type>::type(derived()); + } +}; + +/*************************************************************************** + * Definition/implementation of MRP + ***************************************************************************/ + +/** + * + * \class MRP + * + * \brief The MRP class used to represent 3D orientations and rotations + * + * \tparam _Scalar the scalar type, i.e., the type of the coefficients + * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is + * AutoAlign. + * + * This class represents a MRP \f$ (x,y,z) \f$ that is a convenient representation of + * orientations and rotations of objects in three dimensions. Compared to other representations + * like Euler angles or 3x3 matrices, MRPs offer the following advantages: + * \li \b compact storage (3 scalars) + * \li \b efficient to compose (28 flops), + * + * The following two typedefs are provided for convenience: + * \li \c MRPf for \c float + * \li \c MRPd for \c double + * + * \warning Operations interpreting the MRP as rotation have undefined behavior if the MRP is not normalized. + * + * \sa class AngleAxis, class Transform + */ - /** - * \brief Expression of a MRP from a memory buffer - * - * \tparam _Scalar the type of the MRP coefficients - * \tparam _Options see class Map - * - * This is a specialization of class Map for MRP. This class allows to view - * a 3 scalar memory buffer as an Eigen's MRP object. - * - * \sa class Map, class MRP, class MRPBase - */ - template - class Map, _Options > - : public MRPBase, _Options> > +namespace internal { +template +/*! structure definition */ +struct traits> +{ + /** struct definition */ + typedef MRP<_Scalar, _Options> PlainObject; + /** struct definition */ + typedef _Scalar Scalar; + /** struct definition */ + typedef Matrix<_Scalar, 3, 1, _Options> Coefficients; + enum { - typedef MRPBase, _Options> > Base; - - public: - typedef _Scalar Scalar; //!< variable - typedef typename internal::traits::Coefficients Coefficients; //!< variable - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) - using Base::operator*=; - - /** Constructs a Mapped MRP object from the pointer \a coeffs - * - * The pointer \a coeffs must reference the three coefficients of MRP in the following order: - * \code *coeffs == {x, y, z} \endcode - * - * If the template parameter _Options is set to Aligned, then the pointer coeffs must be aligned. */ - EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {} - - inline Coefficients& coeffs() { return m_coeffs; } //!< method - inline const Coefficients& coeffs() const { return m_coeffs; } //!< method - - protected: - Coefficients m_coeffs; //!< variable + IsAligned = internal::traits::Flags & PacketAccessBit, + Flags = IsAligned ? (PacketAccessBit | LvalueBit) : LvalueBit }; - - /** - * Map an unaligned array of single precision scalars as a MRP */ - typedef Map, 0> MRPMapf; - /** - * Map an unaligned array of double precision scalars as a MRP */ - typedef Map, 0> MRPMapd; - /** - * Map a 16-byte aligned array of single precision scalars as a MRP */ - typedef Map, Aligned> MRPMapAlignedf; - /** - * Map a 16-byte aligned array of double precision scalars as a MRP */ - typedef Map, Aligned> MRPMapAlignedd; - - /*************************************************************************** - * Implementation of MRPBase methods - ***************************************************************************/ - - // Generic MRP * MRP product - // This product can be specialized for a given architecture via the Arch template argument. - namespace internal { - /*! template definition */ - template struct mrp_product - { - static EIGEN_STRONG_INLINE MRP run(const MRPBase& a, const MRPBase& b){ - Scalar det; //!< variable - Scalar s1N2; //!< variable - Scalar s2N2; //!< variable - MRP s2 = b; - MRP answer; - s1N2 = a.squaredNorm(); - s2N2 = s2.squaredNorm(); - det = Scalar(1.0) + s1N2*s2N2 - 2*a.dot(b); - if (det < 0.01) { - s2 = s2.shadow(); - s2N2 = s2.squaredNorm(); - det = Scalar(1.0) + s1N2*s2N2 - 2*a.dot(b); - } - answer = MRP (((1-s1N2)*s2.vec() + (1-s2N2)*a.vec() - 2*b.vec().cross(a.vec()))/det); - if (answer.squaredNorm() > 1) - answer = answer.shadow(); - return answer; - } - }; - } - - /** \returns the concatenation of two rotations as a MRP-MRP product */ - template - template - EIGEN_STRONG_INLINE MRP::Scalar> - MRPBase::operator* (const MRPBase& other) const +}; +} + +template +class MRP : public MRPBase> +{ + typedef MRPBase> Base; + enum { - EIGEN_STATIC_ASSERT((internal::is_same::value), - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - return internal::mrp_product::Scalar, - internal::traits::IsAligned && internal::traits::IsAligned>::run(*this, other); - } + IsAligned = internal::traits::IsAligned + }; - /** \sa operator*(MRP) */ - template - template - EIGEN_STRONG_INLINE Derived& MRPBase::operator*= (const MRPBase& other) - { - derived() = derived() * other.derived(); - return derived(); - } + public: + typedef _Scalar Scalar; //!< variable - /** \sa operator*(MRP) */ - template - template - EIGEN_STRONG_INLINE Derived& MRPBase::operator+= (const MRPBase& other) - { - derived().coeffs() = derived().coeffs() + other.derived().coeffs(); - return derived(); - } - /** \sa operator*(MRP) */ - template - template - EIGEN_STRONG_INLINE Derived& MRPBase::operator-= (const MRPBase& other) - { - derived().coeffs() = derived().coeffs() - other.derived().coeffs(); - return derived(); - } + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MRP) + using Base::operator*=; + + typedef typename internal::traits::Coefficients Coefficients; //!< variable + typedef typename Base::AngleAxisType AngleAxisType; //!< variable + /** Default constructor leaving the MRP uninitialized. */ + inline MRP() {} - /** Rotation of a vector by a MRP. - * \remarks If the MRP is used to rotate several points (>1) - * then it is much more efficient to first convert it to a 3x3 Matrix. - * Comparison of the operation cost for n transformations: - * - MRP2: 30n - * - Via a Matrix3: 24 + 15n + /** Constructs and initializes the MRP \f$ (x,y,z) \f$ from + * its four coefficients \a x, \a y and \a z. */ - template - EIGEN_STRONG_INLINE typename MRPBase::Vector3 - MRPBase::_transformVector(const Vector3& v) const + inline MRP(const Scalar& x, const Scalar& y, const Scalar& z) + : m_coeffs(x, y, z) { - // Note that this algorithm comes from the optimization by hand - // of the conversion to a Matrix followed by a Matrix/Vector product. - // It appears to be much faster than the common algorithm found - // in the literature (30 versus 39 flops). It also requires two - // Vector3 as temporaries. - Vector3 uv = this->vec().cross(v); - uv += uv; - return v + this->w() * uv + this->vec().cross(uv); } - template - EIGEN_STRONG_INLINE MRPBase& MRPBase::operator=(const MRPBase& other) + /** Constructs and initialize a MRP from the array data */ + inline MRP(const Scalar* data) + : m_coeffs(data) { - coeffs() = other.coeffs(); - return derived(); } + /** Copy constructor */ template - template - EIGEN_STRONG_INLINE Derived& MRPBase::operator=(const MRPBase& other) + EIGEN_STRONG_INLINE MRP(const MRPBase& other) { - coeffs() = other.coeffs(); - return derived(); + this->Base::operator=(other); } - /** Set \c *this from an angle-axis \a aa and returns a reference to \c *this - */ - template - EIGEN_STRONG_INLINE Derived& MRPBase::operator=(const AngleAxisType& aa) - { - using std::tan; - Scalar tanPhi = Scalar(0.25)*aa.angle(); // Scalar(0.25) to suppress precision loss warnings - this->vec() = tanPhi * aa.axis(); - return derived(); - } + /** Constructs and initializes a MRP from the angle-axis \a aa */ + explicit inline MRP(const AngleAxisType& aa) { *this = aa; } - /** Set \c *this from the expression \a xpr: - * - if \a xpr is a 3x1 vector, then \a xpr is assumed to be a MRP - * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix - * and \a xpr is converted to a MRP + /** Constructs and initializes a MRP from either: + * - a rotation matrix expression, + * - a 3D vector expression representing MRP coefficients. */ - - template - template - inline Derived& MRPBase::operator=(const MatrixBase& xpr) + template + explicit inline MRP(const MatrixBase& other) { - EIGEN_STATIC_ASSERT((internal::is_same::value), - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - internal::MRPbase_assign_impl::run(*this, xpr.derived()); - return derived(); + *this = other; } - /** Convert the MRP sigma_B/N to a 3x3 rotation matrix [NB]. - */ - template - inline typename MRPBase::Matrix3 - MRPBase::toRotationMatrix(void) const - { - // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) - // if not inlined then the cost of the return by value is huge ~ +35%, - // however, not inlining this function is an order of magnitude slower, so - // it has to be inlined, and so the return by value is not an issue - Matrix3 res; - - const Scalar ps2 = Scalar(1) + this->coeffs().squaredNorm(); - const Scalar ms2 = Scalar(1) - this->coeffs().squaredNorm(); - const Scalar ms2Sq = ms2 * ms2; - const Scalar s1s2 = Scalar(8)*this->x()*this->y(); - const Scalar s1s3 = Scalar(8)*this->x()*this->z(); - const Scalar s2s3 = Scalar(8)*this->y()*this->z(); - const Scalar s1Sq = this->x()*this->x(); - const Scalar s2Sq = this->y()*this->y(); - const Scalar s3Sq = this->z()*this->z(); - - res.coeffRef(0,0) = Scalar(4)*(+s1Sq - s2Sq - s3Sq)+ms2Sq; - res.coeffRef(0,1) = s1s2 - Scalar(4)*this->z()*ms2; - res.coeffRef(0,2) = s1s3 + Scalar(4)*this->y()*ms2; - res.coeffRef(1,0) = s1s2 + Scalar(4)*this->z()*ms2; - res.coeffRef(1,1) = Scalar(4)*(-s1Sq + s2Sq - s3Sq)+ms2Sq; - res.coeffRef(1,2) = s2s3 - Scalar(4)*this->x()*ms2; - res.coeffRef(2,0) = s1s3 - Scalar(4)*this->y()*ms2; - 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; - } + /** Explicit copy constructor with scalar conversion */ + // template + // explicit inline MRP(const MRP& other) + // { m_coeffs = other.coeffs().template cast(); } - /** Sets \c *this to be a MRP representing a rotation between - * the two arbitrary vectors \a a and \a b. In other words, the built - * rotation represent a rotation sending the line of direction \a a - * to the line of direction \a b, both lines passing through the origin. - * - * \returns a reference to \c *this. - * - * Note that the two input vectors do \b not have to be normalized, and - * do not need to have the same norm. - */ - template template - inline Derived& MRPBase::setFromTwoVectors(const MatrixBase& a, const MatrixBase& b) - { - using std::acos; - using std::tan; + static MRP FromTwoVectors(const MatrixBase& a, const MatrixBase& b); - Vector3 v0 = a.normalized(); - Vector3 v1 = b.normalized(); - Scalar c = v1.dot(v0); + inline Coefficients& coeffs() { return m_coeffs; } //!< method + inline const Coefficients& coeffs() const { return m_coeffs; } //!< method - if (c <= 1 && c >= -1) { - Vector3 axis = v0.cross(v1); - axis.normalize(); - - this->vec() = axis*tan(acos(c)/Scalar(4.0)); - } else { - this->vec() << 0., 0., 0.; - } - return derived(); - } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(IsAligned)) + protected: + Coefficients m_coeffs; //!< variable - /** Returns a MRP representing a rotation between - * the two arbitrary vectors \a a and \a b. In other words, the built - * rotation represent a rotation sending the line of direction \a a - * to the line of direction \a b, both lines passing through the origin. - * - * \returns resulting MRP - * - * Note that the two input vectors do \b not have to be normalized, and - * do not need to have the same norm. +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** + Check template parameters */ - template - template - MRP MRP::FromTwoVectors(const MatrixBase& a, const MatrixBase& b) + static EIGEN_STRONG_INLINE void _check_template_params() //!< method { - MRP sig; - sig.setFromTwoVectors(a, b); - return sig; + EIGEN_STATIC_ASSERT((_Options & DontAlign) == _Options, INVALID_MATRIX_TEMPLATE_PARAMETERS) } - - /** \returns the MRP Shadow set of \c *this +#endif +}; + +/** + * single precision MRP type */ +typedef MRP MRPf; +/** + * double precision MRP type */ +typedef MRP MRPd; + +/*************************************************************************** + * Specialization of Map> + ***************************************************************************/ + +namespace internal { +/** struct definition */ +template +/** struct definition */ +struct traits, _Options>> + : traits> +{ + /** struct definition */ + typedef Map, _Options> Coefficients; +}; +} + +namespace internal { +template +/** struct definition */ +struct traits, _Options>> + : traits> +{ + /** struct definition */ + typedef Map, _Options> Coefficients; + /** struct definition */ + typedef traits> TraitsBase; + enum + { + Flags = TraitsBase::Flags & ~LvalueBit + }; +}; +} + +/** + * \brief MRP expression mapping a constant memory buffer + * + * \tparam _Scalar the type of the MRP coefficients + * \tparam _Options see class Map + * + * This is a specialization of class Map for MRP. This class allows to view + * a 3 scalar memory buffer as an Eigen's MRP object. + * + * \sa class Map, class MRP, class MRPBase + */ +template +class Map, _Options> : public MRPBase, _Options>> +{ + typedef MRPBase, _Options>> Base; + + public: + typedef _Scalar Scalar; //!< variable + typedef typename internal::traits::Coefficients Coefficients; //!< variable + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) + using Base::operator*=; + + /** Constructs a Mapped MRP object from the pointer \a coeffs * - * \sa MRPBase::shadow() - */ - template - inline MRP::Scalar> MRPBase::shadow() const + * The pointer \a coeffs must reference the three coefficients of MRP in the following order: + * \code *coeffs == {x, y, z} \endcode + * + * If the template parameter _Options is set to Aligned, then the pointer coeffs must be aligned. */ + EIGEN_STRONG_INLINE Map(const Scalar* coeffs) + : m_coeffs(coeffs) { - Scalar n2 = this->squaredNorm(); - if (n2 > Scalar(0)) - return MRP(-this->coeffs() / n2); - else { - return MRP(0.,0.,0.); - } } - /** \returns the MRP [B] matrix of \c *this - * This is used in + inline const Coefficients& coeffs() const { return m_coeffs; } //!< method + + protected: + const Coefficients m_coeffs; //!< variable +}; + +/** + * \brief Expression of a MRP from a memory buffer + * + * \tparam _Scalar the type of the MRP coefficients + * \tparam _Options see class Map + * + * This is a specialization of class Map for MRP. This class allows to view + * a 3 scalar memory buffer as an Eigen's MRP object. + * + * \sa class Map, class MRP, class MRPBase + */ +template +class Map, _Options> : public MRPBase, _Options>> +{ + typedef MRPBase, _Options>> Base; + + public: + typedef _Scalar Scalar; //!< variable + typedef typename internal::traits::Coefficients Coefficients; //!< variable + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) + using Base::operator*=; + + /** Constructs a Mapped MRP object from the pointer \a coeffs * - * d(sigmda_B/N) = 1/4 [B(sigma_B/N)] omega_BN_B + * The pointer \a coeffs must reference the three coefficients of MRP in the following order: + * \code *coeffs == {x, y, z} \endcode * - * \sa MRPBase::shadow() - */ - template - inline typename MRPBase::Matrix3 MRPBase::Bmat() const + * If the template parameter _Options is set to Aligned, then the pointer coeffs must be aligned. */ + EIGEN_STRONG_INLINE Map(Scalar* coeffs) + : m_coeffs(coeffs) { - Matrix3 B; - Scalar ms2, s1s2, s1s3, s2s3; - - ms2 = Scalar(1) - this->coeffs().squaredNorm(); - s1s2 = this->x()*this->y(); - s1s3 = this->x()*this->z(); - s2s3 = this->y()*this->z(); - - B.coeffRef(0,0) = ms2 + Scalar(2)*this->x()*this->x(); - B.coeffRef(0,1) = Scalar(2)*(s1s2 - this->z()); - B.coeffRef(0,2) = Scalar(2)*(s1s3 + this->y()); - B.coeffRef(1,0) = Scalar(2)*(s1s2 + this->z()); - B.coeffRef(1,1) = ms2 + Scalar(2)*this->y()*this->y(); - B.coeffRef(1,2) = Scalar(2)*(s2s3 - this->x()); - B.coeffRef(2,0) = Scalar(2)*(s1s3 - this->y()); - B.coeffRef(2,1) = Scalar(2)*(s2s3 + this->x()); - B.coeffRef(2,2) = ms2 + Scalar(2)*this->z()*this->z(); - - return B; } - /** \returns the multiplicative inverse of \c *this - * Note that in most cases, i.e., if you simply want the opposite rotation, - * and/or the MRP is normalized, then it is enough to use the conjugate. - * - * \sa MRPBase::conjugate() - */ - template - inline MRP::Scalar> MRPBase::inverse() const + inline Coefficients& coeffs() { return m_coeffs; } //!< method + inline const Coefficients& coeffs() const { return m_coeffs; } //!< method + + protected: + Coefficients m_coeffs; //!< variable +}; + +/** + * Map an unaligned array of single precision scalars as a MRP */ +typedef Map, 0> MRPMapf; +/** + * Map an unaligned array of double precision scalars as a MRP */ +typedef Map, 0> MRPMapd; +/** + * Map a 16-byte aligned array of single precision scalars as a MRP */ +typedef Map, Aligned> MRPMapAlignedf; +/** + * Map a 16-byte aligned array of double precision scalars as a MRP */ +typedef Map, Aligned> MRPMapAlignedd; + +/*************************************************************************** + * Implementation of MRPBase methods + ***************************************************************************/ + +// Generic MRP * MRP product +// This product can be specialized for a given architecture via the Arch template argument. +namespace internal { +/*! template definition */ +template +struct mrp_product +{ + static EIGEN_STRONG_INLINE MRP run(const MRPBase& a, const MRPBase& b) { - return MRP(-this->coeffs()); + Scalar det; //!< variable + Scalar s1N2; //!< variable + Scalar s2N2; //!< variable + MRP s2 = b; + MRP answer; + s1N2 = a.squaredNorm(); + s2N2 = s2.squaredNorm(); + det = Scalar(1.0) + s1N2 * s2N2 - 2 * a.dot(b); + if (det < 0.01) { + s2 = s2.shadow(); + s2N2 = s2.squaredNorm(); + det = Scalar(1.0) + s1N2 * s2N2 - 2 * a.dot(b); + } + answer = MRP(((1 - s1N2) * s2.vec() + (1 - s2N2) * a.vec() - 2 * b.vec().cross(a.vec())) / det); + if (answer.squaredNorm() > 1) + answer = answer.shadow(); + return answer; } +}; +} + +/** \returns the concatenation of two rotations as a MRP-MRP product */ +template +template +EIGEN_STRONG_INLINE MRP::Scalar> +MRPBase::operator*(const MRPBase& other) const +{ + EIGEN_STATIC_ASSERT( + (internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + return internal::mrp_product < Architecture::Target, Derived, OtherDerived, + typename internal::traits::Scalar, + internal::traits::IsAligned && internal::traits::IsAligned > ::run(*this, other); +} + +/** \sa operator*(MRP) */ +template +template +EIGEN_STRONG_INLINE Derived& +MRPBase::operator*=(const MRPBase& other) +{ + derived() = derived() * other.derived(); + return derived(); +} + +/** \sa operator*(MRP) */ +template +template +EIGEN_STRONG_INLINE Derived& +MRPBase::operator+=(const MRPBase& other) +{ + derived().coeffs() = derived().coeffs() + other.derived().coeffs(); + return derived(); +} +/** \sa operator*(MRP) */ +template +template +EIGEN_STRONG_INLINE Derived& +MRPBase::operator-=(const MRPBase& other) +{ + derived().coeffs() = derived().coeffs() - other.derived().coeffs(); + return derived(); +} + +/** Rotation of a vector by a MRP. + * \remarks If the MRP is used to rotate several points (>1) + * then it is much more efficient to first convert it to a 3x3 Matrix. + * Comparison of the operation cost for n transformations: + * - MRP2: 30n + * - Via a Matrix3: 24 + 15n + */ +template +EIGEN_STRONG_INLINE typename MRPBase::Vector3 +MRPBase::_transformVector(const Vector3& v) const +{ + // Note that this algorithm comes from the optimization by hand + // of the conversion to a Matrix followed by a Matrix/Vector product. + // It appears to be much faster than the common algorithm found + // in the literature (30 versus 39 flops). It also requires two + // Vector3 as temporaries. + Vector3 uv = this->vec().cross(v); + uv += uv; + return v + this->w() * uv + this->vec().cross(uv); +} + +template +EIGEN_STRONG_INLINE MRPBase& +MRPBase::operator=(const MRPBase& other) +{ + coeffs() = other.coeffs(); + return derived(); +} + +template +template +EIGEN_STRONG_INLINE Derived& +MRPBase::operator=(const MRPBase& other) +{ + coeffs() = other.coeffs(); + return derived(); +} + +/** Set \c *this from an angle-axis \a aa and returns a reference to \c *this + */ +template +EIGEN_STRONG_INLINE Derived& +MRPBase::operator=(const AngleAxisType& aa) +{ + using std::tan; + Scalar tanPhi = Scalar(0.25) * aa.angle(); // Scalar(0.25) to suppress precision loss warnings + this->vec() = tanPhi * aa.axis(); + return derived(); +} + +/** Set \c *this from the expression \a xpr: + * - if \a xpr is a 3x1 vector, then \a xpr is assumed to be a MRP + * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix + * and \a xpr is converted to a MRP + */ - /** \returns the conjugate of the \c *this which is equal to the multiplicative inverse - * if the MRP is normalized. - * The conjugate of a MRP represents the opposite rotation. - * - * \sa MRP2::inverse() - */ - template - inline MRP::Scalar> - MRPBase::conjugate() const +template +template +inline Derived& +MRPBase::operator=(const MatrixBase& xpr) +{ + EIGEN_STATIC_ASSERT( + (internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + internal::MRPbase_assign_impl::run(*this, xpr.derived()); + return derived(); +} + +/** Convert the MRP sigma_B/N to a 3x3 rotation matrix [NB]. + */ +template +inline typename MRPBase::Matrix3 +MRPBase::toRotationMatrix(void) const +{ + // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) + // if not inlined then the cost of the return by value is huge ~ +35%, + // however, not inlining this function is an order of magnitude slower, so + // it has to be inlined, and so the return by value is not an issue + Matrix3 res; + + const Scalar ps2 = Scalar(1) + this->coeffs().squaredNorm(); + const Scalar ms2 = Scalar(1) - this->coeffs().squaredNorm(); + const Scalar ms2Sq = ms2 * ms2; + const Scalar s1s2 = Scalar(8) * this->x() * this->y(); + const Scalar s1s3 = Scalar(8) * this->x() * this->z(); + const Scalar s2s3 = Scalar(8) * this->y() * this->z(); + const Scalar s1Sq = this->x() * this->x(); + const Scalar s2Sq = this->y() * this->y(); + const Scalar s3Sq = this->z() * this->z(); + + res.coeffRef(0, 0) = Scalar(4) * (+s1Sq - s2Sq - s3Sq) + ms2Sq; + res.coeffRef(0, 1) = s1s2 - Scalar(4) * this->z() * ms2; + res.coeffRef(0, 2) = s1s3 + Scalar(4) * this->y() * ms2; + res.coeffRef(1, 0) = s1s2 + Scalar(4) * this->z() * ms2; + res.coeffRef(1, 1) = Scalar(4) * (-s1Sq + s2Sq - s3Sq) + ms2Sq; + res.coeffRef(1, 2) = s2s3 - Scalar(4) * this->x() * ms2; + res.coeffRef(2, 0) = s1s3 - Scalar(4) * this->y() * ms2; + 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; +} + +/** Sets \c *this to be a MRP representing a rotation between + * the two arbitrary vectors \a a and \a b. In other words, the built + * rotation represent a rotation sending the line of direction \a a + * to the line of direction \a b, both lines passing through the origin. + * + * \returns a reference to \c *this. + * + * Note that the two input vectors do \b not have to be normalized, and + * do not need to have the same norm. + */ +template +template +inline Derived& +MRPBase::setFromTwoVectors(const MatrixBase& a, const MatrixBase& b) +{ + using std::acos; + using std::tan; + + Vector3 v0 = a.normalized(); + Vector3 v1 = b.normalized(); + Scalar c = v1.dot(v0); + + if (c <= 1 && c >= -1) { + Vector3 axis = v0.cross(v1); + axis.normalize(); + + this->vec() = axis * tan(acos(c) / Scalar(4.0)); + } else { + this->vec() << 0., 0., 0.; + } + return derived(); +} + +/** Returns a MRP representing a rotation between + * the two arbitrary vectors \a a and \a b. In other words, the built + * rotation represent a rotation sending the line of direction \a a + * to the line of direction \a b, both lines passing through the origin. + * + * \returns resulting MRP + * + * Note that the two input vectors do \b not have to be normalized, and + * do not need to have the same norm. + */ +template +template +MRP +MRP::FromTwoVectors(const MatrixBase& a, const MatrixBase& b) +{ + MRP sig; + sig.setFromTwoVectors(a, b); + return sig; +} + +/** \returns the MRP Shadow set of \c *this + * + * \sa MRPBase::shadow() + */ +template +inline MRP::Scalar> +MRPBase::shadow() const +{ + Scalar n2 = this->squaredNorm(); + if (n2 > Scalar(0)) + return MRP(-this->coeffs() / n2); + else { + return MRP(0., 0., 0.); + } +} + +/** \returns the MRP [B] matrix of \c *this + * This is used in + * + * d(sigmda_B/N) = 1/4 [B(sigma_B/N)] omega_BN_B + * + * \sa MRPBase::shadow() + */ +template +inline typename MRPBase::Matrix3 +MRPBase::Bmat() const +{ + Matrix3 B; + Scalar ms2, s1s2, s1s3, s2s3; + + ms2 = Scalar(1) - this->coeffs().squaredNorm(); + s1s2 = this->x() * this->y(); + s1s3 = this->x() * this->z(); + s2s3 = this->y() * this->z(); + + B.coeffRef(0, 0) = ms2 + Scalar(2) * this->x() * this->x(); + B.coeffRef(0, 1) = Scalar(2) * (s1s2 - this->z()); + B.coeffRef(0, 2) = Scalar(2) * (s1s3 + this->y()); + B.coeffRef(1, 0) = Scalar(2) * (s1s2 + this->z()); + B.coeffRef(1, 1) = ms2 + Scalar(2) * this->y() * this->y(); + B.coeffRef(1, 2) = Scalar(2) * (s2s3 - this->x()); + B.coeffRef(2, 0) = Scalar(2) * (s1s3 - this->y()); + B.coeffRef(2, 1) = Scalar(2) * (s2s3 + this->x()); + B.coeffRef(2, 2) = ms2 + Scalar(2) * this->z() * this->z(); + + return B; +} + +/** \returns the multiplicative inverse of \c *this + * Note that in most cases, i.e., if you simply want the opposite rotation, + * and/or the MRP is normalized, then it is enough to use the conjugate. + * + * \sa MRPBase::conjugate() + */ +template +inline MRP::Scalar> +MRPBase::inverse() const +{ + return MRP(-this->coeffs()); +} + +/** \returns the conjugate of the \c *this which is equal to the multiplicative inverse + * if the MRP is normalized. + * The conjugate of a MRP represents the opposite rotation. + * + * \sa MRP2::inverse() + */ +template +inline MRP::Scalar> +MRPBase::conjugate() const +{ + return MRP(-this->coeffs()); +} + +/** \returns the angle (in radian) between two rotations + * \sa dot() + */ +template +template +inline typename internal::traits::Scalar +MRPBase::angularDistance(const MRPBase& other) const +{ + using std::abs; + using std::atan; + MRP d = (*this) * other.conjugate(); + return Scalar(4) * atan(d.norm()); +} + +// /** \returns the spherical linear interpolation between the two MRPs +// * \c *this and \a other at the parameter \a t in [0;1]. +// * +// * This represents an interpolation for a constant motion between \c *this and \a other, +// * see also http://en.wikipedia.org/wiki/Slerp. +// */ +// template +// template +// MRP::Scalar> +// MRPBase::slerp(const Scalar& t, const MRPBase& other) const +// { +// using std::acos; +// using std::sin; +// using std::abs; +// static const Scalar one = Scalar(1) - NumTraits::epsilon(); +// Scalar d = this->dot(other); +// Scalar absD = abs(d); +// +// Scalar scale0; +// Scalar scale1; +// +// if(absD>=one) +// { +// scale0 = Scalar(1) - t; +// scale1 = t; +// } +// else +// { +// // theta is the angle between the 2 MRPs +// Scalar theta = acos(absD); +// Scalar sinTheta = sin(theta); +// +// scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta; +// scale1 = sin( ( t * theta) ) / sinTheta; +// } +// if(d(scale0 * coeffs() + scale1 * other.coeffs()); +// } + +namespace internal { + +// set from a rotation matrix +// this maps the [NB] DCM to the equivalent sigma_B/N set +template +/*! struct definition */ +struct MRPbase_assign_impl +{ + typedef typename Other::Scalar Scalar; //!< variable + typedef DenseIndex Index; //!< variable + /** Class Definition */ + template + static inline void run(MRPBase& sig, const Other& mat) { - return MRP(-this->coeffs()); + Quaternion q; + Scalar num; + + /* convert DCM to quaternions */ + q = mat; + num = Scalar(1) + q.w(); + + /* convert quaternions to MRP */ + sig.x() = q.x() / num; + sig.y() = q.y() / num; + sig.z() = q.z() / num; } +}; - /** \returns the angle (in radian) between two rotations - * \sa dot() - */ - template - template - inline typename internal::traits::Scalar - MRPBase::angularDistance(const MRPBase& other) const +// set from a vector of coefficients assumed to be a MRP +template +/** + structure definition + */ +struct MRPbase_assign_impl +{ + typedef typename Other::Scalar Scalar; //!< variable + /** Class definition */ + template + static inline void run(MRPBase& q, const Other& vec) { - using std::atan; - using std::abs; - MRP d = (*this) * other.conjugate(); - return Scalar(4) * atan( d.norm() ); + q.coeffs() = vec; } +}; +} // end namespace internal - - // /** \returns the spherical linear interpolation between the two MRPs - // * \c *this and \a other at the parameter \a t in [0;1]. - // * - // * This represents an interpolation for a constant motion between \c *this and \a other, - // * see also http://en.wikipedia.org/wiki/Slerp. - // */ - // template - // template - // MRP::Scalar> - // MRPBase::slerp(const Scalar& t, const MRPBase& other) const - // { - // using std::acos; - // using std::sin; - // using std::abs; - // static const Scalar one = Scalar(1) - NumTraits::epsilon(); - // Scalar d = this->dot(other); - // Scalar absD = abs(d); - // - // Scalar scale0; - // Scalar scale1; - // - // if(absD>=one) - // { - // scale0 = Scalar(1) - t; - // scale1 = t; - // } - // else - // { - // // theta is the angle between the 2 MRPs - // Scalar theta = acos(absD); - // Scalar sinTheta = sin(theta); - // - // scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta; - // scale1 = sin( ( t * theta) ) / sinTheta; - // } - // if(d(scale0 * coeffs() + scale1 * other.coeffs()); - // } - - namespace internal { - - // set from a rotation matrix - // this maps the [NB] DCM to the equivalent sigma_B/N set - template - /*! struct definition */ - struct MRPbase_assign_impl - { - typedef typename Other::Scalar Scalar; //!< variable - typedef DenseIndex Index; //!< variable - /** Class Definition */ - template static inline void run(MRPBase& sig, const Other& mat) - { - Quaternion q; - Scalar num; - - /* convert DCM to quaternions */ - q = mat; - num = Scalar(1) + q.w(); - - /* convert quaternions to MRP */ - sig.x() = q.x()/num; - sig.y() = q.y()/num; - sig.z() = q.z()/num; - } - }; - - // set from a vector of coefficients assumed to be a MRP - template - /** - structure definition - */ - struct MRPbase_assign_impl - { - typedef typename Other::Scalar Scalar; //!< variable - /** Class definition */ - template static inline void run(MRPBase& q, const Other& vec) - { - q.coeffs() = vec; - } - }; - - } // end namespace internal - } // end namespace Eigen #endif // EIGEN_MRP_H diff --git a/src/architecture/utilities/avsEigenSupport.cpp b/src/architecture/utilities/avsEigenSupport.cpp index 04f712b75c..de6f6ee2be 100644 --- a/src/architecture/utilities/avsEigenSupport.cpp +++ b/src/architecture/utilities/avsEigenSupport.cpp @@ -36,10 +36,11 @@ in a lot of cases. @param inMat The source Eigen matrix that we are converting @param outArray The destination array (sized by the user!) we copy into */ -void eigenMatrixXd2CArray(Eigen::MatrixXd inMat, double *outArray) +void +eigenMatrixXd2CArray(Eigen::MatrixXd inMat, double* outArray) { - Eigen::MatrixXd tempMat = inMat.transpose(); - memcpy(outArray, tempMat.data(), inMat.rows()*inMat.cols()*sizeof(double)); + Eigen::MatrixXd tempMat = inMat.transpose(); + memcpy(outArray, tempMat.data(), inMat.rows() * inMat.cols() * sizeof(double)); } /*! This function provides a general conversion between an Eigen matrix and @@ -50,10 +51,11 @@ in a lot of cases. @param inMat The source Eigen matrix that we are converting @param outArray The destination array (sized by the user!) we copy into */ -void eigenMatrixXi2CArray(Eigen::MatrixXi inMat, int *outArray) +void +eigenMatrixXi2CArray(Eigen::MatrixXi inMat, int* outArray) { Eigen::MatrixXi tempMat = inMat.transpose(); - memcpy(outArray, tempMat.data(), inMat.rows()*inMat.cols()*sizeof(int)); + memcpy(outArray, tempMat.data(), inMat.rows() * inMat.cols() * sizeof(int)); } /*! This function provides a direct conversion between a 3-vector and an @@ -63,9 +65,10 @@ and the transpose that would have been performed by the general case. @param inMat The source Eigen matrix that we are converting @param outArray The destination array we copy into */ -void eigenVector3d2CArray(Eigen::Vector3d & inMat, double *outArray) +void +eigenVector3d2CArray(Eigen::Vector3d& inMat, double* outArray) { - memcpy(outArray, inMat.data(), 3 * sizeof(double)); + memcpy(outArray, inMat.data(), 3 * sizeof(double)); } /*! This function provides a direct conversion between an MRP and an @@ -75,7 +78,8 @@ and the transpose that would have been performed by the general case. @param inMat The source Eigen MRP that we are converting @param outArray The destination array we copy into */ -void eigenMRPd2CArray(Eigen::Vector3d& inMat, double* outArray) +void +eigenMRPd2CArray(Eigen::Vector3d& inMat, double* outArray) { memcpy(outArray, inMat.data(), 3 * sizeof(double)); } @@ -87,10 +91,11 @@ that would have been performed by the general case. @param inMat The source Eigen matrix that we are converting @param outArray The destination array we copy into */ -void eigenMatrix3d2CArray(Eigen::Matrix3d & inMat, double *outArray) +void +eigenMatrix3d2CArray(Eigen::Matrix3d& inMat, double* outArray) { - Eigen::MatrixXd tempMat = inMat.transpose(); - memcpy(outArray, tempMat.data(), 9 * sizeof(double)); + Eigen::MatrixXd tempMat = inMat.transpose(); + memcpy(outArray, tempMat.data(), 9 * sizeof(double)); } /*! This function performs the general conversion between an input C array @@ -102,11 +107,12 @@ information to ingest the C array. @param nRows @param nCols */ -Eigen::MatrixXd cArray2EigenMatrixXd(double *inArray, int nRows, int nCols) +Eigen::MatrixXd +cArray2EigenMatrixXd(double* inArray, int nRows, int nCols) { Eigen::MatrixXd outMat; outMat.resize(nRows, nCols); - outMat = Eigen::Map(inArray, outMat.rows(), outMat.cols()); + outMat = Eigen::Map(inArray, outMat.rows(), outMat.cols()); return outMat; } @@ -116,7 +122,8 @@ in order to save an unnecessary conversion between types. @return Eigen::Vector3d @param inArray The input array (row-major) */ -Eigen::Vector3d cArray2EigenVector3d(double *inArray) +Eigen::Vector3d +cArray2EigenVector3d(double* inArray) { return Eigen::Map(inArray, 3, 1); } @@ -127,7 +134,8 @@ in order to save an unnecessary conversion between types. @return Eigen::MRPd @param inArray The input array (row-major) */ -Eigen::MRPd cArray2EigenMRPd(double* inArray) +Eigen::MRPd +cArray2EigenMRPd(double* inArray) { Eigen::MRPd sigma_Eigen; sigma_Eigen = cArray2EigenVector3d(inArray); @@ -141,9 +149,10 @@ in order to save an unnecessary conversion between types. @return Eigen::Matrix3d @param inArray The input array (row-major) */ -Eigen::Matrix3d cArray2EigenMatrix3d(double *inArray) +Eigen::Matrix3d +cArray2EigenMatrix3d(double* inArray) { - return Eigen::Map(inArray, 3, 3).transpose(); + return Eigen::Map(inArray, 3, 3).transpose(); } /*! This function performs the conversion between an input C 3x3 @@ -152,7 +161,8 @@ in order to save an unnecessary conversion between types @return Eigen::Matrix3d @param in2DArray The input 2D-array */ -Eigen::Matrix3d c2DArray2EigenMatrix3d(double in2DArray[3][3]) +Eigen::Matrix3d +c2DArray2EigenMatrix3d(double in2DArray[3][3]) { Eigen::Matrix3d outMat; for (int i = 0; i < 3; i++) { @@ -170,16 +180,17 @@ Eigen::Matrix3d c2DArray2EigenMatrix3d(double in2DArray[3][3]) @return Eigen::Matrix3d @param angle The input rotation angle */ -Eigen::Matrix3d eigenM1(double angle) +Eigen::Matrix3d +eigenM1(double angle) { Eigen::Matrix3d mOut; mOut.setIdentity(); - mOut(1,1) = cos(angle); - mOut(1,2) = sin(angle); - mOut(2,1) = -mOut(1,2); - mOut(2,2) = mOut(1,1); + mOut(1, 1) = cos(angle); + mOut(1, 2) = sin(angle); + mOut(2, 1) = -mOut(1, 2); + mOut(2, 2) = mOut(1, 1); return mOut; } @@ -190,16 +201,17 @@ Eigen::Matrix3d eigenM1(double angle) @return Eigen::Matrix3d @param angle The input rotation angle */ -Eigen::Matrix3d eigenM2(double angle) +Eigen::Matrix3d +eigenM2(double angle) { Eigen::Matrix3d mOut; mOut.setIdentity(); - mOut(0,0) = cos(angle); - mOut(0,2) = -sin(angle); - mOut(2,0) = -mOut(0,2); - mOut(2,2) = mOut(0,0); + mOut(0, 0) = cos(angle); + mOut(0, 2) = -sin(angle); + mOut(2, 0) = -mOut(0, 2); + mOut(2, 2) = mOut(0, 0); return mOut; } @@ -210,16 +222,17 @@ Eigen::Matrix3d eigenM2(double angle) @return Eigen::Matrix3d @param angle The input rotation angle */ -Eigen::Matrix3d eigenM3(double angle) +Eigen::Matrix3d +eigenM3(double angle) { Eigen::Matrix3d mOut; mOut.setIdentity(); - mOut(0,0) = cos(angle); - mOut(0,1) = sin(angle); - mOut(1,0) = -mOut(0,1); - mOut(1,1) = mOut(0,0); + mOut(0, 0) = cos(angle); + mOut(0, 1) = sin(angle); + mOut(1, 0) = -mOut(0, 1); + mOut(1, 1) = mOut(0, 0); return mOut; } @@ -230,18 +243,19 @@ Eigen::Matrix3d eigenM3(double angle) @return Eigen::Matrix3d @param vec The input vector */ -Eigen::Matrix3d eigenTilde(Eigen::Vector3d vec) +Eigen::Matrix3d +eigenTilde(Eigen::Vector3d vec) { Eigen::Matrix3d mOut; - mOut(0,0) = mOut(1,1) = mOut(2,2) = 0.0; + mOut(0, 0) = mOut(1, 1) = mOut(2, 2) = 0.0; - mOut(0,1) = -vec(2); - mOut(1,0) = vec(2); - mOut(0,2) = vec(1); - mOut(2,0) = -vec(1); - mOut(1,2) = -vec(0); - mOut(2,1) = vec(0); + mOut(0, 1) = -vec(2); + mOut(1, 0) = vec(2); + mOut(0, 2) = vec(1); + mOut(2, 0) = -vec(1); + mOut(1, 2) = -vec(0); + mOut(2, 1) = vec(0); return mOut; } @@ -250,11 +264,12 @@ Eigen::Matrix3d eigenTilde(Eigen::Vector3d vec) @return Eigen::MRPd @param dcm_Eigen The input DCM */ -Eigen::MRPd eigenC2MRP(Eigen::Matrix3d dcm_Eigen) +Eigen::MRPd +eigenC2MRP(Eigen::Matrix3d dcm_Eigen) { - Eigen::MRPd sigma_Eigen; // output Eigen MRP - double dcm_Array[9]; // C array DCM - double sigma_Array[3]; // C array MRP + Eigen::MRPd sigma_Eigen; // output Eigen MRP + double dcm_Array[9]; // C array DCM + double sigma_Array[3]; // C array MRP eigenMatrix3d2CArray(dcm_Eigen, dcm_Array); C2MRP(RECAST3X3 dcm_Array, sigma_Array); @@ -267,7 +282,8 @@ Eigen::MRPd eigenC2MRP(Eigen::Matrix3d dcm_Eigen) @return Eigen::Vector3d @param mrp The input Vector3d variable */ -Eigen::Vector3d eigenMRPd2Vector3d(Eigen::MRPd mrp) +Eigen::Vector3d +eigenMRPd2Vector3d(Eigen::MRPd mrp) { Eigen::Vector3d vec3d; @@ -285,15 +301,20 @@ Eigen::Vector3d eigenMRPd2Vector3d(Eigen::MRPd mrp) @param f Function to find the zero of @param fPrime First derivative of the function */ -double newtonRaphsonSolve(const double& initialEstimate, const double& accuracy, const std::function& f, const std::function& fPrime) { - double currentEstimate = initialEstimate; - for (int i = 0; i < 100; i++) { +double +newtonRaphsonSolve(const double& initialEstimate, + const double& accuracy, + const std::function& f, + const std::function& fPrime) +{ + double currentEstimate = initialEstimate; + for (int i = 0; i < 100; i++) { if (std::abs(f(currentEstimate)) < accuracy) break; - double functionVal = f(currentEstimate); - double functionDeriv = fPrime(currentEstimate); - currentEstimate = currentEstimate - functionVal/functionDeriv; - } - return currentEstimate; + double functionVal = f(currentEstimate); + double functionDeriv = fPrime(currentEstimate); + currentEstimate = currentEstimate - functionVal / functionDeriv; + } + return currentEstimate; } diff --git a/src/architecture/utilities/avsEigenSupport.h b/src/architecture/utilities/avsEigenSupport.h index 8cdde9d480..6d9e254741 100644 --- a/src/architecture/utilities/avsEigenSupport.h +++ b/src/architecture/utilities/avsEigenSupport.h @@ -17,48 +17,64 @@ */ - #ifndef _AVSEIGENSUPPORT_ #define _AVSEIGENSUPPORT_ #include #include "avsEigenMRP.h" - //!@brief General conversion between any Eigen matrix and output array -void eigenMatrixXd2CArray(Eigen::MatrixXd inMat, double *outArray); +void +eigenMatrixXd2CArray(Eigen::MatrixXd inMat, double* outArray); //!@brief General conversion between any Eigen matrix and output array -void eigenMatrixXi2CArray(Eigen::MatrixXi inMat, int *outArray); +void +eigenMatrixXi2CArray(Eigen::MatrixXi inMat, int* outArray); //!@brief Rapid conversion between 3-vector and output array -void eigenVector3d2CArray(Eigen::Vector3d & inMat, double *outArray); +void +eigenVector3d2CArray(Eigen::Vector3d& inMat, double* outArray); //!@brief Rapid conversion between MRP and output array -void eigenMRPd2CArray(Eigen::Vector3d& inMat, double* outArray); +void +eigenMRPd2CArray(Eigen::Vector3d& inMat, double* outArray); //!@brief Rapid conversion between 3x3 matrix and output array -void eigenMatrix3d2CArray(Eigen::Matrix3d & inMat, double *outArray); +void +eigenMatrix3d2CArray(Eigen::Matrix3d& inMat, double* outArray); //!@brief General conversion between a C array and an Eigen matrix -Eigen::MatrixXd cArray2EigenMatrixXd(double *inArray, int nRows, int nCols); +Eigen::MatrixXd +cArray2EigenMatrixXd(double* inArray, int nRows, int nCols); //!@brief Specific conversion between a C array and an Eigen 3-vector -Eigen::Vector3d cArray2EigenVector3d(double *inArray); +Eigen::Vector3d +cArray2EigenVector3d(double* inArray); //!@brief Specific conversion between a C array and an Eigen MRPs -Eigen::MRPd cArray2EigenMRPd(double* inArray); +Eigen::MRPd +cArray2EigenMRPd(double* inArray); //!@brief Specfici conversion between a C array and an Eigen 3x3 matrix -Eigen::Matrix3d cArray2EigenMatrix3d(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 -Eigen::Matrix3d eigenM1(double angle); +Eigen::Matrix3d +c2DArray2EigenMatrix3d(double in2DArray[3][3]); +//!@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); +Eigen::Matrix3d +eigenM2(double angle); //!@brief returns the third axis DCM with the input angle -Eigen::Matrix3d eigenM3(double angle); +Eigen::Matrix3d +eigenM3(double angle); //!@brief returns the tilde matrix representation of a vector (equivalent to a vector cross product) -Eigen::Matrix3d eigenTilde(Eigen::Vector3d vec); +Eigen::Matrix3d +eigenTilde(Eigen::Vector3d vec); //!@brief converts MRPd to an Vector3d variable -Eigen::Vector3d eigenMRPd2Vector3d(Eigen::MRPd vec); +Eigen::Vector3d +eigenMRPd2Vector3d(Eigen::MRPd vec); //!@brief maps the DCM to MRPs using Eigen variables Eigen::MRPd eigenC2MRP(Eigen::Matrix3d); //!@brief solves for the zero of the provided function -double newtonRaphsonSolve(const double& initialEstimate, const double& accuracy, const std::function& f, const std::function& fPrime); - +double +newtonRaphsonSolve(const double& initialEstimate, + const double& accuracy, + const std::function& f, + const std::function& fPrime); #endif /* _AVSEIGENSUPPORT_ */ diff --git a/src/architecture/utilities/bilinearInterpolation.hpp b/src/architecture/utilities/bilinearInterpolation.hpp index 4d3360362e..7b5f7dc58c 100644 --- a/src/architecture/utilities/bilinearInterpolation.hpp +++ b/src/architecture/utilities/bilinearInterpolation.hpp @@ -36,23 +36,25 @@ @param x Function x coordinate for interpolation @param y Function y coordinate for interpolation */ -double bilinearInterpolation(double x1, - double x2, - double y1, - double y2, - double z11, - double z12, - double z21, - double z22, - double x, - double y) { +double +bilinearInterpolation(double x1, + double x2, + double y1, + double y2, + double z11, + double z12, + double z21, + double z22, + double x, + double y) +{ assert(x1 < x && x < x2); assert(y1 < y && y < y2); - return 1 / ((x2 - x1) * (y2 - y1)) * (z11 * (x2 - x) * (y2 - y) + z21 * (x - x1) * (y2 - y) - + z12 * (x2 - x) * (y - y1) - + z22 * (x - x1) * (y - y1)); + return 1 / ((x2 - x1) * (y2 - y1)) * + (z11 * (x2 - x) * (y2 - y) + z21 * (x - x1) * (y2 - y) + z12 * (x2 - x) * (y - y1) + + z22 * (x - x1) * (y - y1)); } #endif // BILINEARINTERPOLATION_H diff --git a/src/architecture/utilities/bskLogging.cpp b/src/architecture/utilities/bskLogging.cpp index b1e6f8620f..4cee46e112 100644 --- a/src/architecture/utilities/bskLogging.cpp +++ b/src/architecture/utilities/bskLogging.cpp @@ -29,27 +29,25 @@ logLevel_t LogLevel = BSK_DEBUG; /*! This method sets the default logging verbosity @param logLevel */ -void setDefaultLogLevel(logLevel_t logLevel) +void +setDefaultLogLevel(logLevel_t logLevel) { LogLevel = logLevel; } /*! This method gets the default logging verbosity */ -logLevel_t getDefaultLogLevel() +logLevel_t +getDefaultLogLevel() { return LogLevel; } /*! This method prints the default logging verbosity */ -void printDefaultLogLevel() +void +printDefaultLogLevel() { - std::map logLevelMap - { - {0, "BSK_DEBUG"}, - {1, "BSK_INFORMATION"}, - {2, "BSK_WARNING"}, - {3, "BSK_ERROR"}, - {4, "BSK_SILENT"} + std::map logLevelMap{ + { 0, "BSK_DEBUG" }, { 1, "BSK_INFORMATION" }, { 2, "BSK_WARNING" }, { 3, "BSK_ERROR" }, { 4, "BSK_SILENT" } }; const char* defaultLevelStr = logLevelMap[LogLevel]; printf("Default Logging Level: %s\n", defaultLevelStr); @@ -58,7 +56,7 @@ void printDefaultLogLevel() /*! The constructor initialises the logger for a module and uses default verbostiy level for logging */ BSKLogger::BSKLogger() { - //Default print verbosity + // Default print verbosity this->_logLevel = getDefaultLogLevel(); } @@ -72,63 +70,68 @@ BSKLogger::BSKLogger(logLevel_t logLevel) /*! This method changes the logging verbosity after construction */ -void BSKLogger::setLogLevel(logLevel_t logLevel) +void +BSKLogger::setLogLevel(logLevel_t logLevel) { this->_logLevel = logLevel; } /*! This method reads the current logging verbosity */ -void BSKLogger::printLogLevel() +void +BSKLogger::printLogLevel() { const char* currLevelStr = this->logLevelMap[this->_logLevel]; printf("Current Logging Level: %s\n", currLevelStr); } /*! Get the current log level value */ -int BSKLogger::getLogLevel() +int +BSKLogger::getLogLevel() { return this->_logLevel; } -/*! This method logs information. The current behavior is to simply print out the message and the targeted logging level. - This should be the main method called in user code. +/*! This method logs information. The current behavior is to simply print out the message and the targeted logging + level. This should be the main method called in user code. @param targetLevel @param info */ -void BSKLogger::bskLog(logLevel_t targetLevel, const char* info, ...) +void +BSKLogger::bskLog(logLevel_t targetLevel, const char* info, ...) { const char* targetLevelStr = this->logLevelMap[targetLevel]; char formatMessage[MAX_LOGGING_LENGTH]; va_list args; - va_start (args, info); + va_start(args, info); vsnprintf(formatMessage, sizeof(formatMessage), info, args); va_end(args); // Raise an error that swig can pipe to python - if(targetLevel == BSK_ERROR) - { + if (targetLevel == BSK_ERROR) { throw BasiliskError(formatMessage); } // Otherwise, print the message accordingly - if(targetLevel >= this->_logLevel) - { + if (targetLevel >= this->_logLevel) { printf("%s: %s\n", targetLevelStr, formatMessage); } } /*! Section contains C interfacing to C++ object */ -EXTERN BSKLogger* _BSKLogger(void) +EXTERN BSKLogger* +_BSKLogger(void) { return new BSKLogger(); } -EXTERN void _BSKLogger_d(BSKLogger* bskLogger) +EXTERN void +_BSKLogger_d(BSKLogger* bskLogger) { delete bskLogger; } /*! This method reads the current logging verbosity */ -EXTERN void _printLogLevel(BSKLogger* bskLogger) +EXTERN void +_printLogLevel(BSKLogger* bskLogger) { bskLogger->printLogLevel(); } @@ -137,15 +140,17 @@ EXTERN void _printLogLevel(BSKLogger* bskLogger) @param bskLogger @param logLevel */ -EXTERN void _setLogLevel(BSKLogger* bskLogger, logLevel_t logLevel) +EXTERN void +_setLogLevel(BSKLogger* bskLogger, logLevel_t logLevel) { bskLogger->setLogLevel(logLevel); } -/*! This method logs information. The current behavior is to simply print out the message and the targeted logging level. - This should be the main method called in user code. +/*! This method logs information. The current behavior is to simply print out the message and the targeted logging + level. This should be the main method called in user code. */ -EXTERN void _bskLog(BSKLogger* bskLogger, logLevel_t logLevel, const char* info) +EXTERN void +_bskLog(BSKLogger* bskLogger, logLevel_t logLevel, const char* info) { bskLogger->bskLog(logLevel, info); } diff --git a/src/architecture/utilities/bskLogging.h b/src/architecture/utilities/bskLogging.h index f00daa31dc..b4f4ade1d2 100644 --- a/src/architecture/utilities/bskLogging.h +++ b/src/architecture/utilities/bskLogging.h @@ -17,25 +17,24 @@ */ - - #ifndef _BSK_LOG_ #define _BSK_LOG_ - -//maximum length of info to log in a reference to BSKLogging in C, not relevant in C++ +// maximum length of info to log in a reference to BSKLogging in C, not relevant in C++ #define MAX_LOGGING_LENGTH 255 -typedef enum { +typedef enum +{ BSK_DEBUG, BSK_INFORMATION, BSK_WARNING, BSK_ERROR, - BSK_SILENT // the coder should never use this flag when using bskLog(). It is used to turn off all output + BSK_SILENT // the coder should never use this flag when using bskLog(). It is used to turn off all output } logLevel_t; extern logLevel_t LogLevel; -void printDefaultLogLevel(); +void +printDefaultLogLevel(); /// \cond DO_NOT_DOCUMENT @@ -48,45 +47,48 @@ void printDefaultLogLevel(); %include "std_except.i" #endif -void setDefaultLogLevel(logLevel_t logLevel); -logLevel_t getDefaultLogLevel(); +void +setDefaultLogLevel(logLevel_t logLevel); +logLevel_t +getDefaultLogLevel(); /*! Custom Basilisk runtime error class */ -class BasiliskError : public std::runtime_error { -public: +class BasiliskError : public std::runtime_error +{ + public: explicit BasiliskError(const std::string& message) - : std::runtime_error(message) {} + : std::runtime_error(message) + { + } explicit BasiliskError(const char* message) - : std::runtime_error(message) {} + : std::runtime_error(message) + { + } }; - /*! BSK logging class */ class BSKLogger { - public: - BSKLogger(); - BSKLogger(logLevel_t logLevel); - virtual ~BSKLogger() = default; - void setLogLevel(logLevel_t logLevel); - void printLogLevel(); - int getLogLevel(); - void bskLog(logLevel_t targetLevel, const char* info, ...); - - //Provides a mapping from log level enum to str - public: - std::map logLevelMap - { - {0, "BSK_DEBUG"}, - {1, "\033[92mBSK_INFORMATION\033[0m"}, - {2, "\033[93mBSK_WARNING\033[0m"}, - {3, "\033[91mBSK_ERROR\033[0m"}, - {4, "BSK_SILENT"} - }; - - private: - logLevel_t _logLevel; + public: + BSKLogger(); + BSKLogger(logLevel_t logLevel); + virtual ~BSKLogger() = default; + void setLogLevel(logLevel_t logLevel); + void printLogLevel(); + int getLogLevel(); + void bskLog(logLevel_t targetLevel, const char* info, ...); + + // Provides a mapping from log level enum to str + public: + std::map logLevelMap{ { 0, "BSK_DEBUG" }, + { 1, "\033[92mBSK_INFORMATION\033[0m" }, + { 2, "\033[93mBSK_WARNING\033[0m" }, + { 3, "\033[91mBSK_ERROR\033[0m" }, + { 4, "BSK_SILENT" } }; + + private: + logLevel_t _logLevel; }; #else @@ -94,17 +96,21 @@ typedef struct BSKLogger BSKLogger; #endif #ifdef __cplusplus - #define EXTERN extern "C" +#define EXTERN extern "C" #else - #define EXTERN +#define EXTERN #endif -EXTERN BSKLogger* _BSKLogger(void); -EXTERN void _BSKLogger_d(BSKLogger*); -EXTERN void _printLogLevel(BSKLogger*); -EXTERN void _setLogLevel(BSKLogger*, logLevel_t); -EXTERN void _bskLog(BSKLogger*, logLevel_t, const char*); - +EXTERN BSKLogger* +_BSKLogger(void); +EXTERN void +_BSKLogger_d(BSKLogger*); +EXTERN void +_printLogLevel(BSKLogger*); +EXTERN void +_setLogLevel(BSKLogger*, logLevel_t); +EXTERN void +_bskLog(BSKLogger*, logLevel_t, const char*); /// \endcond diff --git a/src/architecture/utilities/bskSemaphore.h b/src/architecture/utilities/bskSemaphore.h index 14d27377e7..cf56b3cb57 100644 --- a/src/architecture/utilities/bskSemaphore.h +++ b/src/architecture/utilities/bskSemaphore.h @@ -24,7 +24,6 @@ #include #include - /*! Basilisk semaphore class */ class BSKSemaphore { @@ -32,31 +31,30 @@ class BSKSemaphore std::condition_variable cv; size_t count; -public: + public: /*! method description */ BSKSemaphore(int count_in = 0) - : count(count_in) + : count(count_in) { } - + /*! release the lock */ inline void release() { { std::unique_lock lock(mutex); ++count; - //notify the waiting thread + // notify the waiting thread } cv.notify_one(); } - + /*! aquire the lock */ inline void acquire() { std::unique_lock lock(mutex); - while (count == 0) - { - //wait on the mutex until notify is called + while (count == 0) { + // wait on the mutex until notify is called cv.wait(lock); } --count; diff --git a/src/architecture/utilities/bsk_Print.h b/src/architecture/utilities/bsk_Print.h index 05b7616de2..a5298eb008 100644 --- a/src/architecture/utilities/bsk_Print.h +++ b/src/architecture/utilities/bsk_Print.h @@ -22,37 +22,47 @@ #include -typedef enum { +typedef enum +{ MSG_DEBUG, MSG_INFORMATION, MSG_WARNING, MSG_ERROR, - MSG_SILENT // the coder should never use this flag when using BSK_PRINT(). It is used to turn off all BSK_PRINT() + MSG_SILENT // the coder should never use this flag when using BSK_PRINT(). It is used to turn off all BSK_PRINT() } msgLevel_t_; /* specify the BSK printing verbosity level. */ #define MSG_LEVEL MSG_DEBUG - #define EXPAND(x) x -#define BSK_MESSAGE(...) { printf(__VA_ARGS__); } +#define BSK_MESSAGE(...) \ + { \ + printf(__VA_ARGS__); \ + } #ifdef _WIN32 -#define BSK_PRINT(X, _fmt, ...) if (EXPAND(X) >= MSG_LEVEL) {printf(#X ": " _fmt "\n", __VA_ARGS__);} -#define BSK_PRINT_BRIEF(X, _fmt, ...) if (EXPAND(X) >= MSG_LEVEL) {printf(#X ": " _fmt "\n", __VA_ARGS__);} - +#define BSK_PRINT(X, _fmt, ...) \ + if (EXPAND(X) >= MSG_LEVEL) { \ + printf(#X ": " _fmt "\n", __VA_ARGS__); \ + } +#define BSK_PRINT_BRIEF(X, _fmt, ...) \ + if (EXPAND(X) >= MSG_LEVEL) { \ + printf(#X ": " _fmt "\n", __VA_ARGS__); \ + } -#else /* macOS and Linux */ +#else /* macOS and Linux */ #define WHERESTR "[FILE : %s, FUNC : %s, LINE : %d]:\n" -#define WHEREARG __FILE__,__func__,__LINE__ -#define BSK_PRINT(X, _fmt, ...) if(X >= MSG_LEVEL) \ - BSK_MESSAGE(WHERESTR #X ": " _fmt "\n", WHEREARG, ##__VA_ARGS__) -#define BSK_PRINT_BRIEF(X, _fmt, ...) if(X >= MSG_LEVEL) \ - BSK_MESSAGE(#X ": " _fmt "\n", ##__VA_ARGS__) +#define WHEREARG __FILE__, __func__, __LINE__ +#define BSK_PRINT(X, _fmt, ...) \ + if (X >= MSG_LEVEL) \ + BSK_MESSAGE(WHERESTR #X ": " _fmt "\n", WHEREARG, ##__VA_ARGS__) +#define BSK_PRINT_BRIEF(X, _fmt, ...) \ + if (X >= MSG_LEVEL) \ + BSK_MESSAGE(#X ": " _fmt "\n", ##__VA_ARGS__) #endif diff --git a/src/architecture/utilities/discretize.cpp b/src/architecture/utilities/discretize.cpp index b3262ed2dd..136569af54 100644 --- a/src/architecture/utilities/discretize.cpp +++ b/src/architecture/utilities/discretize.cpp @@ -29,7 +29,9 @@ Discretize::Discretize() } /*! This lets the user initialized the discretization model to the right size*/ -Discretize::Discretize(uint8_t numStates) : Discretize() { +Discretize::Discretize(uint8_t numStates) + : Discretize() +{ this->numStates = numStates; this->LSB.resize(this->numStates); this->LSB.fill(0.0); @@ -38,55 +40,56 @@ Discretize::Discretize(uint8_t numStates) : Discretize() { } /*! The destructor is a placeholder for one that might do something*/ -Discretize::~Discretize() -{ -} +Discretize::~Discretize() {} ///*! This method calculates the least significant bit size given the maximum state value, // minimum state value, and number of bits to use.. // */ -//void setLSBByBits(uint8_t numBits, double min, double max); +// void setLSBByBits(uint8_t numBits, double min, double max); /*!@brief Sets the round direction (toZero, fromZero, near) for discretization @param direction */ -void Discretize::setRoundDirection(roundDirection_t direction){ +void +Discretize::setRoundDirection(roundDirection_t direction) +{ this->roundDirection = direction; return; } - /*!@brief Discretizes the given truth vector according to a least significant bit (binSize) @param undiscretizedVector @return vector of discretized values*/ -Eigen::VectorXd Discretize::discretize(Eigen::VectorXd undiscretizedVector){ +Eigen::VectorXd +Discretize::discretize(Eigen::VectorXd undiscretizedVector) +{ - if (this->carryError){ + if (this->carryError) { undiscretizedVector += this->discErrors; } - //discretize the data + // discretize the data Eigen::VectorXd workingVector = undiscretizedVector.cwiseQuotient(this->LSB); workingVector = workingVector.cwiseAbs(); - if (this->roundDirection == TO_ZERO){ - for (uint8_t i = 0; i < this->numStates; i++){ + if (this->roundDirection == TO_ZERO) { + for (uint8_t i = 0; i < this->numStates; i++) { workingVector[i] = floor(workingVector[i]); } - }else if(this->roundDirection == FROM_ZERO){ - for (uint8_t i = 0; i < this->numStates; i++){ + } else if (this->roundDirection == FROM_ZERO) { + for (uint8_t i = 0; i < this->numStates; i++) { workingVector[i] = ceil(workingVector[i]); } - }else if(this->roundDirection == NEAR){ - for (uint8_t i = 0; i < this->numStates; i++){ + } else if (this->roundDirection == NEAR) { + for (uint8_t i = 0; i < this->numStates; i++) { workingVector[i] = round(workingVector[i]); } } workingVector = workingVector.cwiseProduct(this->LSB); - for (uint8_t i = 0; i < this->numStates; i++){ + for (uint8_t i = 0; i < this->numStates; i++) { workingVector[i] = copysign(workingVector[i], undiscretizedVector[i]); } this->discErrors = undiscretizedVector - workingVector; diff --git a/src/architecture/utilities/discretize.h b/src/architecture/utilities/discretize.h index e6baec7f0a..1b75693689 100644 --- a/src/architecture/utilities/discretize.h +++ b/src/architecture/utilities/discretize.h @@ -24,37 +24,37 @@ #include #include - -typedef enum { +typedef enum +{ TO_ZERO, FROM_ZERO, NEAR } roundDirection_t; /*! This module discretizes data for output. It has the option to carry over discretization error or not. -*/ + */ class Discretize { -public: + public: Discretize(); Discretize(uint8_t numStates); ~Discretize(); -// void setLSBByBits(uint8_t numBits, double min, double max); -// /*!@brief Method determines the size of an output data bin (bit-value) making sure that zero is -// a possible output and giving proportionate numbers of bits to the size of max and min void*/ + // void setLSBByBits(uint8_t numBits, double min, double max); + // /*!@brief Method determines the size of an output data bin (bit-value) making sure that zero is + // a possible output and giving proportionate numbers of bits to the size of max and min void*/ /*!@brief Avoid calculating bit value (bin size) and just set it because a resolution is known @param givenLSB */ - void setLSB(Eigen::VectorXd givenLSB) {this->LSB = givenLSB;} + void setLSB(Eigen::VectorXd givenLSB) { this->LSB = givenLSB; } void setRoundDirection(roundDirection_t direction); /*!@brief Sets the round direction (toZero, fromZero, near) for discretization @param carryErrorIn */ - void setCarryError(bool carryErrorIn){this->carryError = carryErrorIn;} + void setCarryError(bool carryErrorIn) { this->carryError = carryErrorIn; } /*!@brief Discretizes the given truth vector according to a least significant bit (binSize) @param undiscretizedVector @@ -63,16 +63,16 @@ class Discretize /*!@brief Get the discretization errors @return the errors due to discretization in a corresponding vector*/ - Eigen::VectorXd getDiscretizationErrors(){return(this->discErrors);} + Eigen::VectorXd getDiscretizationErrors() { return (this->discErrors); } - Eigen::VectorXd LSB; //!< -- size of bin, bit value, least significant bit + Eigen::VectorXd LSB; //!< -- size of bin, bit value, least significant bit -private: - roundDirection_t roundDirection; //!< -- Direction to round when discretizing. "toZero", "fromZero", and "near" are the options. - uint8_t numStates; //!< -- Number of states to be discretized (length of vector fed in) - Eigen::VectorXd discErrors; //!< -- Errors from discretization. Can be returned to adjusted integrated values. - bool carryError; //!< -- true if discError should be added next time around, false if not. + private: + roundDirection_t + roundDirection; //!< -- Direction to round when discretizing. "toZero", "fromZero", and "near" are the options. + uint8_t numStates; //!< -- Number of states to be discretized (length of vector fed in) + Eigen::VectorXd discErrors; //!< -- Errors from discretization. Can be returned to adjusted integrated values. + bool carryError; //!< -- true if discError should be added next time around, false if not. }; - #endif /* _discretize_HH_ */ diff --git a/src/architecture/utilities/gauss_markov.cpp b/src/architecture/utilities/gauss_markov.cpp index ba9b50e9a0..e72274bc03 100644 --- a/src/architecture/utilities/gauss_markov.cpp +++ b/src/architecture/utilities/gauss_markov.cpp @@ -34,17 +34,19 @@ GaussMarkov::GaussMarkov(uint64_t size, uint64_t newSeed) this->RNGSeed = newSeed; this->numStates = size; initializeRNG(); - this->propMatrix.resize(size,size); + this->propMatrix.resize(size, size); this->propMatrix.fill(0.0); - this->currentState.resize((int64_t) size); + this->currentState.resize((int64_t)size); this->currentState.fill(0.0); - this->noiseMatrix.resize((int64_t) size, (int64_t) size); + this->noiseMatrix.resize((int64_t)size, (int64_t)size); this->noiseMatrix.fill(0.0); - this->stateBounds.resize((int64_t) size); + this->stateBounds.resize((int64_t)size); this->stateBounds.fill(DEFAULT_BOUND); } -void GaussMarkov::initializeRNG() { +void +GaussMarkov::initializeRNG() +{ //! - Set up standard normal distribution N(0,1) parameters for random number generation std::normal_distribution::param_type updatePair(0.0, 1.0); this->rGen.seed((unsigned int)this->RNGSeed); @@ -52,36 +54,33 @@ void GaussMarkov::initializeRNG() { } /*! The destructor is a placeholder for one that might do something*/ -GaussMarkov::~GaussMarkov() -{ -} +GaussMarkov::~GaussMarkov() {} /*! This method performs almost all of the work for the Gauss Markov random walk. It uses the current random walk configuration, propagates the current state, and then applies appropriate errors to the states to set the current error level. */ -void GaussMarkov::computeNextState() +void +GaussMarkov::computeNextState() { Eigen::VectorXd errorVector; Eigen::VectorXd ranNums; //! - Check for consistent sizes - if((this->propMatrix.size() != this->noiseMatrix.size()) || - ((uint64_t) this->propMatrix.size() != this->numStates*this->numStates)) - { + if ((this->propMatrix.size() != this->noiseMatrix.size()) || + ((uint64_t)this->propMatrix.size() != this->numStates * this->numStates)) { bskLogger.bskLog(BSK_ERROR, "Matrix size mismatch in Gauss Markov model"); return; } - if((uint64_t) this->stateBounds.size() != this->numStates) - { + if ((uint64_t)this->stateBounds.size() != this->numStates) { bskLogger.bskLog(BSK_ERROR, "State bounds size mismatch in Gauss Markov model"); return; } //! - Generate base random numbers - ranNums.resize((int64_t) this->numStates); - for(size_t i = 0; i < this->numStates; i++) { + ranNums.resize((int64_t)this->numStates); + for (size_t i = 0; i < this->numStates; i++) { ranNums[i] = this->rNum(rGen); } @@ -95,9 +94,9 @@ void GaussMarkov::computeNextState() this->currentState += errorVector; //! - Apply bounds if needed - for(size_t i = 0; i < this->numStates; i++) { - if(this->stateBounds[i] > 0.0) { - if(fabs(this->currentState[i]) > this->stateBounds[i]) { + for (size_t i = 0; i < this->numStates; i++) { + if (this->stateBounds[i] > 0.0) { + if (fabs(this->currentState[i]) > this->stateBounds[i]) { this->currentState[i] = copysign(this->stateBounds[i], this->currentState[i]); } } diff --git a/src/architecture/utilities/gauss_markov.h b/src/architecture/utilities/gauss_markov.h index cc9d65455a..42104a5db3 100644 --- a/src/architecture/utilities/gauss_markov.h +++ b/src/architecture/utilities/gauss_markov.h @@ -27,7 +27,6 @@ #include #include "architecture/utilities/bskLogging.h" - /*! @brief This module is used to apply a second-order bounded Gauss-Markov random walk on top of an upper level process. The intent is that the caller will perform the set methods (setUpperBounds, setNoiseMatrix, setPropMatrix) as often as @@ -36,7 +35,7 @@ class GaussMarkov { -public: + public: GaussMarkov(); GaussMarkov(uint64_t size, uint64_t newSeed = 0x1badcad1); //!< class constructor ~GaussMarkov(); @@ -45,23 +44,29 @@ class GaussMarkov /*!@brief Method does just what it says, seeds the random number generator @param newSeed The seed to use in the random number generator */ - void setRNGSeed(uint64_t newSeed) {rGen.seed((unsigned int)newSeed); RNGSeed = newSeed;} + void setRNGSeed(uint64_t newSeed) + { + rGen.seed((unsigned int)newSeed); + RNGSeed = newSeed; + } /*!@brief Method returns the current random walk state from model @return The private currentState which is the vector of random walk values*/ - Eigen::VectorXd getCurrentState() {return(currentState);} + Eigen::VectorXd getCurrentState() { return (currentState); } /*!@brief Set the upper bounds on the random walk to newBounds @param newBounds the bounds to put on the random walk states */ - void setUpperBounds(Eigen::VectorXd newBounds) { + void setUpperBounds(Eigen::VectorXd newBounds) + { // For normal distribution, ~99.7% of values fall within ±3σ // So bounds should be at least 3x the standard deviation - for(int i = 0; i < noiseMatrix.rows(); i++) { + for (int i = 0; i < noiseMatrix.rows(); i++) { // Only check for warning if bounds are positive (random walk enabled) // and noise is non-zero (random walk active) - if(newBounds[i] > 0 && noiseMatrix(i,i) > 0 && newBounds[i] < 3.0 * noiseMatrix(i,i)) { - bskLogger.bskLog(BSK_WARNING, "GaussMarkov bounds set tighter than 3σ - distribution will be truncated"); + if (newBounds[i] > 0 && noiseMatrix(i, i) > 0 && newBounds[i] < 3.0 * noiseMatrix(i, i)) { + bskLogger.bskLog(BSK_WARNING, + "GaussMarkov bounds set tighter than 3σ - distribution will be truncated"); } } stateBounds = newBounds; @@ -70,25 +75,26 @@ class GaussMarkov /*!@brief Set the noiseMatrix that is used to define error sigmas @param noise The new value to use for the noiseMatrix variable (error sigmas) */ - void setNoiseMatrix(Eigen::MatrixXd noise){noiseMatrix = noise;} + void setNoiseMatrix(Eigen::MatrixXd noise) { noiseMatrix = noise; } /*!@brief Set the propagation matrix that is used to propagate the state. @param prop The new value for the state propagation matrix */ - void setPropMatrix(Eigen::MatrixXd prop){propMatrix = prop;} + void setPropMatrix(Eigen::MatrixXd prop) { propMatrix = prop; } Eigen::VectorXd stateBounds; //!< -- Upper bounds to use for markov - Eigen::VectorXd currentState; //!< -- State of the markov model - Eigen::MatrixXd propMatrix; //!< -- Matrix to propagate error state with - Eigen::MatrixXd noiseMatrix; //!< -- Cholesky-decomposition or matrix square root of the covariance matrix to apply errors with - BSKLogger bskLogger; //!< -- BSK Logging + Eigen::VectorXd currentState; //!< -- State of the markov model + Eigen::MatrixXd propMatrix; //!< -- Matrix to propagate error state with + Eigen::MatrixXd + noiseMatrix; //!< -- Cholesky-decomposition or matrix square root of the covariance matrix to apply errors with + BSKLogger bskLogger; //!< -- BSK Logging -private: + private: void initializeRNG(); - uint64_t RNGSeed; //!< -- Seed for random number generator - std::minstd_rand rGen; //!< -- Random number generator for model - std::normal_distribution rNum; //!< -- Random number distribution for model - uint64_t numStates; //!< -- Number of states to generate noise for + uint64_t RNGSeed; //!< -- Seed for random number generator + std::minstd_rand rGen; //!< -- Random number generator for model + std::normal_distribution rNum; //!< -- Random number distribution for model + uint64_t numStates; //!< -- Number of states to generate noise for }; //! Default bound for the Gauss-Markov model diff --git a/src/architecture/utilities/geodeticConversion.cpp b/src/architecture/utilities/geodeticConversion.cpp index 24cfa309cf..e434170f3e 100644 --- a/src/architecture/utilities/geodeticConversion.cpp +++ b/src/architecture/utilities/geodeticConversion.cpp @@ -20,21 +20,24 @@ #include "rigidBodyKinematics.h" #include "avsEigenSupport.h" - -/*! Converts from a planet-centered inertial position (i.e., J2000 ECI) to a planet-centered, planet-fixed position given a rotation matrix. +/*! Converts from a planet-centered inertial position (i.e., J2000 ECI) to a planet-centered, planet-fixed position +given a rotation matrix. @param pciPosition: [m] Position vector in PCI coordinates @param J20002Pfix: [-] 3x3 rotation matrix representing the rotation between PCPF and ECI frames @return pcpfPosition: [m] Position vector in PCPF coordinates */ -Eigen::Vector3d PCI2PCPF(Eigen::Vector3d pciPosition, double J20002Pfix[3][3]){ +Eigen::Vector3d +PCI2PCPF(Eigen::Vector3d pciPosition, double J20002Pfix[3][3]) +{ // cArray2EigenMatrixd expects a column major input, thus the result is transposed - Eigen::MatrixXd dcm_PN = cArray2EigenMatrixXd(*J20002Pfix,3,3).transpose(); + Eigen::MatrixXd dcm_PN = cArray2EigenMatrixXd(*J20002Pfix, 3, 3).transpose(); Eigen::Vector3d pcpfPosition = dcm_PN * pciPosition; return pcpfPosition; } -/*! Converts from a planet-centered, planet-fixed coordinates to latitude/longitude/altitude (LLA) coordinates given a planet radius. +/*! Converts from a planet-centered, planet-fixed coordinates to latitude/longitude/altitude (LLA) coordinates given a +planet radius. @param pcpfPosition: [m] Position vector in PCPF coordinates @param planetEqRad: [m] Planetary radius, assumed to be constant (i.e., spherical) @param planetPoRad: [m] Planetary polar used for elliptical surfaces if provided, otherwise spherical, @@ -43,48 +46,47 @@ Eigen::Vector3d PCI2PCPF(Eigen::Vector3d pciPosition, double J20002Pfix[3][3]){ [1] : [rad] longitude across planetary meridian [2] : [alt] altitude above planet radius */ -Eigen::Vector3d PCPF2LLA(Eigen::Vector3d pcpfPosition, double planetEqRad, double planetPoRad){ - - Eigen::Vector3d llaPosition; - llaPosition.fill(0.0); - if(planetPoRad < 0.0) { - llaPosition[2] = - pcpfPosition.norm() - planetEqRad; // Altitude is the height above assumed-spherical planet surface - llaPosition[1] = atan2(pcpfPosition[1], pcpfPosition[0]); - llaPosition[0] = atan2(pcpfPosition[2], sqrt(pow(pcpfPosition[0], 2.0) + pow(pcpfPosition[1], 2.0))); - } - else - { - - const int nr_kapp_iter = 10; - const double good_kappa_acc = 1.0E-13; - double planetEcc2 = 1.0 - planetPoRad*planetPoRad/(planetEqRad*planetEqRad); - double kappa = 1.0/(1.0-planetEcc2); - double p2 = pcpfPosition[0]*pcpfPosition[0] + pcpfPosition[1]*pcpfPosition[1]; - double z2 = pcpfPosition[2]*pcpfPosition[2]; - for(int i=0; i= 0) - { - planetEcc2 = 1.0 - planetPoRad*planetPoRad/(planetEqRad*planetEqRad); + if (planetPoRad >= 0) { + planetEcc2 = 1.0 - planetPoRad * planetPoRad / (planetEqRad * planetEqRad); } double sPhi = sin(llaPosition[0]); - double nVal = planetEqRad/sqrt(1.0 - planetEcc2*sPhi*sPhi); - pcpfPosition[0] = (nVal+llaPosition[2])*cos(llaPosition[0])*cos(llaPosition[1]); - pcpfPosition[1] = (nVal+llaPosition[2])*cos(llaPosition[0])*sin(llaPosition[1]); - pcpfPosition[2] = ((1.0-planetEcc2)*nVal + llaPosition[2])*sPhi; + double nVal = planetEqRad / sqrt(1.0 - planetEcc2 * sPhi * sPhi); + pcpfPosition[0] = (nVal + llaPosition[2]) * cos(llaPosition[0]) * cos(llaPosition[1]); + pcpfPosition[1] = (nVal + llaPosition[2]) * cos(llaPosition[0]) * sin(llaPosition[1]); + pcpfPosition[2] = ((1.0 - planetEcc2) * nVal + llaPosition[2]) * sPhi; return pcpfPosition; } -/*! Converts from a Lat/Long/Altitude coordinates to planet-centered inertialcoordinates given a planet radius and rotation matrix. +/*! Converts from a Lat/Long/Altitude coordinates to planet-centered inertialcoordinates given a planet radius and +rotation matrix. @param pcpfPosition: [m] Position vector in planet centered, planet fixed coordinates @param J20002Pfix: [-] Rotation between inertial and pcf coordinates. @return pciPosition: [m] Final position in the planet-centered inertial frame. */ -Eigen::Vector3d PCPF2PCI(Eigen::Vector3d pcpfPosition, double J20002Pfix[3][3]) +Eigen::Vector3d +PCPF2PCI(Eigen::Vector3d pcpfPosition, double J20002Pfix[3][3]) { // cArray2EigenMatrixd expects a column major input, thus the result is transposed - Eigen::MatrixXd dcm_NP = cArray2EigenMatrixXd(*J20002Pfix,3,3); + Eigen::MatrixXd dcm_NP = cArray2EigenMatrixXd(*J20002Pfix, 3, 3); Eigen::Vector3d pciPosition = dcm_NP * pcpfPosition; return pciPosition; } -/*! Converts from a planet-centered inertial coordinates to latitutde/longitude/altitude (LLA) coordinates given a planet radius and rotation matrix. +/*! Converts from a planet-centered inertial coordinates to latitutde/longitude/altitude (LLA) coordinates given a +planet radius and rotation matrix. @param llaPosition: Final position in latitude/longitude/altitude coordinates [0] : [rad] latitude above planetary equator [1] : [rad] longitude across planetary meridian @@ -148,21 +156,23 @@ Eigen::Vector3d PCPF2PCI(Eigen::Vector3d pcpfPosition, double J20002Pfix[3][3]) @param planetPoRad: [m] Planetary polar used for elliptical surfaces if provided, otherwise spherical, @return pciPosition : [m] Position in inertial coordinates. */ -Eigen::Vector3d LLA2PCI(Eigen::Vector3d llaPosition, double J20002Pfix[3][3], double planetEqRad, double planetPoRad) +Eigen::Vector3d +LLA2PCI(Eigen::Vector3d llaPosition, double J20002Pfix[3][3], double planetEqRad, double planetPoRad) { - Eigen::Vector3d pcpfPosition = LLA2PCPF(llaPosition, planetEqRad, planetPoRad); - Eigen::Vector3d pciPosition = PCPF2PCI(pcpfPosition, J20002Pfix); - return pciPosition; + Eigen::Vector3d pcpfPosition = LLA2PCPF(llaPosition, planetEqRad, planetPoRad); + Eigen::Vector3d pciPosition = PCPF2PCI(pcpfPosition, J20002Pfix); + return pciPosition; } -Eigen::Matrix3d C_PCPF2SEZ(double lat, double longitude) +Eigen::Matrix3d +C_PCPF2SEZ(double lat, double longitude) { - double m1[3][3]; - double m2[3][3]; - Euler2(M_PI_2-lat, m1); - Euler3(longitude, m2); - - Eigen::MatrixXd rot2 = cArray2EigenMatrix3d(*m1); - Eigen::MatrixXd rot3 = cArray2EigenMatrix3d(*m2); - return rot2*rot3; + double m1[3][3]; + double m2[3][3]; + Euler2(M_PI_2 - lat, m1); + Euler3(longitude, m2); + + Eigen::MatrixXd rot2 = cArray2EigenMatrix3d(*m1); + Eigen::MatrixXd rot3 = cArray2EigenMatrix3d(*m2); + return rot2 * rot3; } diff --git a/src/architecture/utilities/geodeticConversion.h b/src/architecture/utilities/geodeticConversion.h index e12d1bbd27..3979a1fb4d 100644 --- a/src/architecture/utilities/geodeticConversion.h +++ b/src/architecture/utilities/geodeticConversion.h @@ -23,21 +23,28 @@ OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. #include #include - /*! @brief Collection of utility functions for converting in/out of planet-centric reference frames. -The geodeticConversion library contains simple transformations between inertial coordinates and planet-fixed coordinates in a general way. +The geodeticConversion library contains simple transformations between inertial coordinates and planet-fixed coordinates +in a general way. No support is provided for non-spherical bodies. Transformations are scripted from Vallado. */ -Eigen::Vector3d PCI2PCPF(Eigen::Vector3d pciPosition, double J20002Pfix[3][3]); -Eigen::Vector3d PCPF2LLA(Eigen::Vector3d pciPosition, double planetEqRadius, double planetPoRad=-1.0); -Eigen::Vector3d PCI2LLA(Eigen::Vector3d pciPosition, double J20002Pfix[3][3], double planetEqRad, double planetPoRad=-1.0); -Eigen::Vector3d LLA2PCPF(Eigen::Vector3d llaPosition, double planetEqRad, double planetPoRad=-1.0); -Eigen::Vector3d PCPF2PCI(Eigen::Vector3d pcpfPosition, double J20002Pfix[3][3]); -Eigen::Vector3d LLA2PCI(Eigen::Vector3d llaPosition, double J20002Pfix[3][3], double planetEqRad, double planetPoRad=-1.0); -Eigen::Matrix3d C_PCPF2SEZ(double lat, double longitude); +Eigen::Vector3d +PCI2PCPF(Eigen::Vector3d pciPosition, double J20002Pfix[3][3]); +Eigen::Vector3d +PCPF2LLA(Eigen::Vector3d pciPosition, double planetEqRadius, double planetPoRad = -1.0); +Eigen::Vector3d +PCI2LLA(Eigen::Vector3d pciPosition, double J20002Pfix[3][3], double planetEqRad, double planetPoRad = -1.0); +Eigen::Vector3d +LLA2PCPF(Eigen::Vector3d llaPosition, double planetEqRad, double planetPoRad = -1.0); +Eigen::Vector3d +PCPF2PCI(Eigen::Vector3d pcpfPosition, double J20002Pfix[3][3]); +Eigen::Vector3d +LLA2PCI(Eigen::Vector3d llaPosition, double J20002Pfix[3][3], double planetEqRad, double planetPoRad = -1.0); +Eigen::Matrix3d +C_PCPF2SEZ(double lat, double longitude); #endif diff --git a/src/architecture/utilities/keplerianOrbit.cpp b/src/architecture/utilities/keplerianOrbit.cpp index 6316ad0e29..e82c1bc880 100644 --- a/src/architecture/utilities/keplerianOrbit.cpp +++ b/src/architecture/utilities/keplerianOrbit.cpp @@ -28,117 +28,237 @@ KeplerianOrbit::KeplerianOrbit() } /*! The constructor requires orbital elements and a gravitational constant value */ -KeplerianOrbit::KeplerianOrbit(ClassicElements oe, const double mu) : mu(mu), - semi_major_axis(oe.a), - eccentricity(oe.e), - inclination(oe.i), - argument_of_periapsis(oe.omega), - right_ascension(oe.Omega), - true_anomaly(oe.f){ +KeplerianOrbit::KeplerianOrbit(ClassicElements oe, const double mu) + : mu(mu) + , semi_major_axis(oe.a) + , eccentricity(oe.e) + , inclination(oe.i) + , argument_of_periapsis(oe.omega) + , right_ascension(oe.Omega) + , true_anomaly(oe.f) +{ this->change_orbit(); } /*! The copy constructor works with python copy*/ -KeplerianOrbit::KeplerianOrbit(const KeplerianOrbit &orig) : mu(orig.mu), - semi_major_axis(orig.a()), - eccentricity(orig.e()), - inclination(orig.i()), - argument_of_periapsis(orig.omega()), - right_ascension(orig.RAAN()), - true_anomaly(orig.f()) +KeplerianOrbit::KeplerianOrbit(const KeplerianOrbit& orig) + : mu(orig.mu) + , semi_major_axis(orig.a()) + , eccentricity(orig.e()) + , inclination(orig.i()) + , argument_of_periapsis(orig.omega()) + , right_ascension(orig.RAAN()) + , true_anomaly(orig.f()) { this->change_orbit(); } /*! Generic Destructor */ -KeplerianOrbit::~KeplerianOrbit() -{ -} +KeplerianOrbit::~KeplerianOrbit() {} /*! body position vector relative to planet */ -Eigen::Vector3d KeplerianOrbit::r_BP_P() const { +Eigen::Vector3d +KeplerianOrbit::r_BP_P() const +{ return this->position_BP_P; } /*! body velocity vector relative to planet */ -Eigen::Vector3d KeplerianOrbit::v_BP_P() const{ +Eigen::Vector3d +KeplerianOrbit::v_BP_P() const +{ return this->velocity_BP_P; - } /*! angular momentum of body relative to planet */ -Eigen::Vector3d KeplerianOrbit::h_BP_P() const{ +Eigen::Vector3d +KeplerianOrbit::h_BP_P() const +{ return this->orbital_angular_momentum_P; } /*! return mean anomaly angle */ -double KeplerianOrbit::M() const {return this->mean_anomaly;} +double +KeplerianOrbit::M() const +{ + return this->mean_anomaly; +} /*! return mean orbit rate */ -double KeplerianOrbit::n() const {return this->mean_motion;}; //!< return mean orbit rate +double +KeplerianOrbit::n() const +{ + return this->mean_motion; +}; //!< return mean orbit rate /*! return orbit period */ -double KeplerianOrbit::P() const {return this->orbital_period;}; //!< return orbital period +double +KeplerianOrbit::P() const +{ + return this->orbital_period; +}; //!< return orbital period /*! return true anomaly */ -double KeplerianOrbit::f() const {return this->true_anomaly;}; //!< return true anomaly +double +KeplerianOrbit::f() const +{ + return this->true_anomaly; +}; //!< return true anomaly /*! return true anomaly rate */ -double KeplerianOrbit::fDot() const {return this->true_anomaly_rate;}; +double +KeplerianOrbit::fDot() const +{ + return this->true_anomaly_rate; +}; /*! return right ascencion of the ascending node */ -double KeplerianOrbit::RAAN() const {return this->right_ascension;}; +double +KeplerianOrbit::RAAN() const +{ + return this->right_ascension; +}; /*! return argument of periapses */ -double KeplerianOrbit::omega() const {return this->argument_of_periapsis;}; +double +KeplerianOrbit::omega() const +{ + return this->argument_of_periapsis; +}; /*! return inclination angle */ -double KeplerianOrbit::i() const {return this->inclination;}; +double +KeplerianOrbit::i() const +{ + return this->inclination; +}; /*! return eccentricty */ -double KeplerianOrbit::e() const {return this->eccentricity;}; +double +KeplerianOrbit::e() const +{ + return this->eccentricity; +}; /*! return semi-major axis */ -double KeplerianOrbit::a() const {return this->semi_major_axis;}; +double +KeplerianOrbit::a() const +{ + return this->semi_major_axis; +}; /*! return orbital angular momentum magnitude */ -double KeplerianOrbit::h() const {return this->h_BP_P().norm();}; +double +KeplerianOrbit::h() const +{ + return this->h_BP_P().norm(); +}; /*! return orbital energy */ -double KeplerianOrbit::Energy(){return this->orbital_energy;}; +double +KeplerianOrbit::Energy() +{ + return this->orbital_energy; +}; /*! return orbit radius */ -double KeplerianOrbit::r() const {return this->r_BP_P().norm();}; +double +KeplerianOrbit::r() const +{ + return this->r_BP_P().norm(); +}; /*! return velocity magnitude */ -double KeplerianOrbit::v() const {return this->v_BP_P().norm();}; +double +KeplerianOrbit::v() const +{ + return this->v_BP_P().norm(); +}; /*! return radius at apoapses */ -double KeplerianOrbit::r_a() const {return this->r_apogee;}; +double +KeplerianOrbit::r_a() const +{ + return this->r_apogee; +}; /*! return radius at periapses */ -double KeplerianOrbit::r_p() const {return this->r_perigee;}; +double +KeplerianOrbit::r_p() const +{ + return this->r_perigee; +}; /*! return flight path angle */ -double KeplerianOrbit::fpa() const {return this->flight_path_angle;}; +double +KeplerianOrbit::fpa() const +{ + return this->flight_path_angle; +}; /*! return eccentric anomaly angle */ -double KeplerianOrbit::E() const {return this->eccentric_anomaly;}; +double +KeplerianOrbit::E() const +{ + return this->eccentric_anomaly; +}; /*! return semi-latus rectum or the parameter */ -double KeplerianOrbit::p() const {return this->semi_parameter;}; +double +KeplerianOrbit::p() const +{ + return this->semi_parameter; +}; /*! return radius rate */ -double KeplerianOrbit::rDot() const {return this->radial_rate;}; +double +KeplerianOrbit::rDot() const +{ + return this->radial_rate; +}; /*! return escape velocity */ -double KeplerianOrbit::c3() const {return this->v_infinity;}; +double +KeplerianOrbit::c3() const +{ + return this->v_infinity; +}; /*! set semi-major axis */ -void KeplerianOrbit::set_a(double a){this->semi_major_axis = a; this->change_orbit();}; +void +KeplerianOrbit::set_a(double a) +{ + this->semi_major_axis = a; + this->change_orbit(); +}; /*! set eccentricity */ -void KeplerianOrbit::set_e(double e){this->eccentricity = e; this->change_orbit();}; +void +KeplerianOrbit::set_e(double e) +{ + this->eccentricity = e; + this->change_orbit(); +}; /*! set inclination angle */ -void KeplerianOrbit::set_i(double i){this->inclination = i; this->change_orbit();}; +void +KeplerianOrbit::set_i(double i) +{ + this->inclination = i; + this->change_orbit(); +}; /*! set argument of periapsis */ -void KeplerianOrbit::set_omega(double omega){this->argument_of_periapsis = omega; this->change_orbit();}; +void +KeplerianOrbit::set_omega(double omega) +{ + this->argument_of_periapsis = omega; + this->change_orbit(); +}; /*! set right ascension of the ascending node */ -void KeplerianOrbit::set_RAAN(double RAAN){this->right_ascension = RAAN; this->change_orbit();}; +void +KeplerianOrbit::set_RAAN(double RAAN) +{ + this->right_ascension = RAAN; + this->change_orbit(); +}; /*! set true anomaly angle */ -void KeplerianOrbit::set_f(double f){this->true_anomaly = f; this->change_f();}; - - +void +KeplerianOrbit::set_f(double f) +{ + this->true_anomaly = f; + this->change_f(); +}; /*! This method returns the orbital element set for the orbit @return ClassicElements oe */ -ClassicElements KeplerianOrbit::oe(){ +ClassicElements +KeplerianOrbit::oe() +{ ClassicElements elements; elements.a = this->semi_major_axis; elements.e = this->eccentricity; @@ -146,7 +266,7 @@ ClassicElements KeplerianOrbit::oe(){ elements.f = this->true_anomaly; elements.omega = this->argument_of_periapsis; elements.Omega = this->right_ascension; - elements.rApoap= this->r_apogee; + elements.rApoap = this->r_apogee; elements.rPeriap = this->r_perigee; elements.alpha = 1.0 / elements.a; elements.rmag = this->orbit_radius; @@ -155,7 +275,9 @@ ClassicElements KeplerianOrbit::oe(){ /*! This method populates all outputs from orbital elements coherently if any of the * classical orbital elements are changed*/ -void KeplerianOrbit::change_orbit(){ +void +KeplerianOrbit::change_orbit() +{ this->change_f(); this->orbital_angular_momentum_P = this->position_BP_P.cross(this->velocity_BP_P); this->mean_motion = sqrt(this->mu / pow(this->semi_major_axis, 3)); @@ -167,21 +289,26 @@ void KeplerianOrbit::change_orbit(){ } /*! This method only changes the outputs dependent on true anomaly so that one * orbit may be queried at various points along the orbit*/ -void KeplerianOrbit::change_f(){ +void +KeplerianOrbit::change_f() +{ double r[3]; double v[3]; - ClassicElements oe = this->oe(); // - elem2rv(this->mu, &oe, r, v); // - this->position_BP_P = cArray2EigenVector3d(r); // - this->velocity_BP_P = cArray2EigenVector3d(v); // - this->true_anomaly_rate = this->n() * pow(this->a(), 2) * sqrt(1 - pow(this->e(), 2)) / pow(this->r(), 2); // + ClassicElements oe = this->oe(); // + elem2rv(this->mu, &oe, r, v); // + this->position_BP_P = cArray2EigenVector3d(r); // + this->velocity_BP_P = cArray2EigenVector3d(v); // + this->true_anomaly_rate = this->n() * pow(this->a(), 2) * sqrt(1 - pow(this->e(), 2)) / pow(this->r(), 2); // this->radial_rate = this->r() * this->fDot() * this->e() * sin(this->f()) / (1 + this->e() * cos(this->f())); // - this->eccentric_anomaly = safeAcos(this->e() + cos(this->f()) / (1 + this->e() * cos(this->f()))); // - this->mean_anomaly = this->E() - this->e() * sin(this->E()); // - this->flight_path_angle = safeAcos(sqrt((1 - pow(this->e(), 2)) / (1 - pow(this->e(), 2)*pow(cos(this->E()), 2)))); // + this->eccentric_anomaly = safeAcos(this->e() + cos(this->f()) / (1 + this->e() * cos(this->f()))); // + this->mean_anomaly = this->E() - this->e() * sin(this->E()); // + this->flight_path_angle = + safeAcos(sqrt((1 - pow(this->e(), 2)) / (1 - pow(this->e(), 2) * pow(cos(this->E()), 2)))); // } /*! This method sets the gravitational constants of the body being orbited */ -void KeplerianOrbit::set_mu(const double mu){ +void +KeplerianOrbit::set_mu(const double mu) +{ this->mu = mu; } diff --git a/src/architecture/utilities/keplerianOrbit.h b/src/architecture/utilities/keplerianOrbit.h index b65f05343e..b0d9aee6a4 100644 --- a/src/architecture/utilities/keplerianOrbit.h +++ b/src/architecture/utilities/keplerianOrbit.h @@ -23,20 +23,19 @@ #include #include "architecture/utilities/astroConstants.h" - //! @brief The KeplerianOrbit class represents an elliptical orbit and provides a coherent set of //! common outputs such as position and velocity, orbital period, semi-parameter, etc. It uses the //! utility orbitalMotion to do orbital element to position and velocity conversion. -class KeplerianOrbit { -public: +class KeplerianOrbit +{ + public: KeplerianOrbit(); KeplerianOrbit(ClassicElements oe, const double mu); - KeplerianOrbit(const KeplerianOrbit &orig); + KeplerianOrbit(const KeplerianOrbit& orig); ~KeplerianOrbit(); - - Eigen::Vector3d r_BP_P() const; //!< body position vector relative to planet - Eigen::Vector3d v_BP_P() const; //!< body velocity vector relative to planet + Eigen::Vector3d r_BP_P() const; //!< body position vector relative to planet + Eigen::Vector3d v_BP_P() const; //!< body velocity vector relative to planet Eigen::Vector3d h_BP_P() const; //!< angular momentum of body relative to planet double M() const; double n() const; @@ -68,9 +67,9 @@ class KeplerianOrbit { void set_RAAN(double RAAN); void set_f(double f); -private: - double mu = MU_EARTH*pow(10,9); // convert to m^3/s^2 - double semi_major_axis = 1E5*1000; // convert to meters + private: + double mu = MU_EARTH * pow(10, 9); // convert to m^3/s^2 + double semi_major_axis = 1E5 * 1000; // convert to meters double eccentricity = 1E-5; double inclination{}; double argument_of_periapsis{}; @@ -92,7 +91,8 @@ class KeplerianOrbit { Eigen::Vector3d orbital_angular_momentum_P; Eigen::Vector3d position_BP_P; Eigen::Vector3d velocity_BP_P; -private: + + private: void change_orbit(); void change_f(); }; diff --git a/src/architecture/utilities/linearAlgebra.c b/src/architecture/utilities/linearAlgebra.c index 71db037ceb..a886d79d2d 100644 --- a/src/architecture/utilities/linearAlgebra.c +++ b/src/architecture/utilities/linearAlgebra.c @@ -24,126 +24,117 @@ #include #include +#define MOVE_DOUBLE(source, dim, destination) (memmove((void*)(destination), (void*)(source), sizeof(double) * (dim))) - -#define MOVE_DOUBLE(source, dim, destination) (memmove((void*)(destination), (void*)(source), sizeof(double)*(dim))) - -void vElementwiseMult(double *v1, size_t dim, - double *v2, double *result) +void +vElementwiseMult(double* v1, size_t dim, double* v2, double* result) { size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v1[i] * v2[i]; } } -void vCopy(double *v, size_t dim, - double *result) +void +vCopy(double* v, size_t dim, double* result) { size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v[i]; } } -void vSetZero(double *v, - size_t dim) +void +vSetZero(double* v, size_t dim) { size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { v[i] = 0.0; } } -void vSetOnes(double *v, - size_t dim) +void +vSetOnes(double* v, size_t dim) { size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { v[i] = 1.0; } } -void vAdd(double *v1, size_t dim, - double *v2, - double *result) +void +vAdd(double* v1, size_t dim, double* v2, double* result) { size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v1[i] + v2[i]; } } -void vSubtract(double *v1, size_t dim, - double *v2, - double *result) +void +vSubtract(double* v1, size_t dim, double* v2, double* result) { size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v1[i] - v2[i]; } } -void vScale(double scaleFactor, double *v, - size_t dim, - double *result) +void +vScale(double scaleFactor, double* v, size_t dim, double* result) { size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v[i] * scaleFactor; } } -double vDot(double *v1, size_t dim, - double *v2) +double +vDot(double* v1, size_t dim, double* v2) { size_t i; double result = 0.0; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result += v1[i] * v2[i]; } return result; } -void vOuterProduct(double *v1, size_t dim1, - double *v2, size_t dim2, - void *result) +void +vOuterProduct(double* v1, size_t dim1, double* v2, size_t dim2, void* result) { - double *m_result = (double *)result; + double* m_result = (double*)result; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { m_result[MXINDEX(dim2, i, j)] = v1[i] * v2[j]; } } - } -void vtMultM(double *v, - void *mx, size_t dim1, size_t dim2, - void *result) +void +vtMultM(double* v, void* mx, size_t dim1, size_t dim2, void* result) { size_t dim11 = 1; size_t dim12 = dim1; size_t dim22 = dim2; - double *m_mx1 = (double *)v; - double *m_mx2 = (double *)mx; + double* m_mx1 = (double*)v; + double* m_mx2 = (double*)mx; double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - if (dim11*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + if (dim11 * dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } size_t i; size_t j; size_t k; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[MXINDEX(dim22, i, j)] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, i, k)] * m_mx2[MXINDEX(dim22, k, j)]; } } @@ -152,28 +143,26 @@ void vtMultM(double *v, MOVE_DOUBLE(m_result, dim11 * dim22, result); } -void vtMultMt(double *v, - void *mx, size_t dim1, size_t dim2, - void *result) +void +vtMultMt(double* v, void* mx, size_t dim1, size_t dim2, void* result) { size_t dim11 = 1; size_t dim12 = dim2; size_t dim22 = dim1; - double *m_mx1 = (double *)v; - double *m_mx2 = (double *)mx; + double* m_mx1 = (double*)v; + double* m_mx2 = (double*)mx; double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - if (dim11*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + if (dim11 * dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } size_t i; size_t j; size_t k; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[MXINDEX(dim22, i, j)] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, i, k)] * m_mx2[MXINDEX(dim22, j, k)]; } } @@ -182,71 +171,73 @@ void vtMultMt(double *v, MOVE_DOUBLE(m_result, dim11 * dim22, result); } -double vNorm(double *v, size_t dim) +double +vNorm(double* v, size_t dim) { return sqrt(vDot(v, dim, v)); } -double vMax(double *array, size_t dim) +double +vMax(double* array, size_t dim) { size_t i; double result; result = array[0]; - for(i=1; iresult){ + for (i = 1; i < dim; i++) { + if (array[i] > result) { result = array[i]; } } return result; } - -double vMaxAbs(double *array, size_t dim) +double +vMaxAbs(double* array, size_t dim) { size_t i; double result; result = fabs(array[0]); - for(i=1; iresult){ + for (i = 1; i < dim; i++) { + if (fabs(array[i]) > result) { result = fabs(array[i]); } } return result; } - -void vNormalize(double *v, size_t dim, double *result) +void +vNormalize(double* v, size_t dim, double* result) { double norm = vNorm(v, dim); - if(norm > DB0_EPS) { + if (norm > DB0_EPS) { vScale(1.0 / norm, v, dim, result); } else { vSetZero(result, dim); } } -int vIsEqual(double *v1, size_t dim, - double *v2, - double accuracy) +int +vIsEqual(double* v1, size_t dim, double* v2, double accuracy) { size_t i; - for(i = 0; i < dim; i++) { - if(fabs(v1[i] - v2[i]) > accuracy) { + for (i = 0; i < dim; i++) { + if (fabs(v1[i] - v2[i]) > accuracy) { return 0; } } return 1; } -int vIsZero(double *v, size_t dim, double accuracy) +int +vIsZero(double* v, size_t dim, double accuracy) { size_t i; int result = 1; - for(i = 0; i < dim; i++) { - if(fabs(v[i]) > accuracy) { + for (i = 0; i < dim; i++) { + if (fabs(v[i]) > accuracy) { result = 0; break; } @@ -255,13 +246,14 @@ int vIsZero(double *v, size_t dim, double accuracy) return result; } -void vPrint(FILE *pFile, const char *name, double *v, size_t dim) +void +vPrint(FILE* pFile, const char* name, double* v, size_t dim) { size_t i; fprintf(pFile, "%s = [", name); - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { fprintf(pFile, "%20.15g", v[i]); - if(i != dim - 1) { + if (i != dim - 1) { fprintf(pFile, ", "); } } @@ -269,226 +261,213 @@ void vPrint(FILE *pFile, const char *name, double *v, size_t dim) } /*I hope you allocated the output prior to calling this!*/ -void vSort(double *Input, double *Output, size_t dim) +void +vSort(double* Input, double* Output, size_t dim) { size_t i, j; - memcpy(Output, Input, dim*sizeof(double)); - for(i=0; iOutput[j+1]) - { - double temp = Output[j+1]; - Output[j+1] = Output[j]; + memcpy(Output, Input, dim * sizeof(double)); + for (i = 0; i < dim; i++) { + for (j = 0; j < dim - 1; j++) { + if (Output[j] > Output[j + 1]) { + double temp = Output[j + 1]; + Output[j + 1] = Output[j]; Output[j] = temp; } } } } - -void v2Set(double v0, double v1, - double result[2]) +void +v2Set(double v0, double v1, double result[2]) { result[0] = v0; result[1] = v1; } -void v2SetZero(double v[2]) +void +v2SetZero(double v[2]) { size_t dim = 2; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { v[i] = 0.0; } } -void v2Copy(double v[2], - double result[2]) +void +v2Copy(double v[2], double result[2]) { size_t dim = 2; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v[i]; } } -void v2Scale(double scaleFactor, - double v[2], - double result[2]) +void +v2Scale(double scaleFactor, double v[2], double result[2]) { size_t dim = 2; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v[i] * scaleFactor; } } -double v2Dot(double v1[2], - double v2[2]) +double +v2Dot(double v1[2], double v2[2]) { size_t dim = 2; size_t i; double result = 0.0; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result += v1[i] * v2[i]; } return result; } -int v2IsEqual(double v1[2], - double v2[2], - double accuracy) +int +v2IsEqual(double v1[2], double v2[2], double accuracy) { size_t dim = 2; size_t i; - for(i = 0; i < dim; i++) { - if(fabs(v1[i] - v2[i]) > accuracy) { + for (i = 0; i < dim; i++) { + if (fabs(v1[i] - v2[i]) > accuracy) { return 0; } } return 1; } -int v2IsZero(double v[2], - double accuracy) +int +v2IsZero(double v[2], double accuracy) { size_t dim = 2; size_t i; - for(i = 0; i < dim; i++) { - if(fabs(v[i]) > accuracy) { + for (i = 0; i < dim; i++) { + if (fabs(v[i]) > accuracy) { return 0; } } return 1; } -void v2Add(double v1[2], - double v2[2], - double result[2]) +void +v2Add(double v1[2], double v2[2], double result[2]) { size_t dim = 2; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v1[i] + v2[i]; } } -void v2Subtract(double v1[2], - double v2[2], - double result[2]) +void +v2Subtract(double v1[2], double v2[2], double result[2]) { size_t dim = 2; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v1[i] - v2[i]; } } -double v2Norm(double v[2]) +double +v2Norm(double v[2]) { return sqrt(v2Dot(v, v)); } -void v2Normalize(double v[2], double result[2]) +void +v2Normalize(double v[2], double result[2]) { double norm = v2Norm(v); - if(norm > DB0_EPS) { + if (norm > DB0_EPS) { v2Scale(1. / norm, v, result); } else { v2SetZero(result); } } - - - - - -void v3Set(double v0, double v1, double v2, - double result[3]) +void +v3Set(double v0, double v1, double v2, double result[3]) { result[0] = v0; result[1] = v1; result[2] = v2; } -void v3Copy(double v[3], - double result[3]) +void +v3Copy(double v[3], double result[3]) { size_t dim = 3; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v[i]; } } -void v3SetZero(double v[3]) +void +v3SetZero(double v[3]) { size_t dim = 3; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { v[i] = 0.0; } } -void v3Add(double v1[3], - double v2[3], - double result[3]) +void +v3Add(double v1[3], double v2[3], double result[3]) { size_t dim = 3; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v1[i] + v2[i]; } } -void v3Subtract(double v1[3], - double v2[3], - double result[3]) +void +v3Subtract(double v1[3], double v2[3], double result[3]) { size_t dim = 3; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v1[i] - v2[i]; } } -void v3Scale(double scaleFactor, - double v[3], - double result[3]) +void +v3Scale(double scaleFactor, double v[3], double result[3]) { size_t dim = 3; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v[i] * scaleFactor; } } -double v3Dot(double v1[3], - double v2[3]) +double +v3Dot(double v1[3], double v2[3]) { return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; } -void v3OuterProduct(double v1[3], - double v2[3], - double result[3][3]) +void +v3OuterProduct(double v1[3], double v2[3], double result[3][3]) { size_t dim = 3; size_t i; size_t j; - for(i = 0; i < dim; i++) { - for(j = 0; j < dim; j++) { + for (i = 0; i < dim; i++) { + for (j = 0; j < dim; j++) { result[i][j] = v1[i] * v2[j]; } } } -void v3tMultM33(double v[3], - double mx[3][3], - double result[3]) +void +v3tMultM33(double v[3], double mx[3][3], double result[3]) { size_t dim11 = 1; size_t dim12 = 3; @@ -497,10 +476,10 @@ void v3tMultM33(double v[3], size_t j; size_t k; double m_result[3]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[j] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[j] += v[k] * mx[k][j]; } } @@ -508,9 +487,8 @@ void v3tMultM33(double v[3], v3Copy(m_result, result); } -void v3tMultM33t(double v[3], - double mx[3][3], - double result[3]) +void +v3tMultM33t(double v[3], double mx[3][3], double result[3]) { size_t dim11 = 1; size_t dim12 = 3; @@ -519,10 +497,10 @@ void v3tMultM33t(double v[3], size_t j; size_t k; double m_result[3]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[j] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[j] += v[k] * mx[j][k]; } } @@ -530,83 +508,81 @@ void v3tMultM33t(double v[3], v3Copy(m_result, result); } -double v3Norm(double v[3]) +double +v3Norm(double v[3]) { return sqrt(v3Dot(v, v)); } -void v3Normalize(double v[3], double result[3]) +void +v3Normalize(double v[3], double result[3]) { double norm = v3Norm(v); - if(norm > DB0_EPS) { + if (norm > DB0_EPS) { v3Scale(1. / norm, v, result); } else { v3SetZero(result); } } -int v3IsEqual(double v1[3], - double v2[3], - double accuracy) +int +v3IsEqual(double v1[3], double v2[3], double accuracy) { size_t dim = 3; size_t i; - for(i = 0; i < dim; i++) { - if(fabs(v1[i] - v2[i]) > accuracy) { + for (i = 0; i < dim; i++) { + if (fabs(v1[i] - v2[i]) > accuracy) { return 0; } } return 1; } -int v3IsEqualRel(double v1[3], - double v2[3], - double accuracy) +int +v3IsEqualRel(double v1[3], double v2[3], double accuracy) { size_t dim = 3; size_t i; double norm; norm = v3Norm(v1); - for(i = 0; i < dim; i++) { - if(fabs(v1[i] - v2[i])/norm > accuracy) { + for (i = 0; i < dim; i++) { + if (fabs(v1[i] - v2[i]) / norm > accuracy) { return 0; } } return 1; } - - -int v3IsZero(double v[3], - double accuracy) +int +v3IsZero(double v[3], double accuracy) { size_t dim = 3; size_t i; - for(i = 0; i < dim; i++) { - if(fabs(v[i]) > accuracy) { + for (i = 0; i < dim; i++) { + if (fabs(v[i]) > accuracy) { return 0; } } return 1; } -void v3Print(FILE *pFile, const char *name, double v[3]) +void +v3Print(FILE* pFile, const char* name, double v[3]) { size_t dim = 3; size_t i; fprintf(pFile, "%s = [", name); - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { fprintf(pFile, "%20.15g", v[i]); - if(i != dim - 1) { + if (i != dim - 1) { fprintf(pFile, ", "); } } fprintf(pFile, "]\n"); } -void v3Cross(double v1[3], - double v2[3], - double result[3]) +void +v3Cross(double v1[3], double v2[3], double result[3]) { double v1c[3]; double v2c[3]; @@ -617,30 +593,27 @@ void v3Cross(double v1[3], result[2] = v1c[0] * v2c[1] - v1c[1] * v2c[0]; } -void v3Perpendicular(double v[3], - double result[3]) +void +v3Perpendicular(double v[3], double result[3]) { if (fabs(v[0]) > DB0_EPS) { - result[0] = -(v[1]+v[2]) / v[0]; + result[0] = -(v[1] + v[2]) / v[0]; result[1] = 1; result[2] = 1; - } - else if (fabs(v[1]) > DB0_EPS) { + } else if (fabs(v[1]) > DB0_EPS) { result[0] = 1; - result[1] = -(v[0]+v[2]) / v[1]; + result[1] = -(v[0] + v[2]) / v[1]; result[2] = 1; - } - else { + } else { result[0] = 1; result[1] = 1; - result[2] = -(v[0]+v[1]) / v[2]; + result[2] = -(v[0] + v[1]) / v[2]; } v3Normalize(result, result); } - -void v3Tilde(double v[3], - double result[3][3]) +void +v3Tilde(double v[3], double result[3][3]) { result[0][0] = 0.0; result[0][1] = -v[2]; @@ -653,18 +626,18 @@ void v3Tilde(double v[3], result[2][2] = 0.0; } -void v3Sort(double v[3], - double result[3]) +void +v3Sort(double v[3], double result[3]) { double temp; v3Copy(v, result); - if(result[0] < result[1]) { + if (result[0] < result[1]) { temp = result[0]; result[0] = result[1]; result[1] = temp; } - if(result[1] < result[2]) { - if(result[0] < result[2]) { + if (result[1] < result[2]) { + if (result[0] < result[2]) { temp = result[2]; result[2] = result[1]; result[1] = result[0]; @@ -677,13 +650,14 @@ void v3Sort(double v[3], } } -void v3PrintScreen(const char *name, double vec[3]) +void +v3PrintScreen(const char* name, double vec[3]) { printf("%s (%20.15g, %20.15g, %20.15g)\n", name, vec[0], vec[1], vec[2]); } -void v4Set(double v0, double v1, double v2, double v3, - double result[4]) +void +v4Set(double v0, double v1, double v2, double v3, double result[4]) { result[0] = v0; result[1] = v1; @@ -691,71 +665,72 @@ void v4Set(double v0, double v1, double v2, double v3, result[3] = v3; } -void v4Copy(double v[4], - double result[4]) +void +v4Copy(double v[4], double result[4]) { size_t dim = 4; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v[i]; } } -void v4SetZero(double v[4]) +void +v4SetZero(double v[4]) { size_t dim = 4; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { v[i] = 0.0; } } -double v4Dot(double v1[4], - double v2[4]) +double +v4Dot(double v1[4], double v2[4]) { size_t dim = 4; size_t i; double result = 0.0; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result += v1[i] * v2[i]; } return result; } -double v4Norm(double v[4]) +double +v4Norm(double v[4]) { return sqrt(v4Dot(v, v)); } -int v4IsEqual(double v1[4], - double v2[4], - double accuracy) +int +v4IsEqual(double v1[4], double v2[4], double accuracy) { size_t dim = 4; size_t i; - for(i = 0; i < dim; i++) { - if(fabs(v1[i] - v2[i]) > accuracy) { + for (i = 0; i < dim; i++) { + if (fabs(v1[i] - v2[i]) > accuracy) { return 0; } } return 1; } -int v4IsZero(double v[4], - double accuracy) +int +v4IsZero(double v[4], double accuracy) { size_t dim = 4; size_t i; - for(i = 0; i < dim; i++) { - if(fabs(v[i]) > accuracy) { + for (i = 0; i < dim; i++) { + if (fabs(v[i]) > accuracy) { return 0; } } return 1; } -void v6Set(double v0, double v1, double v2, double v3, double v4, double v5, - double result[6]) +void +v6Set(double v0, double v1, double v2, double v3, double v4, double v5, double result[6]) { result[0] = v0; result[1] = v1; @@ -765,73 +740,71 @@ void v6Set(double v0, double v1, double v2, double v3, double v4, double v5, result[5] = v5; } -void v6Copy(double v[6], - double result[6]) +void +v6Copy(double v[6], double result[6]) { size_t dim = 6; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v[i]; } } -double v6Dot(double v1[6], - double v2[6]) +double +v6Dot(double v1[6], double v2[6]) { size_t dim = 6; size_t i; double result = 0.0; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result += v1[i] * v2[i]; } return result; } -void v6Scale(double scaleFactor, - double v[6], - double result[6]) +void +v6Scale(double scaleFactor, double v[6], double result[6]) { size_t dim = 6; size_t i; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result[i] = v[i] * scaleFactor; } } -void v6OuterProduct(double v1[6], - double v2[6], - double result[6][6]) +void +v6OuterProduct(double v1[6], double v2[6], double result[6][6]) { size_t dim = 6; size_t i; size_t j; - for(i = 0; i < dim; i++) { - for(j = 0; j < dim; j++) { + for (i = 0; i < dim; i++) { + for (j = 0; j < dim; j++) { result[i][j] = v1[i] * v2[j]; } } } -int v6IsEqual(double v1[6], - double v2[6], - double accuracy) +int +v6IsEqual(double v1[6], double v2[6], double accuracy) { size_t dim = 6; size_t i; - for(i = 0; i < dim; i++) { - if(fabs(v1[i] - v2[i]) > accuracy) { + for (i = 0; i < dim; i++) { + if (fabs(v1[i] - v2[i]) > accuracy) { return 0; } } return 1; } -void mLeastSquaresInverse(void *mx, size_t dim1, size_t dim2, void *result) +void +mLeastSquaresInverse(void* mx, size_t dim1, size_t dim2, void* result) { /* * Computes the least squares inverse. */ - double *m_result = (double *)result; + double* m_result = (double*)result; double mxTranspose[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; double mxGrammian[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; double mxGrammianInverse[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; @@ -840,16 +813,16 @@ void mLeastSquaresInverse(void *mx, size_t dim1, size_t dim2, void *result) mMultM(mxTranspose, dim2, dim1, mx, dim1, dim2, mxGrammian); mInverse(mxGrammian, dim2, mxGrammianInverse); mMultM(mxGrammianInverse, dim2, dim2, mxTranspose, dim2, dim1, m_result); - } -void mMinimumNormInverse(void *mx, size_t dim1, size_t dim2, void *result) +void +mMinimumNormInverse(void* mx, size_t dim1, size_t dim2, void* result) { /* * Computes the minumum norm inverse. */ - double *m_mx = (double *)mx; - double *m_result = (double *)result; + double* m_mx = (double*)mx; + double* m_result = (double*)result; double mxTranspose[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; double mxMxTranspose[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; double mxMxTransposeInverse[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; @@ -860,75 +833,77 @@ void mMinimumNormInverse(void *mx, size_t dim1, size_t dim2, void *result) mMultM(mxTranspose, dim2, dim1, mxMxTransposeInverse, dim1, dim1, m_result); } -void mCopy(void *mx, size_t dim1, size_t dim2, - void *result) +void +mCopy(void* mx, size_t dim1, size_t dim2, void* result) { - double *m_mx = (double *)mx; - double *m_result = (double *)result; + double* m_mx = (double*)mx; + double* m_result = (double*)result; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { m_result[MXINDEX(dim2, i, j)] = m_mx[MXINDEX(dim2, i, j)]; } } } -void mSetZero(void *result, size_t dim1, size_t dim2) +void +mSetZero(void* result, size_t dim1, size_t dim2) { - double *m_result = (double *)result; + double* m_result = (double*)result; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { m_result[MXINDEX(dim2, i, j)] = 0.0; } } } -void mSetIdentity(void *result, size_t dim1, size_t dim2) +void +mSetIdentity(void* result, size_t dim1, size_t dim2) { - double *m_result = (double *)result; + double* m_result = (double*)result; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { m_result[MXINDEX(dim2, i, j)] = (i == j) ? 1.0 : 0.0; } } } -void mDiag(void *v, size_t dim, void *result) +void +mDiag(void* v, size_t dim, void* result) { - double *m_v = (double *)v; - double *m_result = (double *)result; + double* m_v = (double*)v; + double* m_result = (double*)result; size_t i; size_t j; - for(i = 0; i < dim; i++) { - for(j = 0; j < dim; j++) { + for (i = 0; i < dim; i++) { + for (j = 0; j < dim; j++) { m_result[MXINDEX(dim, i, j)] = (i == j) ? m_v[i] : 0.0; } } } -void mTranspose(void *mx, size_t dim1, size_t dim2, - void *result) +void +mTranspose(void* mx, size_t dim1, size_t dim2, void* result) { - double *m_mx = (double *)mx; + double* m_mx = (double*)mx; double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - if (dim1*dim2 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + if (dim1 * dim2 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { m_result[MXINDEX(dim1, j, i)] = m_mx[MXINDEX(dim2, i, j)]; } } @@ -936,79 +911,74 @@ void mTranspose(void *mx, size_t dim1, size_t dim2, MOVE_DOUBLE(m_result, dim2 * dim1, result); } -void mAdd(void *mx1, size_t dim1, size_t dim2, - void *mx2, - void *result) +void +mAdd(void* mx1, size_t dim1, size_t dim2, void* mx2, void* result) { - double *m_mx1 = (double *)mx1; - double *m_mx2 = (double *)mx2; - double *m_result = (double *)result; + double* m_mx1 = (double*)mx1; + double* m_mx2 = (double*)mx2; + double* m_result = (double*)result; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { m_result[MXINDEX(dim2, i, j)] = m_mx1[MXINDEX(dim2, i, j)] + m_mx2[MXINDEX(dim2, i, j)]; } } } -void mSubtract(void *mx1, size_t dim1, size_t dim2, - void *mx2, - void *result) +void +mSubtract(void* mx1, size_t dim1, size_t dim2, void* mx2, void* result) { - double *m_mx1 = (double *)mx1; - double *m_mx2 = (double *)mx2; - double *m_result = (double *)result; + double* m_mx1 = (double*)mx1; + double* m_mx2 = (double*)mx2; + double* m_result = (double*)result; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { m_result[MXINDEX(dim2, i, j)] = m_mx1[MXINDEX(dim2, i, j)] - m_mx2[MXINDEX(dim2, i, j)]; } } } -void mScale(double scaleFactor, - void *mx, size_t dim1, size_t dim2, - void *result) +void +mScale(double scaleFactor, void* mx, size_t dim1, size_t dim2, void* result) { - double *m_mx = (double *)mx; - double *m_result = (double *)result; + double* m_mx = (double*)mx; + double* m_result = (double*)result; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { m_result[MXINDEX(dim2, i, j)] = scaleFactor * m_mx[MXINDEX(dim2, i, j)]; } } } -void mMultM(void *mx1, size_t dim11, size_t dim12, - void *mx2, size_t dim21, size_t dim22, - void *result) +void +mMultM(void* mx1, size_t dim11, size_t dim12, void* mx2, size_t dim21, size_t dim22, void* result) { - double *m_mx1 = (double *)mx1; - double *m_mx2 = (double *)mx2; + double* m_mx1 = (double*)mx1; + double* m_mx2 = (double*)mx2; double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - if (dim11*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + if (dim11 * dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } size_t i; size_t j; size_t k; - if(dim12 != dim21) { + if (dim12 != dim21) { BSK_PRINT(MSG_ERROR, "Error: mMultM dimensions don't match."); return; } - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[MXINDEX(dim22, i, j)] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, i, k)] * m_mx2[MXINDEX(dim22, k, j)]; } } @@ -1017,29 +987,27 @@ void mMultM(void *mx1, size_t dim11, size_t dim12, MOVE_DOUBLE(m_result, dim11 * dim22, result); } -void mtMultM(void *mx1, size_t dim11, size_t dim12, - void *mx2, size_t dim21, size_t dim22, - void *result) +void +mtMultM(void* mx1, size_t dim11, size_t dim12, void* mx2, size_t dim21, size_t dim22, void* result) { - double *m_mx1 = (double *)mx1; - double *m_mx2 = (double *)mx2; + double* m_mx1 = (double*)mx1; + double* m_mx2 = (double*)mx2; double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - if (dim12*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + if (dim12 * dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } size_t i; size_t j; size_t k; - if(dim11 != dim21) { + if (dim11 != dim21) { BSK_PRINT(MSG_ERROR, "Error: mtMultM dimensions don't match."); return; } - for(i = 0; i < dim12; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim12; i++) { + for (j = 0; j < dim22; j++) { m_result[MXINDEX(dim22, i, j)] = 0.0; - for(k = 0; k < dim11; k++) { + for (k = 0; k < dim11; k++) { m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, k, i)] * m_mx2[MXINDEX(dim22, k, j)]; } } @@ -1048,29 +1016,27 @@ void mtMultM(void *mx1, size_t dim11, size_t dim12, MOVE_DOUBLE(m_result, dim12 * dim22, result); } -void mMultMt(void *mx1, size_t dim11, size_t dim12, - void *mx2, size_t dim21, size_t dim22, - void *result) +void +mMultMt(void* mx1, size_t dim11, size_t dim12, void* mx2, size_t dim21, size_t dim22, void* result) { - double *m_mx1 = (double *)mx1; - double *m_mx2 = (double *)mx2; + double* m_mx1 = (double*)mx1; + double* m_mx2 = (double*)mx2; double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - if (dim11*dim21 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + if (dim11 * dim21 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } size_t i; size_t j; size_t k; - if(dim12 != dim22) { + if (dim12 != dim22) { BSK_PRINT(MSG_ERROR, "Error: mMultMt dimensions don't match."); return; } - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim21; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim21; j++) { m_result[MXINDEX(dim21, i, j)] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[MXINDEX(dim21, i, j)] += m_mx1[MXINDEX(dim12, i, k)] * m_mx2[MXINDEX(dim22, j, k)]; } } @@ -1079,29 +1045,27 @@ void mMultMt(void *mx1, size_t dim11, size_t dim12, MOVE_DOUBLE(m_result, dim11 * dim21, result); } -void mtMultMt(void *mx1, size_t dim11, size_t dim12, - void *mx2, size_t dim21, size_t dim22, - void *result) +void +mtMultMt(void* mx1, size_t dim11, size_t dim12, void* mx2, size_t dim21, size_t dim22, void* result) { - double *m_mx1 = (double *)mx1; - double *m_mx2 = (double *)mx2; + double* m_mx1 = (double*)mx1; + double* m_mx2 = (double*)mx2; double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - if (dim12*dim21 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + if (dim12 * dim21 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } size_t i; size_t j; size_t k; - if(dim11 != dim22) { + if (dim11 != dim22) { BSK_PRINT(MSG_ERROR, "Error: mtMultMt dimensions don't match."); return; } - for(i = 0; i < dim12; i++) { - for(j = 0; j < dim21; j++) { + for (i = 0; i < dim12; i++) { + for (j = 0; j < dim21; j++) { m_result[MXINDEX(dim21, i, j)] = 0.0; - for(k = 0; k < dim11; k++) { + for (k = 0; k < dim11; k++) { m_result[MXINDEX(dim21, i, j)] += m_mx1[MXINDEX(dim12, k, i)] * m_mx2[MXINDEX(dim22, j, k)]; } } @@ -1110,28 +1074,26 @@ void mtMultMt(void *mx1, size_t dim11, size_t dim12, MOVE_DOUBLE(m_result, dim12 * dim21, result); } -void mMultV(void *mx, size_t dim1, size_t dim2, - void *v, - void *result) +void +mMultV(void* mx, size_t dim1, size_t dim2, void* v, void* result) { size_t dim11 = dim1; size_t dim12 = dim2; size_t dim22 = 1; - double *m_mx1 = (double *)mx; - double *m_mx2 = (double *)v; + double* m_mx1 = (double*)mx; + double* m_mx2 = (double*)v; double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - if (dim11*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + if (dim11 * dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } size_t i; size_t j; size_t k; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[MXINDEX(dim22, i, j)] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, i, k)] * m_mx2[MXINDEX(dim22, k, j)]; } } @@ -1140,28 +1102,26 @@ void mMultV(void *mx, size_t dim1, size_t dim2, MOVE_DOUBLE(m_result, dim11 * dim22, result); } -void mtMultV(void *mx, size_t dim1, size_t dim2, - void *v, - void *result) +void +mtMultV(void* mx, size_t dim1, size_t dim2, void* v, void* result) { size_t dim11 = dim1; size_t dim12 = dim2; size_t dim22 = 1; - double *m_mx1 = (double *)mx; - double *m_mx2 = (double *)v; + double* m_mx1 = (double*)mx; + double* m_mx2 = (double*)v; double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - if (dim12*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + if (dim12 * dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } size_t i; size_t j; size_t k; - for(i = 0; i < dim12; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim12; i++) { + for (j = 0; j < dim22; j++) { m_result[MXINDEX(dim22, i, j)] = 0.0; - for(k = 0; k < dim11; k++) { + for (k = 0; k < dim11; k++) { m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, k, i)] * m_mx2[MXINDEX(dim22, k, j)]; } } @@ -1170,22 +1130,24 @@ void mtMultV(void *mx, size_t dim1, size_t dim2, MOVE_DOUBLE(m_result, dim12 * dim22, result); } -double mTrace(void *mx, size_t dim) +double +mTrace(void* mx, size_t dim) { - double *m_mx = (double *)mx; + double* m_mx = (double*)mx; size_t i; double result = 0.0; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result += m_mx[MXINDEX(dim, i, i)]; } return result; } -double mDeterminant(void *mx, size_t dim) +double +mDeterminant(void* mx, size_t dim) { - double *m_mx = (double *)mx; + double* m_mx = (double*)mx; size_t i; size_t j; @@ -1193,24 +1155,23 @@ double mDeterminant(void *mx, size_t dim) size_t ii; double result = 0; double mxTemp[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - if ((dim-1)*(dim-1) > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + if ((dim - 1) * (dim - 1) > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } - if(dim < 1) { + if (dim < 1) { return 0; - } else if(dim == 1) { + } else if (dim == 1) { result = m_mx[MXINDEX(dim, 0, 0)]; - } else if(dim == 2) { - result = m_mx[MXINDEX(dim, 0, 0)] * m_mx[MXINDEX(dim, 1, 1)] - - m_mx[MXINDEX(dim, 1, 0)] * m_mx[MXINDEX(dim, 0, 1)]; + } else if (dim == 2) { + result = + m_mx[MXINDEX(dim, 0, 0)] * m_mx[MXINDEX(dim, 1, 1)] - m_mx[MXINDEX(dim, 1, 0)] * m_mx[MXINDEX(dim, 0, 1)]; } else { - for(k = 0; k < dim; k++) { - for(i = 1; i < dim; i++) { + for (k = 0; k < dim; k++) { + for (i = 1; i < dim; i++) { ii = 0; - for(j = 0; j < dim; j++) { - if(j == k) { + for (j = 0; j < dim; j++) { + if (j == k) { continue; } mxTemp[MXINDEX(dim - 1, i - 1, ii)] = m_mx[MXINDEX(dim, i, j)]; @@ -1220,39 +1181,39 @@ double mDeterminant(void *mx, size_t dim) result += pow(-1.0, 1.0 + k + 1.0) * m_mx[MXINDEX(dim, 0, k)] * mDeterminant(mxTemp, dim - 1); } } - return(result); + return (result); } -void mCofactor(void *mx, size_t dim, void *result) +void +mCofactor(void* mx, size_t dim, void* result) { /* The (j,i)th cofactor of A is defined as (-1)^(i + j)*det(A_(i,j)) where A_(i,j) is the submatrix of A obtained from A by removing the ith row and jth column */ - size_t i; - size_t i0; - size_t i1; - size_t j; - size_t j0; - size_t j1; - double *m_mx = (double *)mx; + size_t i; + size_t i0; + size_t i1; + size_t j; + size_t j0; + size_t j1; + double* m_mx = (double*)mx; double m_mxij[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - double det; - if (dim*dim > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + double det; + if (dim * dim > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } - for(i = 0; i < dim; i++) { - for(j = 0; j < dim; j++) { + for (i = 0; i < dim; i++) { + for (j = 0; j < dim; j++) { /* Form mx_(i,j) */ i1 = 0; - for(i0 = 0; i0 < dim; i0++) { - if(i0 == i) { + for (i0 = 0; i0 < dim; i0++) { + if (i0 == i) { continue; } j1 = 0; - for(j0 = 0; j0 < dim; j0++) { - if(j0 == j) { + for (j0 = 0; j0 < dim; j0++) { + if (j0 == j) { continue; } m_mxij[MXINDEX(dim - 1, i1, j1)] = m_mx[MXINDEX(dim, i0, j0)]; @@ -1272,22 +1233,22 @@ void mCofactor(void *mx, size_t dim, void *result) MOVE_DOUBLE(m_result, dim * dim, result); } -int mInverse(void *mx, size_t dim, void *result) +int +mInverse(void* mx, size_t dim, void* result) { /* Inverse of a square matrix A with non zero determinant is adjoint matrix divided by determinant */ /* The adjoint matrix is the square matrix X such that the (i,j)th entry of X is the (j,i)th cofactor of A */ - size_t i; - size_t j; - int status = 0; - double det = mDeterminant(mx, dim); - double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; - if (dim*dim > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) - { - BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + size_t i; + size_t j; + int status = 0; + double det = mDeterminant(mx, dim); + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if (dim * dim > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) { + BSK_PRINT(MSG_ERROR, "Linear Algegra library array dimension input is too large."); } - if(fabs(det) > DB0_EPS) { + if (fabs(det) > DB0_EPS) { /* Find adjoint matrix */ double m_adjoint[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; mCofactor(mx, dim, m_adjoint); @@ -1296,8 +1257,8 @@ int mInverse(void *mx, size_t dim, void *result) mScale(1.0 / det, m_adjoint, dim, dim, m_result); } else { BSK_PRINT(MSG_ERROR, "Error: cannot invert singular matrix"); - for(i = 0; i < dim; i++) { - for(j = 0; j < dim; j++) { + for (i = 0; i < dim; i++) { + for (j = 0; j < dim; j++) { m_result[MXINDEX(dim, i, j)] = 0.0; } } @@ -1308,18 +1269,17 @@ int mInverse(void *mx, size_t dim, void *result) return status; } -int mIsEqual(void *mx1, size_t dim1, size_t dim2, - void *mx2, - double accuracy) +int +mIsEqual(void* mx1, size_t dim1, size_t dim2, void* mx2, double accuracy) { - double *m_mx1 = (double *)mx1; - double *m_mx2 = (double *)mx2; + double* m_mx1 = (double*)mx1; + double* m_mx2 = (double*)mx2; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { - if(fabs(m_mx1[MXINDEX(dim2, i, j)] - m_mx2[MXINDEX(dim2, i, j)]) > accuracy) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { + if (fabs(m_mx1[MXINDEX(dim2, i, j)] - m_mx2[MXINDEX(dim2, i, j)]) > accuracy) { return 0; } } @@ -1327,16 +1287,16 @@ int mIsEqual(void *mx1, size_t dim1, size_t dim2, return 1; } -int mIsZero(void *mx, size_t dim1, size_t dim2, - double accuracy) +int +mIsZero(void* mx, size_t dim1, size_t dim2, double accuracy) { - double *m_mx = (double *)mx; + double* m_mx = (double*)mx; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { - if(fabs(m_mx[MXINDEX(dim2, i, j)]) > accuracy) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { + if (fabs(m_mx[MXINDEX(dim2, i, j)]) > accuracy) { return 0; } } @@ -1344,84 +1304,96 @@ int mIsZero(void *mx, size_t dim1, size_t dim2, return 1; } -void mPrintScreen(const char *name, void *mx, size_t dim1, size_t dim2) +void +mPrintScreen(const char* name, void* mx, size_t dim1, size_t dim2) { - double *m_mx = (double *)mx; + double* m_mx = (double*)mx; size_t i; size_t j; printf("%s = [", name); - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { printf("%20.15g", m_mx[MXINDEX(dim2, i, j)]); - if(j != dim2 - 1) { + if (j != dim2 - 1) { printf(", "); } } - if(i != dim1 - 1) { + if (i != dim1 - 1) { printf(";\n"); } } printf("];\n"); } - -void mPrint(FILE *pFile, const char *name, void *mx, size_t dim1, size_t dim2) +void +mPrint(FILE* pFile, const char* name, void* mx, size_t dim1, size_t dim2) { - double *m_mx = (double *)mx; + double* m_mx = (double*)mx; size_t i; size_t j; fprintf(pFile, "%s = [", name); - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { fprintf(pFile, "%20.15g", m_mx[MXINDEX(dim2, i, j)]); - if(j != dim2 - 1) { + if (j != dim2 - 1) { fprintf(pFile, ", "); } } - if(i != dim1 - 1) { + if (i != dim1 - 1) { fprintf(pFile, ";\n"); } } fprintf(pFile, "];\n"); } -void mGetSubMatrix(void *mx, size_t dim1, size_t dim2, - size_t dim1Start, size_t dim2Start, - size_t dim1Result, size_t dim2Result, void *result) +void +mGetSubMatrix(void* mx, + size_t dim1, + size_t dim2, + size_t dim1Start, + size_t dim2Start, + size_t dim1Result, + size_t dim2Result, + void* result) { - double *m_mx = (double *)mx; - double *m_result = (double *)result; + double* m_mx = (double*)mx; + double* m_result = (double*)result; size_t i; size_t j; - for(i = dim1Start; i < dim1Start + dim1Result; i++) { - for(j = dim2Start; j < dim2Start + dim2Result; j++) { + for (i = dim1Start; i < dim1Start + dim1Result; i++) { + for (j = dim2Start; j < dim2Start + dim2Result; j++) { m_result[MXINDEX(dim2Result, i - dim1Start, j - dim2Start)] = m_mx[MXINDEX(dim2, i, j)]; } } } -void mSetSubMatrix(void *mx, size_t dim1, size_t dim2, - void *result, size_t dim1Result, size_t dim2Result, - size_t dim1Start, size_t dim2Start) +void +mSetSubMatrix(void* mx, + size_t dim1, + size_t dim2, + void* result, + size_t dim1Result, + size_t dim2Result, + size_t dim1Start, + size_t dim2Start) { - double *m_mx = (double *)mx; - double *m_result = (double *)result; + double* m_mx = (double*)mx; + double* m_result = (double*)result; size_t i; size_t j; - for(i = dim1Start; i < dim1Start + dim1; i++) { - for(j = dim2Start; j < dim2Start + dim2; j++) { + for (i = dim1Start; i < dim1Start + dim1; i++) { + for (j = dim2Start; j < dim2Start + dim2; j++) { m_result[MXINDEX(dim2Result, i, j)] = m_mx[MXINDEX(dim2, i - dim1Start, j - dim2Start)]; } } } -void m22Set(double m00, double m01, - double m10, double m11, - double m[2][2]) +void +m22Set(double m00, double m01, double m10, double m11, double m[2][2]) { m[0][0] = m00; m[0][1] = m01; @@ -1429,109 +1401,107 @@ void m22Set(double m00, double m01, m[1][1] = m11; } -void m22Copy(double mx[2][2], - double result[2][2]) +void +m22Copy(double mx[2][2], double result[2][2]) { size_t dim1 = 2; size_t dim2 = 2; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = mx[i][j]; } } } -void m22SetZero(double result[2][2]) +void +m22SetZero(double result[2][2]) { size_t dim1 = 2; size_t dim2 = 2; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = 0.0; } } } -void m22SetIdentity(double result[2][2]) +void +m22SetIdentity(double result[2][2]) { size_t dim = 2; size_t i; size_t j; - for(i = 0; i < dim; i++) { - for(j = 0; j < dim; j++) { + for (i = 0; i < dim; i++) { + for (j = 0; j < dim; j++) { result[i][j] = (i == j) ? 1.0 : 0.0; } } } -void m22Transpose(double mx[2][2], - double result[2][2]) +void +m22Transpose(double mx[2][2], double result[2][2]) { size_t dim1 = 2; size_t dim2 = 2; size_t i; size_t j; double m_result[2][2]; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { m_result[j][i] = mx[i][j]; } } m22Copy(m_result, result); } -void m22Add(double mx1[2][2], - double mx2[2][2], - double result[2][2]) +void +m22Add(double mx1[2][2], double mx2[2][2], double result[2][2]) { size_t dim1 = 2; size_t dim2 = 2; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = mx1[i][j] + mx2[i][j]; } } } -void m22Subtract(double mx1[2][2], - double mx2[2][2], - double result[2][2]) +void +m22Subtract(double mx1[2][2], double mx2[2][2], double result[2][2]) { size_t dim1 = 2; size_t dim2 = 2; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = mx1[i][j] - mx2[i][j]; } } } -void m22Scale(double scaleFactor, - double mx[2][2], - double result[2][2]) +void +m22Scale(double scaleFactor, double mx[2][2], double result[2][2]) { size_t dim1 = 2; size_t dim2 = 2; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = scaleFactor * mx[i][j]; } } } -void m22MultM22(double mx1[2][2], - double mx2[2][2], - double result[2][2]) +void +m22MultM22(double mx1[2][2], double mx2[2][2], double result[2][2]) { size_t dim11 = 2; size_t dim12 = 2; @@ -1540,10 +1510,10 @@ void m22MultM22(double mx1[2][2], size_t j; size_t k; double m_result[2][2]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i][j] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i][j] += mx1[i][k] * mx2[k][j]; } } @@ -1551,9 +1521,8 @@ void m22MultM22(double mx1[2][2], m22Copy(m_result, result); } -void m22tMultM22(double mx1[2][2], - double mx2[2][2], - double result[2][2]) +void +m22tMultM22(double mx1[2][2], double mx2[2][2], double result[2][2]) { size_t dim11 = 2; size_t dim12 = 2; @@ -1562,10 +1531,10 @@ void m22tMultM22(double mx1[2][2], size_t j; size_t k; double m_result[2][2]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i][j] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i][j] += mx1[k][i] * mx2[k][j]; } } @@ -1573,9 +1542,8 @@ void m22tMultM22(double mx1[2][2], m22Copy(m_result, result); } -void m22MultM22t(double mx1[2][2], - double mx2[2][2], - double result[2][2]) +void +m22MultM22t(double mx1[2][2], double mx2[2][2], double result[2][2]) { size_t dim11 = 2; size_t dim12 = 2; @@ -1584,10 +1552,10 @@ void m22MultM22t(double mx1[2][2], size_t j; size_t k; double m_result[2][2]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim21; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim21; j++) { m_result[i][j] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i][j] += mx1[i][k] * mx2[j][k]; } } @@ -1595,9 +1563,8 @@ void m22MultM22t(double mx1[2][2], m22Copy(m_result, result); } -void m22MultV2(double mx[2][2], - double v[2], - double result[2]) +void +m22MultV2(double mx[2][2], double v[2], double result[2]) { size_t dim11 = 2; size_t dim12 = 2; @@ -1606,10 +1573,10 @@ void m22MultV2(double mx[2][2], size_t j; size_t k; double m_result[2]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i] += mx[i][k] * v[k]; } } @@ -1617,9 +1584,8 @@ void m22MultV2(double mx[2][2], v2Copy(m_result, result); } -void m22tMultV2(double mx[2][2], - double v[2], - double result[2]) +void +m22tMultV2(double mx[2][2], double v[2], double result[2]) { size_t dim11 = 2; size_t dim12 = 2; @@ -1628,10 +1594,10 @@ void m22tMultV2(double mx[2][2], size_t j; size_t k; double m_result[2]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i] += mx[k][i] * v[k]; } } @@ -1639,36 +1605,37 @@ void m22tMultV2(double mx[2][2], v2Copy(m_result, result); } -double m22Trace(double mx[2][2]) +double +m22Trace(double mx[2][2]) { size_t dim = 2; size_t i; double result = 0.0; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result += mx[i][i]; } return result; } -double m22Determinant(double mx[2][2]) +double +m22Determinant(double mx[2][2]) { double value; value = mx[0][0] * mx[1][1] - mx[1][0] * mx[0][1]; return value; } -int m22IsEqual(double mx1[2][2], - double mx2[2][2], - double accuracy) +int +m22IsEqual(double mx1[2][2], double mx2[2][2], double accuracy) { size_t dim1 = 2; size_t dim2 = 2; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { - if(fabs(mx1[i][j] - mx2[i][j]) > accuracy) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { + if (fabs(mx1[i][j] - mx2[i][j]) > accuracy) { return 0; } } @@ -1676,16 +1643,16 @@ int m22IsEqual(double mx1[2][2], return 1; } -int m22IsZero(double mx[2][2], - double accuracy) +int +m22IsZero(double mx[2][2], double accuracy) { size_t dim1 = 2; size_t dim2 = 2; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { - if(fabs(mx[i][j]) > accuracy) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { + if (fabs(mx[i][j]) > accuracy) { return 0; } } @@ -1693,64 +1660,72 @@ int m22IsZero(double mx[2][2], return 1; } -void m22Print(FILE *pFile, const char *name, double mx[2][2]) +void +m22Print(FILE* pFile, const char* name, double mx[2][2]) { size_t dim1 = 2; size_t dim2 = 2; size_t i; size_t j; fprintf(pFile, "%s = [", name); - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { fprintf(pFile, "%20.15g", mx[i][j]); - if(j != dim2 - 1) { + if (j != dim2 - 1) { fprintf(pFile, ", "); } } - if(i != dim1 - 1) { + if (i != dim1 - 1) { fprintf(pFile, ";\n"); } } fprintf(pFile, "]\n"); } -int m22Inverse(double mx[2][2], double result[2][2]) +int +m22Inverse(double mx[2][2], double result[2][2]) { double det = m22Determinant(mx); double detInv; double m_result[2][2]; - int status = 0; + int status = 0; - if(fabs(det) > DB0_EPS) { + if (fabs(det) > DB0_EPS) { detInv = 1.0 / det; - m_result[0][0] = mx[1][1] * detInv; + m_result[0][0] = mx[1][1] * detInv; m_result[0][1] = -mx[0][1] * detInv; m_result[1][0] = -mx[1][0] * detInv; - m_result[1][1] = mx[0][0] * detInv; + m_result[1][1] = mx[0][0] * detInv; } else { BSK_PRINT(MSG_ERROR, "Error: singular 2x2 matrix inverse"); - m22Set(0.0, 0.0, - 0.0, 0.0, - m_result); + m22Set(0.0, 0.0, 0.0, 0.0, m_result); status = 1; } m22Copy(m_result, result); return status; } -void m22PrintScreen(const char *name, double mx[2][2]) +void +m22PrintScreen(const char* name, double mx[2][2]) { int i; printf("%s:\n", name); - for (i=0;i<2;i++) { + for (i = 0; i < 2; i++) { printf("%20.15g, %20.15g\n", mx[i][0], mx[i][1]); } } -void m33Set(double m00, double m01, double m02, - double m10, double m11, double m12, - double m20, double m21, double m22, - double m[3][3]) +void +m33Set(double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, + double m22, + double m[3][3]) { m[0][0] = m00; m[0][1] = m01; @@ -1763,109 +1738,107 @@ void m33Set(double m00, double m01, double m02, m[2][2] = m22; } -void m33Copy(double mx[3][3], - double result[3][3]) +void +m33Copy(double mx[3][3], double result[3][3]) { size_t dim1 = 3; size_t dim2 = 3; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = mx[i][j]; } } } -void m33SetZero(double result[3][3]) +void +m33SetZero(double result[3][3]) { size_t dim1 = 3; size_t dim2 = 3; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = 0.0; } } } -void m33SetIdentity(double result[3][3]) +void +m33SetIdentity(double result[3][3]) { size_t dim = 3; size_t i; size_t j; - for(i = 0; i < dim; i++) { - for(j = 0; j < dim; j++) { + for (i = 0; i < dim; i++) { + for (j = 0; j < dim; j++) { result[i][j] = (i == j) ? 1.0 : 0.0; } } } -void m33Transpose(double mx[3][3], - double result[3][3]) +void +m33Transpose(double mx[3][3], double result[3][3]) { size_t dim1 = 3; size_t dim2 = 3; size_t i; size_t j; double m_result[3][3]; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { m_result[j][i] = mx[i][j]; } } m33Copy(m_result, result); } -void m33Add(double mx1[3][3], - double mx2[3][3], - double result[3][3]) +void +m33Add(double mx1[3][3], double mx2[3][3], double result[3][3]) { size_t dim1 = 3; size_t dim2 = 3; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = mx1[i][j] + mx2[i][j]; } } } -void m33Subtract(double mx1[3][3], - double mx2[3][3], - double result[3][3]) +void +m33Subtract(double mx1[3][3], double mx2[3][3], double result[3][3]) { size_t dim1 = 3; size_t dim2 = 3; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = mx1[i][j] - mx2[i][j]; } } } -void m33Scale(double scaleFactor, - double mx[3][3], - double result[3][3]) +void +m33Scale(double scaleFactor, double mx[3][3], double result[3][3]) { size_t dim1 = 3; size_t dim2 = 3; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = scaleFactor * mx[i][j]; } } } -void m33MultM33(double mx1[3][3], - double mx2[3][3], - double result[3][3]) +void +m33MultM33(double mx1[3][3], double mx2[3][3], double result[3][3]) { size_t dim11 = 3; size_t dim12 = 3; @@ -1874,10 +1847,10 @@ void m33MultM33(double mx1[3][3], size_t j; size_t k; double m_result[3][3]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i][j] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i][j] += mx1[i][k] * mx2[k][j]; } } @@ -1885,9 +1858,8 @@ void m33MultM33(double mx1[3][3], m33Copy(m_result, result); } -void m33tMultM33(double mx1[3][3], - double mx2[3][3], - double result[3][3]) +void +m33tMultM33(double mx1[3][3], double mx2[3][3], double result[3][3]) { size_t dim11 = 3; size_t dim12 = 3; @@ -1896,10 +1868,10 @@ void m33tMultM33(double mx1[3][3], size_t j; size_t k; double m_result[3][3]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i][j] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i][j] += mx1[k][i] * mx2[k][j]; } } @@ -1907,9 +1879,8 @@ void m33tMultM33(double mx1[3][3], m33Copy(m_result, result); } -void m33MultM33t(double mx1[3][3], - double mx2[3][3], - double result[3][3]) +void +m33MultM33t(double mx1[3][3], double mx2[3][3], double result[3][3]) { size_t dim11 = 3; size_t dim12 = 3; @@ -1918,10 +1889,10 @@ void m33MultM33t(double mx1[3][3], size_t j; size_t k; double m_result[3][3]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim21; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim21; j++) { m_result[i][j] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i][j] += mx1[i][k] * mx2[j][k]; } } @@ -1929,9 +1900,8 @@ void m33MultM33t(double mx1[3][3], m33Copy(m_result, result); } -void m33MultV3(double mx[3][3], - double v[3], - double result[3]) +void +m33MultV3(double mx[3][3], double v[3], double result[3]) { size_t dim11 = 3; size_t dim12 = 3; @@ -1940,10 +1910,10 @@ void m33MultV3(double mx[3][3], size_t j; size_t k; double m_result[3]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i] += mx[i][k] * v[k]; } } @@ -1951,9 +1921,8 @@ void m33MultV3(double mx[3][3], v3Copy(m_result, result); } -void m33tMultV3(double mx[3][3], - double v[3], - double result[3]) +void +m33tMultV3(double mx[3][3], double v[3], double result[3]) { size_t dim11 = 3; size_t dim12 = 3; @@ -1962,10 +1931,10 @@ void m33tMultV3(double mx[3][3], size_t j; size_t k; double m_result[3]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i] += mx[k][i] * v[k]; } } @@ -1973,41 +1942,38 @@ void m33tMultV3(double mx[3][3], v3Copy(m_result, result); } -double m33Trace(double mx[3][3]) +double +m33Trace(double mx[3][3]) { size_t dim = 3; size_t i; double result = 0.0; - for(i = 0; i < dim; i++) { + for (i = 0; i < dim; i++) { result += mx[i][i]; } return result; } -double m33Determinant(double mx[3][3]) +double +m33Determinant(double mx[3][3]) { double value; - value = mx[0][0] * mx[1][1] * mx[2][2] - + mx[0][1] * mx[1][2] * mx[2][0] - + mx[0][2] * mx[1][0] * mx[2][1] - - mx[0][0] * mx[1][2] * mx[2][1] - - mx[0][1] * mx[1][0] * mx[2][2] - - mx[0][2] * mx[1][1] * mx[2][0]; + value = mx[0][0] * mx[1][1] * mx[2][2] + mx[0][1] * mx[1][2] * mx[2][0] + mx[0][2] * mx[1][0] * mx[2][1] - + mx[0][0] * mx[1][2] * mx[2][1] - mx[0][1] * mx[1][0] * mx[2][2] - mx[0][2] * mx[1][1] * mx[2][0]; return value; } -int m33IsEqual(double mx1[3][3], - double mx2[3][3], - double accuracy) +int +m33IsEqual(double mx1[3][3], double mx2[3][3], double accuracy) { size_t dim1 = 3; size_t dim2 = 3; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { - if(fabs(mx1[i][j] - mx2[i][j]) > accuracy) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { + if (fabs(mx1[i][j] - mx2[i][j]) > accuracy) { return 0; } } @@ -2015,16 +1981,16 @@ int m33IsEqual(double mx1[3][3], return 1; } -int m33IsZero(double mx[3][3], - double accuracy) +int +m33IsZero(double mx[3][3], double accuracy) { size_t dim1 = 3; size_t dim2 = 3; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { - if(fabs(mx[i][j]) > accuracy) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { + if (fabs(mx[i][j]) > accuracy) { return 0; } } @@ -2032,35 +1998,37 @@ int m33IsZero(double mx[3][3], return 1; } -void m33Print(FILE *pFile, const char *name, double mx[3][3]) +void +m33Print(FILE* pFile, const char* name, double mx[3][3]) { size_t dim1 = 3; size_t dim2 = 3; size_t i; size_t j; fprintf(pFile, "%s = [", name); - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { fprintf(pFile, "%20.15g", mx[i][j]); - if(j != dim2 - 1) { + if (j != dim2 - 1) { fprintf(pFile, ", "); } } - if(i != dim1 - 1) { + if (i != dim1 - 1) { fprintf(pFile, ";\n"); } } fprintf(pFile, "]\n"); } -int m33Inverse(double mx[3][3], double result[3][3]) +int +m33Inverse(double mx[3][3], double result[3][3]) { double det = m33Determinant(mx); double detInv; double m_result[3][3]; - int status = 0; + int status = 0; - if(fabs(det) > DB0_EPS) { + if (fabs(det) > DB0_EPS) { detInv = 1.0 / det; m_result[0][0] = (mx[1][1] * mx[2][2] - mx[1][2] * mx[2][1]) * detInv; m_result[0][1] = -(mx[0][1] * mx[2][2] - mx[0][2] * mx[2][1]) * detInv; @@ -2073,37 +2041,34 @@ int m33Inverse(double mx[3][3], double result[3][3]) m_result[2][2] = (mx[0][0] * mx[1][1] - mx[0][1] * mx[1][0]) * detInv; } else { BSK_PRINT(MSG_ERROR, "Error: singular 3x3 matrix inverse"); - m33Set(0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - m_result); + m33Set(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, m_result); status = 1; } m33Copy(m_result, result); return status; } -void m33SingularValues(double mx[3][3], double result[3]) +void +m33SingularValues(double mx[3][3], double result[3]) { double sv[3]; double a[3]; double mxtmx[3][3]; - int i; + int i; m33tMultM33(mx, mx, mxtmx); /* Compute characteristic polynomial */ a[0] = -m33Determinant(mxtmx); - a[1] = mxtmx[0][0] * mxtmx[1][1] - mxtmx[0][1] * mxtmx[1][0] - + mxtmx[0][0] * mxtmx[2][2] - mxtmx[0][2] * mxtmx[2][0] - + mxtmx[1][1] * mxtmx[2][2] - mxtmx[1][2] * mxtmx[2][1]; + a[1] = mxtmx[0][0] * mxtmx[1][1] - mxtmx[0][1] * mxtmx[1][0] + mxtmx[0][0] * mxtmx[2][2] - + mxtmx[0][2] * mxtmx[2][0] + mxtmx[1][1] * mxtmx[2][2] - mxtmx[1][2] * mxtmx[2][1]; a[2] = -mxtmx[0][0] - mxtmx[1][1] - mxtmx[2][2]; /* Solve cubic equation */ cubicRoots(a, sv); /* take square roots */ - for(i = 0; i < 3; i++) { + for (i = 0; i < 3; i++) { sv[i] = sqrt(sv[i]); } @@ -2111,16 +2076,16 @@ void m33SingularValues(double mx[3][3], double result[3]) v3Sort(sv, result); } -void m33EigenValues(double mx[3][3], double result[3]) +void +m33EigenValues(double mx[3][3], double result[3]) { double sv[3]; double a[3]; /* Compute characteristic polynomial */ a[0] = -m33Determinant(mx); - a[1] = mx[0][0] * mx[1][1] - mx[0][1] * mx[1][0] - + mx[0][0] * mx[2][2] - mx[0][2] * mx[2][0] - + mx[1][1] * mx[2][2] - mx[1][2] * mx[2][1]; + a[1] = mx[0][0] * mx[1][1] - mx[0][1] * mx[1][0] + mx[0][0] * mx[2][2] - mx[0][2] * mx[2][0] + mx[1][1] * mx[2][2] - + mx[1][2] * mx[2][1]; a[2] = -mx[0][0] - mx[1][1] - mx[2][2]; /* Solve cubic equation */ @@ -2130,27 +2095,42 @@ void m33EigenValues(double mx[3][3], double result[3]) v3Sort(sv, result); } -double m33ConditionNumber(double mx[3][3]) +double +m33ConditionNumber(double mx[3][3]) { double sv[3]; m33SingularValues(mx, sv); return (sv[0] / sv[2]); } -void m33PrintScreen(const char *name, double mx[3][3]) +void +m33PrintScreen(const char* name, double mx[3][3]) { int i; printf("%s:\n", name); - for (i=0;i<3;i++) { + for (i = 0; i < 3; i++) { printf("%20.15g, %20.15g, %20.15g\n", mx[i][0], mx[i][1], mx[i][2]); } } -void m44Set(double m00, double m01, double m02, double m03, - double m10, double m11, double m12, double m13, - double m20, double m21, double m22, double m23, - double m30, double m31, double m32, double m33, - double m[4][4]) +void +m44Set(double m00, + double m01, + double m02, + double m03, + double m10, + double m11, + double m12, + double m13, + double m20, + double m21, + double m22, + double m23, + double m30, + double m31, + double m32, + double m33, + double m[4][4]) { m[0][0] = m00; m[0][1] = m01; @@ -2170,36 +2150,36 @@ void m44Set(double m00, double m01, double m02, double m03, m[3][3] = m33; } -void m44Copy(double mx[4][4], - double result[4][4]) +void +m44Copy(double mx[4][4], double result[4][4]) { size_t dim1 = 4; size_t dim2 = 4; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = mx[i][j]; } } } -void m44SetZero(double result[4][4]) +void +m44SetZero(double result[4][4]) { size_t dim1 = 4; size_t dim2 = 4; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = 0.0; } } } -void m44MultV4(double mx[4][4], - double v[4], - double result[4]) +void +m44MultV4(double mx[4][4], double v[4], double result[4]) { size_t dim11 = 4; size_t dim12 = 4; @@ -2208,10 +2188,10 @@ void m44MultV4(double mx[4][4], size_t j; size_t k; double m_result[4]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i] += mx[i][k] * v[k]; } } @@ -2219,47 +2199,35 @@ void m44MultV4(double mx[4][4], v4Copy(m_result, result); } -double m44Determinant(double mx[4][4]) +double +m44Determinant(double mx[4][4]) { double value; - value = mx[0][3] * mx[1][2] * mx[2][1] * mx[3][0] - - mx[0][2] * mx[1][3] * mx[2][1] * mx[3][0] - - mx[0][3] * mx[1][1] * mx[2][2] * mx[3][0] - + mx[0][1] * mx[1][3] * mx[2][2] * mx[3][0] - + mx[0][2] * mx[1][1] * mx[2][3] * mx[3][0] - - mx[0][1] * mx[1][2] * mx[2][3] * mx[3][0] - - mx[0][3] * mx[1][2] * mx[2][0] * mx[3][1] - + mx[0][2] * mx[1][3] * mx[2][0] * mx[3][1] - + mx[0][3] * mx[1][0] * mx[2][2] * mx[3][1] - - mx[0][0] * mx[1][3] * mx[2][2] * mx[3][1] - - mx[0][2] * mx[1][0] * mx[2][3] * mx[3][1] - + mx[0][0] * mx[1][2] * mx[2][3] * mx[3][1] - + mx[0][3] * mx[1][1] * mx[2][0] * mx[3][2] - - mx[0][1] * mx[1][3] * mx[2][0] * mx[3][2] - - mx[0][3] * mx[1][0] * mx[2][1] * mx[3][2] - + mx[0][0] * mx[1][3] * mx[2][1] * mx[3][2] - + mx[0][1] * mx[1][0] * mx[2][3] * mx[3][2] - - mx[0][0] * mx[1][1] * mx[2][3] * mx[3][2] - - mx[0][2] * mx[1][1] * mx[2][0] * mx[3][3] - + mx[0][1] * mx[1][2] * mx[2][0] * mx[3][3] - + mx[0][2] * mx[1][0] * mx[2][1] * mx[3][3] - - mx[0][0] * mx[1][2] * mx[2][1] * mx[3][3] - - mx[0][1] * mx[1][0] * mx[2][2] * mx[3][3] - + mx[0][0] * mx[1][1] * mx[2][2] * mx[3][3]; + value = mx[0][3] * mx[1][2] * mx[2][1] * mx[3][0] - mx[0][2] * mx[1][3] * mx[2][1] * mx[3][0] - + mx[0][3] * mx[1][1] * mx[2][2] * mx[3][0] + mx[0][1] * mx[1][3] * mx[2][2] * mx[3][0] + + mx[0][2] * mx[1][1] * mx[2][3] * mx[3][0] - mx[0][1] * mx[1][2] * mx[2][3] * mx[3][0] - + mx[0][3] * mx[1][2] * mx[2][0] * mx[3][1] + mx[0][2] * mx[1][3] * mx[2][0] * mx[3][1] + + mx[0][3] * mx[1][0] * mx[2][2] * mx[3][1] - mx[0][0] * mx[1][3] * mx[2][2] * mx[3][1] - + mx[0][2] * mx[1][0] * mx[2][3] * mx[3][1] + mx[0][0] * mx[1][2] * mx[2][3] * mx[3][1] + + mx[0][3] * mx[1][1] * mx[2][0] * mx[3][2] - mx[0][1] * mx[1][3] * mx[2][0] * mx[3][2] - + mx[0][3] * mx[1][0] * mx[2][1] * mx[3][2] + mx[0][0] * mx[1][3] * mx[2][1] * mx[3][2] + + mx[0][1] * mx[1][0] * mx[2][3] * mx[3][2] - mx[0][0] * mx[1][1] * mx[2][3] * mx[3][2] - + mx[0][2] * mx[1][1] * mx[2][0] * mx[3][3] + mx[0][1] * mx[1][2] * mx[2][0] * mx[3][3] + + mx[0][2] * mx[1][0] * mx[2][1] * mx[3][3] - mx[0][0] * mx[1][2] * mx[2][1] * mx[3][3] - + mx[0][1] * mx[1][0] * mx[2][2] * mx[3][3] + mx[0][0] * mx[1][1] * mx[2][2] * mx[3][3]; return value; } -int m44IsEqual(double mx1[4][4], - double mx2[4][4], - double accuracy) +int +m44IsEqual(double mx1[4][4], double mx2[4][4], double accuracy) { size_t dim1 = 4; size_t dim2 = 4; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { - if(fabs(mx1[i][j] - mx2[i][j]) > accuracy) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { + if (fabs(mx1[i][j] - mx2[i][j]) > accuracy) { return 0; } } @@ -2267,51 +2235,127 @@ int m44IsEqual(double mx1[4][4], return 1; } -int m44Inverse(double mx[4][4], double result[4][4]) +int +m44Inverse(double mx[4][4], double result[4][4]) { double det = m44Determinant(mx); double detInv; double m_result[4][4]; - int status = 0; + int status = 0; - if(fabs(det) > DB0_EPS) { + if (fabs(det) > DB0_EPS) { detInv = 1.0 / det; - m_result[0][0] = (mx[1][2] * mx[2][3] * mx[3][1] - mx[1][3] * mx[2][2] * mx[3][1] + mx[1][3] * mx[2][1] * mx[3][2] - mx[1][1] * mx[2][3] * mx[3][2] - mx[1][2] * mx[2][1] * mx[3][3] + mx[1][1] * mx[2][2] * mx[3][3]) * detInv; - m_result[0][1] = (mx[0][3] * mx[2][2] * mx[3][1] - mx[0][2] * mx[2][3] * mx[3][1] - mx[0][3] * mx[2][1] * mx[3][2] + mx[0][1] * mx[2][3] * mx[3][2] + mx[0][2] * mx[2][1] * mx[3][3] - mx[0][1] * mx[2][2] * mx[3][3]) * detInv; - m_result[0][2] = (mx[0][2] * mx[1][3] * mx[3][1] - mx[0][3] * mx[1][2] * mx[3][1] + mx[0][3] * mx[1][1] * mx[3][2] - mx[0][1] * mx[1][3] * mx[3][2] - mx[0][2] * mx[1][1] * mx[3][3] + mx[0][1] * mx[1][2] * mx[3][3]) * detInv; - m_result[0][3] = (mx[0][3] * mx[1][2] * mx[2][1] - mx[0][2] * mx[1][3] * mx[2][1] - mx[0][3] * mx[1][1] * mx[2][2] + mx[0][1] * mx[1][3] * mx[2][2] + mx[0][2] * mx[1][1] * mx[2][3] - mx[0][1] * mx[1][2] * mx[2][3]) * detInv; - m_result[1][0] = (mx[1][3] * mx[2][2] * mx[3][0] - mx[1][2] * mx[2][3] * mx[3][0] - mx[1][3] * mx[2][0] * mx[3][2] + mx[1][0] * mx[2][3] * mx[3][2] + mx[1][2] * mx[2][0] * mx[3][3] - mx[1][0] * mx[2][2] * mx[3][3]) * detInv; - m_result[1][1] = (mx[0][2] * mx[2][3] * mx[3][0] - mx[0][3] * mx[2][2] * mx[3][0] + mx[0][3] * mx[2][0] * mx[3][2] - mx[0][0] * mx[2][3] * mx[3][2] - mx[0][2] * mx[2][0] * mx[3][3] + mx[0][0] * mx[2][2] * mx[3][3]) * detInv; - m_result[1][2] = (mx[0][3] * mx[1][2] * mx[3][0] - mx[0][2] * mx[1][3] * mx[3][0] - mx[0][3] * mx[1][0] * mx[3][2] + mx[0][0] * mx[1][3] * mx[3][2] + mx[0][2] * mx[1][0] * mx[3][3] - mx[0][0] * mx[1][2] * mx[3][3]) * detInv; - m_result[1][3] = (mx[0][2] * mx[1][3] * mx[2][0] - mx[0][3] * mx[1][2] * mx[2][0] + mx[0][3] * mx[1][0] * mx[2][2] - mx[0][0] * mx[1][3] * mx[2][2] - mx[0][2] * mx[1][0] * mx[2][3] + mx[0][0] * mx[1][2] * mx[2][3]) * detInv; - m_result[2][0] = (mx[1][1] * mx[2][3] * mx[3][0] - mx[1][3] * mx[2][1] * mx[3][0] + mx[1][3] * mx[2][0] * mx[3][1] - mx[1][0] * mx[2][3] * mx[3][1] - mx[1][1] * mx[2][0] * mx[3][3] + mx[1][0] * mx[2][1] * mx[3][3]) * detInv; - m_result[2][1] = (mx[0][3] * mx[2][1] * mx[3][0] - mx[0][1] * mx[2][3] * mx[3][0] - mx[0][3] * mx[2][0] * mx[3][1] + mx[0][0] * mx[2][3] * mx[3][1] + mx[0][1] * mx[2][0] * mx[3][3] - mx[0][0] * mx[2][1] * mx[3][3]) * detInv; - m_result[2][2] = (mx[0][1] * mx[1][3] * mx[3][0] - mx[0][3] * mx[1][1] * mx[3][0] + mx[0][3] * mx[1][0] * mx[3][1] - mx[0][0] * mx[1][3] * mx[3][1] - mx[0][1] * mx[1][0] * mx[3][3] + mx[0][0] * mx[1][1] * mx[3][3]) * detInv; - m_result[2][3] = (mx[0][3] * mx[1][1] * mx[2][0] - mx[0][1] * mx[1][3] * mx[2][0] - mx[0][3] * mx[1][0] * mx[2][1] + mx[0][0] * mx[1][3] * mx[2][1] + mx[0][1] * mx[1][0] * mx[2][3] - mx[0][0] * mx[1][1] * mx[2][3]) * detInv; - m_result[3][0] = (mx[1][2] * mx[2][1] * mx[3][0] - mx[1][1] * mx[2][2] * mx[3][0] - mx[1][2] * mx[2][0] * mx[3][1] + mx[1][0] * mx[2][2] * mx[3][1] + mx[1][1] * mx[2][0] * mx[3][2] - mx[1][0] * mx[2][1] * mx[3][2]) * detInv; - m_result[3][1] = (mx[0][1] * mx[2][2] * mx[3][0] - mx[0][2] * mx[2][1] * mx[3][0] + mx[0][2] * mx[2][0] * mx[3][1] - mx[0][0] * mx[2][2] * mx[3][1] - mx[0][1] * mx[2][0] * mx[3][2] + mx[0][0] * mx[2][1] * mx[3][2]) * detInv; - m_result[3][2] = (mx[0][2] * mx[1][1] * mx[3][0] - mx[0][1] * mx[1][2] * mx[3][0] - mx[0][2] * mx[1][0] * mx[3][1] + mx[0][0] * mx[1][2] * mx[3][1] + mx[0][1] * mx[1][0] * mx[3][2] - mx[0][0] * mx[1][1] * mx[3][2]) * detInv; - m_result[3][3] = (mx[0][1] * mx[1][2] * mx[2][0] - mx[0][2] * mx[1][1] * mx[2][0] + mx[0][2] * mx[1][0] * mx[2][1] - mx[0][0] * mx[1][2] * mx[2][1] - mx[0][1] * mx[1][0] * mx[2][2] + mx[0][0] * mx[1][1] * mx[2][2]) * detInv; + m_result[0][0] = + (mx[1][2] * mx[2][3] * mx[3][1] - mx[1][3] * mx[2][2] * mx[3][1] + mx[1][3] * mx[2][1] * mx[3][2] - + mx[1][1] * mx[2][3] * mx[3][2] - mx[1][2] * mx[2][1] * mx[3][3] + mx[1][1] * mx[2][2] * mx[3][3]) * + detInv; + m_result[0][1] = + (mx[0][3] * mx[2][2] * mx[3][1] - mx[0][2] * mx[2][3] * mx[3][1] - mx[0][3] * mx[2][1] * mx[3][2] + + mx[0][1] * mx[2][3] * mx[3][2] + mx[0][2] * mx[2][1] * mx[3][3] - mx[0][1] * mx[2][2] * mx[3][3]) * + detInv; + m_result[0][2] = + (mx[0][2] * mx[1][3] * mx[3][1] - mx[0][3] * mx[1][2] * mx[3][1] + mx[0][3] * mx[1][1] * mx[3][2] - + mx[0][1] * mx[1][3] * mx[3][2] - mx[0][2] * mx[1][1] * mx[3][3] + mx[0][1] * mx[1][2] * mx[3][3]) * + detInv; + m_result[0][3] = + (mx[0][3] * mx[1][2] * mx[2][1] - mx[0][2] * mx[1][3] * mx[2][1] - mx[0][3] * mx[1][1] * mx[2][2] + + mx[0][1] * mx[1][3] * mx[2][2] + mx[0][2] * mx[1][1] * mx[2][3] - mx[0][1] * mx[1][2] * mx[2][3]) * + detInv; + m_result[1][0] = + (mx[1][3] * mx[2][2] * mx[3][0] - mx[1][2] * mx[2][3] * mx[3][0] - mx[1][3] * mx[2][0] * mx[3][2] + + mx[1][0] * mx[2][3] * mx[3][2] + mx[1][2] * mx[2][0] * mx[3][3] - mx[1][0] * mx[2][2] * mx[3][3]) * + detInv; + m_result[1][1] = + (mx[0][2] * mx[2][3] * mx[3][0] - mx[0][3] * mx[2][2] * mx[3][0] + mx[0][3] * mx[2][0] * mx[3][2] - + mx[0][0] * mx[2][3] * mx[3][2] - mx[0][2] * mx[2][0] * mx[3][3] + mx[0][0] * mx[2][2] * mx[3][3]) * + detInv; + m_result[1][2] = + (mx[0][3] * mx[1][2] * mx[3][0] - mx[0][2] * mx[1][3] * mx[3][0] - mx[0][3] * mx[1][0] * mx[3][2] + + mx[0][0] * mx[1][3] * mx[3][2] + mx[0][2] * mx[1][0] * mx[3][3] - mx[0][0] * mx[1][2] * mx[3][3]) * + detInv; + m_result[1][3] = + (mx[0][2] * mx[1][3] * mx[2][0] - mx[0][3] * mx[1][2] * mx[2][0] + mx[0][3] * mx[1][0] * mx[2][2] - + mx[0][0] * mx[1][3] * mx[2][2] - mx[0][2] * mx[1][0] * mx[2][3] + mx[0][0] * mx[1][2] * mx[2][3]) * + detInv; + m_result[2][0] = + (mx[1][1] * mx[2][3] * mx[3][0] - mx[1][3] * mx[2][1] * mx[3][0] + mx[1][3] * mx[2][0] * mx[3][1] - + mx[1][0] * mx[2][3] * mx[3][1] - mx[1][1] * mx[2][0] * mx[3][3] + mx[1][0] * mx[2][1] * mx[3][3]) * + detInv; + m_result[2][1] = + (mx[0][3] * mx[2][1] * mx[3][0] - mx[0][1] * mx[2][3] * mx[3][0] - mx[0][3] * mx[2][0] * mx[3][1] + + mx[0][0] * mx[2][3] * mx[3][1] + mx[0][1] * mx[2][0] * mx[3][3] - mx[0][0] * mx[2][1] * mx[3][3]) * + detInv; + m_result[2][2] = + (mx[0][1] * mx[1][3] * mx[3][0] - mx[0][3] * mx[1][1] * mx[3][0] + mx[0][3] * mx[1][0] * mx[3][1] - + mx[0][0] * mx[1][3] * mx[3][1] - mx[0][1] * mx[1][0] * mx[3][3] + mx[0][0] * mx[1][1] * mx[3][3]) * + detInv; + m_result[2][3] = + (mx[0][3] * mx[1][1] * mx[2][0] - mx[0][1] * mx[1][3] * mx[2][0] - mx[0][3] * mx[1][0] * mx[2][1] + + mx[0][0] * mx[1][3] * mx[2][1] + mx[0][1] * mx[1][0] * mx[2][3] - mx[0][0] * mx[1][1] * mx[2][3]) * + detInv; + m_result[3][0] = + (mx[1][2] * mx[2][1] * mx[3][0] - mx[1][1] * mx[2][2] * mx[3][0] - mx[1][2] * mx[2][0] * mx[3][1] + + mx[1][0] * mx[2][2] * mx[3][1] + mx[1][1] * mx[2][0] * mx[3][2] - mx[1][0] * mx[2][1] * mx[3][2]) * + detInv; + m_result[3][1] = + (mx[0][1] * mx[2][2] * mx[3][0] - mx[0][2] * mx[2][1] * mx[3][0] + mx[0][2] * mx[2][0] * mx[3][1] - + mx[0][0] * mx[2][2] * mx[3][1] - mx[0][1] * mx[2][0] * mx[3][2] + mx[0][0] * mx[2][1] * mx[3][2]) * + detInv; + m_result[3][2] = + (mx[0][2] * mx[1][1] * mx[3][0] - mx[0][1] * mx[1][2] * mx[3][0] - mx[0][2] * mx[1][0] * mx[3][1] + + mx[0][0] * mx[1][2] * mx[3][1] + mx[0][1] * mx[1][0] * mx[3][2] - mx[0][0] * mx[1][1] * mx[3][2]) * + detInv; + m_result[3][3] = + (mx[0][1] * mx[1][2] * mx[2][0] - mx[0][2] * mx[1][1] * mx[2][0] + mx[0][2] * mx[1][0] * mx[2][1] - + mx[0][0] * mx[1][2] * mx[2][1] - mx[0][1] * mx[1][0] * mx[2][2] + mx[0][0] * mx[1][1] * mx[2][2]) * + detInv; } else { BSK_PRINT(MSG_ERROR, "Error: singular 4x4 matrix inverse"); - m44Set(0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - m_result); + m44Set(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, m_result); status = 1; } m44Copy(m_result, result); return status; } -void m66Set(double m00, double m01, double m02, double m03, double m04, double m05, - double m10, double m11, double m12, double m13, double m14, double m15, - double m20, double m21, double m22, double m23, double m24, double m25, - double m30, double m31, double m32, double m33, double m34, double m35, - double m40, double m41, double m42, double m43, double m44, double m45, - double m50, double m51, double m52, double m53, double m54, double m55, - double m[6][6]) +void +m66Set(double m00, + double m01, + double m02, + double m03, + double m04, + double m05, + double m10, + double m11, + double m12, + double m13, + double m14, + double m15, + double m20, + double m21, + double m22, + double m23, + double m24, + double m25, + double m30, + double m31, + double m32, + double m33, + double m34, + double m35, + double m40, + double m41, + double m42, + double m43, + double m44, + double m45, + double m50, + double m51, + double m52, + double m53, + double m54, + double m55, + double m[6][6]) { m[0][0] = m00; m[0][1] = m01; @@ -2351,137 +2395,133 @@ void m66Set(double m00, double m01, double m02, double m03, double m04, double m m[5][5] = m55; } -void m66Copy(double mx[6][6], - double result[6][6]) +void +m66Copy(double mx[6][6], double result[6][6]) { size_t dim1 = 6; size_t dim2 = 6; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = mx[i][j]; } } } -void m66SetZero(double result[6][6]) +void +m66SetZero(double result[6][6]) { size_t dim1 = 6; size_t dim2 = 6; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = 0.0; } } } -void m66SetIdentity(double result[6][6]) +void +m66SetIdentity(double result[6][6]) { size_t dim = 6; size_t i; size_t j; - for(i = 0; i < dim; i++) { - for(j = 0; j < dim; j++) { + for (i = 0; i < dim; i++) { + for (j = 0; j < dim; j++) { result[i][j] = (i == j) ? 1.0 : 0.0; } } } -void m66Transpose(double mx[6][6], - double result[6][6]) +void +m66Transpose(double mx[6][6], double result[6][6]) { size_t dim1 = 6; size_t dim2 = 6; size_t i; size_t j; double m_result[6][6]; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { m_result[j][i] = mx[i][j]; } } m66Copy(m_result, result); } -void m66Get33Matrix(size_t row, size_t col, - double m[6][6], - double mij[3][3]) +void +m66Get33Matrix(size_t row, size_t col, double m[6][6], double mij[3][3]) { size_t i; size_t j; /* WARNING! Doesn't error check that row = 0,1,N-1 and col = 0,1,N-1 */ - for(i = 0; i < 3; i++) { - for(j = 0; j < 3; j++) { + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { mij[i][j] = m[row * 3 + i][col * 3 + j]; } } } -void m66Set33Matrix(size_t row, size_t col, - double mij[3][3], - double m[6][6]) +void +m66Set33Matrix(size_t row, size_t col, double mij[3][3], double m[6][6]) { size_t i; size_t j; /* WARNING! Doesn't error check that row = 0,1,N-1 and col = 0,1,N-1 */ - for(i = 0; i < 3; i++) { - for(j = 0; j < 3; j++) { + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { m[row * 3 + i][col * 3 + j] = mij[i][j]; } } } -void m66Scale(double scaleFactor, - double mx[6][6], - double result[6][6]) +void +m66Scale(double scaleFactor, double mx[6][6], double result[6][6]) { size_t dim1 = 6; size_t dim2 = 6; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = scaleFactor * mx[i][j]; } } } -void m66Add(double mx1[6][6], - double mx2[6][6], - double result[6][6]) +void +m66Add(double mx1[6][6], double mx2[6][6], double result[6][6]) { size_t dim1 = 6; size_t dim2 = 6; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = mx1[i][j] + mx2[i][j]; } } } -void m66Subtract(double mx1[6][6], - double mx2[6][6], - double result[6][6]) +void +m66Subtract(double mx1[6][6], double mx2[6][6], double result[6][6]) { size_t dim1 = 6; size_t dim2 = 6; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = mx1[i][j] - mx2[i][j]; } } } -void m66MultM66(double mx1[6][6], - double mx2[6][6], - double result[6][6]) +void +m66MultM66(double mx1[6][6], double mx2[6][6], double result[6][6]) { size_t dim11 = 6; size_t dim12 = 6; @@ -2490,10 +2530,10 @@ void m66MultM66(double mx1[6][6], size_t j; size_t k; double m_result[6][6]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i][j] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i][j] += mx1[i][k] * mx2[k][j]; } } @@ -2501,9 +2541,8 @@ void m66MultM66(double mx1[6][6], m66Copy(m_result, result); } -void m66tMultM66(double mx1[6][6], - double mx2[6][6], - double result[6][6]) +void +m66tMultM66(double mx1[6][6], double mx2[6][6], double result[6][6]) { size_t dim11 = 6; size_t dim12 = 6; @@ -2512,10 +2551,10 @@ void m66tMultM66(double mx1[6][6], size_t j; size_t k; double m_result[6][6]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i][j] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i][j] += mx1[k][i] * mx2[k][j]; } } @@ -2523,9 +2562,8 @@ void m66tMultM66(double mx1[6][6], m66Copy(m_result, result); } -void m66MultM66t(double mx1[6][6], - double mx2[6][6], - double result[6][6]) +void +m66MultM66t(double mx1[6][6], double mx2[6][6], double result[6][6]) { size_t dim11 = 6; size_t dim12 = 6; @@ -2534,10 +2572,10 @@ void m66MultM66t(double mx1[6][6], size_t j; size_t k; double m_result[6][6]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim21; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim21; j++) { m_result[i][j] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i][j] += mx1[i][k] * mx2[j][k]; } } @@ -2545,9 +2583,8 @@ void m66MultM66t(double mx1[6][6], m66Copy(m_result, result); } -void m66MultV6(double mx[6][6], - double v[6], - double result[6]) +void +m66MultV6(double mx[6][6], double v[6], double result[6]) { size_t dim11 = 6; size_t dim12 = 6; @@ -2556,10 +2593,10 @@ void m66MultV6(double mx[6][6], size_t j; size_t k; double m_result[6]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i] += mx[i][k] * v[k]; } } @@ -2567,9 +2604,8 @@ void m66MultV6(double mx[6][6], v6Copy(m_result, result); } -void m66tMultV6(double mx[6][6], - double v[6], - double result[6]) +void +m66tMultV6(double mx[6][6], double v[6], double result[6]) { size_t dim11 = 6; size_t dim12 = 6; @@ -2578,10 +2614,10 @@ void m66tMultV6(double mx[6][6], size_t j; size_t k; double m_result[6]; - for(i = 0; i < dim11; i++) { - for(j = 0; j < dim22; j++) { + for (i = 0; i < dim11; i++) { + for (j = 0; j < dim22; j++) { m_result[i] = 0.0; - for(k = 0; k < dim12; k++) { + for (k = 0; k < dim12; k++) { m_result[i] += mx[k][i] * v[k]; } } @@ -2589,17 +2625,16 @@ void m66tMultV6(double mx[6][6], v6Copy(m_result, result); } -int m66IsEqual(double mx1[6][6], - double mx2[6][6], - double accuracy) +int +m66IsEqual(double mx1[6][6], double mx2[6][6], double accuracy) { size_t dim1 = 6; size_t dim2 = 6; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { - if(fabs(mx1[i][j] - mx2[i][j]) > accuracy) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { + if (fabs(mx1[i][j] - mx2[i][j]) > accuracy) { return 0; } } @@ -2607,16 +2642,16 @@ int m66IsEqual(double mx1[6][6], return 1; } -int m66IsZero(double mx[6][6], - double accuracy) +int +m66IsZero(double mx[6][6], double accuracy) { size_t dim1 = 6; size_t dim2 = 6; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { - if(fabs(mx[i][j]) > accuracy) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { + if (fabs(mx[i][j]) > accuracy) { return 0; } } @@ -2624,20 +2659,22 @@ int m66IsZero(double mx[6][6], return 1; } -void m99SetZero(double result[9][9]) +void +m99SetZero(double result[9][9]) { size_t dim1 = 9; size_t dim2 = 9; size_t i; size_t j; - for(i = 0; i < dim1; i++) { - for(j = 0; j < dim2; j++) { + for (i = 0; i < dim1; i++) { + for (j = 0; j < dim2; j++) { result[i][j] = 0.0; } } } -void cubicRoots(double a[3], double result[3]) +void +cubicRoots(double a[3], double result[3]) { /* Solve cubic formula for x^3 + a[2]*x^2 + a[1]*x + a[0] = 0 */ /* see http://mathworld.wolfram.com/CubicFormula.html */ @@ -2647,7 +2684,7 @@ void cubicRoots(double a[3], double result[3]) double R = (a[2] * (9 * a[1] - 2 * a2sq) - 27 * a[0]) / 54.0; double RdsqrtnQ3 = R / sqrt(-Q * Q * Q); - if(Q < 0.0 && fabs(RdsqrtnQ3) < 1.0) { + if (Q < 0.0 && fabs(RdsqrtnQ3) < 1.0) { /* A = 2*sqrt(-Q) * B = a[2]/3 * result[0] = Acos(t) - B @@ -2674,26 +2711,31 @@ void cubicRoots(double a[3], double result[3]) result[1] = -a2d3 - 0.5 * (S + T); result[2] = result[1]; } - } -double safeAcos (double x) { +double +safeAcos(double x) +{ if (x < -1.0) return acos(-1); else if (x > 1.0) - return acos(1) ; - return acos (x) ; + return acos(1); + return acos(x); } -double safeAsin (double x) { +double +safeAsin(double x) +{ if (x < -1.0) return asin(-1); else if (x > 1.0) - return asin(1) ; - return asin (x) ; + return asin(1); + return asin(x); } -double safeSqrt(double x) { +double +safeSqrt(double x) +{ if (x < 0.0) return 0.0; return sqrt(x); diff --git a/src/architecture/utilities/linearAlgebra.h b/src/architecture/utilities/linearAlgebra.h index aa16b45ad8..36494df84a 100644 --- a/src/architecture/utilities/linearAlgebra.h +++ b/src/architecture/utilities/linearAlgebra.h @@ -28,229 +28,276 @@ /* define a maximum array size for the functions that need to allocate memory within their routine */ -#define LINEAR_ALGEBRA_MAX_ARRAY_SIZE (64*64) +#define LINEAR_ALGEBRA_MAX_ARRAY_SIZE (64 * 64) -#define MXINDEX(dim2, row, col) ((row)*(dim2) + (col)) +#define MXINDEX(dim2, row, col) ((row) * (dim2) + (col)) /* General vectors */ #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif /* N element vectors */ - void vElementwiseMult(double *v1, size_t dim, double *v2, double *result); - void vCopy(double *v, size_t dim, double *result); - void vSetZero(double *v, size_t dim); - void vSetOnes(double *v, size_t dim); - void vAdd(double *v1, size_t dim, double *v2, double *result); - void vSubtract(double *v1, size_t dim, double *v2, double *result); - void vScale(double scaleFactor, double *v, size_t dim, double *result); - double vDot(double *v1, size_t dim, double *v2); - void vOuterProduct(double *v1, size_t dim1, double *v2, size_t dim2, void *result); - void vtMultM(double *v, void *mx, size_t dim1, size_t dim2, void *result); - void vtMultMt(double *v, void *mx, size_t dim1, size_t dim2, void *result); - double vNorm(double *v, size_t dim); - double vMax(double *v, size_t dim); /* Non-sorted, non-optimized algorithm for finding the max of a small 1-D array*/ - double vMaxAbs(double *v, size_t dim); /* Non-sorted, non-optimized algorithm for finding the max of the absolute values of the elements of a small 1-D array*/ - void vNormalize(double *v, size_t dim, double *result); - int vIsEqual(double *v1, size_t dim, double *v2, double accuracy); - int vIsZero(double *v, size_t dim, double accuracy); - void vPrint(FILE *pFile, const char *name, double *v, size_t dim); - void vSort(double *Input, double *Output, size_t dim); + void vElementwiseMult(double* v1, size_t dim, double* v2, double* result); + void vCopy(double* v, size_t dim, double* result); + void vSetZero(double* v, size_t dim); + void vSetOnes(double* v, size_t dim); + void vAdd(double* v1, size_t dim, double* v2, double* result); + void vSubtract(double* v1, size_t dim, double* v2, double* result); + void vScale(double scaleFactor, double* v, size_t dim, double* result); + double vDot(double* v1, size_t dim, double* v2); + void vOuterProduct(double* v1, size_t dim1, double* v2, size_t dim2, void* result); + void vtMultM(double* v, void* mx, size_t dim1, size_t dim2, void* result); + void vtMultMt(double* v, void* mx, size_t dim1, size_t dim2, void* result); + double vNorm(double* v, size_t dim); + double vMax(double* v, + size_t dim); /* Non-sorted, non-optimized algorithm for finding the max of a small 1-D array*/ + double vMaxAbs(double* v, size_t dim); /* Non-sorted, non-optimized algorithm for finding the max of the absolute + values of the elements of a small 1-D array*/ + void vNormalize(double* v, size_t dim, double* result); + int vIsEqual(double* v1, size_t dim, double* v2, double accuracy); + int vIsZero(double* v, size_t dim, double accuracy); + void vPrint(FILE* pFile, const char* name, double* v, size_t dim); + void vSort(double* Input, double* Output, size_t dim); /* 2 element vectors */ - void v2Set(double v0, double v1, double result[2]); - void v2Copy(double v[2], double result[2]); - void v2Scale(double scaleFactor, double v[2], double result[2]); - void v2SetZero(double v[2]); - double v2Dot(double v1[2], double v2[2]); - int v2IsEqual(double v1[2], double v2[2], double accuracy); - int v2IsZero(double v[2], double accuracy); - void v2Add(double v1[2], double v2[2], double result[2]); - void v2Subtract(double v1[2], double v2[2], double result[2]); - double v2Norm(double v1[2]); - void v2Normalize(double v1[2], double result[2]); + void v2Set(double v0, double v1, double result[2]); + void v2Copy(double v[2], double result[2]); + void v2Scale(double scaleFactor, double v[2], double result[2]); + void v2SetZero(double v[2]); + double v2Dot(double v1[2], double v2[2]); + int v2IsEqual(double v1[2], double v2[2], double accuracy); + int v2IsZero(double v[2], double accuracy); + void v2Add(double v1[2], double v2[2], double result[2]); + void v2Subtract(double v1[2], double v2[2], double result[2]); + double v2Norm(double v1[2]); + void v2Normalize(double v1[2], double result[2]); /* 3 element vectors */ - void v3Set(double v0, double v1, double v2, double result[3]); - void v3Copy(double v[3], double result[3]); - void v3SetZero(double v[3]); - void v3Add(double v1[3], double v2[3], double result[3]); - void v3Subtract(double v1[3], double v2[3], double result[3]); - void v3Scale(double scaleFactor, double v[3], double result[3]); - double v3Dot(double v1[3], double v2[3]); - void v3OuterProduct(double v1[3], double v2[3], double result[3][3]); - void v3tMultM33(double v[3], double mx[3][3], double result[3]); - void v3tMultM33t(double v[3], double mx[3][3], double result[3]); - double v3Norm(double v[3]); - void v3Normalize(double v[3], double result[3]); - int v3IsEqual(double v1[3], double v2[3], double accuracy); - int v3IsEqualRel(double v1[3], double v2[3], double accuracy); - int v3IsZero(double v[3], double accuracy); - void v3Print(FILE *pFile, const char *name, double v[3]); - void v3Cross(double v1[3], double v2[3], double result[3]); - void v3Perpendicular(double v[3], double result[3]); - void v3Tilde(double v[3], double result[3][3]); - void v3Sort(double v[3], double result[3]); - void v3PrintScreen(const char *name, double v[3]); + void v3Set(double v0, double v1, double v2, double result[3]); + void v3Copy(double v[3], double result[3]); + void v3SetZero(double v[3]); + void v3Add(double v1[3], double v2[3], double result[3]); + void v3Subtract(double v1[3], double v2[3], double result[3]); + void v3Scale(double scaleFactor, double v[3], double result[3]); + double v3Dot(double v1[3], double v2[3]); + void v3OuterProduct(double v1[3], double v2[3], double result[3][3]); + void v3tMultM33(double v[3], double mx[3][3], double result[3]); + void v3tMultM33t(double v[3], double mx[3][3], double result[3]); + double v3Norm(double v[3]); + void v3Normalize(double v[3], double result[3]); + int v3IsEqual(double v1[3], double v2[3], double accuracy); + int v3IsEqualRel(double v1[3], double v2[3], double accuracy); + int v3IsZero(double v[3], double accuracy); + void v3Print(FILE* pFile, const char* name, double v[3]); + void v3Cross(double v1[3], double v2[3], double result[3]); + void v3Perpendicular(double v[3], double result[3]); + void v3Tilde(double v[3], double result[3][3]); + void v3Sort(double v[3], double result[3]); + void v3PrintScreen(const char* name, double v[3]); /* 4 element vectors */ - void v4Set(double v0, double v1, double v2, double v3, double result[4]); - void v4Copy(double v[4], double result[4]); - void v4SetZero(double v[4]); - double v4Dot(double v1[4], double v2[4]); - double v4Norm(double v[4]); - int v4IsEqual(double v1[4], double v2[4], double accuracy); - int v4IsZero(double v[4], double accuracy); + void v4Set(double v0, double v1, double v2, double v3, double result[4]); + void v4Copy(double v[4], double result[4]); + void v4SetZero(double v[4]); + double v4Dot(double v1[4], double v2[4]); + double v4Norm(double v[4]); + int v4IsEqual(double v1[4], double v2[4], double accuracy); + int v4IsZero(double v[4], double accuracy); /* 6 element vectors */ - void v6Set(double v0, double v1, double v2, double v3, double v4, double v5, double result[6]); - void v6Copy(double v[6], double result[6]); - double v6Dot(double v1[6], double v2[6]); - void v6Scale(double scaleFactor, double v[6], double result[6]); - void v6OuterProduct(double v1[6], double v2[6], double result[6][6]); - int v6IsEqual(double v1[6], double v2[6], double accuracy); + void v6Set(double v0, double v1, double v2, double v3, double v4, double v5, double result[6]); + void v6Copy(double v[6], double result[6]); + double v6Dot(double v1[6], double v2[6]); + void v6Scale(double scaleFactor, double v[6], double result[6]); + void v6OuterProduct(double v1[6], double v2[6], double result[6][6]); + int v6IsEqual(double v1[6], double v2[6], double accuracy); /* NxM matrices */ - void mLeastSquaresInverse(void *mx, size_t dim1, size_t dim2, void *result); - void mMinimumNormInverse(void *mx, size_t dim1, size_t dim2, void *result); - void mCopy(void *mx, size_t dim1, size_t dim2, void *result); - void mSetZero(void *result, size_t dim1, size_t dim2); - void mSetIdentity(void *result, size_t dim1, size_t dim2); - void mDiag(void *v, size_t dim, void *result); - void mTranspose(void *mx, size_t dim1, size_t dim2, void *result); - void mAdd(void *mx1, size_t dim1, size_t dim2, void *mx2, void *result); - void mSubtract(void *mx1, size_t dim1, size_t dim2, void *mx2, void *result); - void mScale(double scaleFactor, void *mx, size_t dim1, size_t dim2, void *result); - void mMultM(void *mx1, size_t dim11, size_t dim12, - void *mx2, size_t dim21, size_t dim22, - void *result); - void mtMultM(void *mx1, size_t dim11, size_t dim12, - void *mx2, size_t dim21, size_t dim22, - void *result); - void mMultMt(void *mx1, size_t dim11, size_t dim12, - void *mx2, size_t dim21, size_t dim22, - void *result); - void mtMultMt(void *mx1, size_t dim11, size_t dim12, - void *mx2, size_t dim21, size_t dim22, - void *result); - void mMultV(void *mx, size_t dim1, size_t dim2, - void *v, - void *result); - void mtMultV(void *mx, size_t dim1, size_t dim2, - void *v, - void *result); - double mTrace(void *mx, size_t dim); - double mDeterminant(void *mx, size_t dim); - void mCofactor(void *mx, size_t dim, void *result); - int mInverse(void *mx, size_t dim, void *result); - int mIsEqual(void *mx1, size_t dim1, size_t dim2, void *mx2, double accuracy); - int mIsZero(void *mx, size_t dim1, size_t dim2, double accuracy); - void mPrintScreen(const char *name, void *mx, size_t dim1, size_t dim2); - void mPrint(FILE *pFile, const char *name, void *mx, size_t dim1, size_t dim2); - void mGetSubMatrix(void *mx, size_t dim1, size_t dim2, - size_t dim1Start, size_t dim2Start, - size_t dim1Result, size_t dim2Result, void *result); - void mSetSubMatrix(void *mx, size_t dim1, size_t dim2, - void *result, size_t dim1Result, size_t dim2Result, - size_t dim1Start, size_t dim2Start); + void mLeastSquaresInverse(void* mx, size_t dim1, size_t dim2, void* result); + void mMinimumNormInverse(void* mx, size_t dim1, size_t dim2, void* result); + void mCopy(void* mx, size_t dim1, size_t dim2, void* result); + void mSetZero(void* result, size_t dim1, size_t dim2); + void mSetIdentity(void* result, size_t dim1, size_t dim2); + void mDiag(void* v, size_t dim, void* result); + void mTranspose(void* mx, size_t dim1, size_t dim2, void* result); + void mAdd(void* mx1, size_t dim1, size_t dim2, void* mx2, void* result); + void mSubtract(void* mx1, size_t dim1, size_t dim2, void* mx2, void* result); + void mScale(double scaleFactor, void* mx, size_t dim1, size_t dim2, void* result); + void mMultM(void* mx1, size_t dim11, size_t dim12, void* mx2, size_t dim21, size_t dim22, void* result); + void mtMultM(void* mx1, size_t dim11, size_t dim12, void* mx2, size_t dim21, size_t dim22, void* result); + void mMultMt(void* mx1, size_t dim11, size_t dim12, void* mx2, size_t dim21, size_t dim22, void* result); + void mtMultMt(void* mx1, size_t dim11, size_t dim12, void* mx2, size_t dim21, size_t dim22, void* result); + void mMultV(void* mx, size_t dim1, size_t dim2, void* v, void* result); + void mtMultV(void* mx, size_t dim1, size_t dim2, void* v, void* result); + double mTrace(void* mx, size_t dim); + double mDeterminant(void* mx, size_t dim); + void mCofactor(void* mx, size_t dim, void* result); + int mInverse(void* mx, size_t dim, void* result); + int mIsEqual(void* mx1, size_t dim1, size_t dim2, void* mx2, double accuracy); + int mIsZero(void* mx, size_t dim1, size_t dim2, double accuracy); + void mPrintScreen(const char* name, void* mx, size_t dim1, size_t dim2); + void mPrint(FILE* pFile, const char* name, void* mx, size_t dim1, size_t dim2); + void mGetSubMatrix(void* mx, + size_t dim1, + size_t dim2, + size_t dim1Start, + size_t dim2Start, + size_t dim1Result, + size_t dim2Result, + void* result); + void mSetSubMatrix(void* mx, + size_t dim1, + size_t dim2, + void* result, + size_t dim1Result, + size_t dim2Result, + size_t dim1Start, + size_t dim2Start); /* 2x2 matrices */ - void m22Set(double m00, double m01, - double m10, double m11, - double m[2][2]); - void m22Copy(double mx[2][2], double result[2][2]); - void m22SetZero(double result[2][2]); - void m22SetIdentity(double result[2][2]); - void m22Transpose(double mx[2][2], double result[2][2]); - void m22Add(double mx1[2][2], double mx2[2][2], double result[2][2]); - void m22Subtract(double mx1[2][2], double mx2[2][2], double result[2][2]); - void m22Scale(double scaleFactor, double mx[2][2], double result[2][2]); - void m22MultM22(double mx1[2][2], double mx2[2][2], double result[2][2]); - void m22tMultM22(double mx1[2][2], double mx2[2][2], double result[2][2]); - void m22MultM22t(double mx1[2][2], double mx2[2][2], double result[2][2]); - void m22MultV2(double mx[2][2], double v[2], double result[2]); - void m22tMultV2(double mx[2][2], double v[2], double result[2]); - double m22Trace(double mx[2][2]); - double m22Determinant(double mx[2][2]); - int m22IsEqual(double mx1[2][2], double mx2[2][2], double accuracy); - int m22IsZero(double mx[2][2], double accuracy); - void m22Print(FILE *pFile, const char *name, double mx[2][2]); - int m22Inverse(double mx[2][2], double result[2][2]); - void m22PrintScreen(const char *name, double mx[2][2]); + void m22Set(double m00, double m01, double m10, double m11, double m[2][2]); + void m22Copy(double mx[2][2], double result[2][2]); + void m22SetZero(double result[2][2]); + void m22SetIdentity(double result[2][2]); + void m22Transpose(double mx[2][2], double result[2][2]); + void m22Add(double mx1[2][2], double mx2[2][2], double result[2][2]); + void m22Subtract(double mx1[2][2], double mx2[2][2], double result[2][2]); + void m22Scale(double scaleFactor, double mx[2][2], double result[2][2]); + void m22MultM22(double mx1[2][2], double mx2[2][2], double result[2][2]); + void m22tMultM22(double mx1[2][2], double mx2[2][2], double result[2][2]); + void m22MultM22t(double mx1[2][2], double mx2[2][2], double result[2][2]); + void m22MultV2(double mx[2][2], double v[2], double result[2]); + void m22tMultV2(double mx[2][2], double v[2], double result[2]); + double m22Trace(double mx[2][2]); + double m22Determinant(double mx[2][2]); + int m22IsEqual(double mx1[2][2], double mx2[2][2], double accuracy); + int m22IsZero(double mx[2][2], double accuracy); + void m22Print(FILE* pFile, const char* name, double mx[2][2]); + int m22Inverse(double mx[2][2], double result[2][2]); + void m22PrintScreen(const char* name, double mx[2][2]); /* 3x3 matrices */ - void m33Set(double m00, double m01, double m02, - double m10, double m11, double m12, - double m20, double m21, double m22, - double m[3][3]); - void m33Copy(double mx[3][3], double result[3][3]); - void m33SetZero(double result[3][3]); - void m33SetIdentity(double result[3][3]); - void m33Transpose(double mx[3][3], double result[3][3]); - void m33Add(double mx1[3][3], double mx2[3][3], double result[3][3]); - void m33Subtract(double mx1[3][3], double mx2[3][3], double result[3][3]); - void m33Scale(double scaleFactor, double mx[3][3], double result[3][3]); - void m33MultM33(double mx1[3][3], double mx2[3][3], double result[3][3]); - void m33tMultM33(double mx1[3][3], double mx2[3][3], double result[3][3]); - void m33MultM33t(double mx1[3][3], double mx2[3][3], double result[3][3]); - void m33MultV3(double mx[3][3], double v[3], double result[3]); - void m33tMultV3(double mx[3][3], double v[3], double result[3]); - double m33Trace(double mx[3][3]); - double m33Determinant(double mx[3][3]); - int m33IsEqual(double mx1[3][3], double mx2[3][3], double accuracy); - int m33IsZero(double mx[3][3], double accuracy); - void m33Print(FILE *pfile, const char *name, double mx[3][3]); - int m33Inverse(double mx[3][3], double result[3][3]); - void m33SingularValues(double mx[3][3], double result[3]); - void m33EigenValues(double mx[3][3], double result[3]); - double m33ConditionNumber(double mx[3][3]); - void m33PrintScreen(const char *name, double mx[3][3]); + void m33Set(double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, + double m22, + double m[3][3]); + void m33Copy(double mx[3][3], double result[3][3]); + void m33SetZero(double result[3][3]); + void m33SetIdentity(double result[3][3]); + void m33Transpose(double mx[3][3], double result[3][3]); + void m33Add(double mx1[3][3], double mx2[3][3], double result[3][3]); + void m33Subtract(double mx1[3][3], double mx2[3][3], double result[3][3]); + void m33Scale(double scaleFactor, double mx[3][3], double result[3][3]); + void m33MultM33(double mx1[3][3], double mx2[3][3], double result[3][3]); + void m33tMultM33(double mx1[3][3], double mx2[3][3], double result[3][3]); + void m33MultM33t(double mx1[3][3], double mx2[3][3], double result[3][3]); + void m33MultV3(double mx[3][3], double v[3], double result[3]); + void m33tMultV3(double mx[3][3], double v[3], double result[3]); + double m33Trace(double mx[3][3]); + double m33Determinant(double mx[3][3]); + int m33IsEqual(double mx1[3][3], double mx2[3][3], double accuracy); + int m33IsZero(double mx[3][3], double accuracy); + void m33Print(FILE* pfile, const char* name, double mx[3][3]); + int m33Inverse(double mx[3][3], double result[3][3]); + void m33SingularValues(double mx[3][3], double result[3]); + void m33EigenValues(double mx[3][3], double result[3]); + double m33ConditionNumber(double mx[3][3]); + void m33PrintScreen(const char* name, double mx[3][3]); /* 4x4 matrices */ - void m44Set(double m00, double m01, double m02, double m03, - double m10, double m11, double m12, double m13, - double m20, double m21, double m22, double m23, - double m30, double m31, double m32, double m33, - double m[4][4]); - void m44Copy(double mx[4][4], double result[4][4]); - void m44SetZero(double result[4][4]); - void m44MultV4(double mx[4][4], double v[4], double result[4]); - double m44Determinant(double mx[4][4]); - int m44IsEqual(double mx1[4][4], double mx2[4][4], double accuracy); - int m44Inverse(double mx[4][4], double result[4][4]); + void m44Set(double m00, + double m01, + double m02, + double m03, + double m10, + double m11, + double m12, + double m13, + double m20, + double m21, + double m22, + double m23, + double m30, + double m31, + double m32, + double m33, + double m[4][4]); + void m44Copy(double mx[4][4], double result[4][4]); + void m44SetZero(double result[4][4]); + void m44MultV4(double mx[4][4], double v[4], double result[4]); + double m44Determinant(double mx[4][4]); + int m44IsEqual(double mx1[4][4], double mx2[4][4], double accuracy); + int m44Inverse(double mx[4][4], double result[4][4]); /* 6x6 matrices */ - void m66Set(double m00, double m01, double m02, double m03, double m04, double m05, - double m10, double m11, double m12, double m13, double m14, double m15, - double m20, double m21, double m22, double m23, double m24, double m25, - double m30, double m31, double m32, double m33, double m34, double m35, - double m40, double m41, double m42, double m43, double m44, double m45, - double m50, double m51, double m52, double m53, double m54, double m55, - double m[6][6]); - void m66Copy(double mx[6][6], double result[6][6]); - void m66SetZero(double result[6][6]); - void m66SetIdentity(double result[6][6]); - void m66Transpose(double mx[6][6], double result[6][6]); - void m66Get33Matrix(size_t row, size_t col, double m[6][6], double mij[3][3]); - void m66Set33Matrix(size_t row, size_t col, double mij[3][3], double m[6][6]); - void m66Scale(double scaleFactor, double mx[6][6], double result[6][6]); - void m66Add(double mx1[6][6], double mx2[6][6], double result[6][6]); - void m66Subtract(double mx1[6][6], double mx2[6][6], double result[6][6]); - void m66MultM66(double mx1[6][6], double mx2[6][6], double result[6][6]); - void m66tMultM66(double mx1[6][6], double mx2[6][6], double result[6][6]); - void m66MultM66t(double mx1[6][6], double mx2[6][6], double result[6][6]); - void m66MultV6(double mx[6][6], double v[6], double result[6]); - void m66tMultV6(double mx[6][6], double v[6], double result[6]); - int m66IsEqual(double mx1[6][6], double mx2[6][6], double accuracy); - int m66IsZero(double mx[6][6], double accuracy); + void m66Set(double m00, + double m01, + double m02, + double m03, + double m04, + double m05, + double m10, + double m11, + double m12, + double m13, + double m14, + double m15, + double m20, + double m21, + double m22, + double m23, + double m24, + double m25, + double m30, + double m31, + double m32, + double m33, + double m34, + double m35, + double m40, + double m41, + double m42, + double m43, + double m44, + double m45, + double m50, + double m51, + double m52, + double m53, + double m54, + double m55, + double m[6][6]); + void m66Copy(double mx[6][6], double result[6][6]); + void m66SetZero(double result[6][6]); + void m66SetIdentity(double result[6][6]); + void m66Transpose(double mx[6][6], double result[6][6]); + void m66Get33Matrix(size_t row, size_t col, double m[6][6], double mij[3][3]); + void m66Set33Matrix(size_t row, size_t col, double mij[3][3], double m[6][6]); + void m66Scale(double scaleFactor, double mx[6][6], double result[6][6]); + void m66Add(double mx1[6][6], double mx2[6][6], double result[6][6]); + void m66Subtract(double mx1[6][6], double mx2[6][6], double result[6][6]); + void m66MultM66(double mx1[6][6], double mx2[6][6], double result[6][6]); + void m66tMultM66(double mx1[6][6], double mx2[6][6], double result[6][6]); + void m66MultM66t(double mx1[6][6], double mx2[6][6], double result[6][6]); + void m66MultV6(double mx[6][6], double v[6], double result[6]); + void m66tMultV6(double mx[6][6], double v[6], double result[6]); + int m66IsEqual(double mx1[6][6], double mx2[6][6], double accuracy); + int m66IsZero(double mx[6][6], double accuracy); /* 9x9 matrices */ - void m99SetZero(double result[9][9]); + void m99SetZero(double result[9][9]); /* additional routines */ /* Solve cubic formula for x^3 + a[2]*x^2 + a[1]*x + a[0] = 0 */ - void cubicRoots(double a[3], double result[3]); + void cubicRoots(double a[3], double result[3]); double safeAcos(double x); double safeAsin(double x); @@ -261,4 +308,3 @@ extern "C" { #endif #endif - diff --git a/src/architecture/utilities/linearInterpolation.hpp b/src/architecture/utilities/linearInterpolation.hpp index f7a26ae9b9..54ab12461f 100644 --- a/src/architecture/utilities/linearInterpolation.hpp +++ b/src/architecture/utilities/linearInterpolation.hpp @@ -31,9 +31,11 @@ @param y2 Function value at point x2 @param x Function x coordinate for interpolation */ -double linearInterpolation(double x1, double x2, double y1, double y2, double x) { +double +linearInterpolation(double x1, double x2, double y1, double y2, double x) +{ - assert(x1 < x && x < x2); + assert(x1 < x && x < x2); return y1 * (x2 - x) / (x2 - x1) + y2 * (x - x1) / (x2 - x1); } diff --git a/src/architecture/utilities/macroDefinitions.h b/src/architecture/utilities/macroDefinitions.h index 45b4c38cfb..2aeeb53acd 100644 --- a/src/architecture/utilities/macroDefinitions.h +++ b/src/architecture/utilities/macroDefinitions.h @@ -26,16 +26,16 @@ #define MAX_NUM_CSS_SENSORS 32 #define MAX_ST_VEH_COUNT 4 -#define NANO2SEC 1e-9 -#define SEC2NANO 1e9 -#define RECAST6X6 (double (*)[6]) -#define RECAST3X3 (double (*)[3]) -#define RECAST2x2 (double (*)[2]) -#define SEC2HOUR 1./3600. - -#include // For uint64_t -#include // For DBL_MANT_DIG -#include // For NAN, fabs() +#define NANO2SEC 1e-9 +#define SEC2NANO 1e9 +#define RECAST6X6 (double (*)[6]) +#define RECAST3X3 (double (*)[3]) +#define RECAST2x2 (double (*)[2]) +#define SEC2HOUR 1. / 3600. + +#include // For uint64_t +#include // For DBL_MANT_DIG +#include // For NAN, fabs() #include #include @@ -43,19 +43,22 @@ * Converts nanoseconds to seconds (double), with basic precision check. * Returns NAN if conversion would lose precision. */ -static inline double nanoToSec(uint64_t nanos) { +static inline double +nanoToSec(uint64_t nanos) +{ // Double precision loses exact integer values past 2^53 - const uint64_t MAX_SAFE_UINT64_FOR_DOUBLE = (1ULL << DBL_MANT_DIG); // Usually 2^53 + const uint64_t MAX_SAFE_UINT64_FOR_DOUBLE = (1ULL << DBL_MANT_DIG); // Usually 2^53 if (nanos > MAX_SAFE_UINT64_FOR_DOUBLE) { fprintf(stderr, - "[nano_to_sec] ERROR: Input %" PRIu64 - " exceeds safe conversion limit (~%" PRIu64 "). Precision may be lost. Returning NAN.\n", - nanos, MAX_SAFE_UINT64_FOR_DOUBLE); - return NAN; // Indicate precision loss + "[nano_to_sec] ERROR: Input %" PRIu64 " exceeds safe conversion limit (~%" PRIu64 + "). Precision may be lost. Returning NAN.\n", + nanos, + MAX_SAFE_UINT64_FOR_DOUBLE); + return NAN; // Indicate precision loss } - return (double)nanos * NANO2SEC; // Convert to seconds + return (double)nanos * NANO2SEC; // Convert to seconds } /** @@ -63,7 +66,9 @@ static inline double nanoToSec(uint64_t nanos) { * with basic precision check. * Returns NAN if conversion would lose precision. */ -static inline double diffNanoToSec(uint64_t time1Nano, uint64_t time2Nano) { +static inline double +diffNanoToSec(uint64_t time1Nano, uint64_t time2Nano) +{ double signedTimeDifference; if (time1Nano >= time2Nano) { @@ -79,24 +84,23 @@ static inline double diffNanoToSec(uint64_t time1Nano, uint64_t time2Nano) { * Converts seconds (double) to nanoseconds (uint64_t). * Returns NAN on error (e.g. negative input or overflow) */ -static inline uint64_t secToNano(double seconds) { +static inline uint64_t +secToNano(double seconds) +{ if (seconds < 0.0) { - fprintf(stderr, - "[secToNano] ERROR: Negative time value passed (%.15f seconds). Returning 0.\n", - seconds); + fprintf(stderr, "[secToNano] ERROR: Negative time value passed (%.15f seconds). Returning 0.\n", seconds); return 0; } double result = seconds * SEC2NANO; if (result > (double)UINT64_MAX) { - fprintf(stderr, - "[secToNano] ERROR: Input time %.15f seconds exceeds uint64_t capacity. Returning 0.\n", - seconds); + fprintf( + stderr, "[secToNano] ERROR: Input time %.15f seconds exceeds uint64_t capacity. Returning 0.\n", seconds); return 0; } - return (uint64_t)(result + 0.5); // Round to nearest integer + return (uint64_t)(result + 0.5); // Round to nearest integer } #endif diff --git a/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.cpp b/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.cpp index 7ab9935fff..70fa7f0328 100644 --- a/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.cpp +++ b/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.cpp @@ -38,27 +38,28 @@ ModuleIdGenerator::ModuleIdGenerator() ModuleIdGenerator::~ModuleIdGenerator() { - delete TheInstance; - TheInstance = NULL; + delete TheInstance; + TheInstance = NULL; } /*! * This gives a pointer to the messaging system to whoever asks for it. * @return ModuleIdGenerator* TheInstance */ -ModuleIdGenerator* ModuleIdGenerator::GetInstance() +ModuleIdGenerator* +ModuleIdGenerator::GetInstance() { - if(TheInstance == NULL) - { + if (TheInstance == NULL) { TheInstance = new ModuleIdGenerator(); } - return(TheInstance); + return (TheInstance); } /*! * This method assigns a module ID to a new module and increments the NextModuleID counter * @return uint64_t nextModuleID the newly minted module ID */ -int64_t ModuleIdGenerator::checkoutModuleID() +int64_t +ModuleIdGenerator::checkoutModuleID() { - return(this->nextModuleID++); + return (this->nextModuleID++); } diff --git a/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h b/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h index 90624cea68..0b2b49c811 100644 --- a/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h +++ b/src/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h @@ -24,24 +24,23 @@ /*! @brief module ID generating class */ #ifdef _WIN32 -class __declspec( dllexport) ModuleIdGenerator +class __declspec(dllexport) ModuleIdGenerator #else class ModuleIdGenerator #endif { -public: - int64_t checkoutModuleID(); //! -- Assigns next integer module ID - static ModuleIdGenerator* GetInstance(); //! -- returns a pointer to the sim instance of ModuleIdGenerator + public: + int64_t checkoutModuleID(); //! -- Assigns next integer module ID + static ModuleIdGenerator* GetInstance(); //! -- returns a pointer to the sim instance of ModuleIdGenerator -private: - int64_t nextModuleID; //!< the next module ID to give out when a module (SysModel sub-class) comes online - static ModuleIdGenerator *TheInstance; //!< instance of simulation module + private: + int64_t nextModuleID; //!< the next module ID to give out when a module (SysModel sub-class) comes online + static ModuleIdGenerator* TheInstance; //!< instance of simulation module ModuleIdGenerator(); ~ModuleIdGenerator(); - ModuleIdGenerator(ModuleIdGenerator const &) {}; - ModuleIdGenerator& operator =(ModuleIdGenerator const &){return(*this);}; - + ModuleIdGenerator(ModuleIdGenerator const&) {}; + ModuleIdGenerator& operator=(ModuleIdGenerator const&) { return (*this); }; }; #endif /* _ModuleIdGenerator_HH_ */ diff --git a/src/architecture/utilities/orbitalMotion.c b/src/architecture/utilities/orbitalMotion.c index 99291e0ffb..e50af0a10c 100644 --- a/src/architecture/utilities/orbitalMotion.c +++ b/src/architecture/utilities/orbitalMotion.c @@ -27,7 +27,6 @@ #include "astroConstants.h" #include "architecture/utilities/bsk_Print.h" - /*! * Purpose: maps inertial position and velocity vectors in the Hill frame DCM HN * Inputs: @@ -36,12 +35,13 @@ * Outputs: * HN: Hill frame DCM relative to inertial frame */ -void hillFrame(double *rc_N, double *vc_N, double HN[3][3]) +void +hillFrame(double* rc_N, double* vc_N, double HN[3][3]) { - double ir_N[3]; /* orbit radial unit direction vector */ - double itheta_N[3]; /* along-track unit direction vector */ - double ih_N[3]; /* orbit plane normal unit direction vector */ - double hVec_N[3]; /* orbit angular momentum vector */ + double ir_N[3]; /* orbit radial unit direction vector */ + double itheta_N[3]; /* along-track unit direction vector */ + double ih_N[3]; /* orbit plane normal unit direction vector */ + double hVec_N[3]; /* orbit angular momentum vector */ v3Normalize(rc_N, ir_N); v3Cross(rc_N, vc_N, hVec_N); @@ -63,21 +63,22 @@ void hillFrame(double *rc_N, double *vc_N, double HN[3][3]) * rd_N: deputy inertial position vector * vd_N: deputy inertial velocity vector */ -void hill2rv(double *rc_N, double *vc_N, double *rho_H, double *rhoPrime_H, double *rd_N, double *vd_N) +void +hill2rv(double* rc_N, double* vc_N, double* rho_H, double* rhoPrime_H, double* rd_N, double* vd_N) { - double HN[3][3]; /* DCM of Hill frame relative to inertial */ - double NH[3][3]; /* DCM of inertial frame relative to Hill */ - double hVec_N[3]; /* orbit angular momentum vector */ - double rc; /* chief orbit radius */ - double fDot; /* chief true anomaly rate */ - double omega_HN_H[3]; /* Hill frame angular velocity */ - double rho_N[3]; /* deputy relative to chief vector in N frame components */ + double HN[3][3]; /* DCM of Hill frame relative to inertial */ + double NH[3][3]; /* DCM of inertial frame relative to Hill */ + double hVec_N[3]; /* orbit angular momentum vector */ + double rc; /* chief orbit radius */ + double fDot; /* chief true anomaly rate */ + double omega_HN_H[3]; /* Hill frame angular velocity */ + double rho_N[3]; /* deputy relative to chief vector in N frame components */ hillFrame(rc_N, vc_N, HN); m33Transpose(HN, NH); v3Cross(rc_N, vc_N, hVec_N); rc = v3Norm(rc_N); - fDot = v3Norm(hVec_N)/rc/rc; + fDot = v3Norm(hVec_N) / rc / rc; v3Set(0, 0, fDot, omega_HN_H); /* compute inertial deputy position */ @@ -91,7 +92,6 @@ void hill2rv(double *rc_N, double *vc_N, double *rho_H, double *rhoPrime_H, dou v3Add(vd_N, vc_N, vd_N); } - /*! * Purpose: maps inertial frame deputy states to Hill inertial position and velocity vectors * Inputs: @@ -103,21 +103,22 @@ void hill2rv(double *rc_N, double *vc_N, double *rho_H, double *rhoPrime_H, dou * rho_H: deputy Hill position vector * rhoPrime_H: deputy Hill velocity vector */ -void rv2hill(double *rc_N, double *vc_N, double *rd_N, double *vd_N, double *rho_H, double *rhoPrime_H) +void +rv2hill(double* rc_N, double* vc_N, double* rd_N, double* vd_N, double* rho_H, double* rhoPrime_H) { - double HN[3][3]; /* DCM of Hill frame relative to inertial */ - double hVec_N[3]; /* orbit angular momentum vector */ - double rc; /* chief orbit radius */ - double fDot; /* chief true anomaly rate */ - double omega_HN_H[3]; /* Hill frame angular velocity */ - double rho_N[3]; /* deputy relative to chief vector in N frame components */ - double rhoDot_N[3]; /* inertial derivative of deputy/chief vector */ - double rhoDot_H[3]; /* inertial derivative of deputy/chief vector */ + double HN[3][3]; /* DCM of Hill frame relative to inertial */ + double hVec_N[3]; /* orbit angular momentum vector */ + double rc; /* chief orbit radius */ + double fDot; /* chief true anomaly rate */ + double omega_HN_H[3]; /* Hill frame angular velocity */ + double rho_N[3]; /* deputy relative to chief vector in N frame components */ + double rhoDot_N[3]; /* inertial derivative of deputy/chief vector */ + double rhoDot_H[3]; /* inertial derivative of deputy/chief vector */ hillFrame(rc_N, vc_N, HN); v3Cross(rc_N, vc_N, hVec_N); rc = v3Norm(rc_N); - fDot = v3Norm(hVec_N)/rc/rc; + fDot = v3Norm(hVec_N) / rc / rc; v3Set(0, 0, fDot, omega_HN_H); /* compute Hill frame position */ @@ -131,7 +132,6 @@ void rv2hill(double *rc_N, double *vc_N, double *rd_N, double *vd_N, double *rho v3Subtract(rhoDot_H, rhoPrime_H, rhoPrime_H); } - /*! * Purpose: Maps eccentric anomaly angles into true anomaly angles. * This function requires the orbit to be either circular or @@ -142,11 +142,12 @@ void rv2hill(double *rc_N, double *vc_N, double *rd_N, double *vd_N, double *rho * Outputs: * f = true anomaly (rad) */ -double E2f(double Ecc, double e) +double +E2f(double Ecc, double e) { double f; - if((e >= 0) && (e < 1)) { + if ((e >= 0) && (e < 1)) { f = 2 * atan2(sqrt(1 + e) * sin(Ecc / 2), sqrt(1 - e) * cos(Ecc / 2)); } else { f = NAN; @@ -166,11 +167,12 @@ double E2f(double Ecc, double e) * Outputs: * M = mean elliptic anomaly (rad) */ -double E2M(double Ecc, double e) +double +E2M(double Ecc, double e) { double M; - if((e >= 0) && (e < 1)) { + if ((e >= 0) && (e < 1)) { M = Ecc - e * sin(Ecc); } else { M = NAN; @@ -190,11 +192,12 @@ double E2M(double Ecc, double e) * Outputs: * Ecc = eccentric anomaly (rad) */ -double f2E(double f, double e) +double +f2E(double f, double e) { double Ecc; - if((e >= 0) && (e < 1)) { + if ((e >= 0) && (e < 1)) { Ecc = 2 * atan2(sqrt(1 - e) * sin(f / 2), sqrt(1 + e) * cos(f / 2)); } else { Ecc = NAN; @@ -213,11 +216,12 @@ double f2E(double f, double e) * Outputs: * H = hyperbolic anomaly (rad) */ -double f2H(double f, double e) +double +f2H(double f, double e) { double H; - if(e > 1) { + if (e > 1) { H = 2 * atanh(sqrt((e - 1) / (e + 1)) * tan(f / 2)); } else { H = NAN; @@ -236,11 +240,12 @@ double f2H(double f, double e) * Outputs: * f = true anomaly angle (rad) */ -double H2f(double H, double e) +double +H2f(double H, double e) { double f; - if(e > 1) { + if (e > 1) { f = 2 * atan(sqrt((e + 1) / (e - 1)) * tanh(H / 2)); } else { f = NAN; @@ -259,11 +264,12 @@ double H2f(double H, double e) * Outputs: * N = mean hyperbolic anomaly (rad) */ -double H2N(double H, double e) +double +H2N(double H, double e) { double N; - if(e > 1) { + if (e > 1) { N = e * sinh(H) - H; } else { N = NAN; @@ -283,19 +289,20 @@ double H2N(double H, double e) * Outputs: * Ecc = eccentric anomaly (rad) */ -double M2E(double M, double e) +double +M2E(double M, double e) { double small = 1e-13; double dE = 10 * small; double E1 = M; - int max = 200; - int count = 0; + int max = 200; + int count = 0; - if((e >= 0) && (e < 1)) { - while(fabs(dE) > small) { + if ((e >= 0) && (e < 1)) { + while (fabs(dE) > small) { dE = (E1 - e * sin(E1) - M) / (1 - e * cos(E1)); E1 -= dE; - if(++count > max) { + if (++count > max) { BSK_PRINT(MSG_ERROR, "iteration error in M2E(%f,%f)", M, e); dE = 0.; } @@ -317,23 +324,23 @@ double M2E(double M, double e) * Outputs: * H = hyperbolic anomaly (rad) */ -double N2H(double N, double e) +double +N2H(double N, double e) { double small = 1e-13; double dH = 10 * small; double H1 = N; - int max = 200; - int count = 0; - if(fabs(H1) > 7.0) - { - H1 = N/fabs(N)*7.0; + int max = 200; + int count = 0; + if (fabs(H1) > 7.0) { + H1 = N / fabs(N) * 7.0; } - if(e > 1) { - while(fabs(dH) > small) { + if (e > 1) { + while (fabs(dH) > small) { dH = (e * sinh(H1) - H1 - N) / (e * cosh(H1) - 1); H1 -= dH; - if(++count > max) { + if (++count > max) { BSK_PRINT(MSG_ERROR, "iteration error in N2H(%f,%f)", N, e); dH = 0.; } @@ -378,21 +385,22 @@ double N2H(double N, double e) * rVec = position vector * vVec = velocity vector */ -void elem2rv(double mu, ClassicElements *elements, double *rVec, double *vVec) +void +elem2rv(double mu, ClassicElements* elements, double* rVec, double* vVec) { - double e; /* eccentricty */ - double a; /* semi-major axis */ - double f; /* true anomaly */ - double r; /* orbit radius */ - double v; /* orbit velocity magnitude */ - double i; /* orbit inclination angle */ - double p; /* the parameter or the semi-latus rectum */ - double AP; /* argument of perigee */ - double AN; /* argument of the ascending node */ - double theta; /* true latitude theta = omega + f */ - double h; /* orbit angular momentum magnitude */ - double ir[3]; /* orbit radius unit vector */ - double eps; /* small numerical value parameter */ + double e; /* eccentricty */ + double a; /* semi-major axis */ + double f; /* true anomaly */ + double r; /* orbit radius */ + double v; /* orbit velocity magnitude */ + double i; /* orbit inclination angle */ + double p; /* the parameter or the semi-latus rectum */ + double AP; /* argument of perigee */ + double AN; /* argument of the ascending node */ + double theta; /* true latitude theta = omega + f */ + double h; /* orbit angular momentum magnitude */ + double ir[3]; /* orbit radius unit vector */ + double eps; /* small numerical value parameter */ /* define what is a small numerical value */ eps = 1e-12; @@ -405,10 +413,10 @@ void elem2rv(double mu, ClassicElements *elements, double *rVec, double *vVec) AP = elements->omega; f = elements->f; - if((fabs(e-1.0) < eps) && (fabs(a) > eps)) { /* 1D rectilinear elliptic/hyperbolic orbit case */ + if ((fabs(e - 1.0) < eps) && (fabs(a) > eps)) { /* 1D rectilinear elliptic/hyperbolic orbit case */ - double angle; /* eccentric/hyperbolic anomaly */ - angle = f; /* f is treated as ecc/hyp anomaly */ + double angle; /* eccentric/hyperbolic anomaly */ + angle = f; /* f is treated as ecc/hyp anomaly */ /* orbit radius */ if (elements->a > 0.0) { r = a * (1 - e * cos(angle)); @@ -420,36 +428,33 @@ void elem2rv(double mu, ClassicElements *elements, double *rVec, double *vVec) ir[1] = sin(AN) * cos(AP) + cos(AN) * sin(AP) * cos(i); ir[2] = sin(AP) * sin(i); v3Scale(r, ir, rVec); - if(sin(angle) > 0) { + if (sin(angle) > 0) { v3Scale(v, ir, vVec); } else { v3Scale(-v, ir, vVec); } } else { /* general 2D orbit case */ /* evaluate semi-latus rectum */ - if(fabs(a) > eps) { - p = a * (1 - e * e); /* elliptic or hyperbolic */ + if (fabs(a) > eps) { + p = a * (1 - e * e); /* elliptic or hyperbolic */ } else { - p = 2 * elements->rPeriap; /* parabolic */ + p = 2 * elements->rPeriap; /* parabolic */ } - r = p / (1 + e * cos(f)); /* orbit radius */ - theta = AP + f; /* true latitude angle */ - h = sqrt(mu * p); /* orbit ang. momentum mag. */ + r = p / (1 + e * cos(f)); /* orbit radius */ + theta = AP + f; /* true latitude angle */ + h = sqrt(mu * p); /* orbit ang. momentum mag. */ rVec[0] = r * (cos(AN) * cos(theta) - sin(AN) * sin(theta) * cos(i)); rVec[1] = r * (sin(AN) * cos(theta) + cos(AN) * sin(theta) * cos(i)); rVec[2] = r * (sin(theta) * sin(i)); - vVec[0] = -mu / h * (cos(AN) * (sin(theta) + e * sin(AP)) + sin(AN) * (cos( - theta) + e * cos(AP)) * cos(i)); - vVec[1] = -mu / h * (sin(AN) * (sin(theta) + e * sin(AP)) - cos(AN) * (cos( - theta) + e * cos(AP)) * cos(i)); + vVec[0] = -mu / h * (cos(AN) * (sin(theta) + e * sin(AP)) + sin(AN) * (cos(theta) + e * cos(AP)) * cos(i)); + vVec[1] = -mu / h * (sin(AN) * (sin(theta) + e * sin(AP)) - cos(AN) * (cos(theta) + e * cos(AP)) * cos(i)); vVec[2] = -mu / h * (-(cos(theta) + e * cos(AP)) * sin(i)); } } - /*! * Purpose: Translates the orbit elements inertial Cartesian position * vector rVec and velocity vector vVec into the corresponding @@ -473,23 +478,24 @@ void elem2rv(double mu, ClassicElements *elements, double *rVec, double *vVec) * Outputs: * elements = orbital elements */ -void rv2elem(double mu, double *rVec, double *vVec, ClassicElements *elements) +void +rv2elem(double mu, double* rVec, double* vVec, ClassicElements* elements) { - double hVec[3]; /* orbit angular momentum vector */ - double ihHat[3]; /* normalized orbit angular momentum vector */ - double h; /* orbit angular momentum magnitude */ - double v3[3]; /* temp vector */ - double n1Hat[3]; /* 1st inertial frame base vector */ - double n3Hat[3]; /* 3rd inertial frame base vector */ - double nVec[3]; /* line of nodes vector */ - double inHat[3]; /* normalized line of nodes vector */ - double irHat[3]; /* normalized position vector */ - double r; /* current orbit radius */ - double v; /* orbit velocity magnitude */ - double eVec[3]; /* eccentricity vector */ - double ieHat[3]; /* normalized eccentricity vector */ - double p; /* the parameter, also called semi-latus rectum */ - double eps; /* small numerical value parameter */ + double hVec[3]; /* orbit angular momentum vector */ + double ihHat[3]; /* normalized orbit angular momentum vector */ + double h; /* orbit angular momentum magnitude */ + double v3[3]; /* temp vector */ + double n1Hat[3]; /* 1st inertial frame base vector */ + double n3Hat[3]; /* 3rd inertial frame base vector */ + double nVec[3]; /* line of nodes vector */ + double inHat[3]; /* normalized line of nodes vector */ + double irHat[3]; /* normalized position vector */ + double r; /* current orbit radius */ + double v; /* orbit velocity magnitude */ + double eVec[3]; /* eccentricity vector */ + double ieHat[3]; /* normalized eccentricity vector */ + double p; /* the parameter, also called semi-latus rectum */ + double eps; /* small numerical value parameter */ /* define what is a small numerical value */ eps = 1e-12; @@ -508,7 +514,7 @@ void rv2elem(double mu, double *rVec, double *vVec, ClassicElements *elements) v3Cross(rVec, vVec, hVec); h = v3Norm(hVec); v3Normalize(hVec, ihHat); - p = h*h / mu; + p = h * h / mu; /* Calculate the line of nodes */ v3Cross(n3Hat, hVec, nVec); @@ -524,7 +530,7 @@ void rv2elem(double mu, double *rVec, double *vVec, ClassicElements *elements) v3Scale(v3Dot(rVec, vVec) / mu, vVec, v3); v3Subtract(eVec, v3, eVec); elements->e = v3Norm(eVec); - elements->rPeriap = p / (1.0 + elements->e); + elements->rPeriap = p / (1.0 + elements->e); /* Orbit eccentricity unit vector */ if (elements->e > eps) { @@ -535,8 +541,8 @@ void rv2elem(double mu, double *rVec, double *vVec, ClassicElements *elements) } /* compute semi-major axis */ - elements->alpha = 2.0 / r - v*v / mu; - if(fabs(elements->alpha) > eps) { + elements->alpha = 2.0 / r - v * v / mu; + if (fabs(elements->alpha) > eps) { /* elliptic or hyperbolic case */ elements->a = 1.0 / elements->alpha; elements->rApoap = p / (1.0 - elements->e); @@ -553,21 +559,21 @@ void rv2elem(double mu, double *rVec, double *vVec, ClassicElements *elements) v3Cross(n1Hat, inHat, v3); elements->Omega = atan2(v3[2], inHat[0]); if (elements->Omega < 0.0) { - elements->Omega += 2*M_PI; + elements->Omega += 2 * M_PI; } /* Calculate Argument of Periapses omega */ v3Cross(inHat, ieHat, v3); - elements->omega = atan2(v3Dot(ihHat,v3), v3Dot(inHat, ieHat)); + elements->omega = atan2(v3Dot(ihHat, v3), v3Dot(inHat, ieHat)); if (elements->omega < 0.0) { - elements->omega += 2*M_PI; + elements->omega += 2 * M_PI; } /* Calculate true anomaly angle f */ v3Cross(ieHat, irHat, v3); - elements->f = atan2(v3Dot(ihHat,v3), v3Dot(ieHat, irHat)); + elements->f = atan2(v3Dot(ihHat, v3), v3Dot(ieHat, irHat)); if (elements->f < 0.0) { - elements->f += 2*M_PI; + elements->f += 2 * M_PI; } } @@ -583,14 +589,15 @@ void rv2elem(double mu, double *rVec, double *vVec, ClassicElements *elements) * Outputs: * density = density at the given altitude in kg/m^3 */ -double atmosphericDensity(double alt) +double +atmosphericDensity(double alt) { double logdensity; double density; double val; /* Smooth exponential drop-off after 1000 km */ - if(alt > 1000.) { + if (alt > 1000.) { logdensity = (-7e-05) * alt - 14.464; density = pow(10., logdensity); return density; @@ -598,8 +605,8 @@ double atmosphericDensity(double alt) /* Calculating the density based on a scaled 6th order polynomial fit to the log of density */ val = (alt - 526.8000) / 292.8563; - logdensity = 0.34047 * pow(val, 6) - 0.5889 * pow(val, 5) - 0.5269 * pow(val, 4) - + 1.0036 * pow(val, 3) + 0.60713 * pow(val, 2) - 2.3024 * val - 12.575; + logdensity = 0.34047 * pow(val, 6) - 0.5889 * pow(val, 5) - 0.5269 * pow(val, 4) + 1.0036 * pow(val, 3) + + 0.60713 * pow(val, 2) - 2.3024 * val - 12.575; /* Calculating density by raising 10 to the log of density */ density = pow(10., logdensity); @@ -617,39 +624,37 @@ double atmosphericDensity(double alt) * Outputs: * debye = debye length given in m */ -double debyeLength(double alt) +double +debyeLength(double alt) { double debyedist; double a; - double X[N_DEBYE_PARAMETERS] = {200, 250, 300, 350, 400, 450, 500, 550, - 600, 650, 700, 750, 800, 850, 900, 950, 1000, 1050, 1100, 1150, - 1200, 1250, 1300, 1350, 1400, 1450, 1500, 1550, 1600, 1650, 1700, - 1750, 1800, 1850, 1900, 1950, 2000 - }; - double Y[N_DEBYE_PARAMETERS] = {5.64E-03, 3.92E-03, 3.24E-03, 3.59E-03, - 4.04E-03, 4.28E-03, 4.54E-03, 5.30E-03, 6.55E-03, 7.30E-03, 8.31E-03, - 8.38E-03, 8.45E-03, 9.84E-03, 1.22E-02, 1.37E-02, 1.59E-02, 1.75E-02, - 1.95E-02, 2.09E-02, 2.25E-02, 2.25E-02, 2.25E-02, 2.47E-02, 2.76E-02, - 2.76E-02, 2.76E-02, 2.76E-02, 2.76E-02, 2.76E-02, 2.76E-02, 3.21E-02, - 3.96E-02, 3.96E-02, 3.96E-02, 3.96E-02, 3.96E-02 - }; + double X[N_DEBYE_PARAMETERS] = { 200, 250, 300, 350, 400, 450, 500, 550, 600, 650, 700, 750, 800, + 850, 900, 950, 1000, 1050, 1100, 1150, 1200, 1250, 1300, 1350, 1400, 1450, + 1500, 1550, 1600, 1650, 1700, 1750, 1800, 1850, 1900, 1950, 2000 }; + double Y[N_DEBYE_PARAMETERS] = { 5.64E-03, 3.92E-03, 3.24E-03, 3.59E-03, 4.04E-03, 4.28E-03, 4.54E-03, 5.30E-03, + 6.55E-03, 7.30E-03, 8.31E-03, 8.38E-03, 8.45E-03, 9.84E-03, 1.22E-02, 1.37E-02, + 1.59E-02, 1.75E-02, 1.95E-02, 2.09E-02, 2.25E-02, 2.25E-02, 2.25E-02, 2.47E-02, + 2.76E-02, 2.76E-02, 2.76E-02, 2.76E-02, 2.76E-02, 2.76E-02, 2.76E-02, 3.21E-02, + 3.96E-02, 3.96E-02, 3.96E-02, 3.96E-02, 3.96E-02 }; int i; /* Flat debyeLength length for altitudes above 2000 km */ - if((alt > 2000.0) && (alt <= 30000.0)) { + if ((alt > 2000.0) && (alt <= 30000.0)) { alt = 2000.0; - } else if((alt > 30000.0) && (alt <= 35000.0)) { + } else if ((alt > 30000.0) && (alt <= 35000.0)) { debyedist = 0.1 * alt - 2999.7; return debyedist; - } else if((alt < 200.0) || (alt > 35000.0)) { - BSK_PRINT(MSG_ERROR, "debyeLength() received alt = %g\nThe value of alt should be in the range of [200 35000]", alt); + } else if ((alt < 200.0) || (alt > 35000.0)) { + BSK_PRINT( + MSG_ERROR, "debyeLength() received alt = %g\nThe value of alt should be in the range of [200 35000]", alt); debyedist = NAN; return debyedist; } /* Interpolation of data */ - for(i = 0; i < N_DEBYE_PARAMETERS - 1; i++) { - if(X[i + 1] > alt) { + for (i = 0; i < N_DEBYE_PARAMETERS - 1; i++) { + if (X[i + 1] > alt) { break; } } @@ -675,8 +680,8 @@ double debyeLength(double alt) * advec = The inertial acceleration vector due to atmospheric * drag in km/sec^2 */ -void atmosphericDrag(double Cd, double A, double m, double *rvec, double *vvec, - double *advec) +void +atmosphericDrag(double Cd, double A, double m, double* rvec, double* vvec, double* advec) { double r; double v; @@ -690,8 +695,13 @@ void atmosphericDrag(double Cd, double A, double m, double *rvec, double *vvec, alt = r - REQ_EARTH; /* Checking if user supplied a orbital position is inside the earth */ - if(alt <= 0.) { - BSK_PRINT(MSG_ERROR, "atmosphericDrag() received rvec = [%g %g %g]The value of rvec should produce a positive altitude for the Earth.", rvec[1], rvec[2], rvec[3]); + if (alt <= 0.) { + BSK_PRINT(MSG_ERROR, + "atmosphericDrag() received rvec = [%g %g %g]The value of rvec should produce a positive altitude " + "for the Earth.", + rvec[1], + rvec[2], + rvec[3]); v3Set(NAN, NAN, NAN, advec); return; } @@ -719,7 +729,8 @@ void atmosphericDrag(double Cd, double A, double m, double *rvec, double *vvec, * ajtot = The total acceleration vector due to the J * perturbations in km/sec^2 [accelx;accely;accelz] */ -void jPerturb(double *rvec, int num, double *ajtot, ...) +void +jPerturb(double* rvec, int num, double* ajtot, ...) { double mu; double req; @@ -734,16 +745,16 @@ void jPerturb(double *rvec, int num, double *ajtot, ...) CelestialObject_t planetID; va_list args; va_start(args, ajtot); - planetID = (CelestialObject_t) va_arg(args, int); + planetID = (CelestialObject_t)va_arg(args, int); va_end(args); v3SetZero(ajtot); - switch(planetID) { + switch (planetID) { case CELESTIAL_MERCURY: - mu = MU_MERCURY; + mu = MU_MERCURY; req = REQ_MERCURY; - J2 = J2_MERCURY; + J2 = J2_MERCURY; J3 = 0.0; J4 = 0.0; J5 = 0.0; @@ -751,9 +762,9 @@ void jPerturb(double *rvec, int num, double *ajtot, ...) break; case CELESTIAL_VENUS: - mu = MU_VENUS; + mu = MU_VENUS; req = REQ_VENUS; - J2 = J2_VENUS; + J2 = J2_VENUS; J3 = 0.0; J4 = 0.0; J5 = 0.0; @@ -761,9 +772,9 @@ void jPerturb(double *rvec, int num, double *ajtot, ...) break; case CELESTIAL_MOON: - mu = MU_MOON; + mu = MU_MOON; req = REQ_MOON; - J2 = J2_MOON; + J2 = J2_MOON; J3 = 0.0; J4 = 0.0; J5 = 0.0; @@ -771,9 +782,9 @@ void jPerturb(double *rvec, int num, double *ajtot, ...) break; case CELESTIAL_MARS: - mu = MU_MARS; + mu = MU_MARS; req = REQ_MARS; - J2 = J2_MARS; + J2 = J2_MARS; J3 = 0.0; J4 = 0.0; J5 = 0.0; @@ -781,9 +792,9 @@ void jPerturb(double *rvec, int num, double *ajtot, ...) break; case CELESTIAL_JUPITER: - mu = MU_JUPITER; + mu = MU_JUPITER; req = REQ_JUPITER; - J2 = J2_JUPITER; + J2 = J2_JUPITER; J3 = 0.0; J4 = 0.0; J5 = 0.0; @@ -791,9 +802,9 @@ void jPerturb(double *rvec, int num, double *ajtot, ...) break; case CELESTIAL_URANUS: - mu = MU_URANUS; + mu = MU_URANUS; req = REQ_URANUS; - J2 = J2_URANUS; + J2 = J2_URANUS; J3 = 0.0; J4 = 0.0; J5 = 0.0; @@ -801,9 +812,9 @@ void jPerturb(double *rvec, int num, double *ajtot, ...) break; case CELESTIAL_NEPTUNE: - mu = MU_NEPTUNE; + mu = MU_NEPTUNE; req = REQ_NEPTUNE; - J2 = J2_NEPTUNE; + J2 = J2_NEPTUNE; J3 = 0.0; J4 = 0.0; J5 = 0.0; @@ -815,17 +826,16 @@ void jPerturb(double *rvec, int num, double *ajtot, ...) return; default: - mu = MU_EARTH; + mu = MU_EARTH; req = REQ_EARTH; - J2 = J2_EARTH; - J3 = J3_EARTH; - J4 = J4_EARTH; - J5 = J5_EARTH; - J6 = J6_EARTH; + J2 = J2_EARTH; + J3 = J3_EARTH; + J4 = J4_EARTH; + J5 = J5_EARTH; + J6 = J6_EARTH; break; } - /* Calculate the J perturbations */ x = rvec[0]; y = rvec[1]; @@ -833,45 +843,50 @@ void jPerturb(double *rvec, int num, double *ajtot, ...) r = v3Norm(rvec); /* Error Checking */ - if((num < 2) || (num > 6)) { + if ((num < 2) || (num > 6)) { BSK_PRINT(MSG_ERROR, "jPerturb() received num = %d. The value of num should be 2 <= num <= 6.", num); v3Set(NAN, NAN, NAN, ajtot); return; } /* Calculating the total acceleration based on user input */ - if(num >= 2) { + if (num >= 2) { v3Set((1.0 - 5.0 * pow(z / r, 2.0)) * (x / r), (1.0 - 5.0 * pow(z / r, 2.0)) * (y / r), - (3.0 - 5.0 * pow(z / r, 2.0)) * (z / r), ajtot); - v3Scale(-3.0 / 2.0 * J2 * (mu / pow(r, 2.0))*pow(req / r, 2.0), ajtot, ajtot); + (3.0 - 5.0 * pow(z / r, 2.0)) * (z / r), + ajtot); + v3Scale(-3.0 / 2.0 * J2 * (mu / pow(r, 2.0)) * pow(req / r, 2.0), ajtot, ajtot); } - if(num >= 3) { + if (num >= 3) { v3Set(5.0 * (7.0 * pow(z / r, 3.0) - 3.0 * (z / r)) * (x / r), 5.0 * (7.0 * pow(z / r, 3.0) - 3.0 * (z / r)) * (y / r), - -3.0 * (10.0 * pow(z / r, 2.0) - (35.0 / 3.0)*pow(z / r, 4.0) - 1.0), temp); - v3Scale(1.0 / 2.0 * J3 * (mu / pow(r, 2.0))*pow(req / r, 3.0), temp, temp2); + -3.0 * (10.0 * pow(z / r, 2.0) - (35.0 / 3.0) * pow(z / r, 4.0) - 1.0), + temp); + v3Scale(1.0 / 2.0 * J3 * (mu / pow(r, 2.0)) * pow(req / r, 3.0), temp, temp2); v3Add(ajtot, temp2, ajtot); } - if(num >= 4) { + if (num >= 4) { v3Set((3.0 - 42.0 * pow(z / r, 2.0) + 63.0 * pow(z / r, 4.0)) * (x / r), (3.0 - 42.0 * pow(z / r, 2.0) + 63.0 * pow(z / r, 4.0)) * (y / r), - (15.0 - 70.0 * pow(z / r, 2) + 63.0 * pow(z / r, 4.0)) * (z / r), temp); - v3Scale(5.0 / 8.0 * J4 * (mu / pow(r, 2.0))*pow(req / r, 4.0), temp, temp2); + (15.0 - 70.0 * pow(z / r, 2) + 63.0 * pow(z / r, 4.0)) * (z / r), + temp); + v3Scale(5.0 / 8.0 * J4 * (mu / pow(r, 2.0)) * pow(req / r, 4.0), temp, temp2); v3Add(ajtot, temp2, ajtot); } - if(num >= 5) { + if (num >= 5) { v3Set(3.0 * (35.0 * (z / r) - 210.0 * pow(z / r, 3.0) + 231.0 * pow(z / r, 5.0)) * (x / r), 3.0 * (35.0 * (z / r) - 210.0 * pow(z / r, 3.0) + 231.0 * pow(z / r, 5.0)) * (y / r), - -(15.0 - 315.0 * pow(z / r, 2.0) + 945.0 * pow(z / r, 4.0) - 693.0 * pow(z / r, 6.0)), temp); - v3Scale(1.0 / 8.0 * J5 * (mu / pow(r, 2.0))*pow(req / r, 5.0), temp, temp2); + -(15.0 - 315.0 * pow(z / r, 2.0) + 945.0 * pow(z / r, 4.0) - 693.0 * pow(z / r, 6.0)), + temp); + v3Scale(1.0 / 8.0 * J5 * (mu / pow(r, 2.0)) * pow(req / r, 5.0), temp, temp2); v3Add(ajtot, temp2, ajtot); } - if(num >= 6) { + if (num >= 6) { v3Set((35.0 - 945.0 * pow(z / r, 2) + 3465.0 * pow(z / r, 4.0) - 3003.0 * pow(z / r, 6.0)) * (x / r), (35.0 - 945.0 * pow(z / r, 2.0) + 3465.0 * pow(z / r, 4.0) - 3003.0 * pow(z / r, 6.0)) * (y / r), - -(3003.0 * pow(z / r, 6.0) - 4851.0 * pow(z / r, 4.0) + 2205.0 * pow(z / r, 2.0) - 245.0) * (z / r), temp); - v3Scale(-1.0 / 16.0 * J6 * (mu / pow(r, 2.0))*pow(req / r, 6.0), temp, temp2); + -(3003.0 * pow(z / r, 6.0) - 4851.0 * pow(z / r, 4.0) + 2205.0 * pow(z / r, 2.0) - 245.0) * (z / r), + temp); + v3Scale(-1.0 / 16.0 * J6 * (mu / pow(r, 2.0)) * pow(req / r, 6.0), temp, temp2); v3Add(ajtot, temp2, ajtot); } } @@ -897,7 +912,8 @@ void jPerturb(double *rvec, int num, double *ajtot, ...) * components of the output are the same as the vector * components of the sunvec input vector. */ -void solarRad(double A, double m, double *sunvec, double *arvec) +void +solarRad(double A, double m, double* sunvec, double* arvec) { double flux; double c; @@ -905,23 +921,25 @@ void solarRad(double A, double m, double *sunvec, double *arvec) double sundist; /* Solar Radiation Flux */ - flux = 1372.5398; /* Watts/m^2 */ + flux = 1372.5398; /* Watts/m^2 */ /* Speed of light */ - c = 299792458.; /* m/s */ + c = 299792458.; /* m/s */ /* Radiation pressure coefficient */ Cr = 1.3; /* Magnitude of position vector */ - sundist = v3Norm(sunvec); /* AU */ + sundist = v3Norm(sunvec); /* AU */ /* Computing the acceleration vector */ v3Scale((-Cr * A * flux) / (m * c * pow(sundist, 3)) / 1000., sunvec, arvec); } /*! maps classical mean orbit elements to Osculating elements */ -void clMeanOscMap(double req, double J2, ClassicElements *elements, ClassicElements *elements_p, double sgn) { +void +clMeanOscMap(double req, double J2, ClassicElements* elements, ClassicElements* elements_p, double sgn) +{ // Classical orbital elements = (a,e,i,Omega,omega,f) // First-order J2 Mapping Between Mean and Osculating Orbital Elements // sgn=1:mean2osc, sgn=-1:osc2mean @@ -929,66 +947,93 @@ void clMeanOscMap(double req, double J2, ClassicElements *elements, ClassicEleme // Hanspeter Schaub, John L. Junkins, 4th edition. // [m] or [km] should be the same both for req and elements->a - double a = elements->a; - double e = elements->e; - double i = elements->i; + double a = elements->a; + double e = elements->e; + double i = elements->i; double Omega = elements->Omega; double omega = elements->omega; - double f = elements->f; - double E = f2E(f, e); - double M = E2M(E, e); + double f = elements->f; + double E = f2E(f, e); + double M = E2M(E, e); - double gamma2 = sgn * J2 / 2.0 * pow((req / a), 2.0); // (F.1),(F.2) + double gamma2 = sgn * J2 / 2.0 * pow((req / a), 2.0); // (F.1),(F.2) double eta = sqrt(1.0 - pow(e, 2.0)); - double gamma2p = gamma2 / pow(eta, 4.0); // (F.3) - double a_r = (1.0 + e * cos(f)) / pow(eta, 2.0); // (F.6) - - double ap = a + a * gamma2 * ((3.0 * pow(cos(i), 2.0) - 1.0) * (pow(a_r, 3.0) - 1.0 / pow(eta, 3.0)) - + 3.0 * (1.0 - pow(cos(i), 2.0)) * pow(a_r, 3.0) * cos(2.0 * omega + 2.0 * f)); // (F.7) - - double de1 = gamma2p / 8.0 * e * pow(eta, 2.0) * (1.0 - 11.0 * pow(cos(i), 2.0) - 40.0 * pow(cos(i), 4.0) - / (1.0 - 5.0 * pow(cos(i), 2.0))) * cos(2.0 * omega); // (F.8) - - double de = de1 + pow(eta, 2.0) / 2.0 * (gamma2 * ((3.0 * pow(cos(i), 2.0) - 1.0) / pow(eta, 6.0) * (e * eta + e / (1.0 + eta) + 3.0 * cos(f) - + 3.0 * e * pow(cos(f), 2.0) + pow(e, 2.0) * pow(cos(f), 3.0)) + 3.0 * (1.0 - pow(cos(i), 2.0)) / pow(eta, 6.0) * (e + 3.0 * cos(f) - + 3 * e * pow(cos(f), 2) + pow(e, 2) * pow(cos(f), 3)) * cos(2 * omega + 2 * f)) - - gamma2p * (1.0 - pow(cos(i), 2.0)) * (3.0 * cos(2.0 * omega + f) + cos(2.0 * omega + 3.0 * f))); // (F.9) - - double di = -e * de1 / pow(eta, 2.0) / tan(i) - + gamma2p / 2.0 * cos(i) * sqrt(1.0 - pow(cos(i), 2.0)) * (3.0 * cos(2.0 * omega + 2.0 * f) + 3.0 * e * cos(2.0 * omega + f) - + e * cos(2.0 * omega + 3.0 * f)); // (F.10) - - double MpopOp = M + omega + Omega + gamma2p / 8.0 * pow(eta, 3.0) * (1.0 - 11.0 * pow(cos(i), 2.0) - 40.0 * pow(cos(i), 4.0) - / (1.0 - 5.0 * pow(cos(i), 2.0))) * sin(2.0 * omega) - gamma2p / 16.0 * (2.0 + pow(e, 2.0) - 11.0 * (2.0 + 3.0 * pow(e, 2.0)) - * pow(cos(i), 2.0) - 40.0 * (2.0 + 5.0 * pow(e, 2.0)) * pow(cos(i), 4.0) / (1.0 - 5.0 * pow(cos(i), 2.0)) - 400.0 * pow(e, 2.0) - * pow(cos(i), 6.0) / pow(1.0 - 5.0 * pow(cos(i), 2.0), 2.0)) * sin(2.0 * omega) + gamma2p / 4.0 * (-6.0 * (1.0 - 5.0 * pow(cos(i), 2.0)) - * (f - M + e * sin(f)) + (3.0 - 5.0 * pow(cos(i), 2.0)) * (3.0 * sin(2.0 * omega + 2.0 * f) + 3.0 * e * sin(2.0 * omega + f) + e - * sin(2.0 * omega + 3.0 * f))) - gamma2p / 8.0 * pow(e, 2.0) * cos(i) * (11.0 + 80.0 * pow(cos(i), 2.0) / (1.0 - 5.0 * pow(cos(i), 2.0)) - + 200.0 * pow(cos(i), 4.0) / pow(1.0 - 5.0 * pow(cos(i), 2.0), 2.0)) * sin(2.0 * omega) - gamma2p / 2.0 * cos(i) - * (6.0 * (f - M + e * sin(f)) - 3.0 * sin(2.0 * omega + 2.0 * f) - 3.0 * e * sin(2.0 * omega + f) - e * sin(2.0 * omega + 3.0 * f)); // (F.11) - - double edM = gamma2p / 8.0 * e * pow(eta, 3.0) * (1.0 - 11.0 * pow(cos(i), 2.0) - - 40.0 * pow(cos(i), 4.0) / (1.0 - 5.0 * pow(cos(i), 2.0))) * sin(2.0 * omega) - - gamma2p / 4.0 * pow(eta, 3.0) * (2.0 * (3.0 * pow(cos(i), 2.0) - 1.0) * (pow(a_r * eta, 2.0) + a_r + 1.0) * sin(f) - + 3.0 * (1.0 - pow(cos(i), 2.0)) * ((-pow(a_r * eta, 2.0) - a_r + 1.0) * sin(2.0 * omega + f) + (pow(a_r * eta, 2.0) + a_r - + 1.0 / 3.0) * sin(2.0 * omega + 3.0 * f))); // (F.12) - - double dOmega = -gamma2p / 8.0 * pow(e, 2.0) * cos(i) * (11.0 + 80.0 * pow(cos(i), 2.0) / (1.0 - 5.0 * pow(cos(i), 2.0)) - + 200.0 * pow(cos(i), 4.0) / pow(1.0 - 5.0 * pow(cos(i), 2.0), 2.0)) * sin(2.0 * omega) - gamma2p / 2.0 * cos(i) - * (6.0 * (f - M + e * sin(f)) - 3.0 * sin(2.0 * omega + 2.0 * f) - 3.0 * e * sin(2.0 * omega + f) - e * sin(2.0 * omega + 3.0 * f)); // (F.13) - - double d1 = (e + de) * sin(M) + edM * cos(M); // (F.14) - double d2 = (e + de) * cos(M) - edM * sin(M); // (F.15) - - double Mp = atan2(d1, d2); // (F.16) - double ep = sqrt(d1 * d1 + d2 * d2); // (F.17) - - double d3 = (sin(i / 2.0) + cos(i / 2.0) * di / 2.0) * sin(Omega) + sin(i / 2.0) * dOmega * cos(Omega); // (F.18) - double d4 = (sin(i / 2.0) + cos(i / 2.0) * di / 2.0) * cos(Omega) - sin(i / 2.0) * dOmega * sin(Omega); // (F.19) - - double Omegap = atan2(d3, d4); // (F.20) - double ip = 2.0 * safeAsin(sqrt(d3 * d3 + d4 * d4)); // (F.21) - double omegap = MpopOp - Mp - Omegap; // (F.22) + double gamma2p = gamma2 / pow(eta, 4.0); // (F.3) + double a_r = (1.0 + e * cos(f)) / pow(eta, 2.0); // (F.6) + + double ap = a + a * gamma2 * + ((3.0 * pow(cos(i), 2.0) - 1.0) * (pow(a_r, 3.0) - 1.0 / pow(eta, 3.0)) + + 3.0 * (1.0 - pow(cos(i), 2.0)) * pow(a_r, 3.0) * cos(2.0 * omega + 2.0 * f)); // (F.7) + + double de1 = gamma2p / 8.0 * e * pow(eta, 2.0) * + (1.0 - 11.0 * pow(cos(i), 2.0) - 40.0 * pow(cos(i), 4.0) / (1.0 - 5.0 * pow(cos(i), 2.0))) * + cos(2.0 * omega); // (F.8) + + double de = + de1 + + pow(eta, 2.0) / 2.0 * + (gamma2 * + ((3.0 * pow(cos(i), 2.0) - 1.0) / pow(eta, 6.0) * + (e * eta + e / (1.0 + eta) + 3.0 * cos(f) + 3.0 * e * pow(cos(f), 2.0) + pow(e, 2.0) * pow(cos(f), 3.0)) + + 3.0 * (1.0 - pow(cos(i), 2.0)) / pow(eta, 6.0) * + (e + 3.0 * cos(f) + 3 * e * pow(cos(f), 2) + pow(e, 2) * pow(cos(f), 3)) * cos(2 * omega + 2 * f)) - + gamma2p * (1.0 - pow(cos(i), 2.0)) * (3.0 * cos(2.0 * omega + f) + cos(2.0 * omega + 3.0 * f))); // (F.9) + + double di = + -e * de1 / pow(eta, 2.0) / tan(i) + + gamma2p / 2.0 * cos(i) * sqrt(1.0 - pow(cos(i), 2.0)) * + (3.0 * cos(2.0 * omega + 2.0 * f) + 3.0 * e * cos(2.0 * omega + f) + e * cos(2.0 * omega + 3.0 * f)); // (F.10) + + double MpopOp = + M + omega + Omega + + gamma2p / 8.0 * pow(eta, 3.0) * + (1.0 - 11.0 * pow(cos(i), 2.0) - 40.0 * pow(cos(i), 4.0) / (1.0 - 5.0 * pow(cos(i), 2.0))) * sin(2.0 * omega) - + gamma2p / 16.0 * + (2.0 + pow(e, 2.0) - 11.0 * (2.0 + 3.0 * pow(e, 2.0)) * pow(cos(i), 2.0) - + 40.0 * (2.0 + 5.0 * pow(e, 2.0)) * pow(cos(i), 4.0) / (1.0 - 5.0 * pow(cos(i), 2.0)) - + 400.0 * pow(e, 2.0) * pow(cos(i), 6.0) / pow(1.0 - 5.0 * pow(cos(i), 2.0), 2.0)) * + sin(2.0 * omega) + + gamma2p / 4.0 * + (-6.0 * (1.0 - 5.0 * pow(cos(i), 2.0)) * (f - M + e * sin(f)) + + (3.0 - 5.0 * pow(cos(i), 2.0)) * + (3.0 * sin(2.0 * omega + 2.0 * f) + 3.0 * e * sin(2.0 * omega + f) + e * sin(2.0 * omega + 3.0 * f))) - + gamma2p / 8.0 * pow(e, 2.0) * cos(i) * + (11.0 + 80.0 * pow(cos(i), 2.0) / (1.0 - 5.0 * pow(cos(i), 2.0)) + + 200.0 * pow(cos(i), 4.0) / pow(1.0 - 5.0 * pow(cos(i), 2.0), 2.0)) * + sin(2.0 * omega) - + gamma2p / 2.0 * cos(i) * + (6.0 * (f - M + e * sin(f)) - 3.0 * sin(2.0 * omega + 2.0 * f) - 3.0 * e * sin(2.0 * omega + f) - + e * sin(2.0 * omega + 3.0 * f)); // (F.11) + + double edM = gamma2p / 8.0 * e * pow(eta, 3.0) * + (1.0 - 11.0 * pow(cos(i), 2.0) - 40.0 * pow(cos(i), 4.0) / (1.0 - 5.0 * pow(cos(i), 2.0))) * + sin(2.0 * omega) - + gamma2p / 4.0 * pow(eta, 3.0) * + (2.0 * (3.0 * pow(cos(i), 2.0) - 1.0) * (pow(a_r * eta, 2.0) + a_r + 1.0) * sin(f) + + 3.0 * (1.0 - pow(cos(i), 2.0)) * + ((-pow(a_r * eta, 2.0) - a_r + 1.0) * sin(2.0 * omega + f) + + (pow(a_r * eta, 2.0) + a_r + 1.0 / 3.0) * sin(2.0 * omega + 3.0 * f))); // (F.12) + + double dOmega = -gamma2p / 8.0 * pow(e, 2.0) * cos(i) * + (11.0 + 80.0 * pow(cos(i), 2.0) / (1.0 - 5.0 * pow(cos(i), 2.0)) + + 200.0 * pow(cos(i), 4.0) / pow(1.0 - 5.0 * pow(cos(i), 2.0), 2.0)) * + sin(2.0 * omega) - + gamma2p / 2.0 * cos(i) * + (6.0 * (f - M + e * sin(f)) - 3.0 * sin(2.0 * omega + 2.0 * f) - 3.0 * e * sin(2.0 * omega + f) - + e * sin(2.0 * omega + 3.0 * f)); // (F.13) + + double d1 = (e + de) * sin(M) + edM * cos(M); // (F.14) + double d2 = (e + de) * cos(M) - edM * sin(M); // (F.15) + + double Mp = atan2(d1, d2); // (F.16) + double ep = sqrt(d1 * d1 + d2 * d2); // (F.17) + + double d3 = (sin(i / 2.0) + cos(i / 2.0) * di / 2.0) * sin(Omega) + sin(i / 2.0) * dOmega * cos(Omega); // (F.18) + double d4 = (sin(i / 2.0) + cos(i / 2.0) * di / 2.0) * cos(Omega) - sin(i / 2.0) * dOmega * sin(Omega); // (F.19) + + double Omegap = atan2(d3, d4); // (F.20) + double ip = 2.0 * safeAsin(sqrt(d3 * d3 + d4 * d4)); // (F.21) + double omegap = MpopOp - Mp - Omegap; // (F.22) double Ep = M2E(Mp, ep); double fp = E2f(Ep, ep); @@ -1002,17 +1047,19 @@ void clMeanOscMap(double req, double J2, ClassicElements *elements, ClassicEleme } /*! maps from classical orbit elements to equinoctial elements */ -void clElem2eqElem(ClassicElements *elements_cl, equinoctialElements *elements_eq) { +void +clElem2eqElem(ClassicElements* elements_cl, equinoctialElements* elements_eq) +{ // conversion // from classical orbital elements (a,e,i,Omega,omega,f) // to equinoctial orbital elements (a,P1,P2,Q1,Q2,l,L) - elements_eq->a = elements_cl->a; + elements_eq->a = elements_cl->a; elements_eq->P1 = elements_cl->e * sin(elements_cl->Omega + elements_cl->omega); elements_eq->P2 = elements_cl->e * cos(elements_cl->Omega + elements_cl->omega); elements_eq->Q1 = tan(elements_cl->i / 2.0) * sin(elements_cl->Omega); elements_eq->Q2 = tan(elements_cl->i / 2.0) * cos(elements_cl->Omega); - double E = f2E(elements_cl->f, elements_cl->e); - double M = E2M(E, elements_cl->e); - elements_eq->l = elements_cl->Omega + elements_cl->omega + M; - elements_eq->L = elements_cl->Omega + elements_cl->omega + elements_cl->f; + double E = f2E(elements_cl->f, elements_cl->e); + double M = E2M(E, elements_cl->e); + elements_eq->l = elements_cl->Omega + elements_cl->omega + M; + elements_eq->L = elements_cl->Omega + elements_cl->omega + elements_cl->f; } diff --git a/src/architecture/utilities/orbitalMotion.h b/src/architecture/utilities/orbitalMotion.h index 5a7a7a4de6..614d1c2420 100644 --- a/src/architecture/utilities/orbitalMotion.h +++ b/src/architecture/utilities/orbitalMotion.h @@ -23,21 +23,23 @@ #define N_DEBYE_PARAMETERS 37 /*! @brief Structure used to define classic orbit elements */ -typedef struct { - double a; //!< object semi-major axis - double e; //!< Eccentricity of the orbit - double i; //!< inclination of the orbital plane - double Omega; //!< Right ascension of the ascending node - double omega; //!< Argument of periapsis of the orbit - double f; //!< True anomaly of the orbit - double rmag; //!< Magnitude of the position vector (extra) - double alpha; //!< Inverted semi-major axis (extra) - double rPeriap; //!< Radius of periapsis (extra) - double rApoap; //!< Radius if apoapsis (extra) +typedef struct +{ + double a; //!< object semi-major axis + double e; //!< Eccentricity of the orbit + double i; //!< inclination of the orbital plane + double Omega; //!< Right ascension of the ascending node + double omega; //!< Argument of periapsis of the orbit + double f; //!< True anomaly of the orbit + double rmag; //!< Magnitude of the position vector (extra) + double alpha; //!< Inverted semi-major axis (extra) + double rPeriap; //!< Radius of periapsis (extra) + double rApoap; //!< Radius if apoapsis (extra) } ClassicElements; /* Celestial object being orbited */ -typedef enum { +typedef enum +{ CELESTIAL_MERCURY, CELESTIAL_VENUS, CELESTIAL_EARTH, @@ -60,18 +62,20 @@ typedef enum { */ /*! equinoctial elment struct definition */ -typedef struct { - double a; //!< semi-major axis - double P1; //!< e*sin(omega+Omega) - double P2; //!< e*cos(omega+Omega) - double Q1; //!< tan(i/2)*sin(Omega) - double Q2; //!< tan(i/2)*cos(Omega) - double l; //!< Omega+omega+M - double L; //!< Omega+omega+f +typedef struct +{ + double a; //!< semi-major axis + double P1; //!< e*sin(omega+Omega) + double P2; //!< e*cos(omega+Omega) + double Q1; //!< tan(i/2)*sin(Omega) + double Q2; //!< tan(i/2)*cos(Omega) + double l; //!< Omega+omega+M + double L; //!< Omega+omega+f } equinoctialElements; #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif /* E = eccentric anomaly @@ -80,28 +84,28 @@ extern "C" { H = hyperbolic anomaly N = mean hyperbolic anomaly */ - double E2f(double E, double e); - double E2M(double E, double e); - double f2E(double f, double e); - double f2H(double f, double e); - double H2f(double H, double e); - double H2N(double H, double e); - double M2E(double M, double e); - double N2H(double N, double e); - void elem2rv(double mu, ClassicElements *elements, double *rVec, double *vVec); - void rv2elem(double mu, double *rVec, double *vVec, ClassicElements *elements); - void clMeanOscMap(double req, double J2, ClassicElements *elements, ClassicElements *elements_p, double sgn); - void clElem2eqElem(ClassicElements *elements_cl, equinoctialElements *elements_eq); + double E2f(double E, double e); + double E2M(double E, double e); + double f2E(double f, double e); + double f2H(double f, double e); + double H2f(double H, double e); + double H2N(double H, double e); + double M2E(double M, double e); + double N2H(double N, double e); + void elem2rv(double mu, ClassicElements* elements, double* rVec, double* vVec); + void rv2elem(double mu, double* rVec, double* vVec, ClassicElements* elements); + void clMeanOscMap(double req, double J2, ClassicElements* elements, ClassicElements* elements_p, double sgn); + void clElem2eqElem(ClassicElements* elements_cl, equinoctialElements* elements_eq); - void hillFrame(double *rc_N, double *vc_N, double HN[3][3]); - void hill2rv(double *rc_N, double *vc_N, double *rho_H, double *rhoPrime_H, double *rd_N, double *vd_N); - void rv2hill(double *rc_N, double *vc_N, double *rd_N, double *vd_N, double *rho_H, double *rhoPrime_H); + void hillFrame(double* rc_N, double* vc_N, double HN[3][3]); + void hill2rv(double* rc_N, double* vc_N, double* rho_H, double* rhoPrime_H, double* rd_N, double* vd_N); + void rv2hill(double* rc_N, double* vc_N, double* rd_N, double* vd_N, double* rho_H, double* rhoPrime_H); - double atmosphericDensity(double alt); - double debyeLength(double alt); - void atmosphericDrag(double Cd, double A, double m, double *rvec, double *vvec, double *advec); - void jPerturb(double *rvec, int num, double *ajtot, ...); - void solarRad(double A, double m, double *sunvec, double *arvec); + double atmosphericDensity(double alt); + double debyeLength(double alt); + void atmosphericDrag(double Cd, double A, double m, double* rvec, double* vvec, double* advec); + void jPerturb(double* rvec, int num, double* ajtot, ...); + void solarRad(double A, double m, double* sunvec, double* arvec); #ifdef __cplusplus } diff --git a/src/architecture/utilities/rigidBodyKinematics.c b/src/architecture/utilities/rigidBodyKinematics.c index 30678de13a..742ba9a680 100644 --- a/src/architecture/utilities/rigidBodyKinematics.c +++ b/src/architecture/utilities/rigidBodyKinematics.c @@ -31,7 +31,8 @@ * which corresponds to performing to successive * rotations B1 and B2. */ -void addEP(double *b1, double *b2, double *result) +void +addEP(double* b1, double* b2, double* result) { result[0] = b2[0] * b1[0] - b2[1] * b1[1] - b2[2] * b1[2] - b2[3] * b1[3]; result[1] = b2[1] * b1[0] + b2[0] * b1[1] + b2[3] * b1[2] - b2[2] * b1[3]; @@ -44,7 +45,8 @@ void addEP(double *b1, double *b2, double *result) * angle vector corresponding to two successive * (1-2-1) rotations E1 and E2. */ -void addEuler121(double *e1, double *e2, double *result) +void +addEuler121(double* e1, double* e2, double* result) { double cp1; double cp2; @@ -70,7 +72,8 @@ void addEuler121(double *e1, double *e2, double *result) * angle vector corresponding to two successive * (1-3-1) rotations E1 and E2. */ -void addEuler131(double *e1, double *e2, double *result) +void +addEuler131(double* e1, double* e2, double* result) { double cp1; double cp2; @@ -96,7 +99,8 @@ void addEuler131(double *e1, double *e2, double *result) * angle vector corresponding to two successive * (1-2-3) rotations E1 and E2. */ -void addEuler123(double *e1, double *e2, double *result) +void +addEuler123(double* e1, double* e2, double* result) { double C1[3][3]; double C2[3][3]; @@ -113,7 +117,8 @@ void addEuler123(double *e1, double *e2, double *result) * angle vector corresponding to two successive * (1-3-2) rotations E1 and E2. */ -void addEuler132(double *e1, double *e2, double *result) +void +addEuler132(double* e1, double* e2, double* result) { double C1[3][3]; double C2[3][3]; @@ -130,7 +135,8 @@ void addEuler132(double *e1, double *e2, double *result) * angle vector corresponding to two successive * (2-1-2) rotations E1 and E2. */ -void addEuler212(double *e1, double *e2, double *result) +void +addEuler212(double* e1, double* e2, double* result) { double cp1; double cp2; @@ -156,7 +162,8 @@ void addEuler212(double *e1, double *e2, double *result) * angle vector corresponding to two successive * (2-1-3) rotations E1 and E2. */ -void addEuler213(double *e1, double *e2, double *result) +void +addEuler213(double* e1, double* e2, double* result) { double C1[3][3]; double C2[3][3]; @@ -173,7 +180,8 @@ void addEuler213(double *e1, double *e2, double *result) * angle vector corresponding to two successive * (2-3-1) rotations E1 and E2. */ -void addEuler231(double *e1, double *e2, double *result) +void +addEuler231(double* e1, double* e2, double* result) { double C1[3][3]; double C2[3][3]; @@ -190,7 +198,8 @@ void addEuler231(double *e1, double *e2, double *result) * angle vector corresponding to two successive * (2-3-2) rotations E1 and E2. */ -void addEuler232(double *e1, double *e2, double *result) +void +addEuler232(double* e1, double* e2, double* result) { double cp1; double cp2; @@ -216,7 +225,8 @@ void addEuler232(double *e1, double *e2, double *result) * angle vector corresponding to two successive * (3-1-2) rotations E1 and E2. */ -void addEuler312(double *e1, double *e2, double *result) +void +addEuler312(double* e1, double* e2, double* result) { double C1[3][3]; double C2[3][3]; @@ -233,7 +243,8 @@ void addEuler312(double *e1, double *e2, double *result) * angle vector corresponding to two successive * (3-1-3) rotations E1 and E2. */ -void addEuler313(double *e1, double *e2, double *result) +void +addEuler313(double* e1, double* e2, double* result) { double cp1; double cp2; @@ -259,7 +270,8 @@ void addEuler313(double *e1, double *e2, double *result) * angle vector corresponding to two successive * (3-2-1) rotations E1 and E2. */ -void addEuler321(double *e1, double *e2, double *result) +void +addEuler321(double* e1, double* e2, double* result) { double C1[3][3]; double C2[3][3]; @@ -276,7 +288,8 @@ void addEuler321(double *e1, double *e2, double *result) * angle vector corresponding to two successive * (3-2-3) rotations E1 and E2. */ -void addEuler323(double *e1, double *e2, double *result) +void +addEuler323(double* e1, double* e2, double* result) { double cp1; double cp2; @@ -302,7 +315,8 @@ void addEuler323(double *e1, double *e2, double *result) * which corresponds to performing to successive * rotations Q1 and Q2. */ -void addGibbs(double *q1, double *q2, double *result) +void +addGibbs(double* q1, double* q2, double* result) { double v1[3]; double v2[3]; @@ -318,7 +332,8 @@ void addGibbs(double *q1, double *q2, double *result) * which corresponds to performing to successive * rotations Q1 and Q2. */ -void addMRP(double *q1, double *q2, double *result) +void +addMRP(double* q1, double* q2, double* result) { double v1[3]; double v2[3]; @@ -327,12 +342,12 @@ void addMRP(double *q1, double *q2, double *result) double mag; v3Copy(q1, s1); - det = (1 + v3Dot(s1, s1)*v3Dot(q2, q2) - 2 * v3Dot(s1, q2)); + det = (1 + v3Dot(s1, s1) * v3Dot(q2, q2) - 2 * v3Dot(s1, q2)); if (fabs(det) < 0.1) { mag = v3Dot(s1, s1); - v3Scale(-1./mag, s1, s1); - det = (1 + v3Dot(s1, s1)*v3Dot(q2, q2) - 2 * v3Dot(s1, q2)); + v3Scale(-1. / mag, s1, s1); + det = (1 + v3Dot(s1, s1) * v3Dot(q2, q2) - 2 * v3Dot(s1, q2)); } v3Cross(s1, q2, v1); @@ -345,8 +360,8 @@ void addMRP(double *q1, double *q2, double *result) /* map MRP to inner set */ mag = v3Dot(result, result); - if (mag > 1.0){ - v3Scale(-1./mag, result, result); + if (mag > 1.0) { + v3Scale(-1. / mag, result, result); } } @@ -355,7 +370,8 @@ void addMRP(double *q1, double *q2, double *result) * which corresponds to performing to successive * prinicipal rotations Q1 and Q2. */ -void addPRV(double *qq1, double *qq2, double *result) +void +addPRV(double* qq1, double* qq2, double* result) { double cp1; double cp2; @@ -371,8 +387,7 @@ void addPRV(double *qq1, double *qq2, double *result) v3Add(qq1, qq2, compSum); - if((v3Norm(qq1) < 1.0E-7 || v3Norm(qq2) < 1.0E-7)) - { + if ((v3Norm(qq1) < 1.0E-7 || v3Norm(qq2) < 1.0E-7)) { v3Add(qq1, qq2, result); return; } @@ -387,8 +402,7 @@ void addPRV(double *qq1, double *qq2, double *result) v3Set(q2[1], q2[2], q2[3], e2); p = 2 * safeAcos(cp1 * cp2 - sp1 * sp2 * v3Dot(e1, e2)); - if(fabs(p) < 1.0E-13) - { + if (fabs(p) < 1.0E-13) { v3SetZero(result); return; } @@ -408,7 +422,8 @@ void addPRV(double *qq1, double *qq2, double *result) * body angular velocity vector w. * w = 2 [B(Q)]^(-1) dQ/dt */ -void BinvEP(double *q, double B[3][4]) +void +BinvEP(double* q, double B[3][4]) { B[0][0] = -q[1]; B[0][1] = q[0]; @@ -431,7 +446,8 @@ void BinvEP(double *q, double B[3][4]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler121(double *q, double B[3][3]) +void +BinvEuler121(double* q, double B[3][3]) { double s2; double c2; @@ -461,7 +477,8 @@ void BinvEuler121(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler123(double *q, double B[3][3]) +void +BinvEuler123(double* q, double B[3][3]) { double s2; double c2; @@ -491,7 +508,8 @@ void BinvEuler123(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler131(double *q, double B[3][3]) +void +BinvEuler131(double* q, double B[3][3]) { double s2; double c2; @@ -521,7 +539,8 @@ void BinvEuler131(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler132(double *q, double B[3][3]) +void +BinvEuler132(double* q, double B[3][3]) { double s2; double c2; @@ -551,7 +570,8 @@ void BinvEuler132(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler212(double *q, double B[3][3]) +void +BinvEuler212(double* q, double B[3][3]) { double s2; double c2; @@ -581,7 +601,8 @@ void BinvEuler212(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler213(double *q, double B[3][3]) +void +BinvEuler213(double* q, double B[3][3]) { double s2; double c2; @@ -611,7 +632,8 @@ void BinvEuler213(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler231(double *q, double B[3][3]) +void +BinvEuler231(double* q, double B[3][3]) { double s2; double c2; @@ -641,7 +663,8 @@ void BinvEuler231(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler232(double *q, double B[3][3]) +void +BinvEuler232(double* q, double B[3][3]) { double s2; double c2; @@ -671,7 +694,8 @@ void BinvEuler232(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler323(double *q, double B[3][3]) +void +BinvEuler323(double* q, double B[3][3]) { double s2; double c2; @@ -701,7 +725,8 @@ void BinvEuler323(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler313(double *q, double B[3][3]) +void +BinvEuler313(double* q, double B[3][3]) { double s2; double c2; @@ -731,7 +756,8 @@ void BinvEuler313(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler321(double *q, double B[3][3]) +void +BinvEuler321(double* q, double B[3][3]) { double s2; double c2; @@ -761,7 +787,8 @@ void BinvEuler321(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvEuler312(double *q, double B[3][3]) +void +BinvEuler312(double* q, double B[3][3]) { double s2; double c2; @@ -791,7 +818,8 @@ void BinvEuler312(double *q, double B[3][3]) * * w = 2 [B(Q)]^(-1) dQ/dt */ -void BinvGibbs(double *q, double B[3][3]) +void +BinvGibbs(double* q, double B[3][3]) { B[0][0] = 1; B[0][1] = q[2]; @@ -806,13 +834,14 @@ void BinvGibbs(double *q, double B[3][3]) } /* -* BinvMRP(Q,B) returns the 3x3 matrix which relates -* the derivative of MRP vector Q to the -* body angular velocity vector w. -* -* w = 4 [B(Q)]^(-1) dQ/dt -*/ -void BinvMRP(double *q, double B[3][3]) + * BinvMRP(Q,B) returns the 3x3 matrix which relates + * the derivative of MRP vector Q to the + * body angular velocity vector w. + * + * w = 4 [B(Q)]^(-1) dQ/dt + */ +void +BinvMRP(double* q, double B[3][3]) { double s2; @@ -836,7 +865,8 @@ void BinvMRP(double *q, double B[3][3]) * * w = [B(Q)]^(-1) dQ/dt */ -void BinvPRV(double *q, double B[3][3]) +void +BinvPRV(double* q, double B[3][3]) { double p; double c1; @@ -864,7 +894,8 @@ void BinvPRV(double *q, double B[3][3]) * * dQ/dt = 1/2 [B(Q)] w */ -void BmatEP(double *q, double B[4][3]) +void +BmatEP(double* q, double B[4][3]) { B[0][0] = -q[1]; B[0][1] = -q[2]; @@ -887,7 +918,8 @@ void BmatEP(double *q, double B[4][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler121(double *q, double B[3][3]) +void +BmatEuler121(double* q, double B[3][3]) { double s2; double c2; @@ -918,7 +950,8 @@ void BmatEuler121(double *q, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler131(double *q, double B[3][3]) +void +BmatEuler131(double* q, double B[3][3]) { double s2; double c2; @@ -949,7 +982,8 @@ void BmatEuler131(double *q, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler123(double *q, double B[3][3]) +void +BmatEuler123(double* q, double B[3][3]) { double s2; double c2; @@ -980,7 +1014,8 @@ void BmatEuler123(double *q, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler132(double *q, double B[3][3]) +void +BmatEuler132(double* q, double B[3][3]) { double s2; double c2; @@ -1011,7 +1046,8 @@ void BmatEuler132(double *q, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler212(double *q, double B[3][3]) +void +BmatEuler212(double* q, double B[3][3]) { double s2; double c2; @@ -1042,7 +1078,8 @@ void BmatEuler212(double *q, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler213(double *q, double B[3][3]) +void +BmatEuler213(double* q, double B[3][3]) { double s2; double c2; @@ -1073,7 +1110,8 @@ void BmatEuler213(double *q, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler231(double *q, double B[3][3]) +void +BmatEuler231(double* q, double B[3][3]) { double s2; double c2; @@ -1104,7 +1142,8 @@ void BmatEuler231(double *q, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler232(double *q, double B[3][3]) +void +BmatEuler232(double* q, double B[3][3]) { double s2; double c2; @@ -1135,7 +1174,8 @@ void BmatEuler232(double *q, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler312(double *q, double B[3][3]) +void +BmatEuler312(double* q, double B[3][3]) { double s2; double c2; @@ -1166,7 +1206,8 @@ void BmatEuler312(double *q, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler313(double *q, double B[3][3]) +void +BmatEuler313(double* q, double B[3][3]) { double s2; double c2; @@ -1197,7 +1238,8 @@ void BmatEuler313(double *q, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler321(double *q, double B[3][3]) +void +BmatEuler321(double* q, double B[3][3]) { double s2; double c2; @@ -1228,7 +1270,8 @@ void BmatEuler321(double *q, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatEuler323(double *q, double B[3][3]) +void +BmatEuler323(double* q, double B[3][3]) { double s2; double c2; @@ -1259,7 +1302,8 @@ void BmatEuler323(double *q, double B[3][3]) * * dQ/dt = 1/2 [B(Q)] w */ -void BmatGibbs(double *q, double B[3][3]) +void +BmatGibbs(double* q, double B[3][3]) { B[0][0] = 1 + q[0] * q[0]; B[0][1] = q[0] * q[1] - q[2]; @@ -1279,7 +1323,8 @@ void BmatGibbs(double *q, double B[3][3]) * * dQ/dt = 1/4 [B(Q)] w */ -void BmatMRP(double *q, double B[3][3]) +void +BmatMRP(double* q, double B[3][3]) { double s2; @@ -1303,20 +1348,21 @@ void BmatMRP(double *q, double B[3][3]) * * (d^2Q)/(dt^2) = 1/4 ( [B(Q)] dw + [Bdot(Q,dQ)] w ) */ -void BdotmatMRP(double *q, double *dq, double B[3][3]) +void +BdotmatMRP(double* q, double* dq, double B[3][3]) { double s; s = -2 * v3Dot(q, dq); - B[0][0] = s + 4 * ( q[0] * dq[0] ); - B[0][1] = 2 * (-dq[2] + q[0] * dq[1] + dq[0] * q[1] ); - B[0][2] = 2 * ( dq[1] + q[0] * dq[2] + dq[0] * q[2] ); - B[1][0] = 2 * ( dq[2] + q[0] * dq[1] + dq[0] * q[1] ); - B[1][1] = s + 4 * ( q[1] * dq[1] ); - B[1][2] = 2 * (-dq[0] + q[1] * dq[2] + dq[1] * q[2] ); - B[2][0] = 2 * (-dq[1] + q[0] * dq[2] + dq[0] * q[2] ); - B[2][1] = 2 * ( dq[0] + q[1] * dq[2] + dq[1] * q[2] ); - B[2][2] = s + 4 * ( q[2] * dq[2] ); + B[0][0] = s + 4 * (q[0] * dq[0]); + B[0][1] = 2 * (-dq[2] + q[0] * dq[1] + dq[0] * q[1]); + B[0][2] = 2 * (dq[1] + q[0] * dq[2] + dq[0] * q[2]); + B[1][0] = 2 * (dq[2] + q[0] * dq[1] + dq[0] * q[1]); + B[1][1] = s + 4 * (q[1] * dq[1]); + B[1][2] = 2 * (-dq[0] + q[1] * dq[2] + dq[1] * q[2]); + B[2][0] = 2 * (-dq[1] + q[0] * dq[2] + dq[0] * q[2]); + B[2][1] = 2 * (dq[0] + q[1] * dq[2] + dq[1] * q[2]); + B[2][2] = s + 4 * (q[2] * dq[2]); } /* @@ -1326,7 +1372,8 @@ void BdotmatMRP(double *q, double *dq, double B[3][3]) * * dQ/dt = [B(Q)] w */ -void BmatPRV(double *q, double B[3][3]) +void +BmatPRV(double* q, double B[3][3]) { double p; double c; @@ -1351,7 +1398,8 @@ void BmatPRV(double *q, double B[3][3]) * using the Stanley method. * */ -void C2EP(double C[3][3], double b[4]) +void +C2EP(double C[3][3], double b[4]) { double tr; double b2[4]; @@ -1367,14 +1415,14 @@ void C2EP(double C[3][3], double b[4]) i = 0; max = b2[0]; - for(j = 1; j < 4; j++) { - if(b2[j] > max) { + for (j = 1; j < 4; j++) { + if (b2[j] > max) { i = j; max = b2[j]; } } - switch(i) { + switch (i) { case 0: b[0] = sqrt(b2[0]); b[1] = (C[1][2] - C[2][1]) / 4 / b[0]; @@ -1384,7 +1432,7 @@ void C2EP(double C[3][3], double b[4]) case 1: b[1] = sqrt(b2[1]); b[0] = (C[1][2] - C[2][1]) / 4 / b[1]; - if(b[0] < 0) { + if (b[0] < 0) { b[1] = -b[1]; b[0] = -b[0]; } @@ -1394,7 +1442,7 @@ void C2EP(double C[3][3], double b[4]) case 2: b[2] = sqrt(b2[2]); b[0] = (C[2][0] - C[0][2]) / 4 / b[2]; - if(b[0] < 0) { + if (b[0] < 0) { b[2] = -b[2]; b[0] = -b[0]; } @@ -1404,7 +1452,7 @@ void C2EP(double C[3][3], double b[4]) case 3: b[3] = sqrt(b2[3]); b[0] = (C[0][1] - C[1][0]) / 4 / b[3]; - if(b[0] < 0) { + if (b[0] < 0) { b[3] = -b[3]; b[0] = -b[0]; } @@ -1418,7 +1466,8 @@ void C2EP(double C[3][3], double b[4]) * C2Euler121(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (1-2-1) Euler angle set. */ -void C2Euler121(double C[3][3], double *q) +void +C2Euler121(double C[3][3], double* q) { q[0] = atan2(C[0][1], -C[0][2]); q[1] = safeAcos(C[0][0]); @@ -1429,7 +1478,8 @@ void C2Euler121(double C[3][3], double *q) * C2Euler123(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (1-2-3) Euler angle set. */ -void C2Euler123(double C[3][3], double *q) +void +C2Euler123(double C[3][3], double* q) { q[0] = atan2(-C[2][1], C[2][2]); q[1] = safeAsin(C[2][0]); @@ -1440,7 +1490,8 @@ void C2Euler123(double C[3][3], double *q) * C2Euler131(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (1-3-1) Euler angle set. */ -void C2Euler131(double C[3][3], double *q) +void +C2Euler131(double C[3][3], double* q) { q[0] = atan2(C[0][2], C[0][1]); q[1] = safeAcos(C[0][0]); @@ -1451,7 +1502,8 @@ void C2Euler131(double C[3][3], double *q) * C2Euler132(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (1-3-2) Euler angle set. */ -void C2Euler132(double C[3][3], double *q) +void +C2Euler132(double C[3][3], double* q) { q[0] = atan2(C[1][2], C[1][1]); q[1] = safeAsin(-C[1][0]); @@ -1462,7 +1514,8 @@ void C2Euler132(double C[3][3], double *q) * C2Euler212(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (2-1-2) Euler angle set. */ -void C2Euler212(double C[3][3], double *q) +void +C2Euler212(double C[3][3], double* q) { q[0] = atan2(C[1][0], C[1][2]); q[1] = safeAcos(C[1][1]); @@ -1473,7 +1526,8 @@ void C2Euler212(double C[3][3], double *q) * C2Euler213(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (2-1-3) Euler angle set. */ -void C2Euler213(double C[3][3], double *q) +void +C2Euler213(double C[3][3], double* q) { q[0] = atan2(C[2][0], C[2][2]); q[1] = safeAsin(-C[2][1]); @@ -1484,7 +1538,8 @@ void C2Euler213(double C[3][3], double *q) * C2Euler231(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (2-3-1) Euler angle set. */ -void C2Euler231(double C[3][3], double *q) +void +C2Euler231(double C[3][3], double* q) { q[0] = atan2(-C[0][2], C[0][0]); q[1] = safeAsin(C[0][1]); @@ -1495,7 +1550,8 @@ void C2Euler231(double C[3][3], double *q) * C2Euler232(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (2-3-2) Euler angle set. */ -void C2Euler232(double C[3][3], double *q) +void +C2Euler232(double C[3][3], double* q) { q[0] = atan2(C[1][2], -C[1][0]); q[1] = safeAcos(C[1][1]); @@ -1506,7 +1562,8 @@ void C2Euler232(double C[3][3], double *q) * C2Euler312(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (3-1-2) Euler angle set. */ -void C2Euler312(double C[3][3], double *q) +void +C2Euler312(double C[3][3], double* q) { q[0] = atan2(-C[1][0], C[1][1]); q[1] = safeAsin(C[1][2]); @@ -1517,7 +1574,8 @@ void C2Euler312(double C[3][3], double *q) * C2Euler313(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (3-1-3) Euler angle set. */ -void C2Euler313(double C[3][3], double *q) +void +C2Euler313(double C[3][3], double* q) { q[0] = atan2(C[2][0], -C[2][1]); q[1] = safeAcos(C[2][2]); @@ -1528,7 +1586,8 @@ void C2Euler313(double C[3][3], double *q) * C2Euler321(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (3-2-1) Euler angle set. */ -void C2Euler321(double C[3][3], double *q) +void +C2Euler321(double C[3][3], double* q) { q[0] = atan2(C[0][1], C[0][0]); q[1] = safeAsin(-C[0][2]); @@ -1539,7 +1598,8 @@ void C2Euler321(double C[3][3], double *q) * C2Euler323(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding (3-2-3) Euler angle set. */ -void C2Euler323(double C[3][3], double *q) +void +C2Euler323(double C[3][3], double* q) { q[0] = atan2(C[2][1], C[2][0]); q[1] = safeAcos(C[2][2]); @@ -1550,7 +1610,8 @@ void C2Euler323(double C[3][3], double *q) * C2Gibbs(C,Q) translates the 3x3 direction cosine matrix * C into the corresponding 3x1 Gibbs vector Q. */ -void C2Gibbs(double C[3][3], double *q) +void +C2Gibbs(double C[3][3], double* q) { double b[4]; @@ -1566,7 +1627,8 @@ void C2Gibbs(double C[3][3], double *q) * C into the corresponding 3x1 MRP vector Q where the * MRP vector is chosen such that |Q| <= 1. */ -void C2MRP(double C[3][3], double *q) +void +C2MRP(double C[3][3], double* q) { double b[4]; @@ -1585,12 +1647,13 @@ void C2MRP(double C[3][3], double *q) * where the first component of Q is the principal rotation angle * phi (0<= phi <= Pi) */ -void C2PRV(double C[3][3], double *q) +void +C2PRV(double C[3][3], double* q) { double beta[4]; - C2EP(C,beta); - EP2PRV(beta,q); + C2EP(C, beta); + EP2PRV(beta, q); } /* @@ -1600,7 +1663,8 @@ void C2PRV(double C[3][3], double *q) * * dQ/dt = 1/2 [B(Q)] w */ -void dEP(double *q, double *w, double *dq) +void +dEP(double* q, double* w, double* dq) { double B[4][3]; int i; @@ -1608,9 +1672,9 @@ void dEP(double *q, double *w, double *dq) BmatEP(q, B); m33MultV3(B, w, dq); - for(i = 0; i < 4; i++) { + for (i = 0; i < 4; i++) { dq[i] = 0.; - for(j = 0; j < 3; j++) { + for (j = 0; j < 3; j++) { dq[i] += B[i][j] * w[j]; } } @@ -1625,7 +1689,8 @@ void dEP(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler121(double *q, double *w, double *dq) +void +dEuler121(double* q, double* w, double* dq) { double B[3][3]; @@ -1640,7 +1705,8 @@ void dEuler121(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler123(double *q, double *w, double *dq) +void +dEuler123(double* q, double* w, double* dq) { double B[3][3]; @@ -1655,7 +1721,8 @@ void dEuler123(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler131(double *q, double *w, double *dq) +void +dEuler131(double* q, double* w, double* dq) { double B[3][3]; @@ -1670,7 +1737,8 @@ void dEuler131(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler132(double *q, double *w, double *dq) +void +dEuler132(double* q, double* w, double* dq) { double B[3][3]; @@ -1685,7 +1753,8 @@ void dEuler132(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler212(double *q, double *w, double *dq) +void +dEuler212(double* q, double* w, double* dq) { double B[3][3]; @@ -1700,7 +1769,8 @@ void dEuler212(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler213(double *q, double *w, double *dq) +void +dEuler213(double* q, double* w, double* dq) { double B[3][3]; @@ -1715,7 +1785,8 @@ void dEuler213(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler231(double *q, double *w, double *dq) +void +dEuler231(double* q, double* w, double* dq) { double B[3][3]; @@ -1730,7 +1801,8 @@ void dEuler231(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler232(double *q, double *w, double *dq) +void +dEuler232(double* q, double* w, double* dq) { double B[3][3]; @@ -1745,7 +1817,8 @@ void dEuler232(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler312(double *q, double *w, double *dq) +void +dEuler312(double* q, double* w, double* dq) { double B[3][3]; @@ -1760,7 +1833,8 @@ void dEuler312(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler313(double *q, double *w, double *dq) +void +dEuler313(double* q, double* w, double* dq) { double B[3][3]; @@ -1775,7 +1849,8 @@ void dEuler313(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler321(double *q, double *w, double *dq) +void +dEuler321(double* q, double* w, double* dq) { double B[3][3]; @@ -1790,7 +1865,8 @@ void dEuler321(double *q, double *w, double *dq) * * dQ/dt = [B(Q)] w */ -void dEuler323(double *q, double *w, double *dq) +void +dEuler323(double* q, double* w, double* dq) { double B[3][3]; @@ -1805,7 +1881,8 @@ void dEuler323(double *q, double *w, double *dq) * * dQ/dt = 1/2 [B(Q)] w */ -void dGibbs(double *q, double *w, double *dq) +void +dGibbs(double* q, double* w, double* dq) { double B[3][3]; @@ -1821,7 +1898,8 @@ void dGibbs(double *q, double *w, double *dq) * * dQ/dt = 1/4 [B(Q)] w */ -void dMRP(double *q, double *w, double *dq) +void +dMRP(double* q, double* w, double* dq) { double B[3][3]; @@ -1837,7 +1915,8 @@ void dMRP(double *q, double *w, double *dq) * * w = 4 [B(Q)]^(-1) dQ/dt */ -void dMRP2Omega(double *q, double *dq, double *w) +void +dMRP2Omega(double* q, double* dq, double* w) { double B[3][3]; @@ -1853,7 +1932,8 @@ void dMRP2Omega(double *q, double *dq, double *w) * * (d^2Q)/(dt^2) = 1/4 ( [B(Q)] dw + [Bdot(Q,dQ)] w ) */ -void ddMRP(double *q, double *dq, double *w, double *dw, double *ddq) +void +ddMRP(double* q, double* dq, double* w, double* dw, double* ddq) { double B[3][3], Bdot[3][3]; double s1[3], s2[3]; @@ -1863,8 +1943,8 @@ void ddMRP(double *q, double *dq, double *w, double *dw, double *ddq) BdotmatMRP(q, dq, Bdot); m33MultV3(B, dw, s1); m33MultV3(Bdot, w, s2); - for(i = 0; i < 3; i++) { - ddq[i] = 0.25 * ( s1[i] + s2[i] ); + for (i = 0; i < 3; i++) { + ddq[i] = 0.25 * (s1[i] + s2[i]); } } @@ -1875,7 +1955,8 @@ void ddMRP(double *q, double *dq, double *w, double *dw, double *ddq) * * dW/dt = 4 [B(Q)]^(-1) ( ddQ - [Bdot(Q,dQ)] [B(Q)]^(-1) dQ ) */ -void ddMRP2dOmega(double *q, double *dq, double *ddq, double *dw) +void +ddMRP2dOmega(double* q, double* dq, double* ddq, double* dw) { double B[3][3], Bdot[3][3]; double s1[3], s2[3], s3[3]; @@ -1885,7 +1966,7 @@ void ddMRP2dOmega(double *q, double *dq, double *ddq, double *dw) BdotmatMRP(q, dq, Bdot); m33MultV3(B, dq, s1); m33MultV3(Bdot, s1, s2); - for(i = 0; i < 3; i++) { + for (i = 0; i < 3; i++) { s3[i] = ddq[i] - s2[i]; } m33MultV3(B, s3, dw); @@ -1899,7 +1980,8 @@ void ddMRP2dOmega(double *q, double *dq, double *ddq, double *dw) * * dQ/dt = [B(Q)] w */ -void dPRV(double *q, double *w, double *dq) +void +dPRV(double* q, double* w, double* dq) { double B[3][3]; @@ -1912,7 +1994,8 @@ void dPRV(double *q, double *w, double *dq) * element set R into the corresponding principal * rotation vector Q. */ -void elem2PRV(double *r, double *q) +void +elem2PRV(double* r, double* q) { q[0] = r[1] * r[0]; q[1] = r[2] * r[0]; @@ -1926,7 +2009,8 @@ void elem2PRV(double *r, double *q) * parameter, while the remain three elements form * the Eulerparameter vector. */ -void EP2C(double *q, double C[3][3]) +void +EP2C(double* q, double C[3][3]) { double q0; double q1; @@ -1954,7 +2038,8 @@ void EP2C(double *q, double C[3][3]) * vector Q into the corresponding (1-2-1) Euler angle * vector E. */ -void EP2Euler121(double *q, double *e) +void +EP2Euler121(double* q, double* e) { double t1; double t2; @@ -1971,7 +2056,8 @@ void EP2Euler121(double *q, double *e) * EP2Euler123(Q,E) translates the Euler parameter vector * Q into the corresponding (1-2-3) Euler angle set. */ -void EP2Euler123(double *q, double *e) +void +EP2Euler123(double* q, double* e) { double q0; double q1; @@ -1993,7 +2079,8 @@ void EP2Euler123(double *q, double *e) * vector Q into the corresponding (1-3-1) Euler angle * vector E. */ -void EP2Euler131(double *q, double *e) +void +EP2Euler131(double* q, double* e) { double t1; double t2; @@ -2010,7 +2097,8 @@ void EP2Euler131(double *q, double *e) * EP2Euler132(Q,E) translates the Euler parameter vector * Q into the corresponding (1-3-2) Euler angle set. */ -void EP2Euler132(double *q, double *e) +void +EP2Euler132(double* q, double* e) { double q0; double q1; @@ -2032,7 +2120,8 @@ void EP2Euler132(double *q, double *e) * vector Q into the corresponding (2-1-2) Euler angle * vector E. */ -void EP2Euler212(double *q, double *e) +void +EP2Euler212(double* q, double* e) { double t1; double t2; @@ -2049,7 +2138,8 @@ void EP2Euler212(double *q, double *e) * EP2Euler213(Q,E) translates the Euler parameter vector * Q into the corresponding (2-1-3) Euler angle set. */ -void EP2Euler213(double *q, double *e) +void +EP2Euler213(double* q, double* e) { double q0; double q1; @@ -2070,7 +2160,8 @@ void EP2Euler213(double *q, double *e) * EP2Euler231(Q,E) translates the Euler parameter vector * Q into the corresponding (2-3-1) Euler angle set. */ -void EP2Euler231(double *q, double *e) +void +EP2Euler231(double* q, double* e) { double q0; double q1; @@ -2092,7 +2183,8 @@ void EP2Euler231(double *q, double *e) * vector Q into the corresponding (2-3-2) Euler angle * vector E. */ -void EP2Euler232(double *q, double *e) +void +EP2Euler232(double* q, double* e) { double t1; double t2; @@ -2109,7 +2201,8 @@ void EP2Euler232(double *q, double *e) * EP2Euler312(Q,E) translates the Euler parameter vector * Q into the corresponding (3-1-2) Euler angle set. */ -void EP2Euler312(double *q, double *e) +void +EP2Euler312(double* q, double* e) { double q0; double q1; @@ -2131,7 +2224,8 @@ void EP2Euler312(double *q, double *e) * vector Q into the corresponding (3-1-3) Euler angle * vector E. */ -void EP2Euler313(double *q, double *e) +void +EP2Euler313(double* q, double* e) { double t1; double t2; @@ -2148,7 +2242,8 @@ void EP2Euler313(double *q, double *e) * EP2Euler321(Q,E) translates the Euler parameter vector * Q into the corresponding (3-2-1) Euler angle set. */ -void EP2Euler321(double *q, double *e) +void +EP2Euler321(double* q, double* e) { double q0; double q1; @@ -2170,7 +2265,8 @@ void EP2Euler321(double *q, double *e) * vector Q into the corresponding (3-2-3) Euler angle * vector E. */ -void EP2Euler323(double *q, double *e) +void +EP2Euler323(double* q, double* e) { double t1; double t2; @@ -2187,7 +2283,8 @@ void EP2Euler323(double *q, double *e) * EP2Gibbs(Q1,Q) translates the Euler parameter vector Q1 * into the Gibbs vector Q. */ -void EP2Gibbs(double *q1, double *q) +void +EP2Gibbs(double* q1, double* q) { q[0] = q1[1] / q1[0]; q[1] = q1[2] / q1[0]; @@ -2198,9 +2295,10 @@ void EP2Gibbs(double *q1, double *q) * EP2MRP(Q1,Q) translates the Euler parameter vector Q1 * into the MRP vector Q. */ -void EP2MRP(double *q1, double *q) +void +EP2MRP(double* q1, double* q) { - if (q1[0] >= 0){ + if (q1[0] >= 0) { q[0] = q1[1] / (1 + q1[0]); q[1] = q1[2] / (1 + q1[0]); q[2] = q1[3] / (1 + q1[0]); @@ -2215,7 +2313,8 @@ void EP2MRP(double *q1, double *q) * EP2PRV(Q1,Q) translates the Euler parameter vector Q1 * into the principal rotation vector Q. */ -void EP2PRV(double *q1, double *q) +void +EP2PRV(double* q1, double* q) { double p; double sp; @@ -2238,7 +2337,8 @@ void EP2PRV(double *q1, double *q) * Returns the elementary rotation matrix about the * first body axis. */ -void Euler1(double x, double m[3][3]) +void +Euler1(double x, double m[3][3]) { m33SetIdentity(m); m[1][1] = cos(x); @@ -2252,7 +2352,8 @@ void Euler1(double x, double m[3][3]) * Returns the elementary rotation matrix about the * second body axis. */ -void Euler2(double x, double m[3][3]) +void +Euler2(double x, double m[3][3]) { m33SetIdentity(m); m[0][0] = cos(x); @@ -2266,7 +2367,8 @@ void Euler2(double x, double m[3][3]) * Returns the elementary rotation matrix about the * third body axis. */ -void Euler3(double x, double m[3][3]) +void +Euler3(double x, double m[3][3]) { m33SetIdentity(m); m[0][0] = cos(x); @@ -2280,7 +2382,8 @@ void Euler3(double x, double m[3][3]) * matrix in terms of the 1-2-1 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler1212C(double *q, double C[3][3]) +void +Euler1212C(double* q, double C[3][3]) { double st1; double ct1; @@ -2311,7 +2414,8 @@ void Euler1212C(double *q, double C[3][3]) * Euler1212EP(E,Q) translates the 121 Euler angle * vector E into the Euler parameter vector Q. */ -void Euler1212EP(double *e, double *q) +void +Euler1212EP(double* e, double* q) { double e1; double e2; @@ -2331,7 +2435,8 @@ void Euler1212EP(double *e, double *q) * Euler1212Gibbs(E,Q) translates the (1-2-1) Euler * angle vector E into the Gibbs vector Q. */ -void Euler1212Gibbs(double *e, double *q) +void +Euler1212Gibbs(double* e, double* q) { double ep[4]; @@ -2343,7 +2448,8 @@ void Euler1212Gibbs(double *e, double *q) * Euler1212MRP(E,Q) translates the (1-2-1) Euler * angle vector E into the MRP vector Q. */ -void Euler1212MRP(double *e, double *q) +void +Euler1212MRP(double* e, double* q) { double ep[4]; @@ -2355,7 +2461,8 @@ void Euler1212MRP(double *e, double *q) * Euler1212PRV(E,Q) translates the (1-2-1) Euler * angle vector E into the principal rotation vector Q. */ -void Euler1212PRV(double *e, double *q) +void +Euler1212PRV(double* e, double* q) { double ep[4]; @@ -2368,7 +2475,8 @@ void Euler1212PRV(double *e, double *q) * matrix in terms of the 1-2-3 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler1232C(double *q, double C[3][3]) +void +Euler1232C(double* q, double C[3][3]) { double st1; double st2; @@ -2399,7 +2507,8 @@ void Euler1232C(double *q, double C[3][3]) * Euler1232EP(E,Q) translates the 123 Euler angle * vector E into the Euler parameter vector Q. */ -void Euler1232EP(double *e, double *q) +void +Euler1232EP(double* e, double* q) { double c1; double c2; @@ -2425,7 +2534,8 @@ void Euler1232EP(double *e, double *q) * Euler1232Gibbs(E,Q) translates the (1-2-3) Euler * angle vector E into the Gibbs vector Q. */ -void Euler1232Gibbs(double *e, double *q) +void +Euler1232Gibbs(double* e, double* q) { double ep[4]; @@ -2437,7 +2547,8 @@ void Euler1232Gibbs(double *e, double *q) * Euler1232MRP(E,Q) translates the (1-2-3) Euler * angle vector E into the MRP vector Q. */ -void Euler1232MRP(double *e, double *q) +void +Euler1232MRP(double* e, double* q) { double ep[4]; @@ -2449,7 +2560,8 @@ void Euler1232MRP(double *e, double *q) * Euler1232PRV(E,Q) translates the (1-2-3) Euler * angle vector E into the principal rotation vector Q. */ -void Euler1232PRV(double *e, double *q) +void +Euler1232PRV(double* e, double* q) { double ep[4]; @@ -2462,7 +2574,8 @@ void Euler1232PRV(double *e, double *q) * matrix in terms of the 1-3-1 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler1312C(double *q, double C[3][3]) +void +Euler1312C(double* q, double C[3][3]) { double st1; double st2; @@ -2493,7 +2606,8 @@ void Euler1312C(double *q, double C[3][3]) * Euler1312EP(E,Q) translates the 131 Euler angle * vector E into the Euler parameter vector Q. */ -void Euler1312EP(double *e, double *q) +void +Euler1312EP(double* e, double* q) { double e1; double e2; @@ -2513,7 +2627,8 @@ void Euler1312EP(double *e, double *q) * Euler1312Gibbs(E,Q) translates the (1-3-1) Euler * angle vector E into the Gibbs vector Q. */ -void Euler1312Gibbs(double *e, double *q) +void +Euler1312Gibbs(double* e, double* q) { double ep[4]; @@ -2525,7 +2640,8 @@ void Euler1312Gibbs(double *e, double *q) * Euler1312MRP(E,Q) translates the (1-3-1) Euler * angle vector E into the MRP vector Q. */ -void Euler1312MRP(double *e, double *q) +void +Euler1312MRP(double* e, double* q) { double ep[4]; @@ -2537,7 +2653,8 @@ void Euler1312MRP(double *e, double *q) * Euler1312PRV(E,Q) translates the (1-3-1) Euler * angle vector E into the principal rotation vector Q. */ -void Euler1312PRV(double *e, double *q) +void +Euler1312PRV(double* e, double* q) { double ep[4]; @@ -2550,7 +2667,8 @@ void Euler1312PRV(double *e, double *q) * matrix in terms of the 1-3-2 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler1322C(double *q, double C[3][3]) +void +Euler1322C(double* q, double C[3][3]) { double st1; double st2; @@ -2581,7 +2699,8 @@ void Euler1322C(double *q, double C[3][3]) * Euler1322EP(E,Q) translates the 132 Euler angle * vector E into the Euler parameter vector Q. */ -void Euler1322EP(double *e, double *q) +void +Euler1322EP(double* e, double* q) { double c1; double c2; @@ -2607,7 +2726,8 @@ void Euler1322EP(double *e, double *q) * Euler1322Gibbs(E,Q) translates the (1-3-2) Euler * angle vector E into the Gibbs vector Q. */ -void Euler1322Gibbs(double *e, double *q) +void +Euler1322Gibbs(double* e, double* q) { double ep[4]; @@ -2619,7 +2739,8 @@ void Euler1322Gibbs(double *e, double *q) * Euler1322MRP(E,Q) translates the (1-3-2) Euler * angle vector E into the MRP vector Q. */ -void Euler1322MRP(double *e, double *q) +void +Euler1322MRP(double* e, double* q) { double ep[4]; @@ -2631,7 +2752,8 @@ void Euler1322MRP(double *e, double *q) * Euler1322PRV(E,Q) translates the (1-3-2) Euler * angle vector E into the principal rotation vector Q. */ -void Euler1322PRV(double *e, double *q) +void +Euler1322PRV(double* e, double* q) { double ep[4]; @@ -2644,7 +2766,8 @@ void Euler1322PRV(double *e, double *q) * matrix in terms of the 2-1-2 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler2122C(double *q, double C[3][3]) +void +Euler2122C(double* q, double C[3][3]) { double st1; double st2; @@ -2675,7 +2798,8 @@ void Euler2122C(double *q, double C[3][3]) * Euler2122EP(E,Q) translates the 212 Euler angle * vector E into the Euler parameter vector Q. */ -void Euler2122EP(double *e, double *q) +void +Euler2122EP(double* e, double* q) { double e1; double e2; @@ -2695,7 +2819,8 @@ void Euler2122EP(double *e, double *q) * Euler2122Gibbs(E,Q) translates the (2-1-2) Euler * angle vector E into the Gibbs vector Q. */ -void Euler2122Gibbs(double *e, double *q) +void +Euler2122Gibbs(double* e, double* q) { double ep[4]; @@ -2707,7 +2832,8 @@ void Euler2122Gibbs(double *e, double *q) * Euler2122MRP(E,Q) translates the (2-1-2) Euler * angle vector E into the MRP vector Q. */ -void Euler2122MRP(double *e, double *q) +void +Euler2122MRP(double* e, double* q) { double ep[4]; @@ -2719,7 +2845,8 @@ void Euler2122MRP(double *e, double *q) * Euler2122PRV(E,Q) translates the (2-1-2) Euler * angle vector E into the principal rotation vector Q. */ -void Euler2122PRV(double *e, double *q) +void +Euler2122PRV(double* e, double* q) { double ep[4]; @@ -2732,7 +2859,8 @@ void Euler2122PRV(double *e, double *q) * matrix in terms of the 2-1-3 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler2132C(double *q, double C[3][3]) +void +Euler2132C(double* q, double C[3][3]) { double st1; double st2; @@ -2763,7 +2891,8 @@ void Euler2132C(double *q, double C[3][3]) * Euler2132EP(E,Q) translates the 213 Euler angle * vector E into the Euler parameter vector Q. */ -void Euler2132EP(double *e, double *q) +void +Euler2132EP(double* e, double* q) { double c1; double c2; @@ -2789,7 +2918,8 @@ void Euler2132EP(double *e, double *q) * Euler2132Gibbs(E,Q) translates the (2-1-3) Euler * angle vector E into the Gibbs vector Q. */ -void Euler2132Gibbs(double *e, double *q) +void +Euler2132Gibbs(double* e, double* q) { double ep[4]; @@ -2801,7 +2931,8 @@ void Euler2132Gibbs(double *e, double *q) * Euler2132MRP(E,Q) translates the (2-1-3) Euler * angle vector E into the MRP vector Q. */ -void Euler2132MRP(double *e, double *q) +void +Euler2132MRP(double* e, double* q) { double ep[4]; @@ -2813,7 +2944,8 @@ void Euler2132MRP(double *e, double *q) * Euler2132PRV(E,Q) translates the (2-1-3) Euler * angle vector E into the principal rotation vector Q. */ -void Euler2132PRV(double *e, double *q) +void +Euler2132PRV(double* e, double* q) { double ep[4]; @@ -2826,7 +2958,8 @@ void Euler2132PRV(double *e, double *q) * matrix in terms of the 2-3-1 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler2312C(double *q, double C[3][3]) +void +Euler2312C(double* q, double C[3][3]) { double st1; double st2; @@ -2857,7 +2990,8 @@ void Euler2312C(double *q, double C[3][3]) * Euler2312EP(E,Q) translates the 231 Euler angle * vector E into the Euler parameter vector Q. */ -void Euler2312EP(double *e, double *q) +void +Euler2312EP(double* e, double* q) { double c1; double c2; @@ -2883,7 +3017,8 @@ void Euler2312EP(double *e, double *q) * Euler2312Gibbs(E,Q) translates the (2-3-1) Euler * angle vector E into the Gibbs vector Q. */ -void Euler2312Gibbs(double *e, double *q) +void +Euler2312Gibbs(double* e, double* q) { double ep[4]; @@ -2895,7 +3030,8 @@ void Euler2312Gibbs(double *e, double *q) * Euler2312MRP(E,Q) translates the (2-3-1) Euler * angle vector E into the MRP vector Q. */ -void Euler2312MRP(double *e, double *q) +void +Euler2312MRP(double* e, double* q) { double ep[4]; @@ -2907,7 +3043,8 @@ void Euler2312MRP(double *e, double *q) * Euler2312PRV(E,Q) translates the (2-3-1) Euler * angle vector E into the principal rotation vector Q. */ -void Euler2312PRV(double *e, double *q) +void +Euler2312PRV(double* e, double* q) { double ep[4]; @@ -2920,7 +3057,8 @@ void Euler2312PRV(double *e, double *q) * matrix in terms of the 2-3-2 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler2322C(double *q, double C[3][3]) +void +Euler2322C(double* q, double C[3][3]) { double st1; double st2; @@ -2948,10 +3086,11 @@ void Euler2322C(double *q, double C[3][3]) } /* -* Euler2322EP(E,Q) translates the 232 Euler angle -* vector E into the Euler parameter vector Q. -*/ -void Euler2322EP(double *e, double *q) + * Euler2322EP(E,Q) translates the 232 Euler angle + * vector E into the Euler parameter vector Q. + */ +void +Euler2322EP(double* e, double* q) { double e1; double e2; @@ -2971,7 +3110,8 @@ void Euler2322EP(double *e, double *q) * Euler2322Gibbs(E) translates the (2-3-2) Euler * angle vector E into the Gibbs vector Q. */ -void Euler2322Gibbs(double *e, double *q) +void +Euler2322Gibbs(double* e, double* q) { double ep[4]; @@ -2983,7 +3123,8 @@ void Euler2322Gibbs(double *e, double *q) * Euler2322MRP(E,Q) translates the (2-3-2) Euler * angle vector E into the MRP vector Q. */ -void Euler2322MRP(double *e, double *q) +void +Euler2322MRP(double* e, double* q) { double ep[4]; @@ -2995,7 +3136,8 @@ void Euler2322MRP(double *e, double *q) * Euler2322PRV(E,Q) translates the (2-3-2) Euler * angle vector E into the principal rotation vector Q. */ -void Euler2322PRV(double *e, double *q) +void +Euler2322PRV(double* e, double* q) { double ep[4]; @@ -3008,7 +3150,8 @@ void Euler2322PRV(double *e, double *q) * matrix in terms of the 1-2-3 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler3122C(double *q, double C[3][3]) +void +Euler3122C(double* q, double C[3][3]) { double st1; double st2; @@ -3039,7 +3182,8 @@ void Euler3122C(double *q, double C[3][3]) * Euler3122EP(E,Q) translates the 312 Euler angle * vector E into the Euler parameter vector Q. */ -void Euler3122EP(double *e, double *q) +void +Euler3122EP(double* e, double* q) { double c1; double c2; @@ -3065,7 +3209,8 @@ void Euler3122EP(double *e, double *q) * Euler3122Gibbs(E,Q) translates the (3-1-2) Euler * angle vector E into the Gibbs vector Q. */ -void Euler3122Gibbs(double *e, double *q) +void +Euler3122Gibbs(double* e, double* q) { double ep[4]; @@ -3077,7 +3222,8 @@ void Euler3122Gibbs(double *e, double *q) * Euler3122MRP(E,Q) translates the (3-1-2) Euler * angle vector E into the MRP vector Q. */ -void Euler3122MRP(double *e, double *q) +void +Euler3122MRP(double* e, double* q) { double ep[4]; @@ -3089,7 +3235,8 @@ void Euler3122MRP(double *e, double *q) * Euler3122PRV(E,Q) translates the (3-1-2) Euler * angle vector E into the principal rotation vector Q. */ -void Euler3122PRV(double *e, double *q) +void +Euler3122PRV(double* e, double* q) { double ep[4]; @@ -3102,7 +3249,8 @@ void Euler3122PRV(double *e, double *q) * matrix in terms of the 3-1-3 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler3132C(double *q, double C[3][3]) +void +Euler3132C(double* q, double C[3][3]) { double st1; double st2; @@ -3133,7 +3281,8 @@ void Euler3132C(double *q, double C[3][3]) * Euler3132EP(E,Q) translates the 313 Euler angle * vector E into the Euler parameter vector Q. */ -void Euler3132EP(double *e, double *q) +void +Euler3132EP(double* e, double* q) { double e1; double e2; @@ -3153,7 +3302,8 @@ void Euler3132EP(double *e, double *q) * Euler3132Gibbs(E,Q) translates the (3-1-3) Euler * angle vector E into the Gibbs vector Q. */ -void Euler3132Gibbs(double *e, double *q) +void +Euler3132Gibbs(double* e, double* q) { double ep[4]; @@ -3165,7 +3315,8 @@ void Euler3132Gibbs(double *e, double *q) * Euler3132MRP(E,Q) translates the (3-1-3) Euler * angle vector E into the MRP vector Q. */ -void Euler3132MRP(double *e, double *q) +void +Euler3132MRP(double* e, double* q) { double ep[4]; @@ -3177,7 +3328,8 @@ void Euler3132MRP(double *e, double *q) * Euler3132PRV(E,Q) translates the (3-1-3) Euler * angle vector E into the principal rotation vector Q. */ -void Euler3132PRV(double *e, double *q) +void +Euler3132PRV(double* e, double* q) { double ep[4]; @@ -3190,7 +3342,8 @@ void Euler3132PRV(double *e, double *q) * matrix in terms of the 3-2-1 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler3212C(double *q, double C[3][3]) +void +Euler3212C(double* q, double C[3][3]) { double st1; double st2; @@ -3221,7 +3374,8 @@ void Euler3212C(double *q, double C[3][3]) * Euler3212EPE,Q) translates the 321 Euler angle * vector E into the Euler parameter vector Q. */ -void Euler3212EP(double *e, double *q) +void +Euler3212EP(double* e, double* q) { double c1; double c2; @@ -3247,7 +3401,8 @@ void Euler3212EP(double *e, double *q) * Euler3212Gibbs(E,Q) translates the (3-2-1) Euler * angle vector E into the Gibbs vector Q. */ -void Euler3212Gibbs(double *e, double *q) +void +Euler3212Gibbs(double* e, double* q) { double ep[4]; @@ -3259,7 +3414,8 @@ void Euler3212Gibbs(double *e, double *q) * Euler3212MRP(E,Q) translates the (3-2-1) Euler * angle vector E into the MRP vector Q. */ -void Euler3212MRP(double *e, double *q) +void +Euler3212MRP(double* e, double* q) { double ep[4]; @@ -3271,7 +3427,8 @@ void Euler3212MRP(double *e, double *q) * Euler3212PRV(E,Q) translates the (3-2-1) Euler * angle vector E into the principal rotation vector Q. */ -void Euler3212PRV(double *e, double *q) +void +Euler3212PRV(double* e, double* q) { double ep[4]; @@ -3284,7 +3441,8 @@ void Euler3212PRV(double *e, double *q) * matrix in terms of the 3-2-3 Euler angles. * Input Q must be a 3x1 vector of Euler angles. */ -void Euler3232C(double *q, double C[3][3]) +void +Euler3232C(double* q, double C[3][3]) { double st1; double st2; @@ -3315,7 +3473,8 @@ void Euler3232C(double *q, double C[3][3]) * Euler3232EP(E,Q) translates the 323 Euler angle * vector E into the Euler parameter vector Q. */ -void Euler3232EP(double *e, double *q) +void +Euler3232EP(double* e, double* q) { double e1; double e2; @@ -3335,7 +3494,8 @@ void Euler3232EP(double *e, double *q) * Euler3232Gibbs(E,Q) translates the (3-2-3) Euler * angle vector E into the Gibbs vector Q. */ -void Euler3232Gibbs(double *e, double *q) +void +Euler3232Gibbs(double* e, double* q) { double ep[4]; @@ -3347,7 +3507,8 @@ void Euler3232Gibbs(double *e, double *q) * Euler3232MRP(E,Q) translates the (3-2-3) Euler * angle vector E into the MRP vector Q. */ -void Euler3232MRP(double *e, double *q) +void +Euler3232MRP(double* e, double* q) { double ep[4]; @@ -3359,7 +3520,8 @@ void Euler3232MRP(double *e, double *q) * Euler3232PRV(E,Q) translates the (3-2-3) Euler * angle vector Q1 into the principal rotation vector Q. */ -void Euler3232PRV(double *e, double *q) +void +Euler3232PRV(double* e, double* q) { double ep[4]; @@ -3371,7 +3533,8 @@ void Euler3232PRV(double *e, double *q) * Gibbs2C(Q,C) returns the direction cosine * matrix in terms of the 3x1 Gibbs vector Q. */ -void Gibbs2C(double *q, double C[3][3]) +void +Gibbs2C(double* q, double C[3][3]) { double q1; double q2; @@ -3399,7 +3562,8 @@ void Gibbs2C(double *q, double C[3][3]) * Gibbs2EP(Q1,Q) translates the Gibbs vector Q1 * into the Euler parameter vector Q. */ -void Gibbs2EP(double *q1, double *q) +void +Gibbs2EP(double* q1, double* q) { q[0] = 1 / sqrt(1 + v3Dot(q1, q1)); q[1] = q1[0] * q[0]; @@ -3411,7 +3575,8 @@ void Gibbs2EP(double *q1, double *q) * Gibbs2Euler121(Q,E) translates the Gibbs * vector Q into the (1-2-1) Euler angle vector E. */ -void Gibbs2Euler121(double *q, double *e) +void +Gibbs2Euler121(double* q, double* e) { double ep[4]; @@ -3423,7 +3588,8 @@ void Gibbs2Euler121(double *q, double *e) * Gibbs2Euler123(Q,E) translates the Gibbs * vector Q into the (1-2-3) Euler angle vector E. */ -void Gibbs2Euler123(double *q, double *e) +void +Gibbs2Euler123(double* q, double* e) { double ep[4]; @@ -3435,7 +3601,8 @@ void Gibbs2Euler123(double *q, double *e) * Gibbs2Euler131(Q,E) translates the Gibbs * vector Q into the (1-3-1) Euler angle vector E. */ -void Gibbs2Euler131(double *q, double *e) +void +Gibbs2Euler131(double* q, double* e) { double ep[4]; @@ -3447,7 +3614,8 @@ void Gibbs2Euler131(double *q, double *e) * Gibbs2Euler132(Q,E) translates the Gibbs * vector Q into the (1-3-2) Euler angle vector E. */ -void Gibbs2Euler132(double *q, double *e) +void +Gibbs2Euler132(double* q, double* e) { double ep[4]; @@ -3459,7 +3627,8 @@ void Gibbs2Euler132(double *q, double *e) * Gibbs2Euler212(Q,E) translates the Gibbs * vector Q into the (2-1-2) Euler angle vector E. */ -void Gibbs2Euler212(double *q, double *e) +void +Gibbs2Euler212(double* q, double* e) { double ep[4]; @@ -3471,7 +3640,8 @@ void Gibbs2Euler212(double *q, double *e) * Gibbs2Euler213(Q,E) translates the Gibbs * vector Q into the (2-1-3) Euler angle vector E. */ -void Gibbs2Euler213(double *q, double *e) +void +Gibbs2Euler213(double* q, double* e) { double ep[4]; @@ -3483,7 +3653,8 @@ void Gibbs2Euler213(double *q, double *e) * Gibbs2Euler231(Q,E) translates the Gibbs * vector Q into the (2-3-1) Euler angle vector E. */ -void Gibbs2Euler231(double *q, double *e) +void +Gibbs2Euler231(double* q, double* e) { double ep[4]; @@ -3495,7 +3666,8 @@ void Gibbs2Euler231(double *q, double *e) * Gibbs2Euler232(Q,E) translates the Gibbs * vector Q into the (2-3-2) Euler angle vector E. */ -void Gibbs2Euler232(double *q, double *e) +void +Gibbs2Euler232(double* q, double* e) { double ep[4]; @@ -3507,7 +3679,8 @@ void Gibbs2Euler232(double *q, double *e) * Gibbs2Euler312(Q,E) translates the Gibbs * vector Q into the (3-1-2) Euler angle vector E. */ -void Gibbs2Euler312(double *q, double *e) +void +Gibbs2Euler312(double* q, double* e) { double ep[4]; @@ -3519,7 +3692,8 @@ void Gibbs2Euler312(double *q, double *e) * Gibbs2Euler313(Q,E) translates the Gibbs * vector Q into the (3-1-3) Euler angle vector E. */ -void Gibbs2Euler313(double *q, double *e) +void +Gibbs2Euler313(double* q, double* e) { double ep[4]; @@ -3531,7 +3705,8 @@ void Gibbs2Euler313(double *q, double *e) * Gibbs2Euler321(Q,E) translates the Gibbs * vector Q into the (3-2-1) Euler angle vector E. */ -void Gibbs2Euler321(double *q, double *e) +void +Gibbs2Euler321(double* q, double* e) { double ep[4]; @@ -3543,7 +3718,8 @@ void Gibbs2Euler321(double *q, double *e) * Gibbs2Euler323(Q,E) translates the Gibbs * vector Q into the (3-2-3) Euler angle vector E. */ -void Gibbs2Euler323(double *q, double *e) +void +Gibbs2Euler323(double* q, double* e) { double ep[4]; @@ -3555,7 +3731,8 @@ void Gibbs2Euler323(double *q, double *e) * Gibbs2MRP(Q1,Q) translates the Gibbs vector Q1 * into the MRP vector Q. */ -void Gibbs2MRP(double *q1, double *q) +void +Gibbs2MRP(double* q1, double* q) { v3Scale(1.0 / (1 + sqrt(1 + v3Dot(q1, q1))), q1, q); } @@ -3564,7 +3741,8 @@ void Gibbs2MRP(double *q1, double *q) * Gibbs2PRV(Q1,Q) translates the Gibbs vector Q1 * into the principal rotation vector Q. */ -void Gibbs2PRV(double *q1, double *q) +void +Gibbs2PRV(double* q1, double* q) { double tp; double p; @@ -3586,7 +3764,8 @@ void Gibbs2PRV(double *q1, double *q) * MRP2C(Q,C) returns the direction cosine * matrix in terms of the 3x1 MRP vector Q. */ -void MRP2C(double *q, double C[3][3]) +void +MRP2C(double* q, double C[3][3]) { double q1; double q2; @@ -3618,7 +3797,8 @@ void MRP2C(double *q, double C[3][3]) * MRP2EP(Q1,Q) translates the MRP vector Q1 * into the Euler parameter vector Q. */ -void MRP2EP(double *q1, double *q) +void +MRP2EP(double* q1, double* q) { double ps; @@ -3633,7 +3813,8 @@ void MRP2EP(double *q1, double *q) * MRP2Euler121(Q,E) translates the MRP * vector Q into the (1-2-1) Euler angle vector E. */ -void MRP2Euler121(double *q, double *e) +void +MRP2Euler121(double* q, double* e) { double ep[4]; @@ -3645,7 +3826,8 @@ void MRP2Euler121(double *q, double *e) * MRP2Euler123(Q,E) translates the MRP * vector Q into the (1-2-3) Euler angle vector E. */ -void MRP2Euler123(double *q, double *e) +void +MRP2Euler123(double* q, double* e) { double ep[4]; @@ -3657,7 +3839,8 @@ void MRP2Euler123(double *q, double *e) * MRP2Euler131(Q,E) translates the MRP * vector Q into the (1-3-1) Euler angle vector E. */ -void MRP2Euler131(double *q, double *e) +void +MRP2Euler131(double* q, double* e) { double ep[4]; @@ -3669,7 +3852,8 @@ void MRP2Euler131(double *q, double *e) * MRP2Euler132(Q,E) translates the MRP * vector Q into the (1-3-2) Euler angle vector E. */ -void MRP2Euler132(double *q, double *e) +void +MRP2Euler132(double* q, double* e) { double ep[4]; @@ -3681,7 +3865,8 @@ void MRP2Euler132(double *q, double *e) * E = MRP2Euler212(Q) translates the MRP * vector Q into the (2-1-2) Euler angle vector E. */ -void MRP2Euler212(double *q, double *e) +void +MRP2Euler212(double* q, double* e) { double ep[4]; @@ -3693,7 +3878,8 @@ void MRP2Euler212(double *q, double *e) * MRP2Euler213(Q,E) translates the MRP * vector Q into the (2-1-3) Euler angle vector E. */ -void MRP2Euler213(double *q, double *e) +void +MRP2Euler213(double* q, double* e) { double ep[4]; @@ -3705,7 +3891,8 @@ void MRP2Euler213(double *q, double *e) * MRP2Euler231(Q,E) translates the MRP * vector Q into the (2-3-1) Euler angle vector E. */ -void MRP2Euler231(double *q, double *e) +void +MRP2Euler231(double* q, double* e) { double ep[4]; @@ -3717,7 +3904,8 @@ void MRP2Euler231(double *q, double *e) * MRP2Euler232(Q,E) translates the MRP * vector Q into the (2-3-2) Euler angle vector E. */ -void MRP2Euler232(double *q, double *e) +void +MRP2Euler232(double* q, double* e) { double ep[4]; @@ -3729,7 +3917,8 @@ void MRP2Euler232(double *q, double *e) * MRP2Euler312(Q,E) translates the MRP * vector Q into the (3-1-2) Euler angle vector E. */ -void MRP2Euler312(double *q, double *e) +void +MRP2Euler312(double* q, double* e) { double ep[4]; @@ -3741,7 +3930,8 @@ void MRP2Euler312(double *q, double *e) * MRP2Euler313(Q,E) translates the MRP * vector Q into the (3-1-3) Euler angle vector E. */ -void MRP2Euler313(double *q, double *e) +void +MRP2Euler313(double* q, double* e) { double ep[4]; @@ -3753,7 +3943,8 @@ void MRP2Euler313(double *q, double *e) * MRP2Euler321(Q,E) translates the MRP * vector Q into the (3-2-1) Euler angle vector E. */ -void MRP2Euler321(double *q, double *e) +void +MRP2Euler321(double* q, double* e) { double ep[4]; @@ -3765,7 +3956,8 @@ void MRP2Euler321(double *q, double *e) * MRP2Euler323(Q,E) translates the MRP * vector Q into the (3-2-3) Euler angle vector E. */ -void MRP2Euler323(double *q, double *e) +void +MRP2Euler323(double* q, double* e) { double ep[4]; @@ -3777,7 +3969,8 @@ void MRP2Euler323(double *q, double *e) * MRP2Gibbs(Q1,Q) translates the MRP vector Q1 * into the Gibbs vector Q. */ -void MRP2Gibbs(double *q1, double *q) +void +MRP2Gibbs(double* q1, double* q) { v3Scale(2. / (1. - v3Dot(q1, q1)), q1, q); } @@ -3786,15 +3979,15 @@ void MRP2Gibbs(double *q1, double *q) * MRP2PRV(Q1,Q) translates the MRP vector Q1 * into the principal rotation vector Q. */ -void MRP2PRV(double *q1, double *q) +void +MRP2PRV(double* q1, double* q) { double tp; double p; tp = sqrt(v3Dot(q1, q1)); - if(tp < nearZero) - { - memset(q, 0x0, 3*sizeof(double)); + if (tp < nearZero) { + memset(q, 0x0, 3 * sizeof(double)); return; } p = 4 * atan(tp); @@ -3807,12 +4000,13 @@ void MRP2PRV(double *q1, double *q) * MRPswitch(Q,s2,s) checks to see if v3Norm(Q) is larger than s2. * If yes, then the MRP vector Q is mapped to its shadow set. */ -void MRPswitch(double *q, double s2, double *s) +void +MRPswitch(double* q, double s2, double* s) { double q2; q2 = v3Dot(q, q); - if(q2 > s2 * s2) { + if (q2 > s2 * s2) { v3Scale(-1. / q2, q, s); } else { v3Copy(q, s); @@ -3822,7 +4016,8 @@ void MRPswitch(double *q, double s2, double *s) /* * MRPshadow forces a switch from the current MRP to its shadow set */ -void MRPshadow(double *qIn, double *qOut) +void +MRPshadow(double* qIn, double* qOut) { double q2; @@ -3834,17 +4029,18 @@ void MRPshadow(double *qIn, double *qOut) /* * Makes sure that the angle x lies within +/- Pi. */ -double wrapToPi(double x) +double +wrapToPi(double x) { double q; q = x; - if(x > M_PI) { + if (x > M_PI) { q = x - 2 * M_PI; } - if(x < -M_PI) { + if (x < -M_PI) { q = x + 2 * M_PI; } @@ -3856,7 +4052,8 @@ double wrapToPi(double x) * matrix in terms of the 3x1 principal rotation vector * Q. */ -void PRV2C(double *q, double C[3][3]) +void +PRV2C(double* q, double C[3][3]) { double q0; double q1; @@ -3866,8 +4063,7 @@ void PRV2C(double *q, double C[3][3]) double sp; double d1; - if(v3Norm(q) == 0.0) - { + if (v3Norm(q) == 0.0) { m33SetIdentity(C); return; } @@ -3895,26 +4091,25 @@ void PRV2C(double *q, double C[3][3]) * PRV2elem(R,Q) translates a prinicpal rotation vector R * into the corresponding principal rotation element set Q. */ -void PRV2elem(double *r, double *q) +void +PRV2elem(double* r, double* q) { q[0] = sqrt(v3Dot(r, r)); - if (q[0] < 1.0E-12) - { - q[1] = q[2] = q[3] = 0.0; - } - else - { - q[1] = r[0] / q[0]; - q[2] = r[1] / q[0]; - q[3] = r[2] / q[0]; - } + if (q[0] < 1.0E-12) { + q[1] = q[2] = q[3] = 0.0; + } else { + q[1] = r[0] / q[0]; + q[2] = r[1] / q[0]; + q[3] = r[2] / q[0]; + } } /* * PRV2EP(Q0,Q) translates the principal rotation vector Q1 * into the Euler parameter vector Q. */ -void PRV2EP(double *q0, double *q) +void +PRV2EP(double* q0, double* q) { double q1[4]; double sp; @@ -3931,7 +4126,8 @@ void PRV2EP(double *q0, double *q) * PRV2Euler121(Q,E) translates the principal rotation * vector Q into the (1-2-1) Euler angle vector E. */ -void PRV2Euler121(double *q, double *e) +void +PRV2Euler121(double* q, double* e) { double ep[4]; @@ -3943,7 +4139,8 @@ void PRV2Euler121(double *q, double *e) * PRV2Euler123(Q,E) translates the principal rotation * vector Q into the (1-2-3) Euler angle vector E. */ -void PRV2Euler123(double *q, double *e) +void +PRV2Euler123(double* q, double* e) { double ep[4]; @@ -3955,7 +4152,8 @@ void PRV2Euler123(double *q, double *e) * PRV2Euler131(Q,E) translates the principal rotation * vector Q into the (1-3-1) Euler angle vector E. */ -void PRV2Euler131(double *q, double *e) +void +PRV2Euler131(double* q, double* e) { double ep[4]; @@ -3967,7 +4165,8 @@ void PRV2Euler131(double *q, double *e) * PRV2Euler132(Q,E) translates the principal rotation * vector Q into the (1-3-2) Euler angle vector E. */ -void PRV2Euler132(double *q, double *e) +void +PRV2Euler132(double* q, double* e) { double ep[4]; @@ -3979,7 +4178,8 @@ void PRV2Euler132(double *q, double *e) * PRV2Euler212(Q,E) translates the principal rotation * vector Q into the (2-1-2) Euler angle vector E. */ -void PRV2Euler212(double *q, double *e) +void +PRV2Euler212(double* q, double* e) { double ep[4]; @@ -3991,7 +4191,8 @@ void PRV2Euler212(double *q, double *e) * PRV2Euler213(Q,E) translates the principal rotation * vector Q into the (2-1-3) Euler angle vector E. */ -void PRV2Euler213(double *q, double *e) +void +PRV2Euler213(double* q, double* e) { double ep[4]; @@ -4003,7 +4204,8 @@ void PRV2Euler213(double *q, double *e) * PRV2Euler231(Q) translates the principal rotation * vector Q into the (2-3-1) Euler angle vector E. */ -void PRV2Euler231(double *q, double *e) +void +PRV2Euler231(double* q, double* e) { double ep[4]; @@ -4015,7 +4217,8 @@ void PRV2Euler231(double *q, double *e) * PRV2Euler232(Q,E) translates the principal rotation * vector Q into the (2-3-2) Euler angle vector E. */ -void PRV2Euler232(double *q, double *e) +void +PRV2Euler232(double* q, double* e) { double ep[4]; @@ -4027,7 +4230,8 @@ void PRV2Euler232(double *q, double *e) * PRV2Euler312(Q,E) translates the principal rotation * vector Q into the (3-1-2) Euler angle vector E. */ -void PRV2Euler312(double *q, double *e) +void +PRV2Euler312(double* q, double* e) { double ep[4]; @@ -4039,7 +4243,8 @@ void PRV2Euler312(double *q, double *e) * PRV2Euler313(Q,E) translates the principal rotation * vector Q into the (3-1-3) Euler angle vector E. */ -void PRV2Euler313(double *q, double *e) +void +PRV2Euler313(double* q, double* e) { double ep[4]; @@ -4051,7 +4256,8 @@ void PRV2Euler313(double *q, double *e) * PRV2Euler321(Q,E) translates the principal rotation * vector Q into the (3-2-1) Euler angle vector E. */ -void PRV2Euler321(double *q, double *e) +void +PRV2Euler321(double* q, double* e) { double ep[4]; @@ -4063,7 +4269,8 @@ void PRV2Euler321(double *q, double *e) * PRV2Euler323(Q,E) translates the principal rotation * vector Q into the (3-2-3) Euler angle vector E. */ -void PRV2Euler323(double *q, double *e) +void +PRV2Euler323(double* q, double* e) { double ep[4]; @@ -4075,7 +4282,8 @@ void PRV2Euler323(double *q, double *e) * PRV2Gibbs(Q0,Q) translates the principal rotation vector Q1 * into the Gibbs vector Q. */ -void PRV2Gibbs(double *q0, double *q) +void +PRV2Gibbs(double* q0, double* q) { double q1[4]; double tp; @@ -4091,7 +4299,8 @@ void PRV2Gibbs(double *q0, double *q) * PRV2MRP(Q0,Q) translates the principal rotation vector Q1 * into the MRP vector Q. */ -void PRV2MRP(double *q0, double *q) +void +PRV2MRP(double* q0, double* q) { double q1[4]; double tp; @@ -4108,7 +4317,8 @@ void PRV2MRP(double *q0, double *q) * which corresponds to relative rotation from B2 * to B1. */ -void subEP(double *b1, double *b2, double *q) +void +subEP(double* b1, double* b2, double* q) { q[0] = b2[0] * b1[0] + b2[1] * b1[1] + b2[2] * b1[2] + b2[3] * b1[3]; q[1] = -b2[1] * b1[0] + b2[0] * b1[1] + b2[3] * b1[2] - b2[2] * b1[3]; @@ -4120,7 +4330,8 @@ void subEP(double *b1, double *b2, double *q) * subEuler121(E,E1,E2) computes the relative * (1-2-1) Euler angle vector from E1 to E. */ -void subEuler121(double *e, double *e1, double *e2) +void +subEuler121(double* e, double* e1, double* e2) { double cp; double cp1; @@ -4145,7 +4356,8 @@ void subEuler121(double *e, double *e1, double *e2) * subEuler123(E,E1,E2) computes the relative * (1-2-3) Euler angle vector from E1 to E. */ -void subEuler123(double *e, double *e1, double *e2) +void +subEuler123(double* e, double* e1, double* e2) { double C[3][3]; double C1[3][3]; @@ -4161,7 +4373,8 @@ void subEuler123(double *e, double *e1, double *e2) * subEuler131(E,E1,E2) computes the relative * (1-3-1) Euler angle vector from E1 to E. */ -void subEuler131(double *e, double *e1, double *e2) +void +subEuler131(double* e, double* e1, double* e2) { double cp; double cp1; @@ -4186,7 +4399,8 @@ void subEuler131(double *e, double *e1, double *e2) * subEuler132(E,E1,E2) computes the relative * (1-3-2) Euler angle vector from E1 to E. */ -void subEuler132(double *e, double *e1, double *e2) +void +subEuler132(double* e, double* e1, double* e2) { double C[3][3]; double C1[3][3]; @@ -4202,7 +4416,8 @@ void subEuler132(double *e, double *e1, double *e2) * subEuler212(E,E1,E2) computes the relative * (2-1-2) Euler angle vector from E1 to E. */ -void subEuler212(double *e, double *e1, double *e2) +void +subEuler212(double* e, double* e1, double* e2) { double cp; double cp1; @@ -4227,7 +4442,8 @@ void subEuler212(double *e, double *e1, double *e2) * subEuler213(E,E1,E2) computes the relative * (2-1-3) Euler angle vector from E1 to E. */ -void subEuler213(double *e, double *e1, double *e2) +void +subEuler213(double* e, double* e1, double* e2) { double C[3][3]; double C1[3][3]; @@ -4243,7 +4459,8 @@ void subEuler213(double *e, double *e1, double *e2) * subEuler231(E,E1,E2) computes the relative * (2-3-1) Euler angle vector from E1 to E. */ -void subEuler231(double *e, double *e1, double *e2) +void +subEuler231(double* e, double* e1, double* e2) { double C[3][3]; double C1[3][3]; @@ -4259,7 +4476,8 @@ void subEuler231(double *e, double *e1, double *e2) * subEuler232(E,E1,E2) computes the relative * (2-3-2) Euler angle vector from E1 to E. */ -void subEuler232(double *e, double *e1, double *e2) +void +subEuler232(double* e, double* e1, double* e2) { double cp; double cp1; @@ -4284,7 +4502,8 @@ void subEuler232(double *e, double *e1, double *e2) * subEuler312(E,E1,E2) computes the relative * (3-1-2) Euler angle vector from E1 to E. */ -void subEuler312(double *e, double *e1, double *e2) +void +subEuler312(double* e, double* e1, double* e2) { double C[3][3]; double C1[3][3]; @@ -4300,7 +4519,8 @@ void subEuler312(double *e, double *e1, double *e2) * subEuler313(E,E1,E2) computes the relative * (3-1-3) Euler angle vector from E1 to E. */ -void subEuler313(double *e, double *e1, double *e2) +void +subEuler313(double* e, double* e1, double* e2) { double cp; double cp1; @@ -4325,7 +4545,8 @@ void subEuler313(double *e, double *e1, double *e2) * subEuler321(E,E1,E2) computes the relative * (3-2-1) Euler angle vector from E1 to E. */ -void subEuler321(double *e, double *e1, double *e2) +void +subEuler321(double* e, double* e1, double* e2) { double C[3][3]; double C1[3][3]; @@ -4341,7 +4562,8 @@ void subEuler321(double *e, double *e1, double *e2) * subEuler323(E,E1,E2) computes the relative * (3-2-3) Euler angle vector from E1 to E. */ -void subEuler323(double *e, double *e1, double *e2) +void +subEuler323(double* e, double* e1, double* e2) { double cp; double cp1; @@ -4367,7 +4589,8 @@ void subEuler323(double *e, double *e1, double *e2) * which corresponds to relative rotation from Q2 * to Q1. */ -void subGibbs(double *q1, double *q2, double *q) +void +subGibbs(double* q1, double* q2, double* q) { double d1[3]; @@ -4382,7 +4605,8 @@ void subGibbs(double *q1, double *q2, double *q) * which corresponds to relative rotation from Q2 * to Q1. */ -void subMRP(double *q1, double *q2, double *q) +void +subMRP(double* q1, double* q2, double* q) { double d1[3]; double s1[3]; @@ -4390,11 +4614,11 @@ void subMRP(double *q1, double *q2, double *q) double mag; v3Copy(q1, s1); - det = (1. + v3Dot(s1, s1)*v3Dot(q2, q2) + 2.*v3Dot(s1, q2)); + det = (1. + v3Dot(s1, s1) * v3Dot(q2, q2) + 2. * v3Dot(s1, q2)); if (fabs(det) < 0.1) { mag = v3Dot(s1, s1); - v3Scale(-1.0/mag, s1, s1); - det = (1. + v3Dot(s1, s1)*v3Dot(q2, q2) + 2.*v3Dot(s1, q2)); + v3Scale(-1.0 / mag, s1, s1); + det = (1. + v3Dot(s1, s1) * v3Dot(q2, q2) + 2. * v3Dot(s1, q2)); } v3Cross(s1, q2, d1); @@ -4407,10 +4631,9 @@ void subMRP(double *q1, double *q2, double *q) /* map MRP to inner set */ mag = v3Dot(q, q); - if (mag > 1.0){ - v3Scale(-1./mag, q, q); + if (mag > 1.0) { + v3Scale(-1. / mag, q, q); } - } /* @@ -4418,7 +4641,8 @@ void subMRP(double *q1, double *q2, double *q) * which corresponds to relative principal rotation from Q2 * to Q1. */ -void subPRV(double *q10, double *q20, double *q) +void +subPRV(double* q10, double* q20, double* q) { double q1[4]; double q2[4]; @@ -4440,7 +4664,7 @@ void subPRV(double *q10, double *q20, double *q) v3Copy(&(q1[1]), e1); v3Copy(&(q2[1]), e2); - p = 2.*safeAcos(cp1 * cp2 + sp1 * sp2 * v3Dot(e1, e2)); + p = 2. * safeAcos(cp1 * cp2 + sp1 * sp2 * v3Dot(e1, e2)); sp = sin(p / 2.); v3Cross(e1, e2, q1); @@ -4456,7 +4680,8 @@ void subPRV(double *q10, double *q20, double *q) * Mi(theta, a, C) returns the rotation matrix corresponding * to a single axis rotation about axis a by the angle theta */ -void Mi(double theta, int a, double C[3][3]) +void +Mi(double theta, int a, double C[3][3]) { double c; double s; @@ -4464,37 +4689,37 @@ void Mi(double theta, int a, double C[3][3]) c = cos(theta); s = sin(theta); - switch(a) { + switch (a) { case 1: C[0][0] = 1.; C[0][1] = 0.; C[0][2] = 0.; C[1][0] = 0.; - C[1][1] = c; - C[1][2] = s; + C[1][1] = c; + C[1][2] = s; C[2][0] = 0.; C[2][1] = -s; - C[2][2] = c; + C[2][2] = c; break; case 2: - C[0][0] = c; + C[0][0] = c; C[0][1] = 0.; C[0][2] = -s; C[1][0] = 0.; C[1][1] = 1.; C[1][2] = 0.; - C[2][0] = s; + C[2][0] = s; C[2][1] = 0.; - C[2][2] = c; + C[2][2] = c; break; case 3: - C[0][0] = c; - C[0][1] = s; + C[0][0] = c; + C[0][1] = s; C[0][2] = 0.; C[1][0] = -s; - C[1][1] = c; + C[1][1] = c; C[1][2] = 0.; C[2][0] = 0.; C[2][1] = 0.; @@ -4509,7 +4734,8 @@ void Mi(double theta, int a, double C[3][3]) /* * tilde(theta, mat) returns the the 3x3 cross product matrix */ -void tilde(double *v, double mat[3][3]) +void +tilde(double* v, double mat[3][3]) { m33SetZero(mat); mat[0][1] = -v[2]; diff --git a/src/architecture/utilities/rigidBodyKinematics.h b/src/architecture/utilities/rigidBodyKinematics.h index 8d76755756..c2dfbc78dc 100644 --- a/src/architecture/utilities/rigidBodyKinematics.h +++ b/src/architecture/utilities/rigidBodyKinematics.h @@ -22,243 +22,244 @@ #include #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif - void addEP(double *b1, double *b2, double *result); - void addEuler121(double *e1, double *e2, double *result); - void addEuler123(double *e1, double *e2, double *result); - void addEuler131(double *e1, double *e2, double *result); - void addEuler132(double *e1, double *e2, double *result); - void addEuler212(double *e1, double *e2, double *result); - void addEuler213(double *e1, double *e2, double *result); - void addEuler231(double *e1, double *e2, double *result); - void addEuler232(double *e1, double *e2, double *result); - void addEuler312(double *e1, double *e2, double *result); - void addEuler313(double *e1, double *e2, double *result); - void addEuler321(double *e1, double *e2, double *result); - void addEuler323(double *e1, double *e2, double *result); - void addGibbs(double *q1, double *q2, double *result); - void addMRP(double *q1, double *q2, double *result); - void addPRV(double *q1, double *q2, double *result); - void BinvEP(double *q, double B[3][4]); - void BinvEuler121(double *q, double B[3][3]); - void BinvEuler123(double *q, double B[3][3]); - void BinvEuler131(double *q, double B[3][3]); - void BinvEuler132(double *q, double B[3][3]); - void BinvEuler212(double *q, double B[3][3]); - void BinvEuler213(double *q, double B[3][3]); - void BinvEuler231(double *q, double B[3][3]); - void BinvEuler232(double *q, double B[3][3]); - void BinvEuler312(double *q, double B[3][3]); - void BinvEuler313(double *q, double B[3][3]); - void BinvEuler321(double *q, double B[3][3]); - void BinvEuler323(double *q, double B[3][3]); - void BinvGibbs(double *q, double B[3][3]); - void BinvMRP(double *q, double B[3][3]); - void BinvPRV(double *q, double B[3][3]); - void BmatEP(double *q, double B[4][3]); - void BmatEuler121(double *q, double B[3][3]); - void BmatEuler131(double *q, double B[3][3]); - void BmatEuler123(double *q, double B[3][3]); - void BmatEuler132(double *q, double B[3][3]); - void BmatEuler212(double *q, double B[3][3]); - void BmatEuler213(double *q, double B[3][3]); - void BmatEuler231(double *q, double B[3][3]); - void BmatEuler232(double *q, double B[3][3]); - void BmatEuler312(double *q, double B[3][3]); - void BmatEuler313(double *q, double B[3][3]); - void BmatEuler321(double *q, double B[3][3]); - void BmatEuler323(double *q, double B[3][3]); - void BmatGibbs(double *q, double B[3][3]); - void BmatMRP(double *q, double B[3][3]); - void BdotmatMRP(double *q, double *dq, double B[3][3]); - void BmatPRV(double *q, double B[3][3]); - void C2EP(double C[3][3], double b[4]); - void C2Euler121(double C[3][3], double *q); - void C2Euler123(double C[3][3], double *q); - void C2Euler131(double C[3][3], double *q); - void C2Euler132(double C[3][3], double *q); - void C2Euler212(double C[3][3], double *q); - void C2Euler213(double C[3][3], double *q); - void C2Euler231(double C[3][3], double *q); - void C2Euler232(double C[3][3], double *q); - void C2Euler312(double C[3][3], double *q); - void C2Euler313(double C[3][3], double *q); - void C2Euler321(double C[3][3], double *q); - void C2Euler323(double C[3][3], double *q); - void C2Gibbs(double C[3][3], double *q); - void C2MRP(double C[3][3], double *q); - void C2PRV(double C[3][3], double *q); - void dEP(double *q, double *w, double *dq); - void dEuler121(double *q, double *w, double *dq); - void dEuler123(double *q, double *w, double *dq); - void dEuler131(double *q, double *w, double *dq); - void dEuler132(double *q, double *w, double *dq); - void dEuler212(double *q, double *w, double *dq); - void dEuler213(double *q, double *w, double *dq); - void dEuler231(double *q, double *w, double *dq); - void dEuler232(double *q, double *w, double *dq); - void dEuler312(double *q, double *w, double *dq); - void dEuler313(double *q, double *w, double *dq); - void dEuler321(double *q, double *w, double *dq); - void dEuler323(double *q, double *w, double *dq); - void dGibbs(double *q, double *w, double *dq); - void dMRP(double *q, double *w, double *dq); - void dMRP2Omega(double *q, double *dq, double *w); - void ddMRP(double *q, double *dq, double *w, double *dw, double *ddq); - void ddMRP2dOmega(double *q, double *dq, double *ddq, double *dw); - void dPRV(double *q, double *w, double *dq); - void elem2PRV(double *r, double *q); - void EP2C(double *q, double C[3][3]); - void EP2Euler121(double *q, double *e); - void EP2Euler123(double *q, double *e); - void EP2Euler131(double *q, double *e); - void EP2Euler132(double *q, double *e); - void EP2Euler212(double *q, double *e); - void EP2Euler213(double *q, double *e); - void EP2Euler231(double *q, double *e); - void EP2Euler232(double *q, double *e); - void EP2Euler312(double *q, double *e); - void EP2Euler313(double *q, double *e); - void EP2Euler321(double *q, double *e); - void EP2Euler323(double *q, double *e); - void EP2Gibbs(double *q1, double *q); - void EP2MRP(double *q1, double *q); - void EP2PRV(double *q1, double *q); - void Euler1(double x, double m[3][3]); - void Euler2(double x, double m[3][3]); - void Euler3(double x, double m[3][3]); - void Euler1212C(double *q, double C[3][3]); - void Euler1212EP(double *e, double *q); - void Euler1212Gibbs(double *e, double *q); - void Euler1212MRP(double *e, double *q); - void Euler1212PRV(double *e, double *q); - void Euler1232C(double *q, double C[3][3]); - void Euler1232EP(double *e, double *q); - void Euler1232Gibbs(double *e, double *q); - void Euler1232MRP(double *e, double *q); - void Euler1232PRV(double *e, double *q); - void Euler1312C(double *q, double C[3][3]); - void Euler1312EP(double *e, double *q); - void Euler1312Gibbs(double *e, double *q); - void Euler1312MRP(double *e, double *q); - void Euler1312PRV(double *e, double *q); - void Euler1322C(double *q, double C[3][3]); - void Euler1322EP(double *e, double *q); - void Euler1322Gibbs(double *e, double *q); - void Euler1322MRP(double *e, double *q); - void Euler1322PRV(double *e, double *q); - void Euler2122C(double *q, double C[3][3]); - void Euler2122EP(double *e, double *q); - void Euler2122Gibbs(double *e, double *q); - void Euler2122MRP(double *e, double *q); - void Euler2122PRV(double *e, double *q); - void Euler2132C(double *q, double C[3][3]); - void Euler2132EP(double *e, double *q); - void Euler2132Gibbs(double *e, double *q); - void Euler2132MRP(double *e, double *q); - void Euler2132PRV(double *e, double *q); - void Euler2312C(double *q, double C[3][3]); - void Euler2312EP(double *e, double *q); - void Euler2312Gibbs(double *e, double *q); - void Euler2312MRP(double *e, double *q); - void Euler2312PRV(double *e, double *q); - void Euler2322C(double *q, double C[3][3]); - void Euler2322EP(double *e, double *q); - void Euler2322Gibbs(double *e, double *q); - void Euler2322MRP(double *e, double *q); - void Euler2322PRV(double *e, double *q); - void Euler3122C(double *q, double C[3][3]); - void Euler3122EP(double *e, double *q); - void Euler3122Gibbs(double *e, double *q); - void Euler3122MRP(double *e, double *q); - void Euler3122PRV(double *e, double *q); - void Euler3132C(double *q, double C[3][3]); - void Euler3132EP(double *e, double *q); - void Euler3132Gibbs(double *e, double *q); - void Euler3132MRP(double *e, double *q); - void Euler3132PRV(double *e, double *q); - void Euler3212C(double *q, double C[3][3]); - void Euler3212EP(double *e, double *q); - void Euler3212Gibbs(double *e, double *q); - void Euler3212MRP(double *e, double *q); - void Euler3212PRV(double *e, double *q); - void Euler3232C(double *q, double C[3][3]); - void Euler3232EP(double *e, double *q); - void Euler3232Gibbs(double *e, double *q); - void Euler3232MRP(double *e, double *q); - void Euler3232PRV(double *e, double *q); - void Gibbs2C(double *q, double C[3][3]); - void Gibbs2EP(double *q1, double *q); - void Gibbs2Euler121(double *q, double *e); - void Gibbs2Euler123(double *q, double *e); - void Gibbs2Euler131(double *q, double *e); - void Gibbs2Euler132(double *q, double *e); - void Gibbs2Euler212(double *q, double *e); - void Gibbs2Euler213(double *q, double *e); - void Gibbs2Euler231(double *q, double *e); - void Gibbs2Euler232(double *q, double *e); - void Gibbs2Euler312(double *q, double *e); - void Gibbs2Euler313(double *q, double *e); - void Gibbs2Euler321(double *q, double *e); - void Gibbs2Euler323(double *q, double *e); - void Gibbs2MRP(double *q1, double *q); - void Gibbs2PRV(double *q1, double *q); - void MRP2C(double *q, double C[3][3]); - void MRP2EP(double *q1, double *q); - void MRP2Euler121(double *q, double *e); - void MRP2Euler123(double *q, double *e); - void MRP2Euler131(double *q, double *e); - void MRP2Euler132(double *q, double *e); - void MRP2Euler212(double *q, double *e); - void MRP2Euler213(double *q, double *e); - void MRP2Euler231(double *q, double *e); - void MRP2Euler232(double *q, double *e); - void MRP2Euler312(double *q, double *e); - void MRP2Euler313(double *q, double *e); - void MRP2Euler321(double *q, double *e); - void MRP2Euler323(double *q, double *e); - void MRP2Gibbs(double *q1, double *q); - void MRP2PRV(double *q1, double *q); - void MRPswitch(double *q, double s2, double *s); - void MRPshadow(double *qIn, double *qOut); + void addEP(double* b1, double* b2, double* result); + void addEuler121(double* e1, double* e2, double* result); + void addEuler123(double* e1, double* e2, double* result); + void addEuler131(double* e1, double* e2, double* result); + void addEuler132(double* e1, double* e2, double* result); + void addEuler212(double* e1, double* e2, double* result); + void addEuler213(double* e1, double* e2, double* result); + void addEuler231(double* e1, double* e2, double* result); + void addEuler232(double* e1, double* e2, double* result); + void addEuler312(double* e1, double* e2, double* result); + void addEuler313(double* e1, double* e2, double* result); + void addEuler321(double* e1, double* e2, double* result); + void addEuler323(double* e1, double* e2, double* result); + void addGibbs(double* q1, double* q2, double* result); + void addMRP(double* q1, double* q2, double* result); + void addPRV(double* q1, double* q2, double* result); + void BinvEP(double* q, double B[3][4]); + void BinvEuler121(double* q, double B[3][3]); + void BinvEuler123(double* q, double B[3][3]); + void BinvEuler131(double* q, double B[3][3]); + void BinvEuler132(double* q, double B[3][3]); + void BinvEuler212(double* q, double B[3][3]); + void BinvEuler213(double* q, double B[3][3]); + void BinvEuler231(double* q, double B[3][3]); + void BinvEuler232(double* q, double B[3][3]); + void BinvEuler312(double* q, double B[3][3]); + void BinvEuler313(double* q, double B[3][3]); + void BinvEuler321(double* q, double B[3][3]); + void BinvEuler323(double* q, double B[3][3]); + void BinvGibbs(double* q, double B[3][3]); + void BinvMRP(double* q, double B[3][3]); + void BinvPRV(double* q, double B[3][3]); + void BmatEP(double* q, double B[4][3]); + void BmatEuler121(double* q, double B[3][3]); + void BmatEuler131(double* q, double B[3][3]); + void BmatEuler123(double* q, double B[3][3]); + void BmatEuler132(double* q, double B[3][3]); + void BmatEuler212(double* q, double B[3][3]); + void BmatEuler213(double* q, double B[3][3]); + void BmatEuler231(double* q, double B[3][3]); + void BmatEuler232(double* q, double B[3][3]); + void BmatEuler312(double* q, double B[3][3]); + void BmatEuler313(double* q, double B[3][3]); + void BmatEuler321(double* q, double B[3][3]); + void BmatEuler323(double* q, double B[3][3]); + void BmatGibbs(double* q, double B[3][3]); + void BmatMRP(double* q, double B[3][3]); + void BdotmatMRP(double* q, double* dq, double B[3][3]); + void BmatPRV(double* q, double B[3][3]); + void C2EP(double C[3][3], double b[4]); + void C2Euler121(double C[3][3], double* q); + void C2Euler123(double C[3][3], double* q); + void C2Euler131(double C[3][3], double* q); + void C2Euler132(double C[3][3], double* q); + void C2Euler212(double C[3][3], double* q); + void C2Euler213(double C[3][3], double* q); + void C2Euler231(double C[3][3], double* q); + void C2Euler232(double C[3][3], double* q); + void C2Euler312(double C[3][3], double* q); + void C2Euler313(double C[3][3], double* q); + void C2Euler321(double C[3][3], double* q); + void C2Euler323(double C[3][3], double* q); + void C2Gibbs(double C[3][3], double* q); + void C2MRP(double C[3][3], double* q); + void C2PRV(double C[3][3], double* q); + void dEP(double* q, double* w, double* dq); + void dEuler121(double* q, double* w, double* dq); + void dEuler123(double* q, double* w, double* dq); + void dEuler131(double* q, double* w, double* dq); + void dEuler132(double* q, double* w, double* dq); + void dEuler212(double* q, double* w, double* dq); + void dEuler213(double* q, double* w, double* dq); + void dEuler231(double* q, double* w, double* dq); + void dEuler232(double* q, double* w, double* dq); + void dEuler312(double* q, double* w, double* dq); + void dEuler313(double* q, double* w, double* dq); + void dEuler321(double* q, double* w, double* dq); + void dEuler323(double* q, double* w, double* dq); + void dGibbs(double* q, double* w, double* dq); + void dMRP(double* q, double* w, double* dq); + void dMRP2Omega(double* q, double* dq, double* w); + void ddMRP(double* q, double* dq, double* w, double* dw, double* ddq); + void ddMRP2dOmega(double* q, double* dq, double* ddq, double* dw); + void dPRV(double* q, double* w, double* dq); + void elem2PRV(double* r, double* q); + void EP2C(double* q, double C[3][3]); + void EP2Euler121(double* q, double* e); + void EP2Euler123(double* q, double* e); + void EP2Euler131(double* q, double* e); + void EP2Euler132(double* q, double* e); + void EP2Euler212(double* q, double* e); + void EP2Euler213(double* q, double* e); + void EP2Euler231(double* q, double* e); + void EP2Euler232(double* q, double* e); + void EP2Euler312(double* q, double* e); + void EP2Euler313(double* q, double* e); + void EP2Euler321(double* q, double* e); + void EP2Euler323(double* q, double* e); + void EP2Gibbs(double* q1, double* q); + void EP2MRP(double* q1, double* q); + void EP2PRV(double* q1, double* q); + void Euler1(double x, double m[3][3]); + void Euler2(double x, double m[3][3]); + void Euler3(double x, double m[3][3]); + void Euler1212C(double* q, double C[3][3]); + void Euler1212EP(double* e, double* q); + void Euler1212Gibbs(double* e, double* q); + void Euler1212MRP(double* e, double* q); + void Euler1212PRV(double* e, double* q); + void Euler1232C(double* q, double C[3][3]); + void Euler1232EP(double* e, double* q); + void Euler1232Gibbs(double* e, double* q); + void Euler1232MRP(double* e, double* q); + void Euler1232PRV(double* e, double* q); + void Euler1312C(double* q, double C[3][3]); + void Euler1312EP(double* e, double* q); + void Euler1312Gibbs(double* e, double* q); + void Euler1312MRP(double* e, double* q); + void Euler1312PRV(double* e, double* q); + void Euler1322C(double* q, double C[3][3]); + void Euler1322EP(double* e, double* q); + void Euler1322Gibbs(double* e, double* q); + void Euler1322MRP(double* e, double* q); + void Euler1322PRV(double* e, double* q); + void Euler2122C(double* q, double C[3][3]); + void Euler2122EP(double* e, double* q); + void Euler2122Gibbs(double* e, double* q); + void Euler2122MRP(double* e, double* q); + void Euler2122PRV(double* e, double* q); + void Euler2132C(double* q, double C[3][3]); + void Euler2132EP(double* e, double* q); + void Euler2132Gibbs(double* e, double* q); + void Euler2132MRP(double* e, double* q); + void Euler2132PRV(double* e, double* q); + void Euler2312C(double* q, double C[3][3]); + void Euler2312EP(double* e, double* q); + void Euler2312Gibbs(double* e, double* q); + void Euler2312MRP(double* e, double* q); + void Euler2312PRV(double* e, double* q); + void Euler2322C(double* q, double C[3][3]); + void Euler2322EP(double* e, double* q); + void Euler2322Gibbs(double* e, double* q); + void Euler2322MRP(double* e, double* q); + void Euler2322PRV(double* e, double* q); + void Euler3122C(double* q, double C[3][3]); + void Euler3122EP(double* e, double* q); + void Euler3122Gibbs(double* e, double* q); + void Euler3122MRP(double* e, double* q); + void Euler3122PRV(double* e, double* q); + void Euler3132C(double* q, double C[3][3]); + void Euler3132EP(double* e, double* q); + void Euler3132Gibbs(double* e, double* q); + void Euler3132MRP(double* e, double* q); + void Euler3132PRV(double* e, double* q); + void Euler3212C(double* q, double C[3][3]); + void Euler3212EP(double* e, double* q); + void Euler3212Gibbs(double* e, double* q); + void Euler3212MRP(double* e, double* q); + void Euler3212PRV(double* e, double* q); + void Euler3232C(double* q, double C[3][3]); + void Euler3232EP(double* e, double* q); + void Euler3232Gibbs(double* e, double* q); + void Euler3232MRP(double* e, double* q); + void Euler3232PRV(double* e, double* q); + void Gibbs2C(double* q, double C[3][3]); + void Gibbs2EP(double* q1, double* q); + void Gibbs2Euler121(double* q, double* e); + void Gibbs2Euler123(double* q, double* e); + void Gibbs2Euler131(double* q, double* e); + void Gibbs2Euler132(double* q, double* e); + void Gibbs2Euler212(double* q, double* e); + void Gibbs2Euler213(double* q, double* e); + void Gibbs2Euler231(double* q, double* e); + void Gibbs2Euler232(double* q, double* e); + void Gibbs2Euler312(double* q, double* e); + void Gibbs2Euler313(double* q, double* e); + void Gibbs2Euler321(double* q, double* e); + void Gibbs2Euler323(double* q, double* e); + void Gibbs2MRP(double* q1, double* q); + void Gibbs2PRV(double* q1, double* q); + void MRP2C(double* q, double C[3][3]); + void MRP2EP(double* q1, double* q); + void MRP2Euler121(double* q, double* e); + void MRP2Euler123(double* q, double* e); + void MRP2Euler131(double* q, double* e); + void MRP2Euler132(double* q, double* e); + void MRP2Euler212(double* q, double* e); + void MRP2Euler213(double* q, double* e); + void MRP2Euler231(double* q, double* e); + void MRP2Euler232(double* q, double* e); + void MRP2Euler312(double* q, double* e); + void MRP2Euler313(double* q, double* e); + void MRP2Euler321(double* q, double* e); + void MRP2Euler323(double* q, double* e); + void MRP2Gibbs(double* q1, double* q); + void MRP2PRV(double* q1, double* q); + void MRPswitch(double* q, double s2, double* s); + void MRPshadow(double* qIn, double* qOut); double wrapToPi(double x); - void PRV2C(double *q, double C[3][3]); - void PRV2elem(double *r, double *q); - void PRV2EP(double *q0, double *q); - void PRV2Euler121(double *q, double *e); - void PRV2Euler123(double *q, double *e); - void PRV2Euler131(double *q, double *e); - void PRV2Euler132(double *q, double *e); - void PRV2Euler212(double *q, double *e); - void PRV2Euler213(double *q, double *e); - void PRV2Euler231(double *q, double *e); - void PRV2Euler232(double *q, double *e); - void PRV2Euler312(double *q, double *e); - void PRV2Euler313(double *q, double *e); - void PRV2Euler321(double *q, double *e); - void PRV2Euler323(double *q, double *e); - void PRV2Gibbs(double *q0, double *q); - void PRV2MRP(double *q0, double *q); - void subEP(double *b1, double *b2, double *q); - void subEuler121(double *e, double *e1, double *e2); - void subEuler123(double *e, double *e1, double *e2); - void subEuler131(double *e, double *e1, double *e2); - void subEuler132(double *e, double *e1, double *e2); - void subEuler212(double *e, double *e1, double *e2); - void subEuler213(double *e, double *e1, double *e2); - void subEuler231(double *e, double *e1, double *e2); - void subEuler232(double *e, double *e1, double *e2); - void subEuler312(double *e, double *e1, double *e2); - void subEuler313(double *e, double *e1, double *e2); - void subEuler321(double *e, double *e1, double *e2); - void subEuler323(double *e, double *e1, double *e2); - void subGibbs(double *q1, double *q2, double *q); - void subMRP(double *q1, double *q2, double *q); - 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]); - + void PRV2C(double* q, double C[3][3]); + void PRV2elem(double* r, double* q); + void PRV2EP(double* q0, double* q); + void PRV2Euler121(double* q, double* e); + void PRV2Euler123(double* q, double* e); + void PRV2Euler131(double* q, double* e); + void PRV2Euler132(double* q, double* e); + void PRV2Euler212(double* q, double* e); + void PRV2Euler213(double* q, double* e); + void PRV2Euler231(double* q, double* e); + void PRV2Euler232(double* q, double* e); + void PRV2Euler312(double* q, double* e); + void PRV2Euler313(double* q, double* e); + void PRV2Euler321(double* q, double* e); + void PRV2Euler323(double* q, double* e); + void PRV2Gibbs(double* q0, double* q); + void PRV2MRP(double* q0, double* q); + void subEP(double* b1, double* b2, double* q); + void subEuler121(double* e, double* e1, double* e2); + void subEuler123(double* e, double* e1, double* e2); + void subEuler131(double* e, double* e1, double* e2); + void subEuler132(double* e, double* e1, double* e2); + void subEuler212(double* e, double* e1, double* e2); + void subEuler213(double* e, double* e1, double* e2); + void subEuler231(double* e, double* e1, double* e2); + void subEuler232(double* e, double* e1, double* e2); + void subEuler312(double* e, double* e1, double* e2); + void subEuler313(double* e, double* e1, double* e2); + void subEuler321(double* e, double* e1, double* e2); + void subEuler323(double* e, double* e1, double* e2); + void subGibbs(double* q1, double* q2, double* q); + void subMRP(double* q1, double* q2, double* q); + 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.cpp b/src/architecture/utilities/saturate.cpp index c39c0e071f..d40f93cd10 100644 --- a/src/architecture/utilities/saturate.cpp +++ b/src/architecture/utilities/saturate.cpp @@ -22,18 +22,16 @@ #include /*! The constructor initialies the random number generator used for the walks*/ -Saturate::Saturate() -{ -} +Saturate::Saturate() {} -Saturate::Saturate(int64_t size) : Saturate() { +Saturate::Saturate(int64_t size) + : Saturate() +{ this->numStates = size; this->stateBounds.resize(numStates, 2); } /*! The destructor is a placeholder for one that might do something*/ -Saturate::~Saturate() -{ -} +Saturate::~Saturate() {} /*! @brief This method should be used as the standard way to saturate an output. It will also be utilized by @@ -41,22 +39,24 @@ other utilities @param unsaturatedStates a vector of the unsaturated states @return saturatedStates */ -Eigen::VectorXd Saturate::saturate(Eigen::VectorXd unsaturatedStates) +Eigen::VectorXd +Saturate::saturate(Eigen::VectorXd unsaturatedStates) { Eigen::VectorXd workingStates; workingStates.resize(this->numStates); - for (int64_t i = 0; i < this->numStates; i++){ - workingStates[(int) i] = std::min(unsaturatedStates[i], this->stateBounds(i, 1)); - workingStates[(int) i] = std::max(workingStates[i], this->stateBounds(i, 0)); + for (int64_t i = 0; i < this->numStates; i++) { + workingStates[(int)i] = std::min(unsaturatedStates[i], this->stateBounds(i, 1)); + workingStates[(int)i] = std::max(workingStates[i], this->stateBounds(i, 0)); } return workingStates; - } /*! @brief sets upper and lower bounds for each state @param bounds one row for each state. lower bounds in left column, upper in right column */ -void Saturate::setBounds(Eigen::MatrixXd bounds) { +void +Saturate::setBounds(Eigen::MatrixXd bounds) +{ this->stateBounds = bounds; } diff --git a/src/architecture/utilities/saturate.h b/src/architecture/utilities/saturate.h index 4550aced14..9c7083ae05 100644 --- a/src/architecture/utilities/saturate.h +++ b/src/architecture/utilities/saturate.h @@ -23,26 +23,24 @@ #include #include - /*! @brief This class is used to saturate an output variable -*/ + */ class Saturate { - -public: + + 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 -}; + 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 +}; #endif /* _saturate_HH_ */ diff --git a/src/architecture/utilities/signalCondition.c b/src/architecture/utilities/signalCondition.c index 4dbaa82271..b37f54f7da 100644 --- a/src/architecture/utilities/signalCondition.c +++ b/src/architecture/utilities/signalCondition.c @@ -19,18 +19,18 @@ #include "architecture/utilities/signalCondition.h" - /*! This method applies the low-pass filter configuration to the newMeas that is passed in. The state is maintained in the LowPassFilterData structure @param lpData The configuration data and history of the LP filter @param newMeas The new measurement to take in to the filter */ -void lowPassFilterSignal(double newMeas, LowPassFilterData *lpData) +void +lowPassFilterSignal(double newMeas, LowPassFilterData* lpData) { /*! See documentation of algorithm in documentation for LP torque filter module*/ - double hOmeg = lpData->hStep*lpData->omegCutoff; - lpData->currentState = (1.0/(2.0+hOmeg)* - (lpData->currentState*(2.0-hOmeg)+hOmeg*(newMeas+lpData->currentMeas))); + double hOmeg = lpData->hStep * lpData->omegCutoff; + lpData->currentState = + (1.0 / (2.0 + hOmeg) * (lpData->currentState * (2.0 - hOmeg) + hOmeg * (newMeas + lpData->currentMeas))); lpData->currentMeas = newMeas; return; } diff --git a/src/architecture/utilities/signalCondition.h b/src/architecture/utilities/signalCondition.h index bfd348c6be..b68ed25561 100644 --- a/src/architecture/utilities/signalCondition.h +++ b/src/architecture/utilities/signalCondition.h @@ -20,21 +20,22 @@ #ifndef _SIGNAL_CONDITION_H_ #define _SIGNAL_CONDITION_H_ - /*! struct definition */ -typedef struct { - double hStep; /*!< [s] filter time step (assumed to be fixed) */ - double omegCutoff; /*!< [rad/s] Cutoff frequency for the filter */ - double currentState; /*!< [-] Current state of the filter */ - double currentMeas; /*!< [-] Current measurement that we read */ -}LowPassFilterData; +typedef struct +{ + double hStep; /*!< [s] filter time step (assumed to be fixed) */ + double omegCutoff; /*!< [rad/s] Cutoff frequency for the filter */ + double currentState; /*!< [-] Current state of the filter */ + double currentMeas; /*!< [-] Current measurement that we read */ +} LowPassFilterData; #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif - - void lowPassFilterSignal(double newMeas, LowPassFilterData *lpData); - + + 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..7369660c6d 100644 --- a/src/architecture/utilities/simDefinitions.h +++ b/src/architecture/utilities/simDefinitions.h @@ -17,13 +17,13 @@ */ -#define EPOCH_YEAR 2019 -#define EPOCH_MONTH 01 -#define EPOCH_DAY 01 -#define EPOCH_HOUR 00 -#define EPOCH_MIN 00 -#define EPOCH_SEC 0.00 +#define EPOCH_YEAR 2019 +#define EPOCH_MONTH 01 +#define EPOCH_DAY 01 +#define EPOCH_HOUR 00 +#define EPOCH_MIN 00 +#define EPOCH_SEC 0.00 -#define SIGNAL_NOMINAL 0 -#define SIGNAL_OFF 1 -#define SIGNAL_STUCK 2 \ No newline at end of file +#define SIGNAL_NOMINAL 0 +#define SIGNAL_OFF 1 +#define SIGNAL_STUCK 2 diff --git a/src/architecture/utilities/svd.c b/src/architecture/utilities/svd.c index 71d752b255..fd37f5b5b9 100644 --- a/src/architecture/utilities/svd.c +++ b/src/architecture/utilities/svd.c @@ -29,26 +29,28 @@ * Sourced from: https://gist.github.com/sasekazu/32f966816ad6d9244259. */ -#define SIGN(a,b) ((b) > 0.0 ? fabs(a) : - fabs(a)) +#define SIGN(a, b) ((b) > 0.0 ? fabs(a) : -fabs(a)) static double maxarg1, maxarg2; -#define DMAX(a,b) (maxarg1 = (a),maxarg2 = (b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2)) +#define DMAX(a, b) (maxarg1 = (a), maxarg2 = (b), (maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2)) static int iminarg1, iminarg2; -#define IMIN(a,b) (iminarg1 = (a),iminarg2 = (b),(iminarg1 < (iminarg2) ? (iminarg1) : iminarg2)) +#define IMIN(a, b) (iminarg1 = (a), iminarg2 = (b), (iminarg1 < (iminarg2) ? (iminarg1) : iminarg2)) static double sqrarg; #define SQR(a) ((sqrarg = (a)) == 0.0 ? 0.0 : sqrarg * sqrarg) // calculates sqrt( a^2 + b^2 ) -double pythag(double a, double b) { +double +pythag(double a, double b) +{ double absa, absb; absa = fabs(a); absb = fabs(b); if (absa > absb) - return (absa * sqrt(1.0 + SQR(absb/absa))); + return (absa * sqrt(1.0 + SQR(absb / absa))); else return (absb == 0.0 ? 0.0 : absb * sqrt(1.0 + SQR(absa / absb))); } @@ -60,7 +62,9 @@ double pythag(double a, double b) { returns. The diagonal matrix W is output as a vector w[dim2]. V (not V transpose) is output as the matrix V[dim2 * dim2]. */ -int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v) { +int +svdcmp(double* mx, size_t dim1, size_t dim2, double* w, double* v) +{ int flag, i, its, j, jj, k, l, nm, cm; double anorm, c, f, g, h, s, scale, x, y, z, max; double rv1[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; @@ -80,7 +84,7 @@ int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v) { s += mx[MXINDEX(dim2, k, i)] * mx[MXINDEX(dim2, k, i)]; } f = mx[MXINDEX(dim2, i, i)]; - g = -SIGN(sqrt(s),f); + g = -SIGN(sqrt(s), f); h = f * g - s; mx[MXINDEX(dim2, i, i)] = f - g; for (j = l; j < dim2; j++) { @@ -105,7 +109,7 @@ int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v) { s += mx[MXINDEX(dim2, i, k)] * mx[MXINDEX(dim2, i, k)]; } f = mx[MXINDEX(dim2, i, l)]; - g = -SIGN(sqrt(s),f); + g = -SIGN(sqrt(s), f); h = f * g - s; mx[MXINDEX(dim2, i, l)] = f - g; for (k = l; k < dim2; k++) @@ -123,7 +127,7 @@ int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v) { anorm = DMAX(anorm, (fabs(w[i]) + fabs(rv1[i]))); } - for (i = (int) dim2 - 1; i >= 0; i--) { + for (i = (int)dim2 - 1; i >= 0; i--) { if (i < dim2 - 1) { if (g) { for (j = l; j < dim2; j++) @@ -143,7 +147,7 @@ int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v) { l = i; } - for (i = IMIN((int) dim1, (int) dim2) - 1; i >= 0; i--) { + for (i = IMIN((int)dim1, (int)dim2) - 1; i >= 0; i--) { l = i + 1; g = w[i]; for (j = l; j < dim2; j++) @@ -165,7 +169,7 @@ int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v) { ++mx[MXINDEX(dim2, i, i)]; } - for (k = (int) dim2 - 1; k >= 0; k--) { + for (k = (int)dim2 - 1; k >= 0; k--) { for (its = 0; its < 30; its++) { flag = 1; for (l = k; l >= 0; l--) { @@ -217,7 +221,7 @@ int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v) { h = rv1[k]; f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y); g = pythag(f, 1.0); - f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g,f)))- h)) / x; + f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x; c = s = 1.0; for (j = l; j <= nm; j++) { i = j + 1; @@ -262,17 +266,17 @@ int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v) { } /* sort by largest singular value */ - for (i = 0; i < dim2 - 1; i++) { + for (i = 0; i < dim2 - 1; i++) { max = w[i]; cm = i; - for (j = i; j < dim2;j++) { - if (w[j]>max) { + for (j = i; j < dim2; j++) { + if (w[j] > max) { max = w[j]; cm = j; } } if (i != cm) { - for (j = 0 ; j < dim1; j++) { + for (j = 0; j < dim1; j++) { f = mx[MXINDEX(dim2, j, i)]; mx[MXINDEX(dim2, j, i)] = mx[MXINDEX(dim2, j, cm)]; mx[MXINDEX(dim2, j, cm)] = f; @@ -291,7 +295,8 @@ int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v) { return (0); } -void solveSVD(double *mx, size_t dim1, size_t dim2, double *x, double *b, double minSV) +void +solveSVD(double* mx, size_t dim1, size_t dim2, double* x, double* b, double minSV) { double mxCopy[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; double w[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; @@ -312,8 +317,7 @@ void solveSVD(double *mx, size_t dim1, size_t dim2, double *x, double *b, double svdcmp(mxCopy, dim1, dim2, w, v); // condition wInvDiag - for (j = 0; j < dim2; j++) - { + for (j = 0; j < dim2; j++) { if (w[j] >= minSV) wInvDiag[MXINDEX(dim2, j, j)] = 1.0 / w[j]; } diff --git a/src/architecture/utilities/svd.h b/src/architecture/utilities/svd.h index c1997655bb..f57670a687 100644 --- a/src/architecture/utilities/svd.h +++ b/src/architecture/utilities/svd.h @@ -23,11 +23,12 @@ #include #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif -int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v); -void solveSVD(double *u, size_t dim1, size_t dim2, double *x, double *b, double minSV); + int svdcmp(double* mx, size_t dim1, size_t dim2, double* w, double* v); + void solveSVD(double* u, size_t dim1, size_t dim2, double* x, double* b, double minSV); #ifdef __cplusplus } diff --git a/src/architecture/utilities/tests/test_avsEigenMRP.cpp b/src/architecture/utilities/tests/test_avsEigenMRP.cpp index 9ffa69ec67..06def5e4df 100644 --- a/src/architecture/utilities/tests/test_avsEigenMRP.cpp +++ b/src/architecture/utilities/tests/test_avsEigenMRP.cpp @@ -21,11 +21,10 @@ #include "architecture/utilities/avsEigenMRP.h" #include - -TEST(eigenMRP, testIdentity) { +TEST(eigenMRP, testIdentity) +{ Eigen::MRPd sigma; sigma = sigma.Identity(); EXPECT_TRUE(sigma.norm() < 1e-10); } - diff --git a/src/architecture/utilities/tests/test_avsEigenSupport.cpp b/src/architecture/utilities/tests/test_avsEigenSupport.cpp index 5851678230..bc6cb0d10e 100644 --- a/src/architecture/utilities/tests/test_avsEigenSupport.cpp +++ b/src/architecture/utilities/tests/test_avsEigenSupport.cpp @@ -22,7 +22,8 @@ #include // Test Vector3d conversions -TEST(avsEigenSupport, Vector3dConversions) { +TEST(avsEigenSupport, Vector3dConversions) +{ Eigen::Vector3d vec(1, 2, 3); double array[3]; @@ -33,11 +34,10 @@ TEST(avsEigenSupport, Vector3dConversions) { } // Test Matrix3d conversions -TEST(avsEigenSupport, Matrix3dConversions) { +TEST(avsEigenSupport, Matrix3dConversions) +{ Eigen::Matrix3d mat; - mat << 1, 2, 3, - 4, 5, 6, - 7, 8, 9; + mat << 1, 2, 3, 4, 5, 6, 7, 8, 9; double array[9]; eigenMatrix3d2CArray(mat, array); @@ -47,61 +47,59 @@ TEST(avsEigenSupport, Matrix3dConversions) { } // Test 2D array to Matrix3d conversion -TEST(avsEigenSupport, C2DArrayToMatrix3d) { - double array2d[3][3] = { - {1, 2, 3}, - {4, 5, 6}, - {7, 8, 9} - }; +TEST(avsEigenSupport, C2DArrayToMatrix3d) +{ + double array2d[3][3] = { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, 9 } }; Eigen::Matrix3d expected; - expected << 1, 2, 3, - 4, 5, 6, - 7, 8, 9; + expected << 1, 2, 3, 4, 5, 6, 7, 8, 9; Eigen::Matrix3d result = c2DArray2EigenMatrix3d(array2d); EXPECT_TRUE(expected.isApprox(result)); } // Test rotation matrices -TEST(avsEigenSupport, RotationMatrices) { +TEST(avsEigenSupport, RotationMatrices) +{ double angle = M_PI / 4.0; // 45 degrees // Test M1 (rotation about x-axis) Eigen::Matrix3d m1 = eigenM1(angle); - EXPECT_NEAR(m1(1,1), cos(angle), 1e-10); - EXPECT_NEAR(m1(1,2), sin(angle), 1e-10); - EXPECT_NEAR(m1(2,1), -sin(angle), 1e-10); + EXPECT_NEAR(m1(1, 1), cos(angle), 1e-10); + EXPECT_NEAR(m1(1, 2), sin(angle), 1e-10); + EXPECT_NEAR(m1(2, 1), -sin(angle), 1e-10); // Test M2 (rotation about y-axis) Eigen::Matrix3d m2 = eigenM2(angle); - EXPECT_NEAR(m2(0,0), cos(angle), 1e-10); - EXPECT_NEAR(m2(0,2), -sin(angle), 1e-10); - EXPECT_NEAR(m2(2,0), sin(angle), 1e-10); + EXPECT_NEAR(m2(0, 0), cos(angle), 1e-10); + EXPECT_NEAR(m2(0, 2), -sin(angle), 1e-10); + EXPECT_NEAR(m2(2, 0), sin(angle), 1e-10); // Test M3 (rotation about z-axis) Eigen::Matrix3d m3 = eigenM3(angle); - EXPECT_NEAR(m3(0,0), cos(angle), 1e-10); - EXPECT_NEAR(m3(0,1), sin(angle), 1e-10); - EXPECT_NEAR(m3(1,0), -sin(angle), 1e-10); + EXPECT_NEAR(m3(0, 0), cos(angle), 1e-10); + EXPECT_NEAR(m3(0, 1), sin(angle), 1e-10); + EXPECT_NEAR(m3(1, 0), -sin(angle), 1e-10); } // Test tilde matrix -TEST(avsEigenSupport, TildeMatrix) { +TEST(avsEigenSupport, TildeMatrix) +{ Eigen::Vector3d vec(1, 2, 3); Eigen::Matrix3d tilde = eigenTilde(vec); // Test properties of skew-symmetric matrix - EXPECT_NEAR(tilde(0,0), 0, 1e-10); - EXPECT_NEAR(tilde(1,1), 0, 1e-10); - EXPECT_NEAR(tilde(2,2), 0, 1e-10); - EXPECT_NEAR(tilde(0,1), -vec(2), 1e-10); - EXPECT_NEAR(tilde(1,2), -vec(0), 1e-10); - EXPECT_NEAR(tilde(2,0), -vec(1), 1e-10); + EXPECT_NEAR(tilde(0, 0), 0, 1e-10); + EXPECT_NEAR(tilde(1, 1), 0, 1e-10); + EXPECT_NEAR(tilde(2, 2), 0, 1e-10); + EXPECT_NEAR(tilde(0, 1), -vec(2), 1e-10); + EXPECT_NEAR(tilde(1, 2), -vec(0), 1e-10); + EXPECT_NEAR(tilde(2, 0), -vec(1), 1e-10); } // Test MRP conversions -TEST(avsEigenSupport, MRPConversions) { +TEST(avsEigenSupport, MRPConversions) +{ Eigen::Vector3d mrp(0.1, 0.2, 0.3); double array[3]; @@ -114,35 +112,35 @@ TEST(avsEigenSupport, MRPConversions) { } // Test Newton-Raphson solver -TEST(avsEigenSupport, NewtonRaphson) { +TEST(avsEigenSupport, NewtonRaphson) +{ // Test with a simple function f(x) = x^2 - 4 - auto f = [](double x) { return x*x - 4; }; - auto fPrime = [](double x) { return 2*x; }; + auto f = [](double x) { return x * x - 4; }; + auto fPrime = [](double x) { return 2 * x; }; double result = newtonRaphsonSolve(3.0, 1e-10, f, fPrime); EXPECT_NEAR(result, 2.0, 1e-10); } -TEST(avsEigenSupport, MatrixXiConversions) { +TEST(avsEigenSupport, MatrixXiConversions) +{ Eigen::MatrixXi mat(2, 3); - mat << 1, 2, 3, - 4, 5, 6; + mat << 1, 2, 3, 4, 5, 6; int array[6]; eigenMatrixXi2CArray(mat, array); // Verify the conversion - for(int i = 0; i < 6; i++) { - EXPECT_EQ(array[i], mat(i/3, i%3)); + for (int i = 0; i < 6; i++) { + EXPECT_EQ(array[i], mat(i / 3, i % 3)); } } -TEST(avsEigenSupport, DCMtoMRP) { +TEST(avsEigenSupport, DCMtoMRP) +{ // Create a simple rotation DCM (90 degrees about 3rd axis) Eigen::Matrix3d dcm; - dcm << 0, 1, 0, - -1, 0, 0, - 0, 0, 1; + dcm << 0, 1, 0, -1, 0, 0, 0, 0, 1; Eigen::MRPd mrp = eigenC2MRP(dcm); @@ -154,7 +152,8 @@ TEST(avsEigenSupport, DCMtoMRP) { EXPECT_NEAR(mrp.z(), 0.41421356, 1e-6); } -TEST(avsEigenSupport, MRPdToVector3d) { +TEST(avsEigenSupport, MRPdToVector3d) +{ Eigen::MRPd mrp(0.1, 0.2, 0.3); Eigen::Vector3d vec = eigenMRPd2Vector3d(mrp); diff --git a/src/architecture/utilities/tests/test_bilinearInterpolation.cpp b/src/architecture/utilities/tests/test_bilinearInterpolation.cpp index afbeb182f3..768f1c597f 100644 --- a/src/architecture/utilities/tests/test_bilinearInterpolation.cpp +++ b/src/architecture/utilities/tests/test_bilinearInterpolation.cpp @@ -26,7 +26,8 @@ std::default_random_engine generator(rd()); std::uniform_real_distribution valueDistribution(-10, 10); std::uniform_real_distribution boundDistribution(0, 2); -TEST(BilinearInterpolationTest, HandlesNormalInputs) { +TEST(BilinearInterpolationTest, HandlesNormalInputs) +{ double x = valueDistribution(generator); double x1 = x - boundDistribution(generator); double x2 = x + boundDistribution(generator); @@ -41,9 +42,9 @@ TEST(BilinearInterpolationTest, HandlesNormalInputs) { double z22 = valueDistribution(generator); // Bilinearly interpolate to solve for z - double z = 1 / ((x2 - x1) * (y2 - y1)) * (z11 * (x2 - x) * (y2 - y) + z21 * (x - x1) * (y2 - y) - + z12 * (x2 - x) * (y - y1) - + z22 * (x - x1) * (y - y1)); + double z = + 1 / ((x2 - x1) * (y2 - y1)) * + (z11 * (x2 - x) * (y2 - y) + z21 * (x - x1) * (y2 - y) + z12 * (x2 - x) * (y - y1) + z22 * (x - x1) * (y - y1)); EXPECT_EQ(bilinearInterpolation(x1, x2, y1, y2, z11, z12, z21, z22, x, y), z); } diff --git a/src/architecture/utilities/tests/test_discretize.cpp b/src/architecture/utilities/tests/test_discretize.cpp index ba364c6a20..367ee1bdee 100644 --- a/src/architecture/utilities/tests/test_discretize.cpp +++ b/src/architecture/utilities/tests/test_discretize.cpp @@ -21,8 +21,8 @@ #include #include - -TEST(Discretize, testRoundToZero) { +TEST(Discretize, testRoundToZero) +{ Discretize discretizor = Discretize(3); discretizor.setRoundDirection(TO_ZERO); discretizor.setCarryError(false); @@ -40,7 +40,8 @@ TEST(Discretize, testRoundToZero) { EXPECT_TRUE(states == expected); } -TEST(Discretize, testRoundFromZero) { +TEST(Discretize, testRoundFromZero) +{ Discretize discretizor = Discretize(3); discretizor.setRoundDirection(FROM_ZERO); discretizor.setCarryError(false); @@ -58,7 +59,8 @@ TEST(Discretize, testRoundFromZero) { EXPECT_TRUE(states == expected); } -TEST(Discretize, testRoundNear) { +TEST(Discretize, testRoundNear) +{ Discretize discretizor = Discretize(3); discretizor.setRoundDirection(NEAR); discretizor.setCarryError(false); @@ -76,7 +78,8 @@ TEST(Discretize, testRoundNear) { EXPECT_TRUE(states == expected); } -TEST(Discretize, testRoundToZeroCarryError) { +TEST(Discretize, testRoundToZeroCarryError) +{ Discretize discretizor = Discretize(3); discretizor.setRoundDirection(TO_ZERO); discretizor.setCarryError(true); @@ -91,7 +94,7 @@ TEST(Discretize, testRoundToZeroCarryError) { states << 0.1, 10.1, 15; Eigen::Vector3d output; - for(uint64_t i = 0; i < 2; i++){ + for (uint64_t i = 0; i < 2; i++) { output = discretizor.discretize(states); EXPECT_TRUE(output == expected); expected << 0, 10, 20; diff --git a/src/architecture/utilities/tests/test_gaussMarkov.cpp b/src/architecture/utilities/tests/test_gaussMarkov.cpp index 8b50672a8d..bb0b996e37 100644 --- a/src/architecture/utilities/tests/test_gaussMarkov.cpp +++ b/src/architecture/utilities/tests/test_gaussMarkov.cpp @@ -21,39 +21,40 @@ #include #include - -Eigen::Vector2d calculateSD(const Eigen::MatrixXd& dat, int64_t numPts) +Eigen::Vector2d +calculateSD(const Eigen::MatrixXd& dat, int64_t numPts) { // Calculate mean properly Eigen::Vector2d means = dat.rowwise().mean(); // Calculate variance using numerically stable algorithm Eigen::Vector2d variance = Eigen::Vector2d::Zero(); - for(int64_t i = 0; i < numPts; i++) { + for (int64_t i = 0; i < numPts; i++) { Eigen::Vector2d diff = dat.col(i) - means; variance += diff.cwiseProduct(diff); } - variance /= (numPts - 1); // Use n-1 for sample standard deviation + variance /= (numPts - 1); // Use n-1 for sample standard deviation return variance.cwiseSqrt(); } -TEST(GaussMarkov, stdDeviationIsExpected) { +TEST(GaussMarkov, stdDeviationIsExpected) +{ uint64_t seedIn = 1000; // Use proper Gauss-Markov propagation matrix (with decay) Eigen::Matrix2d propIn; - propIn << 0.9,0,0,0.9; // Decay factor of 0.9 + propIn << 0.9, 0, 0, 0.9; // Decay factor of 0.9 // Set noise matrix for std=1.0 in steady state // For an autoregressive process of order 1 with coefficient a, // steady state variance is: sigma^2/(1-a^2) // So for a=0.9, noise variance should be (1-0.81) = 0.19 to get std=1.0 Eigen::Matrix2d noiseMatrix; - noiseMatrix << sqrt(0.19),0,0,sqrt(0.19); + noiseMatrix << sqrt(0.19), 0, 0, sqrt(0.19); Eigen::Vector2d bounds; - bounds << 100.0, 100.0; // Large bounds to avoid affecting distribution + bounds << 100.0, 100.0; // Large bounds to avoid affecting distribution GaussMarkov errorModel = GaussMarkov(2); errorModel.setRNGSeed(seedIn); @@ -66,12 +67,12 @@ TEST(GaussMarkov, stdDeviationIsExpected) { noiseOut.resize(2, numPts); // Longer warm up period to reach steady state - for(int64_t i = 0; i < 5000; i++) { + for (int64_t i = 0; i < 5000; i++) { errorModel.computeNextState(); } // Collect samples - for(int64_t i = 0; i < numPts; i++) { + for (int64_t i = 0; i < numPts; i++) { errorModel.computeNextState(); noiseOut.block(0, i, 2, 1) = errorModel.getCurrentState(); } @@ -79,18 +80,19 @@ TEST(GaussMarkov, stdDeviationIsExpected) { Eigen::Vector2d stds = calculateSD(noiseOut, numPts); // Test with appropriate statistical tolerances - EXPECT_NEAR(stds(0), 1.0, 0.1); // Within 10% of expected std=1.0 + EXPECT_NEAR(stds(0), 1.0, 0.1); // Within 10% of expected std=1.0 EXPECT_NEAR(stds(1), 1.0, 0.1); } -TEST(GaussMarkov, meanIsZero) { +TEST(GaussMarkov, meanIsZero) +{ uint64_t seedIn = 1000; Eigen::Matrix2d propIn; - propIn << 1,0,0,1; + propIn << 1, 0, 0, 1; // Set noise matrix as sqrt of covariance for std=1.0 Eigen::Matrix2d noiseMatrix; - noiseMatrix << 1.0,0,0,1.0; // Square root of unit covariance + noiseMatrix << 1.0, 0, 0, 1.0; // Square root of unit covariance Eigen::Vector2d bounds; bounds << 1e-15, 1e-15; @@ -106,12 +108,12 @@ TEST(GaussMarkov, meanIsZero) { noiseOut.resize(2, numPts); // Warm up - for(int64_t i = 0; i < 1000; i++) { + for (int64_t i = 0; i < 1000; i++) { errorModel.computeNextState(); } // Collect samples - for(int64_t i = 0; i < numPts; i++) { + for (int64_t i = 0; i < numPts; i++) { errorModel.computeNextState(); noiseOut.block(0, i, 2, 1) = errorModel.getCurrentState(); } @@ -119,19 +121,20 @@ TEST(GaussMarkov, meanIsZero) { Eigen::Vector2d means = noiseOut.rowwise().mean(); // Test with appropriate statistical tolerances - double tol = 4.0 / sqrt(numPts); // 4-sigma confidence interval + double tol = 4.0 / sqrt(numPts); // 4-sigma confidence interval EXPECT_LT(fabs(means(0)), tol); EXPECT_LT(fabs(means(1)), tol); } -TEST(GaussMarkov, boundsAreRespected) { +TEST(GaussMarkov, boundsAreRespected) +{ uint64_t seedIn = 1500; Eigen::Matrix2d propIn; - propIn << 1,0,0,1; + propIn << 1, 0, 0, 1; // Adjust covariance matrix for std=1.0 Eigen::Matrix2d covar; - covar << 0.5,0,0,0.005; + covar << 0.5, 0, 0, 0.005; Eigen::Vector2d bounds; bounds << 10., 0.1; @@ -150,20 +153,20 @@ TEST(GaussMarkov, boundsAreRespected) { Eigen::Vector2d minOut; minOut.fill(0.0); - for(int64_t i = 0; i < numPts; i++){ + for (int64_t i = 0; i < numPts; i++) { errorModel.computeNextState(); noiseOut.block(0, i, 2, 1) = errorModel.getCurrentState(); - if (noiseOut(0,i) > maxOut(0)){ - maxOut(0) = noiseOut(0,i); + if (noiseOut(0, i) > maxOut(0)) { + maxOut(0) = noiseOut(0, i); } - if (noiseOut(0,i) < minOut(0)){ - minOut(0) = noiseOut(0,i); + if (noiseOut(0, i) < minOut(0)) { + minOut(0) = noiseOut(0, i); } - if (noiseOut(1,i) > maxOut(1)){ - maxOut(1) = noiseOut(1,i); + if (noiseOut(1, i) > maxOut(1)) { + maxOut(1) = noiseOut(1, i); } - if (noiseOut(1,i) < minOut(1)){ - minOut(1) = noiseOut(1,i); + if (noiseOut(1, i) < minOut(1)) { + minOut(1) = noiseOut(1, i); } } @@ -174,18 +177,19 @@ TEST(GaussMarkov, boundsAreRespected) { EXPECT_LT(fabs(-0.11 - minOut(1)) / -0.11, 5e-1); } -TEST(GaussMarkov, gaussianOnlyMode) { +TEST(GaussMarkov, gaussianOnlyMode) +{ uint64_t seedIn = 1000; // Setup matrices for pure Gaussian noise Eigen::Matrix2d propIn; - propIn << 0.0,0,0,0.0; // No state propagation + propIn << 0.0, 0, 0, 0.0; // No state propagation Eigen::Matrix2d noiseMatrix; - noiseMatrix << 1.0,0,0,1.0; // Unit variance + noiseMatrix << 1.0, 0, 0, 1.0; // Unit variance Eigen::Vector2d bounds; - bounds << -1.0, -1.0; // Disable bounds/random walk + bounds << -1.0, -1.0; // Disable bounds/random walk GaussMarkov errorModel = GaussMarkov(2); errorModel.setRNGSeed(seedIn); @@ -198,7 +202,7 @@ TEST(GaussMarkov, gaussianOnlyMode) { Eigen::MatrixXd samples; samples.resize(2, numPts); - for(int64_t i = 0; i < numPts; i++) { + for (int64_t i = 0; i < numPts; i++) { errorModel.computeNextState(); samples.col(i) = errorModel.getCurrentState(); } diff --git a/src/architecture/utilities/tests/test_geodeticConversion.cpp b/src/architecture/utilities/tests/test_geodeticConversion.cpp index 2b3947020c..3d7bd357d6 100644 --- a/src/architecture/utilities/tests/test_geodeticConversion.cpp +++ b/src/architecture/utilities/tests/test_geodeticConversion.cpp @@ -21,16 +21,13 @@ #include "architecture/utilities/linearAlgebra.h" #include - -TEST(GeodeticConversion, testPCI2PCPF) { +TEST(GeodeticConversion, testPCI2PCPF) +{ Eigen::Vector3d pciPosition; pciPosition << 1., 2., 3.; double J20002Pfix[3][3]; - m33Set( 1, 0, 0, - 0, 0, 1, - 0,-1, 0, - J20002Pfix); + m33Set(1, 0, 0, 0, 0, 1, 0, -1, 0, J20002Pfix); Eigen::Vector3d ans = PCI2PCPF(pciPosition, J20002Pfix); Eigen::Vector3d expected; @@ -38,15 +35,13 @@ TEST(GeodeticConversion, testPCI2PCPF) { EXPECT_TRUE(ans == expected); } -TEST(GeodeticConversion, testPCPF2PCI) { +TEST(GeodeticConversion, testPCPF2PCI) +{ Eigen::Vector3d pcpfPosition; pcpfPosition << 1., 2., 3.; double J20002Pfix[3][3]; - m33Set( 1, 0, 0, - 0, 0, 1, - 0,-1, 0, - J20002Pfix); + m33Set(1, 0, 0, 0, 0, 1, 0, -1, 0, J20002Pfix); Eigen::Vector3d ans = PCPF2PCI(pcpfPosition, J20002Pfix); Eigen::Vector3d expected; @@ -54,29 +49,30 @@ TEST(GeodeticConversion, testPCPF2PCI) { EXPECT_TRUE(ans == expected); } -TEST(GeodeticConversion, testLLA2PCPF) { +TEST(GeodeticConversion, testLLA2PCPF) +{ Eigen::Vector3d llaPosition; llaPosition << 0.6935805104270613, 1.832425562445269, 1596.668; - Eigen::Vector3d ans = LLA2PCPF(llaPosition, 6378.1363E3, 6356.7523E3); + Eigen::Vector3d ans = LLA2PCPF(llaPosition, 6378.1363E3, 6356.7523E3); Eigen::Vector3d expected; expected << -1270640.01, 4745322.63, 4056784.62; EXPECT_LT((expected - ans).norm(), 100.0); } -TEST(GeodeticConversion, testPCPF2LLA) { - -Eigen::Vector3d pcpfPosition; -pcpfPosition << -1270640.01, 4745322.63, 4056784.62; +TEST(GeodeticConversion, testPCPF2LLA) +{ -Eigen::Vector3d ans = PCPF2LLA(pcpfPosition, 6378.1363E3, 6356.7523E3); + Eigen::Vector3d pcpfPosition; + pcpfPosition << -1270640.01, 4745322.63, 4056784.62; -Eigen::Vector3d llaPosition; -llaPosition << 0.6935805104270613, 1.832425562445269, 1596.668; + Eigen::Vector3d ans = PCPF2LLA(pcpfPosition, 6378.1363E3, 6356.7523E3); -EXPECT_LT(std::abs(llaPosition[0] - ans[0]), 0.0001); -EXPECT_LT(std::abs(llaPosition[1] - ans[1]), 0.0001); -EXPECT_LT(std::abs(llaPosition[2] - ans[2]), 100); + Eigen::Vector3d llaPosition; + llaPosition << 0.6935805104270613, 1.832425562445269, 1596.668; + EXPECT_LT(std::abs(llaPosition[0] - ans[0]), 0.0001); + EXPECT_LT(std::abs(llaPosition[1] - ans[1]), 0.0001); + EXPECT_LT(std::abs(llaPosition[2] - ans[2]), 100); } diff --git a/src/architecture/utilities/tests/test_linearInterpolation.cpp b/src/architecture/utilities/tests/test_linearInterpolation.cpp index 935d5cf99a..91cac6cad1 100644 --- a/src/architecture/utilities/tests/test_linearInterpolation.cpp +++ b/src/architecture/utilities/tests/test_linearInterpolation.cpp @@ -26,7 +26,8 @@ std::default_random_engine generator(rd()); std::uniform_real_distribution valueDistribution(-10, 10); std::uniform_real_distribution boundDistribution(0, 2); -TEST(LinearInterpolationTest, HandlesNormalInputs) { +TEST(LinearInterpolationTest, HandlesNormalInputs) +{ double x = valueDistribution(generator); double x1 = x - boundDistribution(generator); double x2 = x + boundDistribution(generator); diff --git a/src/architecture/utilities/tests/test_orbitalMotion.cpp b/src/architecture/utilities/tests/test_orbitalMotion.cpp index 7a6a23a557..afc2ba6f36 100644 --- a/src/architecture/utilities/tests/test_orbitalMotion.cpp +++ b/src/architecture/utilities/tests/test_orbitalMotion.cpp @@ -24,44 +24,45 @@ #include "unitTestComparators.h" #include - const double orbitalEnvironmentAccuracy = 1e-10; const double orbitalElementsAccuracy = 1e-11; -TEST(OrbitalMotion, atmosphericDensity_200km) { +TEST(OrbitalMotion, atmosphericDensity_200km) +{ double alt = 200.0; double result = atmosphericDensity(alt); EXPECT_NEAR(result, 1.64100656241e-10, orbitalEnvironmentAccuracy); } -TEST(OrbitalMotion, atmosphericDensity_2000km) { +TEST(OrbitalMotion, atmosphericDensity_2000km) +{ double alt = 2000.0; double result = atmosphericDensity(alt); EXPECT_NEAR(result, 2.48885731828e-15, orbitalEnvironmentAccuracy); } -class DebeyeLengthTests :public ::testing::TestWithParam> {}; +class DebeyeLengthTests : public ::testing::TestWithParam> +{}; -TEST_P(DebeyeLengthTests, checksDebeyeLengthAtAltitude) { +TEST_P(DebeyeLengthTests, checksDebeyeLengthAtAltitude) +{ auto [altitude, expected] = GetParam(); EXPECT_NEAR(expected, debyeLength(altitude), orbitalEnvironmentAccuracy); } -INSTANTIATE_TEST_SUITE_P( - OrbitalMotion, - DebeyeLengthTests, - ::testing::Values( - std::make_tuple(400.0, 0.00404), - std::make_tuple(1000.0, 0.0159), - std::make_tuple(10000.0, 0.0396), - std::make_tuple(34000.0, 400.30000000000018)) - ); - -TEST(OrbitalMotion, atmosphericDrag) { +INSTANTIATE_TEST_SUITE_P(OrbitalMotion, + DebeyeLengthTests, + ::testing::Values(std::make_tuple(400.0, 0.00404), + std::make_tuple(1000.0, 0.0159), + std::make_tuple(10000.0, 0.0396), + std::make_tuple(34000.0, 400.30000000000018))); + +TEST(OrbitalMotion, atmosphericDrag) +{ double check[3]; double ans[3]; - double r[3] = {6200.0, 100.0, 2000.0}; - double v[3] = {1.0, 9.0, 1.0}; + double r[3] = { 6200.0, 100.0, 2000.0 }; + double v[3] = { 1.0, 9.0, 1.0 }; double A = 2.0; double Cd = 0.2; double m = 50.0; @@ -72,10 +73,11 @@ TEST(OrbitalMotion, atmosphericDrag) { EXPECT_TRUE(v3IsEqual(ans, check, 11)); } -TEST(OrbitalMotion, jPerturb_order_6) { +TEST(OrbitalMotion, jPerturb_order_6) +{ double check[3]; double ans[3]; - double r[3] = {6200.0, 100.0, 2000.0}; + double r[3] = { 6200.0, 100.0, 2000.0 }; int order = 6; v3Set(-7.3080959003487213e-006, -1.1787251452175358e-007, -1.1381118473672282e-005, check); @@ -83,12 +85,13 @@ TEST(OrbitalMotion, jPerturb_order_6) { EXPECT_TRUE(v3IsEqual(ans, check, 11)); } -TEST(OrbitalMotion, solarRadiationPressure) { +TEST(OrbitalMotion, solarRadiationPressure) +{ double check[3]; double ans[3]; double A = 2.0; double m = 50.0; - double r[3] = {1.0, 0.3, -0.2}; + double r[3] = { 1.0, 0.3, -0.2 }; v3Set(-1.9825487816e-10, -5.94764634479e-11, 3.96509756319e-11, check); solarRad(A, m, r, ans); @@ -109,8 +112,8 @@ TEST(OrbitalMotion, elem2rv1DEccentric) elements.omega = 113.0 * D2R; elements.f = 23.0 * D2R; elements.alpha = 1.0 / elements.a; - elements.rPeriap = elements.a*(1.-elements.e); - elements.rApoap = elements.a*(1.+elements.e); + elements.rPeriap = elements.a * (1. - elements.e); + elements.rApoap = elements.a * (1. + elements.e); elem2rv(MU_EARTH, &elements, r, v); v3Set(-148.596902253492, -457.100381534593, 352.773096481799, r2); v3Set(-8.93065944520745, -27.4716886950712, 21.2016289595043, v3_2); @@ -131,23 +134,25 @@ TEST(OrbitalMotion, elem2rv1DHyperbolic) elements.omega = 113.0 * D2R; elements.f = 23.0 * D2R; elements.alpha = 1.0 / elements.a; - elements.rPeriap = elements.a*(1.-elements.e); - elements.rApoap = elements.a*(1.+elements.e); + elements.rPeriap = elements.a * (1. - elements.e); + elements.rApoap = elements.a * (1. + elements.e); elem2rv(MU_EARTH, &elements, r, v); v3Set(-152.641873349816, -469.543156608544, 362.375968124408, r2); v3Set(-9.17378720883851, -28.2195763820421, 21.7788208976681, v3_2); EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); } -class TwoDimensionHyperbolic : public ::testing::Test { -protected: +class TwoDimensionHyperbolic : public ::testing::Test +{ + protected: double r[3]; double v[3]; double r2[3]; double v3_2[3]; ClassicElements elements; - void SetUp() override { + void SetUp() override + { elements.a = -7500.0; elements.e = 1.4; elements.i = 40.0 * D2R; @@ -155,8 +160,8 @@ class TwoDimensionHyperbolic : public ::testing::Test { elements.omega = 113.0 * D2R; elements.f = 23.0 * D2R; elements.alpha = 1.0 / elements.a; - elements.rPeriap = elements.a*(1.-elements.e); - elements.rApoap = elements.a*(1.+elements.e); + elements.rPeriap = elements.a * (1. - elements.e); + elements.rApoap = elements.a * (1. + elements.e); v3Set(319.013136281857, -2796.71958333493, 1404.6919948109, r2); v3Set(15.3433051336115, -5.87012423567412, -6.05659420479213, v3_2); @@ -169,7 +174,8 @@ TEST_F(TwoDimensionHyperbolic, elem2rv) EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); } -TEST_F(TwoDimensionHyperbolic, rv2elem) { +TEST_F(TwoDimensionHyperbolic, rv2elem) +{ rv2elem(MU_EARTH, r2, v3_2, &elements); EXPECT_PRED3(isEqualRel, elements.a, -7500.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.e, 1.4, orbitalElementsAccuracy); @@ -178,19 +184,21 @@ TEST_F(TwoDimensionHyperbolic, rv2elem) { EXPECT_PRED3(isEqualRel, elements.omega, 113.0 * D2R, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.f, 23.0 * D2R, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rmag, 3145.881340612725, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, elements.rPeriap,3000.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap, 3000.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rApoap, -18000., orbitalElementsAccuracy); } -class TwoDimensionParabolic : public ::testing::Test { -protected: +class TwoDimensionParabolic : public ::testing::Test +{ + protected: double r[3]; double v[3]; double r2[3]; double v3_2[3]; ClassicElements elements; - void SetUp() override { + void SetUp() override + { elements.alpha = 0.0; /* zero orbit energy, i.e. parabolic */ elements.a = 0.0; elements.rPeriap = 7500.; @@ -204,12 +212,14 @@ class TwoDimensionParabolic : public ::testing::Test { } }; -TEST_F(TwoDimensionParabolic, elem2rv) { +TEST_F(TwoDimensionParabolic, elem2rv) +{ elem2rv(MU_EARTH, &elements, r, v); EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); } -TEST_F(TwoDimensionParabolic, rv2elem) { +TEST_F(TwoDimensionParabolic, rv2elem) +{ rv2elem(MU_EARTH, r2, v3_2, &elements); EXPECT_NEAR(elements.alpha, 0.0, orbitalElementsAccuracy); EXPECT_NEAR(elements.a, 0.0, orbitalElementsAccuracy); @@ -219,40 +229,44 @@ TEST_F(TwoDimensionParabolic, rv2elem) { EXPECT_PRED3(isEqualRel, elements.omega, 113.0 * D2R, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.f, 123.0 * D2R, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rmag, 32940.89997480352, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, elements.rPeriap,7500.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap, 7500.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rApoap, 0.0, orbitalElementsAccuracy); } -class TwoDimensionElliptical : public ::testing::Test { -protected: +class TwoDimensionElliptical : public ::testing::Test +{ + protected: double r[3]; double v[3]; double r2[3]; double v3_2[3]; ClassicElements elements; - void SetUp() override { - elements.a = 7500.0; - elements.e = 0.5; - elements.i = 40.0 * D2R; + void SetUp() override + { + elements.a = 7500.0; + elements.e = 0.5; + elements.i = 40.0 * D2R; elements.Omega = 133.0 * D2R; elements.omega = 113.0 * D2R; - elements.f = 123.0 * D2R; /* true anomaly */ + elements.f = 123.0 * D2R; /* true anomaly */ elements.alpha = 1.0 / elements.a; - elements.rPeriap = elements.a*(1.-elements.e); - elements.rApoap = elements.a*(1.+elements.e); + elements.rPeriap = elements.a * (1. - elements.e); + elements.rApoap = elements.a * (1. + elements.e); v3Set(6538.3506963942027, 186.7227227879431, -4119.3008399778619, r2); v3Set(1.4414106130924005, 5.588901415902356, -4.0828931566657038, v3_2); } }; -TEST_F(TwoDimensionElliptical, elem2rv) { +TEST_F(TwoDimensionElliptical, elem2rv) +{ elem2rv(MU_EARTH, &elements, r, v); EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); } -TEST_F(TwoDimensionElliptical, rv2elem) { +TEST_F(TwoDimensionElliptical, rv2elem) +{ rv2elem(MU_EARTH, r2, v3_2, &elements); EXPECT_PRED3(isEqualRel, elements.a, 7500.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.e, 0.5, orbitalElementsAccuracy); @@ -265,15 +279,17 @@ TEST_F(TwoDimensionElliptical, rv2elem) { EXPECT_PRED3(isEqualRel, elements.rApoap, 11250.0, orbitalElementsAccuracy); } -class NonCircularEquitorial : public ::testing::Test { -protected: +class NonCircularEquitorial : public ::testing::Test +{ + protected: double r[3]; double v[3]; double r2[3]; double v3_2[3]; ClassicElements elements; - void SetUp() override { + void SetUp() override + { elements.a = 7500.0; elements.e = 0.5; elements.i = 0.0 * D2R; @@ -285,26 +301,29 @@ class NonCircularEquitorial : public ::testing::Test { } }; -TEST_F(NonCircularEquitorial, elem2rv) { +TEST_F(NonCircularEquitorial, elem2rv) +{ elem2rv(MU_EARTH, &elements, r, v); EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); } -TEST_F(NonCircularEquitorial, rv2elem) { +TEST_F(NonCircularEquitorial, rv2elem) +{ rv2elem(MU_EARTH, r2, v3_2, &elements); EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.e, 0.5, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.i, 0.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.Omega, 0.0, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, elements.omega, (133+113)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.omega, (133 + 113) * D2R, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.f, 123.0 * D2R, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, elements.rmag, 7730.041048693483, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, elements.rPeriap,3750.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 7730.041048693483, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap, 3750.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rApoap, 11250.0, orbitalElementsAccuracy); } -class NonCircularNearEquitorial : public ::testing::Test { -protected: +class NonCircularNearEquitorial : public ::testing::Test +{ + protected: double r[3]; double v[3]; double r2[3]; @@ -312,7 +331,8 @@ class NonCircularNearEquitorial : public ::testing::Test { ClassicElements elements; double eps2 = 1e-12 * 0.5; - void SetUp() override { + void SetUp() override + { elements.a = 7500.0; elements.e = 0.5; elements.i = eps2; @@ -324,25 +344,29 @@ class NonCircularNearEquitorial : public ::testing::Test { } }; -TEST_F(NonCircularNearEquitorial, elem2rv) { +TEST_F(NonCircularNearEquitorial, elem2rv) +{ elem2rv(MU_EARTH, &elements, r, v); EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); } -TEST_F(NonCircularNearEquitorial, rv2elem) { +TEST_F(NonCircularNearEquitorial, rv2elem) +{ rv2elem(MU_EARTH, r2, v3_2, &elements); EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.e, 0.5, orbitalElementsAccuracy); EXPECT_NEAR(elements.i, eps2, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, wrapToPi(elements.Omega+elements.omega), (133+113-360.)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3( + isEqualRel, wrapToPi(elements.Omega + elements.omega), (133 + 113 - 360.) * D2R, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.f, 123.0 * D2R, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, elements.rmag, 7730.041048693483, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, elements.rPeriap,3750.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 7730.041048693483, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap, 3750.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rApoap, 11250.0, orbitalElementsAccuracy); } -class NonCircularNearEquitorial180Degree : public ::testing::Test { -protected: +class NonCircularNearEquitorial180Degree : public ::testing::Test +{ + protected: double r[3]; double v[3]; double r2[3]; @@ -350,7 +374,8 @@ class NonCircularNearEquitorial180Degree : public ::testing::Test { ClassicElements elements; double eps2 = 1e-12 * 0.5; - void SetUp() override { + void SetUp() override + { elements.a = 7500.0; elements.e = 0.5; elements.i = eps2; @@ -362,34 +387,39 @@ class NonCircularNearEquitorial180Degree : public ::testing::Test { } }; -TEST_F(NonCircularNearEquitorial180Degree, elem2rv) { +TEST_F(NonCircularNearEquitorial180Degree, elem2rv) +{ elem2rv(MU_EARTH, &elements, r, v); EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); } -TEST_F(NonCircularNearEquitorial180Degree, rv2elem) { +TEST_F(NonCircularNearEquitorial180Degree, rv2elem) +{ rv2elem(MU_EARTH, r2, v3_2, &elements); EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.e, 0.5, orbitalElementsAccuracy); EXPECT_NEAR(elements.i, eps2, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, wrapToPi(elements.Omega+elements.omega), (133+113-360.)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3( + isEqualRel, wrapToPi(elements.Omega + elements.omega), (133 + 113 - 360.) * D2R, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.f, 123.0 * D2R, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, elements.rmag, 7730.041048693483, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, elements.rPeriap,3750.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 7730.041048693483, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap, 3750.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rApoap, 11250.0, orbitalElementsAccuracy); } -class CircularInclined : public ::testing::Test { -protected: +class CircularInclined : public ::testing::Test +{ + protected: double r[3]; double v[3]; double r2[3]; double v3_2[3]; ClassicElements elements; - void SetUp() override { + void SetUp() override + { elements.a = 7500.0; - elements.e = 0.0; + elements.e = 0.0; elements.i = 40.0 * D2R; elements.Omega = 133.0 * D2R; elements.omega = 113.0 * D2R; @@ -399,34 +429,38 @@ class CircularInclined : public ::testing::Test { } }; -TEST_F(CircularInclined, elem2rv) { +TEST_F(CircularInclined, elem2rv) +{ elem2rv(MU_EARTH, &elements, r, v); EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); } -TEST_F(CircularInclined, rv2elem) { +TEST_F(CircularInclined, rv2elem) +{ rv2elem(MU_EARTH, r2, v3_2, &elements); EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); EXPECT_NEAR(elements.e, 0.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.i, 40. * D2R, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.Omega, 133. * D2R, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, wrapToPi(elements.omega+elements.f), (113+123-360.)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, wrapToPi(elements.omega + elements.f), (113 + 123 - 360.) * D2R, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rmag, 7500.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rPeriap, 7500.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rApoap, 7500.0, orbitalElementsAccuracy); } -class CircularEquitorial : public ::testing::Test { -protected: +class CircularEquitorial : public ::testing::Test +{ + protected: double r[3]; double v[3]; double r2[3]; double v3_2[3]; ClassicElements elements; - void SetUp() override { + void SetUp() override + { elements.a = 7500.0; - elements.e = 0.0; + elements.e = 0.0; elements.i = 0.0; elements.Omega = 133.0 * D2R; elements.omega = 113.0 * D2R; @@ -436,35 +470,39 @@ class CircularEquitorial : public ::testing::Test { } }; -TEST_F(CircularEquitorial, elem2rv) { +TEST_F(CircularEquitorial, elem2rv) +{ elem2rv(MU_EARTH, &elements, r, v); EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); } -TEST_F(CircularEquitorial, rv2elem) { +TEST_F(CircularEquitorial, rv2elem) +{ rv2elem(MU_EARTH, r2, v3_2, &elements); EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); EXPECT_NEAR(elements.e, 0.0, orbitalElementsAccuracy); EXPECT_NEAR(elements.i, 0.0, orbitalElementsAccuracy); EXPECT_NEAR(elements.Omega, 0.0, orbitalElementsAccuracy); EXPECT_NEAR(elements.omega, 0.0, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, elements.f, (133+113+123-360)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.f, (133 + 113 + 123 - 360) * D2R, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rmag, 7500.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rPeriap, 7500.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rApoap, 7500.0, orbitalElementsAccuracy); } -class CircularEquitorialRetrograde : public ::testing::Test { -protected: +class CircularEquitorialRetrograde : public ::testing::Test +{ + protected: double r[3]; double v[3]; double r2[3]; double v3_2[3]; ClassicElements elements; - void SetUp() override { + void SetUp() override + { elements.a = 7500.0; - elements.e = 0.0; + elements.e = 0.0; elements.i = M_PI; elements.Omega = 133.0 * D2R; elements.omega = 113.0 * D2R; @@ -474,32 +512,35 @@ class CircularEquitorialRetrograde : public ::testing::Test { } }; -TEST_F(CircularEquitorialRetrograde, elem2rv) { +TEST_F(CircularEquitorialRetrograde, elem2rv) +{ elem2rv(MU_EARTH, &elements, r, v); EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); } -TEST_F(CircularEquitorialRetrograde, rv2elem) { +TEST_F(CircularEquitorialRetrograde, rv2elem) +{ rv2elem(MU_EARTH, r2, v3_2, &elements); EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); EXPECT_NEAR(elements.e, 0.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.i, M_PI, orbitalElementsAccuracy); EXPECT_NEAR(elements.Omega, 0.0, orbitalElementsAccuracy); EXPECT_NEAR(elements.omega, 0.0, orbitalElementsAccuracy); - EXPECT_PRED3(isEqualRel, elements.f, (-133+113+123)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.f, (-133 + 113 + 123) * D2R, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rmag, 7500.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rPeriap, 7500.0, orbitalElementsAccuracy); EXPECT_PRED3(isEqualRel, elements.rApoap, 7500.0, orbitalElementsAccuracy); } -TEST(OrbitalMotion, classicElementsToMeanElements) { +TEST(OrbitalMotion, classicElementsToMeanElements) +{ ClassicElements elements; - elements.a = 1000.0; - elements.e = 0.2; - elements.i = 0.2; + elements.a = 1000.0; + elements.e = 0.2; + elements.i = 0.2; elements.Omega = 0.15; elements.omega = 0.5; - elements.f = 0.2; + elements.f = 0.2; double req = 300.0; double J2 = 1e-3; ClassicElements elements_p; @@ -512,14 +553,15 @@ TEST(OrbitalMotion, classicElementsToMeanElements) { EXPECT_PRED3(isEqualRel, elements_p.f, 0.19982315726261962174348241205735, orbitalElementsAccuracy); } -TEST(OrbitalMotion, classicElementsToEquinoctialElements) { +TEST(OrbitalMotion, classicElementsToEquinoctialElements) +{ ClassicElements elements; - elements.a = 1000.0; - elements.e = 0.2; - elements.i = 0.2; + elements.a = 1000.0; + elements.e = 0.2; + elements.i = 0.2; elements.Omega = 0.15; elements.omega = 0.5; - elements.f = 0.2; + elements.f = 0.2; equinoctialElements elements_eq; clElem2eqElem(&elements, &elements_eq); diff --git a/src/architecture/utilities/tests/test_saturate.cpp b/src/architecture/utilities/tests/test_saturate.cpp index fd7b60a5dc..d51299dcd0 100644 --- a/src/architecture/utilities/tests/test_saturate.cpp +++ b/src/architecture/utilities/tests/test_saturate.cpp @@ -21,13 +21,13 @@ #include "architecture/utilities/linearAlgebra.h" #include - -TEST(Saturate, testSaturate) { +TEST(Saturate, testSaturate) +{ Eigen::Vector3d states; states << -555, 1.27, 5000000.; auto saturator = Saturate(3); Eigen::MatrixXd bounds; - bounds.resize(3,2); + bounds.resize(3, 2); bounds << -400., 0, 5, 10, -1, 5000001; saturator.setBounds(bounds); states = saturator.saturate(states); diff --git a/src/architecture/utilities/tests/unitTestComparators.h b/src/architecture/utilities/tests/unitTestComparators.h index 7351751f38..e068a3e355 100644 --- a/src/architecture/utilities/tests/unitTestComparators.h +++ b/src/architecture/utilities/tests/unitTestComparators.h @@ -22,20 +22,22 @@ #include -int isEqual(double a, double b, double accuracy) +int +isEqual(double a, double b, double accuracy) { - if(fabs(a - b) > accuracy) { + if (fabs(a - b) > accuracy) { return 0; } return 1; } -int isEqualRel(double a, double b, double accuracy) +int +isEqualRel(double a, double b, double accuracy) { - if(fabs(a - b)/fabs(a) > accuracy) { + if (fabs(a - b) / fabs(a) > accuracy) { return 0; } return 1; } -#endif //UNITTESTCOMPARATORS_H +#endif // UNITTESTCOMPARATORS_H diff --git a/src/architecture/utilities/ukfUtilities.c b/src/architecture/utilities/ukfUtilities.c index b5c19d1b67..2367e88027 100644 --- a/src/architecture/utilities/ukfUtilities.c +++ b/src/architecture/utilities/ukfUtilities.c @@ -22,360 +22,308 @@ #include "architecture/utilities/bsk_Print.h" #include -void ukfQRDJustR( - double *inMat, int32_t nRow, int32_t nCol, double *destMat) +void +ukfQRDJustR(double* inMat, int32_t nRow, int32_t nCol, double* destMat) { - int32_t i, j, k, dimi, dimj; - double qMat[UKF_MAX_DIM*UKF_MAX_DIM]; - double sourceMat[UKF_MAX_DIM*UKF_MAX_DIM]; - double sum; + int32_t i, j, k, dimi, dimj; + double qMat[UKF_MAX_DIM * UKF_MAX_DIM]; + double sourceMat[UKF_MAX_DIM * UKF_MAX_DIM]; + double sum; - mSetZero(qMat, UKF_MAX_DIM, UKF_MAX_DIM); - mSetZero(sourceMat, UKF_MAX_DIM, UKF_MAX_DIM); - mSetZero(destMat, (size_t) nCol, (size_t) nCol); - mCopy(inMat, (size_t) nRow, (size_t) nCol, sourceMat); + mSetZero(qMat, UKF_MAX_DIM, UKF_MAX_DIM); + mSetZero(sourceMat, UKF_MAX_DIM, UKF_MAX_DIM); + mSetZero(destMat, (size_t)nCol, (size_t)nCol); + mCopy(inMat, (size_t)nRow, (size_t)nCol, sourceMat); - for (i = 0; i= 0; i--) - { - if (sourceMat[i*mat_dim + i] == 0) - { + mSetZero(destMat, (size_t)nRow, (size_t)nCol); + if (nRow != nCol) { + BSK_PRINT(MSG_WARNING, "Can't get a lower-triangular inverse of non-square matrix in ukfLInv."); + return -1; + } + mat_dim = nRow; + for (i = mat_dim - 1; i >= 0; i--) { + if (sourceMat[i * mat_dim + i] == 0) { BSK_PRINT(MSG_WARNING, "Can't invert zero matrix."); return -1; } - destMat[mat_dim*i + i] = 1.0 / sourceMat[i*mat_dim + i]; - for (j = mat_dim - 1; j >= i + 1; j--) - { - destMat[mat_dim*j + i] = 0.0; - for (k = i + 1; k <= j; k++) - { - destMat[j*mat_dim + i] -= sourceMat[mat_dim*k + i] * - destMat[j*mat_dim + k]; - } - destMat[j*mat_dim + i] *= destMat[mat_dim*i + i]; - } - } - + destMat[mat_dim * i + i] = 1.0 / sourceMat[i * mat_dim + i]; + for (j = mat_dim - 1; j >= i + 1; j--) { + destMat[mat_dim * j + i] = 0.0; + for (k = i + 1; k <= j; k++) { + destMat[j * mat_dim + i] -= sourceMat[mat_dim * k + i] * destMat[j * mat_dim + k]; + } + destMat[j * mat_dim + i] *= destMat[mat_dim * i + i]; + } + } - return 0; + return 0; } -int32_t ukfUInv(double *sourceMat, int32_t nRow, int32_t nCol, double *destMat) +int32_t +ukfUInv(double* sourceMat, int32_t nRow, int32_t nCol, double* destMat) { - int i, j, k, mat_dim; + int i, j, k, mat_dim; - mSetZero(destMat, (size_t) nRow, (size_t) nCol); - if (nRow != nCol) - { - BSK_PRINT(MSG_WARNING, "Can't get a lower-triangular inverse of non-square matrix in ukfLInv."); - return -1; - } - mat_dim = nRow; - for (i = mat_dim - 1; i >= 0; i--) - { - if (sourceMat[i*mat_dim + i] == 0) - { + mSetZero(destMat, (size_t)nRow, (size_t)nCol); + if (nRow != nCol) { + BSK_PRINT(MSG_WARNING, "Can't get a lower-triangular inverse of non-square matrix in ukfLInv."); + return -1; + } + mat_dim = nRow; + for (i = mat_dim - 1; i >= 0; i--) { + if (sourceMat[i * mat_dim + i] == 0) { BSK_PRINT(MSG_WARNING, "Can't invert zero matrix."); return -1; } - destMat[mat_dim*i + i] = 1.0 / sourceMat[i*mat_dim + i]; - for (j = mat_dim - 1; j >= i + 1; j--) - { - destMat[mat_dim*i + j] = 0.0; - for (k = i + 1; k <= j; k++) - { - destMat[i*mat_dim + j] -= sourceMat[mat_dim*i + k] * - destMat[k*mat_dim + j]; - } - destMat[i*mat_dim + j] *= destMat[mat_dim*i + i]; - } - } - return 0; + destMat[mat_dim * i + i] = 1.0 / sourceMat[i * mat_dim + i]; + for (j = mat_dim - 1; j >= i + 1; j--) { + destMat[mat_dim * i + j] = 0.0; + for (k = i + 1; k <= j; k++) { + destMat[i * mat_dim + j] -= sourceMat[mat_dim * i + k] * destMat[k * mat_dim + j]; + } + destMat[i * mat_dim + j] *= destMat[mat_dim * i + i]; + } + } + return 0; } -int32_t ukfLUD(double *sourceMat, int32_t nRow, int32_t nCol, - double *destMat, int32_t *indx) +int32_t +ukfLUD(double* sourceMat, int32_t nRow, int32_t nCol, double* destMat, int32_t* indx) { - double vv[UKF_MAX_DIM]; - int32_t rowIndicator, i, j, k, imax; - double big, dum, sum, temp; - double TINY = 1.0E-14; + double vv[UKF_MAX_DIM]; + int32_t rowIndicator, i, j, k, imax; + double big, dum, sum, temp; + double TINY = 1.0E-14; - mSetZero(destMat, (size_t) nRow, (size_t) nCol); - for(i=0; i big ? temp : big; - } - if (big < TINY) - { - BSK_PRINT(MSG_WARNING, "Singlular matrix encountered in ukfLUD LU decomposition."); - return -1; - } - vv[i] = 1.0 / big; - } - for (j = 0; j < nRow; j++) - { - for (i = 0; i < j; i++) - { - sum = destMat[i*nRow + j]; - for (k = 0; k < i; k++) - { - sum -= destMat[i*nRow + k] * destMat[k*nRow + j]; - } - destMat[i*nRow + j] = sum; - } - big = 0.0; - imax = j; - for (i = j; i < nRow; i++) - { - sum = destMat[i*nRow + j]; - for (k = 0; k < j; k++) - { - sum -= destMat[i*nRow + k] * destMat[k*nRow + j]; - } - destMat[i*nRow + j] = sum; - dum = vv[i] * fabs(sum); - if (dum >= big) - { - big = dum; - imax = i; - } - } - if (j != imax) - { - for (k = 0; k < nRow; k++) - { - dum = destMat[imax*nRow + k]; - destMat[imax*nRow + k] = destMat[j*nRow + k]; - destMat[j*nRow + k] = dum; - } - rowIndicator += 1; - vv[imax] = vv[j]; - } - indx[j] = imax; - if (destMat[j*nRow + j] == 0.0) - { - destMat[j*nRow + j] = TINY; - } - if (j != nRow-1) - { - dum = 1.0 / destMat[j*nRow + j]; - for (i = j + 1; i < nRow; i++) - { - destMat[i*nRow + j] *= dum; - } - } - } - return 0; + mCopy(sourceMat, (size_t)nRow, (size_t)nCol, destMat); + vSetZero(vv, (size_t)nRow); + rowIndicator = 1; + for (i = 0; i < nRow; i++) { + big = 0.0; + for (j = 0; j < nRow; j++) { + temp = fabs(destMat[i * nRow + j]); + big = temp > big ? temp : big; + } + if (big < TINY) { + BSK_PRINT(MSG_WARNING, "Singlular matrix encountered in ukfLUD LU decomposition."); + return -1; + } + vv[i] = 1.0 / big; + } + for (j = 0; j < nRow; j++) { + for (i = 0; i < j; i++) { + sum = destMat[i * nRow + j]; + for (k = 0; k < i; k++) { + sum -= destMat[i * nRow + k] * destMat[k * nRow + j]; + } + destMat[i * nRow + j] = sum; + } + big = 0.0; + imax = j; + for (i = j; i < nRow; i++) { + sum = destMat[i * nRow + j]; + for (k = 0; k < j; k++) { + sum -= destMat[i * nRow + k] * destMat[k * nRow + j]; + } + destMat[i * nRow + j] = sum; + dum = vv[i] * fabs(sum); + if (dum >= big) { + big = dum; + imax = i; + } + } + if (j != imax) { + for (k = 0; k < nRow; k++) { + dum = destMat[imax * nRow + k]; + destMat[imax * nRow + k] = destMat[j * nRow + k]; + destMat[j * nRow + k] = dum; + } + rowIndicator += 1; + vv[imax] = vv[j]; + } + indx[j] = imax; + if (destMat[j * nRow + j] == 0.0) { + destMat[j * nRow + j] = TINY; + } + if (j != nRow - 1) { + dum = 1.0 / destMat[j * nRow + j]; + for (i = j + 1; i < nRow; i++) { + destMat[i * nRow + j] *= dum; + } + } + } + return 0; } -int32_t ukfLUBckSlv(double *sourceMat, int32_t nRow, int32_t nCol, - int32_t *indx, double *bmat, double *destMat) +int32_t +ukfLUBckSlv(double* sourceMat, int32_t nRow, int32_t nCol, int32_t* indx, double* bmat, double* destMat) { - int i, ip, j; - int ii = -1; - double sum; + int i, ip, j; + int ii = -1; + double sum; - vSetZero(destMat, (size_t) nRow); - if (nRow != nCol) - { - BSK_PRINT(MSG_WARNING, "Can't get a linear solution of non-square matrix in ukfLUBckSlv."); - return -1; - } - vCopy(bmat, (size_t) nRow, destMat); + vSetZero(destMat, (size_t)nRow); + if (nRow != nCol) { + BSK_PRINT(MSG_WARNING, "Can't get a linear solution of non-square matrix in ukfLUBckSlv."); + return -1; + } + vCopy(bmat, (size_t)nRow, destMat); - for (i = 0; i < nRow; i++) - { - ip = indx[i]; - sum = destMat[ip]; - destMat[ip] = destMat[i]; - if (ii >= 0) - { - for (j = ii; j <= i-1; j++) - { - sum -= sourceMat[i*nRow + j] * destMat[j]; - } - } - else if ((int) sum) - { - ii = i; - } - destMat[i] = sum; - } - for (i = nRow - 1; i >= 0; i--) - { - sum = destMat[i]; - for (j = i + 1; j < nRow; j++) - { - sum -= sourceMat[i*nRow + j] * destMat[j]; - } - if (sourceMat[i*nRow + i] == 0){ + for (i = 0; i < nRow; i++) { + ip = indx[i]; + sum = destMat[ip]; + destMat[ip] = destMat[i]; + if (ii >= 0) { + for (j = ii; j <= i - 1; j++) { + sum -= sourceMat[i * nRow + j] * destMat[j]; + } + } else if ((int)sum) { + ii = i; + } + destMat[i] = sum; + } + for (i = nRow - 1; i >= 0; i--) { + sum = destMat[i]; + for (j = i + 1; j < nRow; j++) { + sum -= sourceMat[i * nRow + j] * destMat[j]; + } + if (sourceMat[i * nRow + i] == 0) { BSK_PRINT(MSG_WARNING, "Dividing by zero."); - return -1;} - destMat[i] = sum / sourceMat[i*nRow + i]; - } + return -1; + } + destMat[i] = sum / sourceMat[i * nRow + i]; + } return 0; } -int32_t ukfMatInv(double *sourceMat, int32_t nRow, int32_t nCol, - double *destMat) +int32_t +ukfMatInv(double* sourceMat, int32_t nRow, int32_t nCol, double* destMat) { - double LUMatrix[UKF_MAX_DIM*UKF_MAX_DIM]; - double invCol[UKF_MAX_DIM]; - double colSolve[UKF_MAX_DIM]; - int indx[UKF_MAX_DIM]; - int32_t i, j, badCall; + double LUMatrix[UKF_MAX_DIM * UKF_MAX_DIM]; + double invCol[UKF_MAX_DIM]; + double colSolve[UKF_MAX_DIM]; + int indx[UKF_MAX_DIM]; + int32_t i, j, badCall; - mSetZero(destMat, (size_t) nRow, (size_t) nCol); - if (nRow != nCol) - { - BSK_PRINT(MSG_WARNING, "Can't invert a non-square matrix in ukfMatInv."); - return -1; - } - ukfLUD(sourceMat, nRow, nCol, LUMatrix, indx); - for (j = 0; j < nRow; j++) - { - vSetZero(colSolve, (size_t) nRow); - colSolve[j] = 1.0; + mSetZero(destMat, (size_t)nRow, (size_t)nCol); + if (nRow != nCol) { + BSK_PRINT(MSG_WARNING, "Can't invert a non-square matrix in ukfMatInv."); + return -1; + } + ukfLUD(sourceMat, nRow, nCol, LUMatrix, indx); + for (j = 0; j < nRow; j++) { + vSetZero(colSolve, (size_t)nRow); + colSolve[j] = 1.0; badCall = ukfLUBckSlv(LUMatrix, nRow, nCol, indx, colSolve, invCol); - for (i = 0; i < nRow; i++) - { - destMat[i*nRow + j] = invCol[i]; - } - } + for (i = 0; i < nRow; i++) { + destMat[i * nRow + j] = invCol[i]; + } + } return badCall; } -int32_t ukfCholDecomp(double *sourceMat, int32_t nRow, int32_t nCol, - double *destMat) +int32_t +ukfCholDecomp(double* sourceMat, int32_t nRow, int32_t nCol, double* destMat) { - int32_t i, j, k; - double sigma; + int32_t i, j, k; + double sigma; - mSetZero(destMat, (size_t) nRow, (size_t) nCol); - if (nRow != nCol) - { - BSK_PRINT(MSG_WARNING, "Can't get a lower-triangular inverse of non-square matrix in ukfCholDecomp."); - return -1; - } + mSetZero(destMat, (size_t)nRow, (size_t)nCol); + if (nRow != nCol) { + BSK_PRINT(MSG_WARNING, "Can't get a lower-triangular inverse of non-square matrix in ukfCholDecomp."); + return -1; + } - for (i = 0; i #include -#include +#include #define UKF_MAX_DIM 20 - #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif - void ukfQRDJustR( - double *sourceMat, int32_t nRow, int32_t nCol, double *destMat); - int32_t ukfLInv( - double *sourceMat, int32_t nRow, int32_t nCol, double *destMat); - int32_t ukfUInv( - double *sourceMat, int32_t nRow, int32_t nCol, double *destMat); - int32_t ukfLUD(double *sourceMat, int32_t nRow, int32_t nCol, - double *destMat, int32_t *indx); - int32_t ukfLUBckSlv(double *sourceMat, int32_t nRow, int32_t nCol, - int32_t *indx, double *bmat, double *destMat); - int32_t ukfMatInv(double *sourceMat, int32_t nRow, int32_t nCol, - double *destMat); - int32_t ukfCholDecomp(double *sourceMat, int32_t nRow, int32_t nCol, - double *destMat); - int32_t ukfCholDownDate(double *rMat, double *xVec, double beta, int32_t nStates, - double *rOut); + void ukfQRDJustR(double* sourceMat, int32_t nRow, int32_t nCol, double* destMat); + int32_t ukfLInv(double* sourceMat, int32_t nRow, int32_t nCol, double* destMat); + int32_t ukfUInv(double* sourceMat, int32_t nRow, int32_t nCol, double* destMat); + int32_t ukfLUD(double* sourceMat, int32_t nRow, int32_t nCol, double* destMat, int32_t* indx); + int32_t ukfLUBckSlv(double* sourceMat, int32_t nRow, int32_t nCol, int32_t* indx, double* bmat, double* destMat); + int32_t ukfMatInv(double* sourceMat, int32_t nRow, int32_t nCol, double* destMat); + int32_t ukfCholDecomp(double* sourceMat, int32_t nRow, int32_t nCol, double* destMat); + int32_t ukfCholDownDate(double* rMat, double* xVec, double beta, int32_t nStates, double* rOut); #ifdef __cplusplus } #endif - #endif 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..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,18 +32,19 @@ 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** - 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,20 +89,19 @@ 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 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/architecture/utilitiesSelfCheck/avsLibrarySelfCheck/avsLibrarySelfCheck.c b/src/architecture/utilitiesSelfCheck/avsLibrarySelfCheck/avsLibrarySelfCheck.c index e30bca187d..1863ee9fe4 100644 --- a/src/architecture/utilitiesSelfCheck/avsLibrarySelfCheck/avsLibrarySelfCheck.c +++ b/src/architecture/utilitiesSelfCheck/avsLibrarySelfCheck/avsLibrarySelfCheck.c @@ -27,7 +27,8 @@ #include "architecture/utilities/tests/unitTestComparators.h" #include "avsLibrarySelfCheck.h" -int testLinearAlgebra(double accuracy) +int +testLinearAlgebra(double accuracy) { int errorCount = 0; @@ -79,17 +80,16 @@ int testLinearAlgebra(double accuracy) /*-----------------------------------------------------------------------*/ /* generally sized vector checks */ - v3Set(4, 5, 16, v3_0); vCopy(v3_0, 3, v3_1); - if(!vIsEqual(v3_0, 3, v3_1, accuracy)) { + if (!vIsEqual(v3_0, 3, v3_1, accuracy)) { printf("vCopy failed\n"); errorCount++; } v3Set(0, 0, 0, v3_0); vSetZero(v3_1, 3); - if(!vIsEqual(v3_0, 3, v3_1, accuracy)) { + if (!vIsEqual(v3_0, 3, v3_1, accuracy)) { printf("vSetZero failed\n"); errorCount++; } @@ -98,7 +98,7 @@ int testLinearAlgebra(double accuracy) v3Set(4, 5, 6, v3_1); v3Set(5, 7, 9, v3_2); vAdd(v3_0, 3, v3_1, v3_0); - if(!vIsEqual(v3_0, 3, v3_2, accuracy)) { + if (!vIsEqual(v3_0, 3, v3_2, accuracy)) { printf("vAdd failed\n"); errorCount++; } @@ -107,7 +107,7 @@ int testLinearAlgebra(double accuracy) v3Set(1, 2, 3, v3_1); v3Set(3, 4, 5, v3_2); vSubtract(v3_0, 3, v3_1, v3_0); - if(!vIsEqual(v3_0, 3, v3_2, accuracy)) { + if (!vIsEqual(v3_0, 3, v3_2, accuracy)) { printf("vSubtract failed\n"); errorCount++; } @@ -115,7 +115,7 @@ int testLinearAlgebra(double accuracy) v3Set(1, 2, 3, v3_0); v3Set(3, 6, 9, v3_2); vScale(3, v3_0, 3, v3_0); - if(!vIsEqual(v3_0, 3, v3_2, accuracy)) { + if (!vIsEqual(v3_0, 3, v3_2, accuracy)) { printf("vScale failed\n"); errorCount++; } @@ -123,61 +123,49 @@ int testLinearAlgebra(double accuracy) v3Set(1, 2, 3, v3_1); v3Set(4, 5, 6, v3_2); a = vDot(v3_1, 3, v3_2); - if(!isEqual(a, 32.0, accuracy)) { + if (!isEqual(a, 32.0, accuracy)) { printf("vDot failed\n"); errorCount++; } v3Set(1, 2, 3, v3_0); v3Set(4, 5, 6, v3_1); - m33Set(4, 5, 6, - 8, 10, 12, - 12, 15, 18, - m33_1); + m33Set(4, 5, 6, 8, 10, 12, 12, 15, 18, m33_1); vOuterProduct(v3_0, 3, v3_1, 3, m33_0); - if(!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { printf("vOuterProduct failed\n"); errorCount++; } v3Set(1, 2, 3, v3_0); - m33Set(4, 5, 6, - 7, 8, 9, - 10, 11, 12, - m33_0); + m33Set(4, 5, 6, 7, 8, 9, 10, 11, 12, m33_0); v3Set(48, 54, 60, v3_1); vtMultM(v3_0, m33_0, 3, 3, v3_0); - if(!vIsEqual(v3_0, 3, v3_1, accuracy)) { + if (!vIsEqual(v3_0, 3, v3_1, accuracy)) { printf("vtMultM failed\n"); errorCount++; } v3Set(1, 2, 3, v3_0); - m33Set(4, 5, 6, - 7, 8, 9, - 10, 11, 12, - m33_0); + m33Set(4, 5, 6, 7, 8, 9, 10, 11, 12, m33_0); v3Set(32, 50, 68, v3_1); vtMultMt(v3_0, m33_0, 3, 3, v3_0); - if(!vIsEqual(v3_0, 3, v3_1, accuracy)) { + if (!vIsEqual(v3_0, 3, v3_1, accuracy)) { printf("vtMultMt failed\n"); errorCount++; } v3Set(1, 2, 3, v3_0); a = vNorm(v3_0, 3); - if(!isEqual(a, 3.74165738677394, accuracy)) { + if (!isEqual(a, 3.74165738677394, accuracy)) { printf("vNorm failed\n"); errorCount++; } v3Set(1, 2, 3, v3_0); - v3Set(0.267261241912424, - 0.534522483824849, - 0.801783725737273, - v3_1); + v3Set(0.267261241912424, 0.534522483824849, 0.801783725737273, v3_1); vNormalize(v3_0, 3, v3_0); - if(!vIsEqual(v3_0, 3, v3_1, accuracy)) { + if (!vIsEqual(v3_0, 3, v3_1, accuracy)) { printf("vNormalize failed\n"); errorCount++; } @@ -187,21 +175,21 @@ int testLinearAlgebra(double accuracy) v2_0[0] = 1; v2_0[1] = 2; v2Set(1, 2, v2_1); - if(!v2IsEqual(v2_0, v2_1, accuracy)) { + if (!v2IsEqual(v2_0, v2_1, accuracy)) { printf("v2IsEqual failed\n"); errorCount++; } v2Set(1, 2, v2_0); v2Copy(v2_0, v2_1); - if(!v2IsEqual(v2_0, v2_1, accuracy)) { + if (!v2IsEqual(v2_0, v2_1, accuracy)) { printf("v2Copy failed\n"); errorCount++; } v2Set(0, 0, v2_0); v2SetZero(v2_1); - if(!v2IsEqual(v2_0, v2_1, accuracy)) { + if (!v2IsEqual(v2_0, v2_1, accuracy)) { printf("v2SetZero failed\n"); errorCount++; } @@ -209,7 +197,7 @@ int testLinearAlgebra(double accuracy) v2Set(1, 2, v2_0); v2Set(4, 5, v2_1); a = v2Dot(v2_0, v2_1); - if(!isEqual(a, 14.0, accuracy)) { + if (!isEqual(a, 14.0, accuracy)) { printf("v2Dot failed\n"); errorCount++; } @@ -218,7 +206,7 @@ int testLinearAlgebra(double accuracy) v2Set(4, 5, v2_1); v2Set(5, 7, v2_2); v2Add(v2_0, v2_1, v2_0); - if(!v2IsEqual(v2_0, v2_2, accuracy)) { + if (!v2IsEqual(v2_0, v2_2, accuracy)) { printf("v2Add failed\n"); errorCount++; } @@ -227,14 +215,14 @@ int testLinearAlgebra(double accuracy) v2Set(1, 2, v2_1); v2Set(3, 4, v2_2); v2Subtract(v2_0, v2_1, v2_0); - if(!v2IsEqual(v2_0, v2_2, accuracy)) { + if (!v2IsEqual(v2_0, v2_2, accuracy)) { printf("v2Subtract failed\n"); errorCount++; } v2Set(3, 4, v2_0); a = v2Norm(v2_0); - if(!isEqual(a, 5.0, accuracy)) { + if (!isEqual(a, 5.0, accuracy)) { printf("v2Norm failed\n"); errorCount++; } @@ -242,42 +230,40 @@ int testLinearAlgebra(double accuracy) v2Set(1, 2, v2_0); v2Set(3, 6, v2_2); v2Scale(3, v2_0, v2_0); - if(!v2IsEqual(v2_0, v2_2, accuracy)) { + if (!v2IsEqual(v2_0, v2_2, accuracy)) { printf("v2Scale failed\n"); errorCount++; } v2Set(1, 1, v2_0); - v2Set(1./sqrt(2), 1./(sqrt(2)), v2_2); + v2Set(1. / sqrt(2), 1. / (sqrt(2)), v2_2); v2Normalize(v2_0, v2_0); - if(!v2IsEqual(v2_0, v2_2, accuracy)) { + if (!v2IsEqual(v2_0, v2_2, accuracy)) { printf("v2Normalize failed\n"); errorCount++; } - - //--------- v3_0[0] = 1; v3_0[1] = 2; v3_0[2] = 3; v3Set(1, 2, 3, v3_1); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3Set failed\n"); errorCount++; } v3Set(4, 5, 16, v3_0); v3Copy(v3_0, v3_1); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3Copy failed\n"); errorCount++; } v3Set(0, 0, 0, v3_0); v3SetZero(v3_1); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3SetZero failed\n"); errorCount++; } @@ -286,7 +272,7 @@ int testLinearAlgebra(double accuracy) v3Set(4, 5, 6, v3_1); v3Set(5, 7, 9, v3_2); v3Add(v3_0, v3_1, v3_0); - if(!v3IsEqual(v3_0, v3_2, accuracy)) { + if (!v3IsEqual(v3_0, v3_2, accuracy)) { printf("v3Add failed\n"); errorCount++; } @@ -295,7 +281,7 @@ int testLinearAlgebra(double accuracy) v3Set(1, 2, 3, v3_1); v3Set(3, 4, 5, v3_2); vSubtract(v3_0, 3, v3_1, v3_0); - if(!vIsEqual(v3_0, 3, v3_2, accuracy)) { + if (!vIsEqual(v3_0, 3, v3_2, accuracy)) { printf("vSubtract failed\n"); errorCount++; } @@ -303,7 +289,7 @@ int testLinearAlgebra(double accuracy) v3Set(1, 2, 3, v3_0); v3Set(3, 6, 9, v3_2); v3Scale(3, v3_0, v3_0); - if(!v3IsEqual(v3_0, v3_2, accuracy)) { + if (!v3IsEqual(v3_0, v3_2, accuracy)) { printf("v3Scale failed\n"); errorCount++; } @@ -311,61 +297,49 @@ int testLinearAlgebra(double accuracy) v3Set(1, 2, 3, v3_1); v3Set(4, 5, 6, v3_2); a = v3Dot(v3_1, v3_2); - if(!isEqual(a, 32.0, accuracy)) { + if (!isEqual(a, 32.0, accuracy)) { printf("v3Dot failed\n"); errorCount++; } v3Set(1, 2, 3, v3_0); v3Set(4, 5, 6, v3_1); - m33Set(4, 5, 6, - 8, 10, 12, - 12, 15, 18, - m33_1); + m33Set(4, 5, 6, 8, 10, 12, 12, 15, 18, m33_1); v3OuterProduct(v3_0, v3_1, m33_0); - if(!m33IsEqual(m33_0, m33_1, accuracy)) { + if (!m33IsEqual(m33_0, m33_1, accuracy)) { printf("v3OuterProduct failed\n"); errorCount++; } v3Set(1, 2, 3, v3_0); - m33Set(4, 5, 6, - 7, 8, 9, - 10, 11, 12, - m33_0); + m33Set(4, 5, 6, 7, 8, 9, 10, 11, 12, m33_0); v3Set(48, 54, 60, v3_1); v3tMultM33(v3_0, m33_0, v3_0); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3tMultM33 failed\n"); errorCount++; } v3Set(1, 2, 3, v3_0); - m33Set(4, 5, 6, - 7, 8, 9, - 10, 11, 12, - m33_0); + m33Set(4, 5, 6, 7, 8, 9, 10, 11, 12, m33_0); v3Set(32, 50, 68, v3_1); v3tMultM33t(v3_0, m33_0, v3_0); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3tMultM33t failed\n"); errorCount++; } v3Set(1, 2, 3, v3_0); a = v3Norm(v3_0); - if(!isEqual(a, 3.74165738677394, accuracy)) { + if (!isEqual(a, 3.74165738677394, accuracy)) { printf("v3Norm failed\n"); errorCount++; } v3Set(1, 2, 3, v3_0); - v3Set(0.267261241912424, - 0.534522483824849, - 0.801783725737273, - v3_1); + v3Set(0.267261241912424, 0.534522483824849, 0.801783725737273, v3_1); v3Normalize(v3_0, v3_0); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3Normalize failed\n"); errorCount++; } @@ -374,7 +348,7 @@ int testLinearAlgebra(double accuracy) v3Set(4, 5, 6, v3_1); v3Set(-3, 6, -3, v3_2); v3Cross(v3_0, v3_1, v3_0); - if(!v3IsEqual(v3_0, v3_2, accuracy)) { + if (!v3IsEqual(v3_0, v3_2, accuracy)) { printf("v3Cross failed\n"); errorCount++; } @@ -383,18 +357,15 @@ int testLinearAlgebra(double accuracy) v3Set(-1, 1, 1, v3_1); v3Normalize(v3_1, v3_1); v3Perpendicular(v3_0, v3_2); - if(!v3IsEqual(v3_2, v3_1, accuracy)) { + if (!v3IsEqual(v3_2, v3_1, accuracy)) { printf("v3Perpendicular failed\n"); errorCount++; } v3Set(1, 2, 3, v3_0); v3Tilde(v3_0, m33_0); - m33Set(0, -3, 2, - 3, 0, -1, - -2, 1, 0, - m33_1); - if(!m33IsEqual(m33_1, m33_0, accuracy)) { + m33Set(0, -3, 2, 3, 0, -1, -2, 1, 0, m33_1); + if (!m33IsEqual(m33_1, m33_0, accuracy)) { printf("v3Tilde failed\n"); errorCount++; } @@ -402,7 +373,7 @@ int testLinearAlgebra(double accuracy) v3Set(1, 2, 3, v3_0); v3Sort(v3_0, v3_0); v3Set(3, 2, 1, v3_1); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3Sort 1 failed\n"); errorCount++; } @@ -410,7 +381,7 @@ int testLinearAlgebra(double accuracy) v3Set(1, 3, 2, v3_0); v3Sort(v3_0, v3_0); v3Set(3, 2, 1, v3_1); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3Sort 2 failed\n"); errorCount++; } @@ -418,7 +389,7 @@ int testLinearAlgebra(double accuracy) v3Set(2, 1, 3, v3_0); v3Sort(v3_0, v3_0); v3Set(3, 2, 1, v3_1); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3Sort 3 failed\n"); errorCount++; } @@ -426,7 +397,7 @@ int testLinearAlgebra(double accuracy) v3Set(2, 3, 1, v3_0); v3Sort(v3_0, v3_0); v3Set(3, 2, 1, v3_1); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3Sort 4 failed\n"); errorCount++; } @@ -434,7 +405,7 @@ int testLinearAlgebra(double accuracy) v3Set(3, 1, 2, v3_0); v3Sort(v3_0, v3_0); v3Set(3, 2, 1, v3_1); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3Sort 5 failed\n"); errorCount++; } @@ -442,7 +413,7 @@ int testLinearAlgebra(double accuracy) v3Set(3, 2, 1, v3_0); v3Sort(v3_0, v3_0); v3Set(3, 2, 1, v3_1); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("v3Sort 6 failed\n"); errorCount++; } @@ -454,21 +425,21 @@ int testLinearAlgebra(double accuracy) v4_0[2] = 3; v4_0[3] = 4; v4Set(1, 2, 3, 4, v4_1); - if(!v4IsEqual(v4_0, v4_1, accuracy)) { + if (!v4IsEqual(v4_0, v4_1, accuracy)) { printf("v4IsEqual failed\n"); errorCount++; } v4Set(4, 5, 16, 22, v4_0); v4Copy(v4_0, v4_1); - if(!v4IsEqual(v4_0, v4_1, accuracy)) { + if (!v4IsEqual(v4_0, v4_1, accuracy)) { printf("v4Copy failed\n"); errorCount++; } v4Set(0, 0, 0, 0, v4_0); v4SetZero(v4_1); - if(!v4IsEqual(v4_0, v4_1, accuracy)) { + if (!v4IsEqual(v4_0, v4_1, accuracy)) { printf("v4SetZero failed\n"); errorCount++; } @@ -476,14 +447,14 @@ int testLinearAlgebra(double accuracy) v4Set(1, 2, 3, 4, v4_1); v4Set(4, 5, 6, 7, v4_2); a = v4Dot(v4_1, v4_2); - if(!isEqual(a, 60.0, accuracy)) { + if (!isEqual(a, 60.0, accuracy)) { printf("v4Dot failed\n"); errorCount++; } v4Set(1, 2, 3, 4, v4_0); a = v4Norm(v4_0); - if(!isEqual(a, 5.47722557505166, accuracy)) { + if (!isEqual(a, 5.47722557505166, accuracy)) { printf("v4Norm failed\n"); errorCount++; } @@ -501,28 +472,28 @@ int testLinearAlgebra(double accuracy) m33_0[2][1] = 8; m33_0[2][2] = 9; m33Set(1, 2, 3, 4, 5, 6, 7, 8, 9, m33_1); - if(!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { printf("mIsEqual failed\n"); errorCount++; } m33Set(1, 2, 3, 4, 5, 6, 7, 8, 9, m33_0); mCopy(m33_0, 3, 3, m33_1); - if(!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { printf("mCopy failed\n"); errorCount++; } m33Set(0, 0, 0, 0, 0, 0, 0, 0, 0, m33_0); mSetZero(m33_1, 3, 3); - if(!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { printf("mSetZero failed\n"); errorCount++; } m33Set(1, 0, 0, 0, 1, 0, 0, 0, 1, m33_0); mSetIdentity(m33_1, 3, 3); - if(!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { printf("mSetIdentity failed\n"); errorCount++; } @@ -530,7 +501,7 @@ int testLinearAlgebra(double accuracy) m33Set(1, 0, 0, 0, 2, 0, 0, 0, 3, m33_0); v3Set(1, 2, 3, v3_0); mDiag(v3_0, 3, m33_1); - if(!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { printf("mDiag failed\n"); errorCount++; } @@ -538,7 +509,7 @@ int testLinearAlgebra(double accuracy) m33Set(1, 2, 3, 4, 5, 6, 7, 8, 9, m33_0); m33Set(1, 4, 7, 2, 5, 8, 3, 6, 9, m33_1); mTranspose(m33_0, 3, 3, m33_0); - if(!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { printf("mTranspose failed\n"); errorCount++; } @@ -561,7 +532,7 @@ int testLinearAlgebra(double accuracy) m42_0[3][0] = 4; m42_0[3][1] = 8; mTranspose(m24_0, 2, 4, m42_1); - if(!mIsEqual(m42_0, 4, 2, m42_1, accuracy)) { + if (!mIsEqual(m42_0, 4, 2, m42_1, accuracy)) { printf("mTranspose failed\n"); errorCount++; } @@ -570,7 +541,7 @@ int testLinearAlgebra(double accuracy) m33Set(10, 11, 12, 13, 14, 15, 16, 17, 18, m33_1); m33Set(11, 13, 15, 17, 19, 21, 23, 25, 27, m33_2); mAdd(m33_0, 3, 3, m33_1, m33_0); - if(!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { printf("mAdd failed\n"); errorCount++; } @@ -579,7 +550,7 @@ int testLinearAlgebra(double accuracy) m33Set(10, 11, 12, 13, 14, 15, 16, 17, 18, m33_1); m33Set(-9, -9, -9, -9, -9, -9, -9, -9, -9, m33_2); mSubtract(m33_0, 3, 3, m33_1, m33_0); - if(!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { printf("mSubtract failed\n"); errorCount++; } @@ -587,7 +558,7 @@ int testLinearAlgebra(double accuracy) m33Set(1, 2, 3, 4, 5, 6, 7, 8, 9, m33_0); m33Set(2, 4, 6, 8, 10, 12, 14, 16, 18, m33_1); mScale(2, m33_0, 3, 3, m33_0); - if(!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { printf("mScale failed\n"); errorCount++; } @@ -596,7 +567,7 @@ int testLinearAlgebra(double accuracy) m33Set(10, 11, 12, 13, 14, 15, 16, 17, 18, m33_1); m33Set(84, 90, 96, 201, 216, 231, 318, 342, 366, m33_2); mMultM(m33_0, 3, 3, m33_1, 3, 3, m33_0); - if(!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { printf("mMultM failed\n"); errorCount++; } @@ -630,7 +601,7 @@ int testLinearAlgebra(double accuracy) m23_0[1][1] = 174; m23_0[1][2] = 278; mMultM(m24_0, 2, 4, m43_0, 4, 3, m23_1); - if(!mIsEqual(m23_1, 2, 3, m23_0, accuracy)) { + if (!mIsEqual(m23_1, 2, 3, m23_0, accuracy)) { printf("mMultM failed\n"); errorCount++; } @@ -639,7 +610,7 @@ int testLinearAlgebra(double accuracy) m33Set(10, 11, 12, 13, 14, 15, 16, 17, 18, m33_1); m33Set(174, 186, 198, 213, 228, 243, 252, 270, 288, m33_2); mtMultM(m33_0, 3, 3, m33_1, 3, 3, m33_0); - if(!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { printf("mtMultM failed\n"); errorCount++; } @@ -673,7 +644,7 @@ int testLinearAlgebra(double accuracy) m43_0[3][1] = 48; m43_0[3][2] = 60; mtMultM(m24_0, 2, 4, m23_0, 2, 3, m43_1); - if(!mIsEqual(m43_0, 4, 3, m43_1, accuracy)) { + if (!mIsEqual(m43_0, 4, 3, m43_1, accuracy)) { printf("mtMult failed\n"); errorCount++; } @@ -682,7 +653,7 @@ int testLinearAlgebra(double accuracy) m33Set(10, 11, 12, 13, 14, 15, 16, 17, 18, m33_1); m33Set(68, 86, 104, 167, 212, 257, 266, 338, 410, m33_2); mMultMt(m33_0, 3, 3, m33_1, 3, 3, m33_0); - if(!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { printf("mMultMt failed\n"); errorCount++; } @@ -717,7 +688,7 @@ int testLinearAlgebra(double accuracy) m24_0[1][3] = 128; mMultMt(m23_0, 2, 3, m43_0, 4, 3, m24_1); - if(!mIsEqual(m24_0, 2, 4, m24_1, accuracy)) { + if (!mIsEqual(m24_0, 2, 4, m24_1, accuracy)) { printf("mMultMt failed\n"); errorCount++; } @@ -726,7 +697,7 @@ int testLinearAlgebra(double accuracy) m33Set(10, 11, 12, 13, 14, 15, 16, 17, 18, m33_1); m33Set(138, 174, 210, 171, 216, 261, 204, 258, 312, m33_2); mtMultMt(m33_0, 3, 3, m33_1, 3, 3, m33_0); - if(!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_2, accuracy)) { printf("mtMultMt failed\n"); errorCount++; } @@ -761,7 +732,7 @@ int testLinearAlgebra(double accuracy) m24_0[1][3] = 136; mtMultMt(m32_0, 3, 2, m43_0, 4, 3, m24_1); - if(!mIsEqual(m24_0, 2, 4, m24_1, accuracy)) { + if (!mIsEqual(m24_0, 2, 4, m24_1, accuracy)) { printf("mtMultMt failed\n"); errorCount++; } @@ -770,7 +741,7 @@ int testLinearAlgebra(double accuracy) v3Set(2, 3, 4, v3_0); v3Set(20, 47, 74, v3_1); mMultV(m33_0, 3, 3, v3_0, v3_0); - if(!vIsEqual(v3_0, 3, v3_1, accuracy)) { + if (!vIsEqual(v3_0, 3, v3_1, accuracy)) { printf("mMultV failed\n"); errorCount++; } @@ -785,7 +756,7 @@ int testLinearAlgebra(double accuracy) v2_1[0] = 20; v2_1[1] = 47; mMultV(m23_0, 2, 3, v3_0, v2_0); - if(!vIsEqual(v2_0, 2, v2_1, accuracy)) { + if (!vIsEqual(v2_0, 2, v2_1, accuracy)) { printf("mMultV failed\n"); errorCount++; } @@ -794,7 +765,7 @@ int testLinearAlgebra(double accuracy) v3Set(2, 3, 4, v3_0); v3Set(42, 51, 60, v3_1); mtMultV(m33_0, 3, 3, v3_0, v3_0); - if(!vIsEqual(v3_0, 3, v3_1, accuracy)) { + if (!vIsEqual(v3_0, 3, v3_1, accuracy)) { printf("mtMultV failed\n"); errorCount++; } @@ -814,21 +785,21 @@ int testLinearAlgebra(double accuracy) v3Set(2, 3, 4, v3_0); v4Set(53, 62, 71, 80, v4_0); mtMultV(m34_0, 3, 4, v3_0, v4_1); - if(!vIsEqual(v4_0, 4, v4_1, accuracy)) { + if (!vIsEqual(v4_0, 4, v4_1, accuracy)) { printf("mtMultV failed\n"); errorCount++; } m33Set(4, 5, 6, 8, 10, 22, 22, 15, 18, m33_0); a = mTrace(m33_0, 3); - if(!isEqual(a, 32.0, accuracy)) { + if (!isEqual(a, 32.0, accuracy)) { printf("mTrace failed %f\n", a); errorCount++; } m33Set(4, 5, 6, 8, 10, 22, 22, 15, 18, m33_0); a = mDeterminant(m33_0, 3); - if(!isEqual(a, 500.0, accuracy)) { + if (!isEqual(a, 500.0, accuracy)) { printf("mDeterminant failed %f\n", a); errorCount++; } @@ -836,7 +807,7 @@ int testLinearAlgebra(double accuracy) m33Set(4, 5, 6, 8, 10, 22, 22, 15, 18, m33_0); m33Set(-0.3, 0.0, 0.1, 0.68, -0.12, -0.08, -0.2, 0.1, 0.0, m33_1); mInverse(m33_0, 3, m33_0); - if(!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { + if (!mIsEqual(m33_0, 3, 3, m33_1, accuracy)) { printf("mInverse failed\n"); errorCount++; } @@ -876,7 +847,7 @@ int testLinearAlgebra(double accuracy) m44_1[3][3] = -25.0 / 12.0; mInverse(m44_0, 4, m44_0); - if(!mIsEqual(m44_0, 4, 4, m44_1, accuracy)) { + if (!mIsEqual(m44_0, 4, 4, m44_1, accuracy)) { printf("mInverse failed\n"); errorCount++; } @@ -888,28 +859,28 @@ int testLinearAlgebra(double accuracy) m22_0[1][0] = 3; m22_0[1][1] = 4; m22Set(1, 2, 3, 4, m22_1); - if(!m22IsEqual(m22_0, m22_1, accuracy)) { + if (!m22IsEqual(m22_0, m22_1, accuracy)) { printf("m22IsEqual failed\n"); errorCount++; } m22Set(1, 2, 3, 4, m22_0); m22Copy(m22_0, m22_1); - if(!m22IsEqual(m22_0, m22_1, accuracy)) { + if (!m22IsEqual(m22_0, m22_1, accuracy)) { printf("m22Copy failed\n"); errorCount++; } m22Set(0, 0, 0, 0, m22_0); m22SetZero(m22_1); - if(!m22IsEqual(m22_0, m22_1, accuracy)) { + if (!m22IsEqual(m22_0, m22_1, accuracy)) { printf("m22SetZero failed\n"); errorCount++; } m22Set(1, 0, 0, 1, m22_0); m22SetIdentity(m22_1); - if(!m22IsEqual(m22_0, m22_1, accuracy)) { + if (!m22IsEqual(m22_0, m22_1, accuracy)) { printf("m22SetIdentity failed\n"); errorCount++; } @@ -917,7 +888,7 @@ int testLinearAlgebra(double accuracy) m22Set(1, 2, 3, 4, m22_0); m22Set(1, 3, 2, 4, m22_1); m22Transpose(m22_0, m22_0); - if(!m22IsEqual(m22_0, m22_1, accuracy)) { + if (!m22IsEqual(m22_0, m22_1, accuracy)) { printf("m22Transpose failed\n"); errorCount++; } @@ -926,7 +897,7 @@ int testLinearAlgebra(double accuracy) m22Set(10, 11, 12, 13, m22_1); m22Set(11, 13, 15, 17, m22_2); m22Add(m22_0, m22_1, m22_0); - if(!m22IsEqual(m22_0, m22_2, accuracy)) { + if (!m22IsEqual(m22_0, m22_2, accuracy)) { printf("m22Add failed\n"); errorCount++; } @@ -935,7 +906,7 @@ int testLinearAlgebra(double accuracy) m22Set(10, 11, 12, 13, m22_1); m22Set(-9, -9, -9, -9, m22_2); m22Subtract(m22_0, m22_1, m22_0); - if(!m22IsEqual(m22_0, m22_2, accuracy)) { + if (!m22IsEqual(m22_0, m22_2, accuracy)) { printf("m22Subtract failed\n"); errorCount++; } @@ -943,7 +914,7 @@ int testLinearAlgebra(double accuracy) m22Set(1, 2, 3, 4, m22_0); m22Set(2, 4, 6, 8, m22_1); m22Scale(2, m22_0, m22_0); - if(!m22IsEqual(m22_0, m22_1, accuracy)) { + if (!m22IsEqual(m22_0, m22_1, accuracy)) { printf("m22Scale failed\n"); errorCount++; } @@ -952,7 +923,7 @@ int testLinearAlgebra(double accuracy) m22Set(10, 11, 12, 13, m22_1); m22Set(34, 37, 78, 85, m22_2); m22MultM22(m22_0, m22_1, m22_0); - if(!m22IsEqual(m22_0, m22_2, accuracy)) { + if (!m22IsEqual(m22_0, m22_2, accuracy)) { printf("m22MultM22 failed\n"); errorCount++; } @@ -961,7 +932,7 @@ int testLinearAlgebra(double accuracy) m22Set(10, 11, 12, 13, m22_1); m22Set(46, 50, 68, 74, m22_2); m22tMultM22(m22_0, m22_1, m22_0); - if(!m22IsEqual(m22_0, m22_2, accuracy)) { + if (!m22IsEqual(m22_0, m22_2, accuracy)) { printf("m22tMultM22 failed\n"); errorCount++; } @@ -970,7 +941,7 @@ int testLinearAlgebra(double accuracy) m22Set(10, 11, 12, 13, m22_1); m22Set(32, 38, 74, 88, m22_2); m22MultM22t(m22_0, m22_1, m22_0); - if(!m22IsEqual(m22_0, m22_2, accuracy)) { + if (!m22IsEqual(m22_0, m22_2, accuracy)) { printf("m22MultM22t failed\n"); errorCount++; } @@ -979,7 +950,7 @@ int testLinearAlgebra(double accuracy) v2Set(2, 3, v2_0); v2Set(8, 18, v2_1); m22MultV2(m22_0, v2_0, v2_0); - if(!v2IsEqual(v2_0, v2_1, accuracy)) { + if (!v2IsEqual(v2_0, v2_1, accuracy)) { printf("m22MultV2 failed\n"); errorCount++; } @@ -988,21 +959,21 @@ int testLinearAlgebra(double accuracy) v2Set(2, 3, v2_0); v2Set(11, 16, v2_1); m22tMultV2(m22_0, v2_0, v2_0); - if(!v2IsEqual(v2_0, v2_1, accuracy)) { + if (!v2IsEqual(v2_0, v2_1, accuracy)) { printf("m22tMultV2 failed\n"); errorCount++; } m22Set(1, 2, 3, 4, m22_0); a = m22Trace(m22_0); - if(!isEqual(a, 5.0, accuracy)) { + if (!isEqual(a, 5.0, accuracy)) { printf("m22Trace failed %f\n", a); errorCount++; } m22Set(1, 2, 3, 4, m22_0); a = m22Determinant(m22_0); - if(!isEqual(a, -2.0, accuracy)) { + if (!isEqual(a, -2.0, accuracy)) { printf("m22Determinant failed %f\n", a); errorCount++; } @@ -1010,7 +981,7 @@ int testLinearAlgebra(double accuracy) m22Set(1, 2, 3, 4, m22_0); m22Set(-2.0, 1.0, 1.5, -0.5, m22_1); m22Inverse(m22_0, m22_0); - if(!m22IsEqual(m22_0, m22_1, accuracy)) { + if (!m22IsEqual(m22_0, m22_1, accuracy)) { printf("m22Inverse failed\n"); errorCount++; } @@ -1019,21 +990,21 @@ int testLinearAlgebra(double accuracy) m33Set(1, 2, 3, 4, 5, 6, 7, 8, 9, m33_0); m33Copy(m33_0, m33_1); - if(!m33IsEqual(m33_0, m33_1, accuracy)) { + if (!m33IsEqual(m33_0, m33_1, accuracy)) { printf("m33Copy failed\n"); errorCount++; } m33Set(0, 0, 0, 0, 0, 0, 0, 0, 0, m33_0); m33SetZero(m33_1); - if(!m33IsEqual(m33_0, m33_1, accuracy)) { + if (!m33IsEqual(m33_0, m33_1, accuracy)) { printf("m33SetZero failed\n"); errorCount++; } m33Set(1, 0, 0, 0, 1, 0, 0, 0, 1, m33_0); m33SetIdentity(m33_1); - if(!m33IsEqual(m33_0, m33_1, accuracy)) { + if (!m33IsEqual(m33_0, m33_1, accuracy)) { printf("m33SetIdentity failed\n"); errorCount++; } @@ -1041,7 +1012,7 @@ int testLinearAlgebra(double accuracy) m33Set(1, 2, 3, 4, 5, 6, 7, 8, 9, m33_0); m33Set(1, 4, 7, 2, 5, 8, 3, 6, 9, m33_1); m33Transpose(m33_0, m33_0); - if(!m33IsEqual(m33_0, m33_1, accuracy)) { + if (!m33IsEqual(m33_0, m33_1, accuracy)) { printf("m33Transpose failed\n"); errorCount++; } @@ -1050,7 +1021,7 @@ int testLinearAlgebra(double accuracy) m33Set(10, 11, 12, 13, 14, 15, 16, 17, 18, m33_1); m33Set(11, 13, 15, 17, 19, 21, 23, 25, 27, m33_2); m33Add(m33_0, m33_1, m33_0); - if(!m33IsEqual(m33_0, m33_2, accuracy)) { + if (!m33IsEqual(m33_0, m33_2, accuracy)) { printf("m33Add failed\n"); errorCount++; } @@ -1059,7 +1030,7 @@ int testLinearAlgebra(double accuracy) m33Set(10, 11, 12, 13, 14, 15, 16, 17, 18, m33_1); m33Set(-9, -9, -9, -9, -9, -9, -9, -9, -9, m33_2); m33Subtract(m33_0, m33_1, m33_0); - if(!m33IsEqual(m33_0, m33_2, accuracy)) { + if (!m33IsEqual(m33_0, m33_2, accuracy)) { printf("m33Subtract failed\n"); errorCount++; } @@ -1067,7 +1038,7 @@ int testLinearAlgebra(double accuracy) m33Set(1, 2, 3, 4, 5, 6, 7, 8, 9, m33_0); m33Set(2, 4, 6, 8, 10, 12, 14, 16, 18, m33_1); m33Scale(2, m33_0, m33_0); - if(!m33IsEqual(m33_0, m33_1, accuracy)) { + if (!m33IsEqual(m33_0, m33_1, accuracy)) { printf("m33Scale failed\n"); errorCount++; } @@ -1076,7 +1047,7 @@ int testLinearAlgebra(double accuracy) m33Set(10, 11, 12, 13, 14, 15, 16, 17, 18, m33_1); m33Set(84, 90, 96, 201, 216, 231, 318, 342, 366, m33_2); m33MultM33(m33_0, m33_1, m33_0); - if(!m33IsEqual(m33_0, m33_2, accuracy)) { + if (!m33IsEqual(m33_0, m33_2, accuracy)) { printf("m33MultM33 failed\n"); errorCount++; } @@ -1085,7 +1056,7 @@ int testLinearAlgebra(double accuracy) m33Set(10, 11, 12, 13, 14, 15, 16, 17, 18, m33_1); m33Set(174, 186, 198, 213, 228, 243, 252, 270, 288, m33_2); m33tMultM33(m33_0, m33_1, m33_0); - if(!m33IsEqual(m33_0, m33_2, accuracy)) { + if (!m33IsEqual(m33_0, m33_2, accuracy)) { printf("m33tMultM33 failed\n"); errorCount++; } @@ -1094,7 +1065,7 @@ int testLinearAlgebra(double accuracy) m33Set(10, 11, 12, 13, 14, 15, 16, 17, 18, m33_1); m33Set(68, 86, 104, 167, 212, 257, 266, 338, 410, m33_2); m33MultM33t(m33_0, m33_1, m33_0); - if(!m33IsEqual(m33_0, m33_2, accuracy)) { + if (!m33IsEqual(m33_0, m33_2, accuracy)) { printf("m33MultM33t failed\n"); errorCount++; } @@ -1103,7 +1074,7 @@ int testLinearAlgebra(double accuracy) v3Set(2, 3, 4, v3_0); v3Set(20, 47, 74, v3_1); m33MultV3(m33_0, v3_0, v3_0); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("m33MultV3 failed\n"); errorCount++; } @@ -1112,21 +1083,21 @@ int testLinearAlgebra(double accuracy) v3Set(2, 3, 4, v3_0); v3Set(42, 51, 60, v3_1); m33tMultV3(m33_0, v3_0, v3_0); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("m33tMultV3 failed\n"); errorCount++; } m33Set(4, 5, 6, 8, 10, 22, 22, 15, 18, m33_0); a = m33Trace(m33_0); - if(!isEqual(a, 32.0, accuracy)) { + if (!isEqual(a, 32.0, accuracy)) { printf("m33Trace failed %f\n", a); errorCount++; } m33Set(4, 5, 6, 8, 10, 22, 22, 15, 18, m33_0); a = m33Determinant(m33_0); - if(!isEqual(a, 500.0, accuracy)) { + if (!isEqual(a, 500.0, accuracy)) { printf("m33Determinant failed %f\n", a); errorCount++; } @@ -1134,7 +1105,7 @@ int testLinearAlgebra(double accuracy) m33Set(4, 5, 6, 8, 10, 22, 22, 15, 18, m33_0); v3Set(40.7786093479462, 9.66938737160798, 1.26805658603441, v3_0); m33SingularValues(m33_0, v3_1); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("m33SingularValues failed\n"); errorCount++; } @@ -1142,14 +1113,14 @@ int testLinearAlgebra(double accuracy) m33Set(4, 5, 6, 7, 8, 9, 22, 15, 20, m33_0); v3Set(32.879131511069, 0.695395691157217, -1.5745272022262, v3_0); m33EigenValues(m33_0, v3_1); - if(!v3IsEqual(v3_0, v3_1, accuracy)) { + if (!v3IsEqual(v3_0, v3_1, accuracy)) { printf("m33EigenValues failed\n"); errorCount++; } m33Set(4, 5, 6, 8, 10, 22, 22, 15, 18, m33_0); a = m33ConditionNumber(m33_0); - if(!isEqual(a, 32.1583514466598, accuracy)) { + if (!isEqual(a, 32.1583514466598, accuracy)) { printf("m33ConditionNumberFailed\n"); errorCount++; } @@ -1157,7 +1128,7 @@ int testLinearAlgebra(double accuracy) m33Set(4, 5, 6, 8, 10, 22, 22, 15, 18, m33_0); m33Set(-0.3, 0.0, 0.1, 0.68, -0.12, -0.08, -0.2, 0.1, 0.0, m33_1); m33Inverse(m33_0, m33_0); - if(!m33IsEqual(m33_0, m33_1, accuracy)) { + if (!m33IsEqual(m33_0, m33_1, accuracy)) { printf("m33Inverse failed\n"); errorCount++; } @@ -1166,14 +1137,14 @@ int testLinearAlgebra(double accuracy) m44Set(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, m44_0); m44Copy(m44_0, m44_1); - if(!m44IsEqual(m44_0, m44_1, accuracy)) { + if (!m44IsEqual(m44_0, m44_1, accuracy)) { printf("m44Copy failed\n"); errorCount++; } m44Set(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, m44_0); m44SetZero(m44_1); - if(!m44IsEqual(m44_0, m44_1, accuracy)) { + if (!m44IsEqual(m44_0, m44_1, accuracy)) { printf("m44SetZero failed\n"); errorCount++; } @@ -1182,14 +1153,14 @@ int testLinearAlgebra(double accuracy) v4Set(2, 3, 4, 5, v4_0); v4Set(40, 96, 152, 208, v4_1); m44MultV4(m44_0, v4_0, v4_0); - if(!v4IsEqual(v4_0, v4_1, accuracy)) { + if (!v4IsEqual(v4_0, v4_1, accuracy)) { printf("m44MultV4 failed\n"); errorCount++; } m44Set(4, 5, 6, 7, 8, 10, 22, 24, 22, 15, 18, 19, 1, 4, 9, 3, m44_0); a = m44Determinant(m44_0); - if(!isEqual(a, -3620.0, accuracy)) { + if (!isEqual(a, -3620.0, accuracy)) { printf("m44Determinant failed %f\n", a); errorCount++; } @@ -1210,297 +1181,1101 @@ int testLinearAlgebra(double accuracy) 0.0856353591160222, 0.0580110497237569, -0.0303867403314917, - -0.138121546961326, m44_1); + -0.138121546961326, + m44_1); m44Inverse(m44_0, m44_0); - if(!m44IsEqual(m44_0, m44_1, accuracy)) { + if (!m44IsEqual(m44_0, m44_1, accuracy)) { printf("m44Inverse failed\n"); errorCount++; } //---------- - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_0); m66Copy(m66_0, m66_1); - if(!m66IsEqual(m66_0, m66_1, accuracy)) { + if (!m66IsEqual(m66_0, m66_1, accuracy)) { printf("m66Copy failed\n"); errorCount++; } - m66Set(0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, + m66Set(0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, m66_0); m66SetZero(m66_1); - if(!m66IsEqual(m66_0, m66_1, accuracy)) { + if (!m66IsEqual(m66_0, m66_1, accuracy)) { printf("m66SetZero failed\n"); errorCount++; } - m66Set( - 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, - m66_0); + m66Set(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, + m66_0); m66SetIdentity(m66_1); - if(!m66IsEqual(m66_0, m66_1, accuracy)) { + if (!m66IsEqual(m66_0, m66_1, accuracy)) { printf("m66SetIdentity failed\n"); errorCount++; } - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, - m66_0); - m66Set(1, 7, 13, 19, 25, 31, - 2, 8, 14, 20, 26, 32, - 3, 9, 15, 21, 27, 33, - 4, 10, 16, 22, 28, 34, - 5, 11, 17, 23, 29, 35, - 6, 12, 18, 24, 30, 36, - m66_1); + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, + m66_0); + m66Set(1, + 7, + 13, + 19, + 25, + 31, + 2, + 8, + 14, + 20, + 26, + 32, + 3, + 9, + 15, + 21, + 27, + 33, + 4, + 10, + 16, + 22, + 28, + 34, + 5, + 11, + 17, + 23, + 29, + 35, + 6, + 12, + 18, + 24, + 30, + 36, + m66_1); m66Transpose(m66_0, m66_0); - if(!m66IsEqual(m66_0, m66_1, accuracy)) { + if (!m66IsEqual(m66_0, m66_1, accuracy)) { printf("m66Transpose failed\n"); errorCount++; } - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_0); - m33Set(4, 5, 6, - 10, 11, 12, - 16, 17, 18, - m33_1); + m33Set(4, 5, 6, 10, 11, 12, 16, 17, 18, m33_1); m66Get33Matrix(0, 1, m66_0, m33_0); - if(!m33IsEqual(m33_0, m33_1, accuracy)) { + if (!m33IsEqual(m33_0, m33_1, accuracy)) { printf("m66Get33Matrix failed\n"); errorCount++; } - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_0); - m33Set(54, 55, 56, - 57, 58, 59, - 60, 61, 62, - m33_0); - m66Set(1, 2, 3, 54, 55, 56, - 7, 8, 9, 57, 58, 59, - 13, 14, 15, 60, 61, 62, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m33Set(54, 55, 56, 57, 58, 59, 60, 61, 62, m33_0); + m66Set(1, + 2, + 3, + 54, + 55, + 56, + 7, + 8, + 9, + 57, + 58, + 59, + 13, + 14, + 15, + 60, + 61, + 62, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_1); m66Set33Matrix(0, 1, m33_0, m66_0); - if(!m66IsEqual(m66_0, m66_1, accuracy)) { + if (!m66IsEqual(m66_0, m66_1, accuracy)) { printf("m66Set33Matrix failed\n"); errorCount++; } - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_0); - m66Set(2, 4, 6, 8, 10, 12, - 14, 16, 18, 20, 22, 24, - 26, 28, 30, 32, 34, 36, - 38, 40, 42, 44, 46, 48, - 50, 52, 54, 56, 58, 60, - 62, 64, 66, 68, 70, 72, + m66Set(2, + 4, + 6, + 8, + 10, + 12, + 14, + 16, + 18, + 20, + 22, + 24, + 26, + 28, + 30, + 32, + 34, + 36, + 38, + 40, + 42, + 44, + 46, + 48, + 50, + 52, + 54, + 56, + 58, + 60, + 62, + 64, + 66, + 68, + 70, + 72, m66_1); m66Scale(2.0, m66_0, m66_0); - if(!m66IsEqual(m66_0, m66_1, accuracy)) { + if (!m66IsEqual(m66_0, m66_1, accuracy)) { printf("m66Scale failed\n"); errorCount++; } - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_0); - m66Set(10, 11, 12, 13, 14, 15, - 16, 17, 18, 19, 20, 21, - 22, 23, 24, 25, 26, 27, - 28, 29, 30, 31, 32, 33, - 34, 35, 36, 37, 38, 39, - 40, 41, 42, 43, 44, 45, + m66Set(10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, + 37, + 38, + 39, + 40, + 41, + 42, + 43, + 44, + 45, m66_1); - m66Set(11, 13, 15, 17, 19, 21, - 23, 25, 27, 29, 31, 33, - 35, 37, 39, 41, 43, 45, - 47, 49, 51, 53, 55, 57, - 59, 61, 63, 65, 67, 69, - 71, 73, 75, 77, 79, 81, + m66Set(11, + 13, + 15, + 17, + 19, + 21, + 23, + 25, + 27, + 29, + 31, + 33, + 35, + 37, + 39, + 41, + 43, + 45, + 47, + 49, + 51, + 53, + 55, + 57, + 59, + 61, + 63, + 65, + 67, + 69, + 71, + 73, + 75, + 77, + 79, + 81, m66_2); m66Add(m66_0, m66_1, m66_0); - if(!m66IsEqual(m66_0, m66_2, accuracy)) { + if (!m66IsEqual(m66_0, m66_2, accuracy)) { printf("m66Add failed\n"); errorCount++; } - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_0); - m66Set(10, 11, 12, 13, 14, 15, - 16, 17, 18, 19, 20, 21, - 22, 23, 24, 25, 26, 27, - 28, 29, 30, 31, 32, 33, - 34, 35, 36, 37, 38, 39, - 40, 41, 42, 43, 44, 45, + m66Set(10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, + 37, + 38, + 39, + 40, + 41, + 42, + 43, + 44, + 45, m66_1); - m66Set(-9, -9, -9, -9, -9, -9, - -9, -9, -9, -9, -9, -9, - -9, -9, -9, -9, -9, -9, - -9, -9, -9, -9, -9, -9, - -9, -9, -9, -9, -9, -9, - -9, -9, -9, -9, -9, -9, + m66Set(-9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, + -9, m66_2); m66Subtract(m66_0, m66_1, m66_0); - if(!m66IsEqual(m66_0, m66_2, accuracy)) { + if (!m66IsEqual(m66_0, m66_2, accuracy)) { printf("m66Subtract failed\n"); errorCount++; } - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_0); - m66Set(10, 11, 12, 13, 14, 15, - 16, 17, 18, 19, 20, 21, - 22, 23, 24, 25, 26, 27, - 28, 29, 30, 31, 32, 33, - 34, 35, 36, 37, 38, 39, - 40, 41, 42, 43, 44, 45, + m66Set(10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, + 37, + 38, + 39, + 40, + 41, + 42, + 43, + 44, + 45, m66_1); - m66Set(630, 651, 672, 693, 714, 735, - 1530, 1587, 1644, 1701, 1758, 1815, - 2430, 2523, 2616, 2709, 2802, 2895, - 3330, 3459, 3588, 3717, 3846, 3975, - 4230, 4395, 4560, 4725, 4890, 5055, - 5130, 5331, 5532, 5733, 5934, 6135, + m66Set(630, + 651, + 672, + 693, + 714, + 735, + 1530, + 1587, + 1644, + 1701, + 1758, + 1815, + 2430, + 2523, + 2616, + 2709, + 2802, + 2895, + 3330, + 3459, + 3588, + 3717, + 3846, + 3975, + 4230, + 4395, + 4560, + 4725, + 4890, + 5055, + 5130, + 5331, + 5532, + 5733, + 5934, + 6135, m66_2); m66MultM66(m66_0, m66_1, m66_0); - if(!m66IsEqual(m66_0, m66_2, accuracy)) { + if (!m66IsEqual(m66_0, m66_2, accuracy)) { printf("m66MultM66 failed\n"); errorCount++; } - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_0); - m66Set(10, 11, 12, 13, 14, 15, - 16, 17, 18, 19, 20, 21, - 22, 23, 24, 25, 26, 27, - 28, 29, 30, 31, 32, 33, - 34, 35, 36, 37, 38, 39, - 40, 41, 42, 43, 44, 45, + m66Set(10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, + 37, + 38, + 39, + 40, + 41, + 42, + 43, + 44, + 45, m66_1); - m66Set(3030, 3126, 3222, 3318, 3414, 3510, - 3180, 3282, 3384, 3486, 3588, 3690, - 3330, 3438, 3546, 3654, 3762, 3870, - 3480, 3594, 3708, 3822, 3936, 4050, - 3630, 3750, 3870, 3990, 4110, 4230, - 3780, 3906, 4032, 4158, 4284, 4410, + m66Set(3030, + 3126, + 3222, + 3318, + 3414, + 3510, + 3180, + 3282, + 3384, + 3486, + 3588, + 3690, + 3330, + 3438, + 3546, + 3654, + 3762, + 3870, + 3480, + 3594, + 3708, + 3822, + 3936, + 4050, + 3630, + 3750, + 3870, + 3990, + 4110, + 4230, + 3780, + 3906, + 4032, + 4158, + 4284, + 4410, m66_2); m66tMultM66(m66_0, m66_1, m66_0); - if(!m66IsEqual(m66_0, m66_2, accuracy)) { + if (!m66IsEqual(m66_0, m66_2, accuracy)) { printf("m66tMultM66 failed\n"); errorCount++; } - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_0); - m66Set(10, 11, 12, 13, 14, 15, - 16, 17, 18, 19, 20, 21, - 22, 23, 24, 25, 26, 27, - 28, 29, 30, 31, 32, 33, - 34, 35, 36, 37, 38, 39, - 40, 41, 42, 43, 44, 45, + m66Set(10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, + 37, + 38, + 39, + 40, + 41, + 42, + 43, + 44, + 45, m66_1); - m66Set(280, 406, 532, 658, 784, 910, - 730, 1072, 1414, 1756, 2098, 2440, - 1180, 1738, 2296, 2854, 3412, 3970, - 1630, 2404, 3178, 3952, 4726, 5500, - 2080, 3070, 4060, 5050, 6040, 7030, - 2530, 3736, 4942, 6148, 7354, 8560, + m66Set(280, + 406, + 532, + 658, + 784, + 910, + 730, + 1072, + 1414, + 1756, + 2098, + 2440, + 1180, + 1738, + 2296, + 2854, + 3412, + 3970, + 1630, + 2404, + 3178, + 3952, + 4726, + 5500, + 2080, + 3070, + 4060, + 5050, + 6040, + 7030, + 2530, + 3736, + 4942, + 6148, + 7354, + 8560, m66_2); m66MultM66t(m66_0, m66_1, m66_0); - if(!m66IsEqual(m66_0, m66_2, accuracy)) { + if (!m66IsEqual(m66_0, m66_2, accuracy)) { printf("m66MultM66t failed\n"); errorCount++; } - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_0); v6Set(10, 11, 12, 13, 14, 15, v6_0); v6Set(280, 730, 1180, 1630, 2080, 2530, v6_1); m66MultV6(m66_0, v6_0, v6_0); - if(!v6IsEqual(v6_0, v6_1, accuracy)) { + if (!v6IsEqual(v6_0, v6_1, accuracy)) { printf("m66MultV6 failed\n"); errorCount++; } - m66Set(1, 2, 3, 4, 5, 6, - 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 29, 30, - 31, 32, 33, 34, 35, 36, + m66Set(1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36, m66_0); v6Set(10, 11, 12, 13, 14, 15, v6_0); v6Set(1305, 1380, 1455, 1530, 1605, 1680, v6_1); m66tMultV6(m66_0, v6_0, v6_0); - if(!v6IsEqual(v6_0, v6_1, accuracy)) { + if (!v6IsEqual(v6_0, v6_1, accuracy)) { printf("m66tMultV6 failed\n"); errorCount++; } @@ -1510,7 +2285,7 @@ int testLinearAlgebra(double accuracy) v3Set(-27, -72, -6, v3_0); v3Set(12.1228937846324, -5.73450994222507, -0.38838384240732, v3_1); cubicRoots(v3_0, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("cubicRoots failed\n"); errorCount++; } @@ -1518,7 +2293,8 @@ int testLinearAlgebra(double accuracy) return errorCount; } -int testOrbitalAnomalies(double accuracy) +int +testOrbitalAnomalies(double accuracy) { int errorCount = 0; @@ -1534,63 +2310,63 @@ int testOrbitalAnomalies(double accuracy) Ecc = 0.3; e = .1; f = E2f(Ecc, e); - if(!isEqual(f, 0.33111382522243943, accuracy)) { + if (!isEqual(f, 0.33111382522243943, accuracy)) { printf("E2f(%g, %g) failed\n", Ecc, e); errorCount++; } Ecc = 0.3 + M_PI; e = .1; f = E2f(Ecc, e); - if(!isEqual(f, 3.413322139966247, accuracy)) { + if (!isEqual(f, 3.413322139966247, accuracy)) { printf("E2f(%g, %g) failed\n", Ecc, e); errorCount++; } Ecc = 0.3 + M_PI; e = .1; M = E2M(Ecc, e); - if(!isEqual(M, 3.471144674255927, accuracy)) { + if (!isEqual(M, 3.471144674255927, accuracy)) { printf("E2M(%g, %g) failed\n", Ecc, e); errorCount++; } f = 0.3; e = .1; Ecc = f2E(f, e); - if(!isEqual(Ecc, 0.2717294863764543, accuracy)) { + if (!isEqual(Ecc, 0.2717294863764543, accuracy)) { printf("f2E(%g, %g) failed\n", f, e); errorCount++; } f = 0.3; e = 2.1; H = f2H(f, e); - if(!isEqual(H, 0.18054632550895094, accuracy)) { + if (!isEqual(H, 0.18054632550895094, accuracy)) { printf("f2H(%g, %g) failed\n", f, e); errorCount++; } H = 0.3; e = 2.1; f = H2f(H, e); - if(!isEqual(f, 0.4898441475256363, accuracy)) { + if (!isEqual(f, 0.4898441475256363, accuracy)) { printf("H2f(%g, %g) failed\n", H, e); errorCount++; } H = 0.3; e = 2.1; N = H2N(H, e); - if(!isEqual(N, 0.33949261623899946, accuracy)) { + if (!isEqual(N, 0.33949261623899946, accuracy)) { printf("H2N(%g, %g) failed\n", H, e); errorCount++; } M = 2.0; e = 0.3; Ecc = M2E(M, e); - if(!isEqual(Ecc, 2.2360314951724365, accuracy)) { + if (!isEqual(Ecc, 2.2360314951724365, accuracy)) { printf("M2E(%g, %g) failed with %g \n", M, e, Ecc); errorCount++; } N = 2.0; e = 2.3; H = N2H(N, e); - if(!isEqual(H, 1.1098189302932016, accuracy)) { + if (!isEqual(H, 1.1098189302932016, accuracy)) { printf("N2H(%g, %g) failed\n", N, e); errorCount++; } @@ -1598,7 +2374,8 @@ int testOrbitalAnomalies(double accuracy) return errorCount; } -int testOrbitalHill(double accuracy) +int +testOrbitalHill(double accuracy) { int errorCount = 0; @@ -1618,10 +2395,9 @@ int testOrbitalHill(double accuracy) v3Set(-7.073840333019544, -0.5666429544308719, 2.6565522055197555, vc_N); hillFrame(rc_N, vc_N, HN); double HNtrue[3][3]; - m33Set(0.0506938, 0.931702, 0.35967, -0.93404, -0.0832604, - 0.347329, 0.353553, -0.353553, 0.866025, HNtrue); - for (int i = 0; i<3; i++) { - if(!v3IsEqualRel(HN[i], HNtrue[i], accuracy)) { + m33Set(0.0506938, 0.931702, 0.35967, -0.93404, -0.0832604, 0.347329, 0.353553, -0.353553, 0.866025, HNtrue); + for (int i = 0; i < 3; i++) { + if (!v3IsEqualRel(HN[i], HNtrue[i], accuracy)) { printf("orbitalMotion:hillFrame failed case %d\n", i); errorCount++; } @@ -1634,12 +2410,12 @@ int testOrbitalHill(double accuracy) v3Set(0.000689358, 0.000620362, 0.000927434, rhoPrime_H); hill2rv(rc_N, vc_N, rho_H, rhoPrime_H, rd_N, vd_N); v3Set(353.6672082996106, 6494.264242564805, 2507.898786238764, trueVector); - if(!v3IsEqualRel(rd_N, trueVector, accuracy)) { + if (!v3IsEqualRel(rd_N, trueVector, accuracy)) { printf("orbitalMotion:hill2rv failed case rd_N\n"); errorCount++; } v3Set(-7.073766857682589, -0.5663665778237081, 2.65770594819381, trueVector); - if(!v3IsEqualRel(vd_N, trueVector, accuracy)) { + if (!v3IsEqualRel(vd_N, trueVector, accuracy)) { printf("orbitalMotion:hill2rv failed case vd_N\n"); errorCount++; } @@ -1651,21 +2427,21 @@ int testOrbitalHill(double accuracy) v3Set(-7.073766857682589, -0.5663665778237081, 2.65770594819381, vd_N); rv2hill(rc_N, vc_N, rd_N, vd_N, rho_H, rhoPrime_H); v3Set(-0.286371, 0.012113, 0.875157, trueVector); - if(!v3IsEqualRel(rho_H, trueVector, accuracy)) { + if (!v3IsEqualRel(rho_H, trueVector, accuracy)) { printf("orbitalMotion:rv2hill failed case rho_H\n"); errorCount++; } v3Set(0.000689358, 0.000620362, 0.000927434, trueVector); - if(!v3IsEqualRel(rhoPrime_H, trueVector, accuracy)) { + if (!v3IsEqualRel(rhoPrime_H, trueVector, accuracy)) { printf("orbitalMotion:rv2hill failed case rhoPrime_H\n"); errorCount++; } - return errorCount; } -int testRigidBodyKinematics(double accuracy) +int +testRigidBodyKinematics(double accuracy) { int errorCount = 0; @@ -1682,7 +2458,7 @@ int testRigidBodyKinematics(double accuracy) v4Set(-0.18663083698528, 0.46657709246321, 0.83983876643378, -0.20529392068381, v3_2); addEP(v3_1, v3_2, v3); v4Set(-0.46986547690254, -0.34044145332460, 0.71745926113861, 0.38545850500388, v3_1); - if(!vIsEqual(v3, 4, v3_1, accuracy)) { + if (!vIsEqual(v3, 4, v3_1, accuracy)) { printf("addEP failed\n"); errorCount++; } @@ -1690,7 +2466,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler121(v3_1, v3_2, v3); v3Set(-2.96705972839036, 2.44346095279206, 1.41371669411541, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler121 failed\n"); errorCount++; } @@ -1699,7 +2475,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler123(v3_1, v3_2, v3); v3Set(2.65556257351773, -0.34257634487528, -2.38843896474589, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler123 failed\n"); errorCount++; } @@ -1708,7 +2484,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler131(v3_1, v3_2, v3); v3Set(-2.96705972839036, 2.44346095279206, 1.41371669411541, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler123 failed\n"); errorCount++; } @@ -1717,7 +2493,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler132(v3_1, v3_2, v3); v3Set(2.93168877067466, -0.89056295435594, -2.11231276758895, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler132 failed\n"); errorCount++; } @@ -1726,7 +2502,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler212(v3_1, v3_2, v3); v3Set(-2.96705972839036, 2.44346095279206, 1.41371669411541, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler212 failed\n"); errorCount++; } @@ -1735,7 +2511,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler213(v3_1, v3_2, v3); v3Set(2.93168877067466, -0.89056295435594, -2.11231276758895, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler213 failed\n"); errorCount++; } @@ -1744,7 +2520,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler231(v3_1, v3_2, v3); v3Set(2.65556257351773, -0.34257634487528, -2.38843896474589, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler231 failed\n"); errorCount++; } @@ -1753,7 +2529,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler232(v3_1, v3_2, v3); v3Set(-2.96705972839036, 2.44346095279206, 1.41371669411541, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler232 failed\n"); errorCount++; } @@ -1762,7 +2538,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler312(v3_1, v3_2, v3); v3Set(2.65556257351773, -0.34257634487528, -2.38843896474589, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler312 failed\n"); errorCount++; } @@ -1771,7 +2547,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler313(v3_1, v3_2, v3); v3Set(-2.96705972839036, 2.44346095279206, 1.41371669411541, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler313 failed\n"); errorCount++; } @@ -1780,7 +2556,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler321(v3_1, v3_2, v3); v3Set(2.93168877067466, -0.89056295435594, -2.11231276758895, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler321 failed\n"); errorCount++; } @@ -1789,7 +2565,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); addEuler323(v3_1, v3_2, v3); v3Set(-2.96705972839036, 2.44346095279206, 1.41371669411541, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addEuler323 failed\n"); errorCount++; } @@ -1798,7 +2574,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.5, 0.25, 0.15, v3_2); addGibbs(v3_1, v3_2, v3); v3Set(0.61290322580645, 0.17741935483871, 0.82258064516129, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addGibbs failed\n"); errorCount++; } @@ -1807,7 +2583,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.5, 0.25, 0.15, v3_2); addMRP(v3_1, v3_2, v3); v3Set(0.58667769962764, -0.34919321472900, 0.43690525444766, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addMRP failed\n"); errorCount++; } @@ -1816,17 +2592,16 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.0, 0.0, 1.0, v3_2); addMRP(v3_1, v3_2, v3); v3Set(0.0, 0.0, 0.0, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addMRP 360 addition test failed\n"); errorCount++; } - v3Set(1.5, 0.5, 0.5, v3_1); v3Set(-0.5, 0.25, 0.15, v3_2); addPRV(v3_1, v3_2, v3); v3Set(1.00227389370983, 0.41720669426711, 0.86837149207759, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("addPRV failed\n"); errorCount++; } @@ -1836,7 +2611,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.76604444311898, 0.0, 1.0, C2[0]); v3Set(-0.16636567534280, 0.96592582628907, 0., C2[1]); v3Set(-0.62088515301485, -0.25881904510252, 0., C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler121 failed\n"); errorCount++; } @@ -1846,7 +2621,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.73994211169385, 0.25881904510252, 0, C2[0]); v3Set(-0.19826689127415, 0.96592582628907, 0, C2[1]); v3Set(-0.64278760968654, 0, 1.00000000000000, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler123 failed\n"); errorCount++; } @@ -1856,7 +2631,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.76604444311898, 0, 1.00000000000000, C2[0]); v3Set(0.62088515301485, 0.25881904510252, 0, C2[1]); v3Set(-0.16636567534280, 0.96592582628907, 0, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler131 failed\n"); errorCount++; } @@ -1866,7 +2641,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.73994211169385, -0.25881904510252, 0, C2[0]); v3Set(0.64278760968654, 0, 1.00000000000000, C2[1]); v3Set(0.19826689127415, 0.96592582628907, 0, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler132 failed\n"); errorCount++; } @@ -1876,7 +2651,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.16636567534280, 0.96592582628907, 0, C2[0]); v3Set(0.76604444311898, 0, 1.00000000000000, C2[1]); v3Set(0.62088515301485, 0.25881904510252, 0, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler212 failed\n"); errorCount++; } @@ -1886,7 +2661,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.19826689127415, 0.96592582628907, 0, C2[0]); v3Set(0.73994211169385, -0.25881904510252, 0, C2[1]); v3Set(0.64278760968654, 0, 1.00000000000000, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler213 failed\n"); errorCount++; } @@ -1896,7 +2671,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.64278760968654, 0, 1.00000000000000, C2[0]); v3Set(0.73994211169385, 0.25881904510252, 0, C2[1]); v3Set(-0.19826689127415, 0.96592582628907, 0, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler231 failed\n"); errorCount++; } @@ -1906,7 +2681,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.62088515301485, -0.25881904510252, 0, C2[0]); v3Set(0.76604444311898, 0, 1.00000000000000, C2[1]); v3Set(-0.16636567534280, 0.96592582628907, 0, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler232 failed\n"); errorCount++; } @@ -1916,7 +2691,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.19826689127415, 0.96592582628907, 0, C2[0]); v3Set(-0.64278760968654, 0, 1.00000000000000, C2[1]); v3Set(0.73994211169385, 0.25881904510252, 0, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler312 failed\n"); errorCount++; } @@ -1926,7 +2701,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.16636567534280, 0.96592582628907, 0, C2[0]); v3Set(-0.62088515301485, -0.25881904510252, 0, C2[1]); v3Set(0.76604444311898, 0, 1.00000000000000, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler313 failed\n"); errorCount++; } @@ -1936,7 +2711,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.64278760968654, 0, 1.00000000000000, C2[0]); v3Set(0.19826689127415, 0.96592582628907, 0, C2[1]); v3Set(0.73994211169385, -0.25881904510252, 0, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler321 failed\n"); errorCount++; } @@ -1946,7 +2721,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.62088515301485, 0.25881904510252, 0, C2[0]); v3Set(-0.16636567534280, 0.96592582628907, 0, C2[1]); v3Set(0.76604444311898, 0, 1.00000000000000, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvEuler323 failed\n"); errorCount++; } @@ -1956,7 +2731,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.64, -0.32, -0.32, C2[0]); v3Set(0.32, 0.64, 0.16, C2[1]); v3Set(0.32, -0.16, 0.64, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvGibbs failed\n"); errorCount++; } @@ -1966,7 +2741,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.2304, -0.3072, -0.512, C2[0]); v3Set(0.512, 0.384, 0, C2[1]); v3Set(0.3072, -0.4096, 0.3840, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvMRP failed\n"); errorCount++; } @@ -1976,7 +2751,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.91897927113877, -0.21824360100796, -0.25875396543858, C2[0]); v3Set(0.25875396543858, 0.94936204446173, 0.07873902718102, C2[1]); v3Set(0.21824360100796, -0.15975975604225, 0.94936204446173, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BinvPRV failed\n"); errorCount++; } @@ -1986,7 +2761,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0, -0.40265095531125, -1.50271382293774, C2[0]); v3Set(0, 0.96592582628907, -0.25881904510252, C2[1]); v3Set(1.00000000000000, 0.30844852683273, 1.15114557365953, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler121 failed\n"); errorCount++; } @@ -1996,7 +2771,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(1.26092661459205, -0.33786426809485, 0, C2[0]); v3Set(0.25881904510252, 0.96592582628907, 0, C2[1]); v3Set(0.81050800458377, -0.21717496528718, 1.00000000000000, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler123 failed\n"); errorCount++; } @@ -2006,7 +2781,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0, 1.50271382293774, -0.40265095531125, C2[0]); v3Set(0, 0.25881904510252, 0.96592582628907, C2[1]); v3Set(1.00000000000000, -1.15114557365953, 0.30844852683273, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler131 failed\n"); errorCount++; } @@ -2016,7 +2791,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(1.26092661459205, 0, 0.33786426809485, C2[0]); v3Set(-0.25881904510252, 0, 0.96592582628907, C2[1]); v3Set(-0.81050800458377, 1.00000000000000, -0.21717496528718, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler132 failed\n"); errorCount++; } @@ -2026,7 +2801,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.40265095531125, 0, 1.50271382293774, C2[0]); v3Set(0.96592582628907, 0, 0.25881904510252, C2[1]); v3Set(0.30844852683273, 1.00000000000000, -1.15114557365953, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler212 failed\n"); errorCount++; } @@ -2036,7 +2811,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.33786426809485, 1.26092661459205, 0, C2[0]); v3Set(0.96592582628907, -0.25881904510252, 0, C2[1]); v3Set(-0.21717496528718, -0.81050800458377, 1.00000000000000, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler213 failed\n"); errorCount++; } @@ -2046,7 +2821,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0, 1.26092661459205, -0.33786426809485, C2[0]); v3Set(0, 0.25881904510252, 0.96592582628907, C2[1]); v3Set(1.00000000000000, 0.81050800458377, -0.21717496528718, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler231 failed\n"); errorCount++; } @@ -2056,7 +2831,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-1.50271382293774, 0, -0.40265095531125, C2[0]); v3Set(-0.25881904510252, 0, 0.96592582628907, C2[1]); v3Set(1.15114557365953, 1.00000000000000, 0.30844852683273, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler232 failed\n"); errorCount++; } @@ -2066,7 +2841,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.33786426809485, 0, 1.26092661459205, C2[0]); v3Set(0.96592582628907, 0, 0.25881904510252, C2[1]); v3Set(-0.21717496528718, 1.00000000000000, 0.81050800458377, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler312 failed\n"); errorCount++; } @@ -2076,7 +2851,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.40265095531125, -1.50271382293774, 0, C2[0]); v3Set(0.96592582628907, -0.25881904510252, 0, C2[1]); v3Set(0.30844852683273, 1.15114557365953, 1.00000000000000, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler313 failed\n"); errorCount++; } @@ -2086,7 +2861,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0, 0.33786426809485, 1.26092661459205, C2[0]); v3Set(0, 0.96592582628907, -0.25881904510252, C2[1]); v3Set(1.00000000000000, -0.21717496528718, -0.81050800458377, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler321 failed\n"); errorCount++; } @@ -2096,7 +2871,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(1.50271382293774, -0.40265095531125, 0, C2[0]); v3Set(0.25881904510252, 0.96592582628907, 0, C2[1]); v3Set(-1.15114557365953, 0.30844852683273, 1.00000000000000, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatEuler323 failed\n"); errorCount++; } @@ -2106,7 +2881,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(1.06250000000000, 0.62500000000000, 0.37500000000000, C2[0]); v3Set(-0.37500000000000, 1.25000000000000, -0.50000000000000, C2[1]); v3Set(-0.62500000000000, 0, 1.25000000000000, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatGibbs failed\n"); errorCount++; } @@ -2116,7 +2891,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.56250000000000, 1.25000000000000, 0.75000000000000, C2[0]); v3Set(-0.75000000000000, 0.93750000000000, -1.00000000000000, C2[1]); v3Set(-1.25000000000000, 0, 0.93750000000000, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatMRP failed\n"); errorCount++; } @@ -2125,10 +2900,10 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.015, 0.045, -0.005, v3_2); v3Scale(1 / D2R, v3_2, v3_2); BdotmatMRP(v3_1, v3_2, C); - v3Set(-0.4583662361046585, 1.7761691649055522, 4.1825919044550091, C2[0]); - v3Set( 0.6302535746439056, -0.1145915590261646, -4.3544792429942563, C2[1]); + v3Set(-0.4583662361046585, 1.7761691649055522, 4.1825919044550091, C2[0]); + v3Set(0.6302535746439056, -0.1145915590261646, -4.3544792429942563, C2[1]); v3Set(-6.1306484078998089, -0.9167324722093173, -0.5729577951308232, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BdotmatMRP failed\n"); errorCount++; } @@ -2138,7 +2913,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.95793740211924, 0.26051564947019, 0.23948435052981, C2[0]); v3Set(-0.23948435052981, 0.97371087632453, -0.14603129894038, C2[1]); v3Set(-0.26051564947019, 0.10396870105962, 0.97371087632453, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("BmatPRV failed\n"); errorCount++; } @@ -2148,7 +2923,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.3694748772194938, -0.9149981110691346, 0.1620702682281828, C[2]); C2EP(C, v3_1); v4Set(0.2526773896521122, 0.4276078901804977, -0.4859180570232927, 0.7191587243944733, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2EP failed\n"); errorCount++; } @@ -2158,73 +2933,73 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.3694748772194938, -0.9149981110691346, 0.1620702682281828, C[2]); C2Euler121(C, v3_1); v3Set(-3.081087141428621, 2.102046098550739, -1.127921895439695, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler121 failed\n"); errorCount++; } C2Euler123(C, v3_1); v3Set(1.395488250243478, 0.3784438476398376, 2.147410157986089, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler123 failed\n"); errorCount++; } C2Euler131(C, v3_1); v3Set(1.631301838956069, 2.102046098550739, 0.4428744313552013, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler131 failed\n"); errorCount++; } C2Euler132(C, v3_1); v3Set(-2.262757475208626, 0.8930615653924096, 2.511467464302149, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler132 failed\n"); errorCount++; } C2Euler212(C, v3_1); v3Set(-2.125637903992466, 1.982395614047245, -0.05691616561213509, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler212 failed\n"); errorCount++; } C2Euler213(C, v3_1); v3Set(1.157420789791818, 1.155503238813826, -3.012011225795042, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler213 failed\n"); errorCount++; } C2Euler231(C, v3_1); v3Set(-2.102846464319881, -0.05215813778076988, 1.982990154077466, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler231 failed\n"); errorCount++; } C2Euler232(C, v3_1); v3Set(-0.5548415771975691, 1.982395614047245, -1.627712492407032, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler232 failed\n"); errorCount++; } C2Euler312(C, v3_1); v3Set(2.045248068737305, -0.5038614866151004, -1.384653359078797, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler312 failed\n"); errorCount++; } C2Euler313(C, v3_1); v3Set(0.3837766626244829, 1.408008028147626, 2.082059614484753, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler313 failed\n"); errorCount++; } C2Euler321(C, v3_1); v3Set(-3.039045355374235, -1.036440549977791, -1.246934586231547, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler321 failed\n"); errorCount++; } C2Euler323(C, v3_1); v3Set(-1.187019664170414, 1.408008028147626, -2.630329365899936, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Euler323 failed\n"); errorCount++; } @@ -2232,26 +3007,26 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.25, 0.5, -0.5, v3_1); C2Gibbs(C, v3_1); v3Set(1.692307692307693, -1.923076923076923, 2.846153846153846, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2Gibbs failed\n"); errorCount++; } C2MRP(C, v3_1); v3Set(0.3413551595269481, -0.3879035903715318, 0.5740973137498672, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2MRP failed\n"); errorCount++; } C2PRV(C, v3_1); v3Set(1.162634795241009, -1.321175903682964, 1.955340337450788, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2PRV failed\n"); errorCount++; } m33SetIdentity(C); C2PRV(C, v3_1); v3Set(0.0, 0.0, 0.0, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2PRV failed\n"); errorCount++; } @@ -2260,7 +3035,7 @@ int testRigidBodyKinematics(double accuracy) C[1][1] = -1.0; C2PRV(C, v3_1); v3Set(0.0, 0.0, M_PI, v3_2); - if(!v3IsEqual(v3_1, v3_2, accuracy)) { + if (!v3IsEqual(v3_1, v3_2, accuracy)) { printf("C2PRV failed\n"); errorCount++; } @@ -2269,7 +3044,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.2, 0.1, -0.5, om); dEP(v3_1, om, v3); v4Set(0.1613247949317332, 0.1107893170013107, 0.1914517144671774, 0.006802852798326098, v3_1); - if(!vIsEqual(v3_1, 4, v3, accuracy)) { + if (!vIsEqual(v3_1, 4, v3, accuracy)) { printf("dEP failed\n"); errorCount++; } @@ -2278,92 +3053,92 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.2, 0.1, -0.5, om); dEuler121(v3_1, om, v3); v3Set(0.7110918159377425, 0.2260021051801672, -0.3447279341464908, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler121 failed\n"); errorCount++; } dEuler123(v3_1, om, v3); v3Set(0.2183988961089258, 0.148356391649411, -0.3596158956119647, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler123 failed\n"); errorCount++; } dEuler131(v3_1, om, v3); v3Set(0.3515968599493992, -0.4570810086342821, -0.06933882078231876, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler131 failed\n"); errorCount++; } dEuler132(v3_1, om, v3); v3Set(0.08325318887098565, -0.5347267221650382, 0.04648588172683711, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler132 failed\n"); errorCount++; } dEuler212(v3_1, om, v3); v3Set(-0.8318871025311179, 0.06377564270655334, 0.7372624921963103, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler212 failed\n"); errorCount++; } dEuler213(v3_1, om, v3); v3Set(0.1936655150781755, 0.1673032607475616, -0.6244857935158128, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler213 failed\n"); errorCount++; } dEuler231(v3_1, om, v3); v3Set(0.2950247955066306, -0.4570810086342821, 0.3896382831019671, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler231 failed\n"); errorCount++; } dEuler232(v3_1, om, v3); v3Set(-0.09921728693192147, -0.5347267221650384, 0.1760048513155397, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler232 failed\n"); errorCount++; } dEuler312(v3_1, om, v3); v3Set(-0.6980361609149971, 0.06377564270655331, -0.3486889953493196, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler312 failed\n"); errorCount++; } dEuler313(v3_1, om, v3); v3Set(-0.2308015733560238, 0.1673032607475616, -0.3231957372675008, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler312 failed\n"); errorCount++; } dEuler321(v3_1, om, v3); v3Set(-0.596676880486542, 0.2260021051801672, 0.5835365057631652, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler312 failed\n"); errorCount++; } dEuler323(v3_1, om, v3); v3Set(0.260277669056422, 0.148356391649411, -0.6993842620486324, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dEuler312 failed\n"); errorCount++; } dGibbs(v3_1, om, v3); v3Set(0.236312018677072, 0.2405875488560276, -0.1665723597065136, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dGibbs failed\n"); errorCount++; } dMRP(v3_1, om, v3); v3Set(0.144807895231133, 0.1948354871330581, 0.062187948908334, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dMRP failed\n"); errorCount++; } dPRV(v3_1, om, v3); v3Set(0.34316538031149, 0.255728121815202, -0.3710557691157747, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("dPRV failed\n"); errorCount++; } @@ -2372,7 +3147,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.0124791041517595, 0.0042760566673861, -0.0043633231299858, v3_1); dMRP2Omega(om, v3_1, v3); v3Set(0.0174532925199433, 0.0349065850398866, -0.0174532925199433, w); - if(!v3IsEqual(v3, w, accuracy)) { + if (!v3IsEqual(v3, w, accuracy)) { printf("dMRP2Omega failed\n"); errorCount++; } @@ -2381,13 +3156,13 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.0022991473184427, 0.0035194052312667, -0.0070466757773158, dw); ddMRP(om, v3_1, w, dw, v3); v3Set(0.0015, 0.0010, -0.0020, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("ddMRP failed\n"); errorCount++; } ddMRP2dOmega(om, v3_1, v3_2, v3); - if(!v3IsEqual(v3, dw, accuracy)) { + if (!v3IsEqual(v3, dw, accuracy)) { printf("ddMRP2dOmega failed\n"); errorCount++; } @@ -2395,7 +3170,7 @@ int testRigidBodyKinematics(double accuracy) v4Set(0.9110886174894189, 0.5746957711326909, -0.7662610281769212, 0.2873478855663454, v3_1); elem2PRV(v3_1, v3); v3Set(0.5235987755982988, -0.6981317007977318, 0.2617993877991494, v3_2); - if(!v3IsEqual(v3, v3_2, accuracy)) { + if (!v3IsEqual(v3, v3_2, accuracy)) { printf("elem2PRV failed\n"); errorCount++; } @@ -2405,97 +3180,97 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.506611258027956, -0.05213449187759728, 0.860596902153381, C2[0]); v3Set(-0.7789950887797505, -0.4000755572346052, -0.4828107291273137, C2[1]); v3Set(0.3694748772194938, -0.9149981110691346, 0.1620702682281828, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("EP2C failed\n"); errorCount++; } EP2Euler121(v3_1, v3_2); v3Set(3.202098165750965, 2.102046098550739, -1.127921895439695, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler121 failed\n"); errorCount++; } EP2Euler123(v3_1, v3_2); v3Set(1.395488250243478, 0.3784438476398376, 2.147410157986089, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler123 failed\n"); errorCount++; } EP2Euler131(v3_1, v3_2); v3Set(1.631301838956069, 2.102046098550739, 0.4428744313552013, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler131 failed\n"); errorCount++; } EP2Euler132(v3_1, v3_2); v3Set(-2.262757475208626, 0.8930615653924096, 2.511467464302149, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler132 failed\n"); errorCount++; } EP2Euler212(v3_1, v3_2); v3Set(-2.125637903992466, 1.982395614047245, -0.05691616561213508, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler212 failed\n"); errorCount++; } EP2Euler213(v3_1, v3_2); v3Set(1.157420789791818, 1.155503238813826, -3.012011225795042, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler213 failed\n"); errorCount++; } EP2Euler231(v3_1, v3_2); v3Set(-2.102846464319881, -0.05215813778076988, 1.982990154077466, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler231 failed\n"); errorCount++; } EP2Euler232(v3_1, v3_2); v3Set(-0.5548415771975691, 1.982395614047245, -1.627712492407032, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler232 failed\n"); errorCount++; } EP2Euler312(v3_1, v3_2); v3Set(2.045248068737305, -0.5038614866151004, -1.384653359078797, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler312 failed\n"); errorCount++; } EP2Euler313(v3_1, v3_2); v3Set(0.3837766626244828, 1.408008028147627, 2.082059614484753, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler313 failed\n"); errorCount++; } EP2Euler321(v3_1, v3_2); v3Set(-3.039045355374235, -1.036440549977791, -1.246934586231547, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler321 failed\n"); errorCount++; } EP2Euler323(v3_1, v3_2); v3Set(-1.187019664170414, 1.408008028147627, 3.65285594127965, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Euler323 failed\n"); errorCount++; } EP2Gibbs(v3_1, v3_2); v3Set(1.692307692307693, -1.923076923076923, 2.846153846153846, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2Gibbs failed\n"); errorCount++; } EP2MRP(v3_1, v3_2); v3Set(0.3413551595269481, -0.3879035903715319, 0.5740973137498672, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2MRP failed\n"); errorCount++; } EP2PRV(v3_1, v3_2); v3Set(1.162634795241009, -1.321175903682965, 1.955340337450788, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2PRV failed\n"); errorCount++; } @@ -2503,32 +3278,30 @@ int testRigidBodyKinematics(double accuracy) v4Set(1.0, 0.0, 0.0, 0.0, v3_1); EP2PRV(v3_1, v3_2); v3Set(0.0, 0.0, 0.0, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2PRV failed\n"); errorCount++; } v4Set(-1.0, 0.0, 0.0, 0.0, v3_1); EP2PRV(v3_1, v3_2); v3Set(0.0, 0.0, 0.0, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2PRV failed\n"); errorCount++; } v4Set(0.0, 1.0, 0.0, 0.0, v3_1); EP2PRV(v3_1, v3_2); v3Set(M_PI, 0.0, 0.0, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("EP2PRV failed\n"); errorCount++; } - - Euler1(1.3, C); v3Set(1, 0, 0, C2[0]); v3Set(0, 0.2674988286245874, 0.963558185417193, C2[1]); v3Set(0, -0.963558185417193, 0.2674988286245874, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler1 failed\n"); errorCount++; } @@ -2536,7 +3309,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.2674988286245874, 0, -0.963558185417193, C2[0]); v3Set(0, 1, 0, C2[1]); v3Set(0.963558185417193, 0, 0.2674988286245874, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler2 failed\n"); errorCount++; } @@ -2544,7 +3317,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.2674988286245874, 0.963558185417193, 0, C2[0]); v3Set(-0.963558185417193, 0.2674988286245874, 0, C2[1]); v3Set(0, 0, 1, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler3 failed\n"); errorCount++; } @@ -2554,31 +3327,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.7205084754311385, -0.3769430728235922, 0.5820493593177511, C2[0]); v3Set(-0.1965294640304305, 0.6939446195986547, 0.692688266609151, C2[1]); v3Set(-0.6650140649638986, -0.6134776155495705, 0.4259125598286639, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler1212C failed\n"); errorCount++; } Euler1212EP(v3_1, v3_2); v4Set(0.8426692196316502, 0.3875084824890354, -0.3699741829975614, -0.05352444488005169, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler1212EP failed\n"); errorCount++; } Euler1212Gibbs(v3_1, v3_2); v3Set(0.4598583565902931, -0.4390503110571495, -0.06351774057138154, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1212Gibbs failed\n"); errorCount++; } Euler1212MRP(v3_1, v3_2); v3Set(0.2102973655610845, -0.2007816590497557, -0.02904723447366817, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1212MRP failed\n"); errorCount++; } Euler1212PRV(v3_1, v3_2); v3Set(0.8184049632304388, -0.7813731087574279, -0.1130418386266624, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1212PRV failed\n"); errorCount++; } @@ -2588,31 +3361,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.6909668228739537, -0.1236057418710468, 0.7122404581768593, C2[0]); v3Set(-0.2041991989591971, 0.9117724894309838, 0.3563335721781613, C2[1]); v3Set(-0.6934461311680212, -0.391653607277317, 0.6047643467291773, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler1232C failed\n"); errorCount++; } Euler1232EP(v3_1, v3_2); v4Set(0.8954752451958283, 0.2088240806958052, -0.3924414987701519, 0.02250019124496444, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler1232EP failed\n"); errorCount++; } Euler1232Gibbs(v3_1, v3_2); v3Set(0.2331991663824702, -0.4382494109977661, 0.02512653628972619, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1232Gibbs failed\n"); errorCount++; } Euler1232MRP(v3_1, v3_2); v3Set(0.1101697746911123, -0.2070412155288303, 0.01187047485953311, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1232MRP failed\n"); errorCount++; } Euler1232PRV(v3_1, v3_2); v3Set(0.4328366663508259, -0.8134266388215754, 0.04663690000825693, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1232PRV failed\n"); errorCount++; } @@ -2622,31 +3395,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.7205084754311385, -0.5820493593177511, -0.3769430728235922, C2[0]); v3Set(0.6650140649638986, 0.4259125598286639, 0.6134776155495705, C2[1]); v3Set(-0.1965294640304305, -0.692688266609151, 0.6939446195986547, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler1312C failed\n"); errorCount++; } Euler1312EP(v3_1, v3_2); v4Set(0.8426692196316502, 0.3875084824890354, 0.05352444488005169, -0.3699741829975614, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler1312EP failed\n"); errorCount++; } Euler1312Gibbs(v3_1, v3_2); v3Set(0.4598583565902931, 0.06351774057138154, -0.4390503110571495, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1312Gibbs failed\n"); errorCount++; } Euler1312MRP(v3_1, v3_2); v3Set(0.2102973655610845, 0.02904723447366817, -0.2007816590497557, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1312MRP failed\n"); errorCount++; } Euler1312PRV(v3_1, v3_2); v3Set(0.8184049632304388, 0.1130418386266624, -0.7813731087574279, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1312PRV failed\n"); errorCount++; } @@ -2656,31 +3429,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.6909668228739537, -0.404128912281835, -0.5993702294453531, C2[0]); v3Set(0.6934461311680212, 0.6047643467291773, 0.391653607277317, C2[1]); v3Set(0.2041991989591971, -0.6862506154337003, 0.6981137299618809, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler1322C failed\n"); errorCount++; } Euler1322EP(v3_1, v3_2); v4Set(0.8651365354042408, 0.3114838463640192, 0.2322088466732818, -0.3171681574333834, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler1322EP failed\n"); errorCount++; } Euler1322Gibbs(v3_1, v3_2); v3Set(0.3600401018996109, 0.2684071671586273, -0.3666105226791566, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1322Gibbs failed\n"); errorCount++; } Euler1322MRP(v3_1, v3_2); v3Set(0.1670032410235906, 0.1244996504360223, -0.1700509058789317, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1322MRP failed\n"); errorCount++; } Euler1322PRV(v3_1, v3_2); v3Set(0.6525765328552258, 0.4864908592507521, -0.6644854907437873, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler1322PRV failed\n"); errorCount++; } @@ -2690,31 +3463,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.6939446195986547, -0.1965294640304305, -0.692688266609151, C2[0]); v3Set(-0.3769430728235922, 0.7205084754311385, -0.5820493593177511, C2[1]); v3Set(0.6134776155495705, 0.6650140649638986, 0.4259125598286639, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler2122C failed\n"); errorCount++; } Euler2122EP(v3_1, v3_2); v4Set(0.8426692196316502, -0.3699741829975614, 0.3875084824890354, 0.05352444488005169, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler2122EP failed\n"); errorCount++; } Euler2122Gibbs(v3_1, v3_2); v3Set(-0.4390503110571495, 0.4598583565902931, 0.06351774057138154, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2122Gibbs failed\n"); errorCount++; } Euler2122MRP(v3_1, v3_2); v3Set(-0.2007816590497557, 0.2102973655610845, 0.02904723447366817, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2122MRP failed\n"); errorCount++; } Euler2122PRV(v3_1, v3_2); v3Set(-0.7813731087574279, 0.8184049632304388, 0.1130418386266624, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2122PRV failed\n"); errorCount++; } @@ -2724,31 +3497,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.6981137299618809, 0.2041991989591971, -0.6862506154337003, C2[0]); v3Set(-0.5993702294453531, 0.6909668228739537, -0.404128912281835, C2[1]); v3Set(0.391653607277317, 0.6934461311680212, 0.6047643467291773, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler2132C failed\n"); errorCount++; } Euler2132EP(v3_1, v3_2); v4Set(0.8651365354042408, -0.3171681574333834, 0.3114838463640192, 0.2322088466732818, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler2132EP failed\n"); errorCount++; } Euler2132Gibbs(v3_1, v3_2); v3Set(-0.3666105226791566, 0.3600401018996109, 0.2684071671586273, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2132Gibbs failed\n"); errorCount++; } Euler2132MRP(v3_1, v3_2); v3Set(-0.1700509058789317, 0.1670032410235906, 0.1244996504360223, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2132MRP failed\n"); errorCount++; } Euler2132PRV(v3_1, v3_2); v3Set(-0.6644854907437873, 0.6525765328552258, 0.4864908592507521, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2132PRV failed\n"); errorCount++; } @@ -2758,31 +3531,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.6047643467291773, -0.6934461311680212, -0.391653607277317, C2[0]); v3Set(0.7122404581768593, 0.6909668228739537, -0.1236057418710468, C2[1]); v3Set(0.3563335721781613, -0.2041991989591971, 0.9117724894309838, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler2312C failed\n"); errorCount++; } Euler2312EP(v3_1, v3_2); v4Set(0.8954752451958283, 0.02250019124496444, 0.2088240806958052, -0.3924414987701519, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler2312EP failed\n"); errorCount++; } Euler2312Gibbs(v3_1, v3_2); v3Set(0.02512653628972619, 0.2331991663824702, -0.4382494109977661, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2312Gibbs failed\n"); errorCount++; } Euler2312MRP(v3_1, v3_2); v3Set(0.01187047485953311, 0.1101697746911123, -0.2070412155288303, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2312MRP failed\n"); errorCount++; } Euler2312PRV(v3_1, v3_2); v3Set(0.04663690000825693, 0.4328366663508259, -0.8134266388215754, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2312PRV failed\n"); errorCount++; } @@ -2792,31 +3565,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.4259125598286639, -0.6650140649638986, -0.6134776155495705, C2[0]); v3Set(0.5820493593177511, 0.7205084754311385, -0.3769430728235922, C2[1]); v3Set(0.692688266609151, -0.1965294640304305, 0.6939446195986547, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler2322C failed\n"); errorCount++; } Euler2322EP(v3_1, v3_2); v4Set(0.8426692196316502, -0.05352444488005169, 0.3875084824890354, -0.3699741829975614, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler2322EP failed\n"); errorCount++; } Euler2322Gibbs(v3_1, v3_2); v3Set(-0.06351774057138154, 0.4598583565902931, -0.4390503110571495, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2322Gibbs failed\n"); errorCount++; } Euler2322MRP(v3_1, v3_2); v3Set(-0.02904723447366817, 0.2102973655610845, -0.2007816590497557, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2322MRP failed\n"); errorCount++; } Euler2322PRV(v3_1, v3_2); v3Set(-0.1130418386266624, 0.8184049632304388, -0.7813731087574279, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler2322PRV failed\n"); errorCount++; } @@ -2826,31 +3599,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.9117724894309838, 0.3563335721781613, -0.2041991989591971, C2[0]); v3Set(-0.391653607277317, 0.6047643467291773, -0.6934461311680212, C2[1]); v3Set(-0.1236057418710468, 0.7122404581768593, 0.6909668228739537, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler3122C failed\n"); errorCount++; } Euler3122EP(v3_1, v3_2); v4Set(0.8954752451958283, -0.3924414987701519, 0.02250019124496444, 0.2088240806958052, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler3122EP failed\n"); errorCount++; } Euler3122Gibbs(v3_1, v3_2); v3Set(-0.4382494109977661, 0.02512653628972619, 0.2331991663824702, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3122Gibbs failed\n"); errorCount++; } Euler3122MRP(v3_1, v3_2); v3Set(-0.2070412155288303, 0.01187047485953311, 0.1101697746911123, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3122MRP failed\n"); errorCount++; } Euler3122PRV(v3_1, v3_2); v3Set(-0.8134266388215754, 0.04663690000825693, 0.4328366663508259, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3122PRV failed\n"); errorCount++; } @@ -2860,31 +3633,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.6939446195986547, 0.692688266609151, -0.1965294640304305, C2[0]); v3Set(-0.6134776155495705, 0.4259125598286639, -0.6650140649638986, C2[1]); v3Set(-0.3769430728235922, 0.5820493593177511, 0.7205084754311385, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler3132C failed\n"); errorCount++; } Euler3132EP(v3_1, v3_2); v4Set(0.8426692196316502, -0.3699741829975614, -0.05352444488005169, 0.3875084824890354, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler3132EP failed\n"); errorCount++; } Euler3132Gibbs(v3_1, v3_2); v3Set(-0.4390503110571495, -0.06351774057138154, 0.4598583565902931, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3132Gibbs failed\n"); errorCount++; } Euler3132MRP(v3_1, v3_2); v3Set(-0.2007816590497557, -0.02904723447366817, 0.2102973655610845, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3132MRP failed\n"); errorCount++; } Euler3132PRV(v3_1, v3_2); v3Set(-0.7813731087574279, -0.1130418386266624, 0.8184049632304388, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3132PRV failed\n"); errorCount++; } @@ -2894,31 +3667,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.6047643467291773, 0.391653607277317, 0.6934461311680212, C2[0]); v3Set(-0.6862506154337003, 0.6981137299618809, 0.2041991989591971, C2[1]); v3Set(-0.404128912281835, -0.5993702294453531, 0.6909668228739537, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler3212C failed\n"); errorCount++; } Euler3212EP(v3_1, v3_2); v4Set(0.8651365354042408, 0.2322088466732818, -0.3171681574333834, 0.3114838463640192, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler3212EP failed\n"); errorCount++; } Euler3212Gibbs(v3_1, v3_2); v3Set(0.2684071671586273, -0.3666105226791566, 0.3600401018996109, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3212Gibbs failed\n"); errorCount++; } Euler3212MRP(v3_1, v3_2); v3Set(0.1244996504360223, -0.1700509058789317, 0.1670032410235906, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3212MRP failed\n"); errorCount++; } Euler3212PRV(v3_1, v3_2); v3Set(0.4864908592507521, -0.6644854907437873, 0.6525765328552258, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3212PRV failed\n"); errorCount++; } @@ -2928,31 +3701,31 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.4259125598286639, 0.6134776155495705, 0.6650140649638986, C2[0]); v3Set(-0.692688266609151, 0.6939446195986547, -0.1965294640304305, C2[1]); v3Set(-0.5820493593177511, -0.3769430728235922, 0.7205084754311385, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler3232C failed\n"); errorCount++; } Euler3232EP(v3_1, v3_2); v4Set(0.8426692196316502, 0.05352444488005169, -0.3699741829975614, 0.3875084824890354, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Euler3232EP failed\n"); errorCount++; } Euler3232Gibbs(v3_1, v3_2); v3Set(0.06351774057138154, -0.4390503110571495, 0.4598583565902931, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3232Gibbs failed\n"); errorCount++; } Euler3232MRP(v3_1, v3_2); v3Set(0.02904723447366817, -0.2007816590497557, 0.2102973655610845, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3232MRP failed\n"); errorCount++; } Euler3232PRV(v3_1, v3_2); v3Set(0.1130418386266624, -0.7813731087574279, 0.8184049632304388, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Euler3232PRV failed\n"); errorCount++; } @@ -2962,104 +3735,104 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.3302752293577981, -0.1530190869107189, 0.9313986428558203, C2[0]); v3Set(-0.7277148580434096, 0.5871559633027522, 0.3545122848941588, C2[1]); v3Set(-0.6011234134980221, -0.794879257371223, 0.08256880733944938, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Gibbs2C failed\n"); errorCount++; } Gibbs2EP(v3_1, v3_2); v4Set(0.7071067811865475, 0.4063712768871578, -0.5418283691828771, 0.2031856384435789, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("Gibbs2EP failed\n"); errorCount++; } Gibbs2Euler121(v3_1, v3_2); v3Set(3.304427597008361, 1.234201174364066, -2.26121636963008, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler121 failed\n"); errorCount++; } Gibbs2Euler123(v3_1, v3_2); v3Set(1.467291629150036, -0.6449061163953342, 1.144743256726005, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler123 failed\n"); errorCount++; } Gibbs2Euler131(v3_1, v3_2); v3Set(1.733631270213465, 1.234201174364066, -0.6904200428351842, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler131 failed\n"); errorCount++; } Gibbs2Euler132(v3_1, v3_2); v3Set(0.54319335066115, 0.8149843403384446, -1.068390851022488, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler132 failed\n"); errorCount++; } Gibbs2Euler212(v3_1, v3_2); v3Set(-1.117474807766432, 0.9432554204540935, -0.1901795897648197, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler212 failed\n"); errorCount++; } Gibbs2Euler213(v3_1, v3_2); v3Set(-1.434293025994105, 0.9188085603647974, -0.2549399408440935, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler213 failed\n"); errorCount++; } Gibbs2Euler231(v3_1, v3_2); v3Set(-1.230028192223063, -0.1536226209659692, 0.9345839026955233, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler231 failed\n"); errorCount++; } Gibbs2Euler232(v3_1, v3_2); v3Set(0.4533215190284649, 0.9432554204540935, -1.760975916559716, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler232 failed\n"); errorCount++; } Gibbs2Euler312(v3_1, v3_2); v3Set(0.8918931304028546, 0.3623924238788913, -1.482377127697951, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler312 failed\n"); errorCount++; } Gibbs2Euler313(v3_1, v3_2); v3Set(-0.6474859022891233, 1.488133410155628, 1.207104533714101, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler313 failed\n"); errorCount++; } Gibbs2Euler321(v3_1, v3_2); v3Set(-0.4338654111289937, -1.198236565236741, 1.341967642658489, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler321 failed\n"); errorCount++; } Gibbs2Euler323(v3_1, v3_2); v3Set(-2.21828222908402, 1.488133410155628, 2.777900860508998, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2Euler321 failed\n"); errorCount++; } Gibbs2MRP(v3_1, v3_2); v3Set(0.2380467826416248, -0.3173957101888331, 0.1190233913208124, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2MRP failed\n"); errorCount++; } Gibbs2PRV(v3_1, v3_2); v3Set(0.9027300063197914, -1.203640008426389, 0.4513650031598956, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2PRV failed\n"); errorCount++; } v3Set(0.0, 0.0, 0.0, v3_1); Gibbs2PRV(v3_1, v3_2); v3Set(0.0, 0.0, 0.0, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("Gibbs2PRV failed\n"); errorCount++; } @@ -3069,138 +3842,137 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.1420873822677549, 0.4001248192538094, 0.9053790945330048, C2[0]); v3Set(-0.9626904702257736, 0.2686646537364468, 0.03234752493088797, C2[1]); v3Set(-0.2303003133666478, -0.876196001388834, 0.4233702077537369, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("MRP2C failed\n"); errorCount++; } MRP2EP(v3_1, v3_2); v4Set(0.6771488469601677, 0.3354297693920336, -0.419287211740042, 0.5031446540880503, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("MRP2EP failed\n"); errorCount++; } MRP2Euler121(v3_1, v3_2); v3Set(2.725460144813494, 1.428226451915784, -1.805609061169705, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler121 failed\n"); errorCount++; } MRP2Euler123(v3_1, v3_2); v3Set(1.120685944613971, -0.2323862804943196, 1.424260216144192, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler123 failed\n"); errorCount++; } MRP2Euler131(v3_1, v3_2); v3Set(1.154663818018597, 1.428226451915784, -0.2348127343748092, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler131 failed\n"); errorCount++; } MRP2Euler132(v3_1, v3_2); v3Set(0.1198243320629901, 1.296774918090265, -1.017995395279125, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler132 failed\n"); errorCount++; } MRP2Euler212(v3_1, v3_2); v3Set(-1.537207795170527, 1.298789879764913, 0.4283796513241308, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler212 failed\n"); errorCount++; } MRP2Euler213(v3_1, v3_2); v3Set(-0.4982011776145131, 1.067911809027856, 0.979488037955722, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler213 failed\n"); errorCount++; } MRP2Euler231(v3_1, v3_2); v3Set(-1.415129132201094, 0.4116530390866675, 1.273271587093173, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler231 failed\n"); errorCount++; } MRP2Euler232(v3_1, v3_2); v3Set(0.03358853162436948, 1.298789879764913, -1.142416675470766, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler232 failed\n"); errorCount++; } MRP2Euler312(v3_1, v3_2); v3Set(1.298643836753137, 0.03235316879424937, -1.133389474325039, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler312 failed\n"); errorCount++; } MRP2Euler313(v3_1, v3_2); v3Set(-0.257027406977469, 1.133634172515794, 1.535083362165219, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler313 failed\n"); errorCount++; } MRP2Euler321(v3_1, v3_2); v3Set(1.22957853325386, -1.13227169191098, 0.0762566635156139, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler321 failed\n"); errorCount++; } MRP2Euler323(v3_1, v3_2); v3Set(-1.827823733772366, 1.133634172515794, 3.105879688960115, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Euler321 failed\n"); errorCount++; } MRP2Gibbs(v3_1, v3_2); v3Set(0.4953560371517029, -0.6191950464396285, 0.7430340557275542, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2Gibbs failed\n"); errorCount++; } MRP2PRV(v3_1, v3_2); v3Set(0.7538859486650076, -0.9423574358312593, 1.130828922997511, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2PRV failed\n"); errorCount++; } MRPswitch(v3_1, 1, v3_2); v3Set(0.2, -0.25, 0.3, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2PRV failed\n"); errorCount++; } MRPswitch(v3_1, 0.4, v3_2); v3Set(-1.038961038961039, 1.298701298701299, -1.558441558441558, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2PRV failed\n"); errorCount++; } - v3Set(0.0 ,0.0, 0.0, v3_1); + v3Set(0.0, 0.0, 0.0, v3_1); MRP2PRV(v3_1, v3_2); v3Set(0.0, 0.0, 0.0, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2PRV failed\n"); errorCount++; } - v3Set(1.0 ,0.0, 0.0, v3_1); + v3Set(1.0, 0.0, 0.0, v3_1); MRP2PRV(v3_1, v3_2); v3Set(M_PI, 0.0, 0.0, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("MRP2PRV failed\n"); errorCount++; } - - if(!isEqual(wrapToPi(1.2), 1.2, accuracy)) { + if (!isEqual(wrapToPi(1.2), 1.2, accuracy)) { printf("wrapToPi(1.2) failed\n"); errorCount++; } - if(!isEqual(wrapToPi(4.2), -2.083185307179586, accuracy)) { + if (!isEqual(wrapToPi(4.2), -2.083185307179586, accuracy)) { printf("wrapToPi(4.2) failed\n"); errorCount++; } - if(!isEqual(wrapToPi(-4.2), 2.083185307179586, accuracy)) { + if (!isEqual(wrapToPi(-4.2), 2.083185307179586, accuracy)) { printf("wrapToPi(-4.2) failed\n"); errorCount++; } @@ -3210,97 +3982,97 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.9249653552860658, 0.2658656942983466, 0.2715778417245783, C2[0]); v3Set(-0.3150687400124018, 0.9360360405717283, 0.1567425271513747, C2[1]); v3Set(-0.212534186867712, -0.2305470957224576, 0.9495668781430935, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("PRV2C failed\n"); errorCount++; } PRV2EP(v3_1, v3_2); v4Set(0.9760338459808767, 0.09919984446969178, -0.1239998055871147, 0.1487997667045377, v3); - if(!vIsEqual(v3_2, 4, v3, accuracy)) { + if (!vIsEqual(v3_2, 4, v3, accuracy)) { printf("PRV2EP failed\n"); errorCount++; } PRV2Euler121(v3_1, v3_2); v3Set(2.366822457545908, 0.3898519008736288, -2.164246748437291, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler121 failed\n"); errorCount++; } PRV2Euler123(v3_1, v3_2); v3Set(0.2381830975647435, -0.2141676691157164, 0.3283009769818029, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler123 failed\n"); errorCount++; } PRV2Euler131(v3_1, v3_2); v3Set(0.796026130751012, 0.3898519008736288, -0.5934504216423945, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler131 failed\n"); errorCount++; } PRV2Euler132(v3_1, v3_2); v3Set(0.1659141638227202, 0.3205290820781828, -0.2258549616703266, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler132 failed\n"); errorCount++; } PRV2Euler212(v3_1, v3_2); v3Set(-1.109161329065078, 0.3596045976550934, 0.8564261174295806, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler212 failed\n"); errorCount++; } PRV2Euler213(v3_1, v3_2); v3Set(-0.2201931522496843, 0.2326398873102022, 0.2767451364802878, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler213 failed\n"); errorCount++; } PRV2Euler231(v3_1, v3_2); v3Set(-0.2855829177825758, 0.269101825006778, 0.2414947191533679, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler231 failed\n"); errorCount++; } PRV2Euler232(v3_1, v3_2); v3Set(0.4616349977298192, 0.3596045976550934, -0.714370209365316, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler232 failed\n"); errorCount++; } PRV2Euler312(v3_1, v3_2); v3Set(0.3246867163622526, 0.1573915425330904, -0.2785654591200913, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler312 failed\n"); errorCount++; } PRV2Euler313(v3_1, v3_2); v3Set(-0.7447668031423726, 0.3189446151924337, 1.047343966000315, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler313 failed\n"); errorCount++; } PRV2Euler321(v3_1, v3_2); v3Set(0.2798880637473677, -0.2750321114914171, 0.1635922230133545, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler321 failed\n"); errorCount++; } PRV2Euler323(v3_1, v3_2); v3Set(-2.315563129937269, 0.3189446151924337, 2.618140292795212, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Euler321 failed\n"); errorCount++; } PRV2Gibbs(v3_1, v3_2); v3Set(0.1016356603597079, -0.1270445754496348, 0.1524534905395618, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2Gibbs failed\n"); errorCount++; } PRV2MRP(v3_1, v3_2); v3Set(0.05020149056224809, -0.06275186320281011, 0.07530223584337212, v3); - if(!v3IsEqual(v3_2, v3, accuracy)) { + if (!v3IsEqual(v3_2, v3, accuracy)) { printf("PRV2MRP failed\n"); errorCount++; } @@ -3309,7 +4081,7 @@ int testRigidBodyKinematics(double accuracy) v4Set(-0.18663083698528, 0.46657709246321, 0.83983876643378, -0.20529392068381, v3_2); subEP(v3_1, v3_2, v3); v4Set(0.3010515331052196, -0.762476312817895, -0.0422034859493331, 0.5711538431809339, v3_1); - if(!vIsEqual(v3, 4, v3_1, accuracy)) { + if (!vIsEqual(v3, 4, v3_1, accuracy)) { printf("subEP failed\n"); errorCount++; } @@ -3318,7 +4090,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler121(v3_1, v3_2, v3); v3Set(2.969124082346242, 2.907100217278789, 2.423943306316236, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler121 failed\n"); errorCount++; } @@ -3327,7 +4099,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler123(v3_1, v3_2, v3); v3Set(3.116108453572625, -0.6539785291371149, -0.9652248604105184, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler123 failed\n"); errorCount++; } @@ -3336,7 +4108,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler131(v3_1, v3_2, v3); v3Set(2.969124082346242, 2.907100217278789, 2.423943306316236, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler131 failed\n"); errorCount++; } @@ -3345,7 +4117,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler132(v3_1, v3_2, v3); v3Set(2.932019083757663, 0.6246626379494424, -1.519867235625338, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler132 failed\n"); errorCount++; } @@ -3354,7 +4126,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler212(v3_1, v3_2, v3); v3Set(2.969124082346242, 2.907100217278789, 2.423943306316236, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler212 failed\n"); errorCount++; } @@ -3363,7 +4135,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler213(v3_1, v3_2, v3); v3Set(2.932019083757663, 0.6246626379494424, -1.519867235625338, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler213 failed\n"); errorCount++; } @@ -3372,7 +4144,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler231(v3_1, v3_2, v3); v3Set(3.116108453572625, -0.6539785291371149, -0.9652248604105185, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler231 failed\n"); errorCount++; } @@ -3381,7 +4153,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler232(v3_1, v3_2, v3); v3Set(2.969124082346242, 2.907100217278789, 2.423943306316236, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler232 failed\n"); errorCount++; } @@ -3390,7 +4162,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler312(v3_1, v3_2, v3); v3Set(3.116108453572625, -0.653978529137115, -0.9652248604105184, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler312 failed\n"); errorCount++; } @@ -3399,7 +4171,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler313(v3_1, v3_2, v3); v3Set(2.969124082346242, 2.907100217278789, 2.423943306316236, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler313 failed\n"); errorCount++; } @@ -3408,7 +4180,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler321(v3_1, v3_2, v3); v3Set(2.932019083757663, 0.6246626379494424, -1.519867235625338, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler321 failed\n"); errorCount++; } @@ -3417,7 +4189,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-30 * D2R, 200 * D2R, 81 * D2R, v3_2); subEuler323(v3_1, v3_2, v3); v3Set(2.969124082346242, 2.907100217278789, 2.423943306316236, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subEuler323 failed\n"); errorCount++; } @@ -3426,7 +4198,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.5, 0.25, 0.15, v3_2); subGibbs(v3_1, v3_2, v3); v3Set(4.333333333333333, -0.5, 2.166666666666667, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subGibbs failed\n"); errorCount++; } @@ -3434,8 +4206,8 @@ int testRigidBodyKinematics(double accuracy) v3Set(1.5, 0.5, 0.5, v3_1); v3Set(-0.5, 0.25, 0.15, v3_2); subMRP(v3_1, v3_2, v3); - v3Set(-0.005376344086021518,0.04301075268817203,-0.4408602150537635, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + v3Set(-0.005376344086021518, 0.04301075268817203, -0.4408602150537635, v3_1); + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subMRP failed\n"); errorCount++; } @@ -3443,8 +4215,8 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.0, 0.0, 1.0, v3_1); v3Set(0.0, 0.0, -1.0, v3_2); subMRP(v3_1, v3_2, v3); - v3Set(0.0,0.0,0.0, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + v3Set(0.0, 0.0, 0.0, v3_1); + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subMRP 360 subtraction failed\n"); errorCount++; } @@ -3453,7 +4225,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(-0.5, 0.25, 0.15, v3_2); subPRV(v3_1, v3_2, v3); v3Set(1.899971363060601, 0.06138537390284331, 0.7174863730592785, v3_1); - if(!v3IsEqual(v3, v3_1, accuracy)) { + if (!v3IsEqual(v3, v3_1, accuracy)) { printf("subPRV failed\n"); errorCount++; } @@ -3463,7 +4235,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.306186217848, 0.883883476483, 0.353553390593, C2[0]); v3Set(-0.918558653544, 0.176776695297, 0.353553390593, C2[1]); v3Set(0.250000000000, -0.433012701892, 0.866025403784, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler3132C failed\n"); errorCount++; } @@ -3473,7 +4245,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.433012701892, 0.750000000000, -0.500000000000, C2[0]); v3Set(-0.435595740399, 0.659739608441, 0.612372435696, C2[1]); v3Set(0.789149130992, -0.0473671727454, 0.612372435696, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Euler3212C failed\n"); errorCount++; } @@ -3482,7 +4254,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.8660254037844387, 0.4999999999999999, 0, C2[0]); v3Set(-0.4999999999999999, 0.8660254037844387, 0, C2[1]); v3Set(0, 0, 1.00000000000000, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Mi(30 deg, 3, C) failed\n"); errorCount++; } @@ -3491,7 +4263,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.8660254037844387, 0, -0.4999999999999999, C2[0]); v3Set(0, 1.00000000000000, 0, C2[1]); v3Set(0.4999999999999999, 0, 0.8660254037844387, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Mi(30 deg, 2, C) failed\n"); errorCount++; } @@ -3500,7 +4272,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(1.0000000000000000, 0, 0, C2[0]); v3Set(0, 0.8660254037844387, 0.4999999999999999, C2[1]); v3Set(0, -0.4999999999999999, 0.8660254037844387, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("Mi(30 deg, 1, C) failed\n"); errorCount++; } @@ -3510,7 +4282,7 @@ int testRigidBodyKinematics(double accuracy) v3Set(0.0, -3.0, 2.0, C2[0]); v3Set(3.0, 0.0, -1.0, C2[1]); v3Set(-2.0, 1.0, 0.0, C2[2]); - if(!m33IsEqual(C, C2, accuracy)) { + if (!m33IsEqual(C, C2, accuracy)) { printf("tilde() failed\n"); errorCount++; } diff --git a/src/architecture/utilitiesSelfCheck/avsLibrarySelfCheck/avsLibrarySelfCheck.h b/src/architecture/utilitiesSelfCheck/avsLibrarySelfCheck/avsLibrarySelfCheck.h index 3512db44d9..589c080325 100644 --- a/src/architecture/utilitiesSelfCheck/avsLibrarySelfCheck/avsLibrarySelfCheck.h +++ b/src/architecture/utilitiesSelfCheck/avsLibrarySelfCheck/avsLibrarySelfCheck.h @@ -20,21 +20,18 @@ #ifndef _AVS_LIBRARY_SELF_CHECK_ #define _AVS_LIBRARY_SELF_CHECK_ - - - #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif -int testLinearAlgebra(double accuracy); -int testOrbitalAnomalies(double accuracy); -int testOrbitalHill(double accuracy); -int testRigidBodyKinematics(double accuracy); + int testLinearAlgebra(double accuracy); + int testOrbitalAnomalies(double accuracy); + int testOrbitalHill(double accuracy); + int testRigidBodyKinematics(double accuracy); #ifdef __cplusplus } #endif - #endif 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/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/_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/_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/smallBodyNavEKF/smallBodyNavEKF.cpp b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.cpp index 00286192e6..ec2b54c2b3 100644 --- a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.cpp +++ b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.cpp @@ -17,7 +17,6 @@ */ - #include "fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.h" #include "architecture/utilities/linearAlgebra.h" #include "architecture/utilities/macroDefinitions.h" @@ -34,7 +33,7 @@ SmallBodyNavEKF::SmallBodyNavEKF() this->o_hat_3_tilde(0, 1) = -1; this->o_hat_3_tilde(1, 0) = 1; this->o_hat_1 << 1, 0, 0; - this->I.setIdentity(3,3); + this->I.setIdentity(3, 3); this->I_full.setIdentity(this->numStates, this->numStates); this->C_SRP = 1.0; this->P_0 = 4.56e-6; @@ -65,12 +64,12 @@ SmallBodyNavEKF::SmallBodyNavEKF() } /*! Module Destructor */ -SmallBodyNavEKF::~SmallBodyNavEKF() -{ -} +SmallBodyNavEKF::~SmallBodyNavEKF() {} /*! Initialize C-wrapped output messages */ -void SmallBodyNavEKF::SelfInit(){ +void +SmallBodyNavEKF::SelfInit() +{ NavTransMsg_C_init(&this->navTransOutMsgC); SmallBodyNavMsg_C_init(&this->smallBodyNavOutMsgC); EphemerisMsg_C_init(&this->asteroidEphemerisOutMsgC); @@ -79,7 +78,8 @@ void SmallBodyNavEKF::SelfInit(){ /*! This method is used to reset the module and checks that required input messages are connect. */ -void SmallBodyNavEKF::Reset(uint64_t CurrentSimNanos) +void +SmallBodyNavEKF::Reset(uint64_t CurrentSimNanos) { /* check that required input messages are connected */ if (!this->navTransInMsg.isLinked()) { @@ -94,13 +94,14 @@ void SmallBodyNavEKF::Reset(uint64_t CurrentSimNanos) if (!this->sunEphemerisInMsg.isLinked()) { bskLogger.bskLog(BSK_ERROR, "SmallBodyNavEKF.sunEphemerisInMsg was not linked."); } - } /*! This method is used to add a thruster to the filter. */ -void SmallBodyNavEKF::addThrusterToFilter(Message *tmpThrusterMsg){ +void +SmallBodyNavEKF::addThrusterToFilter(Message* tmpThrusterMsg) +{ this->thrusterInMsgs.push_back(tmpThrusterMsg->addSubscriber()); return; } @@ -109,9 +110,12 @@ void SmallBodyNavEKF::addThrusterToFilter(Message *tmpThrus @param CurrentSimNanos */ -void SmallBodyNavEKF::readMessages(uint64_t CurrentSimNanos){ +void +SmallBodyNavEKF::readMessages(uint64_t CurrentSimNanos) +{ /* Read in the input messages */ - if ((this->navTransInMsg.timeWritten() + this->navAttInMsg.timeWritten() + this->asteroidEphemerisInMsg.timeWritten() - 3*CurrentSimNanos) == 0){ + if ((this->navTransInMsg.timeWritten() + this->navAttInMsg.timeWritten() + + this->asteroidEphemerisInMsg.timeWritten() - 3 * CurrentSimNanos) == 0) { this->newMeasurements = true; } else { this->newMeasurements = false; @@ -121,39 +125,40 @@ void SmallBodyNavEKF::readMessages(uint64_t CurrentSimNanos){ this->asteroidEphemerisInMsgBuffer = this->asteroidEphemerisInMsg(); this->sunEphemerisInMsgBuffer = this->sunEphemerisInMsg(); - if (this->cmdForceBodyInMsg.isLinked()){ - this->cmdForceBodyInMsgBuffer= this->cmdForceBodyInMsg(); - } else{ + if (this->cmdForceBodyInMsg.isLinked()) { + this->cmdForceBodyInMsgBuffer = this->cmdForceBodyInMsg(); + } else { this->cmdForceBodyInMsgBuffer = this->cmdForceBodyInMsg.zeroMsgPayload; } - /* Read the thruster messages */ THROutputMsgPayload thrusterMsg; this->thrusterInMsgBuffer.clear(); - if (this->thrusterInMsgs.size() > 0){ - for (long unsigned int c = 0; cthrusterInMsgs.size(); c++){ + if (this->thrusterInMsgs.size() > 0) { + for (long unsigned int c = 0; c < this->thrusterInMsgs.size(); c++) { thrusterMsg = this->thrusterInMsgs.at(c)(); this->thrusterInMsgBuffer.push_back(thrusterMsg); } } else { bskLogger.bskLog(BSK_WARNING, "Small Body Nav EKF has no thruster messages to read."); } - } /*! This method performs the KF prediction step @param CurrentSimNanos */ -void SmallBodyNavEKF::predict(uint64_t CurrentSimNanos){ +void +SmallBodyNavEKF::predict(uint64_t CurrentSimNanos) +{ /* Get the orbital elements of the asteroid, we assume the uncertainty on the pos. and vel. of the body are low * enough to consider them known apriori */ rv2elem(mu_sun, asteroidEphemerisInMsgBuffer.r_BdyZero_N, asteroidEphemerisInMsgBuffer.v_BdyZero_N, &oe_ast); /* Compute F_dot and F_ddot */ - F_dot = sqrt(mu_sun/(pow(oe_ast.a*(1-pow(oe_ast.e, 2)), 3)))*pow(1+(oe_ast.e)*cos(oe_ast.f), 2); - F_ddot = -2*(oe_ast.e)*(sqrt(mu_sun/(pow(oe_ast.a*(1-pow(oe_ast.e, 2)), 3))))*sin(oe_ast.f)*(1+oe_ast.e*cos(oe_ast.f))*(F_dot); + F_dot = sqrt(mu_sun / (pow(oe_ast.a * (1 - pow(oe_ast.e, 2)), 3))) * pow(1 + (oe_ast.e) * cos(oe_ast.f), 2); + F_ddot = -2 * (oe_ast.e) * (sqrt(mu_sun / (pow(oe_ast.a * (1 - pow(oe_ast.e, 2)), 3)))) * sin(oe_ast.f) * + (1 + oe_ast.e * cos(oe_ast.f)) * (F_dot); /* Compute the hill frame DCM of the small body */ double dcm_ON_array[3][3]; @@ -162,11 +167,11 @@ void SmallBodyNavEKF::predict(uint64_t CurrentSimNanos){ /* Compute the direction of the sun from the asteroid in the small body's hill frame, assumes heliocentric frame * centered at the origin of the sun, not the solar system's barycenter*/ - Eigen::Vector3d r_ON_N; // inertial to small body pos. vector - Eigen::Vector3d r_SN_N; // inertial to sun pos. vector + Eigen::Vector3d r_ON_N; // inertial to small body pos. vector + Eigen::Vector3d r_SN_N; // inertial to sun pos. vector r_ON_N = cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.r_BdyZero_N); r_SN_N = cArray2EigenVector3d(sunEphemerisInMsgBuffer.r_BdyZero_N); - r_SO_O = dcm_ON*(r_SN_N - r_ON_N); // small body to sun pos vector + r_SO_O = dcm_ON * (r_SN_N - r_ON_N); // small body to sun pos vector /* Compute the total thrust and torque from the thrusters */ thrust_B.setZero(); // Set to zero in case no thrusters are used @@ -190,30 +195,32 @@ void SmallBodyNavEKF::predict(uint64_t CurrentSimNanos){ @param CurrentSimNanos */ -void SmallBodyNavEKF::aprioriState(uint64_t CurrentSimNanos){ +void +SmallBodyNavEKF::aprioriState(uint64_t CurrentSimNanos) +{ /* First RK4 step */ computeEquationsOfMotion(x_hat_k, Phi_k); - k1 = (CurrentSimNanos-prevTime)*NANO2SEC*x_hat_dot_k; - k1_phi = (CurrentSimNanos-prevTime)*NANO2SEC*Phi_dot_k; + k1 = (CurrentSimNanos - prevTime) * NANO2SEC * x_hat_dot_k; + k1_phi = (CurrentSimNanos - prevTime) * NANO2SEC * Phi_dot_k; /* Second RK4 step */ - computeEquationsOfMotion(x_hat_k + k1/2, Phi_k + k1_phi/2); - k2 = (CurrentSimNanos-prevTime)*NANO2SEC*x_hat_dot_k; - k2_phi = (CurrentSimNanos-prevTime)*NANO2SEC*Phi_dot_k; + computeEquationsOfMotion(x_hat_k + k1 / 2, Phi_k + k1_phi / 2); + k2 = (CurrentSimNanos - prevTime) * NANO2SEC * x_hat_dot_k; + k2_phi = (CurrentSimNanos - prevTime) * NANO2SEC * Phi_dot_k; /* Third RK4 step */ - computeEquationsOfMotion(x_hat_k + k2/2, Phi_k + k2_phi/2); - k3 = (CurrentSimNanos-prevTime)*NANO2SEC*x_hat_dot_k; - k3_phi = (CurrentSimNanos-prevTime)*NANO2SEC*Phi_dot_k; + computeEquationsOfMotion(x_hat_k + k2 / 2, Phi_k + k2_phi / 2); + k3 = (CurrentSimNanos - prevTime) * NANO2SEC * x_hat_dot_k; + k3_phi = (CurrentSimNanos - prevTime) * NANO2SEC * Phi_dot_k; /* Fourth RK4 step */ computeEquationsOfMotion(x_hat_k + k3, Phi_k + k3_phi); - k4 = (CurrentSimNanos-prevTime)*NANO2SEC*x_hat_dot_k; - k4_phi = (CurrentSimNanos-prevTime)*NANO2SEC*Phi_dot_k; + k4 = (CurrentSimNanos - prevTime) * NANO2SEC * x_hat_dot_k; + k4_phi = (CurrentSimNanos - prevTime) * NANO2SEC * Phi_dot_k; /* Perform the RK4 integration on the dynamics and STM */ - x_hat_k1_ = x_hat_k + (k1 + 2*k2 + 2*k3 + k4)/6; - Phi_k = Phi_k + (k1_phi + 2*k2_phi + 2*k3_phi + k4_phi)/6; + x_hat_k1_ = x_hat_k + (k1 + 2 * k2 + 2 * k3 + k4) / 6; + Phi_k = Phi_k + (k1_phi + 2 * k2_phi + 2 * k3_phi + k4_phi) / 6; } /*! This method calculates the EOMs of the state vector and state transition matrix @@ -221,64 +228,70 @@ void SmallBodyNavEKF::aprioriState(uint64_t CurrentSimNanos){ @param Phi */ -void SmallBodyNavEKF::computeEquationsOfMotion(Eigen::VectorXd x_hat, Eigen::MatrixXd Phi){ +void +SmallBodyNavEKF::computeEquationsOfMotion(Eigen::VectorXd x_hat, Eigen::MatrixXd Phi) +{ /* Create temporary state vectors for readability */ Eigen::Vector3d x_1; Eigen::Vector3d x_2; Eigen::Vector3d x_3; Eigen::Vector3d x_4; - x_1 << x_hat.segment(0,3); - x_2 << x_hat.segment(3,3); - x_3 << x_hat.segment(6,3); - x_4 << x_hat.segment(9,3); + x_1 << x_hat.segment(0, 3); + x_2 << x_hat.segment(3, 3); + x_3 << x_hat.segment(6, 3); + x_4 << x_hat.segment(9, 3); /* x1_dot */ - x_hat_dot_k.segment(0,3) = x_2; + x_hat_dot_k.segment(0, 3) = x_2; /* x2_dot */ /* First compute dcm_OB, DCM from sc body-frame to orbit frame*/ double dcm_BN_meas[3][3]; MRP2C(this->navAttInMsgBuffer.sigma_BN, dcm_BN_meas); Eigen::Matrix3d dcm_OB; - dcm_OB = dcm_ON*(cArray2EigenMatrixXd(*dcm_BN_meas, 3, 3)); + dcm_OB = dcm_ON * (cArray2EigenMatrixXd(*dcm_BN_meas, 3, 3)); /* Now compute x2_dot */ - x_hat_dot_k.segment(3,3) = - -F_ddot*o_hat_3_tilde*x_1 - 2*F_dot*o_hat_3_tilde*x_2 -pow(F_dot,2)*o_hat_3_tilde*o_hat_3_tilde*x_1 - - mu_ast*x_1/pow(x_1.norm(), 3) - + mu_sun*(3*(r_SO_O/r_SO_O.norm())*(r_SO_O/r_SO_O.norm()).transpose()-I)*x_1/pow(r_SO_O.norm(), 3) - + C_SRP*P_0*(1+rho)*(A_sc/M_sc)*o_hat_1/pow(r_SO_O.norm(), 2) - + dcm_OB*thrust_B/M_sc - + dcm_OB*cmdForce_B/M_sc; + x_hat_dot_k.segment(3, 3) = + -F_ddot * o_hat_3_tilde * x_1 - 2 * F_dot * o_hat_3_tilde * x_2 - + pow(F_dot, 2) * o_hat_3_tilde * o_hat_3_tilde * x_1 - mu_ast * x_1 / pow(x_1.norm(), 3) + + mu_sun * (3 * (r_SO_O / r_SO_O.norm()) * (r_SO_O / r_SO_O.norm()).transpose() - I) * x_1 / pow(r_SO_O.norm(), 3) + + C_SRP * P_0 * (1 + rho) * (A_sc / M_sc) * o_hat_1 / pow(r_SO_O.norm(), 2) + dcm_OB * thrust_B / M_sc + + dcm_OB * cmdForce_B / M_sc; /* x3_dot */ - x_hat_dot_k.segment(6,3) = 0.25*((1-pow(x_3.norm(),2))*I + 2*eigenTilde(x_3) + 2*x_3*x_3.transpose())*x_4; + x_hat_dot_k.segment(6, 3) = + 0.25 * ((1 - pow(x_3.norm(), 2)) * I + 2 * eigenTilde(x_3) + 2 * x_3 * x_3.transpose()) * x_4; /* x4_dot */ - x_hat_dot_k.segment(9,3) << 0, 0, 0; + x_hat_dot_k.segment(9, 3) << 0, 0, 0; /* Re-compute the dynamics matrix and compute Phi_dot */ computeDynamicsMatrix(x_hat); - Phi_dot_k = A_k*Phi; + Phi_dot_k = A_k * Phi; } /*! This method compute the apriori estimation error covariance through euler integration @param CurrentSimNanos */ -void SmallBodyNavEKF::aprioriCovar(uint64_t CurrentSimNanos){ +void +SmallBodyNavEKF::aprioriCovar(uint64_t CurrentSimNanos) +{ /* Compute the apriori covariance */ - P_k1_ = Phi_k*P_k*Phi_k.transpose() + L*Q*L.transpose(); + P_k1_ = Phi_k * P_k * Phi_k.transpose() + L * Q * L.transpose(); } /*! This method checks the propagated MRP states to see if they exceed a norm of 1. If they do, the appropriate states are transferred to the shadow set and the covariance is updated. */ -void SmallBodyNavEKF::checkMRPSwitching(){ +void +SmallBodyNavEKF::checkMRPSwitching() +{ /* Create temporary values for sigma_AN */ Eigen::Vector3d sigma_AN; - sigma_AN << x_hat_k1_.segment(6,3); + sigma_AN << x_hat_k1_.segment(6, 3); /* Create a shadow covariance matrix */ Eigen::MatrixXd P_k1_s; @@ -293,48 +306,50 @@ void SmallBodyNavEKF::checkMRPSwitching(){ Lambda.block(9, 9, 3, 3).setIdentity(); /* Check the attitude of the small body */ - if (sigma_AN.norm() > 1.0){ + if (sigma_AN.norm() > 1.0) { /* Switch MRPs */ - x_hat_k1_.segment(6, 3) = -sigma_AN/pow(sigma_AN.norm(), 2); + x_hat_k1_.segment(6, 3) = -sigma_AN / pow(sigma_AN.norm(), 2); /* Populate lambda block */ - Lambda.block(6, 6, 3, 3) = 2*sigma_AN*sigma_AN.transpose()/pow(sigma_AN.norm(), 4) - I/pow(sigma_AN.norm(), 2); + Lambda.block(6, 6, 3, 3) = + 2 * sigma_AN * sigma_AN.transpose() / pow(sigma_AN.norm(), 4) - I / pow(sigma_AN.norm(), 2); } /* Compute the new apriori covariance */ - P_k1_s = Lambda*P_k1_*Lambda.transpose(); + P_k1_s = Lambda * P_k1_ * Lambda.transpose(); P_k1_ = P_k1_s; } - /*! This method performs the KF measurement update step */ -void SmallBodyNavEKF::measurementUpdate(){ +void +SmallBodyNavEKF::measurementUpdate() +{ /* Compute Kalman gain */ Eigen::MatrixXd K_k1; K_k1.setZero(this->numStates, this->numStates); - K_k1 = P_k1_*H_k1.transpose() * (H_k1*P_k1_*H_k1.transpose() + M*R*M.transpose()).inverse(); + K_k1 = P_k1_ * H_k1.transpose() * (H_k1 * P_k1_ * H_k1.transpose() + M * R * M.transpose()).inverse(); /* Grab the measurements from the input messages */ /* Subtract the asteroid position from the spacecraft position and rotate it into the small body's hill frame*/ Eigen::VectorXd y_k1; y_k1.setZero(this->numStates); - y_k1.segment(0, 3) = dcm_ON*(cArray2EigenVector3d(navTransInMsgBuffer.r_BN_N) - - cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.r_BdyZero_N)); + y_k1.segment(0, 3) = dcm_ON * (cArray2EigenVector3d(navTransInMsgBuffer.r_BN_N) - + cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.r_BdyZero_N)); /* Perform a similar operation for the relative velocity */ - y_k1.segment(3, 3) = dcm_ON*(cArray2EigenVector3d(navTransInMsgBuffer.v_BN_N) - - cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.v_BdyZero_N)); + y_k1.segment(3, 3) = dcm_ON * (cArray2EigenVector3d(navTransInMsgBuffer.v_BN_N) - + cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.v_BdyZero_N)); /* Small body attitude from the ephemeris msg */ - y_k1.segment(6, 3) = cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.sigma_BN); + y_k1.segment(6, 3) = cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.sigma_BN); /* Check if the shadow set measurement must be considered, i.e. |sigma| > 1/3 */ - if (y_k1.segment(6, 3).norm() > 1.0/3.0) { + if (y_k1.segment(6, 3).norm() > 1.0 / 3.0) { /* Create a temporary shadow-set MRP representation */ - Eigen::Vector3d sigma_AN_s = -y_k1.segment(6, 3)/pow(y_k1.segment(6, 3).norm(), 2); + Eigen::Vector3d sigma_AN_s = -y_k1.segment(6, 3) / pow(y_k1.segment(6, 3).norm(), 2); /* Check to see if the shadow set gives a smaller residual */ - if ((sigma_AN_s - x_hat_k1_.segment(6, 3)).norm() < (y_k1.segment(6, 3) - x_hat_k1_.segment(6, 3)).norm()){ + if ((sigma_AN_s - x_hat_k1_.segment(6, 3)).norm() < (y_k1.segment(6, 3) - x_hat_k1_.segment(6, 3)).norm()) { y_k1.segment(6, 3) = sigma_AN_s; } } @@ -343,10 +358,11 @@ void SmallBodyNavEKF::measurementUpdate(){ y_k1.segment(9, 3) = cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.omega_BN_B); /* Update the state estimate */ - x_hat_k1 = x_hat_k1_ + K_k1*(y_k1 - x_hat_k1_); + x_hat_k1 = x_hat_k1_ + K_k1 * (y_k1 - x_hat_k1_); /* Update the covariance */ - P_k1 = (I_full - K_k1*H_k1)*P_k1_*(I_full - K_k1*H_k1).transpose() + K_k1*M*R*M.transpose()*K_k1.transpose(); + P_k1 = (I_full - K_k1 * H_k1) * P_k1_ * (I_full - K_k1 * H_k1).transpose() + + K_k1 * M * R * M.transpose() * K_k1.transpose(); /* Assign the state estimate and covariance to k for the next iteration */ x_hat_k = x_hat_k1; @@ -356,17 +372,19 @@ void SmallBodyNavEKF::measurementUpdate(){ /*! This method computes the state dynamics matrix, A, for the next iteration */ -void SmallBodyNavEKF::computeDynamicsMatrix(Eigen::VectorXd x_hat){ +void +SmallBodyNavEKF::computeDynamicsMatrix(Eigen::VectorXd x_hat) +{ /* Create temporary state vectors for readability */ Eigen::Vector3d x_1; Eigen::Vector3d x_2; Eigen::Vector3d x_3; Eigen::Vector3d x_4; - x_1 << x_hat.segment(0,3); - x_2 << x_hat.segment(3,3); - x_3 << x_hat.segment(6,3); - x_4 << x_hat.segment(9,3); + x_1 << x_hat.segment(0, 3); + x_2 << x_hat.segment(3, 3); + x_3 << x_hat.segment(6, 3); + x_4 << x_hat.segment(9, 3); /* First set the matrix to zero (many indices are zero) */ A_k.setZero(this->numStates, this->numStates); @@ -376,32 +394,31 @@ void SmallBodyNavEKF::computeDynamicsMatrix(Eigen::VectorXd x_hat){ /* x_2 partial */ A_k.block(3, 0, 3, 3) = - - F_ddot*o_hat_3_tilde - - pow(F_dot, 2)*o_hat_3_tilde*o_hat_3_tilde - - mu_ast/pow(x_1.norm(), 3)*I - + 3*mu_ast*x_1*x_1.transpose()/pow(x_1.norm(), 5) - + mu_sun*(3*(r_SO_O*r_SO_O.transpose())/pow(r_SO_O.norm(), 2) - I)/pow(r_SO_O.norm(), 3); + -F_ddot * o_hat_3_tilde - pow(F_dot, 2) * o_hat_3_tilde * o_hat_3_tilde - mu_ast / pow(x_1.norm(), 3) * I + + 3 * mu_ast * x_1 * x_1.transpose() / pow(x_1.norm(), 5) + + mu_sun * (3 * (r_SO_O * r_SO_O.transpose()) / pow(r_SO_O.norm(), 2) - I) / pow(r_SO_O.norm(), 3); - A_k.block(3, 3, 3, 3) = -2*F_dot*o_hat_3_tilde; + A_k.block(3, 3, 3, 3) = -2 * F_dot * o_hat_3_tilde; /* x_3 partial */ - A_k.block(6, 6, 3, 3) = 0.5*(x_3*x_4.transpose() - x_4*x_3.transpose() - eigenTilde(x_4) + (x_4.transpose()*x_3)*I); - A_k.block(6, 9, 3, 3) = 0.25*((1-pow(x_3.norm(), 2))*I + 2*eigenTilde(x_3) + 3*x_3*x_3.transpose()); + A_k.block(6, 6, 3, 3) = + 0.5 * (x_3 * x_4.transpose() - x_4 * x_3.transpose() - eigenTilde(x_4) + (x_4.transpose() * x_3) * I); + A_k.block(6, 9, 3, 3) = 0.25 * ((1 - pow(x_3.norm(), 2)) * I + 2 * eigenTilde(x_3) + 3 * x_3 * x_3.transpose()); } /*! This is the main method that gets called every time the module is updated. */ -void SmallBodyNavEKF::UpdateState(uint64_t CurrentSimNanos) +void +SmallBodyNavEKF::UpdateState(uint64_t CurrentSimNanos) { this->readMessages(CurrentSimNanos); this->predict(CurrentSimNanos); this->checkMRPSwitching(); - if (this->newMeasurements){ + if (this->newMeasurements) { /* Run the measurement update */ this->measurementUpdate(); - } - else{ + } else { /* Assign the apriori state estimate and covariance to k for the next iteration */ x_hat_k = x_hat_k1_; } @@ -412,7 +429,9 @@ void SmallBodyNavEKF::UpdateState(uint64_t CurrentSimNanos) /*! This method writes the output messages */ -void SmallBodyNavEKF::writeMessages(uint64_t CurrentSimNanos){ +void +SmallBodyNavEKF::writeMessages(uint64_t CurrentSimNanos) +{ /* Create output msg buffers */ NavTransMsgPayload navTransOutMsgBuffer; SmallBodyNavMsgPayload smallBodyNavOutMsgBuffer; @@ -425,15 +444,21 @@ void SmallBodyNavEKF::writeMessages(uint64_t CurrentSimNanos){ /* Assign values to the nav trans output message */ navTransOutMsgBuffer.timeTag = navTransInMsgBuffer.timeTag; - eigenMatrixXd2CArray(cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.r_BdyZero_N) + dcm_ON.transpose()*x_hat_k.segment(0,3), navTransOutMsgBuffer.r_BN_N); - eigenMatrixXd2CArray(cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.v_BdyZero_N) + dcm_ON.transpose()*x_hat_k.segment(3,3), navTransOutMsgBuffer.v_BN_N); - v3Copy(navTransOutMsgBuffer.vehAccumDV, navTransInMsgBuffer.vehAccumDV); // Not an estimated parameter, pass through + eigenMatrixXd2CArray(cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.r_BdyZero_N) + + dcm_ON.transpose() * x_hat_k.segment(0, 3), + navTransOutMsgBuffer.r_BN_N); + eigenMatrixXd2CArray(cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.v_BdyZero_N) + + dcm_ON.transpose() * x_hat_k.segment(3, 3), + navTransOutMsgBuffer.v_BN_N); + v3Copy(navTransOutMsgBuffer.vehAccumDV, navTransInMsgBuffer.vehAccumDV); // Not an estimated parameter, pass through /* Assign values to the asteroid ephemeris output message */ - v3Copy(asteroidEphemerisOutMsgBuffer.r_BdyZero_N, asteroidEphemerisInMsgBuffer.r_BdyZero_N); // Not an estimated parameter - v3Copy(asteroidEphemerisOutMsgBuffer.v_BdyZero_N, asteroidEphemerisInMsgBuffer.v_BdyZero_N); // Not an estimated parameter - eigenMatrixXd2CArray(x_hat_k.segment(6,3), asteroidEphemerisOutMsgBuffer.sigma_BN); - eigenMatrixXd2CArray(x_hat_k.segment(9,3), asteroidEphemerisOutMsgBuffer.omega_BN_B); + v3Copy(asteroidEphemerisOutMsgBuffer.r_BdyZero_N, + asteroidEphemerisInMsgBuffer.r_BdyZero_N); // Not an estimated parameter + v3Copy(asteroidEphemerisOutMsgBuffer.v_BdyZero_N, + asteroidEphemerisInMsgBuffer.v_BdyZero_N); // Not an estimated parameter + eigenMatrixXd2CArray(x_hat_k.segment(6, 3), asteroidEphemerisOutMsgBuffer.sigma_BN); + eigenMatrixXd2CArray(x_hat_k.segment(9, 3), asteroidEphemerisOutMsgBuffer.omega_BN_B); asteroidEphemerisOutMsgBuffer.timeTag = asteroidEphemerisInMsgBuffer.timeTag; /* Assign values to the small body navigation output message */ @@ -452,5 +477,6 @@ void SmallBodyNavEKF::writeMessages(uint64_t CurrentSimNanos){ /* Write to the C-wrapped output messages */ NavTransMsg_C_write(&navTransOutMsgBuffer, &this->navTransOutMsgC, this->moduleID, CurrentSimNanos); SmallBodyNavMsg_C_write(&smallBodyNavOutMsgBuffer, &this->smallBodyNavOutMsgC, this->moduleID, CurrentSimNanos); - EphemerisMsg_C_write(&asteroidEphemerisOutMsgBuffer, &this->asteroidEphemerisOutMsgC, this->moduleID, CurrentSimNanos); + EphemerisMsg_C_write( + &asteroidEphemerisOutMsgBuffer, &this->asteroidEphemerisOutMsgC, this->moduleID, CurrentSimNanos); } diff --git a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.h b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.h index 2b0783ffbd..1913ff2bb2 100644 --- a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.h +++ b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavEKF/smallBodyNavEKF.h @@ -17,7 +17,6 @@ */ - #ifndef SMALLBODYNAVEKF_H #define SMALLBODYNAVEKF_H @@ -33,104 +32,106 @@ #include "architecture/utilities/orbitalMotion.h" #include "architecture/utilities/avsEigenSupport.h" -/*! @brief This module estimates relative spacecraft position and velocity with respect to the body and attitude and attitude rate of the body wrt. the inertial frame +/*! @brief This module estimates relative spacecraft position and velocity with respect to the body and attitude and + * attitude rate of the body wrt. the inertial frame */ -class SmallBodyNavEKF: public SysModel { -public: +class SmallBodyNavEKF : public SysModel +{ + public: SmallBodyNavEKF(); ~SmallBodyNavEKF(); - void SelfInit(); //!< Self initialization for C-wrapped messages - void Reset(uint64_t CurrentSimNanos); //!< Resets module - void UpdateState(uint64_t CurrentSimNanos); //!< Updates state - void addThrusterToFilter(Message *tmpThrusterMsg); //!< Adds thruster message - -private: - void readMessages(uint64_t CurrentSimNanos); //!< Reads input messages - void writeMessages(uint64_t CurrentSimNanos); //!< Writes output messages - void predict(uint64_t CurrentSimNanos); //!< Prediction step of Kalman filter - void aprioriState(uint64_t CurrentSimNanos); //!< Computes the apriori state - void aprioriCovar(uint64_t CurrentSimNanos); //!< Computes the apriori covariance - void checkMRPSwitching(); //!< Checks the MRPs for switching - void computeDynamicsMatrix(Eigen::VectorXd x_hat); //!< Computes the new dynamics matrix, A_k - void measurementUpdate(); //!< Computes the measurement update for the EKF - void computeEquationsOfMotion(Eigen::VectorXd x_hat, Eigen::MatrixXd Phi); //!< Computes the EOMs of the state and state transition matrix - -public: - ReadFunctor navTransInMsg; //!< Translational nav input message - ReadFunctor navAttInMsg; //!< Attitude nav input message - ReadFunctor asteroidEphemerisInMsg; //!< Small body ephemeris input message - ReadFunctor sunEphemerisInMsg; //!< Sun ephemeris input message - ReadFunctor cmdForceBodyInMsg; //!< Command force body in message - std::vector> thrusterInMsgs; //!< thruster input msg vector - - Message navTransOutMsg; //!< Translational nav output message - Message smallBodyNavOutMsg; //!< Small body nav output msg - states and covariances - Message asteroidEphemerisOutMsg; //!< Small body ephemeris output message - - NavTransMsg_C navTransOutMsgC = {}; //!< C-wrapped Translational nav output message - SmallBodyNavMsg_C smallBodyNavOutMsgC = {}; //!< C-wrapped Small body nav output msg - states and covariances - EphemerisMsg_C asteroidEphemerisOutMsgC = {}; //!< C-wrapped Small body ephemeris output message - - BSKLogger bskLogger; //!< -- BSK Logging - - double C_SRP; //!< SRP scaling coefficient - double P_0; //!< SRP at 1 AU - double rho; //!< Surface reflectivity - double A_sc; //!< Surface area of the spacecraft - double M_sc; //!< Mass of the spacecraft - double mu_ast; //!< Gravitational constant of the asteroid - Eigen::MatrixXd Q; //!< Process Noise - Eigen::MatrixXd R; //!< Measurement Noise - Eigen::VectorXd x_hat_k; //!< Current state estimate - Eigen::MatrixXd P_k; //!< Current estimation error covariance - - -private: - NavTransMsgPayload navTransInMsgBuffer; //!< Message buffer for input translational nav message - NavAttMsgPayload navAttInMsgBuffer; //!< Message buffer for input attitude nav message - EphemerisMsgPayload asteroidEphemerisInMsgBuffer; //!< Message buffer for asteroid ephemeris - EphemerisMsgPayload sunEphemerisInMsgBuffer; //!< Message buffer for sun ephemeris + void SelfInit(); //!< Self initialization for C-wrapped messages + void Reset(uint64_t CurrentSimNanos); //!< Resets module + void UpdateState(uint64_t CurrentSimNanos); //!< Updates state + void addThrusterToFilter(Message* tmpThrusterMsg); //!< Adds thruster message + + private: + void readMessages(uint64_t CurrentSimNanos); //!< Reads input messages + void writeMessages(uint64_t CurrentSimNanos); //!< Writes output messages + void predict(uint64_t CurrentSimNanos); //!< Prediction step of Kalman filter + void aprioriState(uint64_t CurrentSimNanos); //!< Computes the apriori state + void aprioriCovar(uint64_t CurrentSimNanos); //!< Computes the apriori covariance + void checkMRPSwitching(); //!< Checks the MRPs for switching + void computeDynamicsMatrix(Eigen::VectorXd x_hat); //!< Computes the new dynamics matrix, A_k + void measurementUpdate(); //!< Computes the measurement update for the EKF + void computeEquationsOfMotion(Eigen::VectorXd x_hat, + Eigen::MatrixXd Phi); //!< Computes the EOMs of the state and state transition matrix + + public: + ReadFunctor navTransInMsg; //!< Translational nav input message + ReadFunctor navAttInMsg; //!< Attitude nav input message + ReadFunctor asteroidEphemerisInMsg; //!< Small body ephemeris input message + ReadFunctor sunEphemerisInMsg; //!< Sun ephemeris input message + ReadFunctor cmdForceBodyInMsg; //!< Command force body in message + std::vector> thrusterInMsgs; //!< thruster input msg vector + + Message navTransOutMsg; //!< Translational nav output message + Message smallBodyNavOutMsg; //!< Small body nav output msg - states and covariances + Message asteroidEphemerisOutMsg; //!< Small body ephemeris output message + + NavTransMsg_C navTransOutMsgC = {}; //!< C-wrapped Translational nav output message + SmallBodyNavMsg_C smallBodyNavOutMsgC = {}; //!< C-wrapped Small body nav output msg - states and covariances + EphemerisMsg_C asteroidEphemerisOutMsgC = {}; //!< C-wrapped Small body ephemeris output message + + BSKLogger bskLogger; //!< -- BSK Logging + + double C_SRP; //!< SRP scaling coefficient + double P_0; //!< SRP at 1 AU + double rho; //!< Surface reflectivity + double A_sc; //!< Surface area of the spacecraft + double M_sc; //!< Mass of the spacecraft + double mu_ast; //!< Gravitational constant of the asteroid + Eigen::MatrixXd Q; //!< Process Noise + Eigen::MatrixXd R; //!< Measurement Noise + Eigen::VectorXd x_hat_k; //!< Current state estimate + Eigen::MatrixXd P_k; //!< Current estimation error covariance + + private: + NavTransMsgPayload navTransInMsgBuffer; //!< Message buffer for input translational nav message + NavAttMsgPayload navAttInMsgBuffer; //!< Message buffer for input attitude nav message + EphemerisMsgPayload asteroidEphemerisInMsgBuffer; //!< Message buffer for asteroid ephemeris + EphemerisMsgPayload sunEphemerisInMsgBuffer; //!< Message buffer for sun ephemeris std::vector thrusterInMsgBuffer; //!< Buffer for thruster force and torques - CmdForceBodyMsgPayload cmdForceBodyInMsgBuffer; //!< Buffer for the commanded force input + CmdForceBodyMsgPayload cmdForceBodyInMsgBuffer; //!< Buffer for the commanded force input - uint64_t prevTime; //!< Previous time, ns - uint64_t numStates; //!< Number of states - Eigen::Vector3d thrust_B; //!< Thrust expressed in body-frame components + uint64_t prevTime; //!< Previous time, ns + uint64_t numStates; //!< Number of states + Eigen::Vector3d thrust_B; //!< Thrust expressed in body-frame components Eigen::Vector3d cmdForce_B; //!< External force expressed in body-frame components - Eigen::VectorXd x_hat_dot_k; //!< Rate of change of state estimate - Eigen::VectorXd x_hat_k1_; //!< Apriori state estimate for time k+1 - Eigen::VectorXd x_hat_k1; //!< Update state estimate for time k+1 - Eigen::MatrixXd P_dot_k; //!< Rate of change of estimation error covariance - Eigen::MatrixXd P_k1_; //!< Apriori estimation error covariance - Eigen::MatrixXd P_k1; //!< Updated estimation error covariance - Eigen::MatrixXd A_k; //!< State dynamics matrix - Eigen::MatrixXd Phi_k; //!< State transition matrix - Eigen::MatrixXd Phi_dot_k; //!< Rate of change of STM - Eigen::MatrixXd L; //!< - Eigen::MatrixXd M; //!< - Eigen::MatrixXd H_k1; //!< Jacobian of measurement model - Eigen::MatrixXd I_full; //!< numStates x numStates identity matrix - Eigen::VectorXd k1; //!< k1 constant for RK4 integration - Eigen::VectorXd k2; //!< k2 constant for RK4 integration - Eigen::VectorXd k3; //!< k3 constant for RK4 integration - Eigen::VectorXd k4; //!< k4 constant for RK4 integration - Eigen::MatrixXd k1_phi; //!< k1 STM constant for RK4 integration - Eigen::MatrixXd k2_phi; //!< k2 STM constant for RK4 integration - Eigen::MatrixXd k3_phi; //!< k3 STM constant for RK4 integration - Eigen::MatrixXd k4_phi; //!< k4 STM constant for RK4 integration - bool newMeasurements; //! Keeps track of whether or not new measurements have been written - - double mu_sun; //!< Gravitational parameter of the sun - Eigen::Matrix3d o_hat_3_tilde; //!< Tilde matrix of the third asteroid orbit frame base vector - Eigen::Vector3d o_hat_1; //!< First asteroid orbit frame base vector - Eigen::MatrixXd I; //!< 3 x 3 identity matrix - ClassicElements oe_ast; //!< Orbital elements of the asteroid - double F_dot; //!< Time rate of change of true anomaly - double F_ddot; //!< Second time derivative of true anomaly - Eigen::Matrix3d dcm_ON; //!< DCM from the inertial frame to the small-body's hill frame - Eigen::Vector3d r_SO_O; //!< Vector from the small body's origin to the inertial frame origin in small-body hill frame components + Eigen::VectorXd x_hat_dot_k; //!< Rate of change of state estimate + Eigen::VectorXd x_hat_k1_; //!< Apriori state estimate for time k+1 + Eigen::VectorXd x_hat_k1; //!< Update state estimate for time k+1 + Eigen::MatrixXd P_dot_k; //!< Rate of change of estimation error covariance + Eigen::MatrixXd P_k1_; //!< Apriori estimation error covariance + Eigen::MatrixXd P_k1; //!< Updated estimation error covariance + Eigen::MatrixXd A_k; //!< State dynamics matrix + Eigen::MatrixXd Phi_k; //!< State transition matrix + Eigen::MatrixXd Phi_dot_k; //!< Rate of change of STM + Eigen::MatrixXd L; //!< + Eigen::MatrixXd M; //!< + Eigen::MatrixXd H_k1; //!< Jacobian of measurement model + Eigen::MatrixXd I_full; //!< numStates x numStates identity matrix + Eigen::VectorXd k1; //!< k1 constant for RK4 integration + Eigen::VectorXd k2; //!< k2 constant for RK4 integration + Eigen::VectorXd k3; //!< k3 constant for RK4 integration + Eigen::VectorXd k4; //!< k4 constant for RK4 integration + Eigen::MatrixXd k1_phi; //!< k1 STM constant for RK4 integration + Eigen::MatrixXd k2_phi; //!< k2 STM constant for RK4 integration + Eigen::MatrixXd k3_phi; //!< k3 STM constant for RK4 integration + Eigen::MatrixXd k4_phi; //!< k4 STM constant for RK4 integration + bool newMeasurements; //! Keeps track of whether or not new measurements have been written + + double mu_sun; //!< Gravitational parameter of the sun + Eigen::Matrix3d o_hat_3_tilde; //!< Tilde matrix of the third asteroid orbit frame base vector + Eigen::Vector3d o_hat_1; //!< First asteroid orbit frame base vector + Eigen::MatrixXd I; //!< 3 x 3 identity matrix + ClassicElements oe_ast; //!< Orbital elements of the asteroid + double F_dot; //!< Time rate of change of true anomaly + double F_ddot; //!< Second time derivative of true anomaly + Eigen::Matrix3d dcm_ON; //!< DCM from the inertial frame to the small-body's hill frame + Eigen::Vector3d + r_SO_O; //!< Vector from the small body's origin to the inertial frame origin in small-body hill frame components }; - #endif 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 numStates = 9; this->numMeas = 3; - this->numSigmas = 2*this->numStates + 1; + this->numSigmas = 2 * this->numStates + 1; this->x_hat_k1_.setZero(this->numStates); this->P_k1_.setZero(this->numStates, this->numStates); this->x_hat_k1.setZero(this->numStates); @@ -42,7 +41,7 @@ SmallBodyNavUKF::SmallBodyNavUKF() this->Y_sigma_k1_.setZero(this->numMeas, this->numSigmas); this->H.setZero(this->numStates, this->numMeas); this->K.setZero(this->numStates, this->numMeas); - this->dcm_AN.setIdentity(3,3); + this->dcm_AN.setIdentity(3, 3); this->omega_AN_A.setZero(3); this->alpha = 0; this->beta = 2; @@ -52,19 +51,20 @@ SmallBodyNavUKF::SmallBodyNavUKF() } /*! Module Destructor */ -SmallBodyNavUKF::~SmallBodyNavUKF() -{ -} +SmallBodyNavUKF::~SmallBodyNavUKF() {} /*! Initialize C-wrapped output messages */ -void SmallBodyNavUKF::SelfInit(){ +void +SmallBodyNavUKF::SelfInit() +{ SmallBodyNavUKFMsg_C_init(&this->smallBodyNavUKFOutMsgC); } /*! This method is used to reset the module, check that required input messages are connect and compute weigths. */ -void SmallBodyNavUKF::Reset(uint64_t CurrentSimNanos) +void +SmallBodyNavUKF::Reset(uint64_t CurrentSimNanos) { /* check that required input messages are connected */ if (!this->navTransInMsg.isLinked()) { @@ -76,20 +76,22 @@ void SmallBodyNavUKF::Reset(uint64_t CurrentSimNanos) /* compute UT weights to be used in the UT */ this->wm_sigma(0) = this->kappa / (this->kappa + this->numStates); - this->wc_sigma(0) = this->wm_sigma(0) + 1 - pow(this->alpha,2) + this->beta; + this->wc_sigma(0) = this->wm_sigma(0) + 1 - pow(this->alpha, 2) + this->beta; for (uint64_t i = 0; i < this->numStates; i++) { /* Assign weigths */ - this->wm_sigma(i+1) = 1 / (2*(this->numStates + this->kappa)); - this->wm_sigma(numStates+i+1) = this->wm_sigma(i+1); - this->wc_sigma(i+1) = this->wm_sigma(i+1); - this->wc_sigma(numStates+i+1) = this->wm_sigma(i+1); + this->wm_sigma(i + 1) = 1 / (2 * (this->numStates + this->kappa)); + this->wm_sigma(numStates + i + 1) = this->wm_sigma(i + 1); + this->wc_sigma(i + 1) = this->wm_sigma(i + 1); + this->wc_sigma(numStates + i + 1) = this->wm_sigma(i + 1); } } /*! This method is used to read the input messages. */ -void SmallBodyNavUKF::readMessages(){ +void +SmallBodyNavUKF::readMessages() +{ /* Read in the input messages */ this->navTransInMsgBuffer = this->navTransInMsg(); this->asteroidEphemerisInMsgBuffer = this->asteroidEphemerisInMsg(); @@ -99,7 +101,9 @@ void SmallBodyNavUKF::readMessages(){ @param CurrentSimNanos */ -void SmallBodyNavUKF::processUT(uint64_t CurrentSimNanos){ +void +SmallBodyNavUKF::processUT(uint64_t CurrentSimNanos) +{ /* Read angular velocity of the small body fixed frame */ this->omega_AN_A = cArray2EigenVector3d(this->asteroidEphemerisInMsgBuffer.omega_BN_B); @@ -121,10 +125,8 @@ void SmallBodyNavUKF::processUT(uint64_t CurrentSimNanos){ /* Loop to generate remaining sigma points */ for (uint64_t i = 0; i < this->numStates; i++) { /* Generate sigma points */ - X_sigma_k.col(i+1) = this->x_hat_k - - sqrt(this->numStates + this->kappa) * Psqrt_k.col(i); - X_sigma_k.col(numStates+i+1) = x_hat_k - + sqrt(this->numStates + this->kappa) * Psqrt_k.col(i); + X_sigma_k.col(i + 1) = this->x_hat_k - sqrt(this->numStates + this->kappa) * Psqrt_k.col(i); + X_sigma_k.col(numStates + i + 1) = x_hat_k + sqrt(this->numStates + this->kappa) * Psqrt_k.col(i); } /* Loop to propagate sigma points and compute mean */ @@ -141,20 +143,19 @@ void SmallBodyNavUKF::processUT(uint64_t CurrentSimNanos){ x_sigma_k = X_sigma_k.col(i); /* Compute dynamics derivative */ - r_sigma_k << x_sigma_k.segment(0,3); - v_sigma_k << x_sigma_k.segment(3,3); - a_sigma_k << x_sigma_k.segment(6,3); - x_sigma_dot_k.segment(0,3) = v_sigma_k; - x_sigma_dot_k.segment(3,3) = - 2*this->omega_AN_A.cross(v_sigma_k) - - this->omega_AN_A.cross(this->omega_AN_A.cross(r_sigma_k)) - - this->mu_ast*r_sigma_k/pow(r_sigma_k.norm(), 3) - + a_sigma_k; + r_sigma_k << x_sigma_k.segment(0, 3); + v_sigma_k << x_sigma_k.segment(3, 3); + a_sigma_k << x_sigma_k.segment(6, 3); + x_sigma_dot_k.segment(0, 3) = v_sigma_k; + x_sigma_dot_k.segment(3, 3) = -2 * this->omega_AN_A.cross(v_sigma_k) - + this->omega_AN_A.cross(this->omega_AN_A.cross(r_sigma_k)) - + this->mu_ast * r_sigma_k / pow(r_sigma_k.norm(), 3) + a_sigma_k; /* Use Euler integration to propagate */ - this->X_sigma_k1_.col(i) = x_sigma_k + x_sigma_dot_k*(CurrentSimNanos-prevTime)*NANO2SEC; + this->X_sigma_k1_.col(i) = x_sigma_k + x_sigma_dot_k * (CurrentSimNanos - prevTime) * NANO2SEC; /* Compute average */ - this->x_hat_k1_ = this->x_hat_k1_ + this->wm_sigma(i)*this->X_sigma_k1_.col(i); + this->x_hat_k1_ = this->x_hat_k1_ + this->wm_sigma(i) * this->X_sigma_k1_.col(i); } /* Loop to compute covariance */ @@ -166,7 +167,7 @@ void SmallBodyNavUKF::processUT(uint64_t CurrentSimNanos){ x_sigma_dev_k1_ = this->X_sigma_k1_.col(i) - this->x_hat_k1_; /* Add the deviation to the covariance */ - this->P_k1_ = this->P_k1_ + this->wc_sigma(i)*x_sigma_dev_k1_*x_sigma_dev_k1_.transpose(); + this->P_k1_ = this->P_k1_ + this->wc_sigma(i) * x_sigma_dev_k1_ * x_sigma_dev_k1_.transpose(); } /* Add process noise covariance */ @@ -176,7 +177,9 @@ void SmallBodyNavUKF::processUT(uint64_t CurrentSimNanos){ /*! This method does the UT to the a-priori state to compute the a-priori measurements */ -void SmallBodyNavUKF::measurementUT(){ +void +SmallBodyNavUKF::measurementUT() +{ /* Compute square root matrix of covariance */ Eigen::MatrixXd Psqrt_k1_; Psqrt_k1_ = P_k1_.llt().matrixL(); @@ -187,10 +190,9 @@ void SmallBodyNavUKF::measurementUT(){ /* Loop to generate remaining sigma points */ for (uint64_t i = 0; i < this->numStates; i++) { /* Generate sigma points */ - this->X_sigma_k1_.col(i+1) = this->x_hat_k1_ - - sqrt(this->numStates + this->kappa) * Psqrt_k1_.col(i); - this->X_sigma_k1_.col(this->numStates+i+1) = this->x_hat_k1_ - + sqrt(this->numStates + this->kappa) * Psqrt_k1_.col(i); + this->X_sigma_k1_.col(i + 1) = this->x_hat_k1_ - sqrt(this->numStates + this->kappa) * Psqrt_k1_.col(i); + this->X_sigma_k1_.col(this->numStates + i + 1) = + this->x_hat_k1_ + sqrt(this->numStates + this->kappa) * Psqrt_k1_.col(i); } /* Loop to propagate sigma points and compute mean */ @@ -202,10 +204,10 @@ void SmallBodyNavUKF::measurementUT(){ x_sigma_k1_ = this->X_sigma_k1_.col(i); /* Assign correlation between state and measurement */ - this->Y_sigma_k1_.col(i) = x_sigma_k1_.segment(0,3); + this->Y_sigma_k1_.col(i) = x_sigma_k1_.segment(0, 3); /* Compute average */ - this->y_hat_k1_ = this->y_hat_k1_ + this->wm_sigma(i)*this->Y_sigma_k1_.col(i); + this->y_hat_k1_ = this->y_hat_k1_ + this->wm_sigma(i) * this->Y_sigma_k1_.col(i); } /* Loop to compute measurements covariance and cross-correlation */ @@ -221,8 +223,8 @@ void SmallBodyNavUKF::measurementUT(){ y_sigma_dev_k1_ = this->Y_sigma_k1_.col(i) - this->y_hat_k1_; /* Add the deviation to the measurement and cross-correlation covariances*/ - this->R_k1_ = this->R_k1_ + this->wc_sigma(i)*y_sigma_dev_k1_*y_sigma_dev_k1_.transpose(); - this->H = this->H + this->wc_sigma(i)*x_sigma_dev_k1_*y_sigma_dev_k1_.transpose(); + this->R_k1_ = this->R_k1_ + this->wc_sigma(i) * y_sigma_dev_k1_ * y_sigma_dev_k1_.transpose(); + this->H = this->H + this->wc_sigma(i) * x_sigma_dev_k1_ * y_sigma_dev_k1_.transpose(); } /* Extract dcm of the small body, it transforms from inertial to small body fixed frame */ @@ -237,7 +239,9 @@ void SmallBodyNavUKF::measurementUT(){ /*! This method collects the measurements and updates the estimation */ -void SmallBodyNavUKF::kalmanUpdate(){ +void +SmallBodyNavUKF::kalmanUpdate() +{ /* Read attitude MRP of the small body fixed frame w.r.t. inertial */ Eigen::Vector3d sigma_AN; sigma_AN = cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.sigma_BN); @@ -245,10 +249,11 @@ void SmallBodyNavUKF::kalmanUpdate(){ /* Subtract the asteroid position from the spacecraft position */ Eigen::VectorXd y_k1; y_k1.setZero(this->numMeas); - y_k1.segment(0, 3) = this->dcm_AN*(cArray2EigenVector3d(navTransInMsgBuffer.r_BN_N) - cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.r_BdyZero_N)); + y_k1.segment(0, 3) = this->dcm_AN * (cArray2EigenVector3d(navTransInMsgBuffer.r_BN_N) - + cArray2EigenVector3d(asteroidEphemerisInMsgBuffer.r_BdyZero_N)); /* Compute Kalman gain */ - this->K = this->H*this->R_k1_.inverse(); + this->K = this->H * this->R_k1_.inverse(); /* Compute the Kalman innovation */ Eigen::VectorXd w_k1; @@ -267,7 +272,9 @@ void SmallBodyNavUKF::kalmanUpdate(){ /*! This method writes the output messages */ -void SmallBodyNavUKF::writeMessages(uint64_t CurrentSimNanos){ +void +SmallBodyNavUKF::writeMessages(uint64_t CurrentSimNanos) +{ /* Create output msg buffers */ SmallBodyNavUKFMsgPayload smallBodyNavUKFOutMsgBuffer; @@ -282,13 +289,15 @@ void SmallBodyNavUKF::writeMessages(uint64_t CurrentSimNanos){ this->smallBodyNavUKFOutMsg.write(&smallBodyNavUKFOutMsgBuffer, this->moduleID, CurrentSimNanos); /* Write to the C-wrapped output messages */ - SmallBodyNavUKFMsg_C_write(&smallBodyNavUKFOutMsgBuffer, &this->smallBodyNavUKFOutMsgC, this->moduleID, CurrentSimNanos); + SmallBodyNavUKFMsg_C_write( + &smallBodyNavUKFOutMsgBuffer, &this->smallBodyNavUKFOutMsgC, this->moduleID, CurrentSimNanos); } /*! This is the main method that gets called every time the module is updated. */ -void SmallBodyNavUKF::UpdateState(uint64_t CurrentSimNanos) +void +SmallBodyNavUKF::UpdateState(uint64_t CurrentSimNanos) { this->readMessages(); this->processUT(CurrentSimNanos); diff --git a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.h b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.h index caea8f19cb..c2348692ae 100644 --- a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.h +++ b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.h @@ -17,7 +17,6 @@ */ - #ifndef SMALLBODYNAVUKF_H #define SMALLBODYNAVUKF_H @@ -31,67 +30,69 @@ #include "architecture/utilities/avsEigenSupport.h" #include "architecture/utilities/macroDefinitions.h" - -/*! @brief This module estimates relative spacecraft position, velocity with respect to the body, and the non-Keplerian acceleration perturbing the spacecraft motion, using an unscented Kalman filter (UKF) +/*! @brief This module estimates relative spacecraft position, velocity with respect to the body, and the non-Keplerian + * acceleration perturbing the spacecraft motion, using an unscented Kalman filter (UKF) */ -class SmallBodyNavUKF: public SysModel { -public: +class SmallBodyNavUKF : public SysModel +{ + public: SmallBodyNavUKF(); ~SmallBodyNavUKF(); - void SelfInit(); //!< Self initialization for C-wrapped messages - void Reset(uint64_t CurrentSimNanos); //!< Resets module - void UpdateState(uint64_t CurrentSimNanos); //!< Updates state + void SelfInit(); //!< Self initialization for C-wrapped messages + void Reset(uint64_t CurrentSimNanos); //!< Resets module + void UpdateState(uint64_t CurrentSimNanos); //!< Updates state -private: - void readMessages(); //!< Reads input messages - void writeMessages(uint64_t CurrentSimNanos); //!< Writes output messages - void processUT(uint64_t CurrentSimNanos); //!< Process unscented transform - void measurementUT(); //!< Measurements unscented transform - void kalmanUpdate(); //!< Computes the state and covariance update + private: + void readMessages(); //!< Reads input messages + void writeMessages(uint64_t CurrentSimNanos); //!< Writes output messages + void processUT(uint64_t CurrentSimNanos); //!< Process unscented transform + void measurementUT(); //!< Measurements unscented transform + void kalmanUpdate(); //!< Computes the state and covariance update -public: - ReadFunctor navTransInMsg; //!< Translational nav input message - ReadFunctor asteroidEphemerisInMsg; //!< Small body ephemeris input message - Message smallBodyNavUKFOutMsg; //!< Small body nav UKF output msg - states and covariances - SmallBodyNavUKFMsg_C smallBodyNavUKFOutMsgC = {}; //!< C-wrapped Small body nav UKF output msg - states and covariances + public: + ReadFunctor navTransInMsg; //!< Translational nav input message + ReadFunctor asteroidEphemerisInMsg; //!< Small body ephemeris input message + Message + smallBodyNavUKFOutMsg; //!< Small body nav UKF output msg - states and covariances + SmallBodyNavUKFMsg_C + smallBodyNavUKFOutMsgC = {}; //!< C-wrapped Small body nav UKF output msg - states and covariances - BSKLogger bskLogger; //!< -- BSK Logging + BSKLogger bskLogger; //!< -- BSK Logging - double mu_ast; //!< Gravitational constant of the small body + double mu_ast; //!< Gravitational constant of the small body Eigen::MatrixXd P_proc; //!< Process noise covariance Eigen::MatrixXd R_meas; //!< Measurement noise covariance - Eigen::VectorXd x_hat_k; //!< Current state estimate - Eigen::MatrixXd P_k; //!< Current state estimation covariance - - double alpha; //!< UKF hyper-parameter + Eigen::VectorXd x_hat_k; //!< Current state estimate + Eigen::MatrixXd P_k; //!< Current state estimation covariance + + double alpha; //!< UKF hyper-parameter double beta; //!< UKF hyper-parameter - double kappa; //!< UKF hyper-parameter - - Eigen::Matrix3d dcm_AN; //!< Small body dcm - Eigen::Vector3d omega_AN_A; //!< Small body angular velocity - -private: - NavTransMsgPayload navTransInMsgBuffer; //!< Message buffer for input translational nav message - EphemerisMsgPayload asteroidEphemerisInMsgBuffer; //!< Message buffer for asteroid ephemeris - - uint64_t prevTime; //!< Previous time, ns - uint64_t numStates; //!< Number of states - uint64_t numMeas; //!< Number of measurements - uint64_t numSigmas; //!< Number of sigma points - Eigen::VectorXd x_hat_k1_; //!< Apriori state estimate for time k+1 - Eigen::VectorXd x_hat_k1; //!< Update state estimate for time k+1 - Eigen::VectorXd wm_sigma; //!< Mean sigma weights for UT - Eigen::VectorXd wc_sigma; //!< Covariance sigma weights for UT - Eigen::VectorXd y_hat_k1_; //!< Apriori measurement estimate for time k+1 - Eigen::MatrixXd P_k1_; //!< Apriori state covariance estimate for time k+1 - Eigen::MatrixXd P_k1; //!< Update state covariance estimate for time k+1 - Eigen::MatrixXd X_sigma_k1_; //!< Apriori state sigma points for time k+1 - Eigen::MatrixXd R_k1_; //!< Apriori measurements covariance estimate for time k+1 - Eigen::MatrixXd Y_sigma_k1_; //!< Apriori measurements sigma points for time k+1 - Eigen::MatrixXd H; //!< State-measurements cross-correlation matrix - Eigen::MatrixXd K; //!< Kalman gain matrix + double kappa; //!< UKF hyper-parameter + + Eigen::Matrix3d dcm_AN; //!< Small body dcm + Eigen::Vector3d omega_AN_A; //!< Small body angular velocity + + private: + NavTransMsgPayload navTransInMsgBuffer; //!< Message buffer for input translational nav message + EphemerisMsgPayload asteroidEphemerisInMsgBuffer; //!< Message buffer for asteroid ephemeris + + uint64_t prevTime; //!< Previous time, ns + uint64_t numStates; //!< Number of states + uint64_t numMeas; //!< Number of measurements + uint64_t numSigmas; //!< Number of sigma points + Eigen::VectorXd x_hat_k1_; //!< Apriori state estimate for time k+1 + Eigen::VectorXd x_hat_k1; //!< Update state estimate for time k+1 + Eigen::VectorXd wm_sigma; //!< Mean sigma weights for UT + Eigen::VectorXd wc_sigma; //!< Covariance sigma weights for UT + Eigen::VectorXd y_hat_k1_; //!< Apriori measurement estimate for time k+1 + Eigen::MatrixXd P_k1_; //!< Apriori state covariance estimate for time k+1 + Eigen::MatrixXd P_k1; //!< Update state covariance estimate for time k+1 + Eigen::MatrixXd X_sigma_k1_; //!< Apriori state sigma points for time k+1 + Eigen::MatrixXd R_k1_; //!< Apriori measurements covariance estimate for time k+1 + Eigen::MatrixXd Y_sigma_k1_; //!< Apriori measurements sigma points for time k+1 + Eigen::MatrixXd H; //!< State-measurements cross-correlation matrix + Eigen::MatrixXd K; //!< Kalman gain matrix }; - #endif diff --git a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.rst b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.rst index bb178776c7..07fa38fc74 100644 --- a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.rst +++ b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.rst @@ -6,9 +6,9 @@ This module is only meant to test the possibility of estimating pairs of positio 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_smallBodyNavUKF: @@ -17,7 +17,7 @@ provides information on what this message is used for. Figure 1: ``smallBodyNavUKF()`` Module I/O Illustration -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``. +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``. .. list-table:: Module I/O Messages :widths: 25 25 50 @@ -44,9 +44,9 @@ Detailed Module Description --------------------------- General Function ^^^^^^^^^^^^^^^^ -The ``smallBodyNavUKF()`` module provides a complete translational state estimate for a spacecraft in proximity of a small body. -The relative spacecraft position and velocity, and the non-Keplerian acceleration (e.g. inhomogeneous gravity field) are estimated -by the filter. The filter assumes full observability of the relative position and that the small body state is perfectly known. +The ``smallBodyNavUKF()`` module provides a complete translational state estimate for a spacecraft in proximity of a small body. +The relative spacecraft position and velocity, and the non-Keplerian acceleration (e.g. inhomogeneous gravity field) are estimated +by the filter. The filter assumes full observability of the relative position and that the small body state is perfectly known. Future work may consider augmenting the estimates and using more realistic measurements. The full state vector may be found below: .. math:: @@ -81,13 +81,13 @@ The module start by initializing the weights for the unscented transform :label: eq:UT_weights w^{[0]}_{m}=\kappa / (\kappa + N),\>\>\>\>\>\> 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/_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/stateEstimation/thrustCMEstimation/thrustCMEstimation.cpp b/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.cpp index 1ec42962ea..d9fbeb0217 100644 --- a/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.cpp +++ b/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.cpp @@ -25,7 +25,9 @@ ThrustCMEstimation::ThrustCMEstimation() = default; ThrustCMEstimation::~ThrustCMEstimation() = default; /*! Initialize C-wrapped output messages */ -void ThrustCMEstimation::SelfInit(){ +void +ThrustCMEstimation::SelfInit() +{ VehicleConfigMsg_C_init(&this->vehConfigOutMsgC); } @@ -34,22 +36,22 @@ void ThrustCMEstimation::SelfInit(){ @param CurrentSimNanos The clock time at which the function was called (nanoseconds) */ -void ThrustCMEstimation::Reset(uint64_t CurrentSimNanos) +void +ThrustCMEstimation::Reset(uint64_t CurrentSimNanos) { /*! - Check if the required message has not been connected */ if (!this->thrusterConfigBInMsg.isLinked()) { - bskLogger.bskLog(BSK_ERROR, " thrusterConfigInMsg wasn't connected."); + bskLogger.bskLog(BSK_ERROR, " thrusterConfigInMsg wasn't connected."); } if (!this->intFeedbackTorqueInMsg.isLinked()) { - bskLogger.bskLog(BSK_ERROR, " intFeedbackTorqueInMsg wasn't connected."); + bskLogger.bskLog(BSK_ERROR, " intFeedbackTorqueInMsg wasn't connected."); } if (!this->attGuidInMsg.isLinked()) { - bskLogger.bskLog(BSK_ERROR, " attGuidInMsg wasn't connected."); + bskLogger.bskLog(BSK_ERROR, " attGuidInMsg wasn't connected."); } if (this->vehConfigInMsg.isLinked()) { this->cmKnowledge = true; - } - else { + } else { this->cmKnowledge = false; } @@ -57,10 +59,10 @@ void ThrustCMEstimation::Reset(uint64_t CurrentSimNanos) this->P.setZero(); this->R.setZero(); this->I.setZero(); - for (int i=0; i<3; ++i) { - this->P(i,i) = this->P0[i]; - this->R(i,i) = this->R0[i]; - this->I(i,i) = 1; + for (int i = 0; i < 3; ++i) { + this->P(i, i) = this->P0[i]; + this->R(i, i) = this->R0[i]; + this->I(i, i) = 1; } this->r_CB_est = this->r_CB_B; } @@ -70,7 +72,8 @@ void ThrustCMEstimation::Reset(uint64_t CurrentSimNanos) @param CurrentSimNanos The clock time at which the function was called (nanoseconds) */ -void ThrustCMEstimation::UpdateState(uint64_t CurrentSimNanos) +void +ThrustCMEstimation::UpdateState(uint64_t CurrentSimNanos) { /*! create output message buffers */ VehicleConfigMsgPayload vehConfigOutBuffer = {}; @@ -86,7 +89,7 @@ void ThrustCMEstimation::UpdateState(uint64_t CurrentSimNanos) /*! compute error w.r.t. target attitude */ AttGuidMsgPayload attGuidBuffer = this->attGuidInMsg(); - Eigen::Vector3d sigma_BR = cArray2EigenVector3d(attGuidBuffer.sigma_BR); + Eigen::Vector3d sigma_BR = cArray2EigenVector3d(attGuidBuffer.sigma_BR); Eigen::Vector3d omega_BR_B = cArray2EigenVector3d(attGuidBuffer.omega_BR_B); double attError = pow(sigma_BR.squaredNorm() + omega_BR_B.squaredNorm(), 0.5); @@ -102,8 +105,8 @@ void ThrustCMEstimation::UpdateState(uint64_t CurrentSimNanos) Eigen::Matrix3d S; /*! assign preFit and postFit residuals to NaN, rewrite them in case of measurement update */ - preFit = {nan("1"), nan("1"), nan("1")}; - postFit = {nan("1"), nan("1"), nan("1")}; + preFit = { nan("1"), nan("1"), nan("1") }; + postFit = { nan("1"), nan("1"), nan("1") }; if ((this->attGuidInMsg.isWritten()) && (attError < this->attitudeTol)) { @@ -131,8 +134,8 @@ void ThrustCMEstimation::UpdateState(uint64_t CurrentSimNanos) /*! compute state 1-sigma covariance */ Eigen::Vector3d sigma; - for (int i=0; i<3; ++i) { - sigma[i] = pow(this->P(i,i), 0.5); + for (int i = 0; i < 3; ++i) { + sigma[i] = pow(this->P(i, i), 0.5); } /*! write estimation data to msg buffer */ @@ -147,9 +150,8 @@ void ThrustCMEstimation::UpdateState(uint64_t CurrentSimNanos) Eigen::Vector3d r_CB_B_true = cArray2EigenVector3d(vehConfigBuffer.CoM_B); r_CB_error = this->r_CB_est - r_CB_B_true; eigenVector3d2CArray(r_CB_error, cmEstDataBuffer.stateError); - } - else { - r_CB_error = {nan("1"), nan("1"), nan("1")}; + } else { + r_CB_error = { nan("1"), nan("1"), nan("1") }; } eigenVector3d2CArray(r_CB_error, cmEstDataBuffer.stateError); /*! write output msg */ diff --git a/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.h b/src/fswAlgorithms/stateEstimation/thrustCMEstimation/thrustCMEstimation.h index d466b4ded7..21968c95fa 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,9 +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. - - */ + */ /*! @brief Top level structure for the thrust CM estimation kalman filter. Used to estimate the spacecraft's center of mass position with respect to the B frame. @@ -42,8 +41,9 @@ #include #include -class ThrustCMEstimation: public SysModel { -public: +class ThrustCMEstimation : public SysModel +{ + public: ThrustCMEstimation(); ~ThrustCMEstimation() override; void SelfInit() override; @@ -53,25 +53,25 @@ class ThrustCMEstimation: public SysModel { /*! declare these user-defined quantities */ double attitudeTol; - ReadFunctor thrusterConfigBInMsg; //!< thr config in msg in B-frame coordinates - ReadFunctor intFeedbackTorqueInMsg; //!< integral feedback torque input msg - ReadFunctor attGuidInMsg; //!< attitude guidance input msg - ReadFunctor vehConfigInMsg; //!< (optional) vehicle configuration input msg - Message cmEstDataOutMsg; //!< estimated CM output msg - Message vehConfigOutMsg; //!< output C++ vehicle configuration msg - VehicleConfigMsg_C vehConfigOutMsgC = {}; //!< output C vehicle configuration msg + ReadFunctor thrusterConfigBInMsg; //!< thr config in msg in B-frame coordinates + ReadFunctor intFeedbackTorqueInMsg; //!< integral feedback torque input msg + ReadFunctor attGuidInMsg; //!< attitude guidance input msg + ReadFunctor vehConfigInMsg; //!< (optional) vehicle configuration input msg + Message cmEstDataOutMsg; //!< estimated CM output msg + Message vehConfigOutMsg; //!< output C++ vehicle configuration msg + VehicleConfigMsg_C vehConfigOutMsgC = {}; //!< output C vehicle configuration msg - Eigen::Vector3d r_CB_B; //!< initial CM estimate - Eigen::Vector3d P0; //!< initial CM state covariance - Eigen::Vector3d R0; //!< measurement noise covariance + Eigen::Vector3d r_CB_B; //!< initial CM estimate + Eigen::Vector3d P0; //!< initial CM state covariance + Eigen::Vector3d R0; //!< measurement noise covariance -private: - Eigen::Matrix3d I; //!< identity matrix - Eigen::Matrix3d P; //!< state covariance - Eigen::Matrix3d R; //!< measurement noise covariance - Eigen::Vector3d r_CB_est; //!< CM location estimate + private: + Eigen::Matrix3d I; //!< identity matrix + Eigen::Matrix3d P; //!< state covariance + Eigen::Matrix3d R; //!< measurement noise covariance + Eigen::Vector3d r_CB_est; //!< CM location estimate - bool cmKnowledge; //!< boolean to assess if vehConfigInMsg is connected + bool cmKnowledge; //!< boolean to assess if vehConfigInMsg is connected BSKLogger bskLogger; //!< -- BSK Logging }; 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..2d3b5feac4 100644 --- a/src/fswAlgorithms/transDetermination/_GeneralModuleFiles/ephemerisUtilities.c +++ b/src/fswAlgorithms/transDetermination/_GeneralModuleFiles/ephemerisUtilities.c @@ -19,8 +19,8 @@ #include "ephemerisUtilities.h" -double calculateChebyValue(double *chebyCoeff, uint32_t nCoeff, - double evalValue) +double +calculateChebyValue(double* chebyCoeff, uint32_t nCoeff, double evalValue) { double chebyPrev; double chebyNow; @@ -28,24 +28,22 @@ 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) - { - return(estValue); + valueMult = 2.0 * evalValue; + + estValue = chebyCoeff[0] * chebyPrev; + if (nCoeff <= 1) { + return (estValue); } - estValue += chebyCoeff[1]*chebyNow; - for(i=2; i - #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif /*! Calculate Chebychev Polynominal */ - double calculateChebyValue(double *chebyCoeff, uint32_t nCoeff, - double evalValue); + double calculateChebyValue(double* chebyCoeff, uint32_t nCoeff, double evalValue); #ifdef __cplusplus } 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/chebyPosEphem/chebyPosEphem.c b/src/fswAlgorithms/transDetermination/chebyPosEphem/chebyPosEphem.c index 5d0e1ccf65..7593547f06 100644 --- a/src/fswAlgorithms/transDetermination/chebyPosEphem/chebyPosEphem.c +++ b/src/fswAlgorithms/transDetermination/chebyPosEphem/chebyPosEphem.c @@ -30,12 +30,12 @@ @param configData The configuration data associated with the ephemeris model @param moduleID The Basilisk module identifier */ -void SelfInit_chebyPosEphem(ChebyPosEphemData *configData, int64_t moduleID) +void +SelfInit_chebyPosEphem(ChebyPosEphemData* configData, int64_t moduleID) { EphemerisMsg_C_init(&configData->posFitOutMsg); } - /*! This method takes the chebyshev coefficients loaded for the position estimator and computes the coefficients needed to estimate the time derivative of that position vector (velocity). @@ -44,8 +44,8 @@ void SelfInit_chebyPosEphem(ChebyPosEphemData *configData, int64_t moduleID) @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The Basilisk module identifier */ -void Reset_chebyPosEphem(ChebyPosEphemData *configData, uint64_t callTime, - int64_t moduleID) +void +Reset_chebyPosEphem(ChebyPosEphemData* configData, uint64_t callTime, int64_t moduleID) { // check if the required message has not been connected if (!TDBVehicleClockCorrelationMsg_C_isLinked(&configData->clockCorrInMsg)) { @@ -53,32 +53,26 @@ void Reset_chebyPosEphem(ChebyPosEphemData *configData, uint64_t callTime, } int i, j, k, n; - ChebyEphemRecord *currRec; + ChebyEphemRecord* currRec; double tempCVec[MAX_CHEB_COEFF]; - memset(tempCVec, 0x0, MAX_CHEB_COEFF*sizeof(double)); - for(i=0; i< MAX_CHEB_RECORDS; i++) - { + memset(tempCVec, 0x0, MAX_CHEB_COEFF * sizeof(double)); + for (i = 0; i < MAX_CHEB_RECORDS; i++) { currRec = &(configData->ephArray[i]); - n=currRec->nChebCoeff; - for(k=0; k<3; k++) - { - memset(tempCVec, 0x0, MAX_CHEB_COEFF*sizeof(double)); - vCopy(&(currRec->posChebyCoeff[k*currRec->nChebCoeff]), currRec->nChebCoeff, tempCVec); - for(j=n-2;j>=2;j--) - { - currRec->velChebyCoeff[k*n+j]=2*(j+1)*tempCVec[j+1]; - tempCVec[j - 1] += ((j+1)*tempCVec[j+1])/(j - 1); + n = currRec->nChebCoeff; + for (k = 0; k < 3; k++) { + memset(tempCVec, 0x0, MAX_CHEB_COEFF * sizeof(double)); + vCopy(&(currRec->posChebyCoeff[k * currRec->nChebCoeff]), currRec->nChebCoeff, tempCVec); + for (j = n - 2; j >= 2; j--) { + currRec->velChebyCoeff[k * n + j] = 2 * (j + 1) * tempCVec[j + 1]; + tempCVec[j - 1] += ((j + 1) * tempCVec[j + 1]) / (j - 1); } - currRec->velChebyCoeff[k*n+1] = 4.0*tempCVec[2]; - currRec->velChebyCoeff[k*n+0] = tempCVec[1]; - for(j=0; jvelChebyCoeff[k*n+j] *= 1.0/currRec->ephemTimeRad; + currRec->velChebyCoeff[k * n + 1] = 4.0 * tempCVec[2]; + currRec->velChebyCoeff[k * n + 0] = tempCVec[1]; + for (j = 0; j < n; j++) { + currRec->velChebyCoeff[k * n + j] *= 1.0 / currRec->ephemTimeRad; } } - } - } /*! This method takes the current time and computes the state of the object @@ -89,12 +83,13 @@ void Reset_chebyPosEphem(ChebyPosEphemData *configData, uint64_t callTime, @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The Basilisk module identifier */ -void Update_chebyPosEphem(ChebyPosEphemData *configData, uint64_t callTime, int64_t moduleID) +void +Update_chebyPosEphem(ChebyPosEphemData* configData, uint64_t callTime, int64_t moduleID) { double currentEphTime; double currentScaledValue; - ChebyEphemRecord *currRec; + ChebyEphemRecord* currRec; int i; TDBVehicleClockCorrelationMsgPayload localCorr; @@ -102,43 +97,33 @@ void Update_chebyPosEphem(ChebyPosEphemData *configData, uint64_t callTime, int6 localCorr = TDBVehicleClockCorrelationMsg_C_read(&configData->clockCorrInMsg); configData->outputState = EphemerisMsg_C_zeroMsgPayload(); - currentEphTime = callTime*NANO2SEC; + currentEphTime = callTime * NANO2SEC; currentEphTime += localCorr.ephemerisTime - localCorr.vehicleClockTime; configData->coeffSelector = 0; - for(i=0; iephArray[i].ephemTimeMid) <= - configData->ephArray[i].ephemTimeRad) - { + for (i = 0; i < MAX_CHEB_RECORDS; i++) { + if (fabs(currentEphTime - configData->ephArray[i].ephemTimeMid) <= configData->ephArray[i].ephemTimeRad) { configData->coeffSelector = i; break; } } currRec = &(configData->ephArray[configData->coeffSelector]); - currentScaledValue = (currentEphTime - currRec->ephemTimeMid) - /currRec->ephemTimeRad; - if(fabs(currentScaledValue) > 1.0) - { - currentScaledValue = currentScaledValue/fabs(currentScaledValue); + currentScaledValue = (currentEphTime - currRec->ephemTimeMid) / currRec->ephemTimeRad; + if (fabs(currentScaledValue) > 1.0) { + currentScaledValue = currentScaledValue / fabs(currentScaledValue); } - configData->outputState.timeTag = callTime*NANO2SEC; + configData->outputState.timeTag = callTime * NANO2SEC; - for(i=0; i<3; i++) - { + for (i = 0; i < 3; i++) { configData->outputState.r_BdyZero_N[i] = calculateChebyValue( - &(currRec->posChebyCoeff[i*currRec->nChebCoeff]), - currRec->nChebCoeff, currentScaledValue); + &(currRec->posChebyCoeff[i * currRec->nChebCoeff]), currRec->nChebCoeff, currentScaledValue); configData->outputState.v_BdyZero_N[i] = calculateChebyValue( - &(currRec->velChebyCoeff[i*currRec->nChebCoeff]), - currRec->nChebCoeff, currentScaledValue); - + &(currRec->velChebyCoeff[i * currRec->nChebCoeff]), currRec->nChebCoeff, currentScaledValue); } EphemerisMsg_C_write(&configData->outputState, &configData->posFitOutMsg, moduleID, callTime); return; - } diff --git a/src/fswAlgorithms/transDetermination/chebyPosEphem/chebyPosEphem.h b/src/fswAlgorithms/transDetermination/chebyPosEphem/chebyPosEphem.h old mode 100755 new mode 100644 index d680a43427..ed43f98947 --- a/src/fswAlgorithms/transDetermination/chebyPosEphem/chebyPosEphem.h +++ b/src/fswAlgorithms/transDetermination/chebyPosEphem/chebyPosEphem.h @@ -28,47 +28,46 @@ #define MAX_CHEB_COEFF 40 #define MAX_CHEB_RECORDS 4 - /*! @brief Structure that defines the layout of an Ephemeris "record." This is basically the set of coefficients for the body x/y/z positions and the time factors associated with those coefficients */ -typedef struct { - uint32_t nChebCoeff; /*!< [-] Number chebyshev coefficients loaded into record*/ - double ephemTimeMid; /*!< [s] Ephemeris time (TDB) associated with the mid-point of the curve*/ - double ephemTimeRad; /*!< [s] "Radius" of time that curve is valid for (half of total range*/ - double posChebyCoeff[3*MAX_CHEB_COEFF]; /*!< [-] Set of chebyshev coefficients for position */ - double velChebyCoeff[3*MAX_CHEB_COEFF]; /*!< [-] Set of coefficients for the velocity estimate*/ -}ChebyEphemRecord; - -/*! @brief Top level structure for the Chebyshev position ephemeris +typedef struct +{ + uint32_t nChebCoeff; /*!< [-] Number chebyshev coefficients loaded into record*/ + double ephemTimeMid; /*!< [s] Ephemeris time (TDB) associated with the mid-point of the curve*/ + double ephemTimeRad; /*!< [s] "Radius" of time that curve is valid for (half of total range*/ + double posChebyCoeff[3 * MAX_CHEB_COEFF]; /*!< [-] Set of chebyshev coefficients for position */ + double velChebyCoeff[3 * MAX_CHEB_COEFF]; /*!< [-] Set of coefficients for the velocity estimate*/ +} ChebyEphemRecord; + +/*! @brief Top level structure for the Chebyshev position ephemeris fit system. e */ -typedef struct { - EphemerisMsg_C posFitOutMsg; /*!< [-] output navigation message for pos/vel*/ +typedef struct +{ + EphemerisMsg_C posFitOutMsg; /*!< [-] output navigation message for pos/vel*/ TDBVehicleClockCorrelationMsg_C clockCorrInMsg; /*!< clock correlation input message*/ - ChebyEphemRecord ephArray[MAX_CHEB_RECORDS]; /*!< [-] Array of Chebyshev records for ephemeris*/ + ChebyEphemRecord ephArray[MAX_CHEB_RECORDS]; /*!< [-] Array of Chebyshev records for ephemeris*/ - uint32_t coeffSelector; /*!< [-] Index in the ephArray that we are currently using*/ + uint32_t coeffSelector; /*!< [-] Index in the ephArray that we are currently using*/ EphemerisMsgPayload outputState; /*!< [-] The local storage of the outgoing message data*/ - BSKLogger *bskLogger; //!< BSK Logging -}ChebyPosEphemData; + BSKLogger* bskLogger; //!< BSK Logging +} ChebyPosEphemData; #ifdef __cplusplus -extern "C" { +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); - + + 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 - #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/_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/dvAccumulation/dvAccumulation.c b/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.c index 018d6bf0a7..0eff27ac4a 100644 --- a/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.c +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.c @@ -23,21 +23,20 @@ #include #include "architecture/utilities/bsk_Print.h" - /*! This method initializes the configData for the nav aggregation algorithm. It initializes the output message in the messaging system. @param configData The configuration data associated with the Nav aggregation interface @param moduleID The Basilisk module identifier */ -void SelfInit_dvAccumulation(DVAccumulationData *configData, int64_t moduleID) +void +SelfInit_dvAccumulation(DVAccumulationData* configData, int64_t moduleID) { NavTransMsg_C_init(&configData->dvAcumOutMsg); } - -void Reset_dvAccumulation(DVAccumulationData *configData, uint64_t callTime, - int64_t moduleID) +void +Reset_dvAccumulation(DVAccumulationData* configData, uint64_t callTime, int64_t moduleID) { /*! - Configure accumulator to reset itself*/ AccDataMsgPayload inputAccData; @@ -52,7 +51,7 @@ void Reset_dvAccumulation(DVAccumulationData *configData, uint64_t callTime, inputAccData = AccDataMsg_C_read(&configData->accPktInMsg); /*! - stacks data in time order*/ - dvAccumulation_QuickSort(&(inputAccData.accPkts[0]), 0, MAX_ACC_BUF_PKT-1); + dvAccumulation_QuickSort(&(inputAccData.accPkts[0]), 0, MAX_ACC_BUF_PKT - 1); /*! - reset accumulated DV vector to zero */ v3SetZero(configData->vehAccumDV_B); @@ -64,10 +63,8 @@ void Reset_dvAccumulation(DVAccumulationData *configData, uint64_t callTime, configData->dvInitialized = 0; /*! - If we find valid timestamp, ensure that no "older" meas get ingested*/ - for(i=(MAX_ACC_BUF_PKT-1); i>=0; i--) - { - if(inputAccData.accPkts[i].measTime > 0) - { + for (i = (MAX_ACC_BUF_PKT - 1); i >= 0; i--) { + if (inputAccData.accPkts[i].measTime > 0) { /* store the newest time tag found as the previous time tag */ configData->previousTime = inputAccData.accPkts[i].measTime; break; @@ -76,18 +73,22 @@ void Reset_dvAccumulation(DVAccumulationData *configData, uint64_t callTime, } /* Experimenting QuickSort START */ -void dvAccumulation_swap(AccPktDataMsgPayload *p, AccPktDataMsgPayload *q){ +void +dvAccumulation_swap(AccPktDataMsgPayload* p, AccPktDataMsgPayload* q) +{ AccPktDataMsgPayload t; - t=*p; - *p=*q; - *q=t; + t = *p; + *p = *q; + *q = t; } -int dvAccumulation_partition(AccPktDataMsgPayload *A, int start, int end){ +int +dvAccumulation_partition(AccPktDataMsgPayload* A, int start, int end) +{ int i; - uint64_t pivot=A[end].measTime; - int partitionIndex=start; - for(i=start; i Array to be sorted, @param start --> Starting index, @param end --> Ending index */ -void dvAccumulation_QuickSort (AccPktDataMsgPayload *A, int start, int end) +void +dvAccumulation_QuickSort(AccPktDataMsgPayload* A, int start, int end) { /*! - Create an auxiliary stack array. This contains indicies. */ int stack[MAX_ACC_BUF_PKT]; - if((end-start + 1) > MAX_ACC_BUF_PKT) - { - BSK_PRINT(MSG_ERROR,"dvAccumulation_QuickSort: Stack insufficiently sized for quick-sort somehow."); + if ((end - start + 1) > MAX_ACC_BUF_PKT) { + BSK_PRINT(MSG_ERROR, "dvAccumulation_QuickSort: Stack insufficiently sized for quick-sort somehow."); } /*! - initialize the index of the top of the stack */ int top = -1; /*! - push initial values of l and h to stack */ - stack[ ++top ] = start; - stack[ ++top ] = end; + stack[++top] = start; + stack[++top] = end; /*! - Keep popping from stack while is not empty */ - while ( top >= 0 ) - { + while (top >= 0) { /* Pop h and l */ - end = stack[ top-- ]; - start = stack[ top-- ]; + end = stack[top--]; + start = stack[top--]; /*! - Set pivot element at its correct position in sorted array */ - int partitionIndex = dvAccumulation_partition( A, start, end ); + int partitionIndex = dvAccumulation_partition(A, start, end); /*! - If there are elements on left side of pivot, then push left side to stack */ - if ( partitionIndex-1 > start ) - { - stack[ ++top ] = start; - stack[ ++top ] = partitionIndex - 1; + if (partitionIndex - 1 > start) { + stack[++top] = start; + stack[++top] = partitionIndex - 1; } /*! - If there are elements on right side of pivot, then push right side to stack */ - if ( partitionIndex+1 < end ) - { - stack[ ++top ] = partitionIndex + 1; - stack[ ++top ] = end; + if (partitionIndex + 1 < end) { + stack[++top] = partitionIndex + 1; + stack[++top] = end; } } } /* Experimenting QuickSort END */ - /*! This method takes the navigation message snippets created by the various navigation components in the FSW and aggregates them into a single complete navigation message. @@ -153,13 +150,14 @@ void dvAccumulation_QuickSort (AccPktDataMsgPayload *A, int start, int end) @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The Basilisk module identifier */ -void Update_dvAccumulation(DVAccumulationData *configData, uint64_t callTime, int64_t moduleID) +void +Update_dvAccumulation(DVAccumulationData* configData, uint64_t callTime, int64_t moduleID) { int i; double dt; double frameDV_B[3]; /* [m/s] The DV of an integrated acc measurement */ - AccDataMsgPayload inputAccData; /* [-] Input message container */ - NavTransMsgPayload outputData; /* [-] The local storage of the outgoing message data */ + AccDataMsgPayload inputAccData; /* [-] Input message container */ + NavTransMsgPayload outputData; /* [-] The local storage of the outgoing message data */ /*! - zero output message container */ outputData = NavTransMsg_C_zeroMsgPayload(); @@ -169,15 +167,16 @@ void Update_dvAccumulation(DVAccumulationData *configData, uint64_t callTime, in /*! - stack data in time order */ - dvAccumulation_QuickSort(&(inputAccData.accPkts[0]), 0, MAX_ACC_BUF_PKT-1); /* measTime is the array we want to sort. We're sorting the time calculated for each measurement taken from the accelerometer in order in terms of time. */ + dvAccumulation_QuickSort(&(inputAccData.accPkts[0]), + 0, + MAX_ACC_BUF_PKT - + 1); /* measTime is the array we want to sort. We're sorting the time calculated for each + measurement taken from the accelerometer in order in terms of time. */ /*! - Ensure that the computed dt doesn't get huge.*/ - if(configData->dvInitialized == 0) - { - for(i=0; i configData->previousTime) - { + if (configData->dvInitialized == 0) { + for (i = 0; i < MAX_ACC_BUF_PKT; i++) { + if (inputAccData.accPkts[i].measTime > configData->previousTime) { configData->previousTime = inputAccData.accPkts[i].measTime; configData->dvInitialized = 1; break; @@ -186,11 +185,9 @@ void Update_dvAccumulation(DVAccumulationData *configData, uint64_t callTime, in } /*! - process new accelerometer data to accumulate Delta_v */ - for(i=0; i configData->previousTime) - { + if (inputAccData.accPkts[i].measTime > configData->previousTime) { dt = diffNanoToSec(inputAccData.accPkts[i].measTime, configData->previousTime); v3Scale(dt, inputAccData.accPkts[i].accel_B, frameDV_B); v3Add(configData->vehAccumDV_B, frameDV_B, configData->vehAccumDV_B); @@ -200,12 +197,11 @@ void Update_dvAccumulation(DVAccumulationData *configData, uint64_t callTime, in /*! - Create output message */ - outputData.timeTag = configData->previousTime*NANO2SEC; + outputData.timeTag = configData->previousTime * NANO2SEC; v3Copy(configData->vehAccumDV_B, outputData.vehAccumDV); /*! - write accumulated Dv message */ NavTransMsg_C_write(&outputData, &configData->dvAcumOutMsg, moduleID, callTime); return; - } diff --git a/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.h b/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.h old mode 100755 new mode 100644 index d1175a840a..afccfa51af --- a/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.h +++ b/src/fswAlgorithms/transDetermination/dvAccumulation/dvAccumulation.h @@ -25,37 +25,35 @@ #include "architecture/utilities/bskLogging.h" - /*! @brief Top level structure for the CSS sensor interface system. Contains all parameters for the CSS interface*/ -typedef struct { +typedef struct +{ NavTransMsg_C dvAcumOutMsg; //!< accumulated DV output message - AccDataMsg_C accPktInMsg; //!< [-] input accelerometer 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 - double vehAccumDV_B[3]; //!< [m/s] The accumulated Delta_V in body frame components + double vehAccumDV_B[3]; //!< [m/s] The accumulated Delta_V in body frame components - BSKLogger *bskLogger; //!< BSK Logging -}DVAccumulationData; + BSKLogger* bskLogger; //!< BSK Logging +} DVAccumulationData; #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif - - void SelfInit_dvAccumulation(DVAccumulationData *configData, int64_t moduleID); - void Update_dvAccumulation(DVAccumulationData *configData, uint64_t callTime, - int64_t moduleID); - void Reset_dvAccumulation(DVAccumulationData *configData, uint64_t callTime, - int64_t moduleID); - 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); - + + void SelfInit_dvAccumulation(DVAccumulationData* configData, int64_t moduleID); + void Update_dvAccumulation(DVAccumulationData* configData, uint64_t callTime, int64_t moduleID); + void Reset_dvAccumulation(DVAccumulationData* configData, uint64_t callTime, int64_t moduleID); + 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 - #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/_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/ephemDifference/ephemDifference.c b/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.c index 72d73c201d..57a134fa9f 100644 --- a/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.c +++ b/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.c @@ -25,25 +25,23 @@ @param configData The configuration data associated with the ephemeris model @param moduleID The module identification integer */ -void SelfInit_ephemDifference(EphemDifferenceData *configData, int64_t moduleID) +void +SelfInit_ephemDifference(EphemDifferenceData* configData, int64_t moduleID) { uint32_t i; - for(i = 0; i < MAX_NUM_CHANGE_BODIES; i++) - { + for (i = 0; i < MAX_NUM_CHANGE_BODIES; i++) { EphemerisMsg_C_init(&configData->changeBodies[i].ephOutMsg); } - } - /*! @brief This method resets the module. @param configData The configuration data associated with the ephemeris model @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The module identification integer */ -void Reset_ephemDifference(EphemDifferenceData *configData, uint64_t callTime, - int64_t moduleID) +void +Reset_ephemDifference(EphemDifferenceData* configData, uint64_t callTime, int64_t moduleID) { // check if the required message has not been connected if (!EphemerisMsg_C_isLinked(&configData->ephBaseInMsg)) { @@ -51,8 +49,7 @@ void Reset_ephemDifference(EphemDifferenceData *configData, uint64_t callTime, } configData->ephBdyCount = 0; - for(int i = 0; i < MAX_NUM_CHANGE_BODIES; i++) - { + for (int i = 0; i < MAX_NUM_CHANGE_BODIES; i++) { if (EphemerisMsg_C_isLinked(&configData->changeBodies[i].ephInMsg)) { configData->ephBdyCount++; } else { @@ -61,7 +58,9 @@ void Reset_ephemDifference(EphemDifferenceData *configData, uint64_t callTime, } if (configData->ephBdyCount == 0) { - _bskLog(configData->bskLogger, BSK_WARNING, "Your outgoing ephemeris message count is zero. Be sure to specify desired output messages."); + _bskLog(configData->bskLogger, + BSK_WARNING, + "Your outgoing ephemeris message count is zero. Be sure to specify desired output messages."); } } @@ -73,7 +72,8 @@ void Reset_ephemDifference(EphemDifferenceData *configData, uint64_t callTime, @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The module identification integer */ -void Update_ephemDifference(EphemDifferenceData *configData, uint64_t callTime, int64_t moduleID) +void +Update_ephemDifference(EphemDifferenceData* configData, uint64_t callTime, int64_t moduleID) { uint32_t i; EphemerisMsgPayload tmpBaseEphem; @@ -82,16 +82,11 @@ void Update_ephemDifference(EphemDifferenceData *configData, uint64_t callTime, // read input msg tmpBaseEphem = EphemerisMsg_C_read(&configData->ephBaseInMsg); - for(i = 0; i < configData->ephBdyCount; i++) - { + for (i = 0; i < configData->ephBdyCount; i++) { tmpEphStore = EphemerisMsg_C_read(&configData->changeBodies[i].ephInMsg); - v3Subtract(tmpEphStore.r_BdyZero_N, - tmpBaseEphem.r_BdyZero_N, - tmpEphStore.r_BdyZero_N); - v3Subtract(tmpEphStore.v_BdyZero_N, - tmpBaseEphem.v_BdyZero_N, - tmpEphStore.v_BdyZero_N); + v3Subtract(tmpEphStore.r_BdyZero_N, tmpBaseEphem.r_BdyZero_N, tmpEphStore.r_BdyZero_N); + v3Subtract(tmpEphStore.v_BdyZero_N, tmpBaseEphem.v_BdyZero_N, tmpEphStore.v_BdyZero_N); EphemerisMsg_C_write(&tmpEphStore, &configData->changeBodies[i].ephOutMsg, moduleID, callTime); } diff --git a/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.h b/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.h index a937627b83..ef2f534809 100644 --- a/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.h +++ b/src/fswAlgorithms/transDetermination/ephemDifference/ephemDifference.h @@ -26,36 +26,35 @@ #include "architecture/utilities/bskLogging.h" - /*! @brief Container with paired input/output message names and IDs */ -typedef struct{ +typedef struct +{ EphemerisMsg_C ephInMsg; //!< [-] Input name for the ephemeris message EphemerisMsg_C ephOutMsg; //!< [-] The name converted output message -}EphemChangeConfig; +} EphemChangeConfig; /*! @brief Container holding ephemDifference module variables */ -typedef struct { - EphemerisMsg_C ephBaseInMsg; //!< base ephemeris input message name +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 -}EphemDifferenceData; + BSKLogger* bskLogger; //!< BSK Logging +} EphemDifferenceData; #ifdef __cplusplus -extern "C" { +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); - + + 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 - #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/_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/ephemNavConverter/ephemNavConverter.c b/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.c index 37d4881ca4..256ce91eed 100644 --- a/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.c +++ b/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.c @@ -26,19 +26,20 @@ @param configData The configuration data associated with the ephemeris model @param moduleID The module identification integer */ -void SelfInit_ephemNavConverter(EphemNavConverterData *configData, int64_t moduleID) +void +SelfInit_ephemNavConverter(EphemNavConverterData* configData, int64_t moduleID) { NavTransMsg_C_init(&configData->stateOutMsg); } - /*! This resets the module to original states. @param configData The configuration data associated with the ephemeris model @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The module identification integer */ -void Reset_ephemNavConverter(EphemNavConverterData *configData, uint64_t callTime, int64_t moduleID) +void +Reset_ephemNavConverter(EphemNavConverterData* configData, uint64_t callTime, int64_t moduleID) { // check if the required message has not been connected if (!EphemerisMsg_C_isLinked(&configData->ephInMsg)) { @@ -53,7 +54,8 @@ void Reset_ephemNavConverter(EphemNavConverterData *configData, uint64_t callTim @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The module identification integer */ -void Update_ephemNavConverter(EphemNavConverterData *configData, uint64_t callTime, int64_t moduleID) +void +Update_ephemNavConverter(EphemNavConverterData* configData, uint64_t callTime, int64_t moduleID) { EphemerisMsgPayload tmpEphemeris; NavTransMsgPayload tmpOutputState; @@ -63,9 +65,9 @@ void Update_ephemNavConverter(EphemNavConverterData *configData, uint64_t callTi tmpEphemeris = EphemerisMsg_C_read(&configData->ephInMsg); /*! - map timeTag, position and velocity vector to output message */ - tmpOutputState.timeTag = tmpEphemeris.timeTag; - v3Copy(tmpEphemeris.r_BdyZero_N, tmpOutputState.r_BN_N); - v3Copy(tmpEphemeris.v_BdyZero_N, tmpOutputState.v_BN_N); + tmpOutputState.timeTag = tmpEphemeris.timeTag; + v3Copy(tmpEphemeris.r_BdyZero_N, tmpOutputState.r_BN_N); + v3Copy(tmpEphemeris.v_BdyZero_N, tmpOutputState.v_BN_N); /*! - write output message */ NavTransMsg_C_write(&tmpOutputState, &configData->stateOutMsg, moduleID, callTime); diff --git a/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.h b/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.h index 09b526f179..ac8ca4d39a 100644 --- a/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.h +++ b/src/fswAlgorithms/transDetermination/ephemNavConverter/ephemNavConverter.h @@ -25,28 +25,26 @@ #include "cMsgCInterface/EphemerisMsg_C.h" #include "cMsgCInterface/NavTransMsg_C.h" - /*! @brief The configuration structure for the ephemNavConverter module.*/ -typedef struct { +typedef struct +{ NavTransMsg_C stateOutMsg; //!< [-] output navigation message for pos/vel - EphemerisMsg_C ephInMsg; //!< ephemeris input message + EphemerisMsg_C ephInMsg; //!< ephemeris input message - BSKLogger *bskLogger; //!< BSK Logging -}EphemNavConverterData; + BSKLogger* bskLogger; //!< BSK Logging +} EphemNavConverterData; #ifdef __cplusplus -extern "C" { +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); - + + 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 - #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/_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/navAggregate/navAggregate.c b/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.c old mode 100755 new mode 100644 index 36d7626b5e..8ea388039f --- a/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.c +++ b/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.c @@ -28,84 +28,100 @@ @param configData The configuration data associated with the Nav aggregation interface @param moduleID The Basilisk module identifier */ -void SelfInit_aggregateNav(NavAggregateData *configData, int64_t moduleID) +void +SelfInit_aggregateNav(NavAggregateData* configData, int64_t moduleID) { NavAttMsg_C_init(&configData->navAttOutMsg); NavTransMsg_C_init(&configData->navTransOutMsg); } - /*! This resets the module to original states. @param configData The configuration data associated with this module @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The ID associated with the configData */ -void Reset_aggregateNav(NavAggregateData *configData, uint64_t callTime, int64_t moduleID) +void +Reset_aggregateNav(NavAggregateData* configData, uint64_t callTime, int64_t moduleID) { /*! - ensure incoming message counters are not larger than MAX_AGG_NAV_MSG */ if (configData->attMsgCount > MAX_AGG_NAV_MSG) { char info[MAX_LOGGING_LENGTH]; - snprintf(info, MAX_LOGGING_LENGTH, "The attitude message count %d is larger than allowed (%d). Setting count to max value.", - configData->attMsgCount, MAX_AGG_NAV_MSG); + snprintf(info, + MAX_LOGGING_LENGTH, + "The attitude message count %d is larger than allowed (%d). Setting count to max value.", + configData->attMsgCount, + MAX_AGG_NAV_MSG); _bskLog(configData->bskLogger, BSK_WARNING, info); configData->attMsgCount = MAX_AGG_NAV_MSG; } if (configData->transMsgCount > MAX_AGG_NAV_MSG) { char info[MAX_LOGGING_LENGTH]; - sprintf(info, "The translation message count %d is larger than allowed (%d). Setting count to max value.", - configData->transMsgCount, MAX_AGG_NAV_MSG); + sprintf(info, + "The translation message count %d is larger than allowed (%d). Setting count to max value.", + configData->transMsgCount, + MAX_AGG_NAV_MSG); _bskLog(configData->bskLogger, BSK_WARNING, info); configData->transMsgCount = MAX_AGG_NAV_MSG; } /*! - loop over the number of attitude input messages and make sure they are linked */ - for(uint32_t i=0; iattMsgCount; i=i+1) - { + for (uint32_t i = 0; i < configData->attMsgCount; i = i + 1) { if (!NavAttMsg_C_isLinked(&configData->attMsgs[i].navAttInMsg)) { - _bskLog(configData->bskLogger, BSK_ERROR, "An attitude input message name was not linked. Be sure that attMsgCount is set properly."); + _bskLog(configData->bskLogger, + BSK_ERROR, + "An attitude input message name was not linked. Be sure that attMsgCount is set properly."); } } /*! - loop over the number of translational input messages and make sure they are linked */ - for(uint32_t i=0; itransMsgCount; i=i+1) - { + for (uint32_t i = 0; i < configData->transMsgCount; i = i + 1) { if (!NavTransMsg_C_isLinked(&configData->transMsgs[i].navTransInMsg)) { - _bskLog(configData->bskLogger, BSK_ERROR, "A translation input message name was not specified. Be sure that transMsgCount is set properly."); + _bskLog(configData->bskLogger, + BSK_ERROR, + "A translation input message name was not specified. Be sure that transMsgCount is set properly."); } } /*! - ensure the attitude message index locations are less than MAX_AGG_NAV_MSG */ if (configData->attTimeIdx >= MAX_AGG_NAV_MSG) { char info[MAX_LOGGING_LENGTH]; - sprintf(info, "The attTimeIdx variable %d is too large. Must be less than %d. Setting index to max value.", - configData->attTimeIdx, MAX_AGG_NAV_MSG); + sprintf(info, + "The attTimeIdx variable %d is too large. Must be less than %d. Setting index to max value.", + configData->attTimeIdx, + MAX_AGG_NAV_MSG); _bskLog(configData->bskLogger, BSK_WARNING, info); configData->attTimeIdx = MAX_AGG_NAV_MSG - 1; } if (configData->attIdx >= MAX_AGG_NAV_MSG) { char info[MAX_LOGGING_LENGTH]; - sprintf(info, "The attIdx variable %d is too large. Must be less than %d. Setting index to max value.", - configData->attIdx, MAX_AGG_NAV_MSG); + sprintf(info, + "The attIdx variable %d is too large. Must be less than %d. Setting index to max value.", + configData->attIdx, + MAX_AGG_NAV_MSG); _bskLog(configData->bskLogger, BSK_WARNING, info); configData->attIdx = MAX_AGG_NAV_MSG - 1; } if (configData->rateIdx >= MAX_AGG_NAV_MSG) { char info[MAX_LOGGING_LENGTH]; - sprintf(info, "The rateIdx variable %d is too large. Must be less than %d. Setting index to max value.", - configData->rateIdx, MAX_AGG_NAV_MSG); + sprintf(info, + "The rateIdx variable %d is too large. Must be less than %d. Setting index to max value.", + configData->rateIdx, + MAX_AGG_NAV_MSG); _bskLog(configData->bskLogger, BSK_WARNING, info); configData->rateIdx = MAX_AGG_NAV_MSG - 1; } if (configData->sunIdx >= MAX_AGG_NAV_MSG) { char info[MAX_LOGGING_LENGTH]; - sprintf(info, "The sunIdx variable %d is too large. Must be less than %d. Setting index to max value.", - configData->sunIdx, MAX_AGG_NAV_MSG); + sprintf(info, + "The sunIdx variable %d is too large. Must be less than %d. Setting index to max value.", + configData->sunIdx, + MAX_AGG_NAV_MSG); _bskLog(configData->bskLogger, BSK_WARNING, info); configData->sunIdx = MAX_AGG_NAV_MSG - 1; @@ -114,46 +130,52 @@ void Reset_aggregateNav(NavAggregateData *configData, uint64_t callTime, int64_t /*! - ensure the translational message index locations are less than MAX_AGG_NAV_MSG */ if (configData->transTimeIdx >= MAX_AGG_NAV_MSG) { char info[MAX_LOGGING_LENGTH]; - sprintf(info, "The transTimeIdx variable %d is too large. Must be less than %d. Setting index to max value.", - configData->transTimeIdx, MAX_AGG_NAV_MSG); + sprintf(info, + "The transTimeIdx variable %d is too large. Must be less than %d. Setting index to max value.", + configData->transTimeIdx, + MAX_AGG_NAV_MSG); _bskLog(configData->bskLogger, BSK_WARNING, info); configData->transTimeIdx = MAX_AGG_NAV_MSG - 1; } if (configData->posIdx >= MAX_AGG_NAV_MSG) { char info[MAX_LOGGING_LENGTH]; - sprintf(info, "The posIdx variable %d is too large. Must be less than %d. Setting index to max value.", - configData->posIdx, MAX_AGG_NAV_MSG); + sprintf(info, + "The posIdx variable %d is too large. Must be less than %d. Setting index to max value.", + configData->posIdx, + MAX_AGG_NAV_MSG); _bskLog(configData->bskLogger, BSK_WARNING, info); configData->posIdx = MAX_AGG_NAV_MSG - 1; } if (configData->velIdx >= MAX_AGG_NAV_MSG) { char info[MAX_LOGGING_LENGTH]; - sprintf(info, "The velIdx variable %d is too large. Must be less than %d. Setting index to max value.", - configData->velIdx, MAX_AGG_NAV_MSG); + sprintf(info, + "The velIdx variable %d is too large. Must be less than %d. Setting index to max value.", + configData->velIdx, + MAX_AGG_NAV_MSG); _bskLog(configData->bskLogger, BSK_WARNING, info); configData->velIdx = MAX_AGG_NAV_MSG - 1; } if (configData->dvIdx >= MAX_AGG_NAV_MSG) { char info[MAX_LOGGING_LENGTH]; - sprintf(info, "The dvIdx variable %d is too large. Must be less than %d. Setting index to max value.", - configData->dvIdx, MAX_AGG_NAV_MSG); + sprintf(info, + "The dvIdx variable %d is too large. Must be less than %d. Setting index to max value.", + configData->dvIdx, + MAX_AGG_NAV_MSG); _bskLog(configData->bskLogger, BSK_WARNING, info); configData->dvIdx = MAX_AGG_NAV_MSG - 1; } //! - zero the arrays of input messages - for (uint32_t i=0; i< MAX_AGG_NAV_MSG; i++) { + for (uint32_t i = 0; i < MAX_AGG_NAV_MSG; i++) { configData->attMsgs[i].msgStorage = NavAttMsg_C_zeroMsgPayload(); configData->transMsgs[i].msgStorage = NavTransMsg_C_zeroMsgPayload(); } - } - /*! This method takes the navigation message snippets created by the various navigation components in the FSW and aggregates them into a single complete navigation message. @@ -162,7 +184,8 @@ void Reset_aggregateNav(NavAggregateData *configData, uint64_t callTime, int64_t @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The Basilisk module identifier */ -void Update_aggregateNav(NavAggregateData *configData, uint64_t callTime, int64_t moduleID) +void +Update_aggregateNav(NavAggregateData* configData, uint64_t callTime, int64_t moduleID) { uint32_t i; NavAttMsgPayload navAttOutMsgBuffer; /* [-] The local storage of the outgoing attitude navibation message data*/ @@ -175,8 +198,7 @@ void Update_aggregateNav(NavAggregateData *configData, uint64_t callTime, int64_ /*! - check that attitude navigation messages are present */ if (configData->attMsgCount) { /*! - Iterate through all of the attitude input messages, clear local Msg buffer and archive the new nav data */ - for(i=0; iattMsgCount; i=i+1) - { + for (i = 0; i < configData->attMsgCount; i = i + 1) { configData->attMsgs[i].msgStorage = NavAttMsg_C_read(&configData->attMsgs[i].navAttInMsg); } @@ -185,14 +207,13 @@ void Update_aggregateNav(NavAggregateData *configData, uint64_t callTime, int64_ v3Copy(configData->attMsgs[configData->attIdx].msgStorage.sigma_BN, navAttOutMsgBuffer.sigma_BN); v3Copy(configData->attMsgs[configData->rateIdx].msgStorage.omega_BN_B, navAttOutMsgBuffer.omega_BN_B); v3Copy(configData->attMsgs[configData->sunIdx].msgStorage.vehSunPntBdy, navAttOutMsgBuffer.vehSunPntBdy); - } /*! - check that translation navigation messages are present */ if (configData->transMsgCount) { - /*! - Iterate through all of the translation input messages, clear local Msg buffer and archive the new nav data */ - for(i=0; itransMsgCount; i=i+1) - { + /*! - Iterate through all of the translation input messages, clear local Msg buffer and archive the new nav data + */ + for (i = 0; i < configData->transMsgCount; i = i + 1) { configData->transMsgs[i].msgStorage = NavTransMsg_C_read(&configData->transMsgs[i].navTransInMsg); } diff --git a/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.h b/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.h old mode 100755 new mode 100644 index f90b3e801b..a37ca73868 --- a/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.h +++ b/src/fswAlgorithms/transDetermination/navAggregate/navAggregate.h @@ -27,28 +27,30 @@ #define MAX_AGG_NAV_MSG 10 - /*! structure containing the attitude navigation message name, ID and local buffer*/ -typedef struct { - NavAttMsg_C navAttInMsg; /*!< attitude navigation input message*/ +typedef struct +{ + NavAttMsg_C navAttInMsg; /*!< attitude navigation input message*/ NavAttMsgPayload msgStorage; /*!< [-] Local buffer to store nav message*/ -}AggregateAttInput; +} AggregateAttInput; /*! structure containing the translational navigation message name, ID and local buffer*/ -typedef struct { - NavTransMsg_C navTransInMsg; /*!< translation navigation input message*/ +typedef struct +{ + NavTransMsg_C navTransInMsg; /*!< translation navigation input message*/ NavTransMsgPayload msgStorage; /*!< [-] Local buffer to store nav message*/ -}AggregateTransInput; +} AggregateTransInput; /*! @brief Top level structure for the aggregagted navigation message module. */ -typedef struct { - AggregateAttInput attMsgs[MAX_AGG_NAV_MSG]; /*!< [-] The incoming nav message buffer */ +typedef struct +{ + AggregateAttInput attMsgs[MAX_AGG_NAV_MSG]; /*!< [-] The incoming nav message buffer */ 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 */ + 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*/ uint32_t rateIdx; /*!< [-] The index of the message to use for attitude rate*/ uint32_t posIdx; /*!< [-] The index of the message to use for inertial position*/ @@ -58,20 +60,20 @@ typedef struct { uint32_t attMsgCount; /*!< [-] The total number of messages available as inputs */ uint32_t transMsgCount; /*!< [-] The total number of messages available as inputs */ - BSKLogger *bskLogger; //!< BSK Logging -}NavAggregateData; + BSKLogger* bskLogger; //!< BSK Logging +} NavAggregateData; #ifdef __cplusplus -extern "C" { +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); + + 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); #ifdef __cplusplus } #endif - #endif 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/_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/transDetermination/oeStateEphem/oeStateEphem.c b/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.c index 85974fab98..fd4699e9b9 100644 --- a/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.c +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.c @@ -29,20 +29,20 @@ @param configData The configuration data associated with the ephemeris model @param moduleID The module identification integer */ -void SelfInit_oeStateEphem(OEStateEphemData *configData, int64_t moduleID) +void +SelfInit_oeStateEphem(OEStateEphemData* configData, int64_t moduleID) { EphemerisMsg_C_init(&configData->stateFitOutMsg); } - /*! This Reset method is empty @param configData The configuration data associated with the ephemeris model @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The module identification integer */ -void Reset_oeStateEphem(OEStateEphemData *configData, uint64_t callTime, - int64_t moduleID) +void +Reset_oeStateEphem(OEStateEphemData* configData, uint64_t callTime, int64_t moduleID) { // check if the required message has not been connected if (!TDBVehicleClockCorrelationMsg_C_isLinked(&configData->clockCorrInMsg)) { @@ -58,14 +58,15 @@ void Reset_oeStateEphem(OEStateEphemData *configData, uint64_t callTime, @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The module identification integer */ -void Update_oeStateEphem(OEStateEphemData *configData, uint64_t callTime, int64_t moduleID) +void +Update_oeStateEphem(OEStateEphemData* configData, uint64_t callTime, int64_t moduleID) { - double currentScaledValue; /* [s] scaled time value to within [-1,1] */ - double currentEphTime; /* [s] current ephemeris time */ - double smallestTimeDifference; /* [s] smallest difference to the time interval mid-point */ - double timeDifference; /* [s] time difference with respect to an interval mid-point */ - double anomalyAngle; /* [r] general anomaly angle variable */ - ChebyOERecord *currRec; /* [] pointer to the current Chebyshev record being used */ + double currentScaledValue; /* [s] scaled time value to within [-1,1] */ + double currentEphTime; /* [s] current ephemeris time */ + double smallestTimeDifference; /* [s] smallest difference to the time interval mid-point */ + double timeDifference; /* [s] time difference with respect to an interval mid-point */ + double anomalyAngle; /* [r] general anomaly angle variable */ + ChebyOERecord* currRec; /* [] pointer to the current Chebyshev record being used */ int i; TDBVehicleClockCorrelationMsgPayload localCorr; EphemerisMsgPayload tmpOutputState; @@ -77,44 +78,35 @@ void Update_oeStateEphem(OEStateEphemData *configData, uint64_t callTime, int64_ localCorr = TDBVehicleClockCorrelationMsg_C_read(&configData->clockCorrInMsg); /*! - compute time for fitting interval */ - currentEphTime = callTime*NANO2SEC; + currentEphTime = callTime * NANO2SEC; currentEphTime += localCorr.ephemerisTime - localCorr.vehicleClockTime; /*! - select the fitting coefficients for the nearest fit interval */ configData->coeffSelector = 0; smallestTimeDifference = fabs(currentEphTime - configData->ephArray[0].ephemTimeMid); - for(i=1; iephArray[i].ephemTimeMid); - if(timeDifference < smallestTimeDifference) - { - configData->coeffSelector = (uint32_t) i; + if (timeDifference < smallestTimeDifference) { + configData->coeffSelector = (uint32_t)i; smallestTimeDifference = timeDifference; } } /*! - determine the scaled fitting time */ currRec = &(configData->ephArray[configData->coeffSelector]); - currentScaledValue = (currentEphTime - currRec->ephemTimeMid)/currRec->ephemTimeRad; - if(fabs(currentScaledValue) > 1.0) - { - currentScaledValue = currentScaledValue/fabs(currentScaledValue); + currentScaledValue = (currentEphTime - currRec->ephemTimeMid) / currRec->ephemTimeRad; + if (fabs(currentScaledValue) > 1.0) { + currentScaledValue = currentScaledValue / fabs(currentScaledValue); } /* - determine orbit elements from chebychev polynominals */ - tmpOutputState.timeTag = callTime*NANO2SEC; - orbEl.rPeriap = calculateChebyValue(currRec->rPeriapCoeff, currRec->nChebCoeff, - currentScaledValue); - orbEl.i = calculateChebyValue(currRec->incCoeff, currRec->nChebCoeff, - currentScaledValue); - orbEl.e = calculateChebyValue(currRec->eccCoeff, currRec->nChebCoeff, - currentScaledValue); - orbEl.omega = calculateChebyValue(currRec->argPerCoeff, currRec->nChebCoeff, - currentScaledValue); - orbEl.Omega = calculateChebyValue(currRec->RAANCoeff, currRec->nChebCoeff, - currentScaledValue); - anomalyAngle = calculateChebyValue(currRec->anomCoeff, currRec->nChebCoeff, - currentScaledValue); + tmpOutputState.timeTag = callTime * NANO2SEC; + orbEl.rPeriap = calculateChebyValue(currRec->rPeriapCoeff, currRec->nChebCoeff, currentScaledValue); + orbEl.i = calculateChebyValue(currRec->incCoeff, currRec->nChebCoeff, currentScaledValue); + orbEl.e = calculateChebyValue(currRec->eccCoeff, currRec->nChebCoeff, currentScaledValue); + orbEl.omega = calculateChebyValue(currRec->argPerCoeff, currRec->nChebCoeff, currentScaledValue); + orbEl.Omega = calculateChebyValue(currRec->RAANCoeff, currRec->nChebCoeff, currentScaledValue); + anomalyAngle = calculateChebyValue(currRec->anomCoeff, currRec->nChebCoeff, currentScaledValue); /*! - determine the true anomaly angle */ if (currRec->anomalyFlag == 0) { @@ -130,15 +122,14 @@ void Update_oeStateEphem(OEStateEphemData *configData, uint64_t callTime, int64_ /*! - determine semi-major axis */ if (fabs(orbEl.e - 1.0) > 1e-12) { /* elliptic or hyperbolic case */ - orbEl.a = orbEl.rPeriap/(1.0-orbEl.e); + orbEl.a = orbEl.rPeriap / (1.0 - orbEl.e); } else { /* parabolic case, the elem2rv() function assumes a parabola has a = 0 */ orbEl.a = 0.0; } /*! - Determine position and velocity vectors */ - elem2rv(configData->muCentral, &orbEl, tmpOutputState.r_BdyZero_N, - tmpOutputState.v_BdyZero_N); + elem2rv(configData->muCentral, &orbEl, tmpOutputState.r_BdyZero_N, tmpOutputState.v_BdyZero_N); /*! - Write the output message */ EphemerisMsg_C_write(&tmpOutputState, &configData->stateFitOutMsg, moduleID, callTime); diff --git a/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.h b/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.h old mode 100755 new mode 100644 index 2481c526f5..dd8ee8a47e --- a/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.h +++ b/src/fswAlgorithms/transDetermination/oeStateEphem/oeStateEphem.h @@ -28,53 +28,51 @@ #define MAX_OE_RECORDS 10 #define MAX_OE_COEFF 20 - - /*! @brief Structure that defines the layout of an Ephemeris "record." This is basically the set of coefficients for the ephemeris elements and the time factors associated with those coefficients */ -typedef struct { - uint32_t nChebCoeff; //!< [-] Number chebyshev coefficients loaded into record - double ephemTimeMid; //!< [s] Ephemeris time (TDB) associated with the mid-point of the curve - double ephemTimeRad; //!< [s] "Radius" of time that curve is valid for (half of total range - double rPeriapCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for radius at periapses - double eccCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for eccentrity - double incCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for inclination - double argPerCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for argument of periapses - double RAANCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for right ascention of the ascending node - double anomCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for true anomaly angle - uint32_t anomalyFlag; //!< [-] Flag indicating if the anomaly angle is true (0), mean (1) -}ChebyOERecord; +typedef struct +{ + uint32_t nChebCoeff; //!< [-] Number chebyshev coefficients loaded into record + double ephemTimeMid; //!< [s] Ephemeris time (TDB) associated with the mid-point of the curve + double ephemTimeRad; //!< [s] "Radius" of time that curve is valid for (half of total range + double rPeriapCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for radius at periapses + double eccCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for eccentrity + double incCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for inclination + double argPerCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for argument of periapses + double RAANCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for right ascention of the ascending node + double anomCoeff[MAX_OE_COEFF]; //!< [-] Set of chebyshev coefficients for true anomaly angle + uint32_t anomalyFlag; //!< [-] Flag indicating if the anomaly angle is true (0), mean (1) +} ChebyOERecord; /*! @brief Top level structure for the Chebyshev position ephemeris fit system. Allows the user to specify a set of chebyshev coefficients and then use the input time to determine where a given body is in space */ -typedef struct { - EphemerisMsg_C stateFitOutMsg; //!< [-] output navigation message for pos/vel +typedef struct +{ + EphemerisMsg_C stateFitOutMsg; //!< [-] output navigation message for pos/vel TDBVehicleClockCorrelationMsg_C clockCorrInMsg; //!< clock correlation input message - double muCentral; //!< [m3/s^2] Gravitational parameter for center of orbital elements - ChebyOERecord ephArray[MAX_OE_RECORDS]; //!< [-] Array of Chebyshev records for ephemeris - uint32_t coeffSelector; //!< [-] Index in the ephArray that we are currently using - BSKLogger *bskLogger; //!< BSK Logging -}OEStateEphemData; + double muCentral; //!< [m3/s^2] Gravitational parameter for center of orbital elements + ChebyOERecord ephArray[MAX_OE_RECORDS]; //!< [-] Array of Chebyshev records for ephemeris + uint32_t coeffSelector; //!< [-] Index in the ephArray that we are currently using + BSKLogger* bskLogger; //!< BSK Logging +} OEStateEphemData; #ifdef __cplusplus -extern "C" { +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); - + + 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 - #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/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/fswAlgorithms/vehicleConfigData/vehicleConfigData.c b/src/fswAlgorithms/vehicleConfigData/vehicleConfigData.c old mode 100755 new mode 100644 index 97ae6f616b..b8c7a85877 --- a/src/fswAlgorithms/vehicleConfigData/vehicleConfigData.c +++ b/src/fswAlgorithms/vehicleConfigData/vehicleConfigData.c @@ -27,13 +27,14 @@ @param configData The configuration data associated with the vehcle config interface @param moduleID The ID associated with the configData */ -void SelfInit_vehicleConfigData(VehConfigInputData *configData, int64_t moduleID) +void +SelfInit_vehicleConfigData(VehConfigInputData* configData, int64_t moduleID) { VehicleConfigMsg_C_init(&configData->vecConfigOutMsg); } - -void Reset_vehicleConfigData(VehConfigInputData *configData, uint64_t callTime, int64_t moduleID) +void +Reset_vehicleConfigData(VehConfigInputData* configData, uint64_t callTime, int64_t moduleID) { VehicleConfigMsgPayload localConfigData; /*! Begin function steps*/ @@ -61,7 +62,8 @@ void Reset_vehicleConfigData(VehConfigInputData *configData, uint64_t callTime, @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The ID associated with the configData */ -void Update_vehicleConfigData(VehConfigInputData *configData, uint64_t callTime, int64_t moduleID) +void +Update_vehicleConfigData(VehConfigInputData* configData, uint64_t callTime, int64_t moduleID) { /*! Nothing done in this method. Make sure this is still true!*/ return; diff --git a/src/fswAlgorithms/vehicleConfigData/vehicleConfigData.h b/src/fswAlgorithms/vehicleConfigData/vehicleConfigData.h old mode 100755 new mode 100644 index cf9609d0ec..7f07fda626 --- a/src/fswAlgorithms/vehicleConfigData/vehicleConfigData.h +++ b/src/fswAlgorithms/vehicleConfigData/vehicleConfigData.h @@ -27,26 +27,26 @@ #include "architecture/utilities/bskLogging.h" - /*! @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 CoM_B[3]; /*!< [m] Center of mass of spacecraft in body*/ - double massSC; /*!< [kg] Spacecraft mass */ +typedef struct +{ + 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 */ VehicleConfigMsg_C vecConfigOutMsg; /*!< [-] Name of the output properties message*/ - BSKLogger *bskLogger; //!< BSK Logging -}VehConfigInputData; + BSKLogger* bskLogger; //!< BSK Logging +} VehConfigInputData; #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif -void Update_vehicleConfigData(VehConfigInputData *configData, uint64_t callTime, int64_t moduleID); -void SelfInit_vehicleConfigData(VehConfigInputData *configData, int64_t moduleID); -void Reset_vehicleConfigData(VehConfigInputData *configData, uint64_t callTime, int64_t moduleID); + void Update_vehicleConfigData(VehConfigInputData* configData, uint64_t callTime, int64_t moduleID); + void SelfInit_vehicleConfigData(VehConfigInputData* configData, int64_t moduleID); + void Reset_vehicleConfigData(VehConfigInputData* configData, uint64_t callTime, int64_t moduleID); #ifdef __cplusplus } #endif - #endif 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..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** @@ -59,7 +66,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. @@ -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/cModuleTemplate/cModuleTemplate.c b/src/moduleTemplates/cModuleTemplate/cModuleTemplate.c old mode 100755 new mode 100644 index d79f91ad89..65967804fb --- a/src/moduleTemplates/cModuleTemplate/cModuleTemplate.c +++ b/src/moduleTemplates/cModuleTemplate/cModuleTemplate.c @@ -25,26 +25,23 @@ #include "cModuleTemplate.h" #include "string.h" - - /* Pull in support files from other modules. Be sure to use the absolute path relative to Basilisk directory. */ #include "architecture/utilities/linearAlgebra.h" - /*! This method initializes the output messages for this module. @param configData The configuration data associated with this module @param moduleID The module identifier */ -void SelfInit_cModuleTemplate(cModuleTemplateConfig *configData, int64_t moduleID) +void +SelfInit_cModuleTemplate(cModuleTemplateConfig* configData, int64_t moduleID) { CModuleTemplateMsg_C_init(&configData->dataOutMsg); } - /*! This method performs a complete reset of the module. Local module variables that retain time varying states between function calls are reset to their default values. @@ -52,16 +49,17 @@ void SelfInit_cModuleTemplate(cModuleTemplateConfig *configData, int64_t moduleI @param callTime [ns] time the method is called @param moduleID The module identifier */ -void Reset_cModuleTemplate(cModuleTemplateConfig *configData, uint64_t callTime, int64_t moduleID) +void +Reset_cModuleTemplate(cModuleTemplateConfig* configData, uint64_t callTime, int64_t moduleID) { /*! reset any required variables */ configData->dummy = 0.0; char info[MAX_LOGGING_LENGTH]; - sprintf(info, "Variable dummy set to %f in reset.",configData->dummy); + sprintf(info, "Variable dummy set to %f in reset.", configData->dummy); _bskLog(configData->bskLogger, BSK_INFORMATION, info); /* initialize the output message to zero on reset */ - CModuleTemplateMsgPayload outMsgBuffer; /*!< local output message copy */ + CModuleTemplateMsgPayload outMsgBuffer; /*!< local output message copy */ outMsgBuffer = CModuleTemplateMsg_C_zeroMsgPayload(); CModuleTemplateMsg_C_write(&outMsgBuffer, &configData->dataOutMsg, moduleID, callTime); } @@ -72,11 +70,12 @@ void Reset_cModuleTemplate(cModuleTemplateConfig *configData, uint64_t callTime, @param callTime The clock time at which the function was called (nanoseconds) @param moduleID The module identifier */ -void Update_cModuleTemplate(cModuleTemplateConfig *configData, uint64_t callTime, int64_t moduleID) +void +Update_cModuleTemplate(cModuleTemplateConfig* configData, uint64_t callTime, int64_t moduleID) { - double Lr[3]; /*!< [unit] variable description */ - CModuleTemplateMsgPayload outMsgBuffer; /*!< local output message copy */ - CModuleTemplateMsgPayload inMsgBuffer; /*!< local copy of input message */ + double Lr[3]; /*!< [unit] variable description */ + CModuleTemplateMsgPayload outMsgBuffer; /*!< local output message copy */ + CModuleTemplateMsgPayload inMsgBuffer; /*!< local copy of input message */ // always zero the output buffer first outMsgBuffer = CModuleTemplateMsg_C_zeroMsgPayload(); @@ -102,7 +101,7 @@ void Update_cModuleTemplate(cModuleTemplateConfig *configData, uint64_t callTime /* this logging statement is not typically required. It is done here to see in the quick-start guide which module is being executed */ char info[MAX_LOGGING_LENGTH]; - sprintf(info, "C Module ID %lld ran Update at %fs", (long long int) moduleID, (double) callTime/(1e9)); + sprintf(info, "C Module ID %lld ran Update at %fs", (long long int)moduleID, (double)callTime / (1e9)); _bskLog(configData->bskLogger, BSK_INFORMATION, info); return; diff --git a/src/moduleTemplates/cModuleTemplate/cModuleTemplate.h b/src/moduleTemplates/cModuleTemplate/cModuleTemplate.h old mode 100755 new mode 100644 index 551043a002..4f371ceb1a --- a/src/moduleTemplates/cModuleTemplate/cModuleTemplate.h +++ b/src/moduleTemplates/cModuleTemplate/cModuleTemplate.h @@ -24,33 +24,32 @@ #include "architecture/utilities/bskLogging.h" #include "cMsgCInterface/CModuleTemplateMsg_C.h" - - /*! @brief Top level structure for the sub-module routines. */ -typedef struct { +typedef struct +{ /* declare module private variables */ - double dummy; //!< [units] sample module variable declaration - double dumVector[3]; //!< [units] sample vector variable + double dummy; //!< [units] sample module variable declaration + double dumVector[3]; //!< [units] sample vector variable /* declare module IO interfaces */ - CModuleTemplateMsg_C dataOutMsg; //!< sample output message - CModuleTemplateMsg_C dataInMsg; //!< sample input message + CModuleTemplateMsg_C dataOutMsg; //!< sample output message + CModuleTemplateMsg_C dataInMsg; //!< sample input message - double inputVector[3]; //!< [units] vector description - BSKLogger *bskLogger; //!< BSK Logging -}cModuleTemplateConfig; + double inputVector[3]; //!< [units] vector description + BSKLogger* bskLogger; //!< BSK Logging +} cModuleTemplateConfig; #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif - void SelfInit_cModuleTemplate(cModuleTemplateConfig *configData, int64_t moduleID); - void Update_cModuleTemplate(cModuleTemplateConfig *configData, uint64_t callTime, int64_t moduleID); - void Reset_cModuleTemplate(cModuleTemplateConfig *configData, uint64_t callTime, int64_t moduleID); + void SelfInit_cModuleTemplate(cModuleTemplateConfig* configData, int64_t moduleID); + void Update_cModuleTemplate(cModuleTemplateConfig* configData, uint64_t callTime, int64_t moduleID); + void Reset_cModuleTemplate(cModuleTemplateConfig* configData, uint64_t callTime, int64_t moduleID); #ifdef __cplusplus } #endif - #endif 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/moduleTemplates/cppModuleTemplate/cppModuleTemplate.cpp b/src/moduleTemplates/cppModuleTemplate/cppModuleTemplate.cpp old mode 100755 new mode 100644 index af0239777c..dd9e72e939 --- a/src/moduleTemplates/cppModuleTemplate/cppModuleTemplate.cpp +++ b/src/moduleTemplates/cppModuleTemplate/cppModuleTemplate.cpp @@ -22,9 +22,7 @@ /*! This is the constructor for the module class. It sets default variable values and initializes the various parts of the model */ -CppModuleTemplate::CppModuleTemplate() -{ -} +CppModuleTemplate::CppModuleTemplate() {} /*! Module Destructor. */ CppModuleTemplate::~CppModuleTemplate() @@ -32,31 +30,31 @@ CppModuleTemplate::~CppModuleTemplate() return; } - /*! This method is used to reset the module. */ -void CppModuleTemplate::Reset(uint64_t CurrentSimNanos) +void +CppModuleTemplate::Reset(uint64_t CurrentSimNanos) { /*! - reset any required variables */ this->dummy = 0.0; - bskLogger.bskLog(BSK_INFORMATION, "Variable dummy set to %f in reset.",this->dummy); + bskLogger.bskLog(BSK_INFORMATION, "Variable dummy set to %f in reset.", this->dummy); /* zero output message on reset */ - CModuleTemplateMsgPayload outMsgBuffer={}; /*!< local output message copy */ + CModuleTemplateMsgPayload outMsgBuffer = {}; /*!< local output message copy */ this->dataOutMsg.write(&outMsgBuffer, this->moduleID, CurrentSimNanos); } - /*! This is the main method that gets called every time the module is updated. Provide an appropriate description. */ -void CppModuleTemplate::UpdateState(uint64_t CurrentSimNanos) +void +CppModuleTemplate::UpdateState(uint64_t CurrentSimNanos) { - double Lr[3]; /*!< [unit] variable description */ - CModuleTemplateMsgPayload outMsgBuffer; /*!< local output message copy */ - CModuleTemplateMsgPayload inMsgBuffer; /*!< local copy of input message */ - double inputVector[3]; + double Lr[3]; /*!< [unit] variable description */ + CModuleTemplateMsgPayload outMsgBuffer; /*!< local output message copy */ + CModuleTemplateMsgPayload inMsgBuffer; /*!< local copy of input message */ + double inputVector[3]; // always zero the output buffer first outMsgBuffer = this->dataOutMsg.zeroMsgPayload; @@ -81,21 +79,24 @@ void CppModuleTemplate::UpdateState(uint64_t CurrentSimNanos) /* this logging statement is not typically required. It is done here to see in the quick-start guide which module is being executed */ - bskLogger.bskLog(BSK_INFORMATION, "C++ Module ID %lld ran Update at %fs", this->moduleID, (double) CurrentSimNanos/(1e9)); - + bskLogger.bskLog( + BSK_INFORMATION, "C++ Module ID %lld ran Update at %fs", this->moduleID, (double)CurrentSimNanos / (1e9)); } -void CppModuleTemplate::setDummy(double value) +void +CppModuleTemplate::setDummy(double value) { // check that value is in acceptable range if (value > 0) { this->dummy = value; } else { - bskLogger.bskLog(BSK_ERROR, "CppModuleTemplate: dummy variable must be strictly positive, you tried to set %f", value); + bskLogger.bskLog( + BSK_ERROR, "CppModuleTemplate: dummy variable must be strictly positive, you tried to set %f", value); } } -void CppModuleTemplate::setDumVector(std::array value) +void +CppModuleTemplate::setDumVector(std::array value) { // check that value is in acceptable range for (int i = 0; i < 3; i++) { diff --git a/src/moduleTemplates/cppModuleTemplate/cppModuleTemplate.h b/src/moduleTemplates/cppModuleTemplate/cppModuleTemplate.h old mode 100755 new mode 100644 index 4e360bc3b7..6262d3ea52 --- a/src/moduleTemplates/cppModuleTemplate/cppModuleTemplate.h +++ b/src/moduleTemplates/cppModuleTemplate/cppModuleTemplate.h @@ -28,34 +28,32 @@ #include /*! @brief basic Basilisk C++ module class */ -class CppModuleTemplate: public SysModel { -public: +class CppModuleTemplate : public SysModel +{ + public: CppModuleTemplate(); ~CppModuleTemplate(); void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); - Message dataOutMsg; //!< attitude navigation output msg - ReadFunctor dataInMsg; //!< translation navigation output msg + Message dataOutMsg; //!< attitude navigation output msg + ReadFunctor dataInMsg; //!< translation navigation output msg - BSKLogger bskLogger; //!< BSK Logging + BSKLogger bskLogger; //!< BSK Logging /** setter for `dummy` property */ void setDummy(double value); /** getter for `dummy` property */ - double getDummy() const {return this->dummy;} + double getDummy() const { return this->dummy; } /** setter for `dumVector` property */ void setDumVector(std::array value); /** getter for `dumVector` property */ - std::array getDumVector() const {return this->dumVector;} - -private: - - double dummy = {}; //!< [units] sample module variable declaration - std::array dumVector = {}; //!< [units] sample vector variable + std::array getDumVector() const { return this->dumVector; } + private: + double dummy = {}; //!< [units] sample module variable declaration + std::array dumVector = {}; //!< [units] sample vector variable }; - #endif 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..b7a63145b2 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. """ @@ -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 @@ -106,5 +123,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/_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/encoder/encoder.cpp b/src/simulation/deviceInterface/encoder/encoder.cpp index 9db90ce481..9f30911962 100644 --- a/src/simulation/deviceInterface/encoder/encoder.cpp +++ b/src/simulation/deviceInterface/encoder/encoder.cpp @@ -24,7 +24,7 @@ values and initializes the various parts of the model */ Encoder::Encoder() { - this->numRW = -1; // set the number of reaction wheels to -1 to throw a warning if not set + this->numRW = -1; // set the number of reaction wheels to -1 to throw a warning if not set this->clicksPerRotation = -1; return; @@ -36,28 +36,26 @@ Encoder::~Encoder() return; } - /*! This method is used to reset the module. */ -void Encoder::Reset(uint64_t CurrentSimNanos) +void +Encoder::Reset(uint64_t CurrentSimNanos) { // check if input message is linked - if (!this->rwSpeedInMsg.isLinked()) - { + if (!this->rwSpeedInMsg.isLinked()) { bskLogger.bskLog(BSK_ERROR, "encoder.rwSpeedInMsg is not linked."); } // if the number of clicks is not greater than 0, throw a warning message - if (this->clicksPerRotation <= 0) - { + if (this->clicksPerRotation <= 0) { bskLogger.bskLog(BSK_ERROR, "encoder: number of clicks must be a positive integer."); } // if the number of reaction wheels is not greater than 0, throw a warning message - if (this->numRW <= 0) - { - bskLogger.bskLog(BSK_ERROR, "encoder: number of reaction wheels must be a positive integer. It may not have been set."); + if (this->numRW <= 0) { + bskLogger.bskLog(BSK_ERROR, + "encoder: number of reaction wheels must be a positive integer. It may not have been set."); } // reset the previous time @@ -67,8 +65,7 @@ void Encoder::Reset(uint64_t CurrentSimNanos) this->rwSpeedConverted = this->rwSpeedOutMsg.zeroMsgPayload; // Loop through the RW to set some internal parameters to default - for (int i = 0; i < MAX_EFF_CNT; i++) - { + for (int i = 0; i < MAX_EFF_CNT; i++) { // set all reaction wheels signal to nominal this->rwSignalState[i] = SIGNAL_NOMINAL; // set the remaining clicks to zero @@ -80,7 +77,8 @@ void Encoder::Reset(uint64_t CurrentSimNanos) /*! This method reads the speed input message */ -void Encoder::readInputMessages() +void +Encoder::readInputMessages() { // read the incoming power message this->rwSpeedBuffer = this->rwSpeedInMsg(); @@ -92,7 +90,8 @@ void Encoder::readInputMessages() @param CurrentClock The clock time associated with the model call */ -void Encoder::writeOutputMessages(uint64_t CurrentClock) +void +Encoder::writeOutputMessages(uint64_t CurrentClock) { this->rwSpeedOutMsg.write(&this->rwSpeedConverted, this->moduleID, CurrentClock); @@ -100,8 +99,9 @@ void Encoder::writeOutputMessages(uint64_t CurrentClock) } /*! This method applies an encoder to the reaction wheel speeds. -*/ -void Encoder::encode(uint64_t CurrentSimNanos) + */ +void +Encoder::encode(uint64_t CurrentSimNanos) { double timeStep; double numberClicks; @@ -115,18 +115,13 @@ void Encoder::encode(uint64_t CurrentSimNanos) timeStep = diffNanoToSec(CurrentSimNanos, this->prevTime); // at the beginning of the simulation, the encoder simply outputs the true RW speeds - if (timeStep == 0.0) - { + if (timeStep == 0.0) { this->rwSpeedConverted = this->rwSpeedInMsg(); - } - else - { + } else { // loop through the RW - for (int i = 0; i < this->numRW; i++) - { + for (int i = 0; i < this->numRW; i++) { // check if encoder is operational - if (this->rwSignalState[i] == SIGNAL_NOMINAL) - { + if (this->rwSignalState[i] == SIGNAL_NOMINAL) { // calculate the angle sweeped by the reaction wheel during the time step angle = this->rwSpeedBuffer.wheelSpeeds[i] * timeStep; @@ -140,8 +135,7 @@ void Encoder::encode(uint64_t CurrentSimNanos) this->rwSpeedConverted.wheelSpeeds[i] = numberClicks / (clicksPerRadian * timeStep); } // check if encoder is off - else if (this->rwSignalState[i] == SIGNAL_OFF) - { + else if (this->rwSignalState[i] == SIGNAL_OFF) { // set the outgoing reaction wheel speed to 0 this->rwSpeedConverted.wheelSpeeds[i] = 0.0; @@ -150,7 +144,8 @@ void Encoder::encode(uint64_t CurrentSimNanos) } else if (this->rwSignalState[i] == SIGNAL_STUCK) { // if the encoder is stuck, it will output the previous results } else { - bskLogger.bskLog(BSK_ERROR, "encoder: un-modeled encoder signal mode %d selected.", this->rwSignalState[i]); + bskLogger.bskLog( + BSK_ERROR, "encoder: un-modeled encoder signal mode %d selected.", this->rwSignalState[i]); } } } @@ -158,8 +153,9 @@ void Encoder::encode(uint64_t CurrentSimNanos) } /*! This method runs the encoder module in the sim. -*/ -void Encoder::UpdateState(uint64_t CurrentSimNanos) + */ +void +Encoder::UpdateState(uint64_t CurrentSimNanos) { this->readInputMessages(); this->encode(CurrentSimNanos); diff --git a/src/simulation/deviceInterface/encoder/encoder.h b/src/simulation/deviceInterface/encoder/encoder.h index 60938abec9..b9a8a34529 100644 --- a/src/simulation/deviceInterface/encoder/encoder.h +++ b/src/simulation/deviceInterface/encoder/encoder.h @@ -28,8 +28,9 @@ #include "architecture/messaging/messaging.h" /*! @brief wheel speed encoder module class */ -class Encoder: public SysModel { -public: +class Encoder : public SysModel +{ + public: Encoder(); ~Encoder(); @@ -39,22 +40,20 @@ class Encoder: public SysModel { void writeOutputMessages(uint64_t CurrentClock); void encode(uint64_t CurrentSimNanos); -public: - Message rwSpeedOutMsg; //!< [rad/s] reaction wheel speed output message - ReadFunctor rwSpeedInMsg; //!< [rad/s] reaction wheel speed input message - int rwSignalState[MAX_EFF_CNT]; //!< vector of reaction wheel signal states - int clicksPerRotation; //!< number of clicks per full rotation - int numRW; //!< number of reaction wheels - BSKLogger bskLogger; //!< -- BSK Logging + public: + Message rwSpeedOutMsg; //!< [rad/s] reaction wheel speed output message + ReadFunctor rwSpeedInMsg; //!< [rad/s] reaction wheel speed input message + int rwSignalState[MAX_EFF_CNT]; //!< vector of reaction wheel signal states + int clicksPerRotation; //!< number of clicks per full rotation + int numRW; //!< number of reaction wheels + BSKLogger bskLogger; //!< -- BSK Logging -private: - RWSpeedMsgPayload rwSpeedBuffer; //!< reaction wheel speed buffer for internal calculations - RWSpeedMsgPayload rwSpeedConverted; //!< reaction wheel speed buffer for converted values - double remainingClicks[MAX_EFF_CNT]; //!< remaining clicks from the previous iteration - - uint64_t prevTime; //!< -- Previous simulation time observed + private: + RWSpeedMsgPayload rwSpeedBuffer; //!< reaction wheel speed buffer for internal calculations + RWSpeedMsgPayload rwSpeedConverted; //!< reaction wheel speed buffer for converted values + double remainingClicks[MAX_EFF_CNT]; //!< remaining clicks from the previous iteration + uint64_t prevTime; //!< -- Previous simulation time observed }; - #endif 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..2ffd8fea59 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 @@ -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** @@ -48,23 +48,27 @@ 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. """ - [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 = [] unitTaskName = "unitTask" unitProcessName = "TestProcess" - + # accuracy to which double values compared accuracy = 1e-12 @@ -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,6 +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/hingedBodyLinearProfiler/hingedBodyLinearProfiler.cpp b/src/simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.cpp index c985f09751..bc296f9a8b 100644 --- a/src/simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.cpp +++ b/src/simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.cpp @@ -17,7 +17,6 @@ */ - #include "simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.h" #include "architecture/utilities/macroDefinitions.h" #include @@ -30,47 +29,45 @@ HingedBodyLinearProfiler::HingedBodyLinearProfiler() this->endTheta = 0.0; this->startTime = 0; this->endTime = 0; - } /*! Module Destructor */ -HingedBodyLinearProfiler::~HingedBodyLinearProfiler() -{ -} +HingedBodyLinearProfiler::~HingedBodyLinearProfiler() {} /*! This method is used to reset the module and checks that required input messages are connected. */ -void HingedBodyLinearProfiler::Reset(uint64_t CurrentSimNanos) +void +HingedBodyLinearProfiler::Reset(uint64_t CurrentSimNanos) { // check that required input messages are connected - if(this->endTime-this->startTime > 0){ - this->deploymentSlope = (this->endTheta-this->startTheta) / ((this->endTime-this->startTime) * NANO2SEC); - } else{ + if (this->endTime - this->startTime > 0) { + this->deploymentSlope = (this->endTheta - this->startTheta) / ((this->endTime - this->startTime) * NANO2SEC); + } else { bskLogger.bskLog(BSK_ERROR, "Delta between end time and start time of deployment must exist and be positive."); } } - -/*! This is the main method that gets called every time the module is updated. Outputs a reference theta and theta dot based on the current simulation time - relative to the start and stop times for the linear deployment. +/*! This is the main method that gets called every time the module is updated. Outputs a reference theta and theta dot + based on the current simulation time relative to the start and stop times for the linear deployment. */ -void HingedBodyLinearProfiler::UpdateState(uint64_t CurrentSimNanos) +void +HingedBodyLinearProfiler::UpdateState(uint64_t CurrentSimNanos) { double refTheta; double refThetaDot; - HingedRigidBodyMsgPayload hingedRigidBodyReferenceOutMsgBuffer; //!< local copy of message buffer + HingedRigidBodyMsgPayload hingedRigidBodyReferenceOutMsgBuffer; //!< local copy of message buffer //!< always zero the output message buffers before assigning values hingedRigidBodyReferenceOutMsgBuffer = this->hingedRigidBodyReferenceOutMsg.zeroMsgPayload; - if(CurrentSimNanos < this->startTime) { //!< if deployment has not started + if (CurrentSimNanos < this->startTime) { //!< if deployment has not started refTheta = this->startTheta; refThetaDot = 0.0; - } else if (CurrentSimNanos <= this->endTime){ //!< if deployment is in progress + } else if (CurrentSimNanos <= this->endTime) { //!< if deployment is in progress refThetaDot = this->deploymentSlope; - refTheta = this->startTheta + ((CurrentSimNanos-this->startTime) * NANO2SEC) * refThetaDot; + refTheta = this->startTheta + ((CurrentSimNanos - this->startTime) * NANO2SEC) * refThetaDot; } else { //!< if deployment is over refTheta = this->endTheta; @@ -80,7 +77,6 @@ void HingedBodyLinearProfiler::UpdateState(uint64_t CurrentSimNanos) hingedRigidBodyReferenceOutMsgBuffer.theta = refTheta; hingedRigidBodyReferenceOutMsgBuffer.thetaDot = refThetaDot; - //!< write to the output messages this->hingedRigidBodyReferenceOutMsg.write(&hingedRigidBodyReferenceOutMsgBuffer, this->moduleID, CurrentSimNanos); } diff --git a/src/simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.h b/src/simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.h index 15c53262c3..46d1a3426e 100644 --- a/src/simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.h +++ b/src/simulation/deviceInterface/hingedBodyLinearProfiler/hingedBodyLinearProfiler.h @@ -17,7 +17,6 @@ */ - #ifndef HINGEDBODYLINEARPROFILER_H #define HINGEDBODYLINEARPROFILER_H @@ -28,27 +27,28 @@ /*! @brief Linear deployment profiler for single hinged rigid body. */ -class HingedBodyLinearProfiler: public SysModel { -public: +class HingedBodyLinearProfiler : public SysModel +{ + public: HingedBodyLinearProfiler(); ~HingedBodyLinearProfiler(); void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); -public: + public: uint64_t startTime; //!< [ns] time to begin deployment - uint64_t endTime; //!< [ns] time to end deployment - double startTheta; //!< [rad] starting hinged rigid body theta position - double endTheta; //!< [rad] ending hinged rigid body theta position + uint64_t endTime; //!< [ns] time to end deployment + double startTheta; //!< [rad] starting hinged rigid body theta position + double endTheta; //!< [rad] ending hinged rigid body theta position - Message hingedRigidBodyReferenceOutMsg; //!< -- output message for reference hinged rigid body state (theta, theta dot) + Message + hingedRigidBodyReferenceOutMsg; //!< -- output message for reference hinged rigid body state (theta, theta dot) - BSKLogger bskLogger; //!< -- BSK Logging + BSKLogger bskLogger; //!< -- BSK Logging -private: + private: double deploymentSlope; //!< [rad/s] slope of deployment }; - #endif 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/_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/motorVoltageInterface/motorVoltageInterface.cpp b/src/simulation/deviceInterface/motorVoltageInterface/motorVoltageInterface.cpp index b125cb511e..44b7d1a235 100644 --- a/src/simulation/deviceInterface/motorVoltageInterface/motorVoltageInterface.cpp +++ b/src/simulation/deviceInterface/motorVoltageInterface/motorVoltageInterface.cpp @@ -43,10 +43,10 @@ MotorVoltageInterface::~MotorVoltageInterface() /*! Reset the module to original configuration values. */ -void MotorVoltageInterface::Reset(uint64_t CurrenSimNanos) +void +MotorVoltageInterface::Reset(uint64_t CurrenSimNanos) { - if(!this->motorVoltageInMsg.isLinked()) - { + if (!this->motorVoltageInMsg.isLinked()) { bskLogger.bskLog(BSK_WARNING, "motorVoltageInterface.motorVoltageInMsg is not linked."); return; } @@ -54,7 +54,8 @@ void MotorVoltageInterface::Reset(uint64_t CurrenSimNanos) /*! This method reads the motor voltage input messages */ -void MotorVoltageInterface::readInputMessages() +void +MotorVoltageInterface::readInputMessages() { // read the incoming array of voltages @@ -66,11 +67,13 @@ void MotorVoltageInterface::readInputMessages() /*! This method evaluates the motor torque output states. */ -void MotorVoltageInterface::computeMotorTorque() +void +MotorVoltageInterface::computeMotorTorque() { this->outputTorqueBuffer = this->motorTorqueOutMsg.zeroMsgPayload; - for (uint64_t i=0; i < MAX_EFF_CNT; i++) { - this->outputTorqueBuffer.motorTorque[i] = this->inputVoltageBuffer.voltage[i] * this->voltage2TorqueGain(i) * this->scaleFactor(i) + this->bias(i); + for (uint64_t i = 0; i < MAX_EFF_CNT; i++) { + this->outputTorqueBuffer.motorTorque[i] = + this->inputVoltageBuffer.voltage[i] * this->voltage2TorqueGain(i) * this->scaleFactor(i) + this->bias(i); } return; } @@ -78,10 +81,11 @@ void MotorVoltageInterface::computeMotorTorque() /*! This method sets (per motor) voltage to torque scale factors (linear proportional error) */ -void MotorVoltageInterface::setScaleFactors(Eigen::VectorXd scaleFactors){ - for (int i = 0; i < this->scaleFactor.rows(); i++) - { - if (i < scaleFactors.rows()){ +void +MotorVoltageInterface::setScaleFactors(Eigen::VectorXd scaleFactors) +{ + for (int i = 0; i < this->scaleFactor.rows(); i++) { + if (i < scaleFactors.rows()) { this->scaleFactor(i) = scaleFactors(i); } else { this->scaleFactor(i) = 1.0; @@ -93,11 +97,11 @@ void MotorVoltageInterface::setScaleFactors(Eigen::VectorXd scaleFactors){ /*! This method sets the list of motor voltage to torque gains. */ -void MotorVoltageInterface::setGains(Eigen::VectorXd gains) +void +MotorVoltageInterface::setGains(Eigen::VectorXd gains) { - for (int i = 0; i < this->voltage2TorqueGain.rows(); i++) - { - if (i < gains.rows()){ + for (int i = 0; i < this->voltage2TorqueGain.rows(); i++) { + if (i < gains.rows()) { this->voltage2TorqueGain(i) = gains(i); } else { this->voltage2TorqueGain(i) = 1.0; @@ -109,11 +113,11 @@ void MotorVoltageInterface::setGains(Eigen::VectorXd gains) /*! This method sets the list of voltage to torque biases (per rw) */ -void MotorVoltageInterface::setBiases(Eigen::VectorXd biases) +void +MotorVoltageInterface::setBiases(Eigen::VectorXd biases) { - for (int i = 0; i < this->bias.rows(); i++) - { - if (i < biases.rows()){ + for (int i = 0; i < this->bias.rows(); i++) { + if (i < biases.rows()) { this->bias(i) = biases(i); } else { this->bias(i) = 0.0; @@ -126,7 +130,8 @@ void MotorVoltageInterface::setBiases(Eigen::VectorXd biases) @param CurrentClock The clock time associated with the model call */ -void MotorVoltageInterface::writeOutputMessages(uint64_t CurrentClock) +void +MotorVoltageInterface::writeOutputMessages(uint64_t CurrentClock) { this->motorTorqueOutMsg.write(&this->outputTorqueBuffer, this->moduleID, CurrentClock); @@ -137,7 +142,8 @@ void MotorVoltageInterface::writeOutputMessages(uint64_t CurrentClock) @param CurrentSimNanos The clock time associated with the model call */ -void MotorVoltageInterface::UpdateState(uint64_t CurrentSimNanos) +void +MotorVoltageInterface::UpdateState(uint64_t CurrentSimNanos) { readInputMessages(); computeMotorTorque(); diff --git a/src/simulation/deviceInterface/motorVoltageInterface/motorVoltageInterface.h b/src/simulation/deviceInterface/motorVoltageInterface/motorVoltageInterface.h index 84974f0113..29a670b0fe 100644 --- a/src/simulation/deviceInterface/motorVoltageInterface/motorVoltageInterface.h +++ b/src/simulation/deviceInterface/motorVoltageInterface/motorVoltageInterface.h @@ -32,33 +32,37 @@ #include /*! @brief RW voltage interface class */ -class MotorVoltageInterface: public SysModel { -public: +class MotorVoltageInterface : public SysModel +{ + public: MotorVoltageInterface(); ~MotorVoltageInterface(); - + void computeMotorTorque(); void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); void readInputMessages(); void writeOutputMessages(uint64_t Clock); - 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 - Eigen::VectorXd voltage2TorqueGain; //!< Nm/V gain to convert voltage to motor torque - Eigen::VectorXd scaleFactor; //!< scale the output - like a constant gain error - Eigen::VectorXd bias; //!< Nm A bias to add to the torque output - BSKLogger bskLogger; //!< -- BSK Logging + 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 -private: - ArrayMotorTorqueMsgPayload outputTorqueBuffer;//!< [Nm] copy of module output buffer - uint64_t prevTime; //!< -- Previous simulation time observed - ArrayMotorVoltageMsgPayload inputVoltageBuffer;//!< [V] One-time allocation for time savings -}; + public: + ReadFunctor + motorVoltageInMsg; //!< -- Message that contains motor voltage input states + Message motorTorqueOutMsg; //!< -- Output Message for motor torques + Eigen::VectorXd voltage2TorqueGain; //!< Nm/V gain to convert voltage to motor torque + Eigen::VectorXd scaleFactor; //!< scale the output - like a constant gain error + Eigen::VectorXd bias; //!< Nm A bias to add to the torque output + BSKLogger bskLogger; //!< -- BSK Logging + private: + ArrayMotorTorqueMsgPayload outputTorqueBuffer; //!< [Nm] copy of module output buffer + uint64_t prevTime; //!< -- Previous simulation time observed + ArrayMotorVoltageMsgPayload inputVoltageBuffer; //!< [V] One-time allocation for time savings +}; #endif 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/prescribedLinearTranslation/prescribedLinearTranslation.cpp b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp old mode 100755 new mode 100644 index 1de8a368df..7be3cd6f57 --- a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp +++ b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp @@ -26,7 +26,9 @@ /*! This method self initializes the C-wrapped output message. */ -void PrescribedLinearTranslation::SelfInit() { +void +PrescribedLinearTranslation::SelfInit() +{ PrescribedTranslationMsg_C_init(&this->prescribedTranslationOutMsgC); } @@ -34,11 +36,12 @@ void PrescribedLinearTranslation::SelfInit() { @param callTime [ns] Time the method is called */ -void PrescribedLinearTranslation::Reset(uint64_t callTime) { +void +PrescribedLinearTranslation::Reset(uint64_t callTime) +{ if (!this->linearTranslationRigidBodyInMsg.isLinked()) { - _bskLog(this->bskLogger, - BSK_ERROR, - "prescribedLinearTranslation.linearTranslationRigidBodyInMsg wasn't connected."); + _bskLog( + this->bskLogger, BSK_ERROR, "prescribedLinearTranslation.linearTranslationRigidBodyInMsg wasn't connected."); } this->tInit = 0.0; @@ -54,7 +57,9 @@ The prescribed translational states are then written to the output message. @param callTime [ns] Time the method is called */ -void PrescribedLinearTranslation::UpdateState(uint64_t callTime) { +void +PrescribedLinearTranslation::UpdateState(uint64_t callTime) +{ // Read the input message LinearTranslationRigidBodyMsgPayload linearTranslationRigidBodyIn; if (this->linearTranslationRigidBodyInMsg.isWritten()) { @@ -95,7 +100,9 @@ void PrescribedLinearTranslation::UpdateState(uint64_t callTime) { /*! This intermediate method groups the calculation of translation parameters into a single method. */ -void PrescribedLinearTranslation::computeTranslationParameters() { +void +PrescribedLinearTranslation::computeTranslationParameters() +{ if (this->coastOptionBangDuration > 0.0) { if (this->smoothingDuration > 0.0) { this->computeSmoothedBangCoastBangParameters(); @@ -114,7 +121,9 @@ void PrescribedLinearTranslation::computeTranslationParameters() { /*! This method computes the required parameters for the translation with a non-smoothed bang-bang acceleration profile. */ -void PrescribedLinearTranslation::computeBangBangParametersNoSmoothing() { +void +PrescribedLinearTranslation::computeBangBangParametersNoSmoothing() +{ // Determine the total time required for the translation double totalTransTime = sqrt(((0.5 * fabs(this->transPosRef - this->transPosInit)) * 8.0) / this->transAccelMax); @@ -125,24 +134,27 @@ void PrescribedLinearTranslation::computeBangBangParametersNoSmoothing() { this->t_b1 = this->tInit + (totalTransTime / 2.0); // Define the parabolic constants for the first and second half of the translation - this->a = 0.5 * (this->transPosRef - this->transPosInit) - / ((this->t_b1 - this->tInit) * (this->t_b1 - this->tInit)); - this->b = -0.5 * (this->transPosRef - this->transPosInit) - / ((this->t_b1 - this->t_f) * (this->t_b1 - this->t_f)); + this->a = + 0.5 * (this->transPosRef - this->transPosInit) / ((this->t_b1 - this->tInit) * (this->t_b1 - this->tInit)); + this->b = -0.5 * (this->transPosRef - this->transPosInit) / ((this->t_b1 - this->t_f) * (this->t_b1 - this->t_f)); } -/*! This method computes the required parameters for the translation with a non-smoothed bang-coast-bang acceleration profile. +/*! This method computes the required parameters for the translation with a non-smoothed bang-coast-bang acceleration + profile. */ -void PrescribedLinearTranslation::computeBangCoastBangParametersNoSmoothing() { +void +PrescribedLinearTranslation::computeBangCoastBangParametersNoSmoothing() +{ double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); // Determine the time at the end of the first bang segment t_b1 this->t_b1 = this->tInit + this->coastOptionBangDuration; // Determine the hub-relative position at time t_b1 - this->transPos_tb1 = sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration - * this->coastOptionBangDuration + this->transPosInit; + this->transPos_tb1 = + sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration * this->coastOptionBangDuration + + this->transPosInit; this->transVel_tb1 = sign * this->transAccelMax * this->coastOptionBangDuration; // Determine the distance traveled during the coast period @@ -168,7 +180,9 @@ void PrescribedLinearTranslation::computeBangCoastBangParametersNoSmoothing() { /*! This method computes the required parameters for the translation with a smoothed bang-bang acceleration profile. */ -void PrescribedLinearTranslation::computeSmoothedBangBangParameters() { +void +PrescribedLinearTranslation::computeSmoothedBangBangParameters() +{ double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); // Determine the time at the end of the first smoothing segment t_s1 @@ -176,49 +190,53 @@ void PrescribedLinearTranslation::computeSmoothedBangBangParameters() { // Determine the hub-relative position and velocity at time t_s1 this->transVel_ts1 = sign * 0.5 * this->transAccelMax * this->smoothingDuration; - this->transPos_ts1 = sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transPosInit; + this->transPos_ts1 = sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transPosInit; // Determine the duration of the bang segment bangDuration double aTerm = sign * 0.5 * this->transAccelMax; double bTerm = (sign * this->transAccelMax * this->smoothingDuration + this->transVel_ts1) / aTerm; - double cTerm = (sign * (2.0 / 5.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transVel_ts1 * this->smoothingDuration + this->transPos_ts1 - - 0.5 * (this->transPosRef + this->transPosInit)) / aTerm; - double bangDuration = (- bTerm + sqrt(bTerm * bTerm - 4.0 * cTerm)) / 2.0; + double cTerm = (sign * (2.0 / 5.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_ts1 * this->smoothingDuration + this->transPos_ts1 - + 0.5 * (this->transPosRef + this->transPosInit)) / + aTerm; + double bangDuration = (-bTerm + sqrt(bTerm * bTerm - 4.0 * cTerm)) / 2.0; // Determine the time at the end of the first bang segment t_b1 this->t_b1 = this->t_s1 + bangDuration; // Determine the hub-relative position and velocity at time t_b1 this->transVel_tb1 = sign * this->transAccelMax * bangDuration + this->transVel_ts1; - this->transPos_tb1 = sign * 0.5 * this->transAccelMax * bangDuration * bangDuration - + this->transVel_ts1 * bangDuration + this->transPos_ts1; + this->transPos_tb1 = sign * 0.5 * this->transAccelMax * bangDuration * bangDuration + + this->transVel_ts1 * bangDuration + this->transPos_ts1; // Determine the time at the end of the second smoothing segment t_s2 this->t_s2 = this->t_b1 + 2.0 * this->smoothingDuration; // Determine the hub-relative position and velocity at time t_s2 this->transVel_ts2 = this->transVel_tb1; - this->transPos_ts2 = sign * (4.0 / 5.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transVel_tb1 * 2.0 * this->smoothingDuration + this->transPos_tb1; + this->transPos_ts2 = sign * (4.0 / 5.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_tb1 * 2.0 * this->smoothingDuration + this->transPos_tb1; // Determine the time at the end of the second bang segment t_b2 this->t_b2 = this->t_s2 + bangDuration; // Determine the hub-relative position and velocity at time t_b2 - this->transVel_tb2 = - sign * this->transAccelMax * bangDuration + this->transVel_ts2; - this->transPos_tb2 = - sign * 0.5 * this->transAccelMax * bangDuration * bangDuration - + this->transVel_ts2 * bangDuration + this->transPos_ts2; + this->transVel_tb2 = -sign * this->transAccelMax * bangDuration + this->transVel_ts2; + this->transPos_tb2 = -sign * 0.5 * this->transAccelMax * bangDuration * bangDuration + + this->transVel_ts2 * bangDuration + this->transPos_ts2; // Determine the time when the translation is complete t_f this->t_f = this->t_b2 + this->smoothingDuration; } -/*! This method computes the required parameters for the translation with a smoothed bang-coast-bang acceleration profile. +/*! This method computes the required parameters for the translation with a smoothed bang-coast-bang acceleration + profile. */ -void PrescribedLinearTranslation::computeSmoothedBangCoastBangParameters() { +void +PrescribedLinearTranslation::computeSmoothedBangCoastBangParameters() +{ double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); // Determine the time at the end of the first smoothing segment t_s1 @@ -226,25 +244,25 @@ void PrescribedLinearTranslation::computeSmoothedBangCoastBangParameters() { // Determine the hub-relative position and velocity at time t_s1 this->transVel_ts1 = sign * 0.5 * this->transAccelMax * this->smoothingDuration; - this->transPos_ts1 = sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transPosInit; + this->transPos_ts1 = sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transPosInit; // Determine the time at the end of the first bang segment t_b1 this->t_b1 = this->t_s1 + this->coastOptionBangDuration; // Determine the hub-relative position and velocity at time t_b1 this->transVel_tb1 = sign * this->transAccelMax * this->coastOptionBangDuration + this->transVel_ts1; - this->transPos_tb1 = sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration - * this->coastOptionBangDuration + this->transVel_ts1 * this->coastOptionBangDuration - + this->transPos_ts1; + this->transPos_tb1 = + sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration * this->coastOptionBangDuration + + this->transVel_ts1 * this->coastOptionBangDuration + this->transPos_ts1; // Determine the time at the end of the second smoothing segment t_s2 this->t_s2 = this->t_b1 + this->smoothingDuration; // Determine the hub-relative position and velocity at time t_s2 this->transVel_ts2 = sign * 0.5 * this->transAccelMax * this->smoothingDuration + this->transVel_tb1; - this->transPos_ts2 = sign * (7.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transVel_tb1 * this->smoothingDuration + this->transPos_tb1; + this->transPos_ts2 = sign * (7.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_tb1 * this->smoothingDuration + this->transPos_tb1; // Compute the time at the end of the coast segment t_c double deltaPosCoast = (this->transPosRef - this->transPosInit) - 2 * (this->transPos_ts2 - this->transPosInit); @@ -258,18 +276,19 @@ void PrescribedLinearTranslation::computeSmoothedBangCoastBangParameters() { this->t_s3 = this->t_c + this->smoothingDuration; // Determine the hub-relative position and velocity at time t_s3 - this->transVel_ts3 = - sign * 0.5 * this->transAccelMax * this->smoothingDuration + this->transVel_tc; - this->transPos_ts3 = - sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transVel_tc * this->smoothingDuration + this->transPos_tc; + this->transVel_ts3 = -sign * 0.5 * this->transAccelMax * this->smoothingDuration + this->transVel_tc; + this->transPos_ts3 = + -sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_tc * this->smoothingDuration + this->transPos_tc; // Determine the time at the end of the second bang segment t_b2 this->t_b2 = this->t_s3 + this->coastOptionBangDuration; // Determine the hub-relative position and velocity at time t_b2 - this->transVel_tb2 = - sign * this->transAccelMax * this->coastOptionBangDuration + this->transVel_ts3; - this->transPos_tb2 = - sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration - * this->coastOptionBangDuration + this->transVel_ts3 * this->coastOptionBangDuration - + this->transPos_ts3; + this->transVel_tb2 = -sign * this->transAccelMax * this->coastOptionBangDuration + this->transVel_ts3; + this->transPos_tb2 = + -sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration * this->coastOptionBangDuration + + this->transVel_ts3 * this->coastOptionBangDuration + this->transPos_ts3; // Determine the time when the translation is complete t_f this->t_f = this->t_b2 + this->smoothingDuration; @@ -278,9 +297,11 @@ void PrescribedLinearTranslation::computeSmoothedBangCoastBangParameters() { /*! This intermediate method groups the calculation of the current translational states into a single method. */ -void PrescribedLinearTranslation::computeCurrentState(double t) { +void +PrescribedLinearTranslation::computeCurrentState(double t) +{ if (this->coastOptionBangDuration > 0.0) { - if(this->smoothingDuration > 0.0) { + if (this->smoothingDuration > 0.0) { if (this->isInFirstSmoothedSegment(t)) { this->computeFirstSmoothedSegment(t); } else if (this->isInFirstBangSegment(t)) { @@ -340,7 +361,9 @@ void PrescribedLinearTranslation::computeCurrentState(double t) { @return bool @param t [s] Current simulation time */ -bool PrescribedLinearTranslation::isInFirstBangSegment(double t) const { +bool +PrescribedLinearTranslation::isInFirstBangSegment(double t) const +{ if (this->smoothingDuration > 0.0) { return (t > this->t_s1 && t <= this->t_b1 && this->t_f - this->tInit != 0.0); } else { @@ -352,7 +375,9 @@ bool PrescribedLinearTranslation::isInFirstBangSegment(double t) const { @return bool @param t [s] Current simulation time */ -bool PrescribedLinearTranslation::isInSecondBangSegment(double t) const { +bool +PrescribedLinearTranslation::isInSecondBangSegment(double t) const +{ if (this->coastOptionBangDuration > 0.0) { if (this->smoothingDuration > 0.0) { return (t > this->t_s3 && t <= this->t_b2 && this->t_f - this->tInit != 0.0); @@ -372,15 +397,20 @@ bool PrescribedLinearTranslation::isInSecondBangSegment(double t) const { @return bool @param t [s] Current simulation time */ -bool PrescribedLinearTranslation::isInFirstSmoothedSegment(double t) const { +bool +PrescribedLinearTranslation::isInFirstSmoothedSegment(double t) const +{ return (t <= this->t_s1 && this->t_f - this->tInit != 0.0); } -/*! This method determines if the current time is within the second smoothing segment for the smoothed profiler options.. +/*! This method determines if the current time is within the second smoothing segment for the smoothed profiler + options.. @return bool @param t [s] Current simulation time */ -bool PrescribedLinearTranslation::isInSecondSmoothedSegment(double t) const { +bool +PrescribedLinearTranslation::isInSecondSmoothedSegment(double t) const +{ return (t > this->t_b1 && t <= this->t_s2 && this->t_f - this->tInit != 0.0); } @@ -388,7 +418,9 @@ bool PrescribedLinearTranslation::isInSecondSmoothedSegment(double t) const { @return bool @param t [s] Current simulation time */ -bool PrescribedLinearTranslation::isInThirdSmoothedSegment(double t) const { +bool +PrescribedLinearTranslation::isInThirdSmoothedSegment(double t) const +{ if (this->coastOptionBangDuration > 0.0) { return (t > this->t_c && t <= this->t_s3 && this->t_f - this->tInit != 0.0); } else { @@ -396,11 +428,14 @@ bool PrescribedLinearTranslation::isInThirdSmoothedSegment(double t) const { } } -/*! This method determines if the current time is within the fourth smoothing segment for the smoothed bang-coast-bang option. +/*! This method determines if the current time is within the fourth smoothing segment for the smoothed bang-coast-bang + option. @return bool @param t [s] Current simulation time */ -bool PrescribedLinearTranslation::isInFourthSmoothedSegment(double t) const { +bool +PrescribedLinearTranslation::isInFourthSmoothedSegment(double t) const +{ return (t > this->t_b2 && t <= this->t_f && this->t_f - this->tInit != 0.0); } @@ -408,10 +443,12 @@ bool PrescribedLinearTranslation::isInFourthSmoothedSegment(double t) const { @return bool @param t [s] Current simulation time */ -bool PrescribedLinearTranslation::isInCoastSegment(double t) const { +bool +PrescribedLinearTranslation::isInCoastSegment(double t) const +{ if (this->smoothingDuration > 0.0) { return (t > this->t_s2 && t <= this->t_c && this->t_f - this->tInit != 0.0); - } else{ + } else { return (t > this->t_b1 && t <= this->t_c && this->t_f - this->tInit != 0.0); } } @@ -420,14 +457,16 @@ bool PrescribedLinearTranslation::isInCoastSegment(double t) const { @param t [s] Current simulation time */ -void PrescribedLinearTranslation::computeFirstBangSegment(double t) { +void +PrescribedLinearTranslation::computeFirstBangSegment(double t) +{ double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); this->transAccel = sign * this->transAccelMax; if (this->smoothingDuration > 0.0) { this->transVel = this->transAccel * (t - this->t_s1) + this->transVel_ts1; - this->transPos = 0.5 * this->transAccel * (t - this->t_s1) * (t - this->t_s1) - + this->transVel_ts1 * (t - this->t_s1) + this->transPos_ts1; + this->transPos = 0.5 * this->transAccel * (t - this->t_s1) * (t - this->t_s1) + + this->transVel_ts1 * (t - this->t_s1) + this->transPos_ts1; } else { this->transVel = this->transAccel * (t - this->tInit); this->transPos = this->a * (t - this->tInit) * (t - this->tInit) + this->transPosInit; @@ -438,19 +477,21 @@ void PrescribedLinearTranslation::computeFirstBangSegment(double t) { @param t [s] Current simulation time */ -void PrescribedLinearTranslation::computeSecondBangSegment(double t) { +void +PrescribedLinearTranslation::computeSecondBangSegment(double t) +{ double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); - this->transAccel = - sign * this->transAccelMax; + this->transAccel = -sign * this->transAccelMax; if (this->smoothingDuration > 0.0) { if (this->coastOptionBangDuration > 0.0) { this->transVel = this->transAccel * (t - this->t_s3) + this->transVel_ts3; - this->transPos = 0.5 * this->transAccel * (t - this->t_s3) * (t - this->t_s3) - + this->transVel_ts3 * (t - this->t_s3) + this->transPos_ts3; + this->transPos = 0.5 * this->transAccel * (t - this->t_s3) * (t - this->t_s3) + + this->transVel_ts3 * (t - this->t_s3) + this->transPos_ts3; } else { this->transVel = this->transAccel * (t - this->t_s2) + this->transVel_ts2; - this->transPos = 0.5 * this->transAccel * (t - this->t_s2) * (t - this->t_s2) - + this->transVel_ts2 * (t - this->t_s2) + this->transPos_ts2; + this->transPos = 0.5 * this->transAccel * (t - this->t_s2) * (t - this->t_s2) + + this->transVel_ts2 * (t - this->t_s2) + this->transPos_ts2; } } else { this->transVel = this->transAccel * (t - this->t_f); @@ -462,20 +503,22 @@ void PrescribedLinearTranslation::computeSecondBangSegment(double t) { @param t [s] Current simulation time */ -void PrescribedLinearTranslation::computeFirstSmoothedSegment(double t) { +void +PrescribedLinearTranslation::computeFirstSmoothedSegment(double t) +{ double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); double term1 = (3.0 * (t - this->tInit) * (t - this->tInit)) / (this->smoothingDuration * this->smoothingDuration); - double term2 = (2.0 * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term3 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (this->smoothingDuration * this->smoothingDuration); - double term4 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term5 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - double term6 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term2 = (2.0 * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term3 = + ((t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / (this->smoothingDuration * this->smoothingDuration); + double term4 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term5 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + double term6 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); this->transAccel = sign * this->transAccelMax * (term1 - term2); this->transVel = sign * this->transAccelMax * (term3 - term4); @@ -486,7 +529,9 @@ void PrescribedLinearTranslation::computeFirstSmoothedSegment(double t) { @param t [s] Current simulation time */ -void PrescribedLinearTranslation::computeSecondSmoothedSegment(double t) { +void +PrescribedLinearTranslation::computeSecondSmoothedSegment(double t) +{ double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); double term1; @@ -499,43 +544,45 @@ void PrescribedLinearTranslation::computeSecondSmoothedSegment(double t) { if (this->coastOptionBangDuration > 0.0) { term1 = (3.0 * (t - this->t_b1) * (t - this->t_b1)) / (this->smoothingDuration * this->smoothingDuration); - term2 = (2.0 * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term2 = (2.0 * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); term5 = 0.5 * (t - this->t_b1) * (t - this->t_b1); - term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); } else { term1 = (3.0 * (t - this->t_b1) * (t - this->t_b1)) / (2.0 * this->smoothingDuration * this->smoothingDuration); - term2 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (2.0 * this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (8.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term2 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (2.0 * this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (8.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); term5 = 0.5 * (t - this->t_b1) * (t - this->t_b1); - term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (8.0 * this->smoothingDuration * this->smoothingDuration); - term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (40.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (8.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (40.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); } this->transAccel = sign * this->transAccelMax * (1.0 - term1 + term2); this->transVel = sign * this->transAccelMax * ((t - this->t_b1) - term3 + term4) + this->transVel_tb1; - this->transPos = sign * this->transAccelMax * (term5 - term6 + term7) - + this->transVel_tb1 * (t - this->t_b1) + this->transPos_tb1; + this->transPos = + sign * this->transAccelMax * (term5 - term6 + term7) + this->transVel_tb1 * (t - this->t_b1) + this->transPos_tb1; } /*! This method computes the third smoothing segment scalar translational states for the smoothed profiler options. @param t [s] Current simulation time */ -void PrescribedLinearTranslation::computeThirdSmoothedSegment(double t) { +void +PrescribedLinearTranslation::computeThirdSmoothedSegment(double t) +{ double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); double term1; @@ -548,70 +595,76 @@ void PrescribedLinearTranslation::computeThirdSmoothedSegment(double t) { if (this->coastOptionBangDuration > 0.0) { term1 = (3.0 * (t - this->t_c) * (t - this->t_c)) / (this->smoothingDuration * this->smoothingDuration); - term2 = (2.0 * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term5 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - term6 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - - this->transAccel = - sign * this->transAccelMax * (term1 - term2); - this->transVel = - sign * this->transAccelMax * (term3 - term4) + this->transVel_tc; - this->transPos = - sign * this->transAccelMax * (term5 - term6) + this->transVel_tc * (t - this->t_c) + this->transPos_tc; + term2 = (2.0 * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = + ((t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term5 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + term6 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + + this->transAccel = -sign * this->transAccelMax * (term1 - term2); + this->transVel = -sign * this->transAccelMax * (term3 - term4) + this->transVel_tc; + this->transPos = + -sign * this->transAccelMax * (term5 - term6) + this->transVel_tc * (t - this->t_c) + this->transPos_tc; } else { term1 = (3.0 * (t - this->t_b2) * (t - this->t_b2)) / (this->smoothingDuration * this->smoothingDuration); - term2 = (2.0 * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term5 = - 0.5 * (t - this->t_b2) * (t - this->t_b2); - term6 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - term7 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - - this->transAccel = sign * this->transAccelMax * ( - 1.0 + term1 - term2); - this->transVel = sign * this->transAccelMax * ( - (t - this->t_b2) + term3 - term4) + this->transVel_tb2; - this->transPos = sign * this->transAccelMax * (term5 + term6 - term7) + this->transVel_tb2 * (t - this->t_b2) - + this->transPos_tb2; + term2 = (2.0 * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term5 = -0.5 * (t - this->t_b2) * (t - this->t_b2); + term6 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + + this->transAccel = sign * this->transAccelMax * (-1.0 + term1 - term2); + this->transVel = sign * this->transAccelMax * (-(t - this->t_b2) + term3 - term4) + this->transVel_tb2; + this->transPos = sign * this->transAccelMax * (term5 + term6 - term7) + this->transVel_tb2 * (t - this->t_b2) + + this->transPos_tb2; } } -/*! This method computes the fourth smoothing segment scalar translational states for the smoothed bang-coast-bang option. +/*! This method computes the fourth smoothing segment scalar translational states for the smoothed bang-coast-bang + option. @param t [s] Current simulation time */ -void PrescribedLinearTranslation::computeFourthSmoothedSegment(double t) { +void +PrescribedLinearTranslation::computeFourthSmoothedSegment(double t) +{ double term1 = (3.0 * (this->t_f - t) * (this->t_f - t)) / (this->smoothingDuration * this->smoothingDuration); - double term2 = (2.0 * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term3 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (this->smoothingDuration * this->smoothingDuration); - double term4 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term5 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - double term6 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term2 = (2.0 * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term3 = + ((this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / (this->smoothingDuration * this->smoothingDuration); + double term4 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term5 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + double term6 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); - this->transAccel = - sign * this->transAccelMax * (term1 - term2); + this->transAccel = -sign * this->transAccelMax * (term1 - term2); this->transVel = sign * this->transAccelMax * (term3 - term4); - this->transPos = - sign * this->transAccelMax * (term5 - term6) + this->transPosRef; + this->transPos = -sign * this->transAccelMax * (term5 - term6) + this->transPosRef; } /*! This method computes the coast segment scalar translational states @param t [s] Current simulation time */ -void PrescribedLinearTranslation::computeCoastSegment(double t) { +void +PrescribedLinearTranslation::computeCoastSegment(double t) +{ this->transAccel = 0.0; if (this->smoothingDuration > 0.0) { @@ -626,7 +679,9 @@ void PrescribedLinearTranslation::computeCoastSegment(double t) { /*! This method computes the scalar translational states when the translation is complete. */ -void PrescribedLinearTranslation::computeTranslationComplete() { +void +PrescribedLinearTranslation::computeTranslationComplete() +{ this->transAccel = 0.0; this->transVel = 0.0; this->transPos = this->transPosRef; @@ -636,7 +691,9 @@ void PrescribedLinearTranslation::computeTranslationComplete() { /*! This method writes the module output messages and computes the output message data. */ -void PrescribedLinearTranslation::writeOutputMessages(uint64_t callTime) { +void +PrescribedLinearTranslation::writeOutputMessages(uint64_t callTime) +{ // Create the output buffer message PrescribedTranslationMsgPayload prescribedTranslationMsgOut; LinearTranslationRigidBodyMsgPayload linearTranslationRigidBodyMsgOut; @@ -646,13 +703,13 @@ void PrescribedLinearTranslation::writeOutputMessages(uint64_t callTime) { linearTranslationRigidBodyMsgOut = LinearTranslationRigidBodyMsgPayload(); // Compute the translational body position relative to the mount frame M expressed in M frame components - Eigen::Vector3d r_PM_M = this->transPos * this->transHat_M; // [m] + Eigen::Vector3d r_PM_M = this->transPos * this->transHat_M; // [m] // Compute the translational body velocity relative to the mount frame M expressed in M frame components - Eigen::Vector3d rPrime_PM_M = this->transVel * this->transHat_M; // [m/s] + Eigen::Vector3d rPrime_PM_M = this->transVel * this->transHat_M; // [m/s] // Compute the translational body acceleration relative to the mount frame M expressed in M frame components - Eigen::Vector3d rPrimePrime_PM_M = this->transAccel * this->transHat_M; // [m/s^2] + Eigen::Vector3d rPrimePrime_PM_M = this->transAccel * this->transHat_M; // [m/s^2] // Copy the module variables to the output buffer message eigenVector3d2CArray(r_PM_M, prescribedTranslationMsgOut.r_PM_M); @@ -663,8 +720,8 @@ void PrescribedLinearTranslation::writeOutputMessages(uint64_t callTime) { // Write the output messages this->prescribedTranslationOutMsg.write(&prescribedTranslationMsgOut, this->moduleID, callTime); - PrescribedTranslationMsg_C_write(&prescribedTranslationMsgOut, - &prescribedTranslationOutMsgC, this->moduleID, callTime); + PrescribedTranslationMsg_C_write( + &prescribedTranslationMsgOut, &prescribedTranslationOutMsgC, this->moduleID, callTime); this->linearTranslationRigidBodyOutMsg.write(&linearTranslationRigidBodyMsgOut, this->moduleID, callTime); } @@ -672,7 +729,9 @@ void PrescribedLinearTranslation::writeOutputMessages(uint64_t callTime) { @param coastOptionBangDuration [s] Bang segment time duration */ -void PrescribedLinearTranslation::setCoastOptionBangDuration(const double coastOptionBangDuration) { +void +PrescribedLinearTranslation::setCoastOptionBangDuration(const double coastOptionBangDuration) +{ this->coastOptionBangDuration = coastOptionBangDuration; } @@ -680,7 +739,9 @@ void PrescribedLinearTranslation::setCoastOptionBangDuration(const double coastO @param smoothingDuration [s] Duration the acceleration is smoothed until reaching the given maximum acceleration value */ -void PrescribedLinearTranslation::setSmoothingDuration(const double smoothingDuration) { +void +PrescribedLinearTranslation::setSmoothingDuration(const double smoothingDuration) +{ this->smoothingDuration = smoothingDuration; } @@ -688,7 +749,9 @@ void PrescribedLinearTranslation::setSmoothingDuration(const double smoothingDur @param transAccelMax [m/s^2] Bang segment linear angular acceleration */ -void PrescribedLinearTranslation::setTransAccelMax(const double transAccelMax) { +void +PrescribedLinearTranslation::setTransAccelMax(const double transAccelMax) +{ this->transAccelMax = transAccelMax; } @@ -696,7 +759,9 @@ void PrescribedLinearTranslation::setTransAccelMax(const double transAccelMax) { @param transHat_M Translating body axis of translation (unit vector) */ -void PrescribedLinearTranslation::setTransHat_M(const Eigen::Vector3d &transHat_M) { +void +PrescribedLinearTranslation::setTransHat_M(const Eigen::Vector3d& transHat_M) +{ this->transHat_M = transHat_M; } @@ -704,41 +769,53 @@ void PrescribedLinearTranslation::setTransHat_M(const Eigen::Vector3d &transHat_ @param transPosInit [m] Initial translating body position relative to the hub */ -void PrescribedLinearTranslation::setTransPosInit(const double transPosInit) { +void +PrescribedLinearTranslation::setTransPosInit(const double transPosInit) +{ this->transPosInit = transPosInit; } /*! Getter method for the coast option bang duration. @return double */ -double PrescribedLinearTranslation::getCoastOptionBangDuration() const { +double +PrescribedLinearTranslation::getCoastOptionBangDuration() const +{ return this->coastOptionBangDuration; } /*! Getter method for the duration the acceleration is smoothed until reaching the given maximum acceleration value. @return double */ -double PrescribedLinearTranslation::getSmoothingDuration() const { +double +PrescribedLinearTranslation::getSmoothingDuration() const +{ return this->smoothingDuration; } /*! Getter method for the bang segment scalar linear acceleration. @return double */ -double PrescribedLinearTranslation::getTransAccelMax() const { +double +PrescribedLinearTranslation::getTransAccelMax() const +{ return this->transAccelMax; } /*! Getter method for the translating body axis of translation. @return const Eigen::Vector3d */ -const Eigen::Vector3d &PrescribedLinearTranslation::getTransHat_M() const { +const Eigen::Vector3d& +PrescribedLinearTranslation::getTransHat_M() const +{ return this->transHat_M; } /*! Getter method for the initial translating body position. @return double */ -double PrescribedLinearTranslation::getTransPosInit() const { +double +PrescribedLinearTranslation::getTransPosInit() const +{ return this->transPosInit; } diff --git a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.h b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.h old mode 100755 new mode 100644 index 214aaed4e5..dc52d8ed23 --- a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.h +++ b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.h @@ -28,98 +28,131 @@ #include /*! @brief Prescribed Linear Translation Profiler Class */ -class PrescribedLinearTranslation: public SysModel { -public: - PrescribedLinearTranslation() = default; //!< Constructor - ~PrescribedLinearTranslation() = default; //!< Destructor - - void SelfInit() override; //!< Member function to initialize the C-wrapped output message - void Reset(uint64_t CurrentSimNanos) override; //!< Reset member function - void UpdateState(uint64_t CurrentSimNanos) override; //!< Update member function - void setCoastOptionBangDuration(const double bangDuration); //!< Setter method for the coast option bang duration - void setSmoothingDuration(const double smoothingDuration); //!< Setter method for the duration the acceleration is smoothed until reaching the given maximum acceleration value - void setTransAccelMax(const double transAccelMax); //!< Setter method for the bang segment scalar linear acceleration - void setTransHat_M(const Eigen::Vector3d &transHat_M); //!< Setter method for the translating body axis of translation - void setTransPosInit(const double transPosInit); //!< Setter method for the initial translating body hub-relative position - double getCoastOptionBangDuration() const; //!< Getter method for the coast option bang duration - double getSmoothingDuration() const; //!< Getter method for the duration the acceleration is smoothed until reaching the given maximum acceleration value - double getTransAccelMax() const; //!< Getter method for the bang segment scalar linear acceleration - const Eigen::Vector3d &getTransHat_M() const; //!< Getter method for the translating body axis of translation - double getTransPosInit() const; //!< Getter method for the initial translating body position - - ReadFunctor linearTranslationRigidBodyInMsg; //!< Input msg for the translational reference position and velocity - Message linearTranslationRigidBodyOutMsg; //!< Output msg for the translational reference position and velocity - Message prescribedTranslationOutMsg; //!< Output msg for the translational body prescribed states - PrescribedTranslationMsg_C prescribedTranslationOutMsgC = {}; //!< C-wrapped Output msg for the translational body prescribed states - - BSKLogger *bskLogger; //!< BSK Logging - -private: +class PrescribedLinearTranslation : public SysModel +{ + public: + PrescribedLinearTranslation() = default; //!< Constructor + ~PrescribedLinearTranslation() = default; //!< Destructor + + void SelfInit() override; //!< Member function to initialize the C-wrapped output message + void Reset(uint64_t CurrentSimNanos) override; //!< Reset member function + void UpdateState(uint64_t CurrentSimNanos) override; //!< Update member function + void setCoastOptionBangDuration(const double bangDuration); //!< Setter method for the coast option bang duration + void setSmoothingDuration( + const double smoothingDuration); //!< Setter method for the duration the acceleration is smoothed until reaching + //!< the given maximum acceleration value + void setTransAccelMax( + const double transAccelMax); //!< Setter method for the bang segment scalar linear acceleration + void setTransHat_M( + const Eigen::Vector3d& transHat_M); //!< Setter method for the translating body axis of translation + void setTransPosInit( + const double transPosInit); //!< Setter method for the initial translating body hub-relative position + double getCoastOptionBangDuration() const; //!< Getter method for the coast option bang duration + double getSmoothingDuration() const; //!< Getter method for the duration the acceleration is smoothed until reaching + //!< the given maximum acceleration value + double getTransAccelMax() const; //!< Getter method for the bang segment scalar linear acceleration + const Eigen::Vector3d& getTransHat_M() const; //!< Getter method for the translating body axis of translation + double getTransPosInit() const; //!< Getter method for the initial translating body position + + ReadFunctor + linearTranslationRigidBodyInMsg; //!< Input msg for the translational reference position and velocity + Message + linearTranslationRigidBodyOutMsg; //!< Output msg for the translational reference position and velocity + Message + prescribedTranslationOutMsg; //!< Output msg for the translational body prescribed states + PrescribedTranslationMsg_C + prescribedTranslationOutMsgC = {}; //!< C-wrapped Output msg for the translational body prescribed states + + BSKLogger* bskLogger; //!< BSK Logging + + private: /* Methods for computing the required translation parameters */ - void computeTranslationParameters(); //!< Intermediate method to group the calculation of translation parameters into a single method - void computeBangBangParametersNoSmoothing(); //!< Method for computing the required parameters for the non-smoothed bang-bang profiler option - void computeBangCoastBangParametersNoSmoothing(); //!< Method for computing the required parameters for the non-smoothed bang-coast-bang profiler option - void computeSmoothedBangBangParameters(); //!< Method for computing the required parameters for the translation with a smoothed bang-bang acceleration profile - void computeSmoothedBangCoastBangParameters(); //!< Method for computing the required parameters for the smoothed bang-coast-bang option + void computeTranslationParameters(); //!< Intermediate method to group the calculation of translation parameters + //!< into a single method + void computeBangBangParametersNoSmoothing(); //!< Method for computing the required parameters for the non-smoothed + //!< bang-bang profiler option + void computeBangCoastBangParametersNoSmoothing(); //!< Method for computing the required parameters for the + //!< non-smoothed bang-coast-bang profiler option + void computeSmoothedBangBangParameters(); //!< Method for computing the required parameters for the translation with + //!< a smoothed bang-bang acceleration profile + void computeSmoothedBangCoastBangParameters(); //!< Method for computing the required parameters for the smoothed + //!< bang-coast-bang option /* Methods for computing the current translational states */ - void computeCurrentState(double time); //!< Intermediate method used to group the calculation of the current translational states into a single method - bool isInFirstBangSegment(double time) const; //!< Method for determining if the current time is within the first bang segment - bool isInSecondBangSegment(double time) const; //!< Method for determining if the current time is within the second bang segment - bool isInFirstSmoothedSegment(double time) const; //!< Method for determining if the current time is within the first smoothing segment for the smoothed profiler options - bool isInSecondSmoothedSegment(double time) const; //!< Method for determining if the current time is within the second smoothing segment for the smoothed profiler options - bool isInThirdSmoothedSegment(double time) const; //!< Method for determining if the current time is within the third smoothing segment for the smoothed profiler options - bool isInFourthSmoothedSegment(double time) const; //!< Method for determining if the current time is within the fourth smoothing segment for the smoothed bang-coast-bang option - bool isInCoastSegment(double time) const; //!< Method for determining if the current time is within the coast segment - void computeFirstBangSegment(double time); //!< Method for computing the first bang segment scalar translational states - void computeSecondBangSegment(double time); //!< Method for computing the second bang segment scalar translational states - void computeFirstSmoothedSegment(double time); //!< Method for computing the first smoothing segment scalar translational states for the smoothed profiler options - void computeSecondSmoothedSegment(double time); //!< Method for computing the second smoothing segment scalar translational states for the smoothed profiler options - void computeThirdSmoothedSegment(double time); //!< Method for computing the third smoothing segment scalar translational states for the smoothed profiler options - void computeFourthSmoothedSegment(double time); //!< Method for computing the fourth smoothing segment scalar translational states for the smoothed bang-coast-bang option - void computeCoastSegment(double time); //!< Method for computing the coast segment scalar translational states - void computeTranslationComplete(); //!< Method for computing the scalar translational states when the translation is complete - - void writeOutputMessages(uint64_t CurrentSimNanos); //!< Method for writing the module output messages and computing the output message data + void computeCurrentState(double time); //!< Intermediate method used to group the calculation of the current + //!< translational states into a single method + bool isInFirstBangSegment( + double time) const; //!< Method for determining if the current time is within the first bang segment + bool isInSecondBangSegment( + double time) const; //!< Method for determining if the current time is within the second bang segment + bool isInFirstSmoothedSegment(double time) const; //!< Method for determining if the current time is within the + //!< first smoothing segment for the smoothed profiler options + bool isInSecondSmoothedSegment(double time) const; //!< Method for determining if the current time is within the + //!< second smoothing segment for the smoothed profiler options + bool isInThirdSmoothedSegment(double time) const; //!< Method for determining if the current time is within the + //!< third smoothing segment for the smoothed profiler options + bool isInFourthSmoothedSegment( + double time) const; //!< Method for determining if the current time is within the fourth smoothing segment for the + //!< smoothed bang-coast-bang option + bool isInCoastSegment( + double time) const; //!< Method for determining if the current time is within the coast segment + void computeFirstBangSegment( + double time); //!< Method for computing the first bang segment scalar translational states + void computeSecondBangSegment( + double time); //!< Method for computing the second bang segment scalar translational states + void computeFirstSmoothedSegment(double time); //!< Method for computing the first smoothing segment scalar + //!< translational states for the smoothed profiler options + void computeSecondSmoothedSegment(double time); //!< Method for computing the second smoothing segment scalar + //!< translational states for the smoothed profiler options + void computeThirdSmoothedSegment(double time); //!< Method for computing the third smoothing segment scalar + //!< translational states for the smoothed profiler options + void computeFourthSmoothedSegment(double time); //!< Method for computing the fourth smoothing segment scalar + //!< translational states for the smoothed bang-coast-bang option + void computeCoastSegment(double time); //!< Method for computing the coast segment scalar translational states + void computeTranslationComplete(); //!< Method for computing the scalar translational states when the translation is + //!< complete + + void writeOutputMessages(uint64_t CurrentSimNanos); //!< Method for writing the module output messages and computing + //!< the output message data /* User-configurable variables */ - double coastOptionBangDuration; //!< [s] Time used for the coast option bang segment - double smoothingDuration; //!< [s] Time the acceleration is smoothed to the given maximum acceleration value - double transAccelMax; //!< [m/s^2] Maximum acceleration magnitude - Eigen::Vector3d transHat_M; //!< Axis along the direction of translation expressed in M frame components + double coastOptionBangDuration; //!< [s] Time used for the coast option bang segment + double smoothingDuration; //!< [s] Time the acceleration is smoothed to the given maximum acceleration value + double transAccelMax; //!< [m/s^2] Maximum acceleration magnitude + Eigen::Vector3d transHat_M; //!< Axis along the direction of translation expressed in M frame components /* Scalar translational states */ - double transPosInit; //!< [m] Initial translational body position from M to P frame origin along transHat_M - double transPosRef; //!< [m] Reference translational body position from M to P frame origin along transHat_M - double transPos; //!< [m] Current translational body position along transHat_M - double transVel; //!< [m] Current translational body velocity along transHat_M - double transAccel; //!< [m] Current translational body acceleration along transHat_M - double transPos_tb1; //!< [m] Position at the end of the first bang segment - double transVel_tb1; //!< [m/s] Velocity at the end of the first bang segment - double transPos_tb2; //!< [m] Position at the end of the second bang segment - double transVel_tb2; //!< [m/s] Velocity at the end of the second bang segment - double transPos_ts1; //!< [m] Position at the end of the first smoothed segment - double transVel_ts1; //!< [m/s] Velocity at the end of the first smoothed segment - double transPos_ts2; //!< [m] Position at the end of the second smoothed segment - double transVel_ts2; //!< [m/s] Velocity at the end of the second smoothed segment - double transPos_ts3; //!< [m] Position at the end of the third smoothed segment - double transVel_ts3; //!< [m/s] Velocity at the end of the third smoothed segment - double transPos_tc; //!< [m] Position at the end of the coast segment - double transVel_tc; //!< [m/s] Velocity at the end of the coast segment + double transPosInit; //!< [m] Initial translational body position from M to P frame origin along transHat_M + double transPosRef; //!< [m] Reference translational body position from M to P frame origin along transHat_M + double transPos; //!< [m] Current translational body position along transHat_M + double transVel; //!< [m] Current translational body velocity along transHat_M + double transAccel; //!< [m] Current translational body acceleration along transHat_M + double transPos_tb1; //!< [m] Position at the end of the first bang segment + double transVel_tb1; //!< [m/s] Velocity at the end of the first bang segment + double transPos_tb2; //!< [m] Position at the end of the second bang segment + double transVel_tb2; //!< [m/s] Velocity at the end of the second bang segment + double transPos_ts1; //!< [m] Position at the end of the first smoothed segment + double transVel_ts1; //!< [m/s] Velocity at the end of the first smoothed segment + double transPos_ts2; //!< [m] Position at the end of the second smoothed segment + double transVel_ts2; //!< [m/s] Velocity at the end of the second smoothed segment + double transPos_ts3; //!< [m] Position at the end of the third smoothed segment + double transVel_ts3; //!< [m/s] Velocity at the end of the third smoothed segment + double transPos_tc; //!< [m] Position at the end of the coast segment + double transVel_tc; //!< [m/s] Velocity at the end of the coast segment /* Temporal parameters */ - double tInit; //!< [s] Simulation time at the beginning of the translation - double t_b1; //!< [s] Simulation time at the end of the first bang segment - double t_b2; //!< [s] Simulation time at the end of the second bang segment - double t_s1; //!< [s] Simulation time at the end of the first smoothed segment - double t_s2; //!< [s] Simulation time at the end of the second smoothed segment - double t_s3; //!< [s] Simulation time at the end of the third smoothed segment - double t_c; //!< [s] Simulation time at the end of the coast segment - double t_f; //!< [s] Simulation time when the translation is complete - - bool convergence; //!< Boolean variable is true when the translation is complete - double a; //!< Parabolic constant for the first half of the bang-bang non-smoothed translation - double b; //!< Parabolic constant for the second half of the bang-bang non-smoothed translation + double tInit; //!< [s] Simulation time at the beginning of the translation + double t_b1; //!< [s] Simulation time at the end of the first bang segment + double t_b2; //!< [s] Simulation time at the end of the second bang segment + double t_s1; //!< [s] Simulation time at the end of the first smoothed segment + double t_s2; //!< [s] Simulation time at the end of the second smoothed segment + double t_s3; //!< [s] Simulation time at the end of the third smoothed segment + double t_c; //!< [s] Simulation time at the end of the coast segment + double t_f; //!< [s] Simulation time when the translation is complete + + bool convergence; //!< Boolean variable is true when the translation is complete + double a; //!< Parabolic constant for the first half of the bang-bang non-smoothed translation + double b; //!< Parabolic constant for the second half of the bang-bang non-smoothed translation }; #endif 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/prescribedRotation1DOF/prescribedRotation1DOF.cpp b/src/simulation/deviceInterface/prescribedRotation1DOF/prescribedRotation1DOF.cpp index 08fa7b3226..4ea8b3ec70 100644 --- a/src/simulation/deviceInterface/prescribedRotation1DOF/prescribedRotation1DOF.cpp +++ b/src/simulation/deviceInterface/prescribedRotation1DOF/prescribedRotation1DOF.cpp @@ -27,7 +27,9 @@ /*! This method self initializes the C-wrapped output messages. */ -void PrescribedRotation1DOF::SelfInit() { +void +PrescribedRotation1DOF::SelfInit() +{ HingedRigidBodyMsg_C_init(&this->spinningBodyOutMsgC); PrescribedRotationMsg_C_init(&this->prescribedRotationOutMsgC); } @@ -36,7 +38,9 @@ void PrescribedRotation1DOF::SelfInit() { @param callTime [ns] Time the method is called */ -void PrescribedRotation1DOF::Reset(uint64_t callTime) { +void +PrescribedRotation1DOF::Reset(uint64_t callTime) +{ if (!this->spinningBodyInMsg.isLinked()) { _bskLog(this->bskLogger, BSK_ERROR, "prescribedRotation1DOF.spinningBodyInMsg wasn't connected."); } @@ -54,7 +58,9 @@ void PrescribedRotation1DOF::Reset(uint64_t callTime) { @param callTime [ns] Time the method is called */ -void PrescribedRotation1DOF::UpdateState(uint64_t callTime) { +void +PrescribedRotation1DOF::UpdateState(uint64_t callTime) +{ // Read the input message HingedRigidBodyMsgPayload spinningBodyIn = HingedRigidBodyMsgPayload(); if (this->spinningBodyInMsg.isWritten()) { @@ -94,7 +100,9 @@ void PrescribedRotation1DOF::UpdateState(uint64_t callTime) { /*! This intermediate method groups the calculation of rotation parameters into a single method. */ -void PrescribedRotation1DOF::computeRotationParameters() { +void +PrescribedRotation1DOF::computeRotationParameters() +{ if (this->coastOptionBangDuration > 0.0) { if (this->smoothingDuration > 0.0) { this->computeSmoothedBangCoastBangParameters(); @@ -113,7 +121,9 @@ void PrescribedRotation1DOF::computeRotationParameters() { /*! This method computes the required parameters for the rotation with a non-smoothed bang-bang acceleration profile. */ -void PrescribedRotation1DOF::computeBangBangParametersNoSmoothing() { +void +PrescribedRotation1DOF::computeBangBangParametersNoSmoothing() +{ // Determine the total time required for the rotation double totalRotTime = sqrt(((0.5 * fabs(this->thetaRef - this->thetaInit)) * 8.0) / this->thetaDDotMax); @@ -128,18 +138,21 @@ void PrescribedRotation1DOF::computeBangBangParametersNoSmoothing() { this->b = -0.5 * (this->thetaRef - this->thetaInit) / ((this->t_b1 - this->t_f) * (this->t_b1 - this->t_f)); } -/*! This method computes the required parameters for the rotation with a non-smoothed bang-coast-bang acceleration profile. +/*! This method computes the required parameters for the rotation with a non-smoothed bang-coast-bang acceleration + profile. */ -void PrescribedRotation1DOF::computeBangCoastBangParametersNoSmoothing() { +void +PrescribedRotation1DOF::computeBangCoastBangParametersNoSmoothing() +{ double sign = (this->thetaRef - this->thetaInit) / abs(this->thetaRef - this->thetaInit); // Determine the time at the end of the first bang segment t_b1 this->t_b1 = this->tInit + this->coastOptionBangDuration; // Determine the hub-relative angle and rate at time t_b1 - this->theta_tb1 = sign * 0.5 * this->thetaDDotMax * this->coastOptionBangDuration - * this->coastOptionBangDuration + this->thetaInit; + this->theta_tb1 = + sign * 0.5 * this->thetaDDotMax * this->coastOptionBangDuration * this->coastOptionBangDuration + this->thetaInit; this->thetaDot_tb1 = sign * this->thetaDDotMax * this->coastOptionBangDuration; // Determine the angle traveled during the coast period @@ -165,7 +178,9 @@ void PrescribedRotation1DOF::computeBangCoastBangParametersNoSmoothing() { /*! This method computes the required parameters for the rotation with a smoothed bang-bang acceleration profile. */ -void PrescribedRotation1DOF::computeSmoothedBangBangParameters() { +void +PrescribedRotation1DOF::computeSmoothedBangBangParameters() +{ double sign = (this->thetaRef - this->thetaInit) / abs(this->thetaRef - this->thetaInit); // Determine the time at the end of the first smoothing segment t_s1 @@ -173,40 +188,41 @@ void PrescribedRotation1DOF::computeSmoothedBangBangParameters() { // Determine the hub-relative angle and rate at time t_s1 this->thetaDot_ts1 = sign * 0.5 * this->thetaDDotMax * this->smoothingDuration; - this->theta_ts1 = sign * (3.0 / 20.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration - + this->thetaInit; + this->theta_ts1 = + sign * (3.0 / 20.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration + this->thetaInit; // Determine the duration of the bang segment bangDuration double aTerm = sign * 0.5 * this->thetaDDotMax; double bTerm = (sign * this->thetaDDotMax * this->smoothingDuration + this->thetaDot_ts1) / aTerm; - double cTerm = (sign * (2.0 / 5.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration - + this->thetaDot_ts1 * this->smoothingDuration + this->theta_ts1 - - 0.5 * (this->thetaRef + this->thetaInit)) / aTerm; - double bangDuration = (- bTerm + sqrt(bTerm * bTerm - 4.0 * cTerm)) / 2.0; + double cTerm = + (sign * (2.0 / 5.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration + + this->thetaDot_ts1 * this->smoothingDuration + this->theta_ts1 - 0.5 * (this->thetaRef + this->thetaInit)) / + aTerm; + double bangDuration = (-bTerm + sqrt(bTerm * bTerm - 4.0 * cTerm)) / 2.0; // Determine the time at the end of the first bang segment t_b1 this->t_b1 = this->t_s1 + bangDuration; // Determine the hub-relative angle and rate at time t_b1 this->thetaDot_tb1 = sign * this->thetaDDotMax * bangDuration + this->thetaDot_ts1; - this->theta_tb1 = sign * 0.5 * this->thetaDDotMax * bangDuration * bangDuration - + this->thetaDot_ts1 * bangDuration + this->theta_ts1; + this->theta_tb1 = sign * 0.5 * this->thetaDDotMax * bangDuration * bangDuration + + this->thetaDot_ts1 * bangDuration + this->theta_ts1; // Determine the time at the end of the second smoothing segment t_s2 this->t_s2 = this->t_b1 + 2.0 * this->smoothingDuration; // Determine the hub-relative angle and rate at time t_s2 this->thetaDot_ts2 = this->thetaDot_tb1; - this->theta_ts2 = sign * (4.0 / 5.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration - + this->thetaDot_tb1 * 2.0 * this->smoothingDuration + this->theta_tb1; + this->theta_ts2 = sign * (4.0 / 5.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration + + this->thetaDot_tb1 * 2.0 * this->smoothingDuration + this->theta_tb1; // Determine the time at the end of the second bang segment t_b2 this->t_b2 = this->t_s2 + bangDuration; // Determine the hub-relative angle and rate at time t_b2 - this->thetaDot_tb2 = - sign * this->thetaDDotMax * bangDuration + this->thetaDot_ts2; - this->theta_tb2 = - sign * 0.5 * this->thetaDDotMax * bangDuration * bangDuration - + this->thetaDot_ts2 * bangDuration + this->theta_ts2; + this->thetaDot_tb2 = -sign * this->thetaDDotMax * bangDuration + this->thetaDot_ts2; + this->theta_tb2 = -sign * 0.5 * this->thetaDDotMax * bangDuration * bangDuration + + this->thetaDot_ts2 * bangDuration + this->theta_ts2; // Determine the time when the rotation is complete t_f this->t_f = this->t_b2 + this->smoothingDuration; @@ -215,7 +231,9 @@ void PrescribedRotation1DOF::computeSmoothedBangBangParameters() { /*! This method computes the required parameters for the rotation with a smoothed bang-coast-bang acceleration profile. */ -void PrescribedRotation1DOF::computeSmoothedBangCoastBangParameters() { +void +PrescribedRotation1DOF::computeSmoothedBangCoastBangParameters() +{ double sign = (this->thetaRef - this->thetaInit) / abs(this->thetaRef - this->thetaInit); // Determine the time at the end of the first smoothing segment t_s1 @@ -223,25 +241,24 @@ void PrescribedRotation1DOF::computeSmoothedBangCoastBangParameters() { // Determine the hub-relative angle and rate at time t_s1 this->thetaDot_ts1 = sign * 0.5 * this->thetaDDotMax * this->smoothingDuration; - this->theta_ts1 = sign * (3.0 / 20.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration - + this->thetaInit; + this->theta_ts1 = + sign * (3.0 / 20.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration + this->thetaInit; // Determine the time at the end of the first bang segment t_b1 this->t_b1 = this->t_s1 + this->coastOptionBangDuration; // Determine the hub-relative angle and rate at time t_b1 this->thetaDot_tb1 = sign * this->thetaDDotMax * this->coastOptionBangDuration + this->thetaDot_ts1; - this->theta_tb1 = sign * 0.5 * this->thetaDDotMax * this->coastOptionBangDuration - * this->coastOptionBangDuration + this->thetaDot_ts1 * this->coastOptionBangDuration - + this->theta_ts1; + this->theta_tb1 = sign * 0.5 * this->thetaDDotMax * this->coastOptionBangDuration * this->coastOptionBangDuration + + this->thetaDot_ts1 * this->coastOptionBangDuration + this->theta_ts1; // Determine the time at the end of the second smoothing segment t_s2 this->t_s2 = this->t_b1 + this->smoothingDuration; // Determine the hub-relative angle and rate at time t_s2 this->thetaDot_ts2 = sign * 0.5 * this->thetaDDotMax * this->smoothingDuration + this->thetaDot_tb1; - this->theta_ts2 = sign * (7.0 / 20.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration - + this->thetaDot_tb1 * this->smoothingDuration + this->theta_tb1; + this->theta_ts2 = sign * (7.0 / 20.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration + + this->thetaDot_tb1 * this->smoothingDuration + this->theta_tb1; // Compute the time at the end of the coast segment t_c double deltaThetaCoast = (this->thetaRef - this->thetaInit) - 2 * (this->theta_ts2 - this->thetaInit); @@ -255,18 +272,17 @@ void PrescribedRotation1DOF::computeSmoothedBangCoastBangParameters() { this->t_s3 = this->t_c + this->smoothingDuration; // Determine the hub-relative angle and rate at time t_s3 - this->thetaDot_ts3 = - sign * 0.5 * this->thetaDDotMax * this->smoothingDuration + this->thetaDot_tc; - this->theta_ts3 = - sign * (3.0 / 20.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration - + this->thetaDot_tc * this->smoothingDuration + this->theta_tc; + this->thetaDot_ts3 = -sign * 0.5 * this->thetaDDotMax * this->smoothingDuration + this->thetaDot_tc; + this->theta_ts3 = -sign * (3.0 / 20.0) * this->thetaDDotMax * this->smoothingDuration * this->smoothingDuration + + this->thetaDot_tc * this->smoothingDuration + this->theta_tc; // Determine the time at the end of the second bang segment t_b2 this->t_b2 = this->t_s3 + this->coastOptionBangDuration; // Determine the hub-relative angle and rate at time t_b2 - this->thetaDot_tb2 = - sign * this->thetaDDotMax * this->coastOptionBangDuration + this->thetaDot_ts3; - this->theta_tb2 = - sign * 0.5 * this->thetaDDotMax * this->coastOptionBangDuration - * this->coastOptionBangDuration + this->thetaDot_ts3 * this->coastOptionBangDuration - + this->theta_ts3; + this->thetaDot_tb2 = -sign * this->thetaDDotMax * this->coastOptionBangDuration + this->thetaDot_ts3; + this->theta_tb2 = -sign * 0.5 * this->thetaDDotMax * this->coastOptionBangDuration * this->coastOptionBangDuration + + this->thetaDot_ts3 * this->coastOptionBangDuration + this->theta_ts3; // Determine the time when the rotation is complete t_f this->t_f = this->t_b2 + this->smoothingDuration; @@ -275,9 +291,11 @@ void PrescribedRotation1DOF::computeSmoothedBangCoastBangParameters() { /*! This intermediate method groups the calculation of the current rotational states into a single method. */ -void PrescribedRotation1DOF::computeCurrentState(double t) { +void +PrescribedRotation1DOF::computeCurrentState(double t) +{ if (this->coastOptionBangDuration > 0.0) { - if(this->smoothingDuration > 0.0) { + if (this->smoothingDuration > 0.0) { if (this->isInFirstSmoothedSegment(t)) { this->computeFirstSmoothedSegment(t); } else if (this->isInFirstBangSegment(t)) { @@ -337,7 +355,9 @@ void PrescribedRotation1DOF::computeCurrentState(double t) { @return bool @param t [s] Current simulation time */ -bool PrescribedRotation1DOF::isInFirstBangSegment(double t) const { +bool +PrescribedRotation1DOF::isInFirstBangSegment(double t) const +{ if (this->smoothingDuration > 0.0) { return (t > this->t_s1 && t <= this->t_b1 && this->t_f - this->tInit != 0.0); } else { @@ -349,7 +369,9 @@ bool PrescribedRotation1DOF::isInFirstBangSegment(double t) const { @return bool @param t [s] Current simulation time */ -bool PrescribedRotation1DOF::isInSecondBangSegment(double t) const { +bool +PrescribedRotation1DOF::isInSecondBangSegment(double t) const +{ if (this->coastOptionBangDuration > 0.0) { if (this->smoothingDuration > 0.0) { return (t > this->t_s3 && t <= this->t_b2 && this->t_f - this->tInit != 0.0); @@ -369,15 +391,20 @@ bool PrescribedRotation1DOF::isInSecondBangSegment(double t) const { @return bool @param t [s] Current simulation time */ -bool PrescribedRotation1DOF::isInFirstSmoothedSegment(double t) const { +bool +PrescribedRotation1DOF::isInFirstSmoothedSegment(double t) const +{ return (t <= this->t_s1 && this->t_f - this->tInit != 0.0); } -/*! This method determines if the current time is within the second smoothing segment for the smoothed profiler options.. +/*! This method determines if the current time is within the second smoothing segment for the smoothed profiler + options.. @return bool @param t [s] Current simulation time */ -bool PrescribedRotation1DOF::isInSecondSmoothedSegment(double t) const { +bool +PrescribedRotation1DOF::isInSecondSmoothedSegment(double t) const +{ return (t > this->t_b1 && t <= this->t_s2 && this->t_f - this->tInit != 0.0); } @@ -385,7 +412,9 @@ bool PrescribedRotation1DOF::isInSecondSmoothedSegment(double t) const { @return bool @param t [s] Current simulation time */ -bool PrescribedRotation1DOF::isInThirdSmoothedSegment(double t) const { +bool +PrescribedRotation1DOF::isInThirdSmoothedSegment(double t) const +{ if (this->coastOptionBangDuration > 0.0) { return (t > this->t_c && t <= this->t_s3 && this->t_f - this->tInit != 0.0); } else { @@ -393,11 +422,14 @@ bool PrescribedRotation1DOF::isInThirdSmoothedSegment(double t) const { } } -/*! This method determines if the current time is within the fourth smoothing segment for the smoothed bang-coast-bang option. +/*! This method determines if the current time is within the fourth smoothing segment for the smoothed bang-coast-bang + option. @return bool @param t [s] Current simulation time */ -bool PrescribedRotation1DOF::isInFourthSmoothedSegment(double t) const { +bool +PrescribedRotation1DOF::isInFourthSmoothedSegment(double t) const +{ return (t > this->t_b2 && t <= this->t_f && this->t_f - this->tInit != 0.0); } @@ -405,10 +437,12 @@ bool PrescribedRotation1DOF::isInFourthSmoothedSegment(double t) const { @return bool @param t [s] Current simulation time */ -bool PrescribedRotation1DOF::isInCoastSegment(double t) const { +bool +PrescribedRotation1DOF::isInCoastSegment(double t) const +{ if (this->smoothingDuration > 0.0) { return (t > this->t_s2 && t <= this->t_c && this->t_f - this->tInit != 0.0); - } else{ + } else { return (t > this->t_b1 && t <= this->t_c && this->t_f - this->tInit != 0.0); } } @@ -417,14 +451,16 @@ bool PrescribedRotation1DOF::isInCoastSegment(double t) const { @param t [s] Current simulation time */ -void PrescribedRotation1DOF::computeFirstBangSegment(double t) { +void +PrescribedRotation1DOF::computeFirstBangSegment(double t) +{ double sign = (this->thetaRef - this->thetaInit) / abs(this->thetaRef - this->thetaInit); this->thetaDDot = sign * this->thetaDDotMax; if (this->smoothingDuration > 0.0) { this->thetaDot = this->thetaDDot * (t - this->t_s1) + this->thetaDot_ts1; - this->theta = 0.5 * this->thetaDDot * (t - this->t_s1) * (t - this->t_s1) - + this->thetaDot_ts1 * (t - this->t_s1) + this->theta_ts1; + this->theta = 0.5 * this->thetaDDot * (t - this->t_s1) * (t - this->t_s1) + + this->thetaDot_ts1 * (t - this->t_s1) + this->theta_ts1; } else { this->thetaDot = this->thetaDDot * (t - this->tInit); this->theta = this->a * (t - this->tInit) * (t - this->tInit) + this->thetaInit; @@ -435,19 +471,21 @@ void PrescribedRotation1DOF::computeFirstBangSegment(double t) { @param t [s] Current simulation time */ -void PrescribedRotation1DOF::computeSecondBangSegment(double t) { +void +PrescribedRotation1DOF::computeSecondBangSegment(double t) +{ double sign = (this->thetaRef - this->thetaInit) / abs(this->thetaRef - this->thetaInit); - this->thetaDDot = - sign * this->thetaDDotMax; + this->thetaDDot = -sign * this->thetaDDotMax; if (this->smoothingDuration > 0.0) { if (this->coastOptionBangDuration > 0.0) { this->thetaDot = this->thetaDDot * (t - this->t_s3) + this->thetaDot_ts3; - this->theta = 0.5 * this->thetaDDot * (t - this->t_s3) * (t - this->t_s3) - + this->thetaDot_ts3 * (t - this->t_s3) + this->theta_ts3; + this->theta = 0.5 * this->thetaDDot * (t - this->t_s3) * (t - this->t_s3) + + this->thetaDot_ts3 * (t - this->t_s3) + this->theta_ts3; } else { this->thetaDot = this->thetaDDot * (t - this->t_s2) + this->thetaDot_ts2; - this->theta = 0.5 * this->thetaDDot * (t - this->t_s2) * (t - this->t_s2) - + this->thetaDot_ts2 * (t - this->t_s2) + this->theta_ts2; + this->theta = 0.5 * this->thetaDDot * (t - this->t_s2) * (t - this->t_s2) + + this->thetaDot_ts2 * (t - this->t_s2) + this->theta_ts2; } } else { this->thetaDot = this->thetaDDot * (t - this->t_f); @@ -459,20 +497,22 @@ void PrescribedRotation1DOF::computeSecondBangSegment(double t) { @param t [s] Current simulation time */ -void PrescribedRotation1DOF::computeFirstSmoothedSegment(double t) { +void +PrescribedRotation1DOF::computeFirstSmoothedSegment(double t) +{ double sign = (this->thetaRef - this->thetaInit) / abs(this->thetaRef - this->thetaInit); double term1 = (3.0 * (t - this->tInit) * (t - this->tInit)) / (this->smoothingDuration * this->smoothingDuration); - double term2 = (2.0 * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term3 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (this->smoothingDuration * this->smoothingDuration); - double term4 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term5 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - double term6 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term2 = (2.0 * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term3 = + ((t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / (this->smoothingDuration * this->smoothingDuration); + double term4 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term5 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + double term6 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); this->thetaDDot = sign * this->thetaDDotMax * (term1 - term2); this->thetaDot = sign * this->thetaDDotMax * (term3 - term4); @@ -483,7 +523,9 @@ void PrescribedRotation1DOF::computeFirstSmoothedSegment(double t) { @param t [s] Current simulation time */ -void PrescribedRotation1DOF::computeSecondSmoothedSegment(double t) { +void +PrescribedRotation1DOF::computeSecondSmoothedSegment(double t) +{ double sign = (this->thetaRef - this->thetaInit) / abs(this->thetaRef - this->thetaInit); double term1; @@ -496,43 +538,45 @@ void PrescribedRotation1DOF::computeSecondSmoothedSegment(double t) { if (this->coastOptionBangDuration > 0.0) { term1 = (3.0 * (t - this->t_b1) * (t - this->t_b1)) / (this->smoothingDuration * this->smoothingDuration); - term2 = (2.0 * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term2 = (2.0 * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); term5 = 0.5 * (t - this->t_b1) * (t - this->t_b1); - term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); } else { term1 = (3.0 * (t - this->t_b1) * (t - this->t_b1)) / (2.0 * this->smoothingDuration * this->smoothingDuration); - term2 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (2.0 * this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (8.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term2 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (2.0 * this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (8.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); term5 = 0.5 * (t - this->t_b1) * (t - this->t_b1); - term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (8.0 * this->smoothingDuration * this->smoothingDuration); - term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (40.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (8.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (40.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); } this->thetaDDot = sign * this->thetaDDotMax * (1.0 - term1 + term2); this->thetaDot = sign * this->thetaDDotMax * ((t - this->t_b1) - term3 + term4) + this->thetaDot_tb1; - this->theta = sign * this->thetaDDotMax * (term5 - term6 + term7) - + this->thetaDot_tb1 * (t - this->t_b1) + this->theta_tb1; + this->theta = + sign * this->thetaDDotMax * (term5 - term6 + term7) + this->thetaDot_tb1 * (t - this->t_b1) + this->theta_tb1; } /*! This method computes the third smoothing segment scalar rotational states for the smoothed profiler options. @param t [s] Current simulation time */ -void PrescribedRotation1DOF::computeThirdSmoothedSegment(double t) { +void +PrescribedRotation1DOF::computeThirdSmoothedSegment(double t) +{ double sign = (this->thetaRef - this->thetaInit) / abs(this->thetaRef - this->thetaInit); double term1; @@ -545,38 +589,39 @@ void PrescribedRotation1DOF::computeThirdSmoothedSegment(double t) { if (this->coastOptionBangDuration > 0.0) { term1 = (3.0 * (t - this->t_c) * (t - this->t_c)) / (this->smoothingDuration * this->smoothingDuration); - term2 = (2.0 * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term5 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - term6 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - - this->thetaDDot = - sign * this->thetaDDotMax * (term1 - term2); - this->thetaDot = - sign * this->thetaDDotMax * (term3 - term4) + this->thetaDot_tc; - this->theta = - sign * this->thetaDDotMax * (term5 - term6) + this->thetaDot_tc * (t - this->t_c) + this->theta_tc; + term2 = (2.0 * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = + ((t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term5 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + term6 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + + this->thetaDDot = -sign * this->thetaDDotMax * (term1 - term2); + this->thetaDot = -sign * this->thetaDDotMax * (term3 - term4) + this->thetaDot_tc; + this->theta = + -sign * this->thetaDDotMax * (term5 - term6) + this->thetaDot_tc * (t - this->t_c) + this->theta_tc; } else { term1 = (3.0 * (t - this->t_b2) * (t - this->t_b2)) / (this->smoothingDuration * this->smoothingDuration); - term2 = (2.0 * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term5 = - 0.5 * (t - this->t_b2) * (t - this->t_b2); - term6 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - term7 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - - this->thetaDDot = sign * this->thetaDDotMax * ( - 1.0 + term1 - term2); - this->thetaDot = sign * this->thetaDDotMax * ( - (t - this->t_b2) + term3 - term4) + this->thetaDot_tb2; - this->theta = sign * this->thetaDDotMax * (term5 + term6 - term7) + this->thetaDot_tb2 * (t - this->t_b2) - + this->theta_tb2; + term2 = (2.0 * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term5 = -0.5 * (t - this->t_b2) * (t - this->t_b2); + term6 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + + this->thetaDDot = sign * this->thetaDDotMax * (-1.0 + term1 - term2); + this->thetaDot = sign * this->thetaDDotMax * (-(t - this->t_b2) + term3 - term4) + this->thetaDot_tb2; + this->theta = + sign * this->thetaDDotMax * (term5 + term6 - term7) + this->thetaDot_tb2 * (t - this->t_b2) + this->theta_tb2; } } @@ -584,31 +629,35 @@ void PrescribedRotation1DOF::computeThirdSmoothedSegment(double t) { @param t [s] Current simulation time */ -void PrescribedRotation1DOF::computeFourthSmoothedSegment(double t) { +void +PrescribedRotation1DOF::computeFourthSmoothedSegment(double t) +{ double term1 = (3.0 * (this->t_f - t) * (this->t_f - t)) / (this->smoothingDuration * this->smoothingDuration); - double term2 = (2.0 * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term3 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (this->smoothingDuration * this->smoothingDuration); - double term4 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term5 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - double term6 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term2 = (2.0 * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term3 = + ((this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / (this->smoothingDuration * this->smoothingDuration); + double term4 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term5 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + double term6 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); double sign = (this->thetaRef - this->thetaInit) / abs(this->thetaRef - this->thetaInit); - this->thetaDDot = - sign * this->thetaDDotMax * (term1 - term2); + this->thetaDDot = -sign * this->thetaDDotMax * (term1 - term2); this->thetaDot = sign * this->thetaDDotMax * (term3 - term4); - this->theta = - sign * this->thetaDDotMax * (term5 - term6) + this->thetaRef; + this->theta = -sign * this->thetaDDotMax * (term5 - term6) + this->thetaRef; } /*! This method computes the coast segment scalar rotational states @param t [s] Current simulation time */ -void PrescribedRotation1DOF::computeCoastSegment(double t) { +void +PrescribedRotation1DOF::computeCoastSegment(double t) +{ this->thetaDDot = 0.0; if (this->smoothingDuration > 0.0) { @@ -623,7 +672,9 @@ void PrescribedRotation1DOF::computeCoastSegment(double t) { /*! This method computes the scalar rotational states when the rotation is complete. */ -void PrescribedRotation1DOF::computeRotationComplete() { +void +PrescribedRotation1DOF::computeRotationComplete() +{ this->thetaDDot = 0.0; this->thetaDot = 0.0; this->theta = this->thetaRef; @@ -633,7 +684,9 @@ void PrescribedRotation1DOF::computeRotationComplete() { /*! This method writes the module output messages and computes the output message data. */ -void PrescribedRotation1DOF::writeOutputMessages(uint64_t callTime) { +void +PrescribedRotation1DOF::writeOutputMessages(uint64_t callTime) +{ // Create the output buffer messages HingedRigidBodyMsgPayload spinningBodyOut; PrescribedRotationMsgPayload prescribedRotationOut; @@ -643,10 +696,10 @@ void PrescribedRotation1DOF::writeOutputMessages(uint64_t callTime) { prescribedRotationOut = PrescribedRotationMsgPayload(); // Compute the angular velocity of frame P wrt frame M in P frame components - Eigen::Vector3d omega_PM_P = this->thetaDot * this->rotHat_M; // [rad/s] + Eigen::Vector3d omega_PM_P = this->thetaDot * this->rotHat_M; // [rad/s] // Compute the B frame time derivative of omega_PM_P in P frame components - Eigen::Vector3d omegaPrime_PM_P = this->thetaDDot * this->rotHat_M; // [rad/s^2] + Eigen::Vector3d omegaPrime_PM_P = this->thetaDDot * this->rotHat_M; // [rad/s^2] // Compute the MRP attitude of spinning body frame P with respect to frame M Eigen::Vector3d sigma_PM = this->computeSigma_PM(); @@ -668,7 +721,9 @@ void PrescribedRotation1DOF::writeOutputMessages(uint64_t callTime) { /*! This method computes the current spinning body MRP attitude relative to the mount frame: sigma_PM */ -Eigen::Vector3d PrescribedRotation1DOF::computeSigma_PM() { +Eigen::Vector3d +PrescribedRotation1DOF::computeSigma_PM() +{ // Determine dcm_PP0 for the current spinning body attitude relative to the initial attitude double dcm_PP0[3][3]; double prv_PP0_array[3]; @@ -698,7 +753,9 @@ Eigen::Vector3d PrescribedRotation1DOF::computeSigma_PM() { @param bangDuration [s] Bang segment time duration */ -void PrescribedRotation1DOF::setCoastOptionBangDuration(const double bangDuration) { +void +PrescribedRotation1DOF::setCoastOptionBangDuration(const double bangDuration) +{ this->coastOptionBangDuration = bangDuration; } @@ -706,7 +763,9 @@ void PrescribedRotation1DOF::setCoastOptionBangDuration(const double bangDuratio @param rotHat_M Spinning body rotation axis (unit vector) */ -void PrescribedRotation1DOF::setRotHat_M(const Eigen::Vector3d &rotHat_M) { +void +PrescribedRotation1DOF::setRotHat_M(const Eigen::Vector3d& rotHat_M) +{ this->rotHat_M = rotHat_M / rotHat_M.norm(); } @@ -714,7 +773,9 @@ void PrescribedRotation1DOF::setRotHat_M(const Eigen::Vector3d &rotHat_M) { @param smoothingDuration [s] Duration the acceleration is smoothed until reaching the given maximum acceleration value */ -void PrescribedRotation1DOF::setSmoothingDuration(const double smoothingDuration) { +void +PrescribedRotation1DOF::setSmoothingDuration(const double smoothingDuration) +{ this->smoothingDuration = smoothingDuration; } @@ -722,7 +783,9 @@ void PrescribedRotation1DOF::setSmoothingDuration(const double smoothingDuration @param thetaDDotMax [rad/s^2] Bang segment scalar angular acceleration */ -void PrescribedRotation1DOF::setThetaDDotMax(const double thetaDDotMax) { +void +PrescribedRotation1DOF::setThetaDDotMax(const double thetaDDotMax) +{ this->thetaDDotMax = thetaDDotMax; } @@ -730,41 +793,53 @@ void PrescribedRotation1DOF::setThetaDDotMax(const double thetaDDotMax) { @param thetaInit [rad] Initial spinning body angle */ -void PrescribedRotation1DOF::setThetaInit(const double thetaInit) { +void +PrescribedRotation1DOF::setThetaInit(const double thetaInit) +{ this->thetaInit = thetaInit; } /*! Getter method for the coast option bang duration. @return double */ -double PrescribedRotation1DOF::getCoastOptionBangDuration() const { +double +PrescribedRotation1DOF::getCoastOptionBangDuration() const +{ return this->coastOptionBangDuration; } /*! Getter method for the spinning body rotation axis. @return const Eigen::Vector3d */ -const Eigen::Vector3d &PrescribedRotation1DOF::getRotHat_M() const { +const Eigen::Vector3d& +PrescribedRotation1DOF::getRotHat_M() const +{ return this->rotHat_M; } /*! Getter method for the duration the acceleration is smoothed until reaching the given maximum acceleration value. @return double */ -double PrescribedRotation1DOF::getSmoothingDuration() const { +double +PrescribedRotation1DOF::getSmoothingDuration() const +{ return this->smoothingDuration; } /*! Getter method for the ramp segment scalar angular acceleration. @return double */ -double PrescribedRotation1DOF::getThetaDDotMax() const { +double +PrescribedRotation1DOF::getThetaDDotMax() const +{ return this->thetaDDotMax; } /*! Getter method for the initial spinning body angle. @return double */ -double PrescribedRotation1DOF::getThetaInit() const { +double +PrescribedRotation1DOF::getThetaInit() const +{ return this->thetaInit; } diff --git a/src/simulation/deviceInterface/prescribedRotation1DOF/prescribedRotation1DOF.h b/src/simulation/deviceInterface/prescribedRotation1DOF/prescribedRotation1DOF.h index d901a68d00..aad2f397bc 100644 --- a/src/simulation/deviceInterface/prescribedRotation1DOF/prescribedRotation1DOF.h +++ b/src/simulation/deviceInterface/prescribedRotation1DOF/prescribedRotation1DOF.h @@ -28,100 +28,128 @@ #include /*! @brief Prescribed 1 DOF Rotation Profiler Class */ -class PrescribedRotation1DOF: public SysModel{ -public: - PrescribedRotation1DOF() = default; //!< Constructor - ~PrescribedRotation1DOF() = default; //!< Destructor - - void SelfInit() override; //!< Member function to initialize the C-wrapped output message - void Reset(uint64_t CurrentSimNanos) override; //!< Reset member function - void UpdateState(uint64_t CurrentSimNanos) override; //!< Update member function - void setCoastOptionBangDuration(const double bangDuration); //!< Setter for the coast option bang duration - void setRotHat_M(const Eigen::Vector3d &rotHat_M); //!< Setter for the spinning body rotation axis - void setSmoothingDuration(const double smoothingDuration); //!< Setter method for the duration the acceleration is smoothed until reaching the given maximum acceleration value - void setThetaDDotMax(const double thetaDDotMax); //!< Setter for the bang segment scalar angular acceleration - void setThetaInit(const double thetaInit); //!< Setter for the initial spinning body angle - double getCoastOptionBangDuration() const; //!< Getter for the coast option bang duration - const Eigen::Vector3d &getRotHat_M() const; //!< Getter for the spinning body rotation axis - double getSmoothingDuration() const; //!< Getter method for the duration the acceleration is smoothed until reaching the given maximum acceleration value - double getThetaDDotMax() const; //!< Getter for the bang segment scalar angular acceleration - double getThetaInit() const; //!< Getter for the initial spinning body angle - - ReadFunctor spinningBodyInMsg; //!< Input msg for the spinning body reference angle and angle rate - Message spinningBodyOutMsg; //!< Output msg for the spinning body angle and angle rate - Message prescribedRotationOutMsg; //!< Output msg for the spinning body prescribed rotational states - HingedRigidBodyMsg_C spinningBodyOutMsgC = {}; //!< C-wrapped output msg for the spinning body angle and angle rate - PrescribedRotationMsg_C prescribedRotationOutMsgC = {}; //!< C-wrapped output msg for the spinning body prescribed rotational states - - BSKLogger *bskLogger; //!< BSK Logging - -private: +class PrescribedRotation1DOF : public SysModel +{ + public: + PrescribedRotation1DOF() = default; //!< Constructor + ~PrescribedRotation1DOF() = default; //!< Destructor + + void SelfInit() override; //!< Member function to initialize the C-wrapped output message + void Reset(uint64_t CurrentSimNanos) override; //!< Reset member function + void UpdateState(uint64_t CurrentSimNanos) override; //!< Update member function + void setCoastOptionBangDuration(const double bangDuration); //!< Setter for the coast option bang duration + void setRotHat_M(const Eigen::Vector3d& rotHat_M); //!< Setter for the spinning body rotation axis + void setSmoothingDuration( + const double smoothingDuration); //!< Setter method for the duration the acceleration is smoothed until reaching + //!< the given maximum acceleration value + void setThetaDDotMax(const double thetaDDotMax); //!< Setter for the bang segment scalar angular acceleration + void setThetaInit(const double thetaInit); //!< Setter for the initial spinning body angle + double getCoastOptionBangDuration() const; //!< Getter for the coast option bang duration + const Eigen::Vector3d& getRotHat_M() const; //!< Getter for the spinning body rotation axis + double getSmoothingDuration() const; //!< Getter method for the duration the acceleration is smoothed until reaching + //!< the given maximum acceleration value + double getThetaDDotMax() const; //!< Getter for the bang segment scalar angular acceleration + double getThetaInit() const; //!< Getter for the initial spinning body angle + + ReadFunctor + spinningBodyInMsg; //!< Input msg for the spinning body reference angle and angle rate + Message spinningBodyOutMsg; //!< Output msg for the spinning body angle and angle rate + Message + prescribedRotationOutMsg; //!< Output msg for the spinning body prescribed rotational states + HingedRigidBodyMsg_C spinningBodyOutMsgC = {}; //!< C-wrapped output msg for the spinning body angle and angle rate + PrescribedRotationMsg_C + prescribedRotationOutMsgC = {}; //!< C-wrapped output msg for the spinning body prescribed rotational states + + BSKLogger* bskLogger; //!< BSK Logging + + private: /* Methods for computing the required rotational parameters */ - void computeRotationParameters(); //!< Intermediate method to group the calculation of rotation parameters into a single method - void computeBangBangParametersNoSmoothing(); //!< Method for computing the required parameters for the non-smoothed bang-bang profiler option - void computeBangCoastBangParametersNoSmoothing(); //!< Method for computing the required parameters for the non-smoothed bang-coast-bang profiler option - void computeSmoothedBangBangParameters(); //!< Method for computing the required parameters for the smoothed bang-bang profiler option - void computeSmoothedBangCoastBangParameters(); //!< Method for computing the required parameters for the smoothed bang-coast-bang profiler option + void computeRotationParameters(); //!< Intermediate method to group the calculation of rotation parameters into a + //!< single method + void computeBangBangParametersNoSmoothing(); //!< Method for computing the required parameters for the non-smoothed + //!< bang-bang profiler option + void computeBangCoastBangParametersNoSmoothing(); //!< Method for computing the required parameters for the + //!< non-smoothed bang-coast-bang profiler option + void computeSmoothedBangBangParameters(); //!< Method for computing the required parameters for the smoothed + //!< bang-bang profiler option + void computeSmoothedBangCoastBangParameters(); //!< Method for computing the required parameters for the smoothed + //!< bang-coast-bang profiler option /* Methods for computing the current rotational states */ - void computeCurrentState(double time); //!< Intermediate method used to group the calculation of the current rotational states into a single method - bool isInFirstBangSegment(double time) const; //!< Method for determining if the current time is within the first bang segment - bool isInSecondBangSegment(double time) const; //!< Method for determining if the current time is within the second bang segment - bool isInFirstSmoothedSegment(double time) const; //!< Method for determining if the current time is within the first smoothing segment for the smoothed profiler options - bool isInSecondSmoothedSegment(double time) const; //!< Method for determining if the current time is within the second smoothing segment for the smoothed profiler options - bool isInThirdSmoothedSegment(double time) const; //!< Method for determining if the current time is within the third smoothing segment for the smoothed profiler options - bool isInFourthSmoothedSegment(double time) const; //!< Method for determining if the current time is within the fourth smoothing segment for the smoothed bang-coast-bang option - bool isInCoastSegment(double time) const; //!< Method for determining if the current time is within the coast segment - void computeFirstBangSegment(double time); //!< Method for computing the first bang segment scalar rotational states - void computeSecondBangSegment(double time); //!< Method for computing the second bang segment scalar rotational states - void computeFirstSmoothedSegment(double time); //!< Method for computing the first smoothing segment scalar rotational states for the smoothed profiler options - void computeSecondSmoothedSegment(double time); //!< Method for computing the second smoothing segment scalar rotational states for the smoothed profiler options - void computeThirdSmoothedSegment(double time); //!< Method for computing the third smoothing segment scalar rotational states for the smoothed profiler options - void computeFourthSmoothedSegment(double time); //!< Method for computing the fourth smoothing segment scalar rotational states for the smoothed bang-coast-bang option - void computeCoastSegment(double time); //!< Method for computing the coast segment scalar rotational states - void computeRotationComplete(); //!< Method for computing the scalar rotational states when the rotation is complete - - void writeOutputMessages(uint64_t CurrentSimNanos); //!< Method for writing the module output messages and computing the output message data - Eigen::Vector3d computeSigma_PM(); //!< Method for computing the current spinning body MRP attitude relative to the mount frame: sigma_PM + void computeCurrentState(double time); //!< Intermediate method used to group the calculation of the current + //!< rotational states into a single method + bool isInFirstBangSegment( + double time) const; //!< Method for determining if the current time is within the first bang segment + bool isInSecondBangSegment( + double time) const; //!< Method for determining if the current time is within the second bang segment + bool isInFirstSmoothedSegment(double time) const; //!< Method for determining if the current time is within the + //!< first smoothing segment for the smoothed profiler options + bool isInSecondSmoothedSegment(double time) const; //!< Method for determining if the current time is within the + //!< second smoothing segment for the smoothed profiler options + bool isInThirdSmoothedSegment(double time) const; //!< Method for determining if the current time is within the + //!< third smoothing segment for the smoothed profiler options + bool isInFourthSmoothedSegment( + double time) const; //!< Method for determining if the current time is within the fourth smoothing segment for the + //!< smoothed bang-coast-bang option + bool isInCoastSegment( + double time) const; //!< Method for determining if the current time is within the coast segment + void computeFirstBangSegment(double time); //!< Method for computing the first bang segment scalar rotational states + void computeSecondBangSegment( + double time); //!< Method for computing the second bang segment scalar rotational states + void computeFirstSmoothedSegment(double time); //!< Method for computing the first smoothing segment scalar + //!< rotational states for the smoothed profiler options + void computeSecondSmoothedSegment(double time); //!< Method for computing the second smoothing segment scalar + //!< rotational states for the smoothed profiler options + void computeThirdSmoothedSegment(double time); //!< Method for computing the third smoothing segment scalar + //!< rotational states for the smoothed profiler options + void computeFourthSmoothedSegment(double time); //!< Method for computing the fourth smoothing segment scalar + //!< rotational states for the smoothed bang-coast-bang option + void computeCoastSegment(double time); //!< Method for computing the coast segment scalar rotational states + void computeRotationComplete(); //!< Method for computing the scalar rotational states when the rotation is complete + + void writeOutputMessages(uint64_t CurrentSimNanos); //!< Method for writing the module output messages and computing + //!< the output message data + Eigen::Vector3d computeSigma_PM(); //!< Method for computing the current spinning body MRP attitude relative to the + //!< mount frame: sigma_PM /* User-configurable variables */ - double coastOptionBangDuration; //!< [s] Time used for the coast option bang segment - double smoothingDuration; //!< [s] Time the acceleration is smoothed to the given maximum acceleration value - double thetaDDotMax; //!< [rad/s^2] Maximum angular acceleration of spinning body used in the bang segments - Eigen::Vector3d rotHat_M; //!< Spinning body rotation axis in M frame components + double coastOptionBangDuration; //!< [s] Time used for the coast option bang segment + double smoothingDuration; //!< [s] Time the acceleration is smoothed to the given maximum acceleration value + double thetaDDotMax; //!< [rad/s^2] Maximum angular acceleration of spinning body used in the bang segments + Eigen::Vector3d rotHat_M; //!< Spinning body rotation axis in M frame components /* Scalar rotational states */ - double thetaInit; //!< [rad] Initial spinning body angle from frame M to frame P about rotHat_M - double thetaRef; //!< [rad] Spinning body reference angle from frame M to frame P about rotHat_M - double theta; //!< [rad] Current angle - double thetaDot; //!< [rad/s] Current angle rate - double thetaDDot; //!< [rad/s^2] Current angular acceleration - double theta_tb1; //!< [rad] Angle at the end of the first bang segment - double thetaDot_tb1; //!< [rad/s] Angle rate at the end of the first bang segment - double theta_tb2; //!< [rad] Angle at the end of the second bang segment - double thetaDot_tb2; //!< [rad/s] Angle rate at the end of the second bang segment - double theta_ts1; //!< [rad] Angle at the end of the first smoothed segment - double thetaDot_ts1; //!< [rad/s] Angle rate at the end of the first smoothed segment - double theta_ts2; //!< [rad] Angle at the end of the second smoothed segment - double thetaDot_ts2; //!< [rad/s] Angle rate at the end of the second smoothed segment - double theta_ts3; //!< [rad] Angle at the end of the third smoothed segment - double thetaDot_ts3; //!< [rad/s] Angle rate at the end of the third smoothed segment - double theta_tc; //!< [rad] Angle at the end of the coast segment - double thetaDot_tc; //!< [rad/s] Angle rate at the end of the coast segment + double thetaInit; //!< [rad] Initial spinning body angle from frame M to frame P about rotHat_M + double thetaRef; //!< [rad] Spinning body reference angle from frame M to frame P about rotHat_M + double theta; //!< [rad] Current angle + double thetaDot; //!< [rad/s] Current angle rate + double thetaDDot; //!< [rad/s^2] Current angular acceleration + double theta_tb1; //!< [rad] Angle at the end of the first bang segment + double thetaDot_tb1; //!< [rad/s] Angle rate at the end of the first bang segment + double theta_tb2; //!< [rad] Angle at the end of the second bang segment + double thetaDot_tb2; //!< [rad/s] Angle rate at the end of the second bang segment + double theta_ts1; //!< [rad] Angle at the end of the first smoothed segment + double thetaDot_ts1; //!< [rad/s] Angle rate at the end of the first smoothed segment + double theta_ts2; //!< [rad] Angle at the end of the second smoothed segment + double thetaDot_ts2; //!< [rad/s] Angle rate at the end of the second smoothed segment + double theta_ts3; //!< [rad] Angle at the end of the third smoothed segment + double thetaDot_ts3; //!< [rad/s] Angle rate at the end of the third smoothed segment + double theta_tc; //!< [rad] Angle at the end of the coast segment + double thetaDot_tc; //!< [rad/s] Angle rate at the end of the coast segment /* Temporal parameters */ - double tInit; //!< [s] Simulation time at the beginning of the rotation - double t_b1; //!< [s] Simulation time at the end of the first bang segment - double t_b2; //!< [s] Simulation time at the end of the second bang segment - double t_s1; //!< [s] Simulation time at the end of the first smoothed segment - double t_s2; //!< [s] Simulation time at the end of the second smoothed segment - double t_s3; //!< [s] Simulation time at the end of the third smoothed segment - double t_c; //!< [s] Simulation time at the end of the coast segment - double t_f; //!< [s] Simulation time when the rotation is complete - - bool convergence; //!< Boolean variable is true when the rotation is complete - double a; //!< Parabolic constant for the first half of the bang-bang non-smoothed rotation - double b; //!< Parabolic constant for the second half of the bang-bang non-smoothed rotation + double tInit; //!< [s] Simulation time at the beginning of the rotation + double t_b1; //!< [s] Simulation time at the end of the first bang segment + double t_b2; //!< [s] Simulation time at the end of the second bang segment + double t_s1; //!< [s] Simulation time at the end of the first smoothed segment + double t_s2; //!< [s] Simulation time at the end of the second smoothed segment + double t_s3; //!< [s] Simulation time at the end of the third smoothed segment + double t_c; //!< [s] Simulation time at the end of the coast segment + double t_f; //!< [s] Simulation time when the rotation is complete + + bool convergence; //!< Boolean variable is true when the rotation is complete + double a; //!< Parabolic constant for the first half of the bang-bang non-smoothed rotation + double b; //!< Parabolic constant for the second half of the bang-bang non-smoothed rotation }; #endif 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/deviceInterface/tempMeasurement/tempMeasurement.cpp b/src/simulation/deviceInterface/tempMeasurement/tempMeasurement.cpp index 2f2e23c519..3636cabe3b 100644 --- a/src/simulation/deviceInterface/tempMeasurement/tempMeasurement.cpp +++ b/src/simulation/deviceInterface/tempMeasurement/tempMeasurement.cpp @@ -17,19 +17,18 @@ */ - #include "tempMeasurement.h" #include - /*! This is the constructor for the module class. It sets default variable values and initializes the various parts of the model. Don't allow random walk by default. */ -TempMeasurement::TempMeasurement() : faultState{TEMP_FAULT_NOMINAL}, - walkBounds{1E-15}, - spikeProbability{0.1}, - spikeAmount{2.0} +TempMeasurement::TempMeasurement() + : faultState{ TEMP_FAULT_NOMINAL } + , walkBounds{ 1E-15 } + , spikeProbability{ 0.1 } + , spikeAmount{ 2.0 } { this->noiseModel = GaussMarkov(1, this->RNGSeed); } @@ -39,7 +38,8 @@ TempMeasurement::~TempMeasurement() = default; /*! This method is used to reset the module and checks that required input messages are connected. */ -void TempMeasurement::Reset(uint64_t CurrentSimNanos) +void +TempMeasurement::Reset(uint64_t CurrentSimNanos) { if (!this->tempInMsg.isLinked()) { bskLogger.bskLog(BSK_ERROR, "TempMeasurement.tempInMsg was not linked."); @@ -52,48 +52,49 @@ void TempMeasurement::Reset(uint64_t CurrentSimNanos) this->spikeProbabilityGenerator.seed(this->RNGSeed); this->noiseModel.setRNGSeed(this->RNGSeed); - Eigen::VectorXd nMatrix(1,1); - nMatrix(0,0) = this->senNoiseStd; + Eigen::VectorXd nMatrix(1, 1); + nMatrix(0, 0) = this->senNoiseStd; this->noiseModel.setNoiseMatrix(nMatrix); - Eigen::VectorXd pMatrix(1,1); - pMatrix(0,0) = 1.; + Eigen::VectorXd pMatrix(1, 1); + pMatrix(0, 0) = 1.; this->noiseModel.setPropMatrix(pMatrix); - Eigen::VectorXd bounds(1,1); - bounds(0,0) = this->walkBounds; + Eigen::VectorXd bounds(1, 1); + bounds(0, 0) = this->walkBounds; this->noiseModel.setUpperBounds(bounds); } /*! This method adds noise, bias, and fault behaviors to the read-in temperature message. */ -void TempMeasurement::applySensorErrors() +void +TempMeasurement::applySensorErrors() { // apply noise and bias double sensorError; - if(this->senNoiseStd <= 0.0){ + if (this->senNoiseStd <= 0.0) { sensorError = this->senBias; } else { // get current error from gaussMarkov random number generator this->noiseModel.computeNextState(); Eigen::VectorXd currentErrorEigen = this->noiseModel.getCurrentState(); - double sensorNoise = currentErrorEigen(0,0); + double sensorNoise = currentErrorEigen(0, 0); sensorError = this->senBias + sensorNoise; } this->sensedTemperature = this->trueTemperature + sensorError; // apply fault conditions - if(this->faultState == TEMP_FAULT_STUCK_VALUE){ // stuck at specified value + if (this->faultState == TEMP_FAULT_STUCK_VALUE) { // stuck at specified value this->sensedTemperature = this->stuckValue; - } else if (this->faultState == TEMP_FAULT_STUCK_CURRENT){ // stuck at last value before flag turned on + } else if (this->faultState == TEMP_FAULT_STUCK_CURRENT) { // stuck at last value before flag turned on this->sensedTemperature = this->pastValue; - } else if (this->faultState == TEMP_FAULT_SPIKING){ // spiking periodically with specified probability + } else if (this->faultState == TEMP_FAULT_SPIKING) { // spiking periodically with specified probability // have to make a new distribution every time because SWIG can't parse putting this in the H file....? - std::uniform_real_distribution spikeProbabilityDistribution(0.0,1.0); + std::uniform_real_distribution spikeProbabilityDistribution(0.0, 1.0); double n = spikeProbabilityDistribution(this->spikeProbabilityGenerator); // draw from uniform distribution if (n <= this->spikeProbability) { // if drawn number within probability of spiking - this->sensedTemperature = this->sensedTemperature*this->spikeAmount; + this->sensedTemperature = this->sensedTemperature * this->spikeAmount; } } @@ -103,10 +104,11 @@ void TempMeasurement::applySensorErrors() /*! This is the main method that gets called every time the module is updated. */ -void TempMeasurement::UpdateState(uint64_t CurrentSimNanos) +void +TempMeasurement::UpdateState(uint64_t CurrentSimNanos) { TemperatureMsgPayload tempInMsgBuffer; //!< local copy of message buffer - TemperatureMsgPayload tempOutMsgBuffer; //!< local copy of message buffer + TemperatureMsgPayload tempOutMsgBuffer; //!< local copy of message buffer // always zero the output message buffers before assigning values tempOutMsgBuffer = this->tempOutMsg.zeroMsgPayload; diff --git a/src/simulation/deviceInterface/tempMeasurement/tempMeasurement.h b/src/simulation/deviceInterface/tempMeasurement/tempMeasurement.h index d775cf6ebc..90c7aa7125 100644 --- a/src/simulation/deviceInterface/tempMeasurement/tempMeasurement.h +++ b/src/simulation/deviceInterface/tempMeasurement/tempMeasurement.h @@ -17,7 +17,6 @@ */ - #ifndef TEMPMEASUREMENT_H #define TEMPMEASUREMENT_H @@ -30,7 +29,8 @@ #include "architecture/messaging/messaging.h" #include "architecture/utilities/gauss_markov.h" -typedef enum { +typedef enum +{ TEMP_FAULT_NOMINAL = 0, TEMP_FAULT_STUCK_CURRENT = 1, TEMP_FAULT_STUCK_VALUE = 2, @@ -41,38 +41,38 @@ typedef enum { /*! @brief Models a sensor to add noise, bias, and faults to temperature measurements. */ -class TempMeasurement: public SysModel { -public: +class TempMeasurement : public SysModel +{ + public: TempMeasurement(); ~TempMeasurement(); void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); -private: + private: void applySensorErrors(); -public: - ReadFunctor tempInMsg; //!< True temperature measurement - Message tempOutMsg; //!< Sensed temperature measurement - BSKLogger bskLogger; //!< -- BSK Logging - TempFaultState_t faultState; //!< [-] Fault status variable + public: + ReadFunctor tempInMsg; //!< True temperature measurement + Message tempOutMsg; //!< Sensed temperature measurement + BSKLogger bskLogger; //!< -- BSK Logging + TempFaultState_t faultState; //!< [-] Fault status variable - double senBias{}; //!< [-] Sensor bias value - double senNoiseStd{}; //!< [-] Sensor noise value - double walkBounds; //!< [-] Gauss Markov walk bounds - double stuckValue{}; //!< [C] Value for temp sensor to get stuck at - double spikeProbability; //!< [-] Probability of spiking at each time step (between 0 and 1) - double spikeAmount; //!< [-] Spike multiplier + double senBias{}; //!< [-] Sensor bias value + double senNoiseStd{}; //!< [-] Sensor noise value + double walkBounds; //!< [-] Gauss Markov walk bounds + double stuckValue{}; //!< [C] Value for temp sensor to get stuck at + double spikeProbability; //!< [-] Probability of spiking at each time step (between 0 and 1) + double spikeAmount; //!< [-] Spike multiplier -private: - double trueTemperature{}; //!< [C] Truth value for the temperature measurement - double sensedTemperature{}; //!< [C] Temperature measurement as corrupted by noise and faults - double pastValue{}; //!< [-] Measurement from last update (used only for faults) + private: + double trueTemperature{}; //!< [C] Truth value for the temperature measurement + double sensedTemperature{}; //!< [C] Temperature measurement as corrupted by noise and faults + double pastValue{}; //!< [-] Measurement from last update (used only for faults) std::minstd_rand spikeProbabilityGenerator; //! [-] Number generator for calculating probability of spike if faulty GaussMarkov noiseModel; //! [-] Gauss Markov noise generation model }; - #endif 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/_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/boreAngCalc/boreAngCalc.cpp b/src/simulation/dynamics/DynOutput/boreAngCalc/boreAngCalc.cpp old mode 100755 new mode 100644 index 3b614181af..2a5a199d13 --- a/src/simulation/dynamics/DynOutput/boreAngCalc/boreAngCalc.cpp +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/boreAngCalc.cpp @@ -37,11 +37,11 @@ BoreAngCalc::BoreAngCalc() //! The destructor. BoreAngCalc::~BoreAngCalc() = default; - /*! This method is used to reset the module. */ -void BoreAngCalc::Reset(uint64_t CurrentSimNanos) +void +BoreAngCalc::Reset(uint64_t CurrentSimNanos) { // check if required input messages have not been included if (!this->scStateInMsg.isLinked()) { @@ -50,21 +50,20 @@ void BoreAngCalc::Reset(uint64_t CurrentSimNanos) if (this->celBodyInMsg.isLinked()) { this->useCelestialHeading = true; - } - else if (this->inertialHeadingVec_N.norm() > 1e-8) { + } else if (this->inertialHeadingVec_N.norm() > 1e-8) { this->useInertialHeading = true; + } else { + bskLogger.bskLog( + BSK_ERROR, "Either boreAngCalc.celBodyInMsg was not linked or boreAngCalc.inertialHeadingVec_N was not set."); } - else { - bskLogger.bskLog(BSK_ERROR, "Either boreAngCalc.celBodyInMsg was not linked or boreAngCalc.inertialHeadingVec_N was not set."); - } - } /*! This method writes the output data out into the messaging system. @param CurrentClock The current time in the system for output stamping */ -void BoreAngCalc::WriteOutputMessages(uint64_t CurrentClock) +void +BoreAngCalc::WriteOutputMessages(uint64_t CurrentClock) { this->angOutMsg.write(&this->boresightAng, this->moduleID, CurrentClock); } @@ -73,7 +72,8 @@ void BoreAngCalc::WriteOutputMessages(uint64_t CurrentClock) appropriate parameters */ -void BoreAngCalc::ReadInputs() +void +BoreAngCalc::ReadInputs() { //! - Read the input message into the correct pointer this->localState = this->scStateInMsg(); @@ -91,22 +91,23 @@ void BoreAngCalc::ReadInputs() is used later to compute how far off that vector is in an angular sense. */ -void BoreAngCalc::computeCelestialAxisPoint() +void +BoreAngCalc::computeCelestialAxisPoint() { // Convert planet and body data to Eigen variables - Eigen::Vector3d r_BN_N = cArray2EigenVector3d(this->localState.r_BN_N); // spacecraft's inertial position - Eigen::Vector3d v_BN_N = cArray2EigenVector3d(this->localState.v_BN_N); // spacecraft''s inertial velocity - Eigen::Vector3d r_PN_N = cArray2EigenVector3d(this->localPlanet.PositionVector); // planet's inertial position - Eigen::Vector3d v_PN_N = cArray2EigenVector3d(this->localPlanet.VelocityVector); // planet's inertial velocity + Eigen::Vector3d r_BN_N = cArray2EigenVector3d(this->localState.r_BN_N); // spacecraft's inertial position + Eigen::Vector3d v_BN_N = cArray2EigenVector3d(this->localState.v_BN_N); // spacecraft''s inertial velocity + Eigen::Vector3d r_PN_N = cArray2EigenVector3d(this->localPlanet.PositionVector); // planet's inertial position + Eigen::Vector3d v_PN_N = cArray2EigenVector3d(this->localPlanet.VelocityVector); // planet's inertial velocity // Compute the relative vectors - Eigen::Vector3d r_PB_N = r_PN_N - r_BN_N; // relative position vector - Eigen::Vector3d rHat_PB_N = r_PB_N.normalized(); // unit vector for relative position - Eigen::Vector3d v_PB_N = v_PN_N - v_BN_N; // relative velocity vector - Eigen::Vector3d secHat_N = r_PB_N.cross(v_PB_N).normalized(); // perpendicular unit vector + Eigen::Vector3d r_PB_N = r_PN_N - r_BN_N; // relative position vector + Eigen::Vector3d rHat_PB_N = r_PB_N.normalized(); // unit vector for relative position + Eigen::Vector3d v_PB_N = v_PN_N - v_BN_N; // relative velocity vector + Eigen::Vector3d secHat_N = r_PB_N.cross(v_PB_N).normalized(); // perpendicular unit vector // Calculate the inertial to point DCM - Eigen::Matrix3d dcm_PoN; // dcm, inertial to point frame + Eigen::Matrix3d dcm_PoN; // dcm, inertial to point frame dcm_PoN.row(0) = rHat_PB_N.transpose(); dcm_PoN.row(2) = rHat_PB_N.cross(secHat_N).normalized(); dcm_PoN.row(1) = dcm_PoN.row(2).cross(dcm_PoN.row(0)); @@ -114,7 +115,7 @@ void BoreAngCalc::computeCelestialAxisPoint() // Compute the point to body frame DCM and convert the boresight vector to the Po frame Eigen::MRPd sigma_BN = cArray2EigenMRPd(this->localState.sigma_BN); // mrp, inertial to body frame Eigen::Matrix3d dcm_BN = sigma_BN.toRotationMatrix().transpose(); // dcm, inertial to body frame - Eigen::Matrix3d dcm_BPo = dcm_BN * dcm_PoN.transpose(); // dcm, point to body frame + Eigen::Matrix3d dcm_BPo = dcm_BN * dcm_PoN.transpose(); // dcm, point to body frame this->boreVec_Po = dcm_BPo.transpose() * this->boreVec_B; } @@ -124,7 +125,8 @@ void BoreAngCalc::computeCelestialAxisPoint() desired pointing vector projected into the y/z plane. */ -void BoreAngCalc::computeCelestialOutputData() +void +BoreAngCalc::computeCelestialOutputData() { // Define epsilon that will avoid atan2 giving a NaN. double eps = 1e-10; @@ -134,8 +136,7 @@ void BoreAngCalc::computeCelestialOutputData() this->boresightAng.missAngle = fabs(safeAcos(dotValue)); if (fabs(this->boreVec_Po(1)) < eps) { this->boresightAng.azimuth = 0.0; - } - else { + } else { this->boresightAng.azimuth = atan2(this->boreVec_Po(2), this->boreVec_Po(1)); } } @@ -144,14 +145,16 @@ void BoreAngCalc::computeCelestialOutputData() computed using the body heading and the provided inertial heading */ -void BoreAngCalc::computeInertialOutputData() +void +BoreAngCalc::computeInertialOutputData() { // Compute the DCM from inertial do body frame Eigen::MRPd sigma_BN = cArray2EigenMRPd(this->localState.sigma_BN); // mrp, inertial to body frame Eigen::Matrix3d dcm_BN = sigma_BN.toRotationMatrix().transpose(); // dcm, inertial to body frame // Calculate the inertial heading vector - Eigen::Vector3d inertialHeadingVec_B = dcm_BN * this->inertialHeadingVec_N; // inertial heading written in the body frame + Eigen::Vector3d inertialHeadingVec_B = + dcm_BN * this->inertialHeadingVec_N; // inertial heading written in the body frame double dotValue = this->boreVec_B.dot(inertialHeadingVec_B); this->boresightAng.missAngle = fabs(safeAcos(dotValue)); @@ -165,19 +168,17 @@ void BoreAngCalc::computeInertialOutputData() @param CurrentSimNanos The current simulation time for system */ -void BoreAngCalc::UpdateState(uint64_t CurrentSimNanos) +void +BoreAngCalc::UpdateState(uint64_t CurrentSimNanos) { //! - Read the input message and convert it over appropriately depending on switch ReadInputs(); - if(this->inputsGood) - { - if (this->useCelestialHeading) - { + if (this->inputsGood) { + if (this->useCelestialHeading) { this->computeCelestialAxisPoint(); this->computeCelestialOutputData(); - } - else { + } else { this->computeInertialOutputData(); } } diff --git a/src/simulation/dynamics/DynOutput/boreAngCalc/boreAngCalc.h b/src/simulation/dynamics/DynOutput/boreAngCalc/boreAngCalc.h old mode 100755 new mode 100644 index d3e13f29ed..29e3c5026e --- a/src/simulation/dynamics/DynOutput/boreAngCalc/boreAngCalc.h +++ b/src/simulation/dynamics/DynOutput/boreAngCalc/boreAngCalc.h @@ -32,14 +32,14 @@ #include "architecture/utilities/avsEigenMRP.h" #include "architecture/utilities/avsEigenSupport.h" - /*! @brief A class to perform a range of boresight related calculations. */ -class BoreAngCalc: public SysModel { -public: +class BoreAngCalc : public SysModel +{ + public: BoreAngCalc(); ~BoreAngCalc(); - + void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); void computeCelestialAxisPoint(); @@ -47,25 +47,24 @@ 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 - Eigen::Vector3d boreVec_B; //!< (-) boresight vector in structure - Eigen::Vector3d boreVec_Po; //!< (-) pointing vector in the target relative point frame - Eigen::Vector3d inertialHeadingVec_N; //!< (-) inertial boresight vector + ReadFunctor scStateInMsg; //!< (-) spacecraft state input message + ReadFunctor celBodyInMsg; //!< (-) celestial body state msg at which we pointing at + Message angOutMsg; //!< (-) bore sight output message -private: + Eigen::Vector3d boreVec_B; //!< (-) boresight vector in structure + Eigen::Vector3d boreVec_Po; //!< (-) pointing vector in the target relative point frame + Eigen::Vector3d inertialHeadingVec_N; //!< (-) inertial boresight vector + + private: SpicePlanetStateMsgPayload localPlanet; //!< (-) planet that we are pointing at SCStatesMsgPayload localState; //!< (-) observed state of the spacecraft - BoreAngleMsgPayload boresightAng = {}; //!< (-) Boresight angles relative to target - bool inputsGood = false; //!< (-) Flag indicating that inputs were read correctly - bool useCelestialHeading = false; //!< (-) Flag indicating that the module should use the celestial body heading - bool useInertialHeading = false; //!< (-) Flag indicating that the module should use the inertial heading - BSKLogger bskLogger; //!< -- BSK Logging + BoreAngleMsgPayload boresightAng = {}; //!< (-) Boresight angles relative to target + bool inputsGood = false; //!< (-) Flag indicating that inputs were read correctly + bool useCelestialHeading = false; //!< (-) Flag indicating that the module should use the celestial body heading + bool useInertialHeading = false; //!< (-) Flag indicating that the module should use the inertial heading + BSKLogger bskLogger; //!< -- BSK Logging }; - #endif 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/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/_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/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/DynOutput/orbElemConvert/orbElemConvert.cpp b/src/simulation/dynamics/DynOutput/orbElemConvert/orbElemConvert.cpp old mode 100755 new mode 100644 index 5b9d69eca7..403855c1b8 --- a/src/simulation/dynamics/DynOutput/orbElemConvert/orbElemConvert.cpp +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/orbElemConvert.cpp @@ -32,14 +32,14 @@ OrbElemConvert::~OrbElemConvert() return; } - /*! This method is used to reset the module. */ -void OrbElemConvert::Reset(uint64_t CurrentSimNanos) +void +OrbElemConvert::Reset(uint64_t CurrentSimNanos) { - int numInputs = 0; //!< number of input messages connected - int numOutputs = 0; //!< number of output messages connected + int numInputs = 0; //!< number of input messages connected + int numOutputs = 0; //!< number of output messages connected numInputs += this->scStateInMsg.isLinked(); numInputs += this->spiceStateInMsg.isLinked(); @@ -66,7 +66,8 @@ void OrbElemConvert::Reset(uint64_t CurrentSimNanos) @param CurrentClock The current time in the system for output stamping */ -void OrbElemConvert::WriteOutputMessages(uint64_t CurrentClock) +void +OrbElemConvert::WriteOutputMessages(uint64_t CurrentClock) { if (this->elemOutMsg.isLinked() && this->inputsGood) { auto payload = ClassicElementsMsgPayload(); @@ -101,7 +102,8 @@ void OrbElemConvert::WriteOutputMessages(uint64_t CurrentClock) /*! The name kind of says it all right? Converts CurrentElem to pos/vel. */ -void OrbElemConvert::Elements2Cartesian() +void +OrbElemConvert::Elements2Cartesian() { elem2rv(mu, &CurrentElem, r_N, v_N); } @@ -109,7 +111,8 @@ void OrbElemConvert::Elements2Cartesian() /*! The name kind of says it all right? Converts pos/vel to CurrentElem. */ -void OrbElemConvert::Cartesian2Elements() +void +OrbElemConvert::Cartesian2Elements() { rv2elem(mu, r_N, v_N, &CurrentElem); } @@ -118,7 +121,8 @@ void OrbElemConvert::Cartesian2Elements() appropriate parameters based on which direction the module is running */ -void OrbElemConvert::ReadInputs() +void +OrbElemConvert::ReadInputs() { this->inputsGood = false; if (this->elemInMsg.isLinked()) { @@ -159,16 +163,14 @@ void OrbElemConvert::ReadInputs() @param CurrentSimNanos The current simulation time for system */ -void OrbElemConvert::UpdateState(uint64_t CurrentSimNanos) +void +OrbElemConvert::UpdateState(uint64_t CurrentSimNanos) { //! - Read the input message and convert it over appropriately depending on switch ReadInputs(); - if(this->elemInMsg.isLinked() && inputsGood) - { + if (this->elemInMsg.isLinked() && inputsGood) { Elements2Cartesian(); - } - else if(inputsGood) - { + } else if (inputsGood) { Cartesian2Elements(); } diff --git a/src/simulation/dynamics/DynOutput/orbElemConvert/orbElemConvert.h b/src/simulation/dynamics/DynOutput/orbElemConvert/orbElemConvert.h old mode 100755 new mode 100644 index efe38eed92..db442a908f --- a/src/simulation/dynamics/DynOutput/orbElemConvert/orbElemConvert.h +++ b/src/simulation/dynamics/DynOutput/orbElemConvert/orbElemConvert.h @@ -31,11 +31,10 @@ #include "architecture/utilities/orbitalMotion.h" #include "architecture/utilities/bskLogging.h" - - /*! @brief orbit element converter module class */ -class OrbElemConvert: public SysModel { -public: +class OrbElemConvert : public SysModel +{ + public: OrbElemConvert(); ~OrbElemConvert(); @@ -46,25 +45,23 @@ class OrbElemConvert: public SysModel { void Cartesian2Elements(); void ReadInputs(); -public: - double r_N[3]; //!< m Current position vector (inertial) - double v_N[3]; //!< m/s Current velocity vector (inertial) - double mu; //!< -- Current grav param (inertial) - ClassicElements CurrentElem; //!< -- Current orbital elements - SCStatesMsgPayload statesIn; //!< -- spacecraft state message - SpicePlanetStateMsgPayload planetIn; //!< -- planet state message - ReadFunctor scStateInMsg; //!< -- sc state input message - ReadFunctor spiceStateInMsg; //!< -- spice state input message - ReadFunctor elemInMsg; //!< -- orbit element input message - Message scStateOutMsg; //!< -- sc state output message - Message spiceStateOutMsg; //!< -- spice state input message - Message elemOutMsg; //!< -- orbit element output message - -private: - bool inputsGood; //!< -- flag indicating that inputs are good - BSKLogger bskLogger; //!< -- BSK Logging - + public: + double r_N[3]; //!< m Current position vector (inertial) + double v_N[3]; //!< m/s Current velocity vector (inertial) + double mu; //!< -- Current grav param (inertial) + ClassicElements CurrentElem; //!< -- Current orbital elements + SCStatesMsgPayload statesIn; //!< -- spacecraft state message + SpicePlanetStateMsgPayload planetIn; //!< -- planet state message + ReadFunctor scStateInMsg; //!< -- sc state input message + ReadFunctor spiceStateInMsg; //!< -- spice state input message + ReadFunctor elemInMsg; //!< -- orbit element input message + Message scStateOutMsg; //!< -- sc state output message + Message spiceStateOutMsg; //!< -- spice state input message + Message elemOutMsg; //!< -- orbit element output message + + private: + bool inputsGood; //!< -- flag indicating that inputs are good + BSKLogger bskLogger; //!< -- BSK Logging }; - #endif diff --git a/src/simulation/dynamics/ExtPulsedTorque/ExtPulsedTorque.cpp b/src/simulation/dynamics/ExtPulsedTorque/ExtPulsedTorque.cpp old mode 100755 new mode 100644 index 93291e7d7a..afbfda459c --- a/src/simulation/dynamics/ExtPulsedTorque/ExtPulsedTorque.cpp +++ b/src/simulation/dynamics/ExtPulsedTorque/ExtPulsedTorque.cpp @@ -35,19 +35,19 @@ ExtPulsedTorque::~ExtPulsedTorque() return; } - /*! link the states */ -void ExtPulsedTorque::linkInStates(DynParamManager& statesIn) +void +ExtPulsedTorque::linkInStates(DynParamManager& statesIn) { return; } - /*! This module does not write any output messages. @param currentClock The current time used for time-stamping the message */ -void ExtPulsedTorque::writeOutputMessages(uint64_t currentClock) +void +ExtPulsedTorque::writeOutputMessages(uint64_t currentClock) { return; } @@ -56,7 +56,8 @@ void ExtPulsedTorque::writeOutputMessages(uint64_t currentClock) associated buffer structure. */ -void ExtPulsedTorque::readInputMessages() +void +ExtPulsedTorque::readInputMessages() { return; } @@ -66,30 +67,31 @@ void ExtPulsedTorque::readInputMessages() matrix represnetations in the body (B) and inerial (N) frame components are treated as 2 separate vectors. Only set both if you mean to, as both vectors will be included. */ -void ExtPulsedTorque::computeForceTorque(double integTime, double timeStep) +void +ExtPulsedTorque::computeForceTorque(double integTime, double timeStep) { /* zero the output vector */ this->torqueExternalPntB_B.fill(0.0); /* check if the pulse sequence must restart */ - if (this->c >= this->countOnPulse*2 + this->countOff) { + if (this->c >= this->countOnPulse * 2 + this->countOff) { this->c = 0; } if (this->c < this->countOnPulse) { this->torqueExternalPntB_B += this->pulsedTorqueExternalPntB_B; - } else if (this->c < this->countOnPulse*2) { + } else if (this->c < this->countOnPulse * 2) { this->torqueExternalPntB_B -= this->pulsedTorqueExternalPntB_B; } this->c++; - return; } /*! Module update method */ -void ExtPulsedTorque::UpdateState(uint64_t CurrentSimNanos) +void +ExtPulsedTorque::UpdateState(uint64_t CurrentSimNanos) { return; } diff --git a/src/simulation/dynamics/ExtPulsedTorque/ExtPulsedTorque.h b/src/simulation/dynamics/ExtPulsedTorque/ExtPulsedTorque.h old mode 100755 new mode 100644 index 56729d9106..411afd4d7d --- a/src/simulation/dynamics/ExtPulsedTorque/ExtPulsedTorque.h +++ b/src/simulation/dynamics/ExtPulsedTorque/ExtPulsedTorque.h @@ -24,10 +24,12 @@ #include "simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h" #include "architecture/utilities/bskLogging.h" - /*! @brief external pulsed torque module class */ -class ExtPulsedTorque: public SysModel, public DynamicEffector{ -public: +class ExtPulsedTorque + : public SysModel + , public DynamicEffector +{ + public: ExtPulsedTorque(); ~ExtPulsedTorque(); @@ -37,16 +39,14 @@ class ExtPulsedTorque: public SysModel, public DynamicEffector{ void readInputMessages(); void computeForceTorque(double integTime, double timeStep); -private: - int c; //!< numer of time steps for pulse + private: + int c; //!< numer of time steps for pulse -public: + public: Eigen::Vector3d pulsedTorqueExternalPntB_B; //!< pulsed torque vector about point B, in B frame components int countOnPulse; //!< number of integration time steps to simulate a pulse int countOff; //!< number of integration time steps to have no pulses - BSKLogger bskLogger; //!< -- BSK Logging - + BSKLogger bskLogger; //!< -- BSK Logging }; - #endif 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..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,14 +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/_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/_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/FuelTank/fuelTank.cpp b/src/simulation/dynamics/FuelTank/fuelTank.cpp index bab02b0e73..d16f5752bb 100644 --- a/src/simulation/dynamics/FuelTank/fuelTank.cpp +++ b/src/simulation/dynamics/FuelTank/fuelTank.cpp @@ -21,8 +21,8 @@ #include - -FuelTank::FuelTank() { +FuelTank::FuelTank() +{ this->effProps.mEff = 0.0; this->effProps.IEffPntB_B.setZero(); this->effProps.rEff_CB_B.setZero(); @@ -39,12 +39,15 @@ FuelTank::FuelTank() { uint64_t FuelTank::effectorID = 1; -FuelTank::~FuelTank() { +FuelTank::~FuelTank() +{ FuelTank::effectorID = 1; } /*! optionally set the name of the mass state to be used by the state manager */ -void FuelTank::setNameOfMassState(const std::string nameOfMassState) { +void +FuelTank::setNameOfMassState(const std::string nameOfMassState) +{ this->nameOfMassState = nameOfMassState; } @@ -52,36 +55,48 @@ void FuelTank::setNameOfMassState(const std::string nameOfMassState) { @param model fuel tank model type */ -void FuelTank::setTankModel(FuelTankModel *model) { +void +FuelTank::setTankModel(FuelTankModel* model) +{ this->fuelTankModel = model; } /*! Attach a fuel slosh particle to the tank */ -void FuelTank::pushFuelSloshParticle(FuelSlosh *particle) { +void +FuelTank::pushFuelSloshParticle(FuelSlosh* particle) +{ // Add a fuel slosh particle to the vector of fuel slosh particles this->fuelSloshParticles.push_back(particle); } /*! Attach a thruster dynamic effector to the tank */ -void FuelTank::addThrusterSet(ThrusterDynamicEffector *dynEff) { +void +FuelTank::addThrusterSet(ThrusterDynamicEffector* dynEff) +{ thrDynEffectors.push_back(dynEff); dynEff->fuelMass = this->fuelTankModel->propMassInit; } /*! Attach a thruster state effector to the tank */ -void FuelTank::addThrusterSet(ThrusterStateEffector *stateEff) { +void +FuelTank::addThrusterSet(ThrusterStateEffector* stateEff) +{ thrStateEffectors.push_back(stateEff); } /*! Link states that the module accesses */ -void FuelTank::linkInStates(DynParamManager &statesIn) { +void +FuelTank::linkInStates(DynParamManager& statesIn) +{ // Grab access to the hubs omega_BN_N this->omegaState = statesIn.getStateObject(this->stateNameOfOmega); } /*! Register states. The fuel tank has one state associated with it: mass, and it also has the responsibility to call register states for the fuel slosh particles */ -void FuelTank::registerStates(DynParamManager &statesIn) { +void +FuelTank::registerStates(DynParamManager& statesIn) +{ // Register the mass state associated with the tank Eigen::MatrixXd massMatrix(1, 1); this->massState = statesIn.registerState(1, 1, this->nameOfMassState); @@ -90,15 +105,17 @@ void FuelTank::registerStates(DynParamManager &statesIn) { } /*! Fuel tank add its contributions the mass of the vehicle. */ -void FuelTank::updateEffectorMassProps(double integTime) { +void +FuelTank::updateEffectorMassProps(double integTime) +{ // Add contributions of the mass of the tank double massLocal = this->massState->getState()(0, 0); this->fuelTankModel->computeTankProps(massLocal); this->r_TcB_B = r_TB_B + this->dcm_TB.transpose() * this->fuelTankModel->r_TcT_T; this->effProps.mEff = massLocal; this->ITankPntT_B = this->dcm_TB.transpose() * fuelTankModel->ITankPntT_T * this->dcm_TB; - this->effProps.IEffPntB_B = ITankPntT_B + massLocal * (r_TcB_B.dot(r_TcB_B) * Eigen::Matrix3d::Identity() - - r_TcB_B * r_TcB_B.transpose()); + this->effProps.IEffPntB_B = + ITankPntT_B + massLocal * (r_TcB_B.dot(r_TcB_B) * Eigen::Matrix3d::Identity() - r_TcB_B * r_TcB_B.transpose()); this->effProps.rEff_CB_B = this->r_TcB_B; // This does not incorporate mEffDot into cPrime for high fidelity mass depletion @@ -106,20 +123,19 @@ void FuelTank::updateEffectorMassProps(double integTime) { // Mass depletion (call thrusters attached to this tank to get their mDot, and contributions) this->fuelConsumption = 0.0; - for (auto &dynEffector: this->thrDynEffectors) { + for (auto& dynEffector : this->thrDynEffectors) { dynEffector->computeStateContribution(integTime); this->fuelConsumption += dynEffector->stateDerivContribution(0); } - for (auto &stateEffector: this->thrStateEffectors) { + for (auto& stateEffector : this->thrStateEffectors) { stateEffector->updateEffectorMassProps(integTime); this->fuelConsumption += stateEffector->stateDerivContribution(0); } // Mass depletion (finding total mass in tank) double totalMass = massLocal; - for (auto fuelSloshInt = this->fuelSloshParticles.begin(); - fuelSloshInt < this->fuelSloshParticles.end(); + for (auto fuelSloshInt = this->fuelSloshParticles.begin(); fuelSloshInt < this->fuelSloshParticles.end(); fuelSloshInt++) { // Retrieve current mass value of fuelSlosh particle (*fuelSloshInt)->retrieveMassValue(integTime); @@ -127,8 +143,7 @@ void FuelTank::updateEffectorMassProps(double integTime) { totalMass += (*fuelSloshInt)->fuelMass; } // Set mass depletion rate of fuelSloshParticles - for (auto fuelSloshInt = this->fuelSloshParticles.begin(); - fuelSloshInt < this->fuelSloshParticles.end(); + for (auto fuelSloshInt = this->fuelSloshParticles.begin(); fuelSloshInt < this->fuelSloshParticles.end(); fuelSloshInt++) { // Find fuelSlosh particle mass to fuel tank mass ratio (*fuelSloshInt)->massToTotalTankMassRatio = (*fuelSloshInt)->fuelMass / totalMass; @@ -137,7 +152,7 @@ void FuelTank::updateEffectorMassProps(double integTime) { } // Set total fuel mass parameter for thruster dynamic effectors experiencing blow down effects - for (auto &dynEffector: this->thrDynEffectors) { + for (auto& dynEffector : this->thrDynEffectors) { dynEffector->fuelMass = totalMass; } @@ -147,11 +162,13 @@ void FuelTank::updateEffectorMassProps(double integTime) { } /*! Fuel tank adds its contributions to the matrices for the back-sub method. */ -void FuelTank::updateContributions(double integTime, - BackSubMatrices &backSubContr, - Eigen::Vector3d sigma_BN, - Eigen::Vector3d omega_BN_B, - Eigen::Vector3d g_N) { +void +FuelTank::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) +{ Eigen::Vector3d r_TB_BLocal; Eigen::Vector3d rPrime_TB_BLocal; Eigen::Vector3d rPPrime_TB_BLocal; @@ -169,29 +186,33 @@ void FuelTank::updateContributions(double integTime, rPPrime_TB_BLocal = this->fuelTankModel->rPPrime_TcT_T; omega_BN_BLocal = this->omegaState->getState(); if (!this->updateOnly) { - backSubContr.vecRot = -this->massState->getState()(0, 0) * r_TB_BLocal.cross(rPPrime_TB_BLocal) - - this->massState->getState()(0, 0) * omega_BN_BLocal.cross(r_TB_BLocal.cross(rPrime_TB_BLocal)) - - this->massState->getStateDeriv()(0, 0) * r_TB_BLocal.cross(rPrime_TB_BLocal); + backSubContr.vecRot = + -this->massState->getState()(0, 0) * r_TB_BLocal.cross(rPPrime_TB_BLocal) - + this->massState->getState()(0, 0) * omega_BN_BLocal.cross(r_TB_BLocal.cross(rPrime_TB_BLocal)) - + this->massState->getStateDeriv()(0, 0) * r_TB_BLocal.cross(rPrime_TB_BLocal); backSubContr.vecRot -= this->fuelTankModel->IPrimeTankPntT_T * omega_BN_BLocal; } - } /*! Fuel tank computes its derivative */ -void FuelTank::computeDerivatives(double integTime, - Eigen::Vector3d rDDot_BN_N, - Eigen::Vector3d omegaDot_BN_B, - Eigen::Vector3d sigma_BN) { +void +FuelTank::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) +{ Eigen::MatrixXd conv(1, 1); conv(0, 0) = -this->tankFuelConsumption; this->massState->setDerivative(conv); } /*! Fuel tank contributes to the energy and momentum calculations */ -void FuelTank::updateEnergyMomContributions(double integTime, - Eigen::Vector3d &rotAngMomPntCContr_B, - double &rotEnergyContr, - Eigen::Vector3d omega_BN_B) { +void +FuelTank::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) +{ // Get variables needed for energy momentum calcs Eigen::Vector3d omegaLocal_BN_B; omegaLocal_BN_B = this->omegaState->getState(); @@ -203,15 +224,17 @@ void FuelTank::updateEnergyMomContributions(double integTime, rotAngMomPntCContr_B += this->ITankPntT_B * omegaLocal_BN_B + massLocal * this->r_TcB_B.cross(rDot_TcB_B); // Find rotational energy contribution from the hub - rotEnergyContr += 1.0 / 2.0 * omegaLocal_BN_B.dot(this->ITankPntT_B * omegaLocal_BN_B) + 1.0 / 2.0 * massLocal * - rDot_TcB_B.dot(rDot_TcB_B); + rotEnergyContr += 1.0 / 2.0 * omegaLocal_BN_B.dot(this->ITankPntT_B * omegaLocal_BN_B) + + 1.0 / 2.0 * massLocal * rDot_TcB_B.dot(rDot_TcB_B); } /*! Compute fuel tank mass properties and outputs them as a message. @param currentClock The current simulation time (used for time stamping) */ -void FuelTank::writeOutputMessages(uint64_t currentClock) { +void +FuelTank::writeOutputMessages(uint64_t currentClock) +{ this->fuelTankMassPropMsg = this->fuelTankOutMsg.zeroMsgPayload; this->fuelTankMassPropMsg.fuelMass = this->effProps.mEff; this->fuelTankMassPropMsg.fuelMassDot = this->effProps.mEffDot; @@ -223,6 +246,8 @@ void FuelTank::writeOutputMessages(uint64_t currentClock) { @param currentSimNanos The current simulation time in nanoseconds */ -void FuelTank::UpdateState(uint64_t currentSimNanos) { +void +FuelTank::UpdateState(uint64_t currentSimNanos) +{ this->writeOutputMessages(currentSimNanos); } diff --git a/src/simulation/dynamics/FuelTank/fuelTank.h b/src/simulation/dynamics/FuelTank/fuelTank.h old mode 100755 new mode 100644 index 22cf883ca5..d4d28593bc --- a/src/simulation/dynamics/FuelTank/fuelTank.h +++ b/src/simulation/dynamics/FuelTank/fuelTank.h @@ -32,96 +32,108 @@ #include "architecture/utilities/avsEigenMRP.h" #include "architecture/utilities/avsEigenSupport.h" - /*! Tank model class */ -class FuelTankModel { -public: - double propMassInit{}; //!< [kg] Initial propellant mass in tank - double maxFuelMass = 1.0; //!< [kg] maximum tank mass - Eigen::Vector3d r_TcT_TInit; //!< [m] Initial position vector from B to tank point in B frame comp. - Eigen::Matrix3d ITankPntT_T; //!< [kg m^2] Inertia of tank about pnt T in B frame comp. - Eigen::Matrix3d IPrimeTankPntT_T; //!< [kg m^2/s] Derivative of inertia of tank about pnt T in B frame comp. - Eigen::Vector3d r_TcT_T; //!< [m] position vector from B to tank point in B frame comp. - Eigen::Vector3d rPrime_TcT_T; //!< [m/s] Derivative of position vector from B to tank point in B frame comp. - Eigen::Vector3d rPPrime_TcT_T; //!< [m/s^2] Second derivative of position vector from B to tank point in B frame comp. - virtual void computeTankProps(double mFuel) = 0; //!< class method +class FuelTankModel +{ + public: + double propMassInit{}; //!< [kg] Initial propellant mass in tank + double maxFuelMass = 1.0; //!< [kg] maximum tank mass + Eigen::Vector3d r_TcT_TInit; //!< [m] Initial position vector from B to tank point in B frame comp. + Eigen::Matrix3d ITankPntT_T; //!< [kg m^2] Inertia of tank about pnt T in B frame comp. + Eigen::Matrix3d IPrimeTankPntT_T; //!< [kg m^2/s] Derivative of inertia of tank about pnt T in B frame comp. + Eigen::Vector3d r_TcT_T; //!< [m] position vector from B to tank point in B frame comp. + Eigen::Vector3d rPrime_TcT_T; //!< [m/s] Derivative of position vector from B to tank point in B frame comp. + Eigen::Vector3d + rPPrime_TcT_T; //!< [m/s^2] Second derivative of position vector from B to tank point in B frame comp. + virtual void computeTankProps(double mFuel) = 0; //!< class method virtual void computeTankPropDerivs(double mFuel, double mDotFuel) = 0; //!< class method - FuelTankModel() { - this->r_TcT_TInit.setZero(); - } + FuelTankModel() { this->r_TcT_TInit.setZero(); } virtual ~FuelTankModel() = default; }; /*! Tank constant volume class */ -class FuelTankModelConstantVolume : public FuelTankModel { -public: - double radiusTankInit{}; //!< [m] Initial radius of the spherical tank +class FuelTankModelConstantVolume : public FuelTankModel +{ + public: + double radiusTankInit{}; //!< [m] Initial radius of the spherical tank FuelTankModelConstantVolume() = default; ~FuelTankModelConstantVolume() override = default; - void computeTankProps(double mFuel) override { + void computeTankProps(double mFuel) override + { this->r_TcT_T = this->r_TcT_TInit; - this->ITankPntT_T = 2.0 / 5.0 * mFuel * this->radiusTankInit * this->radiusTankInit * Eigen::Matrix3d::Identity(); + this->ITankPntT_T = + 2.0 / 5.0 * mFuel * this->radiusTankInit * this->radiusTankInit * Eigen::Matrix3d::Identity(); } - void computeTankPropDerivs(double mFuel, double mDotFuel) override { - this->IPrimeTankPntT_T = 2.0 / 5.0 * mDotFuel * this->radiusTankInit * this->radiusTankInit * Eigen::Matrix3d::Identity(); + void computeTankPropDerivs(double mFuel, double mDotFuel) override + { + this->IPrimeTankPntT_T = + 2.0 / 5.0 * mDotFuel * this->radiusTankInit * this->radiusTankInit * Eigen::Matrix3d::Identity(); this->rPrime_TcT_T.setZero(); this->rPPrime_TcT_T.setZero(); } }; /*! Tank constant density class */ -class FuelTankModelConstantDensity : public FuelTankModel { -public: - double radiusTankInit{}; //!< [m] Initial radius of the spherical tank - double radiusTank{}; //!< [m] Current radius of the spherical tank +class FuelTankModelConstantDensity : public FuelTankModel +{ + public: + double radiusTankInit{}; //!< [m] Initial radius of the spherical tank + double radiusTank{}; //!< [m] Current radius of the spherical tank FuelTankModelConstantDensity() = default; ~FuelTankModelConstantDensity() override = default; - void computeTankProps(double mFuel) override { + void computeTankProps(double mFuel) override + { this->radiusTank = std::pow(mFuel / this->propMassInit, 1.0 / 3.0) * this->radiusTankInit; this->r_TcT_T = this->r_TcT_TInit; this->ITankPntT_T = 2.0 / 5.0 * mFuel * this->radiusTank * this->radiusTank * Eigen::Matrix3d::Identity(); } - void computeTankPropDerivs(double mFuel, double mDotFuel) override { - this->IPrimeTankPntT_T = 2.0 / 3.0 * mDotFuel * this->radiusTank * this->radiusTank * Eigen::Matrix3d::Identity(); + void computeTankPropDerivs(double mFuel, double mDotFuel) override + { + this->IPrimeTankPntT_T = + 2.0 / 3.0 * mDotFuel * this->radiusTank * this->radiusTank * Eigen::Matrix3d::Identity(); this->rPrime_TcT_T.setZero(); this->rPPrime_TcT_T.setZero(); } }; /*! Tank model emptying class */ -class FuelTankModelEmptying : public FuelTankModel { -public: - double radiusTankInit{}; //!< [m] Initial radius of the spherical tank - double rhoFuel{}; //!< [kg/m^3] density of the fuel - double thetaStar{}; //!< [rad] angle from vertical to top of fuel - double thetaDotStar{}; //!< [rad/s] derivative of angle from vertical to top of fuel - double thetaDDotStar{}; //!< [rad/s^2] second derivative of angle from vertical to top of fuel - Eigen::Vector3d k3; //!< -- Direction of fuel depletion +class FuelTankModelEmptying : public FuelTankModel +{ + public: + double radiusTankInit{}; //!< [m] Initial radius of the spherical tank + double rhoFuel{}; //!< [kg/m^3] density of the fuel + double thetaStar{}; //!< [rad] angle from vertical to top of fuel + double thetaDotStar{}; //!< [rad/s] derivative of angle from vertical to top of fuel + double thetaDDotStar{}; //!< [rad/s^2] second derivative of angle from vertical to top of fuel + Eigen::Vector3d k3; //!< -- Direction of fuel depletion FuelTankModelEmptying() = default; ~FuelTankModelEmptying() override = default; - void computeTankProps(double mFuel) override { - this->rhoFuel = this->propMassInit / (4.0 / 3.0 * M_PI * this->radiusTankInit * this->radiusTankInit * this->radiusTankInit); + void computeTankProps(double mFuel) override + { + this->rhoFuel = + this->propMassInit / (4.0 / 3.0 * M_PI * this->radiusTankInit * this->radiusTankInit * this->radiusTankInit); double rtank = this->radiusTankInit; double volume; double deltaRadiusK3; - this->k3 << 0, 0, 1; //k3 is zhat + this->k3 << 0, 0, 1; // k3 is zhat if (mFuel != this->propMassInit) { double rhoFuel = this->rhoFuel; std::function f = [rhoFuel, rtank, mFuel](double thetaStar) -> double { return 2.0 / 3.0 * M_PI * rhoFuel * rtank * rtank * rtank * - (1 + 3.0 / 2.0 * cos(thetaStar) - 1.0 / 2.0 * pow(cos(thetaStar), 3)) - mFuel; + (1 + 3.0 / 2.0 * cos(thetaStar) - 1.0 / 2.0 * pow(cos(thetaStar), 3)) - + mFuel; }; std::function fPrime = [rhoFuel, rtank](double thetaStar) -> double { return 2.0 / 3.0 * M_PI * rhoFuel * rtank * rtank * rtank * @@ -144,49 +156,56 @@ class FuelTankModelEmptying : public FuelTankModel { this->r_TcT_T = this->r_TcT_TInit + deltaRadiusK3 * this->k3; this->ITankPntT_T.setZero(); this->IPrimeTankPntT_T.setZero(); - this->ITankPntT_T(2, 2) = 2.0 / 5.0 * M_PI * this->rhoFuel * std::pow(this->radiusTankInit, 5) * - (2.0 / 3.0 + 1.0 / 4.0 * std::cos(this->thetaStar) * std::pow(std::sin(this->thetaStar), 4) - - 1 / 12.0 * (std::cos(3 * this->thetaStar) - 9 * std::cos(this->thetaStar))); - this->ITankPntT_T(0, 0) = this->ITankPntT_T(1, 1) = 2.0 / 5.0 * M_PI * this->rhoFuel * std::pow(this->radiusTankInit, 5) * - (2.0 / 3.0 - 1.0 / 4.0 * std::pow(std::cos(this->thetaStar), 5) + - 1 / 24.0 * (std::cos(3 * this->thetaStar) - 9 * std::cos(this->thetaStar)) + - 5.0 / 4.0 * cos(this->thetaStar) + - 1 / 8.0 * std::cos(this->thetaStar) * std::pow(std::sin(this->thetaStar), 4)); + this->ITankPntT_T(2, 2) = + 2.0 / 5.0 * M_PI * this->rhoFuel * std::pow(this->radiusTankInit, 5) * + (2.0 / 3.0 + 1.0 / 4.0 * std::cos(this->thetaStar) * std::pow(std::sin(this->thetaStar), 4) - + 1 / 12.0 * (std::cos(3 * this->thetaStar) - 9 * std::cos(this->thetaStar))); + this->ITankPntT_T(0, 0) = this->ITankPntT_T(1, 1) = + 2.0 / 5.0 * M_PI * this->rhoFuel * std::pow(this->radiusTankInit, 5) * + (2.0 / 3.0 - 1.0 / 4.0 * std::pow(std::cos(this->thetaStar), 5) + + 1 / 24.0 * (std::cos(3 * this->thetaStar) - 9 * std::cos(this->thetaStar)) + + 5.0 / 4.0 * cos(this->thetaStar) + + 1 / 8.0 * std::cos(this->thetaStar) * std::pow(std::sin(this->thetaStar), 4)); } - void computeTankPropDerivs(double mFuel, double mDotFuel) override { + void computeTankPropDerivs(double mFuel, double mDotFuel) override + { if (mFuel != this->propMassInit) { - this->thetaDotStar = -mDotFuel / (M_PI * this->rhoFuel * std::pow(this->radiusTankInit, 3) * std::sin(this->thetaStar)); + this->thetaDotStar = + -mDotFuel / (M_PI * this->rhoFuel * std::pow(this->radiusTankInit, 3) * std::sin(this->thetaStar)); this->thetaDDotStar = -3 * this->thetaDotStar * this->thetaDotStar * std::cos(this->thetaStar) / - std::sin(this->thetaStar); //This assumes that mddot = 0 + std::sin(this->thetaStar); // This assumes that mddot = 0 } else { this->thetaDotStar = 0.0; this->thetaDDotStar = 0.0; } - this->IPrimeTankPntT_T(2, 2) = 2.0 / 5.0 * M_PI * this->rhoFuel * std::pow(this->radiusTankInit, 5) * this->thetaDotStar * - (std::pow(std::cos(this->thetaStar), 2) * std::pow(std::sin(this->thetaStar), 3) - - 1.0 / 4.0 * std::pow(std::sin(this->thetaStar), 5) + - 1 / 4.0 * std::sin(3 * this->thetaStar) - 3.0 / 4.0 * std::sin(this->thetaStar)); + this->IPrimeTankPntT_T(2, 2) = + 2.0 / 5.0 * M_PI * this->rhoFuel * std::pow(this->radiusTankInit, 5) * this->thetaDotStar * + (std::pow(std::cos(this->thetaStar), 2) * std::pow(std::sin(this->thetaStar), 3) - + 1.0 / 4.0 * std::pow(std::sin(this->thetaStar), 5) + 1 / 4.0 * std::sin(3 * this->thetaStar) - + 3.0 / 4.0 * std::sin(this->thetaStar)); this->IPrimeTankPntT_T(0, 0) = this->IPrimeTankPntT_T(1, 1) = - 2.0 / 5.0 * M_PI * this->rhoFuel * std::pow(this->radiusTankInit, 5) * this->thetaDotStar * - (5.0 / 4.0 * std::sin(this->thetaStar) * std::cos(this->thetaStar) - 5.0 / 4.0 * std::sin(this->thetaStar) - - 1 / 8.0 * std::sin(3 * this->thetaStar) + - 3.0 / 8.0 * sin(this->thetaStar) + - 1 / 2.0 * std::pow(std::cos(this->thetaStar), 2) * std::pow(std::sin(this->thetaStar), 3) - - 1 / 8.0 * std::pow(std::sin(this->thetaStar), 5)); + 2.0 / 5.0 * M_PI * this->rhoFuel * std::pow(this->radiusTankInit, 5) * this->thetaDotStar * + (5.0 / 4.0 * std::sin(this->thetaStar) * std::cos(this->thetaStar) - 5.0 / 4.0 * std::sin(this->thetaStar) - + 1 / 8.0 * std::sin(3 * this->thetaStar) + 3.0 / 8.0 * sin(this->thetaStar) + + 1 / 2.0 * std::pow(std::cos(this->thetaStar), 2) * std::pow(std::sin(this->thetaStar), 3) - + 1 / 8.0 * std::pow(std::sin(this->thetaStar), 5)); if (mFuel != 0) { - this->rPrime_TcT_T = -M_PI * std::pow(this->radiusTankInit, 4) * this->rhoFuel / (4 * mFuel * mFuel) * - (4 * mFuel * this->thetaDotStar * std::pow(std::sin(this->thetaStar), 3) * std::cos(this->thetaStar) + - mDotFuel * (2 * std::pow(std::cos(this->thetaStar), 2) - std::pow(std::cos(this->thetaStar), 4) - 1)) * - this->k3; - - this->rPPrime_TcT_T = -M_PI * std::pow(this->radiusTankInit, 4) * this->rhoFuel / (2 * mFuel * mFuel * mFuel) * - (4 * mFuel * std::pow(std::sin(this->thetaStar), 3) * std::cos(this->thetaStar) * - (this->thetaDDotStar * mFuel - 2 * this->thetaDotStar * mDotFuel) - - 4 * mFuel * mFuel * this->thetaDotStar * this->thetaDotStar * std::pow(std::sin(this->thetaStar), 2) * - (3 * std::pow(std::cos(this->thetaStar), 2) - std::pow(std::sin(this->thetaStar), 2)) + - (2 * std::pow(std::cos(this->thetaStar), 2) - std::pow(std::cos(this->thetaStar), 4) - 1) * - (-2 * mDotFuel * mDotFuel)) * this->k3; + this->rPrime_TcT_T = + -M_PI * std::pow(this->radiusTankInit, 4) * this->rhoFuel / (4 * mFuel * mFuel) * + (4 * mFuel * this->thetaDotStar * std::pow(std::sin(this->thetaStar), 3) * std::cos(this->thetaStar) + + mDotFuel * (2 * std::pow(std::cos(this->thetaStar), 2) - std::pow(std::cos(this->thetaStar), 4) - 1)) * + this->k3; + + this->rPPrime_TcT_T = + -M_PI * std::pow(this->radiusTankInit, 4) * this->rhoFuel / (2 * mFuel * mFuel * mFuel) * + (4 * mFuel * std::pow(std::sin(this->thetaStar), 3) * std::cos(this->thetaStar) * + (this->thetaDDotStar * mFuel - 2 * this->thetaDotStar * mDotFuel) - + 4 * mFuel * mFuel * this->thetaDotStar * this->thetaDotStar * std::pow(std::sin(this->thetaStar), 2) * + (3 * std::pow(std::cos(this->thetaStar), 2) - std::pow(std::sin(this->thetaStar), 2)) + + (2 * std::pow(std::cos(this->thetaStar), 2) - std::pow(std::cos(this->thetaStar), 4) - 1) * + (-2 * mDotFuel * mDotFuel)) * + this->k3; } else { this->rPrime_TcT_T.setZero(); @@ -196,27 +215,30 @@ class FuelTankModelEmptying : public FuelTankModel { }; /*! Tank model class for a uniform burn */ -class FuelTankModelUniformBurn : public FuelTankModel { -public: - double radiusTankInit{}; //!< [m] Initial radius of the cylindrical tank - double lengthTank{}; //!< [m] Length of the tank +class FuelTankModelUniformBurn : public FuelTankModel +{ + public: + double radiusTankInit{}; //!< [m] Initial radius of the cylindrical tank + double lengthTank{}; //!< [m] Length of the tank FuelTankModelUniformBurn() = default; ~FuelTankModelUniformBurn() override = default; - void computeTankProps(double mFuel) override { + void computeTankProps(double mFuel) override + { this->r_TcT_T = this->r_TcT_TInit; this->ITankPntT_T.setZero(); this->ITankPntT_T(0, 0) = this->ITankPntT_T(1, 1) = - mFuel * (this->radiusTankInit * this->radiusTankInit / 4.0 + this->lengthTank * this->lengthTank / 12.0); + mFuel * (this->radiusTankInit * this->radiusTankInit / 4.0 + this->lengthTank * this->lengthTank / 12.0); this->ITankPntT_T(2, 2) = mFuel * this->radiusTankInit * this->radiusTankInit / 2; } - void computeTankPropDerivs(double mFuel, double mDotFuel) override { + void computeTankPropDerivs(double mFuel, double mDotFuel) override + { this->IPrimeTankPntT_T.setZero(); this->IPrimeTankPntT_T(0, 0) = this->IPrimeTankPntT_T(1, 1) = - mDotFuel * (this->radiusTankInit * this->radiusTankInit / 4.0 + this->lengthTank * this->lengthTank / 12.0); + mDotFuel * (this->radiusTankInit * this->radiusTankInit / 4.0 + this->lengthTank * this->lengthTank / 12.0); this->IPrimeTankPntT_T(2, 2) = mDotFuel * this->radiusTankInit * this->radiusTankInit / 2; this->rPrime_TcT_T.setZero(); this->rPPrime_TcT_T.setZero(); @@ -224,33 +246,36 @@ class FuelTankModelUniformBurn : public FuelTankModel { }; /*! Tank model class for a centrifugal burn */ -class FuelTankModelCentrifugalBurn : public FuelTankModel { -public: - double radiusTankInit{}; //!< [m] Initial radius of the cylindrical tank - double lengthTank{}; //!< [m] Length of the tank - double radiusInner{}; //!< [m] Inner radius of the cylindrical tank +class FuelTankModelCentrifugalBurn : public FuelTankModel +{ + public: + double radiusTankInit{}; //!< [m] Initial radius of the cylindrical tank + double lengthTank{}; //!< [m] Length of the tank + double radiusInner{}; //!< [m] Inner radius of the cylindrical tank FuelTankModelCentrifugalBurn() = default; ~FuelTankModelCentrifugalBurn() override = default; - void computeTankProps(double mFuel) override { + void computeTankProps(double mFuel) override + { double rhoFuel = this->propMassInit / (M_PI * this->radiusTankInit * this->radiusTankInit * this->lengthTank); - this->radiusInner = std::sqrt(std::max(this->radiusTankInit * this->radiusTankInit - mFuel / (M_PI * this->lengthTank * rhoFuel), 0.0)); + this->radiusInner = std::sqrt( + std::max(this->radiusTankInit * this->radiusTankInit - mFuel / (M_PI * this->lengthTank * rhoFuel), 0.0)); this->r_TcT_T = this->r_TcT_TInit; this->ITankPntT_T.setZero(); - this->ITankPntT_T(0, 0) = this->ITankPntT_T(1, 1) = mFuel * - ((this->radiusTankInit * this->radiusTankInit + - this->radiusInner * this->radiusInner) / 4.0 + - this->lengthTank * this->lengthTank / 12.0); - this->ITankPntT_T(2, 2) = mFuel * (this->radiusTankInit * this->radiusTankInit + - this->radiusInner * this->radiusInner) / 2; + this->ITankPntT_T(0, 0) = this->ITankPntT_T(1, 1) = + mFuel * ((this->radiusTankInit * this->radiusTankInit + this->radiusInner * this->radiusInner) / 4.0 + + this->lengthTank * this->lengthTank / 12.0); + this->ITankPntT_T(2, 2) = + mFuel * (this->radiusTankInit * this->radiusTankInit + this->radiusInner * this->radiusInner) / 2; } - void computeTankPropDerivs(double mFuel, double mDotFuel) override { + void computeTankPropDerivs(double mFuel, double mDotFuel) override + { this->IPrimeTankPntT_T.setZero(); this->IPrimeTankPntT_T(0, 0) = this->IPrimeTankPntT_T(1, 1) = - mDotFuel * (this->radiusInner * this->radiusInner / 2.0 + this->lengthTank * this->lengthTank / 12.0); + mDotFuel * (this->radiusInner * this->radiusInner / 2.0 + this->lengthTank * this->lengthTank / 12.0); this->IPrimeTankPntT_T(2, 2) = mDotFuel * this->radiusInner * this->radiusInner; this->rPrime_TcT_T.setZero(); this->rPPrime_TcT_T.setZero(); @@ -258,56 +283,57 @@ class FuelTankModelCentrifugalBurn : public FuelTankModel { }; /*! Fuel tank effector model class */ -class FuelTank : - public StateEffector, public SysModel { -public: - std::string nameOfMassState{}; //!< -- name of mass state - std::vector fuelSloshParticles; //!< -- vector of fuel slosh particles - std::vector thrDynEffectors; //!< -- Vector of dynamic effectors for thrusters - std::vector thrStateEffectors; //!< -- Vector of state effectors for thrusters - Eigen::Matrix3d dcm_TB; //!< -- DCM from body frame to tank frame - Eigen::Vector3d r_TB_B; //!< [m] position of tank in B frame - bool updateOnly = true; //!< -- Sets whether to use update only mass depletion - Message fuelTankOutMsg{}; //!< -- fuel tank output message name - FuelTankMsgPayload fuelTankMassPropMsg{}; //!< instance of messaging system message struct - -private: - StateData *omegaState{}; //!< -- state data for omega_BN of the hub - StateData *massState{}; //!< -- state data for mass state - double fuelConsumption{}; //!< [kg/s] rate of fuel being consumed - double tankFuelConsumption{}; //!< [kg/s] rate of fuel being consumed from tank - FuelTankModel *fuelTankModel{}; //!< -- style of tank to simulate +class FuelTank + : public StateEffector + , public SysModel +{ + public: + std::string nameOfMassState{}; //!< -- name of mass state + std::vector fuelSloshParticles; //!< -- vector of fuel slosh particles + std::vector thrDynEffectors; //!< -- Vector of dynamic effectors for thrusters + std::vector thrStateEffectors; //!< -- Vector of state effectors for thrusters + Eigen::Matrix3d dcm_TB; //!< -- DCM from body frame to tank frame + Eigen::Vector3d r_TB_B; //!< [m] position of tank in B frame + bool updateOnly = true; //!< -- Sets whether to use update only mass depletion + Message fuelTankOutMsg{}; //!< -- fuel tank output message name + FuelTankMsgPayload fuelTankMassPropMsg{}; //!< instance of messaging system message struct + + private: + StateData* omegaState{}; //!< -- state data for omega_BN of the hub + StateData* massState{}; //!< -- state data for mass state + double fuelConsumption{}; //!< [kg/s] rate of fuel being consumed + double tankFuelConsumption{}; //!< [kg/s] rate of fuel being consumed from tank + FuelTankModel* fuelTankModel{}; //!< -- style of tank to simulate Eigen::Matrix3d ITankPntT_B; Eigen::Vector3d r_TcB_B; - static uint64_t effectorID; //!< [] ID number of this fuel tank effector + static uint64_t effectorID; //!< [] ID number of this fuel tank effector -public: + public: FuelTank(); ~FuelTank(); void writeOutputMessages(uint64_t currentClock); void UpdateState(uint64_t currentSimNanos) override; - void setTankModel(FuelTankModel *model); - void pushFuelSloshParticle(FuelSlosh *particle); //!< -- Attach fuel slosh particle - void registerStates(DynParamManager &states) override; //!< -- Register mass state with state manager - void linkInStates(DynParamManager &states) override; //!< -- Give the tank access to other states + void setTankModel(FuelTankModel* model); + void pushFuelSloshParticle(FuelSlosh* particle); //!< -- Attach fuel slosh particle + void registerStates(DynParamManager& states) override; //!< -- Register mass state with state manager + void linkInStates(DynParamManager& states) override; //!< -- Give the tank access to other states void updateEffectorMassProps(double integTime) override; //!< -- Add contribution mass props from the tank void setNameOfMassState(const std::string nameOfMassState); //!< -- Setter for fuel tank mass state name - void addThrusterSet(ThrusterDynamicEffector *dynEff); //!< -- Add DynamicEffector thruster - void addThrusterSet(ThrusterStateEffector *stateEff); //!< -- Add StateEffector thruster + void addThrusterSet(ThrusterDynamicEffector* dynEff); //!< -- Add DynamicEffector thruster + void addThrusterSet(ThrusterStateEffector* stateEff); //!< -- Add StateEffector thruster void updateContributions(double integTime, - BackSubMatrices &backSubContr, + BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, - Eigen::Vector3d g_N) override; //!< -- Back-sub contributions + Eigen::Vector3d g_N) override; //!< -- Back-sub contributions void updateEnergyMomContributions(double integTime, - Eigen::Vector3d &rotAngMomPntCContr_B, - double &rotEnergyContr, - Eigen::Vector3d omega_BN_B) override; //!< -- Energy and momentum calculations + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) override; //!< -- Energy and momentum calculations void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) override; //!< -- Calculate stateEffector's derivatives }; - #endif /* FUEL_TANK_H */ 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.cpp b/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.cpp index 97a17e21cb..11cc27e076 100644 --- a/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.cpp +++ b/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.cpp @@ -23,28 +23,29 @@ GravityGradientEffector::GravityGradientEffector() { - return; + return; } /*! The destructor.*/ GravityGradientEffector::~GravityGradientEffector() { - return; + return; } - /*! This method is used to set the effector, and check same module variables */ -void GravityGradientEffector::Reset(uint64_t CurrentSimNanos) +void +GravityGradientEffector::Reset(uint64_t CurrentSimNanos) { /* zero the effector output forces and torques */ this->forceExternal_B.fill(0.0); this->torqueExternalPntB_B.fill(0.0); this->forceExternal_N.fill(0.0); - if (this->planetPropertyNames.size()==0) { - bskLogger.bskLog(BSK_ERROR, "planetPropertyNames array is empty, you must specify at least one planet using addPlanetName()."); + if (this->planetPropertyNames.size() == 0) { + bskLogger.bskLog( + BSK_ERROR, "planetPropertyNames array is empty, you must specify at least one planet using addPlanetName()."); } /* empty the vector of planet state pointers */ @@ -58,31 +59,35 @@ void GravityGradientEffector::Reset(uint64_t CurrentSimNanos) @param planetName The planet name */ -void GravityGradientEffector::addPlanetName(std::string planetName) +void +GravityGradientEffector::addPlanetName(std::string planetName) { this->planetPropertyNames.push_back(planetName); return; } - /*! Write the gravity gradient torque output message. */ -void GravityGradientEffector::WriteOutputMessages(uint64_t CurrentClock) +void +GravityGradientEffector::WriteOutputMessages(uint64_t CurrentClock) { GravityGradientMsgPayload outMsg; eigenVector3d2CArray(this->torqueExternalPntB_B, outMsg.gravityGradientTorque_B); this->gravityGradientOutMsg.write(&outMsg, this->moduleID, CurrentClock); - return; + return; } -/*! This method is used to link the gravity gradient effector to the hub position, inertia tensor and center of mass vector. +/*! This method is used to link the gravity gradient effector to the hub position, inertia tensor and center of mass + vector. */ -void GravityGradientEffector::linkInStates(DynParamManager& states){ +void +GravityGradientEffector::linkInStates(DynParamManager& states) +{ this->hubSigma = states.getStateObject(this->stateNameOfSigma); this->r_BN_N = states.getStateObject(this->stateNameOfPosition); this->ISCPntB_B = states.getPropertyReference(this->propName_inertiaSC); @@ -90,16 +95,18 @@ void GravityGradientEffector::linkInStates(DynParamManager& states){ this->m_SC = states.getPropertyReference(this->propName_m_SC); std::vector::iterator name; - for(name = this->planetPropertyNames.begin(); name != this->planetPropertyNames.end(); name++) { + for (name = this->planetPropertyNames.begin(); name != this->planetPropertyNames.end(); name++) { this->r_PN_N.push_back(states.getPropertyReference(*name + ".r_PN_N")); this->muPlanet.push_back(states.getPropertyReference(*name + ".mu")); } } /*! This method computes the body forces and torques for the gravity gradient effector. -*/ -void GravityGradientEffector::computeForceTorque(double integTime, double timeStep){ - // Zero out the force/torque values to begin with + */ +void +GravityGradientEffector::computeForceTorque(double integTime, double timeStep) +{ + // Zero out the force/torque values to begin with this->torqueExternalPntB_B.setZero(); /* compute DCN [BN] */ @@ -111,15 +118,15 @@ void GravityGradientEffector::computeForceTorque(double integTime, double timeSt Eigen::MatrixXd ISCPntC_B; Eigen::Matrix3d cTilde; cTilde = eigenTilde(*this->c_B); - ISCPntC_B = *this->ISCPntB_B - (*this->m_SC)(0,0)*cTilde*cTilde.transpose(); + ISCPntC_B = *this->ISCPntB_B - (*this->m_SC)(0, 0) * cTilde * cTilde.transpose(); std::vector::iterator it; int c = 0; - for(it = this->planetPropertyNames.begin(); it != this->planetPropertyNames.end(); it++) { - double mu = (*this->muPlanet[c])(0,0); /* in m^3/s^2 */ + for (it = this->planetPropertyNames.begin(); it != this->planetPropertyNames.end(); it++) { + double mu = (*this->muPlanet[c])(0, 0); /* in m^3/s^2 */ /* determine spacecraft CM position relative to planet */ - Eigen::Vector3d r_CP_N = this->r_BN_N->getState() + dcm_BN.transpose()*(*this->c_B) - *(this->r_PN_N[c]); + Eigen::Vector3d r_CP_N = this->r_BN_N->getState() + dcm_BN.transpose() * (*this->c_B) - *(this->r_PN_N[c]); /* find orbit radius */ double rMag = r_CP_N.norm(); @@ -129,7 +136,7 @@ void GravityGradientEffector::computeForceTorque(double integTime, double timeSt /* compute gravity gradient torque */ Eigen::Vector3d ggTorque; - ggTorque = 3.0*mu/rMag/rMag/rMag * (ISCPntC_B * rHat_B); + ggTorque = 3.0 * mu / rMag / rMag / rMag * (ISCPntC_B * rHat_B); ggTorque = rHat_B.cross(ggTorque); /* sum up all gravity gradient contributions */ @@ -137,7 +144,7 @@ void GravityGradientEffector::computeForceTorque(double integTime, double timeSt c++; } - return; + return; } /*! This method is called once per BSK update cycle. It writes out a msg of the @@ -145,8 +152,9 @@ void GravityGradientEffector::computeForceTorque(double integTime, double timeSt @param CurrentSimNanos The current simulation time in nanoseconds */ -void GravityGradientEffector::UpdateState(uint64_t CurrentSimNanos) +void +GravityGradientEffector::UpdateState(uint64_t CurrentSimNanos) { - this->WriteOutputMessages(CurrentSimNanos); - return; + this->WriteOutputMessages(CurrentSimNanos); + return; } diff --git a/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.h b/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.h index 23496097f9..5a68036158 100644 --- a/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.h +++ b/src/simulation/dynamics/GravityGradientEffector/GravityGradientEffector.h @@ -17,7 +17,6 @@ */ - #ifndef GRAVITY_GRADIENT_DYNAMIC_EFFECTOR_H #define GRAVITY_GRADIENT_DYNAMIC_EFFECTOR_H @@ -33,11 +32,12 @@ #include "architecture/msgPayloadDefC/GravityGradientMsgPayload.h" #include "architecture/messaging/messaging.h" - - /*! @brief gravity gradient gradient module */ -class GravityGradientEffector: public SysModel, public DynamicEffector { -public: +class GravityGradientEffector + : public SysModel + , public DynamicEffector +{ + public: GravityGradientEffector(); ~GravityGradientEffector(); void linkInStates(DynParamManager& states); @@ -47,23 +47,20 @@ class GravityGradientEffector: public SysModel, public DynamicEffector { void WriteOutputMessages(uint64_t CurrentClock); void addPlanetName(std::string planetName); + public: + 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 + Eigen::MatrixXd* c_B; //!< [m] Vector from point B to CoM of s/c in B frame components + Eigen::MatrixXd* m_SC; //!< [kg] mass of spacecraft + std::vector r_PN_N; //!< [m] vector of inertial planet positions + std::vector muPlanet; //!< [m^3/s^-2] gravitational constant of planet -public: - 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 - Eigen::MatrixXd *c_B; //!< [m] Vector from point B to CoM of s/c in B frame components - Eigen::MatrixXd *m_SC; //!< [kg] mass of spacecraft - std::vector r_PN_N; //!< [m] vector of inertial planet positions - std::vector muPlanet; //!< [m^3/s^-2] gravitational constant of planet - - BSKLogger bskLogger; //!< BSK Logging - -private: - std::vector planetPropertyNames; //!< Names of planets we want to track + BSKLogger bskLogger; //!< BSK Logging + private: + std::vector planetPropertyNames; //!< Names of planets we want to track }; - #endif /* GRAVITY_GRADIENT_DYNAMIC_EFFECTOR_H */ 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/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/_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/_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/HingedRigidBodies/hingedRigidBodyStateEffector.cpp b/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.cpp old mode 100755 new mode 100644 index 1704547a47..381081cddc --- a/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.cpp +++ b/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.cpp @@ -56,17 +56,17 @@ uint64_t HingedRigidBodyStateEffector::effectorID = 1; /*! This is the destructor, nothing to report here */ HingedRigidBodyStateEffector::~HingedRigidBodyStateEffector() { - this->effectorID = 1; /* reset the panel ID*/ + this->effectorID = 1; /* reset the panel ID*/ return; } - /*! This method takes the computed theta states and outputs them to the m messaging system. @param CurrentClock The current simulation time (used for time stamping) */ -void HingedRigidBodyStateEffector::writeOutputStateMessages(uint64_t CurrentClock) +void +HingedRigidBodyStateEffector::writeOutputStateMessages(uint64_t CurrentClock) { if (this->hingedRigidBodyOutMsg.isLinked()) { @@ -87,10 +87,10 @@ void HingedRigidBodyStateEffector::writeOutputStateMessages(uint64_t CurrentCloc eigenVector3d2CArray(this->omega_SN_S, configLogMsg.omega_BN_B); this->hingedRigidBodyConfigLogOutMsg.write(&configLogMsg, this->moduleID, CurrentClock); } - } -void HingedRigidBodyStateEffector::prependSpacecraftNameToStates() +void +HingedRigidBodyStateEffector::prependSpacecraftNameToStates() { this->nameOfThetaState = this->nameOfSpacecraftAttachedTo + this->nameOfThetaState; this->nameOfThetaDotState = this->nameOfSpacecraftAttachedTo + this->nameOfThetaDotState; @@ -99,7 +99,8 @@ void HingedRigidBodyStateEffector::prependSpacecraftNameToStates() } /*! This method allows the HRB state effector to have access to the hub states and gravity*/ -void HingedRigidBodyStateEffector::linkInStates(DynParamManager& statesIn) +void +HingedRigidBodyStateEffector::linkInStates(DynParamManager& statesIn) { // - Get access to the hubs sigma, omegaBN_B and velocity needed for dynamic coupling and gravity std::string tmpMsgName; @@ -108,23 +109,26 @@ void HingedRigidBodyStateEffector::linkInStates(DynParamManager& statesIn) tmpMsgName = this->nameOfSpacecraftAttachedTo + "centerOfMassPrimeSC"; this->cPrime_B = statesIn.getPropertyReference(tmpMsgName); - this->inertialPositionProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); - this->inertialVelocityProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); + this->inertialPositionProperty = + statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); + this->inertialVelocityProperty = + statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); return; } /*! This method allows the HRB state effector to register its states: theta and thetaDot with the dyn param manager */ -void HingedRigidBodyStateEffector::registerStates(DynParamManager& states) +void +HingedRigidBodyStateEffector::registerStates(DynParamManager& states) { // - Register the states associated with hinged rigid bodies - theta and thetaDot this->thetaState = states.registerState(1, 1, this->nameOfThetaState); - Eigen::MatrixXd thetaInitMatrix(1,1); - thetaInitMatrix(0,0) = this->thetaInit; + Eigen::MatrixXd thetaInitMatrix(1, 1); + thetaInitMatrix(0, 0) = this->thetaInit; this->thetaState->setState(thetaInitMatrix); this->thetaDotState = states.registerState(1, 1, this->nameOfThetaDotState); - Eigen::MatrixXd thetaDotInitMatrix(1,1); - thetaDotInitMatrix(0,0) = this->thetaDotInit; + Eigen::MatrixXd thetaDotInitMatrix(1, 1); + thetaDotInitMatrix(0, 0) = this->thetaDotInit; this->thetaDotState->setState(thetaDotInitMatrix); return; @@ -132,11 +136,12 @@ void HingedRigidBodyStateEffector::registerStates(DynParamManager& states) /*! This method allows the HRB state effector to provide its contributions to the mass props and mass prop rates of the spacecraft */ -void HingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) +void +HingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) { // - Convert intial variables to mother craft frame relative information - this->r_HP_P = this->r_BP_P + this->dcm_BP.transpose()*r_HB_B; - this->dcm_HP = this->dcm_HB*this->dcm_BP; + this->r_HP_P = this->r_BP_P + this->dcm_BP.transpose() * r_HB_B; + this->dcm_HP = this->dcm_HB * this->dcm_BP; // - Give the mass of the hinged rigid body to the effProps mass this->effProps.mEff = this->mass; @@ -147,39 +152,44 @@ void HingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) this->thetaDot = this->thetaDotState->getState()(0, 0); // - Next find the sHat unit vectors this->dcm_SH = eigenM2(this->theta); - this->dcm_SP = this->dcm_SH*this->dcm_HP; + this->dcm_SP = this->dcm_SH * this->dcm_HP; this->sHat1_P = this->dcm_SP.row(0); this->sHat2_P = this->dcm_SP.row(1); this->sHat3_P = this->dcm_SP.row(2); - this->r_SP_P = this->r_HP_P - this->d*this->sHat1_P; + this->r_SP_P = this->r_HP_P - this->d * this->sHat1_P; this->effProps.rEff_CB_B = this->r_SP_P; // - Find the inertia of the hinged rigid body about point B // - Define rTilde_SB_B this->rTilde_SP_P = eigenTilde(this->r_SP_P); - this->effProps.IEffPntB_B = this->dcm_SP.transpose()*this->IPntS_S*this->dcm_SP - + this->mass*this->rTilde_SP_P*this->rTilde_SP_P.transpose(); + this->effProps.IEffPntB_B = this->dcm_SP.transpose() * this->IPntS_S * this->dcm_SP + + this->mass * this->rTilde_SP_P * this->rTilde_SP_P.transpose(); // - Find rPrime_SB_B - this->rPrime_SP_P = this->d*this->thetaDot*this->sHat3_P; + this->rPrime_SP_P = this->d * this->thetaDot * this->sHat3_P; this->effProps.rEffPrime_CB_B = this->rPrime_SP_P; // - Next find the body time derivative of the inertia about point B // - Define tilde matrix of rPrime_SB_B this->rPrimeTilde_SP_P = eigenTilde(this->rPrime_SP_P); // - Find body time derivative of IPntS_B - this->ISPrimePntS_P = this->thetaDot*(this->IPntS_S(2,2) - - this->IPntS_S(0,0))*(this->sHat1_P*this->sHat3_P.transpose() + this->sHat3_P*this->sHat1_P.transpose()); + this->ISPrimePntS_P = this->thetaDot * (this->IPntS_S(2, 2) - this->IPntS_S(0, 0)) * + (this->sHat1_P * this->sHat3_P.transpose() + this->sHat3_P * this->sHat1_P.transpose()); // - Find body time derivative of IPntB_B - this->effProps.IEffPrimePntB_B = this->ISPrimePntS_P - - this->mass*(this->rPrimeTilde_SP_P*this->rTilde_SP_P + this->rTilde_SP_P*this->rPrimeTilde_SP_P); + this->effProps.IEffPrimePntB_B = this->ISPrimePntS_P - this->mass * (this->rPrimeTilde_SP_P * this->rTilde_SP_P + + this->rTilde_SP_P * this->rPrimeTilde_SP_P); return; } /*! This method allows the HRB state effector to give its contributions to the matrices needed for the back-sub method */ -void HingedRigidBodyStateEffector::updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) +void +HingedRigidBodyStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { // - Find dcm_BN Eigen::MRPd sigmaLocal_PN; @@ -194,81 +204,95 @@ void HingedRigidBodyStateEffector::updateContributions(double integTime, BackSub Eigen::Vector3d gLocal_N; Eigen::Vector3d g_P; gLocal_N = g_N; - g_P = dcm_PN*gLocal_N; + g_P = dcm_PN * gLocal_N; // - Define omega_BN_S this->omega_BN_B = omega_BN_B; this->omegaLoc_PN_P = this->omega_BN_B; - this->omega_PN_S = this->dcm_SP*this->omegaLoc_PN_P; + this->omega_PN_S = this->dcm_SP * this->omegaLoc_PN_P; // - Define omegaTildeLoc_BN_B this->omegaTildeLoc_PN_P = eigenTilde(this->omegaLoc_PN_P); // - Define aTheta - this->aTheta = -this->mass*this->d/(this->IPntS_S(1,1) + this->mass*this->d*this->d)*this->sHat3_P; + this->aTheta = -this->mass * this->d / (this->IPntS_S(1, 1) + this->mass * this->d * this->d) * this->sHat3_P; // - Define bTheta this->rTilde_HP_P = eigenTilde(this->r_HP_P); - this->bTheta = -1.0/(this->IPntS_S(1,1) + this->mass*this->d*this->d)*((this->IPntS_S(1,1) - + this->mass*this->d*this->d)*this->sHat2_P + this->mass*this->d*this->rTilde_HP_P*this->sHat3_P); + this->bTheta = -1.0 / (this->IPntS_S(1, 1) + this->mass * this->d * this->d) * + ((this->IPntS_S(1, 1) + this->mass * this->d * this->d) * this->sHat2_P + + this->mass * this->d * this->rTilde_HP_P * this->sHat3_P); // - Define cTheta Eigen::Vector3d gravityTorquePntH_P; - gravityTorquePntH_P = -this->d*this->sHat1_P.cross(this->mass*g_P); - this->cTheta = 1.0/(this->IPntS_S(1,1) + this->mass*this->d*this->d)*(this->u -this->k*(this->theta-this->thetaRef) - this->c*(this->thetaDot-this->thetaDotRef) - + this->sHat2_P.dot(gravityTorquePntH_P) + (this->IPntS_S(2,2) - this->IPntS_S(0,0) - + this->mass*this->d*this->d)*this->omega_PN_S(2)*this->omega_PN_S(0) - this->mass*this->d* - this->sHat3_P.transpose()*this->omegaTildeLoc_PN_P*this->omegaTildeLoc_PN_P*this->r_HP_P); + gravityTorquePntH_P = -this->d * this->sHat1_P.cross(this->mass * g_P); + this->cTheta = 1.0 / (this->IPntS_S(1, 1) + this->mass * this->d * this->d) * + (this->u - this->k * (this->theta - this->thetaRef) - + this->c * (this->thetaDot - this->thetaDotRef) + this->sHat2_P.dot(gravityTorquePntH_P) + + (this->IPntS_S(2, 2) - this->IPntS_S(0, 0) + this->mass * this->d * this->d) * this->omega_PN_S(2) * + this->omega_PN_S(0) - + this->mass * this->d * this->sHat3_P.transpose() * this->omegaTildeLoc_PN_P * + this->omegaTildeLoc_PN_P * this->r_HP_P); // - Start defining them good old contributions - start with translation // - For documentation on contributions see Allard, Diaz, Schaub flex/slosh paper - backSubContr.matrixA = this->mass*this->d*this->sHat3_P*this->aTheta.transpose(); - backSubContr.matrixB = this->mass*this->d*this->sHat3_P*this->bTheta.transpose(); - backSubContr.vecTrans = -(this->mass*this->d*this->thetaDot*this->thetaDot*this->sHat1_P - + this->mass*this->d*this->cTheta*this->sHat3_P); + backSubContr.matrixA = this->mass * this->d * this->sHat3_P * this->aTheta.transpose(); + backSubContr.matrixB = this->mass * this->d * this->sHat3_P * this->bTheta.transpose(); + backSubContr.vecTrans = -(this->mass * this->d * this->thetaDot * this->thetaDot * this->sHat1_P + + this->mass * this->d * this->cTheta * this->sHat3_P); // - Define rotational matrice contributions - backSubContr.matrixC = (this->IPntS_S(1,1)*this->sHat2_P + this->mass*this->d*this->rTilde_SP_P*this->sHat3_P) - *this->aTheta.transpose(); - backSubContr.matrixD = (this->IPntS_S(1,1)*this->sHat2_P + this->mass*this->d*this->rTilde_SP_P*this->sHat3_P) - *this->bTheta.transpose(); + backSubContr.matrixC = + (this->IPntS_S(1, 1) * this->sHat2_P + this->mass * this->d * this->rTilde_SP_P * this->sHat3_P) * + this->aTheta.transpose(); + backSubContr.matrixD = + (this->IPntS_S(1, 1) * this->sHat2_P + this->mass * this->d * this->rTilde_SP_P * this->sHat3_P) * + this->bTheta.transpose(); Eigen::Matrix3d intermediateMatrix; - backSubContr.vecRot = -((this->thetaDot*this->omegaTildeLoc_PN_P + this->cTheta*intermediateMatrix.Identity()) - *(this->IPntS_S(1,1)*this->sHat2_P + this->mass*this->d*this->rTilde_SP_P*this->sHat3_P) - + this->mass*this->d*this->thetaDot*this->thetaDot*this->rTilde_SP_P*this->sHat1_P); + backSubContr.vecRot = + -((this->thetaDot * this->omegaTildeLoc_PN_P + this->cTheta * intermediateMatrix.Identity()) * + (this->IPntS_S(1, 1) * this->sHat2_P + this->mass * this->d * this->rTilde_SP_P * this->sHat3_P) + + this->mass * this->d * this->thetaDot * this->thetaDot * this->rTilde_SP_P * this->sHat1_P); return; } /*! This method is used to find the derivatives for the HRB stateEffector: thetaDDot and the kinematic derivative */ -void HingedRigidBodyStateEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +HingedRigidBodyStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { // - Grab necessarry values from manager (these have been previously computed in hubEffector) Eigen::Vector3d rDDotLoc_PN_N = rDDot_BN_N; Eigen::MRPd sigmaLocal_PN; Eigen::Vector3d omegaDotLoc_PN_P; - this->sigma_BN = sigma_BN; // store current hub attitude + this->sigma_BN = sigma_BN; // store current hub attitude sigmaLocal_PN = this->sigma_BN; // - Find rDDotLoc_BN_B Eigen::Matrix3d dcm_PN; Eigen::Vector3d rDDotLoc_PN_P; dcm_PN = (sigmaLocal_PN.toRotationMatrix()).transpose(); - rDDotLoc_PN_P = dcm_PN*rDDotLoc_PN_N; + rDDotLoc_PN_P = dcm_PN * rDDotLoc_PN_N; // - Compute Derivatives // - First is trivial this->thetaState->setDerivative(thetaDotState->getState()); // - Second, a little more involved - Eigen::MatrixXd thetaDDot(1,1); - thetaDDot(0,0) = this->aTheta.dot(rDDotLoc_PN_P) + this->bTheta.dot(omegaDot_BN_B) + this->cTheta; + Eigen::MatrixXd thetaDDot(1, 1); + thetaDDot(0, 0) = this->aTheta.dot(rDDotLoc_PN_P) + this->bTheta.dot(omegaDot_BN_B) + this->cTheta; this->thetaDotState->setDerivative(thetaDDot); return; } /*! This method is for calculating the contributions of the HRB state effector to the energy and momentum of the s/c */ -void HingedRigidBodyStateEffector::updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B) +void +HingedRigidBodyStateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { // - Get the current omega state Eigen::Vector3d omegaLocal_PN_P; @@ -279,15 +303,16 @@ void HingedRigidBodyStateEffector::updateEnergyMomContributions(double integTime Eigen::Vector3d omega_SN_P; Eigen::Matrix3d IPntS_P; Eigen::Vector3d rDot_SP_P; - omega_SP_P = this->thetaDot*this->sHat2_P; + omega_SP_P = this->thetaDot * this->sHat2_P; omega_SN_P = omega_SP_P + omegaLocal_PN_P; - IPntS_P = this->dcm_SP.transpose()*this->IPntS_S*this->dcm_SP; + IPntS_P = this->dcm_SP.transpose() * this->IPntS_S * this->dcm_SP; rDot_SP_P = this->rPrime_SP_P + omegaLocal_PN_P.cross(this->r_SP_P); - rotAngMomPntCContr_B = IPntS_P*omega_SN_P + this->mass*this->r_SP_P.cross(rDot_SP_P); + rotAngMomPntCContr_B = IPntS_P * omega_SN_P + this->mass * this->r_SP_P.cross(rDot_SP_P); // - Find rotational energy contribution from the hub - rotEnergyContr = 1.0/2.0*omega_SN_P.dot(IPntS_P*omega_SN_P) + 1.0/2.0*this->mass*rDot_SP_P.dot(rDot_SP_P) - + 1.0/2.0*this->k*(this->theta-this->thetaRef)*(this->theta-this->thetaRef); + rotEnergyContr = 1.0 / 2.0 * omega_SN_P.dot(IPntS_P * omega_SN_P) + + 1.0 / 2.0 * this->mass * rDot_SP_P.dot(rDot_SP_P) + + 1.0 / 2.0 * this->k * (this->theta - this->thetaRef) * (this->theta - this->thetaRef); return; } @@ -295,7 +320,8 @@ void HingedRigidBodyStateEffector::updateEnergyMomContributions(double integTime @param CurrentSimNanos The current simulation time in nanoseconds */ -void HingedRigidBodyStateEffector::UpdateState(uint64_t CurrentSimNanos) +void +HingedRigidBodyStateEffector::UpdateState(uint64_t CurrentSimNanos) { //! - Zero the command buffer and read the incoming command array if (this->motorTorqueInMsg.isLinked() && this->motorTorqueInMsg.isWritten()) { @@ -318,7 +344,8 @@ void HingedRigidBodyStateEffector::UpdateState(uint64_t CurrentSimNanos) return; } -void HingedRigidBodyStateEffector::calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B) +void +HingedRigidBodyStateEffector::calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B) { // - Get the current omega state @@ -331,17 +358,20 @@ void HingedRigidBodyStateEffector::calcForceTorqueOnBody(double integTime, Eigen thetaDDotLocal = thetaDotState->getStateDeriv()(0, 0); // - Calculate force that the HRB is applying to the spacecraft - this->forceOnBody_B = -(this->mass*this->d*this->sHat3_P*thetaDDotLocal + this->mass*this->d*this->thetaDot - *this->thetaDot*this->sHat1_P + 2.0*omegaLocalTilde_PN_P*this->mass*this->d*this->thetaDot - *this->sHat3_P); + this->forceOnBody_B = -(this->mass * this->d * this->sHat3_P * thetaDDotLocal + + this->mass * this->d * this->thetaDot * this->thetaDot * this->sHat1_P + + 2.0 * omegaLocalTilde_PN_P * this->mass * this->d * this->thetaDot * this->sHat3_P); // - Calculate torque that the HRB is applying about point B - this->torqueOnBodyPntB_B = -((this->IPntS_S(1,1)*this->sHat2_P + this->mass*this->d*this->rTilde_SP_P*this->sHat3_P) - *thetaDDotLocal + (this->ISPrimePntS_P - this->mass*(this->rPrimeTilde_SP_P*rTilde_SP_P - + this->rTilde_SP_P*this->rPrimeTilde_SP_P))*omegaLocal_PN_P + this->thetaDot - *omegaLocalTilde_PN_P*(this->IPntS_S(1,1)*this->sHat2_P + this->mass*this->d - *this->rTilde_SP_P*this->sHat3_P) + this->mass*this->d*this->thetaDot*this->thetaDot - *this->rTilde_SP_P*this->sHat1_P); + this->torqueOnBodyPntB_B = + -((this->IPntS_S(1, 1) * this->sHat2_P + this->mass * this->d * this->rTilde_SP_P * this->sHat3_P) * + thetaDDotLocal + + (this->ISPrimePntS_P - + this->mass * (this->rPrimeTilde_SP_P * rTilde_SP_P + this->rTilde_SP_P * this->rPrimeTilde_SP_P)) * + omegaLocal_PN_P + + this->thetaDot * omegaLocalTilde_PN_P * + (this->IPntS_S(1, 1) * this->sHat2_P + this->mass * this->d * this->rTilde_SP_P * this->sHat3_P) + + this->mass * this->d * this->thetaDot * this->thetaDot * this->rTilde_SP_P * this->sHat1_P); // - Define values needed to get the torque about point C Eigen::Vector3d cLocal_B = *this->c_B; @@ -352,12 +382,13 @@ void HingedRigidBodyStateEffector::calcForceTorqueOnBody(double integTime, Eigen Eigen::Matrix3d rPrimeTilde_SC_B = eigenTilde(rPrime_SC_B); // - Calculate the torque about point C - this->torqueOnBodyPntC_B = -((this->IPntS_S(1,1)*this->sHat2_P + this->mass*this->d*rTilde_SC_B*this->sHat3_P) - *thetaDDotLocal + (this->ISPrimePntS_P - this->mass*(rPrimeTilde_SC_B*rTilde_SC_B - + rTilde_SC_B*rPrimeTilde_SC_B))*omegaLocal_PN_P + this->thetaDot - *omegaLocalTilde_PN_P*(this->IPntS_S(1,1)*this->sHat2_P + this->mass*this->d - *rTilde_SC_B*this->sHat3_P) + this->mass*this->d*this->thetaDot*this->thetaDot - *rTilde_SC_B*this->sHat1_P); + this->torqueOnBodyPntC_B = + -((this->IPntS_S(1, 1) * this->sHat2_P + this->mass * this->d * rTilde_SC_B * this->sHat3_P) * thetaDDotLocal + + (this->ISPrimePntS_P - this->mass * (rPrimeTilde_SC_B * rTilde_SC_B + rTilde_SC_B * rPrimeTilde_SC_B)) * + omegaLocal_PN_P + + this->thetaDot * omegaLocalTilde_PN_P * + (this->IPntS_S(1, 1) * this->sHat2_P + this->mass * this->d * rTilde_SC_B * this->sHat3_P) + + this->mass * this->d * this->thetaDot * this->thetaDot * rTilde_SC_B * this->sHat1_P); return; } @@ -365,29 +396,28 @@ void HingedRigidBodyStateEffector::calcForceTorqueOnBody(double integTime, Eigen /*! This method computes the panel states relative to the inertial frame */ -void HingedRigidBodyStateEffector::computePanelInertialStates() +void +HingedRigidBodyStateEffector::computePanelInertialStates() { // inertial attitude Eigen::MRPd sigmaBN; sigmaBN = this->sigma_BN; - Eigen::Matrix3d dcm_NP = sigmaBN.toRotationMatrix(); // assumes P and B are idential + Eigen::Matrix3d dcm_NP = sigmaBN.toRotationMatrix(); // assumes P and B are idential Eigen::Matrix3d dcm_SN; - dcm_SN = this->dcm_SP*dcm_NP.transpose(); + dcm_SN = this->dcm_SP * dcm_NP.transpose(); this->sigma_SN = eigenMRPd2Vector3d(eigenC2MRP(dcm_SN)); - // inertial angular velocity Eigen::Vector3d omega_BN_B; omega_BN_B = this->omega_BN_B; - this->omega_SN_S = this->dcm_SP * ( omega_BN_B + this->thetaDot*this->sHat2_P); + this->omega_SN_S = this->dcm_SP * (omega_BN_B + this->thetaDot * this->sHat2_P); // inertial position vector this->r_SN_N = (dcm_NP * this->r_SP_P) + (Eigen::Vector3d)(*this->inertialPositionProperty); // inertial velocity vector - this->v_SN_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) - + this->d * this->thetaDot * this->sHat3_P - this->d * (omega_BN_B.cross(this->sHat1_P)) - + omega_BN_B.cross(this->r_HP_P); + this->v_SN_N = (Eigen::Vector3d)(*this->inertialVelocityProperty) + this->d * this->thetaDot * this->sHat3_P - + this->d * (omega_BN_B.cross(this->sHat1_P)) + omega_BN_B.cross(this->r_HP_P); return; } diff --git a/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.h b/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.h old mode 100755 new mode 100644 index e2ca7fb9e8..66c29f31e4 --- a/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.h +++ b/src/simulation/dynamics/HingedRigidBodies/hingedRigidBodyStateEffector.h @@ -34,16 +34,19 @@ #include "architecture/utilities/bskLogging.h" /*! @brief hinged rigid body state effector class */ -class HingedRigidBodyStateEffector : public StateEffector, public SysModel { -public: +class HingedRigidBodyStateEffector + : public StateEffector + , public SysModel +{ + public: double mass; //!< [kg] mass of hinged rigid body double d; //!< [m] distance from hinge point H to hinged rigid body center of mass S double k; //!< [N-m/rad] torsional spring constant of hinge double c; //!< [N-m-s/rad] rotational damping coefficient of hinge double thetaInit; //!< [rad] Initial hinged rigid body angle double thetaDotInit; //!< [rad/s] Initial hinged rigid body angle rate - double thetaRef; //!< [rad] hinged rigid body reference angle - double thetaDotRef; //!< [rad/s] hinged rigid body reference angle rate + double thetaRef; //!< [rad] hinged rigid body reference angle + double thetaDotRef; //!< [rad/s] hinged rigid body reference angle rate std::string nameOfThetaState; //!< -- Identifier for the theta state data container std::string nameOfThetaDotState; //!< -- Identifier for the thetaDot state data container Eigen::Matrix3d IPntS_S; //!< [kg-m^2] Inertia of hinged rigid body about point S in S frame components @@ -51,67 +54,79 @@ class HingedRigidBodyStateEffector : public StateEffector, public SysModel { Eigen::Matrix3d dcm_HB; //!< -- DCM from body frame to hinge frame Message hingedRigidBodyOutMsg; //!< -- state output message name ReadFunctor motorTorqueInMsg; //!< -- (optional) motor torque input message name - ReadFunctor hingedRigidBodyRefMsg; //!< -- (optional) rigid body reference input message name + ReadFunctor + hingedRigidBodyRefMsg; //!< -- (optional) rigid body reference input message name Message hingedRigidBodyConfigLogOutMsg; //!< panel state config log message name - HingedRigidBodyMsgPayload HRBoutputStates; //!< instance of messaging system message struct - BSKLogger bskLogger; //!< -- BSK Logging - -private: - static uint64_t effectorID; //!< [] ID number of this panel - double theta; //!< [rad] hinged rigid body angle - double thetaDot; //!< [rad/s] hinged rigid body angle rate - double cTheta; //!< -- term needed for back substitution - double u; //!< [N-m] optional motor torque - Eigen::Vector3d r_HP_P; //!< [m] vector pointing from primary body frame P origin to Hinge location. If a single spacecraft body is modeled than P is the same as B - Eigen::Matrix3d dcm_HP; //!< -- DCM from primary body frame to hinge frame - Eigen::Vector3d aTheta; //!< -- term needed for back substitution - Eigen::Vector3d bTheta; //!< -- term needed for back substitution - Eigen::Matrix3d rTilde_HP_P; //!< -- Tilde matrix of rHB_B - Eigen::Matrix3d dcm_SH; //!< -- DCM from hinge to hinged rigid body frame, S - Eigen::Matrix3d dcm_SP; //!< -- DCM from body to S frame - Eigen::Vector3d omega_PN_S; //!< [rad/s] omega_BN in S frame components - Eigen::Vector3d sHat1_P; //!< -- unit direction vector for the first axis of the S frame - Eigen::Vector3d sHat2_P; //!< -- unit direction vector for the second axis of the S frame - Eigen::Vector3d sHat3_P; //!< -- unit direction vector for the third axis of the S frame - Eigen::Vector3d r_SP_P; //!< -- Vector pointing from B to CoM of hinged rigid body in B frame components - Eigen::Matrix3d rTilde_SP_P; //!< -- Tilde matrix of rSB_B - Eigen::Vector3d rPrime_SP_P; //!< [m/s] Body time derivative of rSB_B - Eigen::Matrix3d rPrimeTilde_SP_P; //!< -- Tilde matrix of rPrime_SB_B - Eigen::Matrix3d ISPrimePntS_P; //!< [kg-m^2/s] time body derivative IPntS in body frame components - Eigen::Vector3d omegaLoc_PN_P; //!< [rad/s] local copy of omegaBN + HingedRigidBodyMsgPayload HRBoutputStates; //!< instance of messaging system message struct + BSKLogger bskLogger; //!< -- BSK Logging + + private: + static uint64_t effectorID; //!< [] ID number of this panel + double theta; //!< [rad] hinged rigid body angle + double thetaDot; //!< [rad/s] hinged rigid body angle rate + double cTheta; //!< -- term needed for back substitution + double u; //!< [N-m] optional motor torque + Eigen::Vector3d r_HP_P; //!< [m] vector pointing from primary body frame P origin to Hinge location. If a single + //!< spacecraft body is modeled than P is the same as B + Eigen::Matrix3d dcm_HP; //!< -- DCM from primary body frame to hinge frame + Eigen::Vector3d aTheta; //!< -- term needed for back substitution + Eigen::Vector3d bTheta; //!< -- term needed for back substitution + Eigen::Matrix3d rTilde_HP_P; //!< -- Tilde matrix of rHB_B + Eigen::Matrix3d dcm_SH; //!< -- DCM from hinge to hinged rigid body frame, S + Eigen::Matrix3d dcm_SP; //!< -- DCM from body to S frame + Eigen::Vector3d omega_PN_S; //!< [rad/s] omega_BN in S frame components + Eigen::Vector3d sHat1_P; //!< -- unit direction vector for the first axis of the S frame + Eigen::Vector3d sHat2_P; //!< -- unit direction vector for the second axis of the S frame + Eigen::Vector3d sHat3_P; //!< -- unit direction vector for the third axis of the S frame + Eigen::Vector3d r_SP_P; //!< -- Vector pointing from B to CoM of hinged rigid body in B frame components + Eigen::Matrix3d rTilde_SP_P; //!< -- Tilde matrix of rSB_B + Eigen::Vector3d rPrime_SP_P; //!< [m/s] Body time derivative of rSB_B + Eigen::Matrix3d rPrimeTilde_SP_P; //!< -- Tilde matrix of rPrime_SB_B + Eigen::Matrix3d ISPrimePntS_P; //!< [kg-m^2/s] time body derivative IPntS in body frame components + Eigen::Vector3d omegaLoc_PN_P; //!< [rad/s] local copy of omegaBN Eigen::Matrix3d omegaTildeLoc_PN_P; //!< -- tilde matrix of omegaBN - Eigen::Vector3d r_SN_N; //!< [m] position vector of hinge CM S relative to inertial frame - Eigen::Vector3d v_SN_N; //!< [m/s] inertial velocity vector of S relative to inertial frame - Eigen::Vector3d sigma_SN; //!< -- MRP attitude of panel frame S relative to inertial frame - Eigen::Vector3d omega_SN_S; //!< [rad/s] inertial panel frame angular velocity vector - Eigen::MRPd sigma_BN{0.0, 0.0, 0.0}; //!< Hub/Inertial attitude represented by MRP - Eigen::Vector3d omega_BN_B{0.0, 0.0, 0.0}; //!< Hub/Inertial angular velocity vector in B frame components - StateData *thetaState; //!< -- state manager of theta for hinged rigid body - Eigen::MatrixXd *inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - Eigen::MatrixXd *inertialVelocityProperty; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase - - StateData *thetaDotState; //!< -- state manager of thetaDot for hinged rigid body - Eigen::MatrixXd *c_B; //!< [m] Vector from point B to CoM of s/c in B frame components - - Eigen::MatrixXd *cPrime_B; //!< [m/s] Body time derivative of vector c_B in B frame components - -public: + Eigen::Vector3d r_SN_N; //!< [m] position vector of hinge CM S relative to inertial frame + Eigen::Vector3d v_SN_N; //!< [m/s] inertial velocity vector of S relative to inertial frame + Eigen::Vector3d sigma_SN; //!< -- MRP attitude of panel frame S relative to inertial frame + Eigen::Vector3d omega_SN_S; //!< [rad/s] inertial panel frame angular velocity vector + Eigen::MRPd sigma_BN{ 0.0, 0.0, 0.0 }; //!< Hub/Inertial attitude represented by MRP + Eigen::Vector3d omega_BN_B{ 0.0, 0.0, 0.0 }; //!< Hub/Inertial angular velocity vector in B frame components + StateData* thetaState; //!< -- state manager of theta for hinged rigid body + Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialVelocityProperty; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase + + StateData* thetaDotState; //!< -- state manager of thetaDot for hinged rigid body + Eigen::MatrixXd* c_B; //!< [m] Vector from point B to CoM of s/c in B frame components + + Eigen::MatrixXd* cPrime_B; //!< [m/s] Body time derivative of vector c_B in B frame components + + public: HingedRigidBodyStateEffector(); //!< -- Contructor - ~HingedRigidBodyStateEffector(); //!< -- Destructor + ~HingedRigidBodyStateEffector(); //!< -- Destructor void writeOutputStateMessages(uint64_t CurrentClock); - void UpdateState(uint64_t CurrentSimNanos); - void registerStates(DynParamManager& statesIn); //!< -- Method for registering the HRB states - void linkInStates(DynParamManager& states); //!< -- Method for getting access to other states - void updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N); //!< -- Method for back-sub contributions - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN); //!< -- Method for HRB to compute its derivatives - void updateEffectorMassProps(double integTime); //!< -- Method for giving the s/c the HRB mass props and prop rates - void updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, double & rotEnergyContr, Eigen::Vector3d omega_BN_B); //!< -- Computing energy and momentum for HRBs - void calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B); //!< -- Force and torque on s/c due to HRBs - void prependSpacecraftNameToStates(); //!< class method - -private: + void UpdateState(uint64_t CurrentSimNanos); + void registerStates(DynParamManager& statesIn); //!< -- Method for registering the HRB states + void linkInStates(DynParamManager& states); //!< -- Method for getting access to other states + void updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N); //!< -- Method for back-sub contributions + void computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN); //!< -- Method for HRB to compute its derivatives + void updateEffectorMassProps(double integTime); //!< -- Method for giving the s/c the HRB mass props and prop rates + void updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B); //!< -- Computing energy and momentum for HRBs + void calcForceTorqueOnBody(double integTime, + Eigen::Vector3d omega_BN_B); //!< -- Force and torque on s/c due to HRBs + void prependSpacecraftNameToStates(); //!< class method + + private: void computePanelInertialStates(); }; - #endif /* STATE_EFFECTOR_H */ 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/_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/Integrators/svIntegratorEuler.cpp b/src/simulation/dynamics/Integrators/svIntegratorEuler.cpp index 03a6150821..81e247a908 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorEuler.cpp +++ b/src/simulation/dynamics/Integrators/svIntegratorEuler.cpp @@ -20,15 +20,16 @@ #include "svIntegratorEuler.h" svIntegratorEuler::svIntegratorEuler(DynamicObject* dyn) - : svIntegratorRungeKutta(dyn, svIntegratorEuler::getCoefficients()) + : svIntegratorRungeKutta(dyn, svIntegratorEuler::getCoefficients()) { } -RKCoefficients<1> svIntegratorEuler::getCoefficients() +RKCoefficients<1> +svIntegratorEuler::getCoefficients() { RKCoefficients<1> coefficients; - coefficients.bArray = {1.}; + coefficients.bArray = { 1. }; return coefficients; } diff --git a/src/simulation/dynamics/Integrators/svIntegratorEuler.h b/src/simulation/dynamics/Integrators/svIntegratorEuler.h index 7f1f4c3a27..6059fe857e 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorEuler.h +++ b/src/simulation/dynamics/Integrators/svIntegratorEuler.h @@ -23,7 +23,8 @@ #include "../_GeneralModuleFiles/svIntegratorRungeKutta.h" /*! @brief Euler integrator */ -class svIntegratorEuler : public svIntegratorRungeKutta<1> { +class svIntegratorEuler : public svIntegratorRungeKutta<1> +{ public: svIntegratorEuler(DynamicObject* dyn); //!< class method private: 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.cpp b/src/simulation/dynamics/Integrators/svIntegratorRK2.cpp index d7f190a79f..676c78d4d5 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorRK2.cpp +++ b/src/simulation/dynamics/Integrators/svIntegratorRK2.cpp @@ -20,19 +20,20 @@ #include "svIntegratorRK2.h" svIntegratorRK2::svIntegratorRK2(DynamicObject* dyn) - : svIntegratorRungeKutta(dyn, svIntegratorRK2::getCoefficients()) + : svIntegratorRungeKutta(dyn, svIntegratorRK2::getCoefficients()) { } -RKCoefficients<2> svIntegratorRK2::getCoefficients() +RKCoefficients<2> +svIntegratorRK2::getCoefficients() { RKCoefficients<2> coefficients; coefficients.aMatrix.at(1).at(0) = 1; coefficients.aMatrix.at(1).at(1) = 1; - coefficients.bArray = {0.5, 0.5}; + coefficients.bArray = { 0.5, 0.5 }; - coefficients.cArray = {0., 1.}; + coefficients.cArray = { 0., 1. }; return coefficients; } diff --git a/src/simulation/dynamics/Integrators/svIntegratorRK2.h b/src/simulation/dynamics/Integrators/svIntegratorRK2.h index dbab78dad2..b819cf54e2 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorRK2.h +++ b/src/simulation/dynamics/Integrators/svIntegratorRK2.h @@ -23,7 +23,8 @@ #include "../_GeneralModuleFiles/svIntegratorRungeKutta.h" /*! @brief 2nd order Runge-Kutta integrator */ -class svIntegratorRK2 : public svIntegratorRungeKutta<2> { +class svIntegratorRK2 : public svIntegratorRungeKutta<2> +{ public: svIntegratorRK2(DynamicObject* dyn); //!< class method private: 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.cpp b/src/simulation/dynamics/Integrators/svIntegratorRKF45.cpp index 15ecb87dc9..fde9f2872d 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorRKF45.cpp +++ b/src/simulation/dynamics/Integrators/svIntegratorRKF45.cpp @@ -20,11 +20,12 @@ #include "svIntegratorRKF45.h" svIntegratorRKF45::svIntegratorRKF45(DynamicObject* dyn) - : svIntegratorAdaptiveRungeKutta(dyn, svIntegratorRKF45::getCoefficients(), 5.) + : svIntegratorAdaptiveRungeKutta(dyn, svIntegratorRKF45::getCoefficients(), 5.) { } -RKAdaptiveCoefficients<6> svIntegratorRKF45::getCoefficients() +RKAdaptiveCoefficients<6> +svIntegratorRKF45::getCoefficients() { RKAdaptiveCoefficients<6> coefficients; diff --git a/src/simulation/dynamics/Integrators/svIntegratorRKF45.h b/src/simulation/dynamics/Integrators/svIntegratorRKF45.h index 9640cb761c..c0792a0e1f 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorRKF45.h +++ b/src/simulation/dynamics/Integrators/svIntegratorRKF45.h @@ -23,7 +23,8 @@ #include "../_GeneralModuleFiles/svIntegratorAdaptiveRungeKutta.h" /*! @brief 4th order Runge-Kutta-Fehlberg variable time step integrator */ -class svIntegratorRKF45 : public svIntegratorAdaptiveRungeKutta<6> { +class svIntegratorRKF45 : public svIntegratorAdaptiveRungeKutta<6> +{ public: svIntegratorRKF45(DynamicObject* dyn); //!< class method private: 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.cpp b/src/simulation/dynamics/Integrators/svIntegratorRKF78.cpp index d32a0bfc50..37cd7ca6b0 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorRKF78.cpp +++ b/src/simulation/dynamics/Integrators/svIntegratorRKF78.cpp @@ -20,11 +20,12 @@ #include "svIntegratorRKF78.h" svIntegratorRKF78::svIntegratorRKF78(DynamicObject* dyn) - : svIntegratorAdaptiveRungeKutta(dyn, svIntegratorRKF78::getCoefficients(), 8.) + : svIntegratorAdaptiveRungeKutta(dyn, svIntegratorRKF78::getCoefficients(), 8.) { } -RKAdaptiveCoefficients<13> svIntegratorRKF78::getCoefficients() +RKAdaptiveCoefficients<13> +svIntegratorRKF78::getCoefficients() { RKAdaptiveCoefficients<13> coefficients; diff --git a/src/simulation/dynamics/Integrators/svIntegratorRKF78.h b/src/simulation/dynamics/Integrators/svIntegratorRKF78.h index ed9e1ffcad..f88d3ebbd4 100644 --- a/src/simulation/dynamics/Integrators/svIntegratorRKF78.h +++ b/src/simulation/dynamics/Integrators/svIntegratorRKF78.h @@ -23,7 +23,8 @@ #include "../_GeneralModuleFiles/svIntegratorAdaptiveRungeKutta.h" /*! @brief 7/8 order Runge-Kutta integrator */ -class svIntegratorRKF78 : public svIntegratorAdaptiveRungeKutta<13> { +class svIntegratorRKF78 : public svIntegratorAdaptiveRungeKutta<13> +{ public: svIntegratorRKF78(DynamicObject* dyn); //!< class method private: 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/_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/LinearSpringMassDamper/linearSpringMassDamper.cpp b/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.cpp index 6bb903f906..97a55e4eb3 100644 --- a/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.cpp +++ b/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.cpp @@ -23,28 +23,28 @@ /*! This is the constructor, setting variables to default values */ LinearSpringMassDamper::LinearSpringMassDamper() { - // - zero the contributions for mass props and mass rates - this->effProps.mEff = 0.0; - this->effProps.IEffPntB_B.setZero(); - this->effProps.rEff_CB_B.setZero(); - this->effProps.rEffPrime_CB_B.setZero(); - this->effProps.IEffPrimePntB_B.setZero(); - - // - Initialize the variables to working values - this->massSMD = 1.0; - this->r_PB_B.setZero(); - this->pHat_B.setIdentity(); - this->k = 1.0; - this->c = 0.0; + // - zero the contributions for mass props and mass rates + this->effProps.mEff = 0.0; + this->effProps.IEffPntB_B.setZero(); + this->effProps.rEff_CB_B.setZero(); + this->effProps.rEffPrime_CB_B.setZero(); + this->effProps.IEffPrimePntB_B.setZero(); + + // - Initialize the variables to working values + this->massSMD = 1.0; + this->r_PB_B.setZero(); + this->pHat_B.setIdentity(); + this->k = 1.0; + this->c = 0.0; this->rhoInit = 0.0; this->rhoDotInit = 0.0; this->massInit = 0.0; - this->nameOfRhoState = "linearSpringMassDamperRho" + std::to_string(this->effectorID); - this->nameOfRhoDotState = "linearSpringMassDamperRhoDot" + std::to_string(this->effectorID); - this->nameOfMassState = "linearSpringMassDamperMass" + std::to_string(this->effectorID); + this->nameOfRhoState = "linearSpringMassDamperRho" + std::to_string(this->effectorID); + this->nameOfRhoDotState = "linearSpringMassDamperRhoDot" + std::to_string(this->effectorID); + this->nameOfMassState = "linearSpringMassDamperMass" + std::to_string(this->effectorID); this->effectorID++; - return; + return; } uint64_t LinearSpringMassDamper::effectorID = 1; @@ -52,12 +52,13 @@ uint64_t LinearSpringMassDamper::effectorID = 1; /*! This is the destructor, nothing to report here */ LinearSpringMassDamper::~LinearSpringMassDamper() { - this->effectorID = 1; /* reset the panel ID*/ + this->effectorID = 1; /* reset the panel ID*/ return; } /*! Method for spring mass damper particle to access the states that it needs. It needs gravity and the hub states */ -void LinearSpringMassDamper::linkInStates(DynParamManager& statesIn) +void +LinearSpringMassDamper::linkInStates(DynParamManager& statesIn) { // - Grab access to gravity this->g_N = statesIn.getPropertyReference(this->propName_vehicleGravity); @@ -70,58 +71,61 @@ void LinearSpringMassDamper::linkInStates(DynParamManager& statesIn) } /*! This is the method for the spring mass damper particle to register its states: rho and rhoDot */ -void LinearSpringMassDamper::registerStates(DynParamManager& states) +void +LinearSpringMassDamper::registerStates(DynParamManager& states) { // - Register rho and rhoDot - this->rhoState = states.registerState(1, 1, nameOfRhoState); - Eigen::MatrixXd rhoInitMatrix(1,1); - rhoInitMatrix(0,0) = this->rhoInit; + this->rhoState = states.registerState(1, 1, nameOfRhoState); + Eigen::MatrixXd rhoInitMatrix(1, 1); + rhoInitMatrix(0, 0) = this->rhoInit; this->rhoState->setState(rhoInitMatrix); - this->rhoDotState = states.registerState(1, 1, nameOfRhoDotState); - Eigen::MatrixXd rhoDotInitMatrix(1,1); - rhoDotInitMatrix(0,0) = this->rhoDotInit; + this->rhoDotState = states.registerState(1, 1, nameOfRhoDotState); + Eigen::MatrixXd rhoDotInitMatrix(1, 1); + rhoDotInitMatrix(0, 0) = this->rhoDotInit; this->rhoDotState->setState(rhoDotInitMatrix); - // - Register mass - this->massState = states.registerState(1, 1, nameOfMassState); - Eigen::MatrixXd massInitMatrix(1,1); - massInitMatrix(0,0) = this->massInit; + // - Register mass + this->massState = states.registerState(1, 1, nameOfMassState); + Eigen::MatrixXd massInitMatrix(1, 1); + massInitMatrix(0, 0) = this->massInit; this->massState->setState(massInitMatrix); - return; + return; } /*! This is the method for the SMD to add its contributions to the mass props and mass prop rates of the vehicle */ -void LinearSpringMassDamper::updateEffectorMassProps(double integTime) +void +LinearSpringMassDamper::updateEffectorMassProps(double integTime) { - // - Grab rho from state manager and define r_PcB_B - this->rho = this->rhoState->getState()(0,0); - this->r_PcB_B = this->rho * this->pHat_B + this->r_PB_B; - this->massSMD = this->massState->getState()(0, 0); - - // - Update the effectors mass - this->effProps.mEff = this->massSMD; - // - Update the position of CoM - this->effProps.rEff_CB_B = this->r_PcB_B; - // - Update the inertia about B - this->rTilde_PcB_B = eigenTilde(this->r_PcB_B); - this->effProps.IEffPntB_B = this->massSMD * this->rTilde_PcB_B * this->rTilde_PcB_B.transpose(); - - // - Grab rhoDot from the stateManager and define rPrime_PcB_B - this->rhoDot = this->rhoDotState->getState()(0, 0); - this->rPrime_PcB_B = this->rhoDot * this->pHat_B; - this->effProps.rEffPrime_CB_B = this->rPrime_PcB_B; - - // - Update the body time derivative of inertia about B - this->rPrimeTilde_PcB_B = eigenTilde(this->rPrime_PcB_B); - this->effProps.IEffPrimePntB_B = -this->massSMD*(this->rPrimeTilde_PcB_B*this->rTilde_PcB_B - + this->rTilde_PcB_B*this->rPrimeTilde_PcB_B); + // - Grab rho from state manager and define r_PcB_B + this->rho = this->rhoState->getState()(0, 0); + this->r_PcB_B = this->rho * this->pHat_B + this->r_PB_B; + this->massSMD = this->massState->getState()(0, 0); + + // - Update the effectors mass + this->effProps.mEff = this->massSMD; + // - Update the position of CoM + this->effProps.rEff_CB_B = this->r_PcB_B; + // - Update the inertia about B + this->rTilde_PcB_B = eigenTilde(this->r_PcB_B); + this->effProps.IEffPntB_B = this->massSMD * this->rTilde_PcB_B * this->rTilde_PcB_B.transpose(); + + // - Grab rhoDot from the stateManager and define rPrime_PcB_B + this->rhoDot = this->rhoDotState->getState()(0, 0); + this->rPrime_PcB_B = this->rhoDot * this->pHat_B; + this->effProps.rEffPrime_CB_B = this->rPrime_PcB_B; + + // - Update the body time derivative of inertia about B + this->rPrimeTilde_PcB_B = eigenTilde(this->rPrime_PcB_B); + this->effProps.IEffPrimePntB_B = + -this->massSMD * (this->rPrimeTilde_PcB_B * this->rTilde_PcB_B + this->rTilde_PcB_B * this->rPrimeTilde_PcB_B); return; } /*! This is method is used to pass mass properties information to the fuelTank */ -void LinearSpringMassDamper::retrieveMassValue(double integTime) +void +LinearSpringMassDamper::retrieveMassValue(double integTime) { // Save mass value into the fuelSlosh class variable this->fuelMass = this->massSMD; @@ -130,13 +134,18 @@ void LinearSpringMassDamper::retrieveMassValue(double integTime) } /*! This method is for the SMD to add its contributions to the back-sub method */ -void LinearSpringMassDamper::updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) +void +LinearSpringMassDamper::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { // - Find dcm_BN Eigen::MRPd sigmaLocal_BN; Eigen::Matrix3d dcm_BN; Eigen::Matrix3d dcm_NB; - sigmaLocal_BN = (Eigen::Vector3d ) sigma_BN; + sigmaLocal_BN = (Eigen::Vector3d)sigma_BN; dcm_NB = sigmaLocal_BN.toRotationMatrix(); dcm_BN = dcm_NB.transpose(); @@ -144,65 +153,73 @@ void LinearSpringMassDamper::updateContributions(double integTime, BackSubMatric Eigen::Vector3d gLocal_N; Eigen::Vector3d g_B; gLocal_N = *this->g_N; - g_B = dcm_BN*gLocal_N; + g_B = dcm_BN * gLocal_N; - // - Define aRho + // - Define aRho this->aRho = -this->pHat_B; // - Define bRho - this->bRho = -this->rTilde_PcB_B*this->pHat_B; + this->bRho = -this->rTilde_PcB_B * this->pHat_B; // - Define cRho Eigen::Vector3d omega_BN_B_local = omega_BN_B; Eigen::Matrix3d omegaTilde_BN_B_local; omegaTilde_BN_B_local = eigenTilde(omega_BN_B_local); - cRho = 1.0/(this->massSMD)*(this->pHat_B.dot(this->massSMD * g_B) - this->k*this->rho - this->c*this->rhoDot - - 2 * this->massSMD*this->pHat_B.dot(omegaTilde_BN_B_local * this->rPrime_PcB_B) - - this->massSMD*this->pHat_B.dot(omegaTilde_BN_B_local*omegaTilde_BN_B_local*this->r_PcB_B)); - - // - Compute matrix/vector contributions - backSubContr.matrixA = this->massSMD*this->pHat_B*this->aRho.transpose(); - backSubContr.matrixB = this->massSMD*this->pHat_B*this->bRho.transpose(); - backSubContr.matrixC = this->massSMD*this->rTilde_PcB_B*this->pHat_B*this->aRho.transpose(); - backSubContr.matrixD = this->massSMD*this->rTilde_PcB_B*this->pHat_B*this->bRho.transpose(); - backSubContr.vecTrans = -this->massSMD*this->cRho*this->pHat_B; - backSubContr.vecRot = -this->massSMD*omegaTilde_BN_B_local * this->rTilde_PcB_B *this->rPrime_PcB_B - - this->massSMD*this->cRho*this->rTilde_PcB_B * this->pHat_B; + cRho = 1.0 / (this->massSMD) * + (this->pHat_B.dot(this->massSMD * g_B) - this->k * this->rho - this->c * this->rhoDot - + 2 * this->massSMD * this->pHat_B.dot(omegaTilde_BN_B_local * this->rPrime_PcB_B) - + this->massSMD * this->pHat_B.dot(omegaTilde_BN_B_local * omegaTilde_BN_B_local * this->r_PcB_B)); + + // - Compute matrix/vector contributions + backSubContr.matrixA = this->massSMD * this->pHat_B * this->aRho.transpose(); + backSubContr.matrixB = this->massSMD * this->pHat_B * this->bRho.transpose(); + backSubContr.matrixC = this->massSMD * this->rTilde_PcB_B * this->pHat_B * this->aRho.transpose(); + backSubContr.matrixD = this->massSMD * this->rTilde_PcB_B * this->pHat_B * this->bRho.transpose(); + backSubContr.vecTrans = -this->massSMD * this->cRho * this->pHat_B; + backSubContr.vecRot = -this->massSMD * omegaTilde_BN_B_local * this->rTilde_PcB_B * this->rPrime_PcB_B - + this->massSMD * this->cRho * this->rTilde_PcB_B * this->pHat_B; return; } /*! This method is used to define the derivatives of the SMD. One is the trivial kinematic derivative and the other is derived using the back-sub method */ -void LinearSpringMassDamper::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +LinearSpringMassDamper::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { - // - Find DCM - Eigen::MRPd sigmaLocal_BN; - Eigen::Matrix3d dcm_BN; - sigmaLocal_BN = (Eigen::Vector3d) sigma_BN; - dcm_BN = (sigmaLocal_BN.toRotationMatrix()).transpose(); + // - Find DCM + Eigen::MRPd sigmaLocal_BN; + Eigen::Matrix3d dcm_BN; + sigmaLocal_BN = (Eigen::Vector3d)sigma_BN; + dcm_BN = (sigmaLocal_BN.toRotationMatrix()).transpose(); - // - Set the derivative of rho to rhoDot - this->rhoState->setDerivative(this->rhoDotState->getState()); + // - Set the derivative of rho to rhoDot + this->rhoState->setDerivative(this->rhoDotState->getState()); - // - Compute rhoDDot - Eigen::MatrixXd conv(1,1); + // - Compute rhoDDot + Eigen::MatrixXd conv(1, 1); Eigen::Vector3d omegaDot_BN_B_local = omegaDot_BN_B; Eigen::Vector3d rDDot_BN_N_local = rDDot_BN_N; - Eigen::Vector3d rDDot_BN_B_local = dcm_BN*rDDot_BN_N_local; + Eigen::Vector3d rDDot_BN_B_local = dcm_BN * rDDot_BN_N_local; conv(0, 0) = this->aRho.dot(rDDot_BN_B_local) + this->bRho.dot(omegaDot_BN_B_local) + this->cRho; - this->rhoDotState->setDerivative(conv); + this->rhoDotState->setDerivative(conv); // - Set the massDot already computed from fuelTank to the stateDerivative of mass - conv(0,0) = this->fuelMassDot; + conv(0, 0) = this->fuelMassDot; this->massState->setDerivative(conv); return; } /*! This method is for the SMD to add its contributions to energy and momentum */ -void LinearSpringMassDamper::updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B) +void +LinearSpringMassDamper::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { // - Get variables needed for energy momentum calcs Eigen::Vector3d omegaLocal_BN_B; @@ -211,15 +228,16 @@ void LinearSpringMassDamper::updateEnergyMomContributions(double integTime, Eige // - Find rotational angular momentum contribution from hub rDotPcB_B = this->rPrime_PcB_B + omegaLocal_BN_B.cross(this->r_PcB_B); - rotAngMomPntCContr_B = this->massSMD*this->r_PcB_B.cross(rDotPcB_B); + rotAngMomPntCContr_B = this->massSMD * this->r_PcB_B.cross(rDotPcB_B); // - Find rotational energy contribution from the hub - rotEnergyContr = 1.0/2.0*this->massSMD*rDotPcB_B.dot(rDotPcB_B) + 1.0/2.0*this->k*this->rho*this->rho; + rotEnergyContr = 1.0 / 2.0 * this->massSMD * rDotPcB_B.dot(rDotPcB_B) + 1.0 / 2.0 * this->k * this->rho * this->rho; return; } -void LinearSpringMassDamper::calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B) +void +LinearSpringMassDamper::calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B) { // - Get the current omega state Eigen::Vector3d omegaLocal_BN_B; @@ -232,11 +250,15 @@ void LinearSpringMassDamper::calcForceTorqueOnBody(double integTime, Eigen::Vect rhoDDotLocal = rhoDotState->getStateDeriv()(0, 0); // - Calculate force that the FSP is applying to the spacecraft - this->forceOnBody_B = -(this->massSMD*this->pHat_B*rhoDDotLocal + 2*omegaLocalTilde_BN_B*this->massSMD - *this->rhoDot*this->pHat_B); + this->forceOnBody_B = -(this->massSMD * this->pHat_B * rhoDDotLocal + + 2 * omegaLocalTilde_BN_B * this->massSMD * this->rhoDot * this->pHat_B); // - Calculate torque that the FSP is applying about point B - this->torqueOnBodyPntB_B = -(this->massSMD*this->rTilde_PcB_B*this->pHat_B*rhoDDotLocal + this->massSMD*omegaLocalTilde_BN_B*this->rTilde_PcB_B*this->rPrime_PcB_B - this->massSMD*(this->rPrimeTilde_PcB_B*this->rTilde_PcB_B + this->rTilde_PcB_B*this->rPrimeTilde_PcB_B)*omegaLocal_BN_B); + this->torqueOnBodyPntB_B = + -(this->massSMD * this->rTilde_PcB_B * this->pHat_B * rhoDDotLocal + + this->massSMD * omegaLocalTilde_BN_B * this->rTilde_PcB_B * this->rPrime_PcB_B - + this->massSMD * (this->rPrimeTilde_PcB_B * this->rTilde_PcB_B + this->rTilde_PcB_B * this->rPrimeTilde_PcB_B) * + omegaLocal_BN_B); // - Define values needed to get the torque about point C Eigen::Vector3d cLocal_B = *this->c_B; @@ -247,7 +269,10 @@ void LinearSpringMassDamper::calcForceTorqueOnBody(double integTime, Eigen::Vect Eigen::Matrix3d rPrimeTilde_PcC_B = eigenTilde(rPrime_PcC_B); // - Calculate the torque about point C - this->torqueOnBodyPntC_B = -(this->massSMD*rTilde_PcC_B*this->pHat_B*rhoDDotLocal + this->massSMD*omegaLocalTilde_BN_B*rTilde_PcC_B*rPrime_PcC_B - this->massSMD*(rPrimeTilde_PcC_B*rTilde_PcC_B + rTilde_PcC_B*rPrimeTilde_PcC_B)*omegaLocal_BN_B); + this->torqueOnBodyPntC_B = + -(this->massSMD * rTilde_PcC_B * this->pHat_B * rhoDDotLocal + + this->massSMD * omegaLocalTilde_BN_B * rTilde_PcC_B * rPrime_PcC_B - + this->massSMD * (rPrimeTilde_PcC_B * rTilde_PcC_B + rTilde_PcC_B * rPrimeTilde_PcC_B) * omegaLocal_BN_B); return; } diff --git a/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.h b/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.h old mode 100755 new mode 100644 index d1e89a0975..b40c3e7259 --- a/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.h +++ b/src/simulation/dynamics/LinearSpringMassDamper/linearSpringMassDamper.h @@ -17,7 +17,6 @@ */ - #ifndef LINEAR_SPRING_MASS_DAMPER_H #define LINEAR_SPRING_MASS_DAMPER_H @@ -28,10 +27,12 @@ #include "architecture/utilities/bskLogging.h" /*! @brief linear spring mass damper state effector class */ -class LinearSpringMassDamper : - public StateEffector, public SysModel, public FuelSlosh +class LinearSpringMassDamper + : public StateEffector + , public SysModel + , public FuelSlosh { -public: + public: double k; //!< [N/m] linear spring constant for spring mass damper double c; //!< [N-s/m] linear damping term for spring mass damper double rhoInit; //!< [m] Initial value for spring mass damper particle offset @@ -39,43 +40,53 @@ class LinearSpringMassDamper : double massInit; //!< [m] Initial value for spring mass damper particle mass std::string nameOfRhoState; //!< [-] Identifier for the rho state data container std::string nameOfRhoDotState; //!< [-] Identifier for the rhoDot state data container - std::string nameOfMassState; //!< [-] Identifier for the mass state data container - Eigen::Vector3d r_PB_B; //!< [m] position vector from B point to particle equilibrium, P, in body frame - Eigen::Vector3d pHat_B; //!< [-] particle direction unit vector, in body frame - StateData *massState; //!< -- state data for the particles mass - BSKLogger bskLogger; //!< -- BSK Logging + std::string nameOfMassState; //!< [-] Identifier for the mass state data container + Eigen::Vector3d r_PB_B; //!< [m] position vector from B point to particle equilibrium, P, in body frame + Eigen::Vector3d pHat_B; //!< [-] particle direction unit vector, in body frame + StateData* massState; //!< -- state data for the particles mass + BSKLogger bskLogger; //!< -- BSK Logging -private: - double cRho; //!< -- Term needed for back-sub method - double rho; //!< [m] spring mass damper displacement from equilibrium - double rhoDot; //!< [m/s] time derivative of displacement from equilibrium - double massSMD; //!< [kg] mass of spring mass damper particle - Eigen::Vector3d r_PcB_B; //!< [m] position vector form B to center of mass location of particle - Eigen::Matrix3d rTilde_PcB_B; //!< [m] tilde matrix of r_Pc_B - Eigen::Vector3d rPrime_PcB_B; //!< [m/s] Body time derivative of r_Pc_B - Eigen::Matrix3d rPrimeTilde_PcB_B; //!< [m/s] Tilde matrix of rPrime_PcB_B - Eigen::Vector3d aRho; //!< -- Term needed for back-sub method - Eigen::Vector3d bRho; //!< -- Term needed for back-sub method - Eigen::MatrixXd *g_N; //!< [m/s^2] Gravitational acceleration in N frame components - StateData *rhoState; //!< -- state data for spring mass damper displacement from equilibrium - Eigen::MatrixXd *c_B; //!< [m] Vector from point B to CoM of s/c in B frame components - Eigen::MatrixXd *cPrime_B; //!< [m/s] Body time derivative of vector c_B in B frame components - StateData *rhoDotState; //!< -- state data for time derivative of rho; - static uint64_t effectorID; //!< [] ID number of this panel + private: + double cRho; //!< -- Term needed for back-sub method + double rho; //!< [m] spring mass damper displacement from equilibrium + double rhoDot; //!< [m/s] time derivative of displacement from equilibrium + double massSMD; //!< [kg] mass of spring mass damper particle + Eigen::Vector3d r_PcB_B; //!< [m] position vector form B to center of mass location of particle + Eigen::Matrix3d rTilde_PcB_B; //!< [m] tilde matrix of r_Pc_B + Eigen::Vector3d rPrime_PcB_B; //!< [m/s] Body time derivative of r_Pc_B + Eigen::Matrix3d rPrimeTilde_PcB_B; //!< [m/s] Tilde matrix of rPrime_PcB_B + Eigen::Vector3d aRho; //!< -- Term needed for back-sub method + Eigen::Vector3d bRho; //!< -- Term needed for back-sub method + Eigen::MatrixXd* g_N; //!< [m/s^2] Gravitational acceleration in N frame components + StateData* rhoState; //!< -- state data for spring mass damper displacement from equilibrium + Eigen::MatrixXd* c_B; //!< [m] Vector from point B to CoM of s/c in B frame components + Eigen::MatrixXd* cPrime_B; //!< [m/s] Body time derivative of vector c_B in B frame components + StateData* rhoDotState; //!< -- state data for time derivative of rho; + static uint64_t effectorID; //!< [] ID number of this panel -public: - LinearSpringMassDamper(); //!< -- Contructor - ~LinearSpringMassDamper(); //!< -- Destructor - void registerStates(DynParamManager& states); //!< -- Method for SMD to register its states - void linkInStates(DynParamManager& states); //!< -- Method for SMD to get access of other states + public: + LinearSpringMassDamper(); //!< -- Contructor + ~LinearSpringMassDamper(); //!< -- Destructor + void registerStates(DynParamManager& states); //!< -- Method for SMD to register its states + void linkInStates(DynParamManager& states); //!< -- Method for SMD to get access of other states void retrieveMassValue(double integTime); - void calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B); //!< -- Force and torque on s/c due to linear spring mass damper - void updateEffectorMassProps(double integTime); //!< -- Method for stateEffector to give mass contributions - void updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N); //!< -- Back-sub contributions - void updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B); //!< -- Energy and momentum calculations - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN); //!< -- Method for each stateEffector to calculate derivatives + void calcForceTorqueOnBody( + double integTime, + Eigen::Vector3d omega_BN_B); //!< -- Force and torque on s/c due to linear spring mass damper + void updateEffectorMassProps(double integTime); //!< -- Method for stateEffector to give mass contributions + void updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N); //!< -- Back-sub contributions + void updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B); //!< -- Energy and momentum calculations + void computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN); //!< -- Method for each stateEffector to calculate derivatives }; - #endif /* LINEAR_SPRING_MASS_DAMPER_H */ 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.cpp b/src/simulation/dynamics/MtbEffector/MtbEffector.cpp index c813350021..9cdfec09dd 100644 --- a/src/simulation/dynamics/MtbEffector/MtbEffector.cpp +++ b/src/simulation/dynamics/MtbEffector/MtbEffector.cpp @@ -17,28 +17,23 @@ */ - #include "simulation/dynamics/MtbEffector/MtbEffector.h" #include "architecture/utilities/avsEigenMRP.h" #include "architecture/utilities/avsEigenSupport.h" #include "architecture/utilities/linearAlgebra.h" - /*! This is the constructor for the module class. It sets default variable values and initializes the various parts of the model */ -MtbEffector::MtbEffector() -{ -} +MtbEffector::MtbEffector() {} /*! Module Destructor */ -MtbEffector::~MtbEffector() -{ -} +MtbEffector::~MtbEffector() {} /*! This method is used to reset the module and checks that required input messages are connect. */ -void MtbEffector::Reset(uint64_t CurrentSimNanos) +void +MtbEffector::Reset(uint64_t CurrentSimNanos) { /* * Check that required input messages are connected. @@ -66,7 +61,8 @@ void MtbEffector::Reset(uint64_t CurrentSimNanos) /*! This is the main method that gets called every time the module is updated. Provide an appropriate description. */ -void MtbEffector::UpdateState(uint64_t CurrentSimNanos) +void +MtbEffector::UpdateState(uint64_t CurrentSimNanos) { /* * Write to the output message. @@ -76,11 +72,11 @@ void MtbEffector::UpdateState(uint64_t CurrentSimNanos) return; } - /*! This method is used to link the magnetic torque bar effector to the hub attitude. */ -void MtbEffector::linkInStates(DynParamManager& states) +void +MtbEffector::linkInStates(DynParamManager& states) { /* * Link the Body relative to Inertial frame modified modriguez parameter. @@ -93,7 +89,8 @@ void MtbEffector::linkInStates(DynParamManager& states) /*! This method computes the body torque contribution from all magnetic torque bars. */ -void MtbEffector::computeForceTorque(double integTime, double timeStep) +void +MtbEffector::computeForceTorque(double integTime, double timeStep) { /* * Create local variables. @@ -119,7 +116,6 @@ void MtbEffector::computeForceTorque(double integTime, double timeStep) */ this->torqueExternalPntB_B.setZero(); - /* * Construct bTilde matrix. */ @@ -134,13 +130,13 @@ void MtbEffector::computeForceTorque(double integTime, double timeStep) * Since cArray2EigenMatrixXd expects a column major input, we need to * transpose GtMatrix_B. */ - double GtColMajor[3*MAX_EFF_CNT]; + double GtColMajor[3 * MAX_EFF_CNT]; mSetZero(GtColMajor, 3, this->mtbConfigParams.numMTB); mTranspose(this->mtbConfigParams.GtMatrix_B, 3, this->mtbConfigParams.numMTB, GtColMajor); GtMatrix_B = cArray2EigenMatrixXd(GtColMajor, 3, this->mtbConfigParams.numMTB); /* check if dipole commands are saturating the effector */ - for (int i=0; imtbConfigParams.numMTB; i++) { + for (int i = 0; i < this->mtbConfigParams.numMTB; i++) { if (this->mtbCmdInMsgBuffer.mtbDipoleCmds[i] > this->mtbConfigParams.maxMtbDipoles[i]) { this->mtbCmdInMsgBuffer.mtbDipoleCmds[i] = this->mtbConfigParams.maxMtbDipoles[i]; } else if (this->mtbCmdInMsgBuffer.mtbDipoleCmds[i] < -this->mtbConfigParams.maxMtbDipoles[i]) { @@ -149,7 +145,7 @@ void MtbEffector::computeForceTorque(double integTime, double timeStep) } muCmd_T = Eigen::Map(this->mtbCmdInMsgBuffer.mtbDipoleCmds, this->mtbConfigParams.numMTB, 1); - mtbTorque_B = - bTilde * GtMatrix_B * muCmd_T; + mtbTorque_B = -bTilde * GtMatrix_B * muCmd_T; this->torqueExternalPntB_B = mtbTorque_B; return; @@ -158,7 +154,8 @@ void MtbEffector::computeForceTorque(double integTime, double timeStep) /*! Write the magnetic torque bar output message. */ -void MtbEffector::WriteOutputMessages(uint64_t CurrentClock) +void +MtbEffector::WriteOutputMessages(uint64_t CurrentClock) { /* * Initialize output message buffer. diff --git a/src/simulation/dynamics/MtbEffector/MtbEffector.h b/src/simulation/dynamics/MtbEffector/MtbEffector.h index 06345f4022..69639b786e 100644 --- a/src/simulation/dynamics/MtbEffector/MtbEffector.h +++ b/src/simulation/dynamics/MtbEffector/MtbEffector.h @@ -17,7 +17,6 @@ */ - #ifndef MTBEFFECTOR_H #define MTBEFFECTOR_H @@ -32,12 +31,15 @@ #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: +class MtbEffector + : public SysModel + , public DynamicEffector +{ + + public: MtbEffector(); ~MtbEffector(); void Reset(uint64_t CurrentSimNanos); @@ -45,20 +47,21 @@ 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 - ReadFunctor mtbCmdInMsg; //!< input msg for commanded mtb dipole array in the magnetic torque bar frame T - 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 - MTBArrayConfigMsgPayload mtbConfigParams; //!< msg for layout of magnetic torque bars -}; + public: + Message + mtbOutMsg; //!< output message containing net torque produced by the torque bars in Body components + StateData* hubSigma; //!< Hub/Inertial attitude represented by MRP + ReadFunctor + mtbCmdInMsg; //!< input msg for commanded mtb dipole array in the magnetic torque bar frame T + 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 + MTBArrayConfigMsgPayload mtbConfigParams; //!< msg for layout of magnetic torque bars +}; #endif 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..55e5f3b846 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,21 +14,27 @@ # 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 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** @@ -74,22 +79,23 @@ 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) + [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,61 +142,59 @@ 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] 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) + 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,38 +208,47 @@ 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 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,29 +256,26 @@ 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") else: print("Failed: Mtb Effector") return testFailCount, testMessages -if __name__ == "__main__": - test_MtbEffector( - False, - 1e-12, - 0.1 - ) +if __name__ == "__main__": + test_MtbEffector(False, 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/_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/NHingedRigidBodies/nHingedRigidBodyStateEffector.cpp b/src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.cpp old mode 100755 new mode 100644 index 9802f7dde0..8e2736f826 --- a/src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.cpp +++ b/src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.cpp @@ -32,7 +32,7 @@ NHingedRigidBodyStateEffector::NHingedRigidBodyStateEffector() this->effProps.IEffPrimePntB_B.fill(0.0); this->r_HB_B.setZero(); this->dcm_HB.Identity(); - this->nameOfThetaState ="nHingedRigidBody" + std::to_string(this->effectorID) + "Theta"; + this->nameOfThetaState = "nHingedRigidBody" + std::to_string(this->effectorID) + "Theta"; this->nameOfThetaDotState = "nHingedRigidBody" + std::to_string(this->effectorID) + "ThetaDot"; this->effectorID++; @@ -44,14 +44,14 @@ uint64_t NHingedRigidBodyStateEffector::effectorID = 1; /*! This is the destructor, nothing to report here */ NHingedRigidBodyStateEffector::~NHingedRigidBodyStateEffector() { - this->effectorID = 1; /* reset the panel ID*/ + this->effectorID = 1; /* reset the panel ID*/ return; } - /*! This method reads necessary input messages - */ -void NHingedRigidBodyStateEffector::readInputMessages() + */ +void +NHingedRigidBodyStateEffector::readInputMessages() { return; } @@ -60,13 +60,15 @@ void NHingedRigidBodyStateEffector::readInputMessages() @param CurrentClock The current simulation time (used for time stamping) */ -void NHingedRigidBodyStateEffector::WriteOutputMessages(uint64_t CurrentClock) +void +NHingedRigidBodyStateEffector::WriteOutputMessages(uint64_t CurrentClock) { return; } /*! This method allows the HRB state effector to have access to the hub states and gravity*/ -void NHingedRigidBodyStateEffector::linkInStates(DynParamManager& statesIn) +void +NHingedRigidBodyStateEffector::linkInStates(DynParamManager& statesIn) { // - Get access to the hub states this->g_N = statesIn.getPropertyReference(this->propName_vehicleGravity); @@ -75,25 +77,26 @@ void NHingedRigidBodyStateEffector::linkInStates(DynParamManager& statesIn) } /*! This method allows the HRB state effector to register its states: theta and thetaDot with the dyn param manager */ -void NHingedRigidBodyStateEffector::registerStates(DynParamManager& states) +void +NHingedRigidBodyStateEffector::registerStates(DynParamManager& states) { // - Register the states associated with hinged rigid bodies - theta and thetaDot - Eigen::MatrixXd thetaInitMatrix(this->PanelVec.size(),1); - Eigen::MatrixXd thetaDotInitMatrix(this->PanelVec.size(),1); + Eigen::MatrixXd thetaInitMatrix(this->PanelVec.size(), 1); + Eigen::MatrixXd thetaDotInitMatrix(this->PanelVec.size(), 1); std::vector::iterator PanelIt; int it = 0; this->totalMass = 0; - for(PanelIt=this->PanelVec.begin(); PanelIt!=this->PanelVec.end(); PanelIt++){ - thetaInitMatrix(it,0) = PanelIt->thetaInit; - thetaDotInitMatrix(it,0) = PanelIt->thetaDotInit; + for (PanelIt = this->PanelVec.begin(); PanelIt != this->PanelVec.end(); PanelIt++) { + thetaInitMatrix(it, 0) = PanelIt->thetaInit; + thetaDotInitMatrix(it, 0) = PanelIt->thetaDotInit; // - Looping over hinged rigid bodies to find total mass this->totalMass += PanelIt->mass; it += 1; } - this->thetaState = states.registerState((uint32_t) this->PanelVec.size(), 1, this->nameOfThetaState); + this->thetaState = states.registerState((uint32_t)this->PanelVec.size(), 1, this->nameOfThetaState); this->thetaState->setState(thetaInitMatrix); - this->thetaDotState = states.registerState((uint32_t) this->PanelVec.size(), 1, this->nameOfThetaDotState); + this->thetaDotState = states.registerState((uint32_t)this->PanelVec.size(), 1, this->nameOfThetaDotState); this->thetaDotState->setState(thetaDotInitMatrix); return; @@ -101,7 +104,8 @@ void NHingedRigidBodyStateEffector::registerStates(DynParamManager& states) /*! This method allows the HRB state effector to provide its contributions to the mass props and mass prop rates of the spacecraft */ -void NHingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) +void +NHingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) { // - Define summation variables double sum_Theta = 0; @@ -122,7 +126,7 @@ void NHingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) std::vector::iterator PanelIt; int it = 0; - for(PanelIt=this->PanelVec.begin(); PanelIt!=this->PanelVec.end(); PanelIt++){ + for (PanelIt = this->PanelVec.begin(); PanelIt != this->PanelVec.end(); PanelIt++) { // - Find hinged rigid bodies' position with respect to point B // - First need to grab current states @@ -131,47 +135,48 @@ void NHingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) // - Next find the sHat unit vectors sum_Theta += PanelIt->theta; PanelIt->dcm_SS_prev = eigenM2(PanelIt->theta); - PanelIt->dcm_SB = eigenM2(sum_Theta)*this->dcm_HB; + PanelIt->dcm_SB = eigenM2(sum_Theta) * this->dcm_HB; PanelIt->sHat1_B = PanelIt->dcm_SB.row(0); PanelIt->sHat2_B = PanelIt->dcm_SB.row(1); PanelIt->sHat3_B = PanelIt->dcm_SB.row(2); - PanelIt->r_SB_B = this->r_HB_B - PanelIt->d*PanelIt->sHat1_B + sum_rH; - sum_rH += -2*PanelIt->d*PanelIt->sHat1_B; + PanelIt->r_SB_B = this->r_HB_B - PanelIt->d * PanelIt->sHat1_B + sum_rH; + sum_rH += -2 * PanelIt->d * PanelIt->sHat1_B; // - Define rTilde_SB_B PanelIt->rTilde_SB_B = eigenTilde(PanelIt->r_SB_B); // - Find rPrime_SB_B sum_ThetaDot += PanelIt->thetaDot; - PanelIt->rPrime_SB_B = PanelIt->d*(sum_ThetaDot*PanelIt->sHat3_B + sum_rPrimeH); - sum_rPrimeH += 2*PanelIt->sHat3_B*sum_ThetaDot; + PanelIt->rPrime_SB_B = PanelIt->d * (sum_ThetaDot * PanelIt->sHat3_B + sum_rPrimeH); + sum_rPrimeH += 2 * PanelIt->sHat3_B * sum_ThetaDot; - PanelIt->omega_SB_B = sum_ThetaDot*PanelIt->sHat2_B; + PanelIt->omega_SB_B = sum_ThetaDot * PanelIt->sHat2_B; // - Next find the body time derivative of the inertia about point B // - Define tilde matrix of rPrime_SB_B PanelIt->rPrimeTilde_SB_B = eigenTilde(PanelIt->rPrime_SB_B); // - Find body time derivative of IPntS_B - PanelIt->ISPrimePntS_B = sum_ThetaDot*(PanelIt->IPntS_S(2,2) - PanelIt->IPntS_S(0,0)) - *(PanelIt->sHat1_B*PanelIt->sHat3_B.transpose() + PanelIt->sHat3_B*PanelIt->sHat1_B.transpose()); + PanelIt->ISPrimePntS_B = + sum_ThetaDot * (PanelIt->IPntS_S(2, 2) - PanelIt->IPntS_S(0, 0)) * + (PanelIt->sHat1_B * PanelIt->sHat3_B.transpose() + PanelIt->sHat3_B * PanelIt->sHat1_B.transpose()); // - Mass summation sum_mass += PanelIt->mass; // - Inertia of the panels summation term - sum_PanelInertia += PanelIt->dcm_SB.transpose()*PanelIt->IPntS_S*PanelIt->dcm_SB - + PanelIt->mass*PanelIt->rTilde_SB_B*PanelIt->rTilde_SB_B.transpose(); + sum_PanelInertia += PanelIt->dcm_SB.transpose() * PanelIt->IPntS_S * PanelIt->dcm_SB + + PanelIt->mass * PanelIt->rTilde_SB_B * PanelIt->rTilde_SB_B.transpose(); // - COM position summation terms - sum_COM += PanelIt->mass*PanelIt->r_SB_B; + sum_COM += PanelIt->mass * PanelIt->r_SB_B; - sum_COMprime += PanelIt->mass*PanelIt->rPrime_SB_B; + sum_COMprime += PanelIt->mass * PanelIt->rPrime_SB_B; // - Inertia Prime of the effector summation terms - sum_EffInertia += PanelIt->ISPrimePntS_B - PanelIt->mass*(PanelIt->rPrimeTilde_SB_B*PanelIt->rTilde_SB_B - + PanelIt->rTilde_SB_B*PanelIt->rPrimeTilde_SB_B); + sum_EffInertia += PanelIt->ISPrimePntS_B - PanelIt->mass * (PanelIt->rPrimeTilde_SB_B * PanelIt->rTilde_SB_B + + PanelIt->rTilde_SB_B * PanelIt->rPrimeTilde_SB_B); it += 1; } @@ -180,9 +185,9 @@ void NHingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) this->effProps.mEff = sum_mass; // - update effector COM location - this->effProps.rEff_CB_B = 1.0/this->effProps.mEff*sum_COM; + this->effProps.rEff_CB_B = 1.0 / this->effProps.mEff * sum_COM; - this->effProps.rEffPrime_CB_B = 1.0/this->effProps.mEff*sum_COMprime; + this->effProps.rEffPrime_CB_B = 1.0 / this->effProps.mEff * sum_COMprime; // - Find the inertia of the hinged rigid body about point B this->effProps.IEffPntB_B = sum_PanelInertia; @@ -194,23 +199,31 @@ void NHingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) } //!* Method for defining the Heaviside function for the EOMs */ -double NHingedRigidBodyStateEffector::HeaviFunc(double cond) +double +NHingedRigidBodyStateEffector::HeaviFunc(double cond) { double ans; - if (cond < 0.0) ans = 0.0; - else ans = 1.0; + if (cond < 0.0) + ans = 0.0; + else + ans = 1.0; return ans; } /*! This method allows the HRB state effector to give its contributions to the matrices needed for the back-sub method */ -void NHingedRigidBodyStateEffector::updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) +void +NHingedRigidBodyStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { // - Find dcm_BN Eigen::MRPd sigmaLocal_BN; Eigen::Matrix3d dcm_BN; Eigen::Matrix3d dcm_NB; - sigmaLocal_BN = (Eigen::Vector3d )sigma_BN; + sigmaLocal_BN = (Eigen::Vector3d)sigma_BN; dcm_NB = sigmaLocal_BN.toRotationMatrix(); dcm_BN = dcm_NB.transpose(); @@ -218,36 +231,37 @@ void NHingedRigidBodyStateEffector::updateContributions(double integTime, BackSu Eigen::Vector3d gLocal_N; Eigen::Vector3d g_B; gLocal_N = *this->g_N; - g_B = dcm_BN*gLocal_N; + g_B = dcm_BN * gLocal_N; // - Define omega_BN_S this->omegaLoc_BN_B = omega_BN_B; std::vector::iterator PanelIt; - for(PanelIt=this->PanelVec.begin(); PanelIt!=this->PanelVec.end(); PanelIt++){ - PanelIt->omega_BN_S = PanelIt->dcm_SB*this->omegaLoc_BN_B; + for (PanelIt = this->PanelVec.begin(); PanelIt != this->PanelVec.end(); PanelIt++) { + PanelIt->omega_BN_S = PanelIt->dcm_SB * this->omegaLoc_BN_B; } // - Define omegaTildeLoc_BN_B this->omegaTildeLoc_BN_B = eigenTilde(this->omegaLoc_BN_B); // - Define A matrix for the panel equations std::vector::iterator PanelIt2; - this->matrixADHRB.resize((int) this->PanelVec.size(), (int) this->PanelVec.size()); + this->matrixADHRB.resize((int)this->PanelVec.size(), (int)this->PanelVec.size()); this->matrixADHRB.setZero(); std::vector::iterator PanelIt3; int j = 1; - for(PanelIt=this->PanelVec.begin(); PanelIt!=this->PanelVec.end(); PanelIt++){ + for (PanelIt = this->PanelVec.begin(); PanelIt != this->PanelVec.end(); PanelIt++) { int i = 1; - for(PanelIt2=this->PanelVec.begin(); PanelIt2!=this->PanelVec.end(); PanelIt2++){ + for (PanelIt2 = this->PanelVec.begin(); PanelIt2 != this->PanelVec.end(); PanelIt2++) { Eigen::Vector3d sumTerm1; sumTerm1.setZero(); PanelIt3 = PanelIt2; - for(int k = i; k<= (int) this->PanelVec.size();k++){ - sumTerm1 += 2*PanelIt3->sHat3_B+4*PanelIt3->sHat3_B*((int) this->PanelVec.size() - j) - -HeaviFunc(k-j)*4*PanelIt3->sHat3_B*(k-j); + for (int k = i; k <= (int)this->PanelVec.size(); k++) { + sumTerm1 += 2 * PanelIt3->sHat3_B + 4 * PanelIt3->sHat3_B * ((int)this->PanelVec.size() - j) - + HeaviFunc(k - j) * 4 * PanelIt3->sHat3_B * (k - j); std::advance(PanelIt3, 1); } - this->matrixADHRB(j-1,i-1) = PanelIt->IPntS_S(1,1)*HeaviFunc(j-i) + PanelIt->mass*pow(PanelIt->d,2) - *PanelIt->sHat3_B.transpose()*(sumTerm1-HeaviFunc(j-i)*PanelIt->sHat3_B); + this->matrixADHRB(j - 1, i - 1) = PanelIt->IPntS_S(1, 1) * HeaviFunc(j - i) + + PanelIt->mass * pow(PanelIt->d, 2) * PanelIt->sHat3_B.transpose() * + (sumTerm1 - HeaviFunc(j - i) * PanelIt->sHat3_B); i += 1; } j += 1; @@ -256,55 +270,57 @@ void NHingedRigidBodyStateEffector::updateContributions(double integTime, BackSu this->matrixEDHRB = this->matrixADHRB.inverse(); // - Define F matrix for the panel equations - this->matrixFDHRB.resize((int) this->PanelVec.size(),3); + this->matrixFDHRB.resize((int)this->PanelVec.size(), 3); this->matrixFDHRB.setZero(); j = 1; - for(PanelIt=this->PanelVec.begin(); PanelIt!=this->PanelVec.end(); PanelIt++){ + for (PanelIt = this->PanelVec.begin(); PanelIt != this->PanelVec.end(); PanelIt++) { Eigen::Vector3d sumTerm1; Eigen::Vector3d sumTerm2; sumTerm2.setZero(); - if(j+1<= (int) this->PanelVec.size()){ - for(int i = j+1; i <= (int) this->PanelVec.size(); i++){ - sumTerm2 += 2*PanelIt->mass*PanelIt->d*PanelIt->sHat3_B; + if (j + 1 <= (int)this->PanelVec.size()) { + for (int i = j + 1; i <= (int)this->PanelVec.size(); i++) { + sumTerm2 += 2 * PanelIt->mass * PanelIt->d * PanelIt->sHat3_B; } } - sumTerm1 = - PanelIt->mass*PanelIt->d*PanelIt->sHat3_B.transpose() - sumTerm2.transpose(); - this->matrixFDHRB(j-1,0) = sumTerm1[0]; - this->matrixFDHRB(j-1,1) = sumTerm1[1]; - this->matrixFDHRB(j-1,2) = sumTerm1[2]; + sumTerm1 = -PanelIt->mass * PanelIt->d * PanelIt->sHat3_B.transpose() - sumTerm2.transpose(); + this->matrixFDHRB(j - 1, 0) = sumTerm1[0]; + this->matrixFDHRB(j - 1, 1) = sumTerm1[1]; + this->matrixFDHRB(j - 1, 2) = sumTerm1[2]; j += 1; } // - Define G matrix for the panel equations - this->matrixGDHRB.resize((int) this->PanelVec.size(),3); + this->matrixGDHRB.resize((int)this->PanelVec.size(), 3); matrixGDHRB.setZero(); j = 1; - for(PanelIt=this->PanelVec.begin(); PanelIt!=this->PanelVec.end(); PanelIt++){ + for (PanelIt = this->PanelVec.begin(); PanelIt != this->PanelVec.end(); PanelIt++) { Eigen::Vector3d sumTerm1; Eigen::Vector3d sumTerm2; sumTerm2.setZero(); - if(j+1<= (int) this->PanelVec.size()){ + if (j + 1 <= (int)this->PanelVec.size()) { PanelIt2 = PanelIt; std::advance(PanelIt2, 1); - for(int i = j+1; i<=(int) this->PanelVec.size();i++){ - sumTerm2 += (2*PanelIt->mass*PanelIt->d*PanelIt->sHat3_B.transpose()*PanelIt2->rTilde_SB_B).transpose(); + for (int i = j + 1; i <= (int)this->PanelVec.size(); i++) { + sumTerm2 += + (2 * PanelIt->mass * PanelIt->d * PanelIt->sHat3_B.transpose() * PanelIt2->rTilde_SB_B).transpose(); std::advance(PanelIt2, 1); } } - sumTerm1 = -(PanelIt->IPntS_S(1,1)*PanelIt->sHat2_B.transpose()-PanelIt->mass*PanelIt->d - *PanelIt->sHat3_B.transpose()*PanelIt->rTilde_SB_B - sumTerm2.transpose()); - this->matrixGDHRB(j-1,0) = sumTerm1[0]; - this->matrixGDHRB(j-1,1) = sumTerm1[1]; - this->matrixGDHRB(j-1,2) = sumTerm1[2]; + sumTerm1 = + -(PanelIt->IPntS_S(1, 1) * PanelIt->sHat2_B.transpose() - + PanelIt->mass * PanelIt->d * PanelIt->sHat3_B.transpose() * PanelIt->rTilde_SB_B - sumTerm2.transpose()); + this->matrixGDHRB(j - 1, 0) = sumTerm1[0]; + this->matrixGDHRB(j - 1, 1) = sumTerm1[1]; + this->matrixGDHRB(j - 1, 2) = sumTerm1[2]; j += 1; } // - Define v vector for the panel equations - this->vectorVDHRB.resize((int) this->PanelVec.size()); + this->vectorVDHRB.resize((int)this->PanelVec.size()); this->vectorVDHRB.setZero(); double massOfCurrentPanelAndBefore = 0; // Summation of all of prior panels masses and the current panels mass j = 1; - for(PanelIt=this->PanelVec.begin(); PanelIt!=this->PanelVec.end(); PanelIt++){ + for (PanelIt = this->PanelVec.begin(); PanelIt != this->PanelVec.end(); PanelIt++) { // Add current panel to mass massOfCurrentPanelAndBefore += PanelIt->mass; double sumTerm1; @@ -314,48 +330,52 @@ void NHingedRigidBodyStateEffector::updateContributions(double integTime, BackSu sumTerm3.setZero(); double springTerm; PanelIt2 = PanelIt; - if(j+1 <= (int) this->PanelVec.size()){ + if (j + 1 <= (int)this->PanelVec.size()) { std::advance(PanelIt2, 1); - springTerm = -PanelIt->k*(PanelIt->theta-PanelIt->theta_0)-PanelIt->c*PanelIt->thetaDot - + PanelIt2->k*(PanelIt2->theta - PanelIt2->theta_0) + PanelIt2->c*PanelIt2->thetaDot; + springTerm = -PanelIt->k * (PanelIt->theta - PanelIt->theta_0) - PanelIt->c * PanelIt->thetaDot + + PanelIt2->k * (PanelIt2->theta - PanelIt2->theta_0) + PanelIt2->c * PanelIt2->thetaDot; } else { - springTerm = -PanelIt->k*(PanelIt->theta-PanelIt->theta_0)-PanelIt->c*PanelIt->thetaDot; + springTerm = -PanelIt->k * (PanelIt->theta - PanelIt->theta_0) - PanelIt->c * PanelIt->thetaDot; } - if(j+1<=(int) this->PanelVec.size()){ + if (j + 1 <= (int)this->PanelVec.size()) { PanelIt3 = PanelIt; std::advance(PanelIt3, 1); - for(int i = j+1; i <= (int) this->PanelVec.size(); i++){ - sumTerm2 += 4*this->omegaTildeLoc_BN_B*PanelIt3->rPrime_SB_B - +2*this->omegaTildeLoc_BN_B*this->omegaTildeLoc_BN_B*PanelIt3->r_SB_B; + for (int i = j + 1; i <= (int)this->PanelVec.size(); i++) { + sumTerm2 += 4 * this->omegaTildeLoc_BN_B * PanelIt3->rPrime_SB_B + + 2 * this->omegaTildeLoc_BN_B * this->omegaTildeLoc_BN_B * PanelIt3->r_SB_B; std::advance(PanelIt3, 1); } } double sumThetaDot = 0; int i = 1; - for(PanelIt2=this->PanelVec.begin(); PanelIt2!=this->PanelVec.end(); PanelIt2++){ + for (PanelIt2 = this->PanelVec.begin(); PanelIt2 != this->PanelVec.end(); PanelIt2++) { sumThetaDot += PanelIt2->thetaDot; - sumTerm3 += pow(sumThetaDot,2)*PanelIt2->d*(2*PanelIt2->sHat1_B+4*PanelIt2->sHat1_B*((int) this->PanelVec.size() - j)-HeaviFunc(i-j)*4*PanelIt2->sHat1_B*(i-j)); + sumTerm3 += pow(sumThetaDot, 2) * PanelIt2->d * + (2 * PanelIt2->sHat1_B + 4 * PanelIt2->sHat1_B * ((int)this->PanelVec.size() - j) - + HeaviFunc(i - j) * 4 * PanelIt2->sHat1_B * (i - j)); i += 1; } sumThetaDot = 0; PanelIt2 = this->PanelVec.begin(); - for (int n = 1; n<=j; n++){ + for (int n = 1; n <= j; n++) { sumThetaDot += PanelIt2->thetaDot; std::advance(PanelIt2, 1); } - sumTerm3 -= pow(sumThetaDot, 2)*PanelIt->d*PanelIt->sHat1_B; - sumTerm1 = springTerm -(PanelIt->IPntS_S(0,0) - PanelIt->IPntS_S(2,2))*PanelIt->omega_BN_S(2) - *PanelIt->omega_BN_S(0) - PanelIt->mass*PanelIt->d*PanelIt->sHat3_B.transpose()*(2*this->omegaTildeLoc_BN_B - *PanelIt->rPrime_SB_B+this->omegaTildeLoc_BN_B*this->omegaTildeLoc_BN_B*PanelIt->r_SB_B+sumTerm2+sumTerm3); + sumTerm3 -= pow(sumThetaDot, 2) * PanelIt->d * PanelIt->sHat1_B; + sumTerm1 = springTerm - + (PanelIt->IPntS_S(0, 0) - PanelIt->IPntS_S(2, 2)) * PanelIt->omega_BN_S(2) * PanelIt->omega_BN_S(0) - + PanelIt->mass * PanelIt->d * PanelIt->sHat3_B.transpose() * + (2 * this->omegaTildeLoc_BN_B * PanelIt->rPrime_SB_B + + this->omegaTildeLoc_BN_B * this->omegaTildeLoc_BN_B * PanelIt->r_SB_B + sumTerm2 + sumTerm3); // Add gravity torque to this sumTerm Eigen::Vector3d gravTorqueCurPanel; - gravTorqueCurPanel = -PanelIt->d*PanelIt->sHat1_B.cross(PanelIt->mass*g_B); + gravTorqueCurPanel = -PanelIt->d * PanelIt->sHat1_B.cross(PanelIt->mass * g_B); Eigen::Vector3d gravForceRestOfPanels; double remainingMass; remainingMass = this->totalMass - massOfCurrentPanelAndBefore; - gravForceRestOfPanels = remainingMass*g_B; - this->vectorVDHRB(j-1) = sumTerm1 + PanelIt->sHat2_B.dot(gravTorqueCurPanel) - + 2.0*PanelIt->d*PanelIt->sHat3_B.dot(gravForceRestOfPanels); + gravForceRestOfPanels = remainingMass * g_B; + this->vectorVDHRB(j - 1) = sumTerm1 + PanelIt->sHat2_B.dot(gravTorqueCurPanel) + + 2.0 * PanelIt->d * PanelIt->sHat3_B.dot(gravForceRestOfPanels); j += 1; } @@ -370,27 +390,28 @@ void NHingedRigidBodyStateEffector::updateContributions(double integTime, BackSu Eigen::Vector3d sumTerm2; sumTerm2.setZero(); j = 1; - for(PanelIt=this->PanelVec.begin(); PanelIt!=this->PanelVec.end(); PanelIt++){ + for (PanelIt = this->PanelVec.begin(); PanelIt != this->PanelVec.end(); PanelIt++) { Eigen::Vector3d sumTerm1; sumTerm1.setZero(); sumThetaDot += PanelIt->thetaDot; PanelIt2 = PanelIt; - for(int k = j; k <= (int) this->PanelVec.size();k++){ - sumTerm1 += (2*((int) this->PanelVec.size() - k)+1)*PanelIt2->mass*PanelIt2->d*PanelIt2->sHat3_B; + for (int k = j; k <= (int)this->PanelVec.size(); k++) { + sumTerm1 += (2 * ((int)this->PanelVec.size() - k) + 1) * PanelIt2->mass * PanelIt2->d * PanelIt2->sHat3_B; std::advance(PanelIt2, 1); } - sumTerm2 = pow(sumThetaDot,2)*(2*((int) this->PanelVec.size() - j)+1)*PanelIt->mass*PanelIt->d*PanelIt->sHat1_B; - backSubContr.matrixA += sumTerm1*this->matrixEDHRB.row(j-1)*this->matrixFDHRB; - backSubContr.matrixB += sumTerm1*this->matrixEDHRB.row(j-1)*this->matrixGDHRB; - backSubContr.vecTrans += -sumTerm2 - sumTerm1*this->matrixEDHRB.row(j-1)*this->vectorVDHRB; + sumTerm2 = pow(sumThetaDot, 2) * (2 * ((int)this->PanelVec.size() - j) + 1) * PanelIt->mass * PanelIt->d * + PanelIt->sHat1_B; + backSubContr.matrixA += sumTerm1 * this->matrixEDHRB.row(j - 1) * this->matrixFDHRB; + backSubContr.matrixB += sumTerm1 * this->matrixEDHRB.row(j - 1) * this->matrixGDHRB; + backSubContr.vecTrans += -sumTerm2 - sumTerm1 * this->matrixEDHRB.row(j - 1) * this->vectorVDHRB; j += 1; } Eigen::Vector3d aTheta; Eigen::Vector3d bTheta; - aTheta = this->matrixEDHRB.row(0)*this->matrixFDHRB; - bTheta = this->matrixEDHRB.row(0)*this->matrixGDHRB; + aTheta = this->matrixEDHRB.row(0) * this->matrixFDHRB; + bTheta = this->matrixEDHRB.row(0) * this->matrixGDHRB; // - Rotational contributions backSubContr.matrixC.setZero(); @@ -400,71 +421,76 @@ void NHingedRigidBodyStateEffector::updateContributions(double integTime, BackSu sumTerm2.setZero(); Eigen::Matrix3d sumTerm3; j = 1; - for(PanelIt=this->PanelVec.begin(); PanelIt!=this->PanelVec.end(); PanelIt++){ + for (PanelIt = this->PanelVec.begin(); PanelIt != this->PanelVec.end(); PanelIt++) { Eigen::Vector3d sumTerm1; sumTerm1.setZero(); sumThetaDot += PanelIt->thetaDot; PanelIt2 = PanelIt; - for(int k = j; k <= (int) this->PanelVec.size();k++){ + for (int k = j; k <= (int)this->PanelVec.size(); k++) { sumTerm3.setZero(); - if(k+1<=(int) this->PanelVec.size()){ + if (k + 1 <= (int)this->PanelVec.size()) { PanelIt3 = PanelIt2; std::advance(PanelIt3, 1); - for(int n = k+1; n <= (int) this->PanelVec.size();n++){ - sumTerm3 += 2*PanelIt3->rTilde_SB_B; + for (int n = k + 1; n <= (int)this->PanelVec.size(); n++) { + sumTerm3 += 2 * PanelIt3->rTilde_SB_B; std::advance(PanelIt3, 1); } } - sumTerm1 += PanelIt2->IPntS_S(1,1)*PanelIt2->sHat2_B - +(PanelIt2->rTilde_SB_B+sumTerm3)*PanelIt2->mass*PanelIt2->d*PanelIt2->sHat3_B; + sumTerm1 += PanelIt2->IPntS_S(1, 1) * PanelIt2->sHat2_B + + (PanelIt2->rTilde_SB_B + sumTerm3) * PanelIt2->mass * PanelIt2->d * PanelIt2->sHat3_B; std::advance(PanelIt2, 1); } sumTerm3.setZero(); - if(j+1<=(int) this->PanelVec.size()){ + if (j + 1 <= (int)this->PanelVec.size()) { PanelIt3 = PanelIt; std::advance(PanelIt3, 1); - for(int n = j+1; n <= (int) this->PanelVec.size();n++){ - sumTerm3 += 2*PanelIt3->rTilde_SB_B; + for (int n = j + 1; n <= (int)this->PanelVec.size(); n++) { + sumTerm3 += 2 * PanelIt3->rTilde_SB_B; std::advance(PanelIt3, 1); } } - sumTerm2 = PanelIt->mass*this->omegaTildeLoc_BN_B*PanelIt->rTilde_SB_B*PanelIt->rPrime_SB_B - + pow(sumThetaDot,2)*(PanelIt->rTilde_SB_B+sumTerm3)*PanelIt->mass*PanelIt->d*PanelIt->sHat1_B - + PanelIt->IPntS_S(1,1)*sumThetaDot*this->omegaTildeLoc_BN_B*PanelIt->sHat2_B; - backSubContr.matrixC += sumTerm1*this->matrixEDHRB.row(j-1)*this->matrixFDHRB; - backSubContr.matrixD += sumTerm1*this->matrixEDHRB.row(j-1)*this->matrixGDHRB; - backSubContr.vecRot += -sumTerm2 - sumTerm1*this->matrixEDHRB.row(j-1)*this->vectorVDHRB; + sumTerm2 = + PanelIt->mass * this->omegaTildeLoc_BN_B * PanelIt->rTilde_SB_B * PanelIt->rPrime_SB_B + + pow(sumThetaDot, 2) * (PanelIt->rTilde_SB_B + sumTerm3) * PanelIt->mass * PanelIt->d * PanelIt->sHat1_B + + PanelIt->IPntS_S(1, 1) * sumThetaDot * this->omegaTildeLoc_BN_B * PanelIt->sHat2_B; + backSubContr.matrixC += sumTerm1 * this->matrixEDHRB.row(j - 1) * this->matrixFDHRB; + backSubContr.matrixD += sumTerm1 * this->matrixEDHRB.row(j - 1) * this->matrixGDHRB; + backSubContr.vecRot += -sumTerm2 - sumTerm1 * this->matrixEDHRB.row(j - 1) * this->vectorVDHRB; j += 1; } - return; } /*! This method is used to find the derivatives for the HRB stateEffector: thetaDDot and the kinematic derivative */ -void NHingedRigidBodyStateEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +NHingedRigidBodyStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { // - Grab necessarry values from manager (these have been previously computed in hubEffector) Eigen::Vector3d rDDotLoc_BN_N; Eigen::MRPd sigmaLocal_BN; Eigen::Vector3d omegaDotLoc_BN_B; rDDotLoc_BN_N = rDDot_BN_N; - sigmaLocal_BN = (Eigen::Vector3d )sigma_BN; + sigmaLocal_BN = (Eigen::Vector3d)sigma_BN; omegaDotLoc_BN_B = omegaDot_BN_B; // - Find rDDotLoc_BN_B Eigen::Matrix3d dcm_BN; Eigen::Vector3d rDDotLoc_BN_B; dcm_BN = (sigmaLocal_BN.toRotationMatrix()).transpose(); - rDDotLoc_BN_B = dcm_BN*rDDotLoc_BN_N; + rDDotLoc_BN_B = dcm_BN * rDDotLoc_BN_N; // - Compute Derivatives std::vector::iterator PanelIt; - Eigen::MatrixXd thetaDDot(this->PanelVec.size(),1); + Eigen::MatrixXd thetaDDot(this->PanelVec.size(), 1); int i = 0; - for(PanelIt=this->PanelVec.begin(); PanelIt!=this->PanelVec.end(); PanelIt++){ - thetaDDot(i,0) = this->matrixEDHRB.row(i).dot(this->matrixFDHRB*rDDotLoc_BN_B) - + this->matrixEDHRB.row(i)*this->matrixGDHRB*omegaDotLoc_BN_B + this->matrixEDHRB.row(i)*this->vectorVDHRB; + for (PanelIt = this->PanelVec.begin(); PanelIt != this->PanelVec.end(); PanelIt++) { + thetaDDot(i, 0) = this->matrixEDHRB.row(i).dot(this->matrixFDHRB * rDDotLoc_BN_B) + + this->matrixEDHRB.row(i) * this->matrixGDHRB * omegaDotLoc_BN_B + + this->matrixEDHRB.row(i) * this->vectorVDHRB; i += 1; } // - First is trivial @@ -476,8 +502,11 @@ void NHingedRigidBodyStateEffector::computeDerivatives(double integTime, Eigen:: } /*! This method is for calculating the contributions of the HRB state effector to the energy and momentum of the s/c */ -void NHingedRigidBodyStateEffector::updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B) +void +NHingedRigidBodyStateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { // - Get the current omega state Eigen::Vector3d omegaLocal_BN_B; @@ -490,13 +519,14 @@ void NHingedRigidBodyStateEffector::updateEnergyMomContributions(double integTim Eigen::Vector3d rotAngMomPntCContr_B_Sum; rotAngMomPntCContr_B_Sum.setZero(); double rotEnergyContr_Sum = 0; - for(PanelIt=this->PanelVec.begin(); PanelIt!=this->PanelVec.end(); PanelIt++){ + for (PanelIt = this->PanelVec.begin(); PanelIt != this->PanelVec.end(); PanelIt++) { omega_SN_B = PanelIt->omega_SB_B + omegaLocal_BN_B; - IPntS_B = PanelIt->dcm_SB.transpose()*PanelIt->IPntS_S*PanelIt->dcm_SB; + IPntS_B = PanelIt->dcm_SB.transpose() * PanelIt->IPntS_S * PanelIt->dcm_SB; rDot_SB_B = PanelIt->rPrime_SB_B + omegaLocal_BN_B.cross(PanelIt->r_SB_B); - rotAngMomPntCContr_B_Sum += IPntS_B*omega_SN_B + PanelIt->mass*PanelIt->r_SB_B.cross(rDot_SB_B); - rotEnergyContr_Sum += 0.5*omega_SN_B.dot(IPntS_B*omega_SN_B) + 1.0/2.0*PanelIt->mass*rDot_SB_B.dot(rDot_SB_B) - + 1.0/2.0*PanelIt->k*(PanelIt->theta-PanelIt->theta_0)*(PanelIt->theta-PanelIt->theta_0); + rotAngMomPntCContr_B_Sum += IPntS_B * omega_SN_B + PanelIt->mass * PanelIt->r_SB_B.cross(rDot_SB_B); + rotEnergyContr_Sum += + 0.5 * omega_SN_B.dot(IPntS_B * omega_SN_B) + 1.0 / 2.0 * PanelIt->mass * rDot_SB_B.dot(rDot_SB_B) + + 1.0 / 2.0 * PanelIt->k * (PanelIt->theta - PanelIt->theta_0) * (PanelIt->theta - PanelIt->theta_0); } // - Find rotational angular momentum contribution from hub @@ -511,7 +541,8 @@ void NHingedRigidBodyStateEffector::updateEnergyMomContributions(double integTim @param CurrentSimNanos The current simulation time in nanoseconds */ -void NHingedRigidBodyStateEffector::UpdateState(uint64_t CurrentSimNanos) +void +NHingedRigidBodyStateEffector::UpdateState(uint64_t CurrentSimNanos) { WriteOutputMessages(CurrentSimNanos); diff --git a/src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.h b/src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.h old mode 100755 new mode 100644 index 156715c998..c0b5552287 --- a/src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.h +++ b/src/simulation/dynamics/NHingedRigidBodies/nHingedRigidBodyStateEffector.h @@ -27,82 +27,92 @@ #include "architecture/utilities/avsEigenMRP.h" #include "architecture/utilities/bskLogging.h" - - /*! Struct containing all the panel variables. All members are public by default so they can be changed by methods of the N_hingedRigidBodyStateEffector class. */ -struct HingedPanel { - double mass = 1.0; //!< [kg] mass of hinged panel - double d = 1.0; //!< [m] distance from hinge point to hinged rigid body center of mass - double k = 1.0; //!< [N-m/rad] torsional spring constant of hinge - double c = 0.0; //!< [N-m-s/rad] rotational damping coefficient of hinge - double thetaInit = 0.0; //!< [rad] Initial hinged rigid body angle - double thetaDotInit = 0.0; //!< [rad/s] Initial hinged rigid body angle rate - Eigen::Matrix3d IPntS_S; //!< [kg-m^2] Inertia of hinged rigid body about point S in S frame components - double theta = 0.0; //!< [rad] hinged rigid body angle - double theta_0 = 0.0; //!< [rad] hinged rigid body rest angle - double thetaDot = 0.0; //!< [rad/s] hinged rigid body angle rate - Eigen::Matrix3d dcm_SS_prev; //!< -- DCM from previous S frame to current S frame - Eigen::Matrix3d dcm_SB; //!< -- DCM from body to S frame - Eigen::Vector3d omega_BN_S; //!< [rad/s] omega_BN in S frame components - Eigen::Vector3d omega_SB_B; //!< [rad/s] omega_SB in B frame components - Eigen::Vector3d sHat1_B; //!< -- unit direction vector for the first axis of the S frame - Eigen::Vector3d sHat2_B; //!< -- unit direction vector for the second axis of the S frame - Eigen::Vector3d sHat3_B; //!< -- unit direction vector for the third axis of the S frame - Eigen::Vector3d r_SB_B; //!< -- Vector pointing from B to CoM of hinged rigid body in B frame components - Eigen::Matrix3d rTilde_SB_B; //!< -- Tilde matrix of rSB_B - Eigen::Vector3d rPrime_SB_B; //!< [m/s] Body time derivative of rSB_B - Eigen::Matrix3d rPrimeTilde_SB_B;//!< -- Tilde matrix of rPrime_SB_B - Eigen::Matrix3d ISPrimePntS_B; //!< [kg-m^2/s] time body derivative IPntS in body frame components +struct HingedPanel +{ + double mass = 1.0; //!< [kg] mass of hinged panel + double d = 1.0; //!< [m] distance from hinge point to hinged rigid body center of mass + double k = 1.0; //!< [N-m/rad] torsional spring constant of hinge + double c = 0.0; //!< [N-m-s/rad] rotational damping coefficient of hinge + double thetaInit = 0.0; //!< [rad] Initial hinged rigid body angle + double thetaDotInit = 0.0; //!< [rad/s] Initial hinged rigid body angle rate + Eigen::Matrix3d IPntS_S; //!< [kg-m^2] Inertia of hinged rigid body about point S in S frame components + double theta = 0.0; //!< [rad] hinged rigid body angle + double theta_0 = 0.0; //!< [rad] hinged rigid body rest angle + double thetaDot = 0.0; //!< [rad/s] hinged rigid body angle rate + Eigen::Matrix3d dcm_SS_prev; //!< -- DCM from previous S frame to current S frame + Eigen::Matrix3d dcm_SB; //!< -- DCM from body to S frame + Eigen::Vector3d omega_BN_S; //!< [rad/s] omega_BN in S frame components + Eigen::Vector3d omega_SB_B; //!< [rad/s] omega_SB in B frame components + Eigen::Vector3d sHat1_B; //!< -- unit direction vector for the first axis of the S frame + Eigen::Vector3d sHat2_B; //!< -- unit direction vector for the second axis of the S frame + Eigen::Vector3d sHat3_B; //!< -- unit direction vector for the third axis of the S frame + Eigen::Vector3d r_SB_B; //!< -- Vector pointing from B to CoM of hinged rigid body in B frame components + Eigen::Matrix3d rTilde_SB_B; //!< -- Tilde matrix of rSB_B + Eigen::Vector3d rPrime_SB_B; //!< [m/s] Body time derivative of rSB_B + Eigen::Matrix3d rPrimeTilde_SB_B; //!< -- Tilde matrix of rPrime_SB_B + Eigen::Matrix3d ISPrimePntS_B; //!< [kg-m^2/s] time body derivative IPntS in body frame components }; /*! @brief NHingedRigidBodyStateEffector class */ -class NHingedRigidBodyStateEffector : public StateEffector, public SysModel { -public: +class NHingedRigidBodyStateEffector + : public StateEffector + , public SysModel +{ + public: std::string nameOfThetaState; //!< -- Identifier for the theta state data container std::string nameOfThetaDotState; //!< -- Identifier for the thetaDot state data container Eigen::Vector3d r_HB_B; //!< [m] vector pointing from body frame origin to the first Hinge location Eigen::Matrix3d rTilde_HB_B; //!< -- Tilde matrix of rHB_B Eigen::Matrix3d dcm_HB; //!< -- DCM from body frame to hinge frame - void addHingedPanel(HingedPanel NewPanel) {PanelVec.push_back(NewPanel);} //!< class method - BSKLogger bskLogger; //!< -- BSK Logging + void addHingedPanel(HingedPanel NewPanel) { PanelVec.push_back(NewPanel); } //!< class method + BSKLogger bskLogger; //!< -- BSK Logging -private: - double totalMass; //!< [kg] Total mass of effector - StateData *thetaState; //!< -- state manager of theta for hinged rigid body - StateData *thetaDotState; //!< -- state manager of thetaDot for hinged rigid body - std::vector PanelVec; //!< -- vector containing all the info on the different panels - Eigen::MatrixXd matrixADHRB; //!< [-] term needed for back substitution - Eigen::MatrixXd matrixEDHRB; //!< [-] term needed for back substitution - Eigen::MatrixXd matrixFDHRB; //!< [-] term needed for back substitution - Eigen::MatrixXd matrixGDHRB; //!< [-] term needed for back substitution - Eigen::MatrixXd matrixHDHRB; //!< [-] term needed for back substitution - Eigen::MatrixXd matrixKDHRB; //!< [-] term needed for back substitution - Eigen::MatrixXd matrixLDHRB; //!< [-] term needed for back substitution - Eigen::MatrixXd matrixMDHRB; //!< [-] term needed for back substitution - Eigen::VectorXd vectorVDHRB; //!< [-] term needed for back substitution - Eigen::Vector3d aTheta; //!< -- term needed for back substitution - Eigen::Vector3d bTheta; //!< -- term needed for back substitution - Eigen::Vector3d omegaLoc_BN_B; //!< [rad/s] local copy of omegaBN + private: + double totalMass; //!< [kg] Total mass of effector + StateData* thetaState; //!< -- state manager of theta for hinged rigid body + StateData* thetaDotState; //!< -- state manager of thetaDot for hinged rigid body + std::vector PanelVec; //!< -- vector containing all the info on the different panels + Eigen::MatrixXd matrixADHRB; //!< [-] term needed for back substitution + Eigen::MatrixXd matrixEDHRB; //!< [-] term needed for back substitution + Eigen::MatrixXd matrixFDHRB; //!< [-] term needed for back substitution + Eigen::MatrixXd matrixGDHRB; //!< [-] term needed for back substitution + Eigen::MatrixXd matrixHDHRB; //!< [-] term needed for back substitution + Eigen::MatrixXd matrixKDHRB; //!< [-] term needed for back substitution + Eigen::MatrixXd matrixLDHRB; //!< [-] term needed for back substitution + Eigen::MatrixXd matrixMDHRB; //!< [-] term needed for back substitution + Eigen::VectorXd vectorVDHRB; //!< [-] term needed for back substitution + Eigen::Vector3d aTheta; //!< -- term needed for back substitution + Eigen::Vector3d bTheta; //!< -- term needed for back substitution + Eigen::Vector3d omegaLoc_BN_B; //!< [rad/s] local copy of omegaBN Eigen::Matrix3d omegaTildeLoc_BN_B; //!< -- tilde matrix of omegaBN - Eigen::MatrixXd *g_N; //!< [m/s^2] Gravitational acceleration in N frame components - static uint64_t effectorID; //!< [] ID number of this panel + Eigen::MatrixXd* g_N; //!< [m/s^2] Gravitational acceleration in N frame components + static uint64_t effectorID; //!< [] ID number of this panel -public: + public: NHingedRigidBodyStateEffector(); //!< -- Contructor - ~NHingedRigidBodyStateEffector(); //!< -- Destructor - double HeaviFunc(double cond); //!< -- Heaviside function used for matrix contributions + ~NHingedRigidBodyStateEffector(); //!< -- Destructor + double HeaviFunc(double cond); //!< -- Heaviside function used for matrix contributions void WriteOutputMessages(uint64_t CurrentClock); - void UpdateState(uint64_t CurrentSimNanos); - void registerStates(DynParamManager& statesIn); //!< -- Method for registering the HRB states - void linkInStates(DynParamManager& states); //!< -- Method for getting access to other states - void updateEffectorMassProps(double integTime); //!< -- Method for stateEffector to give mass contributions - void updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N); //!< -- Back-sub contributions - void updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B); //!< -- Energy and momentum calculations - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN); //!< -- Method for each stateEffector to calculate derivatives - void readInputMessages(); //!< -- method to read input messages + void UpdateState(uint64_t CurrentSimNanos); + void registerStates(DynParamManager& statesIn); //!< -- Method for registering the HRB states + void linkInStates(DynParamManager& states); //!< -- Method for getting access to other states + void updateEffectorMassProps(double integTime); //!< -- Method for stateEffector to give mass contributions + void updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N); //!< -- Back-sub contributions + void updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B); //!< -- Energy and momentum calculations + void computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN); //!< -- Method for each stateEffector to calculate derivatives + void readInputMessages(); //!< -- method to read input messages }; - #endif /* N_HINGED_RIGID_BODY_STATE_EFFECTOR_H */ 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/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/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/_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/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/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/RadiationPressure/radiationPressure.cpp b/src/simulation/dynamics/RadiationPressure/radiationPressure.cpp old mode 100755 new mode 100644 index 3366919a8d..7a7aa7887c --- a/src/simulation/dynamics/RadiationPressure/radiationPressure.cpp +++ b/src/simulation/dynamics/RadiationPressure/radiationPressure.cpp @@ -26,10 +26,10 @@ /*! This is the constructor. It sets some default initializers that can be overriden by the user.*/ RadiationPressure::RadiationPressure() - :area(0.0f) - ,coefficientReflection(1.2) - ,srpModel(SRP_CANNONBALL_MODEL) - ,stateRead(false) + : area(0.0f) + , coefficientReflection(1.2) + , srpModel(SRP_CANNONBALL_MODEL) + , stateRead(false) { this->sunVisibilityFactor.shadowFactor = 1.0; this->forceExternal_N.setZero(); @@ -46,15 +46,13 @@ RadiationPressure::~RadiationPressure() return; } - - /*! Reset the module to origina configuration values. */ -void RadiationPressure::Reset(uint64_t CurrenSimNanos) +void +RadiationPressure::Reset(uint64_t CurrenSimNanos) { - if(!this->sunEphmInMsg.isLinked()) - { + if (!this->sunEphmInMsg.isLinked()) { bskLogger.bskLog(BSK_ERROR, "Did not find a valid sun ephemeris message connection."); } } @@ -64,7 +62,8 @@ void RadiationPressure::Reset(uint64_t CurrenSimNanos) @param states Dynamic parameter manager */ -void RadiationPressure::linkInStates(DynParamManager& states) +void +RadiationPressure::linkInStates(DynParamManager& states) { this->hubSigma = states.getStateObject(this->stateNameOfSigma); this->hubR_N = states.getStateObject(this->stateNameOfPosition); @@ -75,7 +74,8 @@ void RadiationPressure::linkInStates(DynParamManager& states) buffer structure. */ -void RadiationPressure::readInputMessages() +void +RadiationPressure::readInputMessages() { /* read in sun state message */ this->sunEphmInBuffer = this->sunEphmInMsg(); @@ -94,7 +94,8 @@ void RadiationPressure::readInputMessages() @param integTime Current simulation integration time @param timeStep Current integration time step used */ -void RadiationPressure::computeForceTorque(double integTime, double timeStep) +void +RadiationPressure::computeForceTorque(double integTime, double timeStep) { this->forceExternal_N.setZero(); this->forceExternal_B.setZero(); @@ -107,17 +108,16 @@ void RadiationPressure::computeForceTorque(double integTime, double timeStep) if (this->srpModel == SRP_CANNONBALL_MODEL) { this->computeCannonballModel(s_N); this->forceExternal_N = this->forceExternal_N * this->sunVisibilityFactor.shadowFactor; - } - else if (this->srpModel == SRP_FACETED_CPU_MODEL) { + } else if (this->srpModel == SRP_FACETED_CPU_MODEL) { Eigen::MRPd sigmaLocal_NB; sigmaLocal_NB = (Eigen::Vector3d)this->hubSigma->getState(); Eigen::Matrix3d dcmLocal_BN = sigmaLocal_NB.toRotationMatrix().transpose(); - Eigen::Vector3d s_B = dcmLocal_BN*(sun_r_N - r_N); + Eigen::Vector3d s_B = dcmLocal_BN * (sun_r_N - r_N); this->computeLookupModel(s_B); this->forceExternal_B = this->forceExternal_B * this->sunVisibilityFactor.shadowFactor; this->torqueExternalPntB_B = this->torqueExternalPntB_B * this->sunVisibilityFactor.shadowFactor; } else { - bskLogger.bskLog(BSK_ERROR,"Requested SRF Model not implemented.\n"); + bskLogger.bskLog(BSK_ERROR, "Requested SRF Model not implemented.\n"); } } @@ -125,7 +125,8 @@ void RadiationPressure::computeForceTorque(double integTime, double timeStep) @param CurrentSimNanos current simulation time in nanoseconds */ -void RadiationPressure::UpdateState(uint64_t CurrentSimNanos) +void +RadiationPressure::UpdateState(uint64_t CurrentSimNanos) { this->readInputMessages(); } @@ -133,7 +134,8 @@ void RadiationPressure::UpdateState(uint64_t CurrentSimNanos) /*! Sets the model to the cannonball model in computing the solar radiation force */ -void RadiationPressure::setUseCannonballModel() +void +RadiationPressure::setUseCannonballModel() { this->srpModel = SRP_CANNONBALL_MODEL; } @@ -141,12 +143,12 @@ void RadiationPressure::setUseCannonballModel() /*! Sets the model to the faceted table-lookup model, evaluted on the CPU, in computing the solar radiation force */ -void RadiationPressure::setUseFacetedCPUModel() +void +RadiationPressure::setUseFacetedCPUModel() { this->srpModel = SRP_FACETED_CPU_MODEL; } - /*! Computes the solar radiation force vector * based on cross-sectional Area and mass of the spacecraft * and the position vector of the spacecraft to the sun. @@ -158,14 +160,16 @@ void RadiationPressure::setUseFacetedCPUModel() @param s_N (m) Position vector to the Sun relative to the inertial frame */ -void RadiationPressure::computeCannonballModel(Eigen::Vector3d s_N) +void +RadiationPressure::computeCannonballModel(Eigen::Vector3d s_N) { /* Magnitude of sun vector in the body frame */ double sunDist = s_N.norm(); /* Computing the force vector [N]*/ - double scaleFactor = (-this->coefficientReflection * this->area * SOLAR_FLUX_EARTH * pow(AU*1000.,2)) / (SPEED_LIGHT * pow(sunDist, 3)); + double scaleFactor = (-this->coefficientReflection * this->area * SOLAR_FLUX_EARTH * pow(AU * 1000., 2)) / + (SPEED_LIGHT * pow(sunDist, 3)); if (stateRead) - this->forceExternal_N = scaleFactor*(s_N); + this->forceExternal_N = scaleFactor * (s_N); else this->forceExternal_N.setZero(); } @@ -179,14 +183,15 @@ void RadiationPressure::computeCannonballModel(Eigen::Vector3d s_N) @param s_B (m) Position vector of the Sun relative to the body frame */ -void RadiationPressure::computeLookupModel(Eigen::Vector3d s_B) +void +RadiationPressure::computeLookupModel(Eigen::Vector3d s_B) { double tmpDotProduct = 0; double currentDotProduct = 0; int currentIdx = 0; double sunDist = s_B.norm(); - Eigen::Vector3d sHat_B = s_B/sunDist; - Eigen::Vector3d tmpLookupSHat_B(0,0,0); + Eigen::Vector3d sHat_B = s_B / sunDist; + Eigen::Vector3d tmpLookupSHat_B(0, 0, 0); if (!this->stateRead) { this->forceExternal_B.setZero(); @@ -200,18 +205,17 @@ void RadiationPressure::computeLookupModel(Eigen::Vector3d s_B) // @TODO: this lookup search should be optimized, possibly by saving the // index for later use and generate lookup table as azimuth and elevation // because then we can use a simple gradient decent search to find the nearest next attitude - for(int i = 0; i < (int) this->lookupSHat_B.size(); i++) { + for (int i = 0; i < (int)this->lookupSHat_B.size(); i++) { tmpLookupSHat_B = this->lookupSHat_B[i]; tmpDotProduct = tmpLookupSHat_B.dot(sHat_B); - if (tmpDotProduct > currentDotProduct) - { + if (tmpDotProduct > currentDotProduct) { currentIdx = i; currentDotProduct = tmpDotProduct; } } - this->forceExternal_B = this->lookupForce_B[currentIdx]*pow(AU*1000/sunDist, 2); - this->torqueExternalPntB_B = this->lookupTorque_B[currentIdx]*pow(AU*1000/sunDist, 2); + this->forceExternal_B = this->lookupForce_B[currentIdx] * pow(AU * 1000 / sunDist, 2); + this->torqueExternalPntB_B = this->lookupTorque_B[currentIdx] * pow(AU * 1000 / sunDist, 2); } /*! Add force vector in the body frame to lookup table. @@ -219,7 +223,8 @@ void RadiationPressure::computeLookupModel(Eigen::Vector3d s_B) @param vec (N) Force vector for particular attitude in lookup table */ -void RadiationPressure::addForceLookupBEntry(Eigen::Vector3d vec) +void +RadiationPressure::addForceLookupBEntry(Eigen::Vector3d vec) { this->lookupForce_B.push_back(vec); } @@ -229,7 +234,8 @@ void RadiationPressure::addForceLookupBEntry(Eigen::Vector3d vec) @param vec (Nm) Torque vector for particular attitude in lookup table */ -void RadiationPressure::addTorqueLookupBEntry(Eigen::Vector3d vec) +void +RadiationPressure::addTorqueLookupBEntry(Eigen::Vector3d vec) { this->lookupTorque_B.push_back(vec); } @@ -239,7 +245,8 @@ void RadiationPressure::addTorqueLookupBEntry(Eigen::Vector3d vec) @param vec sun unit direction vector in body frame */ -void RadiationPressure::addSHatLookupBEntry(Eigen::Vector3d vec) +void +RadiationPressure::addSHatLookupBEntry(Eigen::Vector3d vec) { this->lookupSHat_B.push_back(vec); } diff --git a/src/simulation/dynamics/RadiationPressure/radiationPressure.h b/src/simulation/dynamics/RadiationPressure/radiationPressure.h old mode 100755 new mode 100644 index 718dab364a..3e47eb51a3 --- a/src/simulation/dynamics/RadiationPressure/radiationPressure.h +++ b/src/simulation/dynamics/RadiationPressure/radiationPressure.h @@ -33,20 +33,19 @@ #include "architecture/utilities/bskLogging.h" - - - -typedef enum { +typedef enum +{ SRP_CANNONBALL_MODEL, SRP_FACETED_CPU_MODEL } srpModel_t; - - // SRP effects on body /*! @brief solar radiation pressure dynamic effector */ -class RadiationPressure: public SysModel, public DynamicEffector{ -public: +class RadiationPressure + : public SysModel + , public DynamicEffector +{ + public: RadiationPressure(); ~RadiationPressure(); @@ -60,30 +59,28 @@ class RadiationPressure: public SysModel, public DynamicEffector{ void addForceLookupBEntry(Eigen::Vector3d vec); void addTorqueLookupBEntry(Eigen::Vector3d vec); void addSHatLookupBEntry(Eigen::Vector3d vec); - -private: + + private: void computeCannonballModel(Eigen::Vector3d rSunB_B); void computeLookupModel(Eigen::Vector3d rSunB_B); -public: - double area; //!< m^2 Body surface area - double coefficientReflection; //!< -- Factor grouping surface optical properties - ReadFunctor sunEphmInMsg; //!< -- sun state input message - ReadFunctor sunEclipseInMsg; //!< -- (optional) sun eclipse input message - std::vector lookupForce_B; //!< -- Force on S/C at 1 AU from sun - std::vector lookupTorque_B; //!< -- Torque on S/C - std::vector lookupSHat_B; //!< -- S/C to sun unit vector defined in the body frame. - BSKLogger bskLogger; //!< -- BSK Logging - -private: - srpModel_t srpModel; //!< -- specifies which SRP model to use - SpicePlanetStateMsgPayload sunEphmInBuffer; //!< -- Buffer for incoming ephemeris message data - bool stateRead; //!< -- Indicates a succesful read of incoming SC state message data - EclipseMsgPayload sunVisibilityFactor; //!< [-] scaling parameter from 0 (fully obscured) to 1 (fully visible) - StateData *hubR_N; //!< -- State data accesss to inertial position for the hub - StateData *hubSigma; //!< -- Hub/Inertial attitude represented by MRP - + public: + double area; //!< m^2 Body surface area + double coefficientReflection; //!< -- Factor grouping surface optical properties + ReadFunctor sunEphmInMsg; //!< -- sun state input message + ReadFunctor sunEclipseInMsg; //!< -- (optional) sun eclipse input message + std::vector lookupForce_B; //!< -- Force on S/C at 1 AU from sun + std::vector lookupTorque_B; //!< -- Torque on S/C + std::vector lookupSHat_B; //!< -- S/C to sun unit vector defined in the body frame. + BSKLogger bskLogger; //!< -- BSK Logging + + private: + srpModel_t srpModel; //!< -- specifies which SRP model to use + SpicePlanetStateMsgPayload sunEphmInBuffer; //!< -- Buffer for incoming ephemeris message data + bool stateRead; //!< -- Indicates a succesful read of incoming SC state message data + EclipseMsgPayload sunVisibilityFactor; //!< [-] scaling parameter from 0 (fully obscured) to 1 (fully visible) + StateData* hubR_N; //!< -- State data accesss to inertial position for the hub + StateData* hubSigma; //!< -- Hub/Inertial attitude represented by MRP }; - #endif 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/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/thrusterDynamicEffector/thrusterDynamicEffector.cpp b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp index 5cb369c01c..f0d4504b2f 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.cpp @@ -26,11 +26,11 @@ /*! The Constructor.*/ ThrusterDynamicEffector::ThrusterDynamicEffector() -: stepsInRamp(30) -, mDotTotal(0.0) -, fuelMass(-1.0) -, prevFireTime(0.0) -, prevCommandTime(0xFFFFFFFFFFFFFFFF) + : stepsInRamp(30) + , mDotTotal(0.0) + , fuelMass(-1.0) + , prevFireTime(0.0) + , prevCommandTime(0xFFFFFFFFFFFFFFFF) { CallCounts = 0; forceExternal_B.fill(0.0); @@ -44,17 +44,17 @@ ThrusterDynamicEffector::ThrusterDynamicEffector() /*! The destructor. */ ThrusterDynamicEffector::~ThrusterDynamicEffector() { - for (long unsigned int c=0; cthrusterOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->thrusterOutMsgs.size(); c++) { free(this->thrusterOutMsgs.at(c)); } return; } - /*! This method is used to reset the module. */ -void ThrusterDynamicEffector::Reset(uint64_t CurrentSimNanos) +void +ThrusterDynamicEffector::Reset(uint64_t CurrentSimNanos) { //! Clear out any currently firing thrusters and re-init cmd array NewThrustCmds.clear(); @@ -69,15 +69,15 @@ void ThrusterDynamicEffector::Reset(uint64_t CurrentSimNanos) @param CurrentClock The current time used for time-stamping the message */ -void ThrusterDynamicEffector::writeOutputMessages(uint64_t CurrentClock) +void +ThrusterDynamicEffector::writeOutputMessages(uint64_t CurrentClock) { int idx = 0; std::vector>::iterator itp; std::shared_ptr it; THROutputMsgPayload tmpThruster; - for (itp = this->thrusterData.begin(); itp != this->thrusterData.end(); ++itp) - { + for (itp = this->thrusterData.begin(); itp != this->thrusterData.end(); ++itp) { it = *itp; tmpThruster = this->thrusterOutMsgs[idx]->zeroMsgPayload; eigenVector3d2CArray(it->thrLoc_B, tmpThruster.thrusterLocation); @@ -92,16 +92,16 @@ void ThrusterDynamicEffector::writeOutputMessages(uint64_t CurrentClock) this->thrusterOutMsgs[idx]->write(&tmpThruster, this->moduleID, CurrentClock); - idx ++; + idx++; } } - /*! This method is used to read the incoming command message and set the associated command structure for operating the thrusters. */ -bool ThrusterDynamicEffector::ReadInputs() +bool +ThrusterDynamicEffector::ReadInputs() { uint64_t i; bool dataGood; @@ -112,8 +112,8 @@ bool ThrusterDynamicEffector::ReadInputs() dataGood = this->cmdsInMsg.isWritten(); // Check if message has already been read, if stale return - if(this->prevCommandTime==this->cmdsInMsg.timeWritten() || !dataGood) { - return(false); + if (this->prevCommandTime == this->cmdsInMsg.timeWritten() || !dataGood) { + return (false); } this->prevCommandTime = this->cmdsInMsg.timeWritten(); } else { @@ -122,14 +122,11 @@ bool ThrusterDynamicEffector::ReadInputs() } // Set the NewThrustCmds vector. Using the data() method for raw speed - double *CmdPtr; - for(i=0, CmdPtr = NewThrustCmds.data(); i < this->thrusterData.size(); - CmdPtr++, i++) - { + double* CmdPtr; + for (i = 0, CmdPtr = NewThrustCmds.data(); i < this->thrusterData.size(); CmdPtr++, i++) { *CmdPtr = this->incomingCmdBuffer.OnTimeRequest[i]; } - return(true); - + return (true); } /*! This method is used to read the new commands vector and set the thruster @@ -140,25 +137,23 @@ bool ThrusterDynamicEffector::ReadInputs() @param currentTime The current simulation time converted to a double */ -void ThrusterDynamicEffector::ConfigureThrustRequests(double currentTime) +void +ThrusterDynamicEffector::ConfigureThrustRequests(double currentTime) { std::vector::iterator CmdIt; size_t THIter = 0; // Iterate through the list of thruster commands that we read in. - for(CmdIt = NewThrustCmds.begin(); CmdIt != NewThrustCmds.end(); CmdIt++) - { - if(*CmdIt >= this->thrusterData[THIter]->MinOnTime) // Check to see if we have met minimum for each thruster + for (CmdIt = NewThrustCmds.begin(); CmdIt != NewThrustCmds.end(); CmdIt++) { + if (*CmdIt >= this->thrusterData[THIter]->MinOnTime) // Check to see if we have met minimum for each thruster { // For each case where we are above the minimum firing request, reset the thruster this->thrusterData[THIter]->ThrustOps.ThrustOnCmd = *CmdIt; - this->thrusterData[THIter]->ThrustOps.fireCounter += this->thrusterData[THIter]->ThrustOps.ThrustFactor > 0.0 - ? 0 : 1; - } - else - { + this->thrusterData[THIter]->ThrustOps.fireCounter += + this->thrusterData[THIter]->ThrustOps.ThrustFactor > 0.0 ? 0 : 1; + } else { // Will ensure that thruster shuts down once this cmd expires - this->thrusterData[THIter]->ThrustOps.ThrustOnCmd = this->thrusterData[THIter]->ThrustOps.ThrustFactor > 0.0 - ? *CmdIt : 0.0; + this->thrusterData[THIter]->ThrustOps.ThrustOnCmd = + this->thrusterData[THIter]->ThrustOps.ThrustFactor > 0.0 ? *CmdIt : 0.0; } this->thrusterData[THIter]->ThrustOps.ThrusterStartTime = currentTime; this->thrusterData[THIter]->ThrustOps.PreviousIterTime = currentTime; @@ -169,7 +164,6 @@ void ThrusterDynamicEffector::ConfigureThrustRequests(double currentTime) *CmdIt = 0.0; THIter++; } - } /*! This method is used to update the location and orientation of the thrusters @@ -177,16 +171,18 @@ void ThrusterDynamicEffector::ConfigureThrustRequests(double currentTime) * the hub. */ -void ThrusterDynamicEffector::UpdateThrusterProperties() +void +ThrusterDynamicEffector::UpdateThrusterProperties() { // Save hub variables - Eigen::Vector3d r_BN_N = (Eigen::Vector3d)*this->inertialPositionProperty; + Eigen::Vector3d r_BN_N = (Eigen::Vector3d) * this->inertialPositionProperty; Eigen::Vector3d omega_BN_B = this->hubOmega->getState(); Eigen::MRPd sigma_BN; sigma_BN = (Eigen::Vector3d)this->hubSigma->getState(); Eigen::Matrix3d dcm_BN = (sigma_BN.toRotationMatrix()).transpose(); - // Define the variables related to which body the thruster is attached to. The F frame represents the platform body where the thruster attaches to + // Define the variables related to which body the thruster is attached to. The F frame represents the platform body + // where the thruster attaches to Eigen::MRPd sigma_FN; Eigen::Matrix3d dcm_FN; Eigen::Vector3d omega_FN_F; @@ -198,11 +194,9 @@ void ThrusterDynamicEffector::UpdateThrusterProperties() // Loop through all thrusters std::vector>::iterator it; int index; - for (it = this->attachedBodyInMsgs.begin(), index = 0; it != this->attachedBodyInMsgs.end(); it++, index++) - { + for (it = this->attachedBodyInMsgs.begin(), index = 0; it != this->attachedBodyInMsgs.end(); it++, index++) { // Check if the message is linked, and if so do the conversion - if (it->isLinked() && it->isWritten()) - { + if (it->isLinked() && it->isWritten()) { // Save to buffer this->attachedBodyBuffer = this->attachedBodyInMsgs.at(index)(); @@ -227,17 +221,20 @@ void ThrusterDynamicEffector::UpdateThrusterProperties() @param states The states to link */ -void ThrusterDynamicEffector::linkInStates(DynParamManager& states){ +void +ThrusterDynamicEffector::linkInStates(DynParamManager& states) +{ this->hubSigma = states.getStateObject(this->stateNameOfSigma); this->hubOmega = states.getStateObject(this->stateNameOfOmega); this->inertialPositionProperty = states.getPropertyReference(this->propName_inertialPosition); - for(const auto& thrusterConfig : this->thrusterData) { + for (const auto& thrusterConfig : this->thrusterData) { if (this->fuelMass < 0.0 && (!thrusterConfig->thrBlowDownCoeff.empty() || !thrusterConfig->ispBlowDownCoeff.empty())) { - bskLogger.bskLog(BSK_WARNING,"ThrusterDynamicEffector: blow down coefficients have been " - "specified, but no fuel tank is attached."); + bskLogger.bskLog(BSK_WARNING, + "ThrusterDynamicEffector: blow down coefficients have been " + "specified, but no fuel tank is attached."); } } } @@ -247,7 +244,8 @@ void ThrusterDynamicEffector::linkInStates(DynParamManager& states){ @param integTime Integration time @param timeStep Current integration time step used */ -void ThrusterDynamicEffector::computeForceTorque(double integTime, double timeStep) +void +ThrusterDynamicEffector::computeForceTorque(double integTime, double timeStep) { // Save omega_BN_B Eigen::Vector3d omegaLocal_BN_B = this->hubOmega->getState(); @@ -263,7 +261,7 @@ void ThrusterDynamicEffector::computeForceTorque(double integTime, double timeSt // Expelled momentum variables Eigen::Matrix3d BMj; - Eigen::Matrix3d axesWeightMatrix; + Eigen::Matrix3d axesWeightMatrix; Eigen::Vector3d BM1, BM2, BM3; double mDotNozzle; @@ -273,7 +271,7 @@ void ThrusterDynamicEffector::computeForceTorque(double integTime, double timeSt this->torqueExternalPntB_B.setZero(); double dt = integTime - prevFireTime; - axesWeightMatrix << 2, 0, 0, 0, 1, 0, 0, 0, 1; + axesWeightMatrix << 2, 0, 0, 0, 1, 0, 0, 0, 1; // Loop variables std::shared_ptr it; @@ -281,12 +279,12 @@ void ThrusterDynamicEffector::computeForceTorque(double integTime, double timeSt // Iterate through all of the thrusters to aggregate the force/torque in the system int index; - for(index = 0; index < this->thrusterData.size(); ++index) - { + for (index = 0; index < this->thrusterData.size(); ++index) { it = this->thrusterData[index]; ops = &it->ThrustOps; - // Compute the thruster properties wrt the hub (note that B refers to the F frame when extracting from the thruster info) + // Compute the thruster properties wrt the hub (note that B refers to the F frame when extracting from the + // thruster info) thrustDirection_B = this->bodyToHubInfo.at(index).dcm_BF * it->thrDir_B; thrustLocation_B = this->bodyToHubInfo.at(index).r_FB_B + this->bodyToHubInfo.at(index).dcm_BF * it->thrLoc_B; @@ -296,14 +294,11 @@ void ThrusterDynamicEffector::computeForceTorque(double integTime, double timeSt } // For each thruster see if the on-time is still valid and if so, call ComputeThrusterFire() - if((ops->ThrustOnCmd + ops->ThrusterStartTime - integTime) >= -dt*10E-10 && - ops->ThrustOnCmd > 0.0) - { + if ((ops->ThrustOnCmd + ops->ThrusterStartTime - integTime) >= -dt * 10E-10 && ops->ThrustOnCmd > 0.0) { ComputeThrusterFire(it, integTime); } // If we are not actively firing, continue shutdown process for active thrusters - else if(ops->ThrustFactor > 0.0) - { + else if (ops->ThrustFactor > 0.0) { ComputeThrusterShut(it, integTime); } @@ -316,33 +311,31 @@ void ThrusterDynamicEffector::computeForceTorque(double integTime, double timeSt this->forceExternal_B += SingleThrusterForce; // Compute the point B relative torque and aggregate into the composite body torque - SingleThrusterTorque = thrustLocation_B.cross(SingleThrusterForce) + ops->ThrustFactor * - ops->thrustBlowDownFactor * it->MaxSwirlTorque * thrustDirection_B; + SingleThrusterTorque = thrustLocation_B.cross(SingleThrusterForce) + + ops->ThrustFactor * ops->thrustBlowDownFactor * it->MaxSwirlTorque * thrustDirection_B; this->torqueExternalPntB_B += SingleThrusterTorque; - if (!it->updateOnly) { - // Add the mass depletion force contribution - mDotNozzle = 0.0; - if (it->steadyIsp * ops->IspFactor * ops->ispBlowDownFactor > 0.0) - { - mDotNozzle = tmpThrustMag / (EARTH_GRAV * it->steadyIsp * ops->IspFactor * ops->ispBlowDownFactor); - } - this->forceExternal_B += 2 * mDotNozzle * (this->bodyToHubInfo.at(index).omega_FB_B + - omegaLocal_BN_B).cross(thrustLocation_B); - - // Add the mass depletion torque contribution - BM1 = thrustDirection_B; - BM2 << -BM1(1), BM1(0), BM1(2); - BM3 = BM1.cross(BM2); - BMj.col(0) = BM1; - BMj.col(1) = BM2; - BMj.col(2) = BM3; - this->torqueExternalPntB_B += mDotNozzle * (eigenTilde(thrustDirection_B) * - eigenTilde(thrustDirection_B).transpose() + it->areaNozzle / (4 * M_PI) * - BMj * axesWeightMatrix * BMj.transpose()) * + if (!it->updateOnly) { + // Add the mass depletion force contribution + mDotNozzle = 0.0; + if (it->steadyIsp * ops->IspFactor * ops->ispBlowDownFactor > 0.0) { + mDotNozzle = tmpThrustMag / (EARTH_GRAV * it->steadyIsp * ops->IspFactor * ops->ispBlowDownFactor); + } + this->forceExternal_B += + 2 * mDotNozzle * (this->bodyToHubInfo.at(index).omega_FB_B + omegaLocal_BN_B).cross(thrustLocation_B); + + // Add the mass depletion torque contribution + BM1 = thrustDirection_B; + BM2 << -BM1(1), BM1(0), BM1(2); + BM3 = BM1.cross(BM2); + BMj.col(0) = BM1; + BMj.col(1) = BM2; + BMj.col(2) = BM3; + this->torqueExternalPntB_B += mDotNozzle * + (eigenTilde(thrustDirection_B) * eigenTilde(thrustDirection_B).transpose() + + it->areaNozzle / (4 * M_PI) * BMj * axesWeightMatrix * BMj.transpose()) * (this->bodyToHubInfo.at(index).omega_FB_B + omegaLocal_BN_B); - - } + } // Save force and torque values for messages eigenVector3d2CArray(SingleThrusterForce, it->ThrustOps.opThrustForce_B); eigenVector3d2CArray(SingleThrusterTorque, it->ThrustOps.opThrustTorquePntB_B); @@ -355,7 +348,8 @@ void ThrusterDynamicEffector::computeForceTorque(double integTime, double timeSt @param newThruster thruster sim config(s) */ -void ThrusterDynamicEffector::addThruster(std::shared_ptr newThruster) +void +ThrusterDynamicEffector::addThruster(std::shared_ptr newThruster) { this->thrusterData.push_back(newThruster); @@ -381,7 +375,9 @@ void ThrusterDynamicEffector::addThruster(std::shared_ptr newThrus @param newThruster thruster sim config(s) @param bodyStateMsg body states to which thruster(s) are attached */ -void ThrusterDynamicEffector::addThruster(std::shared_ptr newThruster, Message* bodyStateMsg) +void +ThrusterDynamicEffector::addThruster(std::shared_ptr newThruster, + Message* bodyStateMsg) { this->thrusterData.push_back(newThruster); @@ -403,57 +399,60 @@ void ThrusterDynamicEffector::addThruster(std::shared_ptr newThrus return; } - /*! This method is used to update the blow down effects to the thrust and/or Isp * at every computeForceTorque call when the thrusters are attached to a fuel * tank subject to blow down effects. */ -void ThrusterDynamicEffector::computeBlowDownDecay(std::shared_ptr currentThruster) +void +ThrusterDynamicEffector::computeBlowDownDecay(std::shared_ptr currentThruster) { - THROperation *ops = &(currentThruster->ThrustOps); + THROperation* ops = &(currentThruster->ThrustOps); if (!currentThruster->thrBlowDownCoeff.empty()) { double thrustBlowDown = 0.0; double thrOrder = 1.0; - for(auto thrCoeff = currentThruster->thrBlowDownCoeff.rbegin(); thrCoeff != - currentThruster->thrBlowDownCoeff.rend(); thrCoeff++) { + for (auto thrCoeff = currentThruster->thrBlowDownCoeff.rbegin(); + thrCoeff != currentThruster->thrBlowDownCoeff.rend(); + thrCoeff++) { thrustBlowDown += *thrCoeff * thrOrder; thrOrder *= fuelMass; // Fuel mass assigned in fuel tank's updateEffectorMassProps method } - ops->thrustBlowDownFactor = std::clamp(thrustBlowDown / currentThruster->MaxThrust, double (0.0), double (1.0)); + ops->thrustBlowDownFactor = std::clamp(thrustBlowDown / currentThruster->MaxThrust, double(0.0), double(1.0)); } if (!currentThruster->ispBlowDownCoeff.empty()) { double ispBlowDown = 0.0; double ispOrder = 1.0; - for (auto ispCoeff = currentThruster->ispBlowDownCoeff.rbegin(); ispCoeff != - currentThruster->ispBlowDownCoeff.rend(); ispCoeff++) { + for (auto ispCoeff = currentThruster->ispBlowDownCoeff.rbegin(); + ispCoeff != currentThruster->ispBlowDownCoeff.rend(); + ispCoeff++) { ispBlowDown += *ispCoeff * ispOrder; ispOrder *= fuelMass; // Fuel mass assigned in fuel tank's updateEffectorMassProps method } - ops->ispBlowDownFactor = std::clamp(ispBlowDown / currentThruster->steadyIsp, double (0.0), double (1.0)); + ops->ispBlowDownFactor = std::clamp(ispBlowDown / currentThruster->steadyIsp, double(0.0), double(1.0)); } } /*! This method computes contributions to the fuel mass depletion. */ -void ThrusterDynamicEffector::computeStateContribution(double integTime){ +void +ThrusterDynamicEffector::computeStateContribution(double integTime) +{ std::vector>::iterator itp; std::shared_ptr it; - THROperation *ops; - double mDotSingle=0.0; + THROperation* ops; + double mDotSingle = 0.0; this->mDotTotal = 0.0; - this->stateDerivContribution.setZero(); + this->stateDerivContribution.setZero(); // Iterate through all of the thrusters to aggregate the force/torque in the system - for(itp = this->thrusterData.begin(); itp != this->thrusterData.end(); itp++) - { + for (itp = this->thrusterData.begin(); itp != this->thrusterData.end(); itp++) { it = *itp; ops = &it->ThrustOps; mDotSingle = 0.0; - if(it->steadyIsp * ops->IspFactor * ops->ispBlowDownFactor > 0.0) - { - mDotSingle = it->MaxThrust * ops->ThrustFactor * ops->thrustBlowDownFactor / (EARTH_GRAV * it->steadyIsp * ops->IspFactor * ops->ispBlowDownFactor); + if (it->steadyIsp * ops->IspFactor * ops->ispBlowDownFactor > 0.0) { + mDotSingle = it->MaxThrust * ops->ThrustFactor * ops->thrustBlowDownFactor / + (EARTH_GRAV * it->steadyIsp * ops->IspFactor * ops->ispBlowDownFactor); } this->mDotTotal += mDotSingle; } @@ -462,7 +461,6 @@ void ThrusterDynamicEffector::computeStateContribution(double integTime){ return; } - /*! This method is used to get the current force for a thruster firing. It uses the configuration data associated with a given thruster and the current clock time to determine what state and force the thruster should be in. @@ -470,38 +468,31 @@ void ThrusterDynamicEffector::computeStateContribution(double integTime){ @param CurrentThruster Pointer to the configuration data for a given thruster @param currentTime The current simulation clock time converted to a double */ -void ThrusterDynamicEffector::ComputeThrusterFire(std::shared_ptr CurrentThruster, - double currentTime) +void +ThrusterDynamicEffector::ComputeThrusterFire(std::shared_ptr CurrentThruster, double currentTime) { std::vector::iterator it; - THROperation *ops = &(CurrentThruster->ThrustOps); + THROperation* ops = &(CurrentThruster->ThrustOps); // Set the current ramp time for the thruster firing - if(ops->ThrustOnRampTime == 0.0 && - CurrentThruster->ThrusterOnRamp.size() > 0) - { - ops->ThrustOnRampTime = thrFactorToTime(CurrentThruster, - &(CurrentThruster->ThrusterOnRamp)); + if (ops->ThrustOnRampTime == 0.0 && CurrentThruster->ThrusterOnRamp.size() > 0) { + ops->ThrustOnRampTime = thrFactorToTime(CurrentThruster, &(CurrentThruster->ThrusterOnRamp)); } - double LocalOnRamp = (currentTime - ops->PreviousIterTime) + - ops->ThrustOnRampTime; + double LocalOnRamp = (currentTime - ops->PreviousIterTime) + ops->ThrustOnRampTime; LocalOnRamp = LocalOnRamp >= 0.0 ? LocalOnRamp : 0.0; double prevValidThrFactor = 0.0; double prevValidIspFactor = 0.0; double prevValidDelta = 0.0; // Iterate through the on-ramp for the thruster data to find where we are in ramp - for(it = CurrentThruster->ThrusterOnRamp.begin(); - it != CurrentThruster->ThrusterOnRamp.end(); it++) - { + for (it = CurrentThruster->ThrusterOnRamp.begin(); it != CurrentThruster->ThrusterOnRamp.end(); it++) { // If the current on-time is less than the ramp delta, set that ramp thrust factor - if(LocalOnRamp < it->TimeDelta) - { - ops->ThrustFactor = (it->ThrustFactor - prevValidThrFactor)/ - (it->TimeDelta - prevValidDelta) * - (LocalOnRamp - prevValidDelta) + prevValidThrFactor; - ops->IspFactor = (it->IspFactor - prevValidIspFactor)/ - (it->TimeDelta - prevValidDelta) * - (LocalOnRamp - prevValidDelta) + prevValidIspFactor; + if (LocalOnRamp < it->TimeDelta) { + ops->ThrustFactor = (it->ThrustFactor - prevValidThrFactor) / (it->TimeDelta - prevValidDelta) * + (LocalOnRamp - prevValidDelta) + + prevValidThrFactor; + ops->IspFactor = + (it->IspFactor - prevValidIspFactor) / (it->TimeDelta - prevValidDelta) * (LocalOnRamp - prevValidDelta) + + prevValidIspFactor; ops->ThrustOnRampTime = LocalOnRamp; ops->totalOnTime += (currentTime - ops->PreviousIterTime); ops->PreviousIterTime = currentTime; @@ -520,7 +511,6 @@ void ThrusterDynamicEffector::ComputeThrusterFire(std::shared_ptr ops->ThrustOffRampTime = 0.0; } - /*! This method is used to go through the process of shutting down a thruster once it has been commanded off. It uses the configuration data associated with a given thruster and the current clock time to turn off the thruster according @@ -529,38 +519,31 @@ void ThrusterDynamicEffector::ComputeThrusterFire(std::shared_ptr @param CurrentThruster Pointer to the configuration data for a given thruster @param currentTime The current simulation clock time converted to a double */ -void ThrusterDynamicEffector::ComputeThrusterShut(std::shared_ptr CurrentThruster, - double currentTime) +void +ThrusterDynamicEffector::ComputeThrusterShut(std::shared_ptr CurrentThruster, double currentTime) { std::vector::iterator it; - THROperation *ops = &(CurrentThruster->ThrustOps); + THROperation* ops = &(CurrentThruster->ThrustOps); // Set the current off-ramp time based on the previous clock time and now - if(ops->ThrustOffRampTime == 0.0 && - CurrentThruster->ThrusterOffRamp.size() > 0) - { - ops->ThrustOffRampTime = thrFactorToTime(CurrentThruster, - &(CurrentThruster->ThrusterOffRamp)); + if (ops->ThrustOffRampTime == 0.0 && CurrentThruster->ThrusterOffRamp.size() > 0) { + ops->ThrustOffRampTime = thrFactorToTime(CurrentThruster, &(CurrentThruster->ThrusterOffRamp)); } - double LocalOffRamp = (currentTime - ops->PreviousIterTime) + - ops->ThrustOffRampTime; + double LocalOffRamp = (currentTime - ops->PreviousIterTime) + ops->ThrustOffRampTime; LocalOffRamp = LocalOffRamp >= 0.0 ? LocalOffRamp : 0.0; double prevValidThrFactor = 1.0; double prevValidIspFactor = 1.0; double prevValidDelta = 0.0; // Iterate through the off-ramp to find the place where we are in the shutdown ramp - for(it = CurrentThruster->ThrusterOffRamp.begin(); - it != CurrentThruster->ThrusterOffRamp.end(); it++) - { + for (it = CurrentThruster->ThrusterOffRamp.begin(); it != CurrentThruster->ThrusterOffRamp.end(); it++) { // Once we find the location in the off-ramp, set that thrust factor to current - if(LocalOffRamp < it->TimeDelta) - { - ops->ThrustFactor = (it->ThrustFactor - prevValidThrFactor)/ - (it->TimeDelta - prevValidDelta) * - (LocalOffRamp - prevValidDelta) + prevValidThrFactor; - ops->IspFactor = (it->IspFactor - prevValidIspFactor)/ - (it->TimeDelta - prevValidDelta) * - (LocalOffRamp - prevValidDelta) + prevValidIspFactor; + if (LocalOffRamp < it->TimeDelta) { + ops->ThrustFactor = (it->ThrustFactor - prevValidThrFactor) / (it->TimeDelta - prevValidDelta) * + (LocalOffRamp - prevValidDelta) + + prevValidThrFactor; + ops->IspFactor = (it->IspFactor - prevValidIspFactor) / (it->TimeDelta - prevValidDelta) * + (LocalOffRamp - prevValidDelta) + + prevValidIspFactor; ops->ThrustOffRampTime = LocalOffRamp; ops->PreviousIterTime = currentTime; return; @@ -582,46 +565,41 @@ void ThrusterDynamicEffector::ComputeThrusterShut(std::shared_ptr @param thrData The data for the thruster that we are currently firing @param thrRamp This just allows us to avoid switching to figure out which ramp */ -double ThrusterDynamicEffector::thrFactorToTime(std::shared_ptr thrData, - std::vector *thrRamp) +double +ThrusterDynamicEffector::thrFactorToTime(std::shared_ptr thrData, std::vector* thrRamp) { std::vector::iterator it; // Grab the last element in the ramp and determine if it goes up or down it = thrRamp->end(); it--; double rampTime = it->TimeDelta; - double rampDirection = std::copysign(1.0, - it->ThrustFactor - thrData->ThrustOps.ThrustFactor); + double rampDirection = std::copysign(1.0, it->ThrustFactor - thrData->ThrustOps.ThrustFactor); // Initialize the time computation functiosn based on ramp direction double prevValidThrFactor = rampDirection < 0 ? 1.0 : 0.0; double prevValidDelta = 0.0; - for(it=thrRamp->begin(); it!=thrRamp->end(); it++) - { + for (it = thrRamp->begin(); it != thrRamp->end(); it++) { // Determine if we haven't reached the right place in the ramp - bool pointCheck = rampDirection > 0 ? - it->ThrustFactor <= thrData->ThrustOps.ThrustFactor : - it->ThrustFactor >= thrData->ThrustOps.ThrustFactor; + bool pointCheck = rampDirection > 0 ? it->ThrustFactor <= thrData->ThrustOps.ThrustFactor + : it->ThrustFactor >= thrData->ThrustOps.ThrustFactor; // If we have not located the location in the ramp, continue - if(pointCheck) - { + if (pointCheck) { prevValidThrFactor = it->ThrustFactor; prevValidDelta = it->TimeDelta; continue; } // Linearly interpolate between the points, check for numerical garbage, and return clean interpolation - rampTime = (it->TimeDelta - prevValidDelta)/(it->ThrustFactor - - prevValidThrFactor) * (thrData->ThrustOps.ThrustFactor - - prevValidThrFactor) + prevValidDelta; + rampTime = (it->TimeDelta - prevValidDelta) / (it->ThrustFactor - prevValidThrFactor) * + (thrData->ThrustOps.ThrustFactor - prevValidThrFactor) + + prevValidDelta; rampTime = rampTime < 0.0 ? 0.0 : rampTime; break; } - return(rampTime); + return (rampTime); } - /*! This method is the main cyclical call for the scheduled part of the thruster dynamics model. It reads the current commands array and sets the thruster configuration data based on that incoming command set. Note that the main @@ -630,12 +608,12 @@ double ThrusterDynamicEffector::thrFactorToTime(std::shared_ptr th @param CurrentSimNanos The current simulation time in nanoseconds */ -void ThrusterDynamicEffector::UpdateState(uint64_t CurrentSimNanos) +void +ThrusterDynamicEffector::UpdateState(uint64_t CurrentSimNanos) { // Read the inputs and then call ConfigureThrustRequests to set up dynamics - if(this->ReadInputs()) - { - this->ConfigureThrustRequests(this->prevCommandTime*1.0E-9); + if (this->ReadInputs()) { + this->ConfigureThrustRequests(this->prevCommandTime * 1.0E-9); } this->UpdateThrusterProperties(); this->writeOutputMessages(CurrentSimNanos); diff --git a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h index b3349ee8ca..fc42b8290e 100644 --- a/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h +++ b/src/simulation/dynamics/Thrusters/thrusterDynamicEffector/thrusterDynamicEffector.h @@ -17,7 +17,6 @@ */ - #ifndef THRUSTER_DYNAMIC_EFFECTOR_H #define THRUSTER_DYNAMIC_EFFECTOR_H @@ -38,11 +37,12 @@ #include #include - - /*! @brief thruster dynamic effector class */ -class ThrusterDynamicEffector: public SysModel, public DynamicEffector { -public: +class ThrusterDynamicEffector + : public SysModel + , public DynamicEffector +{ + public: ThrusterDynamicEffector(); ~ThrusterDynamicEffector(); void linkInStates(DynParamManager& states); @@ -60,35 +60,33 @@ class ThrusterDynamicEffector: public SysModel, public DynamicEffector { void UpdateThrusterProperties(); void computeBlowDownDecay(std::shared_ptr CurrentThruster); -public: - ReadFunctor cmdsInMsg; //!< -- input message with thruster commands - std::vector*> thrusterOutMsgs; //!< -- output message vector for thruster data + public: + ReadFunctor cmdsInMsg; //!< -- input message with thruster commands + std::vector*> thrusterOutMsgs; //!< -- output message vector for thruster data - int stepsInRamp; //!< class variable + int stepsInRamp; //!< class variable std::vector> thrusterData; //!< -- Thruster information - std::vector NewThrustCmds; //!< -- Incoming thrust commands - double mDotTotal; //!< kg/s Current mass flow rate of thrusters - double fuelMass; //!< kg Current total fuel mass of connected fuel tank - double prevFireTime; //!< s Previous thruster firing time - double thrFactorToTime(std::shared_ptr thrData, - std::vector *thrRamp); - StateData *hubSigma; //!< pointer to the hub attitude states - StateData *hubOmega; //!< pointer to the hub angular velocity states - Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - - BSKLogger bskLogger; //!< -- BSK Logging - -private: - std::vector thrusterOutBuffer;//!< -- Message buffer for thruster data - THRArrayOnTimeCmdMsgPayload incomingCmdBuffer; //!< -- One-time allocation for savings - - std::vector> attachedBodyInMsgs; //!< vector of body states message where the thrusters attach to + std::vector NewThrustCmds; //!< -- Incoming thrust commands + double mDotTotal; //!< kg/s Current mass flow rate of thrusters + double fuelMass; //!< kg Current total fuel mass of connected fuel tank + double prevFireTime; //!< s Previous thruster firing time + double thrFactorToTime(std::shared_ptr thrData, std::vector* thrRamp); + StateData* hubSigma; //!< pointer to the hub attitude states + StateData* hubOmega; //!< pointer to the hub angular velocity states + Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + + BSKLogger bskLogger; //!< -- BSK Logging + + private: + std::vector thrusterOutBuffer; //!< -- Message buffer for thruster data + THRArrayOnTimeCmdMsgPayload incomingCmdBuffer; //!< -- One-time allocation for savings + + std::vector> + attachedBodyInMsgs; //!< vector of body states message where the thrusters attach to SCStatesMsgPayload attachedBodyBuffer; std::vector bodyToHubInfo; - uint64_t prevCommandTime; //!< -- Time for previous valid thruster firing - + uint64_t prevCommandTime; //!< -- Time for previous valid thruster firing }; - #endif /* THRUSTER_DYNAMIC_EFFECTOR_H */ 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/Thrusters/thrusterStateEffector/thrusterStateEffector.cpp b/src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.cpp index 14f1abfd13..63cf243940 100644 --- a/src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.cpp +++ b/src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.cpp @@ -41,7 +41,7 @@ ThrusterStateEffector::ThrusterStateEffector() // initialize internal variables CallCounts = 0; - this->prevCommandTime = -1.0; // initialize to a negative number to allow an onTime command at t=0 + this->prevCommandTime = -1.0; // initialize to a negative number to allow an onTime command at t=0 this->mDotTotal = 0.0; this->nameOfKappaState = "kappaState" + std::to_string(this->effectorID); this->effectorID++; @@ -60,11 +60,11 @@ uint64_t ThrusterStateEffector::effectorID = 1; ThrusterStateEffector::~ThrusterStateEffector() { // Free memory to avoid errors - for (long unsigned int c=0; cthrusterOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->thrusterOutMsgs.size(); c++) { free(this->thrusterOutMsgs.at(c)); } - this->effectorID = 1; /* reset the panel ID*/ + this->effectorID = 1; /* reset the panel ID*/ return; } @@ -72,7 +72,8 @@ ThrusterStateEffector::~ThrusterStateEffector() /*! This method is used to reset the module. */ -void ThrusterStateEffector::Reset(uint64_t CurrentSimNanos) +void +ThrusterStateEffector::Reset(uint64_t CurrentSimNanos) { // Clear out any currently firing thrusters and re-init cmd array this->NewThrustCmds.clear(); @@ -88,7 +89,8 @@ void ThrusterStateEffector::Reset(uint64_t CurrentSimNanos) associated command structure for operating the thrusters. */ -bool ThrusterStateEffector::ReadInputs() +bool +ThrusterStateEffector::ReadInputs() { // Initialize local variables uint64_t i; @@ -101,8 +103,8 @@ bool ThrusterStateEffector::ReadInputs() dataGood = this->cmdsInMsg.isWritten(); //! - Check if message has already been read, if so then stale return - if(abs(this->prevCommandTime - this->cmdsInMsg.timeWritten() * NANO2SEC) < 1E-9 || !dataGood) { - return(false); + if (abs(this->prevCommandTime - this->cmdsInMsg.timeWritten() * NANO2SEC) < 1E-9 || !dataGood) { + return (false); } this->prevCommandTime = this->cmdsInMsg.timeWritten() * NANO2SEC; } else { @@ -111,14 +113,11 @@ bool ThrusterStateEffector::ReadInputs() } // Set the NewThrustCmds vector. Using the data() method for raw speed - double *CmdPtr; - for(i=0, CmdPtr = NewThrustCmds.data(); i < this->thrusterData.size(); - CmdPtr++, i++) - { + double* CmdPtr; + for (i = 0, CmdPtr = NewThrustCmds.data(); i < this->thrusterData.size(); CmdPtr++, i++) { *CmdPtr = this->incomingCmdBuffer.OnTimeRequest[i]; } - return(true); - + return (true); } /*! This method is here to write the output message structure into the specified @@ -126,15 +125,15 @@ bool ThrusterStateEffector::ReadInputs() @param CurrentClock The current time used for time-stamping the message */ -void ThrusterStateEffector::writeOutputStateMessages(uint64_t CurrentClock) +void +ThrusterStateEffector::writeOutputStateMessages(uint64_t CurrentClock) { int idx = 0; std::vector>::iterator itp; std::shared_ptr it; THROutputMsgPayload tmpThruster; - for (itp = this->thrusterData.begin(); itp != this->thrusterData.end(); ++itp) - { + for (itp = this->thrusterData.begin(); itp != this->thrusterData.end(); ++itp) { it = *itp; tmpThruster = this->thrusterOutMsgs[idx]->zeroMsgPayload; eigenVector3d2CArray(it->thrLoc_B, tmpThruster.thrusterLocation); @@ -158,27 +157,26 @@ void ThrusterStateEffector::writeOutputStateMessages(uint64_t CurrentClock) into NewThrustCmds. */ -void ThrusterStateEffector::ConfigureThrustRequests() +void +ThrusterStateEffector::ConfigureThrustRequests() { std::vector::iterator CmdIt; size_t THIter = 0; //! - Iterate through the list of thruster commands that we read in. - for(CmdIt = NewThrustCmds.begin(); CmdIt != NewThrustCmds.end(); CmdIt++) - { + for (CmdIt = NewThrustCmds.begin(); CmdIt != NewThrustCmds.end(); CmdIt++) { if (*CmdIt >= this->thrusterData[THIter]->MinOnTime) /// - Check to see if we have met minimum for each thruster { //! - For each case where we are above the minimum firing request, reset the thruster this->thrusterData[THIter]->ThrustOps.ThrustOnCmd = *CmdIt; - this->thrusterData[THIter]->ThrustOps.fireCounter += this->thrusterData[THIter]->ThrustOps.ThrustFactor > 0.0 - ? 0 : 1; - } - else - { + this->thrusterData[THIter]->ThrustOps.fireCounter += + this->thrusterData[THIter]->ThrustOps.ThrustFactor > 0.0 ? 0 : 1; + } else { //! - Will ensure that thruster shuts down once this cmd expires - this->thrusterData[THIter]->ThrustOps.ThrustOnCmd = this->thrusterData[THIter]->ThrustOps.ThrustFactor > 1E-5 - ? *CmdIt : 0.0; + this->thrusterData[THIter]->ThrustOps.ThrustOnCmd = + this->thrusterData[THIter]->ThrustOps.ThrustFactor > 1E-5 ? *CmdIt : 0.0; } - this->thrusterData[THIter]->ThrustOps.ThrusterEndTime = this->prevCommandTime + this->thrusterData[THIter]->ThrustOps.ThrustOnCmd; + this->thrusterData[THIter]->ThrustOps.ThrusterEndTime = + this->prevCommandTime + this->thrusterData[THIter]->ThrustOps.ThrustOnCmd; //! After we have assigned the firing to the internal thruster, zero the command request. *CmdIt = 0.0; THIter++; @@ -192,16 +190,18 @@ void ThrusterStateEffector::ConfigureThrustRequests() * the hub. */ -void ThrusterStateEffector::UpdateThrusterProperties() +void +ThrusterStateEffector::UpdateThrusterProperties() { // Save hub variables - Eigen::Vector3d r_BN_N = (Eigen::Vector3d)*this->inertialPositionProperty; + Eigen::Vector3d r_BN_N = (Eigen::Vector3d) * this->inertialPositionProperty; Eigen::Vector3d omega_BN_B = this->hubOmega->getState(); Eigen::MRPd sigma_BN; - sigma_BN = (Eigen::Vector3d) this->hubSigma->getState(); + sigma_BN = (Eigen::Vector3d)this->hubSigma->getState(); Eigen::Matrix3d dcm_BN = (sigma_BN.toRotationMatrix()).transpose(); - // Define the variables related to which body the thruster is attached to. The F frame represents the platform body where the thruster attaches to + // Define the variables related to which body the thruster is attached to. The F frame represents the platform body + // where the thruster attaches to Eigen::MRPd sigma_FN; Eigen::Matrix3d dcm_FN; Eigen::Vector3d omega_FN_F; @@ -213,11 +213,9 @@ void ThrusterStateEffector::UpdateThrusterProperties() // Loop through all thrusters std::vector>::iterator it; int index; - for (it = this->attachedBodyInMsgs.begin(), index = 0; it != this->attachedBodyInMsgs.end(); it++, index++) - { + for (it = this->attachedBodyInMsgs.begin(), index = 0; it != this->attachedBodyInMsgs.end(); it++, index++) { // Check if the message is linked, and if so do the conversion - if (it->isLinked() && it->isWritten()) - { + if (it->isLinked() && it->isWritten()) { // Save to buffer this->attachedBodyBuffer = this->attachedBodyInMsgs.at(index)(); @@ -238,7 +236,8 @@ void ThrusterStateEffector::UpdateThrusterProperties() } } -void ThrusterStateEffector::addThruster(std::shared_ptr newThruster) +void +ThrusterStateEffector::addThruster(std::shared_ptr newThruster) { this->thrusterData.push_back(newThruster); @@ -263,7 +262,8 @@ void ThrusterStateEffector::addThruster(std::shared_ptr newThruste this->bodyToHubInfo.push_back(attachedBodyToHub); } -void ThrusterStateEffector::addThruster(std::shared_ptr newThruster, Message* bodyStateMsg) +void +ThrusterStateEffector::addThruster(std::shared_ptr newThruster, Message* bodyStateMsg) { this->thrusterData.push_back(newThruster); @@ -293,23 +293,29 @@ void ThrusterStateEffector::addThruster(std::shared_ptr newThruste @param states The states to link */ -void ThrusterStateEffector::linkInStates(DynParamManager& states){ +void +ThrusterStateEffector::linkInStates(DynParamManager& states) +{ this->hubSigma = states.getStateObject(this->stateNameOfSigma); this->hubOmega = states.getStateObject(this->stateNameOfOmega); - this->inertialPositionProperty = states.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); + this->inertialPositionProperty = + states.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); } /*! This method allows the thruster state effector to register its state kappa with the dyn param manager */ -void ThrusterStateEffector::registerStates(DynParamManager& states) +void +ThrusterStateEffector::registerStates(DynParamManager& states) { // - Register the states associated with thruster - kappa - this->kappaState = states.registerState((uint32_t) this->thrusterData.size(), 1, this->nameOfKappaState); + this->kappaState = states.registerState((uint32_t)this->thrusterData.size(), 1, this->nameOfKappaState); Eigen::MatrixXd kappaInitMatrix(this->thrusterData.size(), 1); // Loop through all thrusters to initialize each state variable for (uint64_t i = 0; i < this->thrusterData.size(); i++) { // Make sure that the thruster state is between 0 and 1 if (this->kappaInit[i] < 0.0 || this->kappaInit[i] > 1.0) { - bskLogger.bskLog(BSK_ERROR, "thrusterStateEffector: the initial condition for the thrust factor must be between 0 and 1. Setting it to 0."); + bskLogger.bskLog(BSK_ERROR, + "thrusterStateEffector: the initial condition for the thrust factor must be between 0 and " + "1. Setting it to 0."); this->kappaInit[i] = 0.0; } kappaInitMatrix(i, 0) = this->kappaInit[i]; @@ -320,7 +326,11 @@ void ThrusterStateEffector::registerStates(DynParamManager& states) } /*! This method is used to find the derivatives for the thruster stateEffector */ -void ThrusterStateEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +ThrusterStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { std::vector>::iterator itp; std::shared_ptr it; @@ -331,8 +341,7 @@ void ThrusterStateEffector::computeDerivatives(double integTime, Eigen::Vector3d Eigen::MatrixXd kappaDot(this->thrusterData.size(), 1); // Loop through all thrusters to initialize each state variable - for (itp = this->thrusterData.begin(), i = 0; itp != this->thrusterData.end(); itp++, i++) - { + for (itp = this->thrusterData.begin(), i = 0; itp != this->thrusterData.end(); itp++, i++) { it = *itp; // Grab the thruster operations payload ops = &it->ThrustOps; @@ -340,8 +349,7 @@ void ThrusterStateEffector::computeDerivatives(double integTime, Eigen::Vector3d //! - For each thruster check if the end time is greater than the current time, and if so thrust if ((ops->ThrusterEndTime - integTime) >= 0.0 && ops->ThrustOnCmd > 0.0) { kappaDot(i, 0) = (1.0 - this->kappaState->state(i, 0)) * it->cutoffFrequency; - } - else { + } else { kappaDot(i, 0) = -this->kappaState->state(i, 0) * it->cutoffFrequency; } @@ -353,7 +361,8 @@ void ThrusterStateEffector::computeDerivatives(double integTime, Eigen::Vector3d return; } -void ThrusterStateEffector::calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B) +void +ThrusterStateEffector::calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B) { // Save omega_BN_B Eigen::Vector3d omegaLocal_BN_B = omega_BN_B; @@ -369,7 +378,7 @@ void ThrusterStateEffector::calcForceTorqueOnBody(double integTime, Eigen::Vecto // Expelled momentum variables Eigen::Matrix3d BMj; - Eigen::Matrix3d axesWeightMatrix; + Eigen::Matrix3d axesWeightMatrix; Eigen::Vector3d BM1, BM2, BM3; double mDotNozzle; @@ -387,13 +396,13 @@ void ThrusterStateEffector::calcForceTorqueOnBody(double integTime, Eigen::Vecto //! - Iterate through all of the thrusters to aggregate the force/torque in the system int index; - for(index = 0; index < this->thrusterData.size(); ++index) - { + for (index = 0; index < this->thrusterData.size(); ++index) { // Save the thruster ops information it = this->thrusterData[index]; ops = &it->ThrustOps; - // Compute the thruster properties wrt the hub (note that B refers to the F frame when extracting from the thruster info) + // Compute the thruster properties wrt the hub (note that B refers to the F frame when extracting from the + // thruster info) thrustDirection_B = this->bodyToHubInfo.at(index).dcm_BF * it->thrDir_B; thrustLocation_B = this->bodyToHubInfo.at(index).r_FB_B + this->bodyToHubInfo.at(index).dcm_BF * it->thrLoc_B; @@ -405,17 +414,18 @@ void ThrusterStateEffector::calcForceTorqueOnBody(double integTime, Eigen::Vecto this->forceOnBody_B += SingleThrusterForce; //! - Compute the point B relative torque and aggregate into the composite body torque - SingleThrusterTorque = thrustLocation_B.cross(SingleThrusterForce) + ops->ThrustFactor * it->MaxSwirlTorque * thrustDirection_B; + SingleThrusterTorque = + thrustLocation_B.cross(SingleThrusterForce) + ops->ThrustFactor * it->MaxSwirlTorque * thrustDirection_B; this->torqueOnBodyPntB_B += SingleThrusterTorque; if (!it->updateOnly) { //! - Add the mass depletion force contribution mDotNozzle = 0.0; - if (it->steadyIsp * ops->ThrustFactor > 0.0) - { + if (it->steadyIsp * ops->ThrustFactor > 0.0) { mDotNozzle = it->MaxThrust * ops->ThrustFactor / (EARTH_GRAV * it->steadyIsp); } - this->forceOnBody_B += 2 * mDotNozzle * (this->bodyToHubInfo.at(index).omega_FB_B + omegaLocal_BN_B).cross(thrustLocation_B); + this->forceOnBody_B += + 2 * mDotNozzle * (this->bodyToHubInfo.at(index).omega_FB_B + omegaLocal_BN_B).cross(thrustLocation_B); //! - Add the mass depletion torque contribution BM1 = thrustDirection_B; @@ -424,9 +434,10 @@ void ThrusterStateEffector::calcForceTorqueOnBody(double integTime, Eigen::Vecto BMj.col(0) = BM1; BMj.col(1) = BM2; BMj.col(2) = BM3; - this->torqueOnBodyPntB_B += mDotNozzle * (eigenTilde(thrustDirection_B) * eigenTilde(thrustDirection_B).transpose() - + it->areaNozzle / (4 * M_PI) * BMj * axesWeightMatrix * BMj.transpose()) * (this->bodyToHubInfo.at(index).omega_FB_B + omegaLocal_BN_B); - + this->torqueOnBodyPntB_B += mDotNozzle * + (eigenTilde(thrustDirection_B) * eigenTilde(thrustDirection_B).transpose() + + it->areaNozzle / (4 * M_PI) * BMj * axesWeightMatrix * BMj.transpose()) * + (this->bodyToHubInfo.at(index).omega_FB_B + omegaLocal_BN_B); } // - Save force and torque values for messages eigenVector3d2CArray(SingleThrusterForce, it->ThrustOps.opThrustForce_B); @@ -436,16 +447,23 @@ void ThrusterStateEffector::calcForceTorqueOnBody(double integTime, Eigen::Vecto return; } -void ThrusterStateEffector::updateContributions(double integTime, BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) +void +ThrusterStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { // Define the translational and rotational contributions from the computed force and torque backSubContr.vecTrans = this->forceOnBody_B; backSubContr.vecRot = this->torqueOnBodyPntB_B; - } -/*! This is the method for the thruster effector to add its contributions to the mass props and mass prop rates of the vehicle */ -void ThrusterStateEffector::updateEffectorMassProps(double integTime) { +/*! This is the method for the thruster effector to add its contributions to the mass props and mass prop rates of the + * vehicle */ +void +ThrusterStateEffector::updateEffectorMassProps(double integTime) +{ std::vector>::iterator itp; std::shared_ptr it; @@ -454,13 +472,11 @@ void ThrusterStateEffector::updateEffectorMassProps(double integTime) { this->mDotTotal = 0.0; this->stateDerivContribution.setZero(); //! - Iterate through all of the thrusters to aggregate the mass flow rate in the system - for (itp = this->thrusterData.begin(); itp != this->thrusterData.end(); itp++) - { + for (itp = this->thrusterData.begin(); itp != this->thrusterData.end(); itp++) { it = *itp; ops = &it->ThrustOps; mDotSingle = 0.0; - if (it->steadyIsp * ops->ThrustFactor > 0.0) - { + if (it->steadyIsp * ops->ThrustFactor > 0.0) { mDotSingle = it->MaxThrust * ops->ThrustFactor / (EARTH_GRAV * it->steadyIsp); } this->mDotTotal += mDotSingle; @@ -468,7 +484,6 @@ void ThrusterStateEffector::updateEffectorMassProps(double integTime) { this->stateDerivContribution(0) = this->mDotTotal; return; - } /*! This method is the main cyclical call for the scheduled part of the thruster @@ -479,11 +494,11 @@ void ThrusterStateEffector::updateEffectorMassProps(double integTime) { @param CurrentSimNanos The current simulation time in nanoseconds */ -void ThrusterStateEffector::UpdateState(uint64_t CurrentSimNanos) +void +ThrusterStateEffector::UpdateState(uint64_t CurrentSimNanos) { //! - Read the inputs and then call ConfigureThrustRequests to set up dynamics - if (this->ReadInputs()) - { + if (this->ReadInputs()) { this->ConfigureThrustRequests(); } this->UpdateThrusterProperties(); diff --git a/src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.h b/src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.h index 58da634e4f..fb5c3a55f2 100644 --- a/src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.h +++ b/src/simulation/dynamics/Thrusters/thrusterStateEffector/thrusterStateEffector.h @@ -17,7 +17,6 @@ */ - #ifndef THRUSTER_STATE_EFFECTOR_H #define THRUSTER_STATE_EFFECTOR_H @@ -37,65 +36,73 @@ #include #include - - - /*! @brief thruster dynamic effector class */ -class ThrusterStateEffector: public StateEffector, public SysModel { -public: +class ThrusterStateEffector + : public StateEffector + , public SysModel +{ + public: ThrusterStateEffector(); ~ThrusterStateEffector(); void Reset(uint64_t CurrentSimNanos); bool ReadInputs(); void writeOutputStateMessages(uint64_t CurrentClock); - void registerStates(DynParamManager& states); //!< -- Method for the effector to register its states - void linkInStates(DynParamManager& states); //!< -- Method for the effector to get access of other states - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN); //!< -- Method for each stateEffector to calculate derivatives + void registerStates(DynParamManager& states); //!< -- Method for the effector to register its states + void linkInStates(DynParamManager& states); //!< -- Method for the effector to get access of other states + void computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN); //!< -- Method for each stateEffector to calculate derivatives void calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B); - void updateContributions(double integTime, BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N); //!< Method to pass the forces and torques onto the hub + void updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N); //!< Method to pass the forces and torques onto the hub void updateEffectorMassProps(double integTime); void UpdateState(uint64_t CurrentSimNanos); - void addThruster(std::shared_ptr newThruster); //!< -- Add a new thruster to the thruster set - void addThruster(std::shared_ptr newThruster, Message* bodyStateMsg); //!< -- (overloaded) Add a new thruster to the thruster set connect to a body different than the hub + void addThruster(std::shared_ptr newThruster, + Message* bodyStateMsg); //!< -- (overloaded) Add a new thruster to the thruster + //!< set connect to a body different than the hub void ConfigureThrustRequests(); void UpdateThrusterProperties(); -public: + public: // Input and output messages - ReadFunctor cmdsInMsg; //!< -- input message with thruster commands - std::vector*> thrusterOutMsgs; //!< -- output message vector for thruster data - std::vector> thrusterData; //!< -- Thruster information - std::vector NewThrustCmds; //!< -- Incoming thrust commands + ReadFunctor cmdsInMsg; //!< -- input message with thruster commands + std::vector*> thrusterOutMsgs; //!< -- output message vector for thruster data + std::vector> thrusterData; //!< -- Thruster information + std::vector NewThrustCmds; //!< -- Incoming thrust commands // State information - std::vector kappaInit; //!< [] Vector of initial thruster states - std::string nameOfKappaState; //!< -- Identifier for the kappa state data container + std::vector kappaInit; //!< [] Vector of initial thruster states + std::string nameOfKappaState; //!< -- Identifier for the kappa state data container // State structures - StateData *hubSigma; //!< pointer to hub attitude states - StateData *hubOmega; //!< pointer to hub angular velocity states - StateData* kappaState; //!< -- state manager of theta for hinged rigid body - Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + StateData* hubSigma; //!< pointer to hub attitude states + StateData* hubOmega; //!< pointer to hub angular velocity states + StateData* kappaState; //!< -- state manager of theta for hinged rigid body + Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - BSKLogger bskLogger; //!< -- BSK Logging + BSKLogger bskLogger; //!< -- BSK Logging // Mass flow rate - double mDotTotal = 0.0; //!< [kg/s] Current mass flow rate of thrusters + double mDotTotal = 0.0; //!< [kg/s] Current mass flow rate of thrusters -private: - std::vector thrusterOutBuffer;//!< -- Message buffer for thruster data + private: + std::vector thrusterOutBuffer; //!< -- Message buffer for thruster data - THRArrayOnTimeCmdMsgPayload incomingCmdBuffer; //!< -- One-time allocation for savings + THRArrayOnTimeCmdMsgPayload incomingCmdBuffer; //!< -- One-time allocation for savings - std::vector> attachedBodyInMsgs; //!< vector of body states message where the thrusters attach to + std::vector> + attachedBodyInMsgs; //!< vector of body states message where the thrusters attach to SCStatesMsgPayload attachedBodyBuffer; std::vector bodyToHubInfo; - double prevCommandTime; //!< [s] -- Time for previous valid thruster firing - static uint64_t effectorID; //!< [] ID number of this panel + double prevCommandTime; //!< [s] -- Time for previous valid thruster firing + static uint64_t effectorID; //!< [] ID number of this panel }; - #endif /* THRUSTER_STATE_EFFECTOR_H */ 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/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/VSCMGs/vscmgStateEffector.cpp b/src/simulation/dynamics/VSCMGs/vscmgStateEffector.cpp old mode 100755 new mode 100644 index 80313a3c74..4cdc243944 --- a/src/simulation/dynamics/VSCMGs/vscmgStateEffector.cpp +++ b/src/simulation/dynamics/VSCMGs/vscmgStateEffector.cpp @@ -17,40 +17,38 @@ */ - #include "vscmgStateEffector.h" #include #include VSCMGStateEffector::VSCMGStateEffector() { - this->prevCommandTime = 0xFFFFFFFFFFFFFFFF; + this->prevCommandTime = 0xFFFFFFFFFFFFFFFF; this->effProps.mEff = 0.0; this->effProps.IEffPntB_B.setZero(); this->effProps.rEff_CB_B.setZero(); - this->effProps.IEffPrimePntB_B.setZero(); - this->effProps.rEffPrime_CB_B.setZero(); + this->effProps.IEffPrimePntB_B.setZero(); + this->effProps.rEffPrime_CB_B.setZero(); this->nameOfVSCMGOmegasState = "VSCMGOmegas"; this->nameOfVSCMGThetasState = "VSCMGThetas"; - this->nameOfVSCMGGammasState = "VSCMGGammas"; - this->nameOfVSCMGGammaDotsState = "VSCMGGammaDots"; - + this->nameOfVSCMGGammasState = "VSCMGGammas"; + this->nameOfVSCMGGammaDotsState = "VSCMGGammaDots"; return; } - VSCMGStateEffector::~VSCMGStateEffector() { - for (long unsigned int c=0; cvscmgOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->vscmgOutMsgs.size(); c++) { free(this->vscmgOutMsgs.at(c)); } return; } -void VSCMGStateEffector::linkInStates(DynParamManager& statesIn) +void +VSCMGStateEffector::linkInStates(DynParamManager& statesIn) { this->hubOmega = statesIn.getStateObject(this->stateNameOfOmega); //! - Get access to the hub states @@ -59,38 +57,39 @@ void VSCMGStateEffector::linkInStates(DynParamManager& statesIn) return; } -void VSCMGStateEffector::registerStates(DynParamManager& states) +void +VSCMGStateEffector::registerStates(DynParamManager& states) { //! - Find number of VSCMGs and number of VSCMGs with jitter this->numVSCMGJitter = 0; this->numVSCMG = 0; //! zero the Omega and theta values - Eigen::MatrixXd omegasForInit(this->VSCMGData.size(),1); - Eigen::MatrixXd gammasForInit(this->VSCMGData.size(),1); - Eigen::MatrixXd gammaDotsForInit(this->VSCMGData.size(),1); + Eigen::MatrixXd omegasForInit(this->VSCMGData.size(), 1); + Eigen::MatrixXd gammasForInit(this->VSCMGData.size(), 1); + Eigen::MatrixXd gammaDotsForInit(this->VSCMGData.size(), 1); - std::vector::iterator it; - for(it=VSCMGData.begin(); it!=VSCMGData.end(); it++) { + std::vector::iterator it; + for (it = VSCMGData.begin(); it != VSCMGData.end(); it++) { if (it->VSCMGModel == vscmgJitterSimple || it->VSCMGModel == vscmgJitterFullyCoupled) { this->numVSCMGJitter++; } omegasForInit(it - this->VSCMGData.begin(), 0) = it->Omega; - gammasForInit(it - this->VSCMGData.begin(), 0) = it->gamma; - gammaDotsForInit(it - this->VSCMGData.begin(), 0) = it->gammaDot; + gammasForInit(it - this->VSCMGData.begin(), 0) = it->gamma; + gammaDotsForInit(it - this->VSCMGData.begin(), 0) = it->gammaDot; this->numVSCMG++; } - this->OmegasState = states.registerState(this->numVSCMG, 1, this->nameOfVSCMGOmegasState); - this->gammasState = states.registerState(this->numVSCMG, 1, this->nameOfVSCMGGammasState); - this->gammaDotsState = states.registerState(this->numVSCMG, 1, this->nameOfVSCMGGammaDotsState); + this->OmegasState = states.registerState(this->numVSCMG, 1, this->nameOfVSCMGOmegasState); + this->gammasState = states.registerState(this->numVSCMG, 1, this->nameOfVSCMGGammasState); + this->gammaDotsState = states.registerState(this->numVSCMG, 1, this->nameOfVSCMGGammaDotsState); this->OmegasState->setState(omegasForInit); - this->gammasState->setState(gammasForInit); - this->gammaDotsState->setState(gammaDotsForInit); + this->gammasState->setState(gammasForInit); + this->gammaDotsState->setState(gammaDotsForInit); - if (numVSCMGJitter > 0) { - this->thetasState = states.registerState(this->numVSCMGJitter, 1, this->nameOfVSCMGThetasState); - Eigen::MatrixXd thetasForZeroing(this->numVSCMGJitter,1); + if (numVSCMGJitter > 0) { + this->thetasState = states.registerState(this->numVSCMGJitter, 1, this->nameOfVSCMGThetasState); + Eigen::MatrixXd thetasForZeroing(this->numVSCMGJitter, 1); thetasForZeroing.setZero(); this->thetasState->setState(thetasForZeroing); } @@ -98,7 +97,8 @@ void VSCMGStateEffector::registerStates(DynParamManager& states) return; } -void VSCMGStateEffector::updateEffectorMassProps(double integTime) +void +VSCMGStateEffector::updateEffectorMassProps(double integTime) { // - Zero the mass props information because these will be accumulated during this call this->effProps.mEff = 0.; @@ -109,142 +109,138 @@ void VSCMGStateEffector::updateEffectorMassProps(double integTime) int thetaCount = 0; std::vector::iterator it; - for(it=VSCMGData.begin(); it!=VSCMGData.end(); it++) - { - it->Omega = this->OmegasState->getState()(it - VSCMGData.begin(), 0); - it->gamma = this->gammasState->getState()(it - VSCMGData.begin(), 0); - it->gammaDot = this->gammaDotsState->getState()(it - VSCMGData.begin(), 0); - if (it->VSCMGModel == vscmgJitterFullyCoupled || it->VSCMGModel == vscmgJitterSimple) { - it->theta = this->thetasState->getState()(thetaCount, 0); - thetaCount++; - } - - Eigen::Matrix3d dcm_GG0 = eigenM3(it->gamma); - Eigen::Matrix3d dcm_BG0; - dcm_BG0.col(0) = it->gsHat0_B; - dcm_BG0.col(1) = it->gtHat0_B; - dcm_BG0.col(2) = it->ggHat_B; - Eigen::Matrix3d dcm_BG = dcm_BG0 * dcm_GG0.transpose(); - it->gsHat_B = dcm_BG.col(0); - it->gtHat_B = dcm_BG.col(1); - - Eigen::Matrix3d dcm_WG = eigenM1(it->theta); - Eigen::Matrix3d dcm_WG0 = dcm_WG * dcm_GG0; - Eigen::Matrix3d dcm_BW = dcm_BG0 * dcm_WG0.transpose(); - it->w2Hat_B = dcm_BW.col(1); - it->w3Hat_B = dcm_BW.col(2); - - if (it->VSCMGModel == vscmgBalancedWheels || it->VSCMGModel == vscmgJitterSimple) { - - //! gimbal inertia tensor about wheel center of mass represented in B frame - Eigen::Matrix3d IGIMPntGc_G; - IGIMPntGc_G << it->IG1, 0., 0., \ - 0., it->IG2, 0., \ - 0., 0., it->IG3; - it->IGPntGc_B = dcm_BG * IGIMPntGc_G * dcm_BG.transpose(); - - //! gimbal inertia tensor body frame derivative about gimbal center of mass represented in B frame - Eigen::Matrix3d IPrimeGIMPntGc_G; - IPrimeGIMPntGc_G << 0., it->gammaDot*(it->IG1-it->IG2), 0., \ - it->gammaDot*(it->IG1-it->IG2), 0., 0., \ - 0., 0., 0.; - it->IPrimeGPntGc_B = dcm_BG * IPrimeGIMPntGc_G * dcm_BG.transpose(); - - //! wheel inertia tensor about wheel center of mass represented in B frame - Eigen::Matrix3d IRWPntWc_W; - IRWPntWc_W << it->IW1, 0., 0., \ - 0., it->IW2, 0., \ - 0., 0., it->IW3; - it->IWPntWc_B = dcm_BG * IRWPntWc_W * dcm_BG.transpose(); - - //! wheel inertia tensor body frame derivative about wheel center of mass represented in B frame - Eigen::Matrix3d IPrimeRWPntWc_W; - IPrimeRWPntWc_W(0,0) = 0.0; - IPrimeRWPntWc_W(0,1) = it->gammaDot*(it->IW1-it->IW2); - IPrimeRWPntWc_W(0,2) = 0.0; - IPrimeRWPntWc_W(1,0) = IPrimeRWPntWc_W(0,1); - IPrimeRWPntWc_W(1,1) = 0.0; - IPrimeRWPntWc_W(1,2) = 0.0; - IPrimeRWPntWc_W(2,0) = 0.0; - IPrimeRWPntWc_W(2,1) = 0.0; - IPrimeRWPntWc_W(2,2) = 0.0; - it->IPrimeWPntWc_B = dcm_BG * IPrimeRWPntWc_W * dcm_BG.transpose(); - - //! VSCMG center of mass location - //! Note that points W, G, Wc, Gc are coincident - it->rWcB_B = it->rGB_B; - it->rTildeWcB_B = eigenTilde(it->rWcB_B); - it->rPrimeWcB_B.setZero(); - - //! - Give the mass of the VSCMG to the effProps mass - this->effProps.mEff += it->massV; - this->effProps.rEff_CB_B += it->massV*it->rGB_B; - this->effProps.IEffPntB_B += it->IWPntWc_B + it->IGPntGc_B + it->massV*it->rTildeWcB_B*it->rTildeWcB_B.transpose(); - this->effProps.rEffPrime_CB_B += it->massV*it->rPrimeWcB_B; - this->effProps.IEffPrimePntB_B += it->IPrimeWPntWc_B + it->IPrimeGPntGc_B; - - } else if (it->VSCMGModel == vscmgJitterFullyCoupled) { - - it->rGcG_B = dcm_BG * it->rGcG_G; - - //! gimbal inertia tensor about wheel center of mass represented in B frame - Eigen::Matrix3d IGIMPntGc_G; - IGIMPntGc_G << it->IG1, it->IG12, it->IG13, \ - it->IG12, it->IG2, it->IG23, \ - it->IG13, it->IG23, it->IG3; - it->IGPntGc_B = dcm_BG * IGIMPntGc_G * dcm_BG.transpose(); - - //! gimbal inertia tensor body frame derivative about gimbal center of mass represented in B frame - Eigen::Matrix3d IPrimeGIMPntGc_G; - IPrimeGIMPntGc_G << -2*it->IG12, (it->IG1-it->IG2), -it->IG23, \ - (it->IG1-it->IG2), 2*it->IG12, it->IG13, \ - -it->IG23, it->IG13, 0.; - IPrimeGIMPntGc_G *= it->gammaDot; - it->IPrimeGPntGc_B = dcm_BG * IPrimeGIMPntGc_G * dcm_BG.transpose(); - - //! wheel inertia tensor about wheel center of mass represented in B frame - Eigen::Matrix3d IRWPntWc_W; - IRWPntWc_W << it->IW1, 0., it->IW13, \ - 0., it->IW2, 0., \ - it->IW13, 0., it->IW3; - it->IWPntWc_B = dcm_BW * IRWPntWc_W * dcm_BW.transpose(); - - //! wheel inertia tensor body frame derivative about wheel center of mass represented in B frame - double Ia = it->IW1 - it->IW2; - double Ib = it->IW3 - it->IW1; - double Ic = it->IW2 - it->IW3; - Eigen::Matrix3d IPrimeRWPntWc_W; - IPrimeRWPntWc_W(0,0) = 2.0*it->gammaDot*it->IW13*sin(it->theta); - IPrimeRWPntWc_W(0,1) = it->gammaDot*Ia*cos(it->theta)-it->IW13*it->Omega; - IPrimeRWPntWc_W(0,2) = it->gammaDot*Ib*sin(it->theta); - IPrimeRWPntWc_W(1,0) = IPrimeRWPntWc_W(0,1); - IPrimeRWPntWc_W(1,1) = 0.0; - IPrimeRWPntWc_W(1,2) = it->gammaDot*it->IW13*cos(it->theta)+Ic*it->Omega; - IPrimeRWPntWc_W(2,0) = IPrimeRWPntWc_W(0,2); - IPrimeRWPntWc_W(2,1) = IPrimeRWPntWc_W(1,2); - IPrimeRWPntWc_W(2,2) = -2.0*it->IW13*it->gammaDot*sin(it->theta); - it->IPrimeWPntWc_B = dcm_BW * IPrimeRWPntWc_W * dcm_BW.transpose(); - - //! VSCMG center of mass location - it->rGcB_B = it->rGB_B + it->rGcG_B; - it->rTildeGcB_B = eigenTilde(it->rGcB_B); - it->rPrimeGcB_B = it->gammaDot*it->ggHat_B.cross(it->rGcG_B); - it->rPrimeTildeGcB_B = eigenTilde(it->rPrimeGcB_B); - it->rWcG_B = it->L*it->ggHat_B + it->l*it->gsHat_B + it->d*it->w2Hat_B; - it->rWcB_B = it->rGB_B + it->rWcG_B; - it->rTildeWcB_B = eigenTilde(it->rWcB_B); - it->rPrimeWcB_B = it->d*it->Omega*it->w3Hat_B - it->d*it->gammaDot*cos(it->theta)*it->gsHat_B + it->l*it->gammaDot*it->gtHat_B; - it->rPrimeTildeWcB_B = eigenTilde(it->rPrimeWcB_B); - - //! - Give the mass of the VSCMG to the effProps mass - this->effProps.mEff += it->massV; - this->effProps.rEff_CB_B += it->massG*it->rGcB_B + it->massW*it->rWcB_B; - this->effProps.IEffPntB_B += it->IWPntWc_B + it->IGPntGc_B + it->massG*it->rTildeGcB_B*it->rTildeGcB_B.transpose() + it->massW*it->rTildeWcB_B*it->rTildeWcB_B.transpose(); - this->effProps.rEffPrime_CB_B += it->massG*it->rPrimeGcB_B + it->massW*it->rPrimeWcB_B; - this->effProps.IEffPrimePntB_B += it->IPrimeGPntGc_B + it->massG*it->rPrimeTildeGcB_B*it->rTildeGcB_B.transpose() + it->massG*it->rTildeGcB_B*it->rPrimeTildeGcB_B.transpose() - + it->IPrimeWPntWc_B + it->massW*it->rPrimeTildeWcB_B*it->rTildeWcB_B.transpose() + it->massW*it->rTildeWcB_B*it->rPrimeTildeWcB_B.transpose(); - } - } + for (it = VSCMGData.begin(); it != VSCMGData.end(); it++) { + it->Omega = this->OmegasState->getState()(it - VSCMGData.begin(), 0); + it->gamma = this->gammasState->getState()(it - VSCMGData.begin(), 0); + it->gammaDot = this->gammaDotsState->getState()(it - VSCMGData.begin(), 0); + if (it->VSCMGModel == vscmgJitterFullyCoupled || it->VSCMGModel == vscmgJitterSimple) { + it->theta = this->thetasState->getState()(thetaCount, 0); + thetaCount++; + } + + Eigen::Matrix3d dcm_GG0 = eigenM3(it->gamma); + Eigen::Matrix3d dcm_BG0; + dcm_BG0.col(0) = it->gsHat0_B; + dcm_BG0.col(1) = it->gtHat0_B; + dcm_BG0.col(2) = it->ggHat_B; + Eigen::Matrix3d dcm_BG = dcm_BG0 * dcm_GG0.transpose(); + it->gsHat_B = dcm_BG.col(0); + it->gtHat_B = dcm_BG.col(1); + + Eigen::Matrix3d dcm_WG = eigenM1(it->theta); + Eigen::Matrix3d dcm_WG0 = dcm_WG * dcm_GG0; + Eigen::Matrix3d dcm_BW = dcm_BG0 * dcm_WG0.transpose(); + it->w2Hat_B = dcm_BW.col(1); + it->w3Hat_B = dcm_BW.col(2); + + if (it->VSCMGModel == vscmgBalancedWheels || it->VSCMGModel == vscmgJitterSimple) { + + //! gimbal inertia tensor about wheel center of mass represented in B frame + Eigen::Matrix3d IGIMPntGc_G; + IGIMPntGc_G << it->IG1, 0., 0., 0., it->IG2, 0., 0., 0., it->IG3; + it->IGPntGc_B = dcm_BG * IGIMPntGc_G * dcm_BG.transpose(); + + //! gimbal inertia tensor body frame derivative about gimbal center of mass represented in B frame + Eigen::Matrix3d IPrimeGIMPntGc_G; + IPrimeGIMPntGc_G << 0., it->gammaDot * (it->IG1 - it->IG2), 0., it->gammaDot * (it->IG1 - it->IG2), 0., 0., + 0., 0., 0.; + it->IPrimeGPntGc_B = dcm_BG * IPrimeGIMPntGc_G * dcm_BG.transpose(); + + //! wheel inertia tensor about wheel center of mass represented in B frame + Eigen::Matrix3d IRWPntWc_W; + IRWPntWc_W << it->IW1, 0., 0., 0., it->IW2, 0., 0., 0., it->IW3; + it->IWPntWc_B = dcm_BG * IRWPntWc_W * dcm_BG.transpose(); + + //! wheel inertia tensor body frame derivative about wheel center of mass represented in B frame + Eigen::Matrix3d IPrimeRWPntWc_W; + IPrimeRWPntWc_W(0, 0) = 0.0; + IPrimeRWPntWc_W(0, 1) = it->gammaDot * (it->IW1 - it->IW2); + IPrimeRWPntWc_W(0, 2) = 0.0; + IPrimeRWPntWc_W(1, 0) = IPrimeRWPntWc_W(0, 1); + IPrimeRWPntWc_W(1, 1) = 0.0; + IPrimeRWPntWc_W(1, 2) = 0.0; + IPrimeRWPntWc_W(2, 0) = 0.0; + IPrimeRWPntWc_W(2, 1) = 0.0; + IPrimeRWPntWc_W(2, 2) = 0.0; + it->IPrimeWPntWc_B = dcm_BG * IPrimeRWPntWc_W * dcm_BG.transpose(); + + //! VSCMG center of mass location + //! Note that points W, G, Wc, Gc are coincident + it->rWcB_B = it->rGB_B; + it->rTildeWcB_B = eigenTilde(it->rWcB_B); + it->rPrimeWcB_B.setZero(); + + //! - Give the mass of the VSCMG to the effProps mass + this->effProps.mEff += it->massV; + this->effProps.rEff_CB_B += it->massV * it->rGB_B; + this->effProps.IEffPntB_B += + it->IWPntWc_B + it->IGPntGc_B + it->massV * it->rTildeWcB_B * it->rTildeWcB_B.transpose(); + this->effProps.rEffPrime_CB_B += it->massV * it->rPrimeWcB_B; + this->effProps.IEffPrimePntB_B += it->IPrimeWPntWc_B + it->IPrimeGPntGc_B; + + } else if (it->VSCMGModel == vscmgJitterFullyCoupled) { + + it->rGcG_B = dcm_BG * it->rGcG_G; + + //! gimbal inertia tensor about wheel center of mass represented in B frame + Eigen::Matrix3d IGIMPntGc_G; + IGIMPntGc_G << it->IG1, it->IG12, it->IG13, it->IG12, it->IG2, it->IG23, it->IG13, it->IG23, it->IG3; + it->IGPntGc_B = dcm_BG * IGIMPntGc_G * dcm_BG.transpose(); + + //! gimbal inertia tensor body frame derivative about gimbal center of mass represented in B frame + Eigen::Matrix3d IPrimeGIMPntGc_G; + IPrimeGIMPntGc_G << -2 * it->IG12, (it->IG1 - it->IG2), -it->IG23, (it->IG1 - it->IG2), 2 * it->IG12, + it->IG13, -it->IG23, it->IG13, 0.; + IPrimeGIMPntGc_G *= it->gammaDot; + it->IPrimeGPntGc_B = dcm_BG * IPrimeGIMPntGc_G * dcm_BG.transpose(); + + //! wheel inertia tensor about wheel center of mass represented in B frame + Eigen::Matrix3d IRWPntWc_W; + IRWPntWc_W << it->IW1, 0., it->IW13, 0., it->IW2, 0., it->IW13, 0., it->IW3; + it->IWPntWc_B = dcm_BW * IRWPntWc_W * dcm_BW.transpose(); + + //! wheel inertia tensor body frame derivative about wheel center of mass represented in B frame + double Ia = it->IW1 - it->IW2; + double Ib = it->IW3 - it->IW1; + double Ic = it->IW2 - it->IW3; + Eigen::Matrix3d IPrimeRWPntWc_W; + IPrimeRWPntWc_W(0, 0) = 2.0 * it->gammaDot * it->IW13 * sin(it->theta); + IPrimeRWPntWc_W(0, 1) = it->gammaDot * Ia * cos(it->theta) - it->IW13 * it->Omega; + IPrimeRWPntWc_W(0, 2) = it->gammaDot * Ib * sin(it->theta); + IPrimeRWPntWc_W(1, 0) = IPrimeRWPntWc_W(0, 1); + IPrimeRWPntWc_W(1, 1) = 0.0; + IPrimeRWPntWc_W(1, 2) = it->gammaDot * it->IW13 * cos(it->theta) + Ic * it->Omega; + IPrimeRWPntWc_W(2, 0) = IPrimeRWPntWc_W(0, 2); + IPrimeRWPntWc_W(2, 1) = IPrimeRWPntWc_W(1, 2); + IPrimeRWPntWc_W(2, 2) = -2.0 * it->IW13 * it->gammaDot * sin(it->theta); + it->IPrimeWPntWc_B = dcm_BW * IPrimeRWPntWc_W * dcm_BW.transpose(); + + //! VSCMG center of mass location + it->rGcB_B = it->rGB_B + it->rGcG_B; + it->rTildeGcB_B = eigenTilde(it->rGcB_B); + it->rPrimeGcB_B = it->gammaDot * it->ggHat_B.cross(it->rGcG_B); + it->rPrimeTildeGcB_B = eigenTilde(it->rPrimeGcB_B); + it->rWcG_B = it->L * it->ggHat_B + it->l * it->gsHat_B + it->d * it->w2Hat_B; + it->rWcB_B = it->rGB_B + it->rWcG_B; + it->rTildeWcB_B = eigenTilde(it->rWcB_B); + it->rPrimeWcB_B = it->d * it->Omega * it->w3Hat_B - it->d * it->gammaDot * cos(it->theta) * it->gsHat_B + + it->l * it->gammaDot * it->gtHat_B; + it->rPrimeTildeWcB_B = eigenTilde(it->rPrimeWcB_B); + + //! - Give the mass of the VSCMG to the effProps mass + this->effProps.mEff += it->massV; + this->effProps.rEff_CB_B += it->massG * it->rGcB_B + it->massW * it->rWcB_B; + this->effProps.IEffPntB_B += it->IWPntWc_B + it->IGPntGc_B + + it->massG * it->rTildeGcB_B * it->rTildeGcB_B.transpose() + + it->massW * it->rTildeWcB_B * it->rTildeWcB_B.transpose(); + this->effProps.rEffPrime_CB_B += it->massG * it->rPrimeGcB_B + it->massW * it->rPrimeWcB_B; + this->effProps.IEffPrimePntB_B += + it->IPrimeGPntGc_B + it->massG * it->rPrimeTildeGcB_B * it->rTildeGcB_B.transpose() + + it->massG * it->rTildeGcB_B * it->rPrimeTildeGcB_B.transpose() + it->IPrimeWPntWc_B + + it->massW * it->rPrimeTildeWcB_B * it->rTildeWcB_B.transpose() + + it->massW * it->rTildeWcB_B * it->rPrimeTildeWcB_B.transpose(); + } + } // - Need to divide out the total mass of the VSCMGs from rCB_B and rPrimeCB_B if (this->effProps.mEff > 0) { @@ -252,240 +248,298 @@ void VSCMGStateEffector::updateEffectorMassProps(double integTime) this->effProps.rEffPrime_CB_B /= this->effProps.mEff; } - return; + return; } -void VSCMGStateEffector::updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) +void +VSCMGStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { - Eigen::Vector3d omegaLoc_BN_B; - Eigen::Vector3d tempF; - double omegas; - double omegat; - double OmegaSquared; - Eigen::Matrix3d omegaTilde; + Eigen::Vector3d omegaLoc_BN_B; + Eigen::Vector3d tempF; + double omegas; + double omegat; + double OmegaSquared; + Eigen::Matrix3d omegaTilde; Eigen::MRPd sigmaBNLocal; - Eigen::Matrix3d dcm_BN; /* direction cosine matrix from N to B */ - Eigen::Matrix3d dcm_NB; /* direction cosine matrix from B to N */ - Eigen::Vector3d gravityTorquePntW_B; /* torque of gravity on HRB about Pnt H */ - Eigen::Vector3d gravityTorquePntG_B; /* torque of gravity on HRB about Pnt H */ - Eigen::Vector3d gLocal_N; /* gravitational acceleration in N frame */ - Eigen::Vector3d g_B; /* gravitational acceleration in B frame */ - Eigen::Vector3d omega_WB_B; + Eigen::Matrix3d dcm_BN; /* direction cosine matrix from N to B */ + Eigen::Matrix3d dcm_NB; /* direction cosine matrix from B to N */ + Eigen::Vector3d gravityTorquePntW_B; /* torque of gravity on HRB about Pnt H */ + Eigen::Vector3d gravityTorquePntG_B; /* torque of gravity on HRB about Pnt H */ + Eigen::Vector3d gLocal_N; /* gravitational acceleration in N frame */ + Eigen::Vector3d g_B; /* gravitational acceleration in B frame */ + Eigen::Vector3d omega_WB_B; gLocal_N = *this->g_N; - Eigen::Vector3d ur; - Eigen::Vector3d vr; - Eigen::Vector3d kr; - Eigen::Vector3d uomega; - Eigen::Vector3d vomega; - Eigen::Vector3d komega; - + Eigen::Vector3d ur; + Eigen::Vector3d vr; + Eigen::Vector3d kr; + Eigen::Vector3d uomega; + Eigen::Vector3d vomega; + Eigen::Vector3d komega; //! - Find dcm_BN - sigmaBNLocal = (Eigen::Vector3d ) sigma_BN; + sigmaBNLocal = (Eigen::Vector3d)sigma_BN; dcm_NB = sigmaBNLocal.toRotationMatrix(); dcm_BN = dcm_NB.transpose(); //! - Map gravity to body frame - g_B = dcm_BN*gLocal_N; + g_B = dcm_BN * gLocal_N; - omegaLoc_BN_B = omega_BN_B; + omegaLoc_BN_B = omega_BN_B; std::vector::iterator it; - for(it=VSCMGData.begin(); it!=VSCMGData.end(); it++) - { - OmegaSquared = it->Omega * it->Omega; - omegaTilde = eigenTilde(omegaLoc_BN_B); - omega_WB_B = it->gammaDot*it->ggHat_B+it->Omega*it->gsHat_B; - omegas = it->gsHat_B.transpose()*omegaLoc_BN_B; - omegat = it->gtHat_B.transpose()*omegaLoc_BN_B; - - if (it->VSCMGModel == vscmgBalancedWheels || it->VSCMGModel == vscmgJitterSimple) { - backSubContr.matrixD -= it->IV3 * it->ggHat_B * it->ggHat_B.transpose() + it->IW1 * it->gsHat_B * it->gsHat_B.transpose(); - backSubContr.vecRot -= (it->u_s_current-it->IW1*omegat*it->gammaDot)*it->gsHat_B + it->IW1*it->Omega*it->gammaDot*it->gtHat_B + (it->u_g_current+(it->IV1-it->IV2)*omegas*omegat+it->IW1*it->Omega*omegat)*it->ggHat_B - + omegaTilde*it->IGPntGc_B*it->gammaDot*it->ggHat_B + omegaTilde*it->IWPntWc_B*omega_WB_B; - if (it->VSCMGModel == vscmgJitterSimple) { - /* static imbalance force: Fs = Us * Omega^2 */ - tempF = it->U_s * OmegaSquared * it->w2Hat_B; - backSubContr.vecTrans += tempF; - /* static imbalance torque: tau_s = cross(r_B,Fs), dynamic imbalance torque: tau_d = Ud * Omega^2 */ - backSubContr.vecRot += it->rGB_B.cross(tempF) + it->U_d*OmegaSquared*it->w2Hat_B; - } + for (it = VSCMGData.begin(); it != VSCMGData.end(); it++) { + OmegaSquared = it->Omega * it->Omega; + omegaTilde = eigenTilde(omegaLoc_BN_B); + omega_WB_B = it->gammaDot * it->ggHat_B + it->Omega * it->gsHat_B; + omegas = it->gsHat_B.transpose() * omegaLoc_BN_B; + omegat = it->gtHat_B.transpose() * omegaLoc_BN_B; + + if (it->VSCMGModel == vscmgBalancedWheels || it->VSCMGModel == vscmgJitterSimple) { + backSubContr.matrixD -= + it->IV3 * it->ggHat_B * it->ggHat_B.transpose() + it->IW1 * it->gsHat_B * it->gsHat_B.transpose(); + backSubContr.vecRot -= + (it->u_s_current - it->IW1 * omegat * it->gammaDot) * it->gsHat_B + + it->IW1 * it->Omega * it->gammaDot * it->gtHat_B + + (it->u_g_current + (it->IV1 - it->IV2) * omegas * omegat + it->IW1 * it->Omega * omegat) * it->ggHat_B + + omegaTilde * it->IGPntGc_B * it->gammaDot * it->ggHat_B + omegaTilde * it->IWPntWc_B * omega_WB_B; + if (it->VSCMGModel == vscmgJitterSimple) { + /* static imbalance force: Fs = Us * Omega^2 */ + tempF = it->U_s * OmegaSquared * it->w2Hat_B; + backSubContr.vecTrans += tempF; + /* static imbalance torque: tau_s = cross(r_B,Fs), dynamic imbalance torque: tau_d = Ud * Omega^2 */ + backSubContr.vecRot += it->rGB_B.cross(tempF) + it->U_d * OmegaSquared * it->w2Hat_B; + } } else if (it->VSCMGModel == vscmgJitterFullyCoupled) { - gravityTorquePntW_B = it->d*it->w2Hat_B.cross(it->massW*g_B); - gravityTorquePntG_B = it->rGcG_B.cross(it->massG*g_B); - it->gravityTorqueWheel_s = it->gsHat_B.dot(gravityTorquePntW_B); - it->gravityTorqueGimbal_g = it->ggHat_B.dot(gravityTorquePntG_B); - - - double dSquared = it->d * it->d; - double gammaDotSquared = it->gammaDot * it->gammaDot; - Eigen::Matrix3d omegaTildeSquared = omegaTilde*omegaTilde; - Eigen::Matrix3d ggHatTilde_B = eigenTilde(it->ggHat_B); - Eigen::Matrix3d w2HatTilde_B = eigenTilde(it->w2Hat_B); - Eigen::Vector3d omega_GN_B = omegaLoc_BN_B + it->gammaDot*it->ggHat_B; - Eigen::Vector3d omega_WN_B = omega_GN_B + it->Omega*it->gsHat_B; - - Eigen::Vector3d rVcG_B = 1.0/it->massV*(it->massG*it->rGcG_B + it->massW*it->rWcG_B); - Eigen::Vector3d rVcB_B = rVcG_B + it->rGB_B; - Eigen::Matrix3d rTildeVcG_B = eigenTilde(rVcG_B); - Eigen::Matrix3d rTildeVcB_B = eigenTilde(rVcG_B + it->rGB_B); - Eigen::Vector3d rGcVc_B = it->rGcG_B - rVcG_B; - Eigen::Vector3d rWcVc_B = it->rWcG_B - rVcG_B; - Eigen::Matrix3d rTildeGcVc_B = eigenTilde(rGcVc_B); - Eigen::Matrix3d rTildeWcVc_B = eigenTilde(rWcVc_B); - Eigen::Matrix3d IVPntVc_B = it->IGPntGc_B + it->massG*rTildeGcVc_B*rTildeGcVc_B.transpose() - + it->IWPntWc_B + it->massW*rTildeWcVc_B*rTildeWcVc_B.transpose(); - Eigen::Vector3d rPrimeGcG_B = it->rPrimeGcB_B; - Eigen::Vector3d rPrimeWcG_B = it->rPrimeWcB_B; - Eigen::Vector3d rPrimeVcG_B = it->rhoG*rPrimeGcG_B + it->rhoW*rPrimeWcG_B; - Eigen::Vector3d rPrimeVcB_B = rPrimeVcG_B; - Eigen::Vector3d rPrimeGcVc_B = rPrimeGcG_B - rPrimeVcG_B; - Eigen::Vector3d rPrimeWcVc_B = rPrimeWcG_B - rPrimeVcG_B; - - Eigen::Matrix3d P = it->massW*it->rhoG*rTildeWcVc_B - it->massG*it->rhoW*rTildeGcVc_B + it->massW*rTildeVcG_B; - Eigen::Matrix3d Q = it->massG*it->rhoW*rTildeGcVc_B - it->massW*it->rhoG*rTildeWcVc_B + it->massG*rTildeVcG_B; - - it->egamma = it->ggHat_B.dot(it->IGPntGc_B*it->ggHat_B+it->IWPntWc_B*it->ggHat_B+P*(it->l*it->gtHat_B-it->d*cos(it->theta)*it->gsHat_B)+Q*ggHatTilde_B*it->rGcG_B); - it->agamma = 1.0/it->egamma*it->massV*rTildeVcG_B*it->ggHat_B; - it->bgamma = -1.0/it->egamma*(IVPntVc_B.transpose()*it->ggHat_B-it->massV*rTildeVcB_B*rTildeVcG_B*it->ggHat_B); - it->cgamma = -1.0/it->egamma*(it->ggHat_B.dot(it->IWPntWc_B*it->gsHat_B) + it->d*it->ggHat_B.dot(P*it->w3Hat_B)); - it->dgamma = -1.0/it->egamma*it->ggHat_B.dot( it->gammaDot*Q*ggHatTilde_B*rPrimeGcG_B + P*((2.0*it->d*it->gammaDot*it->Omega*sin(it->theta)-it->l*gammaDotSquared)*it->gsHat_B-it->d*gammaDotSquared*cos(it->theta)*it->gtHat_B-it->d*OmegaSquared*it->w2Hat_B) - + it->IPrimeGPntGc_B*omega_GN_B + omegaTilde*it->IGPntGc_B*omega_GN_B + it->IWPntWc_B*it->Omega*it->gammaDot*it->gtHat_B + it->IPrimeWPntWc_B*omega_WN_B + omegaTilde*it->IWPntWc_B*omega_WN_B - + it->massG*rTildeGcVc_B*(2.0*omegaTilde*rPrimeGcVc_B+omegaTilde*omegaTilde*rGcVc_B) + it->massW*rTildeWcVc_B*(2.0*omegaTilde*rPrimeWcVc_B+omegaTildeSquared*rWcVc_B) - + it->massV*rTildeVcG_B*(2.0*omegaTilde*rPrimeVcB_B+omegaTildeSquared*rVcB_B) ) - + 1.0/it->egamma*(it->u_g_current + it->gravityTorqueGimbal_g); - - it->eOmega = it->IW1 + it->massW*dSquared; - it->aOmega = -1.0/it->eOmega*it->massW*it->d*it->w3Hat_B; - it->bOmega = -1.0/it->eOmega*(it->IWPntWc_B.transpose()*it->gsHat_B - it->massW*it->d*it->rTildeWcB_B*w2HatTilde_B*it->gsHat_B); - it->cOmega = -1.0/it->eOmega*(it->IW13*cos(it->theta)-it->massW*it->d*it->l*sin(it->theta)); - it->dOmega = -1.0/it->eOmega*(it->gsHat_B.dot(it->IPrimeWPntWc_B*omega_WN_B) - + it->gsHat_B.transpose()*omegaTilde*it->IWPntWc_B*omega_WN_B - + it->massW*it->d*it->gsHat_B.transpose()*w2HatTilde_B*(2.0*it->rPrimeTildeWcB_B.transpose()*omegaLoc_BN_B+omegaTilde*omegaTilde*it->rWcB_B)) - + (1.0/it->eOmega)*(it->IW13*sin(it->theta)*it->Omega*it->gammaDot - it->massW*dSquared*gammaDotSquared*cos(it->theta)*sin(it->theta) + it->u_s_current + it->gravityTorqueWheel_s); - - it->p = (it->aOmega+it->cOmega*it->agamma)/(1.0-it->cOmega*it->cgamma); - it->q = (it->bOmega+it->cOmega*it->bgamma)/(1.0-it->cOmega*it->cgamma); - it->s = (it->dOmega+it->cOmega*it->dgamma)/(1.0-it->cOmega*it->cgamma); - - ur = it->massG*ggHatTilde_B*it->rGcG_B - it->massW*it->d*cos(it->theta)*it->gsHat_B + it->massW*it->l*it->gtHat_B; - vr = it->massW*it->d*it->w3Hat_B; - kr = it->massG*it->gammaDot*ggHatTilde_B*it->rPrimeGcB_B + it->massW*((2.0*it->d*it->gammaDot*it->Omega*sin(it->theta)-it->l*gammaDotSquared)*it->gsHat_B-it->d*gammaDotSquared*cos(it->theta)*it->gtHat_B-it->d*OmegaSquared*it->w2Hat_B); - - uomega = it->IGPntGc_B*it->ggHat_B + it->massG*it->rTildeGcB_B*ggHatTilde_B*it->rGcG_B + it->IWPntWc_B*it->ggHat_B + it->massW*it->rTildeWcB_B*(it->l*it->gtHat_B-it->d*cos(it->theta)*it->gsHat_B); - vomega = it->IWPntWc_B*it->gsHat_B + it->massW*it->d*it->rTildeWcB_B*it->w3Hat_B; - komega = it->IPrimeGPntGc_B*it->gammaDot*it->ggHat_B + omegaTilde*it->IGPntGc_B*it->gammaDot*it->ggHat_B + it->massG*omegaTilde*it->rTildeGcB_B*it->rPrimeGcB_B + it->massG*it->gammaDot*it->rTildeGcB_B*ggHatTilde_B*rPrimeGcG_B - + it->IWPntWc_B*it->Omega*it->gammaDot*it->gtHat_B + it->IPrimeWPntWc_B*omega_WB_B + omegaTilde*it->IWPntWc_B*omega_WB_B + it->massW*omegaTilde*it->rTildeWcB_B*it->rPrimeWcB_B - + it->massW*it->rTildeWcB_B*((2.0*it->d*it->gammaDot*it->Omega*sin(it->theta)-it->l*gammaDotSquared)*it->gsHat_B-it->d*gammaDotSquared*cos(it->theta)*it->gtHat_B-it->d*OmegaSquared*it->w2Hat_B); - - backSubContr.matrixA += ur*it->agamma.transpose() + (vr+ur*it->cgamma)*it->p.transpose(); - backSubContr.matrixB += ur*it->bgamma.transpose() + (vr+ur*it->cgamma)*it->q.transpose(); - backSubContr.matrixC += uomega*it->agamma.transpose() + (vomega+uomega*it->cgamma)*it->p.transpose(); - backSubContr.matrixD += uomega*it->bgamma.transpose() + (vomega+uomega*it->cgamma)*it->q.transpose(); - backSubContr.vecTrans -= kr + ur*it->dgamma + (vr+ur*it->cgamma)*it->s; - backSubContr.vecRot -= komega + uomega*it->dgamma + (vomega+uomega*it->cgamma)*it->s; - } - } - return; + gravityTorquePntW_B = it->d * it->w2Hat_B.cross(it->massW * g_B); + gravityTorquePntG_B = it->rGcG_B.cross(it->massG * g_B); + it->gravityTorqueWheel_s = it->gsHat_B.dot(gravityTorquePntW_B); + it->gravityTorqueGimbal_g = it->ggHat_B.dot(gravityTorquePntG_B); + + double dSquared = it->d * it->d; + double gammaDotSquared = it->gammaDot * it->gammaDot; + Eigen::Matrix3d omegaTildeSquared = omegaTilde * omegaTilde; + Eigen::Matrix3d ggHatTilde_B = eigenTilde(it->ggHat_B); + Eigen::Matrix3d w2HatTilde_B = eigenTilde(it->w2Hat_B); + Eigen::Vector3d omega_GN_B = omegaLoc_BN_B + it->gammaDot * it->ggHat_B; + Eigen::Vector3d omega_WN_B = omega_GN_B + it->Omega * it->gsHat_B; + + Eigen::Vector3d rVcG_B = 1.0 / it->massV * (it->massG * it->rGcG_B + it->massW * it->rWcG_B); + Eigen::Vector3d rVcB_B = rVcG_B + it->rGB_B; + Eigen::Matrix3d rTildeVcG_B = eigenTilde(rVcG_B); + Eigen::Matrix3d rTildeVcB_B = eigenTilde(rVcG_B + it->rGB_B); + Eigen::Vector3d rGcVc_B = it->rGcG_B - rVcG_B; + Eigen::Vector3d rWcVc_B = it->rWcG_B - rVcG_B; + Eigen::Matrix3d rTildeGcVc_B = eigenTilde(rGcVc_B); + Eigen::Matrix3d rTildeWcVc_B = eigenTilde(rWcVc_B); + Eigen::Matrix3d IVPntVc_B = it->IGPntGc_B + it->massG * rTildeGcVc_B * rTildeGcVc_B.transpose() + + it->IWPntWc_B + it->massW * rTildeWcVc_B * rTildeWcVc_B.transpose(); + Eigen::Vector3d rPrimeGcG_B = it->rPrimeGcB_B; + Eigen::Vector3d rPrimeWcG_B = it->rPrimeWcB_B; + Eigen::Vector3d rPrimeVcG_B = it->rhoG * rPrimeGcG_B + it->rhoW * rPrimeWcG_B; + Eigen::Vector3d rPrimeVcB_B = rPrimeVcG_B; + Eigen::Vector3d rPrimeGcVc_B = rPrimeGcG_B - rPrimeVcG_B; + Eigen::Vector3d rPrimeWcVc_B = rPrimeWcG_B - rPrimeVcG_B; + + Eigen::Matrix3d P = + it->massW * it->rhoG * rTildeWcVc_B - it->massG * it->rhoW * rTildeGcVc_B + it->massW * rTildeVcG_B; + Eigen::Matrix3d Q = + it->massG * it->rhoW * rTildeGcVc_B - it->massW * it->rhoG * rTildeWcVc_B + it->massG * rTildeVcG_B; + + it->egamma = it->ggHat_B.dot(it->IGPntGc_B * it->ggHat_B + it->IWPntWc_B * it->ggHat_B + + P * (it->l * it->gtHat_B - it->d * cos(it->theta) * it->gsHat_B) + + Q * ggHatTilde_B * it->rGcG_B); + it->agamma = 1.0 / it->egamma * it->massV * rTildeVcG_B * it->ggHat_B; + it->bgamma = -1.0 / it->egamma * + (IVPntVc_B.transpose() * it->ggHat_B - it->massV * rTildeVcB_B * rTildeVcG_B * it->ggHat_B); + it->cgamma = -1.0 / it->egamma * + (it->ggHat_B.dot(it->IWPntWc_B * it->gsHat_B) + it->d * it->ggHat_B.dot(P * it->w3Hat_B)); + it->dgamma = + -1.0 / it->egamma * + it->ggHat_B.dot( + it->gammaDot * Q * ggHatTilde_B * rPrimeGcG_B + + P * + ((2.0 * it->d * it->gammaDot * it->Omega * sin(it->theta) - it->l * gammaDotSquared) * it->gsHat_B - + it->d * gammaDotSquared * cos(it->theta) * it->gtHat_B - it->d * OmegaSquared * it->w2Hat_B) + + it->IPrimeGPntGc_B * omega_GN_B + omegaTilde * it->IGPntGc_B * omega_GN_B + + it->IWPntWc_B * it->Omega * it->gammaDot * it->gtHat_B + it->IPrimeWPntWc_B * omega_WN_B + + omegaTilde * it->IWPntWc_B * omega_WN_B + + it->massG * rTildeGcVc_B * (2.0 * omegaTilde * rPrimeGcVc_B + omegaTilde * omegaTilde * rGcVc_B) + + it->massW * rTildeWcVc_B * (2.0 * omegaTilde * rPrimeWcVc_B + omegaTildeSquared * rWcVc_B) + + it->massV * rTildeVcG_B * (2.0 * omegaTilde * rPrimeVcB_B + omegaTildeSquared * rVcB_B)) + + 1.0 / it->egamma * (it->u_g_current + it->gravityTorqueGimbal_g); + + it->eOmega = it->IW1 + it->massW * dSquared; + it->aOmega = -1.0 / it->eOmega * it->massW * it->d * it->w3Hat_B; + it->bOmega = -1.0 / it->eOmega * + (it->IWPntWc_B.transpose() * it->gsHat_B - + it->massW * it->d * it->rTildeWcB_B * w2HatTilde_B * it->gsHat_B); + it->cOmega = -1.0 / it->eOmega * (it->IW13 * cos(it->theta) - it->massW * it->d * it->l * sin(it->theta)); + it->dOmega = + -1.0 / it->eOmega * + (it->gsHat_B.dot(it->IPrimeWPntWc_B * omega_WN_B) + + it->gsHat_B.transpose() * omegaTilde * it->IWPntWc_B * omega_WN_B + + it->massW * it->d * it->gsHat_B.transpose() * w2HatTilde_B * + (2.0 * it->rPrimeTildeWcB_B.transpose() * omegaLoc_BN_B + omegaTilde * omegaTilde * it->rWcB_B)) + + (1.0 / it->eOmega) * (it->IW13 * sin(it->theta) * it->Omega * it->gammaDot - + it->massW * dSquared * gammaDotSquared * cos(it->theta) * sin(it->theta) + + it->u_s_current + it->gravityTorqueWheel_s); + + it->p = (it->aOmega + it->cOmega * it->agamma) / (1.0 - it->cOmega * it->cgamma); + it->q = (it->bOmega + it->cOmega * it->bgamma) / (1.0 - it->cOmega * it->cgamma); + it->s = (it->dOmega + it->cOmega * it->dgamma) / (1.0 - it->cOmega * it->cgamma); + + ur = it->massG * ggHatTilde_B * it->rGcG_B - it->massW * it->d * cos(it->theta) * it->gsHat_B + + it->massW * it->l * it->gtHat_B; + vr = it->massW * it->d * it->w3Hat_B; + kr = it->massG * it->gammaDot * ggHatTilde_B * it->rPrimeGcB_B + + it->massW * + ((2.0 * it->d * it->gammaDot * it->Omega * sin(it->theta) - it->l * gammaDotSquared) * it->gsHat_B - + it->d * gammaDotSquared * cos(it->theta) * it->gtHat_B - it->d * OmegaSquared * it->w2Hat_B); + + uomega = it->IGPntGc_B * it->ggHat_B + it->massG * it->rTildeGcB_B * ggHatTilde_B * it->rGcG_B + + it->IWPntWc_B * it->ggHat_B + + it->massW * it->rTildeWcB_B * (it->l * it->gtHat_B - it->d * cos(it->theta) * it->gsHat_B); + vomega = it->IWPntWc_B * it->gsHat_B + it->massW * it->d * it->rTildeWcB_B * it->w3Hat_B; + komega = + it->IPrimeGPntGc_B * it->gammaDot * it->ggHat_B + + omegaTilde * it->IGPntGc_B * it->gammaDot * it->ggHat_B + + it->massG * omegaTilde * it->rTildeGcB_B * it->rPrimeGcB_B + + it->massG * it->gammaDot * it->rTildeGcB_B * ggHatTilde_B * rPrimeGcG_B + + it->IWPntWc_B * it->Omega * it->gammaDot * it->gtHat_B + it->IPrimeWPntWc_B * omega_WB_B + + omegaTilde * it->IWPntWc_B * omega_WB_B + it->massW * omegaTilde * it->rTildeWcB_B * it->rPrimeWcB_B + + it->massW * it->rTildeWcB_B * + ((2.0 * it->d * it->gammaDot * it->Omega * sin(it->theta) - it->l * gammaDotSquared) * it->gsHat_B - + it->d * gammaDotSquared * cos(it->theta) * it->gtHat_B - it->d * OmegaSquared * it->w2Hat_B); + + backSubContr.matrixA += ur * it->agamma.transpose() + (vr + ur * it->cgamma) * it->p.transpose(); + backSubContr.matrixB += ur * it->bgamma.transpose() + (vr + ur * it->cgamma) * it->q.transpose(); + backSubContr.matrixC += + uomega * it->agamma.transpose() + (vomega + uomega * it->cgamma) * it->p.transpose(); + backSubContr.matrixD += + uomega * it->bgamma.transpose() + (vomega + uomega * it->cgamma) * it->q.transpose(); + backSubContr.vecTrans -= kr + ur * it->dgamma + (vr + ur * it->cgamma) * it->s; + backSubContr.vecRot -= komega + uomega * it->dgamma + (vomega + uomega * it->cgamma) * it->s; + } + } + return; } -void VSCMGStateEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +VSCMGStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { - Eigen::MatrixXd OmegasDot(this->numVSCMG,1); - Eigen::MatrixXd thetasDot(this->numVSCMGJitter,1); - Eigen::MatrixXd gammasDot(this->numVSCMG,1); - Eigen::MatrixXd gammaDotsDot(this->numVSCMG,1); - Eigen::Vector3d omegaDotBNLoc_B; - Eigen::Vector3d omegaLoc_BN_B; - Eigen::MRPd sigmaBNLocal; - Eigen::Matrix3d dcm_BN; /* direction cosine matrix from N to B */ - Eigen::Matrix3d dcm_NB; /* direction cosine matrix from B to N */ - Eigen::Vector3d rDDotBNLoc_N; /* second time derivative of rBN in N frame */ - Eigen::Vector3d rDDotBNLoc_B; /* second time derivative of rBN in B frame */ - double omegas; - double omegat; - int VSCMGi = 0; + Eigen::MatrixXd OmegasDot(this->numVSCMG, 1); + Eigen::MatrixXd thetasDot(this->numVSCMGJitter, 1); + Eigen::MatrixXd gammasDot(this->numVSCMG, 1); + Eigen::MatrixXd gammaDotsDot(this->numVSCMG, 1); + Eigen::Vector3d omegaDotBNLoc_B; + Eigen::Vector3d omegaLoc_BN_B; + Eigen::MRPd sigmaBNLocal; + Eigen::Matrix3d dcm_BN; /* direction cosine matrix from N to B */ + Eigen::Matrix3d dcm_NB; /* direction cosine matrix from B to N */ + Eigen::Vector3d rDDotBNLoc_N; /* second time derivative of rBN in N frame */ + Eigen::Vector3d rDDotBNLoc_B; /* second time derivative of rBN in B frame */ + double omegas; + double omegat; + int VSCMGi = 0; int thetaCount = 0; - std::vector::iterator it; - - //! Grab necessarry values from manager - omegaDotBNLoc_B = omegaDot_BN_B; - omegaLoc_BN_B = this->hubOmega->getState(); - rDDotBNLoc_N = rDDot_BN_N; - sigmaBNLocal = (Eigen::Vector3d ) sigma_BN; - dcm_NB = sigmaBNLocal.toRotationMatrix(); - dcm_BN = dcm_NB.transpose(); - rDDotBNLoc_B = dcm_BN*rDDotBNLoc_N; - - //! - Compute Derivatives - for(it=VSCMGData.begin(); it!=VSCMGData.end(); it++) - { - omegas = it->gsHat_B.transpose()*omegaLoc_BN_B; - omegat = it->gtHat_B.transpose()*omegaLoc_BN_B; - - gammasDot(VSCMGi,0) = it->gammaDot; - if(it->VSCMGModel == vscmgJitterFullyCoupled || it->VSCMGModel == vscmgJitterSimple) { + std::vector::iterator it; + + //! Grab necessarry values from manager + omegaDotBNLoc_B = omegaDot_BN_B; + omegaLoc_BN_B = this->hubOmega->getState(); + rDDotBNLoc_N = rDDot_BN_N; + sigmaBNLocal = (Eigen::Vector3d)sigma_BN; + dcm_NB = sigmaBNLocal.toRotationMatrix(); + dcm_BN = dcm_NB.transpose(); + rDDotBNLoc_B = dcm_BN * rDDotBNLoc_N; + + //! - Compute Derivatives + for (it = VSCMGData.begin(); it != VSCMGData.end(); it++) { + omegas = it->gsHat_B.transpose() * omegaLoc_BN_B; + omegat = it->gtHat_B.transpose() * omegaLoc_BN_B; + + gammasDot(VSCMGi, 0) = it->gammaDot; + if (it->VSCMGModel == vscmgJitterFullyCoupled || it->VSCMGModel == vscmgJitterSimple) { // - Set trivial kinemetic derivative - thetasDot(thetaCount,0) = it->Omega; + thetasDot(thetaCount, 0) = it->Omega; thetaCount++; } - if (it->VSCMGModel == vscmgBalancedWheels || it->VSCMGModel == vscmgJitterSimple) { - OmegasDot(VSCMGi,0) = - omegat*it->gammaDot - it->gsHat_B.transpose()*omegaDotBNLoc_B + (it->u_s_current+it->gravityTorqueWheel_s)/it->IW1; - gammaDotsDot(VSCMGi,0) = 1/it->IV3*((it->u_g_current+it->gravityTorqueGimbal_g)+(it->IV1-it->IV2)*omegas*omegat+it->IW1*it->Omega*omegat-it->IV3*it->ggHat_B.transpose()*omegaDotBNLoc_B); - } else if(it->VSCMGModel == vscmgJitterFullyCoupled) { - OmegasDot(VSCMGi,0) = it->p.dot(rDDotBNLoc_B) + it->q.dot(omegaDotBNLoc_B) + it->s; - gammaDotsDot(VSCMGi,0) = it->agamma.dot(rDDotBNLoc_B) + it->bgamma.dot(omegaDotBNLoc_B) + it->cgamma*OmegasDot(VSCMGi,0) + it->dgamma; - } - VSCMGi++; - } - - OmegasState->setDerivative(OmegasDot); - gammasState->setDerivative(gammasDot); - gammaDotsState->setDerivative(gammaDotsDot); + if (it->VSCMGModel == vscmgBalancedWheels || it->VSCMGModel == vscmgJitterSimple) { + OmegasDot(VSCMGi, 0) = -omegat * it->gammaDot - it->gsHat_B.transpose() * omegaDotBNLoc_B + + (it->u_s_current + it->gravityTorqueWheel_s) / it->IW1; + gammaDotsDot(VSCMGi, 0) = + 1 / it->IV3 * + ((it->u_g_current + it->gravityTorqueGimbal_g) + (it->IV1 - it->IV2) * omegas * omegat + + it->IW1 * it->Omega * omegat - it->IV3 * it->ggHat_B.transpose() * omegaDotBNLoc_B); + } else if (it->VSCMGModel == vscmgJitterFullyCoupled) { + OmegasDot(VSCMGi, 0) = it->p.dot(rDDotBNLoc_B) + it->q.dot(omegaDotBNLoc_B) + it->s; + gammaDotsDot(VSCMGi, 0) = it->agamma.dot(rDDotBNLoc_B) + it->bgamma.dot(omegaDotBNLoc_B) + + it->cgamma * OmegasDot(VSCMGi, 0) + it->dgamma; + } + VSCMGi++; + } + + OmegasState->setDerivative(OmegasDot); + gammasState->setDerivative(gammasDot); + gammaDotsState->setDerivative(gammaDotsDot); if (this->numVSCMGJitter > 0) { thetasState->setDerivative(thetasDot); } } -void VSCMGStateEffector::updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B) +void +VSCMGStateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { - Eigen::MRPd sigmaBNLocal; - Eigen::Matrix3d dcm_BN; /* direction cosine matrix from N to B */ - Eigen::Matrix3d dcm_NB; /* direction cosine matrix from B to N */ - Eigen::Vector3d omegaLoc_BN_B = hubOmega->getState(); + Eigen::MRPd sigmaBNLocal; + Eigen::Matrix3d dcm_BN; /* direction cosine matrix from N to B */ + Eigen::Matrix3d dcm_NB; /* direction cosine matrix from B to N */ + Eigen::Vector3d omegaLoc_BN_B = hubOmega->getState(); //! - Compute energy and momentum contribution of each wheel rotAngMomPntCContr_B.setZero(); std::vector::iterator it; - for(it=VSCMGData.begin(); it!=VSCMGData.end(); it++) - { - Eigen::Vector3d omega_GN_B = omegaLoc_BN_B + it->gammaDot*it->ggHat_B; - Eigen::Vector3d omega_WN_B = omega_GN_B + it->Omega*it->gsHat_B; - if (it->VSCMGModel == vscmgBalancedWheels || it->VSCMGModel == vscmgJitterSimple) { - it->rWcB_B = it->rGB_B; - Eigen::Vector3d rDotWcB_B = omegaLoc_BN_B.cross(it->rWcB_B); - rotAngMomPntCContr_B += it->IWPntWc_B*omega_WN_B + it->IGPntGc_B*omega_GN_B + it->massV*it->rWcB_B.cross(rDotWcB_B); - rotEnergyContr += 1.0/2.0*omega_WN_B.dot(it->IWPntWc_B*omega_WN_B) + 1.0/2.0*omega_GN_B.dot(it->IGPntGc_B*omega_GN_B) + 1.0/2.0*it->massV*rDotWcB_B.dot(rDotWcB_B); - } else if (it->VSCMGModel == vscmgJitterFullyCoupled) { - Eigen::Vector3d rDotWcB_B = it->rPrimeWcB_B + omegaLoc_BN_B.cross(it->rWcB_B); - Eigen::Vector3d rDotGcB_B = it->rPrimeGcB_B + omegaLoc_BN_B.cross(it->rGcB_B); - rotAngMomPntCContr_B += it->IWPntWc_B*omega_WN_B + it->massW*it->rWcB_B.cross(rDotWcB_B); - rotAngMomPntCContr_B += it->IGPntGc_B*omega_GN_B + it->massG*it->rGcB_B.cross(rDotGcB_B); - rotEnergyContr += 1.0/2.0*omega_WN_B.transpose()*it->IWPntWc_B*omega_WN_B + 1.0/2.0*it->massW*rDotWcB_B.dot(rDotWcB_B); - rotEnergyContr += 1.0/2.0*omega_GN_B.transpose()*it->IGPntGc_B*omega_GN_B + 1.0/2.0*it->massG*rDotGcB_B.dot(rDotGcB_B); - } + for (it = VSCMGData.begin(); it != VSCMGData.end(); it++) { + Eigen::Vector3d omega_GN_B = omegaLoc_BN_B + it->gammaDot * it->ggHat_B; + Eigen::Vector3d omega_WN_B = omega_GN_B + it->Omega * it->gsHat_B; + if (it->VSCMGModel == vscmgBalancedWheels || it->VSCMGModel == vscmgJitterSimple) { + it->rWcB_B = it->rGB_B; + Eigen::Vector3d rDotWcB_B = omegaLoc_BN_B.cross(it->rWcB_B); + rotAngMomPntCContr_B += + it->IWPntWc_B * omega_WN_B + it->IGPntGc_B * omega_GN_B + it->massV * it->rWcB_B.cross(rDotWcB_B); + rotEnergyContr += 1.0 / 2.0 * omega_WN_B.dot(it->IWPntWc_B * omega_WN_B) + + 1.0 / 2.0 * omega_GN_B.dot(it->IGPntGc_B * omega_GN_B) + + 1.0 / 2.0 * it->massV * rDotWcB_B.dot(rDotWcB_B); + } else if (it->VSCMGModel == vscmgJitterFullyCoupled) { + Eigen::Vector3d rDotWcB_B = it->rPrimeWcB_B + omegaLoc_BN_B.cross(it->rWcB_B); + Eigen::Vector3d rDotGcB_B = it->rPrimeGcB_B + omegaLoc_BN_B.cross(it->rGcB_B); + rotAngMomPntCContr_B += it->IWPntWc_B * omega_WN_B + it->massW * it->rWcB_B.cross(rDotWcB_B); + rotAngMomPntCContr_B += it->IGPntGc_B * omega_GN_B + it->massG * it->rGcB_B.cross(rDotGcB_B); + rotEnergyContr += 1.0 / 2.0 * omega_WN_B.transpose() * it->IWPntWc_B * omega_WN_B + + 1.0 / 2.0 * it->massW * rDotWcB_B.dot(rDotWcB_B); + rotEnergyContr += 1.0 / 2.0 * omega_GN_B.transpose() * it->IGPntGc_B * omega_GN_B + + 1.0 / 2.0 * it->massG * rDotGcB_B.dot(rDotGcB_B); + } } return; } - /*! Reset the module to origina configuration values. */ -void VSCMGStateEffector::Reset(uint64_t CurrenSimNanos) +void +VSCMGStateEffector::Reset(uint64_t CurrenSimNanos) { VSCMGCmdMsgPayload VSCMGCmdInitializer; VSCMGCmdInitializer.u_s_cmd = 0.0; @@ -493,18 +547,17 @@ void VSCMGStateEffector::Reset(uint64_t CurrenSimNanos) //! - Clear out any currently firing VSCMGs and re-init cmd array this->newVSCMGCmds.clear(); - this->newVSCMGCmds.insert(this->newVSCMGCmds.begin(), this->VSCMGData.size(), VSCMGCmdInitializer ); + this->newVSCMGCmds.insert(this->newVSCMGCmds.begin(), this->VSCMGData.size(), VSCMGCmdInitializer); std::vector::iterator it; - for (it = VSCMGData.begin(); it != VSCMGData.end(); it++) - { + for (it = VSCMGData.begin(); it != VSCMGData.end(); it++) { it->w2Hat0_B = it->gtHat0_B; it->w3Hat0_B = it->ggHat_B; //! Define CoM offset d and off-diagonal inertia IW13 if using fully coupled model if (it->VSCMGModel == vscmgJitterFullyCoupled) { - it->d = it->U_s/it->massW; //!< determine CoM offset from static imbalance parameter - it->IW13 = it->U_d; //!< off-diagonal inertia is equal to dynamic imbalance parameter + it->d = it->U_s / it->massW; //!< determine CoM offset from static imbalance parameter + it->IW13 = it->U_d; //!< off-diagonal inertia is equal to dynamic imbalance parameter } if (it->VSCMGModel == vscmgBalancedWheels || it->VSCMGModel == vscmgJitterSimple) { it->IV1 = it->IW1 + it->IG1; @@ -527,8 +580,8 @@ void VSCMGStateEffector::Reset(uint64_t CurrenSimNanos) it->rGcG_B = dcm_BG * it->rGcG_G; it->massV = it->massG + it->massW; - it->rhoG = it->massG/it->massV; - it->rhoW = it->massW/it->massV; + it->rhoG = it->massG / it->massV; + it->rhoW = it->massW / it->massV; } } @@ -537,13 +590,13 @@ void VSCMGStateEffector::Reset(uint64_t CurrenSimNanos) @param CurrentClock The current time used for time-stamping the message */ -void VSCMGStateEffector::WriteOutputMessages(uint64_t CurrentClock) +void +VSCMGStateEffector::WriteOutputMessages(uint64_t CurrentClock) { this->outputStates = this->speedOutMsg.zeroMsgPayload; - VSCMGConfigMsgPayload tmpVSCMG; - std::vector::iterator it; - for (it = VSCMGData.begin(); it != VSCMGData.end(); it++) - { + VSCMGConfigMsgPayload tmpVSCMG; + std::vector::iterator it; + for (it = VSCMGData.begin(); it != VSCMGData.end(); it++) { tmpVSCMG = this->vscmgOutMsgs[0]->zeroMsgPayload; if (numVSCMGJitter > 0) { double thetaCurrent = this->thetasState->getState()(it - VSCMGData.begin(), 0); @@ -551,37 +604,37 @@ void VSCMGStateEffector::WriteOutputMessages(uint64_t CurrentClock) } double omegaCurrent = this->OmegasState->getState()(it - VSCMGData.begin(), 0); it->Omega = omegaCurrent; - this->outputStates.wheelSpeeds[it - VSCMGData.begin()] = it->Omega; - double gammaCurrent = this->gammasState->getState()(it - VSCMGData.begin(), 0); - it->gamma = gammaCurrent; - this->outputStates.gimbalAngles[it - VSCMGData.begin()] = it->gamma; - double gammaDotCurrent = this->gammaDotsState->getState()(it - VSCMGData.begin(), 0); - it->gammaDot = gammaDotCurrent; - this->outputStates.gimbalRates[it - VSCMGData.begin()] = it->gammaDot; - - tmpVSCMG.u_s_current = it->u_s_current; - tmpVSCMG.u_s_max = it->u_s_max; - tmpVSCMG.u_s_min = it->u_s_min; - tmpVSCMG.u_s_f = it->u_s_f; - tmpVSCMG.u_g_current = it->u_g_current; - tmpVSCMG.u_g_max = it->u_g_max; - tmpVSCMG.u_g_min = it->u_g_min; - tmpVSCMG.u_g_f = it->u_g_f; - tmpVSCMG.theta = it->theta; - tmpVSCMG.Omega = it->Omega; - tmpVSCMG.Omega_max = it->Omega_max; - tmpVSCMG.gamma = it->gamma; - tmpVSCMG.gammaDot = it->gammaDot; - tmpVSCMG.gammaDot_max = it->gammaDot_max; - tmpVSCMG.IW1 = it->IW1; - tmpVSCMG.U_s = it->U_s; - tmpVSCMG.U_d = it->U_d; - tmpVSCMG.VSCMGModel = it->VSCMGModel; - // Write out config data for each VSCMG + this->outputStates.wheelSpeeds[it - VSCMGData.begin()] = it->Omega; + double gammaCurrent = this->gammasState->getState()(it - VSCMGData.begin(), 0); + it->gamma = gammaCurrent; + this->outputStates.gimbalAngles[it - VSCMGData.begin()] = it->gamma; + double gammaDotCurrent = this->gammaDotsState->getState()(it - VSCMGData.begin(), 0); + it->gammaDot = gammaDotCurrent; + this->outputStates.gimbalRates[it - VSCMGData.begin()] = it->gammaDot; + + tmpVSCMG.u_s_current = it->u_s_current; + tmpVSCMG.u_s_max = it->u_s_max; + tmpVSCMG.u_s_min = it->u_s_min; + tmpVSCMG.u_s_f = it->u_s_f; + tmpVSCMG.u_g_current = it->u_g_current; + tmpVSCMG.u_g_max = it->u_g_max; + tmpVSCMG.u_g_min = it->u_g_min; + tmpVSCMG.u_g_f = it->u_g_f; + tmpVSCMG.theta = it->theta; + tmpVSCMG.Omega = it->Omega; + tmpVSCMG.Omega_max = it->Omega_max; + tmpVSCMG.gamma = it->gamma; + tmpVSCMG.gammaDot = it->gammaDot; + tmpVSCMG.gammaDot_max = it->gammaDot_max; + tmpVSCMG.IW1 = it->IW1; + tmpVSCMG.U_s = it->U_s; + tmpVSCMG.U_d = it->U_d; + tmpVSCMG.VSCMGModel = it->VSCMGModel; + // Write out config data for each VSCMG this->vscmgOutMsgs.at(it - VSCMGData.begin())->write(&tmpVSCMG, this->moduleID, CurrentClock); - } + } - // Write this message once for all VSCMGs + // Write this message once for all VSCMGs this->speedOutMsg.write(&this->outputStates, this->moduleID, CurrentClock); } @@ -589,44 +642,38 @@ void VSCMGStateEffector::WriteOutputMessages(uint64_t CurrentClock) associated command structure for operating the VSCMGs. */ -void VSCMGStateEffector::ReadInputs() +void +VSCMGStateEffector::ReadInputs() { - uint64_t i; + uint64_t i; /* zero the incoming commands */ - VSCMGCmdMsgPayload *CmdPtr; - for(i=0, CmdPtr = this->newVSCMGCmds.data(); iVSCMGData.size(); - CmdPtr++, i++) - { + VSCMGCmdMsgPayload* CmdPtr; + for (i = 0, CmdPtr = this->newVSCMGCmds.data(); i < this->VSCMGData.size(); CmdPtr++, i++) { CmdPtr->u_s_cmd = 0.0; CmdPtr->u_g_cmd = 0.0; } //! - If the input message ID is invalid, return without touching states - if(!this->cmdsInMsg.isLinked() || !this->cmdsInMsg.isWritten()) - { - bskLogger.bskLog(BSK_ERROR, "vscmgStateEffector.cmdsInMsg was not linked or written."); - return; - } + if (!this->cmdsInMsg.isLinked() || !this->cmdsInMsg.isWritten()) { + bskLogger.bskLog(BSK_ERROR, "vscmgStateEffector.cmdsInMsg was not linked or written."); + return; + } - //! - Zero the command buffer and read the incoming command array + //! - Zero the command buffer and read the incoming command array this->incomingCmdBuffer = this->cmdsInMsg(); + //! - Check if message has already been read, if stale return + // if(prevCommandTime==LocalHeader.WriteClockNanos) { + // return; + // } + prevCommandTime = this->cmdsInMsg.timeWritten(); - //! - Check if message has already been read, if stale return - // if(prevCommandTime==LocalHeader.WriteClockNanos) { - // return; - // } - prevCommandTime = this->cmdsInMsg.timeWritten(); - - //! - Set the NewVSCMGCmds vector. Using the data() method for raw speed - for(i=0, CmdPtr = this->newVSCMGCmds.data(); iVSCMGData.size(); - CmdPtr++, i++) - { - CmdPtr->u_s_cmd = this->incomingCmdBuffer.wheelTorque[i]; - CmdPtr->u_g_cmd = this->incomingCmdBuffer.gimbalTorque[i]; - } - + //! - Set the NewVSCMGCmds vector. Using the data() method for raw speed + for (i = 0, CmdPtr = this->newVSCMGCmds.data(); i < this->VSCMGData.size(); CmdPtr++, i++) { + CmdPtr->u_s_cmd = this->incomingCmdBuffer.wheelTorque[i]; + CmdPtr->u_g_cmd = this->incomingCmdBuffer.gimbalTorque[i]; + } } /*! This method is used to read the new commands vector and set the VSCMG @@ -635,107 +682,108 @@ void VSCMGStateEffector::ReadInputs() @param CurrentTime The current simulation time converted to a double */ -void VSCMGStateEffector::ConfigureVSCMGRequests(double CurrentTime) +void +VSCMGStateEffector::ConfigureVSCMGRequests(double CurrentTime) { - std::vector::iterator CmdIt; - int it = 0; - double u_s; - double u_g; - double omegaCritical; - double gammaDotCritical; - - // loop through commands - for(CmdIt=this->newVSCMGCmds.begin(); CmdIt!=this->newVSCMGCmds.end(); CmdIt++) - { - // wheel torque saturation - // set u_s_max to less than zero to disable saturation - if (this->VSCMGData[it].u_s_max > 0.0) { - if(CmdIt->u_s_cmd > this->VSCMGData[it].u_s_max) { - CmdIt->u_s_cmd = this->VSCMGData[it].u_s_max; - } else if(CmdIt->u_s_cmd < -this->VSCMGData[it].u_s_max) { - CmdIt->u_s_cmd = -this->VSCMGData[it].u_s_max; - } - } - - //! gimbal torque saturation - //! set u_g_max to less than zero to disable saturation - if (this->VSCMGData[it].u_g_max > 0.0) { - if(CmdIt->u_g_cmd > this->VSCMGData[it].u_g_max) { - CmdIt->u_g_cmd = this->VSCMGData[it].u_g_max; - } else if(CmdIt->u_g_cmd < -this->VSCMGData[it].u_g_max) { - CmdIt->u_g_cmd = -this->VSCMGData[it].u_g_max; - } - } - - //! minimum wheel torque - //! set u_s_min to less than zero to disable minimum torque - if( std::abs(CmdIt->u_s_cmd) < this->VSCMGData[it].u_s_min) { - CmdIt->u_s_cmd = 0.0; - } - - //! minimum gimbal torque - //! set u_g_min to less than zero to disable minimum torque - if( std::abs(CmdIt->u_g_cmd) < this->VSCMGData[it].u_g_min) { - CmdIt->u_g_cmd = 0.0; - } + std::vector::iterator CmdIt; + int it = 0; + double u_s; + double u_g; + double omegaCritical; + double gammaDotCritical; + + // loop through commands + for (CmdIt = this->newVSCMGCmds.begin(); CmdIt != this->newVSCMGCmds.end(); CmdIt++) { + // wheel torque saturation + // set u_s_max to less than zero to disable saturation + if (this->VSCMGData[it].u_s_max > 0.0) { + if (CmdIt->u_s_cmd > this->VSCMGData[it].u_s_max) { + CmdIt->u_s_cmd = this->VSCMGData[it].u_s_max; + } else if (CmdIt->u_s_cmd < -this->VSCMGData[it].u_s_max) { + CmdIt->u_s_cmd = -this->VSCMGData[it].u_s_max; + } + } + + //! gimbal torque saturation + //! set u_g_max to less than zero to disable saturation + if (this->VSCMGData[it].u_g_max > 0.0) { + if (CmdIt->u_g_cmd > this->VSCMGData[it].u_g_max) { + CmdIt->u_g_cmd = this->VSCMGData[it].u_g_max; + } else if (CmdIt->u_g_cmd < -this->VSCMGData[it].u_g_max) { + CmdIt->u_g_cmd = -this->VSCMGData[it].u_g_max; + } + } + + //! minimum wheel torque + //! set u_s_min to less than zero to disable minimum torque + if (std::abs(CmdIt->u_s_cmd) < this->VSCMGData[it].u_s_min) { + CmdIt->u_s_cmd = 0.0; + } + + //! minimum gimbal torque + //! set u_g_min to less than zero to disable minimum torque + if (std::abs(CmdIt->u_g_cmd) < this->VSCMGData[it].u_g_min) { + CmdIt->u_g_cmd = 0.0; + } // Speed saturation - if (std::abs(this->VSCMGData[it].Omega)>= this->VSCMGData[it].Omega_max - && this->VSCMGData[it].Omega_max > 0.0 /* negative Omega_max turns of wheel saturation modeling */ - ) { + if (std::abs(this->VSCMGData[it].Omega) >= this->VSCMGData[it].Omega_max && + this->VSCMGData[it].Omega_max > 0.0 /* negative Omega_max turns of wheel saturation modeling */ + ) { CmdIt->u_s_cmd = 0.0; -// printf("Omega_max = %f\n", this->VSCMGData[it].Omega_max); + // printf("Omega_max = %f\n", this->VSCMGData[it].Omega_max); } - //! wheel Coulomb friction - //! set wheelLinearFrictionRatio to less than zero to disable linear friction - //! set u_s_f to zero to disable all friction - if (this->VSCMGData[it].wheelLinearFrictionRatio > 0.0) { + //! wheel Coulomb friction + //! set wheelLinearFrictionRatio to less than zero to disable linear friction + //! set u_s_f to zero to disable all friction + if (this->VSCMGData[it].wheelLinearFrictionRatio > 0.0) { if (this->VSCMGData[it].Omega_max < 0.0) { - bskLogger.bskLog(BSK_ERROR, "VSCMGStateEffector: Omega_max must be set to a positive value to use wheelLinearFrictionRatio."); + bskLogger.bskLog( + BSK_ERROR, + "VSCMGStateEffector: Omega_max must be set to a positive value to use wheelLinearFrictionRatio."); } - omegaCritical = this->VSCMGData[it].Omega_max * this->VSCMGData[it].wheelLinearFrictionRatio; - } else { - omegaCritical = 0.0; - } - if(this->VSCMGData[it].Omega > omegaCritical) { - u_s = CmdIt->u_s_cmd - this->VSCMGData[it].u_s_f; - } else if(this->VSCMGData[it].Omega < -omegaCritical) { - u_s = CmdIt->u_s_cmd + this->VSCMGData[it].u_s_f; - } else { - if (this->VSCMGData[it].wheelLinearFrictionRatio > 0) { - u_s = CmdIt->u_s_cmd - this->VSCMGData[it].u_s_f*this->VSCMGData[it].Omega/omegaCritical; - } else { - u_s = CmdIt->u_s_cmd; - } - } - - //! gimbal Coulomb friction - //! set gimbalLinearFrictionRatio to less than zero to disable linear friction - //! set u_g_f to zero to disable friction - if (this->VSCMGData[it].gimbalLinearFrictionRatio > 0.0) { - gammaDotCritical = this->VSCMGData[it].gammaDot_max * this->VSCMGData[it].wheelLinearFrictionRatio; - } else { - gammaDotCritical = 0.0; - } - if(this->VSCMGData[it].gammaDot > gammaDotCritical) { - u_g = CmdIt->u_g_cmd - this->VSCMGData[it].u_g_f; - } else if(this->VSCMGData[it].gammaDot < -gammaDotCritical) { - u_g = CmdIt->u_g_cmd + this->VSCMGData[it].u_g_f; - } else { - if (this->VSCMGData[it].gimbalLinearFrictionRatio > 0) { - u_g = CmdIt->u_g_cmd - this->VSCMGData[it].u_g_f*this->VSCMGData[it].gammaDot/gammaDotCritical; - } else { - u_g = CmdIt->u_g_cmd; - } - } - - this->VSCMGData[it].u_s_current = u_s; // save actual torque for wheel motor - this->VSCMGData[it].u_g_current = u_g; // save actual torque for wheel motor - - it++; - - } + omegaCritical = this->VSCMGData[it].Omega_max * this->VSCMGData[it].wheelLinearFrictionRatio; + } else { + omegaCritical = 0.0; + } + if (this->VSCMGData[it].Omega > omegaCritical) { + u_s = CmdIt->u_s_cmd - this->VSCMGData[it].u_s_f; + } else if (this->VSCMGData[it].Omega < -omegaCritical) { + u_s = CmdIt->u_s_cmd + this->VSCMGData[it].u_s_f; + } else { + if (this->VSCMGData[it].wheelLinearFrictionRatio > 0) { + u_s = CmdIt->u_s_cmd - this->VSCMGData[it].u_s_f * this->VSCMGData[it].Omega / omegaCritical; + } else { + u_s = CmdIt->u_s_cmd; + } + } + + //! gimbal Coulomb friction + //! set gimbalLinearFrictionRatio to less than zero to disable linear friction + //! set u_g_f to zero to disable friction + if (this->VSCMGData[it].gimbalLinearFrictionRatio > 0.0) { + gammaDotCritical = this->VSCMGData[it].gammaDot_max * this->VSCMGData[it].wheelLinearFrictionRatio; + } else { + gammaDotCritical = 0.0; + } + if (this->VSCMGData[it].gammaDot > gammaDotCritical) { + u_g = CmdIt->u_g_cmd - this->VSCMGData[it].u_g_f; + } else if (this->VSCMGData[it].gammaDot < -gammaDotCritical) { + u_g = CmdIt->u_g_cmd + this->VSCMGData[it].u_g_f; + } else { + if (this->VSCMGData[it].gimbalLinearFrictionRatio > 0) { + u_g = CmdIt->u_g_cmd - this->VSCMGData[it].u_g_f * this->VSCMGData[it].gammaDot / gammaDotCritical; + } else { + u_g = CmdIt->u_g_cmd; + } + } + + this->VSCMGData[it].u_s_current = u_s; // save actual torque for wheel motor + this->VSCMGData[it].u_g_current = u_g; // save actual torque for wheel motor + + it++; + } } /*! This method is the main cyclical call for the scheduled part of the VSCMG @@ -746,12 +794,13 @@ void VSCMGStateEffector::ConfigureVSCMGRequests(double CurrentTime) @param CurrentSimNanos The current simulation time in nanoseconds */ -void VSCMGStateEffector::UpdateState(uint64_t CurrentSimNanos) +void +VSCMGStateEffector::UpdateState(uint64_t CurrentSimNanos) { - //! - Read the inputs and then call ConfigureVSCMGRequests to set up dynamics - ReadInputs(); - ConfigureVSCMGRequests(CurrentSimNanos*NANO2SEC); - WriteOutputMessages(CurrentSimNanos); + //! - Read the inputs and then call ConfigureVSCMGRequests to set up dynamics + ReadInputs(); + ConfigureVSCMGRequests(CurrentSimNanos * NANO2SEC); + WriteOutputMessages(CurrentSimNanos); } /*! @@ -759,12 +808,13 @@ void VSCMGStateEffector::UpdateState(uint64_t CurrentSimNanos) @param NewVSCMG VSCMG device to be added */ -void VSCMGStateEffector::AddVSCMG(VSCMGConfigMsgPayload *NewVSCMG) +void +VSCMGStateEffector::AddVSCMG(VSCMGConfigMsgPayload* NewVSCMG) { this->VSCMGData.push_back(*NewVSCMG); /* add a VSCMG output message for this device */ - Message *msg; + Message* msg; msg = new Message; this->vscmgOutMsgs.push_back(msg); } diff --git a/src/simulation/dynamics/VSCMGs/vscmgStateEffector.h b/src/simulation/dynamics/VSCMGs/vscmgStateEffector.h old mode 100755 new mode 100644 index 48bc41224b..acba7bf572 --- a/src/simulation/dynamics/VSCMGs/vscmgStateEffector.h +++ b/src/simulation/dynamics/VSCMGs/vscmgStateEffector.h @@ -17,7 +17,6 @@ */ - #ifndef VSCMGSTATEEFFECTOR_H #define VSCMGSTATEEFFECTOR_H @@ -39,56 +38,64 @@ #include "architecture/utilities/avsEigenMRP.h" #include "architecture/utilities/avsEigenSupport.h" - - /*! @brief VSCMG state effector class */ -class VSCMGStateEffector: public SysModel, public StateEffector { -public: +class VSCMGStateEffector + : public SysModel + , public StateEffector +{ + public: VSCMGStateEffector(); - ~VSCMGStateEffector(); - void registerStates(DynParamManager& states); - void linkInStates(DynParamManager& states); + ~VSCMGStateEffector(); + void registerStates(DynParamManager& states); + void linkInStates(DynParamManager& states); void updateEffectorMassProps(double integTime); void Reset(uint64_t CurrentSimNanos); - void AddVSCMG(VSCMGConfigMsgPayload *NewVSCMG); - void UpdateState(uint64_t CurrentSimNanos); - void WriteOutputMessages(uint64_t CurrentClock); - void ReadInputs(); - void ConfigureVSCMGRequests(double CurrentTime); - void updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N); //!< [-] Back-sub contributions - void updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B); //!< [-] Energy and momentum calculations - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN); //!< [-] Method for each stateEffector to calculate derivatives - -public: - std::vector VSCMGData; //!< [-] VSCMG data structure - Eigen::MatrixXd *g_N; //!< [m/s^2] Gravitational acceleration in N frame components - - ReadFunctor cmdsInMsg; //!< [-] motor torque command input message - Message speedOutMsg; //!< [-] VSCMG speed output message - std::vector*> vscmgOutMsgs; //!< [-] vector of VSCMG output messages - - std::vector newVSCMGCmds; //!< [-] Incoming torque commands - VSCMGSpeedMsgPayload outputStates; //!< [-] Output data from the VSCMGs - std::string nameOfVSCMGOmegasState; //!< [-] identifier for the Omegas state data container - std::string nameOfVSCMGThetasState; //!< [-] identifier for the Thetas state data container - std::string nameOfVSCMGGammasState; //!< [-] identifier for the Gammas state data container - std::string nameOfVSCMGGammaDotsState; //!< [-] identifier for the GammaDots state data container - int numVSCMG; //!< [-] number of VSCMGs - int numVSCMGJitter; //!< [-] number of VSCMGs with jitter - BSKLogger bskLogger; //!< [-] BSK Logging - -private: - VSCMGArrayTorqueMsgPayload incomingCmdBuffer; //!< [-] One-time allocation for savings - uint64_t prevCommandTime; //!< [-] Time for previous valid thruster firing - - StateData *hubOmega; //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components - StateData *OmegasState; //!< [rad/s] RW spin state - StateData *thetasState; //!< [rad] RW angle - StateData *gammasState; //!< [rad] CMG gimbal angle - StateData *gammaDotsState; //!< [rad/s] CMG gimbal angle rate - + void AddVSCMG(VSCMGConfigMsgPayload* NewVSCMG); + void UpdateState(uint64_t CurrentSimNanos); + void WriteOutputMessages(uint64_t CurrentClock); + void ReadInputs(); + void ConfigureVSCMGRequests(double CurrentTime); + void updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N); //!< [-] Back-sub contributions + void updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B); //!< [-] Energy and momentum calculations + void computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN); //!< [-] Method for each stateEffector to calculate derivatives + + public: + std::vector VSCMGData; //!< [-] VSCMG data structure + Eigen::MatrixXd* g_N; //!< [m/s^2] Gravitational acceleration in N frame components + + ReadFunctor cmdsInMsg; //!< [-] motor torque command input message + Message speedOutMsg; //!< [-] VSCMG speed output message + std::vector*> vscmgOutMsgs; //!< [-] vector of VSCMG output messages + + std::vector newVSCMGCmds; //!< [-] Incoming torque commands + VSCMGSpeedMsgPayload outputStates; //!< [-] Output data from the VSCMGs + std::string nameOfVSCMGOmegasState; //!< [-] identifier for the Omegas state data container + std::string nameOfVSCMGThetasState; //!< [-] identifier for the Thetas state data container + std::string nameOfVSCMGGammasState; //!< [-] identifier for the Gammas state data container + std::string nameOfVSCMGGammaDotsState; //!< [-] identifier for the GammaDots state data container + int numVSCMG; //!< [-] number of VSCMGs + int numVSCMGJitter; //!< [-] number of VSCMGs with jitter + BSKLogger bskLogger; //!< [-] BSK Logging + + private: + VSCMGArrayTorqueMsgPayload incomingCmdBuffer; //!< [-] One-time allocation for savings + uint64_t prevCommandTime; //!< [-] Time for previous valid thruster firing + + StateData* hubOmega; //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components + StateData* OmegasState; //!< [rad/s] RW spin state + StateData* thetasState; //!< [rad] RW angle + StateData* gammasState; //!< [rad] CMG gimbal angle + StateData* gammaDotsState; //!< [rad/s] CMG gimbal angle rate }; - #endif /* STATE_EFFECTOR_H */ diff --git a/src/simulation/dynamics/_GeneralModuleFiles/BodyToHubInfo.h b/src/simulation/dynamics/_GeneralModuleFiles/BodyToHubInfo.h old mode 100755 new mode 100644 index 2f1a991559..d317e8b65c --- a/src/simulation/dynamics/_GeneralModuleFiles/BodyToHubInfo.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/BodyToHubInfo.h @@ -21,18 +21,15 @@ #define SIM_THRUSTER_BODYTOHUBINFO_H #include - /*! attached body to hub information structure*/ typedef struct -//@cond DOXYGEN_IGNORE -BodyToHubInfo + //@cond DOXYGEN_IGNORE + BodyToHubInfo //@endcond { - Eigen::Vector3d r_FB_B; //!< position vector of the frame F relative to frame B - Eigen::Vector3d omega_FB_B; //!< angular velocity of F relative to B - Eigen::Matrix3d dcm_BF; //!< DCM of B relative to F -}BodyToHubInfo; - - + Eigen::Vector3d r_FB_B; //!< position vector of the frame F relative to frame B + Eigen::Vector3d omega_FB_B; //!< angular velocity of F relative to B + Eigen::Matrix3d dcm_BF; //!< DCM of B relative to F +} BodyToHubInfo; #endif diff --git a/src/simulation/dynamics/_GeneralModuleFiles/RWConfigPayload.h b/src/simulation/dynamics/_GeneralModuleFiles/RWConfigPayload.h old mode 100755 new mode 100644 index 6378fd95d2..5932fc8db7 --- a/src/simulation/dynamics/_GeneralModuleFiles/RWConfigPayload.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/RWConfigPayload.h @@ -23,54 +23,53 @@ #include #include "simulation/dynamics/reactionWheels/reactionWheelSupport.h" - /*! @brief Structure used to define the individual RW configuration data */ typedef struct -//@cond DOXYGEN_IGNORE -RWConfigPayload + //@cond DOXYGEN_IGNORE + RWConfigPayload //@endcond { - Eigen::Vector3d rWB_B; //!< [m], position vector of the RW relative to the spacecraft body frame - Eigen::Vector3d gsHat_B; //!< [-] spin axis unit vector in body frame - Eigen::Vector3d w2Hat0_B; //!< [-] initial torque axis unit vector in body frame - Eigen::Vector3d w3Hat0_B; //!< [-] initial gimbal axis unit vector in body frame - double mass = 1.0; //!< [kg], reaction wheel rotor mass - double theta = 0.0; //!< [rad], wheel angle - double Omega = 0.0; //!< [rad/s], wheel speed - double Js = 1.0; //!< [kg-m^2], spin axis gsHat rotor moment of inertia - double Jt = 1.0; //!< [kg-m^2], gtHat axis rotor moment of inertia - double Jg = 1.0; //!< [kg-m^2], ggHat axis rotor moment of inertia - double U_s = 0.0; //!< [kg-m], static imbalance - double U_d = 0.0; //!< [kg-m^2], dynamic imbalance - double d = 0.0; //!< [m], wheel center of mass offset from wheel frame origin - double J13 = 0.0; //!< [kg-m^2], x-z inertia of wheel about wheel center in wheel frame (imbalance) - double u_current = 0.0; //!< [N-m], current motor torque - double u_max = -1; //!< [N-m], Max torque, negative value turns off saturating the wheel - double u_min = 0.0; //!< [N-m], Min torque - double fCoulomb = 0.0; //!< [N-m], Coulomb friction torque magnitude - double fStatic = 0.0; //!< [N-m], Static friction torque magnitude - double betaStatic = -1.0; //!< Stribeck friction coefficient; For stribeck friction to be activiated, user must change this variable to a positive non-zero number. - double cViscous = 0.0; //!< [N-m-s/rad] Viscous fricion coefficient - double omegaLimitCycle = 0.0001; //!< [rad/s], wheel speed that avoids limit cycle with friction - double frictionTorque = 0.0; //!< [N-m] friction torque, this is a computed value, don't set it directly - double omegaBefore = 0.0; //!< [rad/s], wheel speed one time step before - bool frictionStribeck = 0; //!< [-] Boolenian to determine if stribeck friction model is used or not, 0 is non-stribeck, 1 is stribeck; Parameter is set internally. - double Omega_max = -1.0; //!< [rad/s], max wheel speed, negative values turn off wheel saturation - double P_max = -1.0; //!< [N-m/s], maximum wheel power, negative values turn off power limit - RWModels RWModel = BalancedWheels; //!< [-], Type of imbalance model to use - Eigen::Vector3d aOmega; //!< [-], parameter used in coupled jitter back substitution - Eigen::Vector3d bOmega; //!< [-], parameter used in coupled jitter back substitution - double cOmega = 0.0; //!< [-], parameter used in coupled jitter back substitution - Eigen::Matrix3d IRWPntWc_B; //!< RW inertia about point Wc in B frame components - Eigen::Matrix3d IPrimeRWPntWc_B; //!< RW inertia B-frame derivative - Eigen::Vector3d rWcB_B; //!< position of Wc relative to B in B-frame components - Eigen::Matrix3d rTildeWcB_B; //!< tilde matrix of r_WcB_B - Eigen::Vector3d rPrimeWcB_B; //!< B-frame derivative of r_WcB_B - Eigen::Vector3d w2Hat_B; //!< unit vector - Eigen::Vector3d w3Hat_B; //!< unit vector - char label[10]; //!< [-], label name of the RW device being simulated -}RWConfigPayload; - - + Eigen::Vector3d rWB_B; //!< [m], position vector of the RW relative to the spacecraft body frame + Eigen::Vector3d gsHat_B; //!< [-] spin axis unit vector in body frame + Eigen::Vector3d w2Hat0_B; //!< [-] initial torque axis unit vector in body frame + Eigen::Vector3d w3Hat0_B; //!< [-] initial gimbal axis unit vector in body frame + double mass = 1.0; //!< [kg], reaction wheel rotor mass + double theta = 0.0; //!< [rad], wheel angle + double Omega = 0.0; //!< [rad/s], wheel speed + double Js = 1.0; //!< [kg-m^2], spin axis gsHat rotor moment of inertia + double Jt = 1.0; //!< [kg-m^2], gtHat axis rotor moment of inertia + double Jg = 1.0; //!< [kg-m^2], ggHat axis rotor moment of inertia + double U_s = 0.0; //!< [kg-m], static imbalance + double U_d = 0.0; //!< [kg-m^2], dynamic imbalance + double d = 0.0; //!< [m], wheel center of mass offset from wheel frame origin + double J13 = 0.0; //!< [kg-m^2], x-z inertia of wheel about wheel center in wheel frame (imbalance) + double u_current = 0.0; //!< [N-m], current motor torque + double u_max = -1; //!< [N-m], Max torque, negative value turns off saturating the wheel + double u_min = 0.0; //!< [N-m], Min torque + double fCoulomb = 0.0; //!< [N-m], Coulomb friction torque magnitude + double fStatic = 0.0; //!< [N-m], Static friction torque magnitude + double betaStatic = -1.0; //!< Stribeck friction coefficient; For stribeck friction to be activiated, user must + //!< change this variable to a positive non-zero number. + double cViscous = 0.0; //!< [N-m-s/rad] Viscous fricion coefficient + double omegaLimitCycle = 0.0001; //!< [rad/s], wheel speed that avoids limit cycle with friction + double frictionTorque = 0.0; //!< [N-m] friction torque, this is a computed value, don't set it directly + double omegaBefore = 0.0; //!< [rad/s], wheel speed one time step before + bool frictionStribeck = 0; //!< [-] Boolenian to determine if stribeck friction model is used or not, 0 is + //!< non-stribeck, 1 is stribeck; Parameter is set internally. + double Omega_max = -1.0; //!< [rad/s], max wheel speed, negative values turn off wheel saturation + double P_max = -1.0; //!< [N-m/s], maximum wheel power, negative values turn off power limit + RWModels RWModel = BalancedWheels; //!< [-], Type of imbalance model to use + Eigen::Vector3d aOmega; //!< [-], parameter used in coupled jitter back substitution + Eigen::Vector3d bOmega; //!< [-], parameter used in coupled jitter back substitution + double cOmega = 0.0; //!< [-], parameter used in coupled jitter back substitution + Eigen::Matrix3d IRWPntWc_B; //!< RW inertia about point Wc in B frame components + Eigen::Matrix3d IPrimeRWPntWc_B; //!< RW inertia B-frame derivative + Eigen::Vector3d rWcB_B; //!< position of Wc relative to B in B-frame components + Eigen::Matrix3d rTildeWcB_B; //!< tilde matrix of r_WcB_B + Eigen::Vector3d rPrimeWcB_B; //!< B-frame derivative of r_WcB_B + Eigen::Vector3d w2Hat_B; //!< unit vector + Eigen::Vector3d w3Hat_B; //!< unit vector + char label[10]; //!< [-], label name of the RW device being simulated +} RWConfigPayload; #endif diff --git a/src/simulation/dynamics/_GeneralModuleFiles/THROperation.h b/src/simulation/dynamics/_GeneralModuleFiles/THROperation.h old mode 100755 new mode 100644 index bb668071d4..3daf098e83 --- a/src/simulation/dynamics/_GeneralModuleFiles/THROperation.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/THROperation.h @@ -21,38 +21,34 @@ #define SIM_THRUSTER_OPERATION_H #include - - //! @brief Container for current operational data of a given thruster /*! This structure is used to determine the current state of a given thruster. It defines where in the cycle the thruster is and how much longer it should be on for. It is intended to have the previous firing remain resident for logging*/ typedef struct -//@cond DOXYGEN_IGNORE -THROperation + //@cond DOXYGEN_IGNORE + THROperation //@endcond { - uint64_t fireCounter; //!< Number of times thruster fired - double ThrustFactor; //!< Current thrust scaling factor due to ramp up/down - double IspFactor; //!< Current fractional ISP - double thrustBlowDownFactor = 1.0; //!< Current thrust scaling factor due to tank blow down - double ispBlowDownFactor = 1.0; //!< Current Isp scaling due to tank blow down - double ThrustOnCmd; //!< [s] Time Thruster was requested - double totalOnTime; //!< [s] Total amount of time thruster has fired - double opThrustForce_B[3] = {0}; //!< [N] Thrust force vector in body frame components - double opThrustTorquePntB_B[3] = {0}; //!< [N-m] Thrust torque about point B in body frame components + uint64_t fireCounter; //!< Number of times thruster fired + double ThrustFactor; //!< Current thrust scaling factor due to ramp up/down + double IspFactor; //!< Current fractional ISP + double thrustBlowDownFactor = 1.0; //!< Current thrust scaling factor due to tank blow down + double ispBlowDownFactor = 1.0; //!< Current Isp scaling due to tank blow down + double ThrustOnCmd; //!< [s] Time Thruster was requested + double totalOnTime; //!< [s] Total amount of time thruster has fired + double opThrustForce_B[3] = { 0 }; //!< [N] Thrust force vector in body frame components + double opThrustTorquePntB_B[3] = { 0 }; //!< [N-m] Thrust torque about point B in body frame components // The following are used in thrusterDynamicEffector - double ThrustOnRampTime; //!< [s] Time thruster has been on for - double ThrustOnSteadyTime; //!< [s] Time thruster has been on steady - double ThrustOffRampTime; //!< [s] Time thruster has been turning off - double ThrusterStartTime; //!< [s] Time thruster has been executing total - double PreviousIterTime; //!< [s] Previous thruster int time + double ThrustOnRampTime; //!< [s] Time thruster has been on for + double ThrustOnSteadyTime; //!< [s] Time thruster has been on steady + double ThrustOffRampTime; //!< [s] Time thruster has been turning off + double ThrusterStartTime; //!< [s] Time thruster has been executing total + double PreviousIterTime; //!< [s] Previous thruster int time // The following is used in thrusterStateEffector - double ThrusterEndTime; //!< [s] Time thruster stops burning -}THROperation; - - + double ThrusterEndTime; //!< [s] Time thruster stops burning +} THROperation; #endif diff --git a/src/simulation/dynamics/_GeneralModuleFiles/THRSimConfig.h b/src/simulation/dynamics/_GeneralModuleFiles/THRSimConfig.h old mode 100755 new mode 100644 index 79dd72b9d9..d99b4f6947 --- a/src/simulation/dynamics/_GeneralModuleFiles/THRSimConfig.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/THRSimConfig.h @@ -1,21 +1,21 @@ - /* - 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. - */ +*/ #ifndef SIM_THRUSTER_CONFIG_H #define SIM_THRUSTER_CONFIG_H @@ -25,35 +25,36 @@ #include "simulation/dynamics/_GeneralModuleFiles/THROperation.h" #include "simulation/dynamics/_GeneralModuleFiles/THRTimePair.h" - //! @brief Container for overall thruster configuration data for single thruster /*! This structure is used to define the overall configuration of an entire thruster. It holds the current operational data for the thruster, the ramp/max/min configuration data, and the physical location/orientation data for - a thruster. This structure is managed using a `std::shared_ptr` to avoid duplication of configuration data across the simulation and to enable access and updates to the parameters during simulation.*/ + a thruster. This structure is managed using a `std::shared_ptr` to avoid duplication of configuration data across the + simulation and to enable access and updates to the parameters during simulation.*/ typedef struct -//@cond DOXYGEN_IGNORE -THRSimConfig + //@cond DOXYGEN_IGNORE + THRSimConfig //@endcond { - Eigen::Vector3d thrLoc_B; //!< [m] Thruster location expressed in body - Eigen::Vector3d thrDir_B; //!< Thruster force direction unit vector in body - std::vector ThrusterOnRamp; //!< Percentage of max thrust for ramp up - std::vector ThrusterOffRamp; //!< Percentage of max thrust for ramp down - double areaNozzle; //!< [m^2] Area of nozzle - double MaxThrust; //!< [N] Steady state thrust of thruster - double steadyIsp; //!< [s] Steady state specific impulse of thruster - double MinOnTime; //!< [s] Minimum allowable on-time - THROperation ThrustOps; //!< Thruster operating data - double thrusterMagDisp; //!< Percentage of magnitude dispersion - std::vector thrusterDirectionDisp; //!< Unit vector of dispersed thruster pointing - bool updateOnly = true; //!< Use update only calculations - char label[10]; //!< label name of the TH device being simulated - double cutoffFrequency; //!< [rad/s] cutoff frequency for first-order behavior - double MaxSwirlTorque; //!< [Nm] Steady state magnitude of the swirl torque produced by ionic thrusters - std::vector thrBlowDownCoeff; //!< Polynomial coefficients for fuel mass to thrust blow down model in descending order - std::vector ispBlowDownCoeff; //!< Polynomial coefficients for fuel mass to Isp blow down model in descending order -}THRSimConfig; - + Eigen::Vector3d thrLoc_B; //!< [m] Thruster location expressed in body + Eigen::Vector3d thrDir_B; //!< Thruster force direction unit vector in body + std::vector ThrusterOnRamp; //!< Percentage of max thrust for ramp up + std::vector ThrusterOffRamp; //!< Percentage of max thrust for ramp down + double areaNozzle; //!< [m^2] Area of nozzle + double MaxThrust; //!< [N] Steady state thrust of thruster + double steadyIsp; //!< [s] Steady state specific impulse of thruster + double MinOnTime; //!< [s] Minimum allowable on-time + THROperation ThrustOps; //!< Thruster operating data + double thrusterMagDisp; //!< Percentage of magnitude dispersion + std::vector thrusterDirectionDisp; //!< Unit vector of dispersed thruster pointing + bool updateOnly = true; //!< Use update only calculations + char label[10]; //!< label name of the TH device being simulated + double cutoffFrequency; //!< [rad/s] cutoff frequency for first-order behavior + double MaxSwirlTorque; //!< [Nm] Steady state magnitude of the swirl torque produced by ionic thrusters + std::vector + thrBlowDownCoeff; //!< Polynomial coefficients for fuel mass to thrust blow down model in descending order + std::vector + ispBlowDownCoeff; //!< Polynomial coefficients for fuel mass to Isp blow down model in descending order +} THRSimConfig; #endif diff --git a/src/simulation/dynamics/_GeneralModuleFiles/THRTimePair.h b/src/simulation/dynamics/_GeneralModuleFiles/THRTimePair.h old mode 100755 new mode 100644 index 27019d159b..3d307706dc --- a/src/simulation/dynamics/_GeneralModuleFiles/THRTimePair.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/THRTimePair.h @@ -20,17 +20,15 @@ #ifndef SIM_THRUSTER_PAIR_H #define SIM_THRUSTER_PAIR_H - - //! @brief Container for time/value pairs for ramp on and ramp off profiles. /*! This structure is used to build up the thruster on and thruster off ramps. It pairs the time-delta from on command and thrust factor (percentage/100.0) for the entire ramp.*/ -typedef struct { - double ThrustFactor; //!< -- Percentage of max thrust - double IspFactor; //!< [s] fraction specific impulse - double TimeDelta; //!< [s] Time delta from start of event -}THRTimePair; - +typedef struct +{ + double ThrustFactor; //!< -- Percentage of max thrust + double IspFactor; //!< [s] fraction specific impulse + double TimeDelta; //!< [s] Time delta from start of event +} THRTimePair; #endif diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.cpp b/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.cpp old mode 100755 new mode 100644 index b1982a2791..f7baee0a92 --- a/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.cpp @@ -36,39 +36,44 @@ StateVector::operator=(const StateVector& other) return *this; } -void StateVector::setStates(const StateVector& operand) +void +StateVector::setStates(const StateVector& operand) { - for (auto [it1, it2] = std::tuple{std::begin(this->stateMap), std::begin(operand.stateMap)}; + for (auto [it1, it2] = std::tuple{ std::begin(this->stateMap), std::begin(operand.stateMap) }; it1 != std::end(this->stateMap) && it2 != std::end(operand.stateMap); ++it1, ++it2) { it1->second->setState(it2->second->getState()); } } -void StateVector::addStates(const StateVector& operand) +void +StateVector::addStates(const StateVector& operand) { - for (auto [it1, it2] = std::tuple{std::begin(this->stateMap), std::begin(operand.stateMap)}; + for (auto [it1, it2] = std::tuple{ std::begin(this->stateMap), std::begin(operand.stateMap) }; it1 != std::end(this->stateMap) && it2 != std::end(operand.stateMap); ++it1, ++it2) { it1->second->addState(*it2->second); } } -void StateVector::scaleStates(double scaleFactor) +void +StateVector::scaleStates(double scaleFactor) { for (const auto& [key, value] : stateMap) { value->scaleState(scaleFactor); } } -void StateVector::propagateStates(double dt) +void +StateVector::propagateStates(double dt) { for (const auto& [key, value] : stateMap) { value->propagateState(dt); } } -StateData* DynParamManager::getStateObject(std::string stateName) +StateData* +DynParamManager::getStateObject(std::string stateName) { if (stateContainer.stateMap.count(stateName) > 0) { return stateContainer.stateMap.at(stateName).get(); @@ -78,56 +83,58 @@ StateData* DynParamManager::getStateObject(std::string stateName) Either the state name was miss-spelled, or the state simply doesn't exit in the current simulaiton setup (i.e. asking for the hub attitude in a translation only simulation setup */ - bskLogger.bskLog( - BSK_WARNING, - "You requested this non-existent state name: %s You either miss-typed the stateName, or " - "you asked for a state that doesn't exist in your simulation setup.", - stateName.c_str()); + bskLogger.bskLog(BSK_WARNING, + "You requested this non-existent state name: %s You either miss-typed the stateName, or " + "you asked for a state that doesn't exist in your simulation setup.", + stateName.c_str()); return nullptr; } -void DynParamManager::updateStateVector(const StateVector& newState) +void +DynParamManager::updateStateVector(const StateVector& newState) { this->stateContainer.setStates(newState); } -void DynParamManager::propagateStateVector(double dt) { this->stateContainer.propagateStates(dt); } +void +DynParamManager::propagateStateVector(double dt) +{ + this->stateContainer.propagateStates(dt); +} -Eigen::MatrixXd* DynParamManager::createProperty(std::string propName, - const Eigen::MatrixXd& propValue) +Eigen::MatrixXd* +DynParamManager::createProperty(std::string propName, const Eigen::MatrixXd& propValue) { std::map::iterator it; it = dynProperties.find(propName); if (it == dynProperties.end()) { dynProperties.insert(std::pair(propName, propValue)); - } - else { - bskLogger.bskLog( - BSK_WARNING, - "You created the dynamic property: %s more than once. You shouldn't be doing that.", - propName.c_str()); + } else { + bskLogger.bskLog(BSK_WARNING, + "You created the dynamic property: %s more than once. You shouldn't be doing that.", + propName.c_str()); it->second = propValue; } return (&(dynProperties.find(propName)->second)); } -Eigen::MatrixXd* DynParamManager::getPropertyReference(std::string propName) +Eigen::MatrixXd* +DynParamManager::getPropertyReference(std::string propName) { std::map::iterator it; it = dynProperties.find(propName); if (it == dynProperties.end()) { - bskLogger.bskLog(BSK_ERROR, - "You requested the property: %s which doesn't exist. Null returned.", - propName.c_str()); + bskLogger.bskLog( + BSK_ERROR, "You requested the property: %s which doesn't exist. Null returned.", propName.c_str()); return nullptr; - } - else { + } else { return (&(it->second)); } } -void DynParamManager::setPropertyValue(const std::string propName, const Eigen::MatrixXd& propValue) +void +DynParamManager::setPropertyValue(const std::string propName, const Eigen::MatrixXd& propValue) { std::map::iterator it; it = dynProperties.find(propName); @@ -136,8 +143,7 @@ void DynParamManager::setPropertyValue(const std::string propName, const Eigen:: "You tried to set the property value for: %s which has not been created " "yet. I can't do that.", propName.c_str()); - } - else { + } else { it->second = propValue; } } diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.h b/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.h old mode 100755 new mode 100644 index fb85e9b7ab..be679e0faa --- a/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynParamManager.h @@ -31,7 +31,8 @@ /** StateVector represents an ordered collection of StateData, * with each state having a unique name. */ -class StateVector { +class StateVector +{ public: /** All states managed by the StateVector, cached * by name. @@ -51,7 +52,7 @@ class StateVector { StateVector(const StateVector& other); /** Assignment operator */ - StateVector& operator= (const StateVector&); + StateVector& operator=(const StateVector&); /** Sets the values of the states of this StateVector to a copy the * values of the states of the other StateVector. @@ -75,7 +76,8 @@ class StateVector { }; /** A class that manages a set of states and properties. */ -class DynParamManager { +class DynParamManager +{ public: /** A map of properties managed by this class. * @@ -101,8 +103,8 @@ class DynParamManager { * This method may optionally be templated to create StateData of * subclasses of StateData. */ - template , bool> = true> + template, bool> = true> StateDataType* registerState(uint32_t nRow, uint32_t nCol, std::string stateName); /** Retrieves the handler to a previously registered StateData. @@ -144,21 +146,19 @@ class DynParamManager { void propagateStateVector(double dt); }; -template , bool>> -StateDataType* DynParamManager::registerState(uint32_t nRow, uint32_t nCol, std::string stateName) +template, bool>> +StateDataType* +DynParamManager::registerState(uint32_t nRow, uint32_t nCol, std::string stateName) { if (stateName == "") { - bskLogger.bskLog(BSK_ERROR, - "Your state name can't be an empty string. Come on. You get null."); + bskLogger.bskLog(BSK_ERROR, "Your state name can't be an empty string. Come on. You get null."); return nullptr; } if (stateContainer.stateMap.count(stateName) > 0) { - bskLogger.bskLog( - BSK_WARNING, - "You created a state with the name: %s more than once. Go ahead and don't do this.", - stateName.c_str()); + bskLogger.bskLog(BSK_WARNING, + "You created a state with the name: %s more than once. Go ahead and don't do this.", + stateName.c_str()); auto& stateData = stateContainer.stateMap.at(stateName); 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/dynamicEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp old mode 100755 new mode 100644 index 529fee66c5..fc5384412a --- a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.cpp @@ -38,12 +38,14 @@ DynamicEffector::~DynamicEffector() /*! This method is an optional method by a dynamic effector and allows the dynamics effector to add direct contributions to a state effector derivative. Example - a thruster's mDot will impact a fuel tanks total mDot */ -void DynamicEffector::computeStateContribution(double integTime) +void +DynamicEffector::computeStateContribution(double integTime) { return; } -void DynamicEffector::setStateNameOfPosition(std::string value) +void +DynamicEffector::setStateNameOfPosition(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -53,7 +55,8 @@ void DynamicEffector::setStateNameOfPosition(std::string value) } } -void DynamicEffector::setStateNameOfVelocity(std::string value) +void +DynamicEffector::setStateNameOfVelocity(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -63,7 +66,8 @@ void DynamicEffector::setStateNameOfVelocity(std::string value) } } -void DynamicEffector::setStateNameOfSigma(std::string value) +void +DynamicEffector::setStateNameOfSigma(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -73,7 +77,8 @@ void DynamicEffector::setStateNameOfSigma(std::string value) } } -void DynamicEffector::setStateNameOfOmega(std::string value) +void +DynamicEffector::setStateNameOfOmega(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -83,7 +88,8 @@ void DynamicEffector::setStateNameOfOmega(std::string value) } } -void DynamicEffector::setPropName_m_SC(std::string value) +void +DynamicEffector::setPropName_m_SC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -93,7 +99,8 @@ void DynamicEffector::setPropName_m_SC(std::string value) } } -void DynamicEffector::setPropName_mDot_SC(std::string value) +void +DynamicEffector::setPropName_mDot_SC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -103,7 +110,8 @@ void DynamicEffector::setPropName_mDot_SC(std::string value) } } -void DynamicEffector::setPropName_centerOfMassSC(std::string value) +void +DynamicEffector::setPropName_centerOfMassSC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -113,7 +121,8 @@ void DynamicEffector::setPropName_centerOfMassSC(std::string value) } } -void DynamicEffector::setPropName_inertiaSC(std::string value) +void +DynamicEffector::setPropName_inertiaSC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -123,7 +132,8 @@ void DynamicEffector::setPropName_inertiaSC(std::string value) } } -void DynamicEffector::setPropName_inertiaPrimeSC(std::string value) +void +DynamicEffector::setPropName_inertiaPrimeSC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -133,17 +143,20 @@ void DynamicEffector::setPropName_inertiaPrimeSC(std::string value) } } -void DynamicEffector::setPropName_centerOfMassPrimeSC(std::string value) +void +DynamicEffector::setPropName_centerOfMassPrimeSC(std::string value) { // check that value is acceptable if (!value.empty()) { this->propName_centerOfMassPrimeSC = value; } else { - bskLogger.bskLog(BSK_ERROR, "DynamicEffector: propName_centerOfMassPrimeSC variable must be a non-empty string"); + bskLogger.bskLog(BSK_ERROR, + "DynamicEffector: propName_centerOfMassPrimeSC variable must be a non-empty string"); } } -void DynamicEffector::setPropName_centerOfMassDotSC(std::string value) +void +DynamicEffector::setPropName_centerOfMassDotSC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -153,7 +166,8 @@ void DynamicEffector::setPropName_centerOfMassDotSC(std::string value) } } -void DynamicEffector::setPropName_inertialPosition(std::string value) +void +DynamicEffector::setPropName_inertialPosition(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -163,7 +177,8 @@ void DynamicEffector::setPropName_inertialPosition(std::string value) } } -void DynamicEffector::setPropName_inertialVelocity(std::string value) +void +DynamicEffector::setPropName_inertialVelocity(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -173,7 +188,8 @@ void DynamicEffector::setPropName_inertialVelocity(std::string value) } } -void DynamicEffector::setPropName_vehicleGravity(std::string value) +void +DynamicEffector::setPropName_vehicleGravity(std::string value) { // check that value is acceptable if (!value.empty()) { diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h old mode 100755 new mode 100644 index ac01c0b086..3cf1dfbf20 --- a/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynamicEffector.h @@ -25,24 +25,28 @@ #include "architecture/utilities/bskLogging.h" /*! @brief dynamic effector class */ -class DynamicEffector { -public: - DynamicEffector(); //!< Constructor - virtual ~DynamicEffector(); //!< Destructor +class DynamicEffector +{ + public: + DynamicEffector(); //!< Constructor + virtual ~DynamicEffector(); //!< Destructor virtual void computeStateContribution(double integTime); - virtual void linkInStates(DynParamManager& states) = 0; //!< Method to get access to other states/stateEffectors - virtual void computeForceTorque(double integTime, double timeStep) = 0; //!< -- Method to computeForce and torque on the body + virtual void linkInStates(DynParamManager& states) = 0; //!< Method to get access to other states/stateEffectors + virtual void computeForceTorque(double integTime, + double timeStep) = 0; //!< -- Method to computeForce and torque on the body -public: + public: Eigen::VectorXd stateDerivContribution; //!< DynamicEffectors contribution to a stateEffector - Eigen::Vector3d forceExternal_N = Eigen::Vector3d::Zero(); //!< [N] External force applied by this effector in inertial components - Eigen::Vector3d forceExternal_B = Eigen::Vector3d::Zero(); //!< [N] External force applied by this effector in body frame components + Eigen::Vector3d forceExternal_N = + Eigen::Vector3d::Zero(); //!< [N] External force applied by this effector in inertial components + Eigen::Vector3d forceExternal_B = + Eigen::Vector3d::Zero(); //!< [N] External force applied by this effector in body frame components Eigen::Vector3d torqueExternalPntB_B = Eigen::Vector3d::Zero(); //!< [Nm] External torque applied by this effector /** setter for `stateNameOfPosition` property */ void setStateNameOfPosition(std::string value); /** getter for `stateNameOfPosition` property */ - const std::string getStateNameOfPosition() const {return this->stateNameOfPosition; } + const std::string getStateNameOfPosition() const { return this->stateNameOfPosition; } /** setter for `stateNameOfVelocity` property */ void setStateNameOfVelocity(std::string value); /** getter for `stateNameOfVelocity` property */ @@ -96,26 +100,24 @@ class DynamicEffector { /** getter for `propName_vehicleGravity` property */ const std::string getPropName_vehicleGravity() const { return this->propName_vehicleGravity; } - BSKLogger bskLogger; //!< BSK Logging + BSKLogger bskLogger; //!< BSK Logging -protected: - std::string stateNameOfPosition = ""; //!< state engine name of the parent rigid body inertial position vector - std::string stateNameOfVelocity = ""; //!< state engine name of the parent rigid body inertial velocity vector - std::string stateNameOfSigma = ""; //!< state engine name of the parent rigid body inertial attitude - std::string stateNameOfOmega = ""; //!< state engine name of the parent rigid body inertial angular velocity vector - - std::string propName_m_SC = ""; //!< property name of m_SC - std::string propName_mDot_SC = ""; //!< property name of mDot_SC - std::string propName_centerOfMassSC = ""; //!< property name of centerOfMassSC - std::string propName_inertiaSC = ""; //!< property name of inertiaSC - std::string propName_inertiaPrimeSC = ""; //!< property name of inertiaPrimeSC - std::string propName_centerOfMassPrimeSC = ""; //!< property name of centerOfMassPrimeSC - std::string propName_centerOfMassDotSC = ""; //!< property name of centerOfMassDotSC - std::string propName_inertialPosition = ""; //!< property name of inertialPosition - std::string propName_inertialVelocity = ""; //!< property name of inertialVelocity - std::string propName_vehicleGravity = ""; //!< property name of vehicleGravity + protected: + std::string stateNameOfPosition = ""; //!< state engine name of the parent rigid body inertial position vector + std::string stateNameOfVelocity = ""; //!< state engine name of the parent rigid body inertial velocity vector + std::string stateNameOfSigma = ""; //!< state engine name of the parent rigid body inertial attitude + std::string stateNameOfOmega = ""; //!< state engine name of the parent rigid body inertial angular velocity vector + std::string propName_m_SC = ""; //!< property name of m_SC + std::string propName_mDot_SC = ""; //!< property name of mDot_SC + std::string propName_centerOfMassSC = ""; //!< property name of centerOfMassSC + std::string propName_inertiaSC = ""; //!< property name of inertiaSC + std::string propName_inertiaPrimeSC = ""; //!< property name of inertiaPrimeSC + std::string propName_centerOfMassPrimeSC = ""; //!< property name of centerOfMassPrimeSC + std::string propName_centerOfMassDotSC = ""; //!< property name of centerOfMassDotSC + std::string propName_inertialPosition = ""; //!< property name of inertialPosition + std::string propName_inertialVelocity = ""; //!< property name of inertialVelocity + std::string propName_vehicleGravity = ""; //!< property name of vehicleGravity }; - #endif /* DYNAMIC_EFFECTOR_H */ diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynamicObject.cpp b/src/simulation/dynamics/_GeneralModuleFiles/dynamicObject.cpp old mode 100755 new mode 100644 index 0a4f8c1f4a..8bbea87eac --- a/src/simulation/dynamics/_GeneralModuleFiles/dynamicObject.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynamicObject.cpp @@ -20,14 +20,14 @@ #include "dynamicObject.h" #include "architecture/utilities/macroDefinitions.h" -void DynamicObject::setIntegrator(StateVecIntegrator* newIntegrator) +void +DynamicObject::setIntegrator(StateVecIntegrator* newIntegrator) { if (this->isDynamicsSynced) { - bskLogger.bskLog( - BSK_WARNING, - "You cannot set the integrator of a DynamicObject with synced integration. " - "If you want to change the integrator, change the integrator of the primary " - "DynamicObject."); + bskLogger.bskLog(BSK_WARNING, + "You cannot set the integrator of a DynamicObject with synced integration. " + "If you want to change the integrator, change the integrator of the primary " + "DynamicObject."); return; } @@ -37,8 +37,7 @@ void DynamicObject::setIntegrator(StateVecIntegrator* newIntegrator) } if (newIntegrator->dynPtrs.at(0) != this) { - bskLogger.bskLog(BSK_ERROR, - "New integrator must have been created using this DynamicObject"); + bskLogger.bskLog(BSK_ERROR, "New integrator must have been created using this DynamicObject"); return; } @@ -53,15 +52,18 @@ void DynamicObject::setIntegrator(StateVecIntegrator* newIntegrator) this->integrator = newIntegrator; } -void DynamicObject::syncDynamicsIntegration(DynamicObject* dynPtr) +void +DynamicObject::syncDynamicsIntegration(DynamicObject* dynPtr) { this->integrator->dynPtrs.push_back(dynPtr); dynPtr->isDynamicsSynced = true; } -void DynamicObject::integrateState(uint64_t integrateToThisTimeNanos) +void +DynamicObject::integrateState(uint64_t integrateToThisTimeNanos) { - if (this->isDynamicsSynced) return; + if (this->isDynamicsSynced) + return; for (const auto& dynPtr : this->integrator->dynPtrs) { dynPtr->preIntegration(integrateToThisTimeNanos); diff --git a/src/simulation/dynamics/_GeneralModuleFiles/dynamicObject.h b/src/simulation/dynamics/_GeneralModuleFiles/dynamicObject.h old mode 100755 new mode 100644 index 059418df0d..ee4faa1a05 --- a/src/simulation/dynamics/_GeneralModuleFiles/dynamicObject.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/dynamicObject.h @@ -30,7 +30,8 @@ #include /** A DynamicObject is a Basilisk model with states that must be integrated */ -class DynamicObject : public SysModel { +class DynamicObject : public SysModel +{ public: DynParamManager dynManager; /**< Dynamics parameter manager for all effectors */ StateVecIntegrator* integrator; /**< Integrator used to propagate state forward */ @@ -57,10 +58,10 @@ class DynamicObject : public SysModel { virtual void postIntegration(uint64_t callTimeNanos) = 0; /** Initializes the dynamics and variables */ - virtual void initializeDynamics(){}; + virtual void initializeDynamics() {}; /** Computes energy and momentum of the system */ - virtual void computeEnergyMomentum(double t){}; + virtual void computeEnergyMomentum(double t) {}; /** Prepares the dynamic object to be integrated, integrates the states * forward in time, and finally performs the post-integration steps. @@ -78,8 +79,8 @@ class DynamicObject : public SysModel { public: /** flag indicating that another spacecraft object is controlling the integration */ bool isDynamicsSynced = false; - double timeStep = 0.0; /**< [s] integration time step */ - double timeBefore = 0.0; /**< [s] prior time value */ + double timeStep = 0.0; /**< [s] integration time step */ + double timeBefore = 0.0; /**< [s] prior time value */ uint64_t timeBeforeNanos = 0; /**< [ns] prior time value */ }; diff --git a/src/simulation/dynamics/_GeneralModuleFiles/extendedStateVector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/extendedStateVector.cpp index 587c9a674a..ab52b7b6ad 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/extendedStateVector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/extendedStateVector.cpp @@ -19,19 +19,21 @@ #include "extendedStateVector.h" -ExtendedStateVector ExtendedStateVector::fromStates(const std::vector& dynPtrs) +ExtendedStateVector +ExtendedStateVector::fromStates(const std::vector& dynPtrs) { return fromStateData(dynPtrs, [](const StateData& data) { return data.getState(); }); } -ExtendedStateVector ExtendedStateVector::fromStateDerivs(const std::vector& dynPtrs) +ExtendedStateVector +ExtendedStateVector::fromStateDerivs(const std::vector& dynPtrs) { return fromStateData(dynPtrs, [](const StateData& data) { return data.getStateDeriv(); }); } -ExtendedStateVector ExtendedStateVector::map( - std::function - functor) const +ExtendedStateVector +ExtendedStateVector::map( + std::function functor) const { ExtendedStateVector result; result.reserve(this->size()); @@ -44,8 +46,8 @@ ExtendedStateVector ExtendedStateVector::map( return result; } -void ExtendedStateVector::apply( - std::function functor) const +void +ExtendedStateVector::apply(std::function functor) const { for (auto&& [extendedStateId, stateMatrix] : *this) { const auto& [dynObjIndex, stateName] = extendedStateId; @@ -53,8 +55,8 @@ void ExtendedStateVector::apply( } } -void ExtendedStateVector::modify( - std::function functor) +void +ExtendedStateVector::modify(std::function functor) { for (auto&& [extendedStateId, stateMatrix] : *this) { const auto& [dynObjIndex, stateName] = extendedStateId; @@ -62,57 +64,51 @@ void ExtendedStateVector::modify( } } -ExtendedStateVector ExtendedStateVector::operator+=(const ExtendedStateVector& rhs) +ExtendedStateVector +ExtendedStateVector::operator+=(const ExtendedStateVector& rhs) { - this->modify([&rhs](const size_t& dynObjIndex, - const std::string& stateName, - Eigen::MatrixXd& thisState) { - thisState += rhs.at({dynObjIndex, stateName}); + this->modify([&rhs](const size_t& dynObjIndex, const std::string& stateName, Eigen::MatrixXd& thisState) { + thisState += rhs.at({ dynObjIndex, stateName }); }); return *this; } -ExtendedStateVector ExtendedStateVector::operator-(const ExtendedStateVector& rhs) const +ExtendedStateVector +ExtendedStateVector::operator-(const ExtendedStateVector& rhs) const { ExtendedStateVector copy = *this; - copy.modify([&rhs](const size_t& dynObjIndex, - const std::string& stateName, - Eigen::MatrixXd& thisState) { - thisState -= rhs.at({dynObjIndex, stateName}); + copy.modify([&rhs](const size_t& dynObjIndex, const std::string& stateName, Eigen::MatrixXd& thisState) { + thisState -= rhs.at({ dynObjIndex, stateName }); }); return copy; } -ExtendedStateVector ExtendedStateVector::operator*(const double rhs) const +ExtendedStateVector +ExtendedStateVector::operator*(const double rhs) const { - return this->map([rhs](const size_t& dynObjIndex, - const std::string& stateName, - const Eigen::MatrixXd& thisState) { return thisState * rhs; }); + return this->map([rhs](const size_t& dynObjIndex, const std::string& stateName, const Eigen::MatrixXd& thisState) { + return thisState * rhs; + }); } -void ExtendedStateVector::setStates(std::vector& dynPtrs) const +void +ExtendedStateVector::setStates(std::vector& dynPtrs) const { - this->apply([&dynPtrs](const size_t& dynObjIndex, - const std::string& stateName, - const Eigen::MatrixXd& thisState) { - dynPtrs.at(dynObjIndex) - ->dynManager.stateContainer.stateMap.at(stateName) - ->setState(thisState); + this->apply([&dynPtrs](const size_t& dynObjIndex, const std::string& stateName, const Eigen::MatrixXd& thisState) { + dynPtrs.at(dynObjIndex)->dynManager.stateContainer.stateMap.at(stateName)->setState(thisState); }); } -void ExtendedStateVector::setDerivatives(std::vector& dynPtrs) const +void +ExtendedStateVector::setDerivatives(std::vector& dynPtrs) const { - this->apply([&dynPtrs](const size_t& dynObjIndex, - const std::string& stateName, - const Eigen::MatrixXd& thisDerivative) { - dynPtrs.at(dynObjIndex) - ->dynManager.stateContainer.stateMap.at(stateName) - ->setDerivative(thisDerivative); - }); + this->apply( + [&dynPtrs](const size_t& dynObjIndex, const std::string& stateName, const Eigen::MatrixXd& thisDerivative) { + dynPtrs.at(dynObjIndex)->dynManager.stateContainer.stateMap.at(stateName)->setDerivative(thisDerivative); + }); } ExtendedStateVector @@ -122,8 +118,7 @@ ExtendedStateVector::fromStateData(const std::vector& dynPtrs, ExtendedStateVector result; for (size_t dynIndex = 0; dynIndex < dynPtrs.size(); dynIndex++) { - for (auto&& [stateName, stateData] : - dynPtrs.at(dynIndex)->dynManager.stateContainer.stateMap) { + for (auto&& [stateName, stateData] : dynPtrs.at(dynIndex)->dynManager.stateContainer.stateMap) { result.emplace(std::make_pair(dynIndex, stateName), functor(*stateData.get())); } } diff --git a/src/simulation/dynamics/_GeneralModuleFiles/extendedStateVector.h b/src/simulation/dynamics/_GeneralModuleFiles/extendedStateVector.h index 2671f4ce61..749b34c610 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/extendedStateVector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/extendedStateVector.h @@ -60,7 +60,8 @@ makes performing state-wise operations easier. using ExtendedStateId = std::pair; /** ExtendedStateIdHash is required to make ExtendedStateId hashable (usable as a key in a map) */ -struct ExtendedStateIdHash { +struct ExtendedStateIdHash +{ /** Generates a hash value (integer) from an ExtendedStateId object */ std::size_t operator()(const ExtendedStateId& p) const { @@ -77,8 +78,8 @@ struct ExtendedStateIdHash { * * It also supports several utility functions. */ -class ExtendedStateVector - : public std::unordered_map { +class ExtendedStateVector : public std::unordered_map +{ public: /** * Builds a ExtendedStateVector from all states in the given @@ -100,17 +101,15 @@ class ExtendedStateVector * A new ExtendedStateVector is built from the results of each call * of the functor. */ - ExtendedStateVector - map(std::function - functor) const; + ExtendedStateVector map( + std::function functor) const; /** * Similar to the map method, except that no * ExtendedStateVector is returned because the given functor * does not produce any values. */ - void apply(std::function - functor) const; + void apply(std::function functor) const; /** * Modifies each Eigen::MatrixXd stored in this object according to diff --git a/src/simulation/dynamics/_GeneralModuleFiles/fuelSlosh.h b/src/simulation/dynamics/_GeneralModuleFiles/fuelSlosh.h old mode 100755 new mode 100644 index 7ea03de263..96ae9bec9e --- a/src/simulation/dynamics/_GeneralModuleFiles/fuelSlosh.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/fuelSlosh.h @@ -20,21 +20,22 @@ #ifndef FUEL_SLOSH_H #define FUEL_SLOSH_H - - -//Fuel tank models +// Fuel tank models /*! @brief This class is a class that has the defines a generic fuel slosh paricle*/ -class FuelSlosh { -public: - FuelSlosh(){return;}; //!< -- Contructor - virtual ~FuelSlosh(){return;}; //!< -- Destructor - virtual void retrieveMassValue(double integTime){return;}; //!< -- retrieve current mass value of fuelSlosh particle - -public: +class FuelSlosh +{ + public: + FuelSlosh() { return; }; //!< -- Contructor + virtual ~FuelSlosh() { return; }; //!< -- Destructor + virtual void retrieveMassValue(double integTime) + { + return; + }; //!< -- retrieve current mass value of fuelSlosh particle + + public: double fuelMass = 0.0; //!< [kg] mass of fuelSlosh particle double massToTotalTankMassRatio = 0.0; //!< -- ratio of fuelSlosh particle mass to total mass of fuel tank double fuelMassDot = 0.0; //!< [kg/s] mass depletion rate of fuelSlosh particle }; - #endif /* FUEL_SLOSH_H */ diff --git a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp index 501ab939c1..815bc61a08 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp @@ -23,46 +23,43 @@ #include "architecture/utilities/linearAlgebra.h" #include "architecture/utilities/macroDefinitions.h" - GravBodyData::GravBodyData() - : gravityModel{std::make_shared()}, - localPlanet{std::invoke([]() { - SpicePlanetStateMsgPayload payload{}; - m33SetIdentity(payload.J20002Pfix); // Initialize rotation matrix to identity - return payload; - })} + : gravityModel{ std::make_shared() } + , localPlanet{ std::invoke([]() { + SpicePlanetStateMsgPayload payload{}; + m33SetIdentity(payload.J20002Pfix); // Initialize rotation matrix to identity + return payload; + }) } { } -void GravBodyData::initBody(int64_t moduleID) +void +GravBodyData::initBody(int64_t moduleID) { std::optional errorMessage; if (this->gravityModel) { this->gravityModel->bskLogger = &bskLogger; errorMessage = this->gravityModel->initializeParameters(*this); - } - else { + } else { errorMessage = "Gravity model is null"; } if (errorMessage) { - std::string fullMsg = - "Error initializating body '" + this->planetName + "'. " + errorMessage.value(); + std::string fullMsg = "Error initializating body '" + this->planetName + "'. " + errorMessage.value(); this->bskLogger.bskLog(BSK_ERROR, fullMsg.c_str()); } } -Eigen::Vector3d GravBodyData::computeGravityInertial(Eigen::Vector3d r_I, uint64_t simTimeNanos) +Eigen::Vector3d +GravBodyData::computeGravityInertial(Eigen::Vector3d r_I, uint64_t simTimeNanos) { double dt = diffNanoToSec(simTimeNanos, this->timeWritten); Eigen::Matrix3d dcm_PfixN = c2DArray2EigenMatrix3d(this->localPlanet.J20002Pfix).transpose(); - if (dcm_PfixN - .isZero()) { // Sanity check for connected messages that do not initialize J20002Pfix + if (dcm_PfixN.isZero()) { // Sanity check for connected messages that do not initialize J20002Pfix dcm_PfixN = Eigen::Matrix3d::Identity(); } - Eigen::Matrix3d dcm_PfixN_dot = - c2DArray2EigenMatrix3d(this->localPlanet.J20002Pfix_dot).transpose(); + Eigen::Matrix3d dcm_PfixN_dot = c2DArray2EigenMatrix3d(this->localPlanet.J20002Pfix_dot).transpose(); dcm_PfixN += dcm_PfixN_dot * dt; // store the current planet orientation and rates @@ -77,20 +74,21 @@ Eigen::Vector3d GravBodyData::computeGravityInertial(Eigen::Vector3d r_I, uint64 return dcm_PfixN * grav_Pfix; } -void GravBodyData::loadEphemeris() +void +GravBodyData::loadEphemeris() { if (this->planetBodyInMsg.isLinked()) { this->localPlanet = this->planetBodyInMsg(); this->timeWritten = this->planetBodyInMsg.timeWritten(); - } - else { + } else { // use default zero planet state information this->localPlanet = this->planetBodyInMsg.zeroMsgPayload; this->timeWritten = 0; } } -void GravBodyData::registerProperties(DynParamManager& statesIn) +void +GravBodyData::registerProperties(DynParamManager& statesIn) { if (this->planetName == "") { auto errorMessage = "You must specify a planetary body name in GravBodyData"; @@ -105,14 +103,14 @@ void GravBodyData::registerProperties(DynParamManager& statesIn) Eigen::MatrixXd muInit = Eigen::MatrixXd::Zero(1, 1); this->muPlanet = statesIn.createProperty(this->planetName + ".mu", muInit); - this->J20002Pfix = statesIn.createProperty( - this->planetName + ".J20002Pfix", c2DArray2EigenMatrix3d(this->localPlanet.J20002Pfix)); - this->J20002Pfix_dot = - statesIn.createProperty(this->planetName + ".J20002Pfix_dot", - c2DArray2EigenMatrix3d(this->localPlanet.J20002Pfix_dot)); + this->J20002Pfix = + statesIn.createProperty(this->planetName + ".J20002Pfix", c2DArray2EigenMatrix3d(this->localPlanet.J20002Pfix)); + this->J20002Pfix_dot = statesIn.createProperty(this->planetName + ".J20002Pfix_dot", + c2DArray2EigenMatrix3d(this->localPlanet.J20002Pfix_dot)); } -void GravityEffector::Reset(uint64_t currentSimNanos) +void +GravityEffector::Reset(uint64_t currentSimNanos) { // Initializes the bodies for (auto&& body : this->gravBodies) { @@ -120,22 +118,23 @@ void GravityEffector::Reset(uint64_t currentSimNanos) } } -void GravityEffector::UpdateState(uint64_t currentSimNanos) +void +GravityEffector::UpdateState(uint64_t currentSimNanos) { // Updates the central body this->centralBody.reset(); for (auto&& body : this->gravBodies) { body->loadEphemeris(); - if (!body->isCentralBody) continue; + if (!body->isCentralBody) + continue; if (this->centralBody) // A centralBody was already set { auto errorMessage = "Specified two central bodies at the same time"; this->bskLogger.bskLog(BSK_ERROR, errorMessage); throw std::invalid_argument(errorMessage); - } - else { + } else { this->centralBody = body; } } @@ -143,32 +142,30 @@ void GravityEffector::UpdateState(uint64_t currentSimNanos) this->writeOutputMessages(currentSimNanos); } -void GravityEffector::writeOutputMessages(uint64_t currentSimNanos) +void +GravityEffector::writeOutputMessages(uint64_t currentSimNanos) { if (this->centralBodyOutMsg.isLinked() && this->centralBody) { - this->centralBodyOutMsg.write(&this->centralBody->localPlanet, this->moduleID, - currentSimNanos); + this->centralBodyOutMsg.write(&this->centralBody->localPlanet, this->moduleID, currentSimNanos); } } -void GravityEffector::prependSpacecraftNameToStates() +void +GravityEffector::prependSpacecraftNameToStates() { - this->inertialPositionPropName = - this->nameOfSpacecraftAttachedTo + this->inertialPositionPropName; - this->inertialVelocityPropName = - this->nameOfSpacecraftAttachedTo + this->inertialVelocityPropName; + this->inertialPositionPropName = this->nameOfSpacecraftAttachedTo + this->inertialPositionPropName; + this->inertialVelocityPropName = this->nameOfSpacecraftAttachedTo + this->inertialVelocityPropName; this->vehicleGravityPropName = this->nameOfSpacecraftAttachedTo + this->vehicleGravityPropName; } -void GravityEffector::registerProperties(DynParamManager& statesIn) +void +GravityEffector::registerProperties(DynParamManager& statesIn) { static const Eigen::Vector3d zeroVector3d = Eigen::Vector3d::Zero(); this->gravProperty = statesIn.createProperty(this->vehicleGravityPropName, zeroVector3d); - this->inertialPositionProperty = - statesIn.createProperty(this->inertialPositionPropName, zeroVector3d); - this->inertialVelocityProperty = - statesIn.createProperty(this->inertialVelocityPropName, zeroVector3d); + this->inertialPositionProperty = statesIn.createProperty(this->inertialPositionPropName, zeroVector3d); + this->inertialVelocityProperty = statesIn.createProperty(this->inertialVelocityPropName, zeroVector3d); // register planet position and velocity state vectors as parameters in the // state engine @@ -177,12 +174,14 @@ void GravityEffector::registerProperties(DynParamManager& statesIn) } } -void GravityEffector::linkInStates(DynParamManager& statesIn) +void +GravityEffector::linkInStates(DynParamManager& statesIn) { this->timeCorr = statesIn.getPropertyReference(this->systemTimeCorrPropName); } -void GravityEffector::computeGravityField(Eigen::Vector3d r_cF_N, Eigen::Vector3d rDot_cF_N) +void +GravityEffector::computeGravityField(Eigen::Vector3d r_cF_N, Eigen::Vector3d rDot_cF_N) { uint64_t systemClock = (uint64_t)this->timeCorr->data()[0]; Eigen::Vector3d r_cN_N; // position of s/c CoM wrt N @@ -194,8 +193,7 @@ void GravityEffector::computeGravityField(Eigen::Vector3d r_cF_N, Eigen::Vector3 r_CN_N = getEulerSteppedGravBodyPosition(this->centralBody); r_cN_N = r_cF_N + r_CN_N; // shift s/c to be wrt inertial frame origin // if it isn't already - } - else { + } else { r_cN_N = r_cF_N; } @@ -227,7 +225,8 @@ void GravityEffector::computeGravityField(Eigen::Vector3d r_cF_N, Eigen::Vector3 *this->gravProperty = rDotDot_cF_N; } -void GravityEffector::updateInertialPosAndVel(Eigen::Vector3d r_BF_N, Eigen::Vector3d rDot_BF_N) +void +GravityEffector::updateInertialPosAndVel(Eigen::Vector3d r_BF_N, Eigen::Vector3d rDot_BF_N) { // Here we add the central body inertial position and velocities to the // central-body-relative position and velicities which are propogated @@ -237,9 +236,8 @@ void GravityEffector::updateInertialPosAndVel(Eigen::Vector3d r_BF_N, Eigen::Vec Eigen::Vector3d r_CN_N = getEulerSteppedGravBodyPosition(this->centralBody); *this->inertialPositionProperty = r_CN_N + r_BF_N; *this->inertialVelocityProperty = - cArray2EigenMatrixXd(this->centralBody->localPlanet.VelocityVector, 3, 1) + rDot_BF_N; - } - else { + cArray2EigenMatrixXd(this->centralBody->localPlanet.VelocityVector, 3, 1) + rDot_BF_N; + } else { *this->inertialPositionProperty = r_BF_N; *this->inertialVelocityProperty = rDot_BF_N; } @@ -255,7 +253,8 @@ GravityEffector::getEulerSteppedGravBodyPosition(std::shared_ptr b return r_PN_N; } -void GravityEffector::updateEnergyContributions(Eigen::Vector3d r_cF_N, double& orbPotEnergyContr) +void +GravityEffector::updateEnergyContributions(Eigen::Vector3d r_cF_N, double& orbPotEnergyContr) { Eigen::Vector3d r_CN_N; // C is central body. position of C wrt N in N Eigen::Vector3d r_cN_N; // position c wrt N in N @@ -265,8 +264,7 @@ void GravityEffector::updateEnergyContributions(Eigen::Vector3d r_cF_N, double& r_CN_N = getEulerSteppedGravBodyPosition(this->centralBody); r_cN_N = r_cF_N + r_CN_N; // shift s/c to be wrt inertial frame origin // if it isn't already - } - else { + } else { r_cN_N = r_cF_N; } @@ -283,17 +281,18 @@ void GravityEffector::updateEnergyContributions(Eigen::Vector3d r_cF_N, double& // relative potential energy solution orbPotEnergyContr += body->gravityModel->computePotentialEnergy(r_PN_N - r_CN_N); } - orbPotEnergyContr = body->gravityModel->computePotentialEnergy( - r_cP_N); // Potential w/in current planet field + orbPotEnergyContr = body->gravityModel->computePotentialEnergy(r_cP_N); // Potential w/in current planet field } } -void GravityEffector::setGravBodies(std::vector> gravBodies) +void +GravityEffector::setGravBodies(std::vector> gravBodies) { this->gravBodies = std::move(gravBodies); } -void GravityEffector::addGravBody(std::shared_ptr gravBody) +void +GravityEffector::addGravBody(std::shared_ptr gravBody) { this->gravBodies.push_back(gravBody); } diff --git a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.h index e55a9d90e9..bb25f2d31b 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.h @@ -33,20 +33,20 @@ #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. */ class GravBodyData { -public: + public: friend class GravityEffector; /** Initializes `localPlanet` to zero except for `localPlanet.J20002Pfix`, which * is initialized to the identity matrix. Additionally, initializes `gravityModel` * to a `PointMassGravityModel`. - */ + */ GravBodyData(); /** Initializes the gravityModel, logging and raising an error if @@ -67,42 +67,46 @@ 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. */ - void registerProperties(DynParamManager &statesIn); + void registerProperties(DynParamManager& statesIn); -public: + public: bool isCentralBody = false; /**< Flag indicating that object is center */ std::shared_ptr gravityModel; /**< Model used to compute the gravity of the object */ - double mu = 0; /**< [m3/s^2] central body gravitational param */ - double radEquator = 0; /**< [m] Equatorial radius for the body */ - double radiusRatio = 1; /**< [] ratio of polar over equatorial radius */ - std::string planetName = ""; /**< Gravitational body name, this is used as the Spice name if spiceInterface is used */ - std::string displayName = ""; /**< This is the name that is displayed in Vizard. If not set, Vizard shows planetName */ - std::string modelDictionaryKey = ""; /**< "" will result in using the current default for the celestial body's given name, otherwise key will be matched if possible to available model in internal model dictionary */ + double mu = 0; /**< [m3/s^2] central body gravitational param */ + double radEquator = 0; /**< [m] Equatorial radius for the body */ + double radiusRatio = 1; /**< [] ratio of polar over equatorial radius */ + std::string planetName = + ""; /**< Gravitational body name, this is used as the Spice name if spiceInterface is used */ + std::string displayName = + ""; /**< This is the name that is displayed in Vizard. If not set, Vizard shows planetName */ + std::string modelDictionaryKey = + ""; /**< "" will result in using the current default for the celestial body's given name, otherwise key will be + matched if possible to available model in internal model dictionary */ ReadFunctor planetBodyInMsg; /**< planet spice ephemeris input message */ - SpicePlanetStateMsgPayload localPlanet; /**< [-] Class storage of ephemeris info from scheduled portion */ + SpicePlanetStateMsgPayload localPlanet; /**< [-] Class storage of ephemeris info from scheduled portion */ BSKLogger bskLogger; /**< -- BSK Logging */ -private: - Eigen::MatrixXd *r_PN_N; /**< [m] (state engine property) planet inertial position vector */ - Eigen::MatrixXd *v_PN_N; /**< [m/s] (state engine property) planet inertial velocity vector */ - Eigen::MatrixXd *muPlanet; /**< [m/s] (state engine property) planet inertial velocity vector */ - Eigen::MatrixXd *J20002Pfix; /**< [m/s] (state engine property) planet attitude [PN] */ - Eigen::MatrixXd *J20002Pfix_dot; /**< [m/s] (state engine property) planet attitude rate [PN_dot] */ + private: + Eigen::MatrixXd* r_PN_N; /**< [m] (state engine property) planet inertial position vector */ + Eigen::MatrixXd* v_PN_N; /**< [m/s] (state engine property) planet inertial velocity vector */ + Eigen::MatrixXd* muPlanet; /**< [m/s] (state engine property) planet inertial velocity vector */ + Eigen::MatrixXd* J20002Pfix; /**< [m/s] (state engine property) planet attitude [PN] */ + Eigen::MatrixXd* J20002Pfix_dot; /**< [m/s] (state engine property) planet attitude rate [PN_dot] */ uint64_t timeWritten = 0; /**< [ns] time the input planet state message was written */ }; @@ -110,7 +114,7 @@ class GravBodyData /*! @brief gravity effector class */ class GravityEffector : public SysModel { -public: + public: /** Initializes every `GravBodyData` associated with this `GravityEffector` */ void Reset(uint64_t currentSimNanos); @@ -119,15 +123,15 @@ class GravityEffector : public SysModel void UpdateState(uint64_t currentSimNanos); /** Links the correlation between times property */ - void linkInStates(DynParamManager &statesIn); + void linkInStates(DynParamManager& statesIn); /** Registers the gravity, inertial position, and inertial velocity properties. * It also calls `registerProperties` for every associated `GravBodyData`. */ - void registerProperties(DynParamManager &statesIn); + 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 */ @@ -137,7 +141,7 @@ class GravityEffector : public SysModel void updateInertialPosAndVel(Eigen::Vector3d r_BF_N, Eigen::Vector3d rDot_BF_N); /** Computes the Potential Energy Contributions from every associated `GravBodyData` */ - void updateEnergyContributions(Eigen::Vector3d r_CN_N, double &orbPotEnergyContr); + void updateEnergyContributions(Eigen::Vector3d r_CN_N, double& orbPotEnergyContr); /** Sets the `GravBodyData` associated with this effector */ void setGravBodies(std::vector> gravBodies); @@ -145,14 +149,14 @@ 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. - */ + */ void prependSpacecraftNameToStates(); -private: + private: /** Compute planet position with Euler integration @param bodyData planet data @@ -162,7 +166,7 @@ class GravityEffector : public SysModel /** Writes to centralBodyOutMsg if it is linked and there is a central body */ void writeOutputMessages(uint64_t currentSimNanos); -public: + public: std::vector> gravBodies; /**< [-] Vector of bodies we feel gravity from */ std::shared_ptr centralBody; /**< Central body */ @@ -176,11 +180,13 @@ class GravityEffector : public SysModel BSKLogger bskLogger; /**< -- BSK Logging */ -private: - Eigen::MatrixXd *gravProperty; /**< [-] g_N property for output */ - Eigen::MatrixXd *timeCorr; /**< [-] Time correlation property */ - Eigen::MatrixXd *inertialPositionProperty; /**< [m] r_N inertial position relative to system spice zeroBase/refBase coordinate frame, property for output. */ - Eigen::MatrixXd *inertialVelocityProperty; /**< [m/s] v_N inertial velocity relative to system spice zeroBase/refBase coordinate frame, property for output. */ + private: + Eigen::MatrixXd* gravProperty; /**< [-] g_N property for output */ + Eigen::MatrixXd* timeCorr; /**< [-] Time correlation property */ + Eigen::MatrixXd* inertialPositionProperty; /**< [m] r_N inertial position relative to system spice zeroBase/refBase + coordinate frame, property for output. */ + Eigen::MatrixXd* inertialVelocityProperty; /**< [m/s] v_N inertial velocity relative to system spice + zeroBase/refBase coordinate frame, property for output. */ }; #endif /* GRAVITY_EFFECTOR_H */ 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/gravityModel.h b/src/simulation/dynamics/_GeneralModuleFiles/gravityModel.h index 5788c0251f..b4a6473598 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/gravityModel.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/gravityModel.h @@ -33,7 +33,8 @@ class GravBodyData; * for computing the gravity field generated by a body at some arbitrary * point in space. */ -class GravityModel { +class GravityModel +{ public: // Core Guidelines C.67 and C.127 virtual ~GravityModel() = default; @@ -84,7 +85,7 @@ class GravityModel { virtual double computePotentialEnergy(const Eigen::Vector3d& positionWrtPlanet_N) const = 0; public: - BSKLogger *bskLogger; /*!< pointer to bsk logging instance */ + BSKLogger* bskLogger; /*!< pointer to bsk logging instance */ }; #endif /* GRAVITY_MODEL_H */ diff --git a/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.cpp index 30914367ee..0162636bfd 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.cpp @@ -59,13 +59,15 @@ HubEffector::~HubEffector() /*! This method allows the hub access to gravity and also gets access to the properties in the dyn Manager because uses these values in the computeDerivatives method call */ -void HubEffector::linkInStates(DynParamManager& statesIn) +void +HubEffector::linkInStates(DynParamManager& statesIn) { this->g_N = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + "g_N"); return; } -void HubEffector::prependSpacecraftNameToStates() +void +HubEffector::prependSpacecraftNameToStates() { this->nameOfHubPosition = this->nameOfSpacecraftAttachedTo + this->nameOfHubPosition; this->nameOfHubVelocity = this->nameOfSpacecraftAttachedTo + this->nameOfHubVelocity; @@ -78,7 +80,8 @@ void HubEffector::prependSpacecraftNameToStates() } /*! This method allows the hub to register its states: r_BN_N, v_BN_N, sigma_BN and omega_BN_B */ -void HubEffector::registerStates(DynParamManager& states) +void +HubEffector::registerStates(DynParamManager& states) { // - Register the hub states and set with initial values this->posState = states.registerState(3, 1, this->nameOfHubPosition); @@ -100,18 +103,19 @@ void HubEffector::registerStates(DynParamManager& states) } /*! This method allows the hub to give its mass properties to the spacecraft */ -void HubEffector::updateEffectorMassProps(double integTime) +void +HubEffector::updateEffectorMassProps(double integTime) { // - Give the mass to mass props this->effProps.mEff = this->mHub; // - Provide information about multi-spacecraft origin if needed - this->r_BcP_P = this->r_BP_P + this->dcm_BP.transpose()*(this->r_BcB_B); - this->IHubPntBc_P = this->dcm_BP.transpose()*this->IHubPntBc_B*this->dcm_BP; + this->r_BcP_P = this->r_BP_P + this->dcm_BP.transpose() * (this->r_BcB_B); + this->IHubPntBc_P = this->dcm_BP.transpose() * this->IHubPntBc_B * this->dcm_BP; // - Give inertia of hub about point B to mass props - this->effProps.IEffPntB_B = this->IHubPntBc_P - + this->mHub*eigenTilde(this->r_BcP_P)*eigenTilde(this->r_BcP_P).transpose(); + this->effProps.IEffPntB_B = + this->IHubPntBc_P + this->mHub * eigenTilde(this->r_BcP_P) * eigenTilde(this->r_BcP_P).transpose(); // - Give position of center of mass of hub with respect to point B to mass props this->effProps.rEff_CB_B = this->r_BcP_P; @@ -125,7 +129,11 @@ void HubEffector::updateEffectorMassProps(double integTime) /*! This method is for computing the derivatives of the hub: rDDot_BN_N and omegaDot_BN_B, along with the kinematic derivatives */ -void HubEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +HubEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { // - Get variables from state manager Eigen::Vector3d rDotLocal_BN_N; @@ -135,12 +143,12 @@ void HubEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_ Eigen::Vector3d cPrimeLocal_B; Eigen::Vector3d gLocal_N; rDotLocal_BN_N = velocityState->getState(); - sigmaLocal_BN = (Eigen::Vector3d )sigmaState->getState(); + sigmaLocal_BN = (Eigen::Vector3d)sigmaState->getState(); omegaLocal_BN_B = omegaState->getState(); gLocal_N = *this->g_N; // - Set kinematic derivative - sigmaState->setDerivative(1.0/4.0*sigmaLocal_BN.Bmat()*omegaLocal_BN_B); + sigmaState->setDerivative(1.0 / 4.0 * sigmaLocal_BN.Bmat() * omegaLocal_BN_B); // - Define dcm's Eigen::Matrix3d dcm_NB; @@ -152,13 +160,17 @@ void HubEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_ Eigen::Vector3d omegaDotLocal_BN_B; Eigen::Matrix3d intermediateMatrix; Eigen::Vector3d intermediateVector; - intermediateVector = this->hubBackSubMatrices.vecRot - this->hubBackSubMatrices.matrixC*this->hubBackSubMatrices.matrixA.inverse()*this->hubBackSubMatrices.vecTrans; - intermediateMatrix = hubBackSubMatrices.matrixD - hubBackSubMatrices.matrixC*hubBackSubMatrices.matrixA.inverse()*hubBackSubMatrices.matrixB; - omegaDotLocal_BN_B = intermediateMatrix.inverse()*intermediateVector; + intermediateVector = this->hubBackSubMatrices.vecRot - this->hubBackSubMatrices.matrixC * + this->hubBackSubMatrices.matrixA.inverse() * + this->hubBackSubMatrices.vecTrans; + intermediateMatrix = hubBackSubMatrices.matrixD - + hubBackSubMatrices.matrixC * hubBackSubMatrices.matrixA.inverse() * hubBackSubMatrices.matrixB; + omegaDotLocal_BN_B = intermediateMatrix.inverse() * intermediateVector; omegaState->setDerivative(omegaDotLocal_BN_B); // - Solve for rDDot_BN_N - velocityState->setDerivative(dcm_NB*hubBackSubMatrices.matrixA.inverse()*(hubBackSubMatrices.vecTrans - hubBackSubMatrices.matrixB*omegaDotLocal_BN_B)); + velocityState->setDerivative(dcm_NB * hubBackSubMatrices.matrixA.inverse() * + (hubBackSubMatrices.vecTrans - hubBackSubMatrices.matrixB * omegaDotLocal_BN_B)); // - Set gravity velocity derivatives gravVelocityState->setDerivative(gLocal_N); @@ -171,8 +183,11 @@ void HubEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_ } /*! This method is for computing the energy and momentum contributions from the hub */ -void HubEffector::updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B) +void +HubEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { // - Get variables needed for energy momentum calcs Eigen::Vector3d omegaLocal_BN_B; @@ -181,22 +196,24 @@ void HubEffector::updateEnergyMomContributions(double integTime, Eigen::Vector3d // - Find rotational angular momentum contribution from hub Eigen::Vector3d rDot_BcB_B; rDot_BcB_B = omegaLocal_BN_B.cross(r_BcP_P); - rotAngMomPntCContr_B = IHubPntBc_P*omegaLocal_BN_B + mHub*r_BcP_P.cross(rDot_BcB_B); + rotAngMomPntCContr_B = IHubPntBc_P * omegaLocal_BN_B + mHub * r_BcP_P.cross(rDot_BcB_B); // - 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); - + 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; } /*! This method is for switching the MRPs */ -void HubEffector::modifyStates(double integTime) +void +HubEffector::modifyStates(double integTime) { // Lets switch those MRPs!! Eigen::Vector3d sigmaBNLoc; - sigmaBNLoc = (Eigen::Vector3d) this->sigmaState->getState(); + sigmaBNLoc = (Eigen::Vector3d)this->sigmaState->getState(); if (sigmaBNLoc.norm() > 1) { - sigmaBNLoc = -sigmaBNLoc/(sigmaBNLoc.dot(sigmaBNLoc)); + sigmaBNLoc = -sigmaBNLoc / (sigmaBNLoc.dot(sigmaBNLoc)); this->sigmaState->setState(sigmaBNLoc); this->MRPSwitchCount++; } @@ -204,8 +221,9 @@ void HubEffector::modifyStates(double integTime) } /*! This method is used to set the gravitational velocity state equal to the base velocity state */ -void HubEffector::matchGravitytoVelocityState(Eigen::Vector3d v_CN_N) +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.h b/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.h index 0b2ac2846d..1adeab5deb 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/hubEffector.h @@ -27,48 +27,54 @@ #include "architecture/utilities/bskLogging.h" /*! @brief spaecraft hub effector class */ -class HubEffector : public StateEffector { -public: - double mHub; //!< [kg] mass of the hub - uint64_t MRPSwitchCount; //!< -- Count on times we've shadowed - std::string nameOfHubPosition; //!< -- Identifier for hub position states - std::string nameOfHubVelocity; //!< -- Identifier for hub velocity states - std::string nameOfHubSigma; //!< -- Identifier for hub sigmaBN states - std::string nameOfHubOmega; //!< -- Identifier for hub omegaBN_B states - std::string nameOfHubGravVelocity; //!< -- Identified for hub gravitational DV state - std::string nameOfBcGravVelocity; //!< -- Identified for point Bc gravitational DV state - Eigen::Vector3d r_BcB_B; //!< [m] vector from point B to CoM of hub in B frame components - Eigen::Matrix3d IHubPntBc_B; //!< [kg m^2] Inertia of hub about point Bc in B frame components - BackSubMatrices hubBackSubMatrices; //!< class method - Eigen::Vector3d r_CN_NInit; //!< [m] Initial position of the spacecraft wrt to base - Eigen::Vector3d v_CN_NInit; //!< [m/s Initial velocity of the spacecraft wrt base - Eigen::Vector3d sigma_BNInit; //!< -- Initial attitude of the spacecraft wrt base - Eigen::Vector3d omega_BN_BInit; //!< [r/s] Initial attitude rate of the spacecraf wrt base - BSKLogger bskLogger; //!< -- BSK Logging +class HubEffector : public StateEffector +{ + public: + double mHub; //!< [kg] mass of the hub + uint64_t MRPSwitchCount; //!< -- Count on times we've shadowed + std::string nameOfHubPosition; //!< -- Identifier for hub position states + std::string nameOfHubVelocity; //!< -- Identifier for hub velocity states + std::string nameOfHubSigma; //!< -- Identifier for hub sigmaBN states + std::string nameOfHubOmega; //!< -- Identifier for hub omegaBN_B states + std::string nameOfHubGravVelocity; //!< -- Identified for hub gravitational DV state + std::string nameOfBcGravVelocity; //!< -- Identified for point Bc gravitational DV state + Eigen::Vector3d r_BcB_B; //!< [m] vector from point B to CoM of hub in B frame components + Eigen::Matrix3d IHubPntBc_B; //!< [kg m^2] Inertia of hub about point Bc in B frame components + BackSubMatrices hubBackSubMatrices; //!< class method + Eigen::Vector3d r_CN_NInit; //!< [m] Initial position of the spacecraft wrt to base + Eigen::Vector3d v_CN_NInit; //!< [m/s Initial velocity of the spacecraft wrt base + Eigen::Vector3d sigma_BNInit; //!< -- Initial attitude of the spacecraft wrt base + Eigen::Vector3d omega_BN_BInit; //!< [r/s] Initial attitude rate of the spacecraf wrt base + BSKLogger bskLogger; //!< -- BSK Logging -public: - HubEffector(); //!< -- Contructor - ~HubEffector(); //!< -- Destructor - void linkInStates(DynParamManager& statesIn); //!< -- Method to give the hub access to states - void registerStates(DynParamManager& states); //!< -- Method for the hub to register some states - void updateEffectorMassProps(double integTime); //!< -- Method for the hub to update its mass props for the s/c - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN); //!< -- Method for the hub to compute it's derivatives - void updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B); //!< -- Add contributions to energy and momentum - void modifyStates(double integTime); //!< -- Method to switch MRPs - void prependSpacecraftNameToStates(); //!< class method + public: + HubEffector(); //!< -- Contructor + ~HubEffector(); //!< -- Destructor + void linkInStates(DynParamManager& statesIn); //!< -- Method to give the hub access to states + void registerStates(DynParamManager& states); //!< -- Method for the hub to register some states + void updateEffectorMassProps(double integTime); //!< -- Method for the hub to update its mass props for the s/c + void computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN); //!< -- Method for the hub to compute it's derivatives + void updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B); //!< -- Add contributions to energy and momentum + void modifyStates(double integTime); //!< -- Method to switch MRPs + void prependSpacecraftNameToStates(); //!< class method void matchGravitytoVelocityState(Eigen::Vector3d v_CN_N); //!< method to set the gravity velocity to base velocity -private: - Eigen::Vector3d r_BcP_P; //!< [m] vector from point B to CoM of hub in B frame components - Eigen::Matrix3d IHubPntBc_P; //!< [kg m^2] Inertia of hub about point Bc in B frame components - Eigen::MatrixXd *g_N; //!< [m/s^2] Gravitational acceleration in N frame components - StateData *posState; //!< [-] State data container for hub position - StateData *velocityState; //!< [-] State data container for hub velocity - StateData *sigmaState; //!< [-] State data container for hub sigma_BN - StateData *omegaState; //!< [-] State data container for hub omegaBN_B - StateData *gravVelocityState; //!< [-] State data container for hub gravitational velocity - StateData *gravVelocityBcState; //!< [-] State data container for point Bc gravitational velocity + private: + Eigen::Vector3d r_BcP_P; //!< [m] vector from point B to CoM of hub in B frame components + Eigen::Matrix3d IHubPntBc_P; //!< [kg m^2] Inertia of hub about point Bc in B frame components + Eigen::MatrixXd* g_N; //!< [m/s^2] Gravitational acceleration in N frame components + StateData* posState; //!< [-] State data container for hub position + StateData* velocityState; //!< [-] State data container for hub velocity + StateData* sigmaState; //!< [-] State data container for hub sigma_BN + StateData* omegaState; //!< [-] State data container for hub omegaBN_B + StateData* gravVelocityState; //!< [-] State data container for hub gravitational velocity + StateData* gravVelocityBcState; //!< [-] State data container for point Bc gravitational velocity }; #endif /* HUB_EFFECTOR_H */ 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/interpolator.h b/src/simulation/dynamics/_GeneralModuleFiles/interpolator.h index 4b4225b244..81edb635a8 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/interpolator.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/interpolator.h @@ -45,9 +45,10 @@ * continous first and second derivatives, which might be useful for * certain applications. */ -template class Interpolator : public SysModel +template +class Interpolator : public SysModel { -public: + public: /** Set the points to use for interpolation. The first column corresponds * to the independent value, the simulation time in nanoseconds, while the * rest of columns are the data to use for interpolation. @@ -60,16 +61,14 @@ template class Interpolator : public SysModel void setDataPoints(const Eigen::MatrixXd& points, uint16_t degrees = 1) { if (points.cols() != (nArgs + 1)) { - auto error = - "Expected points to have exactly " + std::to_string(nArgs + 1) + " columns."; + auto error = "Expected points to have exactly " + std::to_string(nArgs + 1) + " columns."; this->bskLogger.bskLog(BSK_ERROR, error.c_str()); throw std::invalid_argument(error); } if (points.rows() <= int(degrees)) { - auto error = - "Provided 'degrees' is higher or equal than number of data points (illegal for " - "interpolation)."; + auto error = "Provided 'degrees' is higher or equal than number of data points (illegal for " + "interpolation)."; this->bskLogger.bskLog(BSK_ERROR, error); throw std::invalid_argument(error); } @@ -91,7 +90,7 @@ template class Interpolator : public SysModel * where eps is a small value (1 nanosecond) so that the numbers are not the same. */ if (degrees == 0) { - Eigen::MatrixXd modPoints{(points.rows() - 1) * 2 + 1, nArgs + 1}; + Eigen::MatrixXd modPoints{ (points.rows() - 1) * 2 + 1, nArgs + 1 }; for (int i = 0; i < points.rows() - 1; i++) { modPoints.row(i * 2) = points.row(i); @@ -111,7 +110,7 @@ template class Interpolator : public SysModel * * @param CurrentSimNanos The current simulation time in nanoseconds used to update * the state of the interpolator and generate the output message. - */ + */ void Reset(uint64_t CurrentSimNanos) { UpdateState(CurrentSimNanos); } /** @@ -135,23 +134,19 @@ template class Interpolator : public SysModel } auto payload = this->interpolatedOutMsg.zeroMsgPayload; - this->setPayloadValues( - payload, - CurrentSimNanos, - this->spline(scale(double(CurrentSimNanos))) - ); + this->setPayloadValues(payload, CurrentSimNanos, this->spline(scale(double(CurrentSimNanos)))); interpolatedOutMsg.write(&payload, this->moduleID, CurrentSimNanos); } -public: + public: /** Result message of the interpolation */ Message interpolatedOutMsg; /** Logger used to log errors */ BSKLogger bskLogger; -protected: + protected: /** Called by `UpdateState` with the simulation time and the interpolated values. * * This method should modify the given paylod object and fill it with the @@ -171,10 +166,7 @@ template class Interpolator : public SysModel * @param val The value to scale. * @return The scaled value in the range [0, 1]. */ - double scale(double val) - { - return std::clamp((val - this->xMin) / (this->xMax - this->xMin), 0., 1.); - } + double scale(double val) { return std::clamp((val - this->xMin) / (this->xMax - this->xMin), 0., 1.); } /** * Scales a vector of values to the range [0, 1] based on the object's xMin and xMax. @@ -188,8 +180,7 @@ template class Interpolator : public SysModel */ Eigen::VectorXd scale(Eigen::VectorXd val) { - return (val - Eigen::VectorXd::Constant(val.size(), this->xMin)) / - (this->xMax - this->xMin); + return (val - Eigen::VectorXd::Constant(val.size(), this->xMin)) / (this->xMax - this->xMin); } /** @@ -213,15 +204,14 @@ template class Interpolator : public SysModel throw std::invalid_argument(error); } - this->spline = Eigen::SplineFitting::Interpolate( - points.rightCols(nArgs).transpose(), // yp - degrees, - scale(points.col(0))); + this->spline = Eigen::SplineFitting::Interpolate(points.rightCols(nArgs).transpose(), // yp + degrees, + scale(points.col(0))); } -protected: - double xMin = 0; /**< Minimum independent value in the data points */ - double xMax = 0; /**< Maximum independent value in the data points */ + protected: + double xMin = 0; /**< Minimum independent value in the data points */ + double xMax = 0; /**< Maximum independent value in the data points */ Eigen::Spline spline; /**< Spline object */ }; diff --git a/src/simulation/dynamics/_GeneralModuleFiles/pointMassGravityModel.cpp b/src/simulation/dynamics/_GeneralModuleFiles/pointMassGravityModel.cpp index 956e9b523b..9690af8402 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/pointMassGravityModel.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/pointMassGravityModel.cpp @@ -20,7 +20,8 @@ #include "pointMassGravityModel.h" #include "gravityEffector.h" -std::optional PointMassGravityModel::initializeParameters(const GravBodyData& body) +std::optional +PointMassGravityModel::initializeParameters(const GravBodyData& body) { this->muBody = body.mu; return this->initializeParameters(); diff --git a/src/simulation/dynamics/_GeneralModuleFiles/pointMassGravityModel.h b/src/simulation/dynamics/_GeneralModuleFiles/pointMassGravityModel.h index f7dcfbefd7..0435bbb4cb 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/pointMassGravityModel.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/pointMassGravityModel.h @@ -26,7 +26,8 @@ /** * The point mass gravity model */ -class PointMassGravityModel : public GravityModel { +class PointMassGravityModel : public GravityModel +{ public: /** Does nothing, as the point-mass gravity model has no parameters other than * `muBody`, which must be set separately */ diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateData.cpp b/src/simulation/dynamics/_GeneralModuleFiles/stateData.cpp old mode 100755 new mode 100644 index 0b886304a9..023baf5dfd --- a/src/simulation/dynamics/_GeneralModuleFiles/stateData.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateData.cpp @@ -17,46 +17,50 @@ */ - #include "stateData.h" -std::unique_ptr StateData::clone() const +std::unique_ptr +StateData::clone() const { auto result = std::make_unique(this->stateName, this->state); result->stateDeriv = this->stateDeriv; return result; } -StateData::StateData(std::string inName, const Eigen::MatrixXd & newState) -: stateName(inName) +StateData::StateData(std::string inName, const Eigen::MatrixXd& newState) + : stateName(inName) { setState(newState); stateDeriv.resizeLike(state); - setDerivative( Eigen::MatrixXd::Zero(state.innerSize(), state.outerSize()) ); + setDerivative(Eigen::MatrixXd::Zero(state.innerSize(), state.outerSize())); } -void StateData::setState(const Eigen::MatrixXd & newState) +void +StateData::setState(const Eigen::MatrixXd& newState) { state = newState; } -void StateData::propagateState(double dt) +void +StateData::propagateState(double dt) { state += stateDeriv * dt; } - -void StateData::setDerivative(const Eigen::MatrixXd & newDeriv) +void +StateData::setDerivative(const Eigen::MatrixXd& newDeriv) { stateDeriv = newDeriv; } -void StateData::scaleState(double scaleFactor) +void +StateData::scaleState(double scaleFactor) { state *= scaleFactor; } -void StateData::addState(const StateData& other) +void +StateData::addState(const StateData& other) { state += other.state; } diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateData.h b/src/simulation/dynamics/_GeneralModuleFiles/stateData.h old mode 100755 new mode 100644 index ab99701414..ae4f2a4add --- a/src/simulation/dynamics/_GeneralModuleFiles/stateData.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateData.h @@ -27,13 +27,13 @@ /** @brief Represents a physical state, which has a name, a value, and a derivative. */ class StateData { -public: + public: Eigen::MatrixXd state; //!< [-] State value storage Eigen::MatrixXd stateDeriv; //!< [-] State derivative value storage const std::string stateName; //!< [-] Name of the state BSKLogger bskLogger; //!< -- BSK Logging -public: + public: /** Creates a new state with the given name and set's the initial state. * * The state derivative will be resized to the same size as the state and zero'd. diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp old mode 100755 new mode 100644 index 2d1de789f7..580787c5c6 --- a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.cpp @@ -47,12 +47,14 @@ StateEffector::~StateEffector() /*! This method is for the state effector to provide its contributions of mass and mass rates to the dynamicObject. This allows for the dynamicObject to have access to the total mass, and inerita, mass and inertia rates*/ -void StateEffector::updateEffectorMassProps(double integTime) +void +StateEffector::updateEffectorMassProps(double integTime) { return; } -void StateEffector::receiveMotherSpacecraftData(Eigen::Vector3d rSC_BP_P, Eigen::Matrix3d dcmSC_BP) +void +StateEffector::receiveMotherSpacecraftData(Eigen::Vector3d rSC_BP_P, Eigen::Matrix3d dcmSC_BP) { this->r_BP_P = rSC_BP_P; this->dcm_BP = dcmSC_BP; @@ -63,7 +65,12 @@ void StateEffector::receiveMotherSpacecraftData(Eigen::Vector3d rSC_BP_P, Eigen: /*! This method is strictly for the back-substituion method for computing the dynamics of the spacecraft. The back-sub method first computes rDDot_BN_N and omegaDot_BN_B for the spacecraft using these contributions from the state effectors. Then computeDerivatives is called to compute the stateEffectors derivatives using rDDot_BN_N omegaDot_BN_B*/ -void StateEffector::updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) +void +StateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { return; } @@ -71,37 +78,46 @@ void StateEffector::updateContributions(double integTime, BackSubMatrices & back /*! This method allows for an individual stateEffector to add its energy and momentum calculations to the dynamicObject. The analytical devlopement of these contributions can be seen in Basilisk/simulation/dynamics/_Documentation/Basilisk-EnergyAndMomentum-20161219.pdf*/ -void StateEffector::updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B) +void +StateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { return; } /*! This method allows for an individual stateEffector to modify their states after integration*/ -void StateEffector::modifyStates(double integTime) +void +StateEffector::modifyStates(double integTime) { return; } -/*! This method allows for an individual stateEffector to find the force and torque that the stateEffector is placing on to the body */ -void StateEffector::calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B) +/*! This method allows for an individual stateEffector to find the force and torque that the stateEffector is placing on + * to the body */ +void +StateEffector::calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B) { return; } /*! This method ensures that all dynamics states have their messages written after integation */ -void StateEffector::writeOutputStateMessages(uint64_t integTimeNanos) +void +StateEffector::writeOutputStateMessages(uint64_t integTimeNanos) { return; } /*! This method ensures that stateEffectors can be implemented using the multi-spacecraft archticture */ -void StateEffector::prependSpacecraftNameToStates() +void +StateEffector::prependSpacecraftNameToStates() { return; } -void StateEffector::setStateNameOfPosition(std::string value) +void +StateEffector::setStateNameOfPosition(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -111,7 +127,8 @@ void StateEffector::setStateNameOfPosition(std::string value) } } -void StateEffector::setStateNameOfVelocity(std::string value) +void +StateEffector::setStateNameOfVelocity(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -121,7 +138,8 @@ void StateEffector::setStateNameOfVelocity(std::string value) } } -void StateEffector::setStateNameOfSigma(std::string value) +void +StateEffector::setStateNameOfSigma(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -131,7 +149,8 @@ void StateEffector::setStateNameOfSigma(std::string value) } } -void StateEffector::setStateNameOfOmega(std::string value) +void +StateEffector::setStateNameOfOmega(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -141,7 +160,8 @@ void StateEffector::setStateNameOfOmega(std::string value) } } -void StateEffector::setPropName_m_SC(std::string value) +void +StateEffector::setPropName_m_SC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -151,7 +171,8 @@ void StateEffector::setPropName_m_SC(std::string value) } } -void StateEffector::setPropName_mDot_SC(std::string value) +void +StateEffector::setPropName_mDot_SC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -161,7 +182,8 @@ void StateEffector::setPropName_mDot_SC(std::string value) } } -void StateEffector::setPropName_centerOfMassSC(std::string value) +void +StateEffector::setPropName_centerOfMassSC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -171,7 +193,8 @@ void StateEffector::setPropName_centerOfMassSC(std::string value) } } -void StateEffector::setPropName_inertiaSC(std::string value) +void +StateEffector::setPropName_inertiaSC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -181,7 +204,8 @@ void StateEffector::setPropName_inertiaSC(std::string value) } } -void StateEffector::setPropName_inertiaPrimeSC(std::string value) +void +StateEffector::setPropName_inertiaPrimeSC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -191,7 +215,8 @@ void StateEffector::setPropName_inertiaPrimeSC(std::string value) } } -void StateEffector::setPropName_centerOfMassPrimeSC(std::string value) +void +StateEffector::setPropName_centerOfMassPrimeSC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -201,7 +226,8 @@ void StateEffector::setPropName_centerOfMassPrimeSC(std::string value) } } -void StateEffector::setPropName_centerOfMassDotSC(std::string value) +void +StateEffector::setPropName_centerOfMassDotSC(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -211,7 +237,8 @@ void StateEffector::setPropName_centerOfMassDotSC(std::string value) } } -void StateEffector::setPropName_inertialPosition(std::string value) +void +StateEffector::setPropName_inertialPosition(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -221,7 +248,8 @@ void StateEffector::setPropName_inertialPosition(std::string value) } } -void StateEffector::setPropName_inertialVelocity(std::string value) +void +StateEffector::setPropName_inertialVelocity(std::string value) { // check that value is acceptable if (!value.empty()) { @@ -231,7 +259,8 @@ void StateEffector::setPropName_inertialVelocity(std::string value) } } -void StateEffector::setPropName_vehicleGravity(std::string value) +void +StateEffector::setPropName_vehicleGravity(std::string value) { // check that value is acceptable if (!value.empty()) { diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h old mode 100755 new mode 100644 index a5d1c6b777..832f79a0b1 --- a/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateEffector.h @@ -25,45 +25,50 @@ #include "dynParamManager.h" #include "architecture/utilities/bskLogging.h" - /*! back substitution matrix structure*/ -struct BackSubMatrices { - Eigen::Matrix3d matrixA; //!< Back-Substitution matrix A - Eigen::Matrix3d matrixB; //!< Back-Substitution matrix B - Eigen::Matrix3d matrixC; //!< Back-Substitution matrix C - Eigen::Matrix3d matrixD; //!< Back-Substitution matrix D - Eigen::Vector3d vecTrans; //!< Back-Substitution translation vector - Eigen::Vector3d vecRot; //!< Back-Substitution rotation vector +struct BackSubMatrices +{ + Eigen::Matrix3d matrixA; //!< Back-Substitution matrix A + Eigen::Matrix3d matrixB; //!< Back-Substitution matrix B + Eigen::Matrix3d matrixC; //!< Back-Substitution matrix C + Eigen::Matrix3d matrixD; //!< Back-Substitution matrix D + Eigen::Vector3d vecTrans; //!< Back-Substitution translation vector + Eigen::Vector3d vecRot; //!< Back-Substitution rotation vector }; /*! @brief Abstract class that is used to implement an effector attached to the dynamicObject that has a state that needs to be integrated. For example: reaction wheels, flexing solar panels, fuel slosh etc */ -typedef struct { - double mEff; //!< [kg] Mass of the effector - double mEffDot; //!< [kg/s] Time derivate of mEff - Eigen::Matrix3d IEffPntB_B; //!< [kg m^2] Inertia of effector relative to point B in B frame components - Eigen::Vector3d rEff_CB_B; //!< [m] Center of mass of effector with respect to point B in B frame comp - Eigen::Vector3d rEffPrime_CB_B; //!< [m/s] Time derivative with respect to the body of rEff_CB_B - Eigen::Matrix3d IEffPrimePntB_B; //!< [kg m^2/s] Time derivative with respect to the body of IEffPntB_B -}EffectorMassProps; +typedef struct +{ + double mEff; //!< [kg] Mass of the effector + double mEffDot; //!< [kg/s] Time derivate of mEff + Eigen::Matrix3d IEffPntB_B; //!< [kg m^2] Inertia of effector relative to point B in B frame components + Eigen::Vector3d rEff_CB_B; //!< [m] Center of mass of effector with respect to point B in B frame comp + Eigen::Vector3d rEffPrime_CB_B; //!< [m/s] Time derivative with respect to the body of rEff_CB_B + Eigen::Matrix3d IEffPrimePntB_B; //!< [kg m^2/s] Time derivative with respect to the body of IEffPntB_B +} EffectorMassProps; /*! @brief state effector class */ -class StateEffector { -public: - std::string nameOfSpacecraftAttachedTo="";//!< class variable - std::string parentSpacecraftName=""; //!< name of the spacecraft the state effector is attached to - EffectorMassProps effProps; //!< stateEffectors instantiation of effector mass props - Eigen::VectorXd stateDerivContribution; //!< stateEffector contribution to another stateEffector to prevent double-counting - Eigen::Vector3d forceOnBody_B; //!< [N] Force that the state effector applies to the s/c - Eigen::Vector3d torqueOnBodyPntB_B; //!< [N] Torque that the state effector applies to the body about point B - Eigen::Vector3d torqueOnBodyPntC_B; //!< [N] Torque that the state effector applies to the body about point B - Eigen::Vector3d r_BP_P; //!< position vector of the spacecraft mody frame origin B relative to the primary spacecraft body frame P. This is used in the SpacecraftSystem module where multiple spacecraft hubs can be a single spacecraft - Eigen::Matrix3d dcm_BP; //!< DCM of the spacecraft body frame B relative to primary spacecraft body frame P +class StateEffector +{ + public: + std::string nameOfSpacecraftAttachedTo = ""; //!< class variable + std::string parentSpacecraftName = ""; //!< name of the spacecraft the state effector is attached to + EffectorMassProps effProps; //!< stateEffectors instantiation of effector mass props + Eigen::VectorXd + stateDerivContribution; //!< stateEffector contribution to another stateEffector to prevent double-counting + Eigen::Vector3d forceOnBody_B; //!< [N] Force that the state effector applies to the s/c + Eigen::Vector3d torqueOnBodyPntB_B; //!< [N] Torque that the state effector applies to the body about point B + Eigen::Vector3d torqueOnBodyPntC_B; //!< [N] Torque that the state effector applies to the body about point B + Eigen::Vector3d r_BP_P; //!< position vector of the spacecraft mody frame origin B relative to the primary + //!< spacecraft body frame P. This is used in the SpacecraftSystem module where multiple + //!< spacecraft hubs can be a single spacecraft + Eigen::Matrix3d dcm_BP; //!< DCM of the spacecraft body frame B relative to primary spacecraft body frame P /** setter for `stateNameOfPosition` property */ void setStateNameOfPosition(std::string value); /** getter for `stateNameOfPosition` property */ - const std::string getStateNameOfPosition() const {return this->stateNameOfPosition; } + const std::string getStateNameOfPosition() const { return this->stateNameOfPosition; } /** setter for `stateNameOfVelocity` property */ void setStateNameOfVelocity(std::string value); /** getter for `stateNameOfVelocity` property */ @@ -117,42 +122,51 @@ class StateEffector { /** getter for `propName_vehicleGravity` property */ const std::string getPropName_vehicleGravity() const { return this->propName_vehicleGravity; } - BSKLogger bskLogger; //!< BSK Logging - -public: - StateEffector(); //!< Contructor - virtual ~StateEffector(); //!< Destructor - virtual void updateEffectorMassProps(double integTime); //!< Method for stateEffector to give mass contributions - virtual void updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N); //!< Back-sub contributions - virtual void updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B); //!< Energy and momentum calculations - virtual void modifyStates(double integTime); //!< Modify state values after integration - virtual void calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B); //!< Force and torque on s/c due to stateEffector + BSKLogger bskLogger; //!< BSK Logging + + public: + StateEffector(); //!< Contructor + virtual ~StateEffector(); //!< Destructor + virtual void updateEffectorMassProps(double integTime); //!< Method for stateEffector to give mass contributions + virtual void updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N); //!< Back-sub contributions + virtual void updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B); //!< Energy and momentum calculations + virtual void modifyStates(double integTime); //!< Modify state values after integration + virtual void calcForceTorqueOnBody(double integTime, + Eigen::Vector3d omega_BN_B); //!< Force and torque on s/c due to stateEffector virtual void writeOutputStateMessages(uint64_t integTimeNanos); //!< Write State Messages after integration - virtual void registerStates(DynParamManager& states) = 0; //!< Method for stateEffectors to register states - virtual void linkInStates(DynParamManager& states) = 0; //!< Method for stateEffectors to get other states - virtual void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN)=0; //!< Method for each stateEffector to calculate derivatives + virtual void registerStates(DynParamManager& states) = 0; //!< Method for stateEffectors to register states + virtual void linkInStates(DynParamManager& states) = 0; //!< Method for stateEffectors to get other states + virtual void computeDerivatives( + double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) = 0; //!< Method for each stateEffector to calculate derivatives virtual void prependSpacecraftNameToStates(); virtual void receiveMotherSpacecraftData(Eigen::Vector3d rSC_BP_P, Eigen::Matrix3d dcmSC_BP); //!< class method -protected: - std::string stateNameOfPosition = ""; //!< state engine name of the parent rigid body inertial position vector - std::string stateNameOfVelocity = ""; //!< state engine name of the parent rigid body inertial velocity vector - std::string stateNameOfSigma = ""; //!< state engine name of the parent rigid body inertial attitude - std::string stateNameOfOmega = ""; //!< state engine name of the parent rigid body inertial angular velocity vector - - std::string propName_m_SC = ""; //!< property name of m_SC - std::string propName_mDot_SC = ""; //!< property name of mDot_SC - std::string propName_centerOfMassSC = ""; //!< property name of centerOfMassSC - std::string propName_inertiaSC = ""; //!< property name of inertiaSC - std::string propName_inertiaPrimeSC = ""; //!< property name of inertiaPrimeSC - std::string propName_centerOfMassPrimeSC = ""; //!< property name of centerOfMassPrimeSC - std::string propName_centerOfMassDotSC = ""; //!< property name of centerOfMassDotSC - std::string propName_inertialPosition = ""; //!< property name of inertialPosition - std::string propName_inertialVelocity = ""; //!< property name of inertialVelocity - std::string propName_vehicleGravity = ""; //!< property name of vehicleGravity - + protected: + std::string stateNameOfPosition = ""; //!< state engine name of the parent rigid body inertial position vector + std::string stateNameOfVelocity = ""; //!< state engine name of the parent rigid body inertial velocity vector + std::string stateNameOfSigma = ""; //!< state engine name of the parent rigid body inertial attitude + std::string stateNameOfOmega = ""; //!< state engine name of the parent rigid body inertial angular velocity vector + + std::string propName_m_SC = ""; //!< property name of m_SC + std::string propName_mDot_SC = ""; //!< property name of mDot_SC + std::string propName_centerOfMassSC = ""; //!< property name of centerOfMassSC + std::string propName_inertiaSC = ""; //!< property name of inertiaSC + std::string propName_inertiaPrimeSC = ""; //!< property name of inertiaPrimeSC + std::string propName_centerOfMassPrimeSC = ""; //!< property name of centerOfMassPrimeSC + std::string propName_centerOfMassDotSC = ""; //!< property name of centerOfMassDotSC + std::string propName_inertialPosition = ""; //!< property name of inertialPosition + std::string propName_inertialVelocity = ""; //!< property name of inertialVelocity + std::string propName_vehicleGravity = ""; //!< property name of vehicleGravity }; - #endif /* STATE_EFFECTOR_H */ diff --git a/src/simulation/dynamics/_GeneralModuleFiles/stateVecIntegrator.h b/src/simulation/dynamics/_GeneralModuleFiles/stateVecIntegrator.h index 30248d2bf9..23f3eb7588 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/stateVecIntegrator.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/stateVecIntegrator.h @@ -17,7 +17,6 @@ */ - #ifndef stateVecIntegrator_h #define stateVecIntegrator_h @@ -29,13 +28,12 @@ class DynamicObject; class StateVecIntegrator { -public: + public: StateVecIntegrator(DynamicObject* dynIn); virtual ~StateVecIntegrator(void); virtual void integrate(double currentTime, double timeStep) = 0; //!< class method - std::vector dynPtrs; //!< This is an object that contains the method equationsOfMotion(), also known as the F function. - + std::vector + dynPtrs; //!< This is an object that contains the method equationsOfMotion(), also known as the F function. }; - #endif /* StateVecIntegrator_h */ diff --git a/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorAdaptiveRungeKutta.h b/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorAdaptiveRungeKutta.h index 7c06d26194..f9f3b3ee3c 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorAdaptiveRungeKutta.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorAdaptiveRungeKutta.h @@ -39,7 +39,9 @@ * ----- * b* */ -template struct RKAdaptiveCoefficients : public RKCoefficients { +template +struct RKAdaptiveCoefficients : public RKCoefficients +{ /** "b*" Coefficients of the Adaptive RK method ("b" coefficients of the higher order method) */ typename RKCoefficients::StageSizedArray bStarArray = {}; @@ -54,8 +56,9 @@ template struct RKAdaptiveCoefficients : public RKCoeffici * and, if it's too large, internally use smaller time steps until the error tolerances * are met. */ -template -class svIntegratorAdaptiveRungeKutta : public svIntegratorRungeKutta { +template +class svIntegratorAdaptiveRungeKutta : public svIntegratorRungeKutta +{ public: /** * Constructs the integrator for the given dynamic object and specified Adaptive RK @@ -143,8 +146,7 @@ class svIntegratorAdaptiveRungeKutta : public svIntegratorRungeKutta getRelativeTolerance(const DynamicObject& dynamicObject, - std::string stateName); + std::optional getRelativeTolerance(const DynamicObject& dynamicObject, std::string stateName); /** * Sets the absolute tolerance for the given DynamicObject * and the state identified by the given name. */ - void - setAbsoluteTolerance(const DynamicObject& dynamicObject, std::string stateName, double absTol); + void setAbsoluteTolerance(const DynamicObject& dynamicObject, std::string stateName, double absTol); /** * Returns the absolute tolerance for he given DynamicObject @@ -170,8 +170,7 @@ class svIntegratorAdaptiveRungeKutta : public svIntegratorRungeKutta getAbsoluteTolerance(const DynamicObject& dynamicObject, - std::string stateName); + std::optional getAbsoluteTolerance(const DynamicObject& dynamicObject, std::string stateName); /** Maximum relative truncation error allowed. * @@ -213,10 +212,9 @@ class svIntegratorAdaptiveRungeKutta : public svIntegratorRungeKutta dynObjectStateSpecificAbsTol; }; -template +template svIntegratorAdaptiveRungeKutta::svIntegratorAdaptiveRungeKutta( - DynamicObject* dynIn, - const RKAdaptiveCoefficients& coefficients, - const double methodLargestOrder) - : svIntegratorRungeKutta::svIntegratorRungeKutta( - dynIn, - (std::unique_ptr>)std::move( - std::make_unique>(coefficients))), - methodLargestOrder(methodLargestOrder) + DynamicObject* dynIn, + const RKAdaptiveCoefficients& coefficients, + const double methodLargestOrder) + : svIntegratorRungeKutta::svIntegratorRungeKutta( + dynIn, + (std::unique_ptr>)std::move( + std::make_unique>(coefficients))) + , methodLargestOrder(methodLargestOrder) { } -template -void svIntegratorAdaptiveRungeKutta::integrate(double startingTime, - double desiredTimeStep) +template +void +svIntegratorAdaptiveRungeKutta::integrate(double startingTime, double desiredTimeStep) { double time = startingTime; double timeStep = desiredTimeStep; ExtendedStateVector state = ExtendedStateVector::fromStates(this->dynPtrs); typename svIntegratorRungeKutta::KCoefficientsValues kValues; - auto castCoefficients = - static_cast*>(this->coefficients.get()); + auto castCoefficients = static_cast*>(this->coefficients.get()); // Continue until we are done with the desired time step while (time < startingTime + desiredTimeStep) { @@ -285,17 +281,9 @@ void svIntegratorAdaptiveRungeKutta::integrate(double startingTime // Now we generate two solutions, one of low order and one of // high order by using either the b or b* coefficients ExtendedStateVector lowOrderNextStep = - this->propagateStateWithKVectors(timeStep, - state, - kValues, - castCoefficients->bArray, - numberStages); + this->propagateStateWithKVectors(timeStep, state, kValues, castCoefficients->bArray, numberStages); ExtendedStateVector highOrderNextStep = - this->propagateStateWithKVectors(timeStep, - state, - kValues, - castCoefficients->bStarArray, - numberStages); + this->propagateStateWithKVectors(timeStep, state, kValues, castCoefficients->bStarArray, numberStages); // For the adaptive RK, we also compute the maximum // relationship between error and tolerance @@ -313,12 +301,11 @@ void svIntegratorAdaptiveRungeKutta::integrate(double startingTime } // Regardless of accepting or not the step, we compute a new time step - double newTimeStep = this->safetyFactorForNextStepSize * timeStep * - std::pow(1.0 / maxRelError, 1.0 / this->methodLargestOrder); + double newTimeStep = + this->safetyFactorForNextStepSize * timeStep * std::pow(1.0 / maxRelError, 1.0 / this->methodLargestOrder); newTimeStep = std::min(newTimeStep, timeStep * this->maximumFactorIncreaseForNextStepSize); newTimeStep = std::max(newTimeStep, timeStep * this->minimumFactorDecreaseForNextStepSize); - newTimeStep = - std::min(newTimeStep, startingTime + desiredTimeStep - time); // Avoid over-stepping + newTimeStep = std::min(newTimeStep, startingTime + desiredTimeStep - time); // Avoid over-stepping timeStep = newTimeStep; } @@ -326,11 +313,12 @@ void svIntegratorAdaptiveRungeKutta::integrate(double startingTime state.setStates(this->dynPtrs); } -template -double svIntegratorAdaptiveRungeKutta::computeMaxRelativeError( - double timeStep, - const ExtendedStateVector& lowOrderNextStep, - const ExtendedStateVector& highOrderNextStep) const +template +double +svIntegratorAdaptiveRungeKutta::computeMaxRelativeError( + double timeStep, + const ExtendedStateVector& lowOrderNextStep, + const ExtendedStateVector& highOrderNextStep) const { // Compute the absolute truncation error for every state ExtendedStateVector truncationError = highOrderNextStep - lowOrderNextStep; @@ -342,51 +330,53 @@ double svIntegratorAdaptiveRungeKutta::computeMaxRelativeError( // truncation error and tolerance. double maxRelativeError = 0; auto maxRelativeErrorRef = std::ref(maxRelativeError); - highOrderNextStep.apply([this, &maxRelativeErrorRef, &truncationError]( - const size_t& dynObjIndex, - const std::string& stateName, - const Eigen::MatrixXd& thisState) { - double thisTruncationError = truncationError.at({dynObjIndex, stateName}).norm(); + highOrderNextStep.apply([this, &maxRelativeErrorRef, &truncationError](const size_t& dynObjIndex, + const std::string& stateName, + const Eigen::MatrixXd& thisState) { + double thisTruncationError = truncationError.at({ dynObjIndex, stateName }).norm(); double thisErrorTolerance = this->getTolerance(dynObjIndex, stateName, thisState.norm()); - maxRelativeErrorRef.get() = - std::max(maxRelativeErrorRef.get(), thisTruncationError / thisErrorTolerance); + maxRelativeErrorRef.get() = std::max(maxRelativeErrorRef.get(), thisTruncationError / thisErrorTolerance); }); return maxRelativeError; } -template -void svIntegratorAdaptiveRungeKutta::setRelativeTolerance(double relTol) +template +void +svIntegratorAdaptiveRungeKutta::setRelativeTolerance(double relTol) { this->relTol = relTol; } -template -double svIntegratorAdaptiveRungeKutta::getRelativeTolerance() +template +double +svIntegratorAdaptiveRungeKutta::getRelativeTolerance() { return this->relTol; } -template -void svIntegratorAdaptiveRungeKutta::setAbsoluteTolerance(double absTol) +template +void +svIntegratorAdaptiveRungeKutta::setAbsoluteTolerance(double absTol) { this->absTol = absTol; } -template -double svIntegratorAdaptiveRungeKutta::getAbsoluteTolerance() +template +double +svIntegratorAdaptiveRungeKutta::getAbsoluteTolerance() { return this->absTol; } -template -void svIntegratorAdaptiveRungeKutta::setRelativeTolerance(std::string stateName, - double relTol) +template +void +svIntegratorAdaptiveRungeKutta::setRelativeTolerance(std::string stateName, double relTol) { this->stateSpecificRelTol.at(stateName) = relTol; } -template +template std::optional svIntegratorAdaptiveRungeKutta::getRelativeTolerance(std::string stateName) { @@ -396,14 +386,14 @@ svIntegratorAdaptiveRungeKutta::getRelativeTolerance(std::string s return std::optional(); } -template -void svIntegratorAdaptiveRungeKutta::setAbsoluteTolerance(std::string stateName, - double absTol) +template +void +svIntegratorAdaptiveRungeKutta::setAbsoluteTolerance(std::string stateName, double absTol) { this->stateSpecificAbsTol.at(stateName) = absTol; } -template +template std::optional svIntegratorAdaptiveRungeKutta::getAbsoluteTolerance(std::string stateName) { @@ -413,83 +403,79 @@ svIntegratorAdaptiveRungeKutta::getAbsoluteTolerance(std::string s return std::optional(); } -template -void svIntegratorAdaptiveRungeKutta::setRelativeTolerance( - const DynamicObject& dynamicObject, - std::string stateName, - double relTol) +template +void +svIntegratorAdaptiveRungeKutta::setRelativeTolerance(const DynamicObject& dynamicObject, + std::string stateName, + double relTol) { - this->dynObjectStateSpecificRelTol.at( - {this->findDynamicObjectIndex(dynamicObject), stateName}) = relTol; + this->dynObjectStateSpecificRelTol.at({ this->findDynamicObjectIndex(dynamicObject), stateName }) = relTol; } -template -inline std::optional svIntegratorAdaptiveRungeKutta::getRelativeTolerance( - const DynamicObject& dynamicObject, - std::string stateName) +template +inline std::optional +svIntegratorAdaptiveRungeKutta::getRelativeTolerance(const DynamicObject& dynamicObject, + std::string stateName) { - const ExtendedStateId key = {this->findDynamicObjectIndex(dynamicObject), stateName}; + const ExtendedStateId key = { this->findDynamicObjectIndex(dynamicObject), stateName }; if (this->dynObjectStateSpecificRelTol.count(key) > 0) { return std::optional(this->dynObjectStateSpecificRelTol.at(key)); } return std::optional(); } -template -void svIntegratorAdaptiveRungeKutta::setAbsoluteTolerance( - const DynamicObject& dynamicObject, - std::string stateName, - double absTol) +template +void +svIntegratorAdaptiveRungeKutta::setAbsoluteTolerance(const DynamicObject& dynamicObject, + std::string stateName, + double absTol) { - this->dynObjectStateSpecificAbsTol.at( - {this->findDynamicObjectIndex(dynamicObject), stateName}) = absTol; + this->dynObjectStateSpecificAbsTol.at({ this->findDynamicObjectIndex(dynamicObject), stateName }) = absTol; } -template -inline std::optional svIntegratorAdaptiveRungeKutta::getAbsoluteTolerance( - const DynamicObject& dynamicObject, - std::string stateName) +template +inline std::optional +svIntegratorAdaptiveRungeKutta::getAbsoluteTolerance(const DynamicObject& dynamicObject, + std::string stateName) { - const ExtendedStateId key = {this->findDynamicObjectIndex(dynamicObject), stateName}; + const ExtendedStateId key = { this->findDynamicObjectIndex(dynamicObject), stateName }; if (this->dynObjectStateSpecificAbsTol.count(key) > 0) { return std::optional(this->dynObjectStateSpecificAbsTol.at(key)); } return std::optional(); } -template -size_t svIntegratorAdaptiveRungeKutta::findDynamicObjectIndex( - const DynamicObject& dynamicObject) const +template +size_t +svIntegratorAdaptiveRungeKutta::findDynamicObjectIndex(const DynamicObject& dynamicObject) const { auto it = std::find(this->dynPtrs.cbegin(), this->dynPtrs.cend(), &dynamicObject); if (it == this->dynPtrs.end()) { - throw std::invalid_argument( - "Given DynamicObject is not integrated by this integrator object"); + throw std::invalid_argument("Given DynamicObject is not integrated by this integrator object"); } return std::distance(this->dynPtrs.begin(), it); } -template -double svIntegratorAdaptiveRungeKutta::getTolerance(size_t dynamicObjectIndex, - const std::string& stateName, - double stateNorm) const +template +double +svIntegratorAdaptiveRungeKutta::getTolerance(size_t dynamicObjectIndex, + const std::string& stateName, + double stateNorm) const { - const ExtendedStateId id{dynamicObjectIndex, stateName}; + const ExtendedStateId id{ dynamicObjectIndex, stateName }; - double relTol{this->relTol}; - double absTol{this->absTol}; + double relTol{ this->relTol }; + double absTol{ this->absTol }; if (this->dynObjectStateSpecificRelTol.count(id) > 0) { relTol = this->dynObjectStateSpecificRelTol.at(id); - } - else if (this->stateSpecificRelTol.count(stateName) > 0) { + } else if (this->stateSpecificRelTol.count(stateName) > 0) { relTol = this->stateSpecificRelTol.at(stateName); } if (this->dynObjectStateSpecificAbsTol.count(id) > 0) { absTol = this->dynObjectStateSpecificAbsTol.at(id); - } - else if (this->stateSpecificAbsTol.count(stateName) > 0) { + } else if (this->stateSpecificAbsTol.count(stateName) > 0) { absTol = this->stateSpecificAbsTol.at(stateName); } diff --git a/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.cpp b/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.cpp index b3f4c786b3..b08df2e802 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.cpp @@ -20,20 +20,21 @@ #include "svIntegratorRK4.h" svIntegratorRK4::svIntegratorRK4(DynamicObject* dyn) - : svIntegratorRungeKutta(dyn, svIntegratorRK4::getCoefficients()) + : svIntegratorRungeKutta(dyn, svIntegratorRK4::getCoefficients()) { } -RKCoefficients<4> svIntegratorRK4::getCoefficients() +RKCoefficients<4> +svIntegratorRK4::getCoefficients() { RKCoefficients<4> coefficients; coefficients.aMatrix.at(1).at(0) = 0.5; coefficients.aMatrix.at(2).at(1) = 0.5; coefficients.aMatrix.at(3).at(2) = 1.0; - coefficients.bArray = {1. / 6., 1. / 3., 1. / 3., 1. / 6.}; + coefficients.bArray = { 1. / 6., 1. / 3., 1. / 3., 1. / 6. }; - coefficients.cArray = {0., 1. / 2., 1. / 2., 1.}; + coefficients.cArray = { 0., 1. / 2., 1. / 2., 1. }; return coefficients; } diff --git a/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.h b/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.h index 5ab2bb8edd..40e4b79eab 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.h @@ -23,7 +23,8 @@ #include "svIntegratorRungeKutta.h" /*! @brief 4th order Runge-Kutta integrator */ -class svIntegratorRK4 : public svIntegratorRungeKutta<4> { +class svIntegratorRK4 : public svIntegratorRungeKutta<4> +{ public: svIntegratorRK4(DynamicObject* dyn); //!< class method private: 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/_GeneralModuleFiles/svIntegratorRungeKutta.h b/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRungeKutta.h index f31080c8ab..337229228f 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRungeKutta.h +++ b/src/simulation/dynamics/_GeneralModuleFiles/svIntegratorRungeKutta.h @@ -41,7 +41,9 @@ * ----- * b */ -template struct RKCoefficients { +template +struct RKCoefficients +{ /** Array with size = numberStages */ using StageSizedArray = std::array; @@ -66,7 +68,9 @@ template struct RKCoefficients { * Note that the order of the integrator is lower or equal to the stage number. * A RK method of order 5, for example, requires 7 stages. */ -template class svIntegratorRungeKutta : public StateVecIntegrator { +template +class svIntegratorRungeKutta : public StateVecIntegrator +{ public: static_assert(numberStages > 0, "One cannot declare Runge Kutta integrators of stage 0"); @@ -83,8 +87,7 @@ template class svIntegratorRungeKutta : public StateVecInt * Can be used by subclasses to support passing coefficients * that are subclasses of RKCoefficients */ - svIntegratorRungeKutta(DynamicObject* dynIn, - std::unique_ptr>&& coefficients); + svIntegratorRungeKutta(DynamicObject* dynIn, std::unique_ptr>&& coefficients); /** * For an s-stage RK method, s number of "k" coefficients are @@ -99,8 +102,7 @@ template class svIntegratorRungeKutta : public StateVecInt * Internally, this sets the states on the dynamic objects and * calls the equationsOfMotion methods. */ - ExtendedStateVector - computeDerivatives(double time, double timeStep, const ExtendedStateVector& states); + ExtendedStateVector computeDerivatives(double time, double timeStep, const ExtendedStateVector& states); /** * Computes the "k" coefficients of the Runge-Kutta method @@ -118,12 +120,11 @@ template class svIntegratorRungeKutta : public StateVecInt * where y_0 is currentStates, dt is timeStep, [coeff_0, coeff_1, ...] is coefficients * and [k_0, k_1, ...] is kVectors. */ - ExtendedStateVector - propagateStateWithKVectors(double timeStep, - const ExtendedStateVector& currentStates, - const KCoefficientsValues& kVectors, - const std::array& coefficients, - size_t maxStage); + ExtendedStateVector propagateStateWithKVectors(double timeStep, + const ExtendedStateVector& currentStates, + const KCoefficientsValues& kVectors, + const std::array& coefficients, + size_t maxStage); protected: // coefficients is stored as a pointer to support polymorphism @@ -131,37 +132,35 @@ template class svIntegratorRungeKutta : public StateVecInt const std::unique_ptr> coefficients; }; -template -svIntegratorRungeKutta::svIntegratorRungeKutta( - DynamicObject* dynIn, - const RKCoefficients& coefficients) - : StateVecIntegrator(dynIn), - coefficients(std::make_unique>(coefficients)) +template +svIntegratorRungeKutta::svIntegratorRungeKutta(DynamicObject* dynIn, + const RKCoefficients& coefficients) + : StateVecIntegrator(dynIn) + , coefficients(std::make_unique>(coefficients)) { } -template +template svIntegratorRungeKutta::svIntegratorRungeKutta( - DynamicObject* dynIn, - std::unique_ptr>&& coefficients) - : StateVecIntegrator(dynIn), coefficients(std::move(coefficients)) + DynamicObject* dynIn, + std::unique_ptr>&& coefficients) + : StateVecIntegrator(dynIn) + , coefficients(std::move(coefficients)) { } -template -void svIntegratorRungeKutta::integrate(double currentTime, double timeStep) +template +void +svIntegratorRungeKutta::integrate(double currentTime, double timeStep) { ExtendedStateVector currentState = ExtendedStateVector::fromStates(this->dynPtrs); KCoefficientsValues kValues = this->computeKCoefficients(currentTime, timeStep, currentState); - ExtendedStateVector nextState = this->propagateStateWithKVectors(timeStep, - currentState, - kValues, - this->coefficients->bArray, - numberStages); + ExtendedStateVector nextState = + this->propagateStateWithKVectors(timeStep, currentState, kValues, this->coefficients->bArray, numberStages); nextState.setStates(this->dynPtrs); } -template +template ExtendedStateVector svIntegratorRungeKutta::computeDerivatives(double time, double timeStep, @@ -176,55 +175,50 @@ svIntegratorRungeKutta::computeDerivatives(double time, return ExtendedStateVector::fromStateDerivs(this->dynPtrs); } -template -auto svIntegratorRungeKutta::computeKCoefficients( - double currentTime, - double timeStep, - const ExtendedStateVector& currentStates) -> KCoefficientsValues +template +auto +svIntegratorRungeKutta::computeKCoefficients(double currentTime, + double timeStep, + const ExtendedStateVector& currentStates) + -> KCoefficientsValues { KCoefficientsValues kVectors; for (size_t stageIndex = 0; stageIndex < numberStages; stageIndex++) { double timeToComputeK = currentTime + this->coefficients->cArray.at(stageIndex) * timeStep; - auto stateToComputeTheDerivatesAt = - this->propagateStateWithKVectors(timeStep, - currentStates, - kVectors, - this->coefficients->aMatrix.at(stageIndex), - stageIndex); - kVectors.at(stageIndex) = - this->computeDerivatives(timeToComputeK, timeStep, stateToComputeTheDerivatesAt); + auto stateToComputeTheDerivatesAt = this->propagateStateWithKVectors( + timeStep, currentStates, kVectors, this->coefficients->aMatrix.at(stageIndex), stageIndex); + kVectors.at(stageIndex) = this->computeDerivatives(timeToComputeK, timeStep, stateToComputeTheDerivatesAt); } return kVectors; } -template -ExtendedStateVector svIntegratorRungeKutta::propagateStateWithKVectors( - double timeStep, - const ExtendedStateVector& currentStates, - const KCoefficientsValues& kVectors, - const std::array& coefficients, - size_t maxStage) +template +ExtendedStateVector +svIntegratorRungeKutta::propagateStateWithKVectors(double timeStep, + const ExtendedStateVector& currentStates, + const KCoefficientsValues& kVectors, + const std::array& coefficients, + size_t maxStage) { ExtendedStateVector derivative; for (size_t stageIndex = 0; stageIndex < maxStage; stageIndex++) { - if (coefficients.at(stageIndex) == 0) continue; + if (coefficients.at(stageIndex) == 0) + continue; auto scaledKVector = kVectors.at(stageIndex) * coefficients.at(stageIndex); if (derivative.empty()) { derivative = scaledKVector; - } - else { + } else { derivative += scaledKVector; } } if (derivative.empty()) { return currentStates; - } - else { + } else { currentStates.setStates(this->dynPtrs); derivative.setDerivatives(this->dynPtrs); 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/constraintEffector/constraintDynamicEffector.cpp b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp index cc62a868df..fbf74de17a 100644 --- a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp +++ b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.cpp @@ -19,26 +19,21 @@ #include "constraintDynamicEffector.h" #include "architecture/utilities/avsEigenSupport.h" -#include -#include -#include +#include +#include +#include /*! This is the constructor, nothing to report here */ -ConstraintDynamicEffector::ConstraintDynamicEffector() -{ - -} +ConstraintDynamicEffector::ConstraintDynamicEffector() {} /*! This is the destructor, nothing to report here */ -ConstraintDynamicEffector::~ConstraintDynamicEffector() -{ - -} +ConstraintDynamicEffector::~ConstraintDynamicEffector() {} /*! This method is used to reset the module. */ -void ConstraintDynamicEffector::Reset(uint64_t CurrentSimNanos) +void +ConstraintDynamicEffector::Reset(uint64_t CurrentSimNanos) { // check if any individual gains are not specified bool gainset = this->k_d != 0 || this->c_d != 0 || this->k_a != 0 || this->c_a != 0; @@ -50,32 +45,40 @@ void ConstraintDynamicEffector::Reset(uint64_t CurrentSimNanos) } // if individual k's or c's are already set, don't use alpha & beta if (this->k_d == 0) { - this->k_d = pow(this->alpha,2); + this->k_d = pow(this->alpha, 2); } if (this->c_d == 0) { - this->c_d = 2*this->beta; + this->c_d = 2 * this->beta; } if (this->k_a == 0) { - this->k_a = pow(this->alpha,2); + this->k_a = pow(this->alpha, 2); } if (this->c_a == 0) { - this->c_a = 2*this->beta; + this->c_a = 2 * this->beta; } } -void ConstraintDynamicEffector::setR_P2P1_B1Init(Eigen::Vector3d r_P2P1_B1Init) { +void +ConstraintDynamicEffector::setR_P2P1_B1Init(Eigen::Vector3d r_P2P1_B1Init) +{ this->r_P2P1_B1Init = r_P2P1_B1Init; } -void ConstraintDynamicEffector::setR_P1B1_B1(Eigen::Vector3d r_P1B1_B1) { +void +ConstraintDynamicEffector::setR_P1B1_B1(Eigen::Vector3d r_P1B1_B1) +{ this->r_P1B1_B1 = r_P1B1_B1; } -void ConstraintDynamicEffector::setR_P2B2_B2(Eigen::Vector3d r_P2B2_B2) { +void +ConstraintDynamicEffector::setR_P2B2_B2(Eigen::Vector3d r_P2B2_B2) +{ this->r_P2B2_B2 = r_P2B2_B2; } -void ConstraintDynamicEffector::setAlpha(double alpha) { +void +ConstraintDynamicEffector::setAlpha(double alpha) +{ if (alpha > 0.0) this->alpha = alpha; else { @@ -83,7 +86,9 @@ void ConstraintDynamicEffector::setAlpha(double alpha) { } } -void ConstraintDynamicEffector::setBeta(double beta) { +void +ConstraintDynamicEffector::setBeta(double beta) +{ if (beta > 0.0) this->beta = beta; else { @@ -91,7 +96,9 @@ void ConstraintDynamicEffector::setBeta(double beta) { } } -void ConstraintDynamicEffector::setK_d(double k_d) { +void +ConstraintDynamicEffector::setK_d(double k_d) +{ if (k_d > 0.0) this->k_d = k_d; else { @@ -99,7 +106,9 @@ void ConstraintDynamicEffector::setK_d(double k_d) { } } -void ConstraintDynamicEffector::setC_d(double c_d) { +void +ConstraintDynamicEffector::setC_d(double c_d) +{ if (c_d > 0.0) this->c_d = c_d; else { @@ -107,7 +116,9 @@ void ConstraintDynamicEffector::setC_d(double c_d) { } } -void ConstraintDynamicEffector::setK_a(double k_a) { +void +ConstraintDynamicEffector::setK_a(double k_a) +{ if (k_a > 0.0) this->k_a = k_a; else { @@ -115,7 +126,9 @@ void ConstraintDynamicEffector::setK_a(double k_a) { } } -void ConstraintDynamicEffector::setC_a(double c_a) { +void +ConstraintDynamicEffector::setC_a(double c_a) +{ if (c_a > 0.0) this->c_a = c_a; else { @@ -123,23 +136,27 @@ void ConstraintDynamicEffector::setC_a(double c_a) { } } -/*! This method allows the user to set the cut-off frequency of the low pass filter which is then used to calculate the coefficients for numerical low pass filtering based on a second-order low pass filter design. +/*! This method allows the user to set the cut-off frequency of the low pass filter which is then used to calculate the + coefficients for numerical low pass filtering based on a second-order low pass filter design. @param wc The cut-off frequency of the low pass filter. @param h The constant digital time step. @param k The damping coefficient */ -void ConstraintDynamicEffector::setFilter_Data(double wc, double h, double k){ - if (wc>0){ - std::array num_coeffs = {pow(wc*h,2),2*pow(wc*h,2),pow(wc*h,2)}; - std::array denom_coeffs = {-4+4*k*h-pow(wc*h,2),8-2*pow(wc*h,2),4+4*k*h+pow(wc*h,2)}; - this->a = denom_coeffs[1]/denom_coeffs[2]; - this->b = denom_coeffs[0]/denom_coeffs[2]; - this->c = num_coeffs[2]/denom_coeffs[2]; - this->d = num_coeffs[1]/denom_coeffs[2]; - this->e = num_coeffs[0]/denom_coeffs[2]; - } - else{ +void +ConstraintDynamicEffector::setFilter_Data(double wc, double h, double k) +{ + if (wc > 0) { + std::array num_coeffs = { pow(wc * h, 2), 2 * pow(wc * h, 2), pow(wc * h, 2) }; + std::array denom_coeffs = { -4 + 4 * k * h - pow(wc * h, 2), + 8 - 2 * pow(wc * h, 2), + 4 + 4 * k * h + pow(wc * h, 2) }; + this->a = denom_coeffs[1] / denom_coeffs[2]; + this->b = denom_coeffs[0] / denom_coeffs[2]; + this->c = num_coeffs[2] / denom_coeffs[2]; + this->d = num_coeffs[1] / denom_coeffs[2]; + this->e = num_coeffs[0] / denom_coeffs[2]; + } else { bskLogger.bskLog(BSK_ERROR, "Cut off frequency of low pass filter w_c must be greater than 0."); } } @@ -147,29 +164,31 @@ void ConstraintDynamicEffector::setFilter_Data(double wc, double h, double k){ /*! This method allows the user to set the status of the constraint dynamic effector */ -void ConstraintDynamicEffector::readInputMessage(){ - if(this->effectorStatusInMsg.isLinked()){ +void +ConstraintDynamicEffector::readInputMessage() +{ + if (this->effectorStatusInMsg.isLinked()) { DeviceStatusMsgPayload statusMsg; statusMsg = this->effectorStatusInMsg(); this->effectorStatus = statusMsg.deviceStatus; - } - else{ + } else { this->effectorStatus = 1; - } + } } /*! This method allows the constraint effector to have access to the parent states @param states The states to link */ -void ConstraintDynamicEffector::linkInStates(DynParamManager& states) +void +ConstraintDynamicEffector::linkInStates(DynParamManager& states) { if (this->scInitCounter > 1) { bskLogger.bskLog(BSK_ERROR, "constraintDynamicEffector: tried to attach more than 2 spacecraft"); } this->hubSigma.push_back(states.getStateObject("hubSigma")); - this->hubOmega.push_back(states.getStateObject("hubOmega")); + this->hubOmega.push_back(states.getStateObject("hubOmega")); this->hubPosition.push_back(states.getStateObject("hubPosition")); this->hubVelocity.push_back(states.getStateObject("hubVelocity")); @@ -181,7 +200,8 @@ void ConstraintDynamicEffector::linkInStates(DynParamManager& states) @param integTime Integration time @param timeStep Current integration time step used */ -void ConstraintDynamicEffector::computeForceTorque(double integTime, double timeStep) +void +ConstraintDynamicEffector::computeForceTorque(double integTime, double timeStep) { if (this->scInitCounter == 2) { // only proceed once both spacecraft are added // alternate assigning the constraint force and torque @@ -223,7 +243,8 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time this->omega_B2B1_B2 = omega_B2N_B2 - omega_B1N_B2; // difference in angular rate // calculate the constraint force - this->Fc_N = this->k_d * this->psi_N + this->c_d * this->psiPrime_N; // store the constraint force for spacecraft 2 + this->Fc_N = + this->k_d * this->psi_N + this->c_d * this->psiPrime_N; // store the constraint force for spacecraft 2 this->forceExternal_N = this->Fc_N; // calculate the torque on each spacecraft from the direction constraint @@ -234,21 +255,21 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time // calculate the constraint torque imparted on each spacecraft from the attitude constraint Eigen::Matrix3d dcm_B1B2 = dcm_B1N * dcm_B2N.transpose(); - Eigen::Vector3d L_B2_att = -this->k_a * eigenMRPd2Vector3d(sigma_B2B1) - this->c_a * 0.25 * sigma_B2B1.Bmat() * omega_B2B1_B2; - Eigen::Vector3d L_B1_att = - dcm_B1B2 * L_B2_att; + Eigen::Vector3d L_B2_att = + -this->k_a * eigenMRPd2Vector3d(sigma_B2B1) - this->c_a * 0.25 * sigma_B2B1.Bmat() * omega_B2B1_B2; + Eigen::Vector3d L_B1_att = -dcm_B1B2 * L_B2_att; this->T_B2 = L_B2_len + L_B2_att; // store the constraint torque for spacecraft 2 // assign forces and torques for spacecraft 1 this->forceExternal_N = this->Fc_N; this->torqueExternalPntB_B = L_B1_len + L_B1_att; this->T_B1 = this->torqueExternalPntB_B; - } - else if (this->scID == 1) { + } else if (this->scID == 1) { // assign forces and torques for spacecraft 2 - this->forceExternal_N = - this->Fc_N; + this->forceExternal_N = -this->Fc_N; this->torqueExternalPntB_B = this->T_B2; } - this->scID = (1 + pow(-1,this->scID))/2; // toggle spacecraft to be assigned forces and torques + this->scID = (1 + pow(-1, this->scID)) / 2; // toggle spacecraft to be assigned forces and torques } } @@ -257,28 +278,30 @@ void ConstraintDynamicEffector::computeForceTorque(double integTime, double time @param CurrentClock The current simulation time (used for time stamping) */ -void ConstraintDynamicEffector::writeOutputStateMessage(uint64_t CurrentClock) +void +ConstraintDynamicEffector::writeOutputStateMessage(uint64_t CurrentClock) { ConstDynEffectorMsgPayload outputForces; outputForces = this->constraintElements.zeroMsgPayload; - eigenVector3d2CArray(this->forceExternal_N,outputForces.Fc_N); - eigenVector3d2CArray(this->T_B1,outputForces.L1_B1); - eigenVector3d2CArray(this->T_B2,outputForces.L2_B2); - eigenVector3d2CArray(this->psi_N,outputForces.psi_N); + eigenVector3d2CArray(this->forceExternal_N, outputForces.Fc_N); + eigenVector3d2CArray(this->T_B1, outputForces.L1_B1); + eigenVector3d2CArray(this->T_B2, outputForces.L2_B2); + eigenVector3d2CArray(this->psi_N, outputForces.psi_N); outputForces.Fc_mag_filtered = this->F_filtered_mag_t; outputForces.L1_mag_filtered = this->T1_filtered_mag_t; outputForces.L2_mag_filtered = this->T2_filtered_mag_t; - this->constraintElements.write(&outputForces,this->moduleID,CurrentClock); + this->constraintElements.write(&outputForces, this->moduleID, CurrentClock); } /*! Update state method @param CurrentSimNanos The current simulation time */ -void ConstraintDynamicEffector::UpdateState(uint64_t CurrentSimNanos) +void +ConstraintDynamicEffector::UpdateState(uint64_t CurrentSimNanos) { this->readInputMessage(); - if(this->effectorStatus){ + if (this->effectorStatus) { this->computeFilteredForce(CurrentSimNanos); this->computeFilteredTorque(CurrentSimNanos); this->writeOutputStateMessage(CurrentSimNanos); @@ -289,15 +312,15 @@ void ConstraintDynamicEffector::UpdateState(uint64_t CurrentSimNanos) @param CurrentClock The current simulation time (used for time stamping) */ -void ConstraintDynamicEffector::computeFilteredForce(uint64_t CurrentClock) +void +ConstraintDynamicEffector::computeFilteredForce(uint64_t CurrentClock) { double F_t[3]; - eigenVector3d2CArray(this->Fc_N,F_t); - this->F_mag_t = std::sqrt(pow(F_t[0],2)+pow(F_t[1],2)+pow(F_t[2],2)); - this->F_filtered_mag_t = this->a*this->F_filtered_mag_tminus1 + - this->b*this->F_filtered_mag_tminus2+this->c*this->F_mag_t+ - this->d*this->F_mag_tminus1+this->e*this->F_mag_tminus2; + eigenVector3d2CArray(this->Fc_N, F_t); + this->F_mag_t = std::sqrt(pow(F_t[0], 2) + pow(F_t[1], 2) + pow(F_t[2], 2)); + this->F_filtered_mag_t = this->a * this->F_filtered_mag_tminus1 + this->b * this->F_filtered_mag_tminus2 + + this->c * this->F_mag_t + this->d * this->F_mag_tminus1 + this->e * this->F_mag_tminus2; this->F_filtered_mag_tminus2 = this->F_filtered_mag_tminus1; this->F_filtered_mag_tminus1 = this->F_filtered_mag_t; this->F_mag_tminus2 = this->F_mag_tminus1; @@ -308,24 +331,27 @@ void ConstraintDynamicEffector::computeFilteredForce(uint64_t CurrentClock) @param CurrentClock The current simulation time (used for time stamping) */ -void ConstraintDynamicEffector::computeFilteredTorque(uint64_t CurrentClock) +void +ConstraintDynamicEffector::computeFilteredTorque(uint64_t CurrentClock) { - double T_t1[3]; - eigenVector3d2CArray(this->T_B1,T_t1); - this->T1_mag_t = std::sqrt(pow(T_t1[0],2)+pow(T_t1[1],2)+pow(T_t1[2],2)); - this->T1_filtered_mag_t = this->a*this->T1_filtered_mag_tminus1 + - this->b*this->T1_filtered_mag_tminus2+this->c*this->T1_mag_t+ - this->d*this->T1_mag_tminus1+this->e*this->T1_mag_tminus2; - this->T1_filtered_mag_tminus2 = this->T1_filtered_mag_tminus1; - this->T1_filtered_mag_tminus1 = this->T1_filtered_mag_t; - this->T1_mag_tminus2 = this->T1_mag_tminus1; - this->T1_mag_tminus1 = this->T1_mag_t; - double T_t2[3]; - eigenVector3d2CArray(this->T_B2,T_t2); - this->T2_mag_t = std::sqrt(pow(T_t2[0],2)+pow(T_t2[1],2)+pow(T_t2[2],2)); - this->T2_filtered_mag_t = this->a*this->T2_filtered_mag_tminus1 + this->b*this->T2_filtered_mag_tminus2+this->c*this->T2_mag_t+this->d*this->T2_mag_tminus1+this->e*this->T2_mag_tminus2; - this->T2_filtered_mag_tminus2 = this->T2_filtered_mag_tminus1; - this->T2_filtered_mag_tminus1 = this->T2_filtered_mag_t; - this->T2_mag_tminus2 = this->T2_mag_tminus1; - this->T2_mag_tminus1 = this->T2_mag_t; + double T_t1[3]; + eigenVector3d2CArray(this->T_B1, T_t1); + this->T1_mag_t = std::sqrt(pow(T_t1[0], 2) + pow(T_t1[1], 2) + pow(T_t1[2], 2)); + this->T1_filtered_mag_t = this->a * this->T1_filtered_mag_tminus1 + this->b * this->T1_filtered_mag_tminus2 + + this->c * this->T1_mag_t + this->d * this->T1_mag_tminus1 + + this->e * this->T1_mag_tminus2; + this->T1_filtered_mag_tminus2 = this->T1_filtered_mag_tminus1; + this->T1_filtered_mag_tminus1 = this->T1_filtered_mag_t; + this->T1_mag_tminus2 = this->T1_mag_tminus1; + this->T1_mag_tminus1 = this->T1_mag_t; + double T_t2[3]; + eigenVector3d2CArray(this->T_B2, T_t2); + this->T2_mag_t = std::sqrt(pow(T_t2[0], 2) + pow(T_t2[1], 2) + pow(T_t2[2], 2)); + this->T2_filtered_mag_t = this->a * this->T2_filtered_mag_tminus1 + this->b * this->T2_filtered_mag_tminus2 + + this->c * this->T2_mag_t + this->d * this->T2_mag_tminus1 + + this->e * this->T2_mag_tminus2; + this->T2_filtered_mag_tminus2 = this->T2_filtered_mag_tminus1; + this->T2_filtered_mag_tminus1 = this->T2_filtered_mag_t; + this->T2_mag_tminus2 = this->T2_mag_tminus1; + this->T2_mag_tminus1 = this->T2_mag_t; } diff --git a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h index 712e24eb68..98b12590d7 100644 --- a/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h +++ b/src/simulation/dynamics/constraintEffector/constraintDynamicEffector.h @@ -17,7 +17,6 @@ */ - #ifndef CONSTRAINT_DYNAMIC_EFFECTOR_H #define CONSTRAINT_DYNAMIC_EFFECTOR_H @@ -34,8 +33,11 @@ #include "architecture/messaging/messaging.h" /*! @brief constraint dynamic effector class */ -class ConstraintDynamicEffector: public SysModel, public DynamicEffector { -public: +class ConstraintDynamicEffector + : public SysModel + , public DynamicEffector +{ + public: ConstraintDynamicEffector(); ~ConstraintDynamicEffector(); void Reset(uint64_t CurrentSimNanos); @@ -66,87 +68,92 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { /** setter for `c_a` gain */ void setC_a(double c_a); /** setter for `a,b,s,c,d,e` coefficients of low pass filter */ - void setFilter_Data(double wc,double h, double k); + void setFilter_Data(double wc, double h, double k); /** getter for `r_P2P1_B1Init` initial spacecraft separation */ - Eigen::Vector3d getR_P2P1_B1Init() const {return this->r_P2P1_B1Init;}; + Eigen::Vector3d getR_P2P1_B1Init() const { return this->r_P2P1_B1Init; }; /** getter for `r_P1B1_B1` connection point position on spacecraft 1 */ - Eigen::Vector3d getR_P1B1_B1() const {return this->r_P1B1_B1;}; + Eigen::Vector3d getR_P1B1_B1() const { return this->r_P1B1_B1; }; /** getter for `r_P2B2_B2` connection point position on spacecraft 2 */ - Eigen::Vector3d getR_P2B2_B2() const {return this->r_P2B2_B2;}; + Eigen::Vector3d getR_P2B2_B2() const { return this->r_P2B2_B2; }; /** getter for `alpha` gain tuning parameter */ - double getAlpha() const {return this->alpha;}; + double getAlpha() const { return this->alpha; }; /** getter for `beta` gain tuning parameter */ - double getBeta() const {return this->beta;}; + double getBeta() const { return this->beta; }; /** getter for `k_d` gain */ - double getK_d() const {return this->k_d;}; + double getK_d() const { return this->k_d; }; /** getter for `c_d` gain */ - double getC_d() const {return this->c_d;}; + double getC_d() const { return this->c_d; }; /** getter for `k_a` gain */ - double getK_a() const {return this->k_a;}; + double getK_a() const { return this->k_a; }; /** getter for `c_a` gain */ - double getC_a() const {return this->c_a;}; + double getC_a() const { return this->c_a; }; - Message constraintElements; //!< output message with constraint force and torque on connected s/c + Message + constraintElements; //!< output message with constraint force and torque on connected s/c ReadFunctor effectorStatusInMsg; //!< input message to record device status - uint64_t effectorStatus=1; //!< internal variable to toggle effector status - -private: + uint64_t effectorStatus = 1; //!< internal variable to toggle effector status + private: // Counters and flags int scInitCounter = 0; //!< counter to kill simulation if more than two spacecraft initialized - int scID = 1; //!< 0,1 alternating spacecraft tracker to output appropriate force/torque + int scID = 1; //!< 0,1 alternating spacecraft tracker to output appropriate force/torque // Constraint length and direction - Eigen::Vector3d r_P1B1_B1 = Eigen::Vector3d::Zero(); //!< [m] position vector from spacecraft 1 hub to its connection point P1 - Eigen::Vector3d r_P2B2_B2 = Eigen::Vector3d::Zero(); //!< [m] position vector from spacecraft 2 hub to its connection point P2 - Eigen::Vector3d r_P2P1_B1Init = Eigen::Vector3d::Zero(); //!< [m] precribed position vector from spacecraft 1 connection point to spacecraft 2 connection point + Eigen::Vector3d r_P1B1_B1 = + Eigen::Vector3d::Zero(); //!< [m] position vector from spacecraft 1 hub to its connection point P1 + Eigen::Vector3d r_P2B2_B2 = + Eigen::Vector3d::Zero(); //!< [m] position vector from spacecraft 2 hub to its connection point P2 + Eigen::Vector3d r_P2P1_B1Init = Eigen::Vector3d::Zero(); //!< [m] precribed position vector from spacecraft 1 + //!< connection point to spacecraft 2 connection point // Gains for PD controller double alpha = 0.0; //!< Baumgarte stabilization gain tuning variable - double beta = 0.0; //!< Baumgarte stabilization gain tuning variable - double k_d = 0.0; //!< direction constraint proportional gain - double c_d = 0.0; //!< direction constraint derivative gain - double k_a = 0.0; //!< attitude constraint proportional gain - double c_a = 0.0; //!< attitude constraint derivative gain - double a = 0.0; //!< coefficient in numerical low pass filter - double b = 0.0; //!< coefficient in numerical low pass filter - double c = 0.0; //!< coefficient in numerical low pass filter - double d = 0.0; //!< coefficient in numerical low pass filter - double e = 0.0; //!< coefficient in numerical low pass filter - - double F_mag_tminus2 = 0.0; //!< Magnitude of unfiltered constraint force at t-2 time step - double F_mag_tminus1 = 0.0; //!< Magnitude of unfiltered constraint force at t-1 time step - double F_mag_t = 0.0; //!< Magnitude of unfiltered constraint force at t time step - double F_filtered_mag_t = 0.0; //!< Magnitude of filtered constraint force at t time step + double beta = 0.0; //!< Baumgarte stabilization gain tuning variable + double k_d = 0.0; //!< direction constraint proportional gain + double c_d = 0.0; //!< direction constraint derivative gain + double k_a = 0.0; //!< attitude constraint proportional gain + double c_a = 0.0; //!< attitude constraint derivative gain + double a = 0.0; //!< coefficient in numerical low pass filter + double b = 0.0; //!< coefficient in numerical low pass filter + double c = 0.0; //!< coefficient in numerical low pass filter + double d = 0.0; //!< coefficient in numerical low pass filter + double e = 0.0; //!< coefficient in numerical low pass filter + + double F_mag_tminus2 = 0.0; //!< Magnitude of unfiltered constraint force at t-2 time step + double F_mag_tminus1 = 0.0; //!< Magnitude of unfiltered constraint force at t-1 time step + double F_mag_t = 0.0; //!< Magnitude of unfiltered constraint force at t time step + double F_filtered_mag_t = 0.0; //!< Magnitude of filtered constraint force at t time step double F_filtered_mag_tminus1 = 0.0; //!< Magnitude of filtered constraint force at t-1 time step double F_filtered_mag_tminus2 = 0.0; //!< Magnitude of filtered constraint force at t-2 time step - double T1_mag_tminus2 = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 1 at t-2 time step - double T1_mag_tminus1 = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 1 at t-1 time step - double T1_mag_t = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 1 at t time step - double T1_filtered_mag_t = 0.0; //!< Magnitude of filtered constraint torque on s/c 1 at t time step + double T1_mag_tminus2 = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 1 at t-2 time step + double T1_mag_tminus1 = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 1 at t-1 time step + double T1_mag_t = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 1 at t time step + double T1_filtered_mag_t = 0.0; //!< Magnitude of filtered constraint torque on s/c 1 at t time step double T1_filtered_mag_tminus1 = 0.0; //!< Magnitude of filtered constraint torque on s/c 1 at t-1 time step double T1_filtered_mag_tminus2 = 0.0; //!< Magnitude of filtered constraint torque on s/c 1 at t-2 time step - double T2_mag_tminus2 = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 2 at t-2 time step - double T2_mag_tminus1 = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 2at t-1 time step - double T2_mag_t = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 2 at t time step - double T2_filtered_mag_t = 0.0; //!< Magnitude of filtered constraint torque on s/c 2 at t time step + double T2_mag_tminus2 = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 2 at t-2 time step + double T2_mag_tminus1 = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 2at t-1 time step + double T2_mag_t = 0.0; //!< Magnitude of unfiltered constraint torque on s/c 2 at t time step + double T2_filtered_mag_t = 0.0; //!< Magnitude of filtered constraint torque on s/c 2 at t time step double T2_filtered_mag_tminus1 = 0.0; //!< Magnitude of filtered constraint torque on s/c 2 at t-1 time step double T2_filtered_mag_tminus2 = 0.0; //!< Magnitude of filtered constraint torque on s/c 2 at t-2 time step // Simulation variable pointers - std::vector hubPosition; //!< [m] parent inertial position vector - std::vector hubVelocity; //!< [m/s] parent inertial velocity vector - std::vector hubSigma; //!< parent attitude Modified Rodrigues Parameters (MRPs) - std::vector hubOmega; //!< [rad/s] parent inertial angular velocity vector + std::vector hubPosition; //!< [m] parent inertial position vector + std::vector hubVelocity; //!< [m/s] parent inertial velocity vector + std::vector hubSigma; //!< parent attitude Modified Rodrigues Parameters (MRPs) + std::vector hubOmega; //!< [rad/s] parent inertial angular velocity vector // Constraint violations Eigen::Vector3d psi_N = Eigen::Vector3d::Zero(); //!< [m] direction constraint violation in inertial frame - Eigen::Vector3d psiPrime_N = Eigen::Vector3d::Zero(); //!< [m/s] direction rate constraint violation in inertial frame + Eigen::Vector3d psiPrime_N = + Eigen::Vector3d::Zero(); //!< [m/s] direction rate constraint violation in inertial frame Eigen::MRPd sigma_B2B1 = Eigen::MRPd::Identity(); //!< attitude constraint violation - Eigen::Vector3d omega_B2B1_B2 = Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity constraint violation in spacecraft 2 body frame + Eigen::Vector3d omega_B2B1_B2 = + Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity constraint violation in spacecraft 2 body frame // Force and torque quantities stored to be assigned on the alternating call of computeForceTorque Eigen::Vector3d Fc_N = Eigen::Vector3d::Zero(); //!< [N] force applied on each spacecraft COM in the inertial frame @@ -154,5 +161,4 @@ class ConstraintDynamicEffector: public SysModel, public DynamicEffector { Eigen::Vector3d T_B1 = Eigen::Vector3d::Zero(); //!< [N-m] torque applied on spacecraft 1 in its body frame }; - #endif /* CONSTRAINT_DYNAMIC_EFFECTOR_H */ 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/dragEffector/dragDynamicEffector.cpp b/src/simulation/dynamics/dragEffector/dragDynamicEffector.cpp index 30dc15db62..a0abba8e4a 100644 --- a/src/simulation/dynamics/dragEffector/dragDynamicEffector.cpp +++ b/src/simulation/dynamics/dragEffector/dragDynamicEffector.cpp @@ -24,14 +24,14 @@ DragDynamicEffector::DragDynamicEffector() { - this->coreParams.projectedArea = 0.0; - this->coreParams.dragCoeff = 0.0; + this->coreParams.projectedArea = 0.0; + this->coreParams.dragCoeff = 0.0; this->coreParams.comOffset.setZero(); - this->modelType = "cannonball"; - this->forceExternal_B.fill(0.0); - this->torqueExternalPntB_B.fill(0.0); - this->v_B.fill(0.0); - this->v_hat_B.fill(0.0); + this->modelType = "cannonball"; + this->forceExternal_B.fill(0.0); + this->torqueExternalPntB_B.fill(0.0); + this->v_B.fill(0.0); + this->v_hat_B.fill(0.0); return; } @@ -39,41 +39,41 @@ DragDynamicEffector::DragDynamicEffector() /*! The destructor.*/ DragDynamicEffector::~DragDynamicEffector() { - return; + return; } - /*! This method is used to reset the module. */ -void DragDynamicEffector::Reset(uint64_t CurrentSimNanos) +void +DragDynamicEffector::Reset(uint64_t CurrentSimNanos) { // check if input message has not been included if (!this->atmoDensInMsg.isLinked()) { bskLogger.bskLog(BSK_ERROR, "dragDynamicEffector.atmoDensInMsg was not linked."); } - } /*! The DragEffector does not write output messages to the rest of the sim. */ -void DragDynamicEffector::WriteOutputMessages(uint64_t CurrentClock) +void +DragDynamicEffector::WriteOutputMessages(uint64_t CurrentClock) { - return; + return; } - /*! This method is used to read the incoming density message and update the internal density/ atmospheric data. */ -bool DragDynamicEffector::ReadInputs() +bool +DragDynamicEffector::ReadInputs() { - bool dataGood; + bool dataGood; this->atmoInData = this->atmoDensInMsg(); dataGood = this->atmoDensInMsg.isWritten(); - return(dataGood); + return (dataGood); } /*! @@ -82,47 +82,56 @@ bool DragDynamicEffector::ReadInputs() @param states simulation states */ -void DragDynamicEffector::linkInStates(DynParamManager& states){ +void +DragDynamicEffector::linkInStates(DynParamManager& states) +{ this->hubSigma = states.getStateObject(this->stateNameOfSigma); this->hubVelocity = states.getStateObject(this->stateNameOfVelocity); } /*! This method updates the internal drag direction based on the spacecraft velocity vector. -*/ -void DragDynamicEffector::updateDragDir(){ + */ +void +DragDynamicEffector::updateDragDir() +{ Eigen::MRPd sigmaBN; sigmaBN = (Eigen::Vector3d)this->hubSigma->getState(); Eigen::Matrix3d dcm_BN = sigmaBN.toRotationMatrix().transpose(); - this->v_B = dcm_BN*this->hubVelocity->getState(); // [m/s] sc velocity - this->v_hat_B = this->v_B / this->v_B.norm(); + this->v_B = dcm_BN * this->hubVelocity->getState(); // [m/s] sc velocity + this->v_hat_B = this->v_B / this->v_B.norm(); - return; + return; } /*! This method implements a simple "cannnonball" (attitude-independent) drag model. -*/ -void DragDynamicEffector::cannonballDrag(){ - //! Begin method steps - //! - Zero out the structure force/torque for the drag set - this->forceExternal_B.setZero(); + */ +void +DragDynamicEffector::cannonballDrag() +{ + //! Begin method steps + //! - Zero out the structure force/torque for the drag set + this->forceExternal_B.setZero(); this->torqueExternalPntB_B.setZero(); - this->forceExternal_B = 0.5 * this->coreParams.dragCoeff * pow(this->v_B.norm(), 2.0) * this->coreParams.projectedArea * this->atmoInData.neutralDensity * (-1.0)*this->v_hat_B; - this->torqueExternalPntB_B = this->coreParams.comOffset.cross(forceExternal_B); + this->forceExternal_B = 0.5 * this->coreParams.dragCoeff * pow(this->v_B.norm(), 2.0) * + this->coreParams.projectedArea * this->atmoInData.neutralDensity * (-1.0) * this->v_hat_B; + this->torqueExternalPntB_B = this->coreParams.comOffset.cross(forceExternal_B); - return; + return; } /*! This method computes the body forces and torques for the dragEffector in a simulation loop, selecting the model type based on the settable attribute "modelType." */ -void DragDynamicEffector::computeForceTorque(double integTime, double timeStep){ - updateDragDir(); - if(this->modelType == "cannonball"){ - cannonballDrag(); - } - return; +void +DragDynamicEffector::computeForceTorque(double integTime, double timeStep) +{ + updateDragDir(); + if (this->modelType == "cannonball") { + cannonballDrag(); + } + return; } /*! This method is called to update the local atmospheric conditions at each timestep. @@ -130,8 +139,9 @@ Naturally, this means that conditions are held piecewise-constant over an integr @param CurrentSimNanos The current simulation time in nanoseconds */ -void DragDynamicEffector::UpdateState(uint64_t CurrentSimNanos) +void +DragDynamicEffector::UpdateState(uint64_t CurrentSimNanos) { - ReadInputs(); - return; + ReadInputs(); + return; } diff --git a/src/simulation/dynamics/dragEffector/dragDynamicEffector.h b/src/simulation/dynamics/dragEffector/dragDynamicEffector.h index bc42fa962f..9079ccdadf 100644 --- a/src/simulation/dynamics/dragEffector/dragDynamicEffector.h +++ b/src/simulation/dynamics/dragEffector/dragDynamicEffector.h @@ -17,7 +17,6 @@ */ - #ifndef DRAG_DYNAMIC_EFFECTOR_H #define DRAG_DYNAMIC_EFFECTOR_H @@ -34,23 +33,24 @@ #include "architecture/utilities/avsEigenSupport.h" #include "architecture/utilities/bskLogging.h" - - - - -//! @brief Container for basic drag parameters - the spacecraft's atmosphere-relative velocity, its projected area, and its drag coefficient. -typedef struct { - double projectedArea; //!< m^2 Area of spacecraft projected in velocity direction - double dragCoeff; //!< -- Nondimensional drag coefficient - Eigen::Vector3d comOffset; //!< m distance from center of mass to center of projected area -}DragBaseData; +//! @brief Container for basic drag parameters - the spacecraft's atmosphere-relative velocity, its projected area, and +//! its drag coefficient. +typedef struct +{ + double projectedArea; //!< m^2 Area of spacecraft projected in velocity direction + double dragCoeff; //!< -- Nondimensional drag coefficient + Eigen::Vector3d comOffset; //!< m distance from center of mass to center of projected area +} DragBaseData; /*! @brief drag dynamic effector */ -class DragDynamicEffector: public SysModel, public DynamicEffector { -public: +class DragDynamicEffector + : public SysModel + , public DynamicEffector +{ + public: DragDynamicEffector(); ~DragDynamicEffector(); - void linkInStates(DynParamManager& states); //!< class method + void linkInStates(DynParamManager& states); //!< class method void computeForceTorque(double integTime, double timeStep); void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); @@ -59,20 +59,18 @@ class DragDynamicEffector: public SysModel, public DynamicEffector { void cannonballDrag(); void updateDragDir(); -public: - DragBaseData coreParams; //!< -- Struct used to hold drag parameters - ReadFunctor atmoDensInMsg; //!< -- message used to read density inputs - std::string modelType; //!< -- String used to set the type of model used to compute drag - StateData *hubSigma; //!< -- Hub/Inertial attitude represented by MRP - StateData *hubVelocity; //!< m/s Hub inertial velocity vector - Eigen::Vector3d v_B; //!< m/s local variable to hold the inertial velocity - Eigen::Vector3d v_hat_B; //!< -- Drag force direction in the inertial frame - BSKLogger bskLogger; //!< -- BSK Logging - -private: + public: + DragBaseData coreParams; //!< -- Struct used to hold drag parameters + ReadFunctor atmoDensInMsg; //!< -- message used to read density inputs + std::string modelType; //!< -- String used to set the type of model used to compute drag + StateData* hubSigma; //!< -- Hub/Inertial attitude represented by MRP + StateData* hubVelocity; //!< m/s Hub inertial velocity vector + Eigen::Vector3d v_B; //!< m/s local variable to hold the inertial velocity + Eigen::Vector3d v_hat_B; //!< -- Drag force direction in the inertial frame + BSKLogger bskLogger; //!< -- BSK Logging + + private: AtmoPropsMsgPayload atmoInData; - }; - #endif /* THRUSTER_DYNAMIC_EFFECTOR_H */ 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/_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/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.cpp b/src/simulation/dynamics/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.cpp old mode 100755 new mode 100644 index e299661a1b..d133e88f4f --- a/src/simulation/dynamics/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.cpp +++ b/src/simulation/dynamics/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.cpp @@ -17,7 +17,6 @@ */ - #include "dualHingedRigidBodyStateEffector.h" #include "architecture/utilities/avsEigenSupport.h" #include @@ -61,8 +60,8 @@ DualHingedRigidBodyStateEffector::DualHingedRigidBodyStateEffector() this->nameOfTheta2DotState = "DualHingedRigidBodyStateEffectorTheta2Dot" + std::to_string(this->effectorID); this->effectorID++; - Message *panelMsg; - Message *scMsg; + Message* panelMsg; + Message* scMsg; for (int c = 0; c < 2; c++) { panelMsg = new Message; this->dualHingedRigidBodyOutMsgs.push_back(panelMsg); @@ -79,26 +78,27 @@ uint64_t DualHingedRigidBodyStateEffector::effectorID = 1; DualHingedRigidBodyStateEffector::~DualHingedRigidBodyStateEffector() { - for (int c=0; c<2; c++) { + for (int c = 0; c < 2; c++) { free(this->dualHingedRigidBodyOutMsgs.at(c)); free(this->dualHingedRigidBodyConfigLogOutMsgs.at(c)); } - this->effectorID = 1; /* reset the panel ID*/ + this->effectorID = 1; /* reset the panel ID*/ return; } - /*! This method is used to reset the module. */ -void DualHingedRigidBodyStateEffector::Reset(uint64_t CurrentSimNanos) +void +DualHingedRigidBodyStateEffector::Reset(uint64_t CurrentSimNanos) { return; } -void DualHingedRigidBodyStateEffector::prependSpacecraftNameToStates() +void +DualHingedRigidBodyStateEffector::prependSpacecraftNameToStates() { this->nameOfTheta1State = this->nameOfSpacecraftAttachedTo + this->nameOfTheta1State; this->nameOfTheta1DotState = this->nameOfSpacecraftAttachedTo + this->nameOfTheta1DotState; @@ -108,20 +108,23 @@ void DualHingedRigidBodyStateEffector::prependSpacecraftNameToStates() return; } - -void DualHingedRigidBodyStateEffector::linkInStates(DynParamManager& statesIn) +void +DualHingedRigidBodyStateEffector::linkInStates(DynParamManager& statesIn) { // - Get access to the hubs sigma, omegaBN_B and velocity needed for dynamic coupling this->g_N = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + "g_N"); - this->inertialPositionProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); - this->inertialVelocityProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); + this->inertialPositionProperty = + statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); + this->inertialVelocityProperty = + statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); this->v_BN_NState = statesIn.getStateObject(this->nameOfSpacecraftAttachedTo + this->stateNameOfVelocity); return; } -void DualHingedRigidBodyStateEffector::registerStates(DynParamManager& states) +void +DualHingedRigidBodyStateEffector::registerStates(DynParamManager& states) { // - Register the states associated with hinged rigid bodies - theta and thetaDot this->theta1State = states.registerState(1, 1, this->nameOfTheta1State); @@ -130,27 +133,28 @@ void DualHingedRigidBodyStateEffector::registerStates(DynParamManager& states) this->theta2DotState = states.registerState(1, 1, this->nameOfTheta2DotState); // - Add this code to allow for non-zero initial conditions, as well hingedRigidBody - Eigen::MatrixXd theta1InitMatrix(1,1); - theta1InitMatrix(0,0) = this->theta1Init; + Eigen::MatrixXd theta1InitMatrix(1, 1); + theta1InitMatrix(0, 0) = this->theta1Init; this->theta1State->setState(theta1InitMatrix); - Eigen::MatrixXd theta1DotInitMatrix(1,1); - theta1DotInitMatrix(0,0) = this->theta1DotInit; + Eigen::MatrixXd theta1DotInitMatrix(1, 1); + theta1DotInitMatrix(0, 0) = this->theta1DotInit; this->theta1DotState->setState(theta1DotInitMatrix); - Eigen::MatrixXd theta2InitMatrix(1,1); - theta2InitMatrix(0,0) = this->theta2Init; + Eigen::MatrixXd theta2InitMatrix(1, 1); + theta2InitMatrix(0, 0) = this->theta2Init; this->theta2State->setState(theta2InitMatrix); - Eigen::MatrixXd theta2DotInitMatrix(1,1); - theta2DotInitMatrix(0,0) = this->theta2DotInit; + Eigen::MatrixXd theta2DotInitMatrix(1, 1); + theta2DotInitMatrix(0, 0) = this->theta2DotInit; this->theta2DotState->setState(theta2DotInitMatrix); return; } -void DualHingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) +void +DualHingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) { // - Convert intial variables to mother craft frame relative information - this->r_H1P_P = this->r_BP_P + this->dcm_BP.transpose()*this->r_H1B_B; - this->dcm_H1P = this->dcm_H1B*this->dcm_BP; + this->r_H1P_P = this->r_BP_P + this->dcm_BP.transpose() * this->r_H1B_B; + this->dcm_H1P = this->dcm_H1B * this->dcm_BP; // - Give the mass of the hinged rigid body to the effProps mass this->effProps.mEff = this->mass1 + this->mass2; @@ -164,11 +168,11 @@ void DualHingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) // - Next find the sHat unit vectors Eigen::Matrix3d dcmS1H1; dcmS1H1 = eigenM2(this->theta1); - this->dcm_S1P = dcmS1H1*this->dcm_H1P; + this->dcm_S1P = dcmS1H1 * this->dcm_H1P; Eigen::Matrix3d dcmH2S1; dcmH2S1 = eigenM2(this->thetaH2S1); Eigen::Matrix3d dcmH2P; - dcmH2P = dcmH2S1*this->dcm_S1P; + dcmH2P = dcmH2S1 * this->dcm_S1P; Eigen::Matrix3d dcmS2H2; dcmS2H2 = eigenM2(this->theta2); this->dcm_S2P = dcmS2H2 * dcmH2P; @@ -178,43 +182,59 @@ void DualHingedRigidBodyStateEffector::updateEffectorMassProps(double integTime) this->sHat21_P = this->dcm_S2P.row(0); this->sHat22_P = this->dcm_S2P.row(1); this->sHat23_P = this->dcm_S2P.row(2); - this->r_S1P_P = this->r_H1P_P - this->d1*this->sHat11_P; - this->r_S2P_P = this->r_H1P_P - this->l1*this->sHat11_P - this->d2*this->sHat21_P; - this->effProps.rEff_CB_B = 1.0/this->effProps.mEff*(this->mass1*this->r_S1P_P + this->mass2*this->r_S2P_P); + this->r_S1P_P = this->r_H1P_P - this->d1 * this->sHat11_P; + this->r_S2P_P = this->r_H1P_P - this->l1 * this->sHat11_P - this->d2 * this->sHat21_P; + this->effProps.rEff_CB_B = 1.0 / this->effProps.mEff * (this->mass1 * this->r_S1P_P + this->mass2 * this->r_S2P_P); // - Find the inertia of the hinged rigid body about point B // - Define rTildeSB_B this->rTildeS1P_P = eigenTilde(this->r_S1P_P); this->rTildeS2P_P = eigenTilde(this->r_S2P_P); - this->effProps.IEffPntB_B = this->dcm_S1P.transpose()*this->IPntS1_S1*this->dcm_S1P + this->mass1*this->rTildeS1P_P*this->rTildeS1P_P.transpose() + this->dcm_S2P.transpose()*this->IPntS2_S2*this->dcm_S2P + this->mass2*this->rTildeS2P_P*this->rTildeS2P_P.transpose(); + this->effProps.IEffPntB_B = this->dcm_S1P.transpose() * this->IPntS1_S1 * this->dcm_S1P + + this->mass1 * this->rTildeS1P_P * this->rTildeS1P_P.transpose() + + this->dcm_S2P.transpose() * this->IPntS2_S2 * this->dcm_S2P + + this->mass2 * this->rTildeS2P_P * this->rTildeS2P_P.transpose(); // First, find the rPrimeSB_B - this->rPrimeS1P_P = this->d1*this->theta1Dot*this->sHat13_P; - this->rPrimeS2P_P = this->l1*this->theta1Dot*this->sHat13_P + this->d2*(this->theta1Dot + this->theta2Dot)*this->sHat23_P; - this->effProps.rEffPrime_CB_B = 1.0/this->effProps.mEff*(this->mass1*this->rPrimeS1P_P + this->mass2*this->rPrimeS2P_P); + this->rPrimeS1P_P = this->d1 * this->theta1Dot * this->sHat13_P; + this->rPrimeS2P_P = + this->l1 * this->theta1Dot * this->sHat13_P + this->d2 * (this->theta1Dot + this->theta2Dot) * this->sHat23_P; + this->effProps.rEffPrime_CB_B = + 1.0 / this->effProps.mEff * (this->mass1 * this->rPrimeS1P_P + this->mass2 * this->rPrimeS2P_P); // - Next find the body time derivative of the inertia about point B // - Define tilde matrix of rPrimeSB_B this->rPrimeTildeS1P_P = eigenTilde(this->rPrimeS1P_P); this->rPrimeTildeS2P_P = eigenTilde(this->rPrimeS2P_P); // - Find body time derivative of IPntS_B - this->IS1PrimePntS1_P = this->theta1Dot*(this->IPntS1_S1(2,2) - this->IPntS1_S1(0,0))*(this->sHat11_P*this->sHat13_P.transpose() + this->sHat13_P*this->sHat11_P.transpose()); - this->IS2PrimePntS2_P = (this->theta1Dot+this->theta2Dot)*(this->IPntS2_S2(2,2) - this->IPntS2_S2(0,0))*(this->sHat21_P*this->sHat23_P.transpose() + this->sHat23_P*this->sHat21_P.transpose()); + this->IS1PrimePntS1_P = this->theta1Dot * (this->IPntS1_S1(2, 2) - this->IPntS1_S1(0, 0)) * + (this->sHat11_P * this->sHat13_P.transpose() + this->sHat13_P * this->sHat11_P.transpose()); + this->IS2PrimePntS2_P = (this->theta1Dot + this->theta2Dot) * (this->IPntS2_S2(2, 2) - this->IPntS2_S2(0, 0)) * + (this->sHat21_P * this->sHat23_P.transpose() + this->sHat23_P * this->sHat21_P.transpose()); // - Find body time derivative of IPntB_B - this->effProps.IEffPrimePntB_B = this->IS1PrimePntS1_P - this->mass1*(this->rPrimeTildeS1P_P*this->rTildeS1P_P + this->rTildeS1P_P*this->rPrimeTildeS1P_P) + this->IS2PrimePntS2_P - this->mass2*(this->rPrimeTildeS2P_P*this->rTildeS2P_P + this->rTildeS2P_P*this->rPrimeTildeS2P_P); + this->effProps.IEffPrimePntB_B = + this->IS1PrimePntS1_P - + this->mass1 * (this->rPrimeTildeS1P_P * this->rTildeS1P_P + this->rTildeS1P_P * this->rPrimeTildeS1P_P) + + this->IS2PrimePntS2_P - + this->mass2 * (this->rPrimeTildeS2P_P * this->rTildeS2P_P + this->rTildeS2P_P * this->rPrimeTildeS2P_P); return; } -void DualHingedRigidBodyStateEffector::updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) +void +DualHingedRigidBodyStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { Eigen::MRPd sigmaPNLocal; - Eigen::Matrix3d dcmPN; /* direction cosine matrix from N to B */ - Eigen::Matrix3d dcmNP; /* direction cosine matrix from B to N */ - Eigen::Vector3d gravityTorquePntH1_P; /* torque of gravity on HRB about Pnt H */ - Eigen::Vector3d gravityTorquePntH2_P; /* torque of gravity on HRB about Pnt H */ - Eigen::Vector3d gLocal_N; /* gravitational acceleration in N frame */ - Eigen::Vector3d g_P; /* gravitational acceleration in B frame */ + Eigen::Matrix3d dcmPN; /* direction cosine matrix from N to B */ + Eigen::Matrix3d dcmNP; /* direction cosine matrix from B to N */ + Eigen::Vector3d gravityTorquePntH1_P; /* torque of gravity on HRB about Pnt H */ + Eigen::Vector3d gravityTorquePntH2_P; /* torque of gravity on HRB about Pnt H */ + Eigen::Vector3d gLocal_N; /* gravitational acceleration in N frame */ + Eigen::Vector3d g_P; /* gravitational acceleration in B frame */ gLocal_N = *this->g_N; // - Find dcmBN @@ -223,76 +243,134 @@ void DualHingedRigidBodyStateEffector::updateContributions(double integTime, Bac dcmNP = sigmaPNLocal.toRotationMatrix(); dcmPN = dcmNP.transpose(); // - Map gravity to body frame - g_P = dcmPN*gLocal_N; + g_P = dcmPN * gLocal_N; // - Define gravity terms - Eigen::Vector3d gravTorquePan1PntH1 = -this->d1*this->sHat11_P.cross(this->mass1*g_P); - Eigen::Vector3d gravForcePan2 = this->mass2*g_P; - Eigen::Vector3d gravTorquePan2PntH2 = -this->d2*this->sHat21_P.cross(this->mass2*g_P); + Eigen::Vector3d gravTorquePan1PntH1 = -this->d1 * this->sHat11_P.cross(this->mass1 * g_P); + Eigen::Vector3d gravForcePan2 = this->mass2 * g_P; + Eigen::Vector3d gravTorquePan2PntH2 = -this->d2 * this->sHat21_P.cross(this->mass2 * g_P); // - Define omegaBN_S this->omega_BN_B = omega_BN_B; this->omega_PNLoc_P = this->omega_BN_B; - this->omega_PN_S1 = this->dcm_S1P*this->omega_PNLoc_P; - this->omega_PN_S2 = this->dcm_S2P*this->omega_PNLoc_P; + this->omega_PN_S1 = this->dcm_S1P * this->omega_PNLoc_P; + this->omega_PN_S2 = this->dcm_S2P * this->omega_PNLoc_P; // - Define omegaTildeBNLoc_B this->omegaTildePNLoc_P = eigenTilde(this->omega_PNLoc_P); // - Define matrices needed for back substitution - //gravityTorquePntH1_B = -this->d1*this->sHat11_B.cross(this->mass1*g_B); //Need to review these equations and implement them - SJKC - //gravityTorquePntH2_B = -this->d2*this->sHat21_B.cross(this->mass2*g_B); //Need to review these equations and implement them - SJKC - this->matrixADHRB(0,0) = this->IPntS1_S1(1,1) + this->mass1*this->d1*this->d1 + this->mass2*this->l1*this->l1 + this->mass2*this->l1*this->d2*this->sHat13_P.transpose()*(this->sHat23_P); - this->matrixADHRB(0,1) = this->mass2*this->l1*this->d2*this->sHat13_P.transpose()*(this->sHat23_P); - this->matrixADHRB(1,0) = IPntS2_S2(1,1) + this->mass2*this->d2*this->d2 + this->mass2*this->l1*this->d2*this->sHat23_P.transpose()*this->sHat13_P; - this->matrixADHRB(1,1) = this->IPntS2_S2(1,1) + this->mass2*this->d2*this->d2; + // gravityTorquePntH1_B = -this->d1*this->sHat11_B.cross(this->mass1*g_B); //Need to review these equations and + // implement them - SJKC gravityTorquePntH2_B = -this->d2*this->sHat21_B.cross(this->mass2*g_B); //Need to review + // these equations and implement them - SJKC + this->matrixADHRB(0, 0) = this->IPntS1_S1(1, 1) + this->mass1 * this->d1 * this->d1 + + this->mass2 * this->l1 * this->l1 + + this->mass2 * this->l1 * this->d2 * this->sHat13_P.transpose() * (this->sHat23_P); + this->matrixADHRB(0, 1) = this->mass2 * this->l1 * this->d2 * this->sHat13_P.transpose() * (this->sHat23_P); + this->matrixADHRB(1, 0) = IPntS2_S2(1, 1) + this->mass2 * this->d2 * this->d2 + + this->mass2 * this->l1 * this->d2 * this->sHat23_P.transpose() * this->sHat13_P; + this->matrixADHRB(1, 1) = this->IPntS2_S2(1, 1) + this->mass2 * this->d2 * this->d2; this->matrixEDHRB = this->matrixADHRB.inverse(); - this->matrixFDHRB.row(0) = -(this->mass2*this->l1 + this->mass1*this->d1)*this->sHat13_P.transpose(); - this->matrixFDHRB.row(1) = -this->mass2*this->d2*this->sHat23_P.transpose(); - - this->matrixGDHRB.row(0) = -(this->IPntS1_S1(1,1)*this->sHat12_P.transpose() - this->mass1*this->d1*this->sHat13_P.transpose()*this->rTildeS1P_P - this->mass2*this->l1*this->sHat13_P.transpose()*this->rTildeS2P_P); - this->matrixGDHRB.row(1) = -(this->IPntS2_S2(1,1)*this->sHat22_P.transpose() - this->mass2*this->d2*this->sHat23_P.transpose()*this->rTildeS2P_P); - - this->vectorVDHRB(0) = -(this->IPntS1_S1(0,0) - this->IPntS1_S1(2,2))*this->omega_PN_S1(2)*this->omega_PN_S1(0) - + this->u1 - this->k1*this->theta1 - this->c1*this->theta1Dot + this->k2*this->theta2 + this->c2*this->theta2Dot + this->sHat12_P.dot(gravTorquePan1PntH1) + this->l1*this->sHat13_P.dot(gravForcePan2) - - this->mass1*this->d1*this->sHat13_P.transpose()*(2*this->omegaTildePNLoc_P*this->rPrimeS1P_P + this->omegaTildePNLoc_P*this->omegaTildePNLoc_P*this->r_S1P_P) - - this->mass2*this->l1*this->sHat13_P.transpose()*(2*this->omegaTildePNLoc_P*this->rPrimeS2P_P + this->omegaTildePNLoc_P*this->omegaTildePNLoc_P*this->r_S2P_P + this->l1*this->theta1Dot*this->theta1Dot*this->sHat11_P + this->d2*(this->theta1Dot + this->theta2Dot)*(this->theta1Dot + this->theta2Dot)*this->sHat21_P); //still missing torque and force terms - SJKC - - this->vectorVDHRB(1) = -(this->IPntS2_S2(0,0) - this->IPntS2_S2(2,2))*this->omega_PN_S2(2)*this->omega_PN_S2(0) - + this->u2 - this->k2*this->theta2 - this->c2*this->theta2Dot + this->sHat22_P.dot(gravTorquePan2PntH2) - this->mass2*this->d2*this->sHat23_P.transpose()*(2*this->omegaTildePNLoc_P*this->rPrimeS2P_P + this->omegaTildePNLoc_P*this->omegaTildePNLoc_P*this->r_S2P_P + this->l1*this->theta1Dot*this->theta1Dot*this->sHat11_P); // still missing torque term. - SJKC + this->matrixFDHRB.row(0) = -(this->mass2 * this->l1 + this->mass1 * this->d1) * this->sHat13_P.transpose(); + this->matrixFDHRB.row(1) = -this->mass2 * this->d2 * this->sHat23_P.transpose(); + + this->matrixGDHRB.row(0) = -(this->IPntS1_S1(1, 1) * this->sHat12_P.transpose() - + this->mass1 * this->d1 * this->sHat13_P.transpose() * this->rTildeS1P_P - + this->mass2 * this->l1 * this->sHat13_P.transpose() * this->rTildeS2P_P); + this->matrixGDHRB.row(1) = -(this->IPntS2_S2(1, 1) * this->sHat22_P.transpose() - + this->mass2 * this->d2 * this->sHat23_P.transpose() * this->rTildeS2P_P); + + this->vectorVDHRB(0) = + -(this->IPntS1_S1(0, 0) - this->IPntS1_S1(2, 2)) * this->omega_PN_S1(2) * this->omega_PN_S1(0) + this->u1 - + this->k1 * this->theta1 - this->c1 * this->theta1Dot + this->k2 * this->theta2 + this->c2 * this->theta2Dot + + this->sHat12_P.dot(gravTorquePan1PntH1) + this->l1 * this->sHat13_P.dot(gravForcePan2) - + this->mass1 * this->d1 * this->sHat13_P.transpose() * + (2 * this->omegaTildePNLoc_P * this->rPrimeS1P_P + + this->omegaTildePNLoc_P * this->omegaTildePNLoc_P * this->r_S1P_P) - + this->mass2 * this->l1 * this->sHat13_P.transpose() * + (2 * this->omegaTildePNLoc_P * this->rPrimeS2P_P + + this->omegaTildePNLoc_P * this->omegaTildePNLoc_P * this->r_S2P_P + + this->l1 * this->theta1Dot * this->theta1Dot * this->sHat11_P + + this->d2 * (this->theta1Dot + this->theta2Dot) * (this->theta1Dot + this->theta2Dot) * + this->sHat21_P); // still missing torque and force terms - SJKC + + this->vectorVDHRB(1) = + -(this->IPntS2_S2(0, 0) - this->IPntS2_S2(2, 2)) * this->omega_PN_S2(2) * this->omega_PN_S2(0) + this->u2 - + this->k2 * this->theta2 - this->c2 * this->theta2Dot + this->sHat22_P.dot(gravTorquePan2PntH2) - + this->mass2 * this->d2 * this->sHat23_P.transpose() * + (2 * this->omegaTildePNLoc_P * this->rPrimeS2P_P + + this->omegaTildePNLoc_P * this->omegaTildePNLoc_P * this->r_S2P_P + + this->l1 * this->theta1Dot * this->theta1Dot * this->sHat11_P); // still missing torque term. - SJKC // - Start defining them good old contributions - start with translation // - For documentation on contributions see Allard, Diaz, Schaub flex/slosh paper - backSubContr.matrixA = (this->mass1*this->d1*this->sHat13_P + this->mass2*this->l1*this->sHat13_P + this->mass2*this->d2*this->sHat23_P)*matrixEDHRB.row(0)*this->matrixFDHRB + this->mass2*this->d2*this->sHat23_P*this->matrixEDHRB.row(1)*this->matrixFDHRB; - backSubContr.matrixB = (this->mass1*this->d1*this->sHat13_P + this->mass2*this->l1*this->sHat13_P + this->mass2*this->d2*this->sHat23_P)*this->matrixEDHRB.row(0)*(matrixGDHRB) + this->mass2*this->d2*this->sHat23_P*this->matrixEDHRB.row(1)*(matrixGDHRB); - backSubContr.vecTrans = -(this->mass1*this->d1*this->theta1Dot*this->theta1Dot*this->sHat11_P + this->mass2*(this->l1*this->theta1Dot*this->theta1Dot*this->sHat11_P + this->d2*(this->theta1Dot+this->theta2Dot)*(this->theta1Dot+this->theta2Dot)*this->sHat21_P) - + (this->mass1*this->d1*this->sHat13_P + this->mass2*this->l1*this->sHat13_P + this->mass2*this->d2*this->sHat23_P)*this->matrixEDHRB.row(0)*this->vectorVDHRB + this->mass2*this->d2*this->sHat23_P*this->matrixEDHRB.row(1)*this->vectorVDHRB); + backSubContr.matrixA = (this->mass1 * this->d1 * this->sHat13_P + this->mass2 * this->l1 * this->sHat13_P + + this->mass2 * this->d2 * this->sHat23_P) * + matrixEDHRB.row(0) * this->matrixFDHRB + + this->mass2 * this->d2 * this->sHat23_P * this->matrixEDHRB.row(1) * this->matrixFDHRB; + backSubContr.matrixB = (this->mass1 * this->d1 * this->sHat13_P + this->mass2 * this->l1 * this->sHat13_P + + this->mass2 * this->d2 * this->sHat23_P) * + this->matrixEDHRB.row(0) * (matrixGDHRB) + + this->mass2 * this->d2 * this->sHat23_P * this->matrixEDHRB.row(1) * (matrixGDHRB); + backSubContr.vecTrans = -(this->mass1 * this->d1 * this->theta1Dot * this->theta1Dot * this->sHat11_P + + this->mass2 * (this->l1 * this->theta1Dot * this->theta1Dot * this->sHat11_P + + this->d2 * (this->theta1Dot + this->theta2Dot) * + (this->theta1Dot + this->theta2Dot) * this->sHat21_P) + + (this->mass1 * this->d1 * this->sHat13_P + this->mass2 * this->l1 * this->sHat13_P + + this->mass2 * this->d2 * this->sHat23_P) * + this->matrixEDHRB.row(0) * this->vectorVDHRB + + this->mass2 * this->d2 * this->sHat23_P * this->matrixEDHRB.row(1) * this->vectorVDHRB); // - Define rotational matrice contributions (Eq 96 in paper) - backSubContr.matrixC = (this->IPntS1_S1(1,1)*this->sHat12_P + this->mass1*this->d1*this->rTildeS1P_P*this->sHat13_P + this->IPntS2_S2(1,1)*this->sHat22_P + this->mass2*this->l1*this->rTildeS2P_P*this->sHat13_P + this->mass2*this->d2*this->rTildeS2P_P*this->sHat23_P)*this->matrixEDHRB.row(0)*this->matrixFDHRB - + (this->IPntS2_S2(1,1)*this->sHat22_P + this->mass2*this->d2*this->rTildeS2P_P*this->sHat23_P)*this->matrixEDHRB.row(1)*this->matrixFDHRB; - - backSubContr.matrixD = (this->IPntS1_S1(1,1)*this->sHat12_P + this->mass1*this->d1*this->rTildeS1P_P*this->sHat13_P + this->IPntS2_S2(1,1)*this->sHat22_P + this->mass2*this->l1*this->rTildeS2P_P*this->sHat13_P + this->mass2*this->d2*this->rTildeS2P_P*this->sHat23_P)*this->matrixEDHRB.row(0)*this->matrixGDHRB - +(this->IPntS2_S2(1,1)*this->sHat22_P + this->mass2*this->d2*this->rTildeS2P_P*this->sHat23_P)*this->matrixEDHRB.row(1)*this->matrixGDHRB; - - backSubContr.vecRot = -(this->theta1Dot*this->IPntS1_S1(1,1)*this->omegaTildePNLoc_P*this->sHat12_P - + this->mass1*this->omegaTildePNLoc_P*this->rTildeS1P_P*this->rPrimeS1P_P + this->mass1*this->d1*this->theta1Dot*this->theta1Dot*this->rTildeS1P_P*this->sHat11_P + (this->theta1Dot+this->theta2Dot)*this->IPntS2_S2(1,1)*this->omegaTildePNLoc_P*this->sHat22_P + this->mass2*this->omegaTildePNLoc_P*this->rTildeS2P_P*this->rPrimeS2P_P - + this->mass2*this->rTildeS2P_P*(this->l1*this->theta1Dot*this->theta1Dot*this->sHat11_P + this->d2*(this->theta1Dot+this->theta2Dot)*(this->theta1Dot+this->theta2Dot)*this->sHat21_P) + (this->IPntS1_S1(1,1)*this->sHat12_P + this->mass1*this->d1*this->rTildeS1P_P*this->sHat13_P + this->IPntS2_S2(1,1)*this->sHat22_P - + this->mass2*this->l1*this->rTildeS2P_P*this->sHat13_P + this->mass2*this->d2*this->rTildeS2P_P*this->sHat23_P)*this->matrixEDHRB.row(0)*this->vectorVDHRB + (this->IPntS2_S2(1,1)*this->sHat22_P + this->mass2*this->d2*this->rTildeS2P_P*this->sHat23_P)*this->matrixEDHRB.row(1)*this->vectorVDHRB); + backSubContr.matrixC = + (this->IPntS1_S1(1, 1) * this->sHat12_P + this->mass1 * this->d1 * this->rTildeS1P_P * this->sHat13_P + + this->IPntS2_S2(1, 1) * this->sHat22_P + this->mass2 * this->l1 * this->rTildeS2P_P * this->sHat13_P + + this->mass2 * this->d2 * this->rTildeS2P_P * this->sHat23_P) * + this->matrixEDHRB.row(0) * this->matrixFDHRB + + (this->IPntS2_S2(1, 1) * this->sHat22_P + this->mass2 * this->d2 * this->rTildeS2P_P * this->sHat23_P) * + this->matrixEDHRB.row(1) * this->matrixFDHRB; + + backSubContr.matrixD = + (this->IPntS1_S1(1, 1) * this->sHat12_P + this->mass1 * this->d1 * this->rTildeS1P_P * this->sHat13_P + + this->IPntS2_S2(1, 1) * this->sHat22_P + this->mass2 * this->l1 * this->rTildeS2P_P * this->sHat13_P + + this->mass2 * this->d2 * this->rTildeS2P_P * this->sHat23_P) * + this->matrixEDHRB.row(0) * this->matrixGDHRB + + (this->IPntS2_S2(1, 1) * this->sHat22_P + this->mass2 * this->d2 * this->rTildeS2P_P * this->sHat23_P) * + this->matrixEDHRB.row(1) * this->matrixGDHRB; + + backSubContr.vecRot = + -(this->theta1Dot * this->IPntS1_S1(1, 1) * this->omegaTildePNLoc_P * this->sHat12_P + + this->mass1 * this->omegaTildePNLoc_P * this->rTildeS1P_P * this->rPrimeS1P_P + + this->mass1 * this->d1 * this->theta1Dot * this->theta1Dot * this->rTildeS1P_P * this->sHat11_P + + (this->theta1Dot + this->theta2Dot) * this->IPntS2_S2(1, 1) * this->omegaTildePNLoc_P * this->sHat22_P + + this->mass2 * this->omegaTildePNLoc_P * this->rTildeS2P_P * this->rPrimeS2P_P + + this->mass2 * this->rTildeS2P_P * + (this->l1 * this->theta1Dot * this->theta1Dot * this->sHat11_P + + this->d2 * (this->theta1Dot + this->theta2Dot) * (this->theta1Dot + this->theta2Dot) * this->sHat21_P) + + (this->IPntS1_S1(1, 1) * this->sHat12_P + this->mass1 * this->d1 * this->rTildeS1P_P * this->sHat13_P + + this->IPntS2_S2(1, 1) * this->sHat22_P + this->mass2 * this->l1 * this->rTildeS2P_P * this->sHat13_P + + this->mass2 * this->d2 * this->rTildeS2P_P * this->sHat23_P) * + this->matrixEDHRB.row(0) * this->vectorVDHRB + + (this->IPntS2_S2(1, 1) * this->sHat22_P + this->mass2 * this->d2 * this->rTildeS2P_P * this->sHat23_P) * + this->matrixEDHRB.row(1) * this->vectorVDHRB); return; } -void DualHingedRigidBodyStateEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +DualHingedRigidBodyStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { // - Define necessarry variables Eigen::MRPd sigmaBNLocal; - Eigen::Matrix3d dcmBN; /* direction cosine matrix from N to B */ - Eigen::Matrix3d dcmNB; /* direction cosine matrix from B to N */ - Eigen::MatrixXd theta1DDot(1,1); /* thetaDDot variable to send to state manager */ - Eigen::MatrixXd theta2DDot(1,1); /* thetaDDot variable to send to state manager */ - Eigen::Vector3d rDDotBNLoc_N; /* second time derivative of rBN in N frame */ - Eigen::Vector3d rDDotBNLoc_B; /* second time derivative of rBN in B frame */ - Eigen::Vector3d omegaDotBNLoc_B; /* time derivative of omegaBN in B frame */ + Eigen::Matrix3d dcmBN; /* direction cosine matrix from N to B */ + Eigen::Matrix3d dcmNB; /* direction cosine matrix from B to N */ + Eigen::MatrixXd theta1DDot(1, 1); /* thetaDDot variable to send to state manager */ + Eigen::MatrixXd theta2DDot(1, 1); /* thetaDDot variable to send to state manager */ + Eigen::Vector3d rDDotBNLoc_N; /* second time derivative of rBN in N frame */ + Eigen::Vector3d rDDotBNLoc_B; /* second time derivative of rBN in B frame */ + Eigen::Vector3d omegaDotBNLoc_B; /* time derivative of omegaBN in B frame */ // Grab necessarry values from manager (these have been previously computed in hubEffector) rDDotBNLoc_N = this->v_BN_NState->getStateDeriv(); @@ -301,22 +379,30 @@ void DualHingedRigidBodyStateEffector::computeDerivatives(double integTime, Eige omegaDotBNLoc_B = omegaDot_BN_B; dcmNB = sigmaBNLocal.toRotationMatrix(); dcmBN = dcmNB.transpose(); - rDDotBNLoc_B = dcmBN*rDDotBNLoc_N; + rDDotBNLoc_B = dcmBN * rDDotBNLoc_N; // - Compute Derivatives // - First is trivial this->theta1State->setDerivative(theta1DotState->getState()); // - Second, a little more involved - see Allard, Diaz, Schaub flex/slosh paper - theta1DDot(0,0) = this->matrixEDHRB.row(0).dot(this->matrixFDHRB*rDDotBNLoc_B) + this->matrixEDHRB.row(0)*this->matrixGDHRB*omegaDotBNLoc_B + this->matrixEDHRB.row(0)*this->vectorVDHRB; + theta1DDot(0, 0) = this->matrixEDHRB.row(0).dot(this->matrixFDHRB * rDDotBNLoc_B) + + this->matrixEDHRB.row(0) * this->matrixGDHRB * omegaDotBNLoc_B + + this->matrixEDHRB.row(0) * this->vectorVDHRB; this->theta1DotState->setDerivative(theta1DDot); this->theta2State->setDerivative(theta2DotState->getState()); - theta2DDot(0,0) = this->matrixEDHRB.row(1)*(this->matrixFDHRB*rDDotBNLoc_B) + this->matrixEDHRB.row(1).dot(this->matrixGDHRB*omegaDotBNLoc_B) + this->matrixEDHRB.row(1)*this->vectorVDHRB; + theta2DDot(0, 0) = this->matrixEDHRB.row(1) * (this->matrixFDHRB * rDDotBNLoc_B) + + this->matrixEDHRB.row(1).dot(this->matrixGDHRB * omegaDotBNLoc_B) + + this->matrixEDHRB.row(1) * this->vectorVDHRB; this->theta2DotState->setDerivative(theta2DDot); return; } /*! This method is for calculating the contributions of the DHRB state effector to the energy and momentum of the s/c */ -void DualHingedRigidBodyStateEffector::updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_P, double & rotEnergyContr, Eigen::Vector3d omega_BN_B) +void +DualHingedRigidBodyStateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_P, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { // - Get the current omega state Eigen::Vector3d omegaLocal_PN_P; @@ -332,26 +418,24 @@ void DualHingedRigidBodyStateEffector::updateEnergyMomContributions(double integ Eigen::Matrix3d IPntS2_P; Eigen::Vector3d rDot_S1P_P; Eigen::Vector3d rDot_S2P_P; - omega_S1P_P = this->theta1Dot*this->sHat12_P; - omega_S2P_P = (this->theta1Dot + this->theta2Dot)*this->sHat22_P; + omega_S1P_P = this->theta1Dot * this->sHat12_P; + omega_S2P_P = (this->theta1Dot + this->theta2Dot) * this->sHat22_P; omega_S1N_P = omega_S1P_P + omegaLocal_PN_P; omega_S2N_P = omega_S2P_P + omegaLocal_PN_P; - IPntS1_P = this->dcm_S1P.transpose()*this->IPntS1_S1*this->dcm_S1P; - IPntS2_P = this->dcm_S2P.transpose()*this->IPntS2_S2*this->dcm_S2P; + IPntS1_P = this->dcm_S1P.transpose() * this->IPntS1_S1 * this->dcm_S1P; + IPntS2_P = this->dcm_S2P.transpose() * this->IPntS2_S2 * this->dcm_S2P; rDot_S1P_P = this->rPrimeS1P_P + omegaLocal_PN_P.cross(this->r_S1P_P); rDot_S2P_P = this->rPrimeS2P_P + omegaLocal_PN_P.cross(this->r_S2P_P); - rotAngMomPntCContr_P = IPntS1_P*omega_S1N_P + this->mass1*this->r_S1P_P.cross(rDot_S1P_P) - + IPntS2_P*omega_S2N_P + this->mass2*this->r_S2P_P.cross(rDot_S2P_P); + rotAngMomPntCContr_P = IPntS1_P * omega_S1N_P + this->mass1 * this->r_S1P_P.cross(rDot_S1P_P) + + IPntS2_P * omega_S2N_P + this->mass2 * this->r_S2P_P.cross(rDot_S2P_P); // - Find rotational energy contribution from the hub double rotEnergyContrS1; double rotEnergyContrS2; - rotEnergyContrS1 = 0.5*omega_S1N_P.dot(IPntS1_P*omega_S1N_P) - + 0.5*this->mass1*rDot_S1P_P.dot(rDot_S1P_P) - + 0.5*this->k1*this->theta1*this->theta1; - rotEnergyContrS2 = 0.5*omega_S2N_P.dot(IPntS2_P*omega_S2N_P) - + 0.5*this->mass2*rDot_S2P_P.dot(rDot_S2P_P) - + 0.5*this->k2*this->theta2*this->theta2; + rotEnergyContrS1 = 0.5 * omega_S1N_P.dot(IPntS1_P * omega_S1N_P) + 0.5 * this->mass1 * rDot_S1P_P.dot(rDot_S1P_P) + + 0.5 * this->k1 * this->theta1 * this->theta1; + rotEnergyContrS2 = 0.5 * omega_S2N_P.dot(IPntS2_P * omega_S2N_P) + 0.5 * this->mass2 * rDot_S2P_P.dot(rDot_S2P_P) + + 0.5 * this->k2 * this->theta2 * this->theta2; rotEnergyContr = rotEnergyContrS1 + rotEnergyContrS2; return; @@ -362,10 +446,11 @@ void DualHingedRigidBodyStateEffector::updateEnergyMomContributions(double integ @param CurrentClock The current simulation time (used for time stamping) */ -void DualHingedRigidBodyStateEffector::writeOutputStateMessages(uint64_t CurrentClock) +void +DualHingedRigidBodyStateEffector::writeOutputStateMessages(uint64_t CurrentClock) { - HingedRigidBodyMsgPayload panelOutputStates; //!< instance of messaging system message struct + HingedRigidBodyMsgPayload panelOutputStates; //!< instance of messaging system message struct // panel 1 states panelOutputStates.theta = this->theta1; @@ -376,11 +461,10 @@ void DualHingedRigidBodyStateEffector::writeOutputStateMessages(uint64_t Current panelOutputStates.thetaDot = this->theta2Dot; this->dualHingedRigidBodyOutMsgs[1]->write(&panelOutputStates, this->moduleID, CurrentClock); - // write out the panel state config log message SCStatesMsgPayload configLogMsg; // Note, logging the hinge frame S is the body frame B of that object - for (int i=0; i<2; i++) { + for (int i = 0; i < 2; i++) { configLogMsg = this->dualHingedRigidBodyConfigLogOutMsgs[i]->zeroMsgPayload; eigenVector3d2CArray(this->r_SN_N[i], configLogMsg.r_BN_N); eigenVector3d2CArray(this->v_SN_N[i], configLogMsg.v_BN_N); @@ -390,12 +474,12 @@ void DualHingedRigidBodyStateEffector::writeOutputStateMessages(uint64_t Current } } - /*! This method is used so that the simulation will ask DHRB to update messages. @param CurrentSimNanos The current simulation time in nanoseconds */ -void DualHingedRigidBodyStateEffector::UpdateState(uint64_t CurrentSimNanos) +void +DualHingedRigidBodyStateEffector::UpdateState(uint64_t CurrentSimNanos) { //! - Zero the command buffer and read the incoming command array if (this->motorTorqueInMsg.isLinked()) { @@ -416,20 +500,21 @@ void DualHingedRigidBodyStateEffector::UpdateState(uint64_t CurrentSimNanos) /*! This method computes the panel states relative to the inertial frame */ -void DualHingedRigidBodyStateEffector::computePanelInertialStates() +void +DualHingedRigidBodyStateEffector::computePanelInertialStates() { // inertial attitudes Eigen::MRPd sigmaPN; sigmaPN = this->sigma_BN; Eigen::Matrix3d dcm_NP = sigmaPN.toRotationMatrix(); - this->sigma_SN[0] = eigenMRPd2Vector3d(eigenC2MRP(this->dcm_S1P*dcm_NP.transpose())); - this->sigma_SN[1] = eigenMRPd2Vector3d(eigenC2MRP(this->dcm_S2P*dcm_NP.transpose())); + this->sigma_SN[0] = eigenMRPd2Vector3d(eigenC2MRP(this->dcm_S1P * dcm_NP.transpose())); + this->sigma_SN[1] = eigenMRPd2Vector3d(eigenC2MRP(this->dcm_S2P * dcm_NP.transpose())); // inertial angular velocities Eigen::Vector3d omega_PN_P; omega_PN_P = this->omega_BN_B; - this->omega_SN_S[0] = this->dcm_S1P * ( omega_PN_P + this->theta1Dot*this->sHat12_P); - this->omega_SN_S[1] = this->dcm_S1P * ( omega_PN_P + this->theta2Dot*this->sHat22_P); + this->omega_SN_S[0] = this->dcm_S1P * (omega_PN_P + this->theta1Dot * this->sHat12_P); + this->omega_SN_S[1] = this->dcm_S1P * (omega_PN_P + this->theta2Dot * this->sHat22_P); // inertial position vectors Eigen::Vector3d r_PN_N; @@ -439,14 +524,12 @@ void DualHingedRigidBodyStateEffector::computePanelInertialStates() // inertial velocity vectors Eigen::Vector3d omega_S1N_P = this->theta1Dot * this->sHat12_P + omega_PN_P; - this->v_SN_N[0] = (Eigen::Vector3d)(*this->inertialVelocityProperty) - + omega_S1N_P.cross( -this->d1 * this->sHat11_P) - + omega_PN_P.cross(this->r_H1P_P); + this->v_SN_N[0] = (Eigen::Vector3d)(*this->inertialVelocityProperty) + + omega_S1N_P.cross(-this->d1 * this->sHat11_P) + omega_PN_P.cross(this->r_H1P_P); Eigen::Vector3d omega_S2N_P = this->theta2Dot * this->sHat22_P + omega_S1N_P; - this->v_SN_N[1] = (Eigen::Vector3d)(*this->inertialVelocityProperty) - + omega_S2N_P.cross( -this->d2 * this->sHat21_P) - + omega_S1N_P.cross( -this->l1 * this->sHat11_P) - + omega_PN_P.cross(this->r_H1P_P); + this->v_SN_N[1] = (Eigen::Vector3d)(*this->inertialVelocityProperty) + + omega_S2N_P.cross(-this->d2 * this->sHat21_P) + omega_S1N_P.cross(-this->l1 * this->sHat11_P) + + omega_PN_P.cross(this->r_H1P_P); return; } diff --git a/src/simulation/dynamics/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.h b/src/simulation/dynamics/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.h old mode 100755 new mode 100644 index af41fdc8c7..b2a8a62272 --- a/src/simulation/dynamics/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.h +++ b/src/simulation/dynamics/dualHingedRigidBodies/dualHingedRigidBodyStateEffector.h @@ -17,7 +17,6 @@ */ - #ifndef DUAL_HINGED_RIGID_BODY_STATE_EFFECTOR_H #define DUAL_HINGED_RIGID_BODY_STATE_EFFECTOR_H @@ -34,30 +33,39 @@ #include "architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h" #include "architecture/messaging/messaging.h" - - - /*! @brief dual hinged rigid body state effector */ -class DualHingedRigidBodyStateEffector : public StateEffector, public SysModel { -public: +class DualHingedRigidBodyStateEffector + : public StateEffector + , public SysModel +{ + public: DualHingedRigidBodyStateEffector(); ~DualHingedRigidBodyStateEffector(); - void registerStates(DynParamManager& statesIn); //!< class method - void linkInStates(DynParamManager& states); //!< class method - void updateEffectorMassProps(double integTime); //!< class method - void updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N); //!< -- Back-sub contributions - void updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B); //!< -- Energy and momentum calculations - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN); //!< -- Method for each stateEffector to calculate derivatives + void registerStates(DynParamManager& statesIn); //!< class method + void linkInStates(DynParamManager& states); //!< class method + void updateEffectorMassProps(double integTime); //!< class method + void updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N); //!< -- Back-sub contributions + void updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B); //!< -- Energy and momentum calculations + void computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN); //!< -- Method for each stateEffector to calculate derivatives void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); void writeOutputStateMessages(uint64_t CurrentClock); -private: + private: void computePanelInertialStates(); void prependSpacecraftNameToStates(); //!< class method -public: + public: double mass1; //!< [kg] mass of 1st hinged rigid body double mass2; //!< [kg] mass of 2nd hinged rigid body double d1; //!< [m] distance from hinge point H1 to hinged rigid body center of mass S1 @@ -80,65 +88,68 @@ class DualHingedRigidBodyStateEffector : public StateEffector, public SysModel { std::string nameOfTheta1DotState; //!< [-] Identifier for the thetaDot state data container std::string nameOfTheta2State; //!< [-] Identifier for the theta state data container std::string nameOfTheta2DotState; //!< [-] Identifier for the thetaDot state data container - BSKLogger bskLogger; //!< -- BSK Logging + BSKLogger bskLogger; //!< -- BSK Logging ReadFunctor motorTorqueInMsg; //!< -- (optional) motor torque input message - std::vector*> dualHingedRigidBodyOutMsgs; //!< -- state output message vector for all panels - std::vector*> dualHingedRigidBodyConfigLogOutMsgs; //!< panel state config log message vector for all panels - -private: - static uint64_t effectorID; //!< [] ID number of this panel - Eigen::Vector3d r_H1P_P; //!< [m] vector pointing from primary body frame P origin to Hinge 1 location. If a single spacecraft body is modeled than P is the same as B - Eigen::Matrix3d dcm_H1P; //!< -- DCM from primary body frame to hinge 1 frame - double u1; //!< [N-m] motor torques on panel 1 - double u2; //!< [N-m] motor torques on panel 2 - Eigen::Matrix3d rTildeH1B_B; //!< [-] Tilde matrix of rHB_B - Eigen::Matrix3d dcm_S1P; //!< [-] DCM from primary body to S1 frame - Eigen::Matrix3d dcm_S2P; //!< [-] DCM from primary body to S2 frame - Eigen::Vector3d omega_PN_S1; //!< [rad/s] omega_PN in S1 frame components - Eigen::Vector3d omega_PN_S2; //!< [rad/s] omega_PN in S2 frame components - Eigen::Vector3d sHat11_P; //!< [-] unit direction vector for the first axis of the S frame - Eigen::Vector3d sHat12_P; //!< [-] unit direction vector for the second axis of the S frame - Eigen::Vector3d sHat13_P; //!< [-] unit direction vector for the third axis of the S frame - Eigen::Vector3d sHat21_P; //!< [-] unit direction vector for the first axis of the S frame - Eigen::Vector3d sHat22_P; //!< [-] unit direction vector for the second axis of the S frame - Eigen::Vector3d sHat23_P; //!< [-] unit direction vector for the third axis of the S frame - Eigen::Vector3d r_S1P_P; //!< [-] Vector pointing from body origin to CoM of hinged rigid body in P frame comp - Eigen::Vector3d r_S2P_P; //!< [-] Vector pointing from body origin to CoM of hinged rigid body in P frame comp - Eigen::Matrix3d rTildeS1P_P; //!< [-] Tilde matrix of rSP_P - Eigen::Matrix3d rTildeS2P_P; //!< [-] Tilde matrix of rSP_P - Eigen::Vector3d rPrimeS1P_P; //!< [m/s] Body time derivative of rSP_P - Eigen::Vector3d rPrimeS2P_P; //!< [m/s] Body time derivative of rSBP_P - Eigen::Matrix3d rPrimeTildeS1P_P; //!< [-] Tilde matrix of rPrime_SP_P - Eigen::Matrix3d rPrimeTildeS2P_P; //!< [-] Tilde matrix of rPrime_SP_P - Eigen::Matrix3d IS1PrimePntS1_P; //!< [kg-m^2/s] time body derivative IPntS in primary body frame components - Eigen::Matrix3d IS2PrimePntS2_P; //!< [kg-m^2/s] time body derivative IPntS in primary body frame components - Eigen::Vector3d omega_PNLoc_P; //!< [rad/s] local copy of omegaPN - Eigen::Matrix3d omegaTildePNLoc_P;//!< [-] tilde matrix of omegaPN - double theta1; //!< [rad] hinged rigid body angle - double theta1Dot; //!< [rad/s] hinged rigid body angle rate - double theta2; //!< [rad] hinged rigid body angle - double theta2Dot; //!< [rad/s] hinged rigid body angle rate - Eigen::Matrix2d matrixADHRB; //!< [-] term needed for back substitution - Eigen::Matrix2d matrixEDHRB; //!< [-] term needed for back substitution + std::vector*> + dualHingedRigidBodyOutMsgs; //!< -- state output message vector for all panels + std::vector*> + dualHingedRigidBodyConfigLogOutMsgs; //!< panel state config log message vector for all panels + + private: + static uint64_t effectorID; //!< [] ID number of this panel + Eigen::Vector3d r_H1P_P; //!< [m] vector pointing from primary body frame P origin to Hinge 1 location. If a single + //!< spacecraft body is modeled than P is the same as B + Eigen::Matrix3d dcm_H1P; //!< -- DCM from primary body frame to hinge 1 frame + double u1; //!< [N-m] motor torques on panel 1 + double u2; //!< [N-m] motor torques on panel 2 + Eigen::Matrix3d rTildeH1B_B; //!< [-] Tilde matrix of rHB_B + Eigen::Matrix3d dcm_S1P; //!< [-] DCM from primary body to S1 frame + Eigen::Matrix3d dcm_S2P; //!< [-] DCM from primary body to S2 frame + Eigen::Vector3d omega_PN_S1; //!< [rad/s] omega_PN in S1 frame components + Eigen::Vector3d omega_PN_S2; //!< [rad/s] omega_PN in S2 frame components + Eigen::Vector3d sHat11_P; //!< [-] unit direction vector for the first axis of the S frame + Eigen::Vector3d sHat12_P; //!< [-] unit direction vector for the second axis of the S frame + Eigen::Vector3d sHat13_P; //!< [-] unit direction vector for the third axis of the S frame + Eigen::Vector3d sHat21_P; //!< [-] unit direction vector for the first axis of the S frame + Eigen::Vector3d sHat22_P; //!< [-] unit direction vector for the second axis of the S frame + Eigen::Vector3d sHat23_P; //!< [-] unit direction vector for the third axis of the S frame + Eigen::Vector3d r_S1P_P; //!< [-] Vector pointing from body origin to CoM of hinged rigid body in P frame comp + Eigen::Vector3d r_S2P_P; //!< [-] Vector pointing from body origin to CoM of hinged rigid body in P frame comp + Eigen::Matrix3d rTildeS1P_P; //!< [-] Tilde matrix of rSP_P + Eigen::Matrix3d rTildeS2P_P; //!< [-] Tilde matrix of rSP_P + Eigen::Vector3d rPrimeS1P_P; //!< [m/s] Body time derivative of rSP_P + Eigen::Vector3d rPrimeS2P_P; //!< [m/s] Body time derivative of rSBP_P + Eigen::Matrix3d rPrimeTildeS1P_P; //!< [-] Tilde matrix of rPrime_SP_P + Eigen::Matrix3d rPrimeTildeS2P_P; //!< [-] Tilde matrix of rPrime_SP_P + Eigen::Matrix3d IS1PrimePntS1_P; //!< [kg-m^2/s] time body derivative IPntS in primary body frame components + Eigen::Matrix3d IS2PrimePntS2_P; //!< [kg-m^2/s] time body derivative IPntS in primary body frame components + Eigen::Vector3d omega_PNLoc_P; //!< [rad/s] local copy of omegaPN + Eigen::Matrix3d omegaTildePNLoc_P; //!< [-] tilde matrix of omegaPN + double theta1; //!< [rad] hinged rigid body angle + double theta1Dot; //!< [rad/s] hinged rigid body angle rate + double theta2; //!< [rad] hinged rigid body angle + double theta2Dot; //!< [rad/s] hinged rigid body angle rate + Eigen::Matrix2d matrixADHRB; //!< [-] term needed for back substitution + Eigen::Matrix2d matrixEDHRB; //!< [-] term needed for back substitution Eigen::MatrixXd matrixFDHRB; Eigen::MatrixXd matrixGDHRB; Eigen::Vector2d vectorVDHRB; - StateData *theta1State; //!< [-] state manager of theta for hinged rigid body - StateData *theta1DotState; //!< [-] state manager of thetaDot for hinged rigid body - StateData *theta2State; //!< [-] state manager of theta for hinged rigid body - StateData *theta2DotState; //!< [-] state manager of thetaDot for hinged rigid body - Eigen::Vector3d r_SN_N[2]; //!< [m] position vector of hinge CM S relative to inertial frame - Eigen::Vector3d v_SN_N[2]; //!< [m/s] inertial velocity vector of S relative to inertial frame - Eigen::Vector3d sigma_SN[2]; //!< -- MRP attitude of panel frame S relative to inertial frame - Eigen::Vector3d omega_SN_S[2]; //!< [rad/s] inertial panel frame angular velocity vector - Eigen::MRPd sigma_BN{0.0, 0.0, 0.0}; //!< Hub/Inertial attitude represented by MRP of body relative to inertial frame - Eigen::Vector3d omega_BN_B{0.0, 0.0, 0.0}; //!< Hub/Inertial angular velocity vector in B frame components - StateData *v_BN_NState; //!< Hub/Inertial velocity vector in inertial frame components - Eigen::MatrixXd *inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - Eigen::MatrixXd *inertialVelocityProperty; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase - Eigen::MatrixXd *g_N; //!< [m/s^2] Gravitational acceleration in N frame components - + StateData* theta1State; //!< [-] state manager of theta for hinged rigid body + StateData* theta1DotState; //!< [-] state manager of thetaDot for hinged rigid body + StateData* theta2State; //!< [-] state manager of theta for hinged rigid body + StateData* theta2DotState; //!< [-] state manager of thetaDot for hinged rigid body + Eigen::Vector3d r_SN_N[2]; //!< [m] position vector of hinge CM S relative to inertial frame + Eigen::Vector3d v_SN_N[2]; //!< [m/s] inertial velocity vector of S relative to inertial frame + Eigen::Vector3d sigma_SN[2]; //!< -- MRP attitude of panel frame S relative to inertial frame + Eigen::Vector3d omega_SN_S[2]; //!< [rad/s] inertial panel frame angular velocity vector + Eigen::MRPd sigma_BN{ 0.0, + 0.0, + 0.0 }; //!< Hub/Inertial attitude represented by MRP of body relative to inertial frame + Eigen::Vector3d omega_BN_B{ 0.0, 0.0, 0.0 }; //!< Hub/Inertial angular velocity vector in B frame components + StateData* v_BN_NState; //!< Hub/Inertial velocity vector in inertial frame components + Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialVelocityProperty; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase + Eigen::MatrixXd* g_N; //!< [m/s^2] Gravitational acceleration in N frame components }; - #endif /* DUAL_STATE_EFFECTOR_H */ 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/_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/extForceTorque/extForceTorque.cpp b/src/simulation/dynamics/extForceTorque/extForceTorque.cpp old mode 100755 new mode 100644 index 8a88764e38..f165538e16 --- a/src/simulation/dynamics/extForceTorque/extForceTorque.cpp +++ b/src/simulation/dynamics/extForceTorque/extForceTorque.cpp @@ -20,7 +20,6 @@ #include #include "architecture/utilities/avsEigenSupport.h" - /*! This is the constructor. It sets some default initializers that can be overriden by the user.*/ ExtForceTorque::ExtForceTorque() @@ -40,11 +39,11 @@ ExtForceTorque::~ExtForceTorque() return; } - /*! This method is used to reset the module. */ -void ExtForceTorque::Reset(uint64_t CurrentSimNanos) +void +ExtForceTorque::Reset(uint64_t CurrentSimNanos) { /* zero the input messages */ this->incomingCmdTorqueBuffer = this->cmdTorqueInMsg.zeroMsgPayload; @@ -52,38 +51,36 @@ void ExtForceTorque::Reset(uint64_t CurrentSimNanos) this->incomingCmdForceInertialBuffer = this->cmdForceInertialInMsg.zeroMsgPayload; } - -void ExtForceTorque::linkInStates(DynParamManager& statesIn) +void +ExtForceTorque::linkInStates(DynParamManager& statesIn) { - } - /*! This module does not write any output messages. @param currentClock The current time used for time-stamping the message */ -void ExtForceTorque::writeOutputMessages(uint64_t currentClock) +void +ExtForceTorque::writeOutputMessages(uint64_t currentClock) { - } /*! This method is used to read the incoming message and set the associated buffer structure. */ -void ExtForceTorque::readInputMessages() +void +ExtForceTorque::readInputMessages() { - if(this->cmdTorqueInMsg.isLinked()){ + if (this->cmdTorqueInMsg.isLinked()) { this->incomingCmdTorqueBuffer = this->cmdTorqueInMsg(); } - if(this->cmdForceBodyInMsg.isLinked()){ + if (this->cmdForceBodyInMsg.isLinked()) { this->incomingCmdForceBodyBuffer = this->cmdForceBodyInMsg(); } - if(this->cmdForceInertialInMsg.isLinked()){ + if (this->cmdForceInertialInMsg.isLinked()) { this->incomingCmdForceInertialBuffer = this->cmdForceInertialInMsg(); } - } /*! This method is used to compute the RHS forces and torques. @@ -91,38 +88,40 @@ void ExtForceTorque::readInputMessages() matrix representations in the body (B) and inerial (N) frame components are treated as 2 separate vectors. Only set both if you mean to, as both vectors will be included. */ -void ExtForceTorque::computeForceTorque(double integTime, double timeStep) +void +ExtForceTorque::computeForceTorque(double integTime, double timeStep) { - Eigen::Vector3d cmdVec; + Eigen::Vector3d cmdVec; /* add the cmd force in inertial frame components set via Python */ this->forceExternal_N = this->extForce_N; /* add the cmd force in inertial frame components set via FSW communication */ - if(this->cmdForceInertialInMsg.isLinked()){ - cmdVec = cArray2EigenVector3d(this->incomingCmdForceInertialBuffer.forceRequestInertial); - this->forceExternal_N += cmdVec; + if (this->cmdForceInertialInMsg.isLinked()) { + cmdVec = cArray2EigenVector3d(this->incomingCmdForceInertialBuffer.forceRequestInertial); + this->forceExternal_N += cmdVec; } /* add the cmd force in body frame components set via Python */ this->forceExternal_B = this->extForce_B; /* add the cmd force in body frame components set via FSW communication */ - if(this->cmdForceBodyInMsg.isLinked()){ - cmdVec = cArray2EigenVector3d(this->incomingCmdForceBodyBuffer.forceRequestBody); - this->forceExternal_B +=cmdVec; + if (this->cmdForceBodyInMsg.isLinked()) { + cmdVec = cArray2EigenVector3d(this->incomingCmdForceBodyBuffer.forceRequestBody); + this->forceExternal_B += cmdVec; } /* add the cmd torque about Point B in body frame components set via Python */ this->torqueExternalPntB_B = this->extTorquePntB_B; /* add the cmd torque about Point B in body frame components set via FSW communication */ - if(this->cmdTorqueInMsg.isLinked()){ - cmdVec = cArray2EigenVector3d(this->incomingCmdTorqueBuffer.torqueRequestBody); + if (this->cmdTorqueInMsg.isLinked()) { + cmdVec = cArray2EigenVector3d(this->incomingCmdTorqueBuffer.torqueRequestBody); this->torqueExternalPntB_B += cmdVec; } return; } -void ExtForceTorque::UpdateState(uint64_t CurrentSimNanos) +void +ExtForceTorque::UpdateState(uint64_t CurrentSimNanos) { this->readInputMessages(); } diff --git a/src/simulation/dynamics/extForceTorque/extForceTorque.h b/src/simulation/dynamics/extForceTorque/extForceTorque.h old mode 100755 new mode 100644 index faee26a968..b40d033ff0 --- a/src/simulation/dynamics/extForceTorque/extForceTorque.h +++ b/src/simulation/dynamics/extForceTorque/extForceTorque.h @@ -30,38 +30,36 @@ #include "architecture/utilities/bskLogging.h" - - /*! @brief external force and torque dynamic efector class */ -class ExtForceTorque: public SysModel, public DynamicEffector{ -public: +class ExtForceTorque + : public SysModel + , public DynamicEffector +{ + public: ExtForceTorque(); ~ExtForceTorque(); void Reset(uint64_t CurrentSimNanos); - void UpdateState(uint64_t CurrentSimNanos); //!< class method - void linkInStates(DynParamManager& statesIn); //!< class method - void writeOutputMessages(uint64_t currentClock); //!< class method + void UpdateState(uint64_t CurrentSimNanos); //!< class method + void linkInStates(DynParamManager& statesIn); //!< class method + void writeOutputMessages(uint64_t currentClock); //!< class method void readInputMessages(); void computeForceTorque(double integTime, double timeStep); -private: - CmdTorqueBodyMsgPayload incomingCmdTorqueBuffer; //!< -- One-time allocation for savings - CmdForceInertialMsgPayload incomingCmdForceInertialBuffer; //!< -- One-time allocation for savings - CmdForceBodyMsgPayload incomingCmdForceBodyBuffer; //!< -- One-time allocation for savings - + private: + CmdTorqueBodyMsgPayload incomingCmdTorqueBuffer; //!< -- One-time allocation for savings + CmdForceInertialMsgPayload incomingCmdForceInertialBuffer; //!< -- One-time allocation for savings + CmdForceBodyMsgPayload incomingCmdForceBodyBuffer; //!< -- One-time allocation for savings -public: - Eigen::Vector3d extForce_N; //!< [N] external force in inertial frame components - Eigen::Vector3d extForce_B; //!< [N] external force in body frame components - Eigen::Vector3d extTorquePntB_B; //!< [Nm] external torque in body frame components + public: + Eigen::Vector3d extForce_N; //!< [N] external force in inertial frame components + Eigen::Vector3d extForce_B; //!< [N] external force in body frame components + Eigen::Vector3d extTorquePntB_B; //!< [Nm] external torque in body frame components - BSKLogger bskLogger; //!< -- BSK Logging + BSKLogger bskLogger; //!< -- BSK Logging ReadFunctor cmdTorqueInMsg; //!< commanded torque input msg ReadFunctor cmdForceBodyInMsg; //!< commanded force input msg in B frame - ReadFunctorcmdForceInertialInMsg; //!< commanded force input msg in N frame - + ReadFunctor cmdForceInertialInMsg; //!< commanded force input msg in N frame }; - #endif 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/_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/facetDragEffector/facetDragDynamicEffector.cpp b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp index 588adcd5eb..2bed2b05d9 100644 --- a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp +++ b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp @@ -28,24 +28,23 @@ FacetDragDynamicEffector::FacetDragDynamicEffector() this->torqueExternalPntB_B.fill(0.0); this->v_B.fill(0.0); this->v_hat_B.fill(0.0); - this->numFacets = 0; - return; + this->numFacets = 0; + return; } /*! The destructor.*/ FacetDragDynamicEffector::~FacetDragDynamicEffector() { - return; + return; } - - -void FacetDragDynamicEffector::Reset(uint64_t CurrentSimNanos) +void +FacetDragDynamicEffector::Reset(uint64_t CurrentSimNanos) { - // check if input message has not been included - if (!this->atmoDensInMsg.isLinked()) { - bskLogger.bskLog(BSK_ERROR, "facetDragDynamicEffector.atmoDensInMsg was not linked."); - } + // check if input message has not been included + if (!this->atmoDensInMsg.isLinked()) { + bskLogger.bskLog(BSK_ERROR, "facetDragDynamicEffector.atmoDensInMsg was not linked."); + } return; } @@ -53,22 +52,23 @@ void FacetDragDynamicEffector::Reset(uint64_t CurrentSimNanos) /*! The DragEffector does not write output messages to the rest of the sim. */ -void FacetDragDynamicEffector::WriteOutputMessages(uint64_t CurrentClock) +void +FacetDragDynamicEffector::WriteOutputMessages(uint64_t CurrentClock) { - return; + return; } - /*! This method is used to read the incoming density message and update the internal density/ atmospheric data. */ -bool FacetDragDynamicEffector::ReadInputs() +bool +FacetDragDynamicEffector::ReadInputs() { bool dataGood; this->atmoInData = this->atmoDensInMsg(); dataGood = this->atmoDensInMsg.isWritten(); - return(dataGood); + return (dataGood); } /*! @@ -78,12 +78,17 @@ bool FacetDragDynamicEffector::ReadInputs() @param B_normal_hat @param B_location */ -void FacetDragDynamicEffector::addFacet(double area, double dragCoeff, Eigen::Vector3d B_normal_hat, Eigen::Vector3d B_location){ - this->scGeometry.facetAreas.push_back(area); - this->scGeometry.facetCoeffs.push_back(dragCoeff); - this->scGeometry.facetNormals_B.push_back(B_normal_hat); - this->scGeometry.facetLocations_B.push_back(B_location); - this->numFacets = this->numFacets + 1; +void +FacetDragDynamicEffector::addFacet(double area, + double dragCoeff, + Eigen::Vector3d B_normal_hat, + Eigen::Vector3d B_location) +{ + this->scGeometry.facetAreas.push_back(area); + this->scGeometry.facetCoeffs.push_back(dragCoeff); + this->scGeometry.facetNormals_B.push_back(B_normal_hat); + this->scGeometry.facetLocations_B.push_back(B_location); + this->numFacets = this->numFacets + 1; } /*! This method is used to link the dragEffector to the hub attitude and velocity, @@ -92,19 +97,23 @@ which are required for calculating drag forces and torques. @param states dynamic parameter states */ -void FacetDragDynamicEffector::linkInStates(DynParamManager& states){ - this->hubSigma = states.getStateObject(this->stateNameOfSigma); - this->hubVelocity = states.getStateObject(this->stateNameOfVelocity); +void +FacetDragDynamicEffector::linkInStates(DynParamManager& states) +{ + this->hubSigma = states.getStateObject(this->stateNameOfSigma); + this->hubVelocity = states.getStateObject(this->stateNameOfVelocity); } /*! This method updates the internal drag direction based on the spacecraft velocity vector. -*/ -void FacetDragDynamicEffector::updateDragDir(){ + */ +void +FacetDragDynamicEffector::updateDragDir() +{ Eigen::MRPd sigmaBN; sigmaBN = (Eigen::Vector3d)this->hubSigma->getState(); Eigen::Matrix3d dcm_BN = sigmaBN.toRotationMatrix().transpose(); - this->v_B = dcm_BN*this->hubVelocity->getState(); // [m/s] sc velocity + this->v_B = dcm_BN * this->hubVelocity->getState(); // [m/s] sc velocity this->v_hat_B = this->v_B / this->v_B.norm(); return; @@ -113,42 +122,46 @@ void FacetDragDynamicEffector::updateDragDir(){ /*! This method WILL implement a more complex flat-plate aerodynamics model with attitude dependence and lift forces. */ -void FacetDragDynamicEffector::plateDrag(){ - Eigen::Vector3d facetDragForce, facetDragTorque; - Eigen::Vector3d totalDragForce, totalDragTorque; +void +FacetDragDynamicEffector::plateDrag() +{ + Eigen::Vector3d facetDragForce, facetDragTorque; + Eigen::Vector3d totalDragForce, totalDragTorque; - //! - Zero out the structure force/torque for the drag set + //! - Zero out the structure force/torque for the drag set double projectedArea = 0.0; double projectionTerm = 0.0; - totalDragForce.setZero(); - totalDragTorque.setZero(); + totalDragForce.setZero(); + totalDragTorque.setZero(); this->forceExternal_B.setZero(); this->torqueExternalPntB_B.setZero(); - for(size_t i = 0; i < this->numFacets; i++){ - projectionTerm = this->scGeometry.facetNormals_B[i].dot(this->v_hat_B); - projectedArea = this->scGeometry.facetAreas[i] * projectionTerm; - if(projectedArea > 0.0){ - facetDragForce = 0.5 * pow(this->v_B.norm(), 2.0) * this->scGeometry.facetCoeffs[i] * projectedArea * this->atmoInData.neutralDensity * (-1.0)*this->v_hat_B; - facetDragTorque = (-1)*facetDragForce.cross(this->scGeometry.facetLocations_B[i]); - totalDragForce = totalDragForce + facetDragForce; - totalDragTorque = totalDragTorque + facetDragTorque; - } - } - this->forceExternal_B = totalDragForce; - this->torqueExternalPntB_B = totalDragTorque; - - return; -} + for (size_t i = 0; i < this->numFacets; i++) { + projectionTerm = this->scGeometry.facetNormals_B[i].dot(this->v_hat_B); + projectedArea = this->scGeometry.facetAreas[i] * projectionTerm; + if (projectedArea > 0.0) { + facetDragForce = 0.5 * pow(this->v_B.norm(), 2.0) * this->scGeometry.facetCoeffs[i] * projectedArea * + this->atmoInData.neutralDensity * (-1.0) * this->v_hat_B; + facetDragTorque = (-1) * facetDragForce.cross(this->scGeometry.facetLocations_B[i]); + totalDragForce = totalDragForce + facetDragForce; + totalDragTorque = totalDragTorque + facetDragTorque; + } + } + this->forceExternal_B = totalDragForce; + this->torqueExternalPntB_B = totalDragTorque; + return; +} /*! This method computes the body forces and torques for the dragEffector in a simulation loop, selecting the model type based on the settable attribute "modelType." */ -void FacetDragDynamicEffector::computeForceTorque(double integTime, double timeStep){ - updateDragDir(); - plateDrag(); - return; +void +FacetDragDynamicEffector::computeForceTorque(double integTime, double timeStep) +{ + updateDragDir(); + plateDrag(); + return; } /*! This method is called to update the local atmospheric conditions at each timestep. @@ -156,8 +169,9 @@ Naturally, this means that conditions are held piecewise-constant over an integr @param CurrentSimNanos The current simulation time in nanoseconds */ -void FacetDragDynamicEffector::UpdateState(uint64_t CurrentSimNanos) +void +FacetDragDynamicEffector::UpdateState(uint64_t CurrentSimNanos) { - ReadInputs(); - return; + ReadInputs(); + return; } diff --git a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h index 2bee2130ec..7b70521e0f 100644 --- a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h +++ b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h @@ -17,7 +17,6 @@ */ - #ifndef FACET_DRAG_DYNAMIC_EFFECTOR_H #define FACET_DRAG_DYNAMIC_EFFECTOR_H @@ -33,51 +32,47 @@ #include "architecture/utilities/rigidBodyKinematics.h" #include "architecture/utilities/bskLogging.h" - - - - /*! @brief spacecraft geometry data */ -typedef struct { - std::vector facetAreas; //!< vector of facet areas - std::vector facetCoeffs; //!< vector of facet coefficients - std::vector facetNormals_B; //!< vector of facet normals - std::vector facetLocations_B; //!< vector of facet locations -}SpacecraftGeometryData; - +typedef struct +{ + std::vector facetAreas; //!< vector of facet areas + std::vector facetCoeffs; //!< vector of facet coefficients + std::vector facetNormals_B; //!< vector of facet normals + std::vector facetLocations_B; //!< vector of facet locations +} SpacecraftGeometryData; /*! @brief faceted atmospheric drag dynamic effector */ -class FacetDragDynamicEffector: public SysModel, public DynamicEffector { -public: - - +class FacetDragDynamicEffector + : public SysModel + , public DynamicEffector +{ + public: FacetDragDynamicEffector(); ~FacetDragDynamicEffector(); void linkInStates(DynParamManager& states); void computeForceTorque(double integTime, double timeStep); - void Reset(uint64_t CurrentSimNanos); //!< class method + void Reset(uint64_t CurrentSimNanos); //!< class method void UpdateState(uint64_t CurrentSimNanos); void WriteOutputMessages(uint64_t CurrentClock); bool ReadInputs(); void addFacet(double area, double dragCoeff, Eigen::Vector3d B_normal_hat, Eigen::Vector3d B_location); -private: - + private: void plateDrag(); void updateDragDir(); -public: + + public: uint64_t numFacets; //!< number of facets ReadFunctor atmoDensInMsg; //!< atmospheric density input message - StateData *hubSigma; //!< -- Hub/Inertial attitude represented by MRP - StateData *hubVelocity; //!< m/s Hub inertial velocity vector + StateData* hubSigma; //!< -- Hub/Inertial attitude represented by MRP + StateData* hubVelocity; //!< m/s Hub inertial velocity vector Eigen::Vector3d v_B; //!< m/s local variable to hold the inertial velocity Eigen::Vector3d v_hat_B; //!< class variable BSKLogger bskLogger; //!< -- BSK Logging -private: + private: AtmoPropsMsgPayload atmoInData; - SpacecraftGeometryData scGeometry; //!< -- Struct to hold spacecraft facet data - + SpacecraftGeometryData scGeometry; //!< -- Struct to hold spacecraft facet data }; -#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/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/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp index 03a376a2e7..30be49ae7f 100644 --- a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp +++ b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp @@ -22,15 +22,17 @@ #include "architecture/utilities/avsEigenSupport.h" #include -const double speedLight = 299792458.0; // [m/s] Speed of light -const double AstU = 149597870700.0; // [m] Astronomical unit -const double solarRadFlux = 1368.0; // [W/m^2] Solar radiation flux at 1 AU +const double speedLight = 299792458.0; // [m/s] Speed of light +const double AstU = 149597870700.0; // [m] Astronomical unit +const double solarRadFlux = 1368.0; // [W/m^2] Solar radiation flux at 1 AU /*! This method resets required module variables and checks the input messages to ensure they are linked. @param currentSimNanos [ns] Time the method is called */ -void FacetSRPDynamicEffector::Reset(uint64_t currentSimNanos) { +void +FacetSRPDynamicEffector::Reset(uint64_t currentSimNanos) +{ if (!this->sunInMsg.isLinked()) { bskLogger.bskLog(BSK_ERROR, "FacetSRPDynamicEffector.sunInMsg was not linked."); } @@ -46,13 +48,15 @@ void FacetSRPDynamicEffector::Reset(uint64_t currentSimNanos) { @param diffuseCoeff Facet diffuse reflection optical coefficient @param specularCoeff Facet spectral reflection optical coefficient */ -void FacetSRPDynamicEffector::addFacet(double area, - Eigen::Matrix3d dcm_F0B, - Eigen::Vector3d nHat_F, - Eigen::Vector3d rotHat_F, - Eigen::Vector3d r_CopB_B, - double diffuseCoeff, - double specularCoeff) { +void +FacetSRPDynamicEffector::addFacet(double area, + Eigen::Matrix3d dcm_F0B, + Eigen::Vector3d nHat_F, + Eigen::Vector3d rotHat_F, + Eigen::Vector3d r_CopB_B, + double diffuseCoeff, + double specularCoeff) +{ this->scGeometry.facetAreaList.push_back(area); this->scGeometry.facetDcm_F0BList.push_back(dcm_F0B); this->scGeometry.facetNHat_FList.push_back(nHat_F); @@ -69,7 +73,9 @@ articulatedFacetDataInMsgs input messages. @param tmpMsg hingedRigidBody input message containing facet articulation angle data */ -void FacetSRPDynamicEffector::addArticulatedFacet(Message *tmpMsg) { +void +FacetSRPDynamicEffector::addArticulatedFacet(Message* tmpMsg) +{ this->articulatedFacetDataInMsgs.push_back(tmpMsg->addSubscriber()); } @@ -77,7 +83,9 @@ void FacetSRPDynamicEffector::addArticulatedFacet(MessagehubSigma = states.getStateObject(this->stateNameOfSigma); this->hubPosition = states.getStateObject(this->stateNameOfPosition); } @@ -86,7 +94,9 @@ void FacetSRPDynamicEffector::linkInStates(DynParamManager& states) { the articulation angle messages are also read. */ -void FacetSRPDynamicEffector::ReadMessages() { +void +FacetSRPDynamicEffector::ReadMessages() +{ // Read the Sun state input message if (this->sunInMsg.isLinked() && this->sunInMsg.isWritten()) { SpicePlanetStateMsgPayload sunMsgBuffer; @@ -101,10 +111,10 @@ void FacetSRPDynamicEffector::ReadMessages() { this->facetArticulationAngleList.clear(); for (uint64_t i = 0; i < this->numArticulatedFacets; i++) { if (this->articulatedFacetDataInMsgs[i].isLinked() && this->articulatedFacetDataInMsgs[i].isWritten()) { - facetAngleMsg = this->articulatedFacetDataInMsgs[i](); - this->facetArticulationAngleList.push_back(facetAngleMsg.theta); - this->facetAngleMsgRead = true; - } else { + facetAngleMsg = this->articulatedFacetDataInMsgs[i](); + this->facetArticulationAngleList.push_back(facetAngleMsg.theta); + this->facetAngleMsgRead = true; + } else { this->facetAngleMsgRead = false; } } @@ -118,7 +128,9 @@ void FacetSRPDynamicEffector::ReadMessages() { @param callTime [s] Time the method is called @param timeStep [s] Simulation time step */ -void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeStep) { +void +FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeStep) +{ // Read the input messages ReadMessages(); @@ -167,9 +179,9 @@ void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeSte double articulationAngle = facetArticulationAngleList.at(articulatedIndex); // Determine the required DCM that rotates the facet normal vector through the articulation angle - double prv_FF0Array[3] = {articulationAngle * this->scGeometry.facetRotHat_FList[i][0], - articulationAngle * this->scGeometry.facetRotHat_FList[i][1], - articulationAngle * this->scGeometry.facetRotHat_FList[i][2]}; + double prv_FF0Array[3] = { articulationAngle * this->scGeometry.facetRotHat_FList[i][0], + articulationAngle * this->scGeometry.facetRotHat_FList[i][1], + articulationAngle * this->scGeometry.facetRotHat_FList[i][2] }; PRV2C(prv_FF0Array, dcm_FF0Array); dcm_FF0 = c2DArray2EigenMatrix3d(dcm_FF0Array); @@ -184,11 +196,12 @@ void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeSte // Compute the SRP force and torque acting on the facet only if the facet is in view of the Sun if (projectedArea > 0.0) { - facetSRPForcePntB_B = -SRPPressure * projectedArea - * ((1 - this->scGeometry.facetSpecularCoeffList[i]) - * sHat + 2 * ((this->scGeometry.facetDiffuseCoeffList[i] / 3) - + this->scGeometry.facetSpecularCoeffList[i] * cosTheta) - * this->facetNHat_BList[i]); + facetSRPForcePntB_B = -SRPPressure * projectedArea * + ((1 - this->scGeometry.facetSpecularCoeffList[i]) * sHat + + 2 * + ((this->scGeometry.facetDiffuseCoeffList[i] / 3) + + this->scGeometry.facetSpecularCoeffList[i] * cosTheta) * + this->facetNHat_BList[i]); facetSRPTorquePntB_B = this->scGeometry.facetR_CopB_BList[i].cross(facetSRPForcePntB_B); // Add the facet contribution to the total SRP force and torque acting on the spacecraft @@ -206,7 +219,9 @@ void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeSte @param numFacets Total number of spacecraft facets */ -void FacetSRPDynamicEffector::setNumFacets(const uint64_t numFacets) { +void +FacetSRPDynamicEffector::setNumFacets(const uint64_t numFacets) +{ this->numFacets = numFacets; } @@ -214,20 +229,26 @@ void FacetSRPDynamicEffector::setNumFacets(const uint64_t numFacets) { @param numArticulatedFacets Number of articulated spacecraft facets */ -void FacetSRPDynamicEffector::setNumArticulatedFacets(const uint64_t numArticulatedFacets) { +void +FacetSRPDynamicEffector::setNumArticulatedFacets(const uint64_t numArticulatedFacets) +{ this->numArticulatedFacets = numArticulatedFacets; } /*! Getter method for the total number of facets used to model the spacecraft structure. @return uint64_t */ -uint64_t FacetSRPDynamicEffector::getNumFacets() const { +uint64_t +FacetSRPDynamicEffector::getNumFacets() const +{ return this->numFacets; } /*! Getter method for the number of articulated facets used to model the spacecraft articulating components. @return uint64_t */ -uint64_t FacetSRPDynamicEffector::getNumArticulatedFacets() const { +uint64_t +FacetSRPDynamicEffector::getNumArticulatedFacets() const +{ return this->numArticulatedFacets; } diff --git a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h index efd09eee32..90da08bde9 100644 --- a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h +++ b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h @@ -33,51 +33,62 @@ #include "architecture/utilities/avsEigenMRP.h" /*! @brief Spacecraft Geometry Data */ -typedef struct { - std::vector facetAreaList; //!< [m^2] Vector of facet areas - std::vector facetDcm_F0BList; //!< Vector of facet frame F initial attitude DCMs relative to the B frame - std::vector facetNHat_FList; //!< Vector of facet normals expressed in facet F frame components - std::vector facetRotHat_FList; //!< [m] Vector of facet rotation axes expressed in facet F frame components - std::vector facetR_CopB_BList; //!< [m] Vector of facet COP locations wrt point B expressed in B frame components - std::vector facetDiffuseCoeffList; //!< Vector of facet diffuse reflection optical coefficients - std::vector facetSpecularCoeffList; //!< Vector of facet spectral reflection optical coefficients -}FacetedSRPSpacecraftGeometryData; +typedef struct +{ + std::vector facetAreaList; //!< [m^2] Vector of facet areas + std::vector + facetDcm_F0BList; //!< Vector of facet frame F initial attitude DCMs relative to the B frame + std::vector facetNHat_FList; //!< Vector of facet normals expressed in facet F frame components + std::vector + facetRotHat_FList; //!< [m] Vector of facet rotation axes expressed in facet F frame components + std::vector + facetR_CopB_BList; //!< [m] Vector of facet COP locations wrt point B expressed in B frame components + std::vector facetDiffuseCoeffList; //!< Vector of facet diffuse reflection optical coefficients + std::vector facetSpecularCoeffList; //!< Vector of facet spectral reflection optical coefficients +} FacetedSRPSpacecraftGeometryData; /*! @brief Faceted Solar Radiation Pressure Dynamic Effector */ -class FacetSRPDynamicEffector: public SysModel, public DynamicEffector { -public: - FacetSRPDynamicEffector() = default; //!< Constructor - ~FacetSRPDynamicEffector() = default; //!< Destructor - void linkInStates(DynParamManager& states) override; //!< Method for giving the effector access to the hub states - void computeForceTorque(double callTime, double timeStep) override; //!< Method for computing the total SRP force and torque about point B - void Reset(uint64_t currentSimNanos) override; //!< Reset method - void setNumFacets(const uint64_t numFacets); //!< Setter method for the total number of spacecraft facets - void setNumArticulatedFacets(const uint64_t numArticulatedFacets); //!< Setter method for the number of articulated facets - uint64_t getNumFacets() const; //!< Getter method for the total number of spacecraft facets - uint64_t getNumArticulatedFacets() const; //!< Getter method for the number of articulated facets +class FacetSRPDynamicEffector + : public SysModel + , public DynamicEffector +{ + public: + FacetSRPDynamicEffector() = default; //!< Constructor + ~FacetSRPDynamicEffector() = default; //!< Destructor + void linkInStates(DynParamManager& states) override; //!< Method for giving the effector access to the hub states + void computeForceTorque(double callTime, double timeStep) + override; //!< Method for computing the total SRP force and torque about point B + void Reset(uint64_t currentSimNanos) override; //!< Reset method + void setNumFacets(const uint64_t numFacets); //!< Setter method for the total number of spacecraft facets + void setNumArticulatedFacets( + const uint64_t numArticulatedFacets); //!< Setter method for the number of articulated facets + uint64_t getNumFacets() const; //!< Getter method for the total number of spacecraft facets + uint64_t getNumArticulatedFacets() const; //!< Getter method for the number of articulated facets void addFacet(double area, Eigen::Matrix3d dcm_F0B, Eigen::Vector3d nHat_F, Eigen::Vector3d rotHat_F, Eigen::Vector3d r_CopB_B, double diffuseCoeff, - double specularCoeff); //!< Method for adding facets to the spacecraft geometry structure - void addArticulatedFacet(Message *tmpMsg); //!< Method for adding articulated facets to the spacecraft geometry structure - void ReadMessages(); //!< Method to read input messages + double specularCoeff); //!< Method for adding facets to the spacecraft geometry structure + void addArticulatedFacet(Message* + tmpMsg); //!< Method for adding articulated facets to the spacecraft geometry structure + void ReadMessages(); //!< Method to read input messages - ReadFunctor sunInMsg; //!< Sun spice ephemeris input message + ReadFunctor sunInMsg; //!< Sun spice ephemeris input message - uint64_t numFacets = 0; //!< Total number of spacecraft facets - uint64_t numArticulatedFacets = 0; //!< Number of articulated facets -private: - std::vector> articulatedFacetDataInMsgs; //!< Articulated facet angle data input message - std::vector facetArticulationAngleList; //!< [rad] Vector of facet rotation angles - std::vector facetNHat_BList; //!< Vector of facet normals expressed in B frame components - FacetedSRPSpacecraftGeometryData scGeometry; //!< Spacecraft facet data structure - Eigen::Vector3d r_SN_N; //!< [m] Sun inertial position vector - StateData *hubPosition = nullptr; //!< [m] Hub inertial position vector - StateData *hubSigma = nullptr; //!< Hub MRP inertial attitude - bool facetAngleMsgRead = false; //!< Boolean variable signaling that the facet articulation messages are read + uint64_t numFacets = 0; //!< Total number of spacecraft facets + uint64_t numArticulatedFacets = 0; //!< Number of articulated facets + private: + std::vector> + articulatedFacetDataInMsgs; //!< Articulated facet angle data input message + std::vector facetArticulationAngleList; //!< [rad] Vector of facet rotation angles + std::vector facetNHat_BList; //!< Vector of facet normals expressed in B frame components + FacetedSRPSpacecraftGeometryData scGeometry; //!< Spacecraft facet data structure + Eigen::Vector3d r_SN_N; //!< [m] Sun inertial position vector + StateData* hubPosition = nullptr; //!< [m] Hub inertial position vector + StateData* hubSigma = nullptr; //!< Hub MRP inertial attitude + bool facetAngleMsgRead = false; //!< Boolean variable signaling that the facet articulation messages are read }; #endif 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 old mode 100755 new mode 100644 index fe059dad83..7d92948b0e --- a/src/simulation/dynamics/gravityEffector/_UnitTest/Validation_code/computePolyAcc.c +++ b/src/simulation/dynamics/gravityEffector/_UnitTest/Validation_code/computePolyAcc.c @@ -11,114 +11,122 @@ #include "mex.h" /* The function that computes a dot product of two 3D vector */ -double dotProduct(double *a, double *b){ +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]; - + c = a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; + /* Return output */ return c; } /* The function that computes a cross product of two 3D vectors */ -void crossProduct(double *c, 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[2] = a[0]*b[1] - a[1]*b[0]; + c[0] = a[1] * b[2] - a[2] * b[1]; + c[1] = a[2] * b[0] - a[0] * b[2]; + c[2] = a[0] * b[1] - a[1] * b[0]; } /* The function that computes a 3D vector norm L2 */ -double normVector(double *a){ +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++){ + 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; } /* The function that computes the min between two integer numbers */ -int intmin(int a, int b){ +int +intmin(int a, int b) +{ /* Define output number */ int c; - + /* If statement to check what number is lower */ - if (a < b){ + if (a < b) { c = a; - } - else if (b < a){ + } else if (b < a) { c = b; - } - else{ + } else { c = a; } - + /* Return min value */ return c; } /* The function that computes the dyad product between two 3D vectors */ -void dyadProduct(double c[3][3], double *a, double *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++){ + for (i = 0; i < 3; i++) { /*Loop through columns */ - for (j = 0; j < 3; j++){ - c[i][j] = a[i]*b[j]; + for (j = 0; j < 3; j++) { + c[i][j] = a[i] * b[j]; } } } /* The function that computes a product between a 3D matrix and vector and multiplies the result with a desired scalar if neccesary*/ -void matrixvectorProduct(double *c, double a[3][3], double *b, - double factor, int flag){ +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){ + if (flag == 1) { /* Premultiply by the left side (row vector) */ - for (i = 0; i < 3; i++){ - for (j = 0; j < 3; j++){ - c[i] = c[i] + a[j][i]*b[j]; + for (i = 0; i < 3; i++) { + 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; } - } - else{ + } else { /* By default multiply by the right side (column vector) */ - for (i = 0; i < 3; i++){ - for (j = 0; j < 3; j++){ - c[i] = c[i] + a[i][j]*b[j]; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + 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; } @@ -126,224 +134,224 @@ 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] - +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 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}; - + + 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 + for (m_order = 0; m_order < M_order; m_order++) { + /* 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++){ + for (i_3D = 0; i_3D < 3; i_3D++) { /* Compute vectors going from each vertex to the evaluation * point */ - 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]; - + 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++){ + for (n_order = 0; n_order < N_order; n_order++) { /* Switch to determine edge being computed */ - switch(n_order){ - case 0 : + switch (n_order) { + case 0: /* Obtain smallest vertex index, , in this way, the * same vertex is used both times the calculations are * 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++){ + 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]; + re[i_3D] = xyz[i_min + M_xyz * i_3D] - r[i_3D]; } - + /* Assign norm */ a = norm_ri_idx; b = norm_rj_idx; break; - case 1 : + case 1: /* Obtain smallest vertex index, , in this way, the * same vertex is used both times the calculations are * 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++){ + 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]; + re[i_3D] = xyz[i_min + M_xyz * i_3D] - r[i_3D]; } - + /* Assign norm */ a = norm_rj_idx; b = norm_rk_idx; break; - case 2 : + case 2: /* Obtain smallest vertex index, , in this way, the * same vertex is used both times the calculations are * 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++){ + 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]; + 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)); - + 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 +360,34 @@ 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_rk_idx*dotProduct(ri_idx, rj_idx)); - + 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_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; - + 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; - + 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]); - + 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/_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 99feb26a25..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) @@ -84,24 +90,27 @@ 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 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/gravityEffector/polyhedralGravityModel.cpp b/src/simulation/dynamics/gravityEffector/polyhedralGravityModel.cpp old mode 100755 new mode 100644 index 4d5eba0552..10a6b6067b --- a/src/simulation/dynamics/gravityEffector/polyhedralGravityModel.cpp +++ b/src/simulation/dynamics/gravityEffector/polyhedralGravityModel.cpp @@ -22,28 +22,28 @@ namespace { // Computes the facet solid angle. -inline double computeSolidangle(const Eigen::Vector3d ri, const Eigen::Vector3d rj, const Eigen::Vector3d rk) +inline double +computeSolidangle(const Eigen::Vector3d ri, const Eigen::Vector3d rj, const Eigen::Vector3d rk) { // Computes solid angle double wy, wx, wf; - wy = ri.transpose()*rj.cross(rk); - wx = ri.norm()*rj.norm()*rk.norm() - + ri.norm()*rj.transpose()*rk - + rj.norm()*rk.transpose()*ri - + rk.norm()*ri.transpose()*rj; - wf = 2*atan2(wy, wx); + wy = ri.transpose() * rj.cross(rk); + wx = ri.norm() * rj.norm() * rk.norm() + ri.norm() * rj.transpose() * rk + rj.norm() * rk.transpose() * ri + + rk.norm() * ri.transpose() * rj; + wf = 2 * atan2(wy, wx); return wf; } } -std::optional PolyhedralGravityModel::initializeParameters() +std::optional +PolyhedralGravityModel::initializeParameters() { // If data hasn't been loaded, quit and return failure - if (this->xyzVertex.size() == 0 || this->orderFacet.size() == 0) - { + if (this->xyzVertex.size() == 0 || this->orderFacet.size() == 0) { return "Could not initialize polyhedral data: the vertex (xyzVertex) or facet (orderFacet) " - "were not provided.";; + "were not provided."; + ; } // Initializes facet parameters @@ -55,7 +55,8 @@ std::optional PolyhedralGravityModel::initializeParameters() return {}; } -std::optional PolyhedralGravityModel::initializeParameters(const GravBodyData& body) +std::optional +PolyhedralGravityModel::initializeParameters(const GravBodyData& body) { this->muBody = body.mu; return this->initializeParameters(); @@ -65,7 +66,7 @@ Eigen::Vector3d PolyhedralGravityModel::computeField(const Eigen::Vector3d& pos_BP_P) const { const size_t nFacet = this->orderFacet.rows(); - const size_t nEdge = int(3*nFacet/2); + const size_t nEdge = int(3 * nFacet / 2); // For the facet in loop: declare vertex indexes // and relative positions w.r.t. spacecraft @@ -91,27 +92,24 @@ PolyhedralGravityModel::computeField(const Eigen::Vector3d& pos_BP_P) const dUf.setZero(3); // Loop through edges - for (unsigned int n = 0; n < nEdge; n++){ + for (unsigned int n = 0; n < nEdge; n++) { // Get edge dyad matrix Ee = this->EeDyad[n]; // Compute vector from spacecraft to an edge point - re = this->xyzVertex.row(this->edgeVertex(n,0)).transpose() - - pos_BP_P; + re = this->xyzVertex.row(this->edgeVertex(n, 0)).transpose() - pos_BP_P; // Compute edge wire potential - a = (this->xyzVertex.row(this->edgeVertex(n,0)).transpose() - - pos_BP_P).norm(); - b = (this->xyzVertex.row(this->edgeVertex(n,1)).transpose() - - pos_BP_P).norm(); + a = (this->xyzVertex.row(this->edgeVertex(n, 0)).transpose() - pos_BP_P).norm(); + b = (this->xyzVertex.row(this->edgeVertex(n, 1)).transpose() - pos_BP_P).norm(); e = this->edgeLength(n); - Le = log((a+b+e) / (a+b-e)); + Le = log((a + b + e) / (a + b - e)); // Add current edge contribution - dUe += Ee*re*Le; + dUe += Ee * re * Le; // Loop through facets - if (n < nFacet){ + if (n < nFacet) { // Get facet dyad matrix Ff = this->FfDyad[n]; @@ -133,13 +131,13 @@ PolyhedralGravityModel::computeField(const Eigen::Vector3d& pos_BP_P) const rf = ri; // Add facet contribution - dUf += Ff*rf*wf; + dUf += Ff * rf * wf; } } // Compute gravity acceleration Eigen::Vector3d acc; - acc = (this->muBody/this->volPoly)*(-dUe + dUf); + acc = (this->muBody / this->volPoly) * (-dUe + dUf); return acc; } @@ -148,7 +146,7 @@ double PolyhedralGravityModel::computePotentialEnergy(const Eigen::Vector3d& pos_BP_P) const { const size_t nFacet = this->orderFacet.rows(); - const size_t nEdge = int(3*nFacet/2); + const size_t nEdge = int(3 * nFacet / 2); // For the facet in loop: declare vertex indexes // and positions @@ -174,27 +172,24 @@ PolyhedralGravityModel::computePotentialEnergy(const Eigen::Vector3d& pos_BP_P) Uf = 0; // Loop through edges - for (unsigned int n = 0; n < nEdge; n++){ + for (unsigned int n = 0; n < nEdge; n++) { // Get edge dyad matrix Ee = this->EeDyad[n]; // Compute vector from spacecraft to an edge point - re = this->xyzVertex.row(this->edgeVertex(n,0)).transpose() - - pos_BP_P; + re = this->xyzVertex.row(this->edgeVertex(n, 0)).transpose() - pos_BP_P; // Compute edge wire potential - a = (this->xyzVertex.row(this->edgeVertex(n,0)).transpose() - - pos_BP_P).norm(); - b = (this->xyzVertex.row(this->edgeVertex(n,1)).transpose() - - pos_BP_P).norm(); + a = (this->xyzVertex.row(this->edgeVertex(n, 0)).transpose() - pos_BP_P).norm(); + b = (this->xyzVertex.row(this->edgeVertex(n, 1)).transpose() - pos_BP_P).norm(); e = this->edgeLength(n); - Le = log((a+b+e) / (a+b-e)); + Le = log((a + b + e) / (a + b - e)); // Add current edge contribution - Ue += re.dot(Ee*re)*Le; + Ue += re.dot(Ee * re) * Le; // Loop through facets - if (n < nFacet){ + if (n < nFacet) { // Get facet dyad matrix Ff = this->FfDyad[n]; @@ -216,22 +211,22 @@ PolyhedralGravityModel::computePotentialEnergy(const Eigen::Vector3d& pos_BP_P) rf = ri; // Add facet contribution - Uf += rf.dot(Ff*rf)*wf; + Uf += rf.dot(Ff * rf) * wf; } } /* Compute gravity potential */ double U; - U = (this->muBody/this->volPoly) * (Ue - Uf)/2; + U = (this->muBody / this->volPoly) * (Ue - Uf) / 2; return U; } - -void PolyhedralGravityModel::initializeFacets() +void +PolyhedralGravityModel::initializeFacets() { const size_t nFacet = this->orderFacet.rows(); - const size_t nEdge = int(3*nFacet/2); + const size_t nEdge = int(3 * nFacet / 2); // Preallocate facet normal and center Eigen::Vector3d nf; @@ -262,8 +257,7 @@ void PolyhedralGravityModel::initializeFacets() int idxEdge = 0; // Loop through each facet - for (unsigned int m = 0; m < nFacet; m++) - { + for (unsigned int m = 0; m < nFacet; m++) { // Obtain vertex indexes of the facet v = this->orderFacet.row(m); i = v[0] - 1; @@ -283,34 +277,32 @@ void PolyhedralGravityModel::initializeFacets() this->xyzFacet.row(m) = (xyz3 + xyz2 + xyz1) / 3; // Add facet contribution to volume - this->volPoly += abs(xyz1.cross(xyz2).transpose()*xyz3)/6; + this->volPoly += abs(xyz1.cross(xyz2).transpose() * xyz3) / 6; // Store facet dyad product - this->FfDyad.push_back(nf*nf.transpose()); + this->FfDyad.push_back(nf * nf.transpose()); // Get edge vertexes for this facet - edgeCurrentFacet << i, j, - j, k, - k, i; + edgeCurrentFacet << i, j, j, k, k, i; // Loop through each facet edge - for (unsigned int n = 0; n < 3; n++){ + for (unsigned int n = 0; n < 3; n++) { // Add edge if non-repeated */ - isEdgeRepeat = this->addEdge(edgeCurrentFacet.row(n), - idxEdge, m); + isEdgeRepeat = this->addEdge(edgeCurrentFacet.row(n), idxEdge, m); // If not repeated, advance edge index - if (isEdgeRepeat == false){ + if (isEdgeRepeat == false) { idxEdge += 1; } } } } -void PolyhedralGravityModel::initializeEdges() +void +PolyhedralGravityModel::initializeEdges() { const size_t nFacet = this->orderFacet.rows(); - const size_t nEdge = int(3*nFacet/2); + const size_t nEdge = int(3 * nFacet / 2); // Declare shared-facet normals, edge line // and outward normals to edge-facet @@ -321,14 +313,14 @@ void PolyhedralGravityModel::initializeEdges() this->edgeLength.setZero(nEdge); // Loop through edges - for (unsigned int n = 0; n < nEdge; n++){ + for (unsigned int n = 0; n < nEdge; n++) { // Obtain normal of facets sharing the edge nFA = this->normalFacet.row(this->edgeFacet(n, 0)).transpose(); nFB = this->normalFacet.row(this->edgeFacet(n, 1)).transpose(); // Compute the edge line and length - edgeLine = (this->xyzVertex.row(this->edgeVertex(n, 1)) - - this->xyzVertex.row(this->edgeVertex(n, 0))).transpose(); + edgeLine = + (this->xyzVertex.row(this->edgeVertex(n, 1)) - this->xyzVertex.row(this->edgeVertex(n, 0))).transpose(); this->edgeLength(n) = edgeLine.norm(); edgeLine /= this->edgeLength(n); @@ -337,20 +329,21 @@ void PolyhedralGravityModel::initializeEdges() n21 = -edgeLine.cross(nFB); // Store edge dyad product - this->EeDyad.push_back(nFA*n12.transpose() + nFB*n21.transpose()); + this->EeDyad.push_back(nFA * n12.transpose() + nFB * n21.transpose()); } } -bool PolyhedralGravityModel::addEdge(Eigen::Vector2i edge, int idxEdge, int idxFacet) +bool +PolyhedralGravityModel::addEdge(Eigen::Vector2i edge, int idxEdge, int idxFacet) { // Flag telling if an edge is already stored bool isEdgeRepeat = false; // Loop through previously stored edges - for (int i = 0; i < idxEdge; i++){ + for (int i = 0; i < idxEdge; i++) { // Check if the edge is already stored - if ((this->edgeVertex(i, 0) == edge(0) && this->edgeVertex(i, 1) == edge(1)) - || (this->edgeVertex(i, 1) == edge(0) && this->edgeVertex(i, 0) == edge(1))){ + if ((this->edgeVertex(i, 0) == edge(0) && this->edgeVertex(i, 1) == edge(1)) || + (this->edgeVertex(i, 1) == edge(0) && this->edgeVertex(i, 0) == edge(1))) { // If edge is repeated set flag to true and just store the other facet isEdgeRepeat = true; this->edgeFacet(i, 1) = idxFacet; diff --git a/src/simulation/dynamics/gravityEffector/polyhedralGravityModel.h b/src/simulation/dynamics/gravityEffector/polyhedralGravityModel.h old mode 100755 new mode 100644 index 7a31c20ecc..61aa218b98 --- a/src/simulation/dynamics/gravityEffector/polyhedralGravityModel.h +++ b/src/simulation/dynamics/gravityEffector/polyhedralGravityModel.h @@ -30,7 +30,8 @@ * each vertex is defined by its position relative to the center of * mass of the body. */ -class PolyhedralGravityModel : public GravityModel { +class PolyhedralGravityModel : public GravityModel +{ public: /** Initialize all parameters necessary for the computation of gravity. * @@ -70,7 +71,7 @@ class PolyhedralGravityModel : public GravityModel { bool addEdge(Eigen::Vector2i edge, int idx_edge, int idx_facet); public: - double muBody = 0; /**< [m^3/s^2] Gravitation parameter for the planet */ + double muBody = 0; /**< [m^3/s^2] Gravitation parameter for the planet */ /** * This matrix contains the position of every vertex of this @@ -91,7 +92,7 @@ class PolyhedralGravityModel : public GravityModel { Eigen::MatrixX3i orderFacet; private: - double volPoly = 0; /**< [m^3] Volume of the polyhedral */ + double volPoly = 0; /**< [m^3] Volume of the polyhedral */ /** * This matrix contains the outward normal of each facet [-]. diff --git a/src/simulation/dynamics/gravityEffector/sphericalHarmonicsGravityModel.cpp b/src/simulation/dynamics/gravityEffector/sphericalHarmonicsGravityModel.cpp index 84de2e925a..fe5960fc96 100644 --- a/src/simulation/dynamics/gravityEffector/sphericalHarmonicsGravityModel.cpp +++ b/src/simulation/dynamics/gravityEffector/sphericalHarmonicsGravityModel.cpp @@ -22,13 +22,15 @@ namespace { // Computes the term (2 - d_l), where d_l is the kronecker delta. -inline double getK(const size_t degree) +inline double +getK(const size_t degree) { return (degree == 0) ? 1.0 : 2.0; } } -std::optional SphericalHarmonicsGravityModel::initializeParameters() +std::optional +SphericalHarmonicsGravityModel::initializeParameters() { if (this->cBar.size() == 0 || this->sBar.size() == 0) { return "Could not initialize spherical harmonics: the 'C' or 'S' parameters were not " @@ -39,18 +41,17 @@ std::optional SphericalHarmonicsGravityModel::initializeParameters( std::vector aRow, n1Row, n2Row; aRow.resize(i + 1, 0.0); // Diagonal elements of A_bar - if (i == 0) { aRow[i] = 1.0; } - else { - aRow[i] = sqrt(double((2 * i + 1) * getK(i)) / (2 * i * getK(i - 1))) * - this->aBar[i - 1][i - 1]; + if (i == 0) { + aRow[i] = 1.0; + } else { + aRow[i] = sqrt(double((2 * i + 1) * getK(i)) / (2 * i * getK(i - 1))) * this->aBar[i - 1][i - 1]; } n1Row.resize(i + 1, 0.0); n2Row.resize(i + 1, 0.0); for (size_t m = 0; m <= i; m++) { if (i >= m + 2) { n1Row[m] = sqrt(double((2 * i + 1) * (2 * i - 1)) / ((i - m) * (i + m))); - n2Row[m] = sqrt(double((i + m - 1) * (2 * i + 1) * (i - m - 1)) / - ((i + m) * (i - m) * (2 * i - 3))); + n2Row[m] = sqrt(double((i + m - 1) * (2 * i + 1) * (i - m - 1)) / ((i + m) * (i - m) * (2 * i - 3))); } } this->n1.push_back(n1Row); @@ -64,9 +65,10 @@ std::optional SphericalHarmonicsGravityModel::initializeParameters( nq1Row.resize(l + 1, 0.0); nq2Row.resize(l + 1, 0.0); for (size_t m = 0; m <= l; m++) { - if (m < l) { nq1Row[m] = sqrt(double((l - m) * getK(m) * (l + m + 1)) / getK(m + 1)); } - nq2Row[m] = sqrt(double((l + m + 2) * (l + m + 1) * (2 * l + 1) * getK(m)) / - ((2 * l + 3) * getK(m + 1))); + if (m < l) { + nq1Row[m] = sqrt(double((l - m) * getK(m) * (l + m + 1)) / getK(m + 1)); + } + nq2Row[m] = sqrt(double((l + m + 2) * (l + m + 1) * (2 * l + 1) * getK(m)) / ((2 * l + 3) * getK(m + 1))); } this->nQuot1.push_back(nq1Row); this->nQuot2.push_back(nq2Row); @@ -91,11 +93,11 @@ SphericalHarmonicsGravityModel::computeField(const Eigen::Vector3d& position_pla Eigen::Vector3d SphericalHarmonicsGravityModel::computeField(const Eigen::Vector3d& position_planetFixed, - size_t degree, bool include_zero_degree) const + size_t degree, + bool include_zero_degree) const { if (degree > this->maxDeg) { - auto errorMsg = - "Requested degree greater than maximum degree in Spherical Harmonics gravity model"; + auto errorMsg = "Requested degree greater than maximum degree in Spherical Harmonics gravity model"; bskLogger.bskLog(BSK_ERROR, errorMsg); } @@ -126,16 +128,14 @@ SphericalHarmonicsGravityModel::computeField(const Eigen::Vector3d& position_pla // Lower terms of A_bar for (size_t m = 0; m <= order + 1; m++) { for (size_t l = m + 2; l <= degree + 1; l++) { - this->aBar[l][m] = - u * this->n1[l][m] * this->aBar[l - 1][m] - this->n2[l][m] * this->aBar[l - 2][m]; + this->aBar[l][m] = u * this->n1[l][m] * this->aBar[l - 1][m] - this->n2[l][m] * this->aBar[l - 2][m]; } // Computation of real and imaginary parts of (2+j*t)^m if (m == 0) { rE.push_back(1.0); iM.push_back(0.0); - } - else { + } else { rE.push_back(s * rE[m - 1] - t * iM[m - 1]); iM.push_back(s * iM[m - 1] + t * rE[m - 1]); } @@ -175,15 +175,16 @@ SphericalHarmonicsGravityModel::computeField(const Eigen::Vector3d& position_pla if (m == 0) { E = 0.0; F = 0.0; - } - else { + } else { E = this->cBar[l][m] * rE[m - 1] + this->sBar[l][m] * iM[m - 1]; F = this->sBar[l][m] * rE[m - 1] - this->cBar[l][m] * iM[m - 1]; } sum_a1 = sum_a1 + m * this->aBar[l][m] * E; sum_a2 = sum_a2 + m * this->aBar[l][m] * F; - if (m < l) { sum_a3 = sum_a3 + this->nQuot1[l][m] * this->aBar[l][m + 1] * D; } + if (m < l) { + sum_a3 = sum_a3 + this->nQuot1[l][m] * this->aBar[l][m + 1] * D; + } sum_a4 = sum_a4 + this->nQuot2[l][m] * this->aBar[l + 1][m + 1] * D; } @@ -193,11 +194,11 @@ SphericalHarmonicsGravityModel::computeField(const Eigen::Vector3d& position_pla a4 = a4 - rhol[l + 1] / radEquator * sum_a4; } - return {a1 + s * a4, a2 + t * a4, a3 + u * a4}; + return { a1 + s * a4, a2 + t * a4, a3 + u * a4 }; } -double SphericalHarmonicsGravityModel::computePotentialEnergy( - const Eigen::Vector3d& positionWrtPlanet_N) const +double +SphericalHarmonicsGravityModel::computePotentialEnergy(const Eigen::Vector3d& positionWrtPlanet_N) const { return -this->muBody / positionWrtPlanet_N.norm(); } diff --git a/src/simulation/dynamics/gravityEffector/sphericalHarmonicsGravityModel.h b/src/simulation/dynamics/gravityEffector/sphericalHarmonicsGravityModel.h index aa2074ba90..3242d26509 100644 --- a/src/simulation/dynamics/gravityEffector/sphericalHarmonicsGravityModel.h +++ b/src/simulation/dynamics/gravityEffector/sphericalHarmonicsGravityModel.h @@ -28,7 +28,8 @@ /** * The Spherical Harmonics gravity model */ -class SphericalHarmonicsGravityModel : public GravityModel { +class SphericalHarmonicsGravityModel : public GravityModel +{ public: /** Initialize all parameters necessary for the computation of gravity. * @@ -69,7 +70,8 @@ class SphericalHarmonicsGravityModel : public GravityModel { * If include_zero_degree is false the degree that corresponds to the spherical * term (point-mass) of the gravity is ignored. */ - Eigen::Vector3d computeField(const Eigen::Vector3d& position_planetFixed, size_t degree, + Eigen::Vector3d computeField(const Eigen::Vector3d& position_planetFixed, + size_t degree, bool include_zero_degree) const; /** Returns the gravitational potential energy at a position around this body. @@ -83,8 +85,8 @@ class SphericalHarmonicsGravityModel : public GravityModel { double computePotentialEnergy(const Eigen::Vector3d& positionWrtPlanet_N) const override; public: - double radEquator = 0; /**< [m] Reference radius for the planet */ - double muBody = 0; /**< [m^3/s^2] Gravitation parameter for the planet */ + double radEquator = 0; /**< [m] Reference radius for the planet */ + double muBody = 0; /**< [m^3/s^2] Gravitation parameter for the planet */ /** The maximum degree of Spherical Harmonics to use * @@ -109,11 +111,11 @@ class SphericalHarmonicsGravityModel : public GravityModel { * They are coefficients used in the method of Pines for the gravity due to SH. * For their definition, see the 'Basilisk-GravityEffector' documentation. */ - mutable std::vector> aBar; /**< [-] Eq. 61 */ - std::vector> n1; /**< [-] Eq. 63 */ - std::vector> n2; /**< [-] Eq. 64 */ - std::vector> nQuot1; /**< [-] Eq. 79 */ - std::vector> nQuot2; /**< [-] Eq. 80 */ + mutable std::vector> aBar; /**< [-] Eq. 61 */ + std::vector> n1; /**< [-] Eq. 63 */ + std::vector> n2; /**< [-] Eq. 64 */ + std::vector> nQuot1; /**< [-] Eq. 79 */ + std::vector> nQuot2; /**< [-] Eq. 80 */ }; #endif /* SH_GRAVITY_MODEL_H */ diff --git a/src/simulation/dynamics/hingedRigidBodyMotor/_UnitTest/test_hingedRigidBodyMotor.py b/src/simulation/dynamics/hingedRigidBodyMotor/_UnitTest/test_hingedRigidBodyMotor.py index 04208a9792..e677a9840b 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 @@ -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** @@ -50,15 +52,19 @@ 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. """ - [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,17 +85,21 @@ 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) module.hingedBodyStateReferenceInMsg.subscribeTo(hingedBodyStateReferenceInMsg) - + module.K = K module.P = P @@ -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,6 +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/hingedRigidBodyMotor/hingedRigidBodyMotor.cpp b/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.cpp index 6617ba3bf1..855939b3f8 100644 --- a/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.cpp +++ b/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.cpp @@ -17,7 +17,6 @@ */ - #include "simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.h" #include @@ -31,14 +30,13 @@ HingedRigidBodyMotor::HingedRigidBodyMotor() } /*! Module Destructor */ -HingedRigidBodyMotor::~HingedRigidBodyMotor() -{ -} +HingedRigidBodyMotor::~HingedRigidBodyMotor() {} /*! This method is used to reset the module and checks that required input messages are connect. */ -void HingedRigidBodyMotor::Reset(uint64_t CurrentSimNanos) +void +HingedRigidBodyMotor::Reset(uint64_t CurrentSimNanos) { //! check that required input messages are connected if (!this->hingedBodyStateSensedInMsg.isLinked()) { @@ -50,14 +48,14 @@ void HingedRigidBodyMotor::Reset(uint64_t CurrentSimNanos) if (this->K <= 0.0 || this->P <= 0.0) { bskLogger.bskLog(BSK_ERROR, "HingedRigidBodyMotor K and P must be set to positive values."); } - } - -/*! This is the main method that gets called every time the module is updated. It calculates a motor torque on a hinged rigid body using a simple PD control law. +/*! This is the main method that gets called every time the module is updated. It calculates a motor torque on a hinged + rigid body using a simple PD control law. */ -void HingedRigidBodyMotor::UpdateState(uint64_t CurrentSimNanos) +void +HingedRigidBodyMotor::UpdateState(uint64_t CurrentSimNanos) { //! local variables double sensedTheta; @@ -66,9 +64,9 @@ void HingedRigidBodyMotor::UpdateState(uint64_t CurrentSimNanos) double refThetaDot; double torque; - HingedRigidBodyMsgPayload hingedBodyStateSensedInMsgBuffer; //!< local copy of message buffer for reference - HingedRigidBodyMsgPayload hingedBodyStateReferenceInMsgBuffer; //!< local copy of message buffer for measurement - ArrayMotorTorqueMsgPayload motorTorqueOutMsgBuffer; //!< local copy of message buffer for motor torque + HingedRigidBodyMsgPayload hingedBodyStateSensedInMsgBuffer; //!< local copy of message buffer for reference + HingedRigidBodyMsgPayload hingedBodyStateReferenceInMsgBuffer; //!< local copy of message buffer for measurement + ArrayMotorTorqueMsgPayload motorTorqueOutMsgBuffer; //!< local copy of message buffer for motor torque //! zero the output message buffers before assigning values motorTorqueOutMsgBuffer = this->motorTorqueOutMsg.zeroMsgPayload; diff --git a/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.h b/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.h index 04baf6640e..d0fd62a810 100644 --- a/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.h +++ b/src/simulation/dynamics/hingedRigidBodyMotor/hingedRigidBodyMotor.h @@ -17,7 +17,6 @@ */ - #ifndef HINGEDRIGIDBODYMOTOR_H #define HINGEDRIGIDBODYMOTOR_H @@ -27,30 +26,29 @@ #include "architecture/utilities/bskLogging.h" #include "architecture/messaging/messaging.h" -/*! @brief Calculates a motor torque to drive a hinged panel to a reference angle state. A sensed and reference hinged rigid body angle - drives a simple PD control law. +/*! @brief Calculates a motor torque to drive a hinged panel to a reference angle state. A sensed and reference hinged + rigid body angle drives a simple PD control law. */ -class HingedRigidBodyMotor: public SysModel { -public: +class HingedRigidBodyMotor : public SysModel +{ + public: HingedRigidBodyMotor(); ~HingedRigidBodyMotor(); void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); -public: - - double K; //!< gain on theta + public: + double K; //!< gain on theta double P; //!< gain on theta dot - ReadFunctor hingedBodyStateSensedInMsg; //!< sensed rigid body state (theta, theta dot) - ReadFunctor hingedBodyStateReferenceInMsg; //!< reference hinged rigid body state (theta, theta dot) - - Message motorTorqueOutMsg; //!< motor torque on hinged rigid body + ReadFunctor hingedBodyStateSensedInMsg; //!< sensed rigid body state (theta, theta dot) + ReadFunctor + hingedBodyStateReferenceInMsg; //!< reference hinged rigid body state (theta, theta dot) - BSKLogger bskLogger; //!< -- BSK Logging + Message motorTorqueOutMsg; //!< motor torque on hinged rigid body + BSKLogger bskLogger; //!< -- BSK Logging }; - #endif 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/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/linearTranslationBodiesNDOF/linearTranslationNDOFStateEffector.cpp b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesNDOF/linearTranslationNDOFStateEffector.cpp index 84e8429b13..6555295893 100644 --- a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesNDOF/linearTranslationNDOFStateEffector.cpp +++ b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesNDOF/linearTranslationNDOFStateEffector.cpp @@ -42,10 +42,12 @@ uint64_t linearTranslationNDOFStateEffector::effectorID = 1; /*! This is the destructor, nothing to report here */ linearTranslationNDOFStateEffector::~linearTranslationNDOFStateEffector() { - linearTranslationNDOFStateEffector::effectorID --; /* reset the panel ID*/ + linearTranslationNDOFStateEffector::effectorID--; /* reset the panel ID*/ } -void translatingBody::setMass(double mass) { +void +translatingBody::setMass(double mass) +{ if (mass > 0.0) this->mass = mass; else { @@ -53,16 +55,19 @@ void translatingBody::setMass(double mass) { } } -void translatingBody::setFHat_P(Eigen::Vector3d fHat_P) { +void +translatingBody::setFHat_P(Eigen::Vector3d fHat_P) +{ if (fHat_P.norm() > 0.01) { this->fHat_P = fHat_P.normalized(); - } - else { + } else { this->bskLogger.bskLog(BSK_ERROR, "Norm of fHat must be greater than 0."); } } -void translatingBody::setK(double k) { +void +translatingBody::setK(double k) +{ if (k >= 0.0) this->k = k; else { @@ -70,7 +75,9 @@ void translatingBody::setK(double k) { } } -void translatingBody::setC(double c) { +void +translatingBody::setC(double c) +{ if (c >= 0.0) this->c = c; else { @@ -79,20 +86,22 @@ void translatingBody::setC(double c) { } /*! This method is used to reset the module. */ -void linearTranslationNDOFStateEffector::Reset(uint64_t CurrentClock) +void +linearTranslationNDOFStateEffector::Reset(uint64_t CurrentClock) { - for(auto& translatingBody: this->translatingBodyVec) { + for (auto& translatingBody : this->translatingBodyVec) { if (translatingBody.fHat_P.norm() > 0.0) { translatingBody.fHat_P.normalize(); - } - else { + } else { bskLogger.bskLog(BSK_ERROR, "Norm of fHat must be greater than 0. sHat may not have been set by the user."); } } } /*! This method is used to add a translating body. */ -void linearTranslationNDOFStateEffector::addTranslatingBody(const translatingBody& newBody) { +void +linearTranslationNDOFStateEffector::addTranslatingBody(const translatingBody& newBody) +{ // Pushback new body translatingBodyVec.push_back(newBody); this->N++; @@ -103,20 +112,21 @@ void linearTranslationNDOFStateEffector::addTranslatingBody(const translatingBod this->translatingBodyRefInMsgs.push_back(ReadFunctor()); // resize A B and C - this->ARho.conservativeResize(this->ARho.rows()+1, 3); - this->BRho.conservativeResize(this->BRho.rows()+1, 3); - this->CRho.conservativeResize(this->CRho.rows()+1); + this->ARho.conservativeResize(this->ARho.rows() + 1, 3); + this->BRho.conservativeResize(this->BRho.rows() + 1, 3); + this->CRho.conservativeResize(this->CRho.rows() + 1); } /*! This method reads motor force, lock flag, and reference state messages. */ -void linearTranslationNDOFStateEffector::readInputMessages() +void +linearTranslationNDOFStateEffector::readInputMessages() { //! - Read the incoming command array if (this->motorForceInMsg.isLinked() && this->motorForceInMsg.isWritten()) { ArrayMotorForceMsgPayload incomingCmdBuffer; incomingCmdBuffer = this->motorForceInMsg(); int i = 0; - for(auto& translatingBody: this->translatingBodyVec) { + for (auto& translatingBody : this->translatingBodyVec) { translatingBody.u = incomingCmdBuffer.motorForce[i]; i++; } @@ -127,15 +137,16 @@ void linearTranslationNDOFStateEffector::readInputMessages() ArrayEffectorLockMsgPayload incomingLockBuffer; incomingLockBuffer = this->motorLockInMsg(); int i = 0; - for(auto& translatingBody: this->translatingBodyVec) { + for (auto& translatingBody : this->translatingBodyVec) { translatingBody.isAxisLocked = incomingLockBuffer.effectorLockFlag[i]; i++; } } int translatingBodyIndex = 0; - for(auto& translatingBody: this->translatingBodyVec) { - if (this->translatingBodyRefInMsgs[translatingBodyIndex].isLinked() && this->translatingBodyRefInMsgs[translatingBodyIndex].isWritten()) { + for (auto& translatingBody : this->translatingBodyVec) { + if (this->translatingBodyRefInMsgs[translatingBodyIndex].isLinked() && + this->translatingBodyRefInMsgs[translatingBodyIndex].isWritten()) { LinearTranslationRigidBodyMsgPayload incomingRefBuffer; incomingRefBuffer = this->translatingBodyRefInMsgs[translatingBodyIndex](); translatingBody.rhoRef = incomingRefBuffer.rho; @@ -146,13 +157,14 @@ void linearTranslationNDOFStateEffector::readInputMessages() } /*! This method takes the computed rho states and outputs them to the messaging system. */ -void linearTranslationNDOFStateEffector::writeOutputStateMessages(uint64_t CurrentClock) +void +linearTranslationNDOFStateEffector::writeOutputStateMessages(uint64_t CurrentClock) { // Write out the translating body output messages int i = 0; LinearTranslationRigidBodyMsgPayload translatingBodyBuffer; SCStatesMsgPayload configLogMsg; - for(auto& translatingBody: this->translatingBodyVec) { + for (auto& translatingBody : this->translatingBodyVec) { if (this->translatingBodyOutMsgs[i]->isLinked()) { translatingBodyBuffer = this->translatingBodyOutMsgs[i]->zeroMsgPayload; translatingBodyBuffer.rho = translatingBody.rho; @@ -176,31 +188,35 @@ void linearTranslationNDOFStateEffector::writeOutputStateMessages(uint64_t Curre } /*! This method prepends the name of the spacecraft for multi-spacecraft simulations.*/ -void linearTranslationNDOFStateEffector::prependSpacecraftNameToStates() +void +linearTranslationNDOFStateEffector::prependSpacecraftNameToStates() { this->nameOfRhoState = this->nameOfSpacecraftAttachedTo + this->nameOfRhoState; this->nameOfRhoDotState = this->nameOfSpacecraftAttachedTo + this->nameOfRhoDotState; } /*! This method allows the TB state effector to have access to the hub states and gravity*/ -void linearTranslationNDOFStateEffector::linkInStates(DynParamManager& statesIn) +void +linearTranslationNDOFStateEffector::linkInStates(DynParamManager& statesIn) { this->inertialPositionProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + "r_BN_N"); this->inertialVelocityProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + "v_BN_N"); } -/*! This method allows the TB state effector to register its states: rho and rhoDot with the dynamic parameter manager */ -void linearTranslationNDOFStateEffector::registerStates(DynParamManager& states) +/*! This method allows the TB state effector to register its states: rho and rhoDot with the dynamic parameter manager + */ +void +linearTranslationNDOFStateEffector::registerStates(DynParamManager& states) { // Register the rho states this->rhoState = states.registerState(N, 1, this->nameOfRhoState); this->rhoDotState = states.registerState(N, 1, this->nameOfRhoDotState); - Eigen::MatrixXd RhoInitMatrix(N,1); - Eigen::MatrixXd RhoDotInitMatrix(N,1); + Eigen::MatrixXd RhoInitMatrix(N, 1); + Eigen::MatrixXd RhoDotInitMatrix(N, 1); int i = 0; - for(const auto& translatingBody: this->translatingBodyVec) { - RhoInitMatrix(i,0) = translatingBody.rhoInit; - RhoDotInitMatrix(i,0) = translatingBody.rhoDotInit; + for (const auto& translatingBody : this->translatingBodyVec) { + RhoInitMatrix(i, 0) = translatingBody.rhoInit; + RhoDotInitMatrix(i, 0) = translatingBody.rhoDotInit; i++; } this->rhoState->setState(RhoInitMatrix); @@ -209,7 +225,8 @@ void linearTranslationNDOFStateEffector::registerStates(DynParamManager& states) /*! This method allows the TB state effector to provide its contributions to the mass props and mass prop rates of the spacecraft */ -void linearTranslationNDOFStateEffector::updateEffectorMassProps(double integTime) +void +linearTranslationNDOFStateEffector::updateEffectorMassProps(double integTime) { this->effProps.mEff = 0.0; this->effProps.rEff_CB_B = Eigen::Vector3d::Zero(); @@ -218,7 +235,7 @@ void linearTranslationNDOFStateEffector::updateEffectorMassProps(double integTim this->effProps.IEffPrimePntB_B = Eigen::Matrix3d::Zero(); int i = 0; - for(auto& translatingBody: this->translatingBodyVec) { + for (auto& translatingBody : this->translatingBodyVec) { if (translatingBody.isAxisLocked) { auto rhoDotVector = this->rhoDotState->getState(); rhoDotVector(i) = 0.0; @@ -236,8 +253,8 @@ void linearTranslationNDOFStateEffector::updateEffectorMassProps(double integTim translatingBody.dcm_FB = translatingBody.dcm_FP; translatingBody.fHat_B = translatingBody.fHat_P; } else { - translatingBody.dcm_FB = translatingBody.dcm_FP * this->translatingBodyVec[i-1].dcm_FB; - translatingBody.fHat_B = this->translatingBodyVec[i-1].dcm_FB.transpose() * translatingBody.fHat_P; + translatingBody.dcm_FB = translatingBody.dcm_FP * this->translatingBodyVec[i - 1].dcm_FB; + translatingBody.fHat_B = this->translatingBodyVec[i - 1].dcm_FB.transpose() * translatingBody.fHat_P; } translatingBody.r_FF0_B = translatingBody.rho * translatingBody.fHat_B; @@ -249,17 +266,19 @@ void linearTranslationNDOFStateEffector::updateEffectorMassProps(double integTim translatingBody.r_FP_B = translatingBody.r_F0P_B + translatingBody.r_FF0_B; translatingBody.r_FB_B = translatingBody.r_FP_B; } else { - translatingBody.r_F0P_B = this->translatingBodyVec[i-1].dcm_FB.transpose() * translatingBody.r_F0P_P; + translatingBody.r_F0P_B = this->translatingBodyVec[i - 1].dcm_FB.transpose() * translatingBody.r_F0P_P; translatingBody.r_FP_B = translatingBody.r_F0P_B + translatingBody.r_FF0_B; - translatingBody.r_FB_B = translatingBody.r_FP_B + this->translatingBodyVec[i-1].r_FB_B; + translatingBody.r_FB_B = translatingBody.r_FP_B + this->translatingBodyVec[i - 1].r_FB_B; } translatingBody.r_FcB_B = translatingBody.r_FcF_B + translatingBody.r_FB_B; this->effProps.rEff_CB_B += translatingBody.mass * translatingBody.r_FcB_B; // Find the inertia of the bodies about point B translatingBody.rTilde_FcB_B = eigenTilde(translatingBody.r_FcB_B); - translatingBody.IPntFc_B = translatingBody.dcm_FB.transpose() * translatingBody.IPntFc_F * translatingBody.dcm_FB; - this->effProps.IEffPntB_B += translatingBody.IPntFc_B - translatingBody.mass * translatingBody.rTilde_FcB_B * translatingBody.rTilde_FcB_B; + translatingBody.IPntFc_B = + translatingBody.dcm_FB.transpose() * translatingBody.IPntFc_F * translatingBody.dcm_FB; + this->effProps.IEffPntB_B += + translatingBody.IPntFc_B - translatingBody.mass * translatingBody.rTilde_FcB_B * translatingBody.rTilde_FcB_B; // Find rPrime_FcB_B translatingBody.rPrime_FcF_B = Eigen::Vector3d::Zero(); @@ -268,7 +287,7 @@ void linearTranslationNDOFStateEffector::updateEffectorMassProps(double integTim if (i == 0) { translatingBody.rPrime_FB_B = translatingBody.rPrime_FP_B; } else { - translatingBody.rPrime_FB_B = translatingBody.rPrime_FP_B + this->translatingBodyVec[i-1].rPrime_FB_B; + translatingBody.rPrime_FB_B = translatingBody.rPrime_FP_B + this->translatingBodyVec[i - 1].rPrime_FB_B; } translatingBody.rPrime_FcB_B = translatingBody.rPrime_FcF_B + translatingBody.rPrime_FB_B; this->effProps.rEffPrime_CB_B += translatingBody.mass * translatingBody.rPrime_FcB_B; @@ -276,7 +295,9 @@ void linearTranslationNDOFStateEffector::updateEffectorMassProps(double integTim // Find the body-frame time derivative of the inertia of each arm and the entire spacecraft translatingBody.IPrimePntFc_B = Eigen::Matrix3d::Zero(); Eigen::Matrix3d rPrimeTilde_FcB_B = eigenTilde(translatingBody.rPrime_FcB_B); - this->effProps.IEffPrimePntB_B += translatingBody.IPrimePntFc_B - translatingBody.mass * (rPrimeTilde_FcB_B * translatingBody.rTilde_FcB_B + translatingBody.rTilde_FcB_B * rPrimeTilde_FcB_B); + this->effProps.IEffPrimePntB_B += + translatingBody.IPrimePntFc_B - translatingBody.mass * (rPrimeTilde_FcB_B * translatingBody.rTilde_FcB_B + + translatingBody.rTilde_FcB_B * rPrimeTilde_FcB_B); i++; } @@ -286,7 +307,12 @@ void linearTranslationNDOFStateEffector::updateEffectorMassProps(double integTim /*! This method allows the TB state effector to give its contributions to the matrices needed for the back-sub method */ -void linearTranslationNDOFStateEffector::updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) +void +linearTranslationNDOFStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { // Find the DCM from N to B frames this->sigma_BN = sigma_BN; @@ -311,51 +337,55 @@ void linearTranslationNDOFStateEffector::updateContributions(double integTime, B } /*! This method compute MRho for back-sub */ -void linearTranslationNDOFStateEffector::computeMRho(Eigen::MatrixXd& MRho) +void +linearTranslationNDOFStateEffector::computeMRho(Eigen::MatrixXd& MRho) { - for (int n = 0; nN; n++) { - for (int i = 0; iN; i++) { - MRho(n,i) = 0.0; + for (int n = 0; n < this->N; n++) { + for (int i = 0; i < this->N; i++) { + MRho(n, i) = 0.0; if ((this->translatingBodyVec[n].isAxisLocked || this->translatingBodyVec[i].isAxisLocked) && n != i) continue; - for (int j = (i<=n) ? n : i; jN; j++) { - MRho(n,i) += this->translatingBodyVec[n].fHat_B.transpose() * this->translatingBodyVec[j].mass - * this->translatingBodyVec[i].fHat_B; + for (int j = (i <= n) ? n : i; j < this->N; j++) { + MRho(n, i) += this->translatingBodyVec[n].fHat_B.transpose() * this->translatingBodyVec[j].mass * + this->translatingBodyVec[i].fHat_B; } } } } /*! This method compute ARhoStar for back-sub */ -void linearTranslationNDOFStateEffector::computeARhoStar(Eigen::MatrixXd& ARhoStar) +void +linearTranslationNDOFStateEffector::computeARhoStar(Eigen::MatrixXd& ARhoStar) { - for (int n = 0; nN; n++) { + for (int n = 0; n < this->N; n++) { if (this->translatingBodyVec[n].isAxisLocked) continue; - for (int i = n; iN; i++) { + for (int i = n; i < this->N; i++) { ARhoStar.row(n) -= this->translatingBodyVec[n].fHat_B.transpose() * this->translatingBodyVec[i].mass; } } } /*! This method compute BRhoStar for back-sub */ -void linearTranslationNDOFStateEffector::computeBRhoStar(Eigen::MatrixXd& BRhoStar) +void +linearTranslationNDOFStateEffector::computeBRhoStar(Eigen::MatrixXd& BRhoStar) { - for (int n = 0; nN; n++) { + for (int n = 0; n < this->N; n++) { if (this->translatingBodyVec[n].isAxisLocked) continue; - for (int i = n; iN; i++) { + for (int i = n; i < this->N; i++) { Eigen::Vector3d r_FciB_B = this->translatingBodyVec[i].r_FcB_B; Eigen::Matrix3d rTilde_FciB_B = eigenTilde(r_FciB_B); - BRhoStar.row(n) += this->translatingBodyVec[n].fHat_B.transpose() * this->translatingBodyVec[i].mass * rTilde_FciB_B; + BRhoStar.row(n) += + this->translatingBodyVec[n].fHat_B.transpose() * this->translatingBodyVec[i].mass * rTilde_FciB_B; } } } /*! This method compute CRhoStar for back-sub */ -void linearTranslationNDOFStateEffector::computeCRhoStar(Eigen::VectorXd& CRhoStar, - const Eigen::Vector3d& g_N) +void +linearTranslationNDOFStateEffector::computeCRhoStar(Eigen::VectorXd& CRhoStar, const Eigen::Vector3d& g_N) { Eigen::Matrix3d omegaTilde_BN_B = eigenTilde(this->omega_BN_B); @@ -364,51 +394,63 @@ void linearTranslationNDOFStateEffector::computeCRhoStar(Eigen::VectorXd& CRhoSt g_B = this->dcm_BN * g_N; Eigen::Vector3d F_g = Eigen::Vector3d::Zero().transpose(); - for (int n = 0; nN; n++) { + for (int n = 0; n < this->N; n++) { if (this->translatingBodyVec[n].isAxisLocked) continue; - CRhoStar(n, 0) = this->translatingBodyVec[n].u - - this->translatingBodyVec[n].k * (this->translatingBodyVec[n].rho - - this->translatingBodyVec[n].rhoRef) - this->translatingBodyVec[n].c * - (this->translatingBodyVec[n].rhoDot - this->translatingBodyVec[n].rhoDotRef); - for (int i = n; iN; i++) { + CRhoStar(n, 0) = + this->translatingBodyVec[n].u - + this->translatingBodyVec[n].k * (this->translatingBodyVec[n].rho - this->translatingBodyVec[n].rhoRef) - + this->translatingBodyVec[n].c * (this->translatingBodyVec[n].rhoDot - this->translatingBodyVec[n].rhoDotRef); + for (int i = n; i < this->N; i++) { Eigen::Vector3d r_FciB_B = this->translatingBodyVec[i].r_FcB_B; Eigen::Vector3d rPrime_FciB_B = this->translatingBodyVec[i].rPrime_FcB_B; F_g = this->translatingBodyVec[i].mass * g_B; - CRhoStar(n, 0) += this->translatingBodyVec[n].fHat_B.transpose() * (F_g - this->translatingBodyVec[i].mass * - (omegaTilde_BN_B * omegaTilde_BN_B * r_FciB_B + 2 * omegaTilde_BN_B * rPrime_FciB_B)); + CRhoStar(n, 0) += this->translatingBodyVec[n].fHat_B.transpose() * + (F_g - this->translatingBodyVec[i].mass * (omegaTilde_BN_B * omegaTilde_BN_B * r_FciB_B + + 2 * omegaTilde_BN_B * rPrime_FciB_B)); } } } /*! This method computes the back-sub contributions of the system */ -void linearTranslationNDOFStateEffector::computeBackSubContributions(BackSubMatrices& backSubContr) const +void +linearTranslationNDOFStateEffector::computeBackSubContributions(BackSubMatrices& backSubContr) const { Eigen::Matrix3d omegaTilde_BN_B = eigenTilde(this->omega_BN_B); - for (int i = 0; iN; i++) { - Eigen::Matrix3d rTilde_FciB_B = eigenTilde(this->translatingBodyVec[i].r_FcB_B); - Eigen::Vector3d rPrime_FciB_B = this->translatingBodyVec[i].rPrime_FcB_B; - backSubContr.vecRot -= this->translatingBodyVec[i].mass * omegaTilde_BN_B * rTilde_FciB_B * rPrime_FciB_B; + for (int i = 0; i < this->N; i++) { + Eigen::Matrix3d rTilde_FciB_B = eigenTilde(this->translatingBodyVec[i].r_FcB_B); + Eigen::Vector3d rPrime_FciB_B = this->translatingBodyVec[i].rPrime_FcB_B; + backSubContr.vecRot -= this->translatingBodyVec[i].mass * omegaTilde_BN_B * rTilde_FciB_B * rPrime_FciB_B; for (int j = i; j < this->N; j++) { Eigen::Matrix3d rTilde_FcjB_B = eigenTilde(this->translatingBodyVec[j].r_FcB_B); // Translation contributions - backSubContr.matrixA += this->translatingBodyVec[j].mass * this->translatingBodyVec[i].fHat_B * this->ARho.row(i); - backSubContr.matrixB += this->translatingBodyVec[j].mass * this->translatingBodyVec[i].fHat_B * this->BRho.row(i); - backSubContr.vecTrans -= this->translatingBodyVec[j].mass * this->translatingBodyVec[i].fHat_B * this->CRho.row(i); + backSubContr.matrixA += + this->translatingBodyVec[j].mass * this->translatingBodyVec[i].fHat_B * this->ARho.row(i); + backSubContr.matrixB += + this->translatingBodyVec[j].mass * this->translatingBodyVec[i].fHat_B * this->BRho.row(i); + backSubContr.vecTrans -= + this->translatingBodyVec[j].mass * this->translatingBodyVec[i].fHat_B * this->CRho.row(i); // Rotation contributions - backSubContr.matrixC += this->translatingBodyVec[j].mass * rTilde_FcjB_B * this->translatingBodyVec[i].fHat_B * this->ARho.row(i); - backSubContr.matrixD += this->translatingBodyVec[j].mass * rTilde_FcjB_B * this->translatingBodyVec[i].fHat_B * this->BRho.row(i); - backSubContr.vecRot -= this->translatingBodyVec[j].mass * rTilde_FcjB_B * this->translatingBodyVec[i].fHat_B * this->CRho.row(i); + backSubContr.matrixC += + this->translatingBodyVec[j].mass * rTilde_FcjB_B * this->translatingBodyVec[i].fHat_B * this->ARho.row(i); + backSubContr.matrixD += + this->translatingBodyVec[j].mass * rTilde_FcjB_B * this->translatingBodyVec[i].fHat_B * this->BRho.row(i); + backSubContr.vecRot -= + this->translatingBodyVec[j].mass * rTilde_FcjB_B * this->translatingBodyVec[i].fHat_B * this->CRho.row(i); } } } /*! This method is used to find the derivatives for the TB stateEffector: rhoDDot and the kinematic derivative */ -void linearTranslationNDOFStateEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +linearTranslationNDOFStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { // Find rDDotLoc_BN_B const Eigen::Vector3d& rDDotLocal_BN_N = rDDot_BN_N; @@ -420,11 +462,13 @@ void linearTranslationNDOFStateEffector::computeDerivatives(double integTime, Ei this->rhoDotState->setDerivative(rhoDDot); } -/*! This method is for calculating the contributions of the TB state effector to the energy and momentum of the spacecraft */ -void linearTranslationNDOFStateEffector::updateEnergyMomContributions(double integTime, - Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, - Eigen::Vector3d omega_BN_B) +/*! This method is for calculating the contributions of the TB state effector to the energy and momentum of the + * spacecraft */ +void +linearTranslationNDOFStateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { this->omega_BN_B = omega_BN_B; Eigen::Matrix3d omegaTilde_BN_B = eigenTilde(this->omega_BN_B); @@ -432,7 +476,7 @@ void linearTranslationNDOFStateEffector::updateEnergyMomContributions(double int rotAngMomPntCContr_B = Eigen::Vector3d::Zero(); rotEnergyContr = 0.0; - for(auto& translatingBody: this->translatingBodyVec) { + for (auto& translatingBody : this->translatingBodyVec) { // Update omega_FN_B translatingBody.omega_FN_B = this->omega_BN_B; @@ -440,20 +484,23 @@ void linearTranslationNDOFStateEffector::updateEnergyMomContributions(double int translatingBody.rDot_FcB_B = translatingBody.rPrime_FcB_B + omegaTilde_BN_B * translatingBody.r_FcB_B; // Find rotational angular momentum contribution from hub - rotAngMomPntCContr_B += translatingBody.IPntFc_B * translatingBody.omega_FN_B + translatingBody.mass * translatingBody.rTilde_FcB_B * translatingBody.rDot_FcB_B; + rotAngMomPntCContr_B += translatingBody.IPntFc_B * translatingBody.omega_FN_B + + translatingBody.mass * translatingBody.rTilde_FcB_B * translatingBody.rDot_FcB_B; // Find rotational energy contribution from the hub - rotEnergyContr += 1.0 / 2.0 * translatingBody.omega_FN_B.dot(translatingBody.IPntFc_B * translatingBody.omega_FN_B) - + 1.0 / 2.0 * translatingBody.mass * translatingBody.rDot_FcB_B.dot(translatingBody.rDot_FcB_B) - + 1.0 / 2.0 * translatingBody.k * (translatingBody.rho - translatingBody.rhoRef) * - (translatingBody.rho - translatingBody.rhoRef); + rotEnergyContr += + 1.0 / 2.0 * translatingBody.omega_FN_B.dot(translatingBody.IPntFc_B * translatingBody.omega_FN_B) + + 1.0 / 2.0 * translatingBody.mass * translatingBody.rDot_FcB_B.dot(translatingBody.rDot_FcB_B) + + 1.0 / 2.0 * translatingBody.k * (translatingBody.rho - translatingBody.rhoRef) * + (translatingBody.rho - translatingBody.rhoRef); } } /*! This method computes the translating body states relative to the inertial frame */ -void linearTranslationNDOFStateEffector::computeTranslatingBodyInertialStates() +void +linearTranslationNDOFStateEffector::computeTranslatingBodyInertialStates() { - for(auto& translatingBody: this->translatingBodyVec) { + for (auto& translatingBody : this->translatingBodyVec) { // Compute the rotational properties Eigen::Matrix3d dcm_FN; dcm_FN = translatingBody.dcm_FB * this->dcm_BN; @@ -461,13 +508,16 @@ void linearTranslationNDOFStateEffector::computeTranslatingBodyInertialStates() translatingBody.omega_FN_F = translatingBody.dcm_FB.transpose().transpose() * translatingBody.omega_FN_B; // Compute the translation properties - translatingBody.r_FcN_N = (Eigen::Vector3d)*this->inertialPositionProperty + this->dcm_BN.transpose() * translatingBody.r_FcB_B; - translatingBody.v_FcN_N = (Eigen::Vector3d)*this->inertialVelocityProperty + this->dcm_BN.transpose() * translatingBody.rDot_FcB_B; + translatingBody.r_FcN_N = + (Eigen::Vector3d) * this->inertialPositionProperty + this->dcm_BN.transpose() * translatingBody.r_FcB_B; + translatingBody.v_FcN_N = + (Eigen::Vector3d) * this->inertialVelocityProperty + this->dcm_BN.transpose() * translatingBody.rDot_FcB_B; } } /*! This method is used so that the simulation will ask TB to update messages */ -void linearTranslationNDOFStateEffector::UpdateState(uint64_t CurrentSimNanos) +void +linearTranslationNDOFStateEffector::UpdateState(uint64_t CurrentSimNanos) { this->readInputMessages(); this->computeTranslatingBodyInertialStates(); diff --git a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesNDOF/linearTranslationNDOFStateEffector.h b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesNDOF/linearTranslationNDOFStateEffector.h index 60c109530f..c20bfd880e 100644 --- a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesNDOF/linearTranslationNDOFStateEffector.h +++ b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesNDOF/linearTranslationNDOFStateEffector.h @@ -34,8 +34,9 @@ #include "architecture/messaging/messaging.h" /*! @brief translating body structure */ -struct translatingBody { -public: +struct translatingBody +{ + public: /** setter for `mass` property */ void setMass(double mass); /** setter for `k` property */ @@ -43,109 +44,119 @@ struct translatingBody { /** setter for `c` property */ void setC(double c); /** setter for `rhoInit` property */ - void setRhoInit(double rhoInit) {this->rhoInit = rhoInit;}; + void setRhoInit(double rhoInit) { this->rhoInit = rhoInit; }; /** setter for `rhoDotInit` property */ - void setRhoDotInit(double rhoDotInit) {this->rhoDotInit = rhoDotInit;}; + void setRhoDotInit(double rhoDotInit) { this->rhoDotInit = rhoDotInit; }; /** setter for `fHat_P` property */ void setFHat_P(Eigen::Vector3d fHat_P); /** setter for `r_FcF_F` property */ - void setR_FcF_F(Eigen::Vector3d r_FcF_F) {this->r_FcF_F = r_FcF_F;}; + void setR_FcF_F(Eigen::Vector3d r_FcF_F) { this->r_FcF_F = r_FcF_F; }; /** setter for `r_F0B_B` property */ - void setR_F0P_P(Eigen::Vector3d r_F0P_P) {this->r_F0P_P = r_F0P_P;}; + void setR_F0P_P(Eigen::Vector3d r_F0P_P) { this->r_F0P_P = r_F0P_P; }; /** setter for `IPntFc_F` property */ - void setIPntFc_F(Eigen::Matrix3d IPntFc_F) {this->IPntFc_F = IPntFc_F;}; + void setIPntFc_F(Eigen::Matrix3d IPntFc_F) { this->IPntFc_F = IPntFc_F; }; /** setter for `dcm_FB` property */ - void setDCM_FP(Eigen::Matrix3d dcm_FP) {this->dcm_FP = dcm_FP;}; + void setDCM_FP(Eigen::Matrix3d dcm_FP) { this->dcm_FP = dcm_FP; }; /** getter for `mass` property */ - double getMass() const {return this->mass;}; + double getMass() const { return this->mass; }; /** getter for `k` property */ - double getK() const {return this->k;}; + double getK() const { return this->k; }; /** getter for `c` property */ - double getC() const {return this->c;}; + double getC() const { return this->c; }; /** getter for `rhoInit` property */ - double getRhoInit() const {return this->rhoInit;}; + double getRhoInit() const { return this->rhoInit; }; /** getter for `rhoDotInit` property */ - double getRhoDotInit() const {return this->rhoDotInit;}; + double getRhoDotInit() const { return this->rhoDotInit; }; /** getter for `fHat_P` property */ - Eigen::Vector3d getFHat_P() const {return this->fHat_P;}; + Eigen::Vector3d getFHat_P() const { return this->fHat_P; }; /** getter for `r_FcF_F` property */ - Eigen::Vector3d getR_FcF_F() const {return this->r_FcF_F;}; + Eigen::Vector3d getR_FcF_F() const { return this->r_FcF_F; }; /** getter for `r_F0P_P` property */ - Eigen::Vector3d getR_F0P_P() const {return this->r_F0P_P;}; + Eigen::Vector3d getR_F0P_P() const { return this->r_F0P_P; }; /** getter for `IPntFc_F` property */ - Eigen::Matrix3d getIPntFc_F() const {return IPntFc_F;}; + Eigen::Matrix3d getIPntFc_F() const { return IPntFc_F; }; /** getter for `dcm_FP` property */ - Eigen::Matrix3d getDCM_FP() const {return dcm_FP;}; + Eigen::Matrix3d getDCM_FP() const { return dcm_FP; }; -private: + private: friend class linearTranslationNDOFStateEffector; // user-defined properties - double mass = 0.0; //!< [kg] mass of translating arm - double k = 0.0; //!< [N/m] translational spring constant - double c = 0.0; //!< [N-s/m] translational damping coefficient - double rhoInit = 0.0; //!< [m] initial translating body distance from equilibrium - double rhoDotInit = 0.0; //!< [m/s] initial translating body velocity of F frame wrt F0 frame - double rhoRef = 0.0; //!< [m] reference translating body distance from equilibrium - double rhoDotRef = 0.0; //!< [m/s] reference translating body velocity of F frame wrt F0 frame - double u = 0.0; //!< [N] motor force acting along the translating axis of the body - bool isAxisLocked = false; //!< -- lock flag - Eigen::Matrix3d IPntFc_F = Eigen::Matrix3d::Identity(); //!< [kg-m^2] Inertia of body about point Fc in F frame components - Eigen::Vector3d r_FcF_F = Eigen::Vector3d::Zero(); //!< [m] vector pointing from translating frame F origin to point Fc (center of mass of arm) in F frame components - Eigen::Vector3d r_F0P_P = Eigen::Vector3d::Zero(); //!< [m] vector pointing from parent origin to translating frame F origin in F frame components - Eigen::Vector3d fHat_P{1.0, 0.0, 0.0}; //!< -- translating axis in parent frame components. - Eigen::Matrix3d dcm_FP = Eigen::Matrix3d::Identity(); //!< -- DCM from parent frame to current F frame + double mass = 0.0; //!< [kg] mass of translating arm + double k = 0.0; //!< [N/m] translational spring constant + double c = 0.0; //!< [N-s/m] translational damping coefficient + double rhoInit = 0.0; //!< [m] initial translating body distance from equilibrium + double rhoDotInit = 0.0; //!< [m/s] initial translating body velocity of F frame wrt F0 frame + double rhoRef = 0.0; //!< [m] reference translating body distance from equilibrium + double rhoDotRef = 0.0; //!< [m/s] reference translating body velocity of F frame wrt F0 frame + double u = 0.0; //!< [N] motor force acting along the translating axis of the body + bool isAxisLocked = false; //!< -- lock flag + Eigen::Matrix3d IPntFc_F = + Eigen::Matrix3d::Identity(); //!< [kg-m^2] Inertia of body about point Fc in F frame components + Eigen::Vector3d r_FcF_F = Eigen::Vector3d::Zero(); //!< [m] vector pointing from translating frame F origin to point + //!< Fc (center of mass of arm) in F frame components + Eigen::Vector3d r_F0P_P = Eigen::Vector3d::Zero(); //!< [m] vector pointing from parent origin to translating frame + //!< F origin in F frame components + Eigen::Vector3d fHat_P{ 1.0, 0.0, 0.0 }; //!< -- translating axis in parent frame components. + Eigen::Matrix3d dcm_FP = Eigen::Matrix3d::Identity(); //!< -- DCM from parent frame to current F frame // Scalar Properties - double rho = 0.0; //!< [m] translating body distance from equilibrium - double rhoDot = 0.0; //!< [m/s] translating body velocity of F frame wrt F0 frame + double rho = 0.0; //!< [m] translating body distance from equilibrium + double rhoDot = 0.0; //!< [m/s] translating body velocity of F frame wrt F0 frame // Vector quantities - Eigen::Vector3d r_FF0_B; //!< [m] vector pointing from translating frame F to translating frame F0 (magnitude rho) - Eigen::Vector3d r_F0P_B; //!< [m] vector pointing from parent translating frame P to translating frame F0 - Eigen::Vector3d fHat_B; //!< -- translating axis in B frame components. - Eigen::Vector3d r_FcF_B; //!< [m] vector pointing from translating frame F origin to point Fc (center of mass of arm) in B frame components - Eigen::Vector3d r_FB_B; //!< [m] vector pointing from body frame B origin to F frame in B frame components - Eigen::Vector3d r_FcB_B; //!< [m] vector pointing from body frame B origin to Fc in B frame components - Eigen::Vector3d r_FP_B; //!< [m] vector from parent frame to current F frame in B frame componentes - Eigen::Vector3d r_FP_P; //!< [m] vector from parent frame to current F frame in parent frame components - Eigen::Vector3d rPrime_FB_B; //!< [m/s] body frame time derivative of r_FB_B - Eigen::Vector3d rPrime_FcF_B; //!< [m/s] body frame time derivative of r_FcF_B - Eigen::Vector3d rPrime_FcB_B; //!< [m/s] body frame time derivative of r_FcB_B - Eigen::Vector3d rPrime_FP_B; //!< [m/s] body frame time derivative of r_FP_B - Eigen::Vector3d rPrime_FF0_B; //!< [m/s] body frame time derivative of r_FF0_B - Eigen::Vector3d rDot_FcB_B; //!< [m/s] inertial frame time derivative of r_FcB_B - Eigen::Vector3d omega_FN_B; //!< [rad/s] angular velocity of the F frame wrt the N frame in B frame components + Eigen::Vector3d r_FF0_B; //!< [m] vector pointing from translating frame F to translating frame F0 (magnitude rho) + Eigen::Vector3d r_F0P_B; //!< [m] vector pointing from parent translating frame P to translating frame F0 + Eigen::Vector3d fHat_B; //!< -- translating axis in B frame components. + Eigen::Vector3d r_FcF_B; //!< [m] vector pointing from translating frame F origin to point Fc (center of mass of + //!< arm) in B frame components + Eigen::Vector3d r_FB_B; //!< [m] vector pointing from body frame B origin to F frame in B frame components + Eigen::Vector3d r_FcB_B; //!< [m] vector pointing from body frame B origin to Fc in B frame components + Eigen::Vector3d r_FP_B; //!< [m] vector from parent frame to current F frame in B frame componentes + Eigen::Vector3d r_FP_P; //!< [m] vector from parent frame to current F frame in parent frame components + Eigen::Vector3d rPrime_FB_B; //!< [m/s] body frame time derivative of r_FB_B + Eigen::Vector3d rPrime_FcF_B; //!< [m/s] body frame time derivative of r_FcF_B + Eigen::Vector3d rPrime_FcB_B; //!< [m/s] body frame time derivative of r_FcB_B + Eigen::Vector3d rPrime_FP_B; //!< [m/s] body frame time derivative of r_FP_B + Eigen::Vector3d rPrime_FF0_B; //!< [m/s] body frame time derivative of r_FF0_B + Eigen::Vector3d rDot_FcB_B; //!< [m/s] inertial frame time derivative of r_FcB_B + Eigen::Vector3d omega_FN_B; //!< [rad/s] angular velocity of the F frame wrt the N frame in B frame components // Matrix quantities - Eigen::Matrix3d dcm_FB; //!< -- DCM from body frame to F frame - Eigen::Matrix3d IPntFc_B; //!< [kg-m^2] Inertia of body about point Fc in B frame components - Eigen::Matrix3d IPrimePntFc_B; //!< [kg-m^2/s] body frame time derivative of IPntFc_B - Eigen::Matrix3d rTilde_FcB_B; //!< [m] tilde matrix of r_FcB_B - Eigen::Matrix3d omegaTilde_FB_B; //!< [rad/s] tilde matrix of omega_FB_B + Eigen::Matrix3d dcm_FB; //!< -- DCM from body frame to F frame + Eigen::Matrix3d IPntFc_B; //!< [kg-m^2] Inertia of body about point Fc in B frame components + Eigen::Matrix3d IPrimePntFc_B; //!< [kg-m^2/s] body frame time derivative of IPntFc_B + Eigen::Matrix3d rTilde_FcB_B; //!< [m] tilde matrix of r_FcB_B + Eigen::Matrix3d omegaTilde_FB_B; //!< [rad/s] tilde matrix of omega_FB_B // Inertial properties - Eigen::Vector3d r_FcN_N; //!< [m] position vector of translating body's center of mass Fc relative to the inertial frame origin N - Eigen::Vector3d v_FcN_N; //!< [m/s] inertial velocity vector of Fc relative to inertial frame - Eigen::Vector3d sigma_FN; //!< -- MRP attitude of frame S relative to inertial frame - Eigen::Vector3d omega_FN_F; //!< [rad/s] inertial translating body frame angular velocity vector + Eigen::Vector3d + r_FcN_N; //!< [m] position vector of translating body's center of mass Fc relative to the inertial frame origin N + Eigen::Vector3d v_FcN_N; //!< [m/s] inertial velocity vector of Fc relative to inertial frame + Eigen::Vector3d sigma_FN; //!< -- MRP attitude of frame S relative to inertial frame + Eigen::Vector3d omega_FN_F; //!< [rad/s] inertial translating body frame angular velocity vector BSKLogger bskLogger; }; /*! @brief translating body state effector class */ -class linearTranslationNDOFStateEffector: public StateEffector, public SysModel { -public: - - linearTranslationNDOFStateEffector(); //!< -- Constructor - ~linearTranslationNDOFStateEffector() final; //!< -- Destructor - - std::vector*> translatingBodyOutMsgs; //!< vector of state output messages - std::vector*> translatingBodyConfigLogOutMsgs; //!< vector of translating body state config log messages - std::vector> translatingBodyRefInMsgs; //!< (optional) reference state input message - ReadFunctor motorForceInMsg; //!< -- (optional) motor force input message name - ReadFunctor motorLockInMsg; //!< -- (optional) motor lock input message name +class linearTranslationNDOFStateEffector + : public StateEffector + , public SysModel +{ + public: + linearTranslationNDOFStateEffector(); //!< -- Constructor + ~linearTranslationNDOFStateEffector() final; //!< -- Destructor + + std::vector*> + translatingBodyOutMsgs; //!< vector of state output messages + std::vector*> + translatingBodyConfigLogOutMsgs; //!< vector of translating body state config log messages + std::vector> + translatingBodyRefInMsgs; //!< (optional) reference state input message + ReadFunctor motorForceInMsg; //!< -- (optional) motor force input message name + ReadFunctor motorLockInMsg; //!< -- (optional) motor lock input message name /** method for adding a new translating body */ void addTranslatingBody(translatingBody const& newBody); @@ -158,28 +169,30 @@ class linearTranslationNDOFStateEffector: public StateEffector, public SysModel /** getter for `nameOfRhoDotState` property */ std::string getNameOfRhoDotState() const { return this->nameOfRhoDotState; }; -private: - static uint64_t effectorID; //!< [] ID number of this effector - int N = 0; //!< -- number of translating body axes defined in the system + private: + static uint64_t effectorID; //!< [] ID number of this effector + int N = 0; //!< -- number of translating body axes defined in the system std::vector translatingBodyVec; //!< -- vector of TB effector structs // Terms needed for back substitution - Eigen::MatrixXd ARho; //!< -- rDDot_BN term for back substitution - Eigen::MatrixXd BRho; //!< -- omegaDot_BN term for back substitution - Eigen::VectorXd CRho; //!< -- scalar term for back substitution + Eigen::MatrixXd ARho; //!< -- rDDot_BN term for back substitution + Eigen::MatrixXd BRho; //!< -- omegaDot_BN term for back substitution + Eigen::VectorXd CRho; //!< -- scalar term for back substitution // Hub properties - Eigen::Vector3d omega_BN_B; //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components - Eigen::MRPd sigma_BN; //!< -- body frame attitude wrt to the N frame in MRPs - Eigen::Matrix3d dcm_BN; //!< -- DCM from inertial frame to body frame + Eigen::Vector3d omega_BN_B; //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components + Eigen::MRPd sigma_BN; //!< -- body frame attitude wrt to the N frame in MRPs + Eigen::Matrix3d dcm_BN; //!< -- DCM from inertial frame to body frame // States - Eigen::MatrixXd* inertialPositionProperty = nullptr; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - Eigen::MatrixXd* inertialVelocityProperty = nullptr; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialPositionProperty = + nullptr; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialVelocityProperty = + nullptr; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase StateData* rhoState = nullptr; StateData* rhoDotState = nullptr; - std::string nameOfRhoState; //!< -- identifier for the theta state data container - std::string nameOfRhoDotState; //!< -- identifier for the thetaDot state data container + std::string nameOfRhoState; //!< -- identifier for the theta state data container + std::string nameOfRhoDotState; //!< -- identifier for the thetaDot state data container // module functions void Reset(uint64_t CurrentClock) final; 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/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.cpp b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.cpp index e129d390a1..a87fc6d915 100644 --- a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.cpp +++ b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.cpp @@ -22,14 +22,15 @@ linearTranslationOneDOFStateEffector::linearTranslationOneDOFStateEffector() { - this->effProps.mEff = 0.0; - this->effProps.IEffPntB_B.setZero(); - this->effProps.rEff_CB_B.setZero(); - this->effProps.rEffPrime_CB_B.setZero(); - this->effProps.IEffPrimePntB_B.setZero(); - - this->nameOfRhoState = "linearTranslationRho" + std::to_string(linearTranslationOneDOFStateEffector::effectorID); - this->nameOfRhoDotState = "linearTranslationRhoDot" + std::to_string(linearTranslationOneDOFStateEffector::effectorID); + this->effProps.mEff = 0.0; + this->effProps.IEffPntB_B.setZero(); + this->effProps.rEff_CB_B.setZero(); + this->effProps.rEffPrime_CB_B.setZero(); + this->effProps.IEffPrimePntB_B.setZero(); + + this->nameOfRhoState = "linearTranslationRho" + std::to_string(linearTranslationOneDOFStateEffector::effectorID); + this->nameOfRhoDotState = + "linearTranslationRhoDot" + std::to_string(linearTranslationOneDOFStateEffector::effectorID); linearTranslationOneDOFStateEffector::effectorID++; } @@ -40,10 +41,14 @@ linearTranslationOneDOFStateEffector::~linearTranslationOneDOFStateEffector() linearTranslationOneDOFStateEffector::effectorID = 1; } -void linearTranslationOneDOFStateEffector::Reset(uint64_t CurrentClock) { +void +linearTranslationOneDOFStateEffector::Reset(uint64_t CurrentClock) +{ } -void linearTranslationOneDOFStateEffector::setMass(double mass) { +void +linearTranslationOneDOFStateEffector::setMass(double mass) +{ if (mass > 0.0) this->mass = mass; else { @@ -51,16 +56,19 @@ void linearTranslationOneDOFStateEffector::setMass(double mass) { } } -void linearTranslationOneDOFStateEffector::setFHat_B(Eigen::Vector3d fHat_B) { +void +linearTranslationOneDOFStateEffector::setFHat_B(Eigen::Vector3d fHat_B) +{ if (fHat_B.norm() > 0.01) { this->fHat_B = fHat_B.normalized(); - } - else { + } else { this->bskLogger.bskLog(BSK_ERROR, "Norm of fHat must be greater than 0."); } } -void linearTranslationOneDOFStateEffector::setK(double k) { +void +linearTranslationOneDOFStateEffector::setK(double k) +{ if (k >= 0.0) this->k = k; else { @@ -68,7 +76,9 @@ void linearTranslationOneDOFStateEffector::setK(double k) { } } -void linearTranslationOneDOFStateEffector::setC(double c) { +void +linearTranslationOneDOFStateEffector::setC(double c) +{ if (c >= 0.0) this->c = c; else { @@ -76,27 +86,32 @@ void linearTranslationOneDOFStateEffector::setC(double c) { } } -void linearTranslationOneDOFStateEffector::linkInStates(DynParamManager& statesIn) +void +linearTranslationOneDOFStateEffector::linkInStates(DynParamManager& statesIn) { - this->inertialPositionProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); - this->inertialVelocityProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); + this->inertialPositionProperty = + statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); + this->inertialVelocityProperty = + statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); this->g_N = statesIn.getPropertyReference("g_N"); } -void linearTranslationOneDOFStateEffector::registerStates(DynParamManager& states) +void +linearTranslationOneDOFStateEffector::registerStates(DynParamManager& states) { - this->rhoState = states.registerState(1, 1, nameOfRhoState); - Eigen::MatrixXd rhoInitMatrix(1,1); - rhoInitMatrix(0,0) = this->rhoInit; + this->rhoState = states.registerState(1, 1, nameOfRhoState); + Eigen::MatrixXd rhoInitMatrix(1, 1); + rhoInitMatrix(0, 0) = this->rhoInit; this->rhoState->setState(rhoInitMatrix); - this->rhoDotState = states.registerState(1, 1, nameOfRhoDotState); - Eigen::MatrixXd rhoDotInitMatrix(1,1); - rhoDotInitMatrix(0,0) = this->rhoDotInit; + this->rhoDotState = states.registerState(1, 1, nameOfRhoDotState); + Eigen::MatrixXd rhoDotInitMatrix(1, 1); + rhoDotInitMatrix(0, 0) = this->rhoDotInit; this->rhoDotState->setState(rhoDotInitMatrix); } -void linearTranslationOneDOFStateEffector::readInputMessages() +void +linearTranslationOneDOFStateEffector::readInputMessages() { if (this->motorForceInMsg.isLinked() && this->motorForceInMsg.isWritten()) { ArrayMotorForceMsgPayload incomingCmdBuffer; @@ -118,7 +133,8 @@ void linearTranslationOneDOFStateEffector::readInputMessages() } } -void linearTranslationOneDOFStateEffector::writeOutputStateMessages(uint64_t currentSimNanos) +void +linearTranslationOneDOFStateEffector::writeOutputStateMessages(uint64_t currentSimNanos) { if (this->translatingBodyOutMsg.isLinked()) { LinearTranslationRigidBodyMsgPayload translatingBodyBuffer; @@ -141,36 +157,38 @@ void linearTranslationOneDOFStateEffector::writeOutputStateMessages(uint64_t cur } } -void linearTranslationOneDOFStateEffector::updateEffectorMassProps(double integTime) +void +linearTranslationOneDOFStateEffector::updateEffectorMassProps(double integTime) { - this->rho = this->rhoState->getState()(0,0); + this->rho = this->rhoState->getState()(0, 0); this->rhoDot = this->rhoDotState->getState()(0, 0); - if (this->isAxisLocked) - { + if (this->isAxisLocked) { Eigen::MatrixXd zeroMatrix = Eigen::MatrixXd::Constant(1, 1, 0.0); this->rhoDotState->setState(zeroMatrix); } this->r_FcF0_B = this->dcm_FB.transpose() * this->r_FcF_F + this->rho * this->fHat_B; - this->r_FcB_B = this->r_F0B_B + this->r_FcF0_B; + this->r_FcB_B = this->r_F0B_B + this->r_FcF0_B; this->rTilde_FcB_B = eigenTilde(this->r_FcB_B); this->rPrime_FcB_B = this->rhoDot * this->fHat_B; this->rPrimeTilde_FcB_B = eigenTilde(this->rPrime_FcB_B); this->IPntFc_B = this->dcm_FB.transpose() * this->IPntFc_F * this->dcm_FB; - this->effProps.mEff = this->mass; - this->effProps.rEff_CB_B = this->r_FcB_B; + this->effProps.mEff = this->mass; + this->effProps.rEff_CB_B = this->r_FcB_B; this->effProps.rEffPrime_CB_B = this->rPrime_FcB_B; - this->effProps.IEffPntB_B = this->IPntFc_B + this->mass * this->rTilde_FcB_B * this->rTilde_FcB_B.transpose(); - this->effProps.IEffPrimePntB_B = -this->mass * (this->rPrimeTilde_FcB_B * this->rTilde_FcB_B - + this->rTilde_FcB_B * this->rPrimeTilde_FcB_B); + this->effProps.IEffPntB_B = this->IPntFc_B + this->mass * this->rTilde_FcB_B * this->rTilde_FcB_B.transpose(); + this->effProps.IEffPrimePntB_B = + -this->mass * (this->rPrimeTilde_FcB_B * this->rTilde_FcB_B + this->rTilde_FcB_B * this->rPrimeTilde_FcB_B); } -void linearTranslationOneDOFStateEffector::updateContributions(double integTime, BackSubMatrices & backSubContr, - Eigen::Vector3d sigma_BN, - Eigen::Vector3d omega_BN_B, - Eigen::Vector3d g_N) +void +linearTranslationOneDOFStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { Eigen::MRPd sigmaLocal_BN; sigmaLocal_BN = sigma_BN; @@ -185,12 +203,12 @@ void linearTranslationOneDOFStateEffector::updateContributions(double integTime, computeBackSubContributions(backSubContr, F_g); } -void linearTranslationOneDOFStateEffector::computeBackSubContributions(BackSubMatrices & backSubContr, - const Eigen::Vector3d& F_g) +void +linearTranslationOneDOFStateEffector::computeBackSubContributions(BackSubMatrices& backSubContr, + const Eigen::Vector3d& F_g) { // There are no contributions if the effector is locked - if (this->isAxisLocked) - { + if (this->isAxisLocked) { this->aRho.setZero(); this->bRho.setZero(); this->cRho = 0.0; @@ -198,42 +216,45 @@ void linearTranslationOneDOFStateEffector::computeBackSubContributions(BackSubMa return; } - this->aRho = - this->fHat_B.transpose(); + this->aRho = -this->fHat_B.transpose(); this->bRho = this->fHat_B.transpose() * this->rTilde_FcB_B; - this->cRho = 1.0 / this->mass * (this->motorForce - this->k * (this->rho - this->rhoRef) - - this->c * (this->rhoDot - this->rhoDotRef) - + this->fHat_B.transpose() * (F_g - this->mass * (2 * this->omegaTilde_BN_B * this->rPrime_FcB_B - + this->omegaTilde_BN_B*this->omegaTilde_BN_B*this->r_FcB_B))); + this->cRho = + 1.0 / this->mass * + (this->motorForce - this->k * (this->rho - this->rhoRef) - this->c * (this->rhoDot - this->rhoDotRef) + + this->fHat_B.transpose() * (F_g - this->mass * (2 * this->omegaTilde_BN_B * this->rPrime_FcB_B + + this->omegaTilde_BN_B * this->omegaTilde_BN_B * this->r_FcB_B))); - backSubContr.matrixA = this->mass * this->fHat_B * this->aRho.transpose(); + backSubContr.matrixA = this->mass * this->fHat_B * this->aRho.transpose(); backSubContr.matrixB = this->mass * this->fHat_B * this->bRho.transpose(); backSubContr.matrixC = this->mass * this->rTilde_FcB_B * this->fHat_B * this->aRho.transpose(); - backSubContr.matrixD = this->mass * this->rTilde_FcB_B * this->fHat_B * this->bRho.transpose(); - backSubContr.vecTrans = - this->mass * this->cRho * this->fHat_B; - backSubContr.vecRot = - this->mass * this->omegaTilde_BN_B * this->rTilde_FcB_B * this->rPrime_FcB_B - - this->mass * this->cRho * this->rTilde_FcB_B * this->fHat_B; + backSubContr.matrixD = this->mass * this->rTilde_FcB_B * this->fHat_B * this->bRho.transpose(); + backSubContr.vecTrans = -this->mass * this->cRho * this->fHat_B; + backSubContr.vecRot = -this->mass * this->omegaTilde_BN_B * this->rTilde_FcB_B * this->rPrime_FcB_B - + this->mass * this->cRho * this->rTilde_FcB_B * this->fHat_B; } -void linearTranslationOneDOFStateEffector::computeDerivatives(double integTime, - Eigen::Vector3d rDDot_BN_N, - Eigen::Vector3d omegaDot_BN_B, - Eigen::Vector3d sigma_BN) +void +linearTranslationOneDOFStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { - Eigen::MRPd sigmaLocal_BN; - sigmaLocal_BN = sigma_BN; - this->dcm_BN = sigmaLocal_BN.toRotationMatrix().transpose(); - - Eigen::MatrixXd rhoDDot(1,1); - Eigen::Vector3d rDDot_BN_B = dcm_BN * rDDot_BN_N; - rhoDDot(0,0) = this->aRho.dot(rDDot_BN_B) + this->bRho.dot(omegaDot_BN_B) + this->cRho; - this->rhoDotState->setDerivative(rhoDDot); + Eigen::MRPd sigmaLocal_BN; + sigmaLocal_BN = sigma_BN; + this->dcm_BN = sigmaLocal_BN.toRotationMatrix().transpose(); + + Eigen::MatrixXd rhoDDot(1, 1); + Eigen::Vector3d rDDot_BN_B = dcm_BN * rDDot_BN_N; + rhoDDot(0, 0) = this->aRho.dot(rDDot_BN_B) + this->bRho.dot(omegaDot_BN_B) + this->cRho; + this->rhoDotState->setDerivative(rhoDDot); this->rhoState->setDerivative(this->rhoDotState->getState()); } -void linearTranslationOneDOFStateEffector::updateEnergyMomContributions(double integTime, - Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, - Eigen::Vector3d omega_BN_B) +void +linearTranslationOneDOFStateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { this->omega_BN_B = omega_BN_B; this->omegaTilde_BN_B = eigenTilde(this->omega_BN_B); @@ -241,23 +262,25 @@ void linearTranslationOneDOFStateEffector::updateEnergyMomContributions(double i Eigen::Vector3d rDotFcB_B = this->rPrime_FcB_B + this->omegaTilde_BN_B * this->r_FcB_B; rotAngMomPntCContr_B = this->IPntFc_B * omega_FN_B + this->mass * this->r_FcB_B.cross(rDotFcB_B); - rotEnergyContr = 1.0 / 2.0 * omega_FN_B.dot(this->IPntFc_B * omega_FN_B) - + 1.0 / 2.0 * this->mass * rDotFcB_B.dot(rDotFcB_B) - + 1.0 / 2.0 * this->k * (this->rho - this->rhoRef) * (this->rho - this->rhoRef); + rotEnergyContr = 1.0 / 2.0 * omega_FN_B.dot(this->IPntFc_B * omega_FN_B) + + 1.0 / 2.0 * this->mass * rDotFcB_B.dot(rDotFcB_B) + + 1.0 / 2.0 * this->k * (this->rho - this->rhoRef) * (this->rho - this->rhoRef); } -void linearTranslationOneDOFStateEffector::computeTranslatingBodyInertialStates() +void +linearTranslationOneDOFStateEffector::computeTranslatingBodyInertialStates() { Eigen::Matrix3d dcm_FN = this->dcm_FB * this->dcm_BN; this->sigma_FN = eigenMRPd2Vector3d(eigenC2MRP(dcm_FN)); this->omega_FN_F = this->dcm_FB.transpose() * this->omega_BN_B; - this->r_FcN_N = (Eigen::Vector3d)*this->inertialPositionProperty + this->dcm_BN.transpose() * this->r_FcB_B; + this->r_FcN_N = (Eigen::Vector3d) * this->inertialPositionProperty + this->dcm_BN.transpose() * this->r_FcB_B; Eigen::Vector3d rDot_FcB_B = this->rPrime_FcB_B + this->omegaTilde_BN_B * this->r_FcB_B; - this->v_FcN_N = (Eigen::Vector3d)*this->inertialVelocityProperty + this->dcm_BN.transpose() * rDot_FcB_B; + this->v_FcN_N = (Eigen::Vector3d) * this->inertialVelocityProperty + this->dcm_BN.transpose() * rDot_FcB_B; } -void linearTranslationOneDOFStateEffector::UpdateState(uint64_t currentSimNanos) +void +linearTranslationOneDOFStateEffector::UpdateState(uint64_t currentSimNanos) { this->readInputMessages(); this->computeTranslatingBodyInertialStates(); diff --git a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h index 487d95b291..a2b0d0cd7d 100644 --- a/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h +++ b/src/simulation/dynamics/linearTranslationalBodies/linearTranslationBodiesOneDOF/linearTranslationOneDOFStateEffector.h @@ -17,7 +17,6 @@ */ - #ifndef LINEAR_TRANSLATION_ONE_DOF_STATE_EFFECTOR_H #define LINEAR_TRANSLATION_ONE_DOF_STATE_EFFECTOR_H @@ -33,124 +32,142 @@ #include "architecture/messaging/messaging.h" /*! @brief linear spring mass damper state effector class */ -class linearTranslationOneDOFStateEffector : - public StateEffector, public SysModel +class linearTranslationOneDOFStateEffector + : public StateEffector + , public SysModel { -public: - Message translatingBodyOutMsg; //!< state output message - Message translatingBodyConfigLogOutMsg; //!< translating body state config log message - ReadFunctor motorForceInMsg; //!< (optional) motor force input message - ReadFunctor translatingBodyRefInMsg; //!< (optional) reference state input message - ReadFunctor motorLockInMsg; //!< (optional) lock flag input message + public: + Message translatingBodyOutMsg; //!< state output message + Message translatingBodyConfigLogOutMsg; //!< translating body state config log message + ReadFunctor motorForceInMsg; //!< (optional) motor force input message + ReadFunctor + translatingBodyRefInMsg; //!< (optional) reference state input message + ReadFunctor motorLockInMsg; //!< (optional) lock flag input message - linearTranslationOneDOFStateEffector(); //!< Constructor - ~linearTranslationOneDOFStateEffector(); //!< Destructor + linearTranslationOneDOFStateEffector(); //!< Constructor + ~linearTranslationOneDOFStateEffector(); //!< Destructor /** setter for `mass` property */ void setMass(double mass); /** 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;}; + void setRhoInit(double rhoInit) { this->rhoInit = rhoInit; }; /** setter for `rhoDotInit` property */ - void setRhoDotInit(double rhoDotInit) {this->rhoDotInit = rhoDotInit;}; + void setRhoDotInit(double rhoDotInit) { this->rhoDotInit = rhoDotInit; }; /** setter for `fHat_B` property */ void setFHat_B(Eigen::Vector3d fHat_B); /** setter for `r_FcF_F` property */ - void setR_FcF_F(Eigen::Vector3d r_FcF_F) {this->r_FcF_F = r_FcF_F;}; + void setR_FcF_F(Eigen::Vector3d r_FcF_F) { this->r_FcF_F = r_FcF_F; }; /** setter for `r_F0B_B` property */ - void setR_F0B_B(Eigen::Vector3d r_F0B_B) {this->r_F0B_B = r_F0B_B;}; + void setR_F0B_B(Eigen::Vector3d r_F0B_B) { this->r_F0B_B = r_F0B_B; }; /** setter for `IPntFc_F` property */ - void setIPntFc_F(Eigen::Matrix3d IPntFc_F) {this->IPntFc_F = IPntFc_F;}; + void setIPntFc_F(Eigen::Matrix3d IPntFc_F) { this->IPntFc_F = IPntFc_F; }; /** setter for `dcm_FB` property */ - void setDCM_FB(Eigen::Matrix3d dcm_FB) {this->dcm_FB = dcm_FB;}; + void setDCM_FB(Eigen::Matrix3d dcm_FB) { this->dcm_FB = dcm_FB; }; /** setter for `mass` property */ - double getMass() const {return this->mass;}; + double getMass() const { return this->mass; }; /** setter for `k` property */ - double getK() const {return this->k;}; + double getK() const { return this->k; }; /** setter for `c` property */ - double getC() const {return this->c;}; + double getC() const { return this->c; }; /** setter for `rhoInit` property */ - double getRhoInit() const {return this->rhoInit;}; + double getRhoInit() const { return this->rhoInit; }; /** setter for `rhoDotInit` property */ - double getRhoDotInit() const {return this->rhoDotInit;}; + double getRhoDotInit() const { return this->rhoDotInit; }; /** setter for `fHat_B` property */ - Eigen::Vector3d getFHat_B() const {return this->fHat_B;}; + Eigen::Vector3d getFHat_B() const { return this->fHat_B; }; /** setter for `r_FcF_F` property */ - Eigen::Vector3d getR_FcF_F() const {return this->r_FcF_F;}; + Eigen::Vector3d getR_FcF_F() const { return this->r_FcF_F; }; /** setter for `r_F0B_B` property */ - Eigen::Vector3d getR_F0B_B() const {return this->r_F0B_B;}; + Eigen::Vector3d getR_F0B_B() const { return this->r_F0B_B; }; /** setter for `IPntFc_F` property */ - Eigen::Matrix3d getIPntFc_F() const {return IPntFc_F;}; + Eigen::Matrix3d getIPntFc_F() const { return IPntFc_F; }; /** setter for `dcm_FB` property */ - Eigen::Matrix3d getDCM_FB() const {return dcm_FB;}; - -private: - double mass = 1.0; //!< [kg] mass of effector - double k = 0; //!< [N/m] linear spring constant - double c = 0; //!< [N-s/m] linear damping term - double rhoInit = 0; //!< [m] initial displacement offset - double rhoDotInit = 0; //!< [m/s] Initial displacement rate offset - Eigen::Vector3d fHat_B {1.0, 0.0, 0.0}; //!< unit vector axis of translation in B frame components. - Eigen::Vector3d r_FcF_F = Eigen::Vector3d::Zero(); //!< [m] vector pointing from location F to FC in F frame components - Eigen::Vector3d r_F0B_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from body frame B origin to point to F0 origin of F frame in B frame components - Eigen::Matrix3d IPntFc_F = Eigen::Matrix3d::Identity(); //!< [kg-m^2] Inertia of pc about point Fc in F frame component - Eigen::Matrix3d dcm_FB = Eigen::Matrix3d::Identity(); //!< DCM from the F frame to the body frame - std::string nameOfRhoState{}; //!< Identifier for the rho state data container - std::string nameOfRhoDotState{}; //!< Identifier for the rhoDot state data container - - bool isAxisLocked = false; //!< flag for locking the translation axis - double rho = 0.0; //!< [m] displacement from equilibrium - double rhoDot = 0.0; //!< [m/s] time derivative of displacement from equilibrium - double rhoRef = 0.0; //!< [m] translating body reference position - double rhoDotRef = 0.0; //!< [m/s] translating body reference velocity - double motorForce = 0.0; //!< [N] optional motor force - Eigen::Vector3d r_FcB_B = Eigen::Vector3d::Zero(); //!< [m] position vector from B to center of mass location of effector - Eigen::Vector3d r_FcF0_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from point p0 origin of F frame to center of mass location of effector in B frame components - Eigen::Matrix3d rTilde_FcF_B = Eigen::Matrix3d::Zero(); //!< [m] tilde matrix of r_FcF_B - Eigen::Vector3d rPrime_FcF_B = Eigen::Vector3d::Zero(); //!< [m/s] Body time derivative of r_FcF_B - Eigen::Matrix3d rPrimeTilde_FcF_B = Eigen::Matrix3d::Zero(); //!< [m/s] Tilde matrix of rPrime_FcF_B - Eigen::Matrix3d rTilde_FcB_B = Eigen::Matrix3d::Zero(); //!< [m] tilde matrix of r_FcB_B - Eigen::Vector3d rPrime_FcB_B = Eigen::Vector3d::Zero(); //!< [m/s] Body time derivative of r_FcB_B - Eigen::Matrix3d rPrimeTilde_FcB_B = Eigen::Matrix3d::Zero(); //!< [m/s] Tilde matrix of rPrime_FcB_B - Eigen::Matrix3d IPntFc_B = Eigen::Matrix3d::Identity(); //!< [kg-m^2] Inertia of Fc about point B in B frame components - Eigen::Matrix3d dcm_BN = Eigen::Matrix3d::Identity(); //!< DCM from the B frame to the N frame - Eigen::Vector3d omega_BN_B = Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components. - Eigen::Matrix3d omegaTilde_BN_B = Eigen::Matrix3d::Zero(); //!< [rad/s] tilde matrix of omega_BN_B - - Eigen::Vector3d aRho = Eigen::Vector3d::Zero(); //!< Term needed for back-sub method - Eigen::Vector3d bRho = Eigen::Vector3d::Zero(); //!< Term needed for back-sub method - double cRho = 0.0; //!< Term needed for back-sub method - - StateData *rhoState = nullptr; //!< state data for displacement from equilibrium - StateData *rhoDotState = nullptr; //!< state data for time derivative of rho; - Eigen::MatrixXd *g_N = nullptr; //!< [m/s^2] gravitational acceleration in N frame components - Eigen::MatrixXd* inertialPositionProperty = nullptr; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - Eigen::MatrixXd* inertialVelocityProperty = nullptr; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase - static uint64_t effectorID; //!< ID number of this panel - - Eigen::Vector3d r_FcN_N = Eigen::Vector3d::Zero(); //!< [m] position vector of translating body's center of mass Fc relative to the inertial frame origin N - Eigen::Vector3d v_FcN_N = Eigen::Vector3d::Zero(); //!< [m/s] inertial velocity vector of Fc relative to inertial frame - Eigen::Vector3d sigma_FN = Eigen::Vector3d::Zero(); //!< MRP attitude of frame F relative to inertial frame - Eigen::Vector3d omega_FN_F = Eigen::Vector3d::Zero(); //!< [rad/s] inertial translating body frame angular velocity vector + Eigen::Matrix3d getDCM_FB() const { return dcm_FB; }; + + private: + double mass = 1.0; //!< [kg] mass of effector + double k = 0; //!< [N/m] linear spring constant + double c = 0; //!< [N-s/m] linear damping term + double rhoInit = 0; //!< [m] initial displacement offset + double rhoDotInit = 0; //!< [m/s] Initial displacement rate offset + Eigen::Vector3d fHat_B{ 1.0, 0.0, 0.0 }; //!< unit vector axis of translation in B frame components. + Eigen::Vector3d r_FcF_F = + Eigen::Vector3d::Zero(); //!< [m] vector pointing from location F to FC in F frame components + Eigen::Vector3d r_F0B_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from body frame B origin to point to F0 + //!< origin of F frame in B frame components + Eigen::Matrix3d IPntFc_F = + Eigen::Matrix3d::Identity(); //!< [kg-m^2] Inertia of pc about point Fc in F frame component + Eigen::Matrix3d dcm_FB = Eigen::Matrix3d::Identity(); //!< DCM from the F frame to the body frame + std::string nameOfRhoState{}; //!< Identifier for the rho state data container + std::string nameOfRhoDotState{}; //!< Identifier for the rhoDot state data container + + bool isAxisLocked = false; //!< flag for locking the translation axis + double rho = 0.0; //!< [m] displacement from equilibrium + double rhoDot = 0.0; //!< [m/s] time derivative of displacement from equilibrium + double rhoRef = 0.0; //!< [m] translating body reference position + double rhoDotRef = 0.0; //!< [m/s] translating body reference velocity + double motorForce = 0.0; //!< [N] optional motor force + Eigen::Vector3d r_FcB_B = + Eigen::Vector3d::Zero(); //!< [m] position vector from B to center of mass location of effector + Eigen::Vector3d r_FcF0_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from point p0 origin of F frame to + //!< center of mass location of effector in B frame components + Eigen::Matrix3d rTilde_FcF_B = Eigen::Matrix3d::Zero(); //!< [m] tilde matrix of r_FcF_B + Eigen::Vector3d rPrime_FcF_B = Eigen::Vector3d::Zero(); //!< [m/s] Body time derivative of r_FcF_B + Eigen::Matrix3d rPrimeTilde_FcF_B = Eigen::Matrix3d::Zero(); //!< [m/s] Tilde matrix of rPrime_FcF_B + Eigen::Matrix3d rTilde_FcB_B = Eigen::Matrix3d::Zero(); //!< [m] tilde matrix of r_FcB_B + Eigen::Vector3d rPrime_FcB_B = Eigen::Vector3d::Zero(); //!< [m/s] Body time derivative of r_FcB_B + Eigen::Matrix3d rPrimeTilde_FcB_B = Eigen::Matrix3d::Zero(); //!< [m/s] Tilde matrix of rPrime_FcB_B + Eigen::Matrix3d IPntFc_B = + Eigen::Matrix3d::Identity(); //!< [kg-m^2] Inertia of Fc about point B in B frame components + Eigen::Matrix3d dcm_BN = Eigen::Matrix3d::Identity(); //!< DCM from the B frame to the N frame + Eigen::Vector3d omega_BN_B = + Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components. + Eigen::Matrix3d omegaTilde_BN_B = Eigen::Matrix3d::Zero(); //!< [rad/s] tilde matrix of omega_BN_B + + Eigen::Vector3d aRho = Eigen::Vector3d::Zero(); //!< Term needed for back-sub method + Eigen::Vector3d bRho = Eigen::Vector3d::Zero(); //!< Term needed for back-sub method + double cRho = 0.0; //!< Term needed for back-sub method + + StateData* rhoState = nullptr; //!< state data for displacement from equilibrium + StateData* rhoDotState = nullptr; //!< state data for time derivative of rho; + Eigen::MatrixXd* g_N = nullptr; //!< [m/s^2] gravitational acceleration in N frame components + Eigen::MatrixXd* inertialPositionProperty = + nullptr; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialVelocityProperty = + nullptr; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase + static uint64_t effectorID; //!< ID number of this panel + + Eigen::Vector3d r_FcN_N = Eigen::Vector3d::Zero(); //!< [m] position vector of translating body's center of mass Fc + //!< relative to the inertial frame origin N + Eigen::Vector3d v_FcN_N = + Eigen::Vector3d::Zero(); //!< [m/s] inertial velocity vector of Fc relative to inertial frame + Eigen::Vector3d sigma_FN = Eigen::Vector3d::Zero(); //!< MRP attitude of frame F relative to inertial frame + Eigen::Vector3d omega_FN_F = + Eigen::Vector3d::Zero(); //!< [rad/s] inertial translating body frame angular velocity vector void Reset(uint64_t CurrentClock) override; - void registerStates(DynParamManager& states) override; - void linkInStates(DynParamManager& states) override; + void registerStates(DynParamManager& states) override; + void linkInStates(DynParamManager& states) override; void writeOutputStateMessages(uint64_t CurrentSimNanos) override; void updateEffectorMassProps(double integTime) override; void updateContributions(double integTime, - BackSubMatrices & backSubContr, + BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) override; - void updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B) override; - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, - Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) override; + void updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) override; + void computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) override; void UpdateState(uint64_t CurrentSimNanos) override; void computeTranslatingBodyInertialStates(); 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..d70e491bfd 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 @@ -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) @@ -168,5 +182,3 @@ def msmForceTorqueTestFunction(show_plots, accuracy): if __name__ == "__main__": test_msmForceTorque(False, 1e-4) - - diff --git a/src/simulation/dynamics/msmForceTorque/msmForceTorque.cpp b/src/simulation/dynamics/msmForceTorque/msmForceTorque.cpp index 6dfc2bdda5..d27ececd99 100644 --- a/src/simulation/dynamics/msmForceTorque/msmForceTorque.cpp +++ b/src/simulation/dynamics/msmForceTorque/msmForceTorque.cpp @@ -17,27 +17,24 @@ */ - #include "simulation/dynamics/msmForceTorque/msmForceTorque.h" #include /*! This is the constructor for the module class. It sets default variable values and initializes the various parts of the model */ -MsmForceTorque::MsmForceTorque() -{ -} +MsmForceTorque::MsmForceTorque() {} /*! Module Destructor */ MsmForceTorque::~MsmForceTorque() { /* free up output message objects */ - for (long unsigned int c=0; ceTorqueOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->eTorqueOutMsgs.size(); c++) { delete this->eTorqueOutMsgs.at(c); } - for (long unsigned int c=0; ceForceOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->eForceOutMsgs.size(); c++) { delete this->eForceOutMsgs.at(c); } - for (long unsigned int c=0; cchargeMsmOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->chargeMsmOutMsgs.size(); c++) { delete this->chargeMsmOutMsgs.at(c); } } @@ -45,29 +42,31 @@ MsmForceTorque::~MsmForceTorque() /*! This method is used to reset the module and checks that required input messages are connect. */ -void MsmForceTorque::Reset(uint64_t CurrentSimNanos) +void +MsmForceTorque::Reset(uint64_t CurrentSimNanos) { // check that required input messages are connected - for (long unsigned int c=0; c < this->scStateInMsgs.size(); c++ ){ + for (long unsigned int c = 0; c < this->scStateInMsgs.size(); c++) { if (!this->scStateInMsgs.at(c).isLinked()) { bskLogger.bskLog(BSK_ERROR, "MsmForceTorque.scStateInMsgs[%d] was not linked.", c); } } - for (long unsigned int c=0; c < this->voltInMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->voltInMsgs.size(); c++) { if (!this->voltInMsgs.at(c).isLinked()) { bskLogger.bskLog(BSK_ERROR, "MsmForceTorque.voltInMsgs[%d] was not linked.", c); } } - this->numSat = (uint32_t) this->scStateInMsgs.size(); + this->numSat = (uint32_t)this->scStateInMsgs.size(); if (this->numSat < 2) { - bskLogger.bskLog(BSK_ERROR, "MsmForceTorque must have 2 or more spacecraft components added. You added %lu.", this->numSat); + bskLogger.bskLog( + BSK_ERROR, "MsmForceTorque must have 2 or more spacecraft components added. You added %lu.", this->numSat); } /* determine number of spheres being modeled */ this->numSpheres = 0; - for (long unsigned int c=0; c < this->numSat; c++) { + for (long unsigned int c = 0; c < this->numSat; c++) { this->numSpheres += this->radiiList.at(c).size(); } if (this->numSpheres == 0) { @@ -79,9 +78,10 @@ void MsmForceTorque::Reset(uint64_t CurrentSimNanos) /*! Subscribe to the spacecraft state message and store the corresponding MSM radii and sphere positions */ -void MsmForceTorque::addSpacecraftToModel(Message *tmpScMsg - , std::vector radii - , std::vector r_SB_B) +void +MsmForceTorque::addSpacecraftToModel(Message* tmpScMsg, + std::vector radii, + std::vector r_SB_B) { /* add the message reader to the vector of input spacecraft state messages */ this->scStateInMsgs.push_back(tmpScMsg->addSubscriber()); @@ -92,7 +92,11 @@ void MsmForceTorque::addSpacecraftToModel(Message *tmpScMsg /* store MSM sphere radii and location information */ if (radii.size() != r_SB_B.size()) { - bskLogger.bskLog(BSK_ERROR, "MsmForceTorque:addSpacecraftToModel() The vector of MSM radii and positions must have the same size, they have sizes %lu and %lu.", radii.size(), r_SB_B.size()); + bskLogger.bskLog(BSK_ERROR, + "MsmForceTorque:addSpacecraftToModel() The vector of MSM radii and positions must have the " + "same size, they have sizes %lu and %lu.", + radii.size(), + r_SB_B.size()); } this->radiiList.push_back(radii); this->r_SB_BList.push_back(r_SB_B); @@ -106,27 +110,27 @@ void MsmForceTorque::addSpacecraftToModel(Message *tmpScMsg this->sigma_BNList.push_back(zeroMRP); /* create output message objects */ - Message *msgTorque; + Message* msgTorque; msgTorque = new Message; this->eTorqueOutMsgs.push_back(msgTorque); - Message *msgForce; + Message* msgForce; msgForce = new Message; this->eForceOutMsgs.push_back(msgForce); - Message *msmCharge; + Message* msmCharge; msmCharge = new Message; this->chargeMsmOutMsgs.push_back(msmCharge); - } /*! Read in the input messages */ -void MsmForceTorque::readMessages() +void +MsmForceTorque::readMessages() { - VoltMsgPayload voltInMsgBuffer; //!< local copy of voltage input message buffer - SCStatesMsgPayload scStateInMsgsBuffer; //!< local copy of spacecraft state input message buffer - long unsigned int c; //!< spacecraft loop counter + VoltMsgPayload voltInMsgBuffer; //!< local copy of voltage input message buffer + SCStatesMsgPayload scStateInMsgsBuffer; //!< local copy of spacecraft state input message buffer + long unsigned int c; //!< spacecraft loop counter for (c = 0; c < this->numSat; c++) { voltInMsgBuffer = this->voltInMsgs.at(c)(); @@ -138,29 +142,29 @@ void MsmForceTorque::readMessages() } } - /*! This is the main method that gets called every time the module is updated. Provide an appropriate description. */ -void MsmForceTorque::UpdateState(uint64_t CurrentSimNanos) +void +MsmForceTorque::UpdateState(uint64_t CurrentSimNanos) { // read the input messages this->readMessages(); // compute the electrostatic forces and torques - Eigen::MatrixXd S; //!< [1/m] Elastance matrix divided by kc - Eigen::VectorXd V; //!< [V] vector of sphere voltages - Eigen::VectorXd q; //!< [C] vector of sphere charges - double kc; //!< [Nm^2/C^2] Coulomb's constant - std::vector r_SN_NList; //!< [m] list of inertial sphere locations - Eigen::Matrix3d dcm_NB; //!< [] DCM from body B to inertial frame N - Eigen::Vector3d r_BN_N; //!< [m] spacecraft inertial position vector - Eigen::Vector3d r_ij_N; //!< [m] relative position vector between ith and jth spheres - double r_ij; //!< [m] norm of r_ij_N - long unsigned int counter; //!< [] loop counter - CmdForceInertialMsgPayload forceMsgBuffer; //!< [] force out message buffer - CmdTorqueBodyMsgPayload torqueMsgBuffer; //!< [] torque out message buffer - ChargeMsmMsgPayload chargeMsmMsgBuffer; //!< [] MSM charge message buffer + Eigen::MatrixXd S; //!< [1/m] Elastance matrix divided by kc + Eigen::VectorXd V; //!< [V] vector of sphere voltages + Eigen::VectorXd q; //!< [C] vector of sphere charges + double kc; //!< [Nm^2/C^2] Coulomb's constant + std::vector r_SN_NList; //!< [m] list of inertial sphere locations + Eigen::Matrix3d dcm_NB; //!< [] DCM from body B to inertial frame N + Eigen::Vector3d r_BN_N; //!< [m] spacecraft inertial position vector + Eigen::Vector3d r_ij_N; //!< [m] relative position vector between ith and jth spheres + double r_ij; //!< [m] norm of r_ij_N + long unsigned int counter; //!< [] loop counter + CmdForceInertialMsgPayload forceMsgBuffer; //!< [] force out message buffer + CmdTorqueBodyMsgPayload torqueMsgBuffer; //!< [] torque out message buffer + ChargeMsmMsgPayload chargeMsmMsgBuffer; //!< [] MSM charge message buffer kc = 8.99e9; @@ -170,10 +174,10 @@ void MsmForceTorque::UpdateState(uint64_t CurrentSimNanos) q.resize(this->numSpheres); /* determine inertial sphere locations */ - for (long unsigned int c=0; c < this->numSat; c++) { + for (long unsigned int c = 0; c < this->numSat; c++) { dcm_NB = this->sigma_BNList.at(c).toRotationMatrix(); r_BN_N = this->r_BN_NList.at(c); - for (long unsigned int k=0; k < this->radiiList.at(c).size(); k++) { + for (long unsigned int k = 0; k < this->radiiList.at(c).size(); k++) { r_SN_NList.push_back(r_BN_N + dcm_NB * this->r_SB_BList.at(c).at(k)); } } @@ -183,20 +187,20 @@ void MsmForceTorque::UpdateState(uint64_t CurrentSimNanos) */ /* setup diagonal S matrix and voltage components */ counter = 0; - for (long unsigned int c=0; c < this->numSat; c++) { - for (long unsigned int k=0; k < this->radiiList.at(c).size(); k++) { - S(counter, counter) = kc/this->radiiList.at(c).at(k); + for (long unsigned int c = 0; c < this->numSat; c++) { + for (long unsigned int k = 0; k < this->radiiList.at(c).size(); k++) { + S(counter, counter) = kc / this->radiiList.at(c).at(k); V(counter) = this->volt.at(c); counter++; } } /* setup off-diagonal components */ - for (long unsigned int i=0; i < this->numSpheres; i++) { - for (long unsigned int j=0; j < this->numSpheres; j++) { + for (long unsigned int i = 0; i < this->numSpheres; i++) { + for (long unsigned int j = 0; j < this->numSpheres; j++) { if (i != j) { r_ij_N = r_SN_NList.at(i) - r_SN_NList.at(j); - S(i,j) = kc / r_ij_N.norm(); - S(j,i) = S(i,j); + S(i, j) = kc / r_ij_N.norm(); + S(j, i) = S(i, j); } } } @@ -206,14 +210,14 @@ void MsmForceTorque::UpdateState(uint64_t CurrentSimNanos) /* find forces and torques acting on each space object */ counter = 0; - long unsigned int i0 = 0; // counter where the MSM sphere charges start in the q vector - long unsigned int i1; // counter where the next spacecraft MSM sphere charges start + long unsigned int i0 = 0; // counter where the MSM sphere charges start in the q vector + long unsigned int i1; // counter where the next spacecraft MSM sphere charges start // loop over all satellites - for (long unsigned int c=0; c < this->numSat; c++) { - Eigen::Vector3d netForce_N; // net force acting on spacecraft - Eigen::Vector3d netTorque_B; // net torque acting on spacecraft in B frame - Eigen::Vector3d force_N; // force acting on a sphere - Eigen::Matrix3d dcm_BN; // DCM from inertial frame N to body B + for (long unsigned int c = 0; c < this->numSat; c++) { + Eigen::Vector3d netForce_N; // net force acting on spacecraft + Eigen::Vector3d netTorque_B; // net torque acting on spacecraft in B frame + Eigen::Vector3d force_N; // force acting on a sphere + Eigen::Matrix3d dcm_BN; // DCM from inertial frame N to body B netForce_N.setZero(); netTorque_B.setZero(); @@ -229,18 +233,21 @@ void MsmForceTorque::UpdateState(uint64_t CurrentSimNanos) chargeMsmMsgBuffer = this->chargeMsmOutMsgs.at(c)->zeroMsgPayload; // loop over current body spheres - for (long unsigned int j=i0; jnumSpheres; i++) { - if (i=i1) { + for (long unsigned int i = 0; i < this->numSpheres; i++) { + if (i < i0 || i >= i1) { r_ij_N = r_SN_NList.at(i) - r_SN_NList.at(j); r_ij = r_ij_N.norm(); // check if separation is larger then current MSM sphere radius - if (r_ij > this->radiiList.at(c).at(j-i0)) { - force_N -= kc * q(j)*q(i) * r_ij_N/r_ij/r_ij/r_ij; + if (r_ij > this->radiiList.at(c).at(j - i0)) { + force_N -= kc * q(j) * q(i) * r_ij_N / r_ij / r_ij / r_ij; } else { - bskLogger.bskLog(BSK_WARNING, "MsmForceTorque: spacecraft %lu, sphere %lu is too close to another sphere.", c, j-i0); + bskLogger.bskLog(BSK_WARNING, + "MsmForceTorque: spacecraft %lu, sphere %lu is too close to another sphere.", + c, + j - i0); } } } @@ -248,7 +255,7 @@ void MsmForceTorque::UpdateState(uint64_t CurrentSimNanos) netForce_N += force_N; // add to total torque acting on spacecraft - netTorque_B += this->r_SB_BList.at(c).at(j-i0).cross(dcm_BN * force_N); + netTorque_B += this->r_SB_BList.at(c).at(j - i0).cross(dcm_BN * force_N); } // store net force and torque acting on body @@ -258,11 +265,10 @@ void MsmForceTorque::UpdateState(uint64_t CurrentSimNanos) this->eTorqueOutMsgs.at(c)->write(&torqueMsgBuffer, this->moduleID, CurrentSimNanos); // store MSM charges to output message - chargeMsmMsgBuffer.q = q.segment(i0, i1-i0); + chargeMsmMsgBuffer.q = q.segment(i0, i1 - i0); this->chargeMsmOutMsgs.at(c)->write(&chargeMsmMsgBuffer, this->moduleID, CurrentSimNanos); // set the body sphere start counter i0 = i1; } - } diff --git a/src/simulation/dynamics/msmForceTorque/msmForceTorque.h b/src/simulation/dynamics/msmForceTorque/msmForceTorque.h index ce47b217c9..5191e9c932 100644 --- a/src/simulation/dynamics/msmForceTorque/msmForceTorque.h +++ b/src/simulation/dynamics/msmForceTorque/msmForceTorque.h @@ -17,7 +17,6 @@ */ - #ifndef MSMFORCETORQUE_H #define MSMFORCETORQUE_H @@ -33,39 +32,46 @@ #include #include -/*! @brief This module uses the Multi-Sphere-Method (MSM) to evaluate the mutual electrostatic force and torque interactions between a series of spacecraft object. The charging is specified through a voltage where the object is assumed to have aconstant voltaged across the surface. The MSM model for each space object is given through a list of body-fixed sphere locations and sphere radii. See `Multi-Sphere Method for Modeling Electrostatic Forces and Torques `__ for more information on the MSM method. +/*! @brief This module uses the Multi-Sphere-Method (MSM) to evaluate the mutual electrostatic force and torque + * interactions between a series of spacecraft object. The charging is specified through a voltage where the object is + * assumed to have aconstant voltaged across the surface. The MSM model for each space object is given through a list + * of body-fixed sphere locations and sphere radii. See `Multi-Sphere Method for Modeling Electrostatic Forces and + * Torques `__ for more information on the MSM method. */ -class MsmForceTorque: public SysModel { -public: +class MsmForceTorque : public SysModel +{ + public: MsmForceTorque(); ~MsmForceTorque(); void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); - void addSpacecraftToModel(Message *tmpScMsg, std::vector radii, std::vector r_SB_B); + void addSpacecraftToModel(Message* tmpScMsg, + std::vector radii, + std::vector r_SB_B); -private: + private: void readMessages(); - -public: + + public: std::vector> scStateInMsgs; //!< vector of spacecraft state input messages - std::vector> voltInMsgs; //!< vector of voltage input messages + std::vector> voltInMsgs; //!< vector of voltage input messages - std::vector*> eTorqueOutMsgs; //!< vector of E-torques in body frame components - std::vector*> eForceOutMsgs; //!< vector of E-forces in inertial frame components - std::vector*> chargeMsmOutMsgs; //!< vector of spacecraft MSM charge values + std::vector*> eTorqueOutMsgs; //!< vector of E-torques in body frame components + std::vector*> + eForceOutMsgs; //!< vector of E-forces in inertial frame components + std::vector*> chargeMsmOutMsgs; //!< vector of spacecraft MSM charge values - BSKLogger bskLogger; //!< -- BSK Logging + BSKLogger bskLogger; //!< -- BSK Logging -private: - std::vector> radiiList; //!< vector of MSM sphere radii - std::vector> r_SB_BList; //!< vector of body-fixed MSM sphere locations - unsigned int numSat; //!< number of satellites - unsigned int numSpheres; //!< numer of spheres being modeled - std::vector volt; //!< [V] input voltage for each spacecrat object - std::vector r_BN_NList; //!< [m] list of inertial satellite position vectors - std::vector sigma_BNList; //!< [m] list of satellite MRP orientations + private: + std::vector> radiiList; //!< vector of MSM sphere radii + std::vector> r_SB_BList; //!< vector of body-fixed MSM sphere locations + unsigned int numSat; //!< number of satellites + unsigned int numSpheres; //!< numer of spheres being modeled + std::vector volt; //!< [V] input voltage for each spacecrat object + std::vector r_BN_NList; //!< [m] list of inertial satellite position vectors + std::vector sigma_BNList; //!< [m] list of satellite MRP orientations }; - #endif 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/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/prescribedMotion/prescribedMotionStateEffector.cpp b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.cpp index e27c6903ba..b9d0e5942e 100644 --- a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.cpp +++ b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.cpp @@ -73,7 +73,8 @@ PrescribedMotionStateEffector::~PrescribedMotionStateEffector() @param currentClock [ns] Time the method is called */ -void PrescribedMotionStateEffector::Reset(uint64_t currentClock) +void +PrescribedMotionStateEffector::Reset(uint64_t currentClock) { } @@ -81,12 +82,12 @@ void PrescribedMotionStateEffector::Reset(uint64_t currentClock) @param currentClock [ns] Time the method is called */ -void PrescribedMotionStateEffector::writeOutputStateMessages(uint64_t currentClock) +void +PrescribedMotionStateEffector::writeOutputStateMessages(uint64_t currentClock) { // Write the prescribed translational motion output message if it is linked - if (this->prescribedTranslationOutMsg.isLinked()) - { + if (this->prescribedTranslationOutMsg.isLinked()) { PrescribedTranslationMsgPayload prescribedTranslationBuffer = this->prescribedTranslationOutMsg.zeroMsgPayload; eigenVector3d2CArray(this->r_PM_M, prescribedTranslationBuffer.r_PM_M); eigenVector3d2CArray(this->rPrime_PM_M, prescribedTranslationBuffer.rPrime_PM_M); @@ -95,8 +96,7 @@ void PrescribedMotionStateEffector::writeOutputStateMessages(uint64_t currentClo } // Write the prescribed rotational motion output message if it is linked - if (this->prescribedRotationOutMsg.isLinked()) - { + if (this->prescribedRotationOutMsg.isLinked()) { PrescribedRotationMsgPayload prescribedRotationBuffer = this->prescribedRotationOutMsg.zeroMsgPayload; eigenVector3d2CArray(this->omega_PM_P, prescribedRotationBuffer.omega_PM_P); eigenVector3d2CArray(this->omegaPrime_PM_P, prescribedRotationBuffer.omegaPrime_PM_P); @@ -106,8 +106,7 @@ void PrescribedMotionStateEffector::writeOutputStateMessages(uint64_t currentClo } // Write the config log message if it is linked - if (this->prescribedMotionConfigLogOutMsg.isLinked()) - { + if (this->prescribedMotionConfigLogOutMsg.isLinked()) { SCStatesMsgPayload configLogMsg = this->prescribedMotionConfigLogOutMsg.zeroMsgPayload; // Note that the configLogMsg B frame represents the effector body frame (frame P) @@ -123,7 +122,8 @@ void PrescribedMotionStateEffector::writeOutputStateMessages(uint64_t currentClo @param statesIn Pointer to give the state effector access the hub states */ -void PrescribedMotionStateEffector::linkInStates(DynParamManager& statesIn) +void +PrescribedMotionStateEffector::linkInStates(DynParamManager& statesIn) { // Get access to the hub states needed for dynamic coupling this->inertialPositionProperty = statesIn.getPropertyReference(this->propName_inertialPosition); @@ -134,15 +134,16 @@ void PrescribedMotionStateEffector::linkInStates(DynParamManager& statesIn) @param states Pointer to give the state effector access the hub states */ -void PrescribedMotionStateEffector::registerStates(DynParamManager& states) +void +PrescribedMotionStateEffector::registerStates(DynParamManager& states) { - this->sigma_PMState = states.registerState(3, 1, this->nameOfsigma_PMState); - Eigen::Vector3d sigma_PM_loc = eigenMRPd2Vector3d(this->sigma_PM); - Eigen::Vector3d sigma_PMInitMatrix; - sigma_PMInitMatrix(0) = sigma_PM_loc[0]; - sigma_PMInitMatrix(1) = sigma_PM_loc[1]; - sigma_PMInitMatrix(2) = sigma_PM_loc[2]; - this->sigma_PMState->setState(sigma_PMInitMatrix); + this->sigma_PMState = states.registerState(3, 1, this->nameOfsigma_PMState); + Eigen::Vector3d sigma_PM_loc = eigenMRPd2Vector3d(this->sigma_PM); + Eigen::Vector3d sigma_PMInitMatrix; + sigma_PMInitMatrix(0) = sigma_PM_loc[0]; + sigma_PMInitMatrix(1) = sigma_PM_loc[1]; + sigma_PMInitMatrix(2) = sigma_PM_loc[2]; + this->sigma_PMState->setState(sigma_PMInitMatrix); } /*! This method allows the state effector to provide its contributions to the mass props and mass prop rates of the @@ -150,7 +151,8 @@ void PrescribedMotionStateEffector::registerStates(DynParamManager& states) @param integTime [s] Time the method is called */ -void PrescribedMotionStateEffector::updateEffectorMassProps(double integTime) +void +PrescribedMotionStateEffector::updateEffectorMassProps(double integTime) { // Update the prescribed states double dt = integTime - this->currentSimTimeSec; @@ -203,10 +205,9 @@ void PrescribedMotionStateEffector::updateEffectorMassProps(double integTime) // Find the B frame time derivative of IPntPc_B Eigen::Matrix3d rPrimeTilde_PcB_B = eigenTilde(this->rPrime_PcB_B); - this->effProps.IEffPrimePntB_B = this->omegaTilde_PB_B * this->IPntPc_B - - this->IPntPc_B * this->omegaTilde_PB_B - + this->mass * (rPrimeTilde_PcB_B * this->rTilde_PcB_B.transpose() - + this->rTilde_PcB_B * rPrimeTilde_PcB_B.transpose()); + this->effProps.IEffPrimePntB_B = this->omegaTilde_PB_B * this->IPntPc_B - this->IPntPc_B * this->omegaTilde_PB_B + + this->mass * (rPrimeTilde_PcB_B * this->rTilde_PcB_B.transpose() + + this->rTilde_PcB_B * rPrimeTilde_PcB_B.transpose()); } /*! This method allows the state effector to give its contributions to the matrices needed for the back-sub. @@ -219,11 +220,12 @@ void PrescribedMotionStateEffector::updateEffectorMassProps(double integTime) components @param g_N [m/s^2] Gravitational acceleration in N frame components */ -void PrescribedMotionStateEffector::updateContributions(double integTime, - BackSubMatrices & backSubContr, - Eigen::Vector3d sigma_BN, - Eigen::Vector3d omega_BN_B, - Eigen::Vector3d g_N) +void +PrescribedMotionStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { // Compute dcm_BN this->sigma_BN = sigma_BN; @@ -237,17 +239,18 @@ void PrescribedMotionStateEffector::updateContributions(double integTime, Eigen::Matrix3d omegaPrimeTilde_PB_B = eigenTilde(this->omegaPrime_PB_B); // Compute rPrimePrime_PcB_B - this->rPrimePrime_PcB_B = (omegaPrimeTilde_PB_B + this->omegaTilde_PB_B * this->omegaTilde_PB_B) * this->r_PcP_B + this->rPrimePrime_PM_B; + this->rPrimePrime_PcB_B = + (omegaPrimeTilde_PB_B + this->omegaTilde_PB_B * this->omegaTilde_PB_B) * this->r_PcP_B + this->rPrimePrime_PM_B; // Translation contributions backSubContr.vecTrans = -this->mass * this->rPrimePrime_PcB_B; // Rotation contributions Eigen::Matrix3d IPrimePntPc_B = this->omegaTilde_PB_B * this->IPntPc_B - this->IPntPc_B * this->omegaTilde_PB_B; - backSubContr.vecRot = -(this->mass * this->rTilde_PcB_B * this->rPrimePrime_PcB_B) - - (IPrimePntPc_B + this->omegaTilde_BN_B * this->IPntPc_B) * this->omega_PB_B - - this->IPntPc_B * this->omegaPrime_PB_B - - this->mass * this->omegaTilde_BN_B * rTilde_PcB_B * this->rPrime_PcB_B; + backSubContr.vecRot = -(this->mass * this->rTilde_PcB_B * this->rPrimePrime_PcB_B) - + (IPrimePntPc_B + this->omegaTilde_BN_B * this->IPntPc_B) * this->omega_PB_B - + this->IPntPc_B * this->omegaPrime_PB_B - + this->mass * this->omegaTilde_BN_B * rTilde_PcB_B * this->rPrime_PcB_B; } /*! This method is for defining the state effector's MRP state derivative @@ -259,14 +262,15 @@ void PrescribedMotionStateEffector::updateContributions(double integTime, inertial frame, expressed in B frame components @param sigma_BN Current B frame attitude with respect to the inertial frame */ -void PrescribedMotionStateEffector::computeDerivatives(double integTime, - Eigen::Vector3d rDDot_BN_N, - Eigen::Vector3d omegaDot_BN_B, - Eigen::Vector3d sigma_BN) +void +PrescribedMotionStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { Eigen::MRPd sigma_PM_loc; sigma_PM_loc = (Eigen::Vector3d)this->sigma_PMState->getState(); - this->sigma_PMState->setDerivative(0.25*sigma_PM_loc.Bmat()*this->omega_PM_P); + this->sigma_PMState->setDerivative(0.25 * sigma_PM_loc.Bmat() * this->omega_PM_P); } /*! This method is for calculating the contributions of the effector to the energy and momentum of the spacecraft. @@ -277,10 +281,11 @@ void PrescribedMotionStateEffector::computeDerivatives(double integTime, @param omega_BN_B [rad/s] Angular velocity of the B frame with respect to the inertial frame, expressed in B frame components */ -void PrescribedMotionStateEffector::updateEnergyMomContributions(double integTime, - Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, - Eigen::Vector3d omega_BN_B) +void +PrescribedMotionStateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { // Update omega_BN_B and omega_PN_B this->omega_BN_B = omega_BN_B; @@ -294,38 +299,39 @@ void PrescribedMotionStateEffector::updateEnergyMomContributions(double integTim rotAngMomPntCContr_B = this->IPntPc_B * this->omega_PN_B + this->mass * this->rTilde_PcB_B * this->rDot_PcB_B; // Find the rotational energy contribution from the hub - rotEnergyContr = 0.5 * this->omega_PN_B.dot(this->IPntPc_B * this->omega_PN_B) - + 0.5 * this->mass * this->rDot_PcB_B.dot(this->rDot_PcB_B); + rotEnergyContr = 0.5 * this->omega_PN_B.dot(this->IPntPc_B * this->omega_PN_B) + + 0.5 * this->mass * this->rDot_PcB_B.dot(this->rDot_PcB_B); } /*! This method computes the effector states relative to the inertial frame. */ -void PrescribedMotionStateEffector::computePrescribedMotionInertialStates() +void +PrescribedMotionStateEffector::computePrescribedMotionInertialStates() { // Compute the effector's attitude with respect to the inertial frame Eigen::Matrix3d dcm_PN = (this->dcm_BP).transpose() * this->dcm_BN; this->sigma_PN = eigenMRPd2Vector3d(eigenC2MRP(dcm_PN)); // Compute the effector's inertial position vector - this->r_PcN_N = (Eigen::Vector3d)*this->inertialPositionProperty + this->dcm_BN.transpose() * this->r_PcB_B; + this->r_PcN_N = (Eigen::Vector3d) * this->inertialPositionProperty + this->dcm_BN.transpose() * this->r_PcB_B; // Compute the effector's inertial velocity vector - this->v_PcN_N = (Eigen::Vector3d)*this->inertialVelocityProperty + this->dcm_BN.transpose() * this->rDot_PcB_B; + this->v_PcN_N = (Eigen::Vector3d) * this->inertialVelocityProperty + this->dcm_BN.transpose() * this->rDot_PcB_B; } /*! This method updates the effector state at the dynamics frequency. @param currentSimNanos [ns] Time the method is called */ -void PrescribedMotionStateEffector::UpdateState(uint64_t currentSimNanos) +void +PrescribedMotionStateEffector::UpdateState(uint64_t currentSimNanos) { // Store the current simulation time this->currentSimTimeSec = currentSimNanos * NANO2SEC; // Read the translational input message if it is linked and written - if (this->prescribedTranslationInMsg.isLinked() && this->prescribedTranslationInMsg.isWritten()) - { + if (this->prescribedTranslationInMsg.isLinked() && this->prescribedTranslationInMsg.isWritten()) { PrescribedTranslationMsgPayload incomingPrescribedTransStates = this->prescribedTranslationInMsg(); this->r_PM_M = cArray2EigenVector3d(incomingPrescribedTransStates.r_PM_M); this->rPrime_PM_M = cArray2EigenVector3d(incomingPrescribedTransStates.rPrime_PM_M); @@ -337,8 +343,7 @@ void PrescribedMotionStateEffector::UpdateState(uint64_t currentSimNanos) } // Read the rotational input message if it is linked and written - if (this->prescribedRotationInMsg.isLinked() && this->prescribedRotationInMsg.isWritten()) - { + if (this->prescribedRotationInMsg.isLinked() && this->prescribedRotationInMsg.isWritten()) { PrescribedRotationMsgPayload incomingPrescribedRotStates = this->prescribedRotationInMsg(); this->omega_PM_P = cArray2EigenVector3d(incomingPrescribedRotStates.omega_PM_P); this->omegaPrime_PM_P = cArray2EigenVector3d(incomingPrescribedRotStates.omegaPrime_PM_P); diff --git a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h index ba04be02d0..7c8618e9b7 100644 --- a/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h +++ b/src/simulation/dynamics/prescribedMotion/prescribedMotionStateEffector.h @@ -31,110 +31,127 @@ #include "architecture/utilities/avsEigenMRP.h" /*! @brief prescribed motion state effector class */ -class PrescribedMotionStateEffector: public StateEffector, public SysModel { -public: +class PrescribedMotionStateEffector + : public StateEffector + , public SysModel +{ + public: PrescribedMotionStateEffector(); ~PrescribedMotionStateEffector(); - void Reset(uint64_t currentClock) override; //!< Method for reset - void writeOutputStateMessages(uint64_t currentClock) override; //!< Method for writing the output messages - void UpdateState(uint64_t currentSimNanos) override; //!< Method for updating the effector states - void registerStates(DynParamManager& statesIn) override; //!< Method for registering the effector's states - void linkInStates(DynParamManager& states) override; //!< Method for giving the effector access to hub states - void updateContributions(double integTime, - BackSubMatrices & backSubContr, - Eigen::Vector3d sigma_BN, - Eigen::Vector3d omega_BN_B, - Eigen::Vector3d g_N) override; //!< Method for computing the effector's back-substitution contributions - void computeDerivatives(double integTime, - Eigen::Vector3d rDDot_BN_N, - Eigen::Vector3d omegaDot_BN_B, - Eigen::Vector3d sigma_BN) override; //!< Method for effector to compute its state derivatives - void updateEffectorMassProps(double integTime) override; //!< Method for calculating the effector mass props and prop rates - void updateEnergyMomContributions(double integTime, - Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, - Eigen::Vector3d omega_BN_B) override; //!< Method for computing the energy and momentum of the effector - void computePrescribedMotionInertialStates(); //!< Method for computing the effector's states relative to the inertial frame - - double currentSimTimeSec; //!< [s] Current simulation time, updated at the dynamics frequency - double mass; //!< [kg] Effector mass - Eigen::Matrix3d IPntPc_P; //!< [kg-m^2] Inertia of the effector about its center of mass in P frame components - Eigen::Vector3d r_MB_B; //!< [m] Position of point M relative to point B in B frame components - Eigen::Vector3d r_PcP_P; //!< [m] Position of the effector center of mass relative to point P in P frame components - Eigen::Vector3d omega_MB_B; //!< [rad/s] Angular velocity of frame M with respect to frame B in B frame components - Eigen::Vector3d omegaPrime_MB_B; //!< [rad/s^2] B frame time derivative of omega_MB_B in B frame components - Eigen::MRPd sigma_MB; //!< MRP attitude of frame M relative to frame B + void Reset(uint64_t currentClock) override; //!< Method for reset + void writeOutputStateMessages(uint64_t currentClock) override; //!< Method for writing the output messages + void UpdateState(uint64_t currentSimNanos) override; //!< Method for updating the effector states + void registerStates(DynParamManager& statesIn) override; //!< Method for registering the effector's states + void linkInStates(DynParamManager& states) override; //!< Method for giving the effector access to hub states + void updateContributions( + double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) override; //!< Method for computing the effector's back-substitution contributions + void computeDerivatives( + double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) override; //!< Method for effector to compute its state derivatives + void updateEffectorMassProps( + double integTime) override; //!< Method for calculating the effector mass props and prop rates + void updateEnergyMomContributions( + double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) override; //!< Method for computing the energy and momentum of the effector + void computePrescribedMotionInertialStates(); //!< Method for computing the effector's states relative to the + //!< inertial frame + + double currentSimTimeSec; //!< [s] Current simulation time, updated at the dynamics frequency + double mass; //!< [kg] Effector mass + Eigen::Matrix3d IPntPc_P; //!< [kg-m^2] Inertia of the effector about its center of mass in P frame components + Eigen::Vector3d r_MB_B; //!< [m] Position of point M relative to point B in B frame components + Eigen::Vector3d r_PcP_P; //!< [m] Position of the effector center of mass relative to point P in P frame components + Eigen::Vector3d omega_MB_B; //!< [rad/s] Angular velocity of frame M with respect to frame B in B frame components + Eigen::Vector3d omegaPrime_MB_B; //!< [rad/s^2] B frame time derivative of omega_MB_B in B frame components + Eigen::MRPd sigma_MB; //!< MRP attitude of frame M relative to frame B // Prescribed parameters - Eigen::Vector3d r_PM_M; //!< [m] Position of point P relative to point M in M frame components - Eigen::Vector3d rPrime_PM_M; //!< [m/s] B frame time derivative of r_PM_M in M frame components - Eigen::Vector3d rPrimePrime_PM_M; //!< [m/s^2] B frame time derivative of rPrime_PM_M in M frame components - Eigen::Vector3d omega_PM_P; //!< [rad/s] Angular velocity of frame P relative to frame M in P frame components - Eigen::Vector3d omegaPrime_PM_P; //!< [rad/s^2] B frame time derivative of omega_PM_P in P frame components - Eigen::MRPd sigma_PM; //!< MRP attitude of frame P relative to frame M - std::string nameOfsigma_PMState; //!< Identifier for the sigma_PM state data container - - ReadFunctor prescribedTranslationInMsg; //!< Input message for the effector's translational prescribed states - ReadFunctor prescribedRotationInMsg; //!< Input message for the effector's rotational prescribed states - Message prescribedTranslationOutMsg; //!< Output message for the effector's translational prescribed states - Message prescribedRotationOutMsg; //!< Output message for the effector's rotational prescribed states - Message prescribedMotionConfigLogOutMsg; //!< Output config log message for the effector's states - -private: - static uint64_t effectorID; //!< ID number of this panel + Eigen::Vector3d r_PM_M; //!< [m] Position of point P relative to point M in M frame components + Eigen::Vector3d rPrime_PM_M; //!< [m/s] B frame time derivative of r_PM_M in M frame components + Eigen::Vector3d rPrimePrime_PM_M; //!< [m/s^2] B frame time derivative of rPrime_PM_M in M frame components + Eigen::Vector3d omega_PM_P; //!< [rad/s] Angular velocity of frame P relative to frame M in P frame components + Eigen::Vector3d omegaPrime_PM_P; //!< [rad/s^2] B frame time derivative of omega_PM_P in P frame components + Eigen::MRPd sigma_PM; //!< MRP attitude of frame P relative to frame M + std::string nameOfsigma_PMState; //!< Identifier for the sigma_PM state data container + + ReadFunctor + prescribedTranslationInMsg; //!< Input message for the effector's translational prescribed states + ReadFunctor + prescribedRotationInMsg; //!< Input message for the effector's rotational prescribed states + Message + prescribedTranslationOutMsg; //!< Output message for the effector's translational prescribed states + Message + prescribedRotationOutMsg; //!< Output message for the effector's rotational prescribed states + Message + prescribedMotionConfigLogOutMsg; //!< Output config log message for the effector's states + + private: + static uint64_t effectorID; //!< ID number of this panel // Given quantities from user in python - Eigen::Matrix3d IPntPc_B; //!< [kg-m^2] Inertia of the effector about its center of mass in B frame components - Eigen::Vector3d r_PB_B; //!< [m] Position of point P relative to point B in B frame components - Eigen::Vector3d r_PcP_B; //!< [m] Position of the effector center of mass relative to point P in B frame components + Eigen::Matrix3d IPntPc_B; //!< [kg-m^2] Inertia of the effector about its center of mass in B frame components + Eigen::Vector3d r_PB_B; //!< [m] Position of point P relative to point B in B frame components + Eigen::Vector3d r_PcP_B; //!< [m] Position of the effector center of mass relative to point P in B frame components // Prescribed parameters in body frame components - Eigen::Vector3d r_PM_B; //!< [m] Position of point P relative to point M in B frame components - Eigen::Vector3d rPrime_PM_B; //!< [m/s] B frame time derivative of position r_PM_B in B frame components - Eigen::Vector3d rPrimePrime_PM_B; //!< [m/s^2] B frame time derivative of rPrime_PM_B in B frame components - Eigen::Vector3d omega_PM_B; //!< [rad/s] Angular velocity of P frame relative to the M frame in B frame components - Eigen::Vector3d omegaPrime_PM_B; //!< [rad/s^2] B frame time derivative of omega_PB_B in B frame components - Eigen::Vector3d omega_PB_B; //!< [rad/s] Angular velocity of frame P relative to frame B in B frame components - Eigen::Vector3d omegaPrime_PB_B; //!< [rad/s^2] B frame time derivative of omega_PB_B in B frame components + Eigen::Vector3d r_PM_B; //!< [m] Position of point P relative to point M in B frame components + Eigen::Vector3d rPrime_PM_B; //!< [m/s] B frame time derivative of position r_PM_B in B frame components + Eigen::Vector3d rPrimePrime_PM_B; //!< [m/s^2] B frame time derivative of rPrime_PM_B in B frame components + Eigen::Vector3d omega_PM_B; //!< [rad/s] Angular velocity of P frame relative to the M frame in B frame components + Eigen::Vector3d omegaPrime_PM_B; //!< [rad/s^2] B frame time derivative of omega_PB_B in B frame components + Eigen::Vector3d omega_PB_B; //!< [rad/s] Angular velocity of frame P relative to frame B in B frame components + Eigen::Vector3d omegaPrime_PB_B; //!< [rad/s^2] B frame time derivative of omega_PB_B in B frame components // Other vector quantities - Eigen::Vector3d r_PcM_B; //!< [m] Position of frame P center of mass relative to point M in B frame components - Eigen::Vector3d r_PcB_B; //!< [m] Position of frame P center of mass relative to point B in B frame components - Eigen::Vector3d rPrime_PcM_B; //!< [m/s] B frame time derivative of r_PcM_B in B frame components - Eigen::Vector3d rPrime_PcB_B; //!< [m/s] B frame time derivative of r_PcB_B in B frame components - Eigen::Vector3d rPrimePrime_PcB_B; //!< [m/s^2] B frame time derivative of rPrime_PcB_B in B frame components - Eigen::Vector3d omega_BN_B; //!< [rad/s] Angular velocity of frame B relative to the inertial frame in B frame components - Eigen::Vector3d omega_PN_B; //!< [rad/s] Angular velocity of frame P relative to the inertial frame in B frame components - Eigen::Vector3d rDot_PcB_B; //!< [m/s] Inertial time derivative of r_PcB_B in B frame components - Eigen::MRPd sigma_BN; //!< MRP attitude of frame B relative to the inertial frame + Eigen::Vector3d r_PcM_B; //!< [m] Position of frame P center of mass relative to point M in B frame components + Eigen::Vector3d r_PcB_B; //!< [m] Position of frame P center of mass relative to point B in B frame components + Eigen::Vector3d rPrime_PcM_B; //!< [m/s] B frame time derivative of r_PcM_B in B frame components + Eigen::Vector3d rPrime_PcB_B; //!< [m/s] B frame time derivative of r_PcB_B in B frame components + Eigen::Vector3d rPrimePrime_PcB_B; //!< [m/s^2] B frame time derivative of rPrime_PcB_B in B frame components + Eigen::Vector3d + omega_BN_B; //!< [rad/s] Angular velocity of frame B relative to the inertial frame in B frame components + Eigen::Vector3d + omega_PN_B; //!< [rad/s] Angular velocity of frame P relative to the inertial frame in B frame components + Eigen::Vector3d rDot_PcB_B; //!< [m/s] Inertial time derivative of r_PcB_B in B frame components + Eigen::MRPd sigma_BN; //!< MRP attitude of frame B relative to the inertial frame // DCMs - Eigen::Matrix3d dcm_BN; //!< DCM from inertial frame to frame B - Eigen::Matrix3d dcm_BM; //!< DCM from frame M to frame B - Eigen::Matrix3d dcm_PM; //!< DCM from frame M to frame P - Eigen::Matrix3d dcm_BP; //!< DCM from frame P to frame B + Eigen::Matrix3d dcm_BN; //!< DCM from inertial frame to frame B + Eigen::Matrix3d dcm_BM; //!< DCM from frame M to frame B + Eigen::Matrix3d dcm_PM; //!< DCM from frame M to frame P + Eigen::Matrix3d dcm_BP; //!< DCM from frame P to frame B // Other matrix quantities - Eigen::Matrix3d rTilde_PcB_B; //!< [m] Tilde cross product matrix of r_PcB_B - Eigen::Matrix3d omegaTilde_BN_B; //!< [rad/s] Tilde cross product matrix of omega_BN_B - Eigen::Matrix3d omegaTilde_PB_B; //!< [rad/s] Tilde cross product matrix of omega_PB_B + Eigen::Matrix3d rTilde_PcB_B; //!< [m] Tilde cross product matrix of r_PcB_B + Eigen::Matrix3d omegaTilde_BN_B; //!< [rad/s] Tilde cross product matrix of omega_BN_B + Eigen::Matrix3d omegaTilde_PB_B; //!< [rad/s] Tilde cross product matrix of omega_PB_B // Effector properties relative to the inertial frame - Eigen::Vector3d r_PcN_N; //!< [m] Position of frame P center of mass relative to the inertial frame in inertial frame components - Eigen::Vector3d v_PcN_N; //!< [m/s] Inertial velocity of frame P center of mass relative to the inertial frame in inertial frame components - Eigen::Vector3d sigma_PN; //!< MRP attitude of frame P relative to the inertial frame - Eigen::Vector3d omega_PN_P; //!< [rad/s] Angular velocity of frame P relative to the inertial frame in P frame components + Eigen::Vector3d + r_PcN_N; //!< [m] Position of frame P center of mass relative to the inertial frame in inertial frame components + Eigen::Vector3d v_PcN_N; //!< [m/s] Inertial velocity of frame P center of mass relative to the inertial frame in + //!< inertial frame components + Eigen::Vector3d sigma_PN; //!< MRP attitude of frame P relative to the inertial frame + Eigen::Vector3d + omega_PN_P; //!< [rad/s] Angular velocity of frame P relative to the inertial frame in P frame components // Hub states - Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N Inertial position relative to system spice zeroBase/refBase - Eigen::MatrixXd* inertialVelocityProperty; //!< [m] v_N Inertial velocity relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N Inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialVelocityProperty; //!< [m] v_N Inertial velocity relative to system spice zeroBase/refBase // Prescribed states at epoch (Dynamics time step) - Eigen::Vector3d rEpoch_PM_M; //!< [m] Position of point P relative to point M in M frame components - Eigen::Vector3d rPrimeEpoch_PM_M; //!< [m/s] B frame time derivative of r_PM_M in M frame components - Eigen::Vector3d omegaEpoch_PM_P; //!< [rad/s] Angular velocity of frame P relative to frame M in P frame components - StateData *sigma_PMState; //!< MRP attitude of frame P relative to frame M - + Eigen::Vector3d rEpoch_PM_M; //!< [m] Position of point P relative to point M in M frame components + Eigen::Vector3d rPrimeEpoch_PM_M; //!< [m/s] B frame time derivative of r_PM_M in M frame components + Eigen::Vector3d omegaEpoch_PM_P; //!< [rad/s] Angular velocity of frame P relative to frame M in P frame components + StateData* sigma_PMState; //!< MRP attitude of frame P relative to frame M }; #endif /* PRESCRIBED_MOTION_STATE_EFFECTOR_H */ 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_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 d7cd22929c..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 @@ -55,8 +57,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. @@ -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/reactionWheels/reactionWheelStateEffector.cpp b/src/simulation/dynamics/reactionWheels/reactionWheelStateEffector.cpp old mode 100755 new mode 100644 index 4b5f4f1470..3d9d3c54f0 --- a/src/simulation/dynamics/reactionWheels/reactionWheelStateEffector.cpp +++ b/src/simulation/dynamics/reactionWheels/reactionWheelStateEffector.cpp @@ -17,25 +17,23 @@ */ - #include "reactionWheelStateEffector.h" ReactionWheelStateEffector::ReactionWheelStateEffector() { - CallCounts = 0; - prevCommandTime = 0xFFFFFFFFFFFFFFFF; + CallCounts = 0; + prevCommandTime = 0xFFFFFFFFFFFFFFFF; effProps.mEff = 0.0; effProps.IEffPntB_B.setZero(); effProps.rEff_CB_B.setZero(); - effProps.IEffPrimePntB_B.setZero(); - effProps.rEffPrime_CB_B.setZero(); + effProps.IEffPrimePntB_B.setZero(); + effProps.rEffPrime_CB_B.setZero(); this->nameOfReactionWheelOmegasState = "reactionWheelOmegas"; this->nameOfReactionWheelThetasState = "reactionWheelThetas"; } - ReactionWheelStateEffector::~ReactionWheelStateEffector() { // Clear output messages vector @@ -51,22 +49,23 @@ ReactionWheelStateEffector::~ReactionWheelStateEffector() ReactionWheelData.clear(); } -void ReactionWheelStateEffector::linkInStates(DynParamManager& statesIn) +void +ReactionWheelStateEffector::linkInStates(DynParamManager& statesIn) { - //! - Get access to the hub states + //! - Get access to the hub states this->g_N = statesIn.getPropertyReference(this->propName_vehicleGravity); } -void ReactionWheelStateEffector::registerStates(DynParamManager& states) +void +ReactionWheelStateEffector::registerStates(DynParamManager& states) { //! - Find number of RWs and number of RWs with jitter this->numRWJitter = 0; this->numRW = 0; //! zero the RW Omega and theta values (is there I should do this?) - Eigen::MatrixXd omegasForInit(this->ReactionWheelData.size(),1); + Eigen::MatrixXd omegasForInit(this->ReactionWheelData.size(), 1); - for (std::size_t i = 0; i < ReactionWheelData.size(); ++i) - { + for (std::size_t i = 0; i < ReactionWheelData.size(); ++i) { const auto& rw = *ReactionWheelData[i]; if (rw.RWModel == JitterSimple || rw.RWModel == JitterFullyCoupled) { this->numRWJitter++; @@ -75,21 +74,22 @@ void ReactionWheelStateEffector::registerStates(DynParamManager& states) this->numRW++; } - this->OmegasState = states.registerState((uint32_t) this->numRW, 1, this->nameOfReactionWheelOmegasState); + this->OmegasState = states.registerState((uint32_t)this->numRW, 1, this->nameOfReactionWheelOmegasState); - if (numRWJitter > 0) { - this->thetasState = states.registerState((uint32_t) this->numRWJitter, 1, this->nameOfReactionWheelThetasState); - } + if (numRWJitter > 0) { + this->thetasState = states.registerState((uint32_t)this->numRWJitter, 1, this->nameOfReactionWheelThetasState); + } this->OmegasState->setState(omegasForInit); if (this->numRWJitter > 0) { - Eigen::MatrixXd thetasForZeroing(this->numRWJitter,1); + Eigen::MatrixXd thetasForZeroing(this->numRWJitter, 1); thetasForZeroing.setZero(); this->thetasState->setState(thetasForZeroing); } } -void ReactionWheelStateEffector::updateEffectorMassProps(double integTime) +void +ReactionWheelStateEffector::updateEffectorMassProps(double integTime) { // - Zero the mass props information because these will be accumulated during this call this->effProps.mEff = 0.; @@ -99,63 +99,60 @@ void ReactionWheelStateEffector::updateEffectorMassProps(double integTime) this->effProps.IEffPrimePntB_B.setZero(); int thetaCount = 0; - for (std::size_t i = 0; i < ReactionWheelData.size(); ++i) - { + for (std::size_t i = 0; i < ReactionWheelData.size(); ++i) { auto& rw = *ReactionWheelData[i]; - rw.Omega = this->OmegasState->getState()(i, 0); - - if (rw.RWModel == JitterFullyCoupled) { - rw.theta = this->thetasState->getState()(thetaCount, 0); - Eigen::Matrix3d dcm_WW0 = eigenM1(rw.theta); - Eigen::Matrix3d dcm_BW0; - dcm_BW0.col(0) = rw.gsHat_B; - dcm_BW0.col(1) = rw.w2Hat0_B; - dcm_BW0.col(2) = rw.w3Hat0_B; - Eigen::Matrix3d dcm_BW = dcm_BW0 * dcm_WW0.transpose(); - rw.w2Hat_B = dcm_BW.col(1); - rw.w3Hat_B = dcm_BW.col(2); - - //! wheel inertia tensor about wheel center of mass represented in B frame - Eigen::Matrix3d IRWPntWc_W; - IRWPntWc_W << rw.Js, 0., rw.J13, \ - 0., rw.Jt, 0., \ - rw.J13, 0., rw.Jg; - rw.IRWPntWc_B = dcm_BW * IRWPntWc_W * dcm_BW.transpose(); - - //! wheel inertia tensor body frame derivative about wheel center of mass represented in B frame - Eigen::Matrix3d IPrimeRWPntWc_W; - IPrimeRWPntWc_W << 0., -rw.J13, 0., \ - -rw.J13, 0., 0., \ - 0., 0., 0.; - IPrimeRWPntWc_W *= rw.Omega; - rw.IPrimeRWPntWc_B = dcm_BW * IPrimeRWPntWc_W * dcm_BW.transpose(); - - //! wheel center of mass location - rw.rWcB_B = rw.rWB_B + rw.d*rw.w2Hat_B; - rw.rTildeWcB_B = eigenTilde(rw.rWcB_B); - rw.rPrimeWcB_B = rw.d*rw.Omega*rw.w3Hat_B; - Eigen::Matrix3d rPrimeTildeWcB_B = eigenTilde(rw.rPrimeWcB_B); - - //! - Give the mass of the reaction wheel to the effProps mass - this->effProps.mEff += rw.mass; - this->effProps.rEff_CB_B += rw.mass*rw.rWcB_B; - this->effProps.IEffPntB_B += rw.IRWPntWc_B + rw.mass*rw.rTildeWcB_B*rw.rTildeWcB_B.transpose(); - this->effProps.rEffPrime_CB_B += rw.mass*rw.rPrimeWcB_B; - this->effProps.IEffPrimePntB_B += rw.IPrimeRWPntWc_B + rw.mass*rPrimeTildeWcB_B*rw.rTildeWcB_B.transpose() + rw.mass*rw.rTildeWcB_B*rPrimeTildeWcB_B.transpose(); + rw.Omega = this->OmegasState->getState()(i, 0); + + if (rw.RWModel == JitterFullyCoupled) { + rw.theta = this->thetasState->getState()(thetaCount, 0); + Eigen::Matrix3d dcm_WW0 = eigenM1(rw.theta); + Eigen::Matrix3d dcm_BW0; + dcm_BW0.col(0) = rw.gsHat_B; + dcm_BW0.col(1) = rw.w2Hat0_B; + dcm_BW0.col(2) = rw.w3Hat0_B; + Eigen::Matrix3d dcm_BW = dcm_BW0 * dcm_WW0.transpose(); + rw.w2Hat_B = dcm_BW.col(1); + rw.w3Hat_B = dcm_BW.col(2); + + //! wheel inertia tensor about wheel center of mass represented in B frame + Eigen::Matrix3d IRWPntWc_W; + IRWPntWc_W << rw.Js, 0., rw.J13, 0., rw.Jt, 0., rw.J13, 0., rw.Jg; + rw.IRWPntWc_B = dcm_BW * IRWPntWc_W * dcm_BW.transpose(); + + //! wheel inertia tensor body frame derivative about wheel center of mass represented in B frame + Eigen::Matrix3d IPrimeRWPntWc_W; + IPrimeRWPntWc_W << 0., -rw.J13, 0., -rw.J13, 0., 0., 0., 0., 0.; + IPrimeRWPntWc_W *= rw.Omega; + rw.IPrimeRWPntWc_B = dcm_BW * IPrimeRWPntWc_W * dcm_BW.transpose(); + + //! wheel center of mass location + rw.rWcB_B = rw.rWB_B + rw.d * rw.w2Hat_B; + rw.rTildeWcB_B = eigenTilde(rw.rWcB_B); + rw.rPrimeWcB_B = rw.d * rw.Omega * rw.w3Hat_B; + Eigen::Matrix3d rPrimeTildeWcB_B = eigenTilde(rw.rPrimeWcB_B); + + //! - Give the mass of the reaction wheel to the effProps mass + this->effProps.mEff += rw.mass; + this->effProps.rEff_CB_B += rw.mass * rw.rWcB_B; + this->effProps.IEffPntB_B += rw.IRWPntWc_B + rw.mass * rw.rTildeWcB_B * rw.rTildeWcB_B.transpose(); + this->effProps.rEffPrime_CB_B += rw.mass * rw.rPrimeWcB_B; + this->effProps.IEffPrimePntB_B += rw.IPrimeRWPntWc_B + + rw.mass * rPrimeTildeWcB_B * rw.rTildeWcB_B.transpose() + + rw.mass * rw.rTildeWcB_B * rPrimeTildeWcB_B.transpose(); + thetaCount++; + } else if (rw.RWModel == JitterSimple) { + rw.theta = this->thetasState->getState()(thetaCount, 0); + Eigen::Matrix3d dcm_WW0 = eigenM1(rw.theta); + Eigen::Matrix3d dcm_BW0; + dcm_BW0.col(0) = rw.gsHat_B; + dcm_BW0.col(1) = rw.w2Hat0_B; + dcm_BW0.col(2) = rw.w3Hat0_B; + Eigen::Matrix3d dcm_BW = dcm_BW0 * dcm_WW0.transpose(); + rw.w2Hat_B = dcm_BW.col(1); + rw.w3Hat_B = dcm_BW.col(2); thetaCount++; - } else if (rw.RWModel == JitterSimple) { - rw.theta = this->thetasState->getState()(thetaCount, 0); - Eigen::Matrix3d dcm_WW0 = eigenM1(rw.theta); - Eigen::Matrix3d dcm_BW0; - dcm_BW0.col(0) = rw.gsHat_B; - dcm_BW0.col(1) = rw.w2Hat0_B; - dcm_BW0.col(2) = rw.w3Hat0_B; - Eigen::Matrix3d dcm_BW = dcm_BW0 * dcm_WW0.transpose(); - rw.w2Hat_B = dcm_BW.col(1); - rw.w3Hat_B = dcm_BW.col(2); - thetaCount++; - } - } + } + } // - Need to divide out the total mass of the reaction wheels from rCB_B and rPrimeCB_B if (this->effProps.mEff > 0) { @@ -164,39 +161,43 @@ void ReactionWheelStateEffector::updateEffectorMassProps(double integTime) } } -void ReactionWheelStateEffector::updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) +void +ReactionWheelStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { - Eigen::Vector3d omegaLoc_BN_B; - Eigen::Vector3d tempF; - double omegas; - double omegaw2; - double omegaw3; - double dSquared; - double OmegaSquared; + Eigen::Vector3d omegaLoc_BN_B; + Eigen::Vector3d tempF; + double omegas; + double omegaw2; + double omegaw3; + double dSquared; + double OmegaSquared; Eigen::MRPd sigmaBNLocal; - Eigen::Matrix3d dcm_BN; /*! direction cosine matrix from N to B */ - Eigen::Matrix3d dcm_NB; /*! direction cosine matrix from B to N */ - Eigen::Vector3d gravityTorquePntW_B; /*! torque of gravity on HRB about Pnt H */ - Eigen::Vector3d gLocal_N; /*! gravitational acceleration in N frame */ - Eigen::Vector3d g_B; /*! gravitational acceleration in B frame */ + Eigen::Matrix3d dcm_BN; /*! direction cosine matrix from N to B */ + Eigen::Matrix3d dcm_NB; /*! direction cosine matrix from B to N */ + Eigen::Vector3d gravityTorquePntW_B; /*! torque of gravity on HRB about Pnt H */ + Eigen::Vector3d gLocal_N; /*! gravitational acceleration in N frame */ + Eigen::Vector3d g_B; /*! gravitational acceleration in B frame */ gLocal_N = *this->g_N; //! - Find dcm_BN - sigmaBNLocal = (Eigen::Vector3d ) sigma_BN; + sigmaBNLocal = (Eigen::Vector3d)sigma_BN; dcm_NB = sigmaBNLocal.toRotationMatrix(); dcm_BN = dcm_NB.transpose(); //! - Map gravity to body frame - g_B = dcm_BN*gLocal_N; + g_B = dcm_BN * gLocal_N; - omegaLoc_BN_B = omega_BN_B; + omegaLoc_BN_B = omega_BN_B; - for (const auto & rwPtr : ReactionWheelData) - { - auto & rw = *rwPtr; - OmegaSquared = rw.Omega * rw.Omega; + for (const auto& rwPtr : ReactionWheelData) { + auto& rw = *rwPtr; + OmegaSquared = rw.Omega * rw.Omega; // Determine which friction model to use (if starting from zero include stribeck) - if (fabs(rw.Omega) < 0.10*rw.omegaLimitCycle && rw.betaStatic > 0) { + if (fabs(rw.Omega) < 0.10 * rw.omegaLimitCycle && rw.betaStatic > 0) { rw.frictionStribeck = 1; } double signOfOmega = ((rw.Omega > 0) - (rw.Omega < 0)); @@ -212,167 +213,201 @@ void ReactionWheelStateEffector::updateContributions(double integTime, BackSubMa double frictionForceAtLimitCycle; // Friction model which uses static, stribeck, coulomb, and viscous friction models if (rw.frictionStribeck == 1) { - frictionForce = sqrt(2.0*exp(1.0))*(rw.fStatic - rw.fCoulomb)*exp(-(rw.Omega/rw.betaStatic)*(rw.Omega/rw.betaStatic)/2.0)*rw.Omega/(rw.betaStatic*sqrt(2.0)) + rw.fCoulomb*tanh(rw.Omega*10.0/rw.betaStatic) + rw.cViscous*rw.Omega; - frictionForceAtLimitCycle = sqrt(2.0*exp(1.0))*(rw.fStatic - rw.fCoulomb)*exp(-(rw.omegaLimitCycle/rw.betaStatic)*(rw.omegaLimitCycle/rw.betaStatic)/2.0)*rw.omegaLimitCycle/(rw.betaStatic*sqrt(2.0)) + rw.fCoulomb*tanh(rw.omegaLimitCycle*10.0/rw.betaStatic) + rw.cViscous*rw.omegaLimitCycle; + frictionForce = sqrt(2.0 * exp(1.0)) * (rw.fStatic - rw.fCoulomb) * + exp(-(rw.Omega / rw.betaStatic) * (rw.Omega / rw.betaStatic) / 2.0) * rw.Omega / + (rw.betaStatic * sqrt(2.0)) + + rw.fCoulomb * tanh(rw.Omega * 10.0 / rw.betaStatic) + rw.cViscous * rw.Omega; + frictionForceAtLimitCycle = + sqrt(2.0 * exp(1.0)) * (rw.fStatic - rw.fCoulomb) * + exp(-(rw.omegaLimitCycle / rw.betaStatic) * (rw.omegaLimitCycle / rw.betaStatic) / 2.0) * + rw.omegaLimitCycle / (rw.betaStatic * sqrt(2.0)) + + rw.fCoulomb * tanh(rw.omegaLimitCycle * 10.0 / rw.betaStatic) + rw.cViscous * rw.omegaLimitCycle; } else { - frictionForce = signOfOmega*rw.fCoulomb + rw.cViscous*rw.Omega; - frictionForceAtLimitCycle = rw.fCoulomb + rw.cViscous*rw.omegaLimitCycle; + frictionForce = signOfOmega * rw.fCoulomb + rw.cViscous * rw.Omega; + frictionForceAtLimitCycle = rw.fCoulomb + rw.cViscous * rw.omegaLimitCycle; } // This line avoids the limit cycle that can occur with friction if (fabs(rw.Omega) < rw.omegaLimitCycle) { - frictionForce = frictionForceAtLimitCycle/rw.omegaLimitCycle*rw.Omega; + frictionForce = frictionForceAtLimitCycle / rw.omegaLimitCycle * rw.Omega; } // Set friction force rw.frictionTorque = -frictionForce; - if (rw.RWModel == BalancedWheels || rw.RWModel == JitterSimple) { - backSubContr.matrixD -= rw.Js * rw.gsHat_B * rw.gsHat_B.transpose(); - backSubContr.vecRot -= rw.gsHat_B * (rw.u_current + rw.frictionTorque) + rw.Js*rw.Omega*omegaLoc_BN_B.cross(rw.gsHat_B); - - //! imbalance torque (simplified external) - if (rw.RWModel == JitterSimple) { - /* Fs = Us * Omega^2 */ // static imbalance force - tempF = rw.U_s * OmegaSquared * rw.w2Hat_B; - backSubContr.vecTrans += tempF; - - //! add in dynamic imbalance torque - /* tau_s = cross(r_B,Fs) */ // static imbalance torque - /* tau_d = Ud * Omega^2 */ // dynamic imbalance torque - backSubContr.vecRot += ( rw.rWB_B.cross(tempF) ) + ( rw.U_d*OmegaSquared * rw.w2Hat_B ); - } + if (rw.RWModel == BalancedWheels || rw.RWModel == JitterSimple) { + backSubContr.matrixD -= rw.Js * rw.gsHat_B * rw.gsHat_B.transpose(); + backSubContr.vecRot -= + rw.gsHat_B * (rw.u_current + rw.frictionTorque) + rw.Js * rw.Omega * omegaLoc_BN_B.cross(rw.gsHat_B); + + //! imbalance torque (simplified external) + if (rw.RWModel == JitterSimple) { + /* Fs = Us * Omega^2 */ // static imbalance force + tempF = rw.U_s * OmegaSquared * rw.w2Hat_B; + backSubContr.vecTrans += tempF; + + //! add in dynamic imbalance torque + /* tau_s = cross(r_B,Fs) */ // static imbalance torque + /* tau_d = Ud * Omega^2 */ // dynamic imbalance torque + backSubContr.vecRot += (rw.rWB_B.cross(tempF)) + (rw.U_d * OmegaSquared * rw.w2Hat_B); + } } else if (rw.RWModel == JitterFullyCoupled) { - omegas = rw.gsHat_B.transpose()*omegaLoc_BN_B; - omegaw2 = rw.w2Hat_B.transpose()*omegaLoc_BN_B; - omegaw3 = rw.w3Hat_B.transpose()*omegaLoc_BN_B; - - gravityTorquePntW_B = rw.d*rw.w2Hat_B.cross(rw.mass*g_B); - - dSquared = rw.d * rw.d; - - rw.aOmega = -rw.mass*rw.d/(rw.Js + rw.mass*dSquared) * rw.w3Hat_B; - rw.bOmega = -1.0/(rw.Js + rw.mass*dSquared)*((rw.Js+rw.mass*dSquared)*rw.gsHat_B + rw.J13*rw.w3Hat_B + rw.mass*rw.d*rw.rWB_B.cross(rw.w3Hat_B)); - rw.cOmega = 1.0/(rw.Js + rw.mass*dSquared)*(omegaw2*omegaw3*(-rw.mass*dSquared)-rw.J13*omegaw2*omegas-rw.mass*rw.d*rw.w3Hat_B.transpose()*omegaLoc_BN_B.cross(omegaLoc_BN_B.cross(rw.rWB_B))+(rw.u_current + rw.frictionTorque) + rw.gsHat_B.dot(gravityTorquePntW_B)); - - backSubContr.matrixA += rw.mass * rw.d * rw.w3Hat_B * rw.aOmega.transpose(); - backSubContr.matrixB += rw.mass * rw.d * rw.w3Hat_B * rw.bOmega.transpose(); - backSubContr.matrixC += (rw.IRWPntWc_B*rw.gsHat_B + rw.mass*rw.d*rw.rWcB_B.cross(rw.w3Hat_B))*rw.aOmega.transpose(); - backSubContr.matrixD += (rw.IRWPntWc_B*rw.gsHat_B + rw.mass*rw.d*rw.rWcB_B.cross(rw.w3Hat_B))*rw.bOmega.transpose(); - backSubContr.vecTrans += rw.mass*rw.d*(OmegaSquared*rw.w2Hat_B - rw.cOmega*rw.w3Hat_B); - backSubContr.vecRot += rw.mass*rw.d*OmegaSquared*rw.rWcB_B.cross(rw.w2Hat_B) - rw.IPrimeRWPntWc_B*rw.Omega*rw.gsHat_B - omegaLoc_BN_B.cross(rw.IRWPntWc_B*rw.Omega*rw.gsHat_B+rw.mass*rw.rWcB_B.cross(rw.rPrimeWcB_B)) - (rw.IRWPntWc_B*rw.gsHat_B+rw.mass*rw.d*rw.rWcB_B.cross(rw.w3Hat_B))*rw.cOmega; - } - } + omegas = rw.gsHat_B.transpose() * omegaLoc_BN_B; + omegaw2 = rw.w2Hat_B.transpose() * omegaLoc_BN_B; + omegaw3 = rw.w3Hat_B.transpose() * omegaLoc_BN_B; + + gravityTorquePntW_B = rw.d * rw.w2Hat_B.cross(rw.mass * g_B); + + dSquared = rw.d * rw.d; + + rw.aOmega = -rw.mass * rw.d / (rw.Js + rw.mass * dSquared) * rw.w3Hat_B; + rw.bOmega = -1.0 / (rw.Js + rw.mass * dSquared) * + ((rw.Js + rw.mass * dSquared) * rw.gsHat_B + rw.J13 * rw.w3Hat_B + + rw.mass * rw.d * rw.rWB_B.cross(rw.w3Hat_B)); + rw.cOmega = 1.0 / (rw.Js + rw.mass * dSquared) * + (omegaw2 * omegaw3 * (-rw.mass * dSquared) - rw.J13 * omegaw2 * omegas - + rw.mass * rw.d * rw.w3Hat_B.transpose() * omegaLoc_BN_B.cross(omegaLoc_BN_B.cross(rw.rWB_B)) + + (rw.u_current + rw.frictionTorque) + rw.gsHat_B.dot(gravityTorquePntW_B)); + + backSubContr.matrixA += rw.mass * rw.d * rw.w3Hat_B * rw.aOmega.transpose(); + backSubContr.matrixB += rw.mass * rw.d * rw.w3Hat_B * rw.bOmega.transpose(); + backSubContr.matrixC += + (rw.IRWPntWc_B * rw.gsHat_B + rw.mass * rw.d * rw.rWcB_B.cross(rw.w3Hat_B)) * rw.aOmega.transpose(); + backSubContr.matrixD += + (rw.IRWPntWc_B * rw.gsHat_B + rw.mass * rw.d * rw.rWcB_B.cross(rw.w3Hat_B)) * rw.bOmega.transpose(); + backSubContr.vecTrans += rw.mass * rw.d * (OmegaSquared * rw.w2Hat_B - rw.cOmega * rw.w3Hat_B); + backSubContr.vecRot += + rw.mass * rw.d * OmegaSquared * rw.rWcB_B.cross(rw.w2Hat_B) - rw.IPrimeRWPntWc_B * rw.Omega * rw.gsHat_B - + omegaLoc_BN_B.cross(rw.IRWPntWc_B * rw.Omega * rw.gsHat_B + rw.mass * rw.rWcB_B.cross(rw.rPrimeWcB_B)) - + (rw.IRWPntWc_B * rw.gsHat_B + rw.mass * rw.d * rw.rWcB_B.cross(rw.w3Hat_B)) * rw.cOmega; + } + } } -void ReactionWheelStateEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +ReactionWheelStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { - Eigen::MatrixXd OmegasDot(this->numRW,1); - Eigen::MatrixXd thetasDot(this->numRWJitter,1); - Eigen::Vector3d omegaDotBNLoc_B; - Eigen::MRPd sigmaBNLocal; - Eigen::Matrix3d dcm_BN; /*! direction cosine matrix from N to B */ - Eigen::Matrix3d dcm_NB; /*! direction cosine matrix from B to N */ - Eigen::Vector3d rDDotBNLoc_N; /*! second time derivative of rBN in N frame */ - Eigen::Vector3d rDDotBNLoc_B; /*! second time derivative of rBN in B frame */ + Eigen::MatrixXd OmegasDot(this->numRW, 1); + Eigen::MatrixXd thetasDot(this->numRWJitter, 1); + Eigen::Vector3d omegaDotBNLoc_B; + Eigen::MRPd sigmaBNLocal; + Eigen::Matrix3d dcm_BN; /*! direction cosine matrix from N to B */ + Eigen::Matrix3d dcm_NB; /*! direction cosine matrix from B to N */ + Eigen::Vector3d rDDotBNLoc_N; /*! second time derivative of rBN in N frame */ + Eigen::Vector3d rDDotBNLoc_B; /*! second time derivative of rBN in B frame */ int thetaCount = 0; - //! Grab necessarry values from manager - omegaDotBNLoc_B = omegaDot_BN_B; - rDDotBNLoc_N = rDDot_BN_N; - sigmaBNLocal = (Eigen::Vector3d ) sigma_BN; - dcm_NB = sigmaBNLocal.toRotationMatrix(); - dcm_BN = dcm_NB.transpose(); - rDDotBNLoc_B = dcm_BN*rDDotBNLoc_N; - - //! - Compute Derivatives - for (std::size_t i = 0; i < ReactionWheelData.size(); ++i) - { + //! Grab necessarry values from manager + omegaDotBNLoc_B = omegaDot_BN_B; + rDDotBNLoc_N = rDDot_BN_N; + sigmaBNLocal = (Eigen::Vector3d)sigma_BN; + dcm_NB = sigmaBNLocal.toRotationMatrix(); + dcm_BN = dcm_NB.transpose(); + rDDotBNLoc_B = dcm_BN * rDDotBNLoc_N; + + //! - Compute Derivatives + for (std::size_t i = 0; i < ReactionWheelData.size(); ++i) { auto& rw = *ReactionWheelData[i]; - if(rw.RWModel == JitterFullyCoupled || rw.RWModel == JitterSimple) { + if (rw.RWModel == JitterFullyCoupled || rw.RWModel == JitterSimple) { // - Set trivial kinemetic derivative - thetasDot(thetaCount,0) = rw.Omega; + thetasDot(thetaCount, 0) = rw.Omega; thetaCount++; } - if (rw.RWModel == BalancedWheels || rw.RWModel == JitterSimple) { - OmegasDot(i,0) = (rw.u_current + rw.frictionTorque)/rw.Js - rw.gsHat_B.transpose()*omegaDotBNLoc_B; + if (rw.RWModel == BalancedWheels || rw.RWModel == JitterSimple) { + OmegasDot(i, 0) = (rw.u_current + rw.frictionTorque) / rw.Js - rw.gsHat_B.transpose() * omegaDotBNLoc_B; // Check for numerical instability due to excessive wheel acceleration - if (std::abs(OmegasDot(i,0)) > this->maxWheelAcceleration) { - bskLogger.bskLog(BSK_WARNING, "Excessive reaction wheel acceleration detected (%.2e rad/s^2). This may be caused by using unlimited torque (useMaxTorque=False) with a small spacecraft inertia. Consider using torque limits or increasing spacecraft inertia.", std::abs(OmegasDot(i,0))); + if (std::abs(OmegasDot(i, 0)) > this->maxWheelAcceleration) { + bskLogger.bskLog(BSK_WARNING, + "Excessive reaction wheel acceleration detected (%.2e rad/s^2). This may be caused by " + "using unlimited torque (useMaxTorque=False) with a small spacecraft inertia. " + "Consider using torque limits or increasing spacecraft inertia.", + std::abs(OmegasDot(i, 0))); // Safety mechanism: limit the wheel acceleration to prevent numerical instability - OmegasDot(i,0) = std::copysign(this->maxWheelAcceleration, OmegasDot(i,0)); + OmegasDot(i, 0) = std::copysign(this->maxWheelAcceleration, OmegasDot(i, 0)); // Recalculate the effective torque for consistency - double effectiveTorque = OmegasDot(i,0) * rw.Js + rw.gsHat_B.transpose()*omegaDotBNLoc_B; + double effectiveTorque = OmegasDot(i, 0) * rw.Js + rw.gsHat_B.transpose() * omegaDotBNLoc_B; rw.u_current = effectiveTorque - rw.frictionTorque; } - } else if(rw.RWModel == JitterFullyCoupled) { - OmegasDot(i,0) = rw.aOmega.dot(rDDotBNLoc_B) + rw.bOmega.dot(omegaDotBNLoc_B) + rw.cOmega; + } else if (rw.RWModel == JitterFullyCoupled) { + OmegasDot(i, 0) = rw.aOmega.dot(rDDotBNLoc_B) + rw.bOmega.dot(omegaDotBNLoc_B) + rw.cOmega; // Check for numerical instability in fully coupled model as well - if (std::abs(OmegasDot(i,0)) > this->maxWheelAcceleration) { - bskLogger.bskLog(BSK_WARNING, "Excessive reaction wheel acceleration detected (%.2e rad/s^2). This may be caused by using unlimited torque (useMaxTorque=False) with a small spacecraft inertia. Consider using torque limits or increasing spacecraft inertia.", std::abs(OmegasDot(i,0))); + if (std::abs(OmegasDot(i, 0)) > this->maxWheelAcceleration) { + bskLogger.bskLog(BSK_WARNING, + "Excessive reaction wheel acceleration detected (%.2e rad/s^2). This may be caused by " + "using unlimited torque (useMaxTorque=False) with a small spacecraft inertia. " + "Consider using torque limits or increasing spacecraft inertia.", + std::abs(OmegasDot(i, 0))); // Safety mechanism: limit the wheel acceleration to prevent numerical instability - OmegasDot(i,0) = std::copysign(this->maxWheelAcceleration, OmegasDot(i,0)); + OmegasDot(i, 0) = std::copysign(this->maxWheelAcceleration, OmegasDot(i, 0)); } - } - } + } + } - OmegasState->setDerivative(OmegasDot); + OmegasState->setDerivative(OmegasDot); if (this->numRWJitter > 0) { thetasState->setDerivative(thetasDot); } } -void ReactionWheelStateEffector::updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B) +void +ReactionWheelStateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { - Eigen::MRPd sigmaBNLocal; - Eigen::Matrix3d dcm_BN; /*! direction cosine matrix from N to B */ - Eigen::Matrix3d dcm_NB; /*! direction cosine matrix from B to N */ - Eigen::Vector3d omegaLoc_BN_B = omega_BN_B; + Eigen::MRPd sigmaBNLocal; + Eigen::Matrix3d dcm_BN; /*! direction cosine matrix from N to B */ + Eigen::Matrix3d dcm_NB; /*! direction cosine matrix from B to N */ + Eigen::Vector3d omegaLoc_BN_B = omega_BN_B; //! - Compute energy and momentum contribution of each wheel rotAngMomPntCContr_B.setZero(); - for (const auto & rwPtr : ReactionWheelData) - { + for (const auto& rwPtr : ReactionWheelData) { auto const& rw = *rwPtr; - if (rw.RWModel == BalancedWheels || rw.RWModel == JitterSimple) { - rotAngMomPntCContr_B += rw.Js*rw.gsHat_B*rw.Omega; - rotEnergyContr += 1.0/2.0*rw.Js*rw.Omega*rw.Omega + rw.Js*rw.Omega*rw.gsHat_B.dot(omegaLoc_BN_B); - } else if (rw.RWModel == JitterFullyCoupled) { - Eigen::Vector3d omega_WN_B = omegaLoc_BN_B + rw.Omega*rw.gsHat_B; - Eigen::Vector3d r_WcB_B = rw.rWcB_B; - Eigen::Vector3d rDot_WcB_B = rw.d*rw.Omega*rw.w3Hat_B + omegaLoc_BN_B.cross(rw.rWcB_B); - - rotEnergyContr += 0.5*omega_WN_B.transpose()*rw.IRWPntWc_B*omega_WN_B + 0.5*rw.mass*rDot_WcB_B.dot(rDot_WcB_B); - rotAngMomPntCContr_B += rw.IRWPntWc_B*omega_WN_B + rw.mass*r_WcB_B.cross(rDot_WcB_B); - } + if (rw.RWModel == BalancedWheels || rw.RWModel == JitterSimple) { + rotAngMomPntCContr_B += rw.Js * rw.gsHat_B * rw.Omega; + rotEnergyContr += + 1.0 / 2.0 * rw.Js * rw.Omega * rw.Omega + rw.Js * rw.Omega * rw.gsHat_B.dot(omegaLoc_BN_B); + } else if (rw.RWModel == JitterFullyCoupled) { + Eigen::Vector3d omega_WN_B = omegaLoc_BN_B + rw.Omega * rw.gsHat_B; + Eigen::Vector3d r_WcB_B = rw.rWcB_B; + Eigen::Vector3d rDot_WcB_B = rw.d * rw.Omega * rw.w3Hat_B + omegaLoc_BN_B.cross(rw.rWcB_B); + + rotEnergyContr += + 0.5 * omega_WN_B.transpose() * rw.IRWPntWc_B * omega_WN_B + 0.5 * rw.mass * rDot_WcB_B.dot(rDot_WcB_B); + rotAngMomPntCContr_B += rw.IRWPntWc_B * omega_WN_B + rw.mass * r_WcB_B.cross(rDot_WcB_B); + } } } /*! add a RW data object to the reactionWheelStateEffector */ -void ReactionWheelStateEffector::addReactionWheel(std::shared_ptr NewRW) +void +ReactionWheelStateEffector::addReactionWheel(std::shared_ptr NewRW) { /* store the RW information */ this->ReactionWheelData.push_back(NewRW); /* add a RW state log output message for this wheel */ - Message *msg; + Message* msg; msg = new Message; this->rwOutMsgs.push_back(msg); } - /*! Reset the module to origina configuration values. */ -void ReactionWheelStateEffector::Reset(uint64_t CurrenSimNanos) +void +ReactionWheelStateEffector::Reset(uint64_t CurrenSimNanos) { RWCmdMsgPayload RWCmdInitializer; RWCmdInitializer.u_cmd = 0.0; @@ -380,19 +415,19 @@ void ReactionWheelStateEffector::Reset(uint64_t CurrenSimNanos) //! - Clear out any currently firing RWs and re-init cmd array this->NewRWCmds.clear(); - for (const auto & rwPtr : ReactionWheelData) - { + for (const auto& rwPtr : ReactionWheelData) { this->NewRWCmds.push_back(RWCmdInitializer); - auto & rw = *rwPtr; - if (rw.betaStatic == 0.0) - { - bskLogger.bskLog(BSK_WARNING, "Stribeck coefficent currently zero and should be positive to active this friction model, or negative to turn it off!"); + auto& rw = *rwPtr; + if (rw.betaStatic == 0.0) { + bskLogger.bskLog(BSK_WARNING, + "Stribeck coefficent currently zero and should be positive to active this friction model, " + "or negative to turn it off!"); } //! Define CoM offset d and off-diagonal inertia J13 if using fully coupled model if (rw.RWModel == JitterFullyCoupled) { - rw.d = rw.U_s/rw.mass; //!< determine CoM offset from static imbalance parameter - rw.J13 = rw.U_d; //!< off-diagonal inertia is equal to dynamic imbalance parameter + rw.d = rw.U_s / rw.mass; //!< determine CoM offset from static imbalance parameter + rw.J13 = rw.U_d; //!< off-diagonal inertia is equal to dynamic imbalance parameter } } @@ -405,10 +440,10 @@ void ReactionWheelStateEffector::Reset(uint64_t CurrenSimNanos) @param CurrentClock The current time used for time-stamping the message */ -void ReactionWheelStateEffector::WriteOutputMessages(uint64_t CurrentClock) +void +ReactionWheelStateEffector::WriteOutputMessages(uint64_t CurrentClock) { - for (std::size_t i = 0; i < ReactionWheelData.size(); ++i) - { + for (std::size_t i = 0; i < ReactionWheelData.size(); ++i) { auto& rw = *ReactionWheelData[i]; if (numRWJitter > 0) { rw.theta = this->thetasState->getState()(i, 0); @@ -416,24 +451,24 @@ void ReactionWheelStateEffector::WriteOutputMessages(uint64_t CurrentClock) rw.Omega = this->OmegasState->getState()(i, 0); RWConfigLogMsgPayload tmpRW = this->rwOutMsgs[i]->zeroMsgPayload; - tmpRW.theta = rw.theta; - tmpRW.u_current = rw.u_current; + tmpRW.theta = rw.theta; + tmpRW.u_current = rw.u_current; tmpRW.frictionTorque = rw.frictionTorque; - tmpRW.u_max = rw.u_max; - tmpRW.u_min = rw.u_min; - tmpRW.u_f = rw.fCoulomb; - tmpRW.Omega = rw.Omega; - tmpRW.Omega_max = rw.Omega_max; - tmpRW.Js = rw.Js; - tmpRW.U_s = rw.U_s; - tmpRW.U_d = rw.U_d; - tmpRW.RWModel = rw.RWModel; + tmpRW.u_max = rw.u_max; + tmpRW.u_min = rw.u_min; + tmpRW.u_f = rw.fCoulomb; + tmpRW.Omega = rw.Omega; + tmpRW.Omega_max = rw.Omega_max; + tmpRW.Js = rw.Js; + tmpRW.U_s = rw.U_s; + tmpRW.U_d = rw.U_d; + tmpRW.RWModel = rw.RWModel; tmpRW.P_max = rw.P_max; eigenVector3d2CArray(rw.gsHat_B, tmpRW.gsHat_B); eigenVector3d2CArray(rw.rWB_B, tmpRW.rWB_B); - // Write out config data for each reaction wheel + // Write out config data for each reaction wheel this->rwOutMsgs[i]->write(&tmpRW, this->moduleID, CurrentClock); - } + } } /*! This method is here to write the output message structure into the specified @@ -441,10 +476,10 @@ void ReactionWheelStateEffector::WriteOutputMessages(uint64_t CurrentClock) @param integTimeNanos The current time used for time-stamping the message */ -void ReactionWheelStateEffector::writeOutputStateMessages(uint64_t integTimeNanos) +void +ReactionWheelStateEffector::writeOutputStateMessages(uint64_t integTimeNanos) { - for (std::size_t i = 0; i < ReactionWheelData.size(); ++i) - { + for (std::size_t i = 0; i < ReactionWheelData.size(); ++i) { auto& rw = *ReactionWheelData[i]; if (numRWJitter > 0) { rw.theta = this->thetasState->getState()(i, 0); @@ -462,10 +497,11 @@ void ReactionWheelStateEffector::writeOutputStateMessages(uint64_t integTimeNano associated command structure for operating the RWs. */ -void ReactionWheelStateEffector::ReadInputs() +void +ReactionWheelStateEffector::ReadInputs() { - //! read the incoming command array, or zero if not connected + //! read the incoming command array, or zero if not connected if (this->rwMotorCmdInMsg.isLinked()) { this->incomingCmdBuffer = this->rwMotorCmdInMsg(); this->prevCommandTime = this->rwMotorCmdInMsg.timeWritten(); @@ -473,13 +509,12 @@ void ReactionWheelStateEffector::ReadInputs() this->incomingCmdBuffer = this->rwMotorCmdInMsg.zeroMsgPayload; } - //! - Set the NewRWCmds vector. Using the data() method for raw speed - RWCmdMsgPayload *CmdPtr; + //! - Set the NewRWCmds vector. Using the data() method for raw speed + RWCmdMsgPayload* CmdPtr; uint64_t i; - for(i=0, CmdPtr = NewRWCmds.data(); iu_cmd = this->incomingCmdBuffer.motorTorque[i]; - } + for (i = 0, CmdPtr = NewRWCmds.data(); i < ReactionWheelData.size(); CmdPtr++, i++) { + CmdPtr->u_cmd = this->incomingCmdBuffer.motorTorque[i]; + } } /*! This method is used to read the new commands vector and set the RW @@ -488,52 +523,57 @@ void ReactionWheelStateEffector::ReadInputs() @param CurrentTime The current simulation time converted to a double */ -void ReactionWheelStateEffector::ConfigureRWRequests(double CurrentTime) +void +ReactionWheelStateEffector::ConfigureRWRequests(double CurrentTime) { - for (std::size_t i = 0; i < NewRWCmds.size(); ++i) - { + for (std::size_t i = 0; i < NewRWCmds.size(); ++i) { auto& cmd = NewRWCmds[i]; - // Torque saturation - if (this->ReactionWheelData[i]->u_max > 0) { - if(cmd.u_cmd > this->ReactionWheelData[i]->u_max) { - cmd.u_cmd = this->ReactionWheelData[i]->u_max; - } else if(cmd.u_cmd < -this->ReactionWheelData[i]->u_max) { - cmd.u_cmd = -this->ReactionWheelData[i]->u_max; - } - } else { + // Torque saturation + if (this->ReactionWheelData[i]->u_max > 0) { + if (cmd.u_cmd > this->ReactionWheelData[i]->u_max) { + cmd.u_cmd = this->ReactionWheelData[i]->u_max; + } else if (cmd.u_cmd < -this->ReactionWheelData[i]->u_max) { + cmd.u_cmd = -this->ReactionWheelData[i]->u_max; + } + } else { // Warning for unlimited torque with potentially small spacecraft static bool warningIssued = false; - if (!warningIssued && std::abs(cmd.u_cmd) > this->largeTorqueThreshold) { // Threshold for "large" torque - bskLogger.bskLog(BSK_WARNING, "Using unlimited reaction wheel torque (u_max <= 0). This can cause numerical instability with small spacecraft inertia. Consider setting useMaxTorque=True or increasing spacecraft inertia."); + if (!warningIssued && std::abs(cmd.u_cmd) > this->largeTorqueThreshold) { // Threshold for "large" torque + bskLogger.bskLog( + BSK_WARNING, + "Using unlimited reaction wheel torque (u_max <= 0). This can cause numerical instability with small " + "spacecraft inertia. Consider setting useMaxTorque=True or increasing spacecraft inertia."); warningIssued = true; } } - // minimum torque - if (std::abs(cmd.u_cmd) < this->ReactionWheelData[i]->u_min) { - cmd.u_cmd = 0.0; - } + // minimum torque + if (std::abs(cmd.u_cmd) < this->ReactionWheelData[i]->u_min) { + cmd.u_cmd = 0.0; + } // Power saturation if (this->ReactionWheelData[i]->P_max > 0) { if (std::abs(cmd.u_cmd * this->ReactionWheelData[i]->Omega) >= this->ReactionWheelData[i]->P_max) { - cmd.u_cmd = std::copysign(this->ReactionWheelData[i]->P_max / this->ReactionWheelData[i]->Omega, cmd.u_cmd); + cmd.u_cmd = + std::copysign(this->ReactionWheelData[i]->P_max / this->ReactionWheelData[i]->Omega, cmd.u_cmd); } } // Speed saturation - if (std::abs(this->ReactionWheelData[i]->Omega) >= this->ReactionWheelData[i]->Omega_max - && this->ReactionWheelData[i]->Omega_max > 0.0 /* negative Omega_max turns of wheel saturation modeling */ - && this->ReactionWheelData[i]->Omega * cmd.u_cmd >= 0.0 // check if torque would accelerate wheel speed beyond Omega_max - ) { + if (std::abs(this->ReactionWheelData[i]->Omega) >= this->ReactionWheelData[i]->Omega_max && + this->ReactionWheelData[i]->Omega_max > 0.0 /* negative Omega_max turns of wheel saturation modeling */ + && this->ReactionWheelData[i]->Omega * cmd.u_cmd >= + 0.0 // check if torque would accelerate wheel speed beyond Omega_max + ) { cmd.u_cmd = 0.0; } - this->ReactionWheelData[i]->u_current = cmd.u_cmd; // save actual torque for reaction wheel motor + this->ReactionWheelData[i]->u_current = cmd.u_cmd; // save actual torque for reaction wheel motor // Save the previous omega for next time this->ReactionWheelData[i]->omegaBefore = this->ReactionWheelData[i]->Omega; - } + } } /*! This method is the main cyclical call for the scheduled part of the RW @@ -544,11 +584,12 @@ void ReactionWheelStateEffector::ConfigureRWRequests(double CurrentTime) @param CurrentSimNanos The current simulation time in nanoseconds */ -void ReactionWheelStateEffector::UpdateState(uint64_t CurrentSimNanos) +void +ReactionWheelStateEffector::UpdateState(uint64_t CurrentSimNanos) { - //! - Read the inputs and then call ConfigureRWRequests to set up dynamics - ReadInputs(); - ConfigureRWRequests(CurrentSimNanos*NANO2SEC); + //! - Read the inputs and then call ConfigureRWRequests to set up dynamics + ReadInputs(); + ConfigureRWRequests(CurrentSimNanos * NANO2SEC); WriteOutputMessages(CurrentSimNanos); -// + // } diff --git a/src/simulation/dynamics/reactionWheels/reactionWheelStateEffector.h b/src/simulation/dynamics/reactionWheels/reactionWheelStateEffector.h old mode 100755 new mode 100644 index 96d5e503f2..bb4bb2b32c --- a/src/simulation/dynamics/reactionWheels/reactionWheelStateEffector.h +++ b/src/simulation/dynamics/reactionWheels/reactionWheelStateEffector.h @@ -17,7 +17,6 @@ */ - #ifndef REACTIONWHEELSTATEEFFECTOR_H #define REACTIONWHEELSTATEEFFECTOR_H @@ -41,55 +40,66 @@ #include "architecture/utilities/avsEigenMRP.h" #include "architecture/utilities/avsEigenSupport.h" - - /*! @brief reaction wheel state effector class */ -class ReactionWheelStateEffector: public SysModel, public StateEffector { -public: +class ReactionWheelStateEffector + : public SysModel + , public StateEffector +{ + public: ReactionWheelStateEffector(); - ~ReactionWheelStateEffector(); - void registerStates(DynParamManager& states); - void linkInStates(DynParamManager& states); + ~ReactionWheelStateEffector(); + void registerStates(DynParamManager& states); + void linkInStates(DynParamManager& states); void writeOutputStateMessages(uint64_t integTimeNanos); - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN); - void updateEffectorMassProps(double integTime); //!< -- Method for stateEffector to give mass contributions - void updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N); //!< -- Back-sub contributions - void updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B); //!< -- Energy and momentum calculations + void computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN); + void updateEffectorMassProps(double integTime); //!< -- Method for stateEffector to give mass contributions + void updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N); //!< -- Back-sub contributions + void updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B); //!< -- Energy and momentum calculations void Reset(uint64_t CurrentSimNanos); void addReactionWheel(std::shared_ptr NewRW); - void UpdateState(uint64_t CurrentSimNanos); - void WriteOutputMessages(uint64_t CurrentClock); - void ReadInputs(); - void ConfigureRWRequests(double CurrentTime); - -public: - std::vector> ReactionWheelData; //!< -- RW information - - ReadFunctor rwMotorCmdInMsg; //!< -- RW motor torque array cmd input message - Message rwSpeedOutMsg; //!< -- RW speed array output message - std::vector*> rwOutMsgs; //!< -- vector of RW log output messages - - std::vector NewRWCmds; //!< -- Incoming attitude commands - RWSpeedMsgPayload rwSpeedMsgBuffer = {}; //!< (-) Output data from the reaction wheels - std::string nameOfReactionWheelOmegasState; //!< class variable - std::string nameOfReactionWheelThetasState; //!< class variable - size_t numRW; //!< number of reaction wheels - size_t numRWJitter; //!< number of RW with jitter - BSKLogger bskLogger; //!< -- BSK Logging - -private: - ArrayMotorTorqueMsgPayload incomingCmdBuffer = {}; //!< -- One-time allocation for savings - uint64_t prevCommandTime; //!< -- Time for previous valid thruster firing - - StateData *OmegasState; //!< class variable - StateData *thetasState; //!< class variable - Eigen::MatrixXd *g_N; //!< [m/s^2] Gravitational acceleration in N frame components - - double maxWheelAcceleration = 1.0e6; //!< [rad/s^2] Maximum allowed wheel acceleration to prevent numerical instability - double largeTorqueThreshold = 10.0; //!< [Nm] Threshold for warning about large torque with unlimited torque setting - -public: + void UpdateState(uint64_t CurrentSimNanos); + void WriteOutputMessages(uint64_t CurrentClock); + void ReadInputs(); + void ConfigureRWRequests(double CurrentTime); + + public: + std::vector> ReactionWheelData; //!< -- RW information + + ReadFunctor rwMotorCmdInMsg; //!< -- RW motor torque array cmd input message + Message rwSpeedOutMsg; //!< -- RW speed array output message + std::vector*> rwOutMsgs; //!< -- vector of RW log output messages + + std::vector NewRWCmds; //!< -- Incoming attitude commands + RWSpeedMsgPayload rwSpeedMsgBuffer = {}; //!< (-) Output data from the reaction wheels + std::string nameOfReactionWheelOmegasState; //!< class variable + std::string nameOfReactionWheelThetasState; //!< class variable + size_t numRW; //!< number of reaction wheels + size_t numRWJitter; //!< number of RW with jitter + BSKLogger bskLogger; //!< -- BSK Logging + + private: + ArrayMotorTorqueMsgPayload incomingCmdBuffer = {}; //!< -- One-time allocation for savings + uint64_t prevCommandTime; //!< -- Time for previous valid thruster firing + + StateData* OmegasState; //!< class variable + StateData* thetasState; //!< class variable + Eigen::MatrixXd* g_N; //!< [m/s^2] Gravitational acceleration in N frame components + + double maxWheelAcceleration = + 1.0e6; //!< [rad/s^2] Maximum allowed wheel acceleration to prevent numerical instability + double largeTorqueThreshold = 10.0; //!< [Nm] Threshold for warning about large torque with unlimited torque setting + + public: /*! @brief Get the maximum wheel acceleration threshold * @return Maximum wheel acceleration in rad/s^2 */ @@ -109,8 +119,6 @@ class ReactionWheelStateEffector: public SysModel, public StateEffector { * @param val New large torque threshold value in Nm */ void setLargeTorqueThreshold(double val) { largeTorqueThreshold = val; } - }; - #endif /* STATE_EFFECTOR_H */ diff --git a/src/simulation/dynamics/reactionWheels/reactionWheelSupport.h b/src/simulation/dynamics/reactionWheels/reactionWheelSupport.h old mode 100755 new mode 100644 index 7bc2b40014..67c490bfc4 --- a/src/simulation/dynamics/reactionWheels/reactionWheelSupport.h +++ b/src/simulation/dynamics/reactionWheels/reactionWheelSupport.h @@ -17,12 +17,15 @@ */ - #ifndef REACTIONWHEELSUPPORT_H #define REACTIONWHEELSUPPORT_H /*! @brief enumeration definiting the types of RW modes */ -enum RWModels { BalancedWheels, JitterSimple, JitterFullyCoupled }; - +enum RWModels +{ + BalancedWheels, + JitterSimple, + JitterFullyCoupled +}; #endif /* STATE_EFFECTOR_H */ 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/_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/spacecraft/spacecraft.cpp b/src/simulation/dynamics/spacecraft/spacecraft.cpp old mode 100755 new mode 100644 index 363420d949..31a448991e --- a/src/simulation/dynamics/spacecraft/spacecraft.cpp +++ b/src/simulation/dynamics/spacecraft/spacecraft.cpp @@ -24,7 +24,6 @@ #include "architecture/utilities/avsEigenMRP.h" #include - /*! This is the constructor, setting variables to default values */ Spacecraft::Spacecraft() { @@ -48,27 +47,25 @@ Spacecraft::Spacecraft() } /*! This is the destructor, nothing to report here */ -Spacecraft::~Spacecraft() -{ -} - +Spacecraft::~Spacecraft() {} /*! This method is used to reset the module. */ -void Spacecraft::Reset(uint64_t CurrentSimNanos) +void +Spacecraft::Reset(uint64_t CurrentSimNanos) { this->gravField.Reset(CurrentSimNanos); // - Call method for initializing the dynamics of spacecraft this->initializeDynamics(); - // compute initial spacecraft states relative to inertial frame, taking into account initial sc states might be defined relative to a planet + // compute initial spacecraft states relative to inertial frame, taking into account initial sc states might be + // defined relative to a planet this->gravField.updateInertialPosAndVel(this->hubR_N->getState(), this->hubV_N->getState()); this->writeOutputStateMessages(CurrentSimNanos); // - Loop over stateEffectors to call writeOutputStateMessages and write initial state output messages std::vector::iterator it; - for(it = this->states.begin(); it != this->states.end(); it++) - { + for (it = this->states.begin(); it != this->states.end(); it++) { // - Call writeOutputStateMessages for stateEffectors (*it)->writeOutputStateMessages(CurrentSimNanos); } @@ -77,25 +74,27 @@ void Spacecraft::Reset(uint64_t CurrentSimNanos) this->timeBeforeNanos = CurrentSimNanos; } - /*! This method attaches a stateEffector to the dynamicObject */ -void Spacecraft::addStateEffector(StateEffector *newStateEffector) +void +Spacecraft::addStateEffector(StateEffector* newStateEffector) { - this->assignStateParamNames(newStateEffector); + this->assignStateParamNames(newStateEffector); this->states.push_back(newStateEffector); } /*! This method attaches a dynamicEffector to the dynamicObject */ -void Spacecraft::addDynamicEffector(DynamicEffector *newDynamicEffector) +void +Spacecraft::addDynamicEffector(DynamicEffector* newDynamicEffector) { - this->assignStateParamNames(newDynamicEffector); + this->assignStateParamNames(newDynamicEffector); this->dynEffectors.push_back(newDynamicEffector); } /*! This is the method where the messages of the state of vehicle are written */ -void Spacecraft::writeOutputStateMessages(uint64_t clockTime) +void +Spacecraft::writeOutputStateMessages(uint64_t clockTime) { // - Populate state output message SCStatesMsgPayload stateOut; @@ -103,10 +102,10 @@ void Spacecraft::writeOutputStateMessages(uint64_t clockTime) eigenMatrixXd2CArray(*this->inertialPositionProperty, stateOut.r_BN_N); eigenMatrixXd2CArray(*this->inertialVelocityProperty, stateOut.v_BN_N); Eigen::MRPd sigmaLocal_BN; - sigmaLocal_BN = (Eigen::Vector3d) this->hubSigma->getState(); + sigmaLocal_BN = (Eigen::Vector3d)this->hubSigma->getState(); Eigen::Matrix3d dcm_NB = sigmaLocal_BN.toRotationMatrix(); - Eigen::Vector3d rLocal_CN_N = (*this->inertialPositionProperty) + dcm_NB*(*this->c_B); - Eigen::Vector3d vLocal_CN_N = (*this->inertialVelocityProperty) + dcm_NB*(*this->cDot_B); + Eigen::Vector3d rLocal_CN_N = (*this->inertialPositionProperty) + dcm_NB * (*this->c_B); + Eigen::Vector3d vLocal_CN_N = (*this->inertialVelocityProperty) + dcm_NB * (*this->cDot_B); eigenVector3d2CArray(rLocal_CN_N, stateOut.r_CN_N); eigenVector3d2CArray(vLocal_CN_N, stateOut.v_CN_N); eigenMatrixXd2CArray(this->hubSigma->getState(), stateOut.sigma_BN); @@ -122,14 +121,16 @@ void Spacecraft::writeOutputStateMessages(uint64_t clockTime) // - Populate mass state output message SCMassPropsMsgPayload massStateOut; massStateOut = this->scMassOutMsg.zeroMsgPayload; - massStateOut.massSC = (*this->m_SC)(0,0); + massStateOut.massSC = (*this->m_SC)(0, 0); eigenMatrixXd2CArray(*this->c_B, massStateOut.c_B); - eigenMatrixXd2CArray(*this->ISCPntB_B, (double *)massStateOut.ISC_PntB_B); + eigenMatrixXd2CArray(*this->ISCPntB_B, (double*)massStateOut.ISC_PntB_B); this->scMassOutMsg.write(&massStateOut, this->moduleID, clockTime); } -/*! If the optional attitude reference input message is set, then read in the reference attitude and set it for the hub*/ -void Spacecraft::readOptionalRefMsg() +/*! If the optional attitude reference input message is set, then read in the reference attitude and set it for the + * hub*/ +void +Spacecraft::readOptionalRefMsg() { if (this->attRefInMsg.isLinked()) { Eigen::Vector3d omega_BN_B; @@ -159,7 +160,8 @@ void Spacecraft::readOptionalRefMsg() } /*! This method is a part of sysModel and is used to integrate the state and update the state in the messaging system */ -void Spacecraft::UpdateState(uint64_t CurrentSimNanos) +void +Spacecraft::UpdateState(uint64_t CurrentSimNanos) { // - Get access to the spice bodies this->gravField.UpdateState(CurrentSimNanos); @@ -178,8 +180,7 @@ void Spacecraft::UpdateState(uint64_t CurrentSimNanos) this->writeOutputStateMessages(CurrentSimNanos); // - Loop over stateEffectors to call writeOutputStateMessages std::vector::iterator it; - for(it = this->states.begin(); it != this->states.end(); it++) - { + for (it = this->states.begin(); it != this->states.end(); it++) { // - Call writeOutputStateMessages for stateEffectors (*it)->writeOutputStateMessages(CurrentSimNanos); } @@ -187,12 +188,13 @@ void Spacecraft::UpdateState(uint64_t CurrentSimNanos) /*! This method allows the spacecraft to have access to the current state of the hub for MRP switching, writing messages, and calculating energy and momentum */ -void Spacecraft::linkInStates(DynParamManager& statesIn) +void +Spacecraft::linkInStates(DynParamManager& statesIn) { // - Get access to all hub states this->hubR_N = statesIn.getStateObject(this->hub.nameOfHubPosition); this->hubV_N = statesIn.getStateObject(this->hub.nameOfHubVelocity); - this->hubSigma = statesIn.getStateObject(this->hub.nameOfHubSigma); /* Need sigmaBN for MRP switching */ + this->hubSigma = statesIn.getStateObject(this->hub.nameOfHubSigma); /* Need sigmaBN for MRP switching */ this->hubOmega_BN_B = statesIn.getStateObject(this->hub.nameOfHubOmega); this->hubGravVelocity = statesIn.getStateObject(this->hub.nameOfHubGravVelocity); this->BcGravVelocity = statesIn.getStateObject(this->hub.nameOfBcGravVelocity); @@ -206,17 +208,18 @@ void Spacecraft::linkInStates(DynParamManager& statesIn) /*! This method is used to initialize the simulation by registering all of the states, linking the dynamicEffectors, stateEffectors, and the hub, initialize gravity, and initialize the sim with the initial conditions specified in python for the simulation */ -void Spacecraft::initializeDynamics() +void +Spacecraft::initializeDynamics() { // - Spacecraft initiates all of the spaceCraft mass properties - Eigen::MatrixXd initM_SC(1,1); - Eigen::MatrixXd initMDot_SC(1,1); - Eigen::MatrixXd initC_B(3,1); - Eigen::MatrixXd initISCPntB_B(3,3); - Eigen::MatrixXd initCPrime_B(3,1); - Eigen::MatrixXd initCDot_B(3,1); - Eigen::MatrixXd initISCPntBPrime_B(3,3); - Eigen::MatrixXd systemTime(2,1); + Eigen::MatrixXd initM_SC(1, 1); + Eigen::MatrixXd initMDot_SC(1, 1); + Eigen::MatrixXd initC_B(3, 1); + Eigen::MatrixXd initISCPntB_B(3, 3); + Eigen::MatrixXd initCPrime_B(3, 1); + Eigen::MatrixXd initCDot_B(3, 1); + Eigen::MatrixXd initISCPntBPrime_B(3, 3); + Eigen::MatrixXd systemTime(2, 1); systemTime.setZero(); // - Create the properties this->m_SC = this->dynManager.createProperty(this->propName_m_SC, initM_SC); @@ -236,8 +239,7 @@ void Spacecraft::initializeDynamics() // - Loop through stateEffectors to register their states std::vector::iterator stateIt; - for(stateIt = this->states.begin(); stateIt != this->states.end(); stateIt++) - { + for (stateIt = this->states.begin(); stateIt != this->states.end(); stateIt++) { (*stateIt)->registerStates(this->dynManager); } @@ -253,27 +255,25 @@ void Spacecraft::initializeDynamics() // - Pulling the state from the hub at this time gives us r_CN_N Eigen::Vector3d rInit_BN_N = this->hubR_N->getState(); Eigen::MRPd sigma_BN; - sigma_BN = (Eigen::Vector3d) this->hubSigma->getState(); + sigma_BN = (Eigen::Vector3d)this->hubSigma->getState(); Eigen::Matrix3d dcm_NB = sigma_BN.toRotationMatrix(); // - Substract off the center mass to leave r_BN_N - rInit_BN_N -= dcm_NB*(*this->c_B); + rInit_BN_N -= dcm_NB * (*this->c_B); // - Subtract off cDot_B to get v_BN_N Eigen::Vector3d vInit_BN_N = this->hubV_N->getState(); - vInit_BN_N -= dcm_NB*(*this->cDot_B); + vInit_BN_N -= dcm_NB * (*this->cDot_B); // - Finally set the translational states r_BN_N and v_BN_N with the corrections this->hubR_N->setState(rInit_BN_N); this->hubV_N->setState(vInit_BN_N); // - Loop through the stateEffectros to link in the states needed - for(stateIt = this->states.begin(); stateIt != this->states.end(); stateIt++) - { + for (stateIt = this->states.begin(); stateIt != this->states.end(); stateIt++) { (*stateIt)->linkInStates(this->dynManager); } // - Loop through the dynamicEffectors to link in the states needed std::vector::iterator dynIt; - for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) - { + for (dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) { (*dynIt)->linkInStates(this->dynManager); } @@ -284,8 +284,10 @@ void Spacecraft::initializeDynamics() this->equationsOfMotion(0.0, 1.0); } -/*! This method is used to update the mass properties of the entire spacecraft using contributions from stateEffectors */ -void Spacecraft::updateSCMassProps(double time) +/*! This method is used to update the mass properties of the entire spacecraft using contributions from stateEffectors + */ +void +Spacecraft::updateSCMassProps(double time) { // - Zero the properties which will get populated in this method (*this->m_SC).setZero(); @@ -298,29 +300,28 @@ void Spacecraft::updateSCMassProps(double time) // Add in hubs mass props to the spacecraft mass props this->hub.updateEffectorMassProps(time); - (*this->m_SC)(0,0) += this->hub.effProps.mEff; + (*this->m_SC)(0, 0) += this->hub.effProps.mEff; (*this->ISCPntB_B) += this->hub.effProps.IEffPntB_B; - (*this->c_B) += this->hub.effProps.mEff*this->hub.effProps.rEff_CB_B; + (*this->c_B) += this->hub.effProps.mEff * this->hub.effProps.rEff_CB_B; // - Loop through state effectors to get mass props std::vector::iterator it; - for(it = this->states.begin(); it != this->states.end(); it++) - { + for (it = this->states.begin(); it != this->states.end(); it++) { (*it)->updateEffectorMassProps(time); // - Add in effectors mass props into mass props of spacecraft - (*this->m_SC)(0,0) += (*it)->effProps.mEff; - (*this->mDot_SC)(0,0) += (*it)->effProps.mEffDot; + (*this->m_SC)(0, 0) += (*it)->effProps.mEff; + (*this->mDot_SC)(0, 0) += (*it)->effProps.mEffDot; (*this->ISCPntB_B) += (*it)->effProps.IEffPntB_B; - (*this->c_B) += (*it)->effProps.mEff*(*it)->effProps.rEff_CB_B; + (*this->c_B) += (*it)->effProps.mEff * (*it)->effProps.rEff_CB_B; (*this->ISCPntBPrime_B) += (*it)->effProps.IEffPrimePntB_B; - (*this->cPrime_B) += (*it)->effProps.mEff*(*it)->effProps.rEffPrime_CB_B; + (*this->cPrime_B) += (*it)->effProps.mEff * (*it)->effProps.rEffPrime_CB_B; // For high fidelity mass depletion, this is left out: += (*it)->effProps.mEffDot*(*it)->effProps.rEff_CB_B } // Divide c_B and cPrime_B by the total mass of the spaceCraft to finalize c_B and cPrime_B - (*this->c_B) = (*this->c_B)/(*this->m_SC)(0,0); - (*this->cPrime_B) = (*this->cPrime_B)/(*this->m_SC)(0,0) - - (*this->mDot_SC)(0,0)*(*this->c_B)/(*this->m_SC)(0,0)/(*this->m_SC)(0,0); + (*this->c_B) = (*this->c_B) / (*this->m_SC)(0, 0); + (*this->cPrime_B) = (*this->cPrime_B) / (*this->m_SC)(0, 0) - + (*this->mDot_SC)(0, 0) * (*this->c_B) / (*this->m_SC)(0, 0) / (*this->m_SC)(0, 0); Eigen::Vector3d omegaLocal_BN_B = hubOmega_BN_B->getState(); Eigen::Vector3d cLocal_B = (*this->c_B); (*this->cDot_B) = (*this->cPrime_B) + omegaLocal_BN_B.cross(cLocal_B); @@ -330,12 +331,13 @@ void Spacecraft::updateSCMassProps(double time) the stateEffectors. The hub also has gravity and dynamicEffectors acting on it and these relationships are controlled in this method. At the end of this method all of the states will have their corresponding state derivatives set in the dynParam Manager thus solving for Xdot*/ -void Spacecraft::equationsOfMotion(double integTimeSeconds, double timeStep) +void +Spacecraft::equationsOfMotion(double integTimeSeconds, double timeStep) { // - Update time to the current time uint64_t integTimeNanos = secToNano(integTimeSeconds); - (*this->sysTime) << (double) integTimeNanos, integTimeSeconds; + (*this->sysTime) << (double)integTimeNanos, integTimeSeconds; // - Zero all Matrices and vectors for back-sub and the dynamics this->hub.hubBackSubMatrices.matrixA.setZero(); @@ -356,18 +358,17 @@ void Spacecraft::equationsOfMotion(double integTimeSeconds, double timeStep) Eigen::Matrix3d dcm_NB; Eigen::Vector3d cLocal_N; - sigmaBNLoc = (Eigen::Vector3d) this->hubSigma->getState(); + sigmaBNLoc = (Eigen::Vector3d)this->hubSigma->getState(); dcm_NB = sigmaBNLoc.toRotationMatrix(); - cLocal_N = dcm_NB*(*this->c_B); - Eigen::Vector3d rLocal_CN_N = this->hubR_N->getState() + dcm_NB*(*this->c_B); - Eigen::Vector3d vLocal_CN_N = this->hubV_N->getState() + dcm_NB*(*this->cDot_B); + cLocal_N = dcm_NB * (*this->c_B); + Eigen::Vector3d rLocal_CN_N = this->hubR_N->getState() + dcm_NB * (*this->c_B); + Eigen::Vector3d vLocal_CN_N = this->hubV_N->getState() + dcm_NB * (*this->cDot_B); this->gravField.computeGravityField(rLocal_CN_N, vLocal_CN_N); // - Loop through dynEffectors to compute force and torque on the s/c std::vector::iterator dynIt; - for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) - { + for (dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) { // - Compute the force and torque contributions from the dynamicEffectors (*dynIt)->computeForceTorque(integTimeSeconds, timeStep); this->sumForceExternal_N += (*dynIt)->forceExternal_N; @@ -377,8 +378,7 @@ void Spacecraft::equationsOfMotion(double integTimeSeconds, double timeStep) // - Loop through state effectors to get contributions for back-substitution std::vector::iterator it; - for(it = this->states.begin(); it != this->states.end(); it++) - { + for (it = this->states.begin(); it != this->states.end(); it++) { /* - Set the contribution matrices to zero (just in case a stateEffector += on the matrix or the stateEffector doesn't have a contribution for a matrix and doesn't set the matrix to zero */ this->backSubContributions.matrixA.setZero(); @@ -389,7 +389,11 @@ void Spacecraft::equationsOfMotion(double integTimeSeconds, double timeStep) this->backSubContributions.vecRot.setZero(); // - Call the update contributions method for the stateEffectors and add in contributions to the hub matrices - (*it)->updateContributions(integTimeSeconds, this->backSubContributions, this->hubSigma->getState(), this->hubOmega_BN_B->getState(), *this->g_N); + (*it)->updateContributions(integTimeSeconds, + this->backSubContributions, + this->hubSigma->getState(), + this->hubOmega_BN_B->getState(), + *this->g_N); this->hub.hubBackSubMatrices.matrixA += this->backSubContributions.matrixA; this->hub.hubBackSubMatrices.matrixB += this->backSubContributions.matrixB; this->hub.hubBackSubMatrices.matrixC += this->backSubContributions.matrixC; @@ -407,20 +411,21 @@ void Spacecraft::equationsOfMotion(double integTimeSeconds, double timeStep) Eigen::Matrix3d intermediateMatrix; Eigen::Vector3d intermediateVector; Eigen::Vector3d omegaLocalBN_B = this->hubOmega_BN_B->getState(); - this->hub.hubBackSubMatrices.matrixA += (*this->m_SC)(0,0)*intermediateMatrix.Identity(); - intermediateMatrix = eigenTilde((*this->c_B)); // make c_B skew symmetric matrix - this->hub.hubBackSubMatrices.matrixB += -(*this->m_SC)(0,0)*intermediateMatrix; - this->hub.hubBackSubMatrices.matrixC += (*this->m_SC)(0,0)*intermediateMatrix; + this->hub.hubBackSubMatrices.matrixA += (*this->m_SC)(0, 0) * intermediateMatrix.Identity(); + intermediateMatrix = eigenTilde((*this->c_B)); // make c_B skew symmetric matrix + this->hub.hubBackSubMatrices.matrixB += -(*this->m_SC)(0, 0) * intermediateMatrix; + this->hub.hubBackSubMatrices.matrixC += (*this->m_SC)(0, 0) * intermediateMatrix; this->hub.hubBackSubMatrices.matrixD += *ISCPntB_B; - this->hub.hubBackSubMatrices.vecTrans += -2.0*(*this->m_SC)(0, 0)*omegaLocalBN_B.cross(cPrimeLocal_B) - - (*this->m_SC)(0, 0)*omegaLocalBN_B.cross(omegaLocalBN_B.cross(cLocal_B)) - - 2.0*(*mDot_SC)(0,0)*(cPrimeLocal_B+omegaLocalBN_B.cross(cLocal_B)); - intermediateVector = *ISCPntB_B*omegaLocalBN_B; - this->hub.hubBackSubMatrices.vecRot += -omegaLocalBN_B.cross(intermediateVector) - *ISCPntBPrime_B*omegaLocalBN_B; + this->hub.hubBackSubMatrices.vecTrans += + -2.0 * (*this->m_SC)(0, 0) * omegaLocalBN_B.cross(cPrimeLocal_B) - + (*this->m_SC)(0, 0) * omegaLocalBN_B.cross(omegaLocalBN_B.cross(cLocal_B)) - + 2.0 * (*mDot_SC)(0, 0) * (cPrimeLocal_B + omegaLocalBN_B.cross(cLocal_B)); + intermediateVector = *ISCPntB_B * omegaLocalBN_B; + this->hub.hubBackSubMatrices.vecRot += -omegaLocalBN_B.cross(intermediateVector) - *ISCPntBPrime_B * omegaLocalBN_B; // - Map external force_N to the body frame Eigen::Vector3d sumForceExternalMappedToB; - sumForceExternalMappedToB = dcm_NB.transpose()*this->sumForceExternal_N; + sumForceExternalMappedToB = dcm_NB.transpose() * this->sumForceExternal_N; // - Edit both v_trans and v_rot with gravity and external force and torque Eigen::Vector3d gLocal_N = *this->g_N; @@ -429,41 +434,48 @@ void Spacecraft::equationsOfMotion(double integTimeSeconds, double timeStep) // - Need to find force of gravity on the spacecraft Eigen::Vector3d gravityForce_N; - gravityForce_N = (*this->m_SC)(0,0)*gLocal_N; + gravityForce_N = (*this->m_SC)(0, 0) * gLocal_N; Eigen::Vector3d gravityForce_B; - gravityForce_B = dcm_NB.transpose()*gravityForce_N; + gravityForce_B = dcm_NB.transpose() * gravityForce_N; this->hub.hubBackSubMatrices.vecTrans += gravityForce_B + sumForceExternalMappedToB + this->sumForceExternal_B; this->hub.hubBackSubMatrices.vecRot += cLocal_B.cross(gravityForce_B) + this->sumTorquePntB_B; // - Compute the derivatives of the hub states before looping through stateEffectors - this->hub.computeDerivatives(integTimeSeconds, this->hubV_N->getStateDeriv(), this->hubOmega_BN_B->getStateDeriv(), this->hubSigma->getState()); + this->hub.computeDerivatives(integTimeSeconds, + this->hubV_N->getStateDeriv(), + this->hubOmega_BN_B->getStateDeriv(), + this->hubSigma->getState()); // - Loop through state effectors for compute derivatives - for(it = states.begin(); it != states.end(); it++) - { - (*it)->computeDerivatives(integTimeSeconds, this->hubV_N->getStateDeriv(), this->hubOmega_BN_B->getStateDeriv(), this->hubSigma->getState()); + for (it = states.begin(); it != states.end(); it++) { + (*it)->computeDerivatives(integTimeSeconds, + this->hubV_N->getStateDeriv(), + this->hubOmega_BN_B->getStateDeriv(), + this->hubSigma->getState()); } } /*! Prepare for integration process @param integrateToThisTimeNanos Time to integrate to */ -void Spacecraft::preIntegration(uint64_t integrateToThisTimeNanos) { +void +Spacecraft::preIntegration(uint64_t integrateToThisTimeNanos) +{ this->timeStep = diffNanoToSec(integrateToThisTimeNanos, this->timeBeforeNanos); // - Find the time step in seconds // - Find v_CN_N before integration for accumulated DV - Eigen::Vector3d oldV_BN_N = this->hubV_N->getState(); // - V_BN_N before integration - Eigen::Vector3d oldV_CN_N; // - V_CN_N before integration - Eigen::Vector3d oldC_B; // - Center of mass offset before integration - Eigen::MRPd oldSigma_BN; // - Sigma_BN before integration + Eigen::Vector3d oldV_BN_N = this->hubV_N->getState(); // - V_BN_N before integration + Eigen::Vector3d oldV_CN_N; // - V_CN_N before integration + Eigen::Vector3d oldC_B; // - Center of mass offset before integration + Eigen::MRPd oldSigma_BN; // - Sigma_BN before integration // - Get the angular rate, oldOmega_BN_B from the dyn manager this->oldOmega_BN_B = this->hubOmega_BN_B->getState(); // - Get center of mass, v_BN_N and dcm_NB from the dyn manager - oldSigma_BN = (Eigen::Vector3d) this->hubSigma->getState(); + oldSigma_BN = (Eigen::Vector3d)this->hubSigma->getState(); // - Finally find v_CN_N Eigen::Matrix3d oldDcm_NB = oldSigma_BN.toRotationMatrix(); // - dcm_NB before integration - oldV_CN_N = oldV_BN_N + oldDcm_NB*(*this->cDot_B); + oldV_CN_N = oldV_BN_N + oldDcm_NB * (*this->cDot_B); // - Integrate the state from the last time (timeBefore) to the integrateToThisTime this->hub.matchGravitytoVelocityState(oldV_CN_N); // Set gravity velocity to base velocity for DV estimation @@ -472,47 +484,49 @@ void Spacecraft::preIntegration(uint64_t integrateToThisTimeNanos) { /*! Perform post-integration steps @param integrateToThisTimeNanos Time to integrate to */ -void Spacecraft::postIntegration(uint64_t integrateToThisTimeNanos) { - this->timeBeforeNanos = integrateToThisTimeNanos; // - copy the current time into previous time for next integrate state call - this->timeBefore = integrateToThisTimeNanos*NANO2SEC; - double integrateToThisTime = integrateToThisTimeNanos*NANO2SEC; // - convert to seconds +void +Spacecraft::postIntegration(uint64_t integrateToThisTimeNanos) +{ + this->timeBeforeNanos = + integrateToThisTimeNanos; // - copy the current time into previous time for next integrate state call + this->timeBefore = integrateToThisTimeNanos * NANO2SEC; + double integrateToThisTime = integrateToThisTimeNanos * NANO2SEC; // - convert to seconds // - Call mass properties to get current info on the mass props of the spacecraft this->updateSCMassProps(integrateToThisTime); // - Find v_CN_N after the integration for accumulated DV Eigen::Vector3d newV_BN_N = this->hubV_N->getState(); // - V_BN_N after integration - Eigen::Vector3d newV_CN_N; // - V_CN_N after integration - Eigen::MRPd newSigma_BN; // - Sigma_BN after integration + Eigen::Vector3d newV_CN_N; // - V_CN_N after integration + Eigen::MRPd newSigma_BN; // - Sigma_BN after integration // - Get center of mass, v_BN_N and dcm_NB Eigen::Vector3d sigmaBNLoc; - sigmaBNLoc = (Eigen::Vector3d) this->hubSigma->getState(); + sigmaBNLoc = (Eigen::Vector3d)this->hubSigma->getState(); newSigma_BN = sigmaBNLoc; - Eigen::Matrix3d newDcm_NB = newSigma_BN.toRotationMatrix(); // - dcm_NB after integration - newV_CN_N = newV_BN_N + newDcm_NB*(*this->cDot_B); + Eigen::Matrix3d newDcm_NB = newSigma_BN.toRotationMatrix(); // - dcm_NB after integration + newV_CN_N = newV_BN_N + newDcm_NB * (*this->cDot_B); // - Find accumulated DV of the center of mass in the body frame - this->dvAccum_CN_B += newDcm_NB.transpose()*(newV_CN_N - - this->BcGravVelocity->getState()); + this->dvAccum_CN_B += newDcm_NB.transpose() * (newV_CN_N - this->BcGravVelocity->getState()); // - Find the accumulated DV of the body frame in the body frame - this->dvAccum_BN_B += newDcm_NB.transpose()*(newV_BN_N - - this->hubGravVelocity->getState()); + this->dvAccum_BN_B += newDcm_NB.transpose() * (newV_BN_N - this->hubGravVelocity->getState()); // - Find the accumulated DV of the center of mass in the inertial frame this->dvAccum_CN_N += newV_CN_N - this->BcGravVelocity->getState(); // - non-conservative acceleration of the body frame in the body frame - this->nonConservativeAccelpntB_B = (newDcm_NB.transpose()*(newV_BN_N - - this->hubGravVelocity->getState()))/this->timeStep; + this->nonConservativeAccelpntB_B = + (newDcm_NB.transpose() * (newV_BN_N - this->hubGravVelocity->getState())) / this->timeStep; // - angular acceleration in the body frame Eigen::Vector3d newOmega_BN_B; newOmega_BN_B = this->hubOmega_BN_B->getState(); if (fabs(this->timeStep) > 1e-10) { - this->omegaDot_BN_B = (newOmega_BN_B - this->oldOmega_BN_B)/this->timeStep; //angular acceleration of B wrt N in the Body frame + this->omegaDot_BN_B = + (newOmega_BN_B - this->oldOmega_BN_B) / this->timeStep; // angular acceleration of B wrt N in the Body frame } else { - this->omegaDot_BN_B = {0., 0., .0}; + this->omegaDot_BN_B = { 0., 0., .0 }; } // - Compute Energy and Momentum @@ -523,8 +537,7 @@ void Spacecraft::postIntegration(uint64_t integrateToThisTimeNanos) { // - Loop over stateEffectors to call modifyStates std::vector::iterator it; - for(it = this->states.begin(); it != this->states.end(); it++) - { + for (it = this->states.begin(); it != this->states.end(); it++) { // - Call energy and momentum calulations for stateEffectors (*it)->modifyStates(integrateToThisTime); } @@ -535,13 +548,14 @@ void Spacecraft::postIntegration(uint64_t integrateToThisTimeNanos) { /*! This method is used to find the total energy and momentum of the spacecraft. It finds the total orbital energy, total orbital angular momentum, total rotational energy and total rotational angular momentum. These values are used for validation purposes. */ -void Spacecraft::computeEnergyMomentum(double time) +void +Spacecraft::computeEnergyMomentum(double time) { // - Grab values from state Manager Eigen::Vector3d rLocal_BN_N = hubR_N->getState(); Eigen::Vector3d rDotLocal_BN_N = hubV_N->getState(); Eigen::MRPd sigmaLocal_BN; - sigmaLocal_BN = (Eigen::Vector3d ) hubSigma->getState(); + sigmaLocal_BN = (Eigen::Vector3d)hubSigma->getState(); // - Find DCM's Eigen::Matrix3d dcmLocal_NB = sigmaLocal_BN.toRotationMatrix(); @@ -550,8 +564,8 @@ void Spacecraft::computeEnergyMomentum(double time) // - Convert from inertial frame to body frame Eigen::Vector3d rBNLocal_B; Eigen::Vector3d rDotBNLocal_B; - rBNLocal_B = dcmLocal_BN*rLocal_BN_N; - rDotBNLocal_B = dcmLocal_BN*rDotLocal_BN_N; + rBNLocal_B = dcmLocal_BN * rLocal_BN_N; + rDotBNLocal_B = dcmLocal_BN * rDotLocal_BN_N; // - zero necessarry variables Eigen::Vector3d totOrbAngMomPntN_B; @@ -566,20 +580,21 @@ void Spacecraft::computeEnergyMomentum(double time) this->rotEnergyContr = 0.0; // - Get the hubs contribution - this->hub.updateEnergyMomContributions(time, this->rotAngMomPntCContr_B, this->rotEnergyContr, this->hubOmega_BN_B->getState()); + this->hub.updateEnergyMomContributions( + time, this->rotAngMomPntCContr_B, this->rotEnergyContr, this->hubOmega_BN_B->getState()); totRotAngMomPntC_B += this->rotAngMomPntCContr_B; this->totRotEnergy += this->rotEnergyContr; // - Loop over stateEffectors to get their contributions to energy and momentum std::vector::iterator it; - for(it = this->states.begin(); it != this->states.end(); it++) - { + for (it = this->states.begin(); it != this->states.end(); it++) { // - Set the matrices to zero this->rotAngMomPntCContr_B.setZero(); this->rotEnergyContr = 0.0; // - Call energy and momentum calulations for stateEffectors - (*it)->updateEnergyMomContributions(time, this->rotAngMomPntCContr_B, this->rotEnergyContr, this->hubOmega_BN_B->getState()); + (*it)->updateEnergyMomContributions( + time, this->rotAngMomPntCContr_B, this->rotEnergyContr, this->hubOmega_BN_B->getState()); totRotAngMomPntC_B += this->rotAngMomPntCContr_B; this->totRotEnergy += this->rotEnergyContr; } @@ -588,39 +603,40 @@ void Spacecraft::computeEnergyMomentum(double time) Eigen::Vector3d cDotLocal_B = (*this->cDot_B); // - Add in orbital kinetic energy into the total orbital energy calculations - this->totOrbEnergy += 1.0/2.0*(*this->m_SC)(0,0)*(rDotBNLocal_B.dot(rDotBNLocal_B) + 2.0*rDotBNLocal_B.dot(cDotLocal_B) - + cDotLocal_B.dot(cDotLocal_B)); + this->totOrbEnergy += + 1.0 / 2.0 * (*this->m_SC)(0, 0) * + (rDotBNLocal_B.dot(rDotBNLocal_B) + 2.0 * rDotBNLocal_B.dot(cDotLocal_B) + cDotLocal_B.dot(cDotLocal_B)); // - Call gravity effector and add in its potential contributions to the total orbital energy calculations this->orbPotentialEnergyContr = 0.0; - Eigen::Vector3d rLocal_CN_N = this->hubR_N->getState() + dcmLocal_NB*(*this->c_B); + Eigen::Vector3d rLocal_CN_N = this->hubR_N->getState() + dcmLocal_NB * (*this->c_B); gravField.updateEnergyContributions(rLocal_CN_N, this->orbPotentialEnergyContr); - this->totOrbEnergy += (*this->m_SC)(0,0)*this->orbPotentialEnergyContr; + this->totOrbEnergy += (*this->m_SC)(0, 0) * this->orbPotentialEnergyContr; // - Find total rotational energy - this->totRotEnergy += -1.0/2.0*(*this->m_SC)(0,0)*cDotLocal_B.dot(cDotLocal_B); + this->totRotEnergy += -1.0 / 2.0 * (*this->m_SC)(0, 0) * cDotLocal_B.dot(cDotLocal_B); // - Find orbital angular momentum for the spacecraft Eigen::Vector3d rCN_N; Eigen::Vector3d rDotCN_N; - rCN_N = rLocal_BN_N + dcmLocal_NB*(*this->c_B); - rDotCN_N = rDotLocal_BN_N + dcmLocal_NB*cDotLocal_B; - this->totOrbAngMomPntN_N = (*this->m_SC)(0,0)*(rCN_N.cross(rDotCN_N)); + rCN_N = rLocal_BN_N + dcmLocal_NB * (*this->c_B); + rDotCN_N = rDotLocal_BN_N + dcmLocal_NB * cDotLocal_B; + this->totOrbAngMomPntN_N = (*this->m_SC)(0, 0) * (rCN_N.cross(rDotCN_N)); // - Find rotational angular momentum for the spacecraft - totRotAngMomPntC_B += -(*this->m_SC)(0,0)*(Eigen::Vector3d (*this->c_B)).cross(cDotLocal_B); - this->totRotAngMomPntC_N = dcmLocal_NB*totRotAngMomPntC_B; + totRotAngMomPntC_B += -(*this->m_SC)(0, 0) * (Eigen::Vector3d(*this->c_B)).cross(cDotLocal_B); + this->totRotAngMomPntC_N = dcmLocal_NB * totRotAngMomPntC_B; } /*! This method is used to find the force and torque that each stateEffector is applying to the spacecraft. These values are held in the stateEffector class. Additionally, the stateDerivative value is behind the state values because they are calculated in the intergrator calls */ -void Spacecraft::calcForceTorqueFromStateEffectors(double time, Eigen::Vector3d omega_BN_B) +void +Spacecraft::calcForceTorqueFromStateEffectors(double time, Eigen::Vector3d omega_BN_B) { // - Loop over stateEffectors to get their contributions to energy and momentum std::vector::iterator it; - for(it = this->states.begin(); it != this->states.end(); it++) - { + for (it = this->states.begin(); it != this->states.end(); it++) { (*it)->calcForceTorqueOnBody(time, omega_BN_B); } } diff --git a/src/simulation/dynamics/spacecraft/spacecraft.h b/src/simulation/dynamics/spacecraft/spacecraft.h old mode 100755 new mode 100644 index cec0819b0f..fffc267f7b --- a/src/simulation/dynamics/spacecraft/spacecraft.h +++ b/src/simulation/dynamics/spacecraft/spacecraft.h @@ -39,61 +39,66 @@ #include "architecture/utilities/bskLogging.h" #include "architecture/messaging/messaging.h" - - /*! @brief spacecraft dynamic effector */ -class Spacecraft : public DynamicObject{ -public: - std::string sysTimePropertyName; //!< Name of the system time property - ReadFunctor attRefInMsg; //!< (optional) reference attitude input message name +class Spacecraft : public DynamicObject +{ + public: + std::string sysTimePropertyName; //!< Name of the system time property + ReadFunctor attRefInMsg; //!< (optional) reference attitude input message name ReadFunctor transRefInMsg; //!< (optional) reference translation input message name - double totOrbEnergy; //!< [J] Total orbital kinetic energy - double totRotEnergy; //!< [J] Total rotational energy - double rotEnergyContr; //!< [J] Contribution of stateEffector to total rotational energy - double orbPotentialEnergyContr; //!< [J] Contribution of stateEffector to total rotational energy - BackSubMatrices backSubContributions;//!< class variable - Eigen::Vector3d sumForceExternal_N; //!< [N] Sum of forces given in the inertial frame - Eigen::Vector3d sumForceExternal_B; //!< [N] Sum of forces given in the body frame - Eigen::Vector3d sumTorquePntB_B; //!< [N-m] Total torque about point B in B frame components - - Eigen::Vector3d dvAccum_CN_B; //!< [m/s] Accumulated delta-v of center of mass relative to inertial frame in body frame coordinates - Eigen::Vector3d dvAccum_BN_B; //!< [m/s] accumulated delta-v of body frame relative to inertial frame in body frame coordinates - Eigen::Vector3d dvAccum_CN_N; //!< [m/s] accumulated delta-v of center of mass relative to inertial frame in inertial frame coordinates - Eigen::Vector3d nonConservativeAccelpntB_B;//!< [m/s/s] Current spacecraft body acceleration in the B frame - Eigen::Vector3d omegaDot_BN_B; //!< [rad/s/s] angular acceleration of body wrt to N in body frame - Eigen::Vector3d totOrbAngMomPntN_N; //!< [kg m^2/s] Total orbital angular momentum about N in N frame compenents - Eigen::Vector3d totRotAngMomPntC_N; //!< [kg m^2/s] Total rotational angular momentum about C in N frame compenents - Eigen::Vector3d rotAngMomPntCContr_B; //!< [kg m^2/s] Contribution of stateEffector to total rotational angular mom. - HubEffector hub; //!< The spacecraft plus needs access to the spacecraft hub - GravityEffector gravField; //!< Gravity effector for gravitational field experienced by spacecraft - std::vector states; //!< Vector of state effectors attached to dynObject - std::vector dynEffectors; //!< Vector of dynamic effectors attached to dynObject - BSKLogger bskLogger; //!< BSK Logging - Message scStateOutMsg; //!< spacecraft state output message - Message scMassOutMsg; //!< spacecraft mass properties output message - -public: - Spacecraft(); //!< Constructor - ~Spacecraft(); //!< Destructor - void initializeDynamics(); //!< This method initializes all of the dynamics and variables for the s/c - void computeEnergyMomentum(double time); //!< This method computes the total energy and momentum of the s/c - void updateSCMassProps(double time); //!< This method computes the total mass properties of the s/c - void calcForceTorqueFromStateEffectors(double time, Eigen::Vector3d omega_BN_B); //!< This method computes the force and torque from the stateEffectors + double totOrbEnergy; //!< [J] Total orbital kinetic energy + double totRotEnergy; //!< [J] Total rotational energy + double rotEnergyContr; //!< [J] Contribution of stateEffector to total rotational energy + double orbPotentialEnergyContr; //!< [J] Contribution of stateEffector to total rotational energy + BackSubMatrices backSubContributions; //!< class variable + Eigen::Vector3d sumForceExternal_N; //!< [N] Sum of forces given in the inertial frame + Eigen::Vector3d sumForceExternal_B; //!< [N] Sum of forces given in the body frame + Eigen::Vector3d sumTorquePntB_B; //!< [N-m] Total torque about point B in B frame components + + Eigen::Vector3d dvAccum_CN_B; //!< [m/s] Accumulated delta-v of center of mass relative to inertial frame in body + //!< frame coordinates + Eigen::Vector3d + dvAccum_BN_B; //!< [m/s] accumulated delta-v of body frame relative to inertial frame in body frame coordinates + Eigen::Vector3d dvAccum_CN_N; //!< [m/s] accumulated delta-v of center of mass relative to inertial frame in + //!< inertial frame coordinates + Eigen::Vector3d nonConservativeAccelpntB_B; //!< [m/s/s] Current spacecraft body acceleration in the B frame + Eigen::Vector3d omegaDot_BN_B; //!< [rad/s/s] angular acceleration of body wrt to N in body frame + Eigen::Vector3d totOrbAngMomPntN_N; //!< [kg m^2/s] Total orbital angular momentum about N in N frame compenents + Eigen::Vector3d totRotAngMomPntC_N; //!< [kg m^2/s] Total rotational angular momentum about C in N frame compenents + Eigen::Vector3d rotAngMomPntCContr_B; //!< [kg m^2/s] Contribution of stateEffector to total rotational angular mom. + HubEffector hub; //!< The spacecraft plus needs access to the spacecraft hub + GravityEffector gravField; //!< Gravity effector for gravitational field experienced by spacecraft + std::vector states; //!< Vector of state effectors attached to dynObject + std::vector dynEffectors; //!< Vector of dynamic effectors attached to dynObject + BSKLogger bskLogger; //!< BSK Logging + Message scStateOutMsg; //!< spacecraft state output message + Message scMassOutMsg; //!< spacecraft mass properties output message + + public: + Spacecraft(); //!< Constructor + ~Spacecraft(); //!< Destructor + void initializeDynamics(); //!< This method initializes all of the dynamics and variables for the s/c + void computeEnergyMomentum(double time); //!< This method computes the total energy and momentum of the s/c + void updateSCMassProps(double time); //!< This method computes the total mass properties of the s/c + void calcForceTorqueFromStateEffectors( + double time, + Eigen::Vector3d omega_BN_B); //!< This method computes the force and torque from the stateEffectors void Reset(uint64_t CurrentSimNanos); - void writeOutputStateMessages(uint64_t clockTime); //!< Method to write all of the class output messages - void UpdateState(uint64_t CurrentSimNanos); //!< Runtime hook back into Basilisk arch - void linkInStates(DynParamManager& statesIn); //!< Method to get access to the hub's states - void equationsOfMotion(double integTimeSeconds, double timeStep); //!< This method computes the equations of motion for the whole system - void addStateEffector(StateEffector *newSateEffector); //!< Attaches a stateEffector to the system - void addDynamicEffector(DynamicEffector *newDynamicEffector); //!< Attaches a dynamicEffector - void preIntegration(uint64_t callTimeNanos) final; //!< method to perform pre-integration steps - void postIntegration(uint64_t callTimeNanos) final; //!< method to perform post-integration steps - -private: - - template + void writeOutputStateMessages(uint64_t clockTime); //!< Method to write all of the class output messages + void UpdateState(uint64_t CurrentSimNanos); //!< Runtime hook back into Basilisk arch + void linkInStates(DynParamManager& statesIn); //!< Method to get access to the hub's states + void equationsOfMotion(double integTimeSeconds, + double timeStep); //!< This method computes the equations of motion for the whole system + void addStateEffector(StateEffector* newSateEffector); //!< Attaches a stateEffector to the system + void addDynamicEffector(DynamicEffector* newDynamicEffector); //!< Attaches a dynamicEffector + void preIntegration(uint64_t callTimeNanos) final; //!< method to perform pre-integration steps + void postIntegration(uint64_t callTimeNanos) final; //!< method to perform post-integration steps + + private: + template /** Assign the state engine state and parameter names to an effector */ - void assignStateParamNames(Type effector) { + void assignStateParamNames(Type effector) + { /* assign the state engine names for the parent rigid body states */ effector->setStateNameOfPosition(this->hub.nameOfHubPosition); effector->setStateNameOfVelocity(this->hub.nameOfHubVelocity); @@ -113,40 +118,38 @@ class Spacecraft : public DynamicObject{ effector->setPropName_vehicleGravity(this->gravField.vehicleGravityPropName); }; - StateData *hubR_N; //!< State data accesss to inertial position for the hub - StateData *hubV_N; //!< State data access to inertial velocity for the hub - StateData *hubOmega_BN_B; //!< State data access to the attitude rate of the hub - StateData *hubSigma; //!< State data access to sigmaBN for the hub - StateData *hubGravVelocity; //!< State data access to the gravity-accumulated DV on the Body frame - StateData *BcGravVelocity; //!< State data access to the gravity-accumulated DV on point Bc - Eigen::MatrixXd *inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - Eigen::MatrixXd *inertialVelocityProperty; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase - - - Eigen::MatrixXd *m_SC; //!< [kg] spacecrafts total mass - Eigen::MatrixXd *mDot_SC; //!< [kg/s] Time derivative of spacecrafts total mass - Eigen::MatrixXd *ISCPntB_B; //!< [kg m^2] Inertia of s/c about point B in B frame components - Eigen::MatrixXd *c_B; //!< [m] Vector from point B to CoM of s/c in B frame components - Eigen::MatrixXd *cPrime_B; //!< [m/s] Body time derivative of c_B - Eigen::MatrixXd *cDot_B; //!< [m/s] Inertial time derivative of c_B - Eigen::MatrixXd *ISCPntBPrime_B; //!< [kg m^2/s] Body time derivative of ISCPntB_B - Eigen::MatrixXd *g_N; //!< [m/s^2] Gravitational acceleration in N frame components - Eigen::MatrixXd *sysTime; //!< [s] System time - - Eigen::Vector3d oldOmega_BN_B; //!< [r/s] prior angular rate of B wrt N in the Body frame - - std::string propName_m_SC; //!< property name of m_SC - std::string propName_mDot_SC; //!< property name of mDot_SC - std::string propName_centerOfMassSC; //!< property name of centerOfMassSC - std::string propName_inertiaSC; //!< property name of inertiaSC - std::string propName_inertiaPrimeSC; //!< property name of inertiaPrimeSC - std::string propName_centerOfMassPrimeSC; //!< property name of centerOfMassPrimeSC - std::string propName_centerOfMassDotSC; //!< property name of centerOfMassDotSC - - -private: - void readOptionalRefMsg(); //!< Read the optional attitude or translational reference input message and set the reference states + StateData* hubR_N; //!< State data accesss to inertial position for the hub + StateData* hubV_N; //!< State data access to inertial velocity for the hub + StateData* hubOmega_BN_B; //!< State data access to the attitude rate of the hub + StateData* hubSigma; //!< State data access to sigmaBN for the hub + StateData* hubGravVelocity; //!< State data access to the gravity-accumulated DV on the Body frame + StateData* BcGravVelocity; //!< State data access to the gravity-accumulated DV on point Bc + Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialVelocityProperty; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase + + Eigen::MatrixXd* m_SC; //!< [kg] spacecrafts total mass + Eigen::MatrixXd* mDot_SC; //!< [kg/s] Time derivative of spacecrafts total mass + Eigen::MatrixXd* ISCPntB_B; //!< [kg m^2] Inertia of s/c about point B in B frame components + Eigen::MatrixXd* c_B; //!< [m] Vector from point B to CoM of s/c in B frame components + Eigen::MatrixXd* cPrime_B; //!< [m/s] Body time derivative of c_B + Eigen::MatrixXd* cDot_B; //!< [m/s] Inertial time derivative of c_B + Eigen::MatrixXd* ISCPntBPrime_B; //!< [kg m^2/s] Body time derivative of ISCPntB_B + Eigen::MatrixXd* g_N; //!< [m/s^2] Gravitational acceleration in N frame components + Eigen::MatrixXd* sysTime; //!< [s] System time + + Eigen::Vector3d oldOmega_BN_B; //!< [r/s] prior angular rate of B wrt N in the Body frame + + std::string propName_m_SC; //!< property name of m_SC + std::string propName_mDot_SC; //!< property name of mDot_SC + std::string propName_centerOfMassSC; //!< property name of centerOfMassSC + std::string propName_inertiaSC; //!< property name of inertiaSC + std::string propName_inertiaPrimeSC; //!< property name of inertiaPrimeSC + std::string propName_centerOfMassPrimeSC; //!< property name of centerOfMassPrimeSC + std::string propName_centerOfMassDotSC; //!< property name of centerOfMassDotSC + + private: + void readOptionalRefMsg(); //!< Read the optional attitude or translational reference input message and set the + //!< reference states }; - #endif /* SPACECRAFT_PLUS_H */ 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.cpp b/src/simulation/dynamics/spacecraftSystem/spacecraftSystem.cpp old mode 100755 new mode 100644 index 5c8f69e848..d82b4d57aa --- a/src/simulation/dynamics/spacecraftSystem/spacecraftSystem.cpp +++ b/src/simulation/dynamics/spacecraftSystem/spacecraftSystem.cpp @@ -42,9 +42,10 @@ SpacecraftUnit::~SpacecraftUnit() return; } -void SpacecraftUnit::addStateEffector(StateEffector *newStateEffector) +void +SpacecraftUnit::addStateEffector(StateEffector* newStateEffector) { - this->assignStateParamNames(newStateEffector); + this->assignStateParamNames(newStateEffector); this->states.push_back(newStateEffector); @@ -52,34 +53,38 @@ void SpacecraftUnit::addStateEffector(StateEffector *newStateEffector) newStateEffector->nameOfSpacecraftAttachedTo = this->spacecraftName; } -void SpacecraftUnit::addDynamicEffector(DynamicEffector *newDynamicEffector) +void +SpacecraftUnit::addDynamicEffector(DynamicEffector* newDynamicEffector) { - this->assignStateParamNames(newDynamicEffector); + this->assignStateParamNames(newDynamicEffector); this->dynEffectors.push_back(newDynamicEffector); } -void SpacecraftUnit::addDockingPort(DockingData *newDockingPort) +void +SpacecraftUnit::addDockingPort(DockingData* newDockingPort) { this->dockingPoints.push_back(newDockingPort); return; } -void SpacecraftUnit::SelfInitSC(int64_t moduleID) +void +SpacecraftUnit::SelfInitSC(int64_t moduleID) { } - /*! This method is used to reset the module. */ -void SpacecraftUnit::ResetSC(uint64_t CurrentSimNanos) +void +SpacecraftUnit::ResetSC(uint64_t CurrentSimNanos) { this->gravField.Reset(CurrentSimNanos); } -void SpacecraftUnit::writeOutputMessagesSC(uint64_t clockTime, int64_t moduleID) +void +SpacecraftUnit::writeOutputMessagesSC(uint64_t clockTime, int64_t moduleID) { // - Write output messages for each spacecraft // - Populate state output message @@ -88,10 +93,10 @@ void SpacecraftUnit::writeOutputMessagesSC(uint64_t clockTime, int64_t moduleID) eigenMatrixXd2CArray(*this->inertialPositionProperty, stateOut.r_BN_N); eigenMatrixXd2CArray(*this->inertialVelocityProperty, stateOut.v_BN_N); Eigen::MRPd sigmaLocal_BN; - sigmaLocal_BN = (Eigen::Vector3d) this->hubSigma->getState(); + sigmaLocal_BN = (Eigen::Vector3d)this->hubSigma->getState(); Eigen::Matrix3d dcm_NB = sigmaLocal_BN.toRotationMatrix(); - Eigen::Vector3d rLocal_CN_N = (*this->inertialPositionProperty) + dcm_NB*(*this->c_B); - Eigen::Vector3d vLocal_CN_N = (*this->inertialVelocityProperty) + dcm_NB*(*this->cDot_B); + Eigen::Vector3d rLocal_CN_N = (*this->inertialPositionProperty) + dcm_NB * (*this->c_B); + Eigen::Vector3d vLocal_CN_N = (*this->inertialVelocityProperty) + dcm_NB * (*this->cDot_B); eigenVector3d2CArray(rLocal_CN_N, stateOut.r_CN_N); eigenVector3d2CArray(vLocal_CN_N, stateOut.v_CN_N); eigenMatrixXd2CArray(this->hubSigma->getState(), stateOut.sigma_BN); @@ -106,9 +111,9 @@ void SpacecraftUnit::writeOutputMessagesSC(uint64_t clockTime, int64_t moduleID) // - Populate mass state output message SCMassPropsMsgPayload massStateOut; massStateOut = this->scMassStateOutMsg.zeroMsgPayload; - massStateOut.massSC = (*this->m_SC)(0,0); + massStateOut.massSC = (*this->m_SC)(0, 0); eigenMatrixXd2CArray(*this->c_B, massStateOut.c_B); - eigenMatrixXd2CArray(*this->ISCPntB_B, (double *)massStateOut.ISC_PntB_B); + eigenMatrixXd2CArray(*this->ISCPntB_B, (double*)massStateOut.ISC_PntB_B); this->scMassStateOutMsg.write(&massStateOut, moduleID, clockTime); // - Populate energy momentum output message @@ -121,12 +126,13 @@ void SpacecraftUnit::writeOutputMessagesSC(uint64_t clockTime, int64_t moduleID) this->scEnergyMomentumOutMsg.write(&energyMomentumOut, moduleID, clockTime); return; } -void SpacecraftUnit::linkInStatesSC(DynParamManager& statesIn) +void +SpacecraftUnit::linkInStatesSC(DynParamManager& statesIn) { // - Get access to all spacecraft hub states this->hubR_N = statesIn.getStateObject(this->hub.nameOfHubPosition); this->hubV_N = statesIn.getStateObject(this->hub.nameOfHubVelocity); - this->hubSigma = statesIn.getStateObject(this->hub.nameOfHubSigma); /* Need sigmaBN for MRP switching */ + this->hubSigma = statesIn.getStateObject(this->hub.nameOfHubSigma); /* Need sigmaBN for MRP switching */ this->hubOmega_BN_B = statesIn.getStateObject(this->hub.nameOfHubOmega); this->hubGravVelocity = statesIn.getStateObject(this->hub.nameOfHubGravVelocity); this->BcGravVelocity = statesIn.getStateObject(this->hub.nameOfBcGravVelocity); @@ -139,16 +145,17 @@ void SpacecraftUnit::linkInStatesSC(DynParamManager& statesIn) return; } -void SpacecraftUnit::initializeDynamicsSC(DynParamManager& statesIn) +void +SpacecraftUnit::initializeDynamicsSC(DynParamManager& statesIn) { // - Spacecraft() initiates all of the spaceCraft mass properties - Eigen::MatrixXd initM_SC(1,1); - Eigen::MatrixXd initMDot_SC(1,1); - Eigen::MatrixXd initC_B(3,1); - Eigen::MatrixXd initISCPntB_B(3,3); - Eigen::MatrixXd initCPrime_B(3,1); - Eigen::MatrixXd initCDot_B(3,1); - Eigen::MatrixXd initISCPntBPrime_B(3,3); + Eigen::MatrixXd initM_SC(1, 1); + Eigen::MatrixXd initMDot_SC(1, 1); + Eigen::MatrixXd initC_B(3, 1); + Eigen::MatrixXd initISCPntB_B(3, 3); + Eigen::MatrixXd initCPrime_B(3, 1); + Eigen::MatrixXd initCDot_B(3, 1); + Eigen::MatrixXd initISCPntBPrime_B(3, 3); // - Create the properties this->propName_m_SC = this->spacecraftName + "m_SC"; @@ -175,8 +182,7 @@ void SpacecraftUnit::initializeDynamicsSC(DynParamManager& statesIn) this->hub.prependSpacecraftNameToStates(); this->gravField.prependSpacecraftNameToStates(); std::vector::iterator stateIt; - for(stateIt = this->states.begin(); stateIt != this->states.end(); stateIt++) - { + for (stateIt = this->states.begin(); stateIt != this->states.end(); stateIt++) { (*stateIt)->prependSpacecraftNameToStates(); } @@ -187,8 +193,7 @@ void SpacecraftUnit::initializeDynamicsSC(DynParamManager& statesIn) this->hub.registerStates(statesIn); // - Loop through stateEffectors to register their states - for(stateIt = this->states.begin(); stateIt != this->states.end(); stateIt++) - { + for (stateIt = this->states.begin(); stateIt != this->states.end(); stateIt++) { (*stateIt)->registerStates(statesIn); } @@ -198,15 +203,13 @@ void SpacecraftUnit::initializeDynamicsSC(DynParamManager& statesIn) this->hub.linkInStates(statesIn); // - Loop through the stateEffectors to link in the states needed - for(stateIt = this->states.begin(); stateIt != this->states.end(); stateIt++) - { + for (stateIt = this->states.begin(); stateIt != this->states.end(); stateIt++) { (*stateIt)->linkInStates(statesIn); } // - Loop through the dynamicEffectors to link in the states needed std::vector::iterator dynIt; - for(dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) - { + for (dynIt = this->dynEffectors.begin(); dynIt != this->dynEffectors.end(); dynIt++) { (*dynIt)->linkInStates(statesIn); } @@ -236,7 +239,8 @@ SpacecraftSystem::~SpacecraftSystem() } /*! This method adds a spacecraft to the simulation as a free floating spacecraft */ -void SpacecraftSystem::addSpacecraftUndocked(SpacecraftUnit *newSpacecraft) +void +SpacecraftSystem::addSpacecraftUndocked(SpacecraftUnit* newSpacecraft) { this->unDockedSpacecraft.push_back(newSpacecraft); @@ -244,67 +248,73 @@ void SpacecraftSystem::addSpacecraftUndocked(SpacecraftUnit *newSpacecraft) } /*! This method attaches a spacecraft to the chain of spacecraft attached to the primary spacecraft */ -void SpacecraftSystem::attachSpacecraftToPrimary(SpacecraftUnit *newSpacecraft, std::string dockingPortNameOfNewSpacecraft, std::string dockingToPortName) +void +SpacecraftSystem::attachSpacecraftToPrimary(SpacecraftUnit* newSpacecraft, + std::string dockingPortNameOfNewSpacecraft, + std::string dockingToPortName) { // Create chain of docked spacecraft std::vector::iterator dockingItPrimary; int checkDock = 0; - for(dockingItPrimary = this->primaryCentralSpacecraft.dockingPoints.begin(); dockingItPrimary != this->primaryCentralSpacecraft.dockingPoints.end(); dockingItPrimary++) - { + for (dockingItPrimary = this->primaryCentralSpacecraft.dockingPoints.begin(); + dockingItPrimary != this->primaryCentralSpacecraft.dockingPoints.end(); + dockingItPrimary++) { std::vector::iterator dockingIt; if (dockingToPortName == (*dockingItPrimary)->portName) { - for(dockingIt = newSpacecraft->dockingPoints.begin(); dockingIt != newSpacecraft->dockingPoints.end(); dockingIt++) - { + for (dockingIt = newSpacecraft->dockingPoints.begin(); dockingIt != newSpacecraft->dockingPoints.end(); + dockingIt++) { if (dockingPortNameOfNewSpacecraft == (*dockingIt)->portName) { (*dockingIt)->r_DP_P = (*dockingItPrimary)->r_DP_P; (*dockingIt)->dcm_DP = (*dockingItPrimary)->dcm_DP; - newSpacecraft->hub.dcm_BP = (*dockingIt)->dcm_DB.transpose()*(*dockingIt)->dcm_DP; - newSpacecraft->hub.r_BP_P = (*dockingIt)->r_DP_P - newSpacecraft->hub.dcm_BP.transpose()*(*dockingIt)->r_DB_B; + newSpacecraft->hub.dcm_BP = (*dockingIt)->dcm_DB.transpose() * (*dockingIt)->dcm_DP; + newSpacecraft->hub.r_BP_P = + (*dockingIt)->r_DP_P - newSpacecraft->hub.dcm_BP.transpose() * (*dockingIt)->r_DB_B; checkDock += 1; break; } else { - std::cerr << __FILE__ <<": Port name given is not the same port name held in the newSpacecraft"; - std::cerr << " Quitting."<::iterator spacecraftConnectedIt; - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { std::vector::iterator dockingItConnected; - for(dockingItConnected = (*spacecraftConnectedIt)->dockingPoints.begin(); dockingItConnected != (*spacecraftConnectedIt)->dockingPoints.end(); dockingItConnected++) - { + for (dockingItConnected = (*spacecraftConnectedIt)->dockingPoints.begin(); + dockingItConnected != (*spacecraftConnectedIt)->dockingPoints.end(); + dockingItConnected++) { std::vector::iterator dockingIt; if (dockingToPortName == (*dockingItConnected)->portName) { - for(dockingIt = newSpacecraft->dockingPoints.begin(); dockingIt != newSpacecraft->dockingPoints.end(); dockingIt++) - { + for (dockingIt = newSpacecraft->dockingPoints.begin(); + dockingIt != newSpacecraft->dockingPoints.end(); + dockingIt++) { if (dockingPortNameOfNewSpacecraft == (*dockingIt)->portName) { (*dockingIt)->r_DP_P = (*dockingItConnected)->r_DP_P; (*dockingIt)->dcm_DP = (*dockingItConnected)->dcm_DP; - newSpacecraft->hub.dcm_BP = (*dockingIt)->dcm_DB.transpose()*(*dockingIt)->dcm_DP; - newSpacecraft->hub.r_BP_P = (*dockingIt)->r_DP_P - newSpacecraft->hub.dcm_BP.transpose()*(*dockingIt)->r_DB_B; + newSpacecraft->hub.dcm_BP = (*dockingIt)->dcm_DB.transpose() * (*dockingIt)->dcm_DP; + newSpacecraft->hub.r_BP_P = + (*dockingIt)->r_DP_P - newSpacecraft->hub.dcm_BP.transpose() * (*dockingIt)->r_DB_B; checkDock += 1; break; } else { - std::cerr << __FILE__ <<": Port name given is not the same port name held in the newSpacecraft"; - std::cerr << " Quitting."<numberOfSCAttachedToPrimary += 1; newSpacecraft->docked = true; @@ -316,25 +326,27 @@ void SpacecraftSystem::attachSpacecraftToPrimary(SpacecraftUnit *newSpacecraft, return; } - /*! This method is used to reset the module. */ -void SpacecraftSystem::Reset(uint64_t CurrentSimNanos) +void +SpacecraftSystem::Reset(uint64_t CurrentSimNanos) { this->primaryCentralSpacecraft.ResetSC(CurrentSimNanos); // - Call this for all of the connected spacecraft std::vector::iterator spacecraftConnectedIt; - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { (*spacecraftConnectedIt)->ResetSC(CurrentSimNanos); } // - Call this for all of the unconnected spacecraft std::vector::iterator spacecraftUnConnectedIt; - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { (*spacecraftUnConnectedIt)->ResetSC(CurrentSimNanos); } @@ -346,22 +358,25 @@ void SpacecraftSystem::Reset(uint64_t CurrentSimNanos) } /*! This is the method where the messages of the state of vehicle are written */ -void SpacecraftSystem::writeOutputMessages(uint64_t clockTime) +void +SpacecraftSystem::writeOutputMessages(uint64_t clockTime) { // - Call writeOutputMessages for primary spacecraft this->primaryCentralSpacecraft.writeOutputMessagesSC(clockTime, this->moduleID); // - Call this for all of the connected spacecraft std::vector::iterator spacecraftConnectedIt; - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { (*spacecraftConnectedIt)->writeOutputMessagesSC(clockTime, this->moduleID); } // - Call this for all of the unconnected spacecraft std::vector::iterator spacecraftUnConnectedIt; - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { (*spacecraftUnConnectedIt)->writeOutputMessagesSC(clockTime, this->moduleID); } @@ -369,7 +384,8 @@ void SpacecraftSystem::writeOutputMessages(uint64_t clockTime) } /*! This method is a part of sysModel and is used to integrate the state and update the state in the messaging system */ -void SpacecraftSystem::UpdateState(uint64_t CurrentSimNanos) +void +SpacecraftSystem::UpdateState(uint64_t CurrentSimNanos) { // - Get access to the spice bodies @@ -377,15 +393,17 @@ void SpacecraftSystem::UpdateState(uint64_t CurrentSimNanos) // - Call this for all of the connected spacecraft std::vector::iterator spacecraftConnectedIt; - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { (*spacecraftConnectedIt)->gravField.UpdateState(CurrentSimNanos); } // - Call this for all of the unconnected spacecraft std::vector::iterator spacecraftUnConnectedIt; - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { (*spacecraftUnConnectedIt)->gravField.UpdateState(CurrentSimNanos); } @@ -398,16 +416,18 @@ void SpacecraftSystem::UpdateState(uint64_t CurrentSimNanos) this->primaryCentralSpacecraft.gravField.updateInertialPosAndVel(rLocal_BN_N, vLocal_BN_N); // - Same thing for all of the connected spacecraft - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { rLocal_BN_N = (*spacecraftConnectedIt)->hubR_N->getState(); vLocal_BN_N = (*spacecraftConnectedIt)->hubV_N->getState(); (*spacecraftConnectedIt)->gravField.updateInertialPosAndVel(rLocal_BN_N, vLocal_BN_N); } // - Same thing for all of the connected spacecraft - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { rLocal_BN_N = (*spacecraftUnConnectedIt)->hubR_N->getState(); vLocal_BN_N = (*spacecraftUnConnectedIt)->hubV_N->getState(); (*spacecraftUnConnectedIt)->gravField.updateInertialPosAndVel(rLocal_BN_N, vLocal_BN_N); @@ -422,9 +442,10 @@ void SpacecraftSystem::UpdateState(uint64_t CurrentSimNanos) /*! This method is used to initialize the simulation by registering all of the states, linking the dynamicEffectors, stateEffectors, and the hub, initialize gravity, and initialize the sim with the initial conditions specified in python for the simulation */ -void SpacecraftSystem::initializeDynamics() +void +SpacecraftSystem::initializeDynamics() { - Eigen::MatrixXd systemTime(2,1); + Eigen::MatrixXd systemTime(2, 1); systemTime.setZero(); this->sysTime = this->dynManager.createProperty(this->sysTimePropertyName, systemTime); @@ -433,15 +454,17 @@ void SpacecraftSystem::initializeDynamics() // - Call this for all of the connected spacecraft std::vector::iterator spacecraftConnectedIt; - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { (*spacecraftConnectedIt)->initializeDynamicsSC(this->dynManager); } // - Call this for all of the unconnected spacecraft std::vector::iterator spacecraftUnConnectedIt; - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { (*spacecraftUnConnectedIt)->initializeDynamicsSC(this->dynManager); } @@ -449,8 +472,9 @@ void SpacecraftSystem::initializeDynamics() this->updateSystemMassProps(0.0); // - Call mass props for all the rest of the spacecraft - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { this->updateSpacecraftMassProps(0.0, (*(*spacecraftUnConnectedIt))); } @@ -462,18 +486,18 @@ void SpacecraftSystem::initializeDynamics() // - Initialize this for all other spacecraft // - Call mass props for all the rest of the spacecraft - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { this->initializeSCPosVelocity((*(*spacecraftUnConnectedIt))); } // - Call all stateEffectors in each spacecraft to give them body frame information std::vector::iterator spacecraftIt; - for(spacecraftIt = this->spacecraftDockedToPrimary.begin(); spacecraftIt != this->spacecraftDockedToPrimary.end(); spacecraftIt++) - { + for (spacecraftIt = this->spacecraftDockedToPrimary.begin(); spacecraftIt != this->spacecraftDockedToPrimary.end(); + spacecraftIt++) { std::vector::iterator stateIt; - for(stateIt = (*spacecraftIt)->states.begin(); stateIt != (*spacecraftIt)->states.end(); stateIt++) - { + for (stateIt = (*spacecraftIt)->states.begin(); stateIt != (*spacecraftIt)->states.end(); stateIt++) { (*stateIt)->receiveMotherSpacecraftData((*spacecraftIt)->hub.r_BP_P, (*spacecraftIt)->hub.dcm_BP); } } @@ -484,8 +508,10 @@ void SpacecraftSystem::initializeDynamics() return; } -/*! This method is used to update the mass properties of the entire spacecraft using contributions from stateEffectors */ -void SpacecraftSystem::updateSpacecraftMassProps(double time, SpacecraftUnit& spacecraft) +/*! This method is used to update the mass properties of the entire spacecraft using contributions from stateEffectors + */ +void +SpacecraftSystem::updateSpacecraftMassProps(double time, SpacecraftUnit& spacecraft) { // - Zero the properties which will get populated in this method (*spacecraft.m_SC).setZero(); @@ -498,29 +524,29 @@ void SpacecraftSystem::updateSpacecraftMassProps(double time, SpacecraftUnit& sp // Add in hubs mass props to the spacecraft mass props spacecraft.hub.updateEffectorMassProps(time); - (*spacecraft.m_SC)(0,0) += spacecraft.hub.effProps.mEff; + (*spacecraft.m_SC)(0, 0) += spacecraft.hub.effProps.mEff; (*spacecraft.ISCPntB_B) += spacecraft.hub.effProps.IEffPntB_B; - (*spacecraft.c_B) += spacecraft.hub.effProps.mEff*spacecraft.hub.effProps.rEff_CB_B; + (*spacecraft.c_B) += spacecraft.hub.effProps.mEff * spacecraft.hub.effProps.rEff_CB_B; // - Loop through state effectors to get mass props std::vector::iterator it; - for(it = spacecraft.states.begin(); it != spacecraft.states.end(); it++) - { + for (it = spacecraft.states.begin(); it != spacecraft.states.end(); it++) { (*it)->updateEffectorMassProps(time); // - Add in effectors mass props into mass props of spacecraft - (*spacecraft.m_SC)(0,0) += (*it)->effProps.mEff; - (*spacecraft.mDot_SC)(0,0) += (*it)->effProps.mEffDot; + (*spacecraft.m_SC)(0, 0) += (*it)->effProps.mEff; + (*spacecraft.mDot_SC)(0, 0) += (*it)->effProps.mEffDot; (*spacecraft.ISCPntB_B) += (*it)->effProps.IEffPntB_B; - (*spacecraft.c_B) += (*it)->effProps.mEff*(*it)->effProps.rEff_CB_B; + (*spacecraft.c_B) += (*it)->effProps.mEff * (*it)->effProps.rEff_CB_B; (*spacecraft.ISCPntBPrime_B) += (*it)->effProps.IEffPrimePntB_B; - (*spacecraft.cPrime_B) += (*it)->effProps.mEff*(*it)->effProps.rEffPrime_CB_B; + (*spacecraft.cPrime_B) += (*it)->effProps.mEff * (*it)->effProps.rEffPrime_CB_B; // For high fidelity mass depletion, this is left out: += (*it)->effProps.mEffDot*(*it)->effProps.rEff_CB_B } // Divide c_B and cPrime_B by the total mass of the spaceCraft to finalize c_B and cPrime_B - (*spacecraft.c_B) = (*spacecraft.c_B)/(*spacecraft.m_SC)(0,0); - (*spacecraft.cPrime_B) = (*spacecraft.cPrime_B)/(*spacecraft.m_SC)(0,0) - - (*spacecraft.mDot_SC)(0,0)*(*spacecraft.c_B)/(*spacecraft.m_SC)(0,0)/(*spacecraft.m_SC)(0,0); + (*spacecraft.c_B) = (*spacecraft.c_B) / (*spacecraft.m_SC)(0, 0); + (*spacecraft.cPrime_B) = + (*spacecraft.cPrime_B) / (*spacecraft.m_SC)(0, 0) - + (*spacecraft.mDot_SC)(0, 0) * (*spacecraft.c_B) / (*spacecraft.m_SC)(0, 0) / (*spacecraft.m_SC)(0, 0); Eigen::Vector3d omegaLocal_BN_B = spacecraft.hubOmega_BN_B->getState(); Eigen::Vector3d cLocal_B = (*spacecraft.c_B); (*spacecraft.cDot_B) = (*spacecraft.cPrime_B) + omegaLocal_BN_B.cross(cLocal_B); @@ -528,7 +554,8 @@ void SpacecraftSystem::updateSpacecraftMassProps(double time, SpacecraftUnit& sp return; } -void SpacecraftSystem::updateSystemMassProps(double time) +void +SpacecraftSystem::updateSystemMassProps(double time) { // - Zero the properties which will get populated in this method (*this->primaryCentralSpacecraft.m_SC).setZero(); @@ -541,73 +568,79 @@ void SpacecraftSystem::updateSystemMassProps(double time) // Add in hubs mass props to the spacecraft mass props this->primaryCentralSpacecraft.hub.updateEffectorMassProps(time); - (*this->primaryCentralSpacecraft.m_SC)(0,0) += this->primaryCentralSpacecraft.hub.effProps.mEff; + (*this->primaryCentralSpacecraft.m_SC)(0, 0) += this->primaryCentralSpacecraft.hub.effProps.mEff; (*this->primaryCentralSpacecraft.ISCPntB_B) += this->primaryCentralSpacecraft.hub.effProps.IEffPntB_B; - (*this->primaryCentralSpacecraft.c_B) += this->primaryCentralSpacecraft.hub.effProps.mEff*this->primaryCentralSpacecraft.hub.effProps.rEff_CB_B; + (*this->primaryCentralSpacecraft.c_B) += + this->primaryCentralSpacecraft.hub.effProps.mEff * this->primaryCentralSpacecraft.hub.effProps.rEff_CB_B; // - Loop through state effectors to get mass props std::vector::iterator it; - for(it = this->primaryCentralSpacecraft.states.begin(); it != this->primaryCentralSpacecraft.states.end(); it++) - { + for (it = this->primaryCentralSpacecraft.states.begin(); it != this->primaryCentralSpacecraft.states.end(); it++) { (*it)->updateEffectorMassProps(time); // - Add in effectors mass props into mass props of this->primaryCentralSpacecraft - (*this->primaryCentralSpacecraft.m_SC)(0,0) += (*it)->effProps.mEff; - (*this->primaryCentralSpacecraft.mDot_SC)(0,0) += (*it)->effProps.mEffDot; + (*this->primaryCentralSpacecraft.m_SC)(0, 0) += (*it)->effProps.mEff; + (*this->primaryCentralSpacecraft.mDot_SC)(0, 0) += (*it)->effProps.mEffDot; (*this->primaryCentralSpacecraft.ISCPntB_B) += (*it)->effProps.IEffPntB_B; - (*this->primaryCentralSpacecraft.c_B) += (*it)->effProps.mEff*(*it)->effProps.rEff_CB_B; + (*this->primaryCentralSpacecraft.c_B) += (*it)->effProps.mEff * (*it)->effProps.rEff_CB_B; (*this->primaryCentralSpacecraft.ISCPntBPrime_B) += (*it)->effProps.IEffPrimePntB_B; - (*this->primaryCentralSpacecraft.cPrime_B) += (*it)->effProps.mEff*(*it)->effProps.rEffPrime_CB_B; + (*this->primaryCentralSpacecraft.cPrime_B) += (*it)->effProps.mEff * (*it)->effProps.rEffPrime_CB_B; // For high fidelity mass depletion, this is left out: += (*it)->effProps.mEffDot*(*it)->effProps.rEff_CB_B } // - Call this for all of the connected spacecraft std::vector::iterator spacecraftConnectedIt; - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { // Add in hubs mass props to the spacecraft mass props (*spacecraftConnectedIt)->hub.updateEffectorMassProps(time); - (*this->primaryCentralSpacecraft.m_SC)(0,0) += (*spacecraftConnectedIt)->hub.effProps.mEff; + (*this->primaryCentralSpacecraft.m_SC)(0, 0) += (*spacecraftConnectedIt)->hub.effProps.mEff; (*this->primaryCentralSpacecraft.ISCPntB_B) += (*spacecraftConnectedIt)->hub.effProps.IEffPntB_B; - (*this->primaryCentralSpacecraft.c_B) += (*spacecraftConnectedIt)->hub.effProps.mEff*(*spacecraftConnectedIt)->hub.effProps.rEff_CB_B; + (*this->primaryCentralSpacecraft.c_B) += + (*spacecraftConnectedIt)->hub.effProps.mEff * (*spacecraftConnectedIt)->hub.effProps.rEff_CB_B; // - Loop through state effectors to get mass props std::vector::iterator it; - for(it = (*spacecraftConnectedIt)->states.begin(); it != (*spacecraftConnectedIt)->states.end(); it++) - { + for (it = (*spacecraftConnectedIt)->states.begin(); it != (*spacecraftConnectedIt)->states.end(); it++) { (*it)->updateEffectorMassProps(time); // - Add in effectors mass props into mass props of this->primaryCentralSpacecraft - (*this->primaryCentralSpacecraft.m_SC)(0,0) += (*it)->effProps.mEff; - (*this->primaryCentralSpacecraft.mDot_SC)(0,0) += (*it)->effProps.mEffDot; + (*this->primaryCentralSpacecraft.m_SC)(0, 0) += (*it)->effProps.mEff; + (*this->primaryCentralSpacecraft.mDot_SC)(0, 0) += (*it)->effProps.mEffDot; (*this->primaryCentralSpacecraft.ISCPntB_B) += (*it)->effProps.IEffPntB_B; - (*this->primaryCentralSpacecraft.c_B) += (*it)->effProps.mEff*(*it)->effProps.rEff_CB_B; + (*this->primaryCentralSpacecraft.c_B) += (*it)->effProps.mEff * (*it)->effProps.rEff_CB_B; (*this->primaryCentralSpacecraft.ISCPntBPrime_B) += (*it)->effProps.IEffPrimePntB_B; - (*this->primaryCentralSpacecraft.cPrime_B) += (*it)->effProps.mEff*(*it)->effProps.rEffPrime_CB_B; + (*this->primaryCentralSpacecraft.cPrime_B) += (*it)->effProps.mEff * (*it)->effProps.rEffPrime_CB_B; // For high fidelity mass depletion, this is left out: += (*it)->effProps.mEffDot*(*it)->effProps.rEff_CB_B } } // Divide c_B and cPrime_B by the total mass of the spaceCraft to finalize c_B and cPrime_B - (*this->primaryCentralSpacecraft.c_B) = (*this->primaryCentralSpacecraft.c_B)/(*this->primaryCentralSpacecraft.m_SC)(0,0); - (*this->primaryCentralSpacecraft.cPrime_B) = (*this->primaryCentralSpacecraft.cPrime_B)/(*this->primaryCentralSpacecraft.m_SC)(0,0) - - (*this->primaryCentralSpacecraft.mDot_SC)(0,0)*(*this->primaryCentralSpacecraft.c_B)/(*this->primaryCentralSpacecraft.m_SC)(0,0)/(*this->primaryCentralSpacecraft.m_SC)(0,0); + (*this->primaryCentralSpacecraft.c_B) = + (*this->primaryCentralSpacecraft.c_B) / (*this->primaryCentralSpacecraft.m_SC)(0, 0); + (*this->primaryCentralSpacecraft.cPrime_B) = + (*this->primaryCentralSpacecraft.cPrime_B) / (*this->primaryCentralSpacecraft.m_SC)(0, 0) - + (*this->primaryCentralSpacecraft.mDot_SC)(0, 0) * (*this->primaryCentralSpacecraft.c_B) / + (*this->primaryCentralSpacecraft.m_SC)(0, 0) / (*this->primaryCentralSpacecraft.m_SC)(0, 0); Eigen::Vector3d omegaLocal_BN_B = this->primaryCentralSpacecraft.hubOmega_BN_B->getState(); Eigen::Vector3d cLocal_B = (*this->primaryCentralSpacecraft.c_B); - (*this->primaryCentralSpacecraft.cDot_B) = (*this->primaryCentralSpacecraft.cPrime_B) + omegaLocal_BN_B.cross(cLocal_B); + (*this->primaryCentralSpacecraft.cDot_B) = + (*this->primaryCentralSpacecraft.cPrime_B) + omegaLocal_BN_B.cross(cLocal_B); return; } -void SpacecraftSystem::initializeSCPosVelocity(SpacecraftUnit &spacecraft) +void +SpacecraftSystem::initializeSCPosVelocity(SpacecraftUnit& spacecraft) { Eigen::Vector3d rInit_BN_N = spacecraft.hubR_N->getState(); Eigen::MRPd sigma_BN; - sigma_BN = (Eigen::Vector3d) spacecraft.hubSigma->getState(); + sigma_BN = (Eigen::Vector3d)spacecraft.hubSigma->getState(); Eigen::Matrix3d dcm_NB = sigma_BN.toRotationMatrix(); // - Substract off the center mass to leave r_BN_N - rInit_BN_N -= dcm_NB*(*spacecraft.c_B); + rInit_BN_N -= dcm_NB * (*spacecraft.c_B); // - Subtract off cDot_B to get v_BN_N Eigen::Vector3d vInit_BN_N = spacecraft.hubV_N->getState(); - vInit_BN_N -= dcm_NB*(*spacecraft.cDot_B); + vInit_BN_N -= dcm_NB * (*spacecraft.cDot_B); // - Finally set the translational states r_BN_N and v_BN_N with the corrections spacecraft.hubR_N->setState(rInit_BN_N); spacecraft.hubV_N->setState(vInit_BN_N); @@ -619,7 +652,8 @@ void SpacecraftSystem::initializeSCPosVelocity(SpacecraftUnit &spacecraft) the stateEffectors. The hub also has gravity and dynamicEffectors acting on it and these relationships are controlled in this method. At the end of this method all of the states will have their corresponding state derivatives set in the dynParam Manager thus solving for Xdot*/ -void SpacecraftSystem::equationsOfMotion(double integTimeSeconds, double timeStep) +void +SpacecraftSystem::equationsOfMotion(double integTimeSeconds, double timeStep) { // - Update time to the current time uint64_t integTimeNanos = secToNano(integTimeSeconds); @@ -629,15 +663,17 @@ void SpacecraftSystem::equationsOfMotion(double integTimeSeconds, double timeSte // Call this for all unconnected spacecraft: // - Call this for all of the unconnected spacecraft std::vector::iterator spacecraftUnConnectedIt; - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { this->equationsOfMotionSC(integTimeSeconds, timeStep, (*(*spacecraftUnConnectedIt))); } return; } -void SpacecraftSystem::equationsOfMotionSC(double integTimeSeconds, double timeStep, SpacecraftUnit& spacecraft) +void +SpacecraftSystem::equationsOfMotionSC(double integTimeSeconds, double timeStep, SpacecraftUnit& spacecraft) { // - Zero all Matrices and vectors for back-sub and the dynamics spacecraft.hub.hubBackSubMatrices.matrixA.setZero(); @@ -658,18 +694,17 @@ void SpacecraftSystem::equationsOfMotionSC(double integTimeSeconds, double timeS Eigen::Matrix3d dcm_NB; Eigen::Vector3d cLocal_N; - sigmaBNLoc = (Eigen::Vector3d) spacecraft.hubSigma->getState(); + sigmaBNLoc = (Eigen::Vector3d)spacecraft.hubSigma->getState(); dcm_NB = sigmaBNLoc.toRotationMatrix(); - cLocal_N = dcm_NB*(*spacecraft.c_B); - Eigen::Vector3d rLocal_CN_N = spacecraft.hubR_N->getState() + dcm_NB*(*spacecraft.c_B); - Eigen::Vector3d vLocal_CN_N = spacecraft.hubV_N->getState() + dcm_NB*(*spacecraft.cDot_B); + cLocal_N = dcm_NB * (*spacecraft.c_B); + Eigen::Vector3d rLocal_CN_N = spacecraft.hubR_N->getState() + dcm_NB * (*spacecraft.c_B); + Eigen::Vector3d vLocal_CN_N = spacecraft.hubV_N->getState() + dcm_NB * (*spacecraft.cDot_B); spacecraft.gravField.computeGravityField(rLocal_CN_N, vLocal_CN_N); // - Loop through dynEffectors to compute force and torque on the s/c std::vector::iterator dynIt; - for(dynIt = spacecraft.dynEffectors.begin(); dynIt != spacecraft.dynEffectors.end(); dynIt++) - { + for (dynIt = spacecraft.dynEffectors.begin(); dynIt != spacecraft.dynEffectors.end(); dynIt++) { // - Compute the force and torque contributions from the dynamicEffectors (*dynIt)->computeForceTorque(integTimeSeconds, timeStep); spacecraft.sumForceExternal_N += (*dynIt)->forceExternal_N; @@ -679,8 +714,7 @@ void SpacecraftSystem::equationsOfMotionSC(double integTimeSeconds, double timeS // - Loop through state effectors to get contributions for back-substitution std::vector::iterator it; - for(it = spacecraft.states.begin(); it != spacecraft.states.end(); it++) - { + for (it = spacecraft.states.begin(); it != spacecraft.states.end(); it++) { /* - Set the contribution matrices to zero (just in case a stateEffector += on the matrix or the stateEffector doesn't have a contribution for a matrix and doesn't set the matrix to zero */ spacecraft.backSubMatricesContributions.matrixA.setZero(); @@ -691,7 +725,11 @@ void SpacecraftSystem::equationsOfMotionSC(double integTimeSeconds, double timeS spacecraft.backSubMatricesContributions.vecRot.setZero(); // - Call the update contributions method for the stateEffectors and add in contributions to the hub matrices - (*it)->updateContributions(integTimeSeconds, spacecraft.backSubMatricesContributions, spacecraft.hubSigma->getState(), spacecraft.hubOmega_BN_B->getState(), *spacecraft.g_N); + (*it)->updateContributions(integTimeSeconds, + spacecraft.backSubMatricesContributions, + spacecraft.hubSigma->getState(), + spacecraft.hubOmega_BN_B->getState(), + *spacecraft.g_N); spacecraft.hub.hubBackSubMatrices.matrixA += spacecraft.backSubMatricesContributions.matrixA; spacecraft.hub.hubBackSubMatrices.matrixB += spacecraft.backSubMatricesContributions.matrixB; spacecraft.hub.hubBackSubMatrices.matrixC += spacecraft.backSubMatricesContributions.matrixC; @@ -709,20 +747,22 @@ void SpacecraftSystem::equationsOfMotionSC(double integTimeSeconds, double timeS Eigen::Matrix3d intermediateMatrix; Eigen::Vector3d intermediateVector; Eigen::Vector3d omegaLocalBN_B = spacecraft.hubOmega_BN_B->getState(); - spacecraft.hub.hubBackSubMatrices.matrixA += (*spacecraft.m_SC)(0,0)*intermediateMatrix.Identity(); - intermediateMatrix = eigenTilde((*spacecraft.c_B)); // make c_B skew symmetric matrix - spacecraft.hub.hubBackSubMatrices.matrixB += -(*spacecraft.m_SC)(0,0)*intermediateMatrix; - spacecraft.hub.hubBackSubMatrices.matrixC += (*spacecraft.m_SC)(0,0)*intermediateMatrix; + spacecraft.hub.hubBackSubMatrices.matrixA += (*spacecraft.m_SC)(0, 0) * intermediateMatrix.Identity(); + intermediateMatrix = eigenTilde((*spacecraft.c_B)); // make c_B skew symmetric matrix + spacecraft.hub.hubBackSubMatrices.matrixB += -(*spacecraft.m_SC)(0, 0) * intermediateMatrix; + spacecraft.hub.hubBackSubMatrices.matrixC += (*spacecraft.m_SC)(0, 0) * intermediateMatrix; spacecraft.hub.hubBackSubMatrices.matrixD += *spacecraft.ISCPntB_B; - spacecraft.hub.hubBackSubMatrices.vecTrans += -2.0*(*spacecraft.m_SC)(0, 0)*omegaLocalBN_B.cross(cPrimeLocal_B) - - (*spacecraft.m_SC)(0, 0)*omegaLocalBN_B.cross(omegaLocalBN_B.cross(cLocal_B)) - - 2.0*(*spacecraft.mDot_SC)(0,0)*(cPrimeLocal_B+omegaLocalBN_B.cross(cLocal_B)); - intermediateVector = *spacecraft.ISCPntB_B*omegaLocalBN_B; - spacecraft.hub.hubBackSubMatrices.vecRot += -omegaLocalBN_B.cross(intermediateVector) - *spacecraft.ISCPntBPrime_B*omegaLocalBN_B; + spacecraft.hub.hubBackSubMatrices.vecTrans += + -2.0 * (*spacecraft.m_SC)(0, 0) * omegaLocalBN_B.cross(cPrimeLocal_B) - + (*spacecraft.m_SC)(0, 0) * omegaLocalBN_B.cross(omegaLocalBN_B.cross(cLocal_B)) - + 2.0 * (*spacecraft.mDot_SC)(0, 0) * (cPrimeLocal_B + omegaLocalBN_B.cross(cLocal_B)); + intermediateVector = *spacecraft.ISCPntB_B * omegaLocalBN_B; + spacecraft.hub.hubBackSubMatrices.vecRot += + -omegaLocalBN_B.cross(intermediateVector) - *spacecraft.ISCPntBPrime_B * omegaLocalBN_B; // - Map external force_N to the body frame Eigen::Vector3d sumForceExternalMappedToB; - sumForceExternalMappedToB = dcm_NB.transpose()*spacecraft.sumForceExternal_N; + sumForceExternalMappedToB = dcm_NB.transpose() * spacecraft.sumForceExternal_N; // - Edit both v_trans and v_rot with gravity and external force and torque Eigen::Vector3d gLocal_N = *spacecraft.g_N; @@ -731,26 +771,33 @@ void SpacecraftSystem::equationsOfMotionSC(double integTimeSeconds, double timeS // - Need to find force of gravity on the spacecraft Eigen::Vector3d gravityForce_N; - gravityForce_N = (*spacecraft.m_SC)(0,0)*gLocal_N; + gravityForce_N = (*spacecraft.m_SC)(0, 0) * gLocal_N; Eigen::Vector3d gravityForce_B; - gravityForce_B = dcm_NB.transpose()*gravityForce_N; - spacecraft.hub.hubBackSubMatrices.vecTrans += gravityForce_B + sumForceExternalMappedToB + spacecraft.sumForceExternal_B; + gravityForce_B = dcm_NB.transpose() * gravityForce_N; + spacecraft.hub.hubBackSubMatrices.vecTrans += + gravityForce_B + sumForceExternalMappedToB + spacecraft.sumForceExternal_B; spacecraft.hub.hubBackSubMatrices.vecRot += cLocal_B.cross(gravityForce_B) + spacecraft.sumTorquePntB_B; // - Compute the derivatives of the hub states before looping through stateEffectors - spacecraft.hub.computeDerivatives(integTimeSeconds, spacecraft.hubV_N->getStateDeriv(), spacecraft.hubOmega_BN_B->getStateDeriv(), spacecraft.hubSigma->getState()); + spacecraft.hub.computeDerivatives(integTimeSeconds, + spacecraft.hubV_N->getStateDeriv(), + spacecraft.hubOmega_BN_B->getStateDeriv(), + spacecraft.hubSigma->getState()); // - Loop through state effectors for compute derivatives - for(it = spacecraft.states.begin(); it != spacecraft.states.end(); it++) - { - (*it)->computeDerivatives(integTimeSeconds, spacecraft.hubV_N->getStateDeriv(), spacecraft.hubOmega_BN_B->getStateDeriv(), spacecraft.hubSigma->getState()); + for (it = spacecraft.states.begin(); it != spacecraft.states.end(); it++) { + (*it)->computeDerivatives(integTimeSeconds, + spacecraft.hubV_N->getStateDeriv(), + spacecraft.hubOmega_BN_B->getStateDeriv(), + spacecraft.hubSigma->getState()); } return; } -void SpacecraftSystem::equationsOfMotionSystem(double integTimeSeconds, double timeStep) +void +SpacecraftSystem::equationsOfMotionSystem(double integTimeSeconds, double timeStep) { // - Zero all Matrices and vectors for back-sub and the dynamics this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixA.setZero(); @@ -771,18 +818,21 @@ void SpacecraftSystem::equationsOfMotionSystem(double integTimeSeconds, double t Eigen::Matrix3d dcm_NB; Eigen::Vector3d cLocal_N; - sigmaBNLoc = (Eigen::Vector3d) this->primaryCentralSpacecraft.hubSigma->getState(); + sigmaBNLoc = (Eigen::Vector3d)this->primaryCentralSpacecraft.hubSigma->getState(); dcm_NB = sigmaBNLoc.toRotationMatrix(); - cLocal_N = dcm_NB*(*this->primaryCentralSpacecraft.c_B); - Eigen::Vector3d rLocal_CN_N = this->primaryCentralSpacecraft.hubR_N->getState() + dcm_NB*(*this->primaryCentralSpacecraft.c_B); - Eigen::Vector3d vLocal_CN_N = this->primaryCentralSpacecraft.hubV_N->getState() + dcm_NB*(*this->primaryCentralSpacecraft.cDot_B); + cLocal_N = dcm_NB * (*this->primaryCentralSpacecraft.c_B); + Eigen::Vector3d rLocal_CN_N = + this->primaryCentralSpacecraft.hubR_N->getState() + dcm_NB * (*this->primaryCentralSpacecraft.c_B); + Eigen::Vector3d vLocal_CN_N = + this->primaryCentralSpacecraft.hubV_N->getState() + dcm_NB * (*this->primaryCentralSpacecraft.cDot_B); this->primaryCentralSpacecraft.gravField.computeGravityField(rLocal_CN_N, vLocal_CN_N); // - Loop through dynEffectors to compute force and torque on the s/c std::vector::iterator dynIt; - for(dynIt = this->primaryCentralSpacecraft.dynEffectors.begin(); dynIt != this->primaryCentralSpacecraft.dynEffectors.end(); dynIt++) - { + for (dynIt = this->primaryCentralSpacecraft.dynEffectors.begin(); + dynIt != this->primaryCentralSpacecraft.dynEffectors.end(); + dynIt++) { // - Compute the force and torque contributions from the dynamicEffectors (*dynIt)->computeForceTorque(integTimeSeconds, timeStep); this->primaryCentralSpacecraft.sumForceExternal_N += (*dynIt)->forceExternal_N; @@ -792,11 +842,13 @@ void SpacecraftSystem::equationsOfMotionSystem(double integTimeSeconds, double t // - Call this for all of the connected spacecraft std::vector::iterator spacecraftConnectedIt; - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { // - Loop through dynEffectors to compute force and torque on the s/c - for(dynIt = (*spacecraftConnectedIt)->dynEffectors.begin(); dynIt != (*spacecraftConnectedIt)->dynEffectors.end(); dynIt++) - { + for (dynIt = (*spacecraftConnectedIt)->dynEffectors.begin(); + dynIt != (*spacecraftConnectedIt)->dynEffectors.end(); + dynIt++) { // - Compute the force and torque contributions from the dynamicEffectors (*dynIt)->computeForceTorque(integTimeSeconds, timeStep); this->primaryCentralSpacecraft.sumForceExternal_N += (*dynIt)->forceExternal_N; @@ -807,8 +859,7 @@ void SpacecraftSystem::equationsOfMotionSystem(double integTimeSeconds, double t // - Loop through state effectors to get contributions for back-substitution std::vector::iterator it; - for(it = this->primaryCentralSpacecraft.states.begin(); it != this->primaryCentralSpacecraft.states.end(); it++) - { + for (it = this->primaryCentralSpacecraft.states.begin(); it != this->primaryCentralSpacecraft.states.end(); it++) { /* - Set the contribution matrices to zero (just in case a stateEffector += on the matrix or the stateEffector doesn't have a contribution for a matrix and doesn't set the matrix to zero */ this->primaryCentralSpacecraft.backSubMatricesContributions.matrixA.setZero(); @@ -819,22 +870,32 @@ void SpacecraftSystem::equationsOfMotionSystem(double integTimeSeconds, double t this->primaryCentralSpacecraft.backSubMatricesContributions.vecRot.setZero(); // - Call the update contributions method for the stateEffectors and add in contributions to the hub matrices - (*it)->updateContributions(integTimeSeconds, this->primaryCentralSpacecraft.backSubMatricesContributions, this->primaryCentralSpacecraft.hubSigma->getState(), this->primaryCentralSpacecraft.hubOmega_BN_B->getState(), *this->primaryCentralSpacecraft.g_N); - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixA += this->primaryCentralSpacecraft.backSubMatricesContributions.matrixA; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixB += this->primaryCentralSpacecraft.backSubMatricesContributions.matrixB; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixC += this->primaryCentralSpacecraft.backSubMatricesContributions.matrixC; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixD += this->primaryCentralSpacecraft.backSubMatricesContributions.matrixD; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecTrans += this->primaryCentralSpacecraft.backSubMatricesContributions.vecTrans; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecRot += this->primaryCentralSpacecraft.backSubMatricesContributions.vecRot; + (*it)->updateContributions(integTimeSeconds, + this->primaryCentralSpacecraft.backSubMatricesContributions, + this->primaryCentralSpacecraft.hubSigma->getState(), + this->primaryCentralSpacecraft.hubOmega_BN_B->getState(), + *this->primaryCentralSpacecraft.g_N); + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixA += + this->primaryCentralSpacecraft.backSubMatricesContributions.matrixA; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixB += + this->primaryCentralSpacecraft.backSubMatricesContributions.matrixB; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixC += + this->primaryCentralSpacecraft.backSubMatricesContributions.matrixC; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixD += + this->primaryCentralSpacecraft.backSubMatricesContributions.matrixD; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecTrans += + this->primaryCentralSpacecraft.backSubMatricesContributions.vecTrans; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecRot += + this->primaryCentralSpacecraft.backSubMatricesContributions.vecRot; } // - Call this for all of the connected spacecraft - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { - for(it = (*spacecraftConnectedIt)->states.begin(); it != (*spacecraftConnectedIt)->states.end(); it++) - { - /* - Set the contribution matrices to zero (just in case a stateEffector += on the matrix or the stateEffector - doesn't have a contribution for a matrix and doesn't set the matrix to zero */ + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { + for (it = (*spacecraftConnectedIt)->states.begin(); it != (*spacecraftConnectedIt)->states.end(); it++) { + /* - Set the contribution matrices to zero (just in case a stateEffector += on the matrix or the + stateEffector doesn't have a contribution for a matrix and doesn't set the matrix to zero */ this->primaryCentralSpacecraft.backSubMatricesContributions.matrixA.setZero(); this->primaryCentralSpacecraft.backSubMatricesContributions.matrixB.setZero(); this->primaryCentralSpacecraft.backSubMatricesContributions.matrixC.setZero(); @@ -842,14 +903,25 @@ void SpacecraftSystem::equationsOfMotionSystem(double integTimeSeconds, double t this->primaryCentralSpacecraft.backSubMatricesContributions.vecTrans.setZero(); this->primaryCentralSpacecraft.backSubMatricesContributions.vecRot.setZero(); - // - Call the update contributions method for the stateEffectors and add in contributions to the hub matrices - (*it)->updateContributions(integTimeSeconds, this->primaryCentralSpacecraft.backSubMatricesContributions, this->primaryCentralSpacecraft.hubSigma->getState(), this->primaryCentralSpacecraft.hubOmega_BN_B->getState(), *this->primaryCentralSpacecraft.g_N); - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixA += this->primaryCentralSpacecraft.backSubMatricesContributions.matrixA; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixB += this->primaryCentralSpacecraft.backSubMatricesContributions.matrixB; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixC += this->primaryCentralSpacecraft.backSubMatricesContributions.matrixC; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixD += this->primaryCentralSpacecraft.backSubMatricesContributions.matrixD; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecTrans += this->primaryCentralSpacecraft.backSubMatricesContributions.vecTrans; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecRot += this->primaryCentralSpacecraft.backSubMatricesContributions.vecRot; + // - Call the update contributions method for the stateEffectors and add in contributions to the hub + // matrices + (*it)->updateContributions(integTimeSeconds, + this->primaryCentralSpacecraft.backSubMatricesContributions, + this->primaryCentralSpacecraft.hubSigma->getState(), + this->primaryCentralSpacecraft.hubOmega_BN_B->getState(), + *this->primaryCentralSpacecraft.g_N); + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixA += + this->primaryCentralSpacecraft.backSubMatricesContributions.matrixA; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixB += + this->primaryCentralSpacecraft.backSubMatricesContributions.matrixB; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixC += + this->primaryCentralSpacecraft.backSubMatricesContributions.matrixC; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixD += + this->primaryCentralSpacecraft.backSubMatricesContributions.matrixD; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecTrans += + this->primaryCentralSpacecraft.backSubMatricesContributions.vecTrans; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecRot += + this->primaryCentralSpacecraft.backSubMatricesContributions.vecRot; } } @@ -862,20 +934,25 @@ void SpacecraftSystem::equationsOfMotionSystem(double integTimeSeconds, double t Eigen::Matrix3d intermediateMatrix; Eigen::Vector3d intermediateVector; Eigen::Vector3d omegaLocalBN_B = this->primaryCentralSpacecraft.hubOmega_BN_B->getState(); - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixA += (*this->primaryCentralSpacecraft.m_SC)(0,0)*intermediateMatrix.Identity(); - intermediateMatrix = eigenTilde((*this->primaryCentralSpacecraft.c_B)); // make c_B skew symmetric matrix - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixB += -(*this->primaryCentralSpacecraft.m_SC)(0,0)*intermediateMatrix; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixC += (*this->primaryCentralSpacecraft.m_SC)(0,0)*intermediateMatrix; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixA += + (*this->primaryCentralSpacecraft.m_SC)(0, 0) * intermediateMatrix.Identity(); + intermediateMatrix = eigenTilde((*this->primaryCentralSpacecraft.c_B)); // make c_B skew symmetric matrix + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixB += + -(*this->primaryCentralSpacecraft.m_SC)(0, 0) * intermediateMatrix; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixC += + (*this->primaryCentralSpacecraft.m_SC)(0, 0) * intermediateMatrix; this->primaryCentralSpacecraft.hub.hubBackSubMatrices.matrixD += *this->primaryCentralSpacecraft.ISCPntB_B; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecTrans += -2.0*(*this->primaryCentralSpacecraft.m_SC)(0, 0)*omegaLocalBN_B.cross(cPrimeLocal_B) - - (*this->primaryCentralSpacecraft.m_SC)(0, 0)*omegaLocalBN_B.cross(omegaLocalBN_B.cross(cLocal_B)) - - 2.0*(*this->primaryCentralSpacecraft.mDot_SC)(0,0)*(cPrimeLocal_B+omegaLocalBN_B.cross(cLocal_B)); - intermediateVector = *this->primaryCentralSpacecraft.ISCPntB_B*omegaLocalBN_B; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecRot += -omegaLocalBN_B.cross(intermediateVector) - *this->primaryCentralSpacecraft.ISCPntBPrime_B*omegaLocalBN_B; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecTrans += + -2.0 * (*this->primaryCentralSpacecraft.m_SC)(0, 0) * omegaLocalBN_B.cross(cPrimeLocal_B) - + (*this->primaryCentralSpacecraft.m_SC)(0, 0) * omegaLocalBN_B.cross(omegaLocalBN_B.cross(cLocal_B)) - + 2.0 * (*this->primaryCentralSpacecraft.mDot_SC)(0, 0) * (cPrimeLocal_B + omegaLocalBN_B.cross(cLocal_B)); + intermediateVector = *this->primaryCentralSpacecraft.ISCPntB_B * omegaLocalBN_B; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecRot += + -omegaLocalBN_B.cross(intermediateVector) - *this->primaryCentralSpacecraft.ISCPntBPrime_B * omegaLocalBN_B; // - Map external force_N to the body frame Eigen::Vector3d sumForceExternalMappedToB; - sumForceExternalMappedToB = dcm_NB.transpose()*this->primaryCentralSpacecraft.sumForceExternal_N; + sumForceExternalMappedToB = dcm_NB.transpose() * this->primaryCentralSpacecraft.sumForceExternal_N; // - Edit both v_trans and v_rot with gravity and external force and torque Eigen::Vector3d gLocal_N = *this->primaryCentralSpacecraft.g_N; @@ -884,61 +961,74 @@ void SpacecraftSystem::equationsOfMotionSystem(double integTimeSeconds, double t // - Need to find force of gravity on the this->primaryCentralSpacecraft Eigen::Vector3d gravityForce_N; - gravityForce_N = (*this->primaryCentralSpacecraft.m_SC)(0,0)*gLocal_N; + gravityForce_N = (*this->primaryCentralSpacecraft.m_SC)(0, 0) * gLocal_N; Eigen::Vector3d gravityForce_B; - gravityForce_B = dcm_NB.transpose()*gravityForce_N; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecTrans += gravityForce_B + sumForceExternalMappedToB + this->primaryCentralSpacecraft.sumForceExternal_B; - this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecRot += cLocal_B.cross(gravityForce_B) + this->primaryCentralSpacecraft.sumTorquePntB_B; + gravityForce_B = dcm_NB.transpose() * gravityForce_N; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecTrans += + gravityForce_B + sumForceExternalMappedToB + this->primaryCentralSpacecraft.sumForceExternal_B; + this->primaryCentralSpacecraft.hub.hubBackSubMatrices.vecRot += + cLocal_B.cross(gravityForce_B) + this->primaryCentralSpacecraft.sumTorquePntB_B; // - Compute the derivatives of the hub states before looping through stateEffectors - this->primaryCentralSpacecraft.hub.computeDerivatives(integTimeSeconds, this->primaryCentralSpacecraft.hubV_N->getStateDeriv(), this->primaryCentralSpacecraft.hubOmega_BN_B->getStateDeriv(), this->primaryCentralSpacecraft.hubSigma->getState()); + this->primaryCentralSpacecraft.hub.computeDerivatives(integTimeSeconds, + this->primaryCentralSpacecraft.hubV_N->getStateDeriv(), + this->primaryCentralSpacecraft.hubOmega_BN_B->getStateDeriv(), + this->primaryCentralSpacecraft.hubSigma->getState()); - // - Need to figure out how to pass accelerations of rBDDot and omegaDot of the primary hub to the other hubs stateEffectors + // - Need to figure out how to pass accelerations of rBDDot and omegaDot of the primary hub to the other hubs + // stateEffectors // - Loop through state effectors for compute derivatives - for(it = this->primaryCentralSpacecraft.states.begin(); it != this->primaryCentralSpacecraft.states.end(); it++) - { - (*it)->computeDerivatives(integTimeSeconds, this->primaryCentralSpacecraft.hubV_N->getStateDeriv(), this->primaryCentralSpacecraft.hubOmega_BN_B->getStateDeriv(), this->primaryCentralSpacecraft.hubSigma->getState()); + for (it = this->primaryCentralSpacecraft.states.begin(); it != this->primaryCentralSpacecraft.states.end(); it++) { + (*it)->computeDerivatives(integTimeSeconds, + this->primaryCentralSpacecraft.hubV_N->getStateDeriv(), + this->primaryCentralSpacecraft.hubOmega_BN_B->getStateDeriv(), + this->primaryCentralSpacecraft.hubSigma->getState()); } // - Call this for all of the connected spacecraft - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { // - Loop through state effectors for compute derivatives - for(it = (*spacecraftConnectedIt)->states.begin(); it != (*spacecraftConnectedIt)->states.end(); it++) - { - (*it)->computeDerivatives(integTimeSeconds, this->primaryCentralSpacecraft.hubV_N->getStateDeriv(), this->primaryCentralSpacecraft.hubOmega_BN_B->getStateDeriv(), this->primaryCentralSpacecraft.hubSigma->getState()); + for (it = (*spacecraftConnectedIt)->states.begin(); it != (*spacecraftConnectedIt)->states.end(); it++) { + (*it)->computeDerivatives(integTimeSeconds, + this->primaryCentralSpacecraft.hubV_N->getStateDeriv(), + this->primaryCentralSpacecraft.hubOmega_BN_B->getStateDeriv(), + this->primaryCentralSpacecraft.hubSigma->getState()); } } return; } -void SpacecraftSystem::findPriorStateInformation(SpacecraftUnit &spacecraft) +void +SpacecraftSystem::findPriorStateInformation(SpacecraftUnit& spacecraft) { // - Find v_CN_N before integration for accumulated DV - spacecraft.oldV_BN_N = spacecraft.hubV_N->getState(); // - V_BN_N before integration - Eigen::Vector3d oldC_B; // - Center of mass offset before integration - Eigen::Vector3d oldOmega_BN_B; // - angular rate of B wrt N in the Body frame - Eigen::MRPd oldSigma_BN; // - Sigma_BN before integration + spacecraft.oldV_BN_N = spacecraft.hubV_N->getState(); // - V_BN_N before integration + Eigen::Vector3d oldC_B; // - Center of mass offset before integration + Eigen::Vector3d oldOmega_BN_B; // - angular rate of B wrt N in the Body frame + Eigen::MRPd oldSigma_BN; // - Sigma_BN before integration // - Get the angular rate, oldOmega_BN_B from the dyn manager oldOmega_BN_B = spacecraft.hubOmega_BN_B->getState(); // - Get center of mass, v_BN_N and dcm_NB from the dyn manager - oldSigma_BN = (Eigen::Vector3d) spacecraft.hubSigma->getState(); + oldSigma_BN = (Eigen::Vector3d)spacecraft.hubSigma->getState(); // - Finally find v_CN_N Eigen::Matrix3d oldDcm_NB = oldSigma_BN.toRotationMatrix(); // - dcm_NB before integration - spacecraft.oldV_CN_N = spacecraft.oldV_BN_N + oldDcm_NB*(*spacecraft.cDot_B); + spacecraft.oldV_CN_N = spacecraft.oldV_BN_N + oldDcm_NB * (*spacecraft.cDot_B); spacecraft.hub.matchGravitytoVelocityState(spacecraft.oldV_CN_N); return; } -void SpacecraftSystem::determineAttachedSCStates() +void +SpacecraftSystem::determineAttachedSCStates() { // Pull out primary spacecraft states Eigen::MRPd sigmaPNLoc; - sigmaPNLoc = (Eigen::Vector3d) this->primaryCentralSpacecraft.hubSigma->getState(); + sigmaPNLoc = (Eigen::Vector3d)this->primaryCentralSpacecraft.hubSigma->getState(); Eigen::Vector3d omegaPNLoc_P = this->primaryCentralSpacecraft.hubOmega_BN_B->getState(); Eigen::Vector3d r_PNLocal_N = this->primaryCentralSpacecraft.hubR_N->getState(); Eigen::Vector3d r_PNDotLocal_N = this->primaryCentralSpacecraft.hubV_N->getState(); @@ -948,9 +1038,10 @@ void SpacecraftSystem::determineAttachedSCStates() // Loop over connected spacecraft to edit the hub states // - Call this for all of the connected spacecraft std::vector::iterator spacecraftConnectedIt; - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { - Eigen::Matrix3d dcm_BN = (*spacecraftConnectedIt)->hub.dcm_BP*dcm_NP.transpose(); + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { + Eigen::Matrix3d dcm_BN = (*spacecraftConnectedIt)->hub.dcm_BP * dcm_NP.transpose(); double dcm_BNArray[9]; double sigma_BNArray[3]; eigenMatrix3d2CArray(dcm_BN, dcm_BNArray); @@ -961,48 +1052,49 @@ void SpacecraftSystem::determineAttachedSCStates() (*spacecraftConnectedIt)->hubSigma->setState(sigmaBNLoc); // Now omega - Eigen::Vector3d omegaBNLocal_B = (*spacecraftConnectedIt)->hub.dcm_BP*omegaPNLoc_P; + Eigen::Vector3d omegaBNLocal_B = (*spacecraftConnectedIt)->hub.dcm_BP * omegaPNLoc_P; (*spacecraftConnectedIt)->hubOmega_BN_B->setState(omegaBNLocal_B); // Now the translation states - Eigen::Vector3d r_BNLocal_N = r_PNLocal_N + dcm_NP*(*spacecraftConnectedIt)->hub.r_BP_P; + Eigen::Vector3d r_BNLocal_N = r_PNLocal_N + dcm_NP * (*spacecraftConnectedIt)->hub.r_BP_P; (*spacecraftConnectedIt)->hubR_N->setState(r_BNLocal_N); - Eigen::Vector3d r_BNDotLocal_N = r_PNDotLocal_N + dcm_NP*omegaPNLoc_P.cross((*spacecraftConnectedIt)->hub.r_BP_P); + Eigen::Vector3d r_BNDotLocal_N = + r_PNDotLocal_N + dcm_NP * omegaPNLoc_P.cross((*spacecraftConnectedIt)->hub.r_BP_P); (*spacecraftConnectedIt)->hubV_N->setState(r_BNDotLocal_N); } return; } -void SpacecraftSystem::calculateDeltaVandAcceleration(SpacecraftUnit &spacecraft, double localTimeStep) +void +SpacecraftSystem::calculateDeltaVandAcceleration(SpacecraftUnit& spacecraft, double localTimeStep) { // - Find v_CN_N after the integration for accumulated DV Eigen::Vector3d newV_BN_N = spacecraft.hubV_N->getState(); // - V_BN_N after integration - Eigen::Vector3d newV_CN_N; // - V_CN_N after integration - Eigen::MRPd newSigma_BN; // - Sigma_BN after integration + Eigen::Vector3d newV_CN_N; // - V_CN_N after integration + Eigen::MRPd newSigma_BN; // - Sigma_BN after integration // - Get center of mass, v_BN_N and dcm_NB Eigen::Vector3d sigmaBNLoc; - sigmaBNLoc = (Eigen::Vector3d) spacecraft.hubSigma->getState(); + sigmaBNLoc = (Eigen::Vector3d)spacecraft.hubSigma->getState(); newSigma_BN = sigmaBNLoc; - Eigen::Matrix3d newDcm_NB = newSigma_BN.toRotationMatrix(); // - dcm_NB after integration - newV_CN_N = newV_BN_N + newDcm_NB*(*spacecraft.cDot_B); + Eigen::Matrix3d newDcm_NB = newSigma_BN.toRotationMatrix(); // - dcm_NB after integration + newV_CN_N = newV_BN_N + newDcm_NB * (*spacecraft.cDot_B); // - Find accumulated DV of the center of mass in the body frame - spacecraft.dvAccum_CN_B += newDcm_NB.transpose()*(newV_CN_N - - spacecraft.BcGravVelocity->getState()); + spacecraft.dvAccum_CN_B += newDcm_NB.transpose() * (newV_CN_N - spacecraft.BcGravVelocity->getState()); // - Find the accumulated DV of the body frame in the body frame - spacecraft.dvAccum_BN_B += newDcm_NB.transpose()*(newV_BN_N - - spacecraft.hubGravVelocity->getState()); + spacecraft.dvAccum_BN_B += newDcm_NB.transpose() * (newV_BN_N - spacecraft.hubGravVelocity->getState()); // - non-conservative acceleration of the body frame in the body frame - spacecraft.nonConservativeAccelpntB_B = (newDcm_NB.transpose()*(newV_BN_N - - spacecraft.hubGravVelocity->getState()))/localTimeStep; + spacecraft.nonConservativeAccelpntB_B = + (newDcm_NB.transpose() * (newV_BN_N - spacecraft.hubGravVelocity->getState())) / localTimeStep; // - angular acceleration in the body frame Eigen::Vector3d newOmega_BN_B; newOmega_BN_B = spacecraft.hubOmega_BN_B->getState(); - spacecraft.omegaDot_BN_B = (newOmega_BN_B - spacecraft.oldOmega_BN_B)/localTimeStep; //angular acceleration of B wrt N in the Body frame + spacecraft.omegaDot_BN_B = + (newOmega_BN_B - spacecraft.oldOmega_BN_B) / localTimeStep; // angular acceleration of B wrt N in the Body frame return; } @@ -1010,27 +1102,30 @@ void SpacecraftSystem::calculateDeltaVandAcceleration(SpacecraftUnit &spacecraft /*! This method is used to find the total energy and momentum of the spacecraft. It finds the total orbital energy, total orbital angular momentum, total rotational energy and total rotational angular momentum. These values are used for validation purposes. */ -void SpacecraftSystem::computeEnergyMomentum(double time) +void +SpacecraftSystem::computeEnergyMomentum(double time) { this->computeEnergyMomentumSystem(time); // - Call this for all of the unconnected spacecraft std::vector::iterator spacecraftUnConnectedIt; - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { this->computeEnergyMomentumSC(time, (*(*spacecraftUnConnectedIt))); } return; } -void SpacecraftSystem::computeEnergyMomentumSC(double time, SpacecraftUnit &spacecraft) +void +SpacecraftSystem::computeEnergyMomentumSC(double time, SpacecraftUnit& spacecraft) { // - Grab values from state Manager Eigen::Vector3d rLocal_BN_N = spacecraft.hubR_N->getState(); Eigen::Vector3d rDotLocal_BN_N = spacecraft.hubV_N->getState(); Eigen::MRPd sigmaLocal_BN; - sigmaLocal_BN = (Eigen::Vector3d ) spacecraft.hubSigma->getState(); + sigmaLocal_BN = (Eigen::Vector3d)spacecraft.hubSigma->getState(); // - Find DCM's Eigen::Matrix3d dcmLocal_NB = sigmaLocal_BN.toRotationMatrix(); @@ -1039,8 +1134,8 @@ void SpacecraftSystem::computeEnergyMomentumSC(double time, SpacecraftUnit &spac // - Convert from inertial frame to body frame Eigen::Vector3d rBNLocal_B; Eigen::Vector3d rDotBNLocal_B; - rBNLocal_B = dcmLocal_BN*rLocal_BN_N; - rDotBNLocal_B = dcmLocal_BN*rDotLocal_BN_N; + rBNLocal_B = dcmLocal_BN * rLocal_BN_N; + rDotBNLocal_B = dcmLocal_BN * rDotLocal_BN_N; // - zero necessarry variables Eigen::Vector3d totOrbAngMomPntN_B; @@ -1055,20 +1150,21 @@ void SpacecraftSystem::computeEnergyMomentumSC(double time, SpacecraftUnit &spac spacecraft.rotEnergyContr = 0.0; // - Get the hubs contribution - spacecraft.hub.updateEnergyMomContributions(time, spacecraft.rotAngMomPntCContr_B, spacecraft.rotEnergyContr, spacecraft.hubOmega_BN_B->getState()); + spacecraft.hub.updateEnergyMomContributions( + time, spacecraft.rotAngMomPntCContr_B, spacecraft.rotEnergyContr, spacecraft.hubOmega_BN_B->getState()); totRotAngMomPntC_B += spacecraft.rotAngMomPntCContr_B; spacecraft.totRotEnergy += spacecraft.rotEnergyContr; // - Loop over stateEffectors to get their contributions to energy and momentum std::vector::iterator it; - for(it = spacecraft.states.begin(); it != spacecraft.states.end(); it++) - { + for (it = spacecraft.states.begin(); it != spacecraft.states.end(); it++) { // - Set the matrices to zero spacecraft.rotAngMomPntCContr_B.setZero(); spacecraft.rotEnergyContr = 0.0; // - Call energy and momentum calulations for stateEffectors - (*it)->updateEnergyMomContributions(time, spacecraft.rotAngMomPntCContr_B, spacecraft.rotEnergyContr, spacecraft.hubOmega_BN_B->getState()); + (*it)->updateEnergyMomContributions( + time, spacecraft.rotAngMomPntCContr_B, spacecraft.rotEnergyContr, spacecraft.hubOmega_BN_B->getState()); totRotAngMomPntC_B += spacecraft.rotAngMomPntCContr_B; spacecraft.totRotEnergy += spacecraft.rotEnergyContr; } @@ -1077,39 +1173,41 @@ void SpacecraftSystem::computeEnergyMomentumSC(double time, SpacecraftUnit &spac Eigen::Vector3d cDotLocal_B = (*spacecraft.cDot_B); // - Add in orbital kinetic energy into the total orbital energy calculations - spacecraft.totOrbEnergy += 1.0/2.0*(*spacecraft.m_SC)(0,0)*(rDotBNLocal_B.dot(rDotBNLocal_B) + 2.0*rDotBNLocal_B.dot(cDotLocal_B) - + cDotLocal_B.dot(cDotLocal_B)); + spacecraft.totOrbEnergy += + 1.0 / 2.0 * (*spacecraft.m_SC)(0, 0) * + (rDotBNLocal_B.dot(rDotBNLocal_B) + 2.0 * rDotBNLocal_B.dot(cDotLocal_B) + cDotLocal_B.dot(cDotLocal_B)); // - Call gravity effector and add in its potential contributions to the total orbital energy calculations spacecraft.orbPotentialEnergyContr = 0.0; - Eigen::Vector3d rLocal_CN_N = spacecraft.hubR_N->getState() + dcmLocal_NB*(*spacecraft.c_B); + Eigen::Vector3d rLocal_CN_N = spacecraft.hubR_N->getState() + dcmLocal_NB * (*spacecraft.c_B); spacecraft.gravField.updateEnergyContributions(rLocal_CN_N, spacecraft.orbPotentialEnergyContr); - spacecraft.totOrbEnergy += (*spacecraft.m_SC)(0,0)*spacecraft.orbPotentialEnergyContr; + spacecraft.totOrbEnergy += (*spacecraft.m_SC)(0, 0) * spacecraft.orbPotentialEnergyContr; // - Find total rotational energy - spacecraft.totRotEnergy += -1.0/2.0*(*spacecraft.m_SC)(0,0)*cDotLocal_B.dot(cDotLocal_B); + spacecraft.totRotEnergy += -1.0 / 2.0 * (*spacecraft.m_SC)(0, 0) * cDotLocal_B.dot(cDotLocal_B); // - Find orbital angular momentum for the spacecraft Eigen::Vector3d rCN_N; Eigen::Vector3d rDotCN_N; - rCN_N = rLocal_BN_N + dcmLocal_NB*(*spacecraft.c_B); - rDotCN_N = rDotLocal_BN_N + dcmLocal_NB*cDotLocal_B; - spacecraft.totOrbAngMomPntN_N = (*spacecraft.m_SC)(0,0)*(rCN_N.cross(rDotCN_N)); + rCN_N = rLocal_BN_N + dcmLocal_NB * (*spacecraft.c_B); + rDotCN_N = rDotLocal_BN_N + dcmLocal_NB * cDotLocal_B; + spacecraft.totOrbAngMomPntN_N = (*spacecraft.m_SC)(0, 0) * (rCN_N.cross(rDotCN_N)); // - Find rotational angular momentum for the spacecraft - totRotAngMomPntC_B += -(*spacecraft.m_SC)(0,0)*(Eigen::Vector3d (*spacecraft.c_B)).cross(cDotLocal_B); - spacecraft.totRotAngMomPntC_N = dcmLocal_NB*totRotAngMomPntC_B; + totRotAngMomPntC_B += -(*spacecraft.m_SC)(0, 0) * (Eigen::Vector3d(*spacecraft.c_B)).cross(cDotLocal_B); + spacecraft.totRotAngMomPntC_N = dcmLocal_NB * totRotAngMomPntC_B; return; } -void SpacecraftSystem::computeEnergyMomentumSystem(double time) +void +SpacecraftSystem::computeEnergyMomentumSystem(double time) { // - Grab values from state Manager Eigen::Vector3d rLocal_BN_N = this->primaryCentralSpacecraft.hubR_N->getState(); Eigen::Vector3d rDotLocal_BN_N = this->primaryCentralSpacecraft.hubV_N->getState(); Eigen::MRPd sigmaLocal_BN; - sigmaLocal_BN = (Eigen::Vector3d ) this->primaryCentralSpacecraft.hubSigma->getState(); + sigmaLocal_BN = (Eigen::Vector3d)this->primaryCentralSpacecraft.hubSigma->getState(); // - Find DCM's Eigen::Matrix3d dcmLocal_NB = sigmaLocal_BN.toRotationMatrix(); @@ -1118,8 +1216,8 @@ void SpacecraftSystem::computeEnergyMomentumSystem(double time) // - Convert from inertial frame to body frame Eigen::Vector3d rBNLocal_B; Eigen::Vector3d rDotBNLocal_B; - rBNLocal_B = dcmLocal_BN*rLocal_BN_N; - rDotBNLocal_B = dcmLocal_BN*rDotLocal_BN_N; + rBNLocal_B = dcmLocal_BN * rLocal_BN_N; + rDotBNLocal_B = dcmLocal_BN * rDotLocal_BN_N; // - zero necessarry variables Eigen::Vector3d totOrbAngMomPntN_B; @@ -1134,43 +1232,56 @@ void SpacecraftSystem::computeEnergyMomentumSystem(double time) this->primaryCentralSpacecraft.rotEnergyContr = 0.0; // - Get the hubs contribution - this->primaryCentralSpacecraft.hub.updateEnergyMomContributions(time, this->primaryCentralSpacecraft.rotAngMomPntCContr_B, this->primaryCentralSpacecraft.rotEnergyContr, this->primaryCentralSpacecraft.hubOmega_BN_B->getState()); + this->primaryCentralSpacecraft.hub.updateEnergyMomContributions( + time, + this->primaryCentralSpacecraft.rotAngMomPntCContr_B, + this->primaryCentralSpacecraft.rotEnergyContr, + this->primaryCentralSpacecraft.hubOmega_BN_B->getState()); totRotAngMomPntC_B += this->primaryCentralSpacecraft.rotAngMomPntCContr_B; this->primaryCentralSpacecraft.totRotEnergy += this->primaryCentralSpacecraft.rotEnergyContr; // - Loop over stateEffectors to get their contributions to energy and momentum std::vector::iterator it; - for(it = this->primaryCentralSpacecraft.states.begin(); it != this->primaryCentralSpacecraft.states.end(); it++) - { + for (it = this->primaryCentralSpacecraft.states.begin(); it != this->primaryCentralSpacecraft.states.end(); it++) { // - Set the matrices to zero this->primaryCentralSpacecraft.rotAngMomPntCContr_B.setZero(); this->primaryCentralSpacecraft.rotEnergyContr = 0.0; // - Call energy and momentum calulations for stateEffectors - (*it)->updateEnergyMomContributions(time, this->primaryCentralSpacecraft.rotAngMomPntCContr_B, this->primaryCentralSpacecraft.rotEnergyContr, this->primaryCentralSpacecraft.hubOmega_BN_B->getState()); + (*it)->updateEnergyMomContributions(time, + this->primaryCentralSpacecraft.rotAngMomPntCContr_B, + this->primaryCentralSpacecraft.rotEnergyContr, + this->primaryCentralSpacecraft.hubOmega_BN_B->getState()); totRotAngMomPntC_B += this->primaryCentralSpacecraft.rotAngMomPntCContr_B; this->primaryCentralSpacecraft.totRotEnergy += this->primaryCentralSpacecraft.rotEnergyContr; } // - Get all of the attached hubs and stateEffectors contributions std::vector::iterator spacecraftConnectedIt; - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { // - Get the hubs contribution - (*spacecraftConnectedIt)->hub.updateEnergyMomContributions(time, this->primaryCentralSpacecraft.rotAngMomPntCContr_B, this->primaryCentralSpacecraft.rotEnergyContr, this->primaryCentralSpacecraft.hubOmega_BN_B->getState()); + (*spacecraftConnectedIt) + ->hub.updateEnergyMomContributions(time, + this->primaryCentralSpacecraft.rotAngMomPntCContr_B, + this->primaryCentralSpacecraft.rotEnergyContr, + this->primaryCentralSpacecraft.hubOmega_BN_B->getState()); totRotAngMomPntC_B += this->primaryCentralSpacecraft.rotAngMomPntCContr_B; this->primaryCentralSpacecraft.totRotEnergy += this->primaryCentralSpacecraft.rotEnergyContr; // - Loop over stateEffectors to get their contributions to energy and momentum std::vector::iterator it; - for(it = (*spacecraftConnectedIt)->states.begin(); it != (*spacecraftConnectedIt)->states.end(); it++) - { + for (it = (*spacecraftConnectedIt)->states.begin(); it != (*spacecraftConnectedIt)->states.end(); it++) { // - Set the matrices to zero this->primaryCentralSpacecraft.rotAngMomPntCContr_B.setZero(); this->primaryCentralSpacecraft.rotEnergyContr = 0.0; // - Call energy and momentum calulations for stateEffectors - (*it)->updateEnergyMomContributions(time, this->primaryCentralSpacecraft.rotAngMomPntCContr_B, this->primaryCentralSpacecraft.rotEnergyContr, this->primaryCentralSpacecraft.hubOmega_BN_B->getState()); + (*it)->updateEnergyMomContributions(time, + this->primaryCentralSpacecraft.rotAngMomPntCContr_B, + this->primaryCentralSpacecraft.rotEnergyContr, + this->primaryCentralSpacecraft.hubOmega_BN_B->getState()); totRotAngMomPntC_B += this->primaryCentralSpacecraft.rotAngMomPntCContr_B; this->primaryCentralSpacecraft.totRotEnergy += this->primaryCentralSpacecraft.rotEnergyContr; } @@ -1180,28 +1291,35 @@ void SpacecraftSystem::computeEnergyMomentumSystem(double time) Eigen::Vector3d cDotLocal_B = (*this->primaryCentralSpacecraft.cDot_B); // - Add in orbital kinetic energy into the total orbital energy calculations - this->primaryCentralSpacecraft.totOrbEnergy += 1.0/2.0*(*this->primaryCentralSpacecraft.m_SC)(0,0)*(rDotBNLocal_B.dot(rDotBNLocal_B) + 2.0*rDotBNLocal_B.dot(cDotLocal_B) - + cDotLocal_B.dot(cDotLocal_B)); + this->primaryCentralSpacecraft.totOrbEnergy += + 1.0 / 2.0 * (*this->primaryCentralSpacecraft.m_SC)(0, 0) * + (rDotBNLocal_B.dot(rDotBNLocal_B) + 2.0 * rDotBNLocal_B.dot(cDotLocal_B) + cDotLocal_B.dot(cDotLocal_B)); // - Call gravity effector and add in its potential contributions to the total orbital energy calculations this->primaryCentralSpacecraft.orbPotentialEnergyContr = 0.0; - Eigen::Vector3d rLocal_CN_N = this->primaryCentralSpacecraft.hubR_N->getState() + dcmLocal_NB*(*this->primaryCentralSpacecraft.c_B); - this->primaryCentralSpacecraft.gravField.updateEnergyContributions(rLocal_CN_N, this->primaryCentralSpacecraft.orbPotentialEnergyContr); - this->primaryCentralSpacecraft.totOrbEnergy += (*this->primaryCentralSpacecraft.m_SC)(0,0)*this->primaryCentralSpacecraft.orbPotentialEnergyContr; + Eigen::Vector3d rLocal_CN_N = + this->primaryCentralSpacecraft.hubR_N->getState() + dcmLocal_NB * (*this->primaryCentralSpacecraft.c_B); + this->primaryCentralSpacecraft.gravField.updateEnergyContributions( + rLocal_CN_N, this->primaryCentralSpacecraft.orbPotentialEnergyContr); + this->primaryCentralSpacecraft.totOrbEnergy += + (*this->primaryCentralSpacecraft.m_SC)(0, 0) * this->primaryCentralSpacecraft.orbPotentialEnergyContr; // - Find total rotational energy - this->primaryCentralSpacecraft.totRotEnergy += -1.0/2.0*(*this->primaryCentralSpacecraft.m_SC)(0,0)*cDotLocal_B.dot(cDotLocal_B); + this->primaryCentralSpacecraft.totRotEnergy += + -1.0 / 2.0 * (*this->primaryCentralSpacecraft.m_SC)(0, 0) * cDotLocal_B.dot(cDotLocal_B); // - Find orbital angular momentum for the spacecraft Eigen::Vector3d rCN_N; Eigen::Vector3d rDotCN_N; - rCN_N = rLocal_BN_N + dcmLocal_NB*(*this->primaryCentralSpacecraft.c_B); - rDotCN_N = rDotLocal_BN_N + dcmLocal_NB*cDotLocal_B; - this->primaryCentralSpacecraft.totOrbAngMomPntN_N = (*this->primaryCentralSpacecraft.m_SC)(0,0)*(rCN_N.cross(rDotCN_N)); + rCN_N = rLocal_BN_N + dcmLocal_NB * (*this->primaryCentralSpacecraft.c_B); + rDotCN_N = rDotLocal_BN_N + dcmLocal_NB * cDotLocal_B; + this->primaryCentralSpacecraft.totOrbAngMomPntN_N = + (*this->primaryCentralSpacecraft.m_SC)(0, 0) * (rCN_N.cross(rDotCN_N)); // - Find rotational angular momentum for the spacecraft - totRotAngMomPntC_B += -(*this->primaryCentralSpacecraft.m_SC)(0,0)*(Eigen::Vector3d (*this->primaryCentralSpacecraft.c_B)).cross(cDotLocal_B); - this->primaryCentralSpacecraft.totRotAngMomPntC_N = dcmLocal_NB*totRotAngMomPntC_B; + totRotAngMomPntC_B += -(*this->primaryCentralSpacecraft.m_SC)(0, 0) * + (Eigen::Vector3d(*this->primaryCentralSpacecraft.c_B)).cross(cDotLocal_B); + this->primaryCentralSpacecraft.totRotAngMomPntC_N = dcmLocal_NB * totRotAngMomPntC_B; return; } @@ -1209,7 +1327,9 @@ void SpacecraftSystem::computeEnergyMomentumSystem(double time) /*! Prepare for integration process, not currently implemented in SpacecraftSystem @param integrateToThisTimeNanos Time to integrate to */ -void SpacecraftSystem::preIntegration(uint64_t integrateToThisTimeNanos) { +void +SpacecraftSystem::preIntegration(uint64_t integrateToThisTimeNanos) +{ // - Find the time step this->timeStep = diffNanoToSec(integrateToThisTimeNanos, this->timeBeforeNanos); @@ -1218,21 +1338,25 @@ void SpacecraftSystem::preIntegration(uint64_t integrateToThisTimeNanos) { // - Call this for all of the unconnected spacecraft std::vector::iterator spacecraftUnConnectedIt; - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { this->findPriorStateInformation((*(*spacecraftUnConnectedIt))); } - } /*! Perform post-integration steps, not currently implemented in SpacecraftSystem @param integrateToThisTimeNanos Time to integrate to */ -void SpacecraftSystem::postIntegration(uint64_t integrateToThisTimeNanos) { +void +SpacecraftSystem::postIntegration(uint64_t integrateToThisTimeNanos) +{ std::vector::iterator spacecraftUnConnectedIt; - this->timeBeforeNanos = integrateToThisTimeNanos; // - copy the current time into previous time for next integrate state call - this->timeBefore = integrateToThisTimeNanos*NANO2SEC; // - copy the current time into previous time for next integrate state call - double integrateToThisTime = integrateToThisTimeNanos*NANO2SEC; // - convert to seconds + this->timeBeforeNanos = + integrateToThisTimeNanos; // - copy the current time into previous time for next integrate state call + this->timeBefore = + integrateToThisTimeNanos * NANO2SEC; // - copy the current time into previous time for next integrate state call + double integrateToThisTime = integrateToThisTimeNanos * NANO2SEC; // - convert to seconds // - Calculate the states of the attached spacecraft from the primary spacecraft this->determineAttachedSCStates(); @@ -1242,40 +1366,41 @@ void SpacecraftSystem::postIntegration(uint64_t integrateToThisTimeNanos) { // - Just in case the MRPs of the attached hubs need to be switched std::vector::iterator spacecraftConnectedIt; - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { (*spacecraftConnectedIt)->hub.modifyStates(integrateToThisTime); } // - Just in case the MRPs of the unattached hubs need to be switched - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { (*spacecraftUnConnectedIt)->hub.modifyStates(integrateToThisTime); } // - Loop over stateEffectors to call modifyStates std::vector::iterator it; - for(it = this->primaryCentralSpacecraft.states.begin(); it != this->primaryCentralSpacecraft.states.end(); it++) - { + for (it = this->primaryCentralSpacecraft.states.begin(); it != this->primaryCentralSpacecraft.states.end(); it++) { // - Call energy and momentum calulations for stateEffectors (*it)->modifyStates(integrateToThisTime); } // - Call this for all of the connected spacecraft - for(spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); spacecraftConnectedIt++) - { - for(it = (*spacecraftConnectedIt)->states.begin(); it != (*spacecraftConnectedIt)->states.end(); it++) - { + for (spacecraftConnectedIt = this->spacecraftDockedToPrimary.begin(); + spacecraftConnectedIt != this->spacecraftDockedToPrimary.end(); + spacecraftConnectedIt++) { + for (it = (*spacecraftConnectedIt)->states.begin(); it != (*spacecraftConnectedIt)->states.end(); it++) { // - Call energy and momentum calulations for stateEffectors (*it)->modifyStates(integrateToThisTime); } } // - Call this for all of the unconnected spacecraft - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { - for(it = (*spacecraftUnConnectedIt)->states.begin(); it != (*spacecraftUnConnectedIt)->states.end(); it++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { + for (it = (*spacecraftUnConnectedIt)->states.begin(); it != (*spacecraftUnConnectedIt)->states.end(); it++) { // - Call energy and momentum calulations for stateEffectors (*it)->modifyStates(integrateToThisTime); } @@ -1285,20 +1410,21 @@ void SpacecraftSystem::postIntegration(uint64_t integrateToThisTimeNanos) { this->updateSystemMassProps(integrateToThisTime); // - Call mass props for all the rest of the spacecraft - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { this->updateSpacecraftMassProps(integrateToThisTime, (*(*spacecraftUnConnectedIt))); } this->calculateDeltaVandAcceleration(this->primaryCentralSpacecraft, this->timeStep); // - Call for the rest of the spacecraft - for(spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); spacecraftUnConnectedIt++) - { + for (spacecraftUnConnectedIt = this->unDockedSpacecraft.begin(); + spacecraftUnConnectedIt != this->unDockedSpacecraft.end(); + spacecraftUnConnectedIt++) { this->calculateDeltaVandAcceleration((*(*spacecraftUnConnectedIt)), this->timeStep); } // - Compute Energy and Momentum this->computeEnergyMomentum(integrateToThisTime); - } diff --git a/src/simulation/dynamics/spacecraftSystem/spacecraftSystem.h b/src/simulation/dynamics/spacecraftSystem/spacecraftSystem.h old mode 100755 new mode 100644 index 944ecfb020..3e77626feb --- a/src/simulation/dynamics/spacecraftSystem/spacecraftSystem.h +++ b/src/simulation/dynamics/spacecraftSystem/spacecraftSystem.h @@ -38,14 +38,14 @@ #include "architecture/utilities/bskLogging.h" - /*! @brief docking data structure */ -struct DockingData { - Eigen::Vector3d r_DB_B; //!< variable - Eigen::Matrix3d dcm_DB; //!< variable - Eigen::Vector3d r_DP_P; //!< variable - Eigen::Matrix3d dcm_DP; //!< variable - std::string portName; //!< variable +struct DockingData +{ + Eigen::Vector3d r_DB_B; //!< variable + Eigen::Matrix3d dcm_DB; //!< variable + Eigen::Vector3d r_DP_P; //!< variable + Eigen::Matrix3d dcm_DP; //!< variable + std::string portName; //!< variable DockingData() { this->r_DB_B.setZero(); @@ -57,72 +57,74 @@ struct DockingData { }; /*! @brief spacecraft dynamic effector */ -class SpacecraftUnit { -public: +class SpacecraftUnit +{ + public: friend class SpacecraftSystem; - bool docked; //!< class variable - std::string spacecraftName; //!< Name of the spacecraft so that multiple spacecraft can be distinguished - Message scStateOutMsg; //!< Name of the state output message - Message scMassStateOutMsg; //!< Name of the state output message - Message scEnergyMomentumOutMsg; //!< Name of the state output message + bool docked; //!< class variable + std::string spacecraftName; //!< Name of the spacecraft so that multiple spacecraft can be distinguished + Message scStateOutMsg; //!< Name of the state output message + Message scMassStateOutMsg; //!< Name of the state output message + Message scEnergyMomentumOutMsg; //!< Name of the state output message - double totOrbEnergy; //!< [J] Total orbital kinetic energy - double totRotEnergy; //!< [J] Total rotational energy + double totOrbEnergy; //!< [J] Total orbital kinetic energy + double totRotEnergy; //!< [J] Total rotational energy - double rotEnergyContr; //!< [J] Contribution of stateEffector to total rotational energy - double orbPotentialEnergyContr; //!< [J] Contribution of stateEffector to total rotational energy - Eigen::Vector3d totOrbAngMomPntN_N; //!< [kg m^2/s] Total orbital angular momentum about N in N frame compenents - Eigen::Vector3d totRotAngMomPntC_N; //!< [kg m^2/s] Total rotational angular momentum about C in N frame compenents - Eigen::Vector3d rotAngMomPntCContr_B; //!< [kg m^2/s] Contribution of stateEffector to total rotational angular mom. + double rotEnergyContr; //!< [J] Contribution of stateEffector to total rotational energy + double orbPotentialEnergyContr; //!< [J] Contribution of stateEffector to total rotational energy + Eigen::Vector3d totOrbAngMomPntN_N; //!< [kg m^2/s] Total orbital angular momentum about N in N frame compenents + Eigen::Vector3d totRotAngMomPntC_N; //!< [kg m^2/s] Total rotational angular momentum about C in N frame compenents + Eigen::Vector3d rotAngMomPntCContr_B; //!< [kg m^2/s] Contribution of stateEffector to total rotational angular mom. BackSubMatrices backSubMatricesContributions; //!< class variable - Eigen::Vector3d sumForceExternal_N; //!< [N] Sum of forces given in the inertial frame - Eigen::Vector3d sumForceExternal_B; //!< [N] Sum of forces given in the body frame - Eigen::Vector3d sumTorquePntB_B; //!< [N-m] Total torque about point B in B frame components - - Eigen::Vector3d oldV_CN_N; //!< class variable - Eigen::Vector3d oldV_BN_N; //!< class variable - Eigen::Vector3d oldOmega_BN_B; //!< class variable - - Eigen::Vector3d dvAccum_CN_B; //!< [m/s] Accumulated delta-v of center of mass relative to inertial frame in body frame coordinates - Eigen::Vector3d dvAccum_BN_B; //!< [m/s] accumulated delta-v of body frame relative to inertial frame in body frame coordinates - Eigen::Vector3d nonConservativeAccelpntB_B;//!< [m/s/s] Current spacecraft body acceleration in the B frame - Eigen::Vector3d omegaDot_BN_B; //!< [rad/s/s] angular acceleration of body wrt to N in body frame - - - - HubEffector hub; //!< class variable - GravityEffector gravField; //!< Gravity effector for gravitational field experienced by spacecraft - std::vector states; //!< Vector of state effectors attached to dynObject - std::vector dynEffectors; //!< Vector of dynamic effectors attached to dynObject + Eigen::Vector3d sumForceExternal_N; //!< [N] Sum of forces given in the inertial frame + Eigen::Vector3d sumForceExternal_B; //!< [N] Sum of forces given in the body frame + Eigen::Vector3d sumTorquePntB_B; //!< [N-m] Total torque about point B in B frame components + + Eigen::Vector3d oldV_CN_N; //!< class variable + Eigen::Vector3d oldV_BN_N; //!< class variable + Eigen::Vector3d oldOmega_BN_B; //!< class variable + + Eigen::Vector3d dvAccum_CN_B; //!< [m/s] Accumulated delta-v of center of mass relative to inertial frame in body + //!< frame coordinates + Eigen::Vector3d + dvAccum_BN_B; //!< [m/s] accumulated delta-v of body frame relative to inertial frame in body frame coordinates + Eigen::Vector3d nonConservativeAccelpntB_B; //!< [m/s/s] Current spacecraft body acceleration in the B frame + Eigen::Vector3d omegaDot_BN_B; //!< [rad/s/s] angular acceleration of body wrt to N in body frame + + HubEffector hub; //!< class variable + GravityEffector gravField; //!< Gravity effector for gravitational field experienced by spacecraft + std::vector states; //!< Vector of state effectors attached to dynObject + std::vector dynEffectors; //!< Vector of dynamic effectors attached to dynObject std::vector dockingPoints; //!< class variable + Eigen::MatrixXd* inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialVelocityProperty; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase - Eigen::MatrixXd *inertialPositionProperty; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - Eigen::MatrixXd *inertialVelocityProperty; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase + BSKLogger bskLogger; //!< BSK Logging - BSKLogger bskLogger; //!< BSK Logging - -public: + public: SpacecraftUnit(); ~SpacecraftUnit(); - void addStateEffector(StateEffector *newStateEffector); //!< Attaches a stateEffector to the system - void addDynamicEffector(DynamicEffector *newDynamicEffector); //!< Attaches a dynamicEffector - void addDockingPort(DockingData *newDockingPort); //!< Attaches a dynamicEffector + void addStateEffector(StateEffector* newStateEffector); //!< Attaches a stateEffector to the system + void addDynamicEffector(DynamicEffector* newDynamicEffector); //!< Attaches a dynamicEffector + void addDockingPort(DockingData* newDockingPort); //!< Attaches a dynamicEffector - void SelfInitSC(int64_t moduleID); //!< Lets spacecraft plus create its own msgs + void SelfInitSC(int64_t moduleID); //!< Lets spacecraft plus create its own msgs void ResetSC(uint64_t CurrentSimNanos); - void writeOutputMessagesSC(uint64_t clockTime, int64_t moduleID); //!< Method to write all of the class output messages - void linkInStatesSC(DynParamManager& statesIn); //!< Method to get access to the hub's states + void writeOutputMessagesSC(uint64_t clockTime, + int64_t moduleID); //!< Method to write all of the class output messages + void linkInStatesSC(DynParamManager& statesIn); //!< Method to get access to the hub's states void initializeDynamicsSC(DynParamManager& statesIn); //!< class method -private: - template - void assignStateParamNames(Type effector) { + private: + template + void assignStateParamNames(Type effector) + { /* assign the state engine names for the parent rigid body states */ effector->setStateNameOfPosition(this->hub.nameOfHubPosition); effector->setStateNameOfVelocity(this->hub.nameOfHubVelocity); @@ -142,73 +144,85 @@ class SpacecraftUnit { effector->setPropName_vehicleGravity(this->gravField.vehicleGravityPropName); }; - Eigen::MatrixXd *m_SC; //!< [kg] spacecrafts total mass - Eigen::MatrixXd *mDot_SC; //!< [kg/s] Time derivative of spacecrafts total mass - Eigen::MatrixXd *ISCPntB_B; //!< [kg m^2] Inertia of s/c about point B in B frame components - Eigen::MatrixXd *c_B; //!< [m] Vector from point B to CoM of s/c in B frame components - Eigen::MatrixXd *cPrime_B; //!< [m/s] Body time derivative of c_B - Eigen::MatrixXd *cDot_B; //!< [m/s] Inertial time derivative of c_B - Eigen::MatrixXd *ISCPntBPrime_B; //!< [kg m^2/s] Body time derivative of ISCPntB_B - - Eigen::MatrixXd *g_N; //!< [m/s^2] Gravitational acceleration in N frame components - StateData *hubR_N; //!< State data accesss to inertial position for the hub - StateData *hubV_N; //!< State data access to inertial velocity for the hub - StateData *hubOmega_BN_B; //!< State data access to the attitude rate of the hub - StateData *hubSigma; //!< State data access to sigmaBN for the hub - StateData *hubGravVelocity; //!< State data access to the gravity-accumulated DV on the Body frame - StateData *BcGravVelocity; //!< State data access to the gravity-accumulated DV on point Bc - - std::string propName_m_SC; //!< property name of m_SC - std::string propName_mDot_SC; //!< property name of mDot_SC - std::string propName_centerOfMassSC; //!< property name of centerOfMassSC - std::string propName_inertiaSC; //!< property name of inertiaSC - std::string propName_inertiaPrimeSC; //!< property name of inertiaPrimeSC - std::string propName_centerOfMassPrimeSC; //!< property name of centerOfMassPrimeSC - std::string propName_centerOfMassDotSC; //!< property name of centerOfMassDotSC - + Eigen::MatrixXd* m_SC; //!< [kg] spacecrafts total mass + Eigen::MatrixXd* mDot_SC; //!< [kg/s] Time derivative of spacecrafts total mass + Eigen::MatrixXd* ISCPntB_B; //!< [kg m^2] Inertia of s/c about point B in B frame components + Eigen::MatrixXd* c_B; //!< [m] Vector from point B to CoM of s/c in B frame components + Eigen::MatrixXd* cPrime_B; //!< [m/s] Body time derivative of c_B + Eigen::MatrixXd* cDot_B; //!< [m/s] Inertial time derivative of c_B + Eigen::MatrixXd* ISCPntBPrime_B; //!< [kg m^2/s] Body time derivative of ISCPntB_B + + Eigen::MatrixXd* g_N; //!< [m/s^2] Gravitational acceleration in N frame components + StateData* hubR_N; //!< State data accesss to inertial position for the hub + StateData* hubV_N; //!< State data access to inertial velocity for the hub + StateData* hubOmega_BN_B; //!< State data access to the attitude rate of the hub + StateData* hubSigma; //!< State data access to sigmaBN for the hub + StateData* hubGravVelocity; //!< State data access to the gravity-accumulated DV on the Body frame + StateData* BcGravVelocity; //!< State data access to the gravity-accumulated DV on point Bc + + std::string propName_m_SC; //!< property name of m_SC + std::string propName_mDot_SC; //!< property name of mDot_SC + std::string propName_centerOfMassSC; //!< property name of centerOfMassSC + std::string propName_inertiaSC; //!< property name of inertiaSC + std::string propName_inertiaPrimeSC; //!< property name of inertiaPrimeSC + std::string propName_centerOfMassPrimeSC; //!< property name of centerOfMassPrimeSC + std::string propName_centerOfMassDotSC; //!< property name of centerOfMassDotSC }; - /*! @brief spacecraft dynamic effector */ -class SpacecraftSystem : public DynamicObject{ -public: - - uint64_t numOutMsgBuffers; //!< Number of output message buffers for I/O - std::string sysTimePropertyName; //!< Name of the system time property - SpacecraftUnit primaryCentralSpacecraft; //!< Primary spacecraft in which other spacecraft can attach/detach to/from - std::vector spacecraftDockedToPrimary; //!< vector of spacecraft currently docked with primary spacecraft - std::vector unDockedSpacecraft; //!< vector of spacecraft currently detached from all other spacecraft - int numberOfSCAttachedToPrimary; //!< class variable - BSKLogger bskLogger; //!< BSK Logging - -public: - SpacecraftSystem(); //!< Constructor - ~SpacecraftSystem(); //!< Destructor - void initializeDynamics(); //!< This method initializes all of the dynamics and variables for the s/c - void computeEnergyMomentum(double time); //!< This method computes the total energy and momentum of the s/c - void computeEnergyMomentumSC(double time, SpacecraftUnit& spacecraft); //!< This method computes the total energy and momentum of the s/c - void computeEnergyMomentumSystem(double time); //!< This method computes the total energy and momentum of the s/c - void updateSpacecraftMassProps(double time, SpacecraftUnit& spacecraft); //!< This method computes the total mass properties of the s/c - void updateSystemMassProps(double time); //!< This method computes the total mass properties of the s/c +class SpacecraftSystem : public DynamicObject +{ + public: + uint64_t numOutMsgBuffers; //!< Number of output message buffers for I/O + std::string sysTimePropertyName; //!< Name of the system time property + SpacecraftUnit primaryCentralSpacecraft; //!< Primary spacecraft in which other spacecraft can attach/detach to/from + std::vector + spacecraftDockedToPrimary; //!< vector of spacecraft currently docked with primary spacecraft + std::vector + unDockedSpacecraft; //!< vector of spacecraft currently detached from all other spacecraft + int numberOfSCAttachedToPrimary; //!< class variable + BSKLogger bskLogger; //!< BSK Logging + + public: + SpacecraftSystem(); //!< Constructor + ~SpacecraftSystem(); //!< Destructor + void initializeDynamics(); //!< This method initializes all of the dynamics and variables for the s/c + void computeEnergyMomentum(double time); //!< This method computes the total energy and momentum of the s/c + void computeEnergyMomentumSC( + double time, + SpacecraftUnit& spacecraft); //!< This method computes the total energy and momentum of the s/c + void computeEnergyMomentumSystem(double time); //!< This method computes the total energy and momentum of the s/c + void updateSpacecraftMassProps( + double time, + SpacecraftUnit& spacecraft); //!< This method computes the total mass properties of the s/c + void updateSystemMassProps(double time); //!< This method computes the total mass properties of the s/c void initializeSCPosVelocity(SpacecraftUnit& spacecraft); //!< class method void Reset(uint64_t CurrentSimNanos); void writeOutputMessages(uint64_t clockTime); //!< Method to write all of the class output messages - void UpdateState(uint64_t CurrentSimNanos); //!< Runtime hook back into Basilisk arch - void equationsOfMotion(double integTimeSeconds, double timeStep); //!< This method computes the equations of motion for the whole system - void equationsOfMotionSC(double integTimeSeconds, double timeStep, SpacecraftUnit& spacecraft); //!< This method computes the equations of motion for the whole system - void equationsOfMotionSystem(double integTimeSeconds, double timeStep); //!< This method computes the equations of motion for the whole system - void findPriorStateInformation(SpacecraftUnit& spacecraft); //!< class method + void UpdateState(uint64_t CurrentSimNanos); //!< Runtime hook back into Basilisk arch + void equationsOfMotion(double integTimeSeconds, + double timeStep); //!< This method computes the equations of motion for the whole system + void equationsOfMotionSC( + double integTimeSeconds, + double timeStep, + SpacecraftUnit& spacecraft); //!< This method computes the equations of motion for the whole system + void equationsOfMotionSystem( + double integTimeSeconds, + double timeStep); //!< This method computes the equations of motion for the whole system + void findPriorStateInformation(SpacecraftUnit& spacecraft); //!< class method void calculateDeltaVandAcceleration(SpacecraftUnit& spacecraft, double localTimeStep); //!< class method - void attachSpacecraftToPrimary(SpacecraftUnit *newSpacecraft, std::string dockingPortNameOfNewSpacecraft, std::string dockingToPortName); //!< Attaches a spacecraft to the primary spacecraft chain - void addSpacecraftUndocked(SpacecraftUnit *newSpacecraft); //!< Attaches a spacecraft to the primary spacecraft chain - void determineAttachedSCStates(); //!< class method + void attachSpacecraftToPrimary( + SpacecraftUnit* newSpacecraft, + std::string dockingPortNameOfNewSpacecraft, + std::string dockingToPortName); //!< Attaches a spacecraft to the primary spacecraft chain + void addSpacecraftUndocked( + SpacecraftUnit* newSpacecraft); //!< Attaches a spacecraft to the primary spacecraft chain + void determineAttachedSCStates(); //!< class method void preIntegration(uint64_t callTime) final; //!< pre-integration steps - void postIntegration(uint64_t callTime) final; //!< post-integration steps - -private: - Eigen::MatrixXd *sysTime; //!< [s] System time + void postIntegration(uint64_t callTime) final; //!< post-integration steps + private: + Eigen::MatrixXd* sysTime; //!< [s] System time }; - #endif /* SPACECRAFT_DYNAMICS_H */ 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/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/sphericalPendulum/sphericalPendulum.cpp b/src/simulation/dynamics/sphericalPendulum/sphericalPendulum.cpp index d109560f17..602eb1a6ef 100644 --- a/src/simulation/dynamics/sphericalPendulum/sphericalPendulum.cpp +++ b/src/simulation/dynamics/sphericalPendulum/sphericalPendulum.cpp @@ -25,37 +25,37 @@ /*! This is the constructor, setting variables to default values */ SphericalPendulum::SphericalPendulum() { - // - zero the contributions for mass props and mass rates - this->effProps.mEff = 0.0; - this->effProps.IEffPntB_B.setZero(); - this->effProps.rEff_CB_B.setZero(); - this->effProps.rEffPrime_CB_B.setZero(); - this->effProps.IEffPrimePntB_B.setZero(); - - // - Initialize the variables to working values - this->massFSP = 0.0; - this->d.setZero(); - this->D.setZero(); - - this->pHat_01 << 1,0,0; - this->pHat_02 << 0,1,0; - this->pHat_03 << 0,0,1; - this->phiInit=0.0; + // - zero the contributions for mass props and mass rates + this->effProps.mEff = 0.0; + this->effProps.IEffPntB_B.setZero(); + this->effProps.rEff_CB_B.setZero(); + this->effProps.rEffPrime_CB_B.setZero(); + this->effProps.IEffPrimePntB_B.setZero(); + + // - Initialize the variables to working values + this->massFSP = 0.0; + this->d.setZero(); + this->D.setZero(); + + this->pHat_01 << 1, 0, 0; + this->pHat_02 << 0, 1, 0; + this->pHat_03 << 0, 0, 1; + this->phiInit = 0.0; this->l_B.setZero(); this->lPrime_B.setZero(); this->lPrime_P0.setZero(); - this->phiDotInit=0.0; - this->thetaInit=0.0; - this->thetaDotInit=0.0; + this->phiDotInit = 0.0; + this->thetaInit = 0.0; + this->thetaDotInit = 0.0; this->massInit = 1.0; - this->nameOfPhiState = "sphericalPendulumPhi" + std::to_string(this->effectorID); - this->nameOfPhiDotState = "sphericalPendulumPhiDot" + std::to_string(this->effectorID); - this->nameOfThetaState = "sphericalPendulumTheta" + std::to_string(this->effectorID); - this->nameOfThetaDotState = "sphericalPendulumThetaDot" + std::to_string(this->effectorID); - this->nameOfMassState = "sphericalPendulumMass" + std::to_string(this->effectorID); + this->nameOfPhiState = "sphericalPendulumPhi" + std::to_string(this->effectorID); + this->nameOfPhiDotState = "sphericalPendulumPhiDot" + std::to_string(this->effectorID); + this->nameOfThetaState = "sphericalPendulumTheta" + std::to_string(this->effectorID); + this->nameOfThetaDotState = "sphericalPendulumThetaDot" + std::to_string(this->effectorID); + this->nameOfMassState = "sphericalPendulumMass" + std::to_string(this->effectorID); this->effectorID++; - return; + return; } uint64_t SphericalPendulum::effectorID = 1; @@ -63,12 +63,13 @@ uint64_t SphericalPendulum::effectorID = 1; /*! This is the destructor, nothing to report here */ SphericalPendulum::~SphericalPendulum() { - this->effectorID = 1; /* reset the panel ID*/ - return; + this->effectorID = 1; /* reset the panel ID*/ + return; } /*! Method for spherical pendulum to access the states that it needs. It needs gravity and the hub states */ -void SphericalPendulum::linkInStates(DynParamManager& statesIn) +void +SphericalPendulum::linkInStates(DynParamManager& statesIn) { // - Grab access to gravity this->g_N = statesIn.getPropertyReference(this->propName_vehicleGravity); @@ -77,118 +78,122 @@ void SphericalPendulum::linkInStates(DynParamManager& statesIn) } /*! This is the method for the spherical pendulum to register its states: l and lDot */ -void SphericalPendulum::registerStates(DynParamManager& states) +void +SphericalPendulum::registerStates(DynParamManager& states) { - // - Register phi, theta, phiDot and thetaDot - this->phiState = states.registerState(1, 1, nameOfPhiState); - Eigen::MatrixXd phiInitMatrix(1,1); - phiInitMatrix(0,0) = this->phiInit; + // - Register phi, theta, phiDot and thetaDot + this->phiState = states.registerState(1, 1, nameOfPhiState); + Eigen::MatrixXd phiInitMatrix(1, 1); + phiInitMatrix(0, 0) = this->phiInit; this->phiState->setState(phiInitMatrix); this->thetaState = states.registerState(1, 1, nameOfThetaState); - Eigen::MatrixXd thetaInitMatrix(1,1); - thetaInitMatrix(0,0) = this->thetaInit; + Eigen::MatrixXd thetaInitMatrix(1, 1); + thetaInitMatrix(0, 0) = this->thetaInit; this->thetaState->setState(thetaInitMatrix); - this->phiDotState = states.registerState(1, 1, nameOfPhiDotState); - Eigen::MatrixXd phiDotInitMatrix(1,1); - phiDotInitMatrix(0,0) = this->phiDotInit; + this->phiDotState = states.registerState(1, 1, nameOfPhiDotState); + Eigen::MatrixXd phiDotInitMatrix(1, 1); + phiDotInitMatrix(0, 0) = this->phiDotInit; this->phiDotState->setState(phiDotInitMatrix); - this->thetaDotState = states.registerState(1, 1, nameOfThetaDotState); - Eigen::MatrixXd thetaDotInitMatrix(1,1); - thetaDotInitMatrix(0,0) = this->thetaDotInit; + this->thetaDotState = states.registerState(1, 1, nameOfThetaDotState); + Eigen::MatrixXd thetaDotInitMatrix(1, 1); + thetaDotInitMatrix(0, 0) = this->thetaDotInit; this->thetaDotState->setState(thetaDotInitMatrix); - // - Register m - this->massState = states.registerState(1, 1, nameOfMassState); - Eigen::MatrixXd massInitMatrix(1,1); - massInitMatrix(0,0) = this->massInit; + // - Register m + this->massState = states.registerState(1, 1, nameOfMassState); + Eigen::MatrixXd massInitMatrix(1, 1); + massInitMatrix(0, 0) = this->massInit; this->massState->setState(massInitMatrix); - return; + return; } /*! This is the method for the FSP to add its contributions to the mass props and mass prop rates of the vehicle */ -void SphericalPendulum::updateEffectorMassProps(double integTime) +void +SphericalPendulum::updateEffectorMassProps(double integTime) { // - Grab phi and theta from state manager and define r_PcB_B - this->phi = this->phiState->getState()(0,0); - this->theta = this->thetaState->getState()(0,0); - - // mantain phi and theta between 0 and 2pi - if (this->phi>2*M_PI) { - this->phi=this->phi-2*M_PI; - Eigen::MatrixXd phiInitMatrix(1,1); - phiInitMatrix(0,0) = this->phi; - this->phiState->setState(phiInitMatrix); - } - if (this->phi<-2*M_PI) { - this->phi=this->phi+2*M_PI; - Eigen::MatrixXd phiInitMatrix(1,1); - phiInitMatrix(0,0) = this->phi; - this->phiState->setState(phiInitMatrix); - } - - if (this->theta>2*M_PI) { - this->theta=this->theta-2*M_PI; - Eigen::MatrixXd thetaInitMatrix(1,1); - thetaInitMatrix(0,0) = this->theta; - this->thetaState->setState(thetaInitMatrix); - } - if (this->theta<-2*M_PI) { - this->theta=this->theta+2*M_PI; - Eigen::MatrixXd thetaInitMatrix(1,1); - thetaInitMatrix(0,0) = this->theta; - this->thetaState->setState(thetaInitMatrix); - } - // define l in P0 frame - Eigen::Vector3d l_P0; - l_P0 << this->pendulumRadius*cos(this->phi)*cos(this->theta), this->pendulumRadius*sin(this->phi)*cos(this->theta), - -this->pendulumRadius*sin(this->theta); - - // define the rotation matrix from P0 to B frame - dcm_B_P0 << pHat_01(0,0), pHat_02(0,0), pHat_03(0,0), - pHat_01(1,0), pHat_02(1,0), pHat_03(1,0), - pHat_01(2,0), pHat_02(2,0), pHat_03(2,0); - this->l_B=dcm_B_P0*l_P0; - - this->r_PcB_B = this->d + this->l_B; - this->massFSP = this->massState->getState()(0, 0); - - // - Update the effectors mass - this->effProps.mEff = this->massFSP; - // - Update the position of CoM - this->effProps.rEff_CB_B = this->r_PcB_B; - // - Update the inertia about B - this->rTilde_PcB_B = eigenTilde(this->r_PcB_B); - this->effProps.IEffPntB_B = this->massFSP * this->rTilde_PcB_B * this->rTilde_PcB_B.transpose(); - - // - Grab phiDot and thetaDot from the stateManager and define rPrime_PcB_B - this->phiDot=this->phiDotState->getState()(0,0); - this->thetaDot=this->thetaDotState->getState()(0,0); - - // define the derivative of l in P0 frame - this->lPrime_P0 << this->pendulumRadius*(-this->phiDot*sin(this->phi)*cos(this->theta)-this->thetaDot*cos(this->phi)*sin(this->theta)), - this->pendulumRadius*(this->phiDot*cos(this->phi)*cos(this->theta)-this->thetaDot*sin(this->phi)*sin(this->theta)), - -this->pendulumRadius*(this->thetaDot*cos(this->theta)); + this->phi = this->phiState->getState()(0, 0); + this->theta = this->thetaState->getState()(0, 0); + + // mantain phi and theta between 0 and 2pi + if (this->phi > 2 * M_PI) { + this->phi = this->phi - 2 * M_PI; + Eigen::MatrixXd phiInitMatrix(1, 1); + phiInitMatrix(0, 0) = this->phi; + this->phiState->setState(phiInitMatrix); + } + if (this->phi < -2 * M_PI) { + this->phi = this->phi + 2 * M_PI; + Eigen::MatrixXd phiInitMatrix(1, 1); + phiInitMatrix(0, 0) = this->phi; + this->phiState->setState(phiInitMatrix); + } + + if (this->theta > 2 * M_PI) { + this->theta = this->theta - 2 * M_PI; + Eigen::MatrixXd thetaInitMatrix(1, 1); + thetaInitMatrix(0, 0) = this->theta; + this->thetaState->setState(thetaInitMatrix); + } + if (this->theta < -2 * M_PI) { + this->theta = this->theta + 2 * M_PI; + Eigen::MatrixXd thetaInitMatrix(1, 1); + thetaInitMatrix(0, 0) = this->theta; + this->thetaState->setState(thetaInitMatrix); + } + // define l in P0 frame + Eigen::Vector3d l_P0; + l_P0 << this->pendulumRadius * cos(this->phi) * cos(this->theta), + this->pendulumRadius * sin(this->phi) * cos(this->theta), -this->pendulumRadius * sin(this->theta); + + // define the rotation matrix from P0 to B frame + dcm_B_P0 << pHat_01(0, 0), pHat_02(0, 0), pHat_03(0, 0), pHat_01(1, 0), pHat_02(1, 0), pHat_03(1, 0), pHat_01(2, 0), + pHat_02(2, 0), pHat_03(2, 0); + this->l_B = dcm_B_P0 * l_P0; + + this->r_PcB_B = this->d + this->l_B; + this->massFSP = this->massState->getState()(0, 0); + + // - Update the effectors mass + this->effProps.mEff = this->massFSP; + // - Update the position of CoM + this->effProps.rEff_CB_B = this->r_PcB_B; + // - Update the inertia about B + this->rTilde_PcB_B = eigenTilde(this->r_PcB_B); + this->effProps.IEffPntB_B = this->massFSP * this->rTilde_PcB_B * this->rTilde_PcB_B.transpose(); + + // - Grab phiDot and thetaDot from the stateManager and define rPrime_PcB_B + this->phiDot = this->phiDotState->getState()(0, 0); + this->thetaDot = this->thetaDotState->getState()(0, 0); + + // define the derivative of l in P0 frame + this->lPrime_P0 << this->pendulumRadius * (-this->phiDot * sin(this->phi) * cos(this->theta) - + this->thetaDot * cos(this->phi) * sin(this->theta)), + this->pendulumRadius * + (this->phiDot * cos(this->phi) * cos(this->theta) - this->thetaDot * sin(this->phi) * sin(this->theta)), + -this->pendulumRadius * (this->thetaDot * cos(this->theta)); // rotate l derivative in B frame - this->lPrime_B = dcm_B_P0*this->lPrime_P0; + this->lPrime_B = dcm_B_P0 * this->lPrime_P0; - this->rPrime_PcB_B = this->lPrime_B; - this->effProps.rEffPrime_CB_B = this->rPrime_PcB_B; + this->rPrime_PcB_B = this->lPrime_B; + this->effProps.rEffPrime_CB_B = this->rPrime_PcB_B; - // - Update the body time derivative of inertia about B - this->rPrimeTilde_PcB_B = eigenTilde(this->rPrime_PcB_B); - this->effProps.IEffPrimePntB_B = -this->massFSP*(this->rPrimeTilde_PcB_B*this->rTilde_PcB_B - + this->rTilde_PcB_B*this->rPrimeTilde_PcB_B); + // - Update the body time derivative of inertia about B + this->rPrimeTilde_PcB_B = eigenTilde(this->rPrime_PcB_B); + this->effProps.IEffPrimePntB_B = + -this->massFSP * (this->rPrimeTilde_PcB_B * this->rTilde_PcB_B + this->rTilde_PcB_B * this->rPrimeTilde_PcB_B); return; } /*! This is method is used to pass mass properties information to the fuelTank */ -void SphericalPendulum::retrieveMassValue(double integTime) +void +SphericalPendulum::retrieveMassValue(double integTime) { // Save mass value into the fuelSlosh class variable this->fuelMass = this->massFSP; @@ -197,138 +202,193 @@ void SphericalPendulum::retrieveMassValue(double integTime) } /*! This method is for the FSP to add its contributions to the back-sub method */ -void SphericalPendulum::updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) +void +SphericalPendulum::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { // - Find dcm_BN Eigen::MRPd sigmaLocal_BN; Eigen::Matrix3d dcm_BN; Eigen::Matrix3d dcm_NB; - sigmaLocal_BN = (Eigen::Vector3d ) sigma_BN; + sigmaLocal_BN = (Eigen::Vector3d)sigma_BN; dcm_NB = sigmaLocal_BN.toRotationMatrix(); dcm_BN = dcm_NB.transpose(); Eigen::Matrix3d dTilde; Eigen::Matrix3d lTilde; dTilde = eigenTilde(this->d); - lTilde = eigenTilde(this->l_B); + lTilde = eigenTilde(this->l_B); - // - Define aPhi.transpose() - this->aPhi = -(this->pHat_03.transpose()*lTilde)/(this->pendulumRadius*this->pendulumRadius*cos(this->theta)*cos(this->theta)); + // - Define aPhi.transpose() + this->aPhi = -(this->pHat_03.transpose() * lTilde) / + (this->pendulumRadius * this->pendulumRadius * cos(this->theta) * cos(this->theta)); // - Define bPhi.transpose() - this->bPhi = this->pHat_03.transpose()*lTilde*(lTilde+dTilde)/(this->pendulumRadius*this->pendulumRadius*cos(this->theta)*cos(this->theta)); - + this->bPhi = this->pHat_03.transpose() * lTilde * (lTilde + dTilde) / + (this->pendulumRadius * this->pendulumRadius * cos(this->theta) * cos(this->theta)); - // - Map gravity to body frame + // - Map gravity to body frame Eigen::Vector3d gLocal_N; Eigen::Vector3d g_B; gLocal_N = *this->g_N; - g_B = dcm_BN*gLocal_N; + g_B = dcm_BN * gLocal_N; Eigen::Vector3d L_T; - L_T=this->l_B.cross(this->massFSP*g_B)-dcm_B_P0*this->D*this->lPrime_P0; + L_T = this->l_B.cross(this->massFSP * g_B) - dcm_B_P0 * this->D * this->lPrime_P0; // - Define cPhi Eigen::Vector3d omega_BN_B_local = omega_BN_B; Eigen::Matrix3d omegaTilde_BN_B_local; omegaTilde_BN_B_local = eigenTilde(omega_BN_B_local); - this->cPhi = 1.0/(this->massFSP*this->pendulumRadius*this->pendulumRadius*cos(this->theta)*cos(this->theta))* - (-this->massFSP*this->pHat_03.transpose().dot(lTilde*omegaTilde_BN_B_local*omegaTilde_BN_B_local*this->d) - +this->pHat_03.transpose()*(L_T) + - 2*this->massFSP*this->pendulumRadius*this->pendulumRadius*this->phiDot*this->thetaDot*cos(this->theta)*sin(this->theta) - -this->massFSP*this->pHat_03.transpose().dot(lTilde*(2*omegaTilde_BN_B_local*this->lPrime_B+omegaTilde_BN_B_local*omegaTilde_BN_B_local*this->l_B))); - - // Define pHat_02_Prime axes of rotation of theta in P0 frame - Eigen::Vector3d pHat_02_Prime_P0; - pHat_02_Prime_P0 << -sin(this->phi),cos(this->phi),0; - // Rotate pHat_02_Prime axes in B frame components - Eigen::Vector3d pHat_02_Prime; - pHat_02_Prime = dcm_B_P0*pHat_02_Prime_P0; - - // Define aTheta.transpose() - this->aTheta = -(pHat_02_Prime.transpose()*lTilde)/(this->pendulumRadius*this->pendulumRadius); - // Define bTheta.transpose() - this->bTheta = pHat_02_Prime.transpose()*lTilde*(lTilde+dTilde)/(this->pendulumRadius*this->pendulumRadius); - // Define cTheta - this->cTheta = 1.0/(this->massFSP*this->pendulumRadius*this->pendulumRadius)* - (-this->massFSP*pHat_02_Prime.transpose().dot(lTilde*omegaTilde_BN_B_local*omegaTilde_BN_B_local*this->d) - +pHat_02_Prime.transpose()*(L_T) - -this->massFSP*this->pendulumRadius*this->pendulumRadius*this->phiDot*this->phiDot*cos(this->theta)*sin(this->theta) - -this->massFSP*pHat_02_Prime.transpose().dot(lTilde*(2*omegaTilde_BN_B_local*this->lPrime_B+omegaTilde_BN_B_local*omegaTilde_BN_B_local*this->l_B))); - - // - Compute matrix/vector contributions - backSubContr.matrixA = -this->massFSP*this->pendulumRadius*((sin(this->phi)*cos(this->theta)*this->pHat_01 - - cos(this->phi)*cos(this->theta)*this->pHat_02)*this->aPhi.transpose()+(cos(this->phi)*sin(this->theta)*this->pHat_01 - +sin(this->phi)*sin(this->theta)*this->pHat_02+cos(this->theta)*this->pHat_03)*this->aTheta.transpose()); - - backSubContr.matrixB = -this->massFSP*this->pendulumRadius*((sin(this->phi)*cos(this->theta)*this->pHat_01 - - cos(this->phi)*cos(this->theta)*this->pHat_02)*this->bPhi.transpose() + (cos(this->phi)*sin(this->theta)*this->pHat_01 - +sin(this->phi)*sin(this->theta)*this->pHat_02+cos(this->theta)*this->pHat_03)*this->bTheta.transpose()); - - backSubContr.matrixC = -this->massFSP*this->pendulumRadius*this->rTilde_PcB_B*((sin(this->phi)*cos(this->theta)*this->pHat_01 - - cos(this->phi)*cos(this->theta)*this->pHat_02)*this->aPhi.transpose()+(cos(this->phi)*sin(this->theta)*this->pHat_01 - +sin(this->phi)*sin(this->theta)*this->pHat_02+cos(this->theta)*this->pHat_03)*this->aTheta.transpose()); - - backSubContr.matrixD = -this->massFSP*this->pendulumRadius*this->rTilde_PcB_B*((sin(this->phi)*cos(this->theta)*this->pHat_01 - - cos(this->phi)*cos(this->theta)*this->pHat_02)*this->bPhi.transpose()+(cos(this->phi)*sin(this->theta)*this->pHat_01 - +sin(this->phi)*sin(this->theta)*this->pHat_02+cos(this->theta)*this->pHat_03)*this->bTheta.transpose()); - - backSubContr.vecTrans = -this->massFSP*this->pendulumRadius*((-cos(this->phi)*cos(this->theta)*this->pHat_01 - -sin(this->phi)*cos(this->theta)*this->pHat_02)*this->phiDot*this->phiDot - +(-cos(this->phi)*cos(this->theta)*this->pHat_01-sin(this->phi)*cos(this->theta)*this->pHat_02+sin(this->theta)*this->pHat_03)*this->thetaDot*this->thetaDot - +(2*sin(this->phi)*sin(this->theta)*this->pHat_01-2*cos(this->phi)*sin(this->theta)*this->pHat_02)*this->phiDot*this->thetaDot - -(sin(this->phi)*cos(this->theta)*this->pHat_01-cos(this->phi)*cos(this->theta)*this->pHat_02)*this->cPhi - -(cos(this->phi)*sin(this->theta)*this->pHat_01+sin(this->phi)*sin(this->theta)*this->pHat_02+cos(theta)*this->pHat_03)*this->cTheta); - - backSubContr.vecRot = -this->massFSP*(omegaTilde_BN_B_local*this->rTilde_PcB_B*this->rPrime_PcB_B - +this->pendulumRadius*rTilde_PcB_B*( - (-cos(this->phi)*cos(this->theta)*this->pHat_01-sin(this->phi)*cos(this->theta)*this->pHat_02)*this->phiDot*this->phiDot - +(-cos(this->phi)*cos(this->theta)*this->pHat_01-sin(this->phi)*cos(this->theta)*this->pHat_02+sin(this->theta)*this->pHat_03)*this->thetaDot*this->thetaDot - +(2*sin(this->phi)*sin(this->theta)*this->pHat_01-2*cos(this->phi)*sin(this->theta)*this->pHat_02)*this->phiDot*this->thetaDot - -(sin(this->phi)*cos(this->theta)*this->pHat_01-cos(this->phi)*cos(this->theta)*this->pHat_02)*this->cPhi - -(cos(this->phi)*sin(this->theta)*this->pHat_01+sin(this->phi)*sin(this->theta)*this->pHat_02+cos(theta)*this->pHat_03)*this->cTheta - ) - ); + this->cPhi = + 1.0 / (this->massFSP * this->pendulumRadius * this->pendulumRadius * cos(this->theta) * cos(this->theta)) * + (-this->massFSP * + this->pHat_03.transpose().dot(lTilde * omegaTilde_BN_B_local * omegaTilde_BN_B_local * this->d) + + this->pHat_03.transpose() * (L_T) + + 2 * this->massFSP * this->pendulumRadius * this->pendulumRadius * this->phiDot * this->thetaDot * + cos(this->theta) * sin(this->theta) - + this->massFSP * + this->pHat_03.transpose().dot(lTilde * (2 * omegaTilde_BN_B_local * this->lPrime_B + + omegaTilde_BN_B_local * omegaTilde_BN_B_local * this->l_B))); + + // Define pHat_02_Prime axes of rotation of theta in P0 frame + Eigen::Vector3d pHat_02_Prime_P0; + pHat_02_Prime_P0 << -sin(this->phi), cos(this->phi), 0; + // Rotate pHat_02_Prime axes in B frame components + Eigen::Vector3d pHat_02_Prime; + pHat_02_Prime = dcm_B_P0 * pHat_02_Prime_P0; + + // Define aTheta.transpose() + this->aTheta = -(pHat_02_Prime.transpose() * lTilde) / (this->pendulumRadius * this->pendulumRadius); + // Define bTheta.transpose() + this->bTheta = + pHat_02_Prime.transpose() * lTilde * (lTilde + dTilde) / (this->pendulumRadius * this->pendulumRadius); + // Define cTheta + this->cTheta = 1.0 / (this->massFSP * this->pendulumRadius * this->pendulumRadius) * + (-this->massFSP * + pHat_02_Prime.transpose().dot(lTilde * omegaTilde_BN_B_local * omegaTilde_BN_B_local * this->d) + + pHat_02_Prime.transpose() * (L_T) - + this->massFSP * this->pendulumRadius * this->pendulumRadius * this->phiDot * this->phiDot * + cos(this->theta) * sin(this->theta) - + this->massFSP * pHat_02_Prime.transpose().dot( + lTilde * (2 * omegaTilde_BN_B_local * this->lPrime_B + + omegaTilde_BN_B_local * omegaTilde_BN_B_local * this->l_B))); + + // - Compute matrix/vector contributions + backSubContr.matrixA = + -this->massFSP * this->pendulumRadius * + ((sin(this->phi) * cos(this->theta) * this->pHat_01 - cos(this->phi) * cos(this->theta) * this->pHat_02) * + this->aPhi.transpose() + + (cos(this->phi) * sin(this->theta) * this->pHat_01 + sin(this->phi) * sin(this->theta) * this->pHat_02 + + cos(this->theta) * this->pHat_03) * + this->aTheta.transpose()); + + backSubContr.matrixB = + -this->massFSP * this->pendulumRadius * + ((sin(this->phi) * cos(this->theta) * this->pHat_01 - cos(this->phi) * cos(this->theta) * this->pHat_02) * + this->bPhi.transpose() + + (cos(this->phi) * sin(this->theta) * this->pHat_01 + sin(this->phi) * sin(this->theta) * this->pHat_02 + + cos(this->theta) * this->pHat_03) * + this->bTheta.transpose()); + + backSubContr.matrixC = + -this->massFSP * this->pendulumRadius * this->rTilde_PcB_B * + ((sin(this->phi) * cos(this->theta) * this->pHat_01 - cos(this->phi) * cos(this->theta) * this->pHat_02) * + this->aPhi.transpose() + + (cos(this->phi) * sin(this->theta) * this->pHat_01 + sin(this->phi) * sin(this->theta) * this->pHat_02 + + cos(this->theta) * this->pHat_03) * + this->aTheta.transpose()); + + backSubContr.matrixD = + -this->massFSP * this->pendulumRadius * this->rTilde_PcB_B * + ((sin(this->phi) * cos(this->theta) * this->pHat_01 - cos(this->phi) * cos(this->theta) * this->pHat_02) * + this->bPhi.transpose() + + (cos(this->phi) * sin(this->theta) * this->pHat_01 + sin(this->phi) * sin(this->theta) * this->pHat_02 + + cos(this->theta) * this->pHat_03) * + this->bTheta.transpose()); + + backSubContr.vecTrans = + -this->massFSP * this->pendulumRadius * + ((-cos(this->phi) * cos(this->theta) * this->pHat_01 - sin(this->phi) * cos(this->theta) * this->pHat_02) * + this->phiDot * this->phiDot + + (-cos(this->phi) * cos(this->theta) * this->pHat_01 - sin(this->phi) * cos(this->theta) * this->pHat_02 + + sin(this->theta) * this->pHat_03) * + this->thetaDot * this->thetaDot + + (2 * sin(this->phi) * sin(this->theta) * this->pHat_01 - 2 * cos(this->phi) * sin(this->theta) * this->pHat_02) * + this->phiDot * this->thetaDot - + (sin(this->phi) * cos(this->theta) * this->pHat_01 - cos(this->phi) * cos(this->theta) * this->pHat_02) * + this->cPhi - + (cos(this->phi) * sin(this->theta) * this->pHat_01 + sin(this->phi) * sin(this->theta) * this->pHat_02 + + cos(theta) * this->pHat_03) * + this->cTheta); + + backSubContr.vecRot = + -this->massFSP * + (omegaTilde_BN_B_local * this->rTilde_PcB_B * this->rPrime_PcB_B + + this->pendulumRadius * rTilde_PcB_B * + ((-cos(this->phi) * cos(this->theta) * this->pHat_01 - sin(this->phi) * cos(this->theta) * this->pHat_02) * + this->phiDot * this->phiDot + + (-cos(this->phi) * cos(this->theta) * this->pHat_01 - sin(this->phi) * cos(this->theta) * this->pHat_02 + + sin(this->theta) * this->pHat_03) * + this->thetaDot * this->thetaDot + + (2 * sin(this->phi) * sin(this->theta) * this->pHat_01 - + 2 * cos(this->phi) * sin(this->theta) * this->pHat_02) * + this->phiDot * this->thetaDot - + (sin(this->phi) * cos(this->theta) * this->pHat_01 - cos(this->phi) * cos(this->theta) * this->pHat_02) * + this->cPhi - + (cos(this->phi) * sin(this->theta) * this->pHat_01 + sin(this->phi) * sin(this->theta) * this->pHat_02 + + cos(theta) * this->pHat_03) * + this->cTheta)); return; } /*! This method is used to define the derivatives of the FSP. One is the trivial kinematic derivative and the other is derived using the back-sub method */ -void SphericalPendulum::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +SphericalPendulum::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { - // - Find DCM - Eigen::MRPd sigmaLocal_BN; - Eigen::Matrix3d dcm_BN; - sigmaLocal_BN = (Eigen::Vector3d) sigma_BN; - dcm_BN = (sigmaLocal_BN.toRotationMatrix()).transpose(); + // - Find DCM + Eigen::MRPd sigmaLocal_BN; + Eigen::Matrix3d dcm_BN; + sigmaLocal_BN = (Eigen::Vector3d)sigma_BN; + dcm_BN = (sigmaLocal_BN.toRotationMatrix()).transpose(); - // - Set the derivative of l to lDot - this->phiState->setDerivative(this->phiDotState->getState()); - this->thetaState->setDerivative(this->thetaDotState->getState()); + // - Set the derivative of l to lDot + this->phiState->setDerivative(this->phiDotState->getState()); + this->thetaState->setDerivative(this->thetaDotState->getState()); - // - Compute lDDot - Eigen::MatrixXd phi_conv(1,1); - Eigen::MatrixXd theta_conv(1,1); + // - Compute lDDot + Eigen::MatrixXd phi_conv(1, 1); + Eigen::MatrixXd theta_conv(1, 1); Eigen::Vector3d omegaDot_BN_B_local = omegaDot_BN_B; Eigen::Vector3d rDDot_BN_N_local = rDDot_BN_N; - Eigen::Vector3d rDDot_BN_B_local = dcm_BN*rDDot_BN_N_local; + Eigen::Vector3d rDDot_BN_B_local = dcm_BN * rDDot_BN_N_local; phi_conv(0, 0) = this->aPhi.dot(rDDot_BN_B_local) + this->bPhi.dot(omegaDot_BN_B_local) + this->cPhi; - this->phiDotState->setDerivative(phi_conv); - theta_conv(0, 0) = this->aTheta.dot(rDDot_BN_B_local) + this->bTheta.dot(omegaDot_BN_B_local) + this->cTheta; - this->thetaDotState->setDerivative(theta_conv); + this->phiDotState->setDerivative(phi_conv); + theta_conv(0, 0) = this->aTheta.dot(rDDot_BN_B_local) + this->bTheta.dot(omegaDot_BN_B_local) + this->cTheta; + this->thetaDotState->setDerivative(theta_conv); // - Set the massDot already computed from fuelTank to the stateDerivative of mass - Eigen::MatrixXd conv(1,1); - conv(0,0) = this->fuelMassDot; + Eigen::MatrixXd conv(1, 1); + conv(0, 0) = this->fuelMassDot; this->massState->setDerivative(conv); return; } /*! This method is for the FSP to add its contributions to energy and momentum */ -void SphericalPendulum::updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B) +void +SphericalPendulum::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { // - Get variables needed for energy momentum calcs @@ -338,64 +398,63 @@ void SphericalPendulum::updateEnergyMomContributions(double integTime, Eigen::Ve // - Find rotational angular momentum contribution from hub rDotPcB_B = this->rPrime_PcB_B + omegaLocal_BN_B.cross(this->r_PcB_B); - rotAngMomPntCContr_B = this->massFSP*this->r_PcB_B.cross(rDotPcB_B); + rotAngMomPntCContr_B = this->massFSP * this->r_PcB_B.cross(rDotPcB_B); // - Find rotational energy contribution from the hub - rotEnergyContr = 1.0/2.0*this->massFSP*rDotPcB_B.dot(rDotPcB_B); - + rotEnergyContr = 1.0 / 2.0 * this->massFSP * rDotPcB_B.dot(rDotPcB_B); return; - } -void SphericalPendulum::modifyStates(double integTime){ - // when theta>45° change reference system in order to avoid singularities on aPhi, bPhi, cPhi - if (fabs(cos(this->theta))phi)*cos(this->theta), -sin(this->phi), cos(this->phi)*sin(this->theta), - sin(this->phi)*cos(this->theta), cos(this->phi), sin(this->phi)*sin(this->theta), - -sin(this->theta), 0, cos(this->theta); - // define the P0new vectors in P0new components - this->pHat_01 << 1,0,0; - this->pHat_02 << 0,1,0; - this->pHat_03 << 0,0,1; - - // rotate these vectors before in P0 frame and then in B frame - this->pHat_01=dcm_B_P0*dcm_P0_P0new*this->pHat_01; - this->pHat_02=dcm_B_P0*dcm_P0_P0new*this->pHat_02; - this->pHat_03=dcm_B_P0*dcm_P0_P0new*this->pHat_03; - - // define the new rotation matrix from P0 to B - dcm_B_P0 << pHat_01(0,0), pHat_02(0,0), pHat_03(0,0), - pHat_01(1,0), pHat_02(1,0), pHat_03(1,0), - pHat_01(2,0), pHat_02(2,0), pHat_03(2,0); // compute lPrime in P0new components - Eigen::Vector3d lPrime_P0new; - lPrime_P0new=dcm_B_P0.transpose()*this->lPrime_B; - // define the new values for phiDot and thetaDot inverting the lPrime definition - this->thetaDot=-lPrime_P0new(2,0)/this->pendulumRadius; - this->phiDot=lPrime_P0new(1,0)/this->pendulumRadius; - // set the new values of theta and phi - this->theta=0; - this->phi=0; - - // set the new state of phi,theta, phiDot, thetaDot - Eigen::MatrixXd phiMatrix(1,1); - phiMatrix(0,0) = this->phi; - this->phiState->setState(phiMatrix); - - Eigen::MatrixXd thetaMatrix(1,1); - thetaMatrix(0,0) = this->theta; - this->thetaState->setState(thetaMatrix); - - Eigen::MatrixXd phiDotMatrix(1,1); - phiDotMatrix(0,0) = this->phiDot; - this->phiDotState->setState(phiDotMatrix); - - Eigen::MatrixXd thetaDotMatrix(1,1); - thetaDotMatrix(0,0) = this->thetaDot; - this->thetaDotState->setState(thetaDotMatrix); +void +SphericalPendulum::modifyStates(double integTime) +{ + // when theta>45° change reference system in order to avoid singularities on aPhi, bPhi, cPhi + if (fabs(cos(this->theta)) < sqrt(2) / 2) { + // define the rotation matrix from P0 to P0new + Eigen::Matrix3d dcm_P0_P0new; + dcm_P0_P0new << cos(this->phi) * cos(this->theta), -sin(this->phi), cos(this->phi) * sin(this->theta), + sin(this->phi) * cos(this->theta), cos(this->phi), sin(this->phi) * sin(this->theta), -sin(this->theta), 0, + cos(this->theta); + // define the P0new vectors in P0new components + this->pHat_01 << 1, 0, 0; + this->pHat_02 << 0, 1, 0; + this->pHat_03 << 0, 0, 1; + + // rotate these vectors before in P0 frame and then in B frame + this->pHat_01 = dcm_B_P0 * dcm_P0_P0new * this->pHat_01; + this->pHat_02 = dcm_B_P0 * dcm_P0_P0new * this->pHat_02; + this->pHat_03 = dcm_B_P0 * dcm_P0_P0new * this->pHat_03; + + // define the new rotation matrix from P0 to B + dcm_B_P0 << pHat_01(0, 0), pHat_02(0, 0), pHat_03(0, 0), pHat_01(1, 0), pHat_02(1, 0), pHat_03(1, 0), + pHat_01(2, 0), pHat_02(2, 0), pHat_03(2, 0); // compute lPrime in P0new components + Eigen::Vector3d lPrime_P0new; + lPrime_P0new = dcm_B_P0.transpose() * this->lPrime_B; + // define the new values for phiDot and thetaDot inverting the lPrime definition + this->thetaDot = -lPrime_P0new(2, 0) / this->pendulumRadius; + this->phiDot = lPrime_P0new(1, 0) / this->pendulumRadius; + // set the new values of theta and phi + this->theta = 0; + this->phi = 0; + + // set the new state of phi,theta, phiDot, thetaDot + Eigen::MatrixXd phiMatrix(1, 1); + phiMatrix(0, 0) = this->phi; + this->phiState->setState(phiMatrix); + + Eigen::MatrixXd thetaMatrix(1, 1); + thetaMatrix(0, 0) = this->theta; + this->thetaState->setState(thetaMatrix); + + Eigen::MatrixXd phiDotMatrix(1, 1); + phiDotMatrix(0, 0) = this->phiDot; + this->phiDotState->setState(phiDotMatrix); + + Eigen::MatrixXd thetaDotMatrix(1, 1); + thetaDotMatrix(0, 0) = this->thetaDot; + this->thetaDotState->setState(thetaDotMatrix); } - return; - } + return; +} diff --git a/src/simulation/dynamics/sphericalPendulum/sphericalPendulum.h b/src/simulation/dynamics/sphericalPendulum/sphericalPendulum.h old mode 100755 new mode 100644 index 96b3a21811..fdaf613ea5 --- a/src/simulation/dynamics/sphericalPendulum/sphericalPendulum.h +++ b/src/simulation/dynamics/sphericalPendulum/sphericalPendulum.h @@ -17,7 +17,6 @@ */ - #ifndef SPHERICAL_PENDULUM_H #define SPHERICAL_PENDULUM_H @@ -28,73 +27,81 @@ #include "architecture/utilities/bskLogging.h" /*! @brief spherical pendulum state effector model */ -class SphericalPendulum : - public StateEffector, public SysModel, public FuelSlosh +class SphericalPendulum + : public StateEffector + , public SysModel + , public FuelSlosh { -public: - double pendulumRadius; //!< [m] distance between the center of the tank and the spherical pendulum mass - Eigen::Matrix3d D; //!< [N*s/m] linear damping matrix for spherical pendulum - double phiDotInit; //!< [rad/s] Initial value for spherical pendulum pendulum offset derivative + public: + double pendulumRadius; //!< [m] distance between the center of the tank and the spherical pendulum mass + Eigen::Matrix3d D; //!< [N*s/m] linear damping matrix for spherical pendulum + double phiDotInit; //!< [rad/s] Initial value for spherical pendulum pendulum offset derivative double thetaDotInit; //!< [rad/s] Initial value for spherical pendulum pendulum offset derivative - double massInit; //!< [m] Initial value for spherical pendulum pendulum mass - std::string nameOfPhiState; //!< [-] Identifier for the phi state data container + double massInit; //!< [m] Initial value for spherical pendulum pendulum mass + std::string nameOfPhiState; //!< [-] Identifier for the phi state data container std::string nameOfThetaState; //!< [-] Identifier for the theta state data container - std::string nameOfPhiDotState; //!< [-] Identifier for the phiDot state data container + std::string nameOfPhiDotState; //!< [-] Identifier for the phiDot state data container std::string nameOfThetaDotState; //!< [-] Identifier for the thetaDot state data container - std::string nameOfMassState; //!< [-] Identifier for the mass state data container - Eigen::Vector3d d; //!< [m] position vector from B point to tank center , T, in body frame - StateData *massState; //!< -- state data for the pendulums mass - Eigen::Vector3d pHat_01; //!<-- first vector of the P0 frame in B frame components - Eigen::Vector3d pHat_02; //!<-- second vector of the P0 frame in B frame components - Eigen::Vector3d pHat_03; //!<-- third vector of the P0 frame in B frame components - BSKLogger bskLogger; //!< -- BSK Logging - -private: - double phiInit; //!< [rad] Initial value for spherical pendulum pendulum offset - double thetaInit; //!< [rad] Initial value for spherical pendulum pendulum offset - double phi; //!< [rad] spherical pendulum displacement in P0 frame - double theta; //!< [rad] spherical pendulum displacement in P0 frame - double phiDot; //!< [rad/s] time derivative of displacement in P0 frame - double thetaDot; //!< [rad/s] time derivative of displacement in P0 frame - double massFSP; //!< [kg] mass of spherical pendulum pendulum - Eigen::Vector3d r_PcB_B; //!< [m] position vector form B to center of mass location of pendulum - Eigen::Matrix3d rTilde_PcB_B; //!< [m] tilde matrix of r_Pc_B - Eigen::Vector3d rPrime_PcB_B; //!< [m/s] Body time derivative of r_Pc_B - Eigen::Matrix3d rPrimeTilde_PcB_B; //!< [m/s] Tilde matrix of rPrime_PcB_B - - Eigen::Vector3d aPhi; //!< -- Term needed for back-sub method - Eigen::Vector3d bPhi; //!< -- Term needed for back-sub method - Eigen::Vector3d aTheta; //!< -- Term needed for back-sub method - Eigen::Vector3d bTheta; //!< -- Term needed for back-sub method - double cPhi; //!< -- Term needed for back-sub method - double cTheta; //!< -- Term needed for back-sub method - - Eigen::MatrixXd *g_N; //!< [m/s^2] Gravitational acceleration in N frame components - Eigen::Vector3d l_B; //!< [m] vector from the center of the tank to the spherical pendulum pendulum in B frame - Eigen::Vector3d lPrime_B; //!< [m/s] derivative of l respect to B frame - Eigen::Vector3d lPrime_P0; //!< [m/s] derivative of l in P0 frame - StateData *phiState; //!< -- state data for spherical pendulum displacement - StateData *thetaState; //!< -- state data for spherical pendulum displacement - StateData *phiDotState; //!< -- state data for time derivative of phi; - StateData *thetaDotState; //!< -- state data for time derivative of theta; - Eigen::Matrix3d dcm_B_P0; // Rotation matrix from P0 to B frame - static uint64_t effectorID; //!< [] ID number of this panel - - - -public: - SphericalPendulum(); //!< -- Contructor - ~SphericalPendulum(); //!< -- Destructor - void registerStates(DynParamManager& states); //!< -- Method for FSP to register its states - void linkInStates(DynParamManager& states); //!< -- Method for FSP to get access of other states - void updateEffectorMassProps(double integTime); //!< -- Method for FSP to add its contributions to mass props - void modifyStates(double integTime); //!<-- Method to force states modification during integration + std::string nameOfMassState; //!< [-] Identifier for the mass state data container + Eigen::Vector3d d; //!< [m] position vector from B point to tank center , T, in body frame + StateData* massState; //!< -- state data for the pendulums mass + Eigen::Vector3d pHat_01; //!<-- first vector of the P0 frame in B frame components + Eigen::Vector3d pHat_02; //!<-- second vector of the P0 frame in B frame components + Eigen::Vector3d pHat_03; //!<-- third vector of the P0 frame in B frame components + BSKLogger bskLogger; //!< -- BSK Logging + + private: + double phiInit; //!< [rad] Initial value for spherical pendulum pendulum offset + double thetaInit; //!< [rad] Initial value for spherical pendulum pendulum offset + double phi; //!< [rad] spherical pendulum displacement in P0 frame + double theta; //!< [rad] spherical pendulum displacement in P0 frame + double phiDot; //!< [rad/s] time derivative of displacement in P0 frame + double thetaDot; //!< [rad/s] time derivative of displacement in P0 frame + double massFSP; //!< [kg] mass of spherical pendulum pendulum + Eigen::Vector3d r_PcB_B; //!< [m] position vector form B to center of mass location of pendulum + Eigen::Matrix3d rTilde_PcB_B; //!< [m] tilde matrix of r_Pc_B + Eigen::Vector3d rPrime_PcB_B; //!< [m/s] Body time derivative of r_Pc_B + Eigen::Matrix3d rPrimeTilde_PcB_B; //!< [m/s] Tilde matrix of rPrime_PcB_B + + Eigen::Vector3d aPhi; //!< -- Term needed for back-sub method + Eigen::Vector3d bPhi; //!< -- Term needed for back-sub method + Eigen::Vector3d aTheta; //!< -- Term needed for back-sub method + Eigen::Vector3d bTheta; //!< -- Term needed for back-sub method + double cPhi; //!< -- Term needed for back-sub method + double cTheta; //!< -- Term needed for back-sub method + + Eigen::MatrixXd* g_N; //!< [m/s^2] Gravitational acceleration in N frame components + Eigen::Vector3d l_B; //!< [m] vector from the center of the tank to the spherical pendulum pendulum in B frame + Eigen::Vector3d lPrime_B; //!< [m/s] derivative of l respect to B frame + Eigen::Vector3d lPrime_P0; //!< [m/s] derivative of l in P0 frame + StateData* phiState; //!< -- state data for spherical pendulum displacement + StateData* thetaState; //!< -- state data for spherical pendulum displacement + StateData* phiDotState; //!< -- state data for time derivative of phi; + StateData* thetaDotState; //!< -- state data for time derivative of theta; + Eigen::Matrix3d dcm_B_P0; // Rotation matrix from P0 to B frame + static uint64_t effectorID; //!< [] ID number of this panel + + public: + SphericalPendulum(); //!< -- Contructor + ~SphericalPendulum(); //!< -- Destructor + void registerStates(DynParamManager& states); //!< -- Method for FSP to register its states + void linkInStates(DynParamManager& states); //!< -- Method for FSP to get access of other states + void updateEffectorMassProps(double integTime); //!< -- Method for FSP to add its contributions to mass props + void modifyStates(double integTime); //!<-- Method to force states modification during integration void retrieveMassValue(double integTime); - void updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N); //!< -- Back-sub contributions - void updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, Eigen::Vector3d omega_BN_B); //!< -- Energy and momentum calculations - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN); //!< -- Method for each stateEffector to calculate derivatives + void updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N); //!< -- Back-sub contributions + void updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B); //!< -- Energy and momentum calculations + void computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN); //!< -- Method for each stateEffector to calculate derivatives }; - #endif /* SPHERICAL_PENDULUM_H */ 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/spinningBodiesNDOF/spinningBodyNDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/spinningBodyNDOFStateEffector.cpp index 196ff5a2dd..49dc1eafb6 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++; @@ -40,16 +40,19 @@ uint64_t SpinningBodyNDOFStateEffector::effectorID = 1; SpinningBodyNDOFStateEffector::~SpinningBodyNDOFStateEffector() { - SpinningBodyNDOFStateEffector::effectorID --; /* reset the panel ID*/ + SpinningBodyNDOFStateEffector::effectorID--; /* reset the panel ID*/ } -void SpinningBodyNDOFStateEffector::Reset(uint64_t CurrentClock) +void +SpinningBodyNDOFStateEffector::Reset(uint64_t CurrentClock) { if (this->spinningBodyVec.back().mass <= 0.0) bskLogger.bskLog(BSK_ERROR, "The mass of the last element must be greater than 0."); } -void SpinningBody::setMass(double mass) { +void +SpinningBody::setMass(double mass) +{ if (mass >= 0.0) this->mass = mass; else { @@ -57,16 +60,19 @@ void SpinningBody::setMass(double mass) { } } -void SpinningBody::setSHat_S(Eigen::Vector3d sHat_S) { +void +SpinningBody::setSHat_S(Eigen::Vector3d sHat_S) +{ if (sHat_S.norm() > 0.01) { this->sHat_S = sHat_S.normalized(); - } - else { + } else { bskLogger.bskLog(BSK_ERROR, "Norm of sHat must be greater than 0."); } } -void SpinningBody::setK(double k) { +void +SpinningBody::setK(double k) +{ if (k >= 0.0) this->k = k; else { @@ -74,7 +80,9 @@ void SpinningBody::setK(double k) { } } -void SpinningBody::setC(double c) { +void +SpinningBody::setC(double c) +{ if (c >= 0.0) this->c = c; else { @@ -82,7 +90,9 @@ void SpinningBody::setC(double c) { } } -void SpinningBodyNDOFStateEffector::addSpinningBody(const SpinningBody& newBody) { +void +SpinningBodyNDOFStateEffector::addSpinningBody(const SpinningBody& newBody) +{ spinningBodyVec.push_back(newBody); this->numberOfDegreesOfFreedom++; @@ -90,18 +100,19 @@ void SpinningBodyNDOFStateEffector::addSpinningBody(const SpinningBody& newBody) this->spinningBodyOutMsgs.push_back(new Message); this->spinningBodyRefInMsgs.push_back(ReadFunctor()); - this->ATheta.conservativeResize(this->ATheta.rows()+1, 3); - this->BTheta.conservativeResize(this->BTheta.rows()+1, 3); - this->CTheta.conservativeResize(this->CTheta.rows()+1); + this->ATheta.conservativeResize(this->ATheta.rows() + 1, 3); + this->BTheta.conservativeResize(this->BTheta.rows() + 1, 3); + this->CTheta.conservativeResize(this->CTheta.rows() + 1); } -void SpinningBodyNDOFStateEffector::readInputMessages() +void +SpinningBodyNDOFStateEffector::readInputMessages() { if (this->motorTorqueInMsg.isLinked() && this->motorTorqueInMsg.isWritten()) { ArrayMotorTorqueMsgPayload incomingCmdBuffer; incomingCmdBuffer = this->motorTorqueInMsg(); int spinningBodyIndex = 0; - for(auto& spinningBody: this->spinningBodyVec) { + for (auto& spinningBody : this->spinningBodyVec) { spinningBody.u = incomingCmdBuffer.motorTorque[spinningBodyIndex]; spinningBodyIndex++; } @@ -111,15 +122,16 @@ void SpinningBodyNDOFStateEffector::readInputMessages() ArrayEffectorLockMsgPayload incomingLockBuffer; incomingLockBuffer = this->motorLockInMsg(); int spinningBodyIndex = 0; - for(auto& spinningBody: this->spinningBodyVec) { + for (auto& spinningBody : this->spinningBodyVec) { spinningBody.isAxisLocked = incomingLockBuffer.effectorLockFlag[spinningBodyIndex]; spinningBodyIndex++; } } int spinningBodyIndex = 0; - for(auto& spinningBody: this->spinningBodyVec) { - if (this->spinningBodyRefInMsgs[spinningBodyIndex].isLinked() && this->spinningBodyRefInMsgs[spinningBodyIndex].isWritten()) { + for (auto& spinningBody : this->spinningBodyVec) { + if (this->spinningBodyRefInMsgs[spinningBodyIndex].isLinked() && + this->spinningBodyRefInMsgs[spinningBodyIndex].isWritten()) { HingedRigidBodyMsgPayload incomingRefBuffer; incomingRefBuffer = this->spinningBodyRefInMsgs[spinningBodyIndex](); spinningBody.thetaRef = incomingRefBuffer.theta; @@ -129,10 +141,11 @@ void SpinningBodyNDOFStateEffector::readInputMessages() } } -void SpinningBodyNDOFStateEffector::writeOutputStateMessages(uint64_t CurrentClock) +void +SpinningBodyNDOFStateEffector::writeOutputStateMessages(uint64_t CurrentClock) { int spinningBodyIndex = 0; - for(auto& spinningBody: this->spinningBodyVec) { + for (auto& spinningBody : this->spinningBodyVec) { if (this->spinningBodyOutMsgs[spinningBodyIndex]->isLinked()) { HingedRigidBodyMsgPayload spinningBodyBuffer = this->spinningBodyOutMsgs[spinningBodyIndex]->zeroMsgPayload; @@ -155,36 +168,40 @@ void SpinningBodyNDOFStateEffector::writeOutputStateMessages(uint64_t CurrentClo } } -void SpinningBodyNDOFStateEffector::prependSpacecraftNameToStates() +void +SpinningBodyNDOFStateEffector::prependSpacecraftNameToStates() { this->nameOfThetaState = this->nameOfSpacecraftAttachedTo + this->nameOfThetaState; this->nameOfThetaDotState = this->nameOfSpacecraftAttachedTo + this->nameOfThetaDotState; } -void SpinningBodyNDOFStateEffector::linkInStates(DynParamManager& statesIn) +void +SpinningBodyNDOFStateEffector::linkInStates(DynParamManager& statesIn) { this->inertialPositionProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + "r_BN_N"); this->inertialVelocityProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + "v_BN_N"); } -void SpinningBodyNDOFStateEffector::registerStates(DynParamManager& states) +void +SpinningBodyNDOFStateEffector::registerStates(DynParamManager& states) { this->thetaState = states.registerState(numberOfDegreesOfFreedom, 1, this->nameOfThetaState); this->thetaDotState = states.registerState(numberOfDegreesOfFreedom, 1, this->nameOfThetaDotState); - Eigen::MatrixXd thetaInitMatrix(numberOfDegreesOfFreedom,1); - Eigen::MatrixXd thetaDotInitMatrix(numberOfDegreesOfFreedom,1); + Eigen::MatrixXd thetaInitMatrix(numberOfDegreesOfFreedom, 1); + Eigen::MatrixXd thetaDotInitMatrix(numberOfDegreesOfFreedom, 1); int i = 0; - for(const auto& spinningBody: this->spinningBodyVec) { - thetaInitMatrix(i,0) = spinningBody.thetaInit; - thetaDotInitMatrix(i,0) = spinningBody.thetaDotInit; + for (const auto& spinningBody : this->spinningBodyVec) { + thetaInitMatrix(i, 0) = spinningBody.thetaInit; + thetaDotInitMatrix(i, 0) = spinningBody.thetaDotInit; i++; } this->thetaState->setState(thetaInitMatrix); this->thetaDotState->setState(thetaDotInitMatrix); } -void SpinningBodyNDOFStateEffector::updateEffectorMassProps(double integTime) +void +SpinningBodyNDOFStateEffector::updateEffectorMassProps(double integTime) { this->effProps.mEff = 0.0; this->effProps.rEff_CB_B = Eigen::Vector3d::Zero(); @@ -193,7 +210,7 @@ void SpinningBodyNDOFStateEffector::updateEffectorMassProps(double integTime) this->effProps.IEffPrimePntB_B = Eigen::Matrix3d::Zero(); int spinningBodyIndex = 0; - for(auto& spinningBody: this->spinningBodyVec) { + for (auto& spinningBody : this->spinningBodyVec) { this->computeAttitudeProperties(spinningBody, spinningBodyIndex); this->computeAngularVelocityProperties(spinningBody, spinningBodyIndex); this->computePositionProperties(spinningBody, spinningBodyIndex); @@ -204,8 +221,11 @@ void SpinningBodyNDOFStateEffector::updateEffectorMassProps(double integTime) this->effProps.mEff += spinningBody.mass; this->effProps.rEff_CB_B += spinningBody.mass * spinningBody.r_ScB_B; this->effProps.rEffPrime_CB_B += spinningBody.mass * spinningBody.rPrime_ScB_B; - this->effProps.IEffPntB_B += spinningBody.ISPntSc_B - spinningBody.mass * spinningBody.rTilde_ScB_B * spinningBody.rTilde_ScB_B; - this->effProps.IEffPrimePntB_B += spinningBody.IPrimeSPntSc_B - spinningBody.mass * (rPrimeTilde_ScB_B * spinningBody.rTilde_ScB_B + spinningBody.rTilde_ScB_B * rPrimeTilde_ScB_B); + this->effProps.IEffPntB_B += + spinningBody.ISPntSc_B - spinningBody.mass * spinningBody.rTilde_ScB_B * spinningBody.rTilde_ScB_B; + this->effProps.IEffPrimePntB_B += + spinningBody.IPrimeSPntSc_B - spinningBody.mass * (rPrimeTilde_ScB_B * spinningBody.rTilde_ScB_B + + spinningBody.rTilde_ScB_B * rPrimeTilde_ScB_B); spinningBodyIndex++; } @@ -214,10 +234,10 @@ void SpinningBodyNDOFStateEffector::updateEffectorMassProps(double integTime) this->effProps.rEffPrime_CB_B /= this->effProps.mEff; } -void SpinningBodyNDOFStateEffector::computeAttitudeProperties(SpinningBody& spinningBody, int spinningBodyIndex) const +void +SpinningBodyNDOFStateEffector::computeAttitudeProperties(SpinningBody& spinningBody, int spinningBodyIndex) const { - if (spinningBody.isAxisLocked) - { + if (spinningBody.isAxisLocked) { auto thetaDotVector = this->thetaDotState->getState(); thetaDotVector(spinningBodyIndex) = 0.0; this->thetaDotState->setState(thetaDotVector); @@ -228,67 +248,74 @@ void SpinningBodyNDOFStateEffector::computeAttitudeProperties(SpinningBody& spin double dcm_S0S[3][3]; double prv_S0S_array[3]; - Eigen::Vector3d prv_S0S = - spinningBody.theta * spinningBody.sHat_S; + Eigen::Vector3d prv_S0S = -spinningBody.theta * spinningBody.sHat_S; eigenVector3d2CArray(prv_S0S, prv_S0S_array); PRV2C(prv_S0S_array, dcm_S0S); if (spinningBodyIndex == 0) { spinningBody.dcm_BS = spinningBody.dcm_S0P.transpose() * c2DArray2EigenMatrix3d(dcm_S0S); } else { - spinningBody.dcm_BS = this->spinningBodyVec[spinningBodyIndex-1].dcm_BS * spinningBody.dcm_S0P.transpose() * c2DArray2EigenMatrix3d(dcm_S0S); + spinningBody.dcm_BS = this->spinningBodyVec[spinningBodyIndex - 1].dcm_BS * spinningBody.dcm_S0P.transpose() * + c2DArray2EigenMatrix3d(dcm_S0S); } spinningBody.sHat_B = spinningBody.dcm_BS * spinningBody.sHat_S; } -void SpinningBodyNDOFStateEffector::computeAngularVelocityProperties(SpinningBody& spinningBody, int spinningBodyIndex) const +void +SpinningBodyNDOFStateEffector::computeAngularVelocityProperties(SpinningBody& spinningBody, int spinningBodyIndex) const { spinningBody.omega_SP_B = spinningBody.thetaDot * spinningBody.sHat_B; spinningBody.omegaTilde_SP_B = eigenTilde(spinningBody.omega_SP_B); if (spinningBodyIndex == 0) { spinningBody.omega_SB_B = spinningBody.omega_SP_B; } else { - spinningBody.omega_SB_B = spinningBody.omega_SP_B + this->spinningBodyVec[spinningBodyIndex-1].omega_SB_B; + spinningBody.omega_SB_B = spinningBody.omega_SP_B + this->spinningBodyVec[spinningBodyIndex - 1].omega_SB_B; } spinningBody.omegaTilde_SB_B = eigenTilde(spinningBody.omega_SB_B); } -void SpinningBodyNDOFStateEffector::computePositionProperties(SpinningBody& spinningBody, int spinningBodyIndex) const +void +SpinningBodyNDOFStateEffector::computePositionProperties(SpinningBody& spinningBody, int spinningBodyIndex) const { spinningBody.r_ScS_B = spinningBody.dcm_BS * spinningBody.r_ScS_S; if (spinningBodyIndex == 0) { spinningBody.r_SP_B = spinningBody.r_SP_P; spinningBody.r_SB_B = spinningBody.r_SP_P; } else { - spinningBody.r_SP_B = this->spinningBodyVec[spinningBodyIndex-1].dcm_BS * spinningBody.r_SP_P; - spinningBody.r_SB_B = spinningBody.r_SP_B + this->spinningBodyVec[spinningBodyIndex-1].r_SB_B; + spinningBody.r_SP_B = this->spinningBodyVec[spinningBodyIndex - 1].dcm_BS * spinningBody.r_SP_P; + spinningBody.r_SB_B = spinningBody.r_SP_B + this->spinningBodyVec[spinningBodyIndex - 1].r_SB_B; } spinningBody.r_ScB_B = spinningBody.r_ScS_B + spinningBody.r_SB_B; spinningBody.rTilde_ScB_B = eigenTilde(spinningBody.r_ScB_B); } -void SpinningBodyNDOFStateEffector::computeVelocityProperties(SpinningBody& spinningBody, int spinningBodyIndex) const +void +SpinningBodyNDOFStateEffector::computeVelocityProperties(SpinningBody& spinningBody, int spinningBodyIndex) const { spinningBody.rPrime_ScS_B = spinningBody.omegaTilde_SB_B * spinningBody.r_ScS_B; if (spinningBodyIndex == 0) { spinningBody.rPrime_SP_B = Eigen::Vector3d::Zero(); spinningBody.rPrime_SB_B = spinningBody.rPrime_SP_B; } else { - spinningBody.rPrime_SP_B = this->spinningBodyVec[spinningBodyIndex-1].omegaTilde_SB_B * spinningBody.r_SP_B; - spinningBody.rPrime_SB_B = spinningBody.rPrime_SP_B + this->spinningBodyVec[spinningBodyIndex-1].rPrime_SB_B; + spinningBody.rPrime_SP_B = this->spinningBodyVec[spinningBodyIndex - 1].omegaTilde_SB_B * spinningBody.r_SP_B; + spinningBody.rPrime_SB_B = spinningBody.rPrime_SP_B + this->spinningBodyVec[spinningBodyIndex - 1].rPrime_SB_B; } spinningBody.rPrime_ScB_B = spinningBody.rPrime_ScS_B + spinningBody.rPrime_SB_B; } -void SpinningBodyNDOFStateEffector::computeInertiaProperties(SpinningBody& spinningBody) const +void +SpinningBodyNDOFStateEffector::computeInertiaProperties(SpinningBody& spinningBody) const { spinningBody.ISPntSc_B = spinningBody.dcm_BS * spinningBody.ISPntSc_S * spinningBody.dcm_BS.transpose(); - spinningBody.IPrimeSPntSc_B = spinningBody.omegaTilde_SB_B * spinningBody.ISPntSc_B - spinningBody.ISPntSc_B * spinningBody.omegaTilde_SB_B; + spinningBody.IPrimeSPntSc_B = + spinningBody.omegaTilde_SB_B * spinningBody.ISPntSc_B - spinningBody.ISPntSc_B * spinningBody.omegaTilde_SB_B; } -void SpinningBodyNDOFStateEffector::updateContributions(double integTime, - BackSubMatrices& backSubContr, - Eigen::Vector3d sigma_BN, - Eigen::Vector3d omega_BN_B, - Eigen::Vector3d g_N) +void +SpinningBodyNDOFStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { this->sigma_BN = sigma_BN; this->dcm_BN = (this->sigma_BN.toRotationMatrix()).transpose(); @@ -313,49 +340,53 @@ void SpinningBodyNDOFStateEffector::updateContributions(double integTime, this->computeBackSubVectors(backSubContr); } -void SpinningBodyNDOFStateEffector::computeMTheta(Eigen::MatrixXd& MTheta) +void +SpinningBodyNDOFStateEffector::computeMTheta(Eigen::MatrixXd& MTheta) { - for (int n = 0; nnumberOfDegreesOfFreedom; n++) { + for (int n = 0; n < this->numberOfDegreesOfFreedom; n++) { this->spinningBodyVec[n].omega_SN_B = this->spinningBodyVec[n].omega_SB_B + this->omega_BN_B; - for (int i = 0; inumberOfDegreesOfFreedom; i++) { + for (int i = 0; i < this->numberOfDegreesOfFreedom; i++) { // Remove cross-coupling terms when axis is locked if ((this->spinningBodyVec[n].isAxisLocked || this->spinningBodyVec[i].isAxisLocked) && n != i) continue; - for (int j = (i<=n) ? n : i; jnumberOfDegreesOfFreedom; j++) { + for (int j = (i <= n) ? n : i; j < this->numberOfDegreesOfFreedom; j++) { Eigen::Vector3d r_ScjSn_B = this->spinningBodyVec[j].r_ScB_B - this->spinningBodyVec[n].r_SB_B; Eigen::Matrix3d rTilde_ScjSn_B = eigenTilde(r_ScjSn_B); Eigen::Vector3d r_ScjSi_B = this->spinningBodyVec[j].r_ScB_B - this->spinningBodyVec[i].r_SB_B; Eigen::Matrix3d rTilde_ScjSi_B = eigenTilde(r_ScjSi_B); - MTheta(n,i) += this->spinningBodyVec[n].sHat_B.transpose() - * (this->spinningBodyVec[j].ISPntSc_B - - this->spinningBodyVec[j].mass * rTilde_ScjSn_B * rTilde_ScjSi_B) - * this->spinningBodyVec[i].sHat_B; + MTheta(n, i) += this->spinningBodyVec[n].sHat_B.transpose() * + (this->spinningBodyVec[j].ISPntSc_B - + this->spinningBodyVec[j].mass * rTilde_ScjSn_B * rTilde_ScjSi_B) * + this->spinningBodyVec[i].sHat_B; } } } } -void SpinningBodyNDOFStateEffector::computeAThetaStar(Eigen::MatrixXd& AThetaStar) +void +SpinningBodyNDOFStateEffector::computeAThetaStar(Eigen::MatrixXd& AThetaStar) { - for (int n = 0; nnumberOfDegreesOfFreedom; n++) { + for (int n = 0; n < this->numberOfDegreesOfFreedom; n++) { if (this->spinningBodyVec[n].isAxisLocked) continue; - for (int i = n; inumberOfDegreesOfFreedom; i++) { + for (int i = n; i < this->numberOfDegreesOfFreedom; i++) { Eigen::Vector3d r_SciSn_B = this->spinningBodyVec[i].r_ScB_B - this->spinningBodyVec[n].r_SB_B; Eigen::Matrix3d rTilde_SciSn_B = eigenTilde(r_SciSn_B); - AThetaStar.row(n) -= this->spinningBodyVec[n].sHat_B.transpose() * this->spinningBodyVec[i].mass * rTilde_SciSn_B; + AThetaStar.row(n) -= + this->spinningBodyVec[n].sHat_B.transpose() * this->spinningBodyVec[i].mass * rTilde_SciSn_B; } } } -void SpinningBodyNDOFStateEffector::computeBThetaStar(Eigen::MatrixXd& BThetaStar) +void +SpinningBodyNDOFStateEffector::computeBThetaStar(Eigen::MatrixXd& BThetaStar) { - for (int n = 0; nnumberOfDegreesOfFreedom; n++) { + for (int n = 0; n < this->numberOfDegreesOfFreedom; n++) { if (this->spinningBodyVec[n].isAxisLocked) continue; @@ -364,115 +395,137 @@ void SpinningBodyNDOFStateEffector::computeBThetaStar(Eigen::MatrixXd& BThetaSta Eigen::Matrix3d rTilde_SciSn_B = eigenTilde(r_SciSn_B); Eigen::Matrix3d rTilde_SciB_B = eigenTilde(this->spinningBodyVec[i].r_ScB_B); - BThetaStar.row(n) -= this->spinningBodyVec[n].sHat_B.transpose() * (this->spinningBodyVec[i].ISPntSc_B - - this->spinningBodyVec[i].mass * - rTilde_SciSn_B * rTilde_SciB_B); + BThetaStar.row(n) -= + this->spinningBodyVec[n].sHat_B.transpose() * + (this->spinningBodyVec[i].ISPntSc_B - this->spinningBodyVec[i].mass * rTilde_SciSn_B * rTilde_SciB_B); } } } -void SpinningBodyNDOFStateEffector::computeCThetaStar(Eigen::VectorXd& CThetaStar, - const Eigen::Vector3d& g_N) +void +SpinningBodyNDOFStateEffector::computeCThetaStar(Eigen::VectorXd& CThetaStar, const Eigen::Vector3d& g_N) { Eigen::Vector3d g_B = this->dcm_BN * g_N; - for (int n = 0; nnumberOfDegreesOfFreedom; n++) { + for (int n = 0; n < this->numberOfDegreesOfFreedom; n++) { if (this->spinningBodyVec[n].isAxisLocked) continue; - CThetaStar(n) += this->spinningBodyVec[n].u - - this->spinningBodyVec[n].k * (this->spinningBodyVec[n].theta - this->spinningBodyVec[n].thetaRef) - - this->spinningBodyVec[n].c * (this->spinningBodyVec[n].thetaDot - this->spinningBodyVec[n].thetaDotRef); + CThetaStar(n) += + this->spinningBodyVec[n].u - + this->spinningBodyVec[n].k * (this->spinningBodyVec[n].theta - this->spinningBodyVec[n].thetaRef) - + this->spinningBodyVec[n].c * (this->spinningBodyVec[n].thetaDot - this->spinningBodyVec[n].thetaDotRef); - for (int i = n; inumberOfDegreesOfFreedom; i++) { + for (int i = n; i < this->numberOfDegreesOfFreedom; i++) { Eigen::Vector3d r_SciSn_B = this->spinningBodyVec[i].r_ScB_B - this->spinningBodyVec[n].r_SB_B; Eigen::Matrix3d rTilde_SciSn_B = eigenTilde(r_SciSn_B); Eigen::Matrix3d omegaTilde_SiN_B = eigenTilde(this->spinningBodyVec[i].omega_SN_B); - CThetaStar(n) -= this->spinningBodyVec[n].sHat_B.transpose() * ( - omegaTilde_SiN_B * this->spinningBodyVec[i].ISPntSc_B * this->spinningBodyVec[i].omega_SN_B - - this->spinningBodyVec[i].ISPntSc_B * this->spinningBodyVec[i].omegaTilde_SB_B * this->omega_BN_B - + this->spinningBodyVec[i].mass * rTilde_SciSn_B * ( - - g_B - + this->omegaTilde_BN_B * this->omegaTilde_BN_B * this->spinningBodyVec[i].r_ScB_B - + 2 * this->omegaTilde_BN_B * this->spinningBodyVec[i].rPrime_ScB_B - + this->spinningBodyVec[i].omegaTilde_SP_B * this->spinningBodyVec[i].rPrime_ScS_B)); - - for(int j=0; j<=i-1; j++) { - Eigen::Vector3d omega_SiSj_B = this->spinningBodyVec[i].omega_SB_B - this->spinningBodyVec[j].omega_SB_B; + CThetaStar(n) -= + this->spinningBodyVec[n].sHat_B.transpose() * + (omegaTilde_SiN_B * this->spinningBodyVec[i].ISPntSc_B * this->spinningBodyVec[i].omega_SN_B - + this->spinningBodyVec[i].ISPntSc_B * this->spinningBodyVec[i].omegaTilde_SB_B * this->omega_BN_B + + this->spinningBodyVec[i].mass * rTilde_SciSn_B * + (-g_B + this->omegaTilde_BN_B * this->omegaTilde_BN_B * this->spinningBodyVec[i].r_ScB_B + + 2 * this->omegaTilde_BN_B * this->spinningBodyVec[i].rPrime_ScB_B + + this->spinningBodyVec[i].omegaTilde_SP_B * this->spinningBodyVec[i].rPrime_ScS_B)); + + for (int j = 0; j <= i - 1; j++) { + Eigen::Vector3d omega_SiSj_B = + this->spinningBodyVec[i].omega_SB_B - this->spinningBodyVec[j].omega_SB_B; Eigen::Matrix3d omegaTilde_SiSj_B = eigenTilde(omega_SiSj_B); - Eigen::Vector3d r_SciSj1 = this->spinningBodyVec[i].r_ScB_B - this->spinningBodyVec[j+1].r_SB_B; + Eigen::Vector3d r_SciSj1 = this->spinningBodyVec[i].r_ScB_B - this->spinningBodyVec[j + 1].r_SB_B; Eigen::Matrix3d rTilde_SciSj1 = eigenTilde(r_SciSj1); - Eigen::Vector3d rPrime_SciSj_B = this->spinningBodyVec[i].rPrime_ScB_B - this->spinningBodyVec[j].rPrime_SB_B; - - CThetaStar(n) -= this->spinningBodyVec[n].sHat_B.transpose() * ( - - this->spinningBodyVec[i].ISPntSc_B * omegaTilde_SiSj_B * this->spinningBodyVec[j].omega_SP_B - + this->spinningBodyVec[i].mass * rTilde_SciSn_B * ( - this->spinningBodyVec[j].omegaTilde_SP_B * rPrime_SciSj_B - - rTilde_SciSj1 * this->spinningBodyVec[j].omegaTilde_SB_B * this->spinningBodyVec[j+1].omega_SP_B)); + Eigen::Vector3d rPrime_SciSj_B = + this->spinningBodyVec[i].rPrime_ScB_B - this->spinningBodyVec[j].rPrime_SB_B; + + CThetaStar(n) -= + this->spinningBodyVec[n].sHat_B.transpose() * + (-this->spinningBodyVec[i].ISPntSc_B * omegaTilde_SiSj_B * this->spinningBodyVec[j].omega_SP_B + + this->spinningBodyVec[i].mass * rTilde_SciSn_B * + (this->spinningBodyVec[j].omegaTilde_SP_B * rPrime_SciSj_B - + rTilde_SciSj1 * this->spinningBodyVec[j].omegaTilde_SB_B * + this->spinningBodyVec[j + 1].omega_SP_B)); } } } } -void SpinningBodyNDOFStateEffector::computeBackSubMatrices(BackSubMatrices& backSubContr) const +void +SpinningBodyNDOFStateEffector::computeBackSubMatrices(BackSubMatrices& backSubContr) const { - for (int i = 0; inumberOfDegreesOfFreedom; i++) { + for (int i = 0; i < this->numberOfDegreesOfFreedom; i++) { for (int j = i; j < this->numberOfDegreesOfFreedom; j++) { Eigen::Vector3d r_ScjSi = this->spinningBodyVec[j].r_ScB_B - this->spinningBodyVec[i].r_SB_B; Eigen::Matrix3d rTilde_ScjSi = eigenTilde(r_ScjSi); Eigen::Matrix3d rTilde_ScjB = eigenTilde(this->spinningBodyVec[j].r_ScB_B); - backSubContr.matrixA -= this->spinningBodyVec[j].mass * rTilde_ScjSi * this->spinningBodyVec[i].sHat_B * this->ATheta.row(i); - backSubContr.matrixB -= this->spinningBodyVec[j].mass * rTilde_ScjSi * this->spinningBodyVec[i].sHat_B * this->BTheta.row(i); - backSubContr.matrixC += (this->spinningBodyVec[j].ISPntSc_B - - this->spinningBodyVec[j].mass * rTilde_ScjB * rTilde_ScjSi) - * this->spinningBodyVec[i].sHat_B * this->ATheta.row(i); - backSubContr.matrixD += (this->spinningBodyVec[j].ISPntSc_B - - this->spinningBodyVec[j].mass * rTilde_ScjB * rTilde_ScjSi) - * this->spinningBodyVec[i].sHat_B * this->BTheta.row(i); + backSubContr.matrixA -= + this->spinningBodyVec[j].mass * rTilde_ScjSi * this->spinningBodyVec[i].sHat_B * this->ATheta.row(i); + backSubContr.matrixB -= + this->spinningBodyVec[j].mass * rTilde_ScjSi * this->spinningBodyVec[i].sHat_B * this->BTheta.row(i); + backSubContr.matrixC += + (this->spinningBodyVec[j].ISPntSc_B - this->spinningBodyVec[j].mass * rTilde_ScjB * rTilde_ScjSi) * + this->spinningBodyVec[i].sHat_B * this->ATheta.row(i); + backSubContr.matrixD += + (this->spinningBodyVec[j].ISPntSc_B - this->spinningBodyVec[j].mass * rTilde_ScjB * rTilde_ScjSi) * + this->spinningBodyVec[i].sHat_B * this->BTheta.row(i); } } } -void SpinningBodyNDOFStateEffector::computeBackSubVectors(BackSubMatrices &backSubContr) const +void +SpinningBodyNDOFStateEffector::computeBackSubVectors(BackSubMatrices& backSubContr) const { - for (int i = 0; inumberOfDegreesOfFreedom; i++) { + for (int i = 0; i < this->numberOfDegreesOfFreedom; i++) { Eigen::Matrix3d omegaTilde_SiN_B = eigenTilde(this->spinningBodyVec[i].omega_SN_B); - backSubContr.vecRot -= omegaTilde_SiN_B * this->spinningBodyVec[i].ISPntSc_B * this->spinningBodyVec[i].omega_SB_B - + this->spinningBodyVec[i].mass * this->omegaTilde_BN_B * this->spinningBodyVec[i].rTilde_ScB_B * this->spinningBodyVec[i].rPrime_ScB_B; + backSubContr.vecRot -= + omegaTilde_SiN_B * this->spinningBodyVec[i].ISPntSc_B * this->spinningBodyVec[i].omega_SB_B + + this->spinningBodyVec[i].mass * this->omegaTilde_BN_B * this->spinningBodyVec[i].rTilde_ScB_B * + this->spinningBodyVec[i].rPrime_ScB_B; - for(int j=0; j<=i-1; j++) { + for (int j = 0; j <= i - 1; j++) { Eigen::Vector3d omega_SiSj_B = this->spinningBodyVec[i].omega_SB_B - this->spinningBodyVec[j].omega_SB_B; Eigen::Matrix3d omegaTilde_SiSj_B = eigenTilde(omega_SiSj_B); - Eigen::Vector3d r_SciSj1 = this->spinningBodyVec[i].r_ScB_B - this->spinningBodyVec[j+1].r_SB_B; + Eigen::Vector3d r_SciSj1 = this->spinningBodyVec[i].r_ScB_B - this->spinningBodyVec[j + 1].r_SB_B; Eigen::Matrix3d rTilde_SciSj1 = eigenTilde(r_SciSj1); - Eigen::Vector3d rPrime_SciSj_B = this->spinningBodyVec[i].rPrime_ScB_B - this->spinningBodyVec[j].rPrime_SB_B; - - backSubContr.vecTrans -= this->spinningBodyVec[i].mass * (this->spinningBodyVec[j].omegaTilde_SP_B * rPrime_SciSj_B - - rTilde_SciSj1 * this->spinningBodyVec[j].omegaTilde_SB_B * this->spinningBodyVec[j+1].omega_SP_B); - backSubContr.vecRot -= - this->spinningBodyVec[i].ISPntSc_B * omegaTilde_SiSj_B * this->spinningBodyVec[j].omega_SP_B - + this->spinningBodyVec[i].mass * this->spinningBodyVec[i].rTilde_ScB_B * ( - this->spinningBodyVec[j].omegaTilde_SP_B * rPrime_SciSj_B - - rTilde_SciSj1 * this->spinningBodyVec[j].omegaTilde_SB_B * this->spinningBodyVec[j+1].omega_SP_B); + Eigen::Vector3d rPrime_SciSj_B = + this->spinningBodyVec[i].rPrime_ScB_B - this->spinningBodyVec[j].rPrime_SB_B; + + backSubContr.vecTrans -= + this->spinningBodyVec[i].mass * + (this->spinningBodyVec[j].omegaTilde_SP_B * rPrime_SciSj_B - + rTilde_SciSj1 * this->spinningBodyVec[j].omegaTilde_SB_B * this->spinningBodyVec[j + 1].omega_SP_B); + backSubContr.vecRot -= + -this->spinningBodyVec[i].ISPntSc_B * omegaTilde_SiSj_B * this->spinningBodyVec[j].omega_SP_B + + this->spinningBodyVec[i].mass * this->spinningBodyVec[i].rTilde_ScB_B * + (this->spinningBodyVec[j].omegaTilde_SP_B * rPrime_SciSj_B - + rTilde_SciSj1 * this->spinningBodyVec[j].omegaTilde_SB_B * this->spinningBodyVec[j + 1].omega_SP_B); } - backSubContr.vecTrans -= this->spinningBodyVec[i].mass * this->spinningBodyVec[i].omegaTilde_SP_B * this->spinningBodyVec[i].rPrime_ScS_B; - backSubContr.vecRot -= this->spinningBodyVec[i].mass * this->spinningBodyVec[i].rTilde_ScB_B * this->spinningBodyVec[i].omegaTilde_SP_B * this->spinningBodyVec[i].rPrime_ScS_B; + backSubContr.vecTrans -= this->spinningBodyVec[i].mass * this->spinningBodyVec[i].omegaTilde_SP_B * + this->spinningBodyVec[i].rPrime_ScS_B; + backSubContr.vecRot -= this->spinningBodyVec[i].mass * this->spinningBodyVec[i].rTilde_ScB_B * + this->spinningBodyVec[i].omegaTilde_SP_B * this->spinningBodyVec[i].rPrime_ScS_B; for (int j = i; j < this->numberOfDegreesOfFreedom; j++) { Eigen::Vector3d r_ScjSi = this->spinningBodyVec[j].r_ScB_B - this->spinningBodyVec[i].r_SB_B; Eigen::Matrix3d rTilde_ScjSi = eigenTilde(r_ScjSi); Eigen::Matrix3d rTilde_ScjB = eigenTilde(this->spinningBodyVec[j].r_ScB_B); - backSubContr.vecTrans += this->spinningBodyVec[j].mass * rTilde_ScjSi * this->spinningBodyVec[i].sHat_B * this->CTheta.row(i); - backSubContr.vecRot -= (this->spinningBodyVec[j].ISPntSc_B - - this->spinningBodyVec[j].mass * rTilde_ScjB * rTilde_ScjSi) - * this->spinningBodyVec[i].sHat_B * this->CTheta.row(i); + backSubContr.vecTrans += + this->spinningBodyVec[j].mass * rTilde_ScjSi * this->spinningBodyVec[i].sHat_B * this->CTheta.row(i); + backSubContr.vecRot -= + (this->spinningBodyVec[j].ISPntSc_B - this->spinningBodyVec[j].mass * rTilde_ScjB * rTilde_ScjSi) * + this->spinningBodyVec[i].sHat_B * this->CTheta.row(i); } } } -void SpinningBodyNDOFStateEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +SpinningBodyNDOFStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { Eigen::Vector3d rDDotLocal_BN_B = this->dcm_BN * rDDot_BN_N; @@ -481,7 +534,11 @@ void SpinningBodyNDOFStateEffector::computeDerivatives(double integTime, Eigen:: this->thetaDotState->setDerivative(thetaDDot); } -void SpinningBodyNDOFStateEffector::updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, double & rotEnergyContr, Eigen::Vector3d omega_BN_B) +void +SpinningBodyNDOFStateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { this->omega_BN_B = omega_BN_B; this->omegaTilde_BN_B = eigenTilde(this->omega_BN_B); @@ -489,32 +546,38 @@ void SpinningBodyNDOFStateEffector::updateEnergyMomContributions(double integTim rotAngMomPntCContr_B = Eigen::Vector3d::Zero(); rotEnergyContr = 0.0; - for(auto& spinningBody: this->spinningBodyVec) { + for (auto& spinningBody : this->spinningBodyVec) { spinningBody.omega_SN_B = spinningBody.omega_SB_B + this->omega_BN_B; spinningBody.rDot_ScB_B = spinningBody.rPrime_ScB_B + this->omegaTilde_BN_B * spinningBody.r_ScB_B; - rotAngMomPntCContr_B += spinningBody.ISPntSc_B * spinningBody.omega_SN_B + spinningBody.mass * spinningBody.rTilde_ScB_B * spinningBody.rDot_ScB_B; - rotEnergyContr += 1.0 / 2.0 * spinningBody.omega_SN_B.dot(spinningBody.ISPntSc_B * spinningBody.omega_SN_B) - + 1.0 / 2.0 * spinningBody.mass * spinningBody.rDot_ScB_B.dot(spinningBody.rDot_ScB_B) - + 1.0 / 2.0 * spinningBody.k * (spinningBody.theta - spinningBody.thetaRef) * (spinningBody.theta - spinningBody.thetaRef); + rotAngMomPntCContr_B += spinningBody.ISPntSc_B * spinningBody.omega_SN_B + + spinningBody.mass * spinningBody.rTilde_ScB_B * spinningBody.rDot_ScB_B; + rotEnergyContr += 1.0 / 2.0 * spinningBody.omega_SN_B.dot(spinningBody.ISPntSc_B * spinningBody.omega_SN_B) + + 1.0 / 2.0 * spinningBody.mass * spinningBody.rDot_ScB_B.dot(spinningBody.rDot_ScB_B) + + 1.0 / 2.0 * spinningBody.k * (spinningBody.theta - spinningBody.thetaRef) * + (spinningBody.theta - spinningBody.thetaRef); } } -void SpinningBodyNDOFStateEffector::computeSpinningBodyInertialStates() +void +SpinningBodyNDOFStateEffector::computeSpinningBodyInertialStates() { - for(auto& spinningBody: this->spinningBodyVec) { + for (auto& spinningBody : this->spinningBodyVec) { // Compute the rotational properties Eigen::Matrix3d dcm_SN = spinningBody.dcm_BS.transpose() * this->dcm_BN; spinningBody.sigma_SN = eigenMRPd2Vector3d(eigenC2MRP(dcm_SN)); spinningBody.omega_SN_S = spinningBody.dcm_BS.transpose() * spinningBody.omega_SN_B; // Compute the translation properties - spinningBody.r_ScN_N = (Eigen::Vector3d)*this->inertialPositionProperty + this->dcm_BN.transpose() * spinningBody.r_ScB_B; - spinningBody.v_ScN_N = (Eigen::Vector3d)*this->inertialVelocityProperty + this->dcm_BN.transpose() * spinningBody.rDot_ScB_B; + spinningBody.r_ScN_N = + (Eigen::Vector3d) * this->inertialPositionProperty + this->dcm_BN.transpose() * spinningBody.r_ScB_B; + spinningBody.v_ScN_N = + (Eigen::Vector3d) * this->inertialVelocityProperty + this->dcm_BN.transpose() * spinningBody.rDot_ScB_B; } } -void SpinningBodyNDOFStateEffector::UpdateState(uint64_t CurrentSimNanos) +void +SpinningBodyNDOFStateEffector::UpdateState(uint64_t CurrentSimNanos) { this->readInputMessages(); this->computeSpinningBodyInertialStates(); diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/spinningBodyNDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/spinningBodyNDOFStateEffector.h index cb122a732e..7f3d73bbe2 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/spinningBodyNDOFStateEffector.h +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesNDOF/spinningBodyNDOFStateEffector.h @@ -35,125 +35,154 @@ #include "architecture/utilities/bskLogging.h" /*! Struct containing all the spinning bodies variables. */ -struct SpinningBody { -public: +struct SpinningBody +{ + public: /** setter for `mass` property */ void setMass(double mass); /** setter for `r_SP_P` property */ - void setR_SP_P(Eigen::Vector3d r_SP_P) {this->r_SP_P = r_SP_P;}; + void setR_SP_P(Eigen::Vector3d r_SP_P) { this->r_SP_P = r_SP_P; }; /** setter for `r_ScS_S` property */ - void setR_ScS_S(Eigen::Vector3d r_ScS_S) {this->r_ScS_S = r_ScS_S;}; + void setR_ScS_S(Eigen::Vector3d r_ScS_S) { this->r_ScS_S = r_ScS_S; }; /** setter for `ISPntSc_S` property */ - void setISPntSc_S(const Eigen::Matrix3d& ISPntSc_S) {this->ISPntSc_S = ISPntSc_S;}; + void setISPntSc_S(const Eigen::Matrix3d& ISPntSc_S) { this->ISPntSc_S = ISPntSc_S; }; /** setter for `sHat_S` property */ void setSHat_S(Eigen::Vector3d sHat_S); /** setter for `dcm_S0P` property */ - void setDCM_S0P(const Eigen::Matrix3d& dcm_S0P) {this->dcm_S0P = dcm_S0P;}; + void setDCM_S0P(const Eigen::Matrix3d& dcm_S0P) { this->dcm_S0P = dcm_S0P; }; /** setter for `k` property */ void setK(double k); /** setter for `c` property */ void setC(double c); /** setter for `thetaInit` property */ - void setThetaInit(double thetaInit) {this->thetaInit = thetaInit;}; + void setThetaInit(double thetaInit) { this->thetaInit = thetaInit; }; /** setter for `thetaDotInit` property */ - void setThetaDotInit(double thetaDotInit) {this->thetaDotInit = thetaDotInit;}; + void setThetaDotInit(double thetaDotInit) { this->thetaDotInit = thetaDotInit; }; /** getter for `mass` property */ - double getMass() const {return this->mass;}; + double getMass() const { return this->mass; }; /** getter for `r_SP_P` property */ - Eigen::Vector3d getR_SP_P() const {return this->r_SP_P;}; + Eigen::Vector3d getR_SP_P() const { return this->r_SP_P; }; /** getter for `r_ScS_S` property */ - Eigen::Vector3d getR_ScS_S() const {return this->r_ScS_S;}; + Eigen::Vector3d getR_ScS_S() const { return this->r_ScS_S; }; /** getter for `ISPntSc_S` property */ - Eigen::Matrix3d setISPntSc_S() const {return this->ISPntSc_S;}; + Eigen::Matrix3d setISPntSc_S() const { return this->ISPntSc_S; }; /** getter for `sHat_S` property */ - Eigen::Vector3d getSHat_S() const {return this->sHat_S;}; + Eigen::Vector3d getSHat_S() const { return this->sHat_S; }; /** getter for `dcm_S0P` property */ - Eigen::Matrix3d getDCM_S0P() const {return this->dcm_S0P;}; + Eigen::Matrix3d getDCM_S0P() const { return this->dcm_S0P; }; /** getter for `k` property */ - double getK() const {return this->k;}; + double getK() const { return this->k; }; /** getter for `c` property */ - double getC() const {return this->c;}; + double getC() const { return this->c; }; /** getter for `thetaInit` property */ - double getThetaInit() const {return this->thetaInit;}; + double getThetaInit() const { return this->thetaInit; }; /** getter for `thetaDotInit` property */ - double getThetaDotInit() const {return this->thetaDotInit;}; + double getThetaDotInit() const { return this->thetaDotInit; }; -private: + private: // Make sure the effector can access each spinning body's private and protected members friend class SpinningBodyNDOFStateEffector; - double thetaInit = 0.0; //!< [rad] initial spinning body angle - double thetaDotInit = 0.0; //!< [rad/s] initial spinning body angle rate - double k = 0.0; //!< [N-m/rad] torsional spring constant - double c = 0.0; //!< [N-m-s/rad] rotational damping coefficient - double mass = 1.0; //!< [kg] spinning body mass - Eigen::Vector3d r_SP_P = Eigen::Vector3d::Zero(); //!< [m] vector pointing from parent frame P origin to spinning frame S origin in P frame components - Eigen::Vector3d r_ScS_S = Eigen::Vector3d::Zero(); //!< [m] vector pointing from spinning frame S origin to point Sc (center of mass of the spinner) in S frame components - Eigen::Vector3d sHat_S = {1.0, 0.0, 0.0}; //!< spinning axis in S frame components - Eigen::Matrix3d dcm_S0P = Eigen::Matrix3d::Identity(); //!< DCM from the parent frame to the S0 frame (S frame for theta=0) - Eigen::Matrix3d ISPntSc_S = Eigen::Matrix3d::Identity(); //!< [kg-m^2] Inertia of spinning body about point Sc in S frame components - - double theta = 0.0; //!< [rad] current spinning body angle - double thetaDot = 0.0; //!< [rad/s] current spinning body angle rate - double thetaRef = 0.0; //!< [rad] reference spinning body angle - double thetaDotRef = 0.0; //!< [rad/s] reference spinning body angle rate - double u = 0.0; //!< [N-m] initial spinning body angle - bool isAxisLocked = false; //!< axis lock flag - - Eigen::Vector3d sHat_B = {1.0, 0.0, 0.0}; //!< spinning axis in B frame components - Eigen::Vector3d r_SP_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from parent frame P origin to spinning frame S origin in B frame components - Eigen::Vector3d r_SB_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from body frame B origin to spinning frame S origin in B frame components - Eigen::Vector3d r_ScS_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from spinning frame S origin to point Sc (center of mass of the spinner) in B frame components - Eigen::Vector3d r_ScB_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from body frame B origin to point Sc (center of mass of the spinner) in B frame components - Eigen::Vector3d rPrime_SP_B = Eigen::Vector3d::Zero(); //!< [m/s] body frame time derivative of r_SP_B in B frame components - Eigen::Vector3d rPrime_SB_B = Eigen::Vector3d::Zero(); //!< [m/s] body frame time derivative of r_SB_B in B frame components - Eigen::Vector3d rPrime_ScS_B = Eigen::Vector3d::Zero(); //!< [m/s] body frame time derivative of r_ScS_B in B frame components - Eigen::Vector3d rPrime_ScB_B = Eigen::Vector3d::Zero(); //!< [m/s] body frame time derivative of r_ScB_B in B frame components - Eigen::Vector3d rDot_ScB_B = Eigen::Vector3d::Zero(); //!< [m/s] inertial time derivative of r_ScB_B in B frame components - Eigen::Vector3d omega_SP_B = Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity of the S frame wrt the P frame in B frame components - Eigen::Vector3d omega_SB_B = Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity of the S frame wrt the B frame in B frame components - Eigen::Vector3d omega_SN_B = Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity of the S frame wrt the N frame in B frame components - - Eigen::Matrix3d ISPntSc_B = Eigen::Matrix3d::Identity(); //!< [kg-m^2] inertia of spinning body about point Sc in S frame components - Eigen::Matrix3d IPrimeSPntSc_B = Eigen::Matrix3d::Zero(); //!< [kg-m^2] body frame derivative of the inertia of spinning body about point Sc in S frame components - Eigen::Matrix3d dcm_BS = Eigen::Matrix3d::Identity(); //!< DCM from spinner frame to body frame - Eigen::Matrix3d rTilde_ScB_B = Eigen::Matrix3d::Zero(); //!< [m] tilde matrix of r_ScB_B - Eigen::Matrix3d omegaTilde_SP_B = Eigen::Matrix3d::Zero(); //!< [rad/s] tilde matrix of omega_SP_B - Eigen::Matrix3d omegaTilde_SB_B = Eigen::Matrix3d::Zero(); //!< [rad/s] tilde matrix of omega_SB_B - - Eigen::Vector3d r_ScN_N = Eigen::Vector3d::Zero(); //!< [m] position vector of the spinning body center of mass Sc relative to the inertial frame origin N - Eigen::Vector3d v_ScN_N = Eigen::Vector3d::Zero(); //!< [m/s] inertial velocity vector of Sc relative to inertial frame - Eigen::Vector3d sigma_SN = Eigen::Vector3d::Zero(); //!< MRP attitude of frame S relative to inertial frame - Eigen::Vector3d omega_SN_S = Eigen::Vector3d::Zero(); //!< [rad/s] inertial spinning body frame angular velocity vector + double thetaInit = 0.0; //!< [rad] initial spinning body angle + double thetaDotInit = 0.0; //!< [rad/s] initial spinning body angle rate + double k = 0.0; //!< [N-m/rad] torsional spring constant + double c = 0.0; //!< [N-m-s/rad] rotational damping coefficient + double mass = 1.0; //!< [kg] spinning body mass + Eigen::Vector3d r_SP_P = Eigen::Vector3d::Zero(); //!< [m] vector pointing from parent frame P origin to spinning + //!< frame S origin in P frame components + Eigen::Vector3d r_ScS_S = Eigen::Vector3d::Zero(); //!< [m] vector pointing from spinning frame S origin to point Sc + //!< (center of mass of the spinner) in S frame components + Eigen::Vector3d sHat_S = { 1.0, 0.0, 0.0 }; //!< spinning axis in S frame components + Eigen::Matrix3d dcm_S0P = + Eigen::Matrix3d::Identity(); //!< DCM from the parent frame to the S0 frame (S frame for theta=0) + Eigen::Matrix3d ISPntSc_S = + Eigen::Matrix3d::Identity(); //!< [kg-m^2] Inertia of spinning body about point Sc in S frame components + + double theta = 0.0; //!< [rad] current spinning body angle + double thetaDot = 0.0; //!< [rad/s] current spinning body angle rate + double thetaRef = 0.0; //!< [rad] reference spinning body angle + double thetaDotRef = 0.0; //!< [rad/s] reference spinning body angle rate + double u = 0.0; //!< [N-m] initial spinning body angle + bool isAxisLocked = false; //!< axis lock flag + + Eigen::Vector3d sHat_B = { 1.0, 0.0, 0.0 }; //!< spinning axis in B frame components + Eigen::Vector3d r_SP_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from parent frame P origin to spinning + //!< frame S origin in B frame components + Eigen::Vector3d r_SB_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from body frame B origin to spinning + //!< frame S origin in B frame components + Eigen::Vector3d r_ScS_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from spinning frame S origin to point Sc + //!< (center of mass of the spinner) in B frame components + Eigen::Vector3d r_ScB_B = Eigen::Vector3d::Zero(); //!< [m] vector pointing from body frame B origin to point Sc + //!< (center of mass of the spinner) in B frame components + Eigen::Vector3d rPrime_SP_B = + Eigen::Vector3d::Zero(); //!< [m/s] body frame time derivative of r_SP_B in B frame components + Eigen::Vector3d rPrime_SB_B = + Eigen::Vector3d::Zero(); //!< [m/s] body frame time derivative of r_SB_B in B frame components + Eigen::Vector3d rPrime_ScS_B = + Eigen::Vector3d::Zero(); //!< [m/s] body frame time derivative of r_ScS_B in B frame components + Eigen::Vector3d rPrime_ScB_B = + Eigen::Vector3d::Zero(); //!< [m/s] body frame time derivative of r_ScB_B in B frame components + Eigen::Vector3d rDot_ScB_B = + Eigen::Vector3d::Zero(); //!< [m/s] inertial time derivative of r_ScB_B in B frame components + Eigen::Vector3d omega_SP_B = + Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity of the S frame wrt the P frame in B frame components + Eigen::Vector3d omega_SB_B = + Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity of the S frame wrt the B frame in B frame components + Eigen::Vector3d omega_SN_B = + Eigen::Vector3d::Zero(); //!< [rad/s] angular velocity of the S frame wrt the N frame in B frame components + + Eigen::Matrix3d ISPntSc_B = + Eigen::Matrix3d::Identity(); //!< [kg-m^2] inertia of spinning body about point Sc in S frame components + Eigen::Matrix3d IPrimeSPntSc_B = Eigen::Matrix3d::Zero(); //!< [kg-m^2] body frame derivative of the inertia of + //!< spinning body about point Sc in S frame components + Eigen::Matrix3d dcm_BS = Eigen::Matrix3d::Identity(); //!< DCM from spinner frame to body frame + Eigen::Matrix3d rTilde_ScB_B = Eigen::Matrix3d::Zero(); //!< [m] tilde matrix of r_ScB_B + Eigen::Matrix3d omegaTilde_SP_B = Eigen::Matrix3d::Zero(); //!< [rad/s] tilde matrix of omega_SP_B + Eigen::Matrix3d omegaTilde_SB_B = Eigen::Matrix3d::Zero(); //!< [rad/s] tilde matrix of omega_SB_B + + Eigen::Vector3d r_ScN_N = Eigen::Vector3d::Zero(); //!< [m] position vector of the spinning body center of mass Sc + //!< relative to the inertial frame origin N + Eigen::Vector3d v_ScN_N = + Eigen::Vector3d::Zero(); //!< [m/s] inertial velocity vector of Sc relative to inertial frame + Eigen::Vector3d sigma_SN = Eigen::Vector3d::Zero(); //!< MRP attitude of frame S relative to inertial frame + Eigen::Vector3d omega_SN_S = + Eigen::Vector3d::Zero(); //!< [rad/s] inertial spinning body frame angular velocity vector BSKLogger bskLogger; }; /*! spinning rigid body state effector class */ -class SpinningBodyNDOFStateEffector: public StateEffector, public SysModel { -public: - std::vector*> spinningBodyOutMsgs; //!< state output message - std::vector*> spinningBodyConfigLogOutMsgs; //!< spinning body state config log message - ReadFunctor motorTorqueInMsg; //!< (optional) motor torque input message - ReadFunctor motorLockInMsg; //!< (optional) lock flag input message - std::vector> spinningBodyRefInMsgs; //!< (optional) reference state input message - - SpinningBodyNDOFStateEffector(); //!< Constructor - ~SpinningBodyNDOFStateEffector() override; //!< Destructor +class SpinningBodyNDOFStateEffector + : public StateEffector + , public SysModel +{ + public: + std::vector*> spinningBodyOutMsgs; //!< state output message + std::vector*> spinningBodyConfigLogOutMsgs; //!< spinning body state config log message + ReadFunctor motorTorqueInMsg; //!< (optional) motor torque input message + ReadFunctor motorLockInMsg; //!< (optional) lock flag input message + std::vector> + spinningBodyRefInMsgs; //!< (optional) reference state input message + + SpinningBodyNDOFStateEffector(); //!< Constructor + ~SpinningBodyNDOFStateEffector() override; //!< Destructor /** method for adding a new spinning body */ void addSpinningBody(SpinningBody const& newBody); /** setter for `nameOfThetaState` property */ - void setNameOfThetaState(const std::string& nameOfThetaState) {this->nameOfThetaState = nameOfThetaState;}; + void setNameOfThetaState(const std::string& nameOfThetaState) { this->nameOfThetaState = nameOfThetaState; }; /** setter for `nameOfThetaDotState` property */ - void setNameOfThetaDotState(const std::string& nameOfThetaDotState) {this->nameOfThetaDotState = nameOfThetaDotState;}; + void setNameOfThetaDotState(const std::string& nameOfThetaDotState) + { + this->nameOfThetaDotState = nameOfThetaDotState; + }; /** getter for `nameOfThetaState` property */ - std::string getNameOfThetaState() const {return this->nameOfThetaState;}; + std::string getNameOfThetaState() const { return this->nameOfThetaState; }; /** getter for `nameOfThetaDotState` property */ - std::string getNameOfThetaDotState() const {return this->nameOfThetaDotState;}; + std::string getNameOfThetaDotState() const { return this->nameOfThetaDotState; }; -private: + private: static uint64_t effectorID; int numberOfDegreesOfFreedom = 0; diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py index a00455db57..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,9 +179,11 @@ 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 unitTestSim.InitializeSimulation() @@ -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/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp index 8594e86fb2..a444059ca3 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp @@ -59,20 +59,20 @@ SpinningBodyOneDOFStateEffector::~SpinningBodyOneDOFStateEffector() } /*! This method is used to reset the module. */ -void SpinningBodyOneDOFStateEffector::Reset(uint64_t CurrentClock) +void +SpinningBodyOneDOFStateEffector::Reset(uint64_t CurrentClock) { // Normalize the sHat vector if (this->sHat_S.norm() > 0.01) { this->sHat_S.normalize(); - } - else { + } else { bskLogger.bskLog(BSK_ERROR, "Norm of sHat must be greater than 0. sHat may not have been set by the user."); } } - /*! This method takes the computed theta states and outputs them to the messaging system. */ -void SpinningBodyOneDOFStateEffector::writeOutputStateMessages(uint64_t CurrentClock) +void +SpinningBodyOneDOFStateEffector::writeOutputStateMessages(uint64_t CurrentClock) { // Write out the spinning body output messages if (this->spinningBodyOutMsg.isLinked()) { @@ -98,46 +98,52 @@ void SpinningBodyOneDOFStateEffector::writeOutputStateMessages(uint64_t CurrentC } /*! This method prepends the name of the spacecraft for multi-spacecraft simulations.*/ -void SpinningBodyOneDOFStateEffector::prependSpacecraftNameToStates() +void +SpinningBodyOneDOFStateEffector::prependSpacecraftNameToStates() { this->nameOfThetaState = this->nameOfSpacecraftAttachedTo + this->nameOfThetaState; this->nameOfThetaDotState = this->nameOfSpacecraftAttachedTo + this->nameOfThetaDotState; } /*! This method allows the SB state effector to have access to the hub states and gravity*/ -void SpinningBodyOneDOFStateEffector::linkInStates(DynParamManager& statesIn) +void +SpinningBodyOneDOFStateEffector::linkInStates(DynParamManager& statesIn) { // - Get access to the hub's states needed for dynamic coupling - this->inertialPositionProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); - this->inertialVelocityProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); + this->inertialPositionProperty = + statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); + this->inertialVelocityProperty = + statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); } -/*! This method allows the SB state effector to register its states: theta and thetaDot with the dynamic parameter manager */ -void SpinningBodyOneDOFStateEffector::registerStates(DynParamManager& states) +/*! This method allows the SB state effector to register its states: theta and thetaDot with the dynamic parameter + * manager */ +void +SpinningBodyOneDOFStateEffector::registerStates(DynParamManager& states) { // Register the theta state this->thetaState = states.registerState(1, 1, this->nameOfThetaState); - Eigen::MatrixXd thetaInitMatrix(1,1); - thetaInitMatrix(0,0) = this->thetaInit; + Eigen::MatrixXd thetaInitMatrix(1, 1); + thetaInitMatrix(0, 0) = this->thetaInit; this->thetaState->setState(thetaInitMatrix); // Register the thetaDot state this->thetaDotState = states.registerState(1, 1, this->nameOfThetaDotState); - Eigen::MatrixXd thetaDotInitMatrix(1,1); - thetaDotInitMatrix(0,0) = this->thetaDotInit; + Eigen::MatrixXd thetaDotInitMatrix(1, 1); + thetaDotInitMatrix(0, 0) = this->thetaDotInit; this->thetaDotState->setState(thetaDotInitMatrix); } /*! This method allows the SB state effector to provide its contributions to the mass props and mass prop rates of the spacecraft */ -void SpinningBodyOneDOFStateEffector::updateEffectorMassProps(double integTime) +void +SpinningBodyOneDOFStateEffector::updateEffectorMassProps(double integTime) { // Give the mass of the spinning body to the effProps mass this->effProps.mEff = this->mass; // Lock the axis if the flag is set to 1 - if (this->lockFlag == 1) - { + if (this->lockFlag == 1) { Eigen::MatrixXd zeroMatrix = Eigen::MatrixXd::Constant(1, 1, 0.0); this->thetaDotState->setState(zeroMatrix); } @@ -176,17 +182,19 @@ void SpinningBodyOneDOFStateEffector::updateEffectorMassProps(double integTime) // Find body time derivative of IPntSc_B Eigen::Matrix3d rPrimeTilde_ScB_B = eigenTilde(this->rPrime_ScB_B); - this->effProps.IEffPrimePntB_B = this->omegaTilde_SB_B* this->IPntSc_B - this->IPntSc_B *this->omegaTilde_SB_B - - this->mass * (rPrimeTilde_ScB_B * this->rTilde_ScB_B + this->rTilde_ScB_B * rPrimeTilde_ScB_B); + this->effProps.IEffPrimePntB_B = + this->omegaTilde_SB_B * this->IPntSc_B - this->IPntSc_B * this->omegaTilde_SB_B - + this->mass * (rPrimeTilde_ScB_B * this->rTilde_ScB_B + this->rTilde_ScB_B * rPrimeTilde_ScB_B); } /*! This method allows the SB state effector to give its contributions to the matrices needed for the back-sub method */ -void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, - BackSubMatrices & backSubContr, - Eigen::Vector3d sigma_BN, - Eigen::Vector3d omega_BN_B, - Eigen::Vector3d g_N) +void +SpinningBodyOneDOFStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { // Define omega_SN_B this->omega_BN_B = omega_BN_B; @@ -210,14 +218,12 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, this->mTheta = this->sHat_B.transpose() * IPntS_B * this->sHat_B; // Lock the axis if the flag is set to 1 - if (this->lockFlag == 1) - { + if (this->lockFlag == 1) { // Define aTheta, bTheta and cTheta this->aTheta.setZero(); this->bTheta.setZero(); this->cTheta = 0.0; - } - else { + } else { // Define aTheta this->aTheta = this->mass * rTilde_ScS_B * this->sHat_B / this->mTheta; @@ -228,35 +234,39 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime, // Define cTheta Eigen::Vector3d rDot_SB_B = this->omegaTilde_BN_B * this->r_SB_B; Eigen::Vector3d gravityTorquePntS_B = rTilde_ScS_B * this->mass * g_B; - this->cTheta = (this->u - this->k * (this->theta - this->thetaRef) - this->c * (this->thetaDot - this->thetaDotRef) - + this->sHat_B.dot(gravityTorquePntS_B - omegaTilde_SN_B * IPntS_B * this->omega_SN_B - - IPntS_B * this->omegaTilde_BN_B * this->omega_SB_B - - this->mass * rTilde_ScS_B * this->omegaTilde_BN_B * rDot_SB_B)) / this->mTheta; + this->cTheta = + (this->u - this->k * (this->theta - this->thetaRef) - this->c * (this->thetaDot - this->thetaDotRef) + + this->sHat_B.dot(gravityTorquePntS_B - omegaTilde_SN_B * IPntS_B * this->omega_SN_B - + IPntS_B * this->omegaTilde_BN_B * this->omega_SB_B - + this->mass * rTilde_ScS_B * this->omegaTilde_BN_B * rDot_SB_B)) / + this->mTheta; } // For documentation on contributions see Vaz Carneiro, Allard, Schaub spinning body paper // Translation contributions backSubContr.matrixA = -this->mass * rTilde_ScS_B * this->sHat_B * this->aTheta.transpose(); backSubContr.matrixB = -this->mass * rTilde_ScS_B * this->sHat_B * this->bTheta.transpose(); - backSubContr.vecTrans = -this->mass * this->omegaTilde_SB_B * this->rPrime_ScS_B - + this->mass * rTilde_ScS_B * this->sHat_B * this->cTheta; + backSubContr.vecTrans = -this->mass * this->omegaTilde_SB_B * this->rPrime_ScS_B + + this->mass * rTilde_ScS_B * this->sHat_B * this->cTheta; // Rotation contributions - backSubContr.matrixC = (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) - * this->sHat_B * this->aTheta.transpose(); - backSubContr.matrixD = (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) - * this->sHat_B * this->bTheta.transpose(); - backSubContr.vecRot = -omegaTilde_SN_B * this->IPntSc_B * this->omega_SB_B - - this->mass * this->omegaTilde_BN_B * this->rTilde_ScB_B * this->rPrime_ScB_B - - this->mass * this->rTilde_ScB_B * this->omegaTilde_SB_B * this->rPrime_ScS_B - - (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) * this->sHat_B * this->cTheta; + backSubContr.matrixC = + (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) * this->sHat_B * this->aTheta.transpose(); + backSubContr.matrixD = + (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) * this->sHat_B * this->bTheta.transpose(); + backSubContr.vecRot = + -omegaTilde_SN_B * this->IPntSc_B * this->omega_SB_B - + this->mass * this->omegaTilde_BN_B * this->rTilde_ScB_B * this->rPrime_ScB_B - + this->mass * this->rTilde_ScB_B * this->omegaTilde_SB_B * this->rPrime_ScS_B - + (this->IPntSc_B - this->mass * this->rTilde_ScB_B * rTilde_ScS_B) * this->sHat_B * this->cTheta; } /*! This method is used to find the derivatives for the SB stateEffector: thetaDDot and the kinematic derivative */ -void SpinningBodyOneDOFStateEffector::computeDerivatives(double integTime, - Eigen::Vector3d rDDot_BN_N, - Eigen::Vector3d omegaDot_BN_B, - Eigen::Vector3d sigma_BN) +void +SpinningBodyOneDOFStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { // Update dcm_BN this->sigma_BN = sigma_BN; @@ -274,16 +284,17 @@ void SpinningBodyOneDOFStateEffector::computeDerivatives(double integTime, // Compute Derivatives this->thetaState->setDerivative(this->thetaDotState->getState()); Eigen::MatrixXd thetaDDot(1, 1); - thetaDDot(0, 0) = this->aTheta.dot(rDDotLocal_BN_B) - + this->bTheta.dot(omegaDotLocal_BN_B) + this->cTheta; + thetaDDot(0, 0) = this->aTheta.dot(rDDotLocal_BN_B) + this->bTheta.dot(omegaDotLocal_BN_B) + this->cTheta; this->thetaDotState->setDerivative(thetaDDot); } -/*! This method is for calculating the contributions of the SB state effector to the energy and momentum of the spacecraft */ -void SpinningBodyOneDOFStateEffector::updateEnergyMomContributions(double integTime, - Eigen::Vector3d & rotAngMomPntCContr_B, - double & rotEnergyContr, - Eigen::Vector3d omega_BN_B) +/*! This method is for calculating the contributions of the SB state effector to the energy and momentum of the + * spacecraft */ +void +SpinningBodyOneDOFStateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { // Update omega_BN_B and omega_SN_B this->omega_BN_B = omega_BN_B; @@ -297,13 +308,14 @@ void SpinningBodyOneDOFStateEffector::updateEnergyMomContributions(double integT rotAngMomPntCContr_B = this->IPntSc_B * this->omega_SN_B + this->mass * this->rTilde_ScB_B * this->rDot_ScB_B; // Find rotational energy contribution from the hub - rotEnergyContr = 1.0 / 2.0 * this->omega_SN_B.dot(this->IPntSc_B * this->omega_SN_B) - + 1.0 / 2.0 * this->mass * this->rDot_ScB_B.dot(this->rDot_ScB_B) - + 1.0 / 2.0 * this->k * (this->theta - this->thetaRef) * (this->theta - this->thetaRef); + rotEnergyContr = 1.0 / 2.0 * this->omega_SN_B.dot(this->IPntSc_B * this->omega_SN_B) + + 1.0 / 2.0 * this->mass * this->rDot_ScB_B.dot(this->rDot_ScB_B) + + 1.0 / 2.0 * this->k * (this->theta - this->thetaRef) * (this->theta - this->thetaRef); } /*! This method computes the spinning body states relative to the inertial frame */ -void SpinningBodyOneDOFStateEffector::computeSpinningBodyInertialStates() +void +SpinningBodyOneDOFStateEffector::computeSpinningBodyInertialStates() { // inertial attitude Eigen::Matrix3d dcm_SN; @@ -311,14 +323,15 @@ void SpinningBodyOneDOFStateEffector::computeSpinningBodyInertialStates() this->sigma_SN = eigenMRPd2Vector3d(eigenC2MRP(dcm_SN)); // inertial position vector - this->r_ScN_N = (Eigen::Vector3d)*this->inertialPositionProperty + this->dcm_BN.transpose() * this->r_ScB_B; + this->r_ScN_N = (Eigen::Vector3d) * this->inertialPositionProperty + this->dcm_BN.transpose() * this->r_ScB_B; // inertial velocity vector - this->v_ScN_N = (Eigen::Vector3d)*this->inertialVelocityProperty + this->dcm_BN.transpose() * this->rDot_ScB_B; + this->v_ScN_N = (Eigen::Vector3d) * this->inertialVelocityProperty + this->dcm_BN.transpose() * this->rDot_ScB_B; } /*! This method is used so that the simulation will ask SB to update messages */ -void SpinningBodyOneDOFStateEffector::UpdateState(uint64_t CurrentSimNanos) +void +SpinningBodyOneDOFStateEffector::UpdateState(uint64_t CurrentSimNanos) { //! - Read the incoming command array if (this->motorTorqueInMsg.isLinked() && this->motorTorqueInMsg.isWritten()) { diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h index 7525b54fa4..c3d072e1ab 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h @@ -35,92 +35,132 @@ #include "architecture/utilities/bskLogging.h" /*! @brief spinning body state effector class */ -class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel { - -public: - double mass = 1.0; //!< [kg] mass of spinning body - double k = 0.0; //!< [N-m/rad] torsional spring constant - double c = 0.0; //!< [N-m-s/rad] rotational damping coefficient - double thetaInit = 0.0; //!< [rad] initial spinning body angle - double thetaDotInit = 0.0; //!< [rad/s] initial spinning body angle rate - std::string nameOfThetaState; //!< -- identifier for the theta state data container - std::string nameOfThetaDotState; //!< -- identifier for the thetaDot state data container - Eigen::Vector3d r_SB_B{0.0, 0.0, 0.0}; //!< [m] vector pointing from body frame B origin to spinning frame S origin in B frame components - Eigen::Vector3d r_ScS_S{0.0, 0.0, 0.0}; //!< [m] vector pointing from spinning frame S origin to point Sc (center of mass of the spinner) in S frame components - Eigen::Vector3d sHat_S{1.0, 0.0, 0.0}; //!< -- spinning axis in S frame components. - Eigen::Matrix3d IPntSc_S; //!< [kg-m^2] Inertia of spinning body about point Sc in S frame components - Eigen::Matrix3d dcm_S0B; //!< -- DCM from the body frame to the S0 frame (S frame for theta=0) - Message spinningBodyOutMsg; //!< state output message - Message spinningBodyConfigLogOutMsg; //!< spinning body state config log message - ReadFunctor motorTorqueInMsg; //!< -- (optional) motor torque input message - ReadFunctor motorLockInMsg; //!< -- (optional) motor lock flag input message - ReadFunctor spinningBodyRefInMsg; //!< -- (optional) spinning body reference input message name - - SpinningBodyOneDOFStateEffector(); //!< -- Contructor - ~SpinningBodyOneDOFStateEffector() override; //!< -- Destructor - void Reset(uint64_t CurrentClock) override; //!< -- Method for reset - void writeOutputStateMessages(uint64_t CurrentClock) override; //!< -- Method for writing the output messages - void UpdateState(uint64_t CurrentSimNanos) override; //!< -- Method for updating information - void registerStates(DynParamManager& statesIn) override; //!< -- Method for registering the SB states - void linkInStates(DynParamManager& states) override; //!< -- Method for getting access to other states +class SpinningBodyOneDOFStateEffector + : public StateEffector + , public SysModel +{ + + public: + double mass = 1.0; //!< [kg] mass of spinning body + double k = 0.0; //!< [N-m/rad] torsional spring constant + double c = 0.0; //!< [N-m-s/rad] rotational damping coefficient + double thetaInit = 0.0; //!< [rad] initial spinning body angle + double thetaDotInit = 0.0; //!< [rad/s] initial spinning body angle rate + std::string nameOfThetaState; //!< -- identifier for the theta state data container + std::string nameOfThetaDotState; //!< -- identifier for the thetaDot state data container + Eigen::Vector3d r_SB_B{ + 0.0, + 0.0, + 0.0 + }; //!< [m] vector pointing from body frame B origin to spinning frame S origin in B frame components + Eigen::Vector3d r_ScS_S{ 0.0, 0.0, 0.0 }; //!< [m] vector pointing from spinning frame S origin to point Sc (center + //!< of mass of the spinner) in S frame components + Eigen::Vector3d sHat_S{ 1.0, 0.0, 0.0 }; //!< -- spinning axis in S frame components. + Eigen::Matrix3d IPntSc_S; //!< [kg-m^2] Inertia of spinning body about point Sc in S frame components + Eigen::Matrix3d dcm_S0B; //!< -- DCM from the body frame to the S0 frame (S frame for theta=0) + Message spinningBodyOutMsg; //!< state output message + Message spinningBodyConfigLogOutMsg; //!< spinning body state config log message + ReadFunctor motorTorqueInMsg; //!< -- (optional) motor torque input message + ReadFunctor motorLockInMsg; //!< -- (optional) motor lock flag input message + ReadFunctor + spinningBodyRefInMsg; //!< -- (optional) spinning body reference input message name + + SpinningBodyOneDOFStateEffector(); //!< -- Contructor + ~SpinningBodyOneDOFStateEffector() override; //!< -- Destructor + void Reset(uint64_t CurrentClock) override; //!< -- Method for reset + void writeOutputStateMessages(uint64_t CurrentClock) override; //!< -- Method for writing the output messages + void UpdateState(uint64_t CurrentSimNanos) override; //!< -- Method for updating information + void registerStates(DynParamManager& statesIn) override; //!< -- Method for registering the SB states + void linkInStates(DynParamManager& states) override; //!< -- Method for getting access to other states void updateContributions(double integTime, - BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, - Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) override; //!< -- Method for back-substitution contributions - void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, - Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) override; //!< -- Method for SB to compute its derivatives - void updateEffectorMassProps(double integTime) override; //!< -- Method for giving the s/c the HRB mass props and prop rates - void updateEnergyMomContributions(double integTime, Eigen::Vector3d& rotAngMomPntCContr_B, - double& rotEnergyContr, Eigen::Vector3d omega_BN_B) override; //!< -- Method for computing energy and momentum for SBs - void prependSpacecraftNameToStates() override; //!< Method used for multiple spacecraft - void computeSpinningBodyInertialStates(); //!< Method for computing the SB's states - -private: - static uint64_t effectorID; //!< [] ID number of this panel - double u = 0.0; //!< [N-m] optional motor torque - int lockFlag = 0; //!< [] flag for locking the rotation axis - double thetaRef = 0.0; //!< [rad] spinning body reference angle - double thetaDotRef = 0.0; //!< [rad] spinning body reference angle rate + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) override; //!< -- Method for back-substitution contributions + void computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) override; //!< -- Method for SB to compute its derivatives + void updateEffectorMassProps( + double integTime) override; //!< -- Method for giving the s/c the HRB mass props and prop rates + void updateEnergyMomContributions( + double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) override; //!< -- Method for computing energy and momentum for SBs + void prependSpacecraftNameToStates() override; //!< Method used for multiple spacecraft + void computeSpinningBodyInertialStates(); //!< Method for computing the SB's states + + private: + static uint64_t effectorID; //!< [] ID number of this panel + double u = 0.0; //!< [N-m] optional motor torque + int lockFlag = 0; //!< [] flag for locking the rotation axis + double thetaRef = 0.0; //!< [rad] spinning body reference angle + double thetaDotRef = 0.0; //!< [rad] spinning body reference angle rate // Terms needed for back substitution - Eigen::Vector3d aTheta{0.0, 0.0, 0.0}; //!< -- rDDot_BN term for back substitution - Eigen::Vector3d bTheta{0.0, 0.0, 0.0}; //!< -- omegaDot_BN term for back substitution - double cTheta = 0.0; //!< -- scalar term for back substitution - double mTheta = 0.0; //!< -- auxiliary term for back substitution + Eigen::Vector3d aTheta{ 0.0, 0.0, 0.0 }; //!< -- rDDot_BN term for back substitution + Eigen::Vector3d bTheta{ 0.0, 0.0, 0.0 }; //!< -- omegaDot_BN term for back substitution + double cTheta = 0.0; //!< -- scalar term for back substitution + double mTheta = 0.0; //!< -- auxiliary term for back substitution // Vector quantities - Eigen::Vector3d sHat_B{1.0, 0.0, 0.0}; //!< -- spinning axis in B frame components - Eigen::Vector3d r_ScS_B{0.0, 0.0, 0.0}; //!< [m] vector pointing from spinning frame S origin to point Sc in B frame components - Eigen::Vector3d r_ScB_B{0.0, 0.0, 0.0}; //!< [m] vector pointing from body frame B origin to point Sc in B frame components. - Eigen::Vector3d rPrime_ScS_B{0.0, 0.0, 0.0}; //!< [m/s] body frame time derivative of r_ScS_B - Eigen::Vector3d rPrime_ScB_B{0.0, 0.0, 0.0}; //!< [m/s] body frame time derivative of r_ScB_B - Eigen::Vector3d rDot_ScB_B{0.0, 0.0, 0.0}; //!< [m/s] inertial frame time derivative of r_ScB_B - Eigen::Vector3d omega_SB_B{0.0, 0.0, 0.0}; //!< [rad/s] angular velocity of the S frame wrt the B frame in B frame components. - Eigen::Vector3d omega_BN_B{0.0, 0.0, 0.0}; //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components. - Eigen::Vector3d omega_SN_B{0.0, 0.0, 0.0}; //!< [rad/s] angular velocity of the S frame wrt the N frame in B frame components. - Eigen::MRPd sigma_BN{0.0, 0.0, 0.0}; //!< -- body frame attitude wrt to the N frame in MRPs + Eigen::Vector3d sHat_B{ 1.0, 0.0, 0.0 }; //!< -- spinning axis in B frame components + Eigen::Vector3d r_ScS_B{ + 0.0, + 0.0, + 0.0 + }; //!< [m] vector pointing from spinning frame S origin to point Sc in B frame components + Eigen::Vector3d r_ScB_B{ 0.0, + 0.0, + 0.0 }; //!< [m] vector pointing from body frame B origin to point Sc in B frame components. + Eigen::Vector3d rPrime_ScS_B{ 0.0, 0.0, 0.0 }; //!< [m/s] body frame time derivative of r_ScS_B + Eigen::Vector3d rPrime_ScB_B{ 0.0, 0.0, 0.0 }; //!< [m/s] body frame time derivative of r_ScB_B + Eigen::Vector3d rDot_ScB_B{ 0.0, 0.0, 0.0 }; //!< [m/s] inertial frame time derivative of r_ScB_B + Eigen::Vector3d omega_SB_B{ + 0.0, + 0.0, + 0.0 + }; //!< [rad/s] angular velocity of the S frame wrt the B frame in B frame components. + Eigen::Vector3d omega_BN_B{ + 0.0, + 0.0, + 0.0 + }; //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components. + Eigen::Vector3d omega_SN_B{ + 0.0, + 0.0, + 0.0 + }; //!< [rad/s] angular velocity of the S frame wrt the N frame in B frame components. + Eigen::MRPd sigma_BN{ 0.0, 0.0, 0.0 }; //!< -- body frame attitude wrt to the N frame in MRPs // Matrix quantities - Eigen::Matrix3d rTilde_ScB_B; //!< [m] tilde matrix of r_ScB_B - Eigen::Matrix3d omegaTilde_SB_B; //!< [rad/s] tilde matrix of omega_SB_B - Eigen::Matrix3d omegaTilde_BN_B; //!< [rad/s] tilde matrix of omega_BN_B - Eigen::Matrix3d dcm_BS; //!< -- DCM from spinner frame to body frame - Eigen::Matrix3d dcm_BN; //!< -- DCM from inertial frame to body frame - Eigen::Matrix3d IPntSc_B; //!< [kg-m^2] inertia of spinning body about point Sc in B frame components + Eigen::Matrix3d rTilde_ScB_B; //!< [m] tilde matrix of r_ScB_B + Eigen::Matrix3d omegaTilde_SB_B; //!< [rad/s] tilde matrix of omega_SB_B + Eigen::Matrix3d omegaTilde_BN_B; //!< [rad/s] tilde matrix of omega_BN_B + Eigen::Matrix3d dcm_BS; //!< -- DCM from spinner frame to body frame + Eigen::Matrix3d dcm_BN; //!< -- DCM from inertial frame to body frame + Eigen::Matrix3d IPntSc_B; //!< [kg-m^2] inertia of spinning body about point Sc in B frame components // Spinning body properties - Eigen::Vector3d r_ScN_N{0.0, 0.0, 0.0}; //!< [m] position vector of spinning body center of mass Sc relative to the inertial frame origin N - Eigen::Vector3d v_ScN_N{0.0, 0.0, 0.0}; //!< [m/s] inertial velocity vector of Sc relative to inertial frame - Eigen::Vector3d sigma_SN{0.0, 0.0, 0.0}; //!< -- MRP attitude of frame S relative to inertial frame - Eigen::Vector3d omega_SN_S{0.0, 0.0, 0.0}; //!< [rad/s] inertial spinning body frame angular velocity vector + Eigen::Vector3d r_ScN_N{ + 0.0, + 0.0, + 0.0 + }; //!< [m] position vector of spinning body center of mass Sc relative to the inertial frame origin N + Eigen::Vector3d v_ScN_N{ 0.0, 0.0, 0.0 }; //!< [m/s] inertial velocity vector of Sc relative to inertial frame + Eigen::Vector3d sigma_SN{ 0.0, 0.0, 0.0 }; //!< -- MRP attitude of frame S relative to inertial frame + Eigen::Vector3d omega_SN_S{ 0.0, 0.0, 0.0 }; //!< [rad/s] inertial spinning body frame angular velocity vector // States - double theta = 0.0; //!< [rad] spinning body angle - double thetaDot = 0.0; //!< [rad/s] spinning body angle rate - Eigen::MatrixXd* inertialPositionProperty = nullptr; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - Eigen::MatrixXd* inertialVelocityProperty = nullptr; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase - StateData *thetaState = nullptr; //!< -- state manager of theta for spinning body - StateData *thetaDotState = nullptr; //!< -- state manager of thetaDot for spinning body + double theta = 0.0; //!< [rad] spinning body angle + double thetaDot = 0.0; //!< [rad/s] spinning body angle rate + Eigen::MatrixXd* inertialPositionProperty = + nullptr; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialVelocityProperty = + nullptr; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase + StateData* thetaState = nullptr; //!< -- state manager of theta for spinning body + StateData* thetaDotState = nullptr; //!< -- state manager of thetaDot for spinning body }; - #endif /* SPINNING_BODY_ONE_DOF_STATE_EFFECTOR_H */ 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..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,9 +212,11 @@ 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 unitTestSim.InitializeSimulation() @@ -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/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp index 3fa2419be3..2c94229f1e 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.cpp @@ -64,31 +64,30 @@ uint64_t SpinningBodyTwoDOFStateEffector::effectorID = 1; /*! This is the destructor, nothing to report here */ SpinningBodyTwoDOFStateEffector::~SpinningBodyTwoDOFStateEffector() { - SpinningBodyTwoDOFStateEffector::effectorID --; /* reset the panel ID*/ + SpinningBodyTwoDOFStateEffector::effectorID--; /* reset the panel ID*/ } /*! This method is used to reset the module. */ -void SpinningBodyTwoDOFStateEffector::Reset(uint64_t CurrentClock) +void +SpinningBodyTwoDOFStateEffector::Reset(uint64_t CurrentClock) { // Normalize both sHat vectors if (this->s1Hat_S1.norm() > 0.01) { this->s1Hat_S1.normalize(); - } - else { + } else { bskLogger.bskLog(BSK_ERROR, "Norm of s1Hat must be greater than 0. s1Hat may not have been set by the user."); } if (this->s2Hat_S2.norm() > 0.01) { this->s2Hat_S2.normalize(); - } - else { + } else { bskLogger.bskLog(BSK_ERROR, "Norm of s2Hat must be greater than 0. s1Hat may not have been set by the user."); } } - /*! This method takes the computed theta states and outputs them to the messaging system. */ -void SpinningBodyTwoDOFStateEffector::writeOutputStateMessages(uint64_t CurrentClock) +void +SpinningBodyTwoDOFStateEffector::writeOutputStateMessages(uint64_t CurrentClock) { // Write out the spinning body output messages HingedRigidBodyMsgPayload spinningBodyBuffer; @@ -132,7 +131,8 @@ void SpinningBodyTwoDOFStateEffector::writeOutputStateMessages(uint64_t CurrentC } /*! This method prepends the name of the spacecraft for multi-spacecraft simulations.*/ -void SpinningBodyTwoDOFStateEffector::prependSpacecraftNameToStates() +void +SpinningBodyTwoDOFStateEffector::prependSpacecraftNameToStates() { this->nameOfTheta1State = this->nameOfSpacecraftAttachedTo + this->nameOfTheta1State; this->nameOfTheta1DotState = this->nameOfSpacecraftAttachedTo + this->nameOfTheta1DotState; @@ -141,37 +141,43 @@ void SpinningBodyTwoDOFStateEffector::prependSpacecraftNameToStates() } /*! This method allows the SB state effector to have access to the hub states and gravity*/ -void SpinningBodyTwoDOFStateEffector::linkInStates(DynParamManager& statesIn) +void +SpinningBodyTwoDOFStateEffector::linkInStates(DynParamManager& statesIn) { - this->inertialPositionProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); - this->inertialVelocityProperty = statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); + this->inertialPositionProperty = + statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialPosition); + this->inertialVelocityProperty = + statesIn.getPropertyReference(this->nameOfSpacecraftAttachedTo + this->propName_inertialVelocity); } -/*! This method allows the SB state effector to register its states: theta and thetaDot with the dynamic parameter manager */ -void SpinningBodyTwoDOFStateEffector::registerStates(DynParamManager& states) +/*! This method allows the SB state effector to register its states: theta and thetaDot with the dynamic parameter + * manager */ +void +SpinningBodyTwoDOFStateEffector::registerStates(DynParamManager& states) { // Register the theta states this->theta1State = states.registerState(1, 1, this->nameOfTheta1State); this->theta2State = states.registerState(1, 1, this->nameOfTheta2State); - Eigen::MatrixXd thetaInitMatrix(2,1); - thetaInitMatrix(0,0) = this->theta1Init; - thetaInitMatrix(1,0) = this->theta2Init; + Eigen::MatrixXd thetaInitMatrix(2, 1); + thetaInitMatrix(0, 0) = this->theta1Init; + thetaInitMatrix(1, 0) = this->theta2Init; this->theta1State->setState(thetaInitMatrix.row(0)); this->theta2State->setState(thetaInitMatrix.row(1)); // Register the thetaDot states this->theta1DotState = states.registerState(1, 1, this->nameOfTheta1DotState); this->theta2DotState = states.registerState(1, 1, this->nameOfTheta2DotState); - Eigen::MatrixXd thetaDotInitMatrix(2,1); - thetaDotInitMatrix(0,0) = this->theta1DotInit; - thetaDotInitMatrix(1,0) = this->theta2DotInit; + Eigen::MatrixXd thetaDotInitMatrix(2, 1); + thetaDotInitMatrix(0, 0) = this->theta1DotInit; + thetaDotInitMatrix(1, 0) = this->theta2DotInit; this->theta1DotState->setState(thetaDotInitMatrix.row(0)); this->theta2DotState->setState(thetaDotInitMatrix.row(1)); } /*! This method allows the SB state effector to provide its contributions to the mass props and mass prop rates of the spacecraft */ -void SpinningBodyTwoDOFStateEffector::updateEffectorMassProps(double integTime) +void +SpinningBodyTwoDOFStateEffector::updateEffectorMassProps(double integTime) { // Give the mass of the spinning body to the effProps mass this->mass = this->mass1 + this->mass2; @@ -179,12 +185,10 @@ void SpinningBodyTwoDOFStateEffector::updateEffectorMassProps(double integTime) // Lock the axis if the flag is set to 1 Eigen::MatrixXd zeroMatrix = Eigen::MatrixXd::Constant(1, 1, 0.0); - if (this->lockFlag1 == 1) - { + if (this->lockFlag1 == 1) { this->theta1DotState->setState(zeroMatrix); } - if (this->lockFlag2 == 1) - { + if (this->lockFlag2 == 1) { this->theta2DotState->setState(zeroMatrix); } @@ -198,11 +202,11 @@ void SpinningBodyTwoDOFStateEffector::updateEffectorMassProps(double integTime) double dcm_S0S[3][3]; double prv_S0S_array[3]; Eigen::Vector3d prv_S0S; - prv_S0S = - this->theta1 * this->s1Hat_S1; + prv_S0S = -this->theta1 * this->s1Hat_S1; eigenVector3d2CArray(prv_S0S, prv_S0S_array); PRV2C(prv_S0S_array, dcm_S0S); this->dcm_BS1 = this->dcm_S10B.transpose() * c2DArray2EigenMatrix3d(dcm_S0S); - prv_S0S = - this->theta2 * this->s2Hat_S2; + prv_S0S = -this->theta2 * this->s2Hat_S2; eigenVector3d2CArray(prv_S0S, prv_S0S_array); PRV2C(prv_S0S_array, dcm_S0S); this->dcm_BS2 = this->dcm_BS1 * this->dcm_S20S1.transpose() * c2DArray2EigenMatrix3d(dcm_S0S); @@ -226,7 +230,9 @@ void SpinningBodyTwoDOFStateEffector::updateEffectorMassProps(double integTime) this->IS1PntSc1_B = this->dcm_BS1 * this->IS1PntSc1_S1 * this->dcm_BS1.transpose(); this->rTilde_Sc2B_B = eigenTilde(this->r_Sc2B_B); this->IS2PntSc2_B = this->dcm_BS2 * this->IS2PntSc2_S2 * this->dcm_BS2.transpose(); - this->effProps.IEffPntB_B = this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * this->rTilde_Sc1B_B - this->mass2 * this->rTilde_Sc2B_B * this->rTilde_Sc2B_B; + this->effProps.IEffPntB_B = this->IS1PntSc1_B + this->IS2PntSc2_B - + this->mass1 * this->rTilde_Sc1B_B * this->rTilde_Sc1B_B - + this->mass2 * this->rTilde_Sc2B_B * this->rTilde_Sc2B_B; // Define omega_S1B_B, omega_S2S1_B, omega_S2B_B, and their cross product operator this->omega_S1B_B = this->theta1Dot * this->s1Hat_B; @@ -252,14 +258,20 @@ void SpinningBodyTwoDOFStateEffector::updateEffectorMassProps(double integTime) // Find body time derivative of IPntSc_B Eigen::Matrix3d rPrimeTilde_Sc1B_B = eigenTilde(this->rPrime_Sc1B_B); Eigen::Matrix3d rPrimeTilde_Sc2B_B = eigenTilde(this->rPrime_Sc2B_B); - this->effProps.IEffPrimePntB_B = this->IPrimeS1PntSc1_B + this->IPrimeS2PntSc2_B - - this->mass1 * (rPrimeTilde_Sc1B_B * this->rTilde_Sc1B_B + this->rTilde_Sc1B_B * rPrimeTilde_Sc1B_B) - - this->mass2 * (rPrimeTilde_Sc2B_B * this->rTilde_Sc2B_B + this->rTilde_Sc2B_B * rPrimeTilde_Sc2B_B); + this->effProps.IEffPrimePntB_B = + this->IPrimeS1PntSc1_B + this->IPrimeS2PntSc2_B - + this->mass1 * (rPrimeTilde_Sc1B_B * this->rTilde_Sc1B_B + this->rTilde_Sc1B_B * rPrimeTilde_Sc1B_B) - + this->mass2 * (rPrimeTilde_Sc2B_B * this->rTilde_Sc2B_B + this->rTilde_Sc2B_B * rPrimeTilde_Sc2B_B); } /*! This method allows the SB state effector to give its contributions to the matrices needed for the back-sub method */ -void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, BackSubMatrices & backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) +void +SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, + BackSubMatrices& backSubContr, + Eigen::Vector3d sigma_BN, + Eigen::Vector3d omega_BN_B, + Eigen::Vector3d g_N) { // Find the DCM from N to B frames this->sigma_BN = sigma_BN; @@ -299,25 +311,32 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back // Define auxiliary inertia matrices Eigen::Matrix3d IS2PntS2_B = this->IS2PntSc2_B - this->mass2 * rTilde_Sc2S2_B * rTilde_Sc2S2_B; - Eigen::Matrix3d ISPntS1_B = this->IS1PntSc1_B - this->mass1 * rTilde_Sc1S1_B * rTilde_Sc1S1_B + this->IS2PntSc2_B - this->mass2 * rTilde_Sc2S1_B * rTilde_Sc2S1_B; - Eigen::Matrix3d IPrimeSPntS1_B = this->IPrimeS1PntSc1_B - this->mass1 * (rPrimeTilde_Sc1S1_B * rTilde_Sc1S1_B + rTilde_Sc1S1_B * rPrimeTilde_Sc1S1_B) - + this->IPrimeS2PntSc2_B - this->mass2 * (rPrimeTilde_Sc2S1_B * rTilde_Sc2S1_B + rTilde_Sc2S1_B * rPrimeTilde_Sc2S1_B); - Eigen::Matrix3d IPrimeS2PntS2_B = this->IPrimeS2PntSc2_B - this->mass2 * (rPrimeTilde_Sc2S2_B * rTilde_Sc2S2_B + rTilde_Sc2S2_B * rPrimeTilde_Sc2S2_B); + Eigen::Matrix3d ISPntS1_B = this->IS1PntSc1_B - this->mass1 * rTilde_Sc1S1_B * rTilde_Sc1S1_B + this->IS2PntSc2_B - + this->mass2 * rTilde_Sc2S1_B * rTilde_Sc2S1_B; + Eigen::Matrix3d IPrimeSPntS1_B = + this->IPrimeS1PntSc1_B - + this->mass1 * (rPrimeTilde_Sc1S1_B * rTilde_Sc1S1_B + rTilde_Sc1S1_B * rPrimeTilde_Sc1S1_B) + + this->IPrimeS2PntSc2_B - + this->mass2 * (rPrimeTilde_Sc2S1_B * rTilde_Sc2S1_B + rTilde_Sc2S1_B * rPrimeTilde_Sc2S1_B); + Eigen::Matrix3d IPrimeS2PntS2_B = this->IPrimeS2PntSc2_B - this->mass2 * (rPrimeTilde_Sc2S2_B * rTilde_Sc2S2_B + + rTilde_Sc2S2_B * rPrimeTilde_Sc2S2_B); // Define and populate the mass matrix for thetaDDot Eigen::Matrix2d MTheta; - MTheta << this->s1Hat_B.transpose() * ISPntS1_B * this->s1Hat_B, this->s1Hat_B.transpose() * (this->IS2PntSc2_B - this->mass2 * rTilde_Sc2S1_B * rTilde_Sc2S2_B) * this->s2Hat_B, - this->s2Hat_B.transpose() * (IS2PntS2_B - this->mass2 * rTilde_Sc2S2_B * rTilde_S2S1_B) * this->s1Hat_B, this->s2Hat_B.transpose() * IS2PntS2_B * this->s2Hat_B; + MTheta << this->s1Hat_B.transpose() * ISPntS1_B * this->s1Hat_B, + this->s1Hat_B.transpose() * (this->IS2PntSc2_B - this->mass2 * rTilde_Sc2S1_B * rTilde_Sc2S2_B) * this->s2Hat_B, + this->s2Hat_B.transpose() * (IS2PntS2_B - this->mass2 * rTilde_Sc2S2_B * rTilde_S2S1_B) * this->s1Hat_B, + this->s2Hat_B.transpose() * IS2PntS2_B * this->s2Hat_B; // Define AThetaStar matrix Eigen::Matrix AThetaStar; - AThetaStar.row(0) = - this->mass * this->s1Hat_B.transpose() * rTilde_ScS1_B; - AThetaStar.row(1) = - this->mass2 * this->s2Hat_B.transpose() * rTilde_Sc2S2_B; + AThetaStar.row(0) = -this->mass * this->s1Hat_B.transpose() * rTilde_ScS1_B; + AThetaStar.row(1) = -this->mass2 * this->s2Hat_B.transpose() * rTilde_Sc2S2_B; // Define BThetaStar matrix Eigen::Matrix BThetaStar; - BThetaStar.row(0) = - this->s1Hat_B.transpose() * (ISPntS1_B - this->mass * rTilde_ScS1_B * rTilde_S1B_B); - BThetaStar.row(1) = - this->s2Hat_B.transpose() * (IS2PntS2_B - this->mass2 * rTilde_Sc2S2_B * rTilde_S2B_B); + BThetaStar.row(0) = -this->s1Hat_B.transpose() * (ISPntS1_B - this->mass * rTilde_ScS1_B * rTilde_S1B_B); + BThetaStar.row(1) = -this->s2Hat_B.transpose() * (IS2PntS2_B - this->mass2 * rTilde_Sc2S2_B * rTilde_S2B_B); // Define CThetaStar vector Eigen::Vector3d rDot_S1B_B = this->omegaTilde_BN_B * this->r_S1B_B; @@ -325,38 +344,41 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back Eigen::Vector3d gravityTorquePntS1_B = rTilde_ScS1_B * this->mass * g_B; Eigen::Vector3d gravityTorquePntS2_B = rTilde_Sc2S2_B * this->mass2 * g_B; Eigen::Vector2d CThetaStar; - CThetaStar(0,0) = this->u1 - this->k1 * (this->theta1 - this->theta1Ref) - - this->c1 * (this->theta1Dot - this->theta1DotRef) + this->s1Hat_B.transpose() * gravityTorquePntS1_B - - this->s1Hat_B.transpose() * ((IPrimeSPntS1_B + this->omegaTilde_BN_B * ISPntS1_B) * this->omega_BN_B - + (this->IPrimeS1PntSc1_B + this->omegaTilde_BN_B * this->IS1PntSc1_B) * this->omega_S1B_B - + (this->IPrimeS2PntSc2_B + this->omegaTilde_BN_B * this->IS2PntSc2_B) * this->omega_S2B_B - + (this->IS2PntSc2_B - this->mass2 * rTilde_Sc2S1_B * rTilde_Sc2S2_B) * this->omegaTilde_S1B_B * this->omega_S2S1_B - + this->mass1 * (rTilde_Sc1S1_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * rTilde_Sc1S1_B) * this->rPrime_Sc1S1_B - + this->mass2 * (rTilde_Sc2S1_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * rTilde_Sc2S1_B) * this->rPrime_Sc2S1_B - + this->mass2 * rTilde_Sc2S1_B * omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - + this->mass * rTilde_ScS1_B * this->omegaTilde_BN_B * rDot_S1B_B); - CThetaStar(1, 0) = this->u2 - this->k2 * (this->theta2 - this->theta2Ref) - - this->c2 * (this->theta2Dot - this->theta2DotRef) + this->s2Hat_B.transpose() * gravityTorquePntS2_B - - this->s2Hat_B.transpose() * ((IPrimeS2PntS2_B + this->omegaTilde_BN_B * IS2PntS2_B) * this->omega_S2N_B - + IS2PntS2_B * this->omegaTilde_S1B_B * this->omega_S2S1_B + this->mass2 * rTilde_Sc2S2_B * omegaTilde_S1N_B * this->rPrime_S2S1_B - + this->mass2 * rTilde_Sc2S2_B * this->omegaTilde_BN_B * (rDot_S2S1_B + rDot_S1B_B)); + CThetaStar(0, 0) = + this->u1 - this->k1 * (this->theta1 - this->theta1Ref) - this->c1 * (this->theta1Dot - this->theta1DotRef) + + this->s1Hat_B.transpose() * gravityTorquePntS1_B - + this->s1Hat_B.transpose() * + ((IPrimeSPntS1_B + this->omegaTilde_BN_B * ISPntS1_B) * this->omega_BN_B + + (this->IPrimeS1PntSc1_B + this->omegaTilde_BN_B * this->IS1PntSc1_B) * this->omega_S1B_B + + (this->IPrimeS2PntSc2_B + this->omegaTilde_BN_B * this->IS2PntSc2_B) * this->omega_S2B_B + + (this->IS2PntSc2_B - this->mass2 * rTilde_Sc2S1_B * rTilde_Sc2S2_B) * this->omegaTilde_S1B_B * + this->omega_S2S1_B + + this->mass1 * (rTilde_Sc1S1_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * rTilde_Sc1S1_B) * + this->rPrime_Sc1S1_B + + this->mass2 * (rTilde_Sc2S1_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * rTilde_Sc2S1_B) * + this->rPrime_Sc2S1_B + + this->mass2 * rTilde_Sc2S1_B * omegaTilde_S2S1_B * this->rPrime_Sc2S2_B + + this->mass * rTilde_ScS1_B * this->omegaTilde_BN_B * rDot_S1B_B); + CThetaStar(1, 0) = + this->u2 - this->k2 * (this->theta2 - this->theta2Ref) - this->c2 * (this->theta2Dot - this->theta2DotRef) + + this->s2Hat_B.transpose() * gravityTorquePntS2_B - + this->s2Hat_B.transpose() * ((IPrimeS2PntS2_B + this->omegaTilde_BN_B * IS2PntS2_B) * this->omega_S2N_B + + IS2PntS2_B * this->omegaTilde_S1B_B * this->omega_S2S1_B + + this->mass2 * rTilde_Sc2S2_B * omegaTilde_S1N_B * this->rPrime_S2S1_B + + this->mass2 * rTilde_Sc2S2_B * this->omegaTilde_BN_B * (rDot_S2S1_B + rDot_S1B_B)); // Check if any of the axis are locked and change dynamics accordingly - if (this->lockFlag1 == 1 || this->lockFlag2 == 1) - { + if (this->lockFlag1 == 1 || this->lockFlag2 == 1) { // Remove cross-coupling MTheta(0, 1) = 0.0; MTheta(1, 0) = 0.0; - if (this->lockFlag1 == 1) - { + if (this->lockFlag1 == 1) { AThetaStar.row(0).setZero(); BThetaStar.row(0).setZero(); CThetaStar.row(0).setZero(); - } - if (this->lockFlag2 == 1) - { + if (this->lockFlag2 == 1) { AThetaStar.row(1).setZero(); BThetaStar.row(1).setZero(); CThetaStar.row(1).setZero(); @@ -370,27 +392,49 @@ void SpinningBodyTwoDOFStateEffector::updateContributions(double integTime, Back // For documentation on contributions see Vaz Carneiro, Allard, Schaub spinning body paper // Translation contributions - backSubContr.matrixA = - this->mass * rTilde_ScS1_B * this->s1Hat_B * this->ATheta.row(0) - this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->ATheta.row(1); - backSubContr.matrixB = - this->mass * rTilde_ScS1_B * this->s1Hat_B * this->BTheta.row(0) - this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->BTheta.row(1); - backSubContr.vecTrans = - this->mass * this->omegaTilde_S1B_B * this->rPrime_ScB_B - this->mass2 * (omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - rTilde_Sc2S2_B * this->omegaTilde_S1B_B * this->omega_S2S1_B) - + this->mass * rTilde_ScS1_B * this->s1Hat_B * this->CTheta.row(0) + this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->CTheta.row(1); + backSubContr.matrixA = -this->mass * rTilde_ScS1_B * this->s1Hat_B * this->ATheta.row(0) - + this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->ATheta.row(1); + backSubContr.matrixB = -this->mass * rTilde_ScS1_B * this->s1Hat_B * this->BTheta.row(0) - + this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->BTheta.row(1); + backSubContr.vecTrans = -this->mass * this->omegaTilde_S1B_B * this->rPrime_ScB_B - + this->mass2 * (omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - + rTilde_Sc2S2_B * this->omegaTilde_S1B_B * this->omega_S2S1_B) + + this->mass * rTilde_ScS1_B * this->s1Hat_B * this->CTheta.row(0) + + this->mass2 * rTilde_Sc2S2_B * this->s2Hat_B * this->CTheta.row(1); // Rotation contributions - backSubContr.matrixC = (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * this->s1Hat_B * this->ATheta.row(0) - + (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->ATheta.row(1); - backSubContr.matrixD = (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * this->s1Hat_B * this->BTheta.row(0) - + (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->BTheta.row(1); - backSubContr.vecRot = - (this->IPrimeS1PntSc1_B + this->omegaTilde_BN_B * this->IS1PntSc1_B) * this->omega_S1B_B - (this->IPrimeS2PntSc2_B + this->omegaTilde_BN_B * this->IS2PntSc2_B) * this->omega_S2B_B - - (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->omegaTilde_S1B_B * this->omega_S2S1_B - - this->mass1 * (this->rTilde_Sc1B_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * this->rTilde_Sc1B_B) * this->rPrime_Sc1B_B - - this->mass2 * (this->rTilde_Sc2B_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * this->rTilde_Sc2B_B) * this->rPrime_Sc2B_B - - this->mass2 * this->rTilde_Sc2B_B * omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - - (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * this->s1Hat_B * this->CTheta.row(0) - - (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->CTheta.row(1); + backSubContr.matrixC = + (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - + this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * + this->s1Hat_B * this->ATheta.row(0) + + (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->ATheta.row(1); + backSubContr.matrixD = + (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - + this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * + this->s1Hat_B * this->BTheta.row(0) + + (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->BTheta.row(1); + backSubContr.vecRot = + -(this->IPrimeS1PntSc1_B + this->omegaTilde_BN_B * this->IS1PntSc1_B) * this->omega_S1B_B - + (this->IPrimeS2PntSc2_B + this->omegaTilde_BN_B * this->IS2PntSc2_B) * this->omega_S2B_B - + (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->omegaTilde_S1B_B * + this->omega_S2S1_B - + this->mass1 * (this->rTilde_Sc1B_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * this->rTilde_Sc1B_B) * + this->rPrime_Sc1B_B - + this->mass2 * (this->rTilde_Sc2B_B * this->omegaTilde_S1B_B + this->omegaTilde_BN_B * this->rTilde_Sc2B_B) * + this->rPrime_Sc2B_B - + this->mass2 * this->rTilde_Sc2B_B * omegaTilde_S2S1_B * this->rPrime_Sc2S2_B - + (this->IS1PntSc1_B + this->IS2PntSc2_B - this->mass1 * this->rTilde_Sc1B_B * rTilde_Sc1S1_B - + this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S1_B) * + this->s1Hat_B * this->CTheta.row(0) - + (this->IS2PntSc2_B - this->mass2 * this->rTilde_Sc2B_B * rTilde_Sc2S2_B) * this->s2Hat_B * this->CTheta.row(1); } /*! This method is used to find the derivatives for the SB stateEffector: thetaDDot and the kinematic derivative */ -void SpinningBodyTwoDOFStateEffector::computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) +void +SpinningBodyTwoDOFStateEffector::computeDerivatives(double integTime, + Eigen::Vector3d rDDot_BN_N, + Eigen::Vector3d omegaDot_BN_B, + Eigen::Vector3d sigma_BN) { // Grab omegaDot_BN_B Eigen::Vector3d omegaDotLocal_BN_B; @@ -409,8 +453,13 @@ void SpinningBodyTwoDOFStateEffector::computeDerivatives(double integTime, Eigen this->theta2DotState->setDerivative(thetaDDot.row(1)); } -/*! This method is for calculating the contributions of the SB state effector to the energy and momentum of the spacecraft */ -void SpinningBodyTwoDOFStateEffector::updateEnergyMomContributions(double integTime, Eigen::Vector3d & rotAngMomPntCContr_B, double & rotEnergyContr, Eigen::Vector3d omega_BN_B) +/*! This method is for calculating the contributions of the SB state effector to the energy and momentum of the + * spacecraft */ +void +SpinningBodyTwoDOFStateEffector::updateEnergyMomContributions(double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B) { // Update omega_BN_B and omega_SN_B this->omega_BN_B = omega_BN_B; @@ -423,16 +472,22 @@ void SpinningBodyTwoDOFStateEffector::updateEnergyMomContributions(double integT this->rDot_Sc2B_B = this->rPrime_Sc2B_B + this->omegaTilde_BN_B * this->r_Sc2B_B; // Find rotational angular momentum contribution from hub - rotAngMomPntCContr_B = this->IS1PntSc1_B * this->omega_S1N_B + this->mass1 * this->rTilde_Sc1B_B * this->rDot_Sc1B_B - + this->IS2PntSc2_B * this->omega_S2N_B + this->mass2 * this->rTilde_Sc2B_B * this->rDot_Sc2B_B; + rotAngMomPntCContr_B = + this->IS1PntSc1_B * this->omega_S1N_B + this->mass1 * this->rTilde_Sc1B_B * this->rDot_Sc1B_B + + this->IS2PntSc2_B * this->omega_S2N_B + this->mass2 * this->rTilde_Sc2B_B * this->rDot_Sc2B_B; // Find rotational energy contribution from the hub - rotEnergyContr = 1.0 / 2.0 * this->omega_S1N_B.dot(this->IS1PntSc1_B * this->omega_S1N_B) + 1.0 / 2.0 * this->mass1 * this->rDot_Sc1B_B.dot(this->rDot_Sc1B_B) + 1.0 / 2.0 * this->k1 * (this->theta1 - this->theta1Ref) * (this->theta1 - this->theta1Ref) - + 1.0 / 2.0 * this->omega_S2N_B.dot(this->IS2PntSc2_B * this->omega_S2N_B) + 1.0 / 2.0 * this->mass2 * this->rDot_Sc2B_B.dot(this->rDot_Sc2B_B) + 1.0 / 2.0 * this->k2 * (this->theta2 - this->theta2Ref) * (this->theta2 - this->theta2Ref); + rotEnergyContr = 1.0 / 2.0 * this->omega_S1N_B.dot(this->IS1PntSc1_B * this->omega_S1N_B) + + 1.0 / 2.0 * this->mass1 * this->rDot_Sc1B_B.dot(this->rDot_Sc1B_B) + + 1.0 / 2.0 * this->k1 * (this->theta1 - this->theta1Ref) * (this->theta1 - this->theta1Ref) + + 1.0 / 2.0 * this->omega_S2N_B.dot(this->IS2PntSc2_B * this->omega_S2N_B) + + 1.0 / 2.0 * this->mass2 * this->rDot_Sc2B_B.dot(this->rDot_Sc2B_B) + + 1.0 / 2.0 * this->k2 * (this->theta2 - this->theta2Ref) * (this->theta2 - this->theta2Ref); } /*! This method computes the spinning body states relative to the inertial frame */ -void SpinningBodyTwoDOFStateEffector::computeSpinningBodyInertialStates() +void +SpinningBodyTwoDOFStateEffector::computeSpinningBodyInertialStates() { // Compute the inertial attitude Eigen::Matrix3d dcm_S1N; @@ -456,7 +511,8 @@ void SpinningBodyTwoDOFStateEffector::computeSpinningBodyInertialStates() } /*! This method is used so that the simulation will ask SB to update messages */ -void SpinningBodyTwoDOFStateEffector::UpdateState(uint64_t CurrentSimNanos) +void +SpinningBodyTwoDOFStateEffector::UpdateState(uint64_t CurrentSimNanos) { //! - Read the incoming command array if (this->motorTorqueInMsg.isLinked() && this->motorTorqueInMsg.isWritten()) { diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h index e33e3da85d..5576db4278 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesTwoDOF/spinningBodyTwoDOFStateEffector.h @@ -35,142 +35,221 @@ #include "architecture/utilities/bskLogging.h" /*! @brief spinning body state effector class */ -class SpinningBodyTwoDOFStateEffector: public StateEffector, public SysModel { -public: - double mass1 = 0.0; //!< [kg] mass of lower spinning body (can be 0) - double mass2 = 1.0; //!< [kg] mass of upper spinning body - double k1 = 0.0; //!< [N-m/rad] torsional spring constant for first rotation axis - double k2 = 0.0; //!< [N-m/rad] torsional spring constant for second rotation axis - double c1 = 0.0; //!< [N-m-s/rad] rotational damping coefficient for first rotation axis - double c2 = 0.0; //!< [N-m-s/rad] rotational damping coefficient for second rotation axis - double theta1Init = 0.0; //!< [rad] initial first axis angle - double theta1DotInit = 0.0; //!< [rad/s] initial first axis angle rate - double theta2Init = 0.0; //!< [rad] initial second axis angle - double theta2DotInit = 0.0; //!< [rad/s] initial second axis angle rate - std::string nameOfTheta1State; //!< -- identifier for the theta1 state data container - std::string nameOfTheta1DotState; //!< -- identifier for the thetaDot1 state data container - std::string nameOfTheta2State; //!< -- identifier for the theta2 state data container - std::string nameOfTheta2DotState; //!< -- identifier for the thetaDot2 state data container - Eigen::Vector3d r_S1B_B{0.0,0.0,0.0}; //!< [m] vector pointing from body frame B origin to lower spinning frame S1 origin in B frame components - Eigen::Vector3d r_S2S1_S1{0.0,0.0,0.0}; //!< [m] vector pointing from lower spinning frame S1 origin to upper spinning frame S2 origin in S1 frame components - Eigen::Vector3d r_Sc1S1_S1{0.0,0.0,0.0}; //!< [m] vector pointing from lower spinning frame S1 origin to point Sc1 (center of mass of the lower spinner) in S1 frame components - Eigen::Vector3d r_Sc2S2_S2{0.0,0.0,0.0}; //!< [m] vector pointing from upper spinning frame S2 origin to point Sc2 (center of mass of the upper spinner) in S2 frame components - Eigen::Vector3d s1Hat_S1{1.0,0.0,0.0}; //!< -- first spinning axis in S1 frame components. - Eigen::Vector3d s2Hat_S2{1.0,0.0,0.0}; //!< -- second spinning axis in S2 frame components. - Eigen::Matrix3d IS1PntSc1_S1; //!< [kg-m^2] Inertia of lower spinning body about point Sc1 in S1 frame components - Eigen::Matrix3d IS2PntSc2_S2; //!< [kg-m^2] Inertia of upper spinning body about point Sc2 in S2 frame components - Eigen::Matrix3d dcm_S10B; //!< -- DCM from the body frame to the S10 frame (S1 frame for theta1=0) - Eigen::Matrix3d dcm_S20S1; //!< -- DCM from the S1 frame to the S20 frame (S2 frame for theta2=0) - std::vector*> spinningBodyOutMsgs {new Message, - new Message}; //!< vector of state output messages - std::vector*> spinningBodyConfigLogOutMsgs {new Message, - new Message}; //!< vector of spinning body state config log messages - ReadFunctor motorTorqueInMsg; //!< -- (optional) motor torque input message name - ReadFunctor motorLockInMsg; //!< -- (optional) motor lock input message name - std::vector> spinningBodyRefInMsgs {ReadFunctor(), - ReadFunctor()}; //!< (optional) vector of spinning body reference input messages - - SpinningBodyTwoDOFStateEffector(); //!< -- Contructor - ~SpinningBodyTwoDOFStateEffector(); //!< -- Destructor - void Reset(uint64_t CurrentClock); //!< -- Method for reset - void writeOutputStateMessages(uint64_t CurrentClock); //!< -- Method for writing the output messages - void UpdateState(uint64_t CurrentSimNanos); //!< -- Method for updating information - void registerStates(DynParamManager& statesIn); //!< -- Method for registering the SB states - void linkInStates(DynParamManager& states); //!< -- Method for getting access to other states +class SpinningBodyTwoDOFStateEffector + : public StateEffector + , public SysModel +{ + public: + double mass1 = 0.0; //!< [kg] mass of lower spinning body (can be 0) + double mass2 = 1.0; //!< [kg] mass of upper spinning body + double k1 = 0.0; //!< [N-m/rad] torsional spring constant for first rotation axis + double k2 = 0.0; //!< [N-m/rad] torsional spring constant for second rotation axis + double c1 = 0.0; //!< [N-m-s/rad] rotational damping coefficient for first rotation axis + double c2 = 0.0; //!< [N-m-s/rad] rotational damping coefficient for second rotation axis + double theta1Init = 0.0; //!< [rad] initial first axis angle + double theta1DotInit = 0.0; //!< [rad/s] initial first axis angle rate + double theta2Init = 0.0; //!< [rad] initial second axis angle + double theta2DotInit = 0.0; //!< [rad/s] initial second axis angle rate + std::string nameOfTheta1State; //!< -- identifier for the theta1 state data container + std::string nameOfTheta1DotState; //!< -- identifier for the thetaDot1 state data container + std::string nameOfTheta2State; //!< -- identifier for the theta2 state data container + std::string nameOfTheta2DotState; //!< -- identifier for the thetaDot2 state data container + Eigen::Vector3d r_S1B_B{ + 0.0, + 0.0, + 0.0 + }; //!< [m] vector pointing from body frame B origin to lower spinning frame S1 origin in B frame components + Eigen::Vector3d r_S2S1_S1{ 0.0, 0.0, 0.0 }; //!< [m] vector pointing from lower spinning frame S1 origin to upper + //!< spinning frame S2 origin in S1 frame components + Eigen::Vector3d r_Sc1S1_S1{ 0.0, 0.0, 0.0 }; //!< [m] vector pointing from lower spinning frame S1 origin to point + //!< Sc1 (center of mass of the lower spinner) in S1 frame components + Eigen::Vector3d r_Sc2S2_S2{ 0.0, 0.0, 0.0 }; //!< [m] vector pointing from upper spinning frame S2 origin to point + //!< Sc2 (center of mass of the upper spinner) in S2 frame components + Eigen::Vector3d s1Hat_S1{ 1.0, 0.0, 0.0 }; //!< -- first spinning axis in S1 frame components. + Eigen::Vector3d s2Hat_S2{ 1.0, 0.0, 0.0 }; //!< -- second spinning axis in S2 frame components. + Eigen::Matrix3d IS1PntSc1_S1; //!< [kg-m^2] Inertia of lower spinning body about point Sc1 in S1 frame components + Eigen::Matrix3d IS2PntSc2_S2; //!< [kg-m^2] Inertia of upper spinning body about point Sc2 in S2 frame components + Eigen::Matrix3d dcm_S10B; //!< -- DCM from the body frame to the S10 frame (S1 frame for theta1=0) + Eigen::Matrix3d dcm_S20S1; //!< -- DCM from the S1 frame to the S20 frame (S2 frame for theta2=0) + std::vector*> spinningBodyOutMsgs{ + new Message, + new Message + }; //!< vector of state output messages + std::vector*> spinningBodyConfigLogOutMsgs{ + new Message, + new Message + }; //!< vector of spinning body state config log messages + ReadFunctor motorTorqueInMsg; //!< -- (optional) motor torque input message name + ReadFunctor motorLockInMsg; //!< -- (optional) motor lock input message name + std::vector> spinningBodyRefInMsgs{ + ReadFunctor(), + ReadFunctor() + }; //!< (optional) vector of spinning body reference input messages + + SpinningBodyTwoDOFStateEffector(); //!< -- Contructor + ~SpinningBodyTwoDOFStateEffector(); //!< -- Destructor + void Reset(uint64_t CurrentClock); //!< -- Method for reset + void writeOutputStateMessages(uint64_t CurrentClock); //!< -- Method for writing the output messages + void UpdateState(uint64_t CurrentSimNanos); //!< -- Method for updating information + void registerStates(DynParamManager& statesIn); //!< -- Method for registering the SB states + void linkInStates(DynParamManager& states); //!< -- Method for getting access to other states void updateContributions(double integTime, BackSubMatrices& backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, - Eigen::Vector3d g_N); //!< -- Method for back-substitution contributions + Eigen::Vector3d g_N); //!< -- Method for back-substitution contributions void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, - Eigen::Vector3d sigma_BN); //!< -- Method for SB to compute its derivatives - void updateEffectorMassProps(double integTime); //!< -- Method for giving the s/c the HRB mass props and prop rates - void updateEnergyMomContributions(double integTime, - Eigen::Vector3d& rotAngMomPntCContr_B, - double& rotEnergyContr, - Eigen::Vector3d omega_BN_B); //!< -- Method for computing energy and momentum for SBs - void prependSpacecraftNameToStates(); //!< Method used for multiple spacecraft - void computeSpinningBodyInertialStates(); //!< Method for computing the SB's states - -private: - static uint64_t effectorID; //!< [] ID number of this panel - double u1 = 0.0; //!< [N-m] optional motor torque for first axis - double u2 = 0.0; //!< [N-m] optional motor torque for second axis - int lockFlag1 = 0; //!< [] flag for locking the first rotation axis - int lockFlag2 = 0; //!< [] flag for locking the second rotation axis - double theta1Ref = 0.0; //!< [rad] spinning body reference angle - double theta1DotRef = 0.0; //!< [rad] spinning body reference angle rate - double theta2Ref = 0.0; //!< [rad] spinning body reference angle - double theta2DotRef = 0.0; //!< [rad] spinning body reference angle rate - double mass = 1.0; //!< [kg] mass of the spinner system + Eigen::Vector3d sigma_BN); //!< -- Method for SB to compute its derivatives + void updateEffectorMassProps(double integTime); //!< -- Method for giving the s/c the HRB mass props and prop rates + void updateEnergyMomContributions( + double integTime, + Eigen::Vector3d& rotAngMomPntCContr_B, + double& rotEnergyContr, + Eigen::Vector3d omega_BN_B); //!< -- Method for computing energy and momentum for SBs + void prependSpacecraftNameToStates(); //!< Method used for multiple spacecraft + void computeSpinningBodyInertialStates(); //!< Method for computing the SB's states + + private: + static uint64_t effectorID; //!< [] ID number of this panel + double u1 = 0.0; //!< [N-m] optional motor torque for first axis + double u2 = 0.0; //!< [N-m] optional motor torque for second axis + int lockFlag1 = 0; //!< [] flag for locking the first rotation axis + int lockFlag2 = 0; //!< [] flag for locking the second rotation axis + double theta1Ref = 0.0; //!< [rad] spinning body reference angle + double theta1DotRef = 0.0; //!< [rad] spinning body reference angle rate + double theta2Ref = 0.0; //!< [rad] spinning body reference angle + double theta2DotRef = 0.0; //!< [rad] spinning body reference angle rate + double mass = 1.0; //!< [kg] mass of the spinner system // Terms needed for back substitution - Eigen::Matrix ATheta; //!< -- rDDot_BN term for back substitution - Eigen::Matrix BTheta; //!< -- omegaDot_BN term for back substitution - Eigen::Vector2d CTheta{0.0,0.0}; //!< -- scalar term for back substitution + Eigen::Matrix ATheta; //!< -- rDDot_BN term for back substitution + Eigen::Matrix BTheta; //!< -- omegaDot_BN term for back substitution + Eigen::Vector2d CTheta{ 0.0, 0.0 }; //!< -- scalar term for back substitution // Vector quantities - Eigen::Vector3d s1Hat_B{1.0,0.0,0.0}; //!< -- first spinning axis in B frame components - Eigen::Vector3d s2Hat_B{1.0,0.0,0.0}; //!< -- second spinning axis in B frame components - Eigen::Vector3d r_Sc1S1_B{0.0,0.0,0.0}; //!< [m] vector pointing from lower spinning frame S1 origin to point Sc1 in B frame components - Eigen::Vector3d r_Sc1B_B{0.0,0.0,0.0}; //!< [m] vector pointing from body frame B origin to point Sc1 in B frame components. - Eigen::Vector3d r_Sc2S2_B{0.0,0.0,0.0}; //!< [m] vector pointing from upper spinning frame S2 origin to point Sc2 in B frame components - Eigen::Vector3d r_S2S1_B{0.0,0.0,0.0}; //!< [m] vector pointing from lower spinning frame S1 origin to upper spinning frame S2 origin in B frame components - Eigen::Vector3d r_Sc2S1_B{0.0,0.0,0.0}; //!< [m] vector pointing from lower spinning frame S1 origin to point Sc2 in B frame components - Eigen::Vector3d r_Sc2B_B{0.0,0.0,0.0}; //!< [m] vector pointing from body frame B origin to point Sc2 in B frame components. - Eigen::Vector3d r_ScB_B{0.0,0.0,0.0}; //!< [m] vector pointing from body frame B origin to point Sc (center of mass of the spinner system) in B frame components. - Eigen::Vector3d rPrime_Sc1S1_B{0.0,0.0,0.0}; //!< [m/s] body frame time derivative of r_Sc1S1_B - Eigen::Vector3d rPrime_Sc2S2_B{0.0,0.0,0.0}; //!< [m/s] body frame time derivative of r_Sc2S2_B - Eigen::Vector3d rPrime_S2S1_B{0.0,0.0,0.0}; //!< [m/s] body frame time derivative of r_S2S1_B - Eigen::Vector3d rPrime_Sc1B_B{0.0,0.0,0.0}; //!< [m/s] body frame time derivative of r_Sc1B_B - Eigen::Vector3d rPrime_Sc2B_B{0.0,0.0,0.0}; //!< [m/s] body frame time derivative of r_Sc2B_B - Eigen::Vector3d rPrime_Sc2S1_B{0.0,0.0,0.0}; //!< [m/s] body frame time derivative of r_Sc2S1_B - Eigen::Vector3d rPrime_ScB_B{0.0,0.0,0.0}; //!< [m/s] body frame time derivative of r_ScB_B - Eigen::Vector3d rDot_Sc1B_B{0.0,0.0,0.0}; //!< [m/s] inertial frame time derivative of r_Sc1B_B - Eigen::Vector3d rDot_Sc2B_B{0.0,0.0,0.0}; //!< [m/s] inertial frame time derivative of r_Sc2B_B - Eigen::Vector3d omega_S1B_B{0.0,0.0,0.0}; //!< [rad/s] angular velocity of the S1 frame wrt the B frame in B frame components - Eigen::Vector3d omega_S2S1_B{0.0,0.0,0.0}; //!< [rad/s] angular velocity of the S2 frame wrt the S1 frame in B frame components - Eigen::Vector3d omega_S2B_B{0.0,0.0,0.0}; //!< [rad/s] angular velocity of the S2 frame wrt the B frame in B frame components - Eigen::Vector3d omega_BN_B{0.0,0.0,0.0}; //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components - Eigen::Vector3d omega_S1N_B{0.0,0.0,0.0}; //!< [rad/s] angular velocity of the S1 frame wrt the N frame in B frame components - Eigen::Vector3d omega_S2N_B{0.0,0.0,0.0}; //!< [rad/s] angular velocity of the S2 frame wrt the N frame in B frame components - Eigen::MRPd sigma_BN{0.0,0.0,0.0}; //!< -- body frame attitude wrt to the N frame in MRPs + Eigen::Vector3d s1Hat_B{ 1.0, 0.0, 0.0 }; //!< -- first spinning axis in B frame components + Eigen::Vector3d s2Hat_B{ 1.0, 0.0, 0.0 }; //!< -- second spinning axis in B frame components + Eigen::Vector3d r_Sc1S1_B{ + 0.0, + 0.0, + 0.0 + }; //!< [m] vector pointing from lower spinning frame S1 origin to point Sc1 in B frame components + Eigen::Vector3d r_Sc1B_B{ + 0.0, + 0.0, + 0.0 + }; //!< [m] vector pointing from body frame B origin to point Sc1 in B frame components. + Eigen::Vector3d r_Sc2S2_B{ + 0.0, + 0.0, + 0.0 + }; //!< [m] vector pointing from upper spinning frame S2 origin to point Sc2 in B frame components + Eigen::Vector3d r_S2S1_B{ 0.0, 0.0, 0.0 }; //!< [m] vector pointing from lower spinning frame S1 origin to upper + //!< spinning frame S2 origin in B frame components + Eigen::Vector3d r_Sc2S1_B{ + 0.0, + 0.0, + 0.0 + }; //!< [m] vector pointing from lower spinning frame S1 origin to point Sc2 in B frame components + Eigen::Vector3d r_Sc2B_B{ + 0.0, + 0.0, + 0.0 + }; //!< [m] vector pointing from body frame B origin to point Sc2 in B frame components. + Eigen::Vector3d r_ScB_B{ 0.0, 0.0, 0.0 }; //!< [m] vector pointing from body frame B origin to point Sc (center of + //!< mass of the spinner system) in B frame components. + Eigen::Vector3d rPrime_Sc1S1_B{ 0.0, 0.0, 0.0 }; //!< [m/s] body frame time derivative of r_Sc1S1_B + Eigen::Vector3d rPrime_Sc2S2_B{ 0.0, 0.0, 0.0 }; //!< [m/s] body frame time derivative of r_Sc2S2_B + Eigen::Vector3d rPrime_S2S1_B{ 0.0, 0.0, 0.0 }; //!< [m/s] body frame time derivative of r_S2S1_B + Eigen::Vector3d rPrime_Sc1B_B{ 0.0, 0.0, 0.0 }; //!< [m/s] body frame time derivative of r_Sc1B_B + Eigen::Vector3d rPrime_Sc2B_B{ 0.0, 0.0, 0.0 }; //!< [m/s] body frame time derivative of r_Sc2B_B + Eigen::Vector3d rPrime_Sc2S1_B{ 0.0, 0.0, 0.0 }; //!< [m/s] body frame time derivative of r_Sc2S1_B + Eigen::Vector3d rPrime_ScB_B{ 0.0, 0.0, 0.0 }; //!< [m/s] body frame time derivative of r_ScB_B + Eigen::Vector3d rDot_Sc1B_B{ 0.0, 0.0, 0.0 }; //!< [m/s] inertial frame time derivative of r_Sc1B_B + Eigen::Vector3d rDot_Sc2B_B{ 0.0, 0.0, 0.0 }; //!< [m/s] inertial frame time derivative of r_Sc2B_B + Eigen::Vector3d omega_S1B_B{ + 0.0, + 0.0, + 0.0 + }; //!< [rad/s] angular velocity of the S1 frame wrt the B frame in B frame components + Eigen::Vector3d omega_S2S1_B{ + 0.0, + 0.0, + 0.0 + }; //!< [rad/s] angular velocity of the S2 frame wrt the S1 frame in B frame components + Eigen::Vector3d omega_S2B_B{ + 0.0, + 0.0, + 0.0 + }; //!< [rad/s] angular velocity of the S2 frame wrt the B frame in B frame components + Eigen::Vector3d omega_BN_B{ + 0.0, + 0.0, + 0.0 + }; //!< [rad/s] angular velocity of the B frame wrt the N frame in B frame components + Eigen::Vector3d omega_S1N_B{ + 0.0, + 0.0, + 0.0 + }; //!< [rad/s] angular velocity of the S1 frame wrt the N frame in B frame components + Eigen::Vector3d omega_S2N_B{ + 0.0, + 0.0, + 0.0 + }; //!< [rad/s] angular velocity of the S2 frame wrt the N frame in B frame components + Eigen::MRPd sigma_BN{ 0.0, 0.0, 0.0 }; //!< -- body frame attitude wrt to the N frame in MRPs // Matrix quantities - Eigen::Matrix3d rTilde_Sc1B_B; //!< [m] tilde matrix of r_Sc1B_B - Eigen::Matrix3d rTilde_Sc2B_B; //!< [m] tilde matrix of r_Sc2B_B - Eigen::Matrix3d omegaTilde_S1B_B; //!< [rad/s] tilde matrix of omega_S1B_B - Eigen::Matrix3d omegaTilde_S2B_B; //!< [rad/s] tilde matrix of omega_S2B_B - Eigen::Matrix3d omegaTilde_BN_B; //!< [rad/s] tilde matrix of omega_BN_B - Eigen::Matrix3d dcm_BS1; //!< -- DCM from lower spinner frame to body frame - Eigen::Matrix3d dcm_BS2; //!< -- DCM from upper spinner frame to body frame - Eigen::Matrix3d dcm_BN; //!< -- DCM from inertial frame to body frame - Eigen::Matrix3d IS1PntSc1_B; //!< [kg-m^2] inertia of lower spinning body about point Sc1 in B frame components - Eigen::Matrix3d IS2PntSc2_B; //!< [kg-m^2] inertia of upper spinning body about point Sc2 in B frame components - Eigen::Matrix3d IPrimeS1PntSc1_B; //!< [kg-m^2] body frame time derivative of inertia of inertia of lower spinning body about point Sc1 in B frame components - Eigen::Matrix3d IPrimeS2PntSc2_B; //!< [kg-m^2] body frame time derivative of inertia of inertia of upper spinning body about point Sc2 in B frame components + Eigen::Matrix3d rTilde_Sc1B_B; //!< [m] tilde matrix of r_Sc1B_B + Eigen::Matrix3d rTilde_Sc2B_B; //!< [m] tilde matrix of r_Sc2B_B + Eigen::Matrix3d omegaTilde_S1B_B; //!< [rad/s] tilde matrix of omega_S1B_B + Eigen::Matrix3d omegaTilde_S2B_B; //!< [rad/s] tilde matrix of omega_S2B_B + Eigen::Matrix3d omegaTilde_BN_B; //!< [rad/s] tilde matrix of omega_BN_B + Eigen::Matrix3d dcm_BS1; //!< -- DCM from lower spinner frame to body frame + Eigen::Matrix3d dcm_BS2; //!< -- DCM from upper spinner frame to body frame + Eigen::Matrix3d dcm_BN; //!< -- DCM from inertial frame to body frame + Eigen::Matrix3d IS1PntSc1_B; //!< [kg-m^2] inertia of lower spinning body about point Sc1 in B frame components + Eigen::Matrix3d IS2PntSc2_B; //!< [kg-m^2] inertia of upper spinning body about point Sc2 in B frame components + Eigen::Matrix3d IPrimeS1PntSc1_B; //!< [kg-m^2] body frame time derivative of inertia of inertia of lower spinning + //!< body about point Sc1 in B frame components + Eigen::Matrix3d IPrimeS2PntSc2_B; //!< [kg-m^2] body frame time derivative of inertia of inertia of upper spinning + //!< body about point Sc2 in B frame components // Spinning body properties - Eigen::Vector3d r_Sc1N_N{0.0,0.0,0.0}; //!< [m] position vector of lower spinning body center of mass Sc1 relative to the inertial frame origin N - Eigen::Vector3d r_Sc2N_N{0.0,0.0,0.0}; //!< [m] position vector of upper spinning body center of mass Sc2 relative to the inertial frame origin N - Eigen::Vector3d v_Sc1N_N{0.0,0.0,0.0}; //!< [m/s] inertial velocity vector of Sc1 relative to inertial frame - Eigen::Vector3d v_Sc2N_N{0.0,0.0,0.0}; //!< [m/s] inertial velocity vector of Sc2 relative to inertial frame - Eigen::Vector3d sigma_S1N{0.0,0.0,0.0}; //!< -- MRP attitude of frame S1 relative to inertial frame - Eigen::Vector3d sigma_S2N{0.0,0.0,0.0}; //!< -- MRP attitude of frame S2 relative to inertial frame - Eigen::Vector3d omega_S1N_S1{0.0,0.0,0.0}; //!< [rad/s] inertial lower spinning body frame angular velocity vector - Eigen::Vector3d omega_S2N_S2{0.0,0.0,0.0}; //!< [rad/s] inertial upper spinning body frame angular velocity vector + Eigen::Vector3d r_Sc1N_N{ + 0.0, + 0.0, + 0.0 + }; //!< [m] position vector of lower spinning body center of mass Sc1 relative to the inertial frame origin N + Eigen::Vector3d r_Sc2N_N{ + 0.0, + 0.0, + 0.0 + }; //!< [m] position vector of upper spinning body center of mass Sc2 relative to the inertial frame origin N + Eigen::Vector3d v_Sc1N_N{ 0.0, 0.0, 0.0 }; //!< [m/s] inertial velocity vector of Sc1 relative to inertial frame + Eigen::Vector3d v_Sc2N_N{ 0.0, 0.0, 0.0 }; //!< [m/s] inertial velocity vector of Sc2 relative to inertial frame + Eigen::Vector3d sigma_S1N{ 0.0, 0.0, 0.0 }; //!< -- MRP attitude of frame S1 relative to inertial frame + Eigen::Vector3d sigma_S2N{ 0.0, 0.0, 0.0 }; //!< -- MRP attitude of frame S2 relative to inertial frame + Eigen::Vector3d omega_S1N_S1{ 0.0, + 0.0, + 0.0 }; //!< [rad/s] inertial lower spinning body frame angular velocity vector + Eigen::Vector3d omega_S2N_S2{ 0.0, + 0.0, + 0.0 }; //!< [rad/s] inertial upper spinning body frame angular velocity vector // States - double theta1 = 0.0; //!< [rad] first axis angle - double theta1Dot = 0.0; //!< [rad/s] first axis angle rate - double theta2 = 0.0; //!< [rad] second axis angle - double theta2Dot = 0.0; //!< [rad/s] second axis angle rate - Eigen::MatrixXd* inertialPositionProperty = nullptr; //!< [m] r_N inertial position relative to system spice zeroBase/refBase - Eigen::MatrixXd* inertialVelocityProperty = nullptr; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase - StateData *theta1State = nullptr; //!< -- state manager of theta1 for spinning body - StateData *theta1DotState = nullptr; //!< -- state manager of theta1Dot for spinning body + double theta1 = 0.0; //!< [rad] first axis angle + double theta1Dot = 0.0; //!< [rad/s] first axis angle rate + double theta2 = 0.0; //!< [rad] second axis angle + double theta2Dot = 0.0; //!< [rad/s] second axis angle rate + Eigen::MatrixXd* inertialPositionProperty = + nullptr; //!< [m] r_N inertial position relative to system spice zeroBase/refBase + Eigen::MatrixXd* inertialVelocityProperty = + nullptr; //!< [m] v_N inertial velocity relative to system spice zeroBase/refBase + StateData* theta1State = nullptr; //!< -- state manager of theta1 for spinning body + StateData* theta1DotState = nullptr; //!< -- state manager of theta1Dot for spinning body StateData* theta2State = nullptr; //!< -- state manager of theta2 for spinning body StateData* theta2DotState = nullptr; //!< -- state manager of theta2Dot for spinning body }; 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/_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/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/ExponentialAtmosphere/exponentialAtmosphere.cpp b/src/simulation/environment/ExponentialAtmosphere/exponentialAtmosphere.cpp index ec9bde826d..fa0a17e8be 100644 --- a/src/simulation/environment/ExponentialAtmosphere/exponentialAtmosphere.cpp +++ b/src/simulation/environment/ExponentialAtmosphere/exponentialAtmosphere.cpp @@ -20,16 +20,17 @@ #include "exponentialAtmosphere.h" #include "architecture/utilities/linearAlgebra.h" -/*! The constructor method initializes the dipole parameters to zero, resuling in a zero magnetic field result by default. +/*! The constructor method initializes the dipole parameters to zero, resuling in a zero magnetic field result by + default. */ ExponentialAtmosphere::ExponentialAtmosphere() { //! - Set the default atmospheric properties to yield a zero response - this->baseDensity = 0.0; // [T] - this->scaleHeight = 1.0; // [m] - this->planetRadius = 0.0; // [m] - this->localTemp = 1.0; // [K] + this->baseDensity = 0.0; // [T] + this->scaleHeight = 1.0; // [m] + this->planetRadius = 0.0; // [m] + this->localTemp = 1.0; // [K] return; } @@ -47,7 +48,8 @@ ExponentialAtmosphere::~ExponentialAtmosphere() @param currentTime current time (s) */ -void ExponentialAtmosphere::evaluateAtmosphereModel(AtmoPropsMsgPayload *msg, double currentTime) +void +ExponentialAtmosphere::evaluateAtmosphereModel(AtmoPropsMsgPayload* msg, double currentTime) { msg->neutralDensity = this->baseDensity * exp(-(this->orbitAltitude) / this->scaleHeight); msg->localTemp = this->localTemp; diff --git a/src/simulation/environment/ExponentialAtmosphere/exponentialAtmosphere.h b/src/simulation/environment/ExponentialAtmosphere/exponentialAtmosphere.h index 7e398f96a7..d5acd6d183 100644 --- a/src/simulation/environment/ExponentialAtmosphere/exponentialAtmosphere.h +++ b/src/simulation/environment/ExponentialAtmosphere/exponentialAtmosphere.h @@ -17,7 +17,6 @@ */ - #ifndef EXPONENTIAL_ATMOSPHERE_H #define EXPONENTIAL_ATMOSPHERE_H @@ -30,21 +29,20 @@ #include "architecture/utilities/bskLogging.h" /*! @brief exponential atmosphere model */ -class ExponentialAtmosphere: public AtmosphereBase { -public: +class ExponentialAtmosphere : public AtmosphereBase +{ + public: ExponentialAtmosphere(); ~ExponentialAtmosphere(); -private: - void evaluateAtmosphereModel(AtmoPropsMsgPayload *msg, double currentTime); - + private: + void evaluateAtmosphereModel(AtmoPropsMsgPayload* msg, double currentTime); -public: - double baseDensity; //!< [kg/m^3] Density at h=0 - double scaleHeight; //!< [m] Exponential characteristic height - double localTemp = 293.0; //!< [K] Local atmospheric temperature; set to be constant. - BSKLogger bskLogger; //!< -- BSK Logging + public: + double baseDensity; //!< [kg/m^3] Density at h=0 + double scaleHeight; //!< [m] Exponential characteristic height + double localTemp = 293.0; //!< [K] Local atmospheric temperature; set to be constant. + BSKLogger bskLogger; //!< -- BSK Logging }; - #endif /* EXPONENTIAL_ATMOSPHERE_H */ 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/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/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.cpp b/src/simulation/environment/MsisAtmosphere/msisAtmosphere.cpp index 7ffb2ddb24..0764a65b38 100644 --- a/src/simulation/environment/MsisAtmosphere/msisAtmosphere.cpp +++ b/src/simulation/environment/MsisAtmosphere/msisAtmosphere.cpp @@ -28,29 +28,29 @@ MsisAtmosphere::MsisAtmosphere() { //! - Set the default atmospheric properties - this->planetRadius = REQ_EARTH*1000.; // must be the radius of Earth for MSIS + this->planetRadius = REQ_EARTH * 1000.; // must be the radius of Earth for MSIS - this->epochDoy = -1; // negative value means this is not set + this->epochDoy = -1; // negative value means this is not set this->f107A = 0.0; this->f107 = 0.0; this->ap = 0.0; - for(int apInd = 0; apInd < 7; ++apInd) { + for (int apInd = 0; apInd < 7; ++apInd) { this->aph.a[apInd] = 0.0; } this->msisInput.ap_a = &this->aph; this->updateInputParams(); - - this->msisFlags.switches[0] = 1; //! NRLMSISE-00 should output in kg/m^3 for consistency with other atmospheric modules. + this->msisFlags.switches[0] = + 1; //! NRLMSISE-00 should output in kg/m^3 for consistency with other atmospheric modules. //! Set default settings for NRLMSISE-00; we're using all the settings by default - for(int switchInd = 1; switchInd < 24; ++switchInd){ + for (int switchInd = 1; switchInd < 24; ++switchInd) { this->msisFlags.switches[switchInd] = 1; } // Set other required interface values for (int c = 0; c < 23; c++) { - ReadFunctor *msgIn; + ReadFunctor* msgIn; msgIn = new ReadFunctor; this->swDataInMsgs.push_back(*msgIn); } @@ -92,59 +92,62 @@ MsisAtmosphere::~MsisAtmosphere() return; } - /*! This method is used to reset the module. */ -void MsisAtmosphere::customReset(uint64_t CurrentSimNanos) +void +MsisAtmosphere::customReset(uint64_t CurrentSimNanos) { - for(int ind = 0; ind < 23; ind++) { + for (int ind = 0; ind < 23; ind++) { if (!this->swDataInMsgs[ind].isLinked()) { bskLogger.bskLog(BSK_ERROR, "Required MSIS input messages No. %d are not connected.", ind); } } } - -/*! Custom customSetEpochFromVariable() method. This allows specifying epochDoy directly from Python. If an epoch message is set then this variable is not used. +/*! Custom customSetEpochFromVariable() method. This allows specifying epochDoy directly from Python. If an epoch + message is set then this variable is not used. */ -void MsisAtmosphere::customSetEpochFromVariable() +void +MsisAtmosphere::customSetEpochFromVariable() { - //! - only convert if the day-in-year variable was set to a non-zero value. Otherwise use the BSK epoch default setup by the base class. + //! - only convert if the day-in-year variable was set to a non-zero value. Otherwise use the BSK epoch default + //! setup by the base class. if (this->epochDoy > 0.0) { - /* here the BSK default epoch year is used on Jan 1, mid-night, and the requested days of year are added and converted to a proper date-time structure */ - this->epochDateTime.tm_mday = this->epochDoy; // assumes 1 is first day of year + /* here the BSK default epoch year is used on Jan 1, mid-night, and the requested days of year are added and + * converted to a proper date-time structure */ + this->epochDateTime.tm_mday = this->epochDoy; // assumes 1 is first day of year mktime(&this->epochDateTime); } return; } - /*! This method is used to write the output densities whose names are established in AddSpacecraftToModel. @param CurrentClock The current time used for time-stamping the message */ -void MsisAtmosphere::customWriteMessages(uint64_t CurrentClock) +void +MsisAtmosphere::customWriteMessages(uint64_t CurrentClock) { - /* [WIP] - Include additional outputs for other MSISE outputs (species count, etc.)*/ - + /* [WIP] - Include additional outputs for other MSISE outputs (species count, etc.)*/ } - /*! This method is used to read the incoming command message and set the associated spacecraft positions for computing the atmosphere. */ -bool MsisAtmosphere::customReadMessages(){ +bool +MsisAtmosphere::customReadMessages() +{ bool swRead = false; int failCount = 0; SwDataMsgPayload tmpSwData; this->swDataList.clear(); //! Iterate over swData message ids - for(int ind = 0; ind < 23; ind++) { + for (int ind = 0; ind < 23; ind++) { tmpSwData = this->swDataInMsgs.at(ind)(); swRead = this->swDataInMsgs.at(ind).isWritten(); if (swRead) { @@ -157,11 +160,11 @@ bool MsisAtmosphere::customReadMessages(){ if (failCount > 0) { swRead = false; } - return(swRead); + return (swRead); } - -void MsisAtmosphere::updateInputParams() +void +MsisAtmosphere::updateInputParams() { this->msisInput.ap = this->ap; this->msisInput.ap_a->a[0] = this->aph.a[0]; @@ -174,10 +177,10 @@ void MsisAtmosphere::updateInputParams() this->msisInput.f107A = this->f107A; this->msisInput.f107 = this->f107; - } -void MsisAtmosphere::updateSwIndices() +void +MsisAtmosphere::updateSwIndices() { this->ap = this->swDataList[0].dataValue; this->aph.a[0] = this->swDataList[0].dataValue; @@ -188,64 +191,61 @@ void MsisAtmosphere::updateSwIndices() uint64_t mth; double tmp_avg = 0.0; - for(mth = 5; mth < 13; mth++){ + for (mth = 5; mth < 13; mth++) { tmp_avg = tmp_avg + this->swDataList[mth].dataValue; } - this->aph.a[5] = tmp_avg/8.0; + this->aph.a[5] = tmp_avg / 8.0; uint64_t nth; tmp_avg = 0.0; - for(nth = 13; nth < 21; nth++){ + for (nth = 13; nth < 21; nth++) { tmp_avg = tmp_avg + this->swDataList[nth].dataValue; } - this->aph.a[6] = tmp_avg/8.0; + this->aph.a[6] = tmp_avg / 8.0; this->f107A = this->swDataList[21].dataValue; this->f107 = this->swDataList[22].dataValue; - } -void MsisAtmosphere::evaluateAtmosphereModel(AtmoPropsMsgPayload *msg, double currentTime) +void +MsisAtmosphere::evaluateAtmosphereModel(AtmoPropsMsgPayload* msg, double currentTime) { this->updateSwIndices(); this->updateInputParams(); //! Compute the geodetic position using the planet orientation. this->currentLLA = PCI2LLA(this->r_BP_N, this->planetState.J20002Pfix, this->planetRadius); - this->msisInput.g_lat = R2D*this->currentLLA[0]; - this->msisInput.g_long = R2D*this->currentLLA[1]; - this->msisInput.alt = this->currentLLA[2]/1000.0; // NRLMSISE Altitude input must be in kilometers! + this->msisInput.g_lat = R2D * this->currentLLA[0]; + this->msisInput.g_long = R2D * this->currentLLA[1]; + this->msisInput.alt = this->currentLLA[2] / 1000.0; // NRLMSISE Altitude input must be in kilometers! //! Update time. - struct tm localDateTime; // [] date/time structure + struct tm localDateTime; // [] date/time structure localDateTime = this->epochDateTime; - localDateTime.tm_sec += (int) currentTime; // sets the current seconds + localDateTime.tm_sec += (int)currentTime; // sets the current seconds mktime(&localDateTime); this->msisInput.year = localDateTime.tm_year + 1900; - this->msisInput.doy = localDateTime.tm_yday + 1; // Jan 1 is the 1st day of year, not 0th - double fracSecond = currentTime - (int) currentTime; - this->msisInput.sec = localDateTime.tm_hour * 3600.0 + localDateTime.tm_min * 60.0 + localDateTime.tm_sec + fracSecond; + this->msisInput.doy = localDateTime.tm_yday + 1; // Jan 1 is the 1st day of year, not 0th + double fracSecond = currentTime - (int)currentTime; + this->msisInput.sec = + localDateTime.tm_hour * 3600.0 + localDateTime.tm_min * 60.0 + localDateTime.tm_sec + fracSecond; // WIP - need to actually figure out how to pull in these values. - this->msisInput.lst = this->msisInput.sec/3600.0 + this->msisInput.g_long/15.0; + this->msisInput.lst = this->msisInput.sec / 3600.0 + this->msisInput.g_long / 15.0; //! NRLMSISE-00 uses different models depending on the altitude. - if(this->msisInput.alt < 500.0){ - gtd7(&this->msisInput, \ - &this->msisFlags, \ - &this->msisOutput); + if (this->msisInput.alt < 500.0) { + gtd7(&this->msisInput, &this->msisFlags, &this->msisOutput); } - /* GTD7D */ - /* This subroutine provides Effective Total Mass Density for output - * d[5] which includes contributions from "anomalous oxygen" which can - * affect satellite drag above 500 km. See the section "output" for - * additional details. - */ - else if(this->msisInput.alt >= 500.0){ - gtd7d(&this->msisInput, \ - &this->msisFlags, \ - &this->msisOutput); + /* GTD7D */ + /* This subroutine provides Effective Total Mass Density for output + * d[5] which includes contributions from "anomalous oxygen" which can + * affect satellite drag above 500 km. See the section "output" for + * additional details. + */ + else if (this->msisInput.alt >= 500.0) { + gtd7d(&this->msisInput, &this->msisFlags, &this->msisOutput); } msg->neutralDensity = this->msisOutput.d[5]; msg->localTemp = this->msisOutput.t[1]; diff --git a/src/simulation/environment/MsisAtmosphere/msisAtmosphere.h b/src/simulation/environment/MsisAtmosphere/msisAtmosphere.h index 6af673b8b2..0133da7b28 100644 --- a/src/simulation/environment/MsisAtmosphere/msisAtmosphere.h +++ b/src/simulation/environment/MsisAtmosphere/msisAtmosphere.h @@ -17,14 +17,13 @@ */ - #ifndef MSIS_ATMOSPHERE_H #define MSIS_ATMOSPHERE_H #include #include #include -#include +#include #include "architecture/_GeneralModuleFiles/sys_model.h" #include "simulation/environment/_GeneralModuleFiles/atmosphereBase.h" @@ -33,48 +32,46 @@ #include "architecture/utilities/bskLogging.h" -extern "C" { - #include "nrlmsise-00.h" +extern "C" +{ +#include "nrlmsise-00.h" } - /*! @brief MSIS athomsphere model */ -class MsisAtmosphere: public AtmosphereBase { -public: +class MsisAtmosphere : public AtmosphereBase +{ + public: MsisAtmosphere(); ~MsisAtmosphere(); -private: + private: void customWriteMessages(uint64_t CurrentClock); bool customReadMessages(); void customReset(uint64_t CurrentClock); bool ReadInputs(); void updateInputParams(); void updateSwIndices(); - void evaluateAtmosphereModel(AtmoPropsMsgPayload *msg, double currentTime); + void evaluateAtmosphereModel(AtmoPropsMsgPayload* msg, double currentTime); void customSetEpochFromVariable(); -public: + public: std::vector> swDataInMsgs; //!< Vector of space weather input message names - int epochDoy; //!< [day] Day-of-Year at epoch - BSKLogger bskLogger; //!< -- BSK Logging - + int epochDoy; //!< [day] Day-of-Year at epoch + BSKLogger bskLogger; //!< -- BSK Logging -private: - Eigen::Vector3d currentLLA; //!< [-] Container for local Latitude, Longitude, Altitude geodetic position; units are rad and km respectively. + private: + Eigen::Vector3d currentLLA; //!< [-] Container for local Latitude, Longitude, Altitude geodetic position; units are + //!< rad and km respectively. std::vector swDataList; //!< Vector of space weather messages // NRLMSISE-00 Specific attributes - nrlmsise_input msisInput; //!< Struct containing NRLMSISE-00 input values; see their doc for details. + nrlmsise_input msisInput; //!< Struct containing NRLMSISE-00 input values; see their doc for details. nrlmsise_output msisOutput; //!< Struct containing NRLMSISE-00 output values; see their doc for details. nrlmsise_flags msisFlags; ap_array aph; double ap; double f107; double f107A; - - }; - #endif /* EXPONENTIAL_ATMOSPHERE_H */ 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..368780542f 100644 --- a/src/simulation/environment/MsisAtmosphere/nrlmsise-00.c +++ b/src/simulation/environment/MsisAtmosphere/nrlmsise-00.c @@ -16,18 +16,14 @@ * updated releases of this package. */ - - /* ------------------------------------------------------------------- */ /* ------------------------------ INCLUDES --------------------------- */ /* ------------------------------------------------------------------- */ -#include "nrlmsise-00.h" /* header for nrlmsise-00.h */ -#include /* maths functions */ -#include /* for error messages. TBD: remove this */ -#include /* for malloc/free */ - - +#include "nrlmsise-00.h" /* header for nrlmsise-00.h */ +#include /* maths functions */ +#include /* for error messages. TBD: remove this */ +#include /* for malloc/free */ /* ------------------------------------------------------------------- */ /* ------------------------- SHARED VARIABLES ------------------------ */ @@ -73,1387 +69,1424 @@ static double c2tloc, s2tloc; static double s3tloc, c3tloc; static double apdf, apt[4]; - - /* ------------------------------------------------------------------- */ /* ------------------------------ TSELEC ----------------------------- */ /* ------------------------------------------------------------------- */ -void tselec(struct nrlmsise_flags *flags) { - int i; - for (i=0;i<24;i++) { - if (i!=9) { - if (flags->switches[i]==1) - flags->sw[i]=1; - else - flags->sw[i]=0; - if (flags->switches[i]>0) - flags->swc[i]=1; - else - flags->swc[i]=0; - } else { - flags->sw[i]=flags->switches[i]; - flags->swc[i]=flags->switches[i]; - } - } +void +tselec(struct nrlmsise_flags* flags) +{ + int i; + for (i = 0; i < 24; i++) { + if (i != 9) { + if (flags->switches[i] == 1) + flags->sw[i] = 1; + else + flags->sw[i] = 0; + if (flags->switches[i] > 0) + flags->swc[i] = 1; + else + flags->swc[i] = 0; + } else { + flags->sw[i] = flags->switches[i]; + flags->swc[i] = flags->switches[i]; + } + } } - - /* ------------------------------------------------------------------- */ /* ------------------------------ GLATF ------------------------------ */ /* ------------------------------------------------------------------- */ -void glatf(double lat, double *gv, double *reff) { - double dgtr = 1.74533E-2; - double c2; - c2 = cos(2.0*dgtr*lat); - *gv = 980.616 * (1.0 - 0.0026373 * c2); - *reff = 2.0 * (*gv) / (3.085462E-6 + 2.27E-9 * c2) * 1.0E-5; +void +glatf(double lat, double* gv, double* reff) +{ + double dgtr = 1.74533E-2; + double c2; + c2 = cos(2.0 * dgtr * lat); + *gv = 980.616 * (1.0 - 0.0026373 * c2); + *reff = 2.0 * (*gv) / (3.085462E-6 + 2.27E-9 * c2) * 1.0E-5; } - - /* ------------------------------------------------------------------- */ /* ------------------------------ CCOR ------------------------------- */ /* ------------------------------------------------------------------- */ -double ccor(double alt, double r, double h1, double zh) { -/* CHEMISTRY/DISSOCIATION CORRECTION FOR MSIS MODELS - * ALT - altitude - * R - target ratio - * H1 - transition scale length - * ZH - altitude of 1/2 R - */ - double e; - double ex; - e = (alt - zh) / h1; - if (e>70) - return exp(0); - if (e<-70) - return exp(r); - ex = exp(e); - e = r / (1.0 + ex); - return exp(e); +double +ccor(double alt, double r, double h1, double zh) +{ + /* CHEMISTRY/DISSOCIATION CORRECTION FOR MSIS MODELS + * ALT - altitude + * R - target ratio + * H1 - transition scale length + * ZH - altitude of 1/2 R + */ + double e; + double ex; + e = (alt - zh) / h1; + if (e > 70) + return exp(0); + if (e < -70) + return exp(r); + ex = exp(e); + e = r / (1.0 + ex); + return exp(e); } - - /* ------------------------------------------------------------------- */ /* ------------------------------ CCOR ------------------------------- */ /* ------------------------------------------------------------------- */ -double ccor2(double alt, double r, double h1, double zh, double h2) { -/* CHEMISTRY/DISSOCIATION CORRECTION FOR MSIS MODELS - * ALT - altitude - * R - target ratio - * H1 - transition scale length - * ZH - altitude of 1/2 R - * H2 - transition scale length #2 ? - */ - double e1, e2; - double ex1, ex2; - double ccor2v; - e1 = (alt - zh) / h1; - e2 = (alt - zh) / h2; - if ((e1 > 70) || (e2 > 70)) - return exp(0); - if ((e1 < -70) && (e2 < -70)) - return exp(r); - ex1 = exp(e1); - ex2 = exp(e2); - ccor2v = r / (1.0 + 0.5 * (ex1 + ex2)); - return exp(ccor2v); +double +ccor2(double alt, double r, double h1, double zh, double h2) +{ + /* CHEMISTRY/DISSOCIATION CORRECTION FOR MSIS MODELS + * ALT - altitude + * R - target ratio + * H1 - transition scale length + * ZH - altitude of 1/2 R + * H2 - transition scale length #2 ? + */ + double e1, e2; + double ex1, ex2; + double ccor2v; + e1 = (alt - zh) / h1; + e2 = (alt - zh) / h2; + if ((e1 > 70) || (e2 > 70)) + return exp(0); + if ((e1 < -70) && (e2 < -70)) + return exp(r); + ex1 = exp(e1); + ex2 = exp(e2); + ccor2v = r / (1.0 + 0.5 * (ex1 + ex2)); + return exp(ccor2v); } - - /* ------------------------------------------------------------------- */ /* ------------------------------- SCALH ----------------------------- */ /* ------------------------------------------------------------------- */ -double scalh(double alt, double xm, double temp) { - double g; - double rgas=831.4; - g = gsurf / (pow((1.0 + alt/re),2.0)); - g = rgas * temp / (g * xm); - return g; +double +scalh(double alt, double xm, double temp) +{ + double g; + double rgas = 831.4; + g = gsurf / (pow((1.0 + alt / re), 2.0)); + g = rgas * temp / (g * xm); + return g; } - - /* ------------------------------------------------------------------- */ /* -------------------------------- DNET ----------------------------- */ /* ------------------------------------------------------------------- */ -double dnet (double dd, double dm, double zhm, double xmm, double xm) { -/* TURBOPAUSE CORRECTION FOR MSIS MODELS - * Root mean density - * DD - diffusive density - * DM - full mixed density - * ZHM - transition scale length - * XMM - full mixed molecular weight - * XM - species molecular weight - * DNET - combined density - */ - double a; - double ylog; - a = zhm / (xmm-xm); - if (!((dm>0) && (dd>0))) { - printf("dnet log error %e %e %e\n",dm,dd,xm); - if ((dd==0) && (dm==0)) - dd=1; - if (dm==0) - return dd; - if (dd==0) - return dm; - } - ylog = a * log(dm/dd); - if (ylog<-10) - return dd; - if (ylog>10) - return dm; - a = dd*pow((1.0 + exp(ylog)),(1.0/a)); - return a; +double +dnet(double dd, double dm, double zhm, double xmm, double xm) +{ + /* TURBOPAUSE CORRECTION FOR MSIS MODELS + * Root mean density + * DD - diffusive density + * DM - full mixed density + * ZHM - transition scale length + * XMM - full mixed molecular weight + * XM - species molecular weight + * DNET - combined density + */ + double a; + double ylog; + a = zhm / (xmm - xm); + if (!((dm > 0) && (dd > 0))) { + printf("dnet log error %e %e %e\n", dm, dd, xm); + if ((dd == 0) && (dm == 0)) + dd = 1; + if (dm == 0) + return dd; + if (dd == 0) + return dm; + } + ylog = a * log(dm / dd); + if (ylog < -10) + return dd; + if (ylog > 10) + return dm; + a = dd * pow((1.0 + exp(ylog)), (1.0 / a)); + return a; } - - /* ------------------------------------------------------------------- */ /* ------------------------------- SPLINI ---------------------------- */ /* ------------------------------------------------------------------- */ -void splini (double *xa, double *ya, double *y2a, int n, double x, double *y) { -/* INTEGRATE CUBIC SPLINE FUNCTION FROM XA(1) TO X - * XA,YA: ARRAYS OF TABULATED FUNCTION IN ASCENDING ORDER BY X - * Y2A: ARRAY OF SECOND DERIVATIVES - * N: SIZE OF ARRAYS XA,YA,Y2A - * X: ABSCISSA ENDPOINT FOR INTEGRATION - * Y: OUTPUT VALUE - */ - double yi=0; - int klo=0; - int khi=1; - double xx, h, a, b, a2, b2; - while ((x>xa[klo]) && (khi xa[klo]) && (khi < n)) { + xx = x; + if (khi < (n - 1)) { + if (x < xa[khi]) + xx = x; + else + xx = xa[khi]; + } + h = xa[khi] - xa[klo]; + a = (xa[khi] - xx) / h; + b = (xx - xa[klo]) / h; + a2 = a * a; + b2 = b * b; + yi += ((1.0 - a2) * ya[klo] / 2.0 + b2 * ya[khi] / 2.0 + + ((-(1.0 + a2 * a2) / 4.0 + a2 / 2.0) * y2a[klo] + (b2 * b2 / 4.0 - b2 / 2.0) * y2a[khi]) * h * h / 6.0) * + h; + klo++; + khi++; + } + *y = yi; } - - /* ------------------------------------------------------------------- */ /* ------------------------------- SPLINT ---------------------------- */ /* ------------------------------------------------------------------- */ -void splint (double *xa, double *ya, double *y2a, int n, double x, double *y) { -/* CALCULATE CUBIC SPLINE INTERP VALUE - * ADAPTED FROM NUMERICAL RECIPES BY PRESS ET AL. - * XA,YA: ARRAYS OF TABULATED FUNCTION IN ASCENDING ORDER BY X - * Y2A: ARRAY OF SECOND DERIVATIVES - * N: SIZE OF ARRAYS XA,YA,Y2A - * X: ABSCISSA FOR INTERPOLATION - * Y: OUTPUT VALUE - */ - int klo=0; - int khi=n-1; - int k; - double h; - double a, b, yi; - while ((khi-klo)>1) { - k=(khi+klo)/2; - if (xa[k]>x) - khi=k; - else - klo=k; - } - h = xa[khi] - xa[klo]; - if (h==0.0) - printf("bad XA input to splint"); - a = (xa[khi] - x)/h; - b = (x - xa[klo])/h; - yi = a * ya[klo] + b * ya[khi] + ((a*a*a - a) * y2a[klo] + (b*b*b - b) * y2a[khi]) * h * h/6.0; - *y = yi; +void +splint(double* xa, double* ya, double* y2a, int n, double x, double* y) +{ + /* CALCULATE CUBIC SPLINE INTERP VALUE + * ADAPTED FROM NUMERICAL RECIPES BY PRESS ET AL. + * XA,YA: ARRAYS OF TABULATED FUNCTION IN ASCENDING ORDER BY X + * Y2A: ARRAY OF SECOND DERIVATIVES + * N: SIZE OF ARRAYS XA,YA,Y2A + * X: ABSCISSA FOR INTERPOLATION + * Y: OUTPUT VALUE + */ + int klo = 0; + int khi = n - 1; + int k; + double h; + double a, b, yi; + while ((khi - klo) > 1) { + k = (khi + klo) / 2; + if (xa[k] > x) + khi = k; + else + klo = k; + } + h = xa[khi] - xa[klo]; + if (h == 0.0) + printf("bad XA input to splint"); + a = (xa[khi] - x) / h; + b = (x - xa[klo]) / h; + yi = a * ya[klo] + b * ya[khi] + ((a * a * a - a) * y2a[klo] + (b * b * b - b) * y2a[khi]) * h * h / 6.0; + *y = yi; } - - /* ------------------------------------------------------------------- */ /* ------------------------------- SPLINE ---------------------------- */ /* ------------------------------------------------------------------- */ -void spline (double *x, double *y, int n, double yp1, double ypn, double *y2) { -/* CALCULATE 2ND DERIVATIVES OF CUBIC SPLINE INTERP FUNCTION - * ADAPTED FROM NUMERICAL RECIPES BY PRESS ET AL - * X,Y: ARRAYS OF TABULATED FUNCTION IN ASCENDING ORDER BY X - * N: SIZE OF ARRAYS X,Y - * YP1,YPN: SPECIFIED DERIVATIVES AT X[0] AND X[N-1]; VALUES - * >= 1E30 SIGNAL SIGNAL SECOND DERIVATIVE ZERO - * Y2: OUTPUT ARRAY OF SECOND DERIVATIVES - */ - double *u; - double sig, p, qn, un; - int i, k; - u=malloc(sizeof(double)*(unsigned int)n); - if (u==NULL) { - printf("Out Of Memory in spline - ERROR"); - return; - } - if (yp1>0.99E30) { - y2[0]=0; - u[0]=0; - } else { - y2[0]=-0.5; - u[0]=(3.0/(x[1]-x[0]))*((y[1]-y[0])/(x[1]-x[0])-yp1); - } - for (i=1;i<(n-1);i++) { - sig = (x[i]-x[i-1])/(x[i+1] - x[i-1]); - p = sig * y2[i-1] + 2.0; - y2[i] = (sig - 1.0) / p; - u[i] = (6.0 * ((y[i+1] - y[i])/(x[i+1] - x[i]) -(y[i] - y[i-1]) / (x[i] - x[i-1]))/(x[i+1] - x[i-1]) - sig * u[i-1])/p; - } - if (ypn>0.99E30) { - qn = 0; - un = 0; - } else { - qn = 0.5; - un = (3.0 / (x[n-1] - x[n-2])) * (ypn - (y[n-1] - y[n-2])/(x[n-1] - x[n-2])); - } - y2[n-1] = (un - qn * u[n-2]) / (qn * y2[n-2] + 1.0); - for (k=n-2;k>=0;k--) - y2[k] = y2[k] * y2[k+1] + u[k]; - - free(u); +void +spline(double* x, double* y, int n, double yp1, double ypn, double* y2) +{ + /* CALCULATE 2ND DERIVATIVES OF CUBIC SPLINE INTERP FUNCTION + * ADAPTED FROM NUMERICAL RECIPES BY PRESS ET AL + * X,Y: ARRAYS OF TABULATED FUNCTION IN ASCENDING ORDER BY X + * N: SIZE OF ARRAYS X,Y + * YP1,YPN: SPECIFIED DERIVATIVES AT X[0] AND X[N-1]; VALUES + * >= 1E30 SIGNAL SIGNAL SECOND DERIVATIVE ZERO + * Y2: OUTPUT ARRAY OF SECOND DERIVATIVES + */ + double* u; + double sig, p, qn, un; + int i, k; + u = malloc(sizeof(double) * (unsigned int)n); + if (u == NULL) { + printf("Out Of Memory in spline - ERROR"); + return; + } + if (yp1 > 0.99E30) { + y2[0] = 0; + u[0] = 0; + } else { + y2[0] = -0.5; + u[0] = (3.0 / (x[1] - x[0])) * ((y[1] - y[0]) / (x[1] - x[0]) - yp1); + } + for (i = 1; i < (n - 1); i++) { + sig = (x[i] - x[i - 1]) / (x[i + 1] - x[i - 1]); + p = sig * y2[i - 1] + 2.0; + y2[i] = (sig - 1.0) / p; + u[i] = (6.0 * ((y[i + 1] - y[i]) / (x[i + 1] - x[i]) - (y[i] - y[i - 1]) / (x[i] - x[i - 1])) / + (x[i + 1] - x[i - 1]) - + sig * u[i - 1]) / + p; + } + if (ypn > 0.99E30) { + qn = 0; + un = 0; + } else { + qn = 0.5; + un = (3.0 / (x[n - 1] - x[n - 2])) * (ypn - (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2])); + } + y2[n - 1] = (un - qn * u[n - 2]) / (qn * y2[n - 2] + 1.0); + for (k = n - 2; k >= 0; k--) + y2[k] = y2[k] * y2[k + 1] + u[k]; + + free(u); } - - /* ------------------------------------------------------------------- */ /* ------------------------------- DENSM ----------------------------- */ /* ------------------------------------------------------------------- */ -__inline_double zeta(double zz, double zl) { - return ((zz-zl)*(re+zl)/(re+zz)); +__inline_double +zeta(double zz, double zl) +{ + return ((zz - zl) * (re + zl) / (re + zz)); } -double densm (double alt, double d0, double xm, double *tz, int mn3, double *zn3, double *tn3, double *tgn3, int mn2, double *zn2, double *tn2, double *tgn2) { -/* Calculate Temperature and Density Profiles for lower atmos. */ - double xs[10], ys[10], y2out[10]; - double rgas = 831.4; - double z, z1, z2, t1, t2, zg, zgdif; - double yd1, yd2; - double x, y, yi; - double expl, gamm, glb; - double densm_tmp; - int mn; - int k; - densm_tmp=d0; - if (alt>zn2[0]) { - if (xm==0.0) - return *tz; - else - return d0; - } - - /* STRATOSPHERE/MESOSPHERE TEMPERATURE */ - if (alt>zn2[mn2-1]) - z=alt; - else - z=zn2[mn2-1]; - mn=mn2; - z1=zn2[0]; - z2=zn2[mn-1]; - t1=tn2[0]; - t2=tn2[mn-1]; - zg = zeta(z, z1); - zgdif = zeta(z2, z1); - - /* set up spline nodes */ - for (k=0;k50.0) - expl=50.0; - - /* Density at altitude */ - densm_tmp = densm_tmp * (t1 / *tz) * exp(-expl); - } - - if (alt>zn3[0]) { - if (xm==0.0) - return *tz; - else - return densm_tmp; - } - - /* troposhere / stratosphere temperature */ - z = alt; - mn = mn3; - z1=zn3[0]; - z2=zn3[mn-1]; - t1=tn3[0]; - t2=tn3[mn-1]; - zg=zeta(z,z1); - zgdif=zeta(z2,z1); - - /* set up spline nodes */ - for (k=0;k50.0) - expl=50.0; - - /* Density at altitude */ - densm_tmp = densm_tmp * (t1 / *tz) * exp(-expl); - } - if (xm==0.0) - return *tz; - else - return densm_tmp; +double +densm(double alt, + double d0, + double xm, + double* tz, + int mn3, + double* zn3, + double* tn3, + double* tgn3, + int mn2, + double* zn2, + double* tn2, + double* tgn2) +{ + /* Calculate Temperature and Density Profiles for lower atmos. */ + double xs[10], ys[10], y2out[10]; + double rgas = 831.4; + double z, z1, z2, t1, t2, zg, zgdif; + double yd1, yd2; + double x, y, yi; + double expl, gamm, glb; + double densm_tmp; + int mn; + int k; + densm_tmp = d0; + if (alt > zn2[0]) { + if (xm == 0.0) + return *tz; + else + return d0; + } + + /* STRATOSPHERE/MESOSPHERE TEMPERATURE */ + if (alt > zn2[mn2 - 1]) + z = alt; + else + z = zn2[mn2 - 1]; + mn = mn2; + z1 = zn2[0]; + z2 = zn2[mn - 1]; + t1 = tn2[0]; + t2 = tn2[mn - 1]; + zg = zeta(z, z1); + zgdif = zeta(z2, z1); + + /* set up spline nodes */ + for (k = 0; k < mn; k++) { + xs[k] = zeta(zn2[k], z1) / zgdif; + ys[k] = 1.0 / tn2[k]; + } + yd1 = -tgn2[0] / (t1 * t1) * zgdif; + yd2 = -tgn2[1] / (t2 * t2) * zgdif * (pow(((re + z2) / (re + z1)), 2.0)); + + /* calculate spline coefficients */ + spline(xs, ys, mn, yd1, yd2, y2out); + x = zg / zgdif; + splint(xs, ys, y2out, mn, x, &y); + + /* temperature at altitude */ + *tz = 1.0 / y; + if (xm != 0.0) { + /* calaculate stratosphere / mesospehere density */ + glb = gsurf / (pow((1.0 + z1 / re), 2.0)); + gamm = xm * glb * zgdif / rgas; + + /* Integrate temperature profile */ + splini(xs, ys, y2out, mn, x, &yi); + expl = gamm * yi; + if (expl > 50.0) + expl = 50.0; + + /* Density at altitude */ + densm_tmp = densm_tmp * (t1 / *tz) * exp(-expl); + } + + if (alt > zn3[0]) { + if (xm == 0.0) + return *tz; + else + return densm_tmp; + } + + /* troposhere / stratosphere temperature */ + z = alt; + mn = mn3; + z1 = zn3[0]; + z2 = zn3[mn - 1]; + t1 = tn3[0]; + t2 = tn3[mn - 1]; + zg = zeta(z, z1); + zgdif = zeta(z2, z1); + + /* set up spline nodes */ + for (k = 0; k < mn; k++) { + xs[k] = zeta(zn3[k], z1) / zgdif; + ys[k] = 1.0 / tn3[k]; + } + yd1 = -tgn3[0] / (t1 * t1) * zgdif; + yd2 = -tgn3[1] / (t2 * t2) * zgdif * (pow(((re + z2) / (re + z1)), 2.0)); + + /* calculate spline coefficients */ + spline(xs, ys, mn, yd1, yd2, y2out); + x = zg / zgdif; + splint(xs, ys, y2out, mn, x, &y); + + /* temperature at altitude */ + *tz = 1.0 / y; + if (xm != 0.0) { + /* calaculate tropospheric / stratosphere density */ + glb = gsurf / (pow((1.0 + z1 / re), 2.0)); + gamm = xm * glb * zgdif / rgas; + + /* Integrate temperature profile */ + splini(xs, ys, y2out, mn, x, &yi); + expl = gamm * yi; + if (expl > 50.0) + expl = 50.0; + + /* Density at altitude */ + densm_tmp = densm_tmp * (t1 / *tz) * exp(-expl); + } + if (xm == 0.0) + return *tz; + else + return densm_tmp; } - - /* ------------------------------------------------------------------- */ /* ------------------------------- DENSU ----------------------------- */ /* ------------------------------------------------------------------- */ -double densu (double alt, double dlb, double tinf, double tlb, double xm, double alpha, double *tz, double zlb, double s2, int mn1, double *zn1, double *tn1, double *tgn1) { -/* Calculate Temperature and Density Profiles for MSIS models - * New lower thermo polynomial - */ - double yd2, yd1, x=0, y; - double rgas=831.4; - double densu_temp=1.0; - double za, z, zg2, tt, ta; - double dta, z1=0, z2, t1=0, t2, zg, zgdif=0; - int mn=0; - int k; - double glb; - double expl; - double yi; - double densa; - double gamma, gamm; - double xs[5], ys[5], y2out[5]; - /* joining altitudes of Bates and spline */ - za=zn1[0]; - if (alt>za) - z=alt; - else - z=za; - - /* geopotential altitude difference from ZLB */ - zg2 = zeta(z, zlb); - - /* Bates temperature */ - tt = tinf - (tinf - tlb) * exp(-s2*zg2); - ta = tt; - *tz = tt; - densu_temp = *tz; - - if (altzn1[mn1-1]) - z=alt; - else - z=zn1[mn1-1]; - mn=mn1; - z1=zn1[0]; - z2=zn1[mn-1]; - t1=tn1[0]; - t2=tn1[mn-1]; - /* geopotental difference from z1 */ - zg = zeta (z, z1); - zgdif = zeta(z2, z1); - /* set up spline nodes */ - for (k=0;k50.0) - expl=50.0; - if (tt<=0) - expl=50.0; - - /* density at altitude */ - densa = dlb * pow((tlb/tt),((1.0+alpha+gamma))) * expl; - densu_temp=densa; - if (alt>=za) - return densu_temp; - - /* calculate density below za */ - glb = gsurf / pow((1.0 + z1/re),2.0); - gamm = xm * glb * zgdif / rgas; - - /* integrate spline temperatures */ - splini (xs, ys, y2out, mn, x, &yi); - expl = gamm * yi; - if (expl>50.0) - expl=50.0; - if (*tz<=0) - expl=50.0; - - /* density at altitude */ - densu_temp = densu_temp * pow ((t1 / *tz),(1.0 + alpha)) * exp(-expl); - return densu_temp; +double +densu(double alt, + double dlb, + double tinf, + double tlb, + double xm, + double alpha, + double* tz, + double zlb, + double s2, + int mn1, + double* zn1, + double* tn1, + double* tgn1) +{ + /* Calculate Temperature and Density Profiles for MSIS models + * New lower thermo polynomial + */ + double yd2, yd1, x = 0, y; + double rgas = 831.4; + double densu_temp = 1.0; + double za, z, zg2, tt, ta; + double dta, z1 = 0, z2, t1 = 0, t2, zg, zgdif = 0; + int mn = 0; + int k; + double glb; + double expl; + double yi; + double densa; + double gamma, gamm; + double xs[5], ys[5], y2out[5]; + /* joining altitudes of Bates and spline */ + za = zn1[0]; + if (alt > za) + z = alt; + else + z = za; + + /* geopotential altitude difference from ZLB */ + zg2 = zeta(z, zlb); + + /* Bates temperature */ + tt = tinf - (tinf - tlb) * exp(-s2 * zg2); + ta = tt; + *tz = tt; + densu_temp = *tz; + + if (alt < za) { + /* calculate temperature below ZA + * temperature gradient at ZA from Bates profile */ + dta = (tinf - ta) * s2 * pow(((re + zlb) / (re + za)), 2.0); + tgn1[0] = dta; + tn1[0] = ta; + if (alt > zn1[mn1 - 1]) + z = alt; + else + z = zn1[mn1 - 1]; + mn = mn1; + z1 = zn1[0]; + z2 = zn1[mn - 1]; + t1 = tn1[0]; + t2 = tn1[mn - 1]; + /* geopotental difference from z1 */ + zg = zeta(z, z1); + zgdif = zeta(z2, z1); + /* set up spline nodes */ + for (k = 0; k < mn; k++) { + xs[k] = zeta(zn1[k], z1) / zgdif; + ys[k] = 1.0 / tn1[k]; + } + /* end node derivatives */ + yd1 = -tgn1[0] / (t1 * t1) * zgdif; + yd2 = -tgn1[1] / (t2 * t2) * zgdif * pow(((re + z2) / (re + z1)), 2.0); + /* calculate spline coefficients */ + spline(xs, ys, mn, yd1, yd2, y2out); + x = zg / zgdif; + splint(xs, ys, y2out, mn, x, &y); + /* temperature at altitude */ + *tz = 1.0 / y; + densu_temp = *tz; + } + if (xm == 0) + return densu_temp; + + /* calculate density above za */ + glb = gsurf / pow((1.0 + zlb / re), 2.0); + gamma = xm * glb / (s2 * rgas * tinf); + expl = exp(-s2 * gamma * zg2); + if (expl > 50.0) + expl = 50.0; + if (tt <= 0) + expl = 50.0; + + /* density at altitude */ + densa = dlb * pow((tlb / tt), ((1.0 + alpha + gamma))) * expl; + densu_temp = densa; + if (alt >= za) + return densu_temp; + + /* calculate density below za */ + glb = gsurf / pow((1.0 + z1 / re), 2.0); + gamm = xm * glb * zgdif / rgas; + + /* integrate spline temperatures */ + splini(xs, ys, y2out, mn, x, &yi); + expl = gamm * yi; + if (expl > 50.0) + expl = 50.0; + if (*tz <= 0) + expl = 50.0; + + /* density at altitude */ + densu_temp = densu_temp * pow((t1 / *tz), (1.0 + alpha)) * exp(-expl); + return densu_temp; } - - /* ------------------------------------------------------------------- */ /* ------------------------------- GLOBE7 ---------------------------- */ /* ------------------------------------------------------------------- */ /* 3hr Magnetic activity functions */ /* Eq. A24d */ -__inline_double g0(double a, double *p) { - return (a - 4.0 + (p[25] - 1.0) * (a - 4.0 + (exp(-sqrt(p[24]*p[24]) * (a - 4.0)) - 1.0) / sqrt(p[24]*p[24]))); +__inline_double +g0(double a, double* p) +{ + return (a - 4.0 + (p[25] - 1.0) * (a - 4.0 + (exp(-sqrt(p[24] * p[24]) * (a - 4.0)) - 1.0) / sqrt(p[24] * p[24]))); } /* Eq. A24c */ -__inline_double sumex(double ex) { - return (1.0 + (1.0 - pow(ex,19.0)) / (1.0 - ex) * pow(ex,0.5)); +__inline_double +sumex(double ex) +{ + return (1.0 + (1.0 - pow(ex, 19.0)) / (1.0 - ex) * pow(ex, 0.5)); } /* Eq. A24a */ -__inline_double sg0(double ex, double *p, double *ap) { - return (g0(ap[1],p) + (g0(ap[2],p)*ex + g0(ap[3],p)*ex*ex + \ - g0(ap[4],p)*pow(ex,3.0) + (g0(ap[5],p)*pow(ex,4.0) + \ - g0(ap[6],p)*pow(ex,12.0))*(1.0-pow(ex,8.0))/(1.0-ex)))/sumex(ex); +__inline_double +sg0(double ex, double* p, double* ap) +{ + return (g0(ap[1], p) + + (g0(ap[2], p) * ex + g0(ap[3], p) * ex * ex + g0(ap[4], p) * pow(ex, 3.0) + + (g0(ap[5], p) * pow(ex, 4.0) + g0(ap[6], p) * pow(ex, 12.0)) * (1.0 - pow(ex, 8.0)) / (1.0 - ex))) / + sumex(ex); } -double globe7(double *p, struct nrlmsise_input *input, struct nrlmsise_flags *flags) { -/* CALCULATE G(L) FUNCTION - * Upper Thermosphere Parameters */ - double t[15]; - int i,j; - double apd; - double tloc; - double c, s, c2, c4, s2; - double sr = 7.2722E-5; - double dgtr = 1.74533E-2; - double dr = 1.72142E-2; - double hr = 0.2618; - double cd32, cd18, cd14, cd39; - double df; - double f1, f2; - double tinf; - struct ap_array *ap; - - tloc=input->lst; - for (j=0;j<14;j++) - t[j]=0; - - /* calculate legendre polynomials */ - c = sin(input->g_lat * dgtr); - s = cos(input->g_lat * dgtr); - c2 = c*c; - c4 = c2*c2; - s2 = s*s; - - plg[0][1] = c; - plg[0][2] = 0.5*(3.0*c2 -1.0); - plg[0][3] = 0.5*(5.0*c*c2-3.0*c); - plg[0][4] = (35.0*c4 - 30.0*c2 + 3.0)/8.0; - plg[0][5] = (63.0*c2*c2*c - 70.0*c2*c + 15.0*c)/8.0; - plg[0][6] = (11.0*c*plg[0][5] - 5.0*plg[0][4])/6.0; -/* plg[0][7] = (13.0*c*plg[0][6] - 6.0*plg[0][5])/7.0; */ - plg[1][1] = s; - plg[1][2] = 3.0*c*s; - plg[1][3] = 1.5*(5.0*c2-1.0)*s; - plg[1][4] = 2.5*(7.0*c2*c-3.0*c)*s; - plg[1][5] = 1.875*(21.0*c4 - 14.0*c2 +1.0)*s; - plg[1][6] = (11.0*c*plg[1][5]-6.0*plg[1][4])/5.0; -/* plg[1][7] = (13.0*c*plg[1][6]-7.0*plg[1][5])/6.0; */ -/* plg[1][8] = (15.0*c*plg[1][7]-8.0*plg[1][6])/7.0; */ - plg[2][2] = 3.0*s2; - plg[2][3] = 15.0*s2*c; - plg[2][4] = 7.5*(7.0*c2 -1.0)*s2; - plg[2][5] = 3.0*c*plg[2][4]-2.0*plg[2][3]; - plg[2][6] =(11.0*c*plg[2][5]-7.0*plg[2][4])/4.0; - plg[2][7] =(13.0*c*plg[2][6]-8.0*plg[2][5])/5.0; - plg[3][3] = 15.0*s2*s; - plg[3][4] = 105.0*s2*s*c; - plg[3][5] =(9.0*c*plg[3][4]-7.*plg[3][3])/2.0; - plg[3][6] =(11.0*c*plg[3][5]-8.*plg[3][4])/3.0; - - if (!(((flags->sw[7]==0)&&(flags->sw[8]==0))&&(flags->sw[14]==0))) { - stloc = sin(hr*tloc); - ctloc = cos(hr*tloc); - s2tloc = sin(2.0*hr*tloc); - c2tloc = cos(2.0*hr*tloc); - s3tloc = sin(3.0*hr*tloc); - c3tloc = cos(3.0*hr*tloc); - } - - cd32 = cos(dr*(input->doy-p[31])); - cd18 = cos(2.0*dr*(input->doy-p[17])); - cd14 = cos(dr*(input->doy-p[13])); - cd39 = cos(2.0*dr*(input->doy-p[38])); - - /* F10.7 EFFECT */ - df = input->f107 - input->f107A; - dfa = input->f107A - 150.0; - t[0] = p[19]*df*(1.0+p[59]*dfa) + p[20]*df*df + p[21]*dfa + p[29]*pow(dfa,2.0); - f1 = 1.0 + (p[47]*dfa +p[19]*df+p[20]*df*df)*flags->swc[1]; - f2 = 1.0 + (p[49]*dfa+p[19]*df+p[20]*df*df)*flags->swc[1]; - - /* TIME INDEPENDENT */ - t[1] = (p[1]*plg[0][2]+ p[2]*plg[0][4]+p[22]*plg[0][6]) + \ - (p[14]*plg[0][2])*dfa*flags->swc[1] +p[26]*plg[0][1]; - - /* SYMMETRICAL ANNUAL */ - t[2] = p[18]*cd32; - - /* SYMMETRICAL SEMIANNUAL */ - t[3] = (p[15]+p[16]*plg[0][2])*cd18; - - /* ASYMMETRICAL ANNUAL */ - t[4] = f1*(p[9]*plg[0][1]+p[10]*plg[0][3])*cd14; - - /* ASYMMETRICAL SEMIANNUAL */ - t[5] = p[37]*plg[0][1]*cd39; - - /* DIURNAL */ - if ((int) flags->sw[7]) { - double t71, t72; - t71 = (p[11]*plg[1][2])*cd14*flags->swc[5]; - t72 = (p[12]*plg[1][2])*cd14*flags->swc[5]; - t[6] = f2*((p[3]*plg[1][1] + p[4]*plg[1][3] + p[27]*plg[1][5] + t71) * \ - ctloc + (p[6]*plg[1][1] + p[7]*plg[1][3] + p[28]*plg[1][5] \ - + t72)*stloc); +double +globe7(double* p, struct nrlmsise_input* input, struct nrlmsise_flags* flags) +{ + /* CALCULATE G(L) FUNCTION + * Upper Thermosphere Parameters */ + double t[15]; + int i, j; + double apd; + double tloc; + double c, s, c2, c4, s2; + double sr = 7.2722E-5; + double dgtr = 1.74533E-2; + double dr = 1.72142E-2; + double hr = 0.2618; + double cd32, cd18, cd14, cd39; + double df; + double f1, f2; + double tinf; + struct ap_array* ap; + + tloc = input->lst; + for (j = 0; j < 14; j++) + t[j] = 0; + + /* calculate legendre polynomials */ + c = sin(input->g_lat * dgtr); + s = cos(input->g_lat * dgtr); + c2 = c * c; + c4 = c2 * c2; + s2 = s * s; + + plg[0][1] = c; + plg[0][2] = 0.5 * (3.0 * c2 - 1.0); + plg[0][3] = 0.5 * (5.0 * c * c2 - 3.0 * c); + plg[0][4] = (35.0 * c4 - 30.0 * c2 + 3.0) / 8.0; + plg[0][5] = (63.0 * c2 * c2 * c - 70.0 * c2 * c + 15.0 * c) / 8.0; + plg[0][6] = (11.0 * c * plg[0][5] - 5.0 * plg[0][4]) / 6.0; + /* plg[0][7] = (13.0*c*plg[0][6] - 6.0*plg[0][5])/7.0; */ + plg[1][1] = s; + plg[1][2] = 3.0 * c * s; + plg[1][3] = 1.5 * (5.0 * c2 - 1.0) * s; + plg[1][4] = 2.5 * (7.0 * c2 * c - 3.0 * c) * s; + plg[1][5] = 1.875 * (21.0 * c4 - 14.0 * c2 + 1.0) * s; + plg[1][6] = (11.0 * c * plg[1][5] - 6.0 * plg[1][4]) / 5.0; + /* plg[1][7] = (13.0*c*plg[1][6]-7.0*plg[1][5])/6.0; */ + /* plg[1][8] = (15.0*c*plg[1][7]-8.0*plg[1][6])/7.0; */ + plg[2][2] = 3.0 * s2; + plg[2][3] = 15.0 * s2 * c; + plg[2][4] = 7.5 * (7.0 * c2 - 1.0) * s2; + plg[2][5] = 3.0 * c * plg[2][4] - 2.0 * plg[2][3]; + plg[2][6] = (11.0 * c * plg[2][5] - 7.0 * plg[2][4]) / 4.0; + plg[2][7] = (13.0 * c * plg[2][6] - 8.0 * plg[2][5]) / 5.0; + plg[3][3] = 15.0 * s2 * s; + plg[3][4] = 105.0 * s2 * s * c; + plg[3][5] = (9.0 * c * plg[3][4] - 7. * plg[3][3]) / 2.0; + plg[3][6] = (11.0 * c * plg[3][5] - 8. * plg[3][4]) / 3.0; + + if (!(((flags->sw[7] == 0) && (flags->sw[8] == 0)) && (flags->sw[14] == 0))) { + stloc = sin(hr * tloc); + ctloc = cos(hr * tloc); + s2tloc = sin(2.0 * hr * tloc); + c2tloc = cos(2.0 * hr * tloc); + s3tloc = sin(3.0 * hr * tloc); + c3tloc = cos(3.0 * hr * tloc); + } + + cd32 = cos(dr * (input->doy - p[31])); + cd18 = cos(2.0 * dr * (input->doy - p[17])); + cd14 = cos(dr * (input->doy - p[13])); + cd39 = cos(2.0 * dr * (input->doy - p[38])); + + /* F10.7 EFFECT */ + df = input->f107 - input->f107A; + dfa = input->f107A - 150.0; + t[0] = p[19] * df * (1.0 + p[59] * dfa) + p[20] * df * df + p[21] * dfa + p[29] * pow(dfa, 2.0); + f1 = 1.0 + (p[47] * dfa + p[19] * df + p[20] * df * df) * flags->swc[1]; + f2 = 1.0 + (p[49] * dfa + p[19] * df + p[20] * df * df) * flags->swc[1]; + + /* TIME INDEPENDENT */ + t[1] = (p[1] * plg[0][2] + p[2] * plg[0][4] + p[22] * plg[0][6]) + (p[14] * plg[0][2]) * dfa * flags->swc[1] + + p[26] * plg[0][1]; + + /* SYMMETRICAL ANNUAL */ + t[2] = p[18] * cd32; + + /* SYMMETRICAL SEMIANNUAL */ + t[3] = (p[15] + p[16] * plg[0][2]) * cd18; + + /* ASYMMETRICAL ANNUAL */ + t[4] = f1 * (p[9] * plg[0][1] + p[10] * plg[0][3]) * cd14; + + /* ASYMMETRICAL SEMIANNUAL */ + t[5] = p[37] * plg[0][1] * cd39; + + /* DIURNAL */ + if ((int)flags->sw[7]) { + double t71, t72; + t71 = (p[11] * plg[1][2]) * cd14 * flags->swc[5]; + t72 = (p[12] * plg[1][2]) * cd14 * flags->swc[5]; + t[6] = f2 * ((p[3] * plg[1][1] + p[4] * plg[1][3] + p[27] * plg[1][5] + t71) * ctloc + + (p[6] * plg[1][1] + p[7] * plg[1][3] + p[28] * plg[1][5] + t72) * stloc); + } + + /* SEMIDIURNAL */ + if ((int)flags->sw[8]) { + double t81, t82; + t81 = (p[23] * plg[2][3] + p[35] * plg[2][5]) * cd14 * flags->swc[5]; + t82 = (p[33] * plg[2][3] + p[36] * plg[2][5]) * cd14 * flags->swc[5]; + t[7] = f2 * ((p[5] * plg[2][2] + p[41] * plg[2][4] + t81) * c2tloc + + (p[8] * plg[2][2] + p[42] * plg[2][4] + t82) * s2tloc); + } + + /* TERDIURNAL */ + if ((int)flags->sw[14]) { + t[13] = f2 * ((p[39] * plg[3][3] + (p[93] * plg[3][4] + p[46] * plg[3][6]) * cd14 * flags->swc[5]) * s3tloc + + (p[40] * plg[3][3] + (p[94] * plg[3][4] + p[48] * plg[3][6]) * cd14 * flags->swc[5]) * c3tloc); + } + + /* magnetic activity based on daily ap */ + if ((int)flags->sw[9] == -1) { + ap = input->ap_a; + if (p[51] != 0) { + double exp1; + exp1 = exp(-10800.0 * sqrt(p[51] * p[51]) / (1.0 + p[138] * (45.0 - sqrt(input->g_lat * input->g_lat)))); + if (exp1 > 0.99999) + exp1 = 0.99999; + if (p[24] < 1.0E-4) + p[24] = 1.0E-4; + apt[0] = sg0(exp1, p, ap->a); + /* apt[1]=sg2(exp1,p,ap->a); + apt[2]=sg0(exp2,p,ap->a); + apt[3]=sg2(exp2,p,ap->a); + */ + if ((int)flags->sw[9]) { + t[8] = apt[0] * (p[50] + p[96] * plg[0][2] + p[54] * plg[0][4] + + (p[125] * plg[0][1] + p[126] * plg[0][3] + p[127] * plg[0][5]) * cd14 * flags->swc[5] + + (p[128] * plg[1][1] + p[129] * plg[1][3] + p[130] * plg[1][5]) * flags->swc[7] * + cos(hr * (tloc - p[131]))); + } + } + } else { + double p44, p45; + apd = input->ap - 4.0; + p44 = p[43]; + p45 = p[44]; + if (p44 < 0) + p44 = 1.0E-5; + apdf = apd + (p45 - 1.0) * (apd + (exp(-p44 * apd) - 1.0) / p44); + if ((int)flags->sw[9]) { + t[8] = apdf * (p[32] + p[45] * plg[0][2] + p[34] * plg[0][4] + + (p[100] * plg[0][1] + p[101] * plg[0][3] + p[102] * plg[0][5]) * cd14 * flags->swc[5] + + (p[121] * plg[1][1] + p[122] * plg[1][3] + p[123] * plg[1][5]) * flags->swc[7] * + cos(hr * (tloc - p[124]))); + } + } + + if (((int)flags->sw[10]) && (input->g_long > -1000.0)) { + + /* longitudinal */ + if ((int)flags->sw[11]) { + t[10] = (1.0 + p[80] * dfa * flags->swc[1]) * + ((p[64] * plg[1][2] + p[65] * plg[1][4] + p[66] * plg[1][6] + p[103] * plg[1][1] + + p[104] * plg[1][3] + p[105] * plg[1][5] + + flags->swc[5] * (p[109] * plg[1][1] + p[110] * plg[1][3] + p[111] * plg[1][5]) * cd14) * + cos(dgtr * input->g_long) + + (p[90] * plg[1][2] + p[91] * plg[1][4] + p[92] * plg[1][6] + p[106] * plg[1][1] + + p[107] * plg[1][3] + p[108] * plg[1][5] + + flags->swc[5] * (p[112] * plg[1][1] + p[113] * plg[1][3] + p[114] * plg[1][5]) * cd14) * + sin(dgtr * input->g_long)); + } + + /* ut and mixed ut, longitude */ + if ((int)flags->sw[12]) { + t[11] = (1.0 + p[95] * plg[0][1]) * (1.0 + p[81] * dfa * flags->swc[1]) * + (1.0 + p[119] * plg[0][1] * flags->swc[5] * cd14) * + ((p[68] * plg[0][1] + p[69] * plg[0][3] + p[70] * plg[0][5]) * cos(sr * (input->sec - p[71]))); + t[11] += flags->swc[11] * (p[76] * plg[2][3] + p[77] * plg[2][5] + p[78] * plg[2][7]) * + cos(sr * (input->sec - p[79]) + 2.0 * dgtr * input->g_long) * (1.0 + p[137] * dfa * flags->swc[1]); + } + + /* ut, longitude magnetic activity */ + if ((int)flags->sw[13]) { + if (flags->sw[9] == -1) { + if ((int)p[51]) { + t[12] = apt[0] * flags->swc[11] * (1. + p[132] * plg[0][1]) * + ((p[52] * plg[1][2] + p[98] * plg[1][4] + p[67] * plg[1][6]) * + cos(dgtr * (input->g_long - p[97]))) + + apt[0] * flags->swc[11] * flags->swc[5] * + (p[133] * plg[1][1] + p[134] * plg[1][3] + p[135] * plg[1][5]) * cd14 * + cos(dgtr * (input->g_long - p[136])) + + apt[0] * flags->swc[12] * (p[55] * plg[0][1] + p[56] * plg[0][3] + p[57] * plg[0][5]) * + cos(sr * (input->sec - p[58])); + } + } else { + t[12] = apdf * flags->swc[11] * (1.0 + p[120] * plg[0][1]) * + ((p[60] * plg[1][2] + p[61] * plg[1][4] + p[62] * plg[1][6]) * + cos(dgtr * (input->g_long - p[63]))) + + apdf * flags->swc[11] * flags->swc[5] * + (p[115] * plg[1][1] + p[116] * plg[1][3] + p[117] * plg[1][5]) * cd14 * + cos(dgtr * (input->g_long - p[118])) + + apdf * flags->swc[12] * (p[83] * plg[0][1] + p[84] * plg[0][3] + p[85] * plg[0][5]) * + cos(sr * (input->sec - p[75])); + } + } + } + + /* parms not used: 82, 89, 99, 139-149 */ + tinf = p[30]; + for (i = 0; i < 14; i++) + tinf = tinf + fabs(flags->sw[i + 1]) * t[i]; + return tinf; } - /* SEMIDIURNAL */ - if ((int) flags->sw[8]) { - double t81, t82; - t81 = (p[23]*plg[2][3]+p[35]*plg[2][5])*cd14*flags->swc[5]; - t82 = (p[33]*plg[2][3]+p[36]*plg[2][5])*cd14*flags->swc[5]; - t[7] = f2*((p[5]*plg[2][2]+ p[41]*plg[2][4] + t81)*c2tloc +(p[8]*plg[2][2] + p[42]*plg[2][4] + t82)*s2tloc); - } - - /* TERDIURNAL */ - if ((int) flags->sw[14]) { - t[13] = f2 * ((p[39]*plg[3][3]+(p[93]*plg[3][4]+p[46]*plg[3][6])*cd14*flags->swc[5])* s3tloc +(p[40]*plg[3][3]+(p[94]*plg[3][4]+p[48]*plg[3][6])*cd14*flags->swc[5])* c3tloc); -} - - /* magnetic activity based on daily ap */ - if ((int) flags->sw[9]==-1) { - ap = input->ap_a; - if (p[51]!=0) { - double exp1; - exp1 = exp(-10800.0*sqrt(p[51]*p[51])/(1.0+p[138]*(45.0-sqrt(input->g_lat*input->g_lat)))); - if (exp1>0.99999) - exp1=0.99999; - if (p[24]<1.0E-4) - p[24]=1.0E-4; - apt[0]=sg0(exp1,p,ap->a); - /* apt[1]=sg2(exp1,p,ap->a); - apt[2]=sg0(exp2,p,ap->a); - apt[3]=sg2(exp2,p,ap->a); - */ - if ((int) flags->sw[9]) { - t[8] = apt[0]*(p[50]+p[96]*plg[0][2]+p[54]*plg[0][4]+ \ - (p[125]*plg[0][1]+p[126]*plg[0][3]+p[127]*plg[0][5])*cd14*flags->swc[5]+ \ - (p[128]*plg[1][1]+p[129]*plg[1][3]+p[130]*plg[1][5])*flags->swc[7]* \ - cos(hr*(tloc-p[131]))); - } - } - } else { - double p44, p45; - apd=input->ap-4.0; - p44=p[43]; - p45=p[44]; - if (p44<0) - p44 = 1.0E-5; - apdf = apd + (p45-1.0)*(apd + (exp(-p44 * apd) - 1.0)/p44); - if ((int) flags->sw[9]) { - t[8]=apdf*(p[32]+p[45]*plg[0][2]+p[34]*plg[0][4]+ \ - (p[100]*plg[0][1]+p[101]*plg[0][3]+p[102]*plg[0][5])*cd14*flags->swc[5]+ - (p[121]*plg[1][1]+p[122]*plg[1][3]+p[123]*plg[1][5])*flags->swc[7]* - cos(hr*(tloc-p[124]))); - } - } - - if (((int) flags->sw[10])&&(input->g_long>-1000.0)) { - - /* longitudinal */ - if ((int) flags->sw[11]) { - t[10] = (1.0 + p[80]*dfa*flags->swc[1])* \ - ((p[64]*plg[1][2]+p[65]*plg[1][4]+p[66]*plg[1][6]\ - +p[103]*plg[1][1]+p[104]*plg[1][3]+p[105]*plg[1][5]\ - +flags->swc[5]*(p[109]*plg[1][1]+p[110]*plg[1][3]+p[111]*plg[1][5])*cd14)* \ - cos(dgtr*input->g_long) \ - +(p[90]*plg[1][2]+p[91]*plg[1][4]+p[92]*plg[1][6]\ - +p[106]*plg[1][1]+p[107]*plg[1][3]+p[108]*plg[1][5]\ - +flags->swc[5]*(p[112]*plg[1][1]+p[113]*plg[1][3]+p[114]*plg[1][5])*cd14)* \ - sin(dgtr*input->g_long)); - } - - /* ut and mixed ut, longitude */ - if ((int) flags->sw[12]){ - t[11]=(1.0+p[95]*plg[0][1])*(1.0+p[81]*dfa*flags->swc[1])*\ - (1.0+p[119]*plg[0][1]*flags->swc[5]*cd14)*\ - ((p[68]*plg[0][1]+p[69]*plg[0][3]+p[70]*plg[0][5])*\ - cos(sr*(input->sec-p[71]))); - t[11]+=flags->swc[11]*\ - (p[76]*plg[2][3]+p[77]*plg[2][5]+p[78]*plg[2][7])*\ - cos(sr*(input->sec-p[79])+2.0*dgtr*input->g_long)*(1.0+p[137]*dfa*flags->swc[1]); - } - - /* ut, longitude magnetic activity */ - if ((int) flags->sw[13]) { - if (flags->sw[9]==-1) { - if ((int) p[51]) { - t[12]=apt[0]*flags->swc[11]*(1.+p[132]*plg[0][1])*\ - ((p[52]*plg[1][2]+p[98]*plg[1][4]+p[67]*plg[1][6])*\ - cos(dgtr*(input->g_long-p[97])))\ - +apt[0]*flags->swc[11]*flags->swc[5]*\ - (p[133]*plg[1][1]+p[134]*plg[1][3]+p[135]*plg[1][5])*\ - cd14*cos(dgtr*(input->g_long-p[136])) \ - +apt[0]*flags->swc[12]* \ - (p[55]*plg[0][1]+p[56]*plg[0][3]+p[57]*plg[0][5])*\ - cos(sr*(input->sec-p[58])); - } - } else { - t[12] = apdf*flags->swc[11]*(1.0+p[120]*plg[0][1])*\ - ((p[60]*plg[1][2]+p[61]*plg[1][4]+p[62]*plg[1][6])*\ - cos(dgtr*(input->g_long-p[63])))\ - +apdf*flags->swc[11]*flags->swc[5]* \ - (p[115]*plg[1][1]+p[116]*plg[1][3]+p[117]*plg[1][5])* \ - cd14*cos(dgtr*(input->g_long-p[118])) \ - + apdf*flags->swc[12]* \ - (p[83]*plg[0][1]+p[84]*plg[0][3]+p[85]*plg[0][5])* \ - cos(sr*(input->sec-p[75])); - } - } - } - - /* parms not used: 82, 89, 99, 139-149 */ - tinf = p[30]; - for (i=0;i<14;i++) - tinf = tinf + fabs(flags->sw[i+1])*t[i]; - return tinf; -} - - - /* ------------------------------------------------------------------- */ /* ------------------------------- GLOB7S ---------------------------- */ /* ------------------------------------------------------------------- */ -double glob7s(double *p, struct nrlmsise_input *input, struct nrlmsise_flags *flags) { -/* VERSION OF GLOBE FOR LOWER ATMOSPHERE 10/26/99 - */ - double pset=2.0; - double t[14]; - double tt; - double cd32, cd18, cd14, cd39; - int i,j; - double dr=1.72142E-2; - double dgtr=1.74533E-2; - /* confirm parameter set */ - if (p[99]==0) - p[99]=pset; - if (p[99]!=pset) { - printf("Wrong parameter set for glob7s\n"); - return -1; - } - for (j=0;j<14;j++) - t[j]=0.0; - cd32 = cos(dr*(input->doy-p[31])); - cd18 = cos(2.0*dr*(input->doy-p[17])); - cd14 = cos(dr*(input->doy-p[13])); - cd39 = cos(2.0*dr*(input->doy-p[38])); - - /* F10.7 */ - t[0] = p[21]*dfa; - - /* time independent */ - t[1]=p[1]*plg[0][2] + p[2]*plg[0][4] + p[22]*plg[0][6] + p[26]*plg[0][1] + p[14]*plg[0][3] + p[59]*plg[0][5]; - - /* SYMMETRICAL ANNUAL */ - t[2]=(p[18]+p[47]*plg[0][2]+p[29]*plg[0][4])*cd32; - - /* SYMMETRICAL SEMIANNUAL */ - t[3]=(p[15]+p[16]*plg[0][2]+p[30]*plg[0][4])*cd18; - - /* ASYMMETRICAL ANNUAL */ - t[4]=(p[9]*plg[0][1]+p[10]*plg[0][3]+p[20]*plg[0][5])*cd14; - - /* ASYMMETRICAL SEMIANNUAL */ - t[5]=(p[37]*plg[0][1])*cd39; - - /* DIURNAL */ - if ((int) flags->sw[7]) { - double t71, t72; - t71 = p[11]*plg[1][2]*cd14*flags->swc[5]; - t72 = p[12]*plg[1][2]*cd14*flags->swc[5]; - t[6] = ((p[3]*plg[1][1] + p[4]*plg[1][3] + t71) * ctloc + (p[6]*plg[1][1] + p[7]*plg[1][3] + t72) * stloc) ; - } - - /* SEMIDIURNAL */ - if ((int) flags->sw[8]) { - double t81, t82; - t81 = (p[23]*plg[2][3]+p[35]*plg[2][5])*cd14*flags->swc[5]; - t82 = (p[33]*plg[2][3]+p[36]*plg[2][5])*cd14*flags->swc[5]; - t[7] = ((p[5]*plg[2][2] + p[41]*plg[2][4] + t81) * c2tloc + (p[8]*plg[2][2] + p[42]*plg[2][4] + t82) * s2tloc); - } - - /* TERDIURNAL */ - if ((int) flags->sw[14]) { - t[13] = p[39] * plg[3][3] * s3tloc + p[40] * plg[3][3] * c3tloc; - } - - /* MAGNETIC ACTIVITY */ - 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) - t[8]=(p[50]*apt[0] + p[96]*plg[0][2] * apt[0]*flags->swc[2]); - } - - /* LONGITUDINAL */ - if (!((flags->sw[10]==0) || (flags->sw[11]==0) || (input->g_long<=-1000.0))) { - t[10] = (1.0 + plg[0][1]*(p[80]*flags->swc[5]*cos(dr*(input->doy-p[81]))\ - +p[85]*flags->swc[6]*cos(2.0*dr*(input->doy-p[86])))\ - +p[83]*flags->swc[3]*cos(dr*(input->doy-p[84]))\ - +p[87]*flags->swc[4]*cos(2.0*dr*(input->doy-p[88])))\ - *((p[64]*plg[1][2]+p[65]*plg[1][4]+p[66]*plg[1][6]\ - +p[74]*plg[1][1]+p[75]*plg[1][3]+p[76]*plg[1][5]\ - )*cos(dgtr*input->g_long)\ - +(p[90]*plg[1][2]+p[91]*plg[1][4]+p[92]*plg[1][6]\ - +p[77]*plg[1][1]+p[78]*plg[1][3]+p[79]*plg[1][5]\ - )*sin(dgtr*input->g_long)); - } - tt=0; - for (i=0;i<14;i++) - tt+=fabs(flags->sw[i+1])*t[i]; - return tt; +double +glob7s(double* p, struct nrlmsise_input* input, struct nrlmsise_flags* flags) +{ + /* VERSION OF GLOBE FOR LOWER ATMOSPHERE 10/26/99 + */ + double pset = 2.0; + double t[14]; + double tt; + double cd32, cd18, cd14, cd39; + int i, j; + double dr = 1.72142E-2; + double dgtr = 1.74533E-2; + /* confirm parameter set */ + if (p[99] == 0) + p[99] = pset; + if (p[99] != pset) { + printf("Wrong parameter set for glob7s\n"); + return -1; + } + for (j = 0; j < 14; j++) + t[j] = 0.0; + cd32 = cos(dr * (input->doy - p[31])); + cd18 = cos(2.0 * dr * (input->doy - p[17])); + cd14 = cos(dr * (input->doy - p[13])); + cd39 = cos(2.0 * dr * (input->doy - p[38])); + + /* F10.7 */ + t[0] = p[21] * dfa; + + /* time independent */ + t[1] = p[1] * plg[0][2] + p[2] * plg[0][4] + p[22] * plg[0][6] + p[26] * plg[0][1] + p[14] * plg[0][3] + + p[59] * plg[0][5]; + + /* SYMMETRICAL ANNUAL */ + t[2] = (p[18] + p[47] * plg[0][2] + p[29] * plg[0][4]) * cd32; + + /* SYMMETRICAL SEMIANNUAL */ + t[3] = (p[15] + p[16] * plg[0][2] + p[30] * plg[0][4]) * cd18; + + /* ASYMMETRICAL ANNUAL */ + t[4] = (p[9] * plg[0][1] + p[10] * plg[0][3] + p[20] * plg[0][5]) * cd14; + + /* ASYMMETRICAL SEMIANNUAL */ + t[5] = (p[37] * plg[0][1]) * cd39; + + /* DIURNAL */ + if ((int)flags->sw[7]) { + double t71, t72; + t71 = p[11] * plg[1][2] * cd14 * flags->swc[5]; + t72 = p[12] * plg[1][2] * cd14 * flags->swc[5]; + t[6] = + ((p[3] * plg[1][1] + p[4] * plg[1][3] + t71) * ctloc + (p[6] * plg[1][1] + p[7] * plg[1][3] + t72) * stloc); + } + + /* SEMIDIURNAL */ + if ((int)flags->sw[8]) { + double t81, t82; + t81 = (p[23] * plg[2][3] + p[35] * plg[2][5]) * cd14 * flags->swc[5]; + t82 = (p[33] * plg[2][3] + p[36] * plg[2][5]) * cd14 * flags->swc[5]; + t[7] = ((p[5] * plg[2][2] + p[41] * plg[2][4] + t81) * c2tloc + + (p[8] * plg[2][2] + p[42] * plg[2][4] + t82) * s2tloc); + } + + /* TERDIURNAL */ + if ((int)flags->sw[14]) { + t[13] = p[39] * plg[3][3] * s3tloc + p[40] * plg[3][3] * c3tloc; + } + + /* MAGNETIC ACTIVITY */ + 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) + t[8] = (p[50] * apt[0] + p[96] * plg[0][2] * apt[0] * flags->swc[2]); + } + + /* LONGITUDINAL */ + if (!((flags->sw[10] == 0) || (flags->sw[11] == 0) || (input->g_long <= -1000.0))) { + t[10] = (1.0 + + plg[0][1] * (p[80] * flags->swc[5] * cos(dr * (input->doy - p[81])) + + p[85] * flags->swc[6] * cos(2.0 * dr * (input->doy - p[86]))) + + p[83] * flags->swc[3] * cos(dr * (input->doy - p[84])) + + p[87] * flags->swc[4] * cos(2.0 * dr * (input->doy - p[88]))) * + ((p[64] * plg[1][2] + p[65] * plg[1][4] + p[66] * plg[1][6] + p[74] * plg[1][1] + p[75] * plg[1][3] + + p[76] * plg[1][5]) * + cos(dgtr * input->g_long) + + (p[90] * plg[1][2] + p[91] * plg[1][4] + p[92] * plg[1][6] + p[77] * plg[1][1] + p[78] * plg[1][3] + + p[79] * plg[1][5]) * + sin(dgtr * input->g_long)); + } + tt = 0; + for (i = 0; i < 14; i++) + tt += fabs(flags->sw[i + 1]) * t[i]; + return tt; } - - /* ------------------------------------------------------------------- */ /* ------------------------------- GTD7 ------------------------------ */ /* ------------------------------------------------------------------- */ -void gtd7(struct nrlmsise_input *input, struct nrlmsise_flags *flags, struct nrlmsise_output *output) { - double xlat; - double xmm; - int mn3 = 5; - double zn3[5]={32.5,20.0,15.0,10.0,0.0}; - int mn2 = 4; - double zn2[4]={72.5,55.0,45.0,32.5}; - double altt; - double zmix=62.5; - double tmp; - double dm28m; - double tz; - double dmc; - double dmr; - double dz28; - struct nrlmsise_output soutput; - int i; - - tselec(flags); - - /* Latitude variation of gravity (none for sw[2]=0) */ - xlat=input->g_lat; - if (flags->sw[2]==0) - xlat=45.0; - glatf(xlat, &gsurf, &re); - - xmm = pdm[2][4]; - - /* THERMOSPHERE / MESOSPHERE (above zn2[0]) */ - if (input->alt>zn2[0]) - altt=input->alt; - else - altt=zn2[0]; - - tmp=input->alt; - input->alt=altt; - gts7(input, flags, &soutput); - altt=input->alt; - input->alt=tmp; - if ((int) flags->sw[0]) /* metric adjustment */ - dm28m=dm28*1.0E6; - else - dm28m=dm28; - output->t[0]=soutput.t[0]; - output->t[1]=soutput.t[1]; - if (input->alt>=zn2[0]) { - for (i=0;i<9;i++) - output->d[i]=soutput.d[i]; - return; - } - -/* LOWER MESOSPHERE/UPPER STRATOSPHERE (between zn3[0] and zn2[0]) - * Temperature at nodes and gradients at end nodes - * Inverse temperature a linear function of spherical harmonics - */ - meso_tgn2[0]=meso_tgn1[1]; - meso_tn2[0]=meso_tn1[4]; - meso_tn2[1]=pma[0][0]*pavgm[0]/(1.0-flags->sw[20]*glob7s(pma[0], input, flags)); - meso_tn2[2]=pma[1][0]*pavgm[1]/(1.0-flags->sw[20]*glob7s(pma[1], input, flags)); - meso_tn2[3]=pma[2][0]*pavgm[2]/(1.0-flags->sw[20]*flags->sw[22]*glob7s(pma[2], input, flags)); - meso_tgn2[1]=pavgm[8]*pma[9][0]*(1.0+flags->sw[20]*flags->sw[22]*glob7s(pma[9], input, flags))*meso_tn2[3]*meso_tn2[3]/(pow((pma[2][0]*pavgm[2]),2.0)); - meso_tn3[0]=meso_tn2[3]; - - if (input->altsw[22]*glob7s(pma[3], input, flags)); - meso_tn3[2]=pma[4][0]*pavgm[4]/(1.0-flags->sw[22]*glob7s(pma[4], input, flags)); - meso_tn3[3]=pma[5][0]*pavgm[5]/(1.0-flags->sw[22]*glob7s(pma[5], input, flags)); - meso_tn3[4]=pma[6][0]*pavgm[6]/(1.0-flags->sw[22]*glob7s(pma[6], input, flags)); - meso_tgn3[1]=pma[7][0]*pavgm[7]*(1.0+flags->sw[22]*glob7s(pma[7], input, flags)) *meso_tn3[4]*meso_tn3[4]/(pow((pma[6][0]*pavgm[6]),2.0)); - } - - /* LINEAR TRANSITION TO FULL MIXING BELOW zn2[0] */ - - dmc=0; - 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); - output->d[2]=output->d[2] * (1.0 + dmr*dmc); - - /**** HE density ****/ - dmr = soutput.d[0] / (dz28 * pdm[0][1]) - 1.0; - output->d[0] = output->d[2] * pdm[0][1] * (1.0 + dmr*dmc); - - /**** O density ****/ - output->d[1] = 0; - output->d[8] = 0; - - /**** O2 density ****/ - dmr = soutput.d[3] / (dz28 * pdm[3][1]) - 1.0; - output->d[3] = output->d[2] * pdm[3][1] * (1.0 + dmr*dmc); - - /**** AR density ***/ - dmr = soutput.d[4] / (dz28 * pdm[4][1]) - 1.0; - output->d[4] = output->d[2] * pdm[4][1] * (1.0 + dmr*dmc); - - /**** Hydrogen density ****/ - output->d[6] = 0; - - /**** Atomic nitrogen density ****/ - output->d[7] = 0; - - /**** Total mass density */ - output->d[5] = 1.66E-24 * (4.0 * output->d[0] + 16.0 * output->d[1] + 28.0 * output->d[2] + 32.0 * output->d[3] + 40.0 * output->d[4] + output->d[6] + 14.0 * output->d[7]); - - if ((int) flags->sw[0]) - output->d[5]=output->d[5]/1000; - - /**** temperature at altitude ****/ - dd = densm(input->alt, 1.0, 0, &tz, mn3, zn3, meso_tn3, meso_tgn3, mn2, zn2, meso_tn2, meso_tgn2); - output->t[1]=tz; - +void +gtd7(struct nrlmsise_input* input, struct nrlmsise_flags* flags, struct nrlmsise_output* output) +{ + double xlat; + double xmm; + int mn3 = 5; + double zn3[5] = { 32.5, 20.0, 15.0, 10.0, 0.0 }; + int mn2 = 4; + double zn2[4] = { 72.5, 55.0, 45.0, 32.5 }; + double altt; + double zmix = 62.5; + double tmp; + double dm28m; + double tz; + double dmc; + double dmr; + double dz28; + struct nrlmsise_output soutput; + int i; + + tselec(flags); + + /* Latitude variation of gravity (none for sw[2]=0) */ + xlat = input->g_lat; + if (flags->sw[2] == 0) + xlat = 45.0; + glatf(xlat, &gsurf, &re); + + xmm = pdm[2][4]; + + /* THERMOSPHERE / MESOSPHERE (above zn2[0]) */ + if (input->alt > zn2[0]) + altt = input->alt; + else + altt = zn2[0]; + + tmp = input->alt; + input->alt = altt; + gts7(input, flags, &soutput); + altt = input->alt; + input->alt = tmp; + if ((int)flags->sw[0]) /* metric adjustment */ + dm28m = dm28 * 1.0E6; + else + dm28m = dm28; + output->t[0] = soutput.t[0]; + output->t[1] = soutput.t[1]; + if (input->alt >= zn2[0]) { + for (i = 0; i < 9; i++) + output->d[i] = soutput.d[i]; + return; + } + + /* LOWER MESOSPHERE/UPPER STRATOSPHERE (between zn3[0] and zn2[0]) + * Temperature at nodes and gradients at end nodes + * Inverse temperature a linear function of spherical harmonics + */ + meso_tgn2[0] = meso_tgn1[1]; + meso_tn2[0] = meso_tn1[4]; + meso_tn2[1] = pma[0][0] * pavgm[0] / (1.0 - flags->sw[20] * glob7s(pma[0], input, flags)); + meso_tn2[2] = pma[1][0] * pavgm[1] / (1.0 - flags->sw[20] * glob7s(pma[1], input, flags)); + meso_tn2[3] = pma[2][0] * pavgm[2] / (1.0 - flags->sw[20] * flags->sw[22] * glob7s(pma[2], input, flags)); + meso_tgn2[1] = pavgm[8] * pma[9][0] * (1.0 + flags->sw[20] * flags->sw[22] * glob7s(pma[9], input, flags)) * + meso_tn2[3] * meso_tn2[3] / (pow((pma[2][0] * pavgm[2]), 2.0)); + meso_tn3[0] = meso_tn2[3]; + + if (input->alt < zn3[0]) { + /* LOWER STRATOSPHERE AND TROPOSPHERE (below zn3[0]) + * Temperature at nodes and gradients at end nodes + * Inverse temperature a linear function of spherical harmonics + */ + meso_tgn3[0] = meso_tgn2[1]; + meso_tn3[1] = pma[3][0] * pavgm[3] / (1.0 - flags->sw[22] * glob7s(pma[3], input, flags)); + meso_tn3[2] = pma[4][0] * pavgm[4] / (1.0 - flags->sw[22] * glob7s(pma[4], input, flags)); + meso_tn3[3] = pma[5][0] * pavgm[5] / (1.0 - flags->sw[22] * glob7s(pma[5], input, flags)); + meso_tn3[4] = pma[6][0] * pavgm[6] / (1.0 - flags->sw[22] * glob7s(pma[6], input, flags)); + meso_tgn3[1] = pma[7][0] * pavgm[7] * (1.0 + flags->sw[22] * glob7s(pma[7], input, flags)) * meso_tn3[4] * + meso_tn3[4] / (pow((pma[6][0] * pavgm[6]), 2.0)); + } + + /* LINEAR TRANSITION TO FULL MIXING BELOW zn2[0] */ + + dmc = 0; + 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); + output->d[2] = output->d[2] * (1.0 + dmr * dmc); + + /**** HE density ****/ + dmr = soutput.d[0] / (dz28 * pdm[0][1]) - 1.0; + output->d[0] = output->d[2] * pdm[0][1] * (1.0 + dmr * dmc); + + /**** O density ****/ + output->d[1] = 0; + output->d[8] = 0; + + /**** O2 density ****/ + dmr = soutput.d[3] / (dz28 * pdm[3][1]) - 1.0; + output->d[3] = output->d[2] * pdm[3][1] * (1.0 + dmr * dmc); + + /**** AR density ***/ + dmr = soutput.d[4] / (dz28 * pdm[4][1]) - 1.0; + output->d[4] = output->d[2] * pdm[4][1] * (1.0 + dmr * dmc); + + /**** Hydrogen density ****/ + output->d[6] = 0; + + /**** Atomic nitrogen density ****/ + output->d[7] = 0; + + /**** Total mass density */ + output->d[5] = 1.66E-24 * (4.0 * output->d[0] + 16.0 * output->d[1] + 28.0 * output->d[2] + 32.0 * output->d[3] + + 40.0 * output->d[4] + output->d[6] + 14.0 * output->d[7]); + + if ((int)flags->sw[0]) + output->d[5] = output->d[5] / 1000; + + /**** temperature at altitude ****/ + dd = densm(input->alt, 1.0, 0, &tz, mn3, zn3, meso_tn3, meso_tgn3, mn2, zn2, meso_tn2, meso_tgn2); + output->t[1] = tz; } - - /* ------------------------------------------------------------------- */ /* ------------------------------- GTD7D ----------------------------- */ /* ------------------------------------------------------------------- */ -void gtd7d(struct nrlmsise_input *input, struct nrlmsise_flags *flags, struct nrlmsise_output *output) { - gtd7(input, flags, output); - output->d[5] = 1.66E-24 * (4.0 * output->d[0] + 16.0 * output->d[1] + 28.0 * output->d[2] + 32.0 * output->d[3] + 40.0 * output->d[4] + output->d[6] + 14.0 * output->d[7] + 16.0 * output->d[8]); - if ((int) flags->sw[0]) - output->d[5]=output->d[5]/1000; +void +gtd7d(struct nrlmsise_input* input, struct nrlmsise_flags* flags, struct nrlmsise_output* output) +{ + gtd7(input, flags, output); + output->d[5] = 1.66E-24 * (4.0 * output->d[0] + 16.0 * output->d[1] + 28.0 * output->d[2] + 32.0 * output->d[3] + + 40.0 * output->d[4] + output->d[6] + 14.0 * output->d[7] + 16.0 * output->d[8]); + if ((int)flags->sw[0]) + output->d[5] = output->d[5] / 1000; } - - /* ------------------------------------------------------------------- */ /* -------------------------------- GHP7 ----------------------------- */ /* ------------------------------------------------------------------- */ -void ghp7(struct nrlmsise_input *input, struct nrlmsise_flags *flags, struct nrlmsise_output *output, double press) { - double bm = 1.3806E-19; - double rgas = 831.4; - double test = 0.00043; - double ltest = 12; - double pl, p; - double zi; - double z; - double cl, cl2; - double ca, cd; - double xn, xm, diff; - double g, sh; - int l; - pl = log10(press); - if (pl >= -5.0) { - if (pl>2.5) - zi = 18.06 * (3.00 - pl); - else if ((pl>0.075) && (pl<=2.5)) - zi = 14.98 * (3.08 - pl); - else if ((pl>-1) && (pl<=0.075)) - zi = 17.80 * (2.72 - pl); - else if ((pl>-2) && (pl<=-1)) - zi = 14.28 * (3.64 - pl); - else if ((pl>-4) && (pl<=-2)) - zi = 12.72 * (4.32 -pl); - else - zi = 25.3 * (0.11 - pl); - cl = input->g_lat/90.0; - cl2 = cl*cl; - if (input->doy<182) - cd = (1.0 - (double) input->doy) / 91.25; - else - cd = ((double) input->doy) / 91.25 - 3.0; - ca = 0; - if ((pl > -1.11) && (pl<=-0.23)) - ca = 1.0; - if (pl > -0.23) - ca = (2.79 - pl) / (2.79 + 0.23); - if ((pl <= -1.11) && (pl>-3)) - ca = (-2.93 - pl)/(-2.93 + 1.11); - z = zi - 4.87 * cl * cd * ca - 1.64 * cl2 * ca + 0.31 * ca * cl; - } else - z = 22.0 * pow((pl + 4.0),2.0) + 110.0; - - /* iteration loop */ - l = 0; - do { - l++; - input->alt = z; - gtd7(input, flags, output); - z = input->alt; - xn = output->d[0] + output->d[1] + output->d[2] + output->d[3] + output->d[4] + output->d[6] + output->d[7]; - p = bm * xn * output->t[1]; - if ((int) flags->sw[0]) - p = p*1.0E-6; - diff = pl - log10(p); - if (sqrt(diff*diff)d[5] / xn / 1.66E-24; - if ((int) flags->sw[0]) - xm = xm * 1.0E3; - g = gsurf / (pow((1.0 + z/re),2.0)); - sh = rgas * output->t[1] / (xm * g); - - /* new altitude estimate using scale height */ - if (l < 6) - z = z - sh * diff * 2.302; - else - z = z - sh * diff; - } while (1==1); +void +ghp7(struct nrlmsise_input* input, struct nrlmsise_flags* flags, struct nrlmsise_output* output, double press) +{ + double bm = 1.3806E-19; + double rgas = 831.4; + double test = 0.00043; + double ltest = 12; + double pl, p; + double zi; + double z; + double cl, cl2; + double ca, cd; + double xn, xm, diff; + double g, sh; + int l; + pl = log10(press); + if (pl >= -5.0) { + if (pl > 2.5) + zi = 18.06 * (3.00 - pl); + else if ((pl > 0.075) && (pl <= 2.5)) + zi = 14.98 * (3.08 - pl); + else if ((pl > -1) && (pl <= 0.075)) + zi = 17.80 * (2.72 - pl); + else if ((pl > -2) && (pl <= -1)) + zi = 14.28 * (3.64 - pl); + else if ((pl > -4) && (pl <= -2)) + zi = 12.72 * (4.32 - pl); + else + zi = 25.3 * (0.11 - pl); + cl = input->g_lat / 90.0; + cl2 = cl * cl; + if (input->doy < 182) + cd = (1.0 - (double)input->doy) / 91.25; + else + cd = ((double)input->doy) / 91.25 - 3.0; + ca = 0; + if ((pl > -1.11) && (pl <= -0.23)) + ca = 1.0; + if (pl > -0.23) + ca = (2.79 - pl) / (2.79 + 0.23); + if ((pl <= -1.11) && (pl > -3)) + ca = (-2.93 - pl) / (-2.93 + 1.11); + z = zi - 4.87 * cl * cd * ca - 1.64 * cl2 * ca + 0.31 * ca * cl; + } else + z = 22.0 * pow((pl + 4.0), 2.0) + 110.0; + + /* iteration loop */ + l = 0; + do { + l++; + input->alt = z; + gtd7(input, flags, output); + z = input->alt; + xn = output->d[0] + output->d[1] + output->d[2] + output->d[3] + output->d[4] + output->d[6] + output->d[7]; + p = bm * xn * output->t[1]; + if ((int)flags->sw[0]) + p = p * 1.0E-6; + diff = pl - log10(p); + if (sqrt(diff * diff) < test) + return; + if (l == ltest) { + printf("ERROR: ghp7 not converging for press %e, diff %e", press, diff); + return; + } + xm = output->d[5] / xn / 1.66E-24; + if ((int)flags->sw[0]) + xm = xm * 1.0E3; + g = gsurf / (pow((1.0 + z / re), 2.0)); + sh = rgas * output->t[1] / (xm * g); + + /* new altitude estimate using scale height */ + if (l < 6) + z = z - sh * diff * 2.302; + else + z = z - sh * diff; + } while (1 == 1); } - - /* ------------------------------------------------------------------- */ /* ------------------------------- GTS7 ------------------------------ */ /* ------------------------------------------------------------------- */ -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! - */ - double za; - int i, j; - double ddum, z; - double zn1[5] = {120.0, 110.0, 100.0, 90.0, 72.5}; - double tinf; - int mn1 = 5; - double g0; - double tlb; - double s; - double db01, db04, db14, db16, db28, db32, db40; - double zh28, zh04, zh16, zh32, zh40, zh01, zh14; - double zhm28, zhm04, zhm16, zhm32, zhm40, zhm01, zhm14; - double xmd; - double b28, b04, b16, b32, b40, b01, b14; - double tz; - double g28, g4, g16, g32, g40, g1, g14; - double zhf, xmm; - double zc04, zc16, zc32, zc40, zc01, zc14; - double hc04, hc16, hc32, hc40, hc01, hc14; - double hcc16, hcc32, hcc01, hcc14; - double zcc16, zcc32, zcc01, zcc14; - double rc16, rc32, rc01, rc14; - double rl; - double g16h, db16h, tho, zsht, zmho, zsho; - double dgtr=1.74533E-2; - double dr=1.72142E-2; - double alpha[9]={-0.38, 0.0, 0.0, 0.0, 0.17, 0.0, -0.38, 0.0, 0.0}; - double altl[8]={200.0, 300.0, 160.0, 250.0, 240.0, 450.0, 320.0, 450.0}; - double dd; - double hc216, hcc232; - za = pdl[1][15]; - zn1[0] = za; - for (j=0;j<9;j++) - output->d[j]=0; - - /* TINF VARIATIONS NOT IMPORTANT BELOW ZA OR ZN1(1) */ - if (input->alt>zn1[0]) - tinf = ptm[0]*pt[0] * \ - (1.0+flags->sw[16]*globe7(pt,input,flags)); - else - tinf = ptm[0]*pt[0]; - output->t[0]=tinf; - - /* GRADIENT VARIATIONS NOT IMPORTANT BELOW ZN1(5) */ - if (input->alt>zn1[4]) - g0 = ptm[3]*ps[0] * \ - (1.0+flags->sw[19]*globe7(ps,input,flags)); - else - g0 = ptm[3]*ps[0]; - tlb = ptm[1] * (1.0 + flags->sw[17]*globe7(pd[3],input,flags))*pd[3][0]; - s = g0 / (tinf - tlb); - -/* Lower thermosphere temp variations not significant for - * density above 300 km */ - if (input->alt<300.0) { - meso_tn1[1]=ptm[6]*ptl[0][0]/(1.0-flags->sw[18]*glob7s(ptl[0], input, flags)); - meso_tn1[2]=ptm[2]*ptl[1][0]/(1.0-flags->sw[18]*glob7s(ptl[1], input, flags)); - meso_tn1[3]=ptm[7]*ptl[2][0]/(1.0-flags->sw[18]*glob7s(ptl[2], input, flags)); - meso_tn1[4]=ptm[4]*ptl[3][0]/(1.0-flags->sw[18]*flags->sw[20]*glob7s(ptl[3], input, flags)); - meso_tgn1[1]=ptm[8]*pma[8][0]*(1.0+flags->sw[18]*flags->sw[20]*glob7s(pma[8], input, flags))*meso_tn1[4]*meso_tn1[4]/(pow((ptm[4]*ptl[3][0]),2.0)); - } else { - meso_tn1[1]=ptm[6]*ptl[0][0]; - meso_tn1[2]=ptm[2]*ptl[1][0]; - meso_tn1[3]=ptm[7]*ptl[2][0]; - meso_tn1[4]=ptm[4]*ptl[3][0]; - meso_tgn1[1]=ptm[8]*pma[8][0]*meso_tn1[4]*meso_tn1[4]/(pow((ptm[4]*ptl[3][0]),2.0)); - } - - /* N2 variation factor at Zlb */ - g28=flags->sw[21]*globe7(pd[2], input, flags); - - /* VARIATION OF TURBOPAUSE HEIGHT */ - zhf=pdl[1][24]*(1.0+flags->sw[5]*pdl[0][24]*sin(dgtr*input->g_lat)*cos(dr*(input->doy-pt[13]))); - output->t[0]=tinf; - xmm = pdm[2][4]; - z = input->alt; - - - /**** N2 DENSITY ****/ - - /* Diffusive density at Zlb */ - db28 = pdm[2][0]*exp(g28)*pd[2][0]; - /* Diffusive density at Alt */ - output->d[2]=densu(z,db28,tinf,tlb,28.0,alpha[2],&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - dd=output->d[2]; - /* Turbopause */ - zh28=pdm[2][2]*zhf; - 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); - if (((int) flags->sw[15])&&(z<=altl[2])) { - /* Mixed density at Alt */ - dm28=densu(z,b28,tinf,tlb,xmm,alpha[2],&tz,ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - /* Net density at Alt */ - output->d[2]=dnet(output->d[2],dm28,zhm28,xmm,28.0); - } - - - /**** HE DENSITY ****/ - - /* Density variation factor at Zlb */ - g4 = flags->sw[21]*globe7(pd[0], input, flags); - /* Diffusive density at Zlb */ - db04 = pdm[0][0]*exp(g4)*pd[0][0]; - /* Diffusive density at Alt */ - output->d[0]=densu(z,db04,tinf,tlb, 4.,alpha[0],&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - dd=output->d[0]; - if (((int) flags->sw[15]) && (zt[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - /* Mixed density at Alt */ - dm04=densu(z,b04,tinf,tlb,xmm,0.,&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - zhm04=zhm28; - /* Net density at Alt */ - output->d[0]=dnet(output->d[0],dm04,zhm04,xmm,4.); - /* Correction to specified mixing ratio at ground */ - rl=log(b28*pdm[0][1]/b04); - zc04=pdm[0][4]*pdl[1][0]; - hc04=pdm[0][5]*pdl[1][1]; - /* Net density corrected at Alt */ - output->d[0]=output->d[0]*ccor(z,rl,hc04,zc04); - } - - - /**** O DENSITY ****/ - - /* Density variation factor at Zlb */ - g16= flags->sw[21]*globe7(pd[1],input,flags); - /* Diffusive density at Zlb */ - db16 = pdm[1][0]*exp(g16)*pd[1][0]; - /* Diffusive density at Alt */ - output->d[1]=densu(z,db16,tinf,tlb, 16.,alpha[1],&output->t[1],ptm[5],s,mn1, zn1,meso_tn1,meso_tgn1); - dd=output->d[1]; - if (((int) flags->sw[15]) && (z<=altl[1])) { - /* Turbopause */ - zh16=pdm[1][2]; - /* Mixed density at Zlb */ - b16=densu(zh16,db16,tinf,tlb,16.0-xmm,(alpha[1]-1.0), &output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - /* Mixed density at Alt */ - dm16=densu(z,b16,tinf,tlb,xmm,0.,&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - zhm16=zhm28; - /* Net density at Alt */ - output->d[1]=dnet(output->d[1],dm16,zhm16,xmm,16.); - rl=pdm[1][1]*pdl[1][16]*(1.0+flags->sw[1]*pdl[0][23]*(input->f107A-150.0)); - hc16=pdm[1][5]*pdl[1][3]; - zc16=pdm[1][4]*pdl[1][2]; - hc216=pdm[1][5]*pdl[1][4]; - output->d[1]=output->d[1]*ccor2(z,rl,hc16,zc16,hc216); - /* Chemistry correction */ - hcc16=pdm[1][7]*pdl[1][13]; - zcc16=pdm[1][6]*pdl[1][12]; - rc16=pdm[1][3]*pdl[1][14]; - /* Net density corrected at Alt */ - output->d[1]=output->d[1]*ccor(z,rc16,hcc16,zcc16); - } - - - /**** O2 DENSITY ****/ - - /* Density variation factor at Zlb */ - g32= flags->sw[21]*globe7(pd[4], input, flags); - /* Diffusive density at Zlb */ - db32 = pdm[3][0]*exp(g32)*pd[4][0]; - /* Diffusive density at Alt */ - output->d[3]=densu(z,db32,tinf,tlb, 32.,alpha[3],&output->t[1],ptm[5],s,mn1, zn1,meso_tn1,meso_tgn1); - dd=output->d[3]; - if ((int) flags->sw[15]) { - if (z<=altl[3]) { - /* Turbopause */ - zh32=pdm[3][2]; - /* Mixed density at Zlb */ - b32=densu(zh32,db32,tinf,tlb,32.-xmm,alpha[3]-1., &output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - /* Mixed density at Alt */ - dm32=densu(z,b32,tinf,tlb,xmm,0.,&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - zhm32=zhm28; - /* Net density at Alt */ - output->d[3]=dnet(output->d[3],dm32,zhm32,xmm,32.); - /* Correction to specified mixing ratio at ground */ - rl=log(b28*pdm[3][1]/b32); - hc32=pdm[3][5]*pdl[1][7]; - zc32=pdm[3][4]*pdl[1][6]; - output->d[3]=output->d[3]*ccor(z,rl,hc32,zc32); - } - /* Correction for general departure from diffusive equilibrium above Zlb */ - hcc32=pdm[3][7]*pdl[1][22]; - hcc232=pdm[3][7]*pdl[0][22]; - zcc32=pdm[3][6]*pdl[1][21]; - rc32=pdm[3][3]*pdl[1][23]*(1.+flags->sw[1]*pdl[0][23]*(input->f107A-150.)); - /* Net density corrected at Alt */ - output->d[3]=output->d[3]*ccor2(z,rc32,hcc32,zcc32,hcc232); - } - - - /**** AR DENSITY ****/ - - /* Density variation factor at Zlb */ - g40= flags->sw[21]*globe7(pd[5],input,flags); - /* Diffusive density at Zlb */ - db40 = pdm[4][0]*exp(g40)*pd[5][0]; - /* Diffusive density at Alt */ - output->d[4]=densu(z,db40,tinf,tlb, 40.,alpha[4],&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - dd=output->d[4]; - if (((int) flags->sw[15]) && (z<=altl[4])) { - /* Turbopause */ - zh40=pdm[4][2]; - /* Mixed density at Zlb */ - b40=densu(zh40,db40,tinf,tlb,40.-xmm,alpha[4]-1.,&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - /* Mixed density at Alt */ - dm40=densu(z,b40,tinf,tlb,xmm,0.,&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - zhm40=zhm28; - /* Net density at Alt */ - output->d[4]=dnet(output->d[4],dm40,zhm40,xmm,40.); - /* Correction to specified mixing ratio at ground */ - rl=log(b28*pdm[4][1]/b40); - hc40=pdm[4][5]*pdl[1][9]; - zc40=pdm[4][4]*pdl[1][8]; - /* Net density corrected at Alt */ - output->d[4]=output->d[4]*ccor(z,rl,hc40,zc40); - } - - - /**** HYDROGEN DENSITY ****/ - - /* Density variation factor at Zlb */ - g1 = flags->sw[21]*globe7(pd[6], input, flags); - /* Diffusive density at Zlb */ - db01 = pdm[5][0]*exp(g1)*pd[6][0]; - /* Diffusive density at Alt */ - output->d[6]=densu(z,db01,tinf,tlb,1.,alpha[6],&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - dd=output->d[6]; - if (((int) flags->sw[15]) && (z<=altl[6])) { - /* Turbopause */ - zh01=pdm[5][2]; - /* Mixed density at Zlb */ - b01=densu(zh01,db01,tinf,tlb,1.-xmm,alpha[6]-1., &output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - /* Mixed density at Alt */ - dm01=densu(z,b01,tinf,tlb,xmm,0.,&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - zhm01=zhm28; - /* Net density at Alt */ - output->d[6]=dnet(output->d[6],dm01,zhm01,xmm,1.); - /* Correction to specified mixing ratio at ground */ - rl=log(b28*pdm[5][1]*sqrt(pdl[1][17]*pdl[1][17])/b01); - hc01=pdm[5][5]*pdl[1][11]; - zc01=pdm[5][4]*pdl[1][10]; - output->d[6]=output->d[6]*ccor(z,rl,hc01,zc01); - /* Chemistry correction */ - hcc01=pdm[5][7]*pdl[1][19]; - zcc01=pdm[5][6]*pdl[1][18]; - rc01=pdm[5][3]*pdl[1][20]; - /* Net density corrected at Alt */ - output->d[6]=output->d[6]*ccor(z,rc01,hcc01,zcc01); -} - - - /**** ATOMIC NITROGEN DENSITY ****/ - - /* Density variation factor at Zlb */ - g14 = flags->sw[21]*globe7(pd[7],input,flags); - /* Diffusive density at Zlb */ - db14 = pdm[6][0]*exp(g14)*pd[7][0]; - /* Diffusive density at Alt */ - output->d[7]=densu(z,db14,tinf,tlb,14.,alpha[7],&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - dd=output->d[7]; - if (((int) flags->sw[15]) && (z<=altl[7])) { - /* Turbopause */ - zh14=pdm[6][2]; - /* Mixed density at Zlb */ - b14=densu(zh14,db14,tinf,tlb,14.-xmm,alpha[7]-1., &output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - /* Mixed density at Alt */ - dm14=densu(z,b14,tinf,tlb,xmm,0.,&output->t[1],ptm[5],s,mn1,zn1,meso_tn1,meso_tgn1); - zhm14=zhm28; - /* Net density at Alt */ - output->d[7]=dnet(output->d[7],dm14,zhm14,xmm,14.); - /* Correction to specified mixing ratio at ground */ - rl=log(b28*pdm[6][1]*sqrt(pdl[0][2]*pdl[0][2])/b14); - hc14=pdm[6][5]*pdl[0][1]; - zc14=pdm[6][4]*pdl[0][0]; - output->d[7]=output->d[7]*ccor(z,rl,hc14,zc14); - /* Chemistry correction */ - hcc14=pdm[6][7]*pdl[0][4]; - zcc14=pdm[6][6]*pdl[0][3]; - rc14=pdm[6][3]*pdl[0][5]; - /* Net density corrected at Alt */ - output->d[7]=output->d[7]*ccor(z,rc14,hcc14,zcc14); - } - - - /**** Anomalous OXYGEN DENSITY ****/ - - g16h = flags->sw[21]*globe7(pd[8],input,flags); - db16h = pdm[7][0]*exp(g16h)*pd[8][0]; - tho = pdm[7][9]*pdl[0][6]; - dd=densu(z,db16h,tho,tho,16.,alpha[8],&output->t[1],ptm[5],s,mn1, zn1,meso_tn1,meso_tgn1); - zsht=pdm[7][5]; - zmho=pdm[7][4]; - zsho=scalh(zmho,16.0,tho); - output->d[8]=dd*exp(-zsht/zsho*(exp(-(z-zmho)/zsht)-1.)); - - - /* total mass density */ - output->d[5] = 1.66E-24*(4.0*output->d[0]+16.0*output->d[1]+28.0*output->d[2]+32.0*output->d[3]+40.0*output->d[4]+ output->d[6]+14.0*output->d[7]); - - - /* temperature */ - z = sqrt(input->alt*input->alt); - ddum = densu(z,1.0, tinf, tlb, 0.0, 0.0, &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); - (void) ddum; /* silence gcc */ - if ((int) flags->sw[0]) { - for(i=0;i<9;i++) - output->d[i]=output->d[i]*1.0E6; - output->d[5]=output->d[5]/1000; - } +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! + */ + double za; + int i, j; + double ddum, z; + double zn1[5] = { 120.0, 110.0, 100.0, 90.0, 72.5 }; + double tinf; + int mn1 = 5; + double g0; + double tlb; + double s; + double db01, db04, db14, db16, db28, db32, db40; + double zh28, zh04, zh16, zh32, zh40, zh01, zh14; + double zhm28, zhm04, zhm16, zhm32, zhm40, zhm01, zhm14; + double xmd; + double b28, b04, b16, b32, b40, b01, b14; + double tz; + double g28, g4, g16, g32, g40, g1, g14; + double zhf, xmm; + double zc04, zc16, zc32, zc40, zc01, zc14; + double hc04, hc16, hc32, hc40, hc01, hc14; + double hcc16, hcc32, hcc01, hcc14; + double zcc16, zcc32, zcc01, zcc14; + double rc16, rc32, rc01, rc14; + double rl; + double g16h, db16h, tho, zsht, zmho, zsho; + double dgtr = 1.74533E-2; + double dr = 1.72142E-2; + double alpha[9] = { -0.38, 0.0, 0.0, 0.0, 0.17, 0.0, -0.38, 0.0, 0.0 }; + double altl[8] = { 200.0, 300.0, 160.0, 250.0, 240.0, 450.0, 320.0, 450.0 }; + double dd; + double hc216, hcc232; + za = pdl[1][15]; + zn1[0] = za; + for (j = 0; j < 9; j++) + output->d[j] = 0; + + /* TINF VARIATIONS NOT IMPORTANT BELOW ZA OR ZN1(1) */ + if (input->alt > zn1[0]) + tinf = ptm[0] * pt[0] * (1.0 + flags->sw[16] * globe7(pt, input, flags)); + else + tinf = ptm[0] * pt[0]; + output->t[0] = tinf; + + /* GRADIENT VARIATIONS NOT IMPORTANT BELOW ZN1(5) */ + if (input->alt > zn1[4]) + g0 = ptm[3] * ps[0] * (1.0 + flags->sw[19] * globe7(ps, input, flags)); + else + g0 = ptm[3] * ps[0]; + tlb = ptm[1] * (1.0 + flags->sw[17] * globe7(pd[3], input, flags)) * pd[3][0]; + s = g0 / (tinf - tlb); + + /* Lower thermosphere temp variations not significant for + * density above 300 km */ + if (input->alt < 300.0) { + meso_tn1[1] = ptm[6] * ptl[0][0] / (1.0 - flags->sw[18] * glob7s(ptl[0], input, flags)); + meso_tn1[2] = ptm[2] * ptl[1][0] / (1.0 - flags->sw[18] * glob7s(ptl[1], input, flags)); + meso_tn1[3] = ptm[7] * ptl[2][0] / (1.0 - flags->sw[18] * glob7s(ptl[2], input, flags)); + meso_tn1[4] = ptm[4] * ptl[3][0] / (1.0 - flags->sw[18] * flags->sw[20] * glob7s(ptl[3], input, flags)); + meso_tgn1[1] = ptm[8] * pma[8][0] * (1.0 + flags->sw[18] * flags->sw[20] * glob7s(pma[8], input, flags)) * + meso_tn1[4] * meso_tn1[4] / (pow((ptm[4] * ptl[3][0]), 2.0)); + } else { + meso_tn1[1] = ptm[6] * ptl[0][0]; + meso_tn1[2] = ptm[2] * ptl[1][0]; + meso_tn1[3] = ptm[7] * ptl[2][0]; + meso_tn1[4] = ptm[4] * ptl[3][0]; + meso_tgn1[1] = ptm[8] * pma[8][0] * meso_tn1[4] * meso_tn1[4] / (pow((ptm[4] * ptl[3][0]), 2.0)); + } + + /* N2 variation factor at Zlb */ + g28 = flags->sw[21] * globe7(pd[2], input, flags); + + /* VARIATION OF TURBOPAUSE HEIGHT */ + zhf = pdl[1][24] * (1.0 + flags->sw[5] * pdl[0][24] * sin(dgtr * input->g_lat) * cos(dr * (input->doy - pt[13]))); + output->t[0] = tinf; + xmm = pdm[2][4]; + z = input->alt; + + /**** N2 DENSITY ****/ + + /* Diffusive density at Zlb */ + db28 = pdm[2][0] * exp(g28) * pd[2][0]; + /* Diffusive density at Alt */ + output->d[2] = densu(z, db28, tinf, tlb, 28.0, alpha[2], &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + dd = output->d[2]; + /* Turbopause */ + zh28 = pdm[2][2] * zhf; + 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); + if (((int)flags->sw[15]) && (z <= altl[2])) { + /* Mixed density at Alt */ + dm28 = densu(z, b28, tinf, tlb, xmm, alpha[2], &tz, ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + /* Net density at Alt */ + output->d[2] = dnet(output->d[2], dm28, zhm28, xmm, 28.0); + } + + /**** HE DENSITY ****/ + + /* Density variation factor at Zlb */ + g4 = flags->sw[21] * globe7(pd[0], input, flags); + /* Diffusive density at Zlb */ + db04 = pdm[0][0] * exp(g4) * pd[0][0]; + /* Diffusive density at Alt */ + output->d[0] = densu(z, db04, tinf, tlb, 4., alpha[0], &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + dd = output->d[0]; + if (((int)flags->sw[15]) && (z < altl[0])) { + /* Turbopause */ + zh04 = pdm[0][2]; + /* Mixed density at Zlb */ + b04 = densu( + zh04, db04, tinf, tlb, 4. - xmm, alpha[0] - 1., &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + /* Mixed density at Alt */ + dm04 = densu(z, b04, tinf, tlb, xmm, 0., &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + zhm04 = zhm28; + /* Net density at Alt */ + output->d[0] = dnet(output->d[0], dm04, zhm04, xmm, 4.); + /* Correction to specified mixing ratio at ground */ + rl = log(b28 * pdm[0][1] / b04); + zc04 = pdm[0][4] * pdl[1][0]; + hc04 = pdm[0][5] * pdl[1][1]; + /* Net density corrected at Alt */ + output->d[0] = output->d[0] * ccor(z, rl, hc04, zc04); + } + + /**** O DENSITY ****/ + + /* Density variation factor at Zlb */ + g16 = flags->sw[21] * globe7(pd[1], input, flags); + /* Diffusive density at Zlb */ + db16 = pdm[1][0] * exp(g16) * pd[1][0]; + /* Diffusive density at Alt */ + output->d[1] = densu(z, db16, tinf, tlb, 16., alpha[1], &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + dd = output->d[1]; + if (((int)flags->sw[15]) && (z <= altl[1])) { + /* Turbopause */ + zh16 = pdm[1][2]; + /* Mixed density at Zlb */ + b16 = densu( + zh16, db16, tinf, tlb, 16.0 - xmm, (alpha[1] - 1.0), &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + /* Mixed density at Alt */ + dm16 = densu(z, b16, tinf, tlb, xmm, 0., &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + zhm16 = zhm28; + /* Net density at Alt */ + output->d[1] = dnet(output->d[1], dm16, zhm16, xmm, 16.); + rl = pdm[1][1] * pdl[1][16] * (1.0 + flags->sw[1] * pdl[0][23] * (input->f107A - 150.0)); + hc16 = pdm[1][5] * pdl[1][3]; + zc16 = pdm[1][4] * pdl[1][2]; + hc216 = pdm[1][5] * pdl[1][4]; + output->d[1] = output->d[1] * ccor2(z, rl, hc16, zc16, hc216); + /* Chemistry correction */ + hcc16 = pdm[1][7] * pdl[1][13]; + zcc16 = pdm[1][6] * pdl[1][12]; + rc16 = pdm[1][3] * pdl[1][14]; + /* Net density corrected at Alt */ + output->d[1] = output->d[1] * ccor(z, rc16, hcc16, zcc16); + } + + /**** O2 DENSITY ****/ + + /* Density variation factor at Zlb */ + g32 = flags->sw[21] * globe7(pd[4], input, flags); + /* Diffusive density at Zlb */ + db32 = pdm[3][0] * exp(g32) * pd[4][0]; + /* Diffusive density at Alt */ + output->d[3] = densu(z, db32, tinf, tlb, 32., alpha[3], &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + dd = output->d[3]; + if ((int)flags->sw[15]) { + if (z <= altl[3]) { + /* Turbopause */ + zh32 = pdm[3][2]; + /* Mixed density at Zlb */ + b32 = densu( + zh32, db32, tinf, tlb, 32. - xmm, alpha[3] - 1., &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + /* Mixed density at Alt */ + dm32 = densu(z, b32, tinf, tlb, xmm, 0., &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + zhm32 = zhm28; + /* Net density at Alt */ + output->d[3] = dnet(output->d[3], dm32, zhm32, xmm, 32.); + /* Correction to specified mixing ratio at ground */ + rl = log(b28 * pdm[3][1] / b32); + hc32 = pdm[3][5] * pdl[1][7]; + zc32 = pdm[3][4] * pdl[1][6]; + output->d[3] = output->d[3] * ccor(z, rl, hc32, zc32); + } + /* Correction for general departure from diffusive equilibrium above Zlb */ + hcc32 = pdm[3][7] * pdl[1][22]; + hcc232 = pdm[3][7] * pdl[0][22]; + zcc32 = pdm[3][6] * pdl[1][21]; + rc32 = pdm[3][3] * pdl[1][23] * (1. + flags->sw[1] * pdl[0][23] * (input->f107A - 150.)); + /* Net density corrected at Alt */ + output->d[3] = output->d[3] * ccor2(z, rc32, hcc32, zcc32, hcc232); + } + + /**** AR DENSITY ****/ + + /* Density variation factor at Zlb */ + g40 = flags->sw[21] * globe7(pd[5], input, flags); + /* Diffusive density at Zlb */ + db40 = pdm[4][0] * exp(g40) * pd[5][0]; + /* Diffusive density at Alt */ + output->d[4] = densu(z, db40, tinf, tlb, 40., alpha[4], &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + dd = output->d[4]; + if (((int)flags->sw[15]) && (z <= altl[4])) { + /* Turbopause */ + zh40 = pdm[4][2]; + /* Mixed density at Zlb */ + b40 = densu( + zh40, db40, tinf, tlb, 40. - xmm, alpha[4] - 1., &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + /* Mixed density at Alt */ + dm40 = densu(z, b40, tinf, tlb, xmm, 0., &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + zhm40 = zhm28; + /* Net density at Alt */ + output->d[4] = dnet(output->d[4], dm40, zhm40, xmm, 40.); + /* Correction to specified mixing ratio at ground */ + rl = log(b28 * pdm[4][1] / b40); + hc40 = pdm[4][5] * pdl[1][9]; + zc40 = pdm[4][4] * pdl[1][8]; + /* Net density corrected at Alt */ + output->d[4] = output->d[4] * ccor(z, rl, hc40, zc40); + } + + /**** HYDROGEN DENSITY ****/ + + /* Density variation factor at Zlb */ + g1 = flags->sw[21] * globe7(pd[6], input, flags); + /* Diffusive density at Zlb */ + db01 = pdm[5][0] * exp(g1) * pd[6][0]; + /* Diffusive density at Alt */ + output->d[6] = densu(z, db01, tinf, tlb, 1., alpha[6], &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + dd = output->d[6]; + if (((int)flags->sw[15]) && (z <= altl[6])) { + /* Turbopause */ + zh01 = pdm[5][2]; + /* Mixed density at Zlb */ + b01 = densu( + zh01, db01, tinf, tlb, 1. - xmm, alpha[6] - 1., &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + /* Mixed density at Alt */ + dm01 = densu(z, b01, tinf, tlb, xmm, 0., &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + zhm01 = zhm28; + /* Net density at Alt */ + output->d[6] = dnet(output->d[6], dm01, zhm01, xmm, 1.); + /* Correction to specified mixing ratio at ground */ + rl = log(b28 * pdm[5][1] * sqrt(pdl[1][17] * pdl[1][17]) / b01); + hc01 = pdm[5][5] * pdl[1][11]; + zc01 = pdm[5][4] * pdl[1][10]; + output->d[6] = output->d[6] * ccor(z, rl, hc01, zc01); + /* Chemistry correction */ + hcc01 = pdm[5][7] * pdl[1][19]; + zcc01 = pdm[5][6] * pdl[1][18]; + rc01 = pdm[5][3] * pdl[1][20]; + /* Net density corrected at Alt */ + output->d[6] = output->d[6] * ccor(z, rc01, hcc01, zcc01); + } + + /**** ATOMIC NITROGEN DENSITY ****/ + + /* Density variation factor at Zlb */ + g14 = flags->sw[21] * globe7(pd[7], input, flags); + /* Diffusive density at Zlb */ + db14 = pdm[6][0] * exp(g14) * pd[7][0]; + /* Diffusive density at Alt */ + output->d[7] = densu(z, db14, tinf, tlb, 14., alpha[7], &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + dd = output->d[7]; + if (((int)flags->sw[15]) && (z <= altl[7])) { + /* Turbopause */ + zh14 = pdm[6][2]; + /* Mixed density at Zlb */ + b14 = densu( + zh14, db14, tinf, tlb, 14. - xmm, alpha[7] - 1., &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + /* Mixed density at Alt */ + dm14 = densu(z, b14, tinf, tlb, xmm, 0., &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + zhm14 = zhm28; + /* Net density at Alt */ + output->d[7] = dnet(output->d[7], dm14, zhm14, xmm, 14.); + /* Correction to specified mixing ratio at ground */ + rl = log(b28 * pdm[6][1] * sqrt(pdl[0][2] * pdl[0][2]) / b14); + hc14 = pdm[6][5] * pdl[0][1]; + zc14 = pdm[6][4] * pdl[0][0]; + output->d[7] = output->d[7] * ccor(z, rl, hc14, zc14); + /* Chemistry correction */ + hcc14 = pdm[6][7] * pdl[0][4]; + zcc14 = pdm[6][6] * pdl[0][3]; + rc14 = pdm[6][3] * pdl[0][5]; + /* Net density corrected at Alt */ + output->d[7] = output->d[7] * ccor(z, rc14, hcc14, zcc14); + } + + /**** Anomalous OXYGEN DENSITY ****/ + + g16h = flags->sw[21] * globe7(pd[8], input, flags); + db16h = pdm[7][0] * exp(g16h) * pd[8][0]; + tho = pdm[7][9] * pdl[0][6]; + dd = densu(z, db16h, tho, tho, 16., alpha[8], &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + zsht = pdm[7][5]; + zmho = pdm[7][4]; + zsho = scalh(zmho, 16.0, tho); + output->d[8] = dd * exp(-zsht / zsho * (exp(-(z - zmho) / zsht) - 1.)); + + /* total mass density */ + output->d[5] = 1.66E-24 * (4.0 * output->d[0] + 16.0 * output->d[1] + 28.0 * output->d[2] + 32.0 * output->d[3] + + 40.0 * output->d[4] + output->d[6] + 14.0 * output->d[7]); + + /* temperature */ + z = sqrt(input->alt * input->alt); + ddum = densu(z, 1.0, tinf, tlb, 0.0, 0.0, &output->t[1], ptm[5], s, mn1, zn1, meso_tn1, meso_tgn1); + (void)ddum; /* silence gcc */ + if ((int)flags->sw[0]) { + for (i = 0; i < 9; i++) + output->d[i] = output->d[i] * 1.0E6; + output->d[5] = output->d[5] / 1000; + } } diff --git a/src/simulation/environment/MsisAtmosphere/nrlmsise-00.h b/src/simulation/environment/MsisAtmosphere/nrlmsise-00.h index 9f5c31a860..a2ad366278 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 * @@ -16,24 +16,23 @@ * updated releases of this package. */ - - /* ------------------------------------------------------------------- */ /* ------------------------------- INPUT ----------------------------- */ /* ------------------------------------------------------------------- */ /*! NRL MSIS flags structure */ -struct nrlmsise_flags { - int switches[24]; //!< variable - double sw[24]; //!< variable - double swc[24]; //!< variable +struct nrlmsise_flags +{ + int switches[24]; //!< variable + 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]: @@ -68,8 +67,9 @@ struct nrlmsise_flags { */ /*! ap array structure */ -struct ap_array { - double a[7]; //!< variable +struct ap_array +{ + double a[7]; //!< variable }; /* Array containing the following magnetic values: * 0 : daily AP @@ -77,30 +77,31 @@ 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 */ -struct nrlmsise_input { - int year; /*!< year, currently ignored */ - int doy; /*!< day of year */ - double sec; /*!< seconds in day (UT) */ - double alt; /*!< altitude in kilometers */ - double g_lat; /*!< geodetic latitude */ - double g_long; /*!< geodetic longitude */ - double lst; /*!< local apparent solar time (hours), see note below */ - double f107A; /*!< 81 day average of F10.7 flux (centered on doy) */ - double f107; /*!< daily F10.7 flux for previous day */ - double ap; /*!< magnetic index(daily) */ - struct ap_array *ap_a; /*!< see above */ +struct nrlmsise_input +{ + int year; /*!< year, currently ignored */ + int doy; /*!< day of year */ + double sec; /*!< seconds in day (UT) */ + double alt; /*!< altitude in kilometers */ + double g_lat; /*!< geodetic latitude */ + double g_long; /*!< geodetic longitude */ + double lst; /*!< local apparent solar time (hours), see note below */ + double f107A; /*!< 81 day average of F10.7 flux (centered on doy) */ + double f107; /*!< daily F10.7 flux for previous day */ + double ap; /*!< magnetic index(daily) */ + 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 @@ -118,30 +119,29 @@ struct nrlmsise_input { * 150., 150., and 4. respectively. */ - - /* ------------------------------------------------------------------- */ /* ------------------------------ OUTPUT ----------------------------- */ /* ------------------------------------------------------------------- */ /*! NRL MSISE output structure */ -struct nrlmsise_output { - double d[9]; /*!< densities */ - double t[2]; /*!< temperatures */ +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 @@ -162,8 +162,6 @@ struct nrlmsise_output { * in this model, INCLUDING anomalous oxygen. */ - - /* ------------------------------------------------------------------- */ /* --------------------------- PROTOTYPES ---------------------------- */ /* ------------------------------------------------------------------- */ @@ -172,10 +170,8 @@ struct nrlmsise_output { /* Neutral Atmosphere Empircial Model from the surface to lower * exosphere. */ -void gtd7 (struct nrlmsise_input *input, \ - struct nrlmsise_flags *flags, \ - struct nrlmsise_output *output); - +void +gtd7(struct nrlmsise_input* input, struct nrlmsise_flags* flags, struct nrlmsise_output* output); /* GTD7D */ /* This subroutine provides Effective Total Mass Density for output @@ -183,29 +179,21 @@ void gtd7 (struct nrlmsise_input *input, \ * affect satellite drag above 500 km. See the section "output" for * additional details. */ -void gtd7d(struct nrlmsise_input *input, \ - struct nrlmsise_flags *flags, \ - struct nrlmsise_output *output); - +void +gtd7d(struct nrlmsise_input* input, struct nrlmsise_flags* flags, struct nrlmsise_output* output); /* GTS7 */ /* Thermospheric portion of NRLMSISE-00 */ -void gts7 (struct nrlmsise_input *input, \ - struct nrlmsise_flags *flags, \ - struct nrlmsise_output *output); - +void +gts7(struct nrlmsise_input* input, struct nrlmsise_flags* flags, struct nrlmsise_output* output); /* GHP7 */ /* To specify outputs at a pressure level (press) rather than at * an altitude. */ -void ghp7 (struct nrlmsise_input *input, \ - struct nrlmsise_flags *flags, \ - struct nrlmsise_output *output, \ - double press); - - +void +ghp7(struct nrlmsise_input* input, struct nrlmsise_flags* flags, struct nrlmsise_output* output, double press); /* ------------------------------------------------------------------- */ /* ----------------------- COMPILATION TWEAKS ------------------------ */ diff --git a/src/simulation/environment/MsisAtmosphere/nrlmsise-00_data.c b/src/simulation/environment/MsisAtmosphere/nrlmsise-00_data.c index 0175e6cfc2..e8c1bc1335 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 * @@ -16,725 +16,591 @@ * updated releases of this package. */ - - /* ------------------------------------------------------------------- */ /* ------------------------ BLOCK DATA GTD7BK ------------------------ */ /* ------------------------------------------------------------------- */ /* TEMPERATURE */ double pt[150] = { - 9.86573E-01, 1.62228E-02, 1.55270E-02,-1.04323E-01,-3.75801E-03, - -1.18538E-03,-1.24043E-01, 4.56820E-03, 8.76018E-03,-1.36235E-01, - -3.52427E-02, 8.84181E-03,-5.92127E-03,-8.61650E+00, 0.00000E+00, - 1.28492E-02, 0.00000E+00, 1.30096E+02, 1.04567E-02, 1.65686E-03, - -5.53887E-06, 2.97810E-03, 0.00000E+00, 5.13122E-03, 8.66784E-02, - 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00,-7.27026E-06, - 0.00000E+00, 6.74494E+00, 4.93933E-03, 2.21656E-03, 2.50802E-03, - 0.00000E+00, 0.00000E+00,-2.08841E-02,-1.79873E+00, 1.45103E-03, - 2.81769E-04,-1.44703E-03,-5.16394E-05, 8.47001E-02, 1.70147E-01, - 5.72562E-03, 5.07493E-05, 4.36148E-03, 1.17863E-04, 4.74364E-03, - 6.61278E-03, 4.34292E-05, 1.44373E-03, 2.41470E-05, 2.84426E-03, - 8.56560E-04, 2.04028E-03, 0.00000E+00,-3.15994E+03,-2.46423E-03, - 1.13843E-03, 4.20512E-04, 0.00000E+00,-9.77214E+01, 6.77794E-03, - 5.27499E-03, 1.14936E-03, 0.00000E+00,-6.61311E-03,-1.84255E-02, - -1.96259E-02, 2.98618E+04, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 6.44574E+02, 8.84668E-04, 5.05066E-04, 0.00000E+00, 4.02881E+03, - -1.89503E-03, 0.00000E+00, 0.00000E+00, 8.21407E-04, 2.06780E-03, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - -1.20410E-02,-3.63963E-03, 9.92070E-05,-1.15284E-04,-6.33059E-05, - -6.05545E-01, 8.34218E-03,-9.13036E+01, 3.71042E-04, 0.00000E+00, - 4.19000E-04, 2.70928E-03, 3.31507E-03,-4.44508E-03,-4.96334E-03, - -1.60449E-03, 3.95119E-03, 2.48924E-03, 5.09815E-04, 4.05302E-03, - 2.24076E-03, 0.00000E+00, 6.84256E-03, 4.66354E-04, 0.00000E+00, - -3.68328E-04, 0.00000E+00, 0.00000E+00,-1.46870E+02, 0.00000E+00, - 0.00000E+00, 1.09501E-03, 4.65156E-04, 5.62583E-04, 3.21596E+00, - 6.43168E-04, 3.14860E-03, 3.40738E-03, 1.78481E-03, 9.62532E-04, - 5.58171E-04, 3.43731E+00,-2.33195E-01, 5.10289E-04, 0.00000E+00, - 0.00000E+00,-9.25347E+04, 0.00000E+00,-1.99639E-03, 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 + 9.86573E-01, 1.62228E-02, 1.55270E-02, -1.04323E-01, -3.75801E-03, -1.18538E-03, -1.24043E-01, 4.56820E-03, + 8.76018E-03, -1.36235E-01, -3.52427E-02, 8.84181E-03, -5.92127E-03, -8.61650E+00, 0.00000E+00, 1.28492E-02, + 0.00000E+00, 1.30096E+02, 1.04567E-02, 1.65686E-03, -5.53887E-06, 2.97810E-03, 0.00000E+00, 5.13122E-03, + 8.66784E-02, 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, -7.27026E-06, 0.00000E+00, 6.74494E+00, + 4.93933E-03, 2.21656E-03, 2.50802E-03, 0.00000E+00, 0.00000E+00, -2.08841E-02, -1.79873E+00, 1.45103E-03, + 2.81769E-04, -1.44703E-03, -5.16394E-05, 8.47001E-02, 1.70147E-01, 5.72562E-03, 5.07493E-05, 4.36148E-03, + 1.17863E-04, 4.74364E-03, 6.61278E-03, 4.34292E-05, 1.44373E-03, 2.41470E-05, 2.84426E-03, 8.56560E-04, + 2.04028E-03, 0.00000E+00, -3.15994E+03, -2.46423E-03, 1.13843E-03, 4.20512E-04, 0.00000E+00, -9.77214E+01, + 6.77794E-03, 5.27499E-03, 1.14936E-03, 0.00000E+00, -6.61311E-03, -1.84255E-02, -1.96259E-02, 2.98618E+04, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 6.44574E+02, 8.84668E-04, 5.05066E-04, 0.00000E+00, 4.02881E+03, + -1.89503E-03, 0.00000E+00, 0.00000E+00, 8.21407E-04, 2.06780E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -1.20410E-02, -3.63963E-03, 9.92070E-05, -1.15284E-04, -6.33059E-05, -6.05545E-01, + 8.34218E-03, -9.13036E+01, 3.71042E-04, 0.00000E+00, 4.19000E-04, 2.70928E-03, 3.31507E-03, -4.44508E-03, + -4.96334E-03, -1.60449E-03, 3.95119E-03, 2.48924E-03, 5.09815E-04, 4.05302E-03, 2.24076E-03, 0.00000E+00, + 6.84256E-03, 4.66354E-04, 0.00000E+00, -3.68328E-04, 0.00000E+00, 0.00000E+00, -1.46870E+02, 0.00000E+00, + 0.00000E+00, 1.09501E-03, 4.65156E-04, 5.62583E-04, 3.21596E+00, 6.43168E-04, 3.14860E-03, 3.40738E-03, + 1.78481E-03, 9.62532E-04, 5.58171E-04, 3.43731E+00, -2.33195E-01, 5.10289E-04, 0.00000E+00, 0.00000E+00, + -9.25347E+04, 0.00000E+00, -1.99639E-03, 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 }; double pd[9][150] = { -/* HE DENSITY */ { - 1.09979E+00,-4.88060E-02,-1.97501E-01,-9.10280E-02,-6.96558E-03, - 2.42136E-02, 3.91333E-01,-7.20068E-03,-3.22718E-02, 1.41508E+00, - 1.68194E-01, 1.85282E-02, 1.09384E-01,-7.24282E+00, 0.00000E+00, - 2.96377E-01,-4.97210E-02, 1.04114E+02,-8.61108E-02,-7.29177E-04, - 1.48998E-06, 1.08629E-03, 0.00000E+00, 0.00000E+00, 8.31090E-02, - 1.12818E-01,-5.75005E-02,-1.29919E-02,-1.78849E-02,-2.86343E-06, - 0.00000E+00,-1.51187E+02,-6.65902E-03, 0.00000E+00,-2.02069E-03, - 0.00000E+00, 0.00000E+00, 4.32264E-02,-2.80444E+01,-3.26789E-03, - 2.47461E-03, 0.00000E+00, 0.00000E+00, 9.82100E-02, 1.22714E-01, - -3.96450E-02, 0.00000E+00,-2.76489E-03, 0.00000E+00, 1.87723E-03, - -8.09813E-03, 4.34428E-05,-7.70932E-03, 0.00000E+00,-2.28894E-03, - -5.69070E-03,-5.22193E-03, 6.00692E-03,-7.80434E+03,-3.48336E-03, - -6.38362E-03,-1.82190E-03, 0.00000E+00,-7.58976E+01,-2.17875E-02, - -1.72524E-02,-9.06287E-03, 0.00000E+00, 2.44725E-02, 8.66040E-02, - 1.05712E-01, 3.02543E+04, 0.00000E+00, 0.00000E+00, 0.00000E+00, - -6.01364E+03,-5.64668E-03,-2.54157E-03, 0.00000E+00, 3.15611E+02, - -5.69158E-03, 0.00000E+00, 0.00000E+00,-4.47216E-03,-4.49523E-03, - 4.64428E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 4.51236E-02, 2.46520E-02, 6.17794E-03, 0.00000E+00, 0.00000E+00, - -3.62944E-01,-4.80022E-02,-7.57230E+01,-1.99656E-03, 0.00000E+00, - -5.18780E-03,-1.73990E-02,-9.03485E-03, 7.48465E-03, 1.53267E-02, - 1.06296E-02, 1.18655E-02, 2.55569E-03, 1.69020E-03, 3.51936E-02, - -1.81242E-02, 0.00000E+00,-1.00529E-01,-5.10574E-03, 0.00000E+00, - 2.10228E-03, 0.00000E+00, 0.00000E+00,-1.73255E+02, 5.07833E-01, - -2.41408E-01, 8.75414E-03, 2.77527E-03,-8.90353E-05,-5.25148E+00, - -5.83899E-03,-2.09122E-02,-9.63530E-03, 9.77164E-03, 4.07051E-03, - 2.53555E-04,-5.52875E+00,-3.55993E-01,-2.49231E-03, 0.00000E+00, - 0.00000E+00, 2.86026E+01, 0.00000E+00, 3.42722E-04, 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 -}, /* O DENSITY */ { - 1.02315E+00,-1.59710E-01,-1.06630E-01,-1.77074E-02,-4.42726E-03, - 3.44803E-02, 4.45613E-02,-3.33751E-02,-5.73598E-02, 3.50360E-01, - 6.33053E-02, 2.16221E-02, 5.42577E-02,-5.74193E+00, 0.00000E+00, - 1.90891E-01,-1.39194E-02, 1.01102E+02, 8.16363E-02, 1.33717E-04, - 6.54403E-06, 3.10295E-03, 0.00000E+00, 0.00000E+00, 5.38205E-02, - 1.23910E-01,-1.39831E-02, 0.00000E+00, 0.00000E+00,-3.95915E-06, - 0.00000E+00,-7.14651E-01,-5.01027E-03, 0.00000E+00,-3.24756E-03, - 0.00000E+00, 0.00000E+00, 4.42173E-02,-1.31598E+01,-3.15626E-03, - 1.24574E-03,-1.47626E-03,-1.55461E-03, 6.40682E-02, 1.34898E-01, - -2.42415E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 6.13666E-04, - -5.40373E-03, 2.61635E-05,-3.33012E-03, 0.00000E+00,-3.08101E-03, - -2.42679E-03,-3.36086E-03, 0.00000E+00,-1.18979E+03,-5.04738E-02, - -2.61547E-03,-1.03132E-03, 1.91583E-04,-8.38132E+01,-1.40517E-02, - -1.14167E-02,-4.08012E-03, 1.73522E-04,-1.39644E-02,-6.64128E-02, - -6.85152E-02,-1.34414E+04, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 6.07916E+02,-4.12220E-03,-2.20996E-03, 0.00000E+00, 1.70277E+03, - -4.63015E-03, 0.00000E+00, 0.00000E+00,-2.25360E-03,-2.96204E-03, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 3.92786E-02, 1.31186E-02,-1.78086E-03, 0.00000E+00, 0.00000E+00, - -3.90083E-01,-2.84741E-02,-7.78400E+01,-1.02601E-03, 0.00000E+00, - -7.26485E-04,-5.42181E-03,-5.59305E-03, 1.22825E-02, 1.23868E-02, - 6.68835E-03,-1.03303E-02,-9.51903E-03, 2.70021E-04,-2.57084E-02, - -1.32430E-02, 0.00000E+00,-3.81000E-02,-3.16810E-03, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-9.05762E-04,-2.14590E-03,-1.17824E-03, 3.66732E+00, - -3.79729E-04,-6.13966E-03,-5.09082E-03,-1.96332E-03,-3.08280E-03, - -9.75222E-04, 4.03315E+00,-2.52710E-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 -}, /* N2 DENSITY */ { - 1.16112E+00, 0.00000E+00, 0.00000E+00, 3.33725E-02, 0.00000E+00, - 3.48637E-02,-5.44368E-03, 0.00000E+00,-6.73940E-02, 1.74754E-01, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.74712E+02, 0.00000E+00, - 1.26733E-01, 0.00000E+00, 1.03154E+02, 5.52075E-02, 0.00000E+00, - 0.00000E+00, 8.13525E-04, 0.00000E+00, 0.00000E+00, 8.66784E-02, - 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-2.50482E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-2.48894E-03, - 6.16053E-04,-5.79716E-04, 2.95482E-03, 8.47001E-02, 1.70147E-01, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 2.47425E-05, 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, 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, - 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 -}, /* TLB */ { - 9.44846E-01, 0.00000E+00, 0.00000E+00,-3.08617E-02, 0.00000E+00, - -2.44019E-02, 6.48607E-03, 0.00000E+00, 3.08181E-02, 4.59392E-02, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.74712E+02, 0.00000E+00, - 2.13260E-02, 0.00000E+00,-3.56958E+02, 0.00000E+00, 1.82278E-04, - 0.00000E+00, 3.07472E-04, 0.00000E+00, 0.00000E+00, 8.66784E-02, - 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 3.83054E-03, 0.00000E+00, 0.00000E+00, - -1.93065E-03,-1.45090E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-1.23493E-03, 1.36736E-03, 8.47001E-02, 1.70147E-01, - 3.71469E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 5.10250E-03, 2.47425E-05, 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, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 3.68756E-03, 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, 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 -}, /* O2 DENSITY */ { - 1.35580E+00, 1.44816E-01, 0.00000E+00, 6.07767E-02, 0.00000E+00, - 2.94777E-02, 7.46900E-02, 0.00000E+00,-9.23822E-02, 8.57342E-02, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.38636E+01, 0.00000E+00, - 7.71653E-02, 0.00000E+00, 8.18751E+01, 1.87736E-02, 0.00000E+00, - 0.00000E+00, 1.49667E-02, 0.00000E+00, 0.00000E+00, 8.66784E-02, - 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-3.67874E+02, 5.48158E-03, 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, 8.47001E-02, 1.70147E-01, - 1.22631E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 8.17187E-03, 3.71617E-05, 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,-2.10826E-03, - -3.13640E-03, 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, - -7.35742E-02,-5.00266E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 1.94965E-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, 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 -}, /* AR DENSITY */ { - 1.04761E+00, 2.00165E-01, 2.37697E-01, 3.68552E-02, 0.00000E+00, - 3.57202E-02,-2.14075E-01, 0.00000E+00,-1.08018E-01,-3.73981E-01, - 0.00000E+00, 3.10022E-02,-1.16305E-03,-2.07596E+01, 0.00000E+00, - 8.64502E-02, 0.00000E+00, 9.74908E+01, 5.16707E-02, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 8.66784E-02, - 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 3.46193E+02, 1.34297E-02, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-3.48509E-03, - -1.54689E-04, 0.00000E+00, 0.00000E+00, 8.47001E-02, 1.70147E-01, - 1.47753E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 1.89320E-02, 3.68181E-05, 1.32570E-02, 0.00000E+00, 0.00000E+00, - 3.59719E-03, 7.44328E-03,-1.00023E-03,-6.50528E+03, 0.00000E+00, - 1.03485E-02,-1.00983E-03,-4.06916E-03,-6.60864E+01,-1.71533E-02, - 1.10605E-02, 1.20300E-02,-5.20034E-03, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - -2.62769E+03, 7.13755E-03, 4.17999E-03, 0.00000E+00, 1.25910E+04, - 0.00000E+00, 0.00000E+00, 0.00000E+00,-2.23595E-03, 4.60217E-03, - 5.71794E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - -3.18353E-02,-2.35526E-02,-1.36189E-02, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 2.03522E-02,-6.67837E+01,-1.09724E-03, 0.00000E+00, - -1.38821E-02, 1.60468E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.51574E-02, - -5.44470E-04, 0.00000E+00, 7.28224E-02, 6.59413E-02, 0.00000E+00, - -5.15692E-03, 0.00000E+00, 0.00000E+00,-3.70367E+03, 0.00000E+00, - 0.00000E+00, 1.36131E-02, 5.38153E-03, 0.00000E+00, 4.76285E+00, - -1.75677E-02, 2.26301E-02, 0.00000E+00, 1.76631E-02, 4.77162E-03, - 0.00000E+00, 5.39354E+00, 0.00000E+00,-7.51710E-03, 0.00000E+00, - 0.00000E+00,-8.82736E+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 -}, /* H DENSITY */ { - 1.26376E+00,-2.14304E-01,-1.49984E-01, 2.30404E-01, 2.98237E-02, - 2.68673E-02, 2.96228E-01, 2.21900E-02,-2.07655E-02, 4.52506E-01, - 1.20105E-01, 3.24420E-02, 4.24816E-02,-9.14313E+00, 0.00000E+00, - 2.47178E-02,-2.88229E-02, 8.12805E+01, 5.10380E-02,-5.80611E-03, - 2.51236E-05,-1.24083E-02, 0.00000E+00, 0.00000E+00, 8.66784E-02, - 1.58727E-01,-3.48190E-02, 0.00000E+00, 0.00000E+00, 2.89885E-05, - 0.00000E+00, 1.53595E+02,-1.68604E-02, 0.00000E+00, 1.01015E-02, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.84552E-04, - -1.22181E-03, 0.00000E+00, 0.00000E+00, 8.47001E-02, 1.70147E-01, - -1.04927E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00,-5.91313E-03, - -2.30501E-02, 3.14758E-05, 0.00000E+00, 0.00000E+00, 1.26956E-02, - 8.35489E-03, 3.10513E-04, 0.00000E+00, 3.42119E+03,-2.45017E-03, - -4.27154E-04, 5.45152E-04, 1.89896E-03, 2.89121E+01,-6.49973E-03, - -1.93855E-02,-1.48492E-02, 0.00000E+00,-5.10576E-02, 7.87306E-02, - 9.51981E-02,-1.49422E+04, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 2.65503E+02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 6.37110E-03, 3.24789E-04, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 6.14274E-02, 1.00376E-02,-8.41083E-04, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-1.27099E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, - -3.94077E-03,-1.28601E-02,-7.97616E-03, 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,-6.71465E-03,-1.69799E-03, 1.93772E-03, 3.81140E+00, - -7.79290E-03,-1.82589E-02,-1.25860E-02,-1.04311E-02,-3.02465E-03, - 2.43063E-03, 3.63237E+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 -}, /* N DENSITY */ { - 7.09557E+01,-3.26740E-01, 0.00000E+00,-5.16829E-01,-1.71664E-03, - 9.09310E-02,-6.71500E-01,-1.47771E-01,-9.27471E-02,-2.30862E-01, - -1.56410E-01, 1.34455E-02,-1.19717E-01, 2.52151E+00, 0.00000E+00, - -2.41582E-01, 5.92939E-02, 4.39756E+00, 9.15280E-02, 4.41292E-03, - 0.00000E+00, 8.66807E-03, 0.00000E+00, 0.00000E+00, 8.66784E-02, - 1.58727E-01, 9.74701E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 6.70217E+01,-1.31660E-03, 0.00000E+00,-1.65317E-02, - 0.00000E+00, 0.00000E+00, 8.50247E-02, 2.77428E+01, 4.98658E-03, - 6.15115E-03, 9.50156E-03,-2.12723E-02, 8.47001E-02, 1.70147E-01, - -2.38645E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.37380E-03, - -8.41918E-03, 2.80145E-05, 7.12383E-03, 0.00000E+00,-1.66209E-02, - 1.03533E-04,-1.68898E-02, 0.00000E+00, 3.64526E+03, 0.00000E+00, - 6.54077E-03, 3.69130E-04, 9.94419E-04, 8.42803E+01,-1.16124E-02, - -7.74414E-03,-1.68844E-03, 1.42809E-03,-1.92955E-03, 1.17225E-01, - -2.41512E-02, 1.50521E+04, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 1.60261E+03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00,-3.54403E-04,-1.87270E-02, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 2.76439E-02, 6.43207E-03,-3.54300E-02, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-2.80221E-02, 8.11228E+01,-6.75255E-04, 0.00000E+00, - -1.05162E-02,-3.48292E-03,-6.97321E-03, 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,-1.45546E-03,-1.31970E-02,-3.57751E-03,-1.09021E+00, - -1.50181E-02,-7.12841E-03,-6.64590E-03,-3.52610E-03,-1.87773E-02, - -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 -}, /* 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, - 0.00000E+00, 0.00000E+00, 0.00000E+00,-8.61650E+00, 1.26454E-02, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 5.50878E-03, 0.00000E+00, 0.00000E+00, 8.66784E-02, - 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 6.23881E-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, 8.47001E-02, 1.70147E-01, - -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, - 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, 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 -}}; + /* HE DENSITY */ { + 1.09979E+00, -4.88060E-02, -1.97501E-01, -9.10280E-02, -6.96558E-03, 2.42136E-02, 3.91333E-01, -7.20068E-03, + -3.22718E-02, 1.41508E+00, 1.68194E-01, 1.85282E-02, 1.09384E-01, -7.24282E+00, 0.00000E+00, 2.96377E-01, + -4.97210E-02, 1.04114E+02, -8.61108E-02, -7.29177E-04, 1.48998E-06, 1.08629E-03, 0.00000E+00, 0.00000E+00, + 8.31090E-02, 1.12818E-01, -5.75005E-02, -1.29919E-02, -1.78849E-02, -2.86343E-06, 0.00000E+00, -1.51187E+02, + -6.65902E-03, 0.00000E+00, -2.02069E-03, 0.00000E+00, 0.00000E+00, 4.32264E-02, -2.80444E+01, -3.26789E-03, + 2.47461E-03, 0.00000E+00, 0.00000E+00, 9.82100E-02, 1.22714E-01, -3.96450E-02, 0.00000E+00, -2.76489E-03, + 0.00000E+00, 1.87723E-03, -8.09813E-03, 4.34428E-05, -7.70932E-03, 0.00000E+00, -2.28894E-03, -5.69070E-03, + -5.22193E-03, 6.00692E-03, -7.80434E+03, -3.48336E-03, -6.38362E-03, -1.82190E-03, 0.00000E+00, -7.58976E+01, + -2.17875E-02, -1.72524E-02, -9.06287E-03, 0.00000E+00, 2.44725E-02, 8.66040E-02, 1.05712E-01, 3.02543E+04, + 0.00000E+00, 0.00000E+00, 0.00000E+00, -6.01364E+03, -5.64668E-03, -2.54157E-03, 0.00000E+00, 3.15611E+02, + -5.69158E-03, 0.00000E+00, 0.00000E+00, -4.47216E-03, -4.49523E-03, 4.64428E-03, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 4.51236E-02, 2.46520E-02, 6.17794E-03, 0.00000E+00, 0.00000E+00, -3.62944E-01, + -4.80022E-02, -7.57230E+01, -1.99656E-03, 0.00000E+00, -5.18780E-03, -1.73990E-02, -9.03485E-03, 7.48465E-03, + 1.53267E-02, 1.06296E-02, 1.18655E-02, 2.55569E-03, 1.69020E-03, 3.51936E-02, -1.81242E-02, 0.00000E+00, + -1.00529E-01, -5.10574E-03, 0.00000E+00, 2.10228E-03, 0.00000E+00, 0.00000E+00, -1.73255E+02, 5.07833E-01, + -2.41408E-01, 8.75414E-03, 2.77527E-03, -8.90353E-05, -5.25148E+00, -5.83899E-03, -2.09122E-02, -9.63530E-03, + 9.77164E-03, 4.07051E-03, 2.53555E-04, -5.52875E+00, -3.55993E-01, -2.49231E-03, 0.00000E+00, 0.00000E+00, + 2.86026E+01, 0.00000E+00, 3.42722E-04, 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 }, + /* O DENSITY */ { 1.02315E+00, -1.59710E-01, -1.06630E-01, -1.77074E-02, -4.42726E-03, 3.44803E-02, 4.45613E-02, + -3.33751E-02, -5.73598E-02, 3.50360E-01, 6.33053E-02, 2.16221E-02, 5.42577E-02, -5.74193E+00, + 0.00000E+00, 1.90891E-01, -1.39194E-02, 1.01102E+02, 8.16363E-02, 1.33717E-04, 6.54403E-06, + 3.10295E-03, 0.00000E+00, 0.00000E+00, 5.38205E-02, 1.23910E-01, -1.39831E-02, 0.00000E+00, + 0.00000E+00, -3.95915E-06, 0.00000E+00, -7.14651E-01, -5.01027E-03, 0.00000E+00, -3.24756E-03, + 0.00000E+00, 0.00000E+00, 4.42173E-02, -1.31598E+01, -3.15626E-03, 1.24574E-03, -1.47626E-03, + -1.55461E-03, 6.40682E-02, 1.34898E-01, -2.42415E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 6.13666E-04, -5.40373E-03, 2.61635E-05, -3.33012E-03, 0.00000E+00, -3.08101E-03, -2.42679E-03, + -3.36086E-03, 0.00000E+00, -1.18979E+03, -5.04738E-02, -2.61547E-03, -1.03132E-03, 1.91583E-04, + -8.38132E+01, -1.40517E-02, -1.14167E-02, -4.08012E-03, 1.73522E-04, -1.39644E-02, -6.64128E-02, + -6.85152E-02, -1.34414E+04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 6.07916E+02, -4.12220E-03, + -2.20996E-03, 0.00000E+00, 1.70277E+03, -4.63015E-03, 0.00000E+00, 0.00000E+00, -2.25360E-03, + -2.96204E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 3.92786E-02, + 1.31186E-02, -1.78086E-03, 0.00000E+00, 0.00000E+00, -3.90083E-01, -2.84741E-02, -7.78400E+01, + -1.02601E-03, 0.00000E+00, -7.26485E-04, -5.42181E-03, -5.59305E-03, 1.22825E-02, 1.23868E-02, + 6.68835E-03, -1.03303E-02, -9.51903E-03, 2.70021E-04, -2.57084E-02, -1.32430E-02, 0.00000E+00, + -3.81000E-02, -3.16810E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -9.05762E-04, -2.14590E-03, -1.17824E-03, 3.66732E+00, -3.79729E-04, + -6.13966E-03, -5.09082E-03, -1.96332E-03, -3.08280E-03, -9.75222E-04, 4.03315E+00, -2.52710E-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 }, + /* N2 DENSITY */ { 1.16112E+00, 0.00000E+00, 0.00000E+00, 3.33725E-02, 0.00000E+00, 3.48637E-02, -5.44368E-03, + 0.00000E+00, -6.73940E-02, 1.74754E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.74712E+02, + 0.00000E+00, 1.26733E-01, 0.00000E+00, 1.03154E+02, 5.52075E-02, 0.00000E+00, 0.00000E+00, + 8.13525E-04, 0.00000E+00, 0.00000E+00, 8.66784E-02, 1.58727E-01, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, -2.50482E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -2.48894E-03, 6.16053E-04, -5.79716E-04, + 2.95482E-03, 8.47001E-02, 1.70147E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 2.47425E-05, 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, + 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, 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 }, + /* TLB */ { 9.44846E-01, 0.00000E+00, 0.00000E+00, -3.08617E-02, 0.00000E+00, -2.44019E-02, 6.48607E-03, + 0.00000E+00, 3.08181E-02, 4.59392E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.74712E+02, + 0.00000E+00, 2.13260E-02, 0.00000E+00, -3.56958E+02, 0.00000E+00, 1.82278E-04, 0.00000E+00, + 3.07472E-04, 0.00000E+00, 0.00000E+00, 8.66784E-02, 1.58727E-01, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 3.83054E-03, 0.00000E+00, 0.00000E+00, + -1.93065E-03, -1.45090E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -1.23493E-03, + 1.36736E-03, 8.47001E-02, 1.70147E-01, 3.71469E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 5.10250E-03, 2.47425E-05, 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, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 3.68756E-03, 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, 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 }, + /* O2 DENSITY */ { 1.35580E+00, 1.44816E-01, 0.00000E+00, 6.07767E-02, 0.00000E+00, 2.94777E-02, 7.46900E-02, + 0.00000E+00, -9.23822E-02, 8.57342E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.38636E+01, + 0.00000E+00, 7.71653E-02, 0.00000E+00, 8.18751E+01, 1.87736E-02, 0.00000E+00, 0.00000E+00, + 1.49667E-02, 0.00000E+00, 0.00000E+00, 8.66784E-02, 1.58727E-01, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, -3.67874E+02, 5.48158E-03, 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, 8.47001E-02, 1.70147E-01, 1.22631E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 8.17187E-03, 3.71617E-05, 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, -2.10826E-03, -3.13640E-03, 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, -7.35742E-02, + -5.00266E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.94965E-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, 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 }, + /* AR DENSITY */ { 1.04761E+00, 2.00165E-01, 2.37697E-01, 3.68552E-02, 0.00000E+00, 3.57202E-02, -2.14075E-01, + 0.00000E+00, -1.08018E-01, -3.73981E-01, 0.00000E+00, 3.10022E-02, -1.16305E-03, -2.07596E+01, + 0.00000E+00, 8.64502E-02, 0.00000E+00, 9.74908E+01, 5.16707E-02, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 8.66784E-02, 1.58727E-01, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 3.46193E+02, 1.34297E-02, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -3.48509E-03, -1.54689E-04, 0.00000E+00, + 0.00000E+00, 8.47001E-02, 1.70147E-01, 1.47753E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 1.89320E-02, 3.68181E-05, 1.32570E-02, 0.00000E+00, 0.00000E+00, 3.59719E-03, + 7.44328E-03, -1.00023E-03, -6.50528E+03, 0.00000E+00, 1.03485E-02, -1.00983E-03, -4.06916E-03, + -6.60864E+01, -1.71533E-02, 1.10605E-02, 1.20300E-02, -5.20034E-03, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -2.62769E+03, 7.13755E-03, + 4.17999E-03, 0.00000E+00, 1.25910E+04, 0.00000E+00, 0.00000E+00, 0.00000E+00, -2.23595E-03, + 4.60217E-03, 5.71794E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -3.18353E-02, + -2.35526E-02, -1.36189E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.03522E-02, -6.67837E+01, + -1.09724E-03, 0.00000E+00, -1.38821E-02, 1.60468E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.51574E-02, -5.44470E-04, 0.00000E+00, + 7.28224E-02, 6.59413E-02, 0.00000E+00, -5.15692E-03, 0.00000E+00, 0.00000E+00, -3.70367E+03, + 0.00000E+00, 0.00000E+00, 1.36131E-02, 5.38153E-03, 0.00000E+00, 4.76285E+00, -1.75677E-02, + 2.26301E-02, 0.00000E+00, 1.76631E-02, 4.77162E-03, 0.00000E+00, 5.39354E+00, 0.00000E+00, + -7.51710E-03, 0.00000E+00, 0.00000E+00, -8.82736E+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 }, + /* H DENSITY */ { 1.26376E+00, -2.14304E-01, -1.49984E-01, 2.30404E-01, 2.98237E-02, 2.68673E-02, 2.96228E-01, + 2.21900E-02, -2.07655E-02, 4.52506E-01, 1.20105E-01, 3.24420E-02, 4.24816E-02, -9.14313E+00, + 0.00000E+00, 2.47178E-02, -2.88229E-02, 8.12805E+01, 5.10380E-02, -5.80611E-03, 2.51236E-05, + -1.24083E-02, 0.00000E+00, 0.00000E+00, 8.66784E-02, 1.58727E-01, -3.48190E-02, 0.00000E+00, + 0.00000E+00, 2.89885E-05, 0.00000E+00, 1.53595E+02, -1.68604E-02, 0.00000E+00, 1.01015E-02, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.84552E-04, -1.22181E-03, 0.00000E+00, + 0.00000E+00, 8.47001E-02, 1.70147E-01, -1.04927E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, + -5.91313E-03, -2.30501E-02, 3.14758E-05, 0.00000E+00, 0.00000E+00, 1.26956E-02, 8.35489E-03, + 3.10513E-04, 0.00000E+00, 3.42119E+03, -2.45017E-03, -4.27154E-04, 5.45152E-04, 1.89896E-03, + 2.89121E+01, -6.49973E-03, -1.93855E-02, -1.48492E-02, 0.00000E+00, -5.10576E-02, 7.87306E-02, + 9.51981E-02, -1.49422E+04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.65503E+02, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 6.37110E-03, + 3.24789E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 6.14274E-02, + 1.00376E-02, -8.41083E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, -1.27099E-02, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -3.94077E-03, -1.28601E-02, -7.97616E-03, 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, -6.71465E-03, -1.69799E-03, 1.93772E-03, 3.81140E+00, -7.79290E-03, + -1.82589E-02, -1.25860E-02, -1.04311E-02, -3.02465E-03, 2.43063E-03, 3.63237E+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 }, + /* N DENSITY */ { 7.09557E+01, -3.26740E-01, 0.00000E+00, -5.16829E-01, -1.71664E-03, 9.09310E-02, -6.71500E-01, + -1.47771E-01, -9.27471E-02, -2.30862E-01, -1.56410E-01, 1.34455E-02, -1.19717E-01, 2.52151E+00, + 0.00000E+00, -2.41582E-01, 5.92939E-02, 4.39756E+00, 9.15280E-02, 4.41292E-03, 0.00000E+00, + 8.66807E-03, 0.00000E+00, 0.00000E+00, 8.66784E-02, 1.58727E-01, 9.74701E-02, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 6.70217E+01, -1.31660E-03, 0.00000E+00, -1.65317E-02, + 0.00000E+00, 0.00000E+00, 8.50247E-02, 2.77428E+01, 4.98658E-03, 6.15115E-03, 9.50156E-03, + -2.12723E-02, 8.47001E-02, 1.70147E-01, -2.38645E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 1.37380E-03, -8.41918E-03, 2.80145E-05, 7.12383E-03, 0.00000E+00, -1.66209E-02, 1.03533E-04, + -1.68898E-02, 0.00000E+00, 3.64526E+03, 0.00000E+00, 6.54077E-03, 3.69130E-04, 9.94419E-04, + 8.42803E+01, -1.16124E-02, -7.74414E-03, -1.68844E-03, 1.42809E-03, -1.92955E-03, 1.17225E-01, + -2.41512E-02, 1.50521E+04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.60261E+03, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -3.54403E-04, + -1.87270E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.76439E-02, + 6.43207E-03, -3.54300E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, -2.80221E-02, 8.11228E+01, + -6.75255E-04, 0.00000E+00, -1.05162E-02, -3.48292E-03, -6.97321E-03, 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, -1.45546E-03, -1.31970E-02, -3.57751E-03, -1.09021E+00, -1.50181E-02, + -7.12841E-03, -6.64590E-03, -3.52610E-03, -1.87773E-02, -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 }, + /* 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, 0.00000E+00, 0.00000E+00, 0.00000E+00, -8.61650E+00, + 1.26454E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 5.50878E-03, 0.00000E+00, 0.00000E+00, 8.66784E-02, 1.58727E-01, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 6.23881E-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, 8.47001E-02, 1.70147E-01, -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, 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, 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 } +}; /* S PARAM */ double ps[150] = { - 9.56827E-01, 6.20637E-02, 3.18433E-02, 0.00000E+00, 0.00000E+00, - 3.94900E-02, 0.00000E+00, 0.00000E+00,-9.24882E-03,-7.94023E-03, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.74712E+02, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 2.74677E-03, 0.00000E+00, 1.54951E-02, 8.66784E-02, - 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00,-6.99007E-04, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 1.24362E-02,-5.28756E-03, 8.47001E-02, 1.70147E-01, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 2.47425E-05, 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, 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, - 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 + 9.56827E-01, 6.20637E-02, 3.18433E-02, 0.00000E+00, 0.00000E+00, 3.94900E-02, 0.00000E+00, 0.00000E+00, + -9.24882E-03, -7.94023E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.74712E+02, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.74677E-03, 0.00000E+00, 1.54951E-02, + 8.66784E-02, 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, -6.99007E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 1.24362E-02, -5.28756E-03, 8.47001E-02, 1.70147E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.47425E-05, 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, 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, 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 }; /* TURBO */ -double pdl[2][25] = { - { 1.09930E+00, 3.90631E+00, 3.07165E+00, 9.86161E-01, 1.63536E+01, - 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 }, - { 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, - 1.23435E+02,-7.60535E-02, 1.68986E-06, 7.44294E-01, 1.03604E+00, - 1.72783E+02, 1.15020E+00, 3.44689E+00,-7.46230E-01, 9.49154E-01 } -}; +double pdl[2][25] = { { 1.09930E+00, 3.90631E+00, 3.07165E+00, 9.86161E-01, 1.63536E+01, 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 }, + { 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, 1.23435E+02, -7.60535E-02, 1.68986E-06, 7.44294E-01, 1.03604E+00, 1.72783E+02, + 1.15020E+00, 3.44689E+00, -7.46230E-01, 9.49154E-01 } }; /* LOWER BOUNDARY */ -double ptm[50] = { - 1.04130E+03, 3.86000E+02, 1.95000E+02, 1.66728E+01, 2.13000E+02, - 1.20000E+02, 2.40000E+02, 1.87000E+02,-2.00000E+00, 0.00000E+00 -}; -double pdm[8][10] = { -{ 2.45600E+07, 6.71072E-06, 1.00000E+02, 0.00000E+00, 1.10000E+02, - 1.00000E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00 },\ -{ 8.59400E+10, 1.00000E+00, 1.05000E+02,-8.00000E+00, 1.10000E+02, - 1.00000E+01, 9.00000E+01, 2.00000E+00, 0.00000E+00, 0.00000E+00 },\ -{ 2.81000E+11, 0.00000E+00, 1.05000E+02, 2.80000E+01, 2.89500E+01, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00 }, -{ 3.30000E+10, 2.68270E-01, 1.05000E+02, 1.00000E+00, 1.10000E+02, - 1.00000E+01, 1.10000E+02,-1.00000E+01, 0.00000E+00, 0.00000E+00 }, -{ 1.33000E+09, 1.19615E-02, 1.05000E+02, 0.00000E+00, 1.10000E+02, - 1.00000E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00 }, -{ 1.76100E+05, 1.00000E+00, 9.50000E+01,-8.00000E+00, 1.10000E+02, - 1.00000E+01, 9.00000E+01, 2.00000E+00, 0.00000E+00, 0.00000E+00, }, -{ 1.00000E+07, 1.00000E+00, 1.05000E+02,-8.00000E+00, 1.10000E+02, - 1.00000E+01, 9.00000E+01, 2.00000E+00, 0.00000E+00, 0.00000E+00 }, -{ 1.00000E+06, 1.00000E+00, 1.05000E+02,-8.00000E+00, 5.50000E+02, - 7.60000E+01, 9.00000E+01, 2.00000E+00, 0.00000E+00, 4.00000E+03 }}; - +double ptm[50] = { 1.04130E+03, 3.86000E+02, 1.95000E+02, 1.66728E+01, 2.13000E+02, + 1.20000E+02, 2.40000E+02, 1.87000E+02, -2.00000E+00, 0.00000E+00 }; +double pdm[8][10] = { { 2.45600E+07, + 6.71072E-06, + 1.00000E+02, + 0.00000E+00, + 1.10000E+02, + 1.00000E+01, + 0.00000E+00, + 0.00000E+00, + 0.00000E+00, + 0.00000E+00 }, + { 8.59400E+10, + 1.00000E+00, + 1.05000E+02, + -8.00000E+00, + 1.10000E+02, + 1.00000E+01, + 9.00000E+01, + 2.00000E+00, + 0.00000E+00, + 0.00000E+00 }, + { 2.81000E+11, + 0.00000E+00, + 1.05000E+02, + 2.80000E+01, + 2.89500E+01, + 0.00000E+00, + 0.00000E+00, + 0.00000E+00, + 0.00000E+00, + 0.00000E+00 }, + { 3.30000E+10, + 2.68270E-01, + 1.05000E+02, + 1.00000E+00, + 1.10000E+02, + 1.00000E+01, + 1.10000E+02, + -1.00000E+01, + 0.00000E+00, + 0.00000E+00 }, + { 1.33000E+09, + 1.19615E-02, + 1.05000E+02, + 0.00000E+00, + 1.10000E+02, + 1.00000E+01, + 0.00000E+00, + 0.00000E+00, + 0.00000E+00, + 0.00000E+00 }, + { + 1.76100E+05, + 1.00000E+00, + 9.50000E+01, + -8.00000E+00, + 1.10000E+02, + 1.00000E+01, + 9.00000E+01, + 2.00000E+00, + 0.00000E+00, + 0.00000E+00, + }, + { 1.00000E+07, + 1.00000E+00, + 1.05000E+02, + -8.00000E+00, + 1.10000E+02, + 1.00000E+01, + 9.00000E+01, + 2.00000E+00, + 0.00000E+00, + 0.00000E+00 }, + { 1.00000E+06, + 1.00000E+00, + 1.05000E+02, + -8.00000E+00, + 5.50000E+02, + 7.60000E+01, + 9.00000E+01, + 2.00000E+00, + 0.00000E+00, + 4.00000E+03 } }; double ptl[4][100] = { -/* TN1(2) */ { - 1.00858E+00, 4.56011E-02,-2.22972E-02,-5.44388E-02, 5.23136E-04, - -1.88849E-02, 5.23707E-02,-9.43646E-03, 6.31707E-03,-7.80460E-02, - -4.88430E-02, 0.00000E+00, 0.00000E+00,-7.60250E+00, 0.00000E+00, - -1.44635E-02,-1.76843E-02,-1.21517E+02, 2.85647E-02, 0.00000E+00, - 0.00000E+00, 6.31792E-04, 0.00000E+00, 5.77197E-03, 8.66784E-02, - 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-8.90272E+03, 3.30611E-03, 3.02172E-03, 0.00000E+00, - -2.13673E-03,-3.20910E-04, 0.00000E+00, 0.00000E+00, 2.76034E-03, - 2.82487E-03,-2.97592E-04,-4.21534E-03, 8.47001E-02, 1.70147E-01, - 8.96456E-03, 0.00000E+00,-1.08596E-02, 0.00000E+00, 0.00000E+00, - 5.57917E-03, 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, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 9.65405E-03, 0.00000E+00, 0.00000E+00, 2.00000E+00 -}, /* TN1(3) */ { - 9.39664E-01, 8.56514E-02,-6.79989E-03, 2.65929E-02,-4.74283E-03, - 1.21855E-02,-2.14905E-02, 6.49651E-03,-2.05477E-02,-4.24952E-02, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.19148E+01, 0.00000E+00, - 1.18777E-02,-7.28230E-02,-8.15965E+01, 1.73887E-02, 0.00000E+00, - 0.00000E+00, 0.00000E+00,-1.44691E-02, 2.80259E-04, 8.66784E-02, - 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 2.16584E+02, 3.18713E-03, 7.37479E-03, 0.00000E+00, - -2.55018E-03,-3.92806E-03, 0.00000E+00, 0.00000E+00,-2.89757E-03, - -1.33549E-03, 1.02661E-03, 3.53775E-04, 8.47001E-02, 1.70147E-01, - -9.17497E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 3.56082E-03, 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, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-1.00902E-02, 0.00000E+00, 0.00000E+00, 2.00000E+00 -}, /* TN1(4) */ { - 9.85982E-01,-4.55435E-02, 1.21106E-02, 2.04127E-02,-2.40836E-03, - 1.11383E-02,-4.51926E-02, 1.35074E-02,-6.54139E-03, 1.15275E-01, - 1.28247E-01, 0.00000E+00, 0.00000E+00,-5.30705E+00, 0.00000E+00, - -3.79332E-02,-6.24741E-02, 7.71062E-01, 2.96315E-02, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 6.81051E-03,-4.34767E-03, 8.66784E-02, - 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 1.07003E+01,-2.76907E-03, 4.32474E-04, 0.00000E+00, - 1.31497E-03,-6.47517E-04, 0.00000E+00,-2.20621E+01,-1.10804E-03, - -8.09338E-04, 4.18184E-04, 4.29650E-03, 8.47001E-02, 1.70147E-01, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - -4.04337E-03, 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,-9.52550E-04, - 8.56253E-04, 4.33114E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.21223E-03, - 2.38694E-04, 9.15245E-04, 1.28385E-03, 8.67668E-04,-5.61425E-06, - 1.04445E+00, 3.41112E+01, 0.00000E+00,-8.40704E-01,-2.39639E+02, - 7.06668E-01,-2.05873E+01,-3.63696E-01, 2.39245E+01, 0.00000E+00, - -1.06657E-03,-7.67292E-04, 1.54534E-04, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 -}, /* TN1(5) TN2(1) */ { - 1.00320E+00, 3.83501E-02,-2.38983E-03, 2.83950E-03, 4.20956E-03, - 5.86619E-04, 2.19054E-02,-1.00946E-02,-3.50259E-03, 4.17392E-02, - -8.44404E-03, 0.00000E+00, 0.00000E+00, 4.96949E+00, 0.00000E+00, - -7.06478E-03,-1.46494E-02, 3.13258E+01,-1.86493E-03, 0.00000E+00, - -1.67499E-02, 0.00000E+00, 0.00000E+00, 5.12686E-04, 8.66784E-02, - 1.58727E-01,-4.64167E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 4.37353E-03,-1.99069E+02, 0.00000E+00,-5.34884E-03, 0.00000E+00, - 1.62458E-03, 2.93016E-03, 2.67926E-03, 5.90449E+02, 0.00000E+00, - 0.00000E+00,-1.17266E-03,-3.58890E-04, 8.47001E-02, 1.70147E-01, - 0.00000E+00, 0.00000E+00, 1.38673E-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, 1.60571E-03, - 6.28078E-04, 5.05469E-05, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-1.57829E-03, - -4.00855E-04, 5.04077E-05,-1.39001E-03,-2.33406E-03,-4.81197E-04, - 1.46758E+00, 6.20332E+00, 0.00000E+00, 3.66476E-01,-6.19760E+01, - 3.09198E-01,-1.98999E+01, 0.00000E+00,-3.29933E+02, 0.00000E+00, - -1.10080E-03,-9.39310E-05, 1.39638E-04, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 -} }; + /* TN1(2) */ { + 1.00858E+00, 4.56011E-02, -2.22972E-02, -5.44388E-02, 5.23136E-04, -1.88849E-02, 5.23707E-02, -9.43646E-03, + 6.31707E-03, -7.80460E-02, -4.88430E-02, 0.00000E+00, 0.00000E+00, -7.60250E+00, 0.00000E+00, -1.44635E-02, + -1.76843E-02, -1.21517E+02, 2.85647E-02, 0.00000E+00, 0.00000E+00, 6.31792E-04, 0.00000E+00, 5.77197E-03, + 8.66784E-02, 1.58727E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -8.90272E+03, + 3.30611E-03, 3.02172E-03, 0.00000E+00, -2.13673E-03, -3.20910E-04, 0.00000E+00, 0.00000E+00, 2.76034E-03, + 2.82487E-03, -2.97592E-04, -4.21534E-03, 8.47001E-02, 1.70147E-01, 8.96456E-03, 0.00000E+00, -1.08596E-02, + 0.00000E+00, 0.00000E+00, 5.57917E-03, 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, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 9.65405E-03, 0.00000E+00, 0.00000E+00, 2.00000E+00 }, + /* TN1(3) */ { 9.39664E-01, 8.56514E-02, -6.79989E-03, 2.65929E-02, -4.74283E-03, 1.21855E-02, -2.14905E-02, + 6.49651E-03, -2.05477E-02, -4.24952E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.19148E+01, + 0.00000E+00, 1.18777E-02, -7.28230E-02, -8.15965E+01, 1.73887E-02, 0.00000E+00, 0.00000E+00, + 0.00000E+00, -1.44691E-02, 2.80259E-04, 8.66784E-02, 1.58727E-01, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.16584E+02, 3.18713E-03, 7.37479E-03, 0.00000E+00, + -2.55018E-03, -3.92806E-03, 0.00000E+00, 0.00000E+00, -2.89757E-03, -1.33549E-03, 1.02661E-03, + 3.53775E-04, 8.47001E-02, 1.70147E-01, -9.17497E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 3.56082E-03, 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, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -1.00902E-02, 0.00000E+00, + 0.00000E+00, 2.00000E+00 }, + /* TN1(4) */ { 9.85982E-01, -4.55435E-02, 1.21106E-02, 2.04127E-02, -2.40836E-03, 1.11383E-02, -4.51926E-02, + 1.35074E-02, -6.54139E-03, 1.15275E-01, 1.28247E-01, 0.00000E+00, 0.00000E+00, -5.30705E+00, + 0.00000E+00, -3.79332E-02, -6.24741E-02, 7.71062E-01, 2.96315E-02, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 6.81051E-03, -4.34767E-03, 8.66784E-02, 1.58727E-01, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.07003E+01, -2.76907E-03, 4.32474E-04, 0.00000E+00, + 1.31497E-03, -6.47517E-04, 0.00000E+00, -2.20621E+01, -1.10804E-03, -8.09338E-04, 4.18184E-04, + 4.29650E-03, 8.47001E-02, 1.70147E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, -4.04337E-03, 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, -9.52550E-04, 8.56253E-04, 4.33114E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.21223E-03, 2.38694E-04, 9.15245E-04, + 1.28385E-03, 8.67668E-04, -5.61425E-06, 1.04445E+00, 3.41112E+01, 0.00000E+00, -8.40704E-01, + -2.39639E+02, 7.06668E-01, -2.05873E+01, -3.63696E-01, 2.39245E+01, 0.00000E+00, -1.06657E-03, + -7.67292E-04, 1.54534E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 2.00000E+00 }, + /* TN1(5) TN2(1) */ { 1.00320E+00, 3.83501E-02, -2.38983E-03, 2.83950E-03, 4.20956E-03, 5.86619E-04, + 2.19054E-02, -1.00946E-02, -3.50259E-03, 4.17392E-02, -8.44404E-03, 0.00000E+00, + 0.00000E+00, 4.96949E+00, 0.00000E+00, -7.06478E-03, -1.46494E-02, 3.13258E+01, + -1.86493E-03, 0.00000E+00, -1.67499E-02, 0.00000E+00, 0.00000E+00, 5.12686E-04, + 8.66784E-02, 1.58727E-01, -4.64167E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 4.37353E-03, -1.99069E+02, 0.00000E+00, -5.34884E-03, 0.00000E+00, 1.62458E-03, + 2.93016E-03, 2.67926E-03, 5.90449E+02, 0.00000E+00, 0.00000E+00, -1.17266E-03, + -3.58890E-04, 8.47001E-02, 1.70147E-01, 0.00000E+00, 0.00000E+00, 1.38673E-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, 1.60571E-03, 6.28078E-04, + 5.05469E-05, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -1.57829E-03, -4.00855E-04, 5.04077E-05, -1.39001E-03, + -2.33406E-03, -4.81197E-04, 1.46758E+00, 6.20332E+00, 0.00000E+00, 3.66476E-01, + -6.19760E+01, 3.09198E-01, -1.98999E+01, 0.00000E+00, -3.29933E+02, 0.00000E+00, + -1.10080E-03, -9.39310E-05, 1.39638E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 } +}; double pma[10][100] = { -/* TN2(2) */ { - 9.81637E-01,-1.41317E-03, 3.87323E-02, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-3.58707E-02, - -8.63658E-03, 0.00000E+00, 0.00000E+00,-2.02226E+00, 0.00000E+00, - -8.69424E-03,-1.91397E-02, 8.76779E+01, 4.52188E-03, 0.00000E+00, - 2.23760E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-7.07572E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, - -4.11210E-03, 3.50060E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00,-8.36657E-03, 1.61347E+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,-1.45130E-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, 1.24152E-03, - 6.43365E-04, 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.33255E-03, - 2.42657E-03, 1.60666E-03,-1.85728E-03,-1.46874E-03,-4.79163E-06, - 1.22464E+00, 3.53510E+01, 0.00000E+00, 4.49223E-01,-4.77466E+01, - 4.70681E-01, 8.41861E+00,-2.88198E-01, 1.67854E+02, 0.00000E+00, - 7.11493E-04, 6.05601E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 -}, /* TN2(3) */ { - 1.00422E+00,-7.11212E-03, 5.24480E-03, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-5.28914E-02, - -2.41301E-02, 0.00000E+00, 0.00000E+00,-2.12219E+01,-1.03830E-02, - -3.28077E-03, 1.65727E-02, 1.68564E+00,-6.68154E-03, 0.00000E+00, - 1.45155E-02, 0.00000E+00, 8.42365E-03, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-4.34645E-03, 0.00000E+00, 0.00000E+00, 2.16780E-02, - 0.00000E+00,-1.38459E+02, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 7.04573E-03,-4.73204E+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, 1.08767E-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,-8.08279E-03, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 5.21769E-04, - -2.27387E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 3.26769E-03, - 3.16901E-03, 4.60316E-04,-1.01431E-04, 1.02131E-03, 9.96601E-04, - 1.25707E+00, 2.50114E+01, 0.00000E+00, 4.24472E-01,-2.77655E+01, - 3.44625E-01, 2.75412E+01, 0.00000E+00, 7.94251E+02, 0.00000E+00, - 2.45835E-03, 1.38871E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 -}, /* TN2(4) TN3(1) */ { - 1.01890E+00,-2.46603E-02, 1.00078E-02, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-6.70977E-02, - -4.02286E-02, 0.00000E+00, 0.00000E+00,-2.29466E+01,-7.47019E-03, - 2.26580E-03, 2.63931E-02, 3.72625E+01,-6.39041E-03, 0.00000E+00, - 9.58383E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-1.85291E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 1.39717E+02, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 9.19771E-03,-3.69121E+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,-1.57067E-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,-7.07265E-03, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-2.92953E-03, - -2.77739E-03,-4.40092E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.47280E-03, - 2.95035E-04,-1.81246E-03, 2.81945E-03, 4.27296E-03, 9.78863E-04, - 1.40545E+00,-6.19173E+00, 0.00000E+00, 0.00000E+00,-7.93632E+01, - 4.44643E-01,-4.03085E+02, 0.00000E+00, 1.15603E+01, 0.00000E+00, - 2.25068E-03, 8.48557E-04,-2.98493E-04, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 -}, /* TN3(2) */ { - 9.75801E-01, 3.80680E-02,-3.05198E-02, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 3.85575E-02, - 5.04057E-02, 0.00000E+00, 0.00000E+00,-1.76046E+02, 1.44594E-02, - -1.48297E-03,-3.68560E-03, 3.02185E+01,-3.23338E-03, 0.00000E+00, - 1.53569E-02, 0.00000E+00,-1.15558E-02, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 4.89620E-03, 0.00000E+00, 0.00000E+00,-1.00616E-02, - -8.21324E-03,-1.57757E+02, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 6.63564E-03, 4.58410E+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,-2.51280E-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, 9.91215E-03, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-8.73148E-04, - -1.29648E-03,-7.32026E-05, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-4.68110E-03, - -4.66003E-03,-1.31567E-03,-7.39390E-04, 6.32499E-04,-4.65588E-04, - -1.29785E+00,-1.57139E+02, 0.00000E+00, 2.58350E-01,-3.69453E+01, - 4.10672E-01, 9.78196E+00,-1.52064E-01,-3.85084E+03, 0.00000E+00, - -8.52706E-04,-1.40945E-03,-7.26786E-04, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 -}, /* TN3(3) */ { - 9.60722E-01, 7.03757E-02,-3.00266E-02, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.22671E-02, - 4.10423E-02, 0.00000E+00, 0.00000E+00,-1.63070E+02, 1.06073E-02, - 5.40747E-04, 7.79481E-03, 1.44908E+02, 1.51484E-04, 0.00000E+00, - 1.97547E-02, 0.00000E+00,-1.41844E-02, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 5.77884E-03, 0.00000E+00, 0.00000E+00, 9.74319E-03, - 0.00000E+00,-2.88015E+03, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00,-4.44902E-03,-2.92760E+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, 2.34419E-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, 5.36685E-03, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-4.65325E-04, - -5.50628E-04, 3.31465E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-2.06179E-03, - -3.08575E-03,-7.93589E-04,-1.08629E-04, 5.95511E-04,-9.05050E-04, - 1.18997E+00, 4.15924E+01, 0.00000E+00,-4.72064E-01,-9.47150E+02, - 3.98723E-01, 1.98304E+01, 0.00000E+00, 3.73219E+03, 0.00000E+00, - -1.50040E-03,-1.14933E-03,-1.56769E-04, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 -}, /* TN3(4) */ { - 1.03123E+00,-7.05124E-02, 8.71615E-03, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-3.82621E-02, - -9.80975E-03, 0.00000E+00, 0.00000E+00, 2.89286E+01, 9.57341E-03, - 0.00000E+00, 0.00000E+00, 8.66153E+01, 7.91938E-04, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 4.68917E-03, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 7.86638E-03, 0.00000E+00, 0.00000E+00, 9.90827E-03, - 0.00000E+00, 6.55573E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00,-4.00200E+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, 7.07457E-03, 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, 5.72268E-03, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-2.04970E-04, - 1.21560E-03,-8.05579E-06, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-2.49941E-03, - -4.57256E-04,-1.59311E-04, 2.96481E-04,-1.77318E-03,-6.37918E-04, - 1.02395E+00, 1.28172E+01, 0.00000E+00, 1.49903E-01,-2.63818E+01, - 0.00000E+00, 4.70628E+01,-2.22139E-01, 4.82292E-02, 0.00000E+00, - -8.67075E-04,-5.86479E-04, 5.32462E-04, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 -}, /* TN3(5) SURFACE TEMP TSL */ { - 1.00828E+00,-9.10404E-02,-2.26549E-02, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-2.32420E-02, - -9.08925E-03, 0.00000E+00, 0.00000E+00, 3.36105E+01, 0.00000E+00, - 0.00000E+00, 0.00000E+00,-1.24957E+01,-5.87939E-03, 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, 2.79765E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.01237E+03, 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.75553E-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, 3.29699E-03, - 1.26659E-03, 2.68402E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.17894E-03, - 1.48746E-03, 1.06478E-04, 1.34743E-04,-2.20939E-03,-6.23523E-04, - 6.36539E-01, 1.13621E+01, 0.00000E+00,-3.93777E-01, 2.38687E+03, - 0.00000E+00, 6.61865E+02,-1.21434E-01, 9.27608E+00, 0.00000E+00, - 1.68478E-04, 1.24892E-03, 1.71345E-03, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 -}, /* TGN3(2) SURFACE GRAD TSLG */ { - 1.57293E+00,-6.78400E-01, 6.47500E-01, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-7.62974E-02, - -3.60423E-01, 0.00000E+00, 0.00000E+00, 1.28358E+02, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 4.68038E+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,-1.67898E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 2.90994E+04, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 3.15706E+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, 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, 2.00000E+00 -}, /* TGN2(1) TGN1(2) */ { - 8.60028E-01, 3.77052E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00,-1.17570E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 7.77757E-03, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 1.01024E+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, 6.54251E+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, 0.00000E+00, 0.00000E+00, 0.00000E+00,-1.56959E-02, - 1.91001E-02, 3.15971E-02, 1.00982E-02,-6.71565E-03, 2.57693E-03, - 1.38692E+00, 2.82132E-01, 0.00000E+00, 0.00000E+00, 3.81511E+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, 2.00000E+00 -}, /* TGN3(1) TGN2(2) */ { - 1.06029E+00,-5.25231E-02, 3.73034E-01, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 3.31072E-02, - -3.88409E-01, 0.00000E+00, 0.00000E+00,-1.65295E+02,-2.13801E-01, - -4.38916E-02,-3.22716E-01,-8.82393E+01, 1.18458E-01, 0.00000E+00, - -4.35863E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00,-1.19782E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 2.62229E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 0.00000E+00, 0.00000E+00, 0.00000E+00,-5.37443E+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,-4.55788E-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, 3.84009E-02, - 3.96733E-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, 5.05494E-02, - 7.39617E-02, 1.92200E-02,-8.46151E-03,-1.34244E-02, 1.96338E-02, - 1.50421E+00, 1.88368E+01, 0.00000E+00, 0.00000E+00,-5.13114E+01, - 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, - 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 -} }; - + /* TN2(2) */ { + 9.81637E-01, -1.41317E-03, 3.87323E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, -3.58707E-02, -8.63658E-03, 0.00000E+00, 0.00000E+00, -2.02226E+00, 0.00000E+00, -8.69424E-03, + -1.91397E-02, 8.76779E+01, 4.52188E-03, 0.00000E+00, 2.23760E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -7.07572E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, -4.11210E-03, 3.50060E+01, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -8.36657E-03, 1.61347E+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, -1.45130E-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, + 1.24152E-03, 6.43365E-04, 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.33255E-03, 2.42657E-03, 1.60666E-03, -1.85728E-03, -1.46874E-03, -4.79163E-06, + 1.22464E+00, 3.53510E+01, 0.00000E+00, 4.49223E-01, -4.77466E+01, 4.70681E-01, 8.41861E+00, -2.88198E-01, + 1.67854E+02, 0.00000E+00, 7.11493E-04, 6.05601E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 }, + /* TN2(3) */ { 1.00422E+00, -7.11212E-03, 5.24480E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -5.28914E-02, -2.41301E-02, 0.00000E+00, 0.00000E+00, -2.12219E+01, + -1.03830E-02, -3.28077E-03, 1.65727E-02, 1.68564E+00, -6.68154E-03, 0.00000E+00, 1.45155E-02, + 0.00000E+00, 8.42365E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, -4.34645E-03, 0.00000E+00, + 0.00000E+00, 2.16780E-02, 0.00000E+00, -1.38459E+02, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 7.04573E-03, -4.73204E+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, 1.08767E-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, -8.08279E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 5.21769E-04, -2.27387E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 3.26769E-03, 3.16901E-03, 4.60316E-04, + -1.01431E-04, 1.02131E-03, 9.96601E-04, 1.25707E+00, 2.50114E+01, 0.00000E+00, 4.24472E-01, + -2.77655E+01, 3.44625E-01, 2.75412E+01, 0.00000E+00, 7.94251E+02, 0.00000E+00, 2.45835E-03, + 1.38871E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 2.00000E+00 }, + /* TN2(4) TN3(1) */ { 1.01890E+00, -2.46603E-02, 1.00078E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, -6.70977E-02, -4.02286E-02, 0.00000E+00, + 0.00000E+00, -2.29466E+01, -7.47019E-03, 2.26580E-03, 2.63931E-02, 3.72625E+01, + -6.39041E-03, 0.00000E+00, 9.58383E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -1.85291E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 1.39717E+02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 9.19771E-03, -3.69121E+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, -1.57067E-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, -7.07265E-03, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -2.92953E-03, -2.77739E-03, + -4.40092E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 2.47280E-03, 2.95035E-04, -1.81246E-03, 2.81945E-03, + 4.27296E-03, 9.78863E-04, 1.40545E+00, -6.19173E+00, 0.00000E+00, 0.00000E+00, + -7.93632E+01, 4.44643E-01, -4.03085E+02, 0.00000E+00, 1.15603E+01, 0.00000E+00, + 2.25068E-03, 8.48557E-04, -2.98493E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 }, + /* TN3(2) */ { 9.75801E-01, 3.80680E-02, -3.05198E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 3.85575E-02, 5.04057E-02, 0.00000E+00, 0.00000E+00, -1.76046E+02, + 1.44594E-02, -1.48297E-03, -3.68560E-03, 3.02185E+01, -3.23338E-03, 0.00000E+00, 1.53569E-02, + 0.00000E+00, -1.15558E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 4.89620E-03, 0.00000E+00, + 0.00000E+00, -1.00616E-02, -8.21324E-03, -1.57757E+02, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 6.63564E-03, 4.58410E+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, -2.51280E-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, 9.91215E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, -8.73148E-04, -1.29648E-03, -7.32026E-05, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -4.68110E-03, -4.66003E-03, -1.31567E-03, + -7.39390E-04, 6.32499E-04, -4.65588E-04, -1.29785E+00, -1.57139E+02, 0.00000E+00, 2.58350E-01, + -3.69453E+01, 4.10672E-01, 9.78196E+00, -1.52064E-01, -3.85084E+03, 0.00000E+00, -8.52706E-04, + -1.40945E-03, -7.26786E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 2.00000E+00 }, + /* TN3(3) */ { 9.60722E-01, 7.03757E-02, -3.00266E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 2.22671E-02, 4.10423E-02, 0.00000E+00, 0.00000E+00, -1.63070E+02, + 1.06073E-02, 5.40747E-04, 7.79481E-03, 1.44908E+02, 1.51484E-04, 0.00000E+00, 1.97547E-02, + 0.00000E+00, -1.41844E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 5.77884E-03, 0.00000E+00, + 0.00000E+00, 9.74319E-03, 0.00000E+00, -2.88015E+03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -4.44902E-03, -2.92760E+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, 2.34419E-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, 5.36685E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, -4.65325E-04, -5.50628E-04, 3.31465E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -2.06179E-03, -3.08575E-03, -7.93589E-04, + -1.08629E-04, 5.95511E-04, -9.05050E-04, 1.18997E+00, 4.15924E+01, 0.00000E+00, -4.72064E-01, + -9.47150E+02, 3.98723E-01, 1.98304E+01, 0.00000E+00, 3.73219E+03, 0.00000E+00, -1.50040E-03, + -1.14933E-03, -1.56769E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 2.00000E+00 }, + /* TN3(4) */ { 1.03123E+00, -7.05124E-02, 8.71615E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -3.82621E-02, -9.80975E-03, 0.00000E+00, 0.00000E+00, 2.89286E+01, + 9.57341E-03, 0.00000E+00, 0.00000E+00, 8.66153E+01, 7.91938E-04, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 4.68917E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 7.86638E-03, 0.00000E+00, + 0.00000E+00, 9.90827E-03, 0.00000E+00, 6.55573E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, -4.00200E+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, 7.07457E-03, 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, 5.72268E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, -2.04970E-04, 1.21560E-03, -8.05579E-06, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -2.49941E-03, -4.57256E-04, -1.59311E-04, + 2.96481E-04, -1.77318E-03, -6.37918E-04, 1.02395E+00, 1.28172E+01, 0.00000E+00, 1.49903E-01, + -2.63818E+01, 0.00000E+00, 4.70628E+01, -2.22139E-01, 4.82292E-02, 0.00000E+00, -8.67075E-04, + -5.86479E-04, 5.32462E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 2.00000E+00 }, + /* TN3(5) SURFACE TEMP TSL */ { 1.00828E+00, -9.10404E-02, -2.26549E-02, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, -2.32420E-02, -9.08925E-03, 0.00000E+00, + 0.00000E+00, 3.36105E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, -1.24957E+01, + -5.87939E-03, 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, 2.79765E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 2.01237E+03, 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.75553E-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, 3.29699E-03, 1.26659E-03, + 2.68402E-04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 1.17894E-03, 1.48746E-03, 1.06478E-04, 1.34743E-04, + -2.20939E-03, -6.23523E-04, 6.36539E-01, 1.13621E+01, 0.00000E+00, -3.93777E-01, + 2.38687E+03, 0.00000E+00, 6.61865E+02, -1.21434E-01, 9.27608E+00, 0.00000E+00, + 1.68478E-04, 1.24892E-03, 1.71345E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 2.00000E+00 }, + /* TGN3(2) SURFACE GRAD TSLG */ { 1.57293E+00, -6.78400E-01, 6.47500E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, -7.62974E-02, -3.60423E-01, 0.00000E+00, + 0.00000E+00, 1.28358E+02, 0.00000E+00, 0.00000E+00, 0.00000E+00, 4.68038E+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, -1.67898E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 2.90994E+04, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 3.15706E+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, 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, 2.00000E+00 }, + /* TGN2(1) TGN1(2) */ { 8.60028E-01, 3.77052E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, -1.17570E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 7.77757E-03, 0.00000E+00, 0.00000E+00, 0.00000E+00, 1.01024E+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, 6.54251E+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, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -1.56959E-02, 1.91001E-02, 3.15971E-02, 1.00982E-02, + -6.71565E-03, 2.57693E-03, 1.38692E+00, 2.82132E-01, 0.00000E+00, 0.00000E+00, + 3.81511E+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, 2.00000E+00 }, + /* TGN3(1) TGN2(2) */ { 1.06029E+00, -5.25231E-02, 3.73034E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, 0.00000E+00, 3.31072E-02, -3.88409E-01, 0.00000E+00, + 0.00000E+00, -1.65295E+02, -2.13801E-01, -4.38916E-02, -3.22716E-01, -8.82393E+01, + 1.18458E-01, 0.00000E+00, -4.35863E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -1.19782E-01, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 2.62229E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 0.00000E+00, 0.00000E+00, -5.37443E+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, -4.55788E-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, 3.84009E-02, 3.96733E-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, 5.05494E-02, 7.39617E-02, 1.92200E-02, -8.46151E-03, + -1.34244E-02, 1.96338E-02, 1.50421E+00, 1.88368E+01, 0.00000E+00, 0.00000E+00, + -5.13114E+01, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, + 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, - 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, - 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, - 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, - 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, - 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, - 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, - 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, - 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, - 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+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, - 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 + 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, + 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, + 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, + 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, + 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+00, + 1.00000E+00, 1.00000E+00, 1.00000E+00, 1.00000E+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, 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 }; - - + /* 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 }; - +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/_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/TabularAtmosphere/tabularAtmosphere.cpp b/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.cpp index 4465cec046..1568992d95 100644 --- a/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.cpp +++ b/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.cpp @@ -41,45 +41,51 @@ TabularAtmosphere::~TabularAtmosphere() return; } -/*! Reset method checks that the data lists for altitude, density, and temperature have been defined with equal nonzero lengths. -* -*/ -void TabularAtmosphere::customReset(uint64_t CurrentClock) +/*! Reset method checks that the data lists for altitude, density, and temperature have been defined with equal nonzero + * lengths. + * + */ +void +TabularAtmosphere::customReset(uint64_t CurrentClock) { - this->altList_length = (int) this->altList.size(); - this->rhoList_length = (int) this->rhoList.size(); - this->tempList_length = (int) this->tempList.size(); - + this->altList_length = (int)this->altList.size(); + this->rhoList_length = (int)this->rhoList.size(); + this->tempList_length = (int)this->tempList.size(); - if((this->altList_length != this->rhoList_length) || (this->altList_length != this->tempList_length)){ + if ((this->altList_length != this->rhoList_length) || (this->altList_length != this->tempList_length)) { bskLogger.bskLog(BSK_ERROR, "Input arrays not of equal length."); } - if(this->altList_length == 0){ + if (this->altList_length == 0) { bskLogger.bskLog(BSK_ERROR, "No data in altitude list."); - } else if(this->rhoList_length == 0){ + } else if (this->rhoList_length == 0) { bskLogger.bskLog(BSK_ERROR, "No data in density list."); - } else if(this->tempList_length == 0){ + } else if (this->tempList_length == 0) { bskLogger.bskLog(BSK_ERROR, "No data in temperature list."); } return; } -/*! evaluate function interpolates from given data lists. Sets density and temp to 0 if altitude outside bounds of input lists OR if outside bounds of envMinReach and envMaxReach. -* -*/ -void TabularAtmosphere::evaluateAtmosphereModel(AtmoPropsMsgPayload *msg, double currentTime) +/*! evaluate function interpolates from given data lists. Sets density and temp to 0 if altitude outside bounds of input + * lists OR if outside bounds of envMinReach and envMaxReach. + * + */ +void +TabularAtmosphere::evaluateAtmosphereModel(AtmoPropsMsgPayload* msg, double currentTime) { if ((this->orbitAltitude < this->altList[0]) || (this->orbitAltitude > this->altList.back())) { msg->neutralDensity = 0.0; msg->localTemp = 0.0; - } - else { + } else { for (uint32_t i = 0; i <= this->altList.size() - 1; i++) { if (this->altList[i] > this->orbitAltitude) { - msg->neutralDensity = this->rhoList[i - 1] + (this->orbitAltitude - this->altList[i - 1]) * (this->rhoList[i] - this->rhoList[i - 1]) / (this->altList[i] - this->altList[i - 1]); - msg->localTemp = this->tempList[i - 1] + (this->orbitAltitude - this->altList[i - 1]) * (this->tempList[i] - this->tempList[i - 1]) / (this->altList[i] - this->altList[i - 1]); + msg->neutralDensity = this->rhoList[i - 1] + (this->orbitAltitude - this->altList[i - 1]) * + (this->rhoList[i] - this->rhoList[i - 1]) / + (this->altList[i] - this->altList[i - 1]); + msg->localTemp = this->tempList[i - 1] + (this->orbitAltitude - this->altList[i - 1]) * + (this->tempList[i] - this->tempList[i - 1]) / + (this->altList[i] - this->altList[i - 1]); break; } } diff --git a/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.h b/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.h index 1107211ac6..d3ab941154 100644 --- a/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.h +++ b/src/simulation/environment/TabularAtmosphere/tabularAtmosphere.h @@ -17,7 +17,6 @@ */ - #ifndef TABULAR_ATMOSPHERE_H #define TABULAR_ATMOSPHERE_H @@ -30,27 +29,27 @@ #include "architecture/utilities/bskLogging.h" /*! @brief tabular atmosphere model */ -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 - - virtual void customReset(uint64_t CurrentClock); // reset if error thrown - - public: - TabularAtmosphere(); - ~TabularAtmosphere(); - std::vector altList; //!< vector of doubles of altitude values extracted from the atmosphere table - std::vector rhoList; //!< vector of doubles of density values extracted from the atmosphere table - std::vector tempList; //!< vector of doubles of temperature values extracted from the atmosphere table - BSKLogger bskLogger; //!< -- BSK Logging +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 + + virtual void customReset(uint64_t CurrentClock); // reset if error thrown + + public: + TabularAtmosphere(); + ~TabularAtmosphere(); + std::vector altList; //!< vector of doubles of altitude values extracted from the atmosphere table + std::vector rhoList; //!< vector of doubles of density values extracted from the atmosphere table + std::vector tempList; //!< vector of doubles of temperature values extracted from the atmosphere table + BSKLogger bskLogger; //!< -- BSK Logging }; - #endif /* TABULAR_ATMOSPHERE_H */ 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.cpp b/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.cpp index d229e82f05..87fa9ccddd 100644 --- a/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.cpp +++ b/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.cpp @@ -29,7 +29,8 @@ AtmosphereBase::AtmosphereBase() { //! - zero class variables - this->planetRadius = 0.0; // [m] Earth magnetic spherical reference radius (see p. 404 in doi:10.1007/978-1-4939-0802-8) + this->planetRadius = + 0.0; // [m] Earth magnetic spherical reference radius (see p. 404 in doi:10.1007/978-1-4939-0802-8) this->r_BP_N.fill(0.0); this->r_BP_P.fill(0.0); this->scStateInMsgs.clear(); @@ -41,7 +42,7 @@ AtmosphereBase::AtmosphereBase() this->epochDateTime.tm_mday = EPOCH_DAY; this->epochDateTime.tm_hour = EPOCH_HOUR; this->epochDateTime.tm_min = EPOCH_MIN; - this->epochDateTime.tm_sec = (int) round(EPOCH_SEC); + this->epochDateTime.tm_sec = (int)round(EPOCH_SEC); this->epochDateTime.tm_isdst = -1; //! - turn off minimum and maximum reach features @@ -64,7 +65,7 @@ AtmosphereBase::AtmosphereBase() */ AtmosphereBase::~AtmosphereBase() { - for (long unsigned int c=0; cenvOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->envOutMsgs.size(); c++) { delete this->envOutMsgs.at(c); } return; @@ -74,13 +75,15 @@ AtmosphereBase::~AtmosphereBase() @param tmpScMsg A spacecraft state message name. */ -void AtmosphereBase::addSpacecraftToModel(Message *tmpScMsg){ +void +AtmosphereBase::addSpacecraftToModel(Message* tmpScMsg) +{ /* add input message */ this->scStateInMsgs.push_back(tmpScMsg->addSubscriber()); /* create output message */ - Message *msg; + Message* msg; msg = new Message; this->envOutMsgs.push_back(msg); @@ -88,16 +91,14 @@ void AtmosphereBase::addSpacecraftToModel(Message *tmpScMsg) AtmoPropsMsgPayload msgAtmoBuffer; this->envOutBuffer.push_back(msgAtmoBuffer); - return; } - - /*! This method is used to reset the module. */ -void AtmosphereBase::Reset(uint64_t CurrentSimNanos) +void +AtmosphereBase::Reset(uint64_t CurrentSimNanos) { //! - call the custom environment module reset method customReset(CurrentSimNanos); @@ -115,7 +116,7 @@ void AtmosphereBase::Reset(uint64_t CurrentSimNanos) this->epochDateTime.tm_mday = epochMsg.day; this->epochDateTime.tm_hour = epochMsg.hours; this->epochDateTime.tm_min = epochMsg.minutes; - this->epochDateTime.tm_sec = (int) round(epochMsg.seconds); + this->epochDateTime.tm_sec = (int)round(epochMsg.seconds); mktime(&this->epochDateTime); } else { customSetEpochFromVariable(); @@ -128,16 +129,17 @@ void AtmosphereBase::Reset(uint64_t CurrentSimNanos) is set by a module variable. */ -void AtmosphereBase::customSetEpochFromVariable() +void +AtmosphereBase::customSetEpochFromVariable() { return; } - /*! Custom Reset() method. This allows a child class to add additional functionality to the Reset() method */ -void AtmosphereBase::customReset(uint64_t CurrentClock) +void +AtmosphereBase::customReset(uint64_t CurrentClock) { return; } @@ -146,10 +148,11 @@ void AtmosphereBase::customReset(uint64_t CurrentClock) @param CurrentClock The current time used for time-stamping the message */ -void AtmosphereBase::writeMessages(uint64_t CurrentClock) +void +AtmosphereBase::writeMessages(uint64_t CurrentClock) { //! - write density output messages for each spacecaft's locations - for(long unsigned int c = 0; c < this->envOutMsgs.size(); c++){ + for (long unsigned int c = 0; c < this->envOutMsgs.size(); c++) { this->envOutMsgs.at(c)->write(&this->envOutBuffer.at(c), this->moduleID, CurrentClock); } @@ -162,7 +165,8 @@ void AtmosphereBase::writeMessages(uint64_t CurrentClock) /*! Custom output message writing method. This allows a child class to add additional functionality. */ -void AtmosphereBase::customWriteMessages(uint64_t CurrentClock) +void +AtmosphereBase::customWriteMessages(uint64_t CurrentClock) { return; } @@ -171,7 +175,8 @@ void AtmosphereBase::customWriteMessages(uint64_t CurrentClock) associated spacecraft positions for computing the atmosphere. */ -bool AtmosphereBase::readMessages() +bool +AtmosphereBase::readMessages() { SCStatesMsgPayload scMsg; @@ -179,10 +184,9 @@ bool AtmosphereBase::readMessages() //! - read in the spacecraft state messages bool scRead; - if(this->scStateInMsgs.size() > 0) - { + if (this->scStateInMsgs.size() > 0) { scRead = true; - for(long unsigned int c = 0; cscStateInMsgs.size(); c++){ + for (long unsigned int c = 0; c < this->scStateInMsgs.size(); c++) { bool tmpScRead; scMsg = this->scStateInMsgs.at(c)(); tmpScRead = this->scStateInMsgs.at(c).isWritten(); @@ -195,10 +199,10 @@ bool AtmosphereBase::readMessages() scRead = false; } - //! - Read in the optional planet message. if no planet message is set, then a zero planet position, velocity and orientation is assumed + //! - Read in the optional planet message. if no planet message is set, then a zero planet position, velocity and + //! orientation is assumed bool planetRead = true; - if(this->planetPosInMsg.isLinked()) - { + if (this->planetPosInMsg.isLinked()) { this->planetState = this->planetPosInMsg(); planetRead = this->planetPosInMsg.isWritten(); } @@ -206,14 +210,14 @@ bool AtmosphereBase::readMessages() //! - call the custom method to perform additional input reading bool customRead = customReadMessages(); - return(planetRead && scRead && customRead); + return (planetRead && scRead && customRead); } - /*! Custom output input reading method. This allows a child class to add additional functionality. */ -bool AtmosphereBase::customReadMessages() +bool +AtmosphereBase::customReadMessages() { return true; } @@ -223,7 +227,8 @@ bool AtmosphereBase::customReadMessages() @param scState A spacecraft states message struct. */ -void AtmosphereBase::updateRelativePos(SpicePlanetStateMsgPayload *planetState, SCStatesMsgPayload *scState) +void +AtmosphereBase::updateRelativePos(SpicePlanetStateMsgPayload* planetState, SCStatesMsgPayload* scState) { //! - compute spacecraft position vector relative to planet v3Subtract(scState->r_BN_N, planetState->PositionVector, this->r_BP_N.data()); @@ -241,14 +246,15 @@ void AtmosphereBase::updateRelativePos(SpicePlanetStateMsgPayload *planetState, /*! This method is used to update the local magnetic field based on each spacecraft's position. */ -void AtmosphereBase::updateLocalAtmosphere(double currentTime) +void +AtmosphereBase::updateLocalAtmosphere(double currentTime) { std::vector::iterator scIt; //! - loop over all the spacecraft std::vector::iterator envMsgIt; envMsgIt = this->envOutBuffer.begin(); - for(scIt = scStates.begin(); scIt != scStates.end(); scIt++, envMsgIt++){ + for (scIt = scStates.begin(); scIt != scStates.end(); scIt++, envMsgIt++) { //! - Computes planet relative state vector this->updateRelativePos(&(this->planetState), &(*scIt)); @@ -256,9 +262,10 @@ void AtmosphereBase::updateLocalAtmosphere(double currentTime) *envMsgIt = this->envOutMsgs[0]->zeroMsgPayload; //! - check if radius is in permissible range - if(this->orbitAltitude > this->envMinReach && - (this->orbitAltitude < this->envMaxReach || this->envMaxReach < 0)) { - //! - compute the local atmosphere data. The evaluateMageticFieldModel() method must be implement for each model + if (this->orbitAltitude > this->envMinReach && + (this->orbitAltitude < this->envMaxReach || this->envMaxReach < 0)) { + //! - compute the local atmosphere data. The evaluateMageticFieldModel() method must be implement for each + //! model evaluateAtmosphereModel(&(*envMsgIt), currentTime); } } @@ -266,22 +273,21 @@ void AtmosphereBase::updateLocalAtmosphere(double currentTime) return; } - /*! Computes the current local magnetic field for each spacecraft and writes their respective messages. @param CurrentSimNanos The current simulation time in nanoseconds */ -void AtmosphereBase::UpdateState(uint64_t CurrentSimNanos) +void +AtmosphereBase::UpdateState(uint64_t CurrentSimNanos) { //! - clear the output buffer std::vector::iterator it; - for(it = this->envOutBuffer.begin(); it!= this->envOutBuffer.end(); it++){ + for (it = this->envOutBuffer.begin(); it != this->envOutBuffer.end(); it++) { *it = this->envOutMsgs[0]->zeroMsgPayload; } //! - update local neutral density information - if(this->readMessages()) - { - this->updateLocalAtmosphere(CurrentSimNanos*NANO2SEC); + if (this->readMessages()) { + this->updateLocalAtmosphere(CurrentSimNanos * NANO2SEC); } //! - write out neutral density message diff --git a/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.h b/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.h index c585abed68..faa4f2a8bc 100644 --- a/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.h +++ b/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.h @@ -34,45 +34,49 @@ #include "architecture/utilities/bskLogging.h" /*! @brief atmospheric density base class */ -class AtmosphereBase: public SysModel { -public: +class AtmosphereBase : public SysModel +{ + public: AtmosphereBase(); ~AtmosphereBase(); void Reset(uint64_t CurrentSimNanos); - void addSpacecraftToModel(Message *tmpScMsg); + void addSpacecraftToModel(Message* tmpScMsg); void UpdateState(uint64_t CurrentSimNanos); -protected: + protected: void writeMessages(uint64_t CurrentClock); bool readMessages(); void updateLocalAtmosphere(double currentTime); - void updateRelativePos(SpicePlanetStateMsgPayload *planetState, SCStatesMsgPayload *scState); - virtual void evaluateAtmosphereModel(AtmoPropsMsgPayload *msg, double currentTime) = 0; //!< class method + void updateRelativePos(SpicePlanetStateMsgPayload* planetState, SCStatesMsgPayload* scState); + virtual void evaluateAtmosphereModel(AtmoPropsMsgPayload* msg, double currentTime) = 0; //!< class method virtual void customReset(uint64_t CurrentClock); virtual void customWriteMessages(uint64_t CurrentClock); virtual bool customReadMessages(); virtual void customSetEpochFromVariable(); -public: - std::vector> scStateInMsgs; //!< Vector of the spacecraft position/velocity input message - std::vector*> envOutMsgs; //!< Vector of message names to be written out by the environment - ReadFunctor planetPosInMsg; //!< Message name for the planet's SPICE position message - ReadFunctor epochInMsg; //!< (optional) epoch date/time input message - double envMinReach; //!< [m] Minimum planet-relative position needed for the environment to work, default is off (neg. value) - double envMaxReach; //!< [m] Maximum distance at which the environment will be calculated, default is off (neg. value) - double planetRadius; //!< [m] Radius of the planet - BSKLogger bskLogger; //!< -- BSK Logging + public: + std::vector> + scStateInMsgs; //!< Vector of the spacecraft position/velocity input message + std::vector*> + envOutMsgs; //!< Vector of message names to be written out by the environment + ReadFunctor planetPosInMsg; //!< Message name for the planet's SPICE position message + ReadFunctor epochInMsg; //!< (optional) epoch date/time input message + double envMinReach; //!< [m] Minimum planet-relative position needed for the environment to work, default is off + //!< (neg. value) + double + envMaxReach; //!< [m] Maximum distance at which the environment will be calculated, default is off (neg. value) + double planetRadius; //!< [m] Radius of the planet + BSKLogger bskLogger; //!< -- BSK Logging -protected: - Eigen::Vector3d r_BP_N; //!< [m] sc position vector relative to planet in inertial N frame components - Eigen::Vector3d r_BP_P; //!< [m] sc position vector relative to planet in planet-fixed frame components - double orbitRadius; //!< [m] sc orbit radius about planet - double orbitAltitude; //!< [m] sc altitude above planetRadius + protected: + Eigen::Vector3d r_BP_N; //!< [m] sc position vector relative to planet in inertial N frame components + Eigen::Vector3d r_BP_P; //!< [m] sc position vector relative to planet in planet-fixed frame components + double orbitRadius; //!< [m] sc orbit radius about planet + double orbitAltitude; //!< [m] sc altitude above planetRadius std::vector envOutBuffer; //!< -- Message buffer for magnetic field messages - std::vector scStates; //!< vector of the spacecraft state messages - SpicePlanetStateMsgPayload planetState; //!< planet state message - struct tm epochDateTime; //!< time/date structure containing the epoch information using a Gregorian calendar + std::vector scStates; //!< vector of the spacecraft state messages + SpicePlanetStateMsgPayload planetState; //!< planet state message + struct tm epochDateTime; //!< time/date structure containing the epoch information using a Gregorian calendar }; - #endif /* Atmosphere_H */ 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/_GeneralModuleFiles/magneticFieldBase.cpp b/src/simulation/environment/_GeneralModuleFiles/magneticFieldBase.cpp index e5bbb80a36..d22e428e39 100644 --- a/src/simulation/environment/_GeneralModuleFiles/magneticFieldBase.cpp +++ b/src/simulation/environment/_GeneralModuleFiles/magneticFieldBase.cpp @@ -43,10 +43,9 @@ MagneticFieldBase::MagneticFieldBase() this->epochDateTime.tm_mday = EPOCH_DAY; this->epochDateTime.tm_hour = EPOCH_HOUR; this->epochDateTime.tm_min = EPOCH_MIN; - this->epochDateTime.tm_sec = (int) round(EPOCH_SEC); + this->epochDateTime.tm_sec = (int)round(EPOCH_SEC); this->epochDateTime.tm_isdst = -1; - //! - zero the planet message, and set the DCM to an identity matrix this->planetState = planetPosInMsg.zeroMsgPayload; m33SetIdentity(this->planetState.J20002Pfix); @@ -59,7 +58,7 @@ MagneticFieldBase::MagneticFieldBase() */ MagneticFieldBase::~MagneticFieldBase() { - for (long unsigned int c=0; cenvOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->envOutMsgs.size(); c++) { delete this->envOutMsgs.at(c); } return; @@ -69,13 +68,15 @@ MagneticFieldBase::~MagneticFieldBase() @param tmpScMsg A spacecraft state message name. */ -void MagneticFieldBase::addSpacecraftToModel(Message *tmpScMsg){ +void +MagneticFieldBase::addSpacecraftToModel(Message* tmpScMsg) +{ /* add input message */ this->scStateInMsgs.push_back(tmpScMsg->addSubscriber()); /* create output message */ - Message *msg; + Message* msg; msg = new Message; this->envOutMsgs.push_back(msg); @@ -86,11 +87,11 @@ void MagneticFieldBase::addSpacecraftToModel(Message *tmpScM return; } - /*! This method is used to reset the module. */ -void MagneticFieldBase::Reset(uint64_t CurrentSimNanos) +void +MagneticFieldBase::Reset(uint64_t CurrentSimNanos) { //! - call the custom environment module reset method customReset(CurrentSimNanos); @@ -105,7 +106,7 @@ void MagneticFieldBase::Reset(uint64_t CurrentSimNanos) this->epochDateTime.tm_mday = epochMsg.day; this->epochDateTime.tm_hour = epochMsg.hours; this->epochDateTime.tm_min = epochMsg.minutes; - this->epochDateTime.tm_sec = (int) round(epochMsg.seconds); + this->epochDateTime.tm_sec = (int)round(epochMsg.seconds); mktime(&this->epochDateTime); } else { customSetEpochFromVariable(); @@ -113,11 +114,11 @@ void MagneticFieldBase::Reset(uint64_t CurrentSimNanos) return; } - /*! Custom Reset() method. This allows a child class to add additional functionality to the Reset() method */ -void MagneticFieldBase::customReset(uint64_t CurrentClock) +void +MagneticFieldBase::customReset(uint64_t CurrentClock) { return; } @@ -126,7 +127,8 @@ void MagneticFieldBase::customReset(uint64_t CurrentClock) is set by a module variable. */ -void MagneticFieldBase::customSetEpochFromVariable() +void +MagneticFieldBase::customSetEpochFromVariable() { return; } @@ -135,9 +137,10 @@ void MagneticFieldBase::customSetEpochFromVariable() @param CurrentClock The current time used for time-stamping the message */ -void MagneticFieldBase::writeMessages(uint64_t CurrentClock) +void +MagneticFieldBase::writeMessages(uint64_t CurrentClock) { - for (long unsigned int c=0; cenvOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->envOutMsgs.size(); c++) { this->envOutMsgs.at(c)->write(&this->magFieldOutBuffer.at(c), this->moduleID, CurrentClock); } @@ -150,7 +153,8 @@ void MagneticFieldBase::writeMessages(uint64_t CurrentClock) /*! Custom output message writing method. This allows a child class to add additional functionality. */ -void MagneticFieldBase::customWriteMessages(uint64_t CurrentClock) +void +MagneticFieldBase::customWriteMessages(uint64_t CurrentClock) { return; } @@ -159,7 +163,8 @@ void MagneticFieldBase::customWriteMessages(uint64_t CurrentClock) associated spacecraft positions for computing the atmosphere. */ -bool MagneticFieldBase::readMessages() +bool +MagneticFieldBase::readMessages() { SCStatesMsgPayload scMsg; @@ -167,10 +172,9 @@ bool MagneticFieldBase::readMessages() //! - read in the spacecraft state messages bool scRead; - if(this->scStateInMsgs.size() > 0) - { + if (this->scStateInMsgs.size() > 0) { scRead = true; - for (long unsigned int c=0; cscStateInMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->scStateInMsgs.size(); c++) { bool tmpScRead; scMsg = this->scStateInMsgs.at(c)(); tmpScRead = this->scStateInMsgs.at(c).isWritten(); @@ -183,10 +187,10 @@ bool MagneticFieldBase::readMessages() scRead = false; } - //! - Read in the optional planet message. if no planet message is set, then a zero planet position, velocity and orientation is assumed + //! - Read in the optional planet message. if no planet message is set, then a zero planet position, velocity and + //! orientation is assumed bool planetRead = true; - if(this->planetPosInMsg.isLinked()) - { + if (this->planetPosInMsg.isLinked()) { this->planetState = this->planetPosInMsg(); planetRead = this->planetPosInMsg.isWritten(); } @@ -194,14 +198,14 @@ bool MagneticFieldBase::readMessages() //! - call the custom method to perform additional input reading bool customRead = customReadMessages(); - return(planetRead && scRead && customRead); + return (planetRead && scRead && customRead); } - /*! Custom output input reading method. This allows a child class to add additional functionality. */ -bool MagneticFieldBase::customReadMessages() +bool +MagneticFieldBase::customReadMessages() { return true; } @@ -209,7 +213,8 @@ bool MagneticFieldBase::customReadMessages() /*! This method is used to update the local magnetic field based on each spacecraft's position. */ -void MagneticFieldBase::updateLocalMagField(double currentTime) +void +MagneticFieldBase::updateLocalMagField(double currentTime) { std::vector::iterator it; uint64_t atmoInd = 0; @@ -217,7 +222,7 @@ void MagneticFieldBase::updateLocalMagField(double currentTime) //! - loop over all the spacecraft std::vector::iterator magMsgIt; magMsgIt = this->magFieldOutBuffer.begin(); - for(it = scStates.begin(); it != scStates.end(); it++, atmoInd++, magMsgIt++){ + for (it = scStates.begin(); it != scStates.end(); it++, atmoInd++, magMsgIt++) { //! - Computes planet relative state vector this->updateRelativePos(&(this->planetState), &(*it)); @@ -225,9 +230,9 @@ void MagneticFieldBase::updateLocalMagField(double currentTime) *magMsgIt = this->envOutMsgs[0]->zeroMsgPayload; //! - check if radius is in permissible range - if(this->orbitRadius > this->envMinReach && - (this->orbitRadius < this->envMaxReach || this->envMaxReach < 0)) { - //! - compute the local magnetic field. The evaluateMageticFieldModel() method must be implement for each model + if (this->orbitRadius > this->envMinReach && (this->orbitRadius < this->envMaxReach || this->envMaxReach < 0)) { + //! - compute the local magnetic field. The evaluateMageticFieldModel() method must be implement for each + //! model evaluateMagneticFieldModel(&(*magMsgIt), currentTime); } } @@ -240,7 +245,8 @@ void MagneticFieldBase::updateLocalMagField(double currentTime) @param scState A spacecraft states message struct. */ -void MagneticFieldBase::updateRelativePos(SpicePlanetStateMsgPayload *planetState, SCStatesMsgPayload *scState) +void +MagneticFieldBase::updateRelativePos(SpicePlanetStateMsgPayload* planetState, SCStatesMsgPayload* scState) { //! - compute spacecraft position vector relative to planet v3Subtract(scState->r_BN_N, planetState->PositionVector, this->r_BP_N.data()); @@ -258,18 +264,18 @@ void MagneticFieldBase::updateRelativePos(SpicePlanetStateMsgPayload *planetStat @param CurrentSimNanos The current simulation time in nanoseconds */ -void MagneticFieldBase::UpdateState(uint64_t CurrentSimNanos) +void +MagneticFieldBase::UpdateState(uint64_t CurrentSimNanos) { //! - clear the output buffer std::vector::iterator it; - for(it = this->magFieldOutBuffer.begin(); it!= this->magFieldOutBuffer.end(); it++){ + for (it = this->magFieldOutBuffer.begin(); it != this->magFieldOutBuffer.end(); it++) { memset(&(*it), 0x0, sizeof(MagneticFieldMsgPayload)); *it = this->envOutMsgs[0]->zeroMsgPayload; } //! - update local neutral density information - if(this->readMessages()) - { - updateLocalMagField(CurrentSimNanos*NANO2SEC); + if (this->readMessages()) { + updateLocalMagField(CurrentSimNanos * NANO2SEC); } //! - write out neutral density message diff --git a/src/simulation/environment/_GeneralModuleFiles/magneticFieldBase.h b/src/simulation/environment/_GeneralModuleFiles/magneticFieldBase.h index faddc1c727..b9d1f7d834 100644 --- a/src/simulation/environment/_GeneralModuleFiles/magneticFieldBase.h +++ b/src/simulation/environment/_GeneralModuleFiles/magneticFieldBase.h @@ -17,7 +17,6 @@ */ - #ifndef MAGNETIC_FIELD_BASE_H #define MAGNETIC_FIELD_BASE_H @@ -36,46 +35,50 @@ #include "architecture/utilities/bskLogging.h" /*! @brief magnetic field base class */ -class MagneticFieldBase: public SysModel { -public: +class MagneticFieldBase : public SysModel +{ + public: MagneticFieldBase(); ~MagneticFieldBase(); void Reset(uint64_t CurrentSimNanos); - void addSpacecraftToModel(Message *tmpScMsg); + void addSpacecraftToModel(Message* tmpScMsg); void UpdateState(uint64_t CurrentSimNanos); -protected: + protected: void writeMessages(uint64_t CurrentClock); bool readMessages(); void updateLocalMagField(double currentTime); - void updateRelativePos(SpicePlanetStateMsgPayload *planetState, SCStatesMsgPayload *scState); - virtual void evaluateMagneticFieldModel(MagneticFieldMsgPayload *msg, double currentTime) = 0; //!< class method + void updateRelativePos(SpicePlanetStateMsgPayload* planetState, SCStatesMsgPayload* scState); + virtual void evaluateMagneticFieldModel(MagneticFieldMsgPayload* msg, double currentTime) = 0; //!< class method virtual void customReset(uint64_t CurrentClock); virtual void customWriteMessages(uint64_t CurrentClock); virtual bool customReadMessages(); virtual void customSetEpochFromVariable(); -public: - std::vector> scStateInMsgs; //!< Vector of the spacecraft position/velocity input message - std::vector*> envOutMsgs; //!< Vector of message names to be written out by the environment - ReadFunctor planetPosInMsg; //!< Message name for the planet's SPICE position message - ReadFunctor epochInMsg; //!< (optional) epoch date/time input message - - double envMinReach; //!< [m] Minimum planet-relative position needed for the environment to work, default is off (neg. value) - double envMaxReach; //!< [m] Maximum distance at which the environment will be calculated, default is off (neg. value) - double planetRadius; //!< [m] Radius of the planet - BSKLogger bskLogger; //!< -- BSK Logging - -protected: - Eigen::Vector3d r_BP_N; //!< [m] sc position vector relative to planet in inertial N frame components - Eigen::Vector3d r_BP_P; //!< [m] sc position vector relative to planet in planet-fixed frame components - double orbitRadius; //!< [m] sc orbit radius about planet + public: + std::vector> + scStateInMsgs; //!< Vector of the spacecraft position/velocity input message + std::vector*> + envOutMsgs; //!< Vector of message names to be written out by the environment + ReadFunctor planetPosInMsg; //!< Message name for the planet's SPICE position message + ReadFunctor epochInMsg; //!< (optional) epoch date/time input message + + double envMinReach; //!< [m] Minimum planet-relative position needed for the environment to work, default is off + //!< (neg. value) + double + envMaxReach; //!< [m] Maximum distance at which the environment will be calculated, default is off (neg. value) + double planetRadius; //!< [m] Radius of the planet + BSKLogger bskLogger; //!< -- BSK Logging + + protected: + Eigen::Vector3d r_BP_N; //!< [m] sc position vector relative to planet in inertial N frame components + Eigen::Vector3d r_BP_P; //!< [m] sc position vector relative to planet in planet-fixed frame components + double orbitRadius; //!< [m] sc orbit radius about planet std::vector magFieldOutBuffer; //!< -- Message buffer for magnetic field messages - std::vector scStates;//!< vector of the spacecraft state messages - SpicePlanetStateMsgPayload planetState; //!< planet state message - struct tm epochDateTime; //!< time/date structure containing the epoch information using a Gregorian calendar + std::vector scStates; //!< vector of the spacecraft state messages + SpicePlanetStateMsgPayload planetState; //!< planet state message + struct tm epochDateTime; //!< time/date structure containing the epoch information using a Gregorian calendar }; - #endif /* MAGNETIC_FIELD_BASE_H */ 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/albedo/albedo.cpp b/src/simulation/environment/albedo/albedo.cpp index 83f8351982..103e7ce29f 100644 --- a/src/simulation/environment/albedo/albedo.cpp +++ b/src/simulation/environment/albedo/albedo.cpp @@ -65,38 +65,44 @@ Albedo::Albedo() */ Albedo::~Albedo() { - for (long unsigned int c=0; calbOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->albOutMsgs.size(); c++) { delete this->albOutMsgs.at(c); } return; } -Config::Config() { +Config::Config() +{ this->fov = -1.0; this->nHat_B.fill(0.0); this->r_IB_B.fill(0.0); return; } -Config::~Config() { +Config::~Config() +{ return; } /*! Adds the instrument configuration and automatically creates an output message name (overloaded function) */ -void Albedo::addInstrumentConfig(instConfig_t configMsg) { +void +Albedo::addInstrumentConfig(instConfig_t configMsg) +{ // add a albedo output message for this instrument - Message *msg; + Message* msg; msg = new Message; this->albOutMsgs.push_back(msg); // Do a sanity check and push fov back to the vector (if not defined, use the default value.) if (configMsg.fov < 0.0) { this->fovs.push_back(this->fov_default); - bskLogger.bskLog(BSK_WARNING, "Albedo Module (addInstrumentConfig): For the instrument (%lu)'s half field of view angle (fov), the default value is used.", (int) this->albOutMsgs.size()-1); - } - else { + bskLogger.bskLog(BSK_WARNING, + "Albedo Module (addInstrumentConfig): For the instrument (%lu)'s half field of view angle " + "(fov), the default value is used.", + (int)this->albOutMsgs.size() - 1); + } else { this->fovs.push_back(configMsg.fov); } @@ -106,10 +112,12 @@ void Albedo::addInstrumentConfig(instConfig_t configMsg) { // Do a sanity check and push nHat_B back to the vector (if not defined, use the default value.) if (!configMsg.nHat_B.isZero()) { this->nHat_Bs.push_back(configMsg.nHat_B / configMsg.nHat_B.norm()); - } - else { + } else { this->nHat_Bs.push_back(nHat_B_default); - bskLogger.bskLog(BSK_WARNING, "Albedo Module (addInstrumentConfig): For the instrument (%lu)'s unit normal vector (nHat_B), the default vector is used.", this->albOutMsgs.size()-1); + bskLogger.bskLog(BSK_WARNING, + "Albedo Module (addInstrumentConfig): For the instrument (%lu)'s unit normal vector (nHat_B), " + "the default vector is used.", + this->albOutMsgs.size() - 1); } return; } @@ -117,18 +125,22 @@ void Albedo::addInstrumentConfig(instConfig_t configMsg) { /*! Adds the instrument configuration and automatically creates an output message name (overloaded function) */ -void Albedo::addInstrumentConfig(double fov, Eigen::Vector3d nHat_B, Eigen::Vector3d r_IB_B) { +void +Albedo::addInstrumentConfig(double fov, Eigen::Vector3d nHat_B, Eigen::Vector3d r_IB_B) +{ // add a albedo output message for this instrument - Message *msg; + Message* msg; msg = new Message; this->albOutMsgs.push_back(msg); // Do a sanity check and push fov back to the vector (if not defined, use the default value.) if (fov < 0.0) { this->fovs.push_back(this->fov_default); - bskLogger.bskLog(BSK_WARNING, "Albedo Module (addInstrumentConfig): Instrument (%lu)'s half field of view angle (fov) cannot be negative, the default value is used instead.", this->albOutMsgs.size()-1); - } - else { + bskLogger.bskLog(BSK_WARNING, + "Albedo Module (addInstrumentConfig): Instrument (%lu)'s half field of view angle (fov) " + "cannot be negative, the default value is used instead.", + this->albOutMsgs.size() - 1); + } else { this->fovs.push_back(fov); } @@ -138,10 +150,12 @@ void Albedo::addInstrumentConfig(double fov, Eigen::Vector3d nHat_B, Eigen::Vect // Do a sanity check and push nHat_B back to the vector (if not defined, use the default value.) if (!nHat_B.isZero()) { this->nHat_Bs.push_back(nHat_B / nHat_B.norm()); - } - else { + } else { this->nHat_Bs.push_back(nHat_B_default); - bskLogger.bskLog(BSK_WARNING, "Albedo Module (addInstrumentConfig): Instrument (%lu)'s unit normal vector (nHat_B) cannot be composed of all zeros, the default vector is used instead.", this->albOutMsgs.size()-1); + bskLogger.bskLog(BSK_WARNING, + "Albedo Module (addInstrumentConfig): Instrument (%lu)'s unit normal vector (nHat_B) cannot " + "be composed of all zeros, the default vector is used instead.", + this->albOutMsgs.size() - 1); } return; @@ -150,7 +164,8 @@ void Albedo::addInstrumentConfig(double fov, Eigen::Vector3d nHat_B, Eigen::Vect /*! This method subscribes to the planet msg and sets the albedo average model (overloaded function) */ -void Albedo::addPlanetandAlbedoAverageModel(Message *planetSpiceMsg) +void +Albedo::addPlanetandAlbedoAverageModel(Message* planetSpiceMsg) { std::string modelName = "ALBEDO_AVG"; this->modelNames.push_back(modelName); @@ -158,7 +173,7 @@ void Albedo::addPlanetandAlbedoAverageModel(Message this->dataPaths.push_back(""); this->numLats.push_back(-1); this->numLons.push_back(-1); - double ALB_avg = -1; // value will be set in Reset() when we can determine the planet name + double ALB_avg = -1; // value will be set in Reset() when we can determine the planet name this->ALB_avgs.push_back(ALB_avg); this->albArray.push_back(false); @@ -172,7 +187,11 @@ void Albedo::addPlanetandAlbedoAverageModel(Message /*! This method subscribes to the planet msg and sets the albedo average model (overloaded function) */ -void Albedo::addPlanetandAlbedoAverageModel(Message *planetSpiceMsg, double ALB_avg, int numLat, int numLon) +void +Albedo::addPlanetandAlbedoAverageModel(Message* planetSpiceMsg, + double ALB_avg, + int numLat, + int numLon) { std::string modelName = "ALBEDO_AVG"; this->modelNames.push_back(modelName); @@ -192,7 +211,10 @@ void Albedo::addPlanetandAlbedoAverageModel(Message /*! This method subscribes to the planet msg and sets the albedo data model */ -void Albedo::addPlanetandAlbedoDataModel(Message *planetSpiceMsg, std::string dataPath, std::string fileName) +void +Albedo::addPlanetandAlbedoDataModel(Message* planetSpiceMsg, + std::string dataPath, + std::string fileName) { std::string modelName = "ALBEDO_DATA"; @@ -211,27 +233,26 @@ void Albedo::addPlanetandAlbedoDataModel(Message *pl this->planetMsgData.push_back(plMsg); } - - /*! Read Messages, calculate albedo then write it out */ -void Albedo::UpdateState(uint64_t CurrentSimNanos) +void +Albedo::UpdateState(uint64_t CurrentSimNanos) { this->readMessages(); this->albOutData.clear(); std::vector::iterator planetIt; int idx; - for (int instIdx = 0; instIdx < (int) this->albOutMsgs.size(); instIdx++) - { + for (int instIdx = 0; instIdx < (int)this->albOutMsgs.size(); instIdx++) { idx = 0; double tmpTot[4] = {}; double outData[4] = {}; - for (planetIt = this->planetMsgData.begin(); planetIt != this->planetMsgData.end(); planetIt++) - { + for (planetIt = this->planetMsgData.begin(); planetIt != this->planetMsgData.end(); planetIt++) { this->computeAlbedo(idx, instIdx, *planetIt, albArray.at(idx), outData); - tmpTot[0] += outData[0]; tmpTot[1] += outData[1]; - tmpTot[2] += outData[2]; tmpTot[3] += outData[3]; + tmpTot[0] += outData[0]; + tmpTot[1] += outData[1]; + tmpTot[2] += outData[2]; + tmpTot[3] += outData[3]; idx++; } this->albOutData.push_back(cArray2EigenMatrixXd(tmpTot, 4, 1)); @@ -242,7 +263,8 @@ void Albedo::UpdateState(uint64_t CurrentSimNanos) /*! This method resets the module */ -void Albedo::Reset(uint64_t CurrentSimNanos) +void +Albedo::Reset(uint64_t CurrentSimNanos) { if (this->modelNames.empty()) { bskLogger.bskLog(BSK_ERROR, "Albedo Module (Reset): Albedo model was not set."); @@ -260,16 +282,18 @@ void Albedo::Reset(uint64_t CurrentSimNanos) int idx = 0; this->ALB.clear(); - this->gdlat.clear(); this->gdlon.clear(); - this->latDiff.clear(); this->lonDiff.clear(); - this->REQ_planets.clear(); this->RP_planets.clear(); + this->gdlat.clear(); + this->gdlon.clear(); + this->latDiff.clear(); + this->lonDiff.clear(); + this->REQ_planets.clear(); + this->RP_planets.clear(); std::vector::iterator planetIt; - for (planetIt = this->planetMsgData.begin(); planetIt != this->planetMsgData.end(); planetIt++) - { + for (planetIt = this->planetMsgData.begin(); planetIt != this->planetMsgData.end(); planetIt++) { /* read in planet information */ *planetIt = this->planetInMsgs.at(idx)(); std::string plName(planetIt->PlanetName); - this->getPlanetRadius(plName); //! - [m] get the planet radius + this->getPlanetRadius(plName); //! - [m] get the planet radius this->evaluateAlbedoModel(idx); idx++; } @@ -278,9 +302,11 @@ void Albedo::Reset(uint64_t CurrentSimNanos) /*! This method reads the messages and saves the values to member attributes */ -void Albedo::readMessages() { +void +Albedo::readMessages() +{ //! - Read in planet state message (required) - for (long unsigned int c=0; cplanetInMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->planetInMsgs.size(); c++) { this->planetMsgData.at(c) = this->planetInMsgs.at(c)(); if (m33Determinant(this->planetMsgData.at(c).J20002Pfix) == 0.0) { m33SetIdentity(this->planetMsgData.at(c).J20002Pfix); @@ -297,12 +323,14 @@ void Albedo::readMessages() { /*! This method writes the output albedo messages */ -void Albedo::writeMessages(uint64_t CurrentSimNanos) { +void +Albedo::writeMessages(uint64_t CurrentSimNanos) +{ AlbedoMsgPayload localMessage; memset(&localMessage, 0x0, sizeof(localMessage)); //! - Write albedo output messages for each instrument - for (long unsigned int idx=0; idxalbOutMsgs.size(); idx++) { + for (long unsigned int idx = 0; idx < this->albOutMsgs.size(); idx++) { localMessage.albedoAtInstrumentMax = this->albOutData.at(idx)[0]; localMessage.albedoAtInstrument = this->albOutData.at(idx)[1]; localMessage.AfluxAtInstrumentMax = this->albOutData.at(idx)[2]; @@ -314,46 +342,41 @@ void Albedo::writeMessages(uint64_t CurrentSimNanos) { /*! Planet's equatorial radii and polar radii (if exists) in meters */ -void Albedo::getPlanetRadius(std::string planetSpiceName) +void +Albedo::getPlanetRadius(std::string planetSpiceName) { if (planetSpiceName == "mercury") { this->REQ_planets.push_back(REQ_MERCURY * 1000.0); // [m] this->RP_planets.push_back(-1.0); // [m] - } - else if (planetSpiceName == "venus") { - this->REQ_planets.push_back(REQ_VENUS * 1000.0); // [m] - this->RP_planets.push_back(-1.0); // [m] - } - else if (planetSpiceName == "earth") { + } else if (planetSpiceName == "venus") { + this->REQ_planets.push_back(REQ_VENUS * 1000.0); // [m] + this->RP_planets.push_back(-1.0); // [m] + } else if (planetSpiceName == "earth") { this->REQ_planets.push_back(REQ_EARTH * 1000.0); // [m] this->RP_planets.push_back(RP_EARTH * 1000.0); // [m] - } - else if (planetSpiceName == "moon") { + } else if (planetSpiceName == "moon") { this->REQ_planets.push_back(REQ_MOON * 1000.0); // [m] this->RP_planets.push_back(-1.0); // [m] - } - else if (planetSpiceName == "mars" || planetSpiceName == "mars barycenter") { + } else if (planetSpiceName == "mars" || planetSpiceName == "mars barycenter") { this->REQ_planets.push_back(REQ_MARS * 1000.0); // [m] this->RP_planets.push_back(RP_MARS * 1000.0); // [m] - } - else if (planetSpiceName == "jupiter") { + } else if (planetSpiceName == "jupiter") { this->REQ_planets.push_back(REQ_JUPITER * 1000.0); // [m] this->RP_planets.push_back(-1.0); // [m] - } - else if (planetSpiceName == "saturn") { + } else if (planetSpiceName == "saturn") { this->REQ_planets.push_back(REQ_SATURN * 1000.0); // [m] this->RP_planets.push_back(-1.0); // [m] - } - else if (planetSpiceName == "uranus") { + } else if (planetSpiceName == "uranus") { this->REQ_planets.push_back(REQ_URANUS * 1000.0); // [m] this->RP_planets.push_back(-1.0); // [m] - } - else if (planetSpiceName == "neptune") { + } else if (planetSpiceName == "neptune") { this->REQ_planets.push_back(REQ_NEPTUNE * 1000.0); // [m] this->RP_planets.push_back(-1.0); // [m] - } - else { - bskLogger.bskLog(BSK_ERROR, "Albedo Module (getPlanetRadius): The planet's radius cannot be obtained. The planet (%s) is not found.", planetSpiceName.c_str()); + } else { + bskLogger.bskLog( + BSK_ERROR, + "Albedo Module (getPlanetRadius): The planet's radius cannot be obtained. The planet (%s) is not found.", + planetSpiceName.c_str()); } return; } @@ -361,45 +384,51 @@ void Albedo::getPlanetRadius(std::string planetSpiceName) /*! This method gets the average albedo value of the planet @return double */ -double Albedo::getAlbedoAverage(std::string planetSpiceName) +double +Albedo::getAlbedoAverage(std::string planetSpiceName) { double ALB_avg; if (planetSpiceName == "mercury") { - ALB_avg = 0.119; return ALB_avg; - } - else if (planetSpiceName == "venus") { - ALB_avg = 0.75; return ALB_avg; - } - else if (planetSpiceName == "earth") { - ALB_avg = 0.29; return ALB_avg; - } - else if (planetSpiceName == "moon") { - ALB_avg = 0.123; return ALB_avg; - } - else if (planetSpiceName == "mars" || planetSpiceName == "mars barycenter") { - ALB_avg = 0.16; return ALB_avg; - } - else if (planetSpiceName == "jupiter") { - ALB_avg = 0.34; return ALB_avg; - } - else if (planetSpiceName == "saturn") { - ALB_avg = 0.34; return ALB_avg; - } - else if (planetSpiceName == "uranus") { - ALB_avg = 0.29; return ALB_avg; - } - else if (planetSpiceName == "neptune") { - ALB_avg = 0.31; return ALB_avg; - } - else { - bskLogger.bskLog(BSK_ERROR, "Albedo Module (getAlbedoAverage): The average albedo value is not defined for the specified planet (%s).", planetSpiceName.c_str()); return 0.; + ALB_avg = 0.119; + return ALB_avg; + } else if (planetSpiceName == "venus") { + ALB_avg = 0.75; + return ALB_avg; + } else if (planetSpiceName == "earth") { + ALB_avg = 0.29; + return ALB_avg; + } else if (planetSpiceName == "moon") { + ALB_avg = 0.123; + return ALB_avg; + } else if (planetSpiceName == "mars" || planetSpiceName == "mars barycenter") { + ALB_avg = 0.16; + return ALB_avg; + } else if (planetSpiceName == "jupiter") { + ALB_avg = 0.34; + return ALB_avg; + } else if (planetSpiceName == "saturn") { + ALB_avg = 0.34; + return ALB_avg; + } else if (planetSpiceName == "uranus") { + ALB_avg = 0.29; + return ALB_avg; + } else if (planetSpiceName == "neptune") { + ALB_avg = 0.31; + return ALB_avg; + } else { + bskLogger.bskLog( + BSK_ERROR, + "Albedo Module (getAlbedoAverage): The average albedo value is not defined for the specified planet (%s).", + planetSpiceName.c_str()); + return 0.; } } /*! This method evaluates the albedo model */ -void Albedo::evaluateAlbedoModel(int idx) +void +Albedo::evaluateAlbedoModel(int idx) { this->readFile = false; auto modelName = this->modelNames.at(idx); @@ -410,8 +439,12 @@ void Albedo::evaluateAlbedoModel(int idx) //! - Obtain the parameters of the specified model if (modelName == "ALBEDO_AVG") { //! - Albedo model based on an average value - if (numLat < 0.0) { numLat = this->defaultNumLat; } - if (numLon < 0.0) { numLon = this->defaultNumLon; } + if (numLat < 0.0) { + numLat = this->defaultNumLat; + } + if (numLon < 0.0) { + numLon = this->defaultNumLon; + } auto albAvg = this->ALB_avgs.at(idx); if (albAvg < 0.0) { // set the albedo average automatically based on the planet's name @@ -420,20 +453,18 @@ void Albedo::evaluateAlbedoModel(int idx) std::string plName(planetInfo.PlanetName); this->ALB_avgs.at(idx) = getAlbedoAverage(plName); } - } - else if (modelName == "ALBEDO_DATA") { + } else if (modelName == "ALBEDO_DATA") { //! - Albedo model based on data //! - Check that required module variables are set if (fileName == "") { bskLogger.bskLog(BSK_ERROR, "Albedo Module (evaluateAlbedoModel): Albedo fileName was not set."); - } - else { + } else { this->readFile = true; } fileName = dataPath + "/" + fileName; - } - else { - bskLogger.bskLog(BSK_ERROR, "Albedo Module (evaluateAlbedoModel): Check the model name (%s).", modelName.c_str()); + } else { + bskLogger.bskLog( + BSK_ERROR, "Albedo Module (evaluateAlbedoModel): Check the model name (%s).", modelName.c_str()); } //! - Read the albedo coefficient file if the model requires if (this->readFile) { @@ -443,53 +474,58 @@ void Albedo::evaluateAlbedoModel(int idx) } std::ifstream input(fileName); if (!input) { - bskLogger.bskLog(BSK_ERROR, "Albedo Module (evaluateAlbedoModel): Albedo module is unable to load file %s", fileName.c_str()); + bskLogger.bskLog(BSK_ERROR, + "Albedo Module (evaluateAlbedoModel): Albedo module is unable to load file %s", + fileName.c_str()); // return to avoid reading/attempting to process the invalid file below return; } //! - Read the albedo coefficients std::string line, field; - std::vector< std::vector > array; // the 2D array - std::vector v; // array of values for one line only - while (getline(input, line)) - { + std::vector> array; // the 2D array + std::vector v; // array of values for one line only + while (getline(input, line)) { v.clear(); std::stringstream ss(line); - while (std::getline(ss, field, ',')) // break line into comma delimitted fields + while (std::getline(ss, field, ',')) // break line into comma delimitted fields { v.push_back(field); } array.push_back(v); } - numLat = (int) array.size(); - numLon = (int) array[0].size(); + numLat = (int)array.size(); + numLon = (int)array[0].size(); //! - Compare if the numLat/numLon are not zero if (!numLat || !numLon) { - bskLogger.bskLog(BSK_ERROR, "Albedo Module (evaluateAlbedoModel): There has been an error in reading albedo data from (%s).", fileName.c_str()); + bskLogger.bskLog( + BSK_ERROR, + "Albedo Module (evaluateAlbedoModel): There has been an error in reading albedo data from (%s).", + fileName.c_str()); } //! - Define the albedo array based on the number of grid points - this->ALB[idx] = std::vector < std::vector < double > >(numLat, std::vector < double >(numLon, 0.0)); + this->ALB[idx] = std::vector>(numLat, std::vector(numLon, 0.0)); //! - Albedo coefficients (2d array) from string array to double - for (auto i = 0; i < numLat; ++i) - { - for (auto j = 0; j < numLon; ++j) - { + for (auto i = 0; i < numLat; ++i) { + for (auto j = 0; j < numLon; ++j) { this->ALB[idx][i][j] = atof(array[i][j].c_str()); // Lat: -90 to +90 deg, Lon: -180 to +180 deg } } + } else { + this->ALB[idx] = std::vector>(1, std::vector(1, 0.0)); } - else { this->ALB[idx] = std::vector < std::vector < double > >(1, std::vector < double >(1, 0.0)); } //! - Construct the latitude and longitude points //! - Geodetic latitude and longitude grid points in radian for albedo calculations - double ilat = 0.0; double ilon = 0.0; - double halfLat = numLat / 2; double halfLon = numLon / 2; - this->gdlat[idx] = std::vector < double >(numLat, 0.0); - this->gdlon[idx] = std::vector < double >(numLon, 0.0); + double ilat = 0.0; + double ilon = 0.0; + double halfLat = numLat / 2; + double halfLon = numLon / 2; + this->gdlat[idx] = std::vector(numLat, 0.0); + this->gdlon[idx] = std::vector(numLon, 0.0); this->latDiff[idx] = (180.0 / numLat) * M_PI / 180.0; this->lonDiff[idx] = (360.0 / numLon) * M_PI / 180.0; for (ilat = -halfLat; ilat < halfLat; ilat++) { - this->gdlat[idx][(int64_t) (ilat + halfLat)] = (ilat + 0.5) * this->latDiff[idx]; + this->gdlat[idx][(int64_t)(ilat + halfLat)] = (ilat + 0.5) * this->latDiff[idx]; } for (ilon = -halfLon; ilon < halfLon; ilon++) { this->gdlon[idx][(int64_t)(ilon + halfLon)] = (ilon + 0.5) * this->lonDiff[idx]; @@ -501,31 +537,33 @@ void Albedo::evaluateAlbedoModel(int idx) /*! This method calculates the albedo at instrument */ -void Albedo::computeAlbedo(int idx, int instIdx, SpicePlanetStateMsgPayload planetMsg, bool albArray, double outData[]) { +void +Albedo::computeAlbedo(int idx, int instIdx, SpicePlanetStateMsgPayload planetMsg, bool albArray, double outData[]) +{ //! - Letters denoting the frames: //! - P: planet frame //! - B: spacecraft body frame //! - N: inertial frame //! - S: sun (helio) frame //! - I: instrument body frame - auto fov = this->fovs[instIdx]; //! - [rad] instrument's field of view half angle - auto nHat_B = this->nHat_Bs[instIdx]; //! - [-] unit normal vector of the instrument (spacecraft body) - auto r_IB_B = this->r_IB_Bs[instIdx]; //! - [m] instrument's misalignment vector wrt spacecraft's body frame - this->r_PN_N = Eigen::Vector3d(planetMsg.PositionVector); //! - [m] Planet's position vector (inertial) - this->r_SN_N = Eigen::Vector3d(this->sunMsgData.PositionVector); //! - [m] Sun's position vector (inertial) - this->r_BN_N = Eigen::Vector3d(this->scStatesMsgData.r_BN_N); //! - [m] Spacecraft's position vector (inertial) - this->sigma_BN = Eigen::Vector3d(this->scStatesMsgData.sigma_BN); //! - [m] Spacecraft's MRPs (inertial) + auto fov = this->fovs[instIdx]; //! - [rad] instrument's field of view half angle + auto nHat_B = this->nHat_Bs[instIdx]; //! - [-] unit normal vector of the instrument (spacecraft body) + auto r_IB_B = this->r_IB_Bs[instIdx]; //! - [m] instrument's misalignment vector wrt spacecraft's body frame + this->r_PN_N = Eigen::Vector3d(planetMsg.PositionVector); //! - [m] Planet's position vector (inertial) + this->r_SN_N = Eigen::Vector3d(this->sunMsgData.PositionVector); //! - [m] Sun's position vector (inertial) + this->r_BN_N = Eigen::Vector3d(this->scStatesMsgData.r_BN_N); //! - [m] Spacecraft's position vector (inertial) + this->sigma_BN = Eigen::Vector3d(this->scStatesMsgData.sigma_BN); //! - [m] Spacecraft's MRPs (inertial) Eigen::Matrix3d dcm_BN = this->sigma_BN.toRotationMatrix().transpose(); //! - inertial to sc body transformation this->nHat_N = dcm_BN.transpose() * nHat_B; //! - instrument's normal vector (inertial) //! - [m] sun's position wrt planet (inertial) auto r_SP_N = this->r_SN_N - this->r_PN_N; //! - Vectors related to spacecraft - auto r_BP_N = this->r_BN_N - this->r_PN_N; //! - [m] spacecraft's position wrt planet (inertial) + auto r_BP_N = this->r_BN_N - this->r_PN_N; //! - [m] spacecraft's position wrt planet (inertial) //! - Vectors related to instrument - auto r_IB_N = dcm_BN.transpose() * r_IB_B; //! - [m] instrument's position vector wrt spacecraft (inertial) - auto r_IP_N = r_IB_N + r_BP_N; //! - [m] instrument's position vector wrt planet (inertial) - this->rHat_PI_N = -r_IP_N / r_IP_N.norm(); //! - [-] direction vector from instrument to planet (inertial) - auto r_SI_N = r_SP_N - r_IP_N; //! - [m] sun's position wrt instrument (inertial) + auto r_IB_N = dcm_BN.transpose() * r_IB_B; //! - [m] instrument's position vector wrt spacecraft (inertial) + auto r_IP_N = r_IB_N + r_BP_N; //! - [m] instrument's position vector wrt planet (inertial) + this->rHat_PI_N = -r_IP_N / r_IP_N.norm(); //! - [-] direction vector from instrument to planet (inertial) + auto r_SI_N = r_SP_N - r_IP_N; //! - [m] sun's position wrt instrument (inertial) //! - Calculate the authalic radius, if the polar radius available double t[3], t_aut[3], e, RA_planet, shadowFactorAtdA; if (this->RP_planets.at(idx) > 0.0) { @@ -533,13 +571,12 @@ void Albedo::computeAlbedo(int idx, int instIdx, SpicePlanetStateMsgPayload plan t[0] = pow(this->REQ_planets.at(idx), 2) * 0.5; t[1] = (1 - e * e) / 2.0 / e; t[2] = log((1 + e) / (1 - e)); - RA_planet = sqrt(t[0] * (1 + t[1] * t[2])); //! - Autalic radius of the planet + RA_planet = sqrt(t[0] * (1 + t[1] * t[2])); //! - Autalic radius of the planet //! - Truncated series expansion for geodetic to authalic latitude t_aut[0] = pow(e, 2) / 3.0 + 31.0 * pow(e, 4) / 180.0 + 59.0 * pow(e, 6) / 560.0; t_aut[1] = 17.0 * pow(e, 4) / 360.0 + 61.0 * pow(e, 6) / 1260.0; t_aut[2] = 383.0 * pow(e, 6) / 45360.0; - } - else { + } else { RA_planet = this->REQ_planets.at(idx); v3SetZero(t_aut); } @@ -551,23 +588,25 @@ void Albedo::computeAlbedo(int idx, int instIdx, SpicePlanetStateMsgPayload plan auto alti_I = r_IP_N.norm() - RA_planet; /* Return zeross if the rate of the instrument's altitude to the planet's radius exceeds the specified limit */ - if (this->altitudeRateLimit >=0 && alti_I / RA_planet > this->altitudeRateLimit) { - bskLogger.bskLog(BSK_WARNING, "Albedo Module (computeAlbedo): The rate (altitude to planet's radii) limit is exceeded for the planet (%s) and albedo set to zero.", planetMsg.PlanetName); + if (this->altitudeRateLimit >= 0 && alti_I / RA_planet > this->altitudeRateLimit) { + bskLogger.bskLog(BSK_WARNING, + "Albedo Module (computeAlbedo): The rate (altitude to planet's radii) limit is exceeded for " + "the planet (%s) and albedo set to zero.", + planetMsg.PlanetName); outData[0] = 0.0; outData[1] = 0.0; outData[2] = 0.0; outData[3] = 0.0; - } - else { + } else { //! - Variable definitions needed for the albedo calculations Eigen::Vector3d gdlla; Eigen::Vector3d r_dAP_N, r_SdA_N, r_IdA_N; Eigen::Vector3d rHat_dAP_N, sHat_SdA_N, rHat_IdA_N; - int ilat = 0, ilon = 0, IIdx = 0; // index + int ilat = 0, ilon = 0, IIdx = 0; // index double lon1 = 0.0, lon2 = 0.0, lat1 = 0.0, lat2 = 0.0, tempmax = 0.0, tempfov = 0.0; double f1 = 0.0, f2 = 0.0, f3 = 0.0; double dArea = 0.0, alb_I = 0.0, alb_Imax = 0.0; - std::vector< double > albLon1, albLat1; + std::vector albLon1, albLat1; for (ilon = 0; ilon < this->numLons.at(idx); ilon++) { lon1 = this->gdlon[idx][ilon] + 0.5 * this->lonDiff[idx]; lon2 = this->gdlon[idx][ilon] - 0.5 * this->lonDiff[idx]; @@ -579,7 +618,9 @@ void Albedo::computeAlbedo(int idx, int instIdx, SpicePlanetStateMsgPayload plan lat1 -= t_aut[0] * sin(2.0 * lat1) - t_aut[1] * sin(4.0 * lat1) + t_aut[2] * sin(6.0 * lat1); lat2 -= t_aut[0] * sin(2.0 * lat2) - t_aut[1] * sin(4.0 * lat2) + t_aut[2] * sin(6.0 * lat2); } - gdlla[0] = this->gdlat[idx][ilat]; gdlla[1] = this->gdlon[idx][ilon]; gdlla[2] = 0.0; + gdlla[0] = this->gdlat[idx][ilat]; + gdlla[1] = this->gdlon[idx][ilon]; + gdlla[2] = 0.0; //! - Vectors related to incremental area //! - [m] position of the incremental area (inertial) r_dAP_N = LLA2PCI(gdlla, planetMsg.J20002Pfix, RA_planet); //! - Assumes that the planet is a sphere. @@ -589,24 +630,25 @@ void Albedo::computeAlbedo(int idx, int instIdx, SpicePlanetStateMsgPayload plan sHat_SdA_N = r_SdA_N / r_SdA_N.norm(); //! - [-] sun direction vector from dA (inertial) rHat_IdA_N = r_IdA_N / r_IdA_N.norm(); //! - [-] dA to instrument direction vector (inertial) //! - Portions of the planet - f1 = rHat_dAP_N.dot(sHat_SdA_N); //! - for sunlit - f2 = rHat_dAP_N.dot(rHat_IdA_N); //! - for instrument's max fov - f3 = this->nHat_N.dot(-rHat_IdA_N); //! - for instrument's config fov + f1 = rHat_dAP_N.dot(sHat_SdA_N); //! - for sunlit + f2 = rHat_dAP_N.dot(rHat_IdA_N); //! - for instrument's max fov + f3 = this->nHat_N.dot(-rHat_IdA_N); //! - for instrument's config fov //! - Detect the sunlit region of the planet seen by the instrument if (f1 > 0 && f2 > 0) { //! - Sunlit portion of the planet seen by the instrument's position (max) //! - Shadow factor at dA (optional) shadowFactorAtdA = this->shadowFactorAtdA; - if (this->eclipseCase) { shadowFactorAtdA = computeEclipseAtdA(RA_planet, r_dAP_N, r_SP_N); } + if (this->eclipseCase) { + shadowFactorAtdA = computeEclipseAtdA(RA_planet, r_dAP_N, r_SP_N); + } //! - Area of the incremental area dArea = (fabs(lon1 - lon2) * fabs(sin(lat1) - sin(lat2)) * pow(r_dAP_N.norm(), 2)); //! - Maximum albedo flux ratio at instrument's position [-] tempmax = f1 * f2 * dArea / (pow(r_IdA_N.norm(), 2) * M_PI); - //if (this->ALB_data >= 0.0) { tempmax = this->ALB_data * tempmax; } + // if (this->ALB_data >= 0.0) { tempmax = this->ALB_data * tempmax; } if (albArray == false) { tempmax = this->ALB_avgs.at(idx) * tempmax; - } - else { + } else { tempmax = this->ALB[idx][ilat][ilon] * tempmax; } alb_Imax = alb_Imax + tempmax * shadowFactorAtdA; @@ -617,8 +659,7 @@ void Albedo::computeAlbedo(int idx, int instIdx, SpicePlanetStateMsgPayload plan tempfov = f1 * f2 * f3 * dArea / (pow(r_IdA_N.norm(), 2) * M_PI); if (albArray == false) { tempfov = this->ALB_avgs.at(idx) * tempfov; - } - else { + } else { tempfov = this->ALB[idx][ilat][ilon] * tempfov; } alb_I = alb_I + tempfov * shadowFactorAtdA; @@ -657,7 +698,8 @@ void Albedo::computeAlbedo(int idx, int instIdx, SpicePlanetStateMsgPayload plan /*! This method computes eclipse at the incremental area if eclipseCase is defined true @return double */ -double Albedo::computeEclipseAtdA(double Rplanet, Eigen::Vector3d r_dAP_N, Eigen::Vector3d r_SP_N) +double +Albedo::computeEclipseAtdA(double Rplanet, Eigen::Vector3d r_dAP_N, Eigen::Vector3d r_SP_N) { //! - Compute the shadow factor at incremental area //! - Note that the eclipse module computes the shadow factor at the spacecraft position @@ -672,25 +714,22 @@ double Albedo::computeEclipseAtdA(double Rplanet, Eigen::Vector3d r_dAP_N, Eigen auto l_1 = c_1 * tan(f_1); auto l_2 = c_2 * tan(f_2); double area = 0.0; - double shadowFactorAtdA = 1.0; //! - Initialise the value for no eclipse - double a = safeAsin(REQ_SUN * 1000 / r_SdA_N.norm()); //! - Apparent radius of sun - double b = safeAsin(Rplanet / r_dAP_N.norm()); //! - Apparent radius of occulting body + double shadowFactorAtdA = 1.0; //! - Initialise the value for no eclipse + double a = safeAsin(REQ_SUN * 1000 / r_SdA_N.norm()); //! - Apparent radius of sun + double b = safeAsin(Rplanet / r_dAP_N.norm()); //! - Apparent radius of occulting body double c = safeAcos((-r_dAP_N.dot(r_SdA_N)) / (r_dAP_N.norm() * r_SdA_N.norm())); - if (fabs(l) < fabs(l_2) || fabs(l) < fabs(l_1)) - { + if (fabs(l) < fabs(l_2) || fabs(l) < fabs(l_1)) { // The order of the conditionals is important. // In particular (c < a + b) must check last to avoid testing // with implausible a, b and c values if (c < b - a) { // total eclipse, implying a < b shadowFactorAtdA = 0.0; - } - else if (c < a - b) { // partial maximum eclipse, implying a > b + } else if (c < a - b) { // partial maximum eclipse, implying a > b double areaSun = M_PI * a * a; double areaBody = M_PI * b * b; area = areaSun - areaBody; shadowFactorAtdA = 1 - area / (M_PI * a * a); - } - else if (c < a + b) { // partial eclipse + } else if (c < a + b) { // partial eclipse double x = (c * c + a * a - b * b) / (2 * c); double y = sqrt(a * a - x * x); area = a * a * safeAcos(x / a) + b * b * safeAcos((c - x) / b) - c * y; @@ -699,8 +738,7 @@ double Albedo::computeEclipseAtdA(double Rplanet, Eigen::Vector3d r_dAP_N, Eigen } if (shadowFactorAtdA < 0.0) { shadowFactorAtdA = 0.0; - } - else if (shadowFactorAtdA > 1.0) { + } else if (shadowFactorAtdA > 1.0) { shadowFactorAtdA = 1.0; } return shadowFactorAtdA; diff --git a/src/simulation/environment/albedo/albedo.h b/src/simulation/environment/albedo/albedo.h index 4b083a3f41..c4fe2dba01 100644 --- a/src/simulation/environment/albedo/albedo.h +++ b/src/simulation/environment/albedo/albedo.h @@ -45,89 +45,110 @@ #include "architecture/utilities/macroDefinitions.h" /*! albedo instrument configuration class */ -typedef class Config { -public: +typedef class Config +{ + public: Config(); ~Config(); - double fov; //!< [rad] instrument's field of view half angle - Eigen::Vector3d nHat_B; //!< [-] unit normal of the instrument (spacecraft body) - Eigen::Vector3d r_IB_B; //!< [m] instrument's misalignment wrt spacecraft's body frame + double fov; //!< [rad] instrument's field of view half angle + Eigen::Vector3d nHat_B; //!< [-] unit normal of the instrument (spacecraft body) + Eigen::Vector3d r_IB_B; //!< [m] instrument's misalignment wrt spacecraft's body frame } instConfig_t; /*! @brief albedo class */ -class Albedo : public SysModel { -public: +class Albedo : public SysModel +{ + public: Albedo(); ~Albedo(); - void UpdateState(uint64_t CurrentSimNanos); //!< @brief updates the state - void Reset(uint64_t CurrentSimNanos); //!< @brief resets the module - - void addInstrumentConfig(instConfig_t configMsg); //!< @brief adds instrument configuration (overloaded function) - void addInstrumentConfig(double fov, Eigen::Vector3d nHat_B, Eigen::Vector3d r_IB_B); //!< @brief adds instrument configuration (overloaded function) - void addPlanetandAlbedoAverageModel(Message *msg); //!< @brief This method adds planet msg and albedo average model name (overloaded function) - void addPlanetandAlbedoAverageModel(Message *msg, double ALB_avg, int numLat, int numLon); //!< @brief This method adds planet name and albedo average model name (overloaded function) - void addPlanetandAlbedoDataModel(Message *msg, std::string dataPath, std::string fileName); //!< @brief This method adds planet name and albedo data model - double getAlbedoAverage(std::string planetSpiceName); //!< @brief gets the average albedo value of the specified planet - -private: - void readMessages(); //!< reads the inpt messages - void writeMessages(uint64_t CurrentSimNanos); //!< writes the outpus messages - void getPlanetRadius(std::string planetSpiceName); //!< gets the planet's radius - void evaluateAlbedoModel(int idx); //!< evaluates the ALB model - void computeAlbedo(int idx, int instIdx, SpicePlanetStateMsgPayload planetMsg, bool AlbArray, double outData[]); //!< computes the albedo at instrument's location - double computeEclipseAtdA(double Rplanet, Eigen::Vector3d r_dAP_N, Eigen::Vector3d r_SP_N); //!< computes the shadow factor at dA - -public: - std::vector*> albOutMsgs; //!< vector of output messages for albedo data - ReadFunctor sunPositionInMsg; //!< input message name for sun data - ReadFunctor spacecraftStateInMsg; //!< input message name for spacecraft data + void UpdateState(uint64_t CurrentSimNanos); //!< @brief updates the state + void Reset(uint64_t CurrentSimNanos); //!< @brief resets the module + + void addInstrumentConfig(instConfig_t configMsg); //!< @brief adds instrument configuration (overloaded function) + void addInstrumentConfig(double fov, + Eigen::Vector3d nHat_B, + Eigen::Vector3d r_IB_B); //!< @brief adds instrument configuration (overloaded function) + void addPlanetandAlbedoAverageModel( + Message* + msg); //!< @brief This method adds planet msg and albedo average model name (overloaded function) + void addPlanetandAlbedoAverageModel( + Message* msg, + double ALB_avg, + int numLat, + int numLon); //!< @brief This method adds planet name and albedo average model name (overloaded function) + void addPlanetandAlbedoDataModel( + Message* msg, + std::string dataPath, + std::string fileName); //!< @brief This method adds planet name and albedo data model + double getAlbedoAverage( + std::string planetSpiceName); //!< @brief gets the average albedo value of the specified planet + + private: + void readMessages(); //!< reads the inpt messages + void writeMessages(uint64_t CurrentSimNanos); //!< writes the outpus messages + void getPlanetRadius(std::string planetSpiceName); //!< gets the planet's radius + void evaluateAlbedoModel(int idx); //!< evaluates the ALB model + void computeAlbedo(int idx, + int instIdx, + SpicePlanetStateMsgPayload planetMsg, + bool AlbArray, + double outData[]); //!< computes the albedo at instrument's location + double computeEclipseAtdA(double Rplanet, + Eigen::Vector3d r_dAP_N, + Eigen::Vector3d r_SP_N); //!< computes the shadow factor at dA + + public: + std::vector*> albOutMsgs; //!< vector of output messages for albedo data + ReadFunctor sunPositionInMsg; //!< input message name for sun data + ReadFunctor spacecraftStateInMsg; //!< input message name for spacecraft data std::vector> planetInMsgs; //!< vector of planet data input data - 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) - double fov_default; //!< [rad] default value for instrument's field of view half angle - bool eclipseCase; //!< consider eclipse at dA, if true - double shadowFactorAtdA; //!< [-] shadow factor at incremental area - double altitudeRateLimit; //!< [-] rate limit of the instrument's altitude to the planet's radius for albedo calculations - -private: - std::vector dataPaths; //!< string with the path to the ALB coefficient folder - std::vector fileNames; //!< file names containing the ALB coefficients - std::vector modelNames; //!< albedo model names - std::vector ALB_avgs; //!< [-] albedo average value vector for each planet defined - std::vector REQ_planets, RP_planets; //!< [m] equatorial and polar radius of the planets - Eigen::MatrixXd albLon, albLat; //!< sunlit area seen by the instroment fov - double scLon, scLat; //!< [deg, deg] spaceccraft footprint - std::vector numLats, numLons; //!< [-] vector of latitude and longitude number - double albedoAtInstrument; //!< [-] total albedo at instrument location - double albedoAtInstrumentMax; //!< [-] max total albedo at instrument location - double SfluxAtInstrument; //!< [W/m2] solar flux at instrument's position - double AfluxAtInstrumentMax; //!< [W/m2] max albedo flux at instrument's position - double AfluxAtInstrument; //!< [W/m2] albedo flux at instrument's position - std::vector < Eigen::Vector3d > r_IB_Bs; //!< [m] instrument's misalignment vector wrt spacecraft's body frame - std::vector < Eigen::Vector3d > nHat_Bs; //!< [-] unit normal vector of the instrument (spacecraft body) - std::vector < double > fovs; //!< [rad] vector containing instrument's field of view half angle - std::vector < Eigen::Vector4d > albOutData; //!< the vector that keeps the albedo output data - std::map < int, std::vector < double > > gdlat; //!< [rad] geodetic latitude - std::map < int, std::vector < double > > gdlon; //!< [rad] geodetic longitude - std::map < int, double > latDiff; //!< [rad] latitude difference between grid points - std::map < int, double > lonDiff; //!< [rad] longitude difference between grid points - std::map < int, std::vector < std::vector < double > > > ALB; //!< [-] ALB coefficients - bool readFile; //!< defines if there is a need for reading an albedo model file or not - std::vector albArray; //!< defines if the albedo data is formatted as array or not - Eigen::Vector3d r_PN_N; //!< [m] planet position (inertial) - Eigen::Vector3d r_SN_N; //!< [m] sun position (inertial) - Eigen::MRPd sigma_BN; //!< [-] Current spaceraft MRPs (inertial) - Eigen::Vector3d r_BN_N; //!< [m] s/c position (inertial) - Eigen::Vector3d nHat_N; //!< [-] Unit normal vector of the instrument (inertial) - Eigen::Vector3d rHat_PI_N; //!< [-] direction vector from instrument to planet + 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) + double fov_default; //!< [rad] default value for instrument's field of view half angle + bool eclipseCase; //!< consider eclipse at dA, if true + double shadowFactorAtdA; //!< [-] shadow factor at incremental area + double + altitudeRateLimit; //!< [-] rate limit of the instrument's altitude to the planet's radius for albedo calculations + + private: + std::vector dataPaths; //!< string with the path to the ALB coefficient folder + std::vector fileNames; //!< file names containing the ALB coefficients + std::vector modelNames; //!< albedo model names + std::vector ALB_avgs; //!< [-] albedo average value vector for each planet defined + std::vector REQ_planets, RP_planets; //!< [m] equatorial and polar radius of the planets + Eigen::MatrixXd albLon, albLat; //!< sunlit area seen by the instroment fov + double scLon, scLat; //!< [deg, deg] spaceccraft footprint + std::vector numLats, numLons; //!< [-] vector of latitude and longitude number + double albedoAtInstrument; //!< [-] total albedo at instrument location + double albedoAtInstrumentMax; //!< [-] max total albedo at instrument location + double SfluxAtInstrument; //!< [W/m2] solar flux at instrument's position + double AfluxAtInstrumentMax; //!< [W/m2] max albedo flux at instrument's position + double AfluxAtInstrument; //!< [W/m2] albedo flux at instrument's position + std::vector r_IB_Bs; //!< [m] instrument's misalignment vector wrt spacecraft's body frame + std::vector nHat_Bs; //!< [-] unit normal vector of the instrument (spacecraft body) + std::vector fovs; //!< [rad] vector containing instrument's field of view half angle + std::vector albOutData; //!< the vector that keeps the albedo output data + std::map> gdlat; //!< [rad] geodetic latitude + std::map> gdlon; //!< [rad] geodetic longitude + std::map latDiff; //!< [rad] latitude difference between grid points + std::map lonDiff; //!< [rad] longitude difference between grid points + std::map>> ALB; //!< [-] ALB coefficients + bool readFile; //!< defines if there is a need for reading an albedo model file or not + std::vector albArray; //!< defines if the albedo data is formatted as array or not + Eigen::Vector3d r_PN_N; //!< [m] planet position (inertial) + Eigen::Vector3d r_SN_N; //!< [m] sun position (inertial) + Eigen::MRPd sigma_BN; //!< [-] Current spaceraft MRPs (inertial) + Eigen::Vector3d r_BN_N; //!< [m] s/c position (inertial) + Eigen::Vector3d nHat_N; //!< [-] Unit normal vector of the instrument (inertial) + Eigen::Vector3d rHat_PI_N; //!< [-] direction vector from instrument to planet std::vector planetMsgData; //!< vector of incoming planet message states - SpicePlanetStateMsgPayload sunMsgData; //!< sun message data - SCStatesMsgPayload scStatesMsgData; //!< spacecraft message data + SpicePlanetStateMsgPayload sunMsgData; //!< sun message data + SCStatesMsgPayload scStatesMsgData; //!< spacecraft message data }; #endif /* ALBEDO_BASE_H */ diff --git a/src/simulation/environment/dentonFluxModel/_UnitTest/test_dentonFluxModel.py b/src/simulation/environment/dentonFluxModel/_UnitTest/test_dentonFluxModel.py index 08c6aa5968..544f9d8a84 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 @@ -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/dentonFluxModel/dentonFluxModel.cpp b/src/simulation/environment/dentonFluxModel/dentonFluxModel.cpp index 763c90a6a3..beeb3a38a0 100644 --- a/src/simulation/environment/dentonFluxModel/dentonFluxModel.cpp +++ b/src/simulation/environment/dentonFluxModel/dentonFluxModel.cpp @@ -33,109 +33,96 @@ DentonFluxModel::~DentonFluxModel() = default; @param CurrentSimNanos current simulation time in nano-seconds */ -void DentonFluxModel::Reset(uint64_t CurrentSimNanos) +void +DentonFluxModel::Reset(uint64_t CurrentSimNanos) { // Check that required input messages are connected - if (!this->scStateInMsg.isLinked()) - { + if (!this->scStateInMsg.isLinked()) { bskLogger.bskLog(BSK_ERROR, "DentonFluxModel.scStateInMsg was not linked."); } - if (!this->earthStateInMsg.isLinked()) - { + if (!this->earthStateInMsg.isLinked()) { bskLogger.bskLog(BSK_ERROR, "DentonFluxModel.earthStateInMsg was not linked."); } - if (!this->sunStateInMsg.isLinked()) - { + if (!this->sunStateInMsg.isLinked()) { bskLogger.bskLog(BSK_ERROR, "DentonFluxModel.sunStateInMsg was not linked."); } // Check that required parameters are set - if (this->numOutputEnergies < 0) - { + if (this->numOutputEnergies < 0) { bskLogger.bskLog(BSK_ERROR, "DentonFluxModel.numEnergies was not set."); } - if (this->kpIndex == "") - { + if (this->kpIndex == "") { bskLogger.bskLog(BSK_ERROR, "DentonFluxModel.kpIndex was not set."); } - if (this->dataPath == "") - { + if (this->dataPath == "") { bskLogger.bskLog(BSK_ERROR, "DentonFluxModel.dataPath was not set."); } // Check the desired array size is not larger than the maximum value - if (this->numOutputEnergies > MAX_PLASMA_FLUX_SIZE) - { + if (this->numOutputEnergies > MAX_PLASMA_FLUX_SIZE) { bskLogger.bskLog(BSK_ERROR, "DentonFluxModel: Maximum denton space weather array size exceeded."); } // Check that the Kp index is a string of length 2 - if (!(this->kpIndex.length() == 2)) - { + if (!(this->kpIndex.length() == 2)) { bskLogger.bskLog(BSK_ERROR, "DentonFluxModel.kpIndex must be a string of length 2, such as '1-', '3o', '4+' etc."); } // Convert Kp index (such as '0o', '1-', '5+' etc.) to Kp index counter (int 0-27) char kpMain = this->kpIndex[0]; // main Kp index, between 0 and 9 - char kpSub = this->kpIndex[1]; // sub Kp index, either '-', 'o', or '+' - int kpMainInt = kpMain - '0'; // convert main Kp from char to int + char kpSub = this->kpIndex[1]; // sub Kp index, either '-', 'o', or '+' + int kpMainInt = kpMain - '0'; // convert main Kp from char to int if (kpMainInt < 0 || kpMainInt > 9) { bskLogger.bskLog(BSK_ERROR, "DentonFluxModel: Kp index not set to a proper value."); } if (kpSub == '-') { - this->kpIndexCounter = 3*kpMainInt - 1; - } - else if (kpSub == 'o') { - this->kpIndexCounter = 3*kpMainInt; - } - else if (kpSub == '+') { - this->kpIndexCounter = 3*kpMainInt + 1; - } - else { + this->kpIndexCounter = 3 * kpMainInt - 1; + } else if (kpSub == 'o') { + this->kpIndexCounter = 3 * kpMainInt; + } else if (kpSub == '+') { + this->kpIndexCounter = 3 * kpMainInt + 1; + } else { bskLogger.bskLog(BSK_ERROR, "DentonFluxModel: Kp index not set to a proper value."); } // Check that Kp index is between 0o and 9o (corresponding to Kp index counter 0-27) - if (this->kpIndexCounter < 0 || this->kpIndexCounter > MAX_NUM_KPS - 1) - { + if (this->kpIndexCounter < 0 || this->kpIndexCounter > MAX_NUM_KPS - 1) { bskLogger.bskLog(BSK_ERROR, "DentonFluxModel: Kp index must be between 0o and 9o. Indices 0- and 9+ do not exist."); } // convert energies to log10 values - for (int k = 0; k < MAX_NUM_ENERGIES; k++) - { + for (int k = 0; k < MAX_NUM_ENERGIES; k++) { this->logEnElec[k] = log(this->enElec[k]); this->logEnProt[k] = log(this->enProt[k]); } // Define Energy Array - double step = (40000 - 1)/this->numOutputEnergies; + double step = (40000 - 1) / this->numOutputEnergies; // start at 100eV // (fluxes of smaller energies are unreliable due to contamination with secondary electrons and photoelectrons, // according to Denton) this->inputEnergies[0] = 100; - for (int i = 1; i < numOutputEnergies; i++) - { - this->inputEnergies[i] = this->inputEnergies[i-1] + step; + for (int i = 1; i < numOutputEnergies; i++) { + this->inputEnergies[i] = this->inputEnergies[i - 1] + step; } // Read in Denton data files readDentonDataFile(this->eDataFileName, this->mean_e_flux); readDentonDataFile(this->iDataFileName, this->mean_i_flux); - } /*! This is the main method that gets called every time the module is updated. Provide an appropriate description. @param CurrentSimNanos current simulation time in nano-seconds */ -void DentonFluxModel::UpdateState(uint64_t CurrentSimNanos) +void +DentonFluxModel::UpdateState(uint64_t CurrentSimNanos) { // Make local copies of messages - SCStatesMsgPayload scStateInMsgBuffer; //!< local copy of spacecraft states - PlasmaFluxMsgPayload fluxOutMsgBuffer; //!< local copy of the plasma flux output message content - SpicePlanetStateMsgPayload sunSpiceInMsgBuffer; //!< local copy of the sun state input message payload - SpicePlanetStateMsgPayload earthSpiceInMsgBuffer; //!< local copy of the earth state input message payload + SCStatesMsgPayload scStateInMsgBuffer; //!< local copy of spacecraft states + PlasmaFluxMsgPayload fluxOutMsgBuffer; //!< local copy of the plasma flux output message content + SpicePlanetStateMsgPayload sunSpiceInMsgBuffer; //!< local copy of the sun state input message payload + SpicePlanetStateMsgPayload earthSpiceInMsgBuffer; //!< local copy of the earth state input message payload // Always zero the output message buffers before assigning values fluxOutMsgBuffer = this->fluxOutMsg.zeroMsgPayload; @@ -150,16 +137,15 @@ void DentonFluxModel::UpdateState(uint64_t CurrentSimNanos) double finalIon; // Calculate both Sun (S) and spacecraft (B) position vectors from Earth (E) in ECI frame - double r_BE_N[3]; /* satellite position relative to Earth in N frame components */ - double r_SE_N[3]; /* sun position relative to Earth in N frame components */ + double r_BE_N[3]; /* satellite position relative to Earth in N frame components */ + double r_SE_N[3]; /* sun position relative to Earth in N frame components */ v3Subtract(scStateInMsgBuffer.r_BN_N, earthSpiceInMsgBuffer.PositionVector, r_BE_N); v3Subtract(sunSpiceInMsgBuffer.PositionVector, earthSpiceInMsgBuffer.PositionVector, r_SE_N); // Check that spacecraft is located in GEO regime (otherwise Denton flux data not valid) double r_GEO = 42000e3; // GEO orbit radius - double tol = 4000e3; // tolerance how far spacecraft can be away from GEO - if (v2Norm(r_BE_N) < r_GEO - tol || v2Norm(r_BE_N) > r_GEO + tol || abs(r_BE_N[2]) > tol) - { + double tol = 4000e3; // tolerance how far spacecraft can be away from GEO + if (v2Norm(r_BE_N) < r_GEO - tol || v2Norm(r_BE_N) > r_GEO + tol || abs(r_BE_N[2]) > tol) { bskLogger.bskLog(BSK_WARNING, "DentonFluxModel: Spacecraft not in GEO regime. Denton Model not valid outside of GEO."); } @@ -168,8 +154,7 @@ void DentonFluxModel::UpdateState(uint64_t CurrentSimNanos) calcLocalTime(r_BE_N, r_SE_N); // For loop to calculate each element of output flux vectors - for (int i = 0; i < this->numOutputEnergies; i++) - { + for (int i = 0; i < this->numOutputEnergies; i++) { // Convert energies to log10 double logInputEnergy = log(this->inputEnergies[i]); @@ -177,21 +162,16 @@ void DentonFluxModel::UpdateState(uint64_t CurrentSimNanos) int eHigherIndex = 0; int eLowerIndex = 0; - for (int j = 0; j < MAX_NUM_ENERGIES; j++) - { - if (this->logEnElec[j] > logInputEnergy) - { + for (int j = 0; j < MAX_NUM_ENERGIES; j++) { + if (this->logEnElec[j] > logInputEnergy) { int k = 0; - if (j == 0) - { - k = j+1; - } - else - { + if (j == 0) { + k = j + 1; + } else { k = j; } eHigherIndex = k; - eLowerIndex = k-1; + eLowerIndex = k - 1; break; } } @@ -200,21 +180,16 @@ void DentonFluxModel::UpdateState(uint64_t CurrentSimNanos) int iHigherIndex = 0; int iLowerIndex = 0; - for (int m = 0; m < MAX_NUM_ENERGIES; m++) - { - if (this->logEnProt[m] > logInputEnergy) - { + for (int m = 0; m < MAX_NUM_ENERGIES; m++) { + if (this->logEnProt[m] > logInputEnergy) { int k; - if (m == 0) - { - k = m+1; - } - else - { + if (m == 0) { + k = m + 1; + } else { k = m; } iHigherIndex = k; - iLowerIndex = k-1; + iLowerIndex = k - 1; break; } } @@ -235,8 +210,15 @@ void DentonFluxModel::UpdateState(uint64_t CurrentSimNanos) flux14 = this->mean_e_flux[this->kpIndexCounter][eHigherIndex][localTimeCeil]; // ELECTRON: Find flux (differential flux in units of [cm^-2 s^-1 sr^-2 eV^-1]) - finalElec = bilinear(localTimeFloor, localTimeCeil, logEnElec[eLowerIndex], logEnElec[eHigherIndex], - logInputEnergy, flux11, flux12, flux13, flux14); + finalElec = bilinear(localTimeFloor, + localTimeCeil, + logEnElec[eLowerIndex], + logEnElec[eHigherIndex], + logInputEnergy, + flux11, + flux12, + flux13, + flux14); finalElec = pow(10.0, finalElec); // ION: Gather four nearest *MEAN* flux values @@ -246,8 +228,15 @@ void DentonFluxModel::UpdateState(uint64_t CurrentSimNanos) flux14 = this->mean_i_flux[this->kpIndexCounter][iHigherIndex][localTimeCeil]; // ION: Find flux (differential flux in units of [cm^-2 s^-1 sr^-2 eV^-1]) - finalIon = bilinear(localTimeFloor, localTimeCeil, logEnProt[iLowerIndex], logEnProt[iHigherIndex], - logInputEnergy, flux11, flux12, flux13, flux14); + finalIon = bilinear(localTimeFloor, + localTimeCeil, + logEnProt[iLowerIndex], + logEnProt[iHigherIndex], + logInputEnergy, + flux11, + flux12, + flux13, + flux14); finalIon = pow(10.0, finalIon); // Store the output message (differential flux in units of [m^-2 s^-1 sr^-2 eV^-1]) @@ -265,55 +254,56 @@ void DentonFluxModel::UpdateState(uint64_t CurrentSimNanos) @param r_BE_N spacecraft position vector relative to the Earth */ -void DentonFluxModel::calcLocalTime(double r_SE_N[3], double r_BE_N[3]) +void +DentonFluxModel::calcLocalTime(double r_SE_N[3], double r_BE_N[3]) { // r_SE_N and r_BE_N are projected onto the equatorial plane to compute angle, // thus only x and y components are used (z component is perpendicular to equator) - double r_BE_N_hat[2]; /* unit vector from Earth to spacecraft */ - double r_SE_N_hat[2]; /* unit vector from Earth to Sun */ + double r_BE_N_hat[2]; /* unit vector from Earth to spacecraft */ + double r_SE_N_hat[2]; /* unit vector from Earth to Sun */ v2Normalize(r_BE_N, r_BE_N_hat); v2Normalize(r_SE_N, r_SE_N_hat); // Determine Local Time: Using atan2() double x = v2Dot(r_BE_N_hat, r_SE_N_hat); - double y = r_BE_N_hat[0]*r_SE_N_hat[1] - r_BE_N_hat[1]*r_SE_N_hat[0]; - double theta = atan2(y,x); + double y = r_BE_N_hat[0] * r_SE_N_hat[1] - r_BE_N_hat[1] * r_SE_N_hat[0]; + double theta = atan2(y, x); - if (x <= -1.0) - { - this->localTime = 0.0; //!< Data files are from 0-23 LT, this results in 24h being 0h - } - else - { - this->localTime = 12.00 + (theta / (2.*M_PI))*24; + if (x <= -1.0) { + this->localTime = 0.0; //!< Data files are from 0-23 LT, this results in 24h being 0h + } else { + this->localTime = 12.00 + (theta / (2. * M_PI)) * 24; } return; - } /*! Bilinear interpolation method */ -double DentonFluxModel::bilinear(int x1, int x2, double y1, double y2, double y, double f11, double f12, double f13, - double f14) +double +DentonFluxModel::bilinear(int x1, + int x2, + double y1, + double y2, + double y, + double f11, + double f12, + double f13, + double f14) { // Define variables double R1, R2, bilinear = 0.0; double x = this->localTime; - if (x1 != x2) - { - R1 = ( (x2 - x) / (x2 - x1) ) * f11 + ( (x - x1) / (x2 - x1) ) * f13; - R2 = ( (x2 - x) / (x2 - x1) ) * f12 + ( (x - x1) / (x2 - x1) ) * f14; - bilinear = ( (y2 - y ) / (y2 - y1) ) * R1 + ( (y - y1) / (y2 - y1) ) * R2; - } - else - { - bilinear = ( (y2 - y ) / (y2 - y1) ) * f11 + ( (y - y1) / (y2 - y1) ) * f13; + if (x1 != x2) { + R1 = ((x2 - x) / (x2 - x1)) * f11 + ((x - x1) / (x2 - x1)) * f13; + R2 = ((x2 - x) / (x2 - x1)) * f12 + ((x - x1) / (x2 - x1)) * f14; + bilinear = ((y2 - y) / (y2 - y1)) * R1 + ((y - y1) / (y2 - y1)) * R2; + } else { + bilinear = ((y2 - y) / (y2 - y1)) * f11 + ((y - y1) / (y2 - y1)) * f13; } return bilinear; - } /*! Read in the Denton data file @@ -321,8 +311,9 @@ double DentonFluxModel::bilinear(int x1, int x2, double y1, double y2, double y, @param data data array pointer */ -void DentonFluxModel::readDentonDataFile(std::string fileName, - double data[MAX_NUM_KPS][MAX_NUM_ENERGIES][MAX_NUM_LOCAL_TIMES]) +void +DentonFluxModel::readDentonDataFile(std::string fileName, + double data[MAX_NUM_KPS][MAX_NUM_ENERGIES][MAX_NUM_LOCAL_TIMES]) { double temp = 0.0; @@ -335,18 +326,15 @@ void DentonFluxModel::readDentonDataFile(std::string fileName, // Read information into array: Data includes information about mean, standard deviation, // median and percentiles (7 types of values in total). Only mean is relevant for this module if (inputFile.is_open()) { - for (int i = 0; i < MAX_NUM_KPS*MAX_NUM_VALUE_TYPES; i++) - { for (int j = 0; j < MAX_NUM_ENERGIES; j++) - { for (int k = 0; k < MAX_NUM_LOCAL_TIMES; k++) - { + for (int i = 0; i < MAX_NUM_KPS * MAX_NUM_VALUE_TYPES; i++) { + for (int j = 0; j < MAX_NUM_ENERGIES; j++) { + for (int k = 0; k < MAX_NUM_LOCAL_TIMES; k++) { // MEAN corresponds to every 7th index - if (i%MAX_NUM_VALUE_TYPES == 0) - { - inputFile >> data[i/MAX_NUM_VALUE_TYPES][j][k]; + if (i % MAX_NUM_VALUE_TYPES == 0) { + inputFile >> data[i / MAX_NUM_VALUE_TYPES][j][k]; } else { inputFile >> temp; } - } } } diff --git a/src/simulation/environment/dentonFluxModel/dentonFluxModel.h b/src/simulation/environment/dentonFluxModel/dentonFluxModel.h index 741ac80aab..ef6e46c6c7 100644 --- a/src/simulation/environment/dentonFluxModel/dentonFluxModel.h +++ b/src/simulation/environment/dentonFluxModel/dentonFluxModel.h @@ -27,15 +27,16 @@ #include "architecture/utilities/bskLogging.h" #include "architecture/messaging/messaging.h" -#define MAX_NUM_KPS 28 -#define MAX_NUM_ENERGIES 40 -#define MAX_NUM_LOCAL_TIMES 24 -#define MAX_NUM_VALUE_TYPES 7 +#define MAX_NUM_KPS 28 +#define MAX_NUM_ENERGIES 40 +#define MAX_NUM_LOCAL_TIMES 24 +#define MAX_NUM_VALUE_TYPES 7 /*! @brief This module provides the 10-year averaged GEO elecon and ion flux as discussed in the paper by Denton. */ -class DentonFluxModel: public SysModel { -public: +class DentonFluxModel : public SysModel +{ + public: // Constructor And Destructor DentonFluxModel(); ~DentonFluxModel(); @@ -43,61 +44,55 @@ 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 - std::string dataPath = ""; //!< -- String with the path to the Denton GEO data + int numOutputEnergies = -1; //!< number of energy bins used in the output message + std::string kpIndex = ""; //!< Kp index + std::string dataPath = ""; //!< -- String with the path to the Denton GEO data std::string eDataFileName = "model_e_array_all.txt"; //!< file name of the electron data file std::string iDataFileName = "model_i_array_all.txt"; //!< file name of the ion data file - ReadFunctor scStateInMsg; //!< spacecraft state input message + ReadFunctor scStateInMsg; //!< spacecraft state input message ReadFunctor earthStateInMsg; //!< Earth planet state input message - ReadFunctor sunStateInMsg; //!< sun state input message + ReadFunctor sunStateInMsg; //!< sun state input message Message fluxOutMsg; //!< output message with ion and electron fluxes BSKLogger bskLogger; //!< -- BSK Logging -private: + private: void calcLocalTime(double v1[3], double v2[3]); double bilinear(int, int, double, double, double, double, double, double, double); void readDentonDataFile(std::string fileName, double data[MAX_NUM_KPS][MAX_NUM_ENERGIES][MAX_NUM_LOCAL_TIMES]); - int kpIndexCounter; //!< Kp index counter (betweeen 0 and 27) - double localTime; //!< spacecraft location time relative to sun heading at GEO - double logEnElec[MAX_NUM_ENERGIES]; //!< log of the electron energies - double logEnProt[MAX_NUM_ENERGIES]; //!< log of the proton energies + int kpIndexCounter; //!< Kp index counter (betweeen 0 and 27) + double localTime; //!< spacecraft location time relative to sun heading at GEO + double logEnElec[MAX_NUM_ENERGIES]; //!< log of the electron energies + double logEnProt[MAX_NUM_ENERGIES]; //!< log of the proton energies double inputEnergies[MAX_NUM_ENERGIES]; //!< input energies considered in this module //!< 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, - 9.836741, 12.543499, 16.062061, 20.876962, - 27.183572, 35.843437, 47.179073, 61.424732, - 80.120170, 104.563461, 136.914871, 179.740982, - 235.406829, 309.020721, 405.806213, 532.664123, - 699.243896, 917.146484, 1205.174438, 1582.510986, - 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, - 32.053474, 41.142940, 53.239536, 68.940170, - 89.082473, 115.585487, 150.529022, 196.249755, - 256.610107, 335.709136, 439.549621, 574.766357, - 749.907531, 982.261108, 1278.967041, 1662.856079, - 2170.886474, 2829.989013, 3691.509765, 4822.499023, - 6300.260742, 8217.569335, 10726.390625, 14001.280273, - 18276.244140, 23856.085937, 31140.962890, 40649.562500}; + double enElec[40] = { 1.034126, 1.346516, 1.817463, 2.399564, 3.161048, 4.153217, + 5.539430, 7.464148, 9.836741, 12.543499, 16.062061, 20.876962, + 27.183572, 35.843437, 47.179073, 61.424732, 80.120170, 104.563461, + 136.914871, 179.740982, 235.406829, 309.020721, 405.806213, 532.664123, + 699.243896, 917.146484, 1205.174438, 1582.510986, 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, + 32.053474, 41.142940, 53.239536, 68.940170, 89.082473, 115.585487, + 150.529022, 196.249755, 256.610107, 335.709136, 439.549621, 574.766357, + 749.907531, 982.261108, 1278.967041, 1662.856079, 2170.886474, 2829.989013, + 3691.509765, 4822.499023, 6300.260742, 8217.569335, 10726.390625, 14001.280273, + 18276.244140, 23856.085937, 31140.962890, 40649.562500 }; }; #endif 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/_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/eclipse/eclipse.cpp b/src/simulation/environment/eclipse/eclipse.cpp old mode 100755 new mode 100644 index fc2dd921e0..720e179866 --- a/src/simulation/environment/eclipse/eclipse.cpp +++ b/src/simulation/environment/eclipse/eclipse.cpp @@ -22,7 +22,6 @@ #include "architecture/utilities/astroConstants.h" #include "architecture/utilities/avsEigenSupport.h" - Eclipse::Eclipse() { rEqCustom = -1.0; @@ -31,45 +30,47 @@ Eclipse::Eclipse() Eclipse::~Eclipse() { - for (long unsigned int c=0; ceclipseOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->eclipseOutMsgs.size(); c++) { delete this->eclipseOutMsgs.at(c); } return; } - - /*! Reset the module to origina configuration values. */ -void Eclipse::Reset(uint64_t CurrenSimNanos) +void +Eclipse::Reset(uint64_t CurrenSimNanos) { if (!this->sunInMsg.isLinked()) { bskLogger.bskLog(BSK_ERROR, "Eclipse: sunInMsg must be linked to sun Spice state message."); } if (this->positionInMsgs.size() == 0) { - bskLogger.bskLog(BSK_ERROR, "Eclipse: positionInMsgs is empty. Must use addSpacecraftToModel() to add at least one spacecraft."); + bskLogger.bskLog( + BSK_ERROR, + "Eclipse: positionInMsgs is empty. Must use addSpacecraftToModel() to add at least one spacecraft."); } if (this->planetInMsgs.size() == 0) { - bskLogger.bskLog(BSK_ERROR, "Eclipse: planetInMsgs is empty. Must use addPlanetToModel() to add at least one planet."); + bskLogger.bskLog(BSK_ERROR, + "Eclipse: planetInMsgs is empty. Must use addPlanetToModel() to add at least one planet."); } - } /*! This method reads the spacecraft state, spice planet states and the sun position from the messaging system. */ -void Eclipse::readInputMessages() +void +Eclipse::readInputMessages() { - for (long unsigned int c = 0; cpositionInMsgs.size(); c++){ + for (long unsigned int c = 0; c < this->positionInMsgs.size(); c++) { this->scStateBuffer.at(c) = this->positionInMsgs.at(c)(); } this->sunInMsgState = this->sunInMsg(); - for (long unsigned int c = 0; cplanetInMsgs.size(); c++){ + for (long unsigned int c = 0; c < this->planetInMsgs.size(); c++) { this->planetBuffer[c] = this->planetInMsgs[c](); } } @@ -79,7 +80,8 @@ void Eclipse::readInputMessages() @param CurrentClock The current simulation time (used for time stamping) */ -void Eclipse::writeOutputMessages(uint64_t CurrentClock) +void +Eclipse::writeOutputMessages(uint64_t CurrentClock) { for (long unsigned int c = 0; c < this->eclipseOutMsgs.size(); c++) { EclipseMsgPayload tmpEclipseMsg = {}; @@ -93,7 +95,8 @@ void Eclipse::writeOutputMessages(uint64_t CurrentClock) @param CurrentSimNanos The current clock time for the simulation */ -void Eclipse::UpdateState(uint64_t CurrentSimNanos) +void +Eclipse::UpdateState(uint64_t CurrentSimNanos) { this->readInputMessages(); @@ -102,22 +105,20 @@ void Eclipse::UpdateState(uint64_t CurrentSimNanos) // B: spacecraft body frame // N: inertial frame // H: sun (helio) frame - Eigen::Vector3d r_PN_N(0.0, 0.0, 0.0); // r_planet + Eigen::Vector3d r_PN_N(0.0, 0.0, 0.0); // r_planet Eigen::Vector3d r_HN_N(this->sunInMsgState.PositionVector); // r_sun - Eigen::Vector3d r_BN_N(0.0, 0.0, 0.0); // r_sc - Eigen::Vector3d s_BP_N(0.0, 0.0, 0.0); // s_sc wrt planet - Eigen::Vector3d s_HP_N(0.0, 0.0, 0.0); // s_sun wrt planet - Eigen::Vector3d r_HB_N(0.0, 0.0, 0.0); // r_sun wrt sc + Eigen::Vector3d r_BN_N(0.0, 0.0, 0.0); // r_sc + Eigen::Vector3d s_BP_N(0.0, 0.0, 0.0); // s_sc wrt planet + Eigen::Vector3d s_HP_N(0.0, 0.0, 0.0); // s_sun wrt planet + Eigen::Vector3d r_HB_N(0.0, 0.0, 0.0); // r_sun wrt sc std::vector::iterator scIt; std::vector::iterator planetIt; - // Index to assign the shadowFactor for each body position (S/C) // being tracked int scIdx = 0; - for(scIt = this->scStateBuffer.begin(); scIt != this->scStateBuffer.end(); scIt++) - { + for (scIt = this->scStateBuffer.begin(); scIt != this->scStateBuffer.end(); scIt++) { double tmpShadowFactor = 1.0; // 1.0 means 100% illumination (no eclipse) double eclipsePlanetDistance = 0.0; int64_t eclipsePlanetKey = -1; @@ -125,8 +126,7 @@ void Eclipse::UpdateState(uint64_t CurrentSimNanos) // Find the closest planet if there is one int idx = 0; - for(planetIt = this->planetBuffer.begin(); planetIt != this->planetBuffer.end(); planetIt++) - { + for (planetIt = this->planetBuffer.begin(); planetIt != this->planetBuffer.end(); planetIt++) { r_PN_N = cArray2EigenVector3d(planetIt->PositionVector); s_HP_N = r_HN_N - r_PN_N; r_HB_N = r_HN_N - r_BN_N; @@ -137,8 +137,7 @@ void Eclipse::UpdateState(uint64_t CurrentSimNanos) if (r_HB_N.norm() < s_HP_N.norm()) { idx++; continue; - } - else{ + } else { // Find the closest planet and save its distance and vector index slot if (s_BP_N.norm() < eclipsePlanetDistance || eclipsePlanetKey < 0) { eclipsePlanetDistance = s_BP_N.norm(); @@ -159,19 +158,19 @@ void Eclipse::UpdateState(uint64_t CurrentSimNanos) double s = s_BP_N.norm(); std::string plName(this->planetBuffer[eclipsePlanetKey].PlanetName); double planetRadius = this->getPlanetEquatorialRadius(plName); - double f_1 = safeAsin((REQ_SUN*1000 + planetRadius)/s_HP_N.norm()); - double f_2 = safeAsin((REQ_SUN*1000 - planetRadius)/s_HP_N.norm()); - double s_0 = (-s_BP_N.dot(s_HP_N))/s_HP_N.norm(); - double c_1 = s_0 + planetRadius/sin(f_1); - double c_2 = s_0 - planetRadius/sin(f_2); - double l = safeSqrt(s*s - s_0*s_0); - double l_1 = c_1*tan(f_1); - double l_2 = c_2*tan(f_2); + double f_1 = safeAsin((REQ_SUN * 1000 + planetRadius) / s_HP_N.norm()); + double f_2 = safeAsin((REQ_SUN * 1000 - planetRadius) / s_HP_N.norm()); + double s_0 = (-s_BP_N.dot(s_HP_N)) / s_HP_N.norm(); + double c_1 = s_0 + planetRadius / sin(f_1); + double c_2 = s_0 - planetRadius / sin(f_2); + double l = safeSqrt(s * s - s_0 * s_0); + double l_1 = c_1 * tan(f_1); + double l_2 = c_2 * tan(f_2); if (fabs(l) < fabs(l_2)) { if (c_2 < 0) { // total eclipse tmpShadowFactor = this->computePercentShadow(planetRadius, r_HB_N, s_BP_N); - } else { //c_2 > 0 // annular + } else { // c_2 > 0 // annular tmpShadowFactor = this->computePercentShadow(planetRadius, r_HB_N, s_BP_N); } } else if (fabs(l) < fabs(l_1)) { // partial @@ -190,15 +189,16 @@ void Eclipse::UpdateState(uint64_t CurrentSimNanos) @param s_BP_N @return double fractionShadow The eclipse shadow fraction. */ -double Eclipse::computePercentShadow(double planetRadius, Eigen::Vector3d r_HB_N, Eigen::Vector3d s_BP_N) +double +Eclipse::computePercentShadow(double planetRadius, Eigen::Vector3d r_HB_N, Eigen::Vector3d s_BP_N) { double area = 0.0; double shadowFraction = 1.0; // Initialise to value for no eclipse double normR_HB_N = r_HB_N.norm(); double normS_BP_N = s_BP_N.norm(); - double a = safeAsin(REQ_SUN*1000/normR_HB_N); // apparent radius of sun - double b = safeAsin(planetRadius/normS_BP_N); // apparent radius of occulting body - double c = safeAcos((-s_BP_N.dot(r_HB_N))/(normS_BP_N*normR_HB_N)); + double a = safeAsin(REQ_SUN * 1000 / normR_HB_N); // apparent radius of sun + double b = safeAsin(planetRadius / normS_BP_N); // apparent radius of occulting body + double c = safeAcos((-s_BP_N.dot(r_HB_N)) / (normS_BP_N * normR_HB_N)); // The order of these conditionals is important. // In particular (c < a + b) must check last to avoid testing @@ -206,15 +206,15 @@ double Eclipse::computePercentShadow(double planetRadius, Eigen::Vector3d r_HB_N if (c < b - a) { // total eclipse, implying a < b shadowFraction = 0.0; } else if (c < a - b) { // partial maximum eclipse, implying a > b - double areaSun = M_PI*a*a; - double areaBody = M_PI*b*b; + double areaSun = M_PI * a * a; + double areaBody = M_PI * b * b; area = areaSun - areaBody; - shadowFraction = 1 - area/(M_PI*a*a); + shadowFraction = 1 - area / (M_PI * a * a); } else if (c < a + b) { // partial eclipse - double x = (c*c + a*a - b*b)/(2*c); - double y = sqrt(a*a - x*x); - area = a*a*safeAcos(x/a) + b*b*safeAcos((c-x)/b) - c*y; - shadowFraction = 1 - area/(M_PI*a*a); + double x = (c * c + a * a - b * b) / (2 * c); + double y = sqrt(a * a - x * x); + area = a * a * safeAcos(x / a) + b * b * safeAcos((c - x) / b) - c * y; + shadowFraction = 1 - area / (M_PI * a * a); } return shadowFraction; } @@ -225,12 +225,13 @@ double Eclipse::computePercentShadow(double planetRadius, Eigen::Vector3d r_HB_N @param tmpScMsg The state output message for the spacecraft for which to compute the eclipse data. */ -void Eclipse::addSpacecraftToModel(Message *tmpScMsg) +void +Eclipse::addSpacecraftToModel(Message* tmpScMsg) { this->positionInMsgs.push_back(tmpScMsg->addSubscriber()); /* create output message */ - Message *msg; + Message* msg; msg = new Message; this->eclipseOutMsgs.push_back(msg); @@ -241,14 +242,14 @@ void Eclipse::addSpacecraftToModel(Message *tmpScMsg) // Now that we know the number of output messages we can size and zero // the eclipse data vector this->eclipseShadowFactors.resize(this->eclipseOutMsgs.size()); - } /*! This method adds planet state data message names to a vector. @param tmpSpMsg The planet name */ -void Eclipse::addPlanetToModel(Message *tmpSpMsg) +void +Eclipse::addPlanetToModel(Message* tmpSpMsg) { this->planetInMsgs.push_back(tmpSpMsg->addSubscriber()); @@ -261,28 +262,29 @@ void Eclipse::addPlanetToModel(Message *tmpSpMsg) @param planetSpiceName The planet name according to the spice NAIF Integer ID codes. @return double The equatorial radius in metres associated with the given planet name. */ -double Eclipse::getPlanetEquatorialRadius(std::string planetSpiceName) +double +Eclipse::getPlanetEquatorialRadius(std::string planetSpiceName) { if (planetSpiceName == "mercury") { - return REQ_MERCURY*1000.0; // [meters] + return REQ_MERCURY * 1000.0; // [meters] } else if (planetSpiceName == "venus") { - return REQ_VENUS*1000.0; + return REQ_VENUS * 1000.0; } else if (planetSpiceName == "earth") { - return REQ_EARTH*1000.0; + return REQ_EARTH * 1000.0; } else if (planetSpiceName == "moon") { - return REQ_MOON*1000.0; + return REQ_MOON * 1000.0; } else if (planetSpiceName == "mars barycenter") { - return REQ_MARS*1000.0; + return REQ_MARS * 1000.0; } else if (planetSpiceName == "mars") { - return REQ_MARS*1000.0; + return REQ_MARS * 1000.0; } else if (planetSpiceName == "jupiter barycenter") { - return REQ_JUPITER*1000.0; + return REQ_JUPITER * 1000.0; } else if (planetSpiceName == "saturn") { - return REQ_SATURN*1000.0; + return REQ_SATURN * 1000.0; } else if (planetSpiceName == "uranus") { - return REQ_URANUS*1000.0; + return REQ_URANUS * 1000.0; } else if (planetSpiceName == "neptune") { - return REQ_NEPTUNE*1000.0; + return REQ_NEPTUNE * 1000.0; } else if (planetSpiceName == "custom") { if (rEqCustom <= 0.0) { bskLogger.bskLog(BSK_ERROR, "Eclipse: Invalid rEqCustom set."); diff --git a/src/simulation/environment/eclipse/eclipse.h b/src/simulation/environment/eclipse/eclipse.h old mode 100755 new mode 100644 index f436baccb1..647d75fa1e --- a/src/simulation/environment/eclipse/eclipse.h +++ b/src/simulation/environment/eclipse/eclipse.h @@ -32,40 +32,42 @@ #include "architecture/utilities/linearAlgebra.h" #include "architecture/utilities/bskLogging.h" - /*! @brief eclipse model class */ -class Eclipse: public SysModel { -public: +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 - std::vector> positionInMsgs; //!< vector of msgs for each spacecraft position state for which to evaluate eclipse conditions. - std::vector*> eclipseOutMsgs;//!< vector of eclispe output msg names - BSKLogger bskLogger; //!< BSK Logging - double rEqCustom; //!< [m] Custom radius + 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 + std::vector> + positionInMsgs; //!< vector of msgs for each spacecraft position state for which to evaluate eclipse conditions. + std::vector*> eclipseOutMsgs; //!< vector of eclispe output msg names + BSKLogger bskLogger; //!< BSK Logging + double rEqCustom; //!< [m] Custom radius -private: - std::vector planetRadii; //!< [m] A vector of planet radii ordered by the sequence in which planet names are added to the module - std::vector scStateBuffer; //!< buffer of the spacecraft state input messages - std::vector planetBuffer; //!< buffer of the spacecraft state input messages - SpicePlanetStateMsgPayload sunInMsgState; //!< copy of sun input msg - std::vector eclipseShadowFactors; //!< vector of shadow factor output values + private: + std::vector planetRadii; //!< [m] A vector of planet radii ordered by the sequence in which planet names are + //!< added to the module + std::vector scStateBuffer; //!< buffer of the spacecraft state input messages + std::vector planetBuffer; //!< buffer of the spacecraft state input messages + SpicePlanetStateMsgPayload sunInMsgState; //!< copy of sun input msg + std::vector eclipseShadowFactors; //!< vector of shadow factor output values -private: + private: void readInputMessages(); double computePercentShadow(double planetRadius, Eigen::Vector3d r_HB_N, Eigen::Vector3d s_BP_N); double getPlanetEquatorialRadius(std::string planetSpiceName); - }; - #endif 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/_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/ephemerisConverter/ephemerisConverter.cpp b/src/simulation/environment/ephemerisConverter/ephemerisConverter.cpp old mode 100755 new mode 100644 index e8bd201080..14fc02d4e1 --- a/src/simulation/environment/ephemerisConverter/ephemerisConverter.cpp +++ b/src/simulation/environment/ephemerisConverter/ephemerisConverter.cpp @@ -21,13 +21,11 @@ #include "architecture/utilities/avsEigenSupport.h" #include "architecture/utilities/macroDefinitions.h" -EphemerisConverter::EphemerisConverter() -{ -} +EphemerisConverter::EphemerisConverter() {} EphemerisConverter::~EphemerisConverter() { - for (long unsigned int c=0; cephemOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->ephemOutMsgs.size(); c++) { free(this->ephemOutMsgs.at(c)); } } @@ -35,7 +33,8 @@ EphemerisConverter::~EphemerisConverter() /*! Reset the module to origina configuration values. */ -void EphemerisConverter::Reset(uint64_t CurrenSimNanos) +void +EphemerisConverter::Reset(uint64_t CurrenSimNanos) { // check if the spiceInMsgs is empty or not if (this->spiceInMsgs.size() == 0) { @@ -46,16 +45,16 @@ void EphemerisConverter::Reset(uint64_t CurrenSimNanos) /*! add a planet spice input message */ -void EphemerisConverter::addSpiceInputMsg(Message *tmpMsg) +void +EphemerisConverter::addSpiceInputMsg(Message* tmpMsg) { this->spiceInMsgs.push_back(tmpMsg->addSubscriber()); /* setup output corresponding message */ - Message *msg; + Message* msg; msg = new Message; this->ephemOutMsgs.push_back(msg); - /* update input and output buffers*/ SpicePlanetStateMsgPayload tmpSpice = {}; this->spiceInBuffers.push_back(tmpSpice); @@ -68,7 +67,8 @@ void EphemerisConverter::addSpiceInputMsg(Message *t convert ephemeris data @param clockNow */ -void EphemerisConverter::convertEphemData(uint64_t clockNow) +void +EphemerisConverter::convertEphemData(uint64_t clockNow) { Eigen::Matrix3d dcm_BN; Eigen::Vector3d sigma_BN; @@ -77,23 +77,21 @@ void EphemerisConverter::convertEphemData(uint64_t clockNow) double omega_tilde_BN_B[3][3]; double omega_tilde_BN_B_array[9]; - for (long unsigned int c=0; c < this->spiceInMsgs.size(); c++) { - v3Copy(this->spiceInBuffers.at(c).PositionVector, - this->ephemOutBuffers.at(c).r_BdyZero_N); + for (long unsigned int c = 0; c < this->spiceInMsgs.size(); c++) { + v3Copy(this->spiceInBuffers.at(c).PositionVector, this->ephemOutBuffers.at(c).r_BdyZero_N); - v3Copy(this->spiceInBuffers.at(c).VelocityVector, - this->ephemOutBuffers.at(c).v_BdyZero_N); + v3Copy(this->spiceInBuffers.at(c).VelocityVector, this->ephemOutBuffers.at(c).v_BdyZero_N); - this->ephemOutBuffers.at(c).timeTag = this->spiceInMsgs.at(c).timeWritten()*1.0E-9; + this->ephemOutBuffers.at(c).timeTag = this->spiceInMsgs.at(c).timeWritten() * 1.0E-9; /* Compute sigma_BN */ dcm_BN = cArray2EigenMatrix3d(*this->spiceInBuffers.at(c).J20002Pfix); sigma_BN = eigenMRPd2Vector3d(eigenC2MRP(dcm_BN)); - eigenVector3d2CArray(sigma_BN, this->ephemOutBuffers.at(c).sigma_BN); //sigma_BN + eigenVector3d2CArray(sigma_BN, this->ephemOutBuffers.at(c).sigma_BN); // sigma_BN /* Compute omega_BN_B */ dcm_BN_dot = cArray2EigenMatrix3d(*this->spiceInBuffers.at(c).J20002Pfix_dot); - omega_tilde_BN_B_eigen = -dcm_BN_dot*dcm_BN.transpose(); + omega_tilde_BN_B_eigen = -dcm_BN_dot * dcm_BN.transpose(); eigenMatrix3d2CArray(omega_tilde_BN_B_eigen, omega_tilde_BN_B_array); m33Copy(RECAST3X3 omega_tilde_BN_B_array, omega_tilde_BN_B); this->ephemOutBuffers.at(c).omega_BN_B[0] = omega_tilde_BN_B[2][1]; @@ -102,9 +100,10 @@ void EphemerisConverter::convertEphemData(uint64_t clockNow) } } -void EphemerisConverter::readInputMessages() +void +EphemerisConverter::readInputMessages() { - for (long unsigned int c=0; c < this->spiceInMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->spiceInMsgs.size(); c++) { this->spiceInBuffers.at(c) = this->spiceInMsgs.at(c)(); } } @@ -113,19 +112,20 @@ void EphemerisConverter::readInputMessages() write output message @param CurrentSimNanos time in nano-seconds */ -void EphemerisConverter::writeOutputMessages(uint64_t CurrentSimNanos) +void +EphemerisConverter::writeOutputMessages(uint64_t CurrentSimNanos) { - for (long unsigned int c=0; c < this->ephemOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->ephemOutMsgs.size(); c++) { this->ephemOutMsgs.at(c)->write(&this->ephemOutBuffers.at(c), this->moduleID, CurrentSimNanos); } } - /*! update module states @param CurrentSimNanos time in nano-seconds */ -void EphemerisConverter::UpdateState(uint64_t CurrentSimNanos) +void +EphemerisConverter::UpdateState(uint64_t CurrentSimNanos) { readInputMessages(); convertEphemData(CurrentSimNanos); diff --git a/src/simulation/environment/ephemerisConverter/ephemerisConverter.h b/src/simulation/environment/ephemerisConverter/ephemerisConverter.h old mode 100755 new mode 100644 index 06c05eb6c4..08f467dbb5 --- a/src/simulation/environment/ephemerisConverter/ephemerisConverter.h +++ b/src/simulation/environment/ephemerisConverter/ephemerisConverter.h @@ -29,29 +29,28 @@ #include "architecture/utilities/bskLogging.h" - /*! @brief ephemeric converter class */ -class EphemerisConverter: public SysModel { -public: +class EphemerisConverter : public SysModel +{ + public: EphemerisConverter(); ~EphemerisConverter(); - + void UpdateState(uint64_t CurrentSimNanos); void Reset(uint64_t CurrentSimNanos); - void readInputMessages(); //!< class method + void readInputMessages(); //!< class method void convertEphemData(uint64_t clockNow); void writeOutputMessages(uint64_t Clock); - void addSpiceInputMsg(Message *msg); + void addSpiceInputMsg(Message* msg); -public: - std::vector*> ephemOutMsgs; //!< vector of planet ephemeris output messages - std::vector> spiceInMsgs; //!< vector of planet spice state input messages + public: + std::vector*> ephemOutMsgs; //!< vector of planet ephemeris output messages + std::vector> spiceInMsgs; //!< vector of planet spice state input messages - BSKLogger bskLogger; //!< -- BSK Logging -private: + BSKLogger bskLogger; //!< -- BSK Logging + private: std::vector ephemOutBuffers; //!< output message buffers std::vector spiceInBuffers; //!< spice input message copies }; - #endif 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/groundLocation/groundLocation.cpp b/src/simulation/environment/groundLocation/groundLocation.cpp index d5d46dad9b..08273ab1fb 100644 --- a/src/simulation/environment/groundLocation/groundLocation.cpp +++ b/src/simulation/environment/groundLocation/groundLocation.cpp @@ -28,12 +28,13 @@ GroundLocation::GroundLocation() { //! - Set some default initial conditions: - this->minimumElevation = 10.*D2R; // [rad] minimum elevation above the local horizon needed to see a spacecraft; defaults to 10 degrees + this->minimumElevation = + 10. * D2R; // [rad] minimum elevation above the local horizon needed to see a spacecraft; defaults to 10 degrees this->maximumRange = -1; // [m] Maximum range for the groundLocation to compute access. this->currentGroundStateBuffer = this->currentGroundStateOutMsg.zeroMsgPayload; - this->planetRadius = REQ_EARTH*1e3; + this->planetRadius = REQ_EARTH * 1e3; this->r_LP_P.fill(0.0); this->r_LP_P_Init.fill(0.0); @@ -51,14 +52,15 @@ GroundLocation::GroundLocation() */ GroundLocation::~GroundLocation() { - for (long unsigned int c=0; caccessOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->accessOutMsgs.size(); c++) { delete this->accessOutMsgs.at(c); } return; } /*! Resets the internal position to the specified initial position.*/ -void GroundLocation::Reset(uint64_t CurrentSimNanos) +void +GroundLocation::Reset(uint64_t CurrentSimNanos) { this->r_LP_P = this->r_LP_P_Init; @@ -73,7 +75,8 @@ void GroundLocation::Reset(uint64_t CurrentSimNanos) * @param longitude * @param alt */ -void GroundLocation::specifyLocation(double lat, double longitude, double alt) +void +GroundLocation::specifyLocation(double lat, double longitude, double alt) { Eigen::Vector3d tmpLLAPosition(lat, longitude, alt); this->r_LP_P_Init = LLA2PCPF(tmpLLAPosition, this->planetRadius); @@ -83,7 +86,9 @@ void GroundLocation::specifyLocation(double lat, double longitude, double alt) /*! Specifies the ground location from planet-centered, planet-fixed coordinates * @param r_LP_P_Loc */ -void GroundLocation::specifyLocationPCPF(Eigen::Vector3d& r_LP_P_Loc){ +void +GroundLocation::specifyLocationPCPF(Eigen::Vector3d& r_LP_P_Loc) +{ /* Assign to r_LP_P_Init */ this->r_LP_P_Init = r_LP_P_Loc; @@ -94,15 +99,16 @@ void GroundLocation::specifyLocationPCPF(Eigen::Vector3d& r_LP_P_Loc){ this->dcm_LP = C_PCPF2SEZ(tmpLLAPosition[0], tmpLLAPosition[1]); } - -/*! Adds a scState message name to the vector of names to be subscribed to. Also creates a corresponding access message output name. -*/ -void GroundLocation::addSpacecraftToModel(Message *tmpScMsg) +/*! Adds a scState message name to the vector of names to be subscribed to. Also creates a corresponding access message + * output name. + */ +void +GroundLocation::addSpacecraftToModel(Message* tmpScMsg) { this->scStateInMsgs.push_back(tmpScMsg->addSubscriber()); /* create output message */ - Message *msg; + Message* msg; msg = new Message; this->accessOutMsgs.push_back(msg); @@ -111,10 +117,10 @@ void GroundLocation::addSpacecraftToModel(Message *tmpScMsg) this->accessMsgBuffer.push_back(accMsg); } - /*! Read module messages -*/ -bool GroundLocation::ReadMessages() + */ +bool +GroundLocation::ReadMessages() { SCStatesMsgPayload scMsg; @@ -123,8 +129,7 @@ bool GroundLocation::ReadMessages() //! - read in the spacecraft state messages bool scRead; - if(!this->scStateInMsgs.empty()) - { + if (!this->scStateInMsgs.empty()) { scRead = true; for (long unsigned int c = 0; c < this->scStateInMsgs.size(); c++) { scMsg = this->scStateInMsgs.at(c)(); @@ -135,29 +140,31 @@ bool GroundLocation::ReadMessages() bskLogger.bskLog(BSK_ERROR, "Ground location has no spacecraft to track."); scRead = false; } - //! - Read in the optional planet message. if no planet message is set, then a zero planet position, velocity and orientation is assumed + //! - Read in the optional planet message. if no planet message is set, then a zero planet position, velocity and + //! orientation is assumed bool planetRead = true; - if(this->planetInMsg.isLinked()) - { + if (this->planetInMsg.isLinked()) { planetRead = this->planetInMsg.isWritten(); this->planetState = this->planetInMsg(); } - return(planetRead && scRead); + return (planetRead && scRead); } /*! write module messages -*/ -void GroundLocation::WriteMessages(uint64_t CurrentClock) + */ +void +GroundLocation::WriteMessages(uint64_t CurrentClock) { //! - write access message for each spacecraft - for (long unsigned int c=0; c< this->accessMsgBuffer.size(); c++) { + for (long unsigned int c = 0; c < this->accessMsgBuffer.size(); c++) { this->accessOutMsgs.at(c)->write(&this->accessMsgBuffer.at(c), this->moduleID, CurrentClock); } this->currentGroundStateOutMsg.write(&this->currentGroundStateBuffer, this->moduleID, CurrentClock); } -void GroundLocation::updateInertialPositions() +void +GroundLocation::updateInertialPositions() { // First, get the rotation matrix from the inertial to planet frame from SPICE: this->dcm_PN = cArray2EigenMatrix3d(*this->planetState.J20002Pfix); @@ -165,17 +172,18 @@ void GroundLocation::updateInertialPositions() this->r_PN_N = cArray2EigenVector3d(this->planetState.PositionVector); // Then, transpose it to get the planet to inertial frame this->r_LP_N = this->dcm_PN.transpose() * this->r_LP_P_Init; - this->rhat_LP_N = this->r_LP_N/this->r_LP_N.norm(); + this->rhat_LP_N = this->r_LP_N / this->r_LP_N.norm(); this->r_LN_N = this->r_PN_N + this->r_LP_N; // Get planet frame angular velocity vector - Eigen::Matrix3d w_tilde_PN = - this->dcm_PN_dot * this->dcm_PN.transpose(); - this->w_PN << w_tilde_PN(2,1), w_tilde_PN(0,2), w_tilde_PN(1,0); + Eigen::Matrix3d w_tilde_PN = -this->dcm_PN_dot * this->dcm_PN.transpose(); + this->w_PN << w_tilde_PN(2, 1), w_tilde_PN(0, 2), w_tilde_PN(1, 0); // Stash updated position in the groundState message eigenVector3d2CArray(this->r_LN_N, this->currentGroundStateBuffer.r_LN_N); eigenVector3d2CArray(this->r_LP_N, this->currentGroundStateBuffer.r_LP_N); } -void GroundLocation::computeAccess() +void +GroundLocation::computeAccess() { // Update the groundLocation's inertial position this->updateInertialPositions(); @@ -183,35 +191,40 @@ void GroundLocation::computeAccess() // Iterate over spacecraft position messages and compute the access for each one std::vector::iterator accessMsgIt; std::vector::iterator scStatesMsgIt; - for(scStatesMsgIt = this->scStatesBuffer.begin(), accessMsgIt = accessMsgBuffer.begin(); scStatesMsgIt != scStatesBuffer.end(); scStatesMsgIt++, accessMsgIt++){ + for (scStatesMsgIt = this->scStatesBuffer.begin(), accessMsgIt = accessMsgBuffer.begin(); + scStatesMsgIt != scStatesBuffer.end(); + scStatesMsgIt++, accessMsgIt++) { //! Compute the relative position of each spacecraft to the site in the planet-centered inertial frame Eigen::Vector3d r_BP_N = cArray2EigenVector3d(scStatesMsgIt->r_BN_N) - this->r_PN_N; Eigen::Vector3d r_BL_N = r_BP_N - this->r_LP_N; auto r_BL_mag = r_BL_N.norm(); Eigen::Vector3d relativeHeading_N = r_BL_N / r_BL_mag; - double viewAngle = (M_PI_2-safeAcos(this->rhat_LP_N.dot(relativeHeading_N))); + double viewAngle = (M_PI_2 - safeAcos(this->rhat_LP_N.dot(relativeHeading_N))); accessMsgIt->slantRange = r_BL_mag; accessMsgIt->elevation = viewAngle; Eigen::Vector3d r_BL_L = this->dcm_LP * this->dcm_PN * r_BL_N; eigenVector3d2CArray(r_BL_L, accessMsgIt->r_BL_L); - double cos_az = -r_BL_L[0]/(sqrt(pow(r_BL_L[0],2) + pow(r_BL_L[1],2))); - double sin_az = r_BL_L[1]/(sqrt(pow(r_BL_L[0],2) + pow(r_BL_L[1],2))); + double cos_az = -r_BL_L[0] / (sqrt(pow(r_BL_L[0], 2) + pow(r_BL_L[1], 2))); + double sin_az = r_BL_L[1] / (sqrt(pow(r_BL_L[0], 2) + pow(r_BL_L[1], 2))); accessMsgIt->azimuth = atan2(sin_az, cos_az); - Eigen::Vector3d v_BL_L = this->dcm_LP * this->dcm_PN * (cArray2EigenVector3d(scStatesMsgIt->v_BN_N) - this->w_PN.cross(r_BP_N)); // V observed from gL wrt P frame, expressed in L frame coords (SEZ) + Eigen::Vector3d v_BL_L = + this->dcm_LP * this->dcm_PN * + (cArray2EigenVector3d(scStatesMsgIt->v_BN_N) - + this->w_PN.cross(r_BP_N)); // V observed from gL wrt P frame, expressed in L frame coords (SEZ) eigenVector3d2CArray(v_BL_L, accessMsgIt->v_BL_L); - accessMsgIt->range_dot = v_BL_L.dot(r_BL_L)/r_BL_mag; - double xy_norm = sqrt(pow(r_BL_L[0],2)+pow(r_BL_L[1],2)); - accessMsgIt->az_dot = (-r_BL_L[0]*v_BL_L[1] + r_BL_L[1]*v_BL_L[0])/pow(xy_norm,2); - accessMsgIt->el_dot = (v_BL_L[2]/xy_norm - r_BL_L[2]*(r_BL_L[0]*v_BL_L[0] + r_BL_L[1]*v_BL_L[1])/pow(xy_norm,3))/(1+pow(r_BL_L[2]/xy_norm,2)); - - if( (viewAngle > this->minimumElevation) && (r_BL_mag <= this->maximumRange || this->maximumRange < 0)){ + accessMsgIt->range_dot = v_BL_L.dot(r_BL_L) / r_BL_mag; + double xy_norm = sqrt(pow(r_BL_L[0], 2) + pow(r_BL_L[1], 2)); + accessMsgIt->az_dot = (-r_BL_L[0] * v_BL_L[1] + r_BL_L[1] * v_BL_L[0]) / pow(xy_norm, 2); + accessMsgIt->el_dot = + (v_BL_L[2] / xy_norm - r_BL_L[2] * (r_BL_L[0] * v_BL_L[0] + r_BL_L[1] * v_BL_L[1]) / pow(xy_norm, 3)) / + (1 + pow(r_BL_L[2] / xy_norm, 2)); + + if ((viewAngle > this->minimumElevation) && (r_BL_mag <= this->maximumRange || this->maximumRange < 0)) { accessMsgIt->hasAccess = 1; - } - else - { + } else { accessMsgIt->hasAccess = 0; } } @@ -221,10 +234,10 @@ void GroundLocation::computeAccess() update module @param CurrentSimNanos */ -void GroundLocation::UpdateState(uint64_t CurrentSimNanos) +void +GroundLocation::UpdateState(uint64_t CurrentSimNanos) { this->ReadMessages(); this->computeAccess(); this->WriteMessages(CurrentSimNanos); - } diff --git a/src/simulation/environment/groundLocation/groundLocation.h b/src/simulation/environment/groundLocation/groundLocation.h index 0a013ef93e..2c2e3119b4 100644 --- a/src/simulation/environment/groundLocation/groundLocation.h +++ b/src/simulation/environment/groundLocation/groundLocation.h @@ -17,7 +17,6 @@ */ - #ifndef GROUND_LOCATION_H #define GROUND_LOCATION_H @@ -37,51 +36,59 @@ #include "architecture/utilities/bskLogging.h" /*! @brief ground location class */ -class GroundLocation: public SysModel { -public: +class GroundLocation : public SysModel +{ + public: GroundLocation(); ~GroundLocation(); void UpdateState(uint64_t CurrentSimNanos); void Reset(uint64_t CurrentSimNanos); bool ReadMessages(); void WriteMessages(uint64_t CurrentClock); - void addSpacecraftToModel(Message *tmpScMsg); + void addSpacecraftToModel(Message* tmpScMsg); void specifyLocation(double lat, double longitude, double alt); void specifyLocationPCPF(Eigen::Vector3d& r_LP_P_Loc); - -private: + + private: void updateInertialPositions(); void computeAccess(); -public: - double planetRadius; //!< [m] Planet radius in meters. - double minimumElevation; //!< [rad] minimum elevation above the local horizon needed to see a spacecraft; defaults to 10 degrees equivalent. - double maximumRange; //!< [m] (optional) Maximum slant range to compute access for; defaults to -1, which represents no maximum range. + public: + double planetRadius; //!< [m] Planet radius in meters. + double minimumElevation; //!< [rad] minimum elevation above the local horizon needed to see a spacecraft; defaults + //!< to 10 degrees equivalent. + double maximumRange; //!< [m] (optional) Maximum slant range to compute access for; defaults to -1, which represents + //!< no maximum range. - ReadFunctor planetInMsg; //!< planet state input message + ReadFunctor planetInMsg; //!< planet state input message Message currentGroundStateOutMsg; //!< ground location output message - std::vector*> accessOutMsgs; //!< vector of ground location access messages + std::vector*> accessOutMsgs; //!< vector of ground location access messages std::vector> scStateInMsgs; //!< vector of sc state input messages - Eigen::Vector3d r_LP_P_Init; //!< [m] Initial position of the location in planet-centric coordinates; can also be set using setGroundLocation. + Eigen::Vector3d r_LP_P_Init; //!< [m] Initial position of the location in planet-centric coordinates; can also be + //!< set using setGroundLocation. BSKLogger bskLogger; //!< -- BSK Logging -private: - std::vector accessMsgBuffer; //!< buffer of access output data - std::vector scStatesBuffer; //!< buffer of spacecraft states - SpicePlanetStateMsgPayload planetState; //!< buffer of planet data - GroundStateMsgPayload currentGroundStateBuffer; //!< buffer of ground station output data + private: + std::vector accessMsgBuffer; //!< buffer of access output data + std::vector scStatesBuffer; //!< buffer of spacecraft states + SpicePlanetStateMsgPayload planetState; //!< buffer of planet data + GroundStateMsgPayload currentGroundStateBuffer; //!< buffer of ground station output data - Eigen::Matrix3d dcm_LP; //!< Rotation matrix from planet-centered, planet-fixed frame P to site-local topographic (SEZ) frame L coordinates. + Eigen::Matrix3d dcm_LP; //!< Rotation matrix from planet-centered, planet-fixed frame P to site-local topographic + //!< (SEZ) frame L coordinates. Eigen::Matrix3d dcm_PN; //!< Rotation matrix from inertial frame N to planet-centered to planet-fixed frame P. - Eigen::Matrix3d dcm_PN_dot; //!< Rotation matrix derivative from inertial frame N to planet-centered to planet-fixed frame P. - Eigen::Vector3d w_PN; //!< [rad/s] Angular velocity of planet-fixed frame P relative to inertial frame N. + Eigen::Matrix3d + dcm_PN_dot; //!< Rotation matrix derivative from inertial frame N to planet-centered to planet-fixed frame P. + Eigen::Vector3d w_PN; //!< [rad/s] Angular velocity of planet-fixed frame P relative to inertial frame N. Eigen::Vector3d r_PN_N; //!< [m] Planet position vector relative to inertial frame origin. - Eigen::Vector3d r_LP_P; //!< [m] Ground Location position vector relative to to planet origin vector in planet frame coordinates. - Eigen::Vector3d r_LP_N; //!< [m] Gound Location position vector relative to planet origin vector in inertial coordinates. - Eigen::Vector3d rhat_LP_N;//!< [-] Surface normal vector from the target location in inertial coordinates. - Eigen::Vector3d r_LN_N; //!< [m] Ground Location position vector relative to inertial frame origin in inertial coordinates. + Eigen::Vector3d + r_LP_P; //!< [m] Ground Location position vector relative to to planet origin vector in planet frame coordinates. + Eigen::Vector3d + r_LP_N; //!< [m] Gound Location position vector relative to planet origin vector in inertial coordinates. + Eigen::Vector3d rhat_LP_N; //!< [-] Surface normal vector from the target location in inertial coordinates. + Eigen::Vector3d + r_LN_N; //!< [m] Ground Location position vector relative to inertial frame origin in inertial coordinates. Eigen::Vector3d r_North_N; //!<[-] Inertial 3rd axis, defined internally as "North". }; - #endif /* GroundLocation */ 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/groundMapping/groundMapping.cpp b/src/simulation/environment/groundMapping/groundMapping.cpp index 5525e01ad9..884015fb63 100644 --- a/src/simulation/environment/groundMapping/groundMapping.cpp +++ b/src/simulation/environment/groundMapping/groundMapping.cpp @@ -17,7 +17,6 @@ */ - #include "simulation/environment//groundMapping/groundMapping.h" #include "architecture/utilities/avsEigenSupport.h" #include "architecture/utilities/linearAlgebra.h" @@ -30,11 +29,12 @@ GroundMapping::GroundMapping() { //! - Set some default initial conditions: - this->minimumElevation = 0.*D2R; // [rad] minimum elevation above the local horizon needed to see a spacecraft; defaults to 0 degrees - this->maximumRange = -1; // [m] Maximum range for the groundLocation to compute access. - this->halfFieldOfView = 10.*D2R; // [rad] half-angle field-of-view of the instrument - this->cameraPos_B.setZero(3); // Default to zero - this->nHat_B.setZero(3); // Default to zero + this->minimumElevation = + 0. * D2R; // [rad] minimum elevation above the local horizon needed to see a spacecraft; defaults to 0 degrees + this->maximumRange = -1; // [m] Maximum range for the groundLocation to compute access. + this->halfFieldOfView = 10. * D2R; // [rad] half-angle field-of-view of the instrument + this->cameraPos_B.setZero(3); // Default to zero + this->nHat_B.setZero(3); // Default to zero this->planetInMsgBuffer = this->planetInMsg.zeroMsgPayload; this->planetInMsgBuffer.J20002Pfix[0][0] = 1; @@ -56,7 +56,8 @@ GroundMapping::~GroundMapping() /*! This method is used to reset the module and checks that required input messages are connect. */ -void GroundMapping::Reset(uint64_t CurrentSimNanos) +void +GroundMapping::Reset(uint64_t CurrentSimNanos) { // check that required input messages are connected if (!this->scStateInMsg.isLinked()) { @@ -64,17 +65,18 @@ void GroundMapping::Reset(uint64_t CurrentSimNanos) } // check that the direction of the camera is provided - if (this->nHat_B.isZero()){ + if (this->nHat_B.isZero()) { bskLogger.bskLog(BSK_ERROR, "GroundMapping.nHat_B vector not set."); } } /*! Read module messages -*/ -void GroundMapping::ReadMessages(){ + */ +void +GroundMapping::ReadMessages() +{ - if(this->planetInMsg.isLinked()) - { + if (this->planetInMsg.isLinked()) { this->planetInMsgBuffer = this->planetInMsg(); } @@ -84,12 +86,14 @@ void GroundMapping::ReadMessages(){ /*! Method to add map points * @param r_LP_P_init: mapping point in planet-fixed frame */ -void GroundMapping::addPointToModel(Eigen::Vector3d& r_LP_P_init){ +void +GroundMapping::addPointToModel(Eigen::Vector3d& r_LP_P_init) +{ /* Add the mapping point */ this->mappingPoints.push_back(r_LP_P_init); /* Create buffer output messages */ - Message *msg; + Message* msg; msg = new Message; this->accessOutMsgs.push_back(msg); @@ -98,7 +102,7 @@ void GroundMapping::addPointToModel(Eigen::Vector3d& r_LP_P_init){ this->accessMsgBuffer.push_back(accMsg); /* Create ground state output message */ - Message *msg_2; + Message* msg_2; msg_2 = new Message; this->currentGroundStateOutMsgs.push_back(msg_2); @@ -110,14 +114,16 @@ void GroundMapping::addPointToModel(Eigen::Vector3d& r_LP_P_init){ /*! Method to compute whether or not the spacecraft has access to the location * @param c: index of the given location */ -void GroundMapping::computeAccess(uint64_t c){ +void +GroundMapping::computeAccess(uint64_t c) +{ //! Zero the output message buffers this->accessMsgBuffer.at(c) = this->accessOutMsgs.at(c)->zeroMsgPayload; this->currentGroundStateMsgBuffer.at(c) = this->currentGroundStateOutMsgs.at(c)->zeroMsgPayload; //! Compute the planet to inertial frame location position this->r_LP_N = this->dcm_PN.transpose() * this->mappingPoints[c]; - this->rhat_LP_N = this->r_LP_N/this->r_LP_N.norm(); + this->rhat_LP_N = this->r_LP_N / this->r_LP_N.norm(); this->r_LN_N = this->r_PN_N + this->r_LP_N; //! Stash updated position in the groundState message @@ -129,38 +135,42 @@ void GroundMapping::computeAccess(uint64_t c){ auto r_BL_mag = r_BL_N.norm(); Eigen::Vector3d relativeHeading_N = r_BL_N / r_BL_mag; - double viewAngle = (M_PI_2-safeAcos(this->rhat_LP_N.dot(relativeHeading_N))); + double viewAngle = (M_PI_2 - safeAcos(this->rhat_LP_N.dot(relativeHeading_N))); this->accessMsgBuffer.at(c).slantRange = r_BL_mag; this->accessMsgBuffer.at(c).elevation = viewAngle; Eigen::Vector3d r_BL_L = this->dcm_LP * this->dcm_PN * r_BL_N; eigenVector3d2CArray(r_BL_L, this->accessMsgBuffer.at(c).r_BL_L); - double cos_az = -r_BL_L[0]/(sqrt(pow(r_BL_L[0],2) + pow(r_BL_L[1],2))); - double sin_az = r_BL_L[1]/(sqrt(pow(r_BL_L[0],2) + pow(r_BL_L[1],2))); + double cos_az = -r_BL_L[0] / (sqrt(pow(r_BL_L[0], 2) + pow(r_BL_L[1], 2))); + double sin_az = r_BL_L[1] / (sqrt(pow(r_BL_L[0], 2) + pow(r_BL_L[1], 2))); this->accessMsgBuffer.at(c).azimuth = atan2(sin_az, cos_az); - Eigen::Vector3d v_BL_L = this->dcm_LP * this->dcm_PN * (cArray2EigenVector3d(scStateInMsgBuffer.v_BN_N) - this->w_PN.cross(r_BP_N)); // V observed from gL wrt P frame, expressed in L frame coords (SEZ) + Eigen::Vector3d v_BL_L = + this->dcm_LP * this->dcm_PN * + (cArray2EigenVector3d(scStateInMsgBuffer.v_BN_N) - + this->w_PN.cross(r_BP_N)); // V observed from gL wrt P frame, expressed in L frame coords (SEZ) eigenVector3d2CArray(v_BL_L, this->accessMsgBuffer.at(c).v_BL_L); - this->accessMsgBuffer.at(c).range_dot = v_BL_L.dot(r_BL_L)/r_BL_mag; - double xy_norm = sqrt(pow(r_BL_L[0],2)+pow(r_BL_L[1],2)); - this->accessMsgBuffer.at(c).az_dot = (-r_BL_L[0]*v_BL_L[1] + r_BL_L[1]*v_BL_L[0])/pow(xy_norm,2); - this->accessMsgBuffer.at(c).el_dot = (v_BL_L[2]/xy_norm - r_BL_L[2]*(r_BL_L[0]*v_BL_L[0] + r_BL_L[1]*v_BL_L[1])/pow(xy_norm,3))/(1+pow(r_BL_L[2]/xy_norm,2)); + this->accessMsgBuffer.at(c).range_dot = v_BL_L.dot(r_BL_L) / r_BL_mag; + double xy_norm = sqrt(pow(r_BL_L[0], 2) + pow(r_BL_L[1], 2)); + this->accessMsgBuffer.at(c).az_dot = (-r_BL_L[0] * v_BL_L[1] + r_BL_L[1] * v_BL_L[0]) / pow(xy_norm, 2); + this->accessMsgBuffer.at(c).el_dot = + (v_BL_L[2] / xy_norm - r_BL_L[2] * (r_BL_L[0] * v_BL_L[0] + r_BL_L[1] * v_BL_L[1]) / pow(xy_norm, 3)) / + (1 + pow(r_BL_L[2] / xy_norm, 2)); uint64_t within_view = this->checkInstrumentFOV(); - if( (viewAngle > this->minimumElevation) && (r_BL_mag <= this->maximumRange || this->maximumRange < 0) && within_view){ + if ((viewAngle > this->minimumElevation) && (r_BL_mag <= this->maximumRange || this->maximumRange < 0) && + within_view) { this->accessMsgBuffer.at(c).hasAccess = 1; - } - else - { + } else { this->accessMsgBuffer.at(c).hasAccess = 0; } - } /*! Method to update the relevant rotations matrices */ -void GroundMapping::updateInertialPositions() +void +GroundMapping::updateInertialPositions() { // First, get the rotation matrix from the inertial to planet frame from SPICE: this->dcm_PN = cArray2EigenMatrix3d(*this->planetInMsgBuffer.J20002Pfix); @@ -174,21 +184,26 @@ void GroundMapping::updateInertialPositions() this->dcm_NB = cArray2EigenMatrixXd(*dcm_BN, 3, 3); // Get planet frame angular velocity vector - Eigen::Matrix3d w_tilde_PN = - this->dcm_PN_dot * this->dcm_PN.transpose(); - this->w_PN << w_tilde_PN(2,1), w_tilde_PN(0,2), w_tilde_PN(1,0); + Eigen::Matrix3d w_tilde_PN = -this->dcm_PN_dot * this->dcm_PN.transpose(); + this->w_PN << w_tilde_PN(2, 1), w_tilde_PN(0, 2), w_tilde_PN(1, 0); } /*! Method to compute whether or not the mapping point is in the instrument's FOV */ -uint64_t GroundMapping::checkInstrumentFOV(){ +uint64_t +GroundMapping::checkInstrumentFOV() +{ /* Compute the projection of the mapping point along the instrument's boresight */ - double boresightNormalProj = ((this->r_LP_N - (this->r_BP_N + this->dcm_NB*cameraPos_B)).transpose())*(dcm_NB*this->nHat_B); + double boresightNormalProj = + ((this->r_LP_N - (this->r_BP_N + this->dcm_NB * cameraPos_B)).transpose()) * (dcm_NB * this->nHat_B); /* Check that the normal projection is within the maximum range*/ - if ((boresightNormalProj >= 0) && (boresightNormalProj <= this->maximumRange || this->maximumRange < 0)){ + if ((boresightNormalProj >= 0) && (boresightNormalProj <= this->maximumRange || this->maximumRange < 0)) { /* Compute the radius of the instrument's cone at the projection distance */ - double coneRadius = boresightNormalProj*tan(this->halfFieldOfView); - double orthDistance = (this->r_LP_N - (this->r_BP_N + this->dcm_NB*cameraPos_B) - boresightNormalProj*dcm_NB*this->nHat_B).norm(); + double coneRadius = boresightNormalProj * tan(this->halfFieldOfView); + double orthDistance = + (this->r_LP_N - (this->r_BP_N + this->dcm_NB * cameraPos_B) - boresightNormalProj * dcm_NB * this->nHat_B) + .norm(); /* Check that the orthogonal distance is less than the cone radius */ if (orthDistance < coneRadius) { return 1; @@ -197,27 +212,29 @@ uint64_t GroundMapping::checkInstrumentFOV(){ } } /* If the first condition does not pass, return 0 */ - else{ + else { return 0; } } /*! write module messages -*/ -void GroundMapping::WriteMessages(uint64_t CurrentClock) + */ +void +GroundMapping::WriteMessages(uint64_t CurrentClock) { //! - write access message for each spacecraft - for (long unsigned int c=0; c< this->accessMsgBuffer.size(); c++) { + for (long unsigned int c = 0; c < this->accessMsgBuffer.size(); c++) { this->accessOutMsgs.at(c)->write(&this->accessMsgBuffer.at(c), this->moduleID, CurrentClock); - this->currentGroundStateOutMsgs.at(c)->write(&this->currentGroundStateMsgBuffer.at(c), this->moduleID, CurrentClock); + this->currentGroundStateOutMsgs.at(c)->write( + &this->currentGroundStateMsgBuffer.at(c), this->moduleID, CurrentClock); } - } /*! This is the main method that gets called every time the module is updated. Provide an appropriate description. */ -void GroundMapping::UpdateState(uint64_t CurrentSimNanos) +void +GroundMapping::UpdateState(uint64_t CurrentSimNanos) { // Read messages this->ReadMessages(); diff --git a/src/simulation/environment/groundMapping/groundMapping.h b/src/simulation/environment/groundMapping/groundMapping.h index fd4382ab75..0dcd74e81b 100644 --- a/src/simulation/environment/groundMapping/groundMapping.h +++ b/src/simulation/environment/groundMapping/groundMapping.h @@ -17,7 +17,6 @@ */ - #ifndef GROUNDMAPPING_H #define GROUNDMAPPING_H @@ -39,8 +38,9 @@ /*! @brief This module checks that a vector of mapping points are visible to a spacecraft's imager, outputting a vector * of accessMessages for each mapping point */ -class GroundMapping: public SysModel { -public: +class GroundMapping : public SysModel +{ + public: GroundMapping(); ~GroundMapping(); @@ -48,48 +48,54 @@ class GroundMapping: public SysModel { void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); -private: + private: void ReadMessages(); void computeAccess(uint64_t c); void WriteMessages(uint64_t CurrentClock); void updateInertialPositions(); uint64_t checkInstrumentFOV(); -public: - double minimumElevation; //!< [rad] (optional) minimum elevation above the local horizon needed to see a spacecraft; defaults to 10 degrees equivalent. - double maximumRange; //!< [m] (optional) Maximum slant range to compute access for; defaults to -1, which represents no maximum range. - Eigen::Vector3d cameraPos_B; //!< [m] (optional) Instrument position in body frame, defaults to (0,0,0) - double halfFieldOfView; //!< [r] Instrument half-fov, defaults to 10 degrees - Eigen::Vector3d nHat_B; //!< [-] Instrument unit direction vector in body frame components + public: + double minimumElevation; //!< [rad] (optional) minimum elevation above the local horizon needed to see a spacecraft; + //!< defaults to 10 degrees equivalent. + double maximumRange; //!< [m] (optional) Maximum slant range to compute access for; defaults to -1, which represents + //!< no maximum range. + Eigen::Vector3d cameraPos_B; //!< [m] (optional) Instrument position in body frame, defaults to (0,0,0) + double halfFieldOfView; //!< [r] Instrument half-fov, defaults to 10 degrees + Eigen::Vector3d nHat_B; //!< [-] Instrument unit direction vector in body frame components - BSKLogger bskLogger; //!< -- BSK Logging + BSKLogger bskLogger; //!< -- BSK Logging - ReadFunctor planetInMsg; //!< Planet state input message - ReadFunctor scStateInMsg; //!< Spacecraft state input message - std::vector*> currentGroundStateOutMsgs; //!< vector of ground location output message - std::vector*> accessOutMsgs; //!< vector of ground location access messages + ReadFunctor planetInMsg; //!< Planet state input message + ReadFunctor scStateInMsg; //!< Spacecraft state input message + std::vector*> + currentGroundStateOutMsgs; //!< vector of ground location output message + std::vector*> accessOutMsgs; //!< vector of ground location access messages -private: + private: std::vector accessMsgBuffer; //!< buffer of access output data - std::vector currentGroundStateMsgBuffer; //!< buffer of access output data - SCStatesMsgPayload scStateInMsgBuffer; //!< buffer of spacecraft states - SpicePlanetStateMsgPayload planetInMsgBuffer; //!< buffer of planet data + std::vector currentGroundStateMsgBuffer; //!< buffer of access output data + SCStatesMsgPayload scStateInMsgBuffer; //!< buffer of spacecraft states + SpicePlanetStateMsgPayload planetInMsgBuffer; //!< buffer of planet data - std::vector mappingPoints; //!< Vector of mapping points - Eigen::Matrix3d dcm_LP; //!< Rotation matrix from planet-centered, planet-fixed frame P to site-local topographic (SEZ) frame L coordinates. + std::vector mappingPoints; //!< Vector of mapping points + Eigen::Matrix3d dcm_LP; //!< Rotation matrix from planet-centered, planet-fixed frame P to site-local topographic + //!< (SEZ) frame L coordinates. Eigen::Matrix3d dcm_PN; //!< Rotation matrix from inertial frame N to planet-centered to planet-fixed frame P. - Eigen::Matrix3d dcm_PN_dot; //!< Rotation matrix derivative from inertial frame N to planet-centered to planet-fixed frame P. - Eigen::Vector3d w_PN; //!< [rad/s] Angular velocity of planet-fixed frame P relative to inertial frame N. + Eigen::Matrix3d + dcm_PN_dot; //!< Rotation matrix derivative from inertial frame N to planet-centered to planet-fixed frame P. + Eigen::Vector3d w_PN; //!< [rad/s] Angular velocity of planet-fixed frame P relative to inertial frame N. Eigen::Vector3d r_PN_N; //!< [m] Planet position vector relative to inertial frame origin. - Eigen::Vector3d r_LP_P; //!< [m] Ground Location position vector relative to to planet origin vector in planet frame coordinates. - Eigen::Vector3d r_LP_N; //!< [m] Gound Location position vector relative to planet origin vector in inertial coordinates. - Eigen::Vector3d rhat_LP_N;//!< [-] Surface normal vector from the target location in inertial coordinates. - Eigen::Vector3d r_LN_N; //!< [m] Ground Location position vector relative to inertial frame origin in inertial coordinates. + Eigen::Vector3d + r_LP_P; //!< [m] Ground Location position vector relative to to planet origin vector in planet frame coordinates. + Eigen::Vector3d + r_LP_N; //!< [m] Gound Location position vector relative to planet origin vector in inertial coordinates. + Eigen::Vector3d rhat_LP_N; //!< [-] Surface normal vector from the target location in inertial coordinates. + Eigen::Vector3d + r_LN_N; //!< [m] Ground Location position vector relative to inertial frame origin in inertial coordinates. Eigen::Vector3d r_North_N; //!<[-] Inertial 3rd axis, defined internally as "North". - Eigen::Vector3d r_BP_N; //!< [m] Inertial position of the body frame wrt the planet - Eigen::Matrix3d dcm_NB; //!< DCM from the body frame to the inertial frame - + Eigen::Vector3d r_BP_N; //!< [m] Inertial position of the body frame wrt the planet + Eigen::Matrix3d dcm_NB; //!< DCM from the body frame to the inertial frame }; - #endif 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/_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/magneticFieldCenteredDipole/magneticFieldCenteredDipole.cpp b/src/simulation/environment/magneticFieldCenteredDipole/magneticFieldCenteredDipole.cpp index 8e1d1f5bfe..a05a59b713 100644 --- a/src/simulation/environment/magneticFieldCenteredDipole/magneticFieldCenteredDipole.cpp +++ b/src/simulation/environment/magneticFieldCenteredDipole/magneticFieldCenteredDipole.cpp @@ -20,16 +20,17 @@ #include "magneticFieldCenteredDipole.h" #include "architecture/utilities/linearAlgebra.h" -/*! The constructor method initializes the dipole parameters to zero, resuling in a zero magnetic field result by default. +/*! The constructor method initializes the dipole parameters to zero, resuling in a zero magnetic field result by + default. */ MagneticFieldCenteredDipole::MagneticFieldCenteredDipole() { //! - Set the default magnetic properties to yield a zero response - this->g10 = 0.0; // [T] - this->g11 = 0.0; // [T] - this->h11 = 0.0; // [T] - this->planetRadius = 0.0; // [m] + this->g10 = 0.0; // [T] + this->g11 = 0.0; // [T] + this->h11 = 0.0; // [T] + this->planetRadius = 0.0; // [m] return; } @@ -47,7 +48,8 @@ MagneticFieldCenteredDipole::~MagneticFieldCenteredDipole() @param currentTime current time (s) */ -void MagneticFieldCenteredDipole::evaluateMagneticFieldModel(MagneticFieldMsgPayload *msg, double currentTime) +void +MagneticFieldCenteredDipole::evaluateMagneticFieldModel(MagneticFieldMsgPayload* msg, double currentTime) { Eigen::Vector3d magField_P; // [T] magnetic field in Planet fixed frame Eigen::Vector3d rHat_P; // [] normalized position vector in E frame components @@ -58,7 +60,8 @@ void MagneticFieldCenteredDipole::evaluateMagneticFieldModel(MagneticFieldMsgPay //! - compute magnetic field vector in E-frame components (see p. 405 in doi:10.1007/978-1-4939-0802-8) dipoleCoefficients << this->g11, this->h11, this->g10; - magField_P = pow(this->planetRadius/this->orbitRadius,3)*(3* rHat_P*rHat_P.dot(dipoleCoefficients) - dipoleCoefficients); + magField_P = pow(this->planetRadius / this->orbitRadius, 3) * + (3 * rHat_P * rHat_P.dot(dipoleCoefficients) - dipoleCoefficients); //! - convert magnetic field vector in N-frame components and store in output message m33tMultV3(this->planetState.J20002Pfix, magField_P.data(), msg->magField_N); diff --git a/src/simulation/environment/magneticFieldCenteredDipole/magneticFieldCenteredDipole.h b/src/simulation/environment/magneticFieldCenteredDipole/magneticFieldCenteredDipole.h index 6fdacaa177..e365f88001 100644 --- a/src/simulation/environment/magneticFieldCenteredDipole/magneticFieldCenteredDipole.h +++ b/src/simulation/environment/magneticFieldCenteredDipole/magneticFieldCenteredDipole.h @@ -17,7 +17,6 @@ */ - #ifndef CENTERED_DIPOLE_MAGNETIC_FIELD_H #define CENTERED_DIPOLE_MAGNETIC_FIELD_H @@ -31,24 +30,22 @@ #include "architecture/utilities/bskLogging.h" /*! @brief magnetic field centered dipole class */ -class MagneticFieldCenteredDipole: public MagneticFieldBase { -public: +class MagneticFieldCenteredDipole : public MagneticFieldBase +{ + public: MagneticFieldCenteredDipole(); ~MagneticFieldCenteredDipole(); -private: - void evaluateMagneticFieldModel(MagneticFieldMsgPayload *msg, double currentTime); - + private: + void evaluateMagneticFieldModel(MagneticFieldMsgPayload* msg, double currentTime); -public: + public: // More info on these IGRF parameters can be found on this [link](https://www.ngdc.noaa.gov/IAGA/vmod/igrf.html) - double g10; //!< [T] IGRF coefficient g_1^0 - double g11; //!< [T] IGRF coefficient g_1^1 - double h11; //!< [T] IGRF coefficient h_1^1 - - BSKLogger bskLogger; //!< -- BSK Logging + double g10; //!< [T] IGRF coefficient g_1^0 + double g11; //!< [T] IGRF coefficient g_1^1 + double h11; //!< [T] IGRF coefficient h_1^1 + BSKLogger bskLogger; //!< -- BSK Logging }; - #endif /* CENTERED_DIPOLE_MAGNETIC_FIELD_H */ 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 @@ -77,11 +77,6 @@ * Adam.Woods@noaa.gov */ - - - - - /****************************************************************************** ************************************Wrapper*********************************** * This grouping consists of functions call groups of other functions to do a @@ -89,18 +84,21 @@ * 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. ****************************************************************************** ******************************************************************************/ - -int MAG_Geomag(MAGtype_Ellipsoid Ellip, MAGtype_CoordSpherical CoordSpherical, MAGtype_CoordGeodetic CoordGeodetic, - MAGtype_MagneticModel *TimedMagneticModel, MAGtype_GeoMagneticElements *GeoMagneticElements) +int +MAG_Geomag(MAGtype_Ellipsoid Ellip, + MAGtype_CoordSpherical CoordSpherical, + MAGtype_CoordGeodetic CoordGeodetic, + MAGtype_MagneticModel* TimedMagneticModel, + MAGtype_GeoMagneticElements* GeoMagneticElements) /* -The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single point. -The function expects the model coefficients and point coordinates as input and returns the magnetic field elements and -their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid -of magnetic field, these are better achieved by the subroutine MAG_Grid. +The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single +point. The function expects the model coefficients and point coordinates as input and returns the magnetic field +elements and their rate of change. Though, this subroutine can be called successively to calculate a time series, +profile or grid of magnetic field, these are better achieved by the subroutine MAG_Grid. INPUT: Ellip CoordSpherical @@ -110,32 +108,55 @@ INPUT: Ellip OUTPUT : GeoMagneticElements CALLS: MAG_AllocateLegendreFunctionMemory(NumTerms); ( For storing the ALF functions ) - MAG_ComputeSphericalHarmonicVariables( Ellip, CoordSpherical, TimedMagneticModel->nMax, &SphVariables); (Compute Spherical Harmonic variables ) - MAG_AssociatedLegendreFunction(CoordSpherical, TimedMagneticModel->nMax, LegendreFunction); Compute ALF - MAG_Summation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSph); Accumulate the spherical harmonic coefficients - MAG_SecVarSummation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSphVar); Sum the Secular Variation Coefficients - MAG_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSph, &MagneticResultsGeo); Map the computed Magnetic fields to Geodetic coordinates - MAG_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); Calculate the Geomagnetic elements - MAG_CalculateSecularVariationElements(MagneticResultsGeoVar, GeoMagneticElements); Calculate the secular variation of each of the Geomagnetic elements + MAG_ComputeSphericalHarmonicVariables( Ellip, CoordSpherical, TimedMagneticModel->nMax, +&SphVariables); (Compute Spherical Harmonic variables ) MAG_AssociatedLegendreFunction(CoordSpherical, +TimedMagneticModel->nMax, LegendreFunction); Compute ALF MAG_Summation(LegendreFunction, TimedMagneticModel, +SphVariables, CoordSpherical, &MagneticResultsSph); Accumulate the spherical harmonic coefficients + MAG_SecVarSummation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, +&MagneticResultsSphVar); Sum the Secular Variation Coefficients MAG_RotateMagneticVector(CoordSpherical, CoordGeodetic, +MagneticResultsSph, &MagneticResultsGeo); Map the computed Magnetic fields to Geodetic coordinates + MAG_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); Calculate the +Geomagnetic elements MAG_CalculateSecularVariationElements(MagneticResultsGeoVar, GeoMagneticElements); Calculate the +secular variation of each of the Geomagnetic elements */ { - MAGtype_LegendreFunction *LegendreFunction; - MAGtype_SphericalHarmonicVariables *SphVariables; + MAGtype_LegendreFunction* LegendreFunction; + MAGtype_SphericalHarmonicVariables* SphVariables; 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 */ + MAG_ComputeSphericalHarmonicVariables( + Ellip, CoordSpherical, TimedMagneticModel->nMax, SphVariables); /* Compute Spherical Harmonic variables */ MAG_AssociatedLegendreFunction(CoordSpherical, TimedMagneticModel->nMax, LegendreFunction); /* Compute ALF */ - MAG_Summation(LegendreFunction, TimedMagneticModel, *SphVariables, CoordSpherical, &MagneticResultsSph); /* Accumulate the spherical harmonic coefficients*/ - MAG_SecVarSummation(LegendreFunction, TimedMagneticModel, *SphVariables, CoordSpherical, &MagneticResultsSphVar); /*Sum the Secular Variation Coefficients */ - MAG_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSph, &MagneticResultsGeo); /* Map the computed Magnetic fields to Geodeitic coordinates */ - MAG_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSphVar, &MagneticResultsGeoVar); /* Map the secular variation field components to Geodetic coordinates*/ - MAG_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); /* Calculate the Geomagnetic elements, Equation 19 , WMM Technical report */ - MAG_CalculateSecularVariationElements(MagneticResultsGeoVar, GeoMagneticElements); /*Calculate the secular variation of each of the Geomagnetic elements*/ + MAG_Summation(LegendreFunction, + TimedMagneticModel, + *SphVariables, + CoordSpherical, + &MagneticResultsSph); /* Accumulate the spherical harmonic coefficients*/ + MAG_SecVarSummation(LegendreFunction, + TimedMagneticModel, + *SphVariables, + CoordSpherical, + &MagneticResultsSphVar); /*Sum the Secular Variation Coefficients */ + MAG_RotateMagneticVector(CoordSpherical, + CoordGeodetic, + MagneticResultsSph, + &MagneticResultsGeo); /* Map the computed Magnetic fields to Geodeitic coordinates */ + MAG_RotateMagneticVector( + CoordSpherical, + CoordGeodetic, + MagneticResultsSphVar, + &MagneticResultsGeoVar); /* Map the secular variation field components to Geodetic coordinates*/ + MAG_CalculateGeoMagneticElements( + &MagneticResultsGeo, + GeoMagneticElements); /* Calculate the Geomagnetic elements, Equation 19 , WMM Technical report */ + MAG_CalculateSecularVariationElements( + MagneticResultsGeoVar, + GeoMagneticElements); /*Calculate the secular variation of each of the Geomagnetic elements*/ MAG_FreeLegendreMemory(LegendreFunction); MAG_FreeSphVarMemory(SphVariables); @@ -143,32 +164,32 @@ CALLS: MAG_AllocateLegendreFunctionMemory(NumTerms); ( For storing the ALF fu return TRUE; } /*MAG_Geomag*/ -void MAG_Gradient(MAGtype_Ellipsoid Ellip, MAGtype_CoordGeodetic CoordGeodetic, MAGtype_MagneticModel *TimedMagneticModel, MAGtype_Gradient *Gradient) +void +MAG_Gradient(MAGtype_Ellipsoid Ellip, + MAGtype_CoordGeodetic CoordGeodetic, + MAGtype_MagneticModel* TimedMagneticModel, + MAGtype_Gradient* Gradient) { /*It should be noted that the x[2], y[2], and z[2] variables are NOT the same 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]; - /*Initialization*/ MAG_GeodeticToSpherical(Ellip, CoordGeodetic, &AdjCoordSpherical); MAG_Geomag(Ellip, AdjCoordSpherical, CoordGeodetic, TimedMagneticModel, &GeomagneticElements); AdjCoordGeodetic = MAG_CoordGeodeticAssign(CoordGeodetic); - - - /*Gradient along x*/ AdjCoordGeodetic.phi = CoordGeodetic.phi + phiDelta; @@ -180,23 +201,23 @@ void MAG_Gradient(MAGtype_Ellipsoid Ellip, MAGtype_CoordGeodetic CoordGeodetic, MAG_Geomag(Ellip, AdjCoordSpherical, AdjCoordGeodetic, TimedMagneticModel, &AdjGeoMagneticElements[1]); MAG_SphericalToCartesian(AdjCoordSpherical, &x[1], &y[1], &z[1]); - - distance = sqrt((x[0] - x[1])*(x[0] - x[1])+(y[0] - y[1])*(y[0] - y[1])+(z[0] - z[1])*(z[0] - z[1])); + distance = sqrt((x[0] - x[1]) * (x[0] - x[1]) + (y[0] - y[1]) * (y[0] - y[1]) + (z[0] - z[1]) * (z[0] - z[1])); Gradient->GradPhi = MAG_GeoMagneticElementsSubtract(AdjGeoMagneticElements[0], AdjGeoMagneticElements[1]); Gradient->GradPhi = MAG_GeoMagneticElementsScale(Gradient->GradPhi, 1 / distance); 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)); - + MAG_GradY( + Ellip, AdjCoordSpherical, CoordGeodetic, TimedMagneticModel, GeomagneticElements, &(Gradient->GradLambda)); + /*Gradient along z*/ AdjCoordGeodetic.HeightAboveEllipsoid = CoordGeodetic.HeightAboveEllipsoid + hDelta; AdjCoordGeodetic.HeightAboveGeoid = CoordGeodetic.HeightAboveGeoid + hDelta; @@ -209,13 +230,14 @@ void MAG_Gradient(MAGtype_Ellipsoid Ellip, MAGtype_CoordGeodetic CoordGeodetic, MAG_Geomag(Ellip, AdjCoordSpherical, AdjCoordGeodetic, TimedMagneticModel, &AdjGeoMagneticElements[1]); MAG_SphericalToCartesian(AdjCoordSpherical, &x[1], &y[1], &z[1]); - distance = sqrt((x[0] - x[1])*(x[0] - x[1])+(y[0] - y[1])*(y[0] - y[1])+(z[0] - z[1])*(z[0] - z[1])); + distance = sqrt((x[0] - x[1]) * (x[0] - x[1]) + (y[0] - y[1]) * (y[0] - y[1]) + (z[0] - z[1]) * (z[0] - z[1])); Gradient->GradZ = MAG_GeoMagneticElementsSubtract(AdjGeoMagneticElements[0], AdjGeoMagneticElements[1]); - Gradient->GradZ = MAG_GeoMagneticElementsScale(Gradient->GradZ, 1/distance); + Gradient->GradZ = MAG_GeoMagneticElementsScale(Gradient->GradZ, 1 / distance); AdjCoordGeodetic = MAG_CoordGeodeticAssign(CoordGeodetic); } -int MAG_SetDefaults(MAGtype_Ellipsoid *Ellip, MAGtype_Geoid *Geoid) +int +MAG_SetDefaults(MAGtype_Ellipsoid* Ellip, MAGtype_Geoid* Geoid) /* Sets default values for WMM subroutines. @@ -228,18 +250,18 @@ int MAG_SetDefaults(MAGtype_Ellipsoid *Ellip, MAGtype_Geoid *Geoid) { /* Sets WGS-84 parameters */ - Ellip->a = 6378.137; /*semi-major axis of the ellipsoid in */ - Ellip->b = 6356.7523142; /*semi-minor axis of the ellipsoid in */ - Ellip->fla = 1 / 298.257223563; /* flattening */ + Ellip->a = 6378.137; /*semi-major axis of the ellipsoid in */ + Ellip->b = 6356.7523142; /*semi-minor axis of the ellipsoid in */ + Ellip->fla = 1 / 298.257223563; /* flattening */ Ellip->eps = sqrt(1 - (Ellip->b * Ellip->b) / (Ellip->a * Ellip->a)); /*first eccentricity */ - Ellip->epssq = (Ellip->eps * Ellip->eps); /*first eccentricity squared */ - Ellip->re = 6371.2; /* Earth's radius */ + Ellip->epssq = (Ellip->eps * Ellip->eps); /*first eccentricity squared */ + Ellip->re = 6371.2; /* Earth's radius */ /* Sets EGM-96 model file parameters */ Geoid->NumbGeoidCols = 1441; /* 360 degrees of longitude at 15 minute spacing */ - Geoid->NumbGeoidRows = 721; /* 180 degrees of latitude at 15 minute spacing */ - Geoid->NumbHeaderItems = 6; /* min, max lat, min, max long, lat, long spacing*/ - Geoid->ScaleFactor = 4; /* 4 grid cells per degree at 15 minute spacing */ + Geoid->NumbGeoidRows = 721; /* 180 degrees of latitude at 15 minute spacing */ + Geoid->NumbHeaderItems = 6; /* min, max lat, min, max long, lat, long spacing*/ + Geoid->ScaleFactor = 4; /* 4 grid cells per degree at 15 minute spacing */ Geoid->NumbGeoidElevs = Geoid->NumbGeoidCols * Geoid->NumbGeoidRows; Geoid->Geoid_Initialized = 0; /* Geoid will be initialized only if this is set to zero */ Geoid->UseGeoid = MAG_USE_GEOID; @@ -247,50 +269,50 @@ int MAG_SetDefaults(MAGtype_Ellipsoid *Ellip, MAGtype_Geoid *Geoid) return TRUE; } /*MAG_SetDefaults */ -int MAG_robustReadMagneticModel_Large(char *filename, char *filenameSV, MAGtype_MagneticModel **MagneticModel) +int +MAG_robustReadMagneticModel_Large(char* filename, char* filenameSV, MAGtype_MagneticModel** MagneticModel) { - char line[MAXLINELENGTH], ModelName[] = "Enhanced Magnetic Model";/*Model Name must be no longer than 31 characters*/ - int n, nMax = 0, nMaxSV = 0, num_terms, a, epochlength=5, i; - FILE *MODELFILE; + char line[MAXLINELENGTH], + ModelName[] = "Enhanced Magnetic Model"; /*Model Name must be no longer than 31 characters*/ + int n, nMax = 0, nMaxSV = 0, num_terms, a, epochlength = 5, i; + FILE* MODELFILE; MODELFILE = fopen(filename, "r"); - if(MODELFILE == 0) { + if (MODELFILE == 0) { return 0; } - if (NULL == fgets(line, MAXLINELENGTH, MODELFILE)) - { + if (NULL == fgets(line, MAXLINELENGTH, MODELFILE)) { return 0; - } - do - { - if(NULL == fgets(line, MAXLINELENGTH, MODELFILE)) + } + do { + if (NULL == fgets(line, MAXLINELENGTH, MODELFILE)) break; a = sscanf(line, "%d", &n); - if(n > nMax && (n < 99999 && a == 1 && n > 0)) + if (n > nMax && (n < 99999 && a == 1 && n > 0)) nMax = n; - } while(n < 99999 && a == 1); + } while (n < 99999 && a == 1); fclose(MODELFILE); MODELFILE = fopen(filenameSV, "r"); - if(MODELFILE == 0) { + if (MODELFILE == 0) { return 0; } n = 0; if (NULL == fgets(line, MAXLINELENGTH, MODELFILE)) return 0; - do - { - if(NULL == fgets(line, MAXLINELENGTH, MODELFILE)) + do { + if (NULL == fgets(line, MAXLINELENGTH, MODELFILE)) break; a = sscanf(line, "%d", &n); - if(n > nMaxSV && (n < 99999 && a == 1 && n > 0)) + if (n > nMaxSV && (n < 99999 && a == 1 && n > 0)) nMaxSV = n; - } while(n < 99999 && a == 1); + } while (n < 99999 && a == 1); fclose(MODELFILE); num_terms = CALCULATE_NUMTERMS(nMax); *MagneticModel = MAG_AllocateModelMemory(num_terms); (*MagneticModel)->nMax = nMax; (*MagneticModel)->nMaxSecVar = nMaxSV; - if(nMaxSV > 0) (*MagneticModel)->SecularVariationUsed = TRUE; - for(i = 0; i < num_terms; i++) { + if (nMaxSV > 0) + (*MagneticModel)->SecularVariationUsed = TRUE; + for (i = 0; i < num_terms; i++) { (*MagneticModel)->Main_Field_Coeff_G[i] = 0; (*MagneticModel)->Main_Field_Coeff_H[i] = 0; (*MagneticModel)->Secular_Var_Coeff_G[i] = 0; @@ -303,32 +325,30 @@ int MAG_robustReadMagneticModel_Large(char *filename, char *filenameSV, MAGtype_ return 1; } /*MAG_robustReadMagneticModel_Large*/ -int MAG_robustReadMagModels(char *filename, MAGtype_MagneticModel *(*magneticmodels)[1], int array_size) +int +MAG_robustReadMagModels(char* filename, MAGtype_MagneticModel* (*magneticmodels)[1], int array_size) { char line[MAXLINELENGTH]; int n, nMax = 0, num_terms, a; - FILE *MODELFILE; + FILE* MODELFILE; MODELFILE = fopen(filename, "r"); - if(MODELFILE == 0) { + if (MODELFILE == 0) { return 0; } - if (NULL==fgets(line, MAXLINELENGTH, MODELFILE)){ + if (NULL == fgets(line, MAXLINELENGTH, MODELFILE)) { return 0; } - if(line[0] == '%'){ + if (line[0] == '%') { MAG_readMagneticModel_SHDF(filename, magneticmodels, array_size); - } - else if(array_size == 1) - { + } else if (array_size == 1) { - do - { - if(NULL == fgets(line, MAXLINELENGTH, MODELFILE)) + do { + if (NULL == fgets(line, MAXLINELENGTH, MODELFILE)) break; a = sscanf(line, "%d", &n); - if(n > nMax && (n < 99999 && a == 1 && n > 0)) + if (n > nMax && (n < 99999 && a == 1 && n > 0)) nMax = n; - } while(n < 99999 && a == 1); + } while (n < 99999 && a == 1); num_terms = CALCULATE_NUMTERMS(nMax); (*magneticmodels)[0] = MAG_AllocateModelMemory(num_terms); (*magneticmodels)[0]->nMax = nMax; @@ -336,7 +356,8 @@ int MAG_robustReadMagModels(char *filename, MAGtype_MagneticModel *(*magneticmod MAG_readMagneticModel(filename, (*magneticmodels)[0]); (*magneticmodels)[0]->CoefficientFileEndDate = (*magneticmodels)[0]->epoch + 5; - } else return 0; + } else + return 0; fclose(MODELFILE); return 1; } /*MAG_robustReadMagModels*/ @@ -345,12 +366,13 @@ 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. ******************************************************************************/ -void MAG_Error(int control) +void +MAG_Error(int control) /*This prints WMM errors. INPUT control Error look up number @@ -359,7 +381,7 @@ CALLS : none */ { - switch(control) { + switch (control) { case 1: printf("\nError allocating in MAG_LegendreFunctionMemory.\n"); break; @@ -397,8 +419,8 @@ CALLS : none printf("\nError in Geomagnetic\n"); break; case 13: - printf("\nError printing user data\n");\ - break; + printf("\nError printing user data\n"); + break; case 14: printf("\nError allocating in MAG_SummationSpecial\n"); break; @@ -431,8 +453,18 @@ CALLS : none } } /*MAG_Error*/ -int MAG_GetUserGrid(MAGtype_CoordGeodetic *minimum, MAGtype_CoordGeodetic *maximum, double *step_size, double *a_step_size, double *step_time, MAGtype_Date - *StartDate, MAGtype_Date *EndDate, int *ElementOption, int *PrintOption, char *OutputFile, MAGtype_Geoid *Geoid) +int +MAG_GetUserGrid(MAGtype_CoordGeodetic* minimum, + MAGtype_CoordGeodetic* maximum, + double* step_size, + double* a_step_size, + double* step_time, + MAGtype_Date* StartDate, + MAGtype_Date* EndDate, + int* ElementOption, + int* PrintOption, + char* OutputFile, + MAGtype_Geoid* Geoid) /* Prompts user to enter parameters to compute a grid - for use with the MAG_grid function Note: The user entries are not validated before here. The function populates the input variables & data structures. @@ -455,7 +487,7 @@ CALLS : none */ { - FILE *fileout; + FILE* fileout; char filename[] = "GridProgramDirective.txt"; char buffer[20]; int dummy; @@ -464,7 +496,7 @@ CALLS : none if (NULL == fgets(buffer, 20, stdin)) { minimum->phi = 0; printf("Unrecognized input default %lf used\n", minimum->phi); - }else { + } else { sscanf(buffer, "%lf", &minimum->phi); } strcpy(buffer, ""); @@ -485,7 +517,7 @@ CALLS : none } strcpy(buffer, ""); printf("Please Enter Maximum Longitude (in decimal degrees):\n"); - if (NULL == fgets(buffer, 20, stdin)){ + if (NULL == fgets(buffer, 20, stdin)) { maximum->lambda = 0; printf("Unrecognized input default %lf used\n", maximum->lambda); } else { @@ -493,7 +525,7 @@ CALLS : none } strcpy(buffer, ""); printf("Please Enter Step Size (in decimal degrees):\n"); - if (NULL == fgets(buffer, 20, stdin)){ + if (NULL == fgets(buffer, 20, stdin)) { *step_size = fmax(maximum->phi - minimum->phi, maximum->lambda - minimum->lambda); printf("Unrecognized input default %lf used\n", *step_size); } else { @@ -506,12 +538,13 @@ CALLS : none printf("Unrecognized option, height above MSL used."); } else { sscanf(buffer, "%d", &dummy); - if(dummy == 2) Geoid->UseGeoid = 0; - else Geoid->UseGeoid = 1; + if (dummy == 2) + Geoid->UseGeoid = 0; + else + Geoid->UseGeoid = 1; } strcpy(buffer, ""); - if(Geoid->UseGeoid == 1) - { + if (Geoid->UseGeoid == 1) { printf("Please Enter Minimum Height above MSL (in km):\n"); if (NULL == fgets(buffer, 20, stdin)) { minimum->HeightAboveGeoid = 0; @@ -529,11 +562,9 @@ CALLS : none } strcpy(buffer, ""); - } else - { + } else { printf("Please Enter Minimum Height above the WGS-84 Ellipsoid (in km):\n"); - if (NULL == fgets(buffer, 20, stdin)) - { + if (NULL == fgets(buffer, 20, stdin)) { minimum->HeightAboveGeoid = 0; printf("Unrecognized input default %lf used\n", minimum->HeightAboveGeoid); } else { @@ -580,32 +611,32 @@ CALLS : none } strcpy(buffer, ""); printf("Enter a geomagnetic element to print. Your options are:\n"); - printf(" 1. Declination 9. Ddot\n 2. Inclination 10. Idot\n 3. F 11. Fdot\n 4. H 12. Hdot\n 5. X 13. Xdot\n 6. Y 14. Ydot\n 7. Z 15. Zdot\n 8. GV 16. GVdot\nFor gradients enter: 17\n"); + printf(" 1. Declination 9. Ddot\n 2. Inclination 10. Idot\n 3. F 11. Fdot\n 4. H 12. Hdot\n 5. X " + "13. Xdot\n 6. Y 14. Ydot\n 7. Z 15. Zdot\n 8. GV 16. GVdot\nFor gradients enter: 17\n"); if (NULL == fgets(buffer, 20, stdin)) { *ElementOption = 1; printf("Unrecognized input, default of %d used\n", *ElementOption); } sscanf(buffer, "%d", ElementOption); strcpy(buffer, ""); - if(*ElementOption == 17) - { + if (*ElementOption == 17) { printf("Enter a gradient element to print. Your options are:\n"); printf(" 1. dX/dphi \t2. dY/dphi \t3. dZ/dphi\n"); printf(" 4. dX/dlambda \t5. dY/dlambda \t6. dZ/dlambda\n"); printf(" 7. dX/dz \t8. dY/dz \t9. dZ/dz\n"); strcpy(buffer, ""); if (NULL == fgets(buffer, 20, stdin)) { - *ElementOption=1; + *ElementOption = 1; printf("Unrecognized input, default of %d used\n", *ElementOption); } else { sscanf(buffer, "%d", ElementOption); } strcpy(buffer, ""); - *ElementOption+=16; + *ElementOption += 16; } printf("Select output :\n"); printf(" 1. Print to a file \n 2. Print to Screen\n"); - if (NULL ==fgets(buffer, 20, stdin)){ + if (NULL == fgets(buffer, 20, stdin)) { *PrintOption = 2; printf("Unrecognized input, default of printing to screen\n"); } else { @@ -613,16 +644,13 @@ CALLS : none } strcpy(buffer, ""); fileout = fopen(filename, "a"); - if(*PrintOption == 1) - { + if (*PrintOption == 1) { printf("Please enter output filename\nfor default ('GridResults.txt') press enter:\n"); - if(NULL==fgets(buffer, 20, stdin) || strlen(buffer) <= 1) - { + if (NULL == fgets(buffer, 20, stdin) || strlen(buffer) <= 1) { strcpy(OutputFile, "GridResults.txt"); fprintf(fileout, "\nResults printed in: GridResults.txt\n"); strcpy(OutputFile, "GridResults.txt"); - } else - { + } else { sscanf(buffer, "%s", OutputFile); fprintf(fileout, "\nResults printed in: %s\n", OutputFile); } @@ -631,17 +659,42 @@ CALLS : none /*sscanf(buffer, "%s", OutputFile);*/ } else fprintf(fileout, "\nResults printed in Console\n"); - fprintf(fileout, "Minimum Latitude: %f\t\tMaximum Latitude: %f\t\tStep Size: %f\nMinimum Longitude: %f\t\tMaximum Longitude: %f\t\tStep Size: %f\n", minimum->phi, maximum->phi, *step_size, minimum->lambda, maximum->lambda, *step_size); - if(Geoid->UseGeoid == 1) - fprintf(fileout, "Minimum Altitude above MSL: %f\tMaximum Altitude above MSL: %f\tStep Size: %f\n", minimum->HeightAboveGeoid, maximum->HeightAboveGeoid, *a_step_size); + fprintf(fileout, + "Minimum Latitude: %f\t\tMaximum Latitude: %f\t\tStep Size: %f\nMinimum Longitude: %f\t\tMaximum " + "Longitude: %f\t\tStep Size: %f\n", + minimum->phi, + maximum->phi, + *step_size, + minimum->lambda, + maximum->lambda, + *step_size); + if (Geoid->UseGeoid == 1) + fprintf(fileout, + "Minimum Altitude above MSL: %f\tMaximum Altitude above MSL: %f\tStep Size: %f\n", + minimum->HeightAboveGeoid, + maximum->HeightAboveGeoid, + *a_step_size); else - fprintf(fileout, "Minimum Altitude above WGS-84 Ellipsoid: %f\tMaximum Altitude above WGS-84 Ellipsoid: %f\tStep Size: %f\n", minimum->HeightAboveEllipsoid, maximum->HeightAboveEllipsoid, *a_step_size); - fprintf(fileout, "Starting Date: %f\t\tEnding Date: %f\t\tStep Time: %f\n\n\n", StartDate->DecimalYear, EndDate->DecimalYear, *step_time); + fprintf( + fileout, + "Minimum Altitude above WGS-84 Ellipsoid: %f\tMaximum Altitude above WGS-84 Ellipsoid: %f\tStep Size: %f\n", + minimum->HeightAboveEllipsoid, + maximum->HeightAboveEllipsoid, + *a_step_size); + fprintf(fileout, + "Starting Date: %f\t\tEnding Date: %f\t\tStep Time: %f\n\n\n", + StartDate->DecimalYear, + EndDate->DecimalYear, + *step_time); fclose(fileout); return TRUE; } -int MAG_GetUserInput(MAGtype_MagneticModel *MagneticModel, MAGtype_Geoid *Geoid, MAGtype_CoordGeodetic *CoordGeodetic, MAGtype_Date *MagneticDate) +int +MAG_GetUserInput(MAGtype_MagneticModel* MagneticModel, + MAGtype_Geoid* Geoid, + MAGtype_CoordGeodetic* CoordGeodetic, + MAGtype_Date* MagneticDate) /* This prompts the user for coordinates, and accepts many entry formats. @@ -649,7 +702,8 @@ It takes the MagneticModel and Geoid as input and outputs the Geographic coordin Returns 0 when the user wants to exit and 1 if the user enters valid input data. INPUT : MagneticModel : Data structure with the following elements used here double epoch; Base time of Geomagnetic model epoch (yrs) - : Geoid Pointer to data structure MAGtype_Geoid (used for converting HeightAboveGeoid to HeightABoveEllipsoid + : Geoid Pointer to data structure MAGtype_Geoid (used for converting HeightAboveGeoid to +HeightABoveEllipsoid OUTPUT: CoordGeodetic : Pointer to data structure. Following elements are updated double lambda; (longitude) @@ -663,9 +717,8 @@ OUTPUT: CoordGeodetic : Pointer to data structure. Following elements are update int Day; (If user directly enters decimal year this field is not populated) double DecimalYear; decimal years -CALLS: MAG_DMSstringToDegree(buffer, &CoordGeodetic->lambda); (The program uses this to convert the string into a decimal longitude.) - MAG_ValidateDMSstringlong(buffer, Error_Message) - MAG_ValidateDMSstringlat(buffer, Error_Message) +CALLS: MAG_DMSstringToDegree(buffer, &CoordGeodetic->lambda); (The program uses this to convert the string into a +decimal longitude.) MAG_ValidateDMSstringlong(buffer, Error_Message) MAG_ValidateDMSstringlat(buffer, Error_Message) MAG_Warnings MAG_ConvertGeoidToEllipsoidHeight MAG_DateToYear @@ -675,40 +728,42 @@ CALLS: MAG_DMSstringToDegree(buffer, &CoordGeodetic->lambda); (The program uses char Error_Message[255]; char buffer[40]; 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}; - char* Qstring = malloc(sizeof(char) * 1028); + 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 }; + 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"); - MAG_GetDeg(Qstring, &CoordGeodetic->phi, lat_bound); + 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"); + MAG_GetDeg(Qstring, &CoordGeodetic->phi, lat_bound); 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) + 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; strcpy(buffer, ""); printf("\nPlease enter the decimal year or calendar date\n (YYYY.yyy, MM DD YYYY or MM/DD/YYYY):\n"); while (NULL == fgets(buffer, 40, stdin)) { printf("\nPlease enter the decimal year or calendar date\n (YYYY.yyy, MM DD YYYY or MM/DD/YYYY):\n"); } - for(i = 0, done = 0; i <= 40 && !done; i++) - { - if(buffer[i] == '.') - { + for (i = 0, done = 0; i <= 40 && !done; i++) { + if (buffer[i] == '.') { j = sscanf(buffer, "%lf", &MagneticDate->DecimalYear); - if(j == 1) + if (j == 1) done = 1; else buffer[i] = '\0'; } - if(buffer[i] == '/') - { + if (buffer[i] == '/') { sscanf(buffer, "%d/%d/%d", &MagneticDate->Month, &MagneticDate->Day, &MagneticDate->Year); - if(!MAG_DateToYear(MagneticDate, Error_Message)) - { + if (!MAG_DateToYear(MagneticDate, Error_Message)) { printf("%s", Error_Message); printf("\nPlease re-enter Date in MM/DD/YYYY or MM DD YYYY format, or as a decimal year\n"); while (NULL == fgets(buffer, 40, stdin)) { @@ -718,57 +773,54 @@ CALLS: MAG_DMSstringToDegree(buffer, &CoordGeodetic->lambda); (The program uses } else done = 1; } - if((buffer[i] == ' ' && buffer[i + 1] != '/') || buffer[i] == '\0') - { - if(3 == sscanf(buffer, "%d %d %d", &a, &b, &c)) - { + if ((buffer[i] == ' ' && buffer[i + 1] != '/') || buffer[i] == '\0') { + if (3 == sscanf(buffer, "%d %d %d", &a, &b, &c)) { MagneticDate->Month = a; MagneticDate->Day = b; MagneticDate->Year = c; MagneticDate->DecimalYear = 99999; - } else if(1 == sscanf(buffer, "%d %d %d", &a, &b, &c)) - { + } else if (1 == sscanf(buffer, "%d %d %d", &a, &b, &c)) { MagneticDate->DecimalYear = a; done = 1; } - if(!(MagneticDate->DecimalYear == a)) - { - if(!MAG_DateToYear(MagneticDate, Error_Message)) - { + if (!(MagneticDate->DecimalYear == a)) { + if (!MAG_DateToYear(MagneticDate, Error_Message)) { printf("%s", Error_Message); strcpy(buffer, ""); - printf("\nError encountered, please re-enter Date in MM/DD/YYYY or MM DD YYYY format, or as a decimal year\n"); - while( NULL== fgets(buffer, 40, stdin)){ - printf("\nError encountered, please re-enter Date in MM/DD/YYYY or MM DD YYYY format, or as a decimal year\n"); + printf("\nError encountered, please re-enter Date in MM/DD/YYYY or MM DD YYYY format, or as a " + "decimal year\n"); + while (NULL == fgets(buffer, 40, stdin)) { + printf("\nError encountered, please re-enter Date in MM/DD/YYYY or MM DD YYYY format, or as a " + "decimal year\n"); } i = -1; } else done = 1; } } - if(buffer[i] == '\0' && i != -1 && done != 1) - { + if (buffer[i] == '\0' && i != -1 && done != 1) { 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"); + while (NULL == fgets(buffer, 40, stdin)) { + printf("\nError encountered, please re-enter as MM/DD/YYYY, MM DD YYYY, or as YYYY.yyy:\n"); } i = -1; } - if(done) - { - if(MagneticDate->DecimalYear > MagneticModel->CoefficientFileEndDate || MagneticDate->DecimalYear < MagneticModel->epoch) - { - switch(MAG_Warnings(4, MagneticDate->DecimalYear, MagneticModel)) { + if (done) { + if (MagneticDate->DecimalYear > MagneticModel->CoefficientFileEndDate || + MagneticDate->DecimalYear < MagneticModel->epoch) { + switch (MAG_Warnings(4, MagneticDate->DecimalYear, MagneticModel)) { case 0: return 0; case 1: done = 0; i = -1; strcpy(buffer, ""); - printf("\nPlease enter the decimal year or calendar date\n (YYYY.yyy, MM DD YYYY or MM/DD/YYYY):\n"); - while(NULL == fgets(buffer, 40, stdin)){ - printf("\nPlease enter the decimal year or calendar date\n (YYYY.yyy, MM DD YYYY or MM/DD/YYYY):\n"); + printf( + "\nPlease enter the decimal year or calendar date\n (YYYY.yyy, MM DD YYYY or MM/DD/YYYY):\n"); + while (NULL == fgets(buffer, 40, stdin)) { + printf("\nPlease enter the decimal year or calendar date\n (YYYY.yyy, MM DD YYYY or " + "MM/DD/YYYY):\n"); } break; case 2: @@ -781,20 +833,47 @@ CALLS: MAG_DMSstringToDegree(buffer, &CoordGeodetic->lambda); (The program uses return TRUE; } /*MAG_GetUserInput*/ -void MAG_PrintGradient(MAGtype_Gradient Gradient) +void +MAG_PrintGradient(MAGtype_Gradient Gradient) { printf("\nGradient\n"); printf("\n Northward Eastward Downward\n"); - printf("X: %7.1f nT/km %9.1f nT/km %9.1f nT/km \n", Gradient.GradPhi.X, Gradient.GradLambda.X, Gradient.GradZ.X); - printf("Y: %7.1f nT/km %9.1f nT/km %9.1f nT/km \n", Gradient.GradPhi.Y, Gradient.GradLambda.Y, Gradient.GradZ.Y); - printf("Z: %7.1f nT/km %9.1f nT/km %9.1f nT/km \n", Gradient.GradPhi.Z, Gradient.GradLambda.Z, Gradient.GradZ.Z); - printf("H: %7.1f nT/km %9.1f nT/km %9.1f nT/km \n", Gradient.GradPhi.H, Gradient.GradLambda.H, Gradient.GradZ.H); - printf("F: %7.1f nT/km %9.1f nT/km %9.1f nT/km \n", Gradient.GradPhi.F, Gradient.GradLambda.F, Gradient.GradZ.F); - printf("Declination: %7.2f min/km %8.2f min/km %8.2f min/km \n", Gradient.GradPhi.Decl * 60, Gradient.GradLambda.Decl * 60, Gradient.GradZ.Decl * 60); - printf("Inclination: %7.2f min/km %8.2f min/km %8.2f min/km \n", Gradient.GradPhi.Incl * 60, Gradient.GradLambda.Incl * 60, Gradient.GradZ.Incl * 60); + printf("X: %7.1f nT/km %9.1f nT/km %9.1f nT/km \n", + Gradient.GradPhi.X, + Gradient.GradLambda.X, + Gradient.GradZ.X); + printf("Y: %7.1f nT/km %9.1f nT/km %9.1f nT/km \n", + Gradient.GradPhi.Y, + Gradient.GradLambda.Y, + Gradient.GradZ.Y); + printf("Z: %7.1f nT/km %9.1f nT/km %9.1f nT/km \n", + Gradient.GradPhi.Z, + Gradient.GradLambda.Z, + Gradient.GradZ.Z); + printf("H: %7.1f nT/km %9.1f nT/km %9.1f nT/km \n", + Gradient.GradPhi.H, + Gradient.GradLambda.H, + Gradient.GradZ.H); + printf("F: %7.1f nT/km %9.1f nT/km %9.1f nT/km \n", + Gradient.GradPhi.F, + Gradient.GradLambda.F, + Gradient.GradZ.F); + printf("Declination: %7.2f min/km %8.2f min/km %8.2f min/km \n", + Gradient.GradPhi.Decl * 60, + Gradient.GradLambda.Decl * 60, + Gradient.GradZ.Decl * 60); + printf("Inclination: %7.2f min/km %8.2f min/km %8.2f min/km \n", + Gradient.GradPhi.Incl * 60, + Gradient.GradLambda.Incl * 60, + Gradient.GradZ.Incl * 60); } -void MAG_PrintUserData(MAGtype_GeoMagneticElements GeomagElements, MAGtype_CoordGeodetic SpaceInput, MAGtype_Date TimeInput, MAGtype_MagneticModel *MagneticModel, MAGtype_Geoid *Geoid) +void +MAG_PrintUserData(MAGtype_GeoMagneticElements GeomagElements, + MAGtype_CoordGeodetic SpaceInput, + MAGtype_Date TimeInput, + MAGtype_MagneticModel* MagneticModel, + MAGtype_Geoid* Geoid) /* This function prints the results in Geomagnetic Elements for a point calculation. It takes the calculated * Geomagnetic elements "GeomagElements" as input. * As well as the coordinates, date, and Magnetic Model. @@ -834,30 +913,29 @@ INPUT : GeomagElements : Data structure MAGtype_GeoMagneticElements with the fo double *Secular_Var_Coeff_H; CD - Gauss coefficients of secular geomagnetic model (nT/yr) int nMax; Maximum degree of spherical harmonic model int nMaxSecVar; Maxumum degree of spherical harmonic secular model - int SecularVariationUsed; Whether or not the magnetic secular variation vector will be needed by program - OUTPUT : none + int SecularVariationUsed; Whether or not the magnetic secular variation vector will be needed by +program OUTPUT : none */ { char DeclString[100]; char InclString[100]; MAG_DegreeToDMSstring(GeomagElements.Incl, 2, InclString); - if(GeomagElements.H < 6000 && GeomagElements.H > 2000) + if (GeomagElements.H < 6000 && GeomagElements.H > 2000) MAG_Warnings(1, GeomagElements.H, MagneticModel); - if(GeomagElements.H < 2000) + if (GeomagElements.H < 2000) MAG_Warnings(2, GeomagElements.H, MagneticModel); - if(MagneticModel->SecularVariationUsed == TRUE) - { + if (MagneticModel->SecularVariationUsed == TRUE) { MAG_DegreeToDMSstring(GeomagElements.Decl, 2, DeclString); printf("\n Results For \n\n"); - if(SpaceInput.phi < 0) + if (SpaceInput.phi < 0) printf("Latitude %.2fS\n", -SpaceInput.phi); else printf("Latitude %.2fN\n", SpaceInput.phi); - if(SpaceInput.lambda < 0) + if (SpaceInput.lambda < 0) printf("Longitude %.2fW\n", -SpaceInput.lambda); else printf("Longitude %.2fE\n", SpaceInput.lambda); - if(Geoid->UseGeoid == 1) + if (Geoid->UseGeoid == 1) printf("Altitude: %.2f Kilometers above mean sea level\n", SpaceInput.HeightAboveGeoid); else printf("Altitude: %.2f Kilometers above the WGS-84 ellipsoid\n", SpaceInput.HeightAboveEllipsoid); @@ -868,27 +946,26 @@ INPUT : GeomagElements : Data structure MAGtype_GeoMagneticElements with the fo printf("X = %-9.1f nT\t\t Xdot = %.1f\tnT/yr\n", GeomagElements.X, GeomagElements.Xdot); printf("Y = %-9.1f nT\t\t Ydot = %.1f\tnT/yr\n", GeomagElements.Y, GeomagElements.Ydot); printf("Z = %-9.1f nT\t\t Zdot = %.1f\tnT/yr\n", GeomagElements.Z, GeomagElements.Zdot); - if(GeomagElements.Decl < 0) + if (GeomagElements.Decl < 0) printf("Decl =%20s (WEST)\t Ddot = %.1f\tMin/yr\n", DeclString, 60 * GeomagElements.Decldot); else printf("Decl =%20s (EAST)\t Ddot = %.1f\tMin/yr\n", DeclString, 60 * GeomagElements.Decldot); - if(GeomagElements.Incl < 0) + if (GeomagElements.Incl < 0) printf("Incl =%20s (UP)\t Idot = %.1f\tMin/yr\n", InclString, 60 * GeomagElements.Incldot); else printf("Incl =%20s (DOWN)\t Idot = %.1f\tMin/yr\n", InclString, 60 * GeomagElements.Incldot); - } else - { + } else { MAG_DegreeToDMSstring(GeomagElements.Decl, 2, DeclString); printf("\n Results For \n\n"); - if(SpaceInput.phi < 0) + if (SpaceInput.phi < 0) printf("Latitude %.2fS\n", -SpaceInput.phi); else printf("Latitude %.2fN\n", SpaceInput.phi); - if(SpaceInput.lambda < 0) + if (SpaceInput.lambda < 0) printf("Longitude %.2fW\n", -SpaceInput.lambda); else printf("Longitude %.2fE\n", SpaceInput.lambda); - if(Geoid->UseGeoid == 1) + if (Geoid->UseGeoid == 1) printf("Altitude: %.2f Kilometers above MSL\n", SpaceInput.HeightAboveGeoid); else printf("Altitude: %.2f Kilometers above WGS-84 Ellipsoid\n", SpaceInput.HeightAboveEllipsoid); @@ -899,26 +976,27 @@ INPUT : GeomagElements : Data structure MAGtype_GeoMagneticElements with the fo printf("X = %-9.1f nT\n", GeomagElements.X); printf("Y = %-9.1f nT\n", GeomagElements.Y); printf("Z = %-9.1f nT\n", GeomagElements.Z); - if(GeomagElements.Decl < 0) + if (GeomagElements.Decl < 0) printf("Decl =%20s (WEST)\n", DeclString); else printf("Decl =%20s (EAST)\n", DeclString); - if(GeomagElements.Incl < 0) + if (GeomagElements.Incl < 0) printf("Incl =%20s (UP)\n", InclString); else printf("Incl =%20s (DOWN)\n", InclString); } - if(SpaceInput.phi <= -55 || SpaceInput.phi >= 55) - /* Print Grid Variation */ + if (SpaceInput.phi <= -55 || SpaceInput.phi >= 55) + /* Print Grid Variation */ { MAG_DegreeToDMSstring(GeomagElements.GV, 2, InclString); printf("\n\n Grid variation =%20s\n", InclString); } -}/*MAG_PrintUserData*/ +} /*MAG_PrintUserData*/ -int MAG_ValidateDMSstring(char *input, int min, int max, char *Error) +int +MAG_ValidateDMSstring(char* input, int min, int max, char* Error) /* Validates a latitude DMS string, and returns 1 for a success and returns 0 for a failure. It copies an error message to the Error string in the event of a failure. @@ -933,60 +1011,61 @@ CALLS : none degree = -1000; minute = -1; second = -1; - n = (int) strlen(input); + n = (int)strlen(input); - for(i = 0; i <= n - 1; i++) /*tests for legal characters*/ + for (i = 0; i <= n - 1; i++) /*tests for legal characters*/ { - if((input[i] < '0' || input[i] > '9') && (input[i] != ',' && input[i] != ' ' && input[i] != '-' && input[i] != '\0' && input[i] != '\n')) - { - strcpy(Error, "\nError: Input contains an illegal character, legal characters for Degree, Minute, Second format are:\n '0-9' ',' '-' '[space]' '[Enter]'\n"); + if ((input[i] < '0' || input[i] > '9') && + (input[i] != ',' && input[i] != ' ' && input[i] != '-' && input[i] != '\0' && input[i] != '\n')) { + strcpy(Error, + "\nError: Input contains an illegal character, legal characters for Degree, Minute, Second format " + "are:\n '0-9' ',' '-' '[space]' '[Enter]'\n"); return FALSE; } - if(input[i] == ',') + if (input[i] == ',') j++; } - if(j == 2) + if (j == 2) j = sscanf(input, "%d, %d, %d", °ree, &minute, &second); /*tests for legal formatting and range*/ else j = sscanf(input, "%d %d %d", °ree, &minute, &second); - if(j == 1) - { + if (j == 1) { minute = 0; second = 0; j = 3; } - if(j != 3) - { - strcpy(Error, "\nError: Not enough numbers used for Degrees, Minutes, Seconds format\n or they were incorrectly formatted\n The legal format is DD,MM,SS or DD MM SS\n"); + if (j != 3) { + strcpy(Error, + "\nError: Not enough numbers used for Degrees, Minutes, Seconds format\n or they were incorrectly " + "formatted\n The legal format is DD,MM,SS or DD MM SS\n"); return FALSE; } - if(degree > max || degree < min) - { + if (degree > max || degree < min) { sprintf(Error, "\nError: Degree input is outside legal range\n The legal range is from %d to %d\n", min, max); return FALSE; } - if(degree == max || degree == min) + if (degree == max || degree == min) max_minute = 0; - if(minute > max_minute || minute < 0) - { + if (minute > max_minute || minute < 0) { strcpy(Error, "\nError: Minute input is outside legal range\n The legal minute range is from 0 to 60\n"); return FALSE; } - if(minute == max_minute) + if (minute == max_minute) max_second = 0; - if(second > max_second || second < 0) - { + if (second > max_second || second < 0) { strcpy(Error, "\nError: Second input is outside legal range\n The legal second range is from 0 to 60\n"); return FALSE; } return TRUE; } /*MAG_ValidateDMSstring*/ -int MAG_Warnings(int control, double value, MAGtype_MagneticModel *MagneticModel) +int +MAG_Warnings(int control, double value, MAGtype_MagneticModel* MagneticModel) /*Return value 0 means end program, Return value 1 means get new data, Return value 2 means continue. - This prints a warning to the screen determined by the control integer. It also takes the value of the parameter causing the warning as a double. This is unnecessary for some warnings. - It requires the MagneticModel to determine the current epoch. + This prints a warning to the screen determined by the control integer. It also takes the value of the parameter +causing the warning as a double. This is unnecessary for some warnings. It requires the MagneticModel to determine the +current epoch. INPUT control :int : (Warning number) value : double: Magnetic field strength @@ -998,34 +1077,35 @@ CALLS : none { char ans[20]; strcpy(ans, ""); - - switch(control) { - case 1:/* Horizontal Field strength low */ + + switch (control) { + case 1: /* Horizontal Field strength low */ do { printf("\nCaution: location is approaching the blackout zone around the magnetic pole as\n"); printf(" defined by the WMM military specification \n"); 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 */ + case 2: /* Horizontal Field strength very low */ do { printf("\nWarning: location is in the blackout zone around the magnetic pole as defined\n"); printf(" by the WMM military specification \n"); printf(" (https://www.ngdc.noaa.gov/geomag/WMM/data/MIL-PRF-89500B.pdf). Compass\n"); printf(" accuracy is highly degraded in this region.\n"); - } while(NULL == fgets(ans, 20, stdin)); + } while (NULL == fgets(ans, 20, stdin)); break; - case 3:/* Elevation outside the recommended range */ - printf("\nWarning: The value you have entered of %.1f km for the elevation is outside of the recommended range.\n Elevations above -10.0 km are recommended for accurate results. \n", value); - while(1) - { + case 3: /* Elevation outside the recommended range */ + printf("\nWarning: The value you have entered of %.1f km for the elevation is outside of the recommended " + "range.\n Elevations above -10.0 km are recommended for accurate results. \n", + value); + while (1) { printf("\nPlease press 'C' to continue, 'G' to get new data or 'X' to exit...\n"); - while( NULL == fgets(ans, 20, stdin)) { + while (NULL == fgets(ans, 20, stdin)) { printf("\nInvalid input\n"); } - switch(ans[0]) { + switch (ans[0]) { case 'X': case 'x': return 0; @@ -1042,7 +1122,7 @@ CALLS : none } break; - case 4:/*Date outside the recommended range*/ + case 4: /*Date outside the recommended range*/ printf("\nWARNING - TIME EXTENDS BEYOND INTENDED USAGE RANGE\n CONTACT NCEI FOR PRODUCT UPDATES:\n"); printf(" National Centers for Environmental Information\n"); printf(" NOAA E/NE42\n"); @@ -1052,15 +1132,15 @@ CALLS : none printf(" Phone: (303) 497-4642 or -6522\n"); printf(" Email: geomag.models@noaa.gov\n"); printf(" Web: http://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml\n"); - printf("\n VALID RANGE = %d - %d\n", (int) MagneticModel->epoch, (int) MagneticModel->CoefficientFileEndDate); + printf( + "\n VALID RANGE = %d - %d\n", (int)MagneticModel->epoch, (int)MagneticModel->CoefficientFileEndDate); printf(" TIME = %f\n", value); - while(1) - { + while (1) { printf("\nPlease press 'C' to continue, 'N' to enter new data or 'X' to exit...\n"); - while (NULL ==fgets(ans, 20, stdin)){ + while (NULL == fgets(ans, 20, stdin)) { printf("\nInvalid input\n"); } - switch(ans[0]) { + switch (ans[0]) { case 'X': case 'x': return 0; @@ -1076,15 +1156,16 @@ CALLS : none } } break; - case 5:/*Elevation outside the allowable range*/ - printf("\nError: The value you have entered of %f km for the elevation is outside of the recommended range.\n Elevations above -10.0 km are recommended for accurate results. \n", value); - while(1) - { + case 5: /*Elevation outside the allowable range*/ + printf("\nError: The value you have entered of %f km for the elevation is outside of the recommended " + "range.\n Elevations above -10.0 km are recommended for accurate results. \n", + value); + while (1) { printf("\nPlease press 'C' to continue, 'G' to get new data or 'X' to exit...\n"); - while (NULL ==fgets(ans, 20, stdin)){ + while (NULL == fgets(ans, 20, stdin)) { printf("\nInvalid input\n"); } - switch(ans[0]) { + switch (ans[0]) { case 'X': case 'x': return 0; @@ -1106,15 +1187,14 @@ CALLS : none /*End of User Interface functions*/ - /****************************************************************************** ********************************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. ******************************************************************************/ - -MAGtype_LegendreFunction *MAG_AllocateLegendreFunctionMemory(int NumTerms) +MAGtype_LegendreFunction* +MAG_AllocateLegendreFunctionMemory(int NumTerms) /* Allocate memory for Associated Legendre Function data types. Should be called before computing Associated Legendre Functions. @@ -1132,31 +1212,29 @@ CALLS : none */ { - MAGtype_LegendreFunction *LegendreFunction; + MAGtype_LegendreFunction* LegendreFunction; - LegendreFunction = (MAGtype_LegendreFunction *) calloc(1, sizeof (MAGtype_LegendreFunction)); + LegendreFunction = (MAGtype_LegendreFunction*)calloc(1, sizeof(MAGtype_LegendreFunction)); - if(!LegendreFunction) - { + if (!LegendreFunction) { MAG_Error(1); return NULL; } - LegendreFunction->Pcup = (double *) malloc((NumTerms + 1) * sizeof ( double)); - if(LegendreFunction->Pcup == 0) - { + LegendreFunction->Pcup = (double*)malloc((NumTerms + 1) * sizeof(double)); + if (LegendreFunction->Pcup == 0) { MAG_Error(1); return NULL; } - LegendreFunction->dPcup = (double *) malloc((NumTerms + 1) * sizeof ( double)); - if(LegendreFunction->dPcup == 0) - { + LegendreFunction->dPcup = (double*)malloc((NumTerms + 1) * sizeof(double)); + if (LegendreFunction->dPcup == 0) { MAG_Error(1); return NULL; } return LegendreFunction; } /*MAGtype_LegendreFunction*/ -MAGtype_MagneticModel *MAG_AllocateModelMemory(int NumTerms) +MAGtype_MagneticModel* +MAG_AllocateModelMemory(int NumTerms) /* Allocate memory for WMM Coefficients * Should be called before reading the model file * @@ -1174,48 +1252,43 @@ MAGtype_MagneticModel *MAG_AllocateModelMemory(int NumTerms) double *Secular_Var_Coeff_H; CD - Gauss coefficients of secular geomagnetic model (nT/yr) int nMax; Maximum degree of spherical harmonic model int nMaxSecVar; Maxumum degree of spherical harmonic secular model - int SecularVariationUsed; Whether or not the magnetic secular variation vector will be needed by program + int SecularVariationUsed; Whether or not the magnetic secular variation vector will be needed by +program FALSE: Failed to allocate memory CALLS : none */ { - MAGtype_MagneticModel *MagneticModel; + MAGtype_MagneticModel* MagneticModel; int i; + MagneticModel = (MAGtype_MagneticModel*)calloc(1, sizeof(MAGtype_MagneticModel)); - MagneticModel = (MAGtype_MagneticModel *) calloc(1, sizeof (MAGtype_MagneticModel)); - - if(MagneticModel == NULL) - { + if (MagneticModel == NULL) { MAG_Error(2); return NULL; } - MagneticModel->Main_Field_Coeff_G = (double *) malloc((NumTerms + 1) * sizeof ( double)); + MagneticModel->Main_Field_Coeff_G = (double*)malloc((NumTerms + 1) * sizeof(double)); - if(MagneticModel->Main_Field_Coeff_G == NULL) - { + if (MagneticModel->Main_Field_Coeff_G == NULL) { MAG_Error(2); return NULL; } - MagneticModel->Main_Field_Coeff_H = (double *) malloc((NumTerms + 1) * sizeof ( double)); + MagneticModel->Main_Field_Coeff_H = (double*)malloc((NumTerms + 1) * sizeof(double)); - if(MagneticModel->Main_Field_Coeff_H == NULL) - { + if (MagneticModel->Main_Field_Coeff_H == NULL) { MAG_Error(2); return NULL; } - MagneticModel->Secular_Var_Coeff_G = (double *) malloc((NumTerms + 1) * sizeof ( double)); - if(MagneticModel->Secular_Var_Coeff_G == NULL) - { + MagneticModel->Secular_Var_Coeff_G = (double*)malloc((NumTerms + 1) * sizeof(double)); + if (MagneticModel->Secular_Var_Coeff_G == NULL) { MAG_Error(2); return NULL; } - MagneticModel->Secular_Var_Coeff_H = (double *) malloc((NumTerms + 1) * sizeof ( double)); - if(MagneticModel->Secular_Var_Coeff_H == NULL) - { + MagneticModel->Secular_Var_Coeff_H = (double*)malloc((NumTerms + 1) * sizeof(double)); + if (MagneticModel->Secular_Var_Coeff_H == NULL) { MAG_Error(2); return NULL; } @@ -1226,33 +1299,35 @@ 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*/ -MAGtype_SphericalHarmonicVariables* MAG_AllocateSphVarMemory(int nMax) +MAGtype_SphericalHarmonicVariables* +MAG_AllocateSphVarMemory(int nMax) { MAGtype_SphericalHarmonicVariables* SphVariables; - SphVariables = (MAGtype_SphericalHarmonicVariables*) calloc(1, sizeof(MAGtype_SphericalHarmonicVariables)); - SphVariables->RelativeRadiusPower = (double *) malloc((nMax + 1) * sizeof ( double)); - SphVariables->cos_mlambda = (double *) malloc((nMax + 1) * sizeof (double)); - SphVariables->sin_mlambda = (double *) malloc((nMax + 1) * sizeof (double)); + SphVariables = (MAGtype_SphericalHarmonicVariables*)calloc(1, sizeof(MAGtype_SphericalHarmonicVariables)); + SphVariables->RelativeRadiusPower = (double*)malloc((nMax + 1) * sizeof(double)); + SphVariables->cos_mlambda = (double*)malloc((nMax + 1) * sizeof(double)); + SphVariables->sin_mlambda = (double*)malloc((nMax + 1) * sizeof(double)); return SphVariables; } /*MAG_AllocateSphVarMemory*/ -void MAG_AssignHeaderValues(MAGtype_MagneticModel *model, char values[][MAXLINELENGTH]) +void +MAG_AssignHeaderValues(MAGtype_MagneticModel* model, char values[][MAXLINELENGTH]) { /* MAGtype_Date releasedate; */ strcpy(model->ModelName, values[MODELNAME]); - /* releasedate.Year = 0; + /* releasedate.Year = 0; releasedate.Day = 0; releasedate.Month = 0; releasedate.DecimalYear = 0; @@ -1263,25 +1338,24 @@ void MAG_AssignHeaderValues(MAGtype_MagneticModel *model, char values[][MAXLINEL model->nMax = atoi(values[INTSTATICDEG]); model->nMaxSecVar = atoi(values[INTSECVARDEG]); model->CoefficientFileEndDate = atof(values[MODELENDYEAR]); - if(model->nMaxSecVar > 0) + if (model->nMaxSecVar > 0) model->SecularVariationUsed = 1; else model->SecularVariationUsed = 0; } -void MAG_AssignMagneticModelCoeffs(MAGtype_MagneticModel *Assignee, MAGtype_MagneticModel *Source, int nMax, int nMaxSecVar) -/* This function assigns the first nMax degrees of the Source model to the Assignee model, leaving the other coefficients - untouched*/ +void +MAG_AssignMagneticModelCoeffs(MAGtype_MagneticModel* Assignee, MAGtype_MagneticModel* Source, int nMax, int nMaxSecVar) +/* This function assigns the first nMax degrees of the Source model to the Assignee model, leaving the other + coefficients untouched*/ { int n, m, index; assert(nMax <= Source->nMax); assert(nMax <= Assignee->nMax); assert(nMaxSecVar <= Source->nMaxSecVar); assert(nMaxSecVar <= Assignee->nMaxSecVar); - for(n = 1; n <= nMaxSecVar; n++) - { - for(m = 0; m <= n; m++) - { + for (n = 1; n <= nMaxSecVar; n++) { + for (m = 0; m <= n; m++) { index = (n * (n + 1) / 2 + m); Assignee->Main_Field_Coeff_G[index] = Source->Main_Field_Coeff_G[index]; Assignee->Main_Field_Coeff_H[index] = Source->Main_Field_Coeff_H[index]; @@ -1289,10 +1363,8 @@ void MAG_AssignMagneticModelCoeffs(MAGtype_MagneticModel *Assignee, MAGtype_Magn Assignee->Secular_Var_Coeff_H[index] = Source->Secular_Var_Coeff_H[index]; } } - for(n = nMaxSecVar + 1; n <= nMax; n++) - { - for(m = 0; m <= n; m++) - { + for (n = nMaxSecVar + 1; n <= nMax; n++) { + for (m = 0; m <= n; m++) { index = (n * (n + 1) / 2 + m); Assignee->Main_Field_Coeff_G[index] = Source->Main_Field_Coeff_G[index]; Assignee->Main_Field_Coeff_H[index] = Source->Main_Field_Coeff_H[index]; @@ -1301,7 +1373,10 @@ void MAG_AssignMagneticModelCoeffs(MAGtype_MagneticModel *Assignee, MAGtype_Magn return; } /*MAG_AssignMagneticModelCoeffs*/ -int MAG_FreeMemory(MAGtype_MagneticModel *MagneticModel, MAGtype_MagneticModel *TimedMagneticModel, MAGtype_LegendreFunction *LegendreFunction) +int +MAG_FreeMemory(MAGtype_MagneticModel* MagneticModel, + MAGtype_MagneticModel* TimedMagneticModel, + MAGtype_LegendreFunction* LegendreFunction) /* Free memory used by WMM functions. Only to be called at the end of the main function. INPUT : MagneticModel pointer to data structure with the following elements @@ -1315,7 +1390,8 @@ INPUT : MagneticModel pointer to data structure with the following elements double *Secular_Var_Coeff_H; CD - Gauss coefficients of secular geomagnetic model (nT/yr) int nMax; Maximum degree of spherical harmonic model int nMaxSecVar; Maxumum degree of spherical harmonic secular model - int SecularVariationUsed; Whether or not the magnetic secular variation vector will be needed by program + int SecularVariationUsed; Whether or not the magnetic secular variation vector will be needed by +program TimedMagneticModel Pointer to data structure similar to the first input. LegendreFunction Pointer to data structure with the following elements @@ -1327,71 +1403,58 @@ CALLS : none */ { - if(MagneticModel->Main_Field_Coeff_G) - { + if (MagneticModel->Main_Field_Coeff_G) { free(MagneticModel->Main_Field_Coeff_G); MagneticModel->Main_Field_Coeff_G = NULL; } - if(MagneticModel->Main_Field_Coeff_H) - { + if (MagneticModel->Main_Field_Coeff_H) { free(MagneticModel->Main_Field_Coeff_H); MagneticModel->Main_Field_Coeff_H = NULL; } - if(MagneticModel->Secular_Var_Coeff_G) - { + if (MagneticModel->Secular_Var_Coeff_G) { free(MagneticModel->Secular_Var_Coeff_G); MagneticModel->Secular_Var_Coeff_G = NULL; } - if(MagneticModel->Secular_Var_Coeff_H) - { + if (MagneticModel->Secular_Var_Coeff_H) { free(MagneticModel->Secular_Var_Coeff_H); MagneticModel->Secular_Var_Coeff_H = NULL; } - if(MagneticModel) - { + if (MagneticModel) { free(MagneticModel); MagneticModel = NULL; } - if(TimedMagneticModel->Main_Field_Coeff_G) - { + if (TimedMagneticModel->Main_Field_Coeff_G) { free(TimedMagneticModel->Main_Field_Coeff_G); TimedMagneticModel->Main_Field_Coeff_G = NULL; } - if(TimedMagneticModel->Main_Field_Coeff_H) - { + if (TimedMagneticModel->Main_Field_Coeff_H) { free(TimedMagneticModel->Main_Field_Coeff_H); TimedMagneticModel->Main_Field_Coeff_H = NULL; } - if(TimedMagneticModel->Secular_Var_Coeff_G) - { + if (TimedMagneticModel->Secular_Var_Coeff_G) { free(TimedMagneticModel->Secular_Var_Coeff_G); TimedMagneticModel->Secular_Var_Coeff_G = NULL; } - if(TimedMagneticModel->Secular_Var_Coeff_H) - { + if (TimedMagneticModel->Secular_Var_Coeff_H) { free(TimedMagneticModel->Secular_Var_Coeff_H); TimedMagneticModel->Secular_Var_Coeff_H = NULL; } - if(TimedMagneticModel) - { + if (TimedMagneticModel) { free(TimedMagneticModel); TimedMagneticModel = NULL; } - if(LegendreFunction->Pcup) - { + if (LegendreFunction->Pcup) { free(LegendreFunction->Pcup); LegendreFunction->Pcup = NULL; } - if(LegendreFunction->dPcup) - { + if (LegendreFunction->dPcup) { free(LegendreFunction->dPcup); LegendreFunction->dPcup = NULL; } - if(LegendreFunction) - { + if (LegendreFunction) { free(LegendreFunction); LegendreFunction = NULL; } @@ -1399,7 +1462,8 @@ CALLS : none return TRUE; } /*MAG_FreeMemory */ -int MAG_FreeMagneticModelMemory(MAGtype_MagneticModel *MagneticModel) +int +MAG_FreeMagneticModelMemory(MAGtype_MagneticModel* MagneticModel) /* Free the magnetic model memory used by WMM functions. INPUT : MagneticModel pointer to data structure with the following elements @@ -1413,35 +1477,31 @@ INPUT : MagneticModel pointer to data structure with the following elements double *Secular_Var_Coeff_H; CD - Gauss coefficients of secular geomagnetic model (nT/yr) int nMax; Maximum degree of spherical harmonic model int nMaxSecVar; Maxumum degree of spherical harmonic secular model - int SecularVariationUsed; Whether or not the magnetic secular variation vector will be needed by program + int SecularVariationUsed; Whether or not the magnetic secular variation vector will be needed by +program OUTPUT none CALLS : none */ { - if(MagneticModel->Main_Field_Coeff_G) - { + if (MagneticModel->Main_Field_Coeff_G) { free(MagneticModel->Main_Field_Coeff_G); MagneticModel->Main_Field_Coeff_G = NULL; } - if(MagneticModel->Main_Field_Coeff_H) - { + if (MagneticModel->Main_Field_Coeff_H) { free(MagneticModel->Main_Field_Coeff_H); MagneticModel->Main_Field_Coeff_H = NULL; } - if(MagneticModel->Secular_Var_Coeff_G) - { + if (MagneticModel->Secular_Var_Coeff_G) { free(MagneticModel->Secular_Var_Coeff_G); MagneticModel->Secular_Var_Coeff_G = NULL; } - if(MagneticModel->Secular_Var_Coeff_H) - { + if (MagneticModel->Secular_Var_Coeff_H) { free(MagneticModel->Secular_Var_Coeff_H); MagneticModel->Secular_Var_Coeff_H = NULL; } - if(MagneticModel) - { + if (MagneticModel) { free(MagneticModel); MagneticModel = NULL; } @@ -1449,7 +1509,8 @@ CALLS : none return TRUE; } /*MAG_FreeMagneticModelMemory */ -int MAG_FreeLegendreMemory(MAGtype_LegendreFunction *LegendreFunction) +int +MAG_FreeLegendreMemory(MAGtype_LegendreFunction* LegendreFunction) /* Free the Legendre Coefficients memory used by the WMM functions. INPUT : LegendreFunction Pointer to data structure with the following elements @@ -1461,18 +1522,15 @@ CALLS : none */ { - if(LegendreFunction->Pcup) - { + if (LegendreFunction->Pcup) { free(LegendreFunction->Pcup); LegendreFunction->Pcup = NULL; } - if(LegendreFunction->dPcup) - { + if (LegendreFunction->dPcup) { free(LegendreFunction->dPcup); LegendreFunction->dPcup = NULL; } - if(LegendreFunction) - { + if (LegendreFunction) { free(LegendreFunction); LegendreFunction = NULL; } @@ -1480,7 +1538,8 @@ CALLS : none return TRUE; } /*MAG_FreeLegendreMemory */ -int MAG_FreeSphVarMemory(MAGtype_SphericalHarmonicVariables *SphVar) +int +MAG_FreeSphVarMemory(MAGtype_SphericalHarmonicVariables* SphVar) /* Free the Spherical Harmonic Variable memory used by the WMM functions. INPUT : LegendreFunction Pointer to data structure with the following elements @@ -1491,23 +1550,19 @@ INPUT : LegendreFunction Pointer to data structure with the following elements CALLS : none */ { - if(SphVar->RelativeRadiusPower) - { + if (SphVar->RelativeRadiusPower) { free(SphVar->RelativeRadiusPower); SphVar->RelativeRadiusPower = NULL; } - if(SphVar->cos_mlambda) - { + if (SphVar->cos_mlambda) { free(SphVar->cos_mlambda); SphVar->cos_mlambda = NULL; } - if(SphVar->sin_mlambda) - { + if (SphVar->sin_mlambda) { free(SphVar->sin_mlambda); SphVar->sin_mlambda = NULL; } - if(SphVar) - { + if (SphVar) { free(SphVar); SphVar = NULL; } @@ -1515,10 +1570,11 @@ INPUT : LegendreFunction Pointer to data structure with the following elements return TRUE; } /*MAG_FreeSphVarMemory*/ -void MAG_PrintWMMFormat(char *filename, MAGtype_MagneticModel *MagneticModel) +void +MAG_PrintWMMFormat(char* filename, MAGtype_MagneticModel* MagneticModel) { int index, n, m; - FILE *OUT; + FILE* OUT; MAGtype_Date Date; char Datestring[11]; @@ -1526,25 +1582,39 @@ void MAG_PrintWMMFormat(char *filename, MAGtype_MagneticModel *MagneticModel) MAG_YearToDate(&Date); sprintf(Datestring, "%d/%d/%d", Date.Month, Date.Day, Date.Year); OUT = fopen(filename, "w"); - fprintf(OUT, " %.1f %s %s\n", MagneticModel->epoch, MagneticModel->ModelName, Datestring); - for(n = 1; n <= MagneticModel->nMax; n++) - { - for(m = 0; m <= n; m++) - { + fprintf( + OUT, " %.1f %s %s\n", MagneticModel->epoch, MagneticModel->ModelName, Datestring); + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { index = (n * (n + 1) / 2 + m); - if(m != 0) - fprintf(OUT, " %2d %2d %9.4f %9.4f %9.4f %9.4f\n", n, m, MagneticModel->Main_Field_Coeff_G[index], MagneticModel->Main_Field_Coeff_H[index], MagneticModel->Secular_Var_Coeff_G[index], MagneticModel->Secular_Var_Coeff_H[index]); + if (m != 0) + fprintf(OUT, + " %2d %2d %9.4f %9.4f %9.4f %9.4f\n", + n, + m, + MagneticModel->Main_Field_Coeff_G[index], + MagneticModel->Main_Field_Coeff_H[index], + MagneticModel->Secular_Var_Coeff_G[index], + MagneticModel->Secular_Var_Coeff_H[index]); else - fprintf(OUT, " %2d %2d %9.4f %9.4f %9.4f %9.4f\n", n, m, MagneticModel->Main_Field_Coeff_G[index], 0.0, MagneticModel->Secular_Var_Coeff_G[index], 0.0); + fprintf(OUT, + " %2d %2d %9.4f %9.4f %9.4f %9.4f\n", + n, + m, + MagneticModel->Main_Field_Coeff_G[index], + 0.0, + MagneticModel->Secular_Var_Coeff_G[index], + 0.0); } } fclose(OUT); } /*MAG_PrintWMMFormat*/ -void MAG_PrintEMMFormat(char *filename, char *filenameSV, MAGtype_MagneticModel *MagneticModel) +void +MAG_PrintEMMFormat(char* filename, char* filenameSV, MAGtype_MagneticModel* MagneticModel) { int index, n, m; - FILE *OUT; + FILE* OUT; MAGtype_Date Date; char Datestring[11]; @@ -1552,27 +1622,34 @@ void MAG_PrintEMMFormat(char *filename, char *filenameSV, MAGtype_MagneticModel MAG_YearToDate(&Date); sprintf(Datestring, "%d/%d/%d", Date.Month, Date.Day, Date.Year); OUT = fopen(filename, "w"); - fprintf(OUT, " %.1f %s %s\n", MagneticModel->epoch, MagneticModel->ModelName, Datestring); - for(n = 1; n <= MagneticModel->nMax; n++) - { - for(m = 0; m <= n; m++) - { + fprintf( + OUT, " %.1f %s %s\n", MagneticModel->epoch, MagneticModel->ModelName, Datestring); + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { index = (n * (n + 1) / 2 + m); - if(m != 0) - fprintf(OUT, " %2d %2d %9.4f %9.4f\n", n, m, MagneticModel->Main_Field_Coeff_G[index], MagneticModel->Main_Field_Coeff_H[index]); + if (m != 0) + fprintf(OUT, + " %2d %2d %9.4f %9.4f\n", + n, + m, + MagneticModel->Main_Field_Coeff_G[index], + MagneticModel->Main_Field_Coeff_H[index]); else fprintf(OUT, " %2d %2d %9.4f %9.4f\n", n, m, MagneticModel->Main_Field_Coeff_G[index], 0.0); } } fclose(OUT); OUT = fopen(filenameSV, "w"); - for(n = 1; n <= MagneticModel->nMaxSecVar; n++) - { - for(m = 0; m <= n; m++) - { + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { + for (m = 0; m <= n; m++) { index = (n * (n + 1) / 2 + m); - if(m != 0) - fprintf(OUT, " %2d %2d %9.4f %9.4f\n", n, m, MagneticModel->Secular_Var_Coeff_G[index], MagneticModel->Secular_Var_Coeff_H[index]); + if (m != 0) + fprintf(OUT, + " %2d %2d %9.4f %9.4f\n", + n, + m, + MagneticModel->Secular_Var_Coeff_G[index], + MagneticModel->Secular_Var_Coeff_H[index]); else fprintf(OUT, " %2d %2d %9.4f %9.4f\n", n, m, MagneticModel->Secular_Var_Coeff_G[index], 0.0); } @@ -1581,60 +1658,83 @@ void MAG_PrintEMMFormat(char *filename, char *filenameSV, MAGtype_MagneticModel return; } /*MAG_PrintEMMFormat*/ -void MAG_PrintSHDFFormat(char *filename, MAGtype_MagneticModel *(*MagneticModel)[], int epochs) +void +MAG_PrintSHDFFormat(char* filename, MAGtype_MagneticModel* (*MagneticModel)[], int epochs) { - int i, n, m, index, epochRange; - FILE *SHDF_file; - SHDF_file = fopen(filename, "w"); - /*lines = (int)(UFM_DEGREE / 2.0 * (UFM_DEGREE + 3));*/ - for(i = 0; i < epochs; i++) - { - if(i < epochs - 1) epochRange = (*MagneticModel)[i+1]->epoch - (*MagneticModel)[i]->epoch; - else epochRange = (*MagneticModel)[i]->epoch - (*MagneticModel)[i-1]->epoch; - fprintf(SHDF_file, "%%SHDF 16695 Definitive Geomagnetic Reference Field Model Coefficient File\n"); - fprintf(SHDF_file, "%%ModelName: %s\n", (*MagneticModel)[i]->ModelName); - fprintf(SHDF_file, "%%Publisher: International Association of Geomagnetism and Aeronomy (IAGA), Working Group V-Mod\n"); - fprintf(SHDF_file, "%%ReleaseDate: Some Number\n"); - fprintf(SHDF_file, "%%DataCutOFF: Some Other Number\n"); - fprintf(SHDF_file, "%%ModelStartYear: %d\n", (int)(*MagneticModel)[i]->epoch); - fprintf(SHDF_file, "%%ModelEndYear: %d\n", (int)(*MagneticModel)[i]->epoch+epochRange); - fprintf(SHDF_file, "%%Epoch: %.0f\n", (*MagneticModel)[i]->epoch); - fprintf(SHDF_file, "%%IntStaticDeg: %d\n", (*MagneticModel)[i]->nMax); - fprintf(SHDF_file, "%%IntSecVarDeg: %d\n", (*MagneticModel)[i]->nMaxSecVar); - 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, "# 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"); - n = 1; - m = 0; - for(n = 1; n <= (*MagneticModel)[i]->nMax; n++) - { - for(m = 0; m <= n; m++) - { - index = (n * (n+1)) / 2 + m; - if(i < epochs - 1) - { - if(m != 0) - fprintf(SHDF_file, "I,%d,%d,%f,%f,%f,%f\n", n, m, (*MagneticModel)[i]->Main_Field_Coeff_G[index], (*MagneticModel)[i]->Main_Field_Coeff_H[index], (*MagneticModel)[i]->Secular_Var_Coeff_G[index], (*MagneticModel)[i]->Secular_Var_Coeff_H[index]); - else - fprintf(SHDF_file, "I,%d,%d,%f,,%f,\n", n, m, (*MagneticModel)[i]->Main_Field_Coeff_G[index], (*MagneticModel)[i]->Secular_Var_Coeff_G[index]); - } - else - { - if(m != 0) - fprintf(SHDF_file, "I,%d,%d,%f,%f,%f,%f\n", n, m, (*MagneticModel)[i]->Main_Field_Coeff_G[index], (*MagneticModel)[i]->Main_Field_Coeff_H[index], (*MagneticModel)[i]->Secular_Var_Coeff_G[index], (*MagneticModel)[i]->Secular_Var_Coeff_H[index]); - else - fprintf(SHDF_file, "I,%d,%d,%f,,%f,\n", n, m, (*MagneticModel)[i]->Main_Field_Coeff_G[index], (*MagneticModel)[i]->Secular_Var_Coeff_G[index]); - } - } - } - } + int i, n, m, index, epochRange; + FILE* SHDF_file; + SHDF_file = fopen(filename, "w"); + /*lines = (int)(UFM_DEGREE / 2.0 * (UFM_DEGREE + 3));*/ + for (i = 0; i < epochs; i++) { + if (i < epochs - 1) + epochRange = (*MagneticModel)[i + 1]->epoch - (*MagneticModel)[i]->epoch; + else + epochRange = (*MagneticModel)[i]->epoch - (*MagneticModel)[i - 1]->epoch; + fprintf(SHDF_file, "%%SHDF 16695 Definitive Geomagnetic Reference Field Model Coefficient File\n"); + fprintf(SHDF_file, "%%ModelName: %s\n", (*MagneticModel)[i]->ModelName); + fprintf(SHDF_file, + "%%Publisher: International Association of Geomagnetism and Aeronomy (IAGA), Working Group V-Mod\n"); + fprintf(SHDF_file, "%%ReleaseDate: Some Number\n"); + fprintf(SHDF_file, "%%DataCutOFF: Some Other Number\n"); + fprintf(SHDF_file, "%%ModelStartYear: %d\n", (int)(*MagneticModel)[i]->epoch); + fprintf(SHDF_file, "%%ModelEndYear: %d\n", (int)(*MagneticModel)[i]->epoch + epochRange); + fprintf(SHDF_file, "%%Epoch: %.0f\n", (*MagneticModel)[i]->epoch); + fprintf(SHDF_file, "%%IntStaticDeg: %d\n", (*MagneticModel)[i]->nMax); + fprintf(SHDF_file, "%%IntSecVarDeg: %d\n", (*MagneticModel)[i]->nMaxSecVar); + 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, "# 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"); + n = 1; + m = 0; + for (n = 1; n <= (*MagneticModel)[i]->nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1)) / 2 + m; + if (i < epochs - 1) { + if (m != 0) + fprintf(SHDF_file, + "I,%d,%d,%f,%f,%f,%f\n", + n, + m, + (*MagneticModel)[i]->Main_Field_Coeff_G[index], + (*MagneticModel)[i]->Main_Field_Coeff_H[index], + (*MagneticModel)[i]->Secular_Var_Coeff_G[index], + (*MagneticModel)[i]->Secular_Var_Coeff_H[index]); + else + fprintf(SHDF_file, + "I,%d,%d,%f,,%f,\n", + n, + m, + (*MagneticModel)[i]->Main_Field_Coeff_G[index], + (*MagneticModel)[i]->Secular_Var_Coeff_G[index]); + } else { + if (m != 0) + fprintf(SHDF_file, + "I,%d,%d,%f,%f,%f,%f\n", + n, + m, + (*MagneticModel)[i]->Main_Field_Coeff_G[index], + (*MagneticModel)[i]->Main_Field_Coeff_H[index], + (*MagneticModel)[i]->Secular_Var_Coeff_G[index], + (*MagneticModel)[i]->Secular_Var_Coeff_H[index]); + else + fprintf(SHDF_file, + "I,%d,%d,%f,,%f,\n", + n, + m, + (*MagneticModel)[i]->Main_Field_Coeff_G[index], + (*MagneticModel)[i]->Secular_Var_Coeff_G[index]); + } + } + } + } } /*MAG_PrintSHDFFormat*/ -int MAG_readMagneticModel(char *filename, MAGtype_MagneticModel * MagneticModel) +int +MAG_readMagneticModel(char* filename, MAGtype_MagneticModel* MagneticModel) { /* READ WORLD Magnetic MODEL SPHERICAL HARMONIC COEFFICIENTS (WMM.cof) @@ -1644,22 +1744,20 @@ int MAG_readMagneticModel(char *filename, MAGtype_MagneticModel * MagneticModel) UPDATES : MagneticModel : Pointer to the data structure with the following fields populated char *ModelName; double epoch; Base time of Geomagnetic model epoch (yrs) - double *Main_Field_Coeff_G; C - Gauss coefficients of main geomagnetic model (nT) - double *Main_Field_Coeff_H; C - Gauss coefficients of main geomagnetic model (nT) - double *Secular_Var_Coeff_G; CD - Gauss coefficients of secular geomagnetic model (nT/yr) - double *Secular_Var_Coeff_H; CD - Gauss coefficients of secular geomagnetic model (nT/yr) - CALLS : none + double *Main_Field_Coeff_G; C - Gauss coefficients of main geomagnetic + model (nT) double *Main_Field_Coeff_H; C - Gauss coefficients of main geomagnetic model (nT) double + *Secular_Var_Coeff_G; CD - Gauss coefficients of secular geomagnetic model (nT/yr) double *Secular_Var_Coeff_H; + CD - Gauss coefficients of secular geomagnetic model (nT/yr) CALLS : none */ - FILE *MAG_COF_File; + FILE* MAG_COF_File; char c_str[81], c_new[5]; /*these strings are used to read a line from coefficient file*/ 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) - { + + if (MAG_COF_File == NULL) { MAG_Error(20); return FALSE; /* should we have a standard error printing routine ?*/ @@ -1671,27 +1769,23 @@ int MAG_readMagneticModel(char *filename, MAGtype_MagneticModel * MagneticModel) fgets(c_str, 80, MAG_COF_File); sscanf(c_str, "%lf%s", &epoch, MagneticModel->ModelName); MagneticModel->epoch = epoch; - while(EOF_Flag == 0) - { - if (NULL == fgets(c_str, 80, MAG_COF_File)){ + while (EOF_Flag == 0) { + if (NULL == fgets(c_str, 80, MAG_COF_File)) { break; } /* CHECK FOR LAST LINE IN FILE */ - for(i = 0; i < 4 && (c_str[i] != '\0'); i++) - { + for (i = 0; i < 4 && (c_str[i] != '\0'); i++) { c_new[i] = c_str[i]; c_new[i + 1] = '\0'; } icomp = strcmp("9999", c_new); - if(icomp == 0) - { + if (icomp == 0) { EOF_Flag = 1; break; } /* END OF FILE NOT ENCOUNTERED, GET VALUES */ sscanf(c_str, "%d%d%lf%lf%lf%lf", &n, &m, &gnm, &hnm, &dgnm, &dhnm); - if(m <= n) - { + if (m <= n) { index = (n * (n + 1) / 2 + m); MagneticModel->Main_Field_Coeff_G[index] = gnm; MagneticModel->Secular_Var_Coeff_G[index] = dgnm; @@ -1704,7 +1798,8 @@ int MAG_readMagneticModel(char *filename, MAGtype_MagneticModel * MagneticModel) return TRUE; } /*MAG_readMagneticModel*/ -int MAG_readMagneticModel_Large(char *filename, char *filenameSV, MAGtype_MagneticModel *MagneticModel) +int +MAG_readMagneticModel_Large(char* filename, char* filenameSV, MAGtype_MagneticModel* MagneticModel) /* To read the high-degree model coefficients (for example, NGDC 720) INPUT : filename file name for static coefficients @@ -1715,23 +1810,21 @@ int MAG_readMagneticModel_Large(char *filename, char *filenameSV, MAGtype_Magnet nMax : Number of static coefficients UPDATES : MagneticModel : Pointer to the data structure with the following fields populated double epoch; Base time of Geomagnetic model epoch (yrs) - double *Main_Field_Coeff_G; C - Gauss coefficients of main geomagnetic model (nT) - double *Main_Field_Coeff_H; C - Gauss coefficients of main geomagnetic model (nT) - double *Secular_Var_Coeff_G; CD - Gauss coefficients of secular geomagnetic model (nT/yr) - double *Secular_Var_Coeff_H; CD - Gauss coefficients of secular geomagnetic model (nT/yr) - CALLS : none + double *Main_Field_Coeff_G; C - Gauss coefficients of main geomagnetic model + (nT) double *Main_Field_Coeff_H; C - Gauss coefficients of main geomagnetic model (nT) double + *Secular_Var_Coeff_G; CD - Gauss coefficients of secular geomagnetic model (nT/yr) double *Secular_Var_Coeff_H; CD + - Gauss coefficients of secular geomagnetic model (nT/yr) CALLS : none */ { - FILE *MAG_COF_File; - FILE *MAG_COFSV_File; + FILE* MAG_COF_File; + FILE* MAG_COFSV_File; char c_str[81], c_str2[81]; /* these strings are used to read a line from coefficient file */ int i, m, n, index, a, b; double epoch, gnm, hnm, dgnm, dhnm; MAG_COF_File = fopen(filename, "r"); MAG_COFSV_File = fopen(filenameSV, "r"); - if(MAG_COF_File == NULL || MAG_COFSV_File == NULL) - { + if (MAG_COF_File == NULL || MAG_COFSV_File == NULL) { MAG_Error(20); return FALSE; } @@ -1739,7 +1832,7 @@ int MAG_readMagneticModel_Large(char *filename, char *filenameSV, MAGtype_Magnet MagneticModel->Main_Field_Coeff_G[0] = 0.0; MagneticModel->Secular_Var_Coeff_H[0] = 0.0; MagneticModel->Secular_Var_Coeff_G[0] = 0.0; - if (NULL == fgets(c_str, 80, MAG_COF_File)){ + if (NULL == fgets(c_str, 80, MAG_COF_File)) { fclose(MAG_COF_File); fclose(MAG_COFSV_File); return FALSE; @@ -1748,22 +1841,20 @@ int MAG_readMagneticModel_Large(char *filename, char *filenameSV, MAGtype_Magnet MagneticModel->epoch = epoch; a = CALCULATE_NUMTERMS(MagneticModel->nMaxSecVar); b = CALCULATE_NUMTERMS(MagneticModel->nMax); - for(i = 0; i < a; i++) - { - if (NULL == fgets(c_str, 80, MAG_COF_File)){ + for (i = 0; i < a; i++) { + if (NULL == fgets(c_str, 80, MAG_COF_File)) { fclose(MAG_COF_File); fclose(MAG_COFSV_File); return FALSE; } sscanf(c_str, "%d%d%lf%lf", &n, &m, &gnm, &hnm); - if (NULL == fgets(c_str2, 80, MAG_COFSV_File)){ + if (NULL == fgets(c_str2, 80, MAG_COFSV_File)) { fclose(MAG_COF_File); fclose(MAG_COFSV_File); return FALSE; } sscanf(c_str2, "%d%d%lf%lf", &n, &m, &dgnm, &dhnm); - if(m <= n) - { + if (m <= n) { index = (n * (n + 1) / 2 + m); MagneticModel->Main_Field_Coeff_G[index] = gnm; MagneticModel->Secular_Var_Coeff_G[index] = dgnm; @@ -1771,23 +1862,20 @@ int MAG_readMagneticModel_Large(char *filename, char *filenameSV, MAGtype_Magnet MagneticModel->Secular_Var_Coeff_H[index] = dhnm; } } - for(i = a; i < b; i++) - { - if (NULL == fgets(c_str, 80, MAG_COF_File)){ + for (i = a; i < b; i++) { + if (NULL == fgets(c_str, 80, MAG_COF_File)) { fclose(MAG_COF_File); fclose(MAG_COFSV_File); return FALSE; } sscanf(c_str, "%d%d%lf%lf", &n, &m, &gnm, &hnm); - if(m <= n) - { + if (m <= n) { index = (n * (n + 1) / 2 + m); MagneticModel->Main_Field_Coeff_G[index] = gnm; MagneticModel->Main_Field_Coeff_H[index] = hnm; } } - if(MAG_COF_File != NULL && MAG_COFSV_File != NULL) - { + if (MAG_COF_File != NULL && MAG_COFSV_File != NULL) { fclose(MAG_COF_File); fclose(MAG_COFSV_File); } @@ -1795,7 +1883,8 @@ int MAG_readMagneticModel_Large(char *filename, char *filenameSV, MAGtype_Magnet return TRUE; } /*MAG_readMagneticModel_Large*/ -int MAG_readMagneticModel_SHDF(char *filename, MAGtype_MagneticModel *(*magneticmodels)[], int array_size) +int +MAG_readMagneticModel_SHDF(char* filename, MAGtype_MagneticModel* (*magneticmodels)[], int array_size) /* * MAG_readMagneticModels - Read the Magnetic Models from an SHDF format file * @@ -1816,26 +1905,14 @@ int MAG_readMagneticModel_SHDF(char *filename, MAGtype_MagneticModel *(*magnetic */ { char paramkeys[NOOFPARAMS][MAXLINELENGTH] = { - "SHDF ", - "ModelName: ", - "Publisher: ", - "ReleaseDate: ", - "DataCutOff: ", - "ModelStartYear: ", - "ModelEndYear: ", - "Epoch: ", - "IntStaticDeg: ", - "IntSecVarDeg: ", - "ExtStaticDeg: ", - "ExtSecVarDeg: ", - "GeoMagRefRad: ", - "Normalization: ", - "SpatBasFunc: " + "SHDF ", "ModelName: ", "Publisher: ", "ReleaseDate: ", "DataCutOff: ", "ModelStartYear: ", + "ModelEndYear: ", "Epoch: ", "IntStaticDeg: ", "IntSecVarDeg: ", "ExtStaticDeg: ", "ExtSecVarDeg: ", + "GeoMagRefRad: ", "Normalization: ", "SpatBasFunc: " }; char paramvalues[NOOFPARAMS][MAXLINELENGTH]; - char *line = (char *) malloc(MAXLINELENGTH); - char *ptrreset; + char* line = (char*)malloc(MAXLINELENGTH); + char* ptrreset; char paramvalue[MAXLINELENGTH]; int paramvaluelength = 0; int paramkeylength = 0; @@ -1851,55 +1928,46 @@ int MAG_readMagneticModel_SHDF(char *filename, MAGtype_MagneticModel *(*magnetic int n, m; double gnm, hnm, dgnm, dhnm, cutoff; int index; - - FILE *stream; + + FILE* stream; ptrreset = line; stream = fopen(filename, READONLYMODE); - if(stream == NULL) - { + if (stream == NULL) { perror("File open error"); return header_index; } /* Read records from the model file and store header information. */ - while(fgets(line, MAXLINELENGTH, stream) != NULL) - { + while (fgets(line, MAXLINELENGTH, stream) != NULL) { j++; - if(strlen(MAG_Trim(line)) == 0) + if (strlen(MAG_Trim(line)) == 0) continue; - if(*line == '%') - { + if (*line == '%') { line++; - if(newrecord) - { - if(header_index > -1) - { + if (newrecord) { + if (header_index > -1) { MAG_AssignHeaderValues((*magneticmodels)[header_index], paramvalues); } header_index++; - if(header_index >= array_size) - { + if (header_index >= array_size) { fprintf(stderr, "Header limit exceeded - too many models in model file. (%d)\n", header_index); return array_size + 1; } newrecord = 0; allocationflag = 0; } - for(i = 0; i < NOOFPARAMS; i++) - { + for (i = 0; i < NOOFPARAMS; i++) { - paramkeylength = (int) strlen(paramkeys[i]); - if(!strncmp(line, paramkeys[i], paramkeylength)) - { - paramvaluelength = (int) strlen(line) - paramkeylength; + paramkeylength = (int)strlen(paramkeys[i]); + if (!strncmp(line, paramkeys[i], paramkeylength)) { + paramvaluelength = (int)strlen(line) - paramkeylength; strncpy(paramvalue, line + paramkeylength, paramvaluelength); paramvalue[paramvaluelength] = '\0'; strcpy(paramvalues[i], paramvalue); - if(!strcmp(paramkeys[i], paramkeys[INTSTATICDEG]) || !strcmp(paramkeys[i], paramkeys[EXTSTATICDEG])) - { + if (!strcmp(paramkeys[i], paramkeys[INTSTATICDEG]) || + !strcmp(paramkeys[i], paramkeys[EXTSTATICDEG])) { tempint = atoi(paramvalues[i]); - if(tempint > 0 && allocationflag == 0) - { + if (tempint > 0 && allocationflag == 0) { numterms = CALCULATE_NUMTERMS(tempint); (*magneticmodels)[header_index] = MAG_AllocateModelMemory(numterms); /* model = (*magneticmodels)[header_index]; */ @@ -1910,27 +1978,22 @@ int MAG_readMagneticModel_SHDF(char *filename, MAGtype_MagneticModel *(*magnetic } } line--; - } else if(*line == '#') - { + } else if (*line == '#') { /* process comments */ - } else if(sscanf(line, "%c,%d,%d", &coefftype, &n, &m) == 3) - { - if(m == 0) - { + } else if (sscanf(line, "%c,%d,%d", &coefftype, &n, &m) == 3) { + if (m == 0) { sscanf(line, "%c,%d,%d,%lf,,%lf,", &coefftype, &n, &m, &gnm, &dgnm); hnm = 0; dhnm = 0; } else sscanf(line, "%c,%d,%d,%lf,%lf,%lf,%lf", &coefftype, &n, &m, &gnm, &hnm, &dgnm, &dhnm); newrecord = 1; - if(!allocationflag) - { + if (!allocationflag) { fprintf(stderr, "Degree not found in model. Memory cannot be allocated.\n"); return _DEGREE_NOT_FOUND; } - if(m <= n) - { + if (m <= n) { index = (n * (n + 1) / 2 + m); (*magneticmodels)[header_index]->Main_Field_Coeff_G[index] = gnm; (*magneticmodels)[header_index]->Secular_Var_Coeff_G[index] = dgnm; @@ -1939,32 +2002,34 @@ int MAG_readMagneticModel_SHDF(char *filename, MAGtype_MagneticModel *(*magnetic } } } - if(header_index > -1) + if (header_index > -1) MAG_AssignHeaderValues((*magneticmodels)[header_index], paramvalues); fclose(stream); cutoff = (*magneticmodels)[array_size - 1]->CoefficientFileEndDate; - for(i = 0; i < array_size; i++) (*magneticmodels)[i]->CoefficientFileEndDate = cutoff; + for (i = 0; i < array_size; i++) + (*magneticmodels)[i]->CoefficientFileEndDate = cutoff; free(ptrreset); line = NULL; ptrreset = NULL; return header_index + 1; -}/*MAG_readMagneticModel_SHDF*/ +} /*MAG_readMagneticModel_SHDF*/ -char *MAG_Trim(char *str) +char* +MAG_Trim(char* str) { - char *end; + char* end; - while(isspace(*str)) + while (isspace(*str)) str++; - if(*str == 0) + if (*str == 0) return str; end = str + strlen(str) - 1; - while(end > str && isspace(*end)) + while (end > str && isspace(*end)) end--; *(end + 1) = 0; @@ -1974,25 +2039,34 @@ char *MAG_Trim(char *str) /*End of Memory and File Processing functions*/ - /****************************************************************************** *************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. ******************************************************************************/ - -void MAG_BaseErrors(double DeclCoef, double DeclBaseline, double InclOffset, double FOffset, double Multiplier, double H, double* DeclErr, double* InclErr, double* FErr) +void +MAG_BaseErrors(double DeclCoef, + double DeclBaseline, + double InclOffset, + double FOffset, + double Multiplier, + double H, + double* DeclErr, + double* InclErr, + double* FErr) { double declHorizontalAdjustmentSq; - declHorizontalAdjustmentSq = (DeclCoef/H) * (DeclCoef/H); - *DeclErr = sqrt(declHorizontalAdjustmentSq + DeclBaseline*DeclBaseline) * Multiplier; - *InclErr = InclOffset*Multiplier; - *FErr = FOffset*Multiplier; + declHorizontalAdjustmentSq = (DeclCoef / H) * (DeclCoef / H); + *DeclErr = sqrt(declHorizontalAdjustmentSq + DeclBaseline * DeclBaseline) * Multiplier; + *InclErr = InclOffset * Multiplier; + *FErr = FOffset * Multiplier; } -int MAG_CalculateGeoMagneticElements(MAGtype_MagneticResults *MagneticResultsGeo, MAGtype_GeoMagneticElements *GeoMagneticElements) +int +MAG_CalculateGeoMagneticElements(MAGtype_MagneticResults* MagneticResultsGeo, + MAGtype_GeoMagneticElements* GeoMagneticElements) /* Calculate all the Geomagnetic elements from X,Y and Z components INPUT MagneticResultsGeo Pointer to data structure with the following elements @@ -2014,15 +2088,18 @@ CALLS : none GeoMagneticElements->Y = MagneticResultsGeo->By; GeoMagneticElements->Z = MagneticResultsGeo->Bz; - GeoMagneticElements->H = sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); - GeoMagneticElements->F = sqrt(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); + GeoMagneticElements->H = + sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); + GeoMagneticElements->F = + sqrt(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); GeoMagneticElements->Decl = RAD2DEG(atan2(GeoMagneticElements->Y, GeoMagneticElements->X)); GeoMagneticElements->Incl = RAD2DEG(atan2(GeoMagneticElements->Z, GeoMagneticElements->H)); return TRUE; } /*MAG_CalculateGeoMagneticElements */ -int MAG_CalculateGridVariation(MAGtype_CoordGeodetic location, MAGtype_GeoMagneticElements *elements) +int +MAG_CalculateGridVariation(MAGtype_CoordGeodetic location, MAGtype_GeoMagneticElements* elements) /*Computes the grid variation for |latitudes| > MAG_MAX_LAT_DEGREE @@ -2044,66 +2121,76 @@ CALLS : MAG_GetTransverseMercator */ { MAGtype_UTMParameters UTMParameters; - if(location.phi >= MAG_PS_MAX_LAT_DEGREE) - { + if (location.phi >= MAG_PS_MAX_LAT_DEGREE) { elements->GV = elements->Decl - location.lambda; return 1; - } else if(location.phi <= MAG_PS_MIN_LAT_DEGREE) - { + } else if (location.phi <= MAG_PS_MIN_LAT_DEGREE) { elements->GV = elements->Decl + location.lambda; return 1; - } else - { + } else { MAG_GetTransverseMercator(location, &UTMParameters); elements->GV = elements->Decl - UTMParameters.ConvergenceOfMeridians; } return 0; } /*MAG_CalculateGridVariation*/ -void MAG_CalculateGradientElements(MAGtype_MagneticResults GradResults, MAGtype_GeoMagneticElements MagneticElements, MAGtype_GeoMagneticElements *GradElements) +void +MAG_CalculateGradientElements(MAGtype_MagneticResults GradResults, + MAGtype_GeoMagneticElements MagneticElements, + MAGtype_GeoMagneticElements* GradElements) { 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); - GradElements->Incl = 180.0 / M_PI * (MagneticElements.H * GradElements->Z - MagneticElements.Z * GradElements->H) / (MagneticElements.F * MagneticElements.F); + + 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); + GradElements->Incl = 180.0 / M_PI * (MagneticElements.H * GradElements->Z - MagneticElements.Z * GradElements->H) / + (MagneticElements.F * MagneticElements.F); GradElements->GV = GradElements->Decl; } -int MAG_CalculateSecularVariationElements(MAGtype_MagneticResults MagneticVariation, MAGtype_GeoMagneticElements *MagneticElements) -/*This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the Geomagnetic elements. - INPUT MagneticVariation Data structure with the following elements - double Bx; ( North ) - double By; ( East ) - double Bz; ( Down ) - OUTPUT MagneticElements Pointer to the data structure with the following elements updated - double Decldot; Yearly Rate of change in declination - double Incldot; Yearly Rate of change in inclination - double Fdot; Yearly rate of change in Magnetic field strength - double Hdot; Yearly rate of change in horizontal field strength - double Xdot; Yearly rate of change in the northern component - double Ydot; Yearly rate of change in the eastern component - double Zdot; Yearly rate of change in the downward component - double GVdot;Yearly rate of chnage in grid variation - CALLS : none +int +MAG_CalculateSecularVariationElements(MAGtype_MagneticResults MagneticVariation, + MAGtype_GeoMagneticElements* MagneticElements) +/*This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the + Geomagnetic elements. INPUT MagneticVariation Data structure with the following elements double Bx; ( North + ) double By; ( East ) double Bz; ( Down ) OUTPUT MagneticElements Pointer to the data structure with the + following elements updated double Decldot; Yearly Rate of change in declination double Incldot; Yearly Rate of change + in inclination double Fdot; Yearly rate of change in Magnetic field strength double Hdot; Yearly rate of change in + horizontal field strength double Xdot; Yearly rate of change in the northern component double Ydot; Yearly rate of + change in the eastern component double Zdot; Yearly rate of change in the downward component double GVdot;Yearly rate + of chnage in grid variation CALLS : none */ { MagneticElements->Xdot = MagneticVariation.Bx; MagneticElements->Ydot = MagneticVariation.By; MagneticElements->Zdot = MagneticVariation.Bz; - MagneticElements->Hdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / MagneticElements->H; /* See equation 19 in the WMM technical report */ - MagneticElements->Fdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F; - MagneticElements->Decldot = 180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot - MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); - MagneticElements->Incldot = 180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot - MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); + MagneticElements->Hdot = + (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / + MagneticElements->H; /* See equation 19 in the WMM technical report */ + MagneticElements->Fdot = + (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot + + MagneticElements->Z * MagneticElements->Zdot) / + MagneticElements->F; + MagneticElements->Decldot = + 180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot - MagneticElements->Y * MagneticElements->Xdot) / + (MagneticElements->H * MagneticElements->H); + MagneticElements->Incldot = + 180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot - MagneticElements->Z * MagneticElements->Hdot) / + (MagneticElements->F * MagneticElements->F); MagneticElements->GVdot = MagneticElements->Decldot; return TRUE; } /*MAG_CalculateSecularVariationElements*/ -void MAG_CartesianToGeodetic(MAGtype_Ellipsoid Ellip, double x, double y, double z, MAGtype_CoordGeodetic *CoordGeodetic) +void +MAG_CartesianToGeodetic(MAGtype_Ellipsoid Ellip, double x, double y, double z, MAGtype_CoordGeodetic* CoordGeodetic) { /*This converts the Cartesian x, y, and z coordinates to Geodetic Coordinates x is defined as the direction pointing out of the core toward the point defined @@ -2111,74 +2198,70 @@ 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; -/* - * 1.0 compute semi-minor axis and set sign to that of z in order - * to get sign of Phi correct - */ + double modified_b, r, e, f, p, q, d, v, g, t, zlong, rlat; - if (z < 0.0) modified_b = -Ellip.b; - else modified_b = Ellip.b; + /* + * 1.0 compute semi-minor axis and set sign to that of z in order + * to get sign of Phi correct + */ -/* - * 2.0 compute intermediate values for latitude - */ - r= sqrt( x*x + y*y ); - e= ( modified_b*z - (Ellip.a*Ellip.a - modified_b*modified_b) ) / ( Ellip.a*r ); - f= ( modified_b*z + (Ellip.a*Ellip.a - modified_b*modified_b) ) / ( Ellip.a*r ); -/* - * 3.0 find solution to: - * t^4 + 2*E*t^3 + 2*F*t - 1 = 0 - */ - p= (4.0 / 3.0) * (e*f + 1.0); - q= 2.0 * (e*e - f*f); - d= p*p*p + q*q; - - if( d >= 0.0 ) - { - v= pow( (sqrt( d ) - q), (1.0 / 3.0) ) - - pow( (sqrt( d ) + q), (1.0 / 3.0) ); - } - else - { - v= 2.0 * sqrt( -p ) - * cos( acos( q/(p * sqrt( -p )) ) / 3.0 ); - } -/* - * 4.0 improve v - * NOTE: not really necessary unless point is near pole - */ - if( v*v < fabs(p) ) { - v= -(v*v*v + 2.0*q) / (3.0*p); - } - g = (sqrt( e*e + v ) + e) / 2.0; - t = sqrt( g*g + (f - v*g)/(2.0*g - e) ) - g; + if (z < 0.0) + modified_b = -Ellip.b; + else + modified_b = Ellip.b; - rlat =atan( (Ellip.a*(1.0 - t*t)) / (2.0*modified_b*t) ); - CoordGeodetic->phi = RAD2DEG(rlat); - -/* - * 5.0 compute height above ellipsoid - */ - CoordGeodetic->HeightAboveEllipsoid = (r - Ellip.a*t) * cos(rlat) + (z - modified_b) * sin(rlat); -/* - * 6.0 compute longitude east of Greenwich - */ - zlong = atan2( y, x ); - if( zlong < 0.0 ) - zlong= zlong + 2*M_PI; + /* + * 2.0 compute intermediate values for latitude + */ + r = sqrt(x * x + y * y); + e = (modified_b * z - (Ellip.a * Ellip.a - modified_b * modified_b)) / (Ellip.a * r); + f = (modified_b * z + (Ellip.a * Ellip.a - modified_b * modified_b)) / (Ellip.a * r); + /* + * 3.0 find solution to: + * t^4 + 2*E*t^3 + 2*F*t - 1 = 0 + */ + p = (4.0 / 3.0) * (e * f + 1.0); + q = 2.0 * (e * e - f * f); + d = p * p * p + q * q; - CoordGeodetic->lambda = RAD2DEG(zlong); - while(CoordGeodetic->lambda > 180) - { - CoordGeodetic->lambda-=360; - } - + if (d >= 0.0) { + v = pow((sqrt(d) - q), (1.0 / 3.0)) - pow((sqrt(d) + q), (1.0 / 3.0)); + } else { + v = 2.0 * sqrt(-p) * cos(acos(q / (p * sqrt(-p))) / 3.0); + } + /* + * 4.0 improve v + * NOTE: not really necessary unless point is near pole + */ + if (v * v < fabs(p)) { + v = -(v * v * v + 2.0 * q) / (3.0 * p); + } + g = (sqrt(e * e + v) + e) / 2.0; + t = sqrt(g * g + (f - v * g) / (2.0 * g - e)) - g; + + rlat = atan((Ellip.a * (1.0 - t * t)) / (2.0 * modified_b * t)); + CoordGeodetic->phi = RAD2DEG(rlat); + + /* + * 5.0 compute height above ellipsoid + */ + CoordGeodetic->HeightAboveEllipsoid = (r - Ellip.a * t) * cos(rlat) + (z - modified_b) * sin(rlat); + /* + * 6.0 compute longitude east of Greenwich + */ + zlong = atan2(y, x); + if (zlong < 0.0) + zlong = zlong + 2 * M_PI; + + CoordGeodetic->lambda = RAD2DEG(zlong); + while (CoordGeodetic->lambda > 180) { + CoordGeodetic->lambda -= 360; + } } -MAGtype_CoordGeodetic MAG_CoordGeodeticAssign(MAGtype_CoordGeodetic CoordGeodetic) +MAGtype_CoordGeodetic +MAG_CoordGeodeticAssign(MAGtype_CoordGeodetic CoordGeodetic) { MAGtype_CoordGeodetic Assignee; Assignee.phi = CoordGeodetic.phi; @@ -2189,7 +2272,8 @@ MAGtype_CoordGeodetic MAG_CoordGeodeticAssign(MAGtype_CoordGeodetic CoordGeodeti return Assignee; } -int MAG_DateToYear(MAGtype_Date *CalendarDate, char *Error) +int +MAG_DateToYear(MAGtype_Date* CalendarDate, char* Error) /* Converts a given calendar date into a decimal year, it also outputs an error string if there is a problem @@ -2209,12 +2293,11 @@ CALLS : none int MonthDays[13]; int ExtraDay = 0; int i; - if(CalendarDate->Month == 0) - { + if (CalendarDate->Month == 0) { CalendarDate->DecimalYear = CalendarDate->Year; return TRUE; } - if((CalendarDate->Year % 4 == 0 && CalendarDate->Year % 100 != 0) || CalendarDate->Year % 400 == 0) + if ((CalendarDate->Year % 4 == 0 && CalendarDate->Year % 100 != 0) || CalendarDate->Year % 400 == 0) ExtraDay = 1; MonthDays[0] = 0; MonthDays[1] = 31; @@ -2231,19 +2314,17 @@ CALLS : none MonthDays[12] = 31; /******************Validation********************************/ - if(CalendarDate->Month <= 0 || CalendarDate->Month > 12) - { + if (CalendarDate->Month <= 0 || CalendarDate->Month > 12) { strcpy(Error, "\nError: The Month entered is invalid, valid months are '1 to 12'\n"); return 0; } - if(CalendarDate->Day <= 0 || CalendarDate->Day > MonthDays[CalendarDate->Month]) - { + if (CalendarDate->Day <= 0 || CalendarDate->Day > MonthDays[CalendarDate->Month]) { printf("\nThe number of days in month %d is %d\n", CalendarDate->Month, MonthDays[CalendarDate->Month]); strcpy(Error, "\nError: The day entered is invalid\n"); return 0; } /****************Calculation of t***************************/ - for(i = 1; i <= CalendarDate->Month; i++) + for (i = 1; i <= CalendarDate->Month; i++) temp += MonthDays[i - 1]; temp += CalendarDate->Day; CalendarDate->DecimalYear = CalendarDate->Year + (temp - 1) / (365.0 + ExtraDay); @@ -2251,7 +2332,8 @@ CALLS : none } /*MAG_DateToYear*/ -void MAG_DegreeToDMSstring(double DegreesOfArc, int UnitDepth, char *DMSstring) +void +MAG_DegreeToDMSstring(double DegreesOfArc, int UnitDepth, char* DMSstring) /*This converts a given decimal degree into a DMS string. INPUT DegreesOfArc decimal degree @@ -2268,12 +2350,11 @@ CALLS : none char tempstring[36] = ""; char tempstring2[32] = ""; strcpy(DMSstring, ""); - if(UnitDepth > 3) + if (UnitDepth > 3) MAG_Error(21); - for(i = 0; i < UnitDepth; i++) - { - DMS[i] = (int) temp; - switch(i) { + for (i = 0; i < UnitDepth; i++) { + DMS[i] = (int)temp; + switch (i) { case 0: strcpy(tempstring2, "Deg"); break; @@ -2284,17 +2365,18 @@ CALLS : none strcpy(tempstring2, "Sec"); break; } - temp = (temp - DMS[i])*60; - if(i == UnitDepth - 1 && temp >= 30) + temp = (temp - DMS[i]) * 60; + if (i == UnitDepth - 1 && temp >= 30) DMS[i]++; - else if(i == UnitDepth - 1 && temp <= -30) + else if (i == UnitDepth - 1 && temp <= -30) DMS[i]--; sprintf(tempstring, "%4d%4s", DMS[i], tempstring2); strcat(DMSstring, tempstring); } } /*MAG_DegreeToDMSstring*/ -void MAG_DMSstringToDegree(char *DMSstring, double *DegreesOfArc) +void +MAG_DMSstringToDegree(char* DMSstring, double* DegreesOfArc) /*This converts a given DMS string into decimal degrees. INPUT DMSstring pointer to DMSString @@ -2304,33 +2386,39 @@ CALLS : none { int second, minute, degree, sign = 1, j = 0; j = sscanf(DMSstring, "%d, %d, %d", °ree, &minute, &second); - if(j != 3) + if (j != 3) sscanf(DMSstring, "%d %d %d", °ree, &minute, &second); - if(degree < 0) + if (degree < 0) sign = -1; degree = degree * sign; *DegreesOfArc = sign * (degree + minute / 60.0 + second / 3600.0); } /*MAG_DMSstringToDegree*/ -void MAG_ErrorCalc(MAGtype_GeoMagneticElements B, MAGtype_GeoMagneticElements* Errors) +void +MAG_ErrorCalc(MAGtype_GeoMagneticElements B, MAGtype_GeoMagneticElements* Errors) { /*Errors.Decl, Errors.Incl, Errors.F are all assumed to exist*/ double cos2D, cos2I, sin2D, sin2I, EDSq, EISq, eD, eI; - cos2D = cos(DEG2RAD(B.Decl))*cos(DEG2RAD(B.Decl)); - cos2I = cos(DEG2RAD(B.Incl))*cos(DEG2RAD(B.Incl)); - sin2D = sin(DEG2RAD(B.Decl))*sin(DEG2RAD(B.Decl)); - sin2I = sin(DEG2RAD(B.Incl))*sin(DEG2RAD(B.Incl)); + cos2D = cos(DEG2RAD(B.Decl)) * cos(DEG2RAD(B.Decl)); + cos2I = cos(DEG2RAD(B.Incl)) * cos(DEG2RAD(B.Incl)); + sin2D = sin(DEG2RAD(B.Decl)) * sin(DEG2RAD(B.Decl)); + sin2I = sin(DEG2RAD(B.Incl)) * sin(DEG2RAD(B.Incl)); eD = DEG2RAD(Errors->Decl); eI = DEG2RAD(Errors->Incl); - EDSq = eD*eD; - EISq = eI*eI; - Errors->X = sqrt(cos2D*cos2I*Errors->F*Errors->F+B.F*B.F*sin2D*cos2I*EDSq+B.F*B.F*cos2D*sin2I*EISq); - Errors->Y = sqrt(sin2D*cos2I*Errors->F*Errors->F+B.F*B.F*cos2D*cos2I*EDSq+B.F*B.F*sin2D*sin2I*EISq); - Errors->Z = sqrt(sin2I*Errors->F*Errors->F+B.F*B.F*cos2I*EISq); - Errors->H = sqrt(cos2I*Errors->F*Errors->F+B.F*B.F*sin2I*EISq); + EDSq = eD * eD; + EISq = eI * eI; + Errors->X = + sqrt(cos2D * cos2I * Errors->F * Errors->F + B.F * B.F * sin2D * cos2I * EDSq + B.F * B.F * cos2D * sin2I * EISq); + Errors->Y = + sqrt(sin2D * cos2I * Errors->F * Errors->F + B.F * B.F * cos2D * cos2I * EDSq + B.F * B.F * sin2D * sin2I * EISq); + Errors->Z = sqrt(sin2I * Errors->F * Errors->F + B.F * B.F * cos2I * EISq); + Errors->H = sqrt(cos2I * Errors->F * Errors->F + B.F * B.F * sin2I * EISq); } -int MAG_GeodeticToSpherical(MAGtype_Ellipsoid Ellip, MAGtype_CoordGeodetic CoordGeodetic, MAGtype_CoordSpherical *CoordSpherical) +int +MAG_GeodeticToSpherical(MAGtype_Ellipsoid Ellip, + MAGtype_CoordGeodetic CoordGeodetic, + MAGtype_CoordSpherical* CoordSpherical) /* Converts Geodetic coordinates to Spherical coordinates @@ -2381,12 +2469,13 @@ CALLS : none CoordSpherical->r = sqrt(xp * xp + zp * zp); CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); /* geocentric latitude */ - CoordSpherical->lambda = CoordGeodetic.lambda; /* longitude */ + CoordSpherical->lambda = CoordGeodetic.lambda; /* longitude */ return TRUE; -}/*MAG_GeodeticToSpherical*/ +} /*MAG_GeodeticToSpherical*/ -MAGtype_GeoMagneticElements MAG_GeoMagneticElementsAssign(MAGtype_GeoMagneticElements Elements) +MAGtype_GeoMagneticElements +MAG_GeoMagneticElementsAssign(MAGtype_GeoMagneticElements Elements) { MAGtype_GeoMagneticElements Assignee; Assignee.X = Elements.X; @@ -2408,9 +2497,10 @@ MAGtype_GeoMagneticElements MAG_GeoMagneticElementsAssign(MAGtype_GeoMagneticEle return Assignee; } -MAGtype_GeoMagneticElements MAG_GeoMagneticElementsScale(MAGtype_GeoMagneticElements Elements, double factor) +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; @@ -2432,37 +2522,39 @@ MAGtype_GeoMagneticElements MAG_GeoMagneticElementsScale(MAGtype_GeoMagneticElem return product; } -MAGtype_GeoMagneticElements MAG_GeoMagneticElementsSubtract(MAGtype_GeoMagneticElements minuend, MAGtype_GeoMagneticElements subtrahend) +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; } -int MAG_GetTransverseMercator(MAGtype_CoordGeodetic CoordGeodetic, MAGtype_UTMParameters *UTMParameters) +int +MAG_GetTransverseMercator(MAGtype_CoordGeodetic CoordGeodetic, MAGtype_UTMParameters* UTMParameters) /* Gets the UTM Parameters for a given Latitude and Longitude. INPUT: CoordGeodetic : Data structure MAGtype_CoordGeodetic. @@ -2487,8 +2579,6 @@ OUTPUT : UTMParameters : Pointer to data structure MAGtype_UTMParameters with th int Zone; char Hemisphere; - - /* Get the map projection parameters */ Lambda = DEG2RAD(CoordGeodetic.lambda); @@ -2497,27 +2587,21 @@ OUTPUT : UTMParameters : Pointer to data structure MAGtype_UTMParameters with th MAG_GetUTMParameters(Phi, Lambda, &Zone, &Hemisphere, &Lam0); K0 = 0.9996; - - - if(Hemisphere == 'n' || Hemisphere == 'N') - { + if (Hemisphere == 'n' || Hemisphere == 'N') { falseN = 0; } - if(Hemisphere == 's' || Hemisphere == 'S') - { + if (Hemisphere == 's' || Hemisphere == 'S') { falseN = 10000000; } falseE = 500000; - /* WGS84 ellipsoid */ Eps = 0.081819190842621494335; Epssq = 0.0066943799901413169961; - K0R4 = 6367449.1458234153093*K0; - K0R4oa = K0R4/6378137; - + K0R4 = 6367449.1458234153093 * K0; + K0R4oa = K0R4 / 6378137; Acoeff[0] = 8.37731820624469723600E-04; Acoeff[1] = 7.60852777357248641400E-07; @@ -2530,35 +2614,27 @@ OUTPUT : UTMParameters : Pointer to data structure MAGtype_UTMParameters with th /* WGS84 ellipsoid */ - /* Execution of the forward T.M. algorithm */ XYonly = 0; - MAG_TMfwd4(Eps, Epssq, K0R4, K0R4oa, Acoeff, - Lam0, K0, falseE, falseN, - XYonly, - Lambda, Phi, - &X, &Y, &pscale, &CoM); + MAG_TMfwd4(Eps, Epssq, K0R4, K0R4oa, Acoeff, Lam0, K0, falseE, falseN, XYonly, Lambda, Phi, &X, &Y, &pscale, &CoM); /* Report results */ - UTMParameters->Easting = X; /* UTM Easting (X) in meters*/ + UTMParameters->Easting = X; /* UTM Easting (X) in meters*/ UTMParameters->Northing = Y; /* UTM Northing (Y) in meters */ - UTMParameters->Zone = Zone; /*UTM Zone*/ + UTMParameters->Zone = Zone; /*UTM Zone*/ UTMParameters->HemiSphere = Hemisphere; - UTMParameters->CentralMeridian = RAD2DEG(Lam0); /* Central Meridian of the UTM Zone */ + UTMParameters->CentralMeridian = RAD2DEG(Lam0); /* Central Meridian of the UTM Zone */ UTMParameters->ConvergenceOfMeridians = RAD2DEG(CoM); /* Convergence of meridians of the UTM Zone and location */ UTMParameters->PointScale = pscale; return 0; } /*MAG_GetTransverseMercator*/ -int MAG_GetUTMParameters(double Latitude, - double Longitude, - int *Zone, - char *Hemisphere, - double *CentralMeridian) +int +MAG_GetUTMParameters(double Latitude, double Longitude, int* Zone, char* Hemisphere, double* CentralMeridian) { /* * The function MAG_GetUTMParameters converts geodetic (latitude and @@ -2578,67 +2654,67 @@ int MAG_GetUTMParameters(double Latitude, long temp_zone; int Error_Code = 0; - - - if((Latitude < DEG2RAD(MAG_UTM_MIN_LAT_DEGREE)) || (Latitude > DEG2RAD(MAG_UTM_MAX_LAT_DEGREE))) - { /* Latitude out of range */ + if ((Latitude < DEG2RAD(MAG_UTM_MIN_LAT_DEGREE)) || + (Latitude > DEG2RAD(MAG_UTM_MAX_LAT_DEGREE))) { /* Latitude out of range */ MAG_Error(23); Error_Code = 1; } - if((Longitude < -M_PI) || (Longitude > (2 * M_PI))) - { /* Longitude out of range */ + if ((Longitude < -M_PI) || (Longitude > (2 * M_PI))) { /* Longitude out of range */ MAG_Error(24); Error_Code = 1; } - if(!Error_Code) - { /* no errors */ - if(Longitude < 0) + if (!Error_Code) { /* no errors */ + if (Longitude < 0) Longitude += (2 * M_PI) + 1.0e-10; - Lat_Degrees = (long) (Latitude * 180.0 / M_PI); - Long_Degrees = (long) (Longitude * 180.0 / M_PI); + Lat_Degrees = (long)(Latitude * 180.0 / M_PI); + Long_Degrees = (long)(Longitude * 180.0 / M_PI); - if(Longitude < M_PI) - temp_zone = (long) (31 + ((Longitude * 180.0 / M_PI) / 6.0)); + if (Longitude < M_PI) + temp_zone = (long)(31 + ((Longitude * 180.0 / M_PI) / 6.0)); else - temp_zone = (long) (((Longitude * 180.0 / M_PI) / 6.0) - 29); - if(temp_zone > 60) + temp_zone = (long)(((Longitude * 180.0 / M_PI) / 6.0) - 29); + if (temp_zone > 60) temp_zone = 1; /* UTM special cases */ - if((Lat_Degrees > 55) && (Lat_Degrees < 64) && (Long_Degrees > -1) - && (Long_Degrees < 3)) + if ((Lat_Degrees > 55) && (Lat_Degrees < 64) && (Long_Degrees > -1) && (Long_Degrees < 3)) temp_zone = 31; - if((Lat_Degrees > 55) && (Lat_Degrees < 64) && (Long_Degrees > 2) - && (Long_Degrees < 12)) + if ((Lat_Degrees > 55) && (Lat_Degrees < 64) && (Long_Degrees > 2) && (Long_Degrees < 12)) temp_zone = 32; - if((Lat_Degrees > 71) && (Long_Degrees > -1) && (Long_Degrees < 9)) + if ((Lat_Degrees > 71) && (Long_Degrees > -1) && (Long_Degrees < 9)) temp_zone = 31; - if((Lat_Degrees > 71) && (Long_Degrees > 8) && (Long_Degrees < 21)) + if ((Lat_Degrees > 71) && (Long_Degrees > 8) && (Long_Degrees < 21)) temp_zone = 33; - if((Lat_Degrees > 71) && (Long_Degrees > 20) && (Long_Degrees < 33)) + if ((Lat_Degrees > 71) && (Long_Degrees > 20) && (Long_Degrees < 33)) temp_zone = 35; - if((Lat_Degrees > 71) && (Long_Degrees > 32) && (Long_Degrees < 42)) + if ((Lat_Degrees > 71) && (Long_Degrees > 32) && (Long_Degrees < 42)) temp_zone = 37; - if(!Error_Code) - { - if(temp_zone >= 31) + if (!Error_Code) { + if (temp_zone >= 31) *CentralMeridian = (6 * temp_zone - 183) * M_PI / 180.0; else *CentralMeridian = (6 * temp_zone + 177) * M_PI / 180.0; - *Zone = (int) temp_zone; - if(Latitude < 0) *Hemisphere = 'S'; - else *Hemisphere = 'N'; + *Zone = (int)temp_zone; + if (Latitude < 0) + *Hemisphere = 'S'; + else + *Hemisphere = 'N'; } } /* END OF if (!Error_Code) */ return (Error_Code); } /* MAG_GetUTMParameters */ -int MAG_isNaN(double d) +int +MAG_isNaN(double d) { return d != d; } -int MAG_RotateMagneticVector(MAGtype_CoordSpherical CoordSpherical, MAGtype_CoordGeodetic CoordGeodetic, MAGtype_MagneticResults MagneticResultsSph, MAGtype_MagneticResults *MagneticResultsGeo) +int +MAG_RotateMagneticVector(MAGtype_CoordSpherical CoordSpherical, + MAGtype_CoordGeodetic CoordGeodetic, + MAGtype_MagneticResults MagneticResultsSph, + MAGtype_MagneticResults* MagneticResultsGeo) /* Rotate the Magnetic Vectors to Geodetic Coordinates Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov Equation 16, WMM Technical report @@ -2679,34 +2755,51 @@ CALLS : none return TRUE; } /*MAG_RotateMagneticVector*/ -void MAG_SphericalToCartesian(MAGtype_CoordSpherical CoordSpherical, double *x, double *y, double *z) +void +MAG_SphericalToCartesian(MAGtype_CoordSpherical CoordSpherical, double* x, double* y, double* z) { 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); return; } -void MAG_SphericalToGeodetic(MAGtype_Ellipsoid Ellip, MAGtype_CoordSpherical CoordSpherical, MAGtype_CoordGeodetic *CoordGeodetic) +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); + double x, y, z; + + MAG_SphericalToCartesian(CoordSpherical, &x, &y, &z); + MAG_CartesianToGeodetic(Ellip, x, y, z, CoordGeodetic); } -void MAG_TMfwd4(double Eps, double Epssq, double K0R4, double K0R4oa, - double Acoeff[], double Lam0, double K0, double falseE, - double falseN, int XYonly, double Lambda, double Phi, - double *X, double *Y, double *pscale, double *CoM) +void +MAG_TMfwd4(double Eps, + double Epssq, + double K0R4, + double K0R4oa, + double Acoeff[], + double Lam0, + double K0, + double falseE, + double falseN, + int XYonly, + double Lambda, + double Phi, + double* X, + double* Y, + double* pscale, + double* CoM) { /* Transverse Mercator forward equations including point-scale and CoM @@ -2837,7 +2930,6 @@ void MAG_TMfwd4(double Eps, double Epssq, double K0R4, double K0R4oa, c8v = 1 - 2 * s4v * s4v; s8v = 2 * c4v * s4v; - /* First plane to second plane ----- ----- -- ------ ----- @@ -2861,16 +2953,13 @@ void MAG_TMfwd4(double Eps, double Epssq, double K0R4, double K0R4oa, *X = K0R4 * Xstar + falseE; *Y = K0R4 * Ystar + falseN; - /* Point-scale and CoM ----- ----- --- --- */ - if(XYonly == 1) - { + if (XYonly == 1) { *pscale = K0; *CoM = 0; - } else - { + } else { sig1 = 8 * Acoeff[3] * c8u * c8v; sig1 = sig1 + 6 * Acoeff[2] * c6u * c6v; sig1 = sig1 + 4 * Acoeff[1] * c4u * c4v; @@ -2883,15 +2972,15 @@ void MAG_TMfwd4(double Eps, double Epssq, double K0R4, double K0R4oa, sig2 = sig2 + 2 * Acoeff[0] * s2u * s2v; /* Combined square roots */ - comroo = sqrt((1 - Epssq * SPhi * SPhi) * denom2 * - (sig1 * sig1 + sig2 * sig2)); + comroo = sqrt((1 - Epssq * SPhi * SPhi) * denom2 * (sig1 * sig1 + sig2 * sig2)); *pscale = K0R4oa * 2 * denom * comroo; *CoM = atan2(SChi * SLam, CLam) + atan2(sig2, sig1); } } /*MAG_TMfwd4*/ -int MAG_YearToDate(MAGtype_Date *CalendarDate) +int +MAG_YearToDate(MAGtype_Date* CalendarDate) /* Converts a given Decimal year into a Year, Month and Date it also outputs an error string if there is a problem @@ -2910,25 +2999,22 @@ CALLS : none int ExtraDay = 0; int i, DayOfTheYear; - - if(CalendarDate->DecimalYear == 0) - { + if (CalendarDate->DecimalYear == 0) { CalendarDate->Year = 0; CalendarDate->Month = 0; CalendarDate->Day = 0; return FALSE; } - CalendarDate->Year = (int) floor(CalendarDate->DecimalYear); + CalendarDate->Year = (int)floor(CalendarDate->DecimalYear); - - if((CalendarDate->Year % 4 == 0 && CalendarDate->Year % 100 != 0) || CalendarDate->Year % 400 == 0) + if ((CalendarDate->Year % 4 == 0 && CalendarDate->Year % 100 != 0) || CalendarDate->Year % 400 == 0) ExtraDay = 1; - DayOfTheYear = floor((CalendarDate->DecimalYear - (double) CalendarDate->Year) * (365.0 + (double) ExtraDay)+0.5) + 1; + DayOfTheYear = + floor((CalendarDate->DecimalYear - (double)CalendarDate->Year) * (365.0 + (double)ExtraDay) + 0.5) + 1; /*The above floor is used for rounding, this only works for positive integers*/ - MonthDays[0] = 0; MonthDays[1] = 31; MonthDays[2] = 28 + ExtraDay; @@ -2943,37 +3029,30 @@ CALLS : none MonthDays[11] = 30; MonthDays[12] = 31; - - for(i = 1; i <= 12; i++) - { + for (i = 1; i <= 12; i++) { CumulativeDays = CumulativeDays + MonthDays[i]; - if(DayOfTheYear <= CumulativeDays) - { + if (DayOfTheYear <= CumulativeDays) { CalendarDate->Month = i; CalendarDate->Day = MonthDays[i] - (CumulativeDays - DayOfTheYear); break; } - - } - - - return TRUE; } /*MAG_YearToDate*/ - - /****************************************************************************** ********************************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) +int +MAG_AssociatedLegendreFunction(MAGtype_CoordSpherical CoordSpherical, + int nMax, + MAGtype_LegendreFunction* LegendreFunction) /* Computes all of the Schmidt-semi normalized associated Legendre functions up to degree nMax. If nMax <= 16, function MAG_PcupLow is used. @@ -2996,17 +3075,18 @@ OUTPUT LegendreFunction Calculated Legendre variables in the data structure sin_phi = sin(DEG2RAD(CoordSpherical.phig)); /* sin (geocentric latitude) */ - if(nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10) /* If nMax is less tha 16 or at the poles */ + if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10) /* If nMax is less tha 16 or at the poles */ FLAG = MAG_PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax); - else FLAG = MAG_PcupHigh(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax); - if(FLAG == 0) /* Error while computing Legendre variables*/ + else + FLAG = MAG_PcupHigh(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax); + if (FLAG == 0) /* Error while computing Legendre variables*/ return FALSE; - return TRUE; } /*MAG_AssociatedLegendreFunction */ -int MAG_CheckGeographicPole(MAGtype_CoordGeodetic *CoordGeodetic) +int +MAG_CheckGeographicPole(MAGtype_CoordGeodetic* CoordGeodetic) /* Check if the latitude is equal to -90 or 90. If it is, offset it by 1e-5 to avoid division by zero. This is not currently used in the Geomagnetic @@ -3024,12 +3104,18 @@ CALLS : none */ { - CoordGeodetic->phi = CoordGeodetic->phi < (-90.0 + MAG_GEO_POLE_TOLERANCE) ? (-90.0 + MAG_GEO_POLE_TOLERANCE) : CoordGeodetic->phi; - CoordGeodetic->phi = CoordGeodetic->phi > (90.0 - MAG_GEO_POLE_TOLERANCE) ? (90.0 - MAG_GEO_POLE_TOLERANCE) : CoordGeodetic->phi; + CoordGeodetic->phi = + CoordGeodetic->phi < (-90.0 + MAG_GEO_POLE_TOLERANCE) ? (-90.0 + MAG_GEO_POLE_TOLERANCE) : CoordGeodetic->phi; + CoordGeodetic->phi = + CoordGeodetic->phi > (90.0 - MAG_GEO_POLE_TOLERANCE) ? (90.0 - MAG_GEO_POLE_TOLERANCE) : CoordGeodetic->phi; return TRUE; } /*MAG_CheckGeographicPole*/ -int MAG_ComputeSphericalHarmonicVariables(MAGtype_Ellipsoid Ellip, MAGtype_CoordSpherical CoordSpherical, int nMax, MAGtype_SphericalHarmonicVariables *SphVariables) +int +MAG_ComputeSphericalHarmonicVariables(MAGtype_Ellipsoid Ellip, + MAGtype_CoordSpherical CoordSpherical, + int nMax, + MAGtype_SphericalHarmonicVariables* SphVariables) /* Computes Spherical variables Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic @@ -3061,8 +3147,7 @@ int MAG_ComputeSphericalHarmonicVariables(MAGtype_Ellipsoid Ellip, MAGtype_Coord /* for n = 0 ... model_order, compute (Radius of Earth / Spherical radius r)^(n+2) for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ SphVariables->RelativeRadiusPower[0] = (Ellip.re / CoordSpherical.r) * (Ellip.re / CoordSpherical.r); - for(n = 1; n <= nMax; n++) - { + for (n = 1; n <= nMax; n++) { SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip.re / CoordSpherical.r); } @@ -3076,84 +3161,101 @@ int MAG_ComputeSphericalHarmonicVariables(MAGtype_Ellipsoid Ellip, MAGtype_Coord SphVariables->cos_mlambda[1] = cos_lambda; SphVariables->sin_mlambda[1] = sin_lambda; - for(m = 2; m <= nMax; m++) - { - SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m - 1] * cos_lambda - SphVariables->sin_mlambda[m - 1] * sin_lambda; - SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m - 1] * sin_lambda + SphVariables->sin_mlambda[m - 1] * cos_lambda; + for (m = 2; m <= nMax; m++) { + SphVariables->cos_mlambda[m] = + SphVariables->cos_mlambda[m - 1] * cos_lambda - SphVariables->sin_mlambda[m - 1] * sin_lambda; + SphVariables->sin_mlambda[m] = + SphVariables->cos_mlambda[m - 1] * sin_lambda + SphVariables->sin_mlambda[m - 1] * cos_lambda; } return TRUE; } /*MAG_ComputeSphericalHarmonicVariables*/ -void MAG_GradY(MAGtype_Ellipsoid Ellip, MAGtype_CoordSpherical CoordSpherical, MAGtype_CoordGeodetic CoordGeodetic, - MAGtype_MagneticModel *TimedMagneticModel, MAGtype_GeoMagneticElements GeoMagneticElements, MAGtype_GeoMagneticElements *GradYElements) +void +MAG_GradY(MAGtype_Ellipsoid Ellip, + MAGtype_CoordSpherical CoordSpherical, + MAGtype_CoordGeodetic CoordGeodetic, + MAGtype_MagneticModel* TimedMagneticModel, + MAGtype_GeoMagneticElements GeoMagneticElements, + MAGtype_GeoMagneticElements* GradYElements) { - MAGtype_LegendreFunction *LegendreFunction; - MAGtype_SphericalHarmonicVariables *SphVariables; + MAGtype_LegendreFunction* LegendreFunction; + MAGtype_SphericalHarmonicVariables* SphVariables; 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 */ + MAG_ComputeSphericalHarmonicVariables( + Ellip, CoordSpherical, TimedMagneticModel->nMax, SphVariables); /* Compute Spherical Harmonic variables */ MAG_AssociatedLegendreFunction(CoordSpherical, TimedMagneticModel->nMax, LegendreFunction); /* Compute ALF */ - 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_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); } -void MAG_GradYSummation(MAGtype_LegendreFunction *LegendreFunction, MAGtype_MagneticModel *MagneticModel, MAGtype_SphericalHarmonicVariables SphVariables, MAGtype_CoordSpherical CoordSpherical, MAGtype_MagneticResults *GradY) +void +MAG_GradYSummation(MAGtype_LegendreFunction* LegendreFunction, + MAGtype_MagneticModel* MagneticModel, + MAGtype_SphericalHarmonicVariables SphVariables, + MAGtype_CoordSpherical CoordSpherical, + MAGtype_MagneticResults* GradY) { int m, n, index; double cos_phi; GradY->Bz = 0.0; GradY->By = 0.0; GradY->Bx = 0.0; - for(n = 1; n <= MagneticModel->nMax; n++) - { - for(m = 0; m <= n; m++) - { + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { index = (n * (n + 1) / 2 + m); GradY->Bz -= SphVariables.RelativeRadiusPower[n] * - (-1 * MagneticModel->Main_Field_Coeff_G[index] * SphVariables.sin_mlambda[m] + - MagneticModel->Main_Field_Coeff_H[index] * SphVariables.cos_mlambda[m]) - * (double) (n + 1) * (double) (m) * LegendreFunction-> Pcup[index] * (1/CoordSpherical.r); + (-1 * MagneticModel->Main_Field_Coeff_G[index] * SphVariables.sin_mlambda[m] + + MagneticModel->Main_Field_Coeff_H[index] * SphVariables.cos_mlambda[m]) * + (double)(n + 1) * (double)(m)*LegendreFunction->Pcup[index] * (1 / CoordSpherical.r); GradY->By += SphVariables.RelativeRadiusPower[n] * - (MagneticModel->Main_Field_Coeff_G[index] * SphVariables.cos_mlambda[m] + - MagneticModel->Main_Field_Coeff_H[index] * SphVariables.sin_mlambda[m]) - * (double) (m * m) * LegendreFunction-> Pcup[index] * (1/CoordSpherical.r); + (MagneticModel->Main_Field_Coeff_G[index] * SphVariables.cos_mlambda[m] + + MagneticModel->Main_Field_Coeff_H[index] * SphVariables.sin_mlambda[m]) * + (double)(m * m) * LegendreFunction->Pcup[index] * (1 / CoordSpherical.r); GradY->Bx -= SphVariables.RelativeRadiusPower[n] * - (-1 * MagneticModel->Main_Field_Coeff_G[index] * SphVariables.sin_mlambda[m] + - MagneticModel->Main_Field_Coeff_H[index] * SphVariables.cos_mlambda[m]) - * (double) (m) * LegendreFunction-> dPcup[index] * (1/CoordSpherical.r); - - - + (-1 * MagneticModel->Main_Field_Coeff_G[index] * SphVariables.sin_mlambda[m] + + MagneticModel->Main_Field_Coeff_H[index] * SphVariables.cos_mlambda[m]) * + (double)(m)*LegendreFunction->dPcup[index] * (1 / CoordSpherical.r); } } cos_phi = cos(DEG2RAD(CoordSpherical.phig)); - if(fabs(cos_phi) > 1.0e-10) - { + if (fabs(cos_phi) > 1.0e-10) { GradY->By = GradY->By / (cos_phi * cos_phi); GradY->Bx = GradY->Bx / (cos_phi); GradY->Bz = GradY->Bz / (cos_phi); } else - /* Special calculation for component - By - at Geographic poles. - * If the user wants to avoid using this function, please make sure that - * the latitude is not exactly +/-90. An option is to make use the function - * MAG_CheckGeographicPoles. - */ + /* Special calculation for component - By - at Geographic poles. + * If the user wants to avoid using this function, please make sure that + * the latitude is not exactly +/-90. An option is to make use the function + * MAG_CheckGeographicPoles. + */ { - /* MAG_SummationSpecial(MagneticModel, SphVariables, CoordSpherical, GradY); */ + /* MAG_SummationSpecial(MagneticModel, SphVariables, CoordSpherical, GradY); */ } } -int MAG_PcupHigh(double *Pcup, double *dPcup, double x, int nMax) +int +MAG_PcupHigh(double* Pcup, double* dPcup, double x, int nMax) /* This function evaluates all of the Schmidt-semi normalized associated Legendre functions up to degree nMax. The functions are initially scaled by @@ -3202,79 +3304,68 @@ int MAG_PcupHigh(double *Pcup, double *dPcup, double x, int nMax) NumTerms = ((nMax + 1) * (nMax + 2) / 2); - - if(fabs(x) == 1.0) - { + if (fabs(x) == 1.0) { printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); return FALSE; } - - f1 = (double *) malloc((NumTerms + 1) * sizeof ( double)); - if(f1 == NULL) - { + f1 = (double*)malloc((NumTerms + 1) * sizeof(double)); + if (f1 == NULL) { MAG_Error(18); return FALSE; } + PreSqr = (double*)malloc((NumTerms + 1) * sizeof(double)); - PreSqr = (double *) malloc((NumTerms + 1) * sizeof ( double)); - - if(PreSqr == NULL) - { + if (PreSqr == NULL) { MAG_Error(18); return FALSE; } - f2 = (double *) malloc((NumTerms + 1) * sizeof ( double)); + f2 = (double*)malloc((NumTerms + 1) * sizeof(double)); - if(f2 == NULL) - { + if (f2 == NULL) { MAG_Error(18); return FALSE; } scalef = 1.0e-280; - for(n = 0; n <= 2 * nMax + 1; ++n) - { - PreSqr[n] = sqrt((double) (n)); + for (n = 0; n <= 2 * nMax + 1; ++n) { + PreSqr[n] = sqrt((double)(n)); } k = 2; - for(n = 2; n <= nMax; n++) - { + for (n = 2; n <= nMax; n++) { k = k + 1; - f1[k] = (double) (2 * n - 1) / (double) (n); - f2[k] = (double) (n - 1) / (double) (n); - for(m = 1; m <= n - 2; m++) - { + f1[k] = (double)(2 * n - 1) / (double)(n); + f2[k] = (double)(n - 1) / (double)(n); + for (m = 1; m <= n - 2; m++) { k = k + 1; - f1[k] = (double) (2 * n - 1) / PreSqr[n + m] / PreSqr[n - m]; + f1[k] = (double)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - m]; f2[k] = PreSqr[n - m - 1] * PreSqr[n + m - 1] / PreSqr[n + m] / PreSqr[n - m]; } k = k + 2; } /*z = sin (geocentric latitude) */ - z = sqrt((1.0 - x)*(1.0 + x)); + z = sqrt((1.0 - x) * (1.0 + x)); pm2 = 1.0; Pcup[0] = 1.0; dPcup[0] = 0.0; - if(nMax == 0) + if (nMax == 0) return FALSE; pm1 = x; Pcup[1] = pm1; dPcup[1] = z; k = 1; - for(n = 2; n <= nMax; n++) - { + for (n = 2; n <= nMax; n++) { k = k + n; plm = f1[k] * x * pm1 - f2[k] * pm2; Pcup[k] = plm; - dPcup[k] = (double) (n) * (pm1 - x * plm) / z; + dPcup[k] = (double)(n) * (pm1 - x * plm) / z; pm2 = pm1; pm1 = plm; } @@ -3283,39 +3374,37 @@ int MAG_PcupHigh(double *Pcup, double *dPcup, double x, int nMax) rescalem = 1.0 / scalef; kstart = 0; - for(m = 1; m <= nMax - 1; ++m) - { - rescalem = rescalem*z; + for (m = 1; m <= nMax - 1; ++m) { + rescalem = rescalem * z; /* Calculate Pcup(m,m)*/ kstart = kstart + m + 1; pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m]; Pcup[kstart] = pmm * rescalem / PreSqr[2 * m + 1]; - dPcup[kstart] = -((double) (m) * x * Pcup[kstart] / z); + dPcup[kstart] = -((double)(m)*x * Pcup[kstart] / z); pm2 = pmm / PreSqr[2 * m + 1]; /* Calculate Pcup(m+1,m)*/ k = kstart + m + 1; pm1 = x * PreSqr[2 * m + 1] * pm2; - Pcup[k] = pm1*rescalem; - dPcup[k] = ((pm2 * rescalem) * PreSqr[2 * m + 1] - x * (double) (m + 1) * Pcup[k]) / z; + Pcup[k] = pm1 * rescalem; + dPcup[k] = ((pm2 * rescalem) * PreSqr[2 * m + 1] - x * (double)(m + 1) * Pcup[k]) / z; /* Calculate Pcup(n,m)*/ - for(n = m + 2; n <= nMax; ++n) - { + for (n = m + 2; n <= nMax; ++n) { k = k + n; plm = x * f1[k] * pm1 - f2[k] * pm2; - Pcup[k] = plm*rescalem; - dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (double) (n) * x * Pcup[k]) / z; + Pcup[k] = plm * rescalem; + dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (double)(n)*x * Pcup[k]) / z; pm2 = pm1; pm1 = plm; } } /* Calculate Pcup(nMax,nMax)*/ - rescalem = rescalem*z; + rescalem = rescalem * z; kstart = kstart + m + 1; pmm = pmm / PreSqr[2 * nMax]; Pcup[kstart] = pmm * rescalem; - dPcup[kstart] = -(double) (nMax) * x * Pcup[kstart] / z; + dPcup[kstart] = -(double)(nMax)*x * Pcup[kstart] / z; free(f1); free(PreSqr); free(f2); @@ -3323,7 +3412,8 @@ int MAG_PcupHigh(double *Pcup, double *dPcup, double x, int nMax) return TRUE; } /* MAG_PcupHigh */ -int MAG_PcupLow(double *Pcup, double *dPcup, double x, int nMax) +int +MAG_PcupLow(double* Pcup, double* dPcup, double x, int nMax) /* This function evaluates all of the Schmidt-semi normalized associated Legendre functions up to degree nMax. @@ -3357,41 +3447,33 @@ int MAG_PcupLow(double *Pcup, double *dPcup, double x, int nMax) z = sqrt((1.0 - x) * (1.0 + x)); NumTerms = ((nMax + 1) * (nMax + 2) / 2); - schmidtQuasiNorm = (double *) malloc((NumTerms + 1) * sizeof ( double)); + schmidtQuasiNorm = (double*)malloc((NumTerms + 1) * sizeof(double)); - if(schmidtQuasiNorm == NULL) - { + if (schmidtQuasiNorm == NULL) { MAG_Error(19); return FALSE; } /* First, Compute the Gauss-normalized associated Legendre functions*/ - for(n = 1; n <= nMax; n++) - { - for(m = 0; m <= n; m++) - { + for (n = 1; n <= nMax; n++) { + for (m = 0; m <= n; m++) { index = (n * (n + 1) / 2 + m); - if(n == m) - { + if (n == m) { index1 = (n - 1) * n / 2 + m - 1; - Pcup [index] = z * Pcup[index1]; + Pcup[index] = z * Pcup[index1]; dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; - } else if(n == 1 && m == 0) - { + } else if (n == 1 && m == 0) { index1 = (n - 1) * n / 2 + m; Pcup[index] = x * Pcup[index1]; dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; - } else if(n > 1 && n != m) - { + } else if (n > 1 && n != m) { index1 = (n - 2) * (n - 1) / 2 + m; index2 = (n - 1) * n / 2 + m; - if(m > n - 2) - { + if (m > n - 2) { Pcup[index] = x * Pcup[index2]; dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; - } else - { - k = (double) (((n - 1) * (n - 1)) - (m * m)) / (double) ((2 * n - 1) * (2 * n - 3)); + } else { + k = (double)(((n - 1) * (n - 1)) - (m * m)) / (double)((2 * n - 1) * (2 * n - 3)); Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; } @@ -3402,30 +3484,26 @@ int MAG_PcupLow(double *Pcup, double *dPcup, double x, int nMax) * functions and the Gauss-normalized version. */ schmidtQuasiNorm[0] = 1.0; - for(n = 1; n <= nMax; n++) - { + for (n = 1; n <= nMax; n++) { index = (n * (n + 1) / 2); index1 = (n - 1) * n / 2; /* for m = 0 */ - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (double) (2 * n - 1) / (double) n; + schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (double)(2 * n - 1) / (double)n; - for(m = 1; m <= n; m++) - { + for (m = 1; m <= n; m++) { index = (n * (n + 1) / 2 + m); index1 = (n * (n + 1) / 2 + m - 1); - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrt((double) ((n - m + 1) * (m == 1 ? 2 : 1)) / (double) (n + m)); + schmidtQuasiNorm[index] = + schmidtQuasiNorm[index1] * sqrt((double)((n - m + 1) * (m == 1 ? 2 : 1)) / (double)(n + m)); } - } /* Converts the Gauss-normalized associated Legendre functions to the Schmidt quasi-normalized version using pre-computed relation stored in the variable schmidtQuasiNorm */ - for(n = 1; n <= nMax; n++) - { - for(m = 0; m <= n; m++) - { + for (n = 1; n <= nMax; n++) { + for (m = 0; m <= n; m++) { index = (n * (n + 1) / 2 + m); Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; dPcup[index] = -dPcup[index] * schmidtQuasiNorm[index]; @@ -3434,12 +3512,17 @@ int MAG_PcupLow(double *Pcup, double *dPcup, double x, int nMax) } } - if(schmidtQuasiNorm) + if (schmidtQuasiNorm) free(schmidtQuasiNorm); return TRUE; } /*MAG_PcupLow */ -int MAG_SecVarSummation(MAGtype_LegendreFunction *LegendreFunction, MAGtype_MagneticModel *MagneticModel, MAGtype_SphericalHarmonicVariables SphVariables, MAGtype_CoordSpherical CoordSpherical, MAGtype_MagneticResults *MagneticResults) +int +MAG_SecVarSummation(MAGtype_LegendreFunction* LegendreFunction, + MAGtype_MagneticModel* MagneticModel, + MAGtype_SphericalHarmonicVariables SphVariables, + MAGtype_CoordSpherical CoordSpherical, + MAGtype_MagneticResults* MagneticResults) { /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. INPUT : LegendreFunction @@ -3457,10 +3540,8 @@ int MAG_SecVarSummation(MAGtype_LegendreFunction *LegendreFunction, MAGtype_Magn MagneticResults->Bz = 0.0; MagneticResults->By = 0.0; MagneticResults->Bx = 0.0; - for(n = 1; n <= MagneticModel->nMaxSecVar; n++) - { - for(m = 0; m <= n; m++) - { + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { + for (m = 0; m <= n; m++) { index = (n * (n + 1) / 2 + m); /* nMax (n+2) n m m m @@ -3468,42 +3549,45 @@ int MAG_SecVarSummation(MAGtype_LegendreFunction *LegendreFunction, MAGtype_Magn n=1 m=0 n n n */ /* Derivative with respect to radius.*/ MagneticResults->Bz -= SphVariables.RelativeRadiusPower[n] * - (MagneticModel->Secular_Var_Coeff_G[index] * SphVariables.cos_mlambda[m] + - MagneticModel->Secular_Var_Coeff_H[index] * SphVariables.sin_mlambda[m]) - * (double) (n + 1) * LegendreFunction-> Pcup[index]; + (MagneticModel->Secular_Var_Coeff_G[index] * SphVariables.cos_mlambda[m] + + MagneticModel->Secular_Var_Coeff_H[index] * SphVariables.sin_mlambda[m]) * + (double)(n + 1) * LegendreFunction->Pcup[index]; /* 1 nMax (n+2) n m m m By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Derivative with respect to longitude, divided by radius. */ MagneticResults->By += SphVariables.RelativeRadiusPower[n] * - (MagneticModel->Secular_Var_Coeff_G[index] * SphVariables.sin_mlambda[m] - - MagneticModel->Secular_Var_Coeff_H[index] * SphVariables.cos_mlambda[m]) - * (double) (m) * LegendreFunction-> Pcup[index]; + (MagneticModel->Secular_Var_Coeff_G[index] * SphVariables.sin_mlambda[m] - + MagneticModel->Secular_Var_Coeff_H[index] * SphVariables.cos_mlambda[m]) * + (double)(m)*LegendreFunction->Pcup[index]; /* nMax (n+2) n m m m Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Derivative with respect to latitude, divided by radius. */ MagneticResults->Bx -= SphVariables.RelativeRadiusPower[n] * - (MagneticModel->Secular_Var_Coeff_G[index] * SphVariables.cos_mlambda[m] + - MagneticModel->Secular_Var_Coeff_H[index] * SphVariables.sin_mlambda[m]) - * LegendreFunction-> dPcup[index]; + (MagneticModel->Secular_Var_Coeff_G[index] * SphVariables.cos_mlambda[m] + + MagneticModel->Secular_Var_Coeff_H[index] * SphVariables.sin_mlambda[m]) * + LegendreFunction->dPcup[index]; } } cos_phi = cos(DEG2RAD(CoordSpherical.phig)); - if(fabs(cos_phi) > 1.0e-10) - { + if (fabs(cos_phi) > 1.0e-10) { MagneticResults->By = MagneticResults->By / cos_phi; } else - /* Special calculation for component By at Geographic poles */ + /* Special calculation for component By at Geographic poles */ { MAG_SecVarSummationSpecial(MagneticModel, SphVariables, CoordSpherical, MagneticResults); } return TRUE; } /*MAG_SecVarSummation*/ -int MAG_SecVarSummationSpecial(MAGtype_MagneticModel *MagneticModel, MAGtype_SphericalHarmonicVariables SphVariables, MAGtype_CoordSpherical CoordSpherical, MAGtype_MagneticResults *MagneticResults) +int +MAG_SecVarSummationSpecial(MAGtype_MagneticModel* MagneticModel, + MAGtype_SphericalHarmonicVariables SphVariables, + MAGtype_CoordSpherical CoordSpherical, + MAGtype_MagneticResults* MagneticResults) { /*Special calculation for the secular variation summation at the poles. @@ -3519,10 +3603,9 @@ int MAG_SecVarSummationSpecial(MAGtype_MagneticModel *MagneticModel, MAGtype_Sph int n, index; double k, sin_phi, *PcupS, schmidtQuasiNorm1, schmidtQuasiNorm2, schmidtQuasiNorm3; - PcupS = (double *) malloc((MagneticModel->nMaxSecVar + 1) * sizeof (double)); + PcupS = (double*)malloc((MagneticModel->nMaxSecVar + 1) * sizeof(double)); - if(PcupS == NULL) - { + if (PcupS == NULL) { MAG_Error(15); return FALSE; } @@ -3533,18 +3616,15 @@ int MAG_SecVarSummationSpecial(MAGtype_MagneticModel *MagneticModel, MAGtype_Sph MagneticResults->By = 0.0; sin_phi = sin(DEG2RAD(CoordSpherical.phig)); - for(n = 1; n <= MagneticModel->nMaxSecVar; n++) - { + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { index = (n * (n + 1) / 2 + 1); - schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double) (2 * n - 1) / (double) n; - schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double) (n * 2) / (double) (n + 1)); + schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double)(2 * n - 1) / (double)n; + schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double)(n * 2) / (double)(n + 1)); schmidtQuasiNorm1 = schmidtQuasiNorm2; - if(n == 1) - { + if (n == 1) { PcupS[n] = PcupS[n - 1]; - } else - { - k = (double) (((n - 1) * (n - 1)) - 1) / (double) ((2 * n - 1) * (2 * n - 3)); + } else { + k = (double)(((n - 1) * (n - 1)) - 1) / (double)((2 * n - 1) * (2 * n - 3)); PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; } @@ -3554,17 +3634,22 @@ int MAG_SecVarSummationSpecial(MAGtype_MagneticModel *MagneticModel, MAGtype_Sph /* Derivative with respect to longitude, divided by radius. */ MagneticResults->By += SphVariables.RelativeRadiusPower[n] * - (MagneticModel->Secular_Var_Coeff_G[index] * SphVariables.sin_mlambda[1] - - MagneticModel->Secular_Var_Coeff_H[index] * SphVariables.cos_mlambda[1]) - * PcupS[n] * schmidtQuasiNorm3; + (MagneticModel->Secular_Var_Coeff_G[index] * SphVariables.sin_mlambda[1] - + MagneticModel->Secular_Var_Coeff_H[index] * SphVariables.cos_mlambda[1]) * + PcupS[n] * schmidtQuasiNorm3; } - if(PcupS) + if (PcupS) free(PcupS); return TRUE; -}/*SecVarSummationSpecial*/ - -int MAG_Summation(MAGtype_LegendreFunction *LegendreFunction, MAGtype_MagneticModel *MagneticModel, MAGtype_SphericalHarmonicVariables SphVariables, MAGtype_CoordSpherical CoordSpherical, MAGtype_MagneticResults *MagneticResults) +} /*SecVarSummationSpecial*/ + +int +MAG_Summation(MAGtype_LegendreFunction* LegendreFunction, + MAGtype_MagneticModel* MagneticModel, + MAGtype_SphericalHarmonicVariables SphVariables, + MAGtype_CoordSpherical CoordSpherical, + MAGtype_MagneticResults* MagneticResults) { /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using spherical harmonic summation. @@ -3595,10 +3680,8 @@ int MAG_Summation(MAGtype_LegendreFunction *LegendreFunction, MAGtype_MagneticMo MagneticResults->Bz = 0.0; MagneticResults->By = 0.0; MagneticResults->Bx = 0.0; - for(n = 1; n <= MagneticModel->nMax; n++) - { - for(m = 0; m <= n; m++) - { + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { index = (n * (n + 1) / 2 + m); /* nMax (n+2) n m m m @@ -3606,50 +3689,50 @@ int MAG_Summation(MAGtype_LegendreFunction *LegendreFunction, MAGtype_MagneticMo n=1 m=0 n n n */ /* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/ MagneticResults->Bz -= SphVariables.RelativeRadiusPower[n] * - (MagneticModel->Main_Field_Coeff_G[index] * SphVariables.cos_mlambda[m] + - MagneticModel->Main_Field_Coeff_H[index] * SphVariables.sin_mlambda[m]) - * (double) (n + 1) * LegendreFunction-> Pcup[index]; + (MagneticModel->Main_Field_Coeff_G[index] * SphVariables.cos_mlambda[m] + + MagneticModel->Main_Field_Coeff_H[index] * SphVariables.sin_mlambda[m]) * + (double)(n + 1) * LegendreFunction->Pcup[index]; /* 1 nMax (n+2) n m m m By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ MagneticResults->By += SphVariables.RelativeRadiusPower[n] * - (MagneticModel->Main_Field_Coeff_G[index] * SphVariables.sin_mlambda[m] - - MagneticModel->Main_Field_Coeff_H[index] * SphVariables.cos_mlambda[m]) - * (double) (m) * LegendreFunction-> Pcup[index]; + (MagneticModel->Main_Field_Coeff_G[index] * SphVariables.sin_mlambda[m] - + MagneticModel->Main_Field_Coeff_H[index] * SphVariables.cos_mlambda[m]) * + (double)(m)*LegendreFunction->Pcup[index]; /* nMax (n+2) n m m m Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) n=1 m=0 n n n */ /* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */ MagneticResults->Bx -= SphVariables.RelativeRadiusPower[n] * - (MagneticModel->Main_Field_Coeff_G[index] * SphVariables.cos_mlambda[m] + - MagneticModel->Main_Field_Coeff_H[index] * SphVariables.sin_mlambda[m]) - * LegendreFunction-> dPcup[index]; - - - + (MagneticModel->Main_Field_Coeff_G[index] * SphVariables.cos_mlambda[m] + + MagneticModel->Main_Field_Coeff_H[index] * SphVariables.sin_mlambda[m]) * + LegendreFunction->dPcup[index]; } } cos_phi = cos(DEG2RAD(CoordSpherical.phig)); - if(fabs(cos_phi) > 1.0e-10) - { + if (fabs(cos_phi) > 1.0e-10) { MagneticResults->By = MagneticResults->By / cos_phi; } else - /* Special calculation for component - By - at Geographic poles. - * If the user wants to avoid using this function, please make sure that - * the latitude is not exactly +/-90. An option is to make use the function - * MAG_CheckGeographicPoles. - */ + /* Special calculation for component - By - at Geographic poles. + * If the user wants to avoid using this function, please make sure that + * the latitude is not exactly +/-90. An option is to make use the function + * MAG_CheckGeographicPoles. + */ { MAG_SummationSpecial(MagneticModel, SphVariables, CoordSpherical, MagneticResults); } return TRUE; -}/*MAG_Summation */ +} /*MAG_Summation */ -int MAG_SummationSpecial(MAGtype_MagneticModel *MagneticModel, MAGtype_SphericalHarmonicVariables SphVariables, MAGtype_CoordSpherical CoordSpherical, MAGtype_MagneticResults *MagneticResults) +int +MAG_SummationSpecial(MAGtype_MagneticModel* MagneticModel, + MAGtype_SphericalHarmonicVariables SphVariables, + MAGtype_CoordSpherical CoordSpherical, + MAGtype_MagneticResults* MagneticResults) /* Special calculation for the component By at Geographic poles. Manoj Nair, June, 2009 manoj.c.nair@noaa.gov INPUT: MagneticModel @@ -3664,9 +3747,8 @@ See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report int n, index; double k, sin_phi, *PcupS, schmidtQuasiNorm1, schmidtQuasiNorm2, schmidtQuasiNorm3; - PcupS = (double *) malloc((MagneticModel->nMax + 1) * sizeof (double)); - if(PcupS == 0) - { + PcupS = (double*)malloc((MagneticModel->nMax + 1) * sizeof(double)); + if (PcupS == 0) { MAG_Error(14); return FALSE; } @@ -3677,23 +3759,20 @@ See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report MagneticResults->By = 0.0; sin_phi = sin(DEG2RAD(CoordSpherical.phig)); - for(n = 1; n <= MagneticModel->nMax; n++) - { + for (n = 1; n <= MagneticModel->nMax; n++) { /*Compute the ration between the Gauss-normalized associated Legendre functions and the Schmidt quasi-normalized version. This is equivalent to sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ index = (n * (n + 1) / 2 + 1); - schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double) (2 * n - 1) / (double) n; - schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double) (n * 2) / (double) (n + 1)); + schmidtQuasiNorm2 = schmidtQuasiNorm1 * (double)(2 * n - 1) / (double)n; + schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((double)(n * 2) / (double)(n + 1)); schmidtQuasiNorm1 = schmidtQuasiNorm2; - if(n == 1) - { + if (n == 1) { PcupS[n] = PcupS[n - 1]; - } else - { - k = (double) (((n - 1) * (n - 1)) - 1) / (double) ((2 * n - 1) * (2 * n - 3)); + } else { + k = (double)(((n - 1) * (n - 1)) - 1) / (double)((2 * n - 1) * (2 * n - 3)); PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2]; } @@ -3703,17 +3782,20 @@ See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ MagneticResults->By += SphVariables.RelativeRadiusPower[n] * - (MagneticModel->Main_Field_Coeff_G[index] * SphVariables.sin_mlambda[1] - - MagneticModel->Main_Field_Coeff_H[index] * SphVariables.cos_mlambda[1]) - * PcupS[n] * schmidtQuasiNorm3; + (MagneticModel->Main_Field_Coeff_G[index] * SphVariables.sin_mlambda[1] - + MagneticModel->Main_Field_Coeff_H[index] * SphVariables.cos_mlambda[1]) * + PcupS[n] * schmidtQuasiNorm3; } - if(PcupS) + if (PcupS) free(PcupS); return TRUE; -}/*MAG_SummationSpecial */ +} /*MAG_SummationSpecial */ -int MAG_TimelyModifyMagneticModel(MAGtype_Date UserDate, MAGtype_MagneticModel *MagneticModel, MAGtype_MagneticModel *TimedMagneticModel) +int +MAG_TimelyModifyMagneticModel(MAGtype_Date UserDate, + MAGtype_MagneticModel* MagneticModel, + MAGtype_MagneticModel* TimedMagneticModel) /* Time change the Model coefficients from the base year of the model using secular variation coefficients. Store the coefficients of the static model with their values advanced from epoch t0 to epoch t. @@ -3735,19 +3817,21 @@ CALLS : none a = TimedMagneticModel->nMaxSecVar; b = (a * (a + 1) / 2 + a); strcpy(TimedMagneticModel->ModelName, MagneticModel->ModelName); - for(n = 1; n <= MagneticModel->nMax; n++) - { - for(m = 0; m <= n; m++) - { + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { index = (n * (n + 1) / 2 + m); - if(index <= b) - { - TimedMagneticModel->Main_Field_Coeff_H[index] = MagneticModel->Main_Field_Coeff_H[index] + (UserDate.DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_H[index]; - TimedMagneticModel->Main_Field_Coeff_G[index] = MagneticModel->Main_Field_Coeff_G[index] + (UserDate.DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_G[index]; - TimedMagneticModel->Secular_Var_Coeff_H[index] = MagneticModel->Secular_Var_Coeff_H[index]; /* We need a copy of the secular var coef to calculate secular change */ + if (index <= b) { + TimedMagneticModel->Main_Field_Coeff_H[index] = + MagneticModel->Main_Field_Coeff_H[index] + + (UserDate.DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_H[index]; + TimedMagneticModel->Main_Field_Coeff_G[index] = + MagneticModel->Main_Field_Coeff_G[index] + + (UserDate.DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_G[index]; + TimedMagneticModel->Secular_Var_Coeff_H[index] = + MagneticModel->Secular_Var_Coeff_H[index]; /* We need a copy of the secular var coef to calculate + secular change */ TimedMagneticModel->Secular_Var_Coeff_G[index] = MagneticModel->Secular_Var_Coeff_G[index]; - } else - { + } else { TimedMagneticModel->Main_Field_Coeff_H[index] = MagneticModel->Main_Field_Coeff_H[index]; TimedMagneticModel->Main_Field_Coeff_G[index] = MagneticModel->Main_Field_Coeff_G[index]; } @@ -3758,16 +3842,15 @@ CALLS : none /*End of Spherical Harmonic Functions*/ - /****************************************************************************** *************************************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). ****************************************************************************** ******************************************************************************/ - -int MAG_ConvertGeoidToEllipsoidHeight(MAGtype_CoordGeodetic *CoordGeodetic, MAGtype_Geoid *Geoid) +int +MAG_ConvertGeoidToEllipsoidHeight(MAGtype_CoordGeodetic* CoordGeodetic, MAGtype_Geoid* Geoid) /* * The function Convert_Geoid_To_Ellipsoid_Height converts the specified WGS84 @@ -3787,25 +3870,22 @@ int MAG_ConvertGeoidToEllipsoidHeight(MAGtype_CoordGeodetic *CoordGeodetic, MAGt int Error_Code; double lat, lon; - if(Geoid->UseGeoid == 1) - { /* Geoid correction required */ - /* To ensure that latitude is less than 90 call MAG_EquivalentLatLon() */ + if (Geoid->UseGeoid == 1) { /* Geoid correction required */ + /* To ensure that latitude is less than 90 call MAG_EquivalentLatLon() */ MAG_EquivalentLatLon(CoordGeodetic->phi, CoordGeodetic->lambda, &lat, &lon); Error_Code = MAG_GetGeoidHeight(lat, lon, &DeltaHeight, Geoid); - CoordGeodetic->HeightAboveEllipsoid = CoordGeodetic->HeightAboveGeoid + DeltaHeight / 1000; /* Input and output should be kilometers, - However MAG_GetGeoidHeight returns Geoid height in meters - Hence division by 1000 */ + CoordGeodetic->HeightAboveEllipsoid = CoordGeodetic->HeightAboveGeoid + DeltaHeight / 1000; /* Input and output + should be kilometers, However MAG_GetGeoidHeight returns Geoid height in meters - Hence division by 1000 */ } else /* Geoid correction not required, copy the MSL height to Ellipsoid height */ { CoordGeodetic->HeightAboveEllipsoid = CoordGeodetic->HeightAboveGeoid; Error_Code = TRUE; } - return ( Error_Code); + return (Error_Code); } /* MAG_ConvertGeoidToEllipsoidHeight*/ -int MAG_GetGeoidHeight(double Latitude, - double Longitude, - double *DeltaHeight, - MAGtype_Geoid *Geoid) +int +MAG_GetGeoidHeight(double Latitude, double Longitude, double* DeltaHeight, MAGtype_Geoid* Geoid) /* * The function MAG_GetGeoidHeight returns the height of the * EGM96 geiod above or below the WGS84 ellipsoid, @@ -3827,29 +3907,23 @@ int MAG_GetGeoidHeight(double Latitude, double UpperY, LowerY; int Error_Code = 0; - if(!Geoid->Geoid_Initialized) - { + if (!Geoid->Geoid_Initialized) { MAG_Error(5); return (FALSE); } - if((Latitude < -90) || (Latitude > 90)) - { /* Latitude out of range */ + if ((Latitude < -90) || (Latitude > 90)) { /* Latitude out of range */ Error_Code |= 1; } - if((Longitude < -180) || (Longitude > 360)) - { /* Longitude out of range */ + if ((Longitude < -180) || (Longitude > 360)) { /* Longitude out of range */ Error_Code |= 1; } - if(!Error_Code) - { /* no errors */ + if (!Error_Code) { /* no errors */ /* Compute X and Y Offsets into Geoid Height Array: */ - if(Longitude < 0.0) - { + if (Longitude < 0.0) { OffsetX = (Longitude + 360.0) * Geoid->ScaleFactor; - } else - { + } else { OffsetX = Longitude * Geoid->ScaleFactor; } OffsetY = (90.0 - Latitude) * Geoid->ScaleFactor; @@ -3858,19 +3932,19 @@ int MAG_GetGeoidHeight(double Latitude, /* Assumes that (0,0) of Geoid Height Array is at Northwest corner: */ PostX = floor(OffsetX); - if((PostX + 1) == Geoid->NumbGeoidCols) + if ((PostX + 1) == Geoid->NumbGeoidCols) PostX--; PostY = floor(OffsetY); - if((PostY + 1) == Geoid->NumbGeoidRows) + if ((PostY + 1) == Geoid->NumbGeoidRows) PostY--; - Index = (long) (PostY * Geoid->NumbGeoidCols + PostX); - ElevationNW = (double) Geoid->GeoidHeightBuffer[ Index ]; - ElevationNE = (double) Geoid->GeoidHeightBuffer[ Index + 1 ]; + Index = (long)(PostY * Geoid->NumbGeoidCols + PostX); + ElevationNW = (double)Geoid->GeoidHeightBuffer[Index]; + ElevationNE = (double)Geoid->GeoidHeightBuffer[Index + 1]; - Index = (long) ((PostY + 1) * Geoid->NumbGeoidCols + PostX); - ElevationSW = (double) Geoid->GeoidHeightBuffer[ Index ]; - ElevationSE = (double) Geoid->GeoidHeightBuffer[ Index + 1 ]; + Index = (long)((PostY + 1) * Geoid->NumbGeoidCols + PostX); + ElevationSW = (double)Geoid->GeoidHeightBuffer[Index]; + ElevationSE = (double)Geoid->GeoidHeightBuffer[Index + 1]; /* Perform Bi-Linear Interpolation to compute Height above Ellipsoid: */ @@ -3881,16 +3955,16 @@ int MAG_GetGeoidHeight(double Latitude, LowerY = ElevationSW + DeltaX * (ElevationSE - ElevationSW); *DeltaHeight = UpperY + DeltaY * (LowerY - UpperY); - } else - { + } else { MAG_Error(17); return (FALSE); } 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.*/ { @@ -3899,27 +3973,26 @@ void MAG_EquivalentLatLon(double lat, double lon, double *repairedLat, double * *repairedLon = lon; if (colat < 0) colat = -colat; - while(colat > 360) { - colat-=360; + while (colat > 360) { + colat -= 360; } if (colat > 180) { - colat-=180; - *repairedLon = *repairedLon+180; + colat -= 180; + *repairedLon = *repairedLon + 180; } *repairedLat = 90 - colat; - if (*repairedLon > 360) - *repairedLon-=360; - if (*repairedLon < -180) - *repairedLon+=360; + if (*repairedLon > 360) + *repairedLon -= 360; + if (*repairedLon < -180) + *repairedLon += 360; } /*End of Geoid Functions*/ - - /*New Error Functions*/ -void MAG_WMMErrorCalc(double H, MAGtype_GeoMagneticElements *Uncertainty) +void +MAG_WMMErrorCalc(double H, MAGtype_GeoMagneticElements* Uncertainty) { double decl_variable, decl_constant; Uncertainty->F = WMM_UNCERTAINTY_F; @@ -3928,41 +4001,41 @@ void MAG_WMMErrorCalc(double H, MAGtype_GeoMagneticElements *Uncertainty) Uncertainty->Z = WMM_UNCERTAINTY_Z; Uncertainty->Incl = WMM_UNCERTAINTY_I; Uncertainty->Y = WMM_UNCERTAINTY_Y; - decl_variable = (WMM_UNCERTAINTY_D_COEF / H); - decl_constant = (WMM_UNCERTAINTY_D_OFFSET); - Uncertainty->Decl = sqrt(decl_constant*decl_constant + decl_variable*decl_variable); - if (Uncertainty->Decl > 180) { - Uncertainty->Decl = 180; - } + decl_variable = (WMM_UNCERTAINTY_D_COEF / H); + decl_constant = (WMM_UNCERTAINTY_D_OFFSET); + Uncertainty->Decl = sqrt(decl_constant * decl_constant + decl_variable * decl_variable); + if (Uncertainty->Decl > 180) { + Uncertainty->Decl = 180; + } } -void MAG_PrintUserDataWithUncertainty(MAGtype_GeoMagneticElements GeomagElements, - MAGtype_GeoMagneticElements Errors, - MAGtype_CoordGeodetic SpaceInput, - MAGtype_Date TimeInput, - MAGtype_MagneticModel *MagneticModel, - MAGtype_Geoid *Geoid) +void +MAG_PrintUserDataWithUncertainty(MAGtype_GeoMagneticElements GeomagElements, + MAGtype_GeoMagneticElements Errors, + MAGtype_CoordGeodetic SpaceInput, + MAGtype_Date TimeInput, + MAGtype_MagneticModel* MagneticModel, + MAGtype_Geoid* Geoid) { char DeclString[100]; char InclString[100]; MAG_DegreeToDMSstring(GeomagElements.Incl, 2, InclString); - if(GeomagElements.H < 6000 && GeomagElements.H > 2000) + if (GeomagElements.H < 6000 && GeomagElements.H > 2000) MAG_Warnings(1, GeomagElements.H, MagneticModel); - if(GeomagElements.H < 2000) + if (GeomagElements.H < 2000) MAG_Warnings(2, GeomagElements.H, MagneticModel); - if(MagneticModel->SecularVariationUsed == TRUE) - { + if (MagneticModel->SecularVariationUsed == TRUE) { MAG_DegreeToDMSstring(GeomagElements.Decl, 2, DeclString); printf("\n Results For \n\n"); - if(SpaceInput.phi < 0) + if (SpaceInput.phi < 0) printf("Latitude %.2fS\n", -SpaceInput.phi); else printf("Latitude %.2fN\n", SpaceInput.phi); - if(SpaceInput.lambda < 0) + if (SpaceInput.lambda < 0) printf("Longitude %.2fW\n", -SpaceInput.lambda); else printf("Longitude %.2fE\n", SpaceInput.lambda); - if(Geoid->UseGeoid == 1) + if (Geoid->UseGeoid == 1) printf("Altitude: %.2f Kilometers above mean sea level\n", SpaceInput.HeightAboveGeoid); else printf("Altitude: %.2f Kilometers above the WGS-84 ellipsoid\n", SpaceInput.HeightAboveEllipsoid); @@ -3973,27 +4046,38 @@ void MAG_PrintUserDataWithUncertainty(MAGtype_GeoMagneticElements GeomagElements printf("X = %9.1f +/- %5.1f nT\t\t Xdot = %5.1f\tnT/yr\n", GeomagElements.X, Errors.X, GeomagElements.Xdot); printf("Y = %9.1f +/- %5.1f nT\t\t Ydot = %5.1f\tnT/yr\n", GeomagElements.Y, Errors.Y, GeomagElements.Ydot); printf("Z = %9.1f +/- %5.1f nT\t\t Zdot = %5.1f\tnT/yr\n", GeomagElements.Z, Errors.Z, GeomagElements.Zdot); - if(GeomagElements.Decl < 0) - printf("Decl =%20s (WEST) +/-%3.0f Min Ddot = %.1f\tMin/yr\n", DeclString, 60 * Errors.Decl, 60 * GeomagElements.Decldot); + if (GeomagElements.Decl < 0) + printf("Decl =%20s (WEST) +/-%3.0f Min Ddot = %.1f\tMin/yr\n", + DeclString, + 60 * Errors.Decl, + 60 * GeomagElements.Decldot); else - printf("Decl =%20s (EAST) +/-%3.0f Min Ddot = %.1f\tMin/yr\n", DeclString, 60 * Errors.Decl, 60 * GeomagElements.Decldot); - if(GeomagElements.Incl < 0) - printf("Incl =%20s (UP) +/-%3.0f Min Idot = %.1f\tMin/yr\n", InclString, 60 * Errors.Incl, 60 * GeomagElements.Incldot); + printf("Decl =%20s (EAST) +/-%3.0f Min Ddot = %.1f\tMin/yr\n", + DeclString, + 60 * Errors.Decl, + 60 * GeomagElements.Decldot); + if (GeomagElements.Incl < 0) + printf("Incl =%20s (UP) +/-%3.0f Min Idot = %.1f\tMin/yr\n", + InclString, + 60 * Errors.Incl, + 60 * GeomagElements.Incldot); else - printf("Incl =%20s (DOWN) +/-%3.0f Min Idot = %.1f\tMin/yr\n", InclString, 60 * Errors.Incl, 60 * GeomagElements.Incldot); - } else - { + printf("Incl =%20s (DOWN) +/-%3.0f Min Idot = %.1f\tMin/yr\n", + InclString, + 60 * Errors.Incl, + 60 * GeomagElements.Incldot); + } else { MAG_DegreeToDMSstring(GeomagElements.Decl, 2, DeclString); printf("\n Results For \n\n"); - if(SpaceInput.phi < 0) + if (SpaceInput.phi < 0) printf("Latitude %.2fS\n", -SpaceInput.phi); else printf("Latitude %.2fN\n", SpaceInput.phi); - if(SpaceInput.lambda < 0) + if (SpaceInput.lambda < 0) printf("Longitude %.2fW\n", -SpaceInput.lambda); else printf("Longitude %.2fE\n", SpaceInput.lambda); - if(Geoid->UseGeoid == 1) + if (Geoid->UseGeoid == 1) printf("Altitude: %.2f Kilometers above MSL\n", SpaceInput.HeightAboveGeoid); else printf("Altitude: %.2f Kilometers above WGS-84 Ellipsoid\n", SpaceInput.HeightAboveEllipsoid); @@ -4004,75 +4088,69 @@ void MAG_PrintUserDataWithUncertainty(MAGtype_GeoMagneticElements GeomagElements printf("X = %-9.1f +/-%5.1f nT\n", GeomagElements.X, Errors.X); printf("Y = %-9.1f +/-%5.1f nT\n", GeomagElements.Y, Errors.Y); printf("Z = %-9.1f +/-%5.1f nT\n", GeomagElements.Z, Errors.Z); - if(GeomagElements.Decl < 0) + if (GeomagElements.Decl < 0) printf("Decl =%20s (WEST)+/-%4f\n", DeclString, 60 * Errors.Decl); else printf("Decl =%20s (EAST)+/-%4f\n", DeclString, 60 * Errors.Decl); - if(GeomagElements.Incl < 0) + if (GeomagElements.Incl < 0) printf("Incl =%20s (UP)+/-%4f\n", InclString, 60 * Errors.Incl); else printf("Incl =%20s (DOWN)+/-%4f\n", InclString, 60 * Errors.Incl); } - if(SpaceInput.phi <= -55 || SpaceInput.phi >= 55) - /* Print Grid Variation */ + if (SpaceInput.phi <= -55 || SpaceInput.phi >= 55) + /* Print Grid Variation */ { MAG_DegreeToDMSstring(GeomagElements.GV, 2, InclString); printf("\n\n Grid variation =%20s\n", InclString); } -}/*MAG_PrintUserDataWithUncertainty*/ +} /*MAG_PrintUserDataWithUncertainty*/ -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)){ +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); } - for(i = 0, done = 0, j = 0; i <= 64 && !done; i++) - { - if(buffer[i] == '.') - { + for (i = 0, done = 0, j = 0; i <= 64 && !done; i++) { + if (buffer[i] == '.') { j = sscanf(buffer, "%lf", latitude); - if(j == 1) + if (j == 1) done = 1; else done = -1; } - if(buffer[i] == ',') - { - if(MAG_ValidateDMSstring(buffer, (int) bounds[0], (int) bounds[1], Error_Message)) - { + if (buffer[i] == ',') { + if (MAG_ValidateDMSstring(buffer, (int)bounds[0], (int)bounds[1], Error_Message)) { MAG_DMSstringToDegree(buffer, latitude); done = 1; } else done = -1; } - if(buffer[i] == ' ')/* This detects if there is a ' ' somewhere in the string, - if there is the program tries to interpret the input as Degrees Minutes Seconds.*/ + if (buffer[i] == ' ') /* This detects if there is a ' ' somewhere in the string, + if there is the program tries to interpret the input as Degrees Minutes Seconds.*/ { - if(MAG_ValidateDMSstring(buffer, (int)bounds[0], (int)bounds[1], Error_Message)) - { + if (MAG_ValidateDMSstring(buffer, (int)bounds[0], (int)bounds[1], Error_Message)) { MAG_DMSstringToDegree(buffer, latitude); done = 1; } else done = -1; } - if(buffer[i] == '\0' || done == -1) - { - if(MAG_ValidateDMSstring(buffer, (int) bounds[0], (int) bounds[1], Error_Message) && done != -1) - { + if (buffer[i] == '\0' || done == -1) { + if (MAG_ValidateDMSstring(buffer, (int)bounds[0], (int)bounds[1], Error_Message) && done != -1) { sscanf(buffer, "%lf", latitude); done = 1; - } else - { + } else { printf("%s", Error_Message); strcpy(buffer, ""); printf("\nError encountered, please re-enter as '(-)DDD,MM,SS' or in Decimal Degrees DD.ddd:\n"); - while(NULL == fgets(buffer, 40, stdin)) { + while (NULL == fgets(buffer, 40, stdin)) { printf("\nError encountered, please re-enter as '(-)DDD,MM,SS' or in Decimal Degrees DD.ddd:\n"); } i = -1; @@ -4082,75 +4160,84 @@ void MAG_GetDeg(char* Query_String, double* latitude, double bounds[2]) { } } -int MAG_GetAltitude(char* Query_String, MAGtype_Geoid *Geoid, MAGtype_CoordGeodetic* coords, int bounds[2], int AltitudeSetting){ - int done, j, UpBoundOn; - char tmp; - char buffer[64]; - double value; - done = 0; - if(bounds[1] != NO_ALT_MAX){ - UpBoundOn = TRUE; +int +MAG_GetAltitude(char* Query_String, + MAGtype_Geoid* Geoid, + MAGtype_CoordGeodetic* coords, + int bounds[2], + int AltitudeSetting) +{ + int done, j, UpBoundOn; + char tmp; + char buffer[64]; + double value; + done = 0; + if (bounds[1] != NO_ALT_MAX) { + UpBoundOn = TRUE; } else { UpBoundOn = FALSE; } printf("%s", Query_String); - - while(!done) - { + + while (!done) { strcpy(buffer, ""); - while(NULL == fgets(buffer, 40, stdin)) { + while (NULL == fgets(buffer, 40, stdin)) { printf("%s", Query_String); } j = 0; - if((AltitudeSetting != MSLON) && (buffer[0] == 'e' || buffer[0] == 'E' || AltitudeSetting == WGS84ON)) /* User entered height above WGS-84 ellipsoid, copy it to CoordGeodetic->HeightAboveEllipsoid */ + if ((AltitudeSetting != MSLON) && + (buffer[0] == 'e' || buffer[0] == 'E' || + AltitudeSetting == WGS84ON)) /* User entered height above WGS-84 ellipsoid, copy it to + CoordGeodetic->HeightAboveEllipsoid */ { - if(buffer[0]=='e' || buffer[0]=='E') { - j = sscanf(buffer, "%c%lf", &tmp, &coords->HeightAboveEllipsoid); - } else { - j = sscanf(buffer, "%lf", &coords->HeightAboveEllipsoid); - } - if(j == 2) + if (buffer[0] == 'e' || buffer[0] == 'E') { + j = sscanf(buffer, "%c%lf", &tmp, &coords->HeightAboveEllipsoid); + } else { + j = sscanf(buffer, "%lf", &coords->HeightAboveEllipsoid); + } + if (j == 2) j = 1; Geoid->UseGeoid = 0; coords->HeightAboveGeoid = coords->HeightAboveEllipsoid; - value = coords->HeightAboveEllipsoid; + value = coords->HeightAboveEllipsoid; } else /* User entered height above MSL, convert it to the height above WGS-84 ellipsoid */ { Geoid->UseGeoid = 1; j = sscanf(buffer, "%lf", &coords->HeightAboveGeoid); MAG_ConvertGeoidToEllipsoidHeight(coords, Geoid); - value = coords->HeightAboveGeoid; + value = coords->HeightAboveGeoid; } - if(j == 1) + if (j == 1) done = 1; else printf("\nIllegal Format, please re-enter as '(-)HHH.hhh:'\n"); - if((value < bounds[0] || (value > bounds[1] && UpBoundOn)) && done == 1) { - if(UpBoundOn) { - done = 0; - printf("\nWarning: The value you have entered of %f km for the elevation is outside of the required range.\n", value); - printf(" An elevation between %d km and %d km is needed. \n", bounds[0], bounds[1]); - if (AltitudeSetting == WGS84ON){ - printf("Please enter height above WGS-84 Ellipsoid (in kilometers):\n"); - } else if (AltitudeSetting==MSLON){ - printf("Please enter height above mean sea level (in kilometers):\n"); - } else { - printf("Please enter height in kilometers (prepend E for height above WGS-84 Ellipsoid):"); - } - } else { - switch(MAG_Warnings(3, value, NULL)) { - case 0: - return USER_GAVE_UP; - case 1: - done = 0; - printf("Please enter height above sea level (in kilometers):\n"); - break; - case 2: - break; - } + if ((value < bounds[0] || (value > bounds[1] && UpBoundOn)) && done == 1) { + if (UpBoundOn) { + done = 0; + printf("\nWarning: The value you have entered of %f km for the elevation is outside of the required " + "range.\n", + value); + printf(" An elevation between %d km and %d km is needed. \n", bounds[0], bounds[1]); + if (AltitudeSetting == WGS84ON) { + printf("Please enter height above WGS-84 Ellipsoid (in kilometers):\n"); + } else if (AltitudeSetting == MSLON) { + printf("Please enter height above mean sea level (in kilometers):\n"); + } else { + printf("Please enter height in kilometers (prepend E for height above WGS-84 Ellipsoid):"); + } + } else { + switch (MAG_Warnings(3, value, NULL)) { + case 0: + return USER_GAVE_UP; + case 1: + done = 0; + printf("Please enter height above sea level (in kilometers):\n"); + break; + case 2: + break; + } } } } 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/_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/magneticFieldWMM/magneticFieldWMM.cpp b/src/simulation/environment/magneticFieldWMM/magneticFieldWMM.cpp index 29a448b166..89a854b9a3 100644 --- a/src/simulation/environment/magneticFieldWMM/magneticFieldWMM.cpp +++ b/src/simulation/environment/magneticFieldWMM/magneticFieldWMM.cpp @@ -22,14 +22,15 @@ #include "architecture/utilities/rigidBodyKinematics.h" #include "EGM9615.h" -/*! The constructor method initializes the dipole parameters to zero, resuling in a zero magnetic field result by default. +/*! The constructor method initializes the dipole parameters to zero, resuling in a zero magnetic field result by + default. */ MagneticFieldWMM::MagneticFieldWMM() { //! - Set the default magnetic field properties - this->planetRadius = REQ_EARTH*1000.; // must be the radius of Earth for WMM - this->magneticModel = nullptr; // a nullptr means no WMM coefficients have been loaded + this->planetRadius = REQ_EARTH * 1000.; // must be the radius of Earth for WMM + this->magneticModel = nullptr; // a nullptr means no WMM coefficients have been loaded this->epochDateFractionalYear = -1; // negative value means this variable has not been set } @@ -46,7 +47,8 @@ MagneticFieldWMM::~MagneticFieldWMM() /*! Custom Reset() method. This loads the WMM coefficient file and gets the model setup. */ -void MagneticFieldWMM::customReset(uint64_t CurrentClock) +void +MagneticFieldWMM::customReset(uint64_t CurrentClock) { if (this->magneticModel != nullptr) { /* clean up the prior initialization */ @@ -55,7 +57,7 @@ void MagneticFieldWMM::customReset(uint64_t CurrentClock) } //! - Check that required module variables are set - if(this->dataPath == "") { + if (this->dataPath == "") { bskLogger.bskLog(BSK_ERROR, "WMM data path was not set. No WMM."); return; } @@ -64,12 +66,15 @@ void MagneticFieldWMM::customReset(uint64_t CurrentClock) initializeWmm(); } -/*! Custom customSetEpochFromVariable() method. This allows specifying epochDateFractionYear directly from Python. If an epoch message is set then this variable is not used. +/*! Custom customSetEpochFromVariable() method. This allows specifying epochDateFractionYear directly from Python. If + an epoch message is set then this variable is not used. */ -void MagneticFieldWMM::customSetEpochFromVariable() +void +MagneticFieldWMM::customSetEpochFromVariable() { - //! - only convert if the fraction year variable was set to a non-zero value. Otherwise use the BSK epoch default setup by the base class. + //! - only convert if the fraction year variable was set to a non-zero value. Otherwise use the BSK epoch default + //! setup by the base class. if (this->epochDateFractionalYear > 0.0) { decimalYear2Gregorian(this->epochDateFractionalYear, &this->epochDateTime); } @@ -78,7 +83,8 @@ void MagneticFieldWMM::customSetEpochFromVariable() /*! Convert a fraction year double value into a time structure with gregorian date/time information */ -void MagneticFieldWMM::decimalYear2Gregorian(double fractionalYear, struct tm *gregorian) +void +MagneticFieldWMM::decimalYear2Gregorian(double fractionalYear, struct tm* gregorian) { //! -Use the WMM routine to get the year, month and day information MAGtype_Date calendar; @@ -92,21 +98,21 @@ void MagneticFieldWMM::decimalYear2Gregorian(double fractionalYear, struct tm *g //! - Find the unused remained of the fractional year and add to the gregorian calendar //! - determine number of days in this year double daysInYear = 365; - if((calendar.Year % 4 == 0 && calendar.Year % 100 != 0) || calendar.Year % 400 == 0) + if ((calendar.Year % 4 == 0 && calendar.Year % 100 != 0) || calendar.Year % 400 == 0) daysInYear = 366; //! - determine missing hours MAG_DateToYear(&calendar, Error); double diff = this->epochDateFractionalYear - calendar.DecimalYear; - this->epochDateTime.tm_hour = (int) round(diff * (24. * daysInYear)); - diff -= this->epochDateTime.tm_hour / ( 24. * daysInYear); + this->epochDateTime.tm_hour = (int)round(diff * (24. * daysInYear)); + diff -= this->epochDateTime.tm_hour / (24. * daysInYear); //! - determine missing minutes - this->epochDateTime.tm_min = (int) round(diff * (24. * 60 * daysInYear)); + this->epochDateTime.tm_min = (int)round(diff * (24. * 60 * daysInYear)); diff -= this->epochDateTime.tm_min / (24. * 60 * daysInYear); //! - determine missing seconds - this->epochDateTime.tm_sec = (int) round(diff * (24. * 60 * 60 * daysInYear)); + this->epochDateTime.tm_sec = (int)round(diff * (24. * 60 * 60 * daysInYear)); //! - ensure that daylight saving flag is off this->epochDateTime.tm_isdst = 0; @@ -118,28 +124,29 @@ void MagneticFieldWMM::decimalYear2Gregorian(double fractionalYear, struct tm *g /*! Convert a time structure with gregorian date/time information into a fraction year value. @return double */ -double MagneticFieldWMM::gregorian2DecimalYear(double currentTime) +double +MagneticFieldWMM::gregorian2DecimalYear(double currentTime) { - double decimalYear; // [years] fraction year date/time format - struct tm localDateTime{}; // [] date/time structure + double decimalYear; // [years] fraction year date/time format + struct tm localDateTime{}; // [] date/time structure //! - compute current decimalYear value MAGtype_Date calendar; char Error_Message[255]; localDateTime = this->epochDateTime; - localDateTime.tm_sec += (int) round(currentTime); // sets the current seconds + localDateTime.tm_sec += (int)round(currentTime); // sets the current seconds mktime(&localDateTime); calendar.Year = localDateTime.tm_year + 1900; calendar.Month = localDateTime.tm_mon + 1; calendar.Day = localDateTime.tm_mday; - if (!MAG_DateToYear(&calendar, Error_Message)){ + if (!MAG_DateToYear(&calendar, Error_Message)) { bskLogger.bskLog(BSK_ERROR, "Could not convert date to decimal year. \nError message: %s", Error_Message); } //! - determine number of days in this year double daysInYear = 365; - if((calendar.Year % 4 == 0 && calendar.Year % 100 != 0) || calendar.Year % 400 == 0) + if ((calendar.Year % 4 == 0 && calendar.Year % 100 != 0) || calendar.Year % 400 == 0) daysInYear = 366; decimalYear = calendar.DecimalYear; @@ -155,14 +162,15 @@ double MagneticFieldWMM::gregorian2DecimalYear(double currentTime) @param currentTime current time (s) */ -void MagneticFieldWMM::evaluateMagneticFieldModel(MagneticFieldMsgPayload *msg, double currentTime) +void +MagneticFieldWMM::evaluateMagneticFieldModel(MagneticFieldMsgPayload* msg, double currentTime) { - Eigen::Vector3d rHat_P; // [] normalized position vector in E frame components - double PM[3][3]; // [] DCM from magnetic field frame to planet frame P - double NM[3][3]; // [] DCM from magnetic field frame to inertial frame N - double B_M[3]; // [T] magnetic field in Magnetic field aligned frame - double M2[3][3]; // [] 2nd axis rotation DCM - double M3[3][3]; // [] 3rd axis rotation DCM + Eigen::Vector3d rHat_P; // [] normalized position vector in E frame components + double PM[3][3]; // [] DCM from magnetic field frame to planet frame P + double NM[3][3]; // [] DCM from magnetic field frame to inertial frame N + double B_M[3]; // [T] magnetic field in Magnetic field aligned frame + double M2[3][3]; // [] 2nd axis rotation DCM + double M3[3][3]; // [] 3rd axis rotation DCM if (this->magneticModel == nullptr) { // no magnetic field was setup, set field to zero and return @@ -175,9 +183,9 @@ void MagneticFieldWMM::evaluateMagneticFieldModel(MagneticFieldMsgPayload *msg, //! - compute spacecraft latitude and longitude in spherical (geocentric) coordinates MAGtype_CoordSpherical coordSpherical; - coordSpherical.phig = R2D * safeAsin(rHat_P[2]); /* degrees North */ + coordSpherical.phig = R2D * safeAsin(rHat_P[2]); /* degrees North */ coordSpherical.lambda = R2D * atan2(rHat_P[1], rHat_P[0]); /* degrees East */ - coordSpherical.r = this->orbitRadius / 1000.0; /* must be in km */ + coordSpherical.r = this->orbitRadius / 1000.0; /* must be in km */ //! - evaluate NED magnetic field computeWmmField(gregorian2DecimalYear(currentTime), coordSpherical, B_M); @@ -193,7 +201,8 @@ void MagneticFieldWMM::evaluateMagneticFieldModel(MagneticFieldMsgPayload *msg, /*! Performs memory cleanup necessary for magnetic field models */ -void MagneticFieldWMM::cleanupEarthMagFieldModel() +void +MagneticFieldWMM::cleanupEarthMagFieldModel() { MAG_FreeMagneticModelMemory(this->timedMagneticModel); MAG_FreeMagneticModelMemory(this->magneticModel); @@ -202,7 +211,8 @@ void MagneticFieldWMM::cleanupEarthMagFieldModel() MAG_FreeSphVarMemory(this->SphVariables); } -void MagneticFieldWMM::computeWmmField(double decimalYear, MAGtype_CoordSpherical coordSpherical, double B_M[3]) +void +MagneticFieldWMM::computeWmmField(double decimalYear, MAGtype_CoordSpherical coordSpherical, double B_M[3]) { MAGtype_MagneticResults MagneticResultsSph; @@ -213,38 +223,41 @@ void MagneticFieldWMM::computeWmmField(double decimalYear, MAGtype_CoordSpherica MAG_TimelyModifyMagneticModel(this->userDate, this->magneticModel, this->timedMagneticModel); /* Compute Spherical Harmonic variables */ - MAG_ComputeSphericalHarmonicVariables(this->ellip, coordSpherical, this->timedMagneticModel->nMax, this->SphVariables); + MAG_ComputeSphericalHarmonicVariables( + this->ellip, coordSpherical, this->timedMagneticModel->nMax, this->SphVariables); /* Compute ALF */ MAG_AssociatedLegendreFunction(coordSpherical, this->timedMagneticModel->nMax, this->LegendreFunction); /* Accumulate the spherical harmonic coefficients*/ - MAG_Summation(this->LegendreFunction, this->timedMagneticModel, *this->SphVariables, coordSpherical, &MagneticResultsSph); + MAG_Summation( + this->LegendreFunction, this->timedMagneticModel, *this->SphVariables, coordSpherical, &MagneticResultsSph); v3Set(MagneticResultsSph.Bx, MagneticResultsSph.By, MagneticResultsSph.Bz, B_M); v3Scale(1e-9, B_M, B_M); /* convert nano-Tesla to Tesla */ } -void MagneticFieldWMM::initializeWmm() +void +MagneticFieldWMM::initializeWmm() { int nMax = 0; int nTerms; auto fileName = this->dataPath + "WMM.COF"; - MAGtype_MagneticModel *models[1]; + MAGtype_MagneticModel* models[1]; if (!MAG_robustReadMagModels(const_cast(fileName.c_str()), &models, 1)) { bskLogger.bskLog(BSK_ERROR, "WMM unable to load file %s", fileName.c_str()); return; } this->magneticModel = models[0]; - if(nMax < magneticModel->nMax) { + if (nMax < magneticModel->nMax) { nMax = magneticModel->nMax; } nTerms = ((nMax + 1) * (nMax + 2) / 2); /* For storing the time modified WMM Model parameters */ this->timedMagneticModel = MAG_AllocateModelMemory(nTerms); - if(this->magneticModel == nullptr || this->timedMagneticModel == nullptr) { + if (this->magneticModel == nullptr || this->timedMagneticModel == nullptr) { MAG_Error(2); } /* Set default values and constants */ diff --git a/src/simulation/environment/magneticFieldWMM/magneticFieldWMM.h b/src/simulation/environment/magneticFieldWMM/magneticFieldWMM.h index 1dc48f1893..24a06204ed 100644 --- a/src/simulation/environment/magneticFieldWMM/magneticFieldWMM.h +++ b/src/simulation/environment/magneticFieldWMM/magneticFieldWMM.h @@ -30,37 +30,36 @@ #include /*! @brief magnetic field WMM class */ -class MagneticFieldWMM: public MagneticFieldBase { -public: +class MagneticFieldWMM : public MagneticFieldBase +{ + public: MagneticFieldWMM(); ~MagneticFieldWMM(); -private: - void evaluateMagneticFieldModel(MagneticFieldMsgPayload *msg, double currentTime); + private: + void evaluateMagneticFieldModel(MagneticFieldMsgPayload* msg, double currentTime); void initializeWmm(); void cleanupEarthMagFieldModel(); void computeWmmField(double decimalYear, MAGtype_CoordSpherical coord, double B_M[3]); void customReset(uint64_t CurrentClock); void customSetEpochFromVariable(); - void decimalYear2Gregorian(double fractionalYear, struct tm *gregorian); + void decimalYear2Gregorian(double fractionalYear, struct tm* gregorian); double gregorian2DecimalYear(double currentTime); -public: - std::string dataPath; //!< -- String with the path to the WMM coefficient file - double epochDateFractionalYear; //!< Specified epoch date as a fractional year - BSKLogger bskLogger; //!< -- BSK Logging + public: + std::string dataPath; //!< -- String with the path to the WMM coefficient file + double epochDateFractionalYear; //!< Specified epoch date as a fractional year + BSKLogger bskLogger; //!< -- BSK Logging -private: - MAGtype_MagneticModel *magneticModel; - MAGtype_MagneticModel *timedMagneticModel; - MAGtype_Ellipsoid ellip; - MAGtype_Geoid geoid; - MAGtype_Date userDate; + private: + MAGtype_MagneticModel* magneticModel; + MAGtype_MagneticModel* timedMagneticModel; + MAGtype_Ellipsoid ellip; + MAGtype_Geoid geoid; + MAGtype_Date userDate; MAGtype_LegendreFunction* LegendreFunction; MAGtype_SphericalHarmonicVariables* SphVariables; - }; - #endif /* WMM_MAGNETIC_FIELD_H */ 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/_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/planetEphemeris/planetEphemeris.cpp b/src/simulation/environment/planetEphemeris/planetEphemeris.cpp old mode 100755 new mode 100644 index 8aba555ed3..9a6f85322e --- a/src/simulation/environment/planetEphemeris/planetEphemeris.cpp +++ b/src/simulation/environment/planetEphemeris/planetEphemeris.cpp @@ -24,7 +24,6 @@ #include "architecture/utilities/macroDefinitions.h" #include "architecture/utilities/rigidBodyKinematics.h" - /*! This constructor initializes the variables. */ PlanetEphemeris::PlanetEphemeris() @@ -35,40 +34,42 @@ PlanetEphemeris::PlanetEphemeris() /*! Module deconstructor */ PlanetEphemeris::~PlanetEphemeris() { - for (long unsigned int c=0; cplanetOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->planetOutMsgs.size(); c++) { delete this->planetOutMsgs.at(c); } return; } /*! add list of planet names */ -void PlanetEphemeris::setPlanetNames(std::vector names) +void +PlanetEphemeris::setPlanetNames(std::vector names) { this->planetNames = names; /* create corresponding output messages */ - for (long unsigned int c=0; cplanetNames.size(); c++) { - Message *spMsg; + for (long unsigned int c = 0; c < this->planetNames.size(); c++) { + Message* spMsg; spMsg = new Message; this->planetOutMsgs.push_back(spMsg); } } - - -void PlanetEphemeris::Reset(uint64_t CurrenSimNanos) +void +PlanetEphemeris::Reset(uint64_t CurrenSimNanos) { - this->epochTime = CurrenSimNanos*NANO2SEC; + this->epochTime = CurrenSimNanos * NANO2SEC; /*! - do sanity checks that the vector arrays for planet names and ephemeris have the same length */ if (this->planetElements.size() != this->planetNames.size()) { - bskLogger.bskLog(BSK_ERROR, "Only %lu planet element sets provided, but %lu plane names are present.", - this->planetElements.size(), this->planetNames.size()); + bskLogger.bskLog(BSK_ERROR, + "Only %lu planet element sets provided, but %lu plane names are present.", + this->planetElements.size(), + this->planetNames.size()); } /*! - See if planet orientation information is set */ - if(this->lst0.size() == 0 && this->rotRate.size() == 0 && - this->declination.size() == 0 && this->rightAscension.size() == 0) { + if (this->lst0.size() == 0 && this->rotRate.size() == 0 && this->declination.size() == 0 && + this->rightAscension.size() == 0) { this->computeAttitudeFlag = 0; return; } else { @@ -78,65 +79,72 @@ void PlanetEphemeris::Reset(uint64_t CurrenSimNanos) if (computeAttitudeFlag) { /*! - check that the right number of planet local sideral time angles are provided */ if (this->lst0.size() != this->planetNames.size()) { - bskLogger.bskLog(BSK_ERROR, "Only %lu planet initial principal rotation angles provided, but %lu planet names are present.", - this->lst0.size(), this->planetNames.size()); + bskLogger.bskLog( + BSK_ERROR, + "Only %lu planet initial principal rotation angles provided, but %lu planet names are present.", + this->lst0.size(), + this->planetNames.size()); } /*! - check that the right number of planet polar axis right ascension angles are provided */ if (this->rightAscension.size() != this->planetNames.size()) { - bskLogger.bskLog(BSK_ERROR, "Only %lu planet right ascension angles provided, but %lu planet names are present.", - this->rightAscension.size(), this->planetNames.size()); + bskLogger.bskLog(BSK_ERROR, + "Only %lu planet right ascension angles provided, but %lu planet names are present.", + this->rightAscension.size(), + this->planetNames.size()); } /*! - check that the right number of planet polar axis declination angles are provided */ if (this->declination.size() != this->planetNames.size()) { - bskLogger.bskLog(BSK_ERROR, "Only %lu planet declination angles provided, but %lu planet names are present.", - this->declination.size(), this->planetNames.size()); + bskLogger.bskLog(BSK_ERROR, + "Only %lu planet declination angles provided, but %lu planet names are present.", + this->declination.size(), + this->planetNames.size()); } /*! - check that the right number of planet polar rotation rates are provided */ if (this->rotRate.size() != this->planetNames.size()) { - bskLogger.bskLog(BSK_ERROR, "Only %lu planet rotation rates provided, but %lu planet names are present.", - this->rotRate.size(), this->planetNames.size()); + bskLogger.bskLog(BSK_ERROR, + "Only %lu planet rotation rates provided, but %lu planet names are present.", + this->rotRate.size(), + this->planetNames.size()); } } return; } - /*! This update routine loops over all the planets and creates their heliocentric position and velocity vectors at the current time. If the planet orientation information is provided, then this is computed as well. The default orientation information is a zero orientation. @param CurrentSimNanos The current clock time for the simulation */ -void PlanetEphemeris::UpdateState(uint64_t CurrentSimNanos) +void +PlanetEphemeris::UpdateState(uint64_t CurrentSimNanos) { std::vector::iterator it; - double time; // [s] time since epoch - double mu; // [m^3/s^2] gravity constant of the sun - uint64_t c; // [] counter - double f0; // [r] true anomaly at epoch - double e; // [] orbit eccentricity - double M0; // [r] mean anomaly at epoch - double M; // [r] current mean anomaly - double lst; // [r] local sidereal time angle - double omega_NP_P[3]; // [r/s] angular velocity of inertial frame relative to planet frame in planet frame components - double tilde[3][3]; // [] skew-symmetric matrix - double theta_PN[3]; // [rad] 3-2-3 planet orientation coordinates - - + double time; // [s] time since epoch + double mu; // [m^3/s^2] gravity constant of the sun + uint64_t c; // [] counter + double f0; // [r] true anomaly at epoch + double e; // [] orbit eccentricity + double M0; // [r] mean anomaly at epoch + double M; // [r] current mean anomaly + double lst; // [r] local sidereal time angle + double + omega_NP_P[3]; // [r/s] angular velocity of inertial frame relative to planet frame in planet frame components + double tilde[3][3]; // [] skew-symmetric matrix + double theta_PN[3]; // [rad] 3-2-3 planet orientation coordinates //! - set time in units of seconds - time = CurrentSimNanos*NANO2SEC; + time = CurrentSimNanos * NANO2SEC; //! - set sun gravity constant - mu = MU_SUN*pow(1000.,3); + mu = MU_SUN * pow(1000., 3); //! - Loop over the planet names and create new data - for(it = this->planetNames.begin(), c=0; it != this->planetNames.end(); it++, c++) - { + for (it = this->planetNames.begin(), c = 0; it != this->planetNames.end(); it++, c++) { //! - Create new planet output message copy SpicePlanetStateMsgPayload newPlanet; newPlanet = this->planetOutMsgs.at(c)->zeroMsgPayload; @@ -150,8 +158,8 @@ void PlanetEphemeris::UpdateState(uint64_t CurrentSimNanos) f0 = this->planetElements[c].f; e = this->planetElements[c].e; M0 = E2M(f2E(f0, e), e); - M = M0 + sqrt(mu/pow(this->planetElements[c].a, 3)) * (time - this->epochTime); - this->planetElements[c].f = E2f(M2E(M, e),e); + M = M0 + sqrt(mu / pow(this->planetElements[c].a, 3)) * (time - this->epochTime); + this->planetElements[c].f = E2f(M2E(M, e), e); elem2rv(mu, &(this->planetElements[c]), newPlanet.PositionVector, newPlanet.VelocityVector); //! - retain planet epoch information @@ -159,7 +167,7 @@ void PlanetEphemeris::UpdateState(uint64_t CurrentSimNanos) if (this->computeAttitudeFlag == 1) { //! - compute current planet principal rotation parameter vector */ - lst = this->lst0[c] + this->rotRate[c]*(time - this->epochTime); + lst = this->lst0[c] + this->rotRate[c] * (time - this->epochTime); v3Set(this->rightAscension.at(c), M_PI_2 - this->declination.at(c), lst, theta_PN); Euler3232C(theta_PN, newPlanet.J20002Pfix); @@ -177,7 +185,6 @@ void PlanetEphemeris::UpdateState(uint64_t CurrentSimNanos) //! - write output message this->planetOutMsgs.at(c)->write(&newPlanet, this->moduleID, CurrentSimNanos); - } return; } diff --git a/src/simulation/environment/planetEphemeris/planetEphemeris.h b/src/simulation/environment/planetEphemeris/planetEphemeris.h old mode 100755 new mode 100644 index 65e2718a6e..9745c66239 --- a/src/simulation/environment/planetEphemeris/planetEphemeris.h +++ b/src/simulation/environment/planetEphemeris/planetEphemeris.h @@ -29,10 +29,10 @@ #include "architecture/utilities/orbitalMotion.h" #include "architecture/utilities/bskLogging.h" - /*! @brief planet ephemeris class */ -class PlanetEphemeris: public SysModel { -public: +class PlanetEphemeris : public SysModel +{ + public: PlanetEphemeris(); ~PlanetEphemeris(); @@ -41,24 +41,23 @@ class PlanetEphemeris: public SysModel { void setPlanetNames(std::vector planetNames); -public: + public: std::vector*> planetOutMsgs; //!< -- vector of planet state output messages - std::vectorplanetElements; //!< -- Vector of planet classical orbit elements + std::vector planetElements; //!< -- Vector of planet classical orbit elements - std::vector rightAscension; //!< [r] right ascension of the north pole rotation axis (pos. 3-axis) - std::vector declination; //!< [r] Declination of the north pole rotation axis (neg. 2-axis) - std::vector lst0; //!< [r] initial planet local sidereal time angle (pos. 3-axis) + std::vector rightAscension; //!< [r] right ascension of the north pole rotation axis (pos. 3-axis) + std::vector declination; //!< [r] Declination of the north pole rotation axis (neg. 2-axis) + std::vector lst0; //!< [r] initial planet local sidereal time angle (pos. 3-axis) - std::vector rotRate; //!< [r/s] planet rotation rate + std::vector rotRate; //!< [r/s] planet rotation rate - BSKLogger bskLogger; //!< -- BSK Logging + BSKLogger bskLogger; //!< -- BSK Logging -private: - std::vector planetNames; //!< -- Vector of planet names - double epochTime; //!< [s] time of provided planet ephemeris epoch - int computeAttitudeFlag; //!< -- flag indicating if the planet orienation information is provided + private: + std::vector planetNames; //!< -- Vector of planet names + double epochTime; //!< [s] time of provided planet ephemeris epoch + int computeAttitudeFlag; //!< -- flag indicating if the planet orienation information is provided }; - #endif 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/_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/solarFlux/solarFlux.cpp b/src/simulation/environment/solarFlux/solarFlux.cpp old mode 100755 new mode 100644 index 6aba95a80d..acc21b5de6 --- a/src/simulation/environment/solarFlux/solarFlux.cpp +++ b/src/simulation/environment/solarFlux/solarFlux.cpp @@ -20,11 +20,11 @@ #include "solarFlux.h" #include "architecture/utilities/astroConstants.h" - /*! This method is used to reset the module. Currently no tasks are required. */ -void SolarFlux::Reset(uint64_t CurrentSimNanos) +void +SolarFlux::Reset(uint64_t CurrentSimNanos) { // check if input message has not been included if (!this->sunPositionInMsg.isLinked()) { @@ -40,7 +40,8 @@ void SolarFlux::Reset(uint64_t CurrentSimNanos) /*! Read Messages and scale the solar flux then write it out */ -void SolarFlux::UpdateState(uint64_t CurrentSimNanos) +void +SolarFlux::UpdateState(uint64_t CurrentSimNanos) { this->readMessages(); @@ -48,7 +49,7 @@ void SolarFlux::UpdateState(uint64_t CurrentSimNanos) auto r_SSc_N = this->r_SN_N - this->r_ScN_N; /*! - compute the scalar distance to the sun. The following math requires this to be in km. */ - double dist_SSc_N = r_SSc_N.norm() / 1000; // to km + double dist_SSc_N = r_SSc_N.norm() / 1000; // to km /*! - compute the local solar flux value */ this->fluxAtSpacecraft = SOLAR_FLUX_EARTH * pow(AU, 2) / pow(dist_SSc_N, 2) * this->eclipseFactor; @@ -59,7 +60,8 @@ void SolarFlux::UpdateState(uint64_t CurrentSimNanos) /*! This method is used to read messages and save values to member attributes */ -void SolarFlux::readMessages() +void +SolarFlux::readMessages() { /*! - read in planet state message (required) */ SpicePlanetStateMsgPayload sunPositionMsgData; @@ -77,13 +79,14 @@ void SolarFlux::readMessages() eclipseInMsgData = this->eclipseInMsg(); this->eclipseFactor = eclipseInMsgData.shadowFactor; } - } /*! This method is used to write the output flux message */ -void SolarFlux::writeMessages(uint64_t CurrentSimNanos) { - SolarFluxMsgPayload fluxMsgOutData = {this->fluxAtSpacecraft}; +void +SolarFlux::writeMessages(uint64_t CurrentSimNanos) +{ + SolarFluxMsgPayload fluxMsgOutData = { this->fluxAtSpacecraft }; this->solarFluxOutMsg.write(&fluxMsgOutData, this->moduleID, CurrentSimNanos); } diff --git a/src/simulation/environment/solarFlux/solarFlux.h b/src/simulation/environment/solarFlux/solarFlux.h old mode 100755 new mode 100644 index dea6490f43..1b4225e7e8 --- a/src/simulation/environment/solarFlux/solarFlux.h +++ b/src/simulation/environment/solarFlux/solarFlux.h @@ -29,30 +29,29 @@ #include "architecture/msgPayloadDefC/EclipseMsgPayload.h" #include "architecture/messaging/messaging.h" - /*! @brief solar flux class */ -class SolarFlux: public SysModel { -public: - SolarFlux(){}; - ~SolarFlux(){}; - +class SolarFlux : public SysModel +{ + public: + SolarFlux() {}; + ~SolarFlux() {}; + void Reset(uint64_t CurrentSimNanos) override; void UpdateState(uint64_t CurrentSimNanos) override; void writeMessages(uint64_t CurrentSimNanos); void readMessages(); -public: - ReadFunctor sunPositionInMsg; //!< sun state input message - ReadFunctor spacecraftStateInMsg; //!< spacecraft state input message - Message solarFluxOutMsg; //!< solar flux output message - ReadFunctor eclipseInMsg; //!< (optional) eclipse input message - - BSKLogger bskLogger; //!< -- BSK Logging + public: + ReadFunctor sunPositionInMsg; //!< sun state input message + ReadFunctor spacecraftStateInMsg; //!< spacecraft state input message + Message solarFluxOutMsg; //!< solar flux output message + ReadFunctor eclipseInMsg; //!< (optional) eclipse input message -private: - double fluxAtSpacecraft; //!< [W/m2] - double eclipseFactor = 1.0; //!< [] 1.0 is full sun, 0.0 is full eclipse - Eigen::Vector3d r_SN_N; //!< [m] sun position - Eigen::Vector3d r_ScN_N; //!< [m] s/c position + BSKLogger bskLogger; //!< -- BSK Logging + private: + double fluxAtSpacecraft; //!< [W/m2] + double eclipseFactor = 1.0; //!< [] 1.0 is full sun, 0.0 is full eclipse + Eigen::Vector3d r_SN_N; //!< [m] sun position + Eigen::Vector3d r_ScN_N; //!< [m] s/c position }; 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/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/spacecraftLocation/spacecraftLocation.cpp b/src/simulation/environment/spacecraftLocation/spacecraftLocation.cpp index c906b9f1d8..e377809890 100644 --- a/src/simulation/environment/spacecraftLocation/spacecraftLocation.cpp +++ b/src/simulation/environment/spacecraftLocation/spacecraftLocation.cpp @@ -165,7 +165,6 @@ SpacecraftLocation::ReadMessages() this->eclipseInMsgData = this->eclipseInMsg(); } - return (planetRead && scRead && sunRead && eclipseRead); } 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/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/environment/spiceInterface/spiceInterface.cpp b/src/simulation/environment/spiceInterface/spiceInterface.cpp old mode 100755 new mode 100644 index bdcff1241c..d3f2305b49 --- a/src/simulation/environment/spiceInterface/spiceInterface.cpp +++ b/src/simulation/environment/spiceInterface/spiceInterface.cpp @@ -47,11 +47,19 @@ SpiceInterface::SpiceInterface() referenceBase = "j2000"; zeroBase = "SSB"; - timeOutPicture = "MON DD,YYYY HR:MN:SC.#### (UTC) ::UTC"; + timeOutPicture = "MON DD,YYYY HR:MN:SC.#### (UTC) ::UTC"; //! - set default epoch time information char string[255]; - snprintf(string, 255, "%4d/%02d/%02d, %02d:%02d:%04.1f (UTC)", EPOCH_YEAR, EPOCH_MONTH, EPOCH_DAY, EPOCH_HOUR, EPOCH_MIN, EPOCH_SEC); + snprintf(string, + 255, + "%4d/%02d/%02d, %02d:%02d:%04.1f (UTC)", + EPOCH_YEAR, + EPOCH_MONTH, + EPOCH_DAY, + EPOCH_HOUR, + EPOCH_MIN, + EPOCH_SEC); this->UTCCalInit = string; return; @@ -61,56 +69,55 @@ SpiceInterface::SpiceInterface() that was allocated in the constructor*/ SpiceInterface::~SpiceInterface() { - for (long unsigned int c=0; cplanetStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->planetStateOutMsgs.size(); c++) { delete this->planetStateOutMsgs.at(c); } - for (long unsigned int c=0; cscStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->scStateOutMsgs.size(); c++) { delete this->scStateOutMsgs.at(c); } - for (long unsigned int c=0; cattRefStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->attRefStateOutMsgs.size(); c++) { delete this->attRefStateOutMsgs.at(c); } - for (long unsigned int c=0; ctransRefStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->transRefStateOutMsgs.size(); c++) { delete this->transRefStateOutMsgs.at(c); } - delete [] this->spiceBuffer; -// if(this->SPICELoaded) -// { -// this->clearKeeper(); -// } + delete[] this->spiceBuffer; + // if(this->SPICELoaded) + // { + // this->clearKeeper(); + // } return; } -void SpiceInterface::clearKeeper() +void +SpiceInterface::clearKeeper() { kclear_c(); } - /*! Reset the module to origina configuration values. */ -void SpiceInterface::Reset(uint64_t CurrenSimNanos) +void +SpiceInterface::Reset(uint64_t CurrenSimNanos) { //! - Bail if the SPICEDataPath is not present - if(this->SPICEDataPath == "") - { + if (this->SPICEDataPath == "") { bskLogger.bskLog(BSK_ERROR, "SPICE data path was not set. No SPICE."); return; } //!- Load the SPICE kernels if they haven't already been loaded - if(!this->SPICELoaded) - { - if(loadSpiceKernel((char *)"naif0012.tls", this->SPICEDataPath.c_str())) { + if (!this->SPICELoaded) { + if (loadSpiceKernel((char*)"naif0012.tls", this->SPICEDataPath.c_str())) { bskLogger.bskLog(BSK_ERROR, "Unable to load %s", "naif0012.tls"); } - if(loadSpiceKernel((char *)"pck00010.tpc", this->SPICEDataPath.c_str())) { + if (loadSpiceKernel((char*)"pck00010.tpc", this->SPICEDataPath.c_str())) { bskLogger.bskLog(BSK_ERROR, "Unable to load %s", "pck00010.tpc"); } - if(loadSpiceKernel((char *)"de-403-masses.tpc", this->SPICEDataPath.c_str())) { + if (loadSpiceKernel((char*)"de-403-masses.tpc", this->SPICEDataPath.c_str())) { bskLogger.bskLog(BSK_ERROR, "Unable to load %s", "de-403-masses.tpc"); } - if(loadSpiceKernel((char *)"de430.bsp", this->SPICEDataPath.c_str())) { + if (loadSpiceKernel((char*)"de430.bsp", this->SPICEDataPath.c_str())) { bskLogger.bskLog(BSK_ERROR, "Unable to load %s", "de430.tpc"); } this->SPICELoaded = true; @@ -123,18 +130,17 @@ void SpiceInterface::Reset(uint64_t CurrenSimNanos) std::vector::iterator planit; size_t c = 0; // celestial object counter - int autoFrame; // flag to set the frame automatically - SpiceChar *name = new SpiceChar[this->charBufferSize]; + int autoFrame; // flag to set the frame automatically + SpiceChar* name = new SpiceChar[this->charBufferSize]; SpiceBoolean frmFound; SpiceInt frmCode; - for(planit = this->planetData.begin(); planit != planetData.end(); planit++) - { + for (planit = this->planetData.begin(); planit != planetData.end(); planit++) { autoFrame = 1; - planit->computeOrient = 0; // turn off by default + planit->computeOrient = 0; // turn off by default if (this->planetFrames.size() > 0) { if (c < this->planetFrames.size()) { if (this->planetFrames[c].length() > 0) { - planit->computeOrient = 1; // turn on as a custom name is provided + planit->computeOrient = 1; // turn on as a custom name is provided autoFrame = 0; } } @@ -142,23 +148,23 @@ void SpiceInterface::Reset(uint64_t CurrenSimNanos) if (autoFrame > 0) { std::string planetFrame = planit->PlanetName; cnmfrm_c(planetFrame.c_str(), this->charBufferSize, &frmCode, name, &frmFound); - planit->computeOrient = frmFound; // set the flag to the Spice response on finding this frame + planit->computeOrient = frmFound; // set the flag to the Spice response on finding this frame } c++; } - delete [] name; + delete[] name; // - Call Update state so that the spice bodies are inputted into the messaging system on reset this->UpdateState(CurrenSimNanos); } - /*! This method is used to initialize the zero-time that will be used to calculate all system time values in the Update method. It also creates the output message for time data */ -void SpiceInterface::initTimeData() +void +SpiceInterface::initTimeData() { double EpochDelteET; @@ -168,11 +174,21 @@ void SpiceInterface::initTimeData() EpochMsgPayload epochMsg; epochMsg = this->epochInMsg(); if (!this->epochInMsg.isWritten()) { - bskLogger.bskLog(BSK_ERROR, "The input epoch message name was set, but the message was never written. Not using the input message."); + bskLogger.bskLog( + BSK_ERROR, + "The input epoch message name was set, but the message was never written. Not using the input message."); } else { // Set the epoch information from the input message char string[255]; - snprintf(string, 255, "%4d/%02d/%02d, %02d:%02d:%04.6f (UTC)", epochMsg.year, epochMsg.month, epochMsg.day, epochMsg.hours, epochMsg.minutes, epochMsg.seconds); + snprintf(string, + 255, + "%4d/%02d/%02d, %02d:%02d:%04.6f (UTC)", + epochMsg.year, + epochMsg.month, + epochMsg.day, + epochMsg.hours, + epochMsg.minutes, + epochMsg.seconds); this->UTCCalInit = string; } } @@ -183,7 +199,6 @@ void SpiceInterface::initTimeData() str2et_c(this->UTCCalInit.c_str(), &this->J2000ETInit); //! - Take the JD epoch and get the elapsed time for it deltet_c(this->JDGPSEpoch, "ET", &EpochDelteET); - } /*! This method computes the GPS time data for the current elapsed time. It uses @@ -191,20 +206,21 @@ void SpiceInterface::initTimeData() compute the GPS time (week, seconds, rollovers) */ -void SpiceInterface::computeGPSData() +void +SpiceInterface::computeGPSData() { double JDDifference; //! - The difference between the epochs in julian date terms is the total JDDifference = this->J2000Current - this->JDGPSEpoch; //! - Scale the elapsed by a week's worth of seconds to get week - this->GPSWeek = (uint16_t) (JDDifference/(7*86400)); + this->GPSWeek = (uint16_t)(JDDifference / (7 * 86400)); //! - Subtract out the GPS week scaled up to seconds to get time in week - this->GPSSeconds = JDDifference - this->GPSWeek*7*86400; + this->GPSSeconds = JDDifference - this->GPSWeek * 7 * 86400; //! - Maximum GPS week is 1024 so get rollovers and subtract out those weeks - this->GPSRollovers = this->GPSWeek/1024; - this->GPSWeek = (uint16_t)(this->GPSWeek-this->GPSRollovers*1024); + this->GPSRollovers = this->GPSWeek / 1024; + this->GPSWeek = (uint16_t)(this->GPSWeek - this->GPSRollovers * 1024); } /*! This method takes the values computed in the model and outputs them. @@ -213,7 +229,8 @@ void SpiceInterface::computeGPSData() @param CurrentClock The current simulation time (used for time stamping) */ -void SpiceInterface::writeOutputMessages(uint64_t CurrentClock) +void +SpiceInterface::writeOutputMessages(uint64_t CurrentClock) { SpiceTimeMsgPayload OutputData; @@ -226,14 +243,12 @@ void SpiceInterface::writeOutputMessages(uint64_t CurrentClock) this->spiceTimeOutMsg.write(&OutputData, this->moduleID, CurrentClock); //! - Iterate through all of the planets that are on and write their outputs - for (long unsigned int c=0; cplanetStateOutMsgs.size(); c++) - { + for (long unsigned int c = 0; c < this->planetStateOutMsgs.size(); c++) { this->planetStateOutMsgs[c]->write(&this->planetData[c], this->moduleID, CurrentClock); } //! - Iterate through all of the spacecraft that are on and write their outputs - for (long unsigned int c=0; cscStateOutMsgs.size(); c++) - { + for (long unsigned int c = 0; c < this->scStateOutMsgs.size(); c++) { SCStatesMsgPayload scStateMsgData = {}; v3Copy(this->scData[c].PositionVector, scStateMsgData.r_BN_N); v3Copy(this->scData[c].PositionVector, scStateMsgData.r_CN_N); @@ -250,7 +265,6 @@ void SpiceInterface::writeOutputMessages(uint64_t CurrentClock) v3Copy(this->scData[c].PositionVector, transRefMsgData.r_RN_N); v3Copy(this->scData[c].VelocityVector, transRefMsgData.v_RN_N); this->transRefStateOutMsgs[c]->write(&transRefMsgData, this->moduleID, CurrentClock); - } } @@ -260,15 +274,15 @@ void SpiceInterface::writeOutputMessages(uint64_t CurrentClock) @param CurrentSimNanos The current clock time for the simulation */ -void SpiceInterface::UpdateState(uint64_t CurrentSimNanos) +void +SpiceInterface::UpdateState(uint64_t CurrentSimNanos) { //! - Increment the J2000 elapsed time based on init value and Current sim - this->J2000Current = this->J2000ETInit + CurrentSimNanos*NANO2SEC; + this->J2000Current = this->J2000ETInit + CurrentSimNanos * NANO2SEC; //! - Compute the current Julian Date string and cast it over to the double - et2utc_c(this->J2000Current, "J", 14, this->charBufferSize - 1, reinterpret_cast - (this->spiceBuffer)); - std::string localString = reinterpret_cast (&this->spiceBuffer[3]); + et2utc_c(this->J2000Current, "J", 14, this->charBufferSize - 1, reinterpret_cast(this->spiceBuffer)); + std::string localString = reinterpret_cast(&this->spiceBuffer[3]); this->julianDateCurrent = std::stod(localString); //! Get GPS and Planet data and then write the message outputs this->computeGPSData(); @@ -279,26 +293,29 @@ void SpiceInterface::UpdateState(uint64_t CurrentSimNanos) /*! take a vector of planet name strings and create the vector of planet state output messages and the vector of planet state message payloads */ -void SpiceInterface::addPlanetNames(std::vector planetNames) { +void +SpiceInterface::addPlanetNames(std::vector planetNames) +{ std::vector::iterator it; /* clear the planet state message and payload vectors */ - for (long unsigned int c=0; cplanetStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->planetStateOutMsgs.size(); c++) { delete this->planetStateOutMsgs.at(c); } this->planetStateOutMsgs.clear(); this->planetData.clear(); for (it = planetNames.begin(); it != planetNames.end(); it++) { - Message *spiceOutMsg; + Message* spiceOutMsg; spiceOutMsg = new Message; this->planetStateOutMsgs.push_back(spiceOutMsg); SpicePlanetStateMsgPayload newPlanet = {}; m33SetIdentity(newPlanet.J20002Pfix); - if(it->size() >= MAX_BODY_NAME_LENGTH) - { - bskLogger.bskLog(BSK_WARNING, "spiceInterface: Warning, your planet name is too long for me. Ignoring: %s", (*it).c_str()); + if (it->size() >= MAX_BODY_NAME_LENGTH) { + bskLogger.bskLog(BSK_WARNING, + "spiceInterface: Warning, your planet name is too long for me. Ignoring: %s", + (*it).c_str()); continue; } strcpy(newPlanet.PlanetName, it->c_str()); @@ -311,20 +328,22 @@ void SpiceInterface::addPlanetNames(std::vector planetNames) { /*! take a vector of spacecraft name strings and create the vectors of spacecraft state output messages and the vector of spacecraft state message payloads */ -void SpiceInterface::addSpacecraftNames(std::vector spacecraftNames) { +void +SpiceInterface::addSpacecraftNames(std::vector spacecraftNames) +{ std::vector::iterator it; - SpiceChar *name = new SpiceChar[this->charBufferSize]; + SpiceChar* name = new SpiceChar[this->charBufferSize]; SpiceBoolean frmFound; SpiceInt frmCode; /* clear the spacecraft state message and payload vectors */ - for (long unsigned int c=0; cscStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->scStateOutMsgs.size(); c++) { delete this->scStateOutMsgs.at(c); } - for (long unsigned int c=0; cattRefStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->attRefStateOutMsgs.size(); c++) { delete this->attRefStateOutMsgs.at(c); } - for (long unsigned int c=0; ctransRefStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->transRefStateOutMsgs.size(); c++) { delete this->transRefStateOutMsgs.at(c); } this->scStateOutMsgs.clear(); @@ -334,23 +353,24 @@ void SpiceInterface::addSpacecraftNames(std::vector spacecraftNames for (it = spacecraftNames.begin(); it != spacecraftNames.end(); it++) { /* append to spacecraft related output messages */ - Message *scStateOutMsg; + Message* scStateOutMsg; scStateOutMsg = new Message; this->scStateOutMsgs.push_back(scStateOutMsg); - Message *attRefOutMsg; + Message* attRefOutMsg; attRefOutMsg = new Message; this->attRefStateOutMsgs.push_back(attRefOutMsg); - Message *transRefOutMsg; + Message* transRefOutMsg; transRefOutMsg = new Message; this->transRefStateOutMsgs.push_back(transRefOutMsg); SpicePlanetStateMsgPayload newSpacecraft = {}; m33SetIdentity(newSpacecraft.J20002Pfix); - if(it->size() >= MAX_BODY_NAME_LENGTH) - { - bskLogger.bskLog(BSK_WARNING, "spiceInterface: Warning, your spacecraft name is too long for me. Ignoring: %s", (*it).c_str()); + if (it->size() >= MAX_BODY_NAME_LENGTH) { + bskLogger.bskLog(BSK_WARNING, + "spiceInterface: Warning, your spacecraft name is too long for me. Ignoring: %s", + (*it).c_str()); continue; } strcpy(newSpacecraft.PlanetName, it->c_str()); @@ -360,17 +380,17 @@ void SpiceInterface::addSpacecraftNames(std::vector spacecraftNames newSpacecraft.computeOrient = frmFound; this->scData.push_back(newSpacecraft); } - delete [] name; + delete[] name; return; } - /*! This method gets the state of each spice item that has been added to the module and saves the information off into the array. */ -void SpiceInterface::pullSpiceData(std::vector *spiceData) +void +SpiceInterface::pullSpiceData(std::vector* spiceData) { std::vector::iterator planit; @@ -382,14 +402,18 @@ void SpiceInterface::pullSpiceData(std::vector *spic -# Time stamp the message appropriately */ size_t c = 0; // celestial body counter - for(planit = spiceData->begin(); planit != spiceData->end(); planit++) - { + for (planit = spiceData->begin(); planit != spiceData->end(); planit++) { double lighttime; double localState[6]; std::string planetFrame = ""; - spkezr_c(planit->PlanetName, this->J2000Current, this->referenceBase.c_str(), - "NONE", this->zeroBase.c_str(), localState, &lighttime); + spkezr_c(planit->PlanetName, + this->J2000Current, + this->referenceBase.c_str(), + "NONE", + this->zeroBase.c_str(), + localState, + &lighttime); v3Copy(&localState[0], planit->PositionVector); v3Copy(&localState[3], planit->VelocityVector); v3Scale(1000., planit->PositionVector, planit->PositionVector); @@ -402,21 +426,24 @@ void SpiceInterface::pullSpiceData(std::vector *spic /* use specific planet frame if specified */ if (this->planetFrames.size() > 0) { if (c < this->planetFrames.size()) { - if (this->planetFrames[c].length() > 0){ + if (this->planetFrames[c].length() > 0) { /* use custom planet frame name */ planetFrame = this->planetFrames[c]; } } } - if(planit->computeOrient) - { - //pxform_c ( referenceBase.c_str(), planetFrame.c_str(), J2000Current, - // planit->second.J20002Pfix); + if (planit->computeOrient) { + // pxform_c ( referenceBase.c_str(), planetFrame.c_str(), J2000Current, + // planit->second.J20002Pfix); double aux[6][6]; - sxform_c(this->referenceBase.c_str(), planetFrame.c_str(), this->J2000Current, aux); //returns attitude of planet (i.e. IAU_EARTH) wrt "j2000". note j2000 is actually ICRF in Spice. + sxform_c( + this->referenceBase.c_str(), + planetFrame.c_str(), + this->J2000Current, + aux); // returns attitude of planet (i.e. IAU_EARTH) wrt "j2000". note j2000 is actually ICRF in Spice. m66Get33Matrix(0, 0, aux, planit->J20002Pfix); @@ -434,10 +461,11 @@ void SpiceInterface::pullSpiceData(std::vector *spic @param kernelName The name of the kernel we are loading @param dataPath The path to the data area on the filesystem */ -int SpiceInterface::loadSpiceKernel(char *kernelName, const char *dataPath) +int +SpiceInterface::loadSpiceKernel(char* kernelName, const char* dataPath) { - char *fileName = new char[this->charBufferSize]; - SpiceChar *name = new SpiceChar[this->charBufferSize]; + char* fileName = new char[this->charBufferSize]; + SpiceChar* name = new SpiceChar[this->charBufferSize]; //! - The required calls come from the SPICE documentation. //! - The most critical call is furnsh_c @@ -452,7 +480,7 @@ int SpiceInterface::loadSpiceKernel(char *kernelName, const char *dataPath) erract_c("SET", this->charBufferSize, name); delete[] fileName; delete[] name; - if(failed_c()) { + if (failed_c()) { return 1; } return 0; @@ -466,10 +494,11 @@ int SpiceInterface::loadSpiceKernel(char *kernelName, const char *dataPath) @param kernelName The name of the kernel we are unloading @param dataPath The path to the data area on the filesystem */ -int SpiceInterface::unloadSpiceKernel(char *kernelName, const char *dataPath) +int +SpiceInterface::unloadSpiceKernel(char* kernelName, const char* dataPath) { - char *fileName = new char[this->charBufferSize]; - SpiceChar *name = new SpiceChar[this->charBufferSize]; + char* fileName = new char[this->charBufferSize]; + SpiceChar* name = new SpiceChar[this->charBufferSize]; //! - The required calls come from the SPICE documentation. //! - The most critical call is furnsh_c @@ -480,29 +509,31 @@ int SpiceInterface::unloadSpiceKernel(char *kernelName, const char *dataPath) unload_c(fileName); delete[] fileName; delete[] name; - if(failed_c()) { + if (failed_c()) { return 1; } return 0; } -std::string SpiceInterface::getCurrentTimeString() +std::string +SpiceInterface::getCurrentTimeString() { - char *spiceOutputBuffer; - int64_t allowedOutputLength; - - allowedOutputLength = (int64_t)this->timeOutPicture.size() - 5; - - if (allowedOutputLength < 0) - { - bskLogger.bskLog(BSK_ERROR, "The output format string is not long enough. It should be much larger than 5 characters. It is currently: %s", this->timeOutPicture.c_str()); - return(""); - } - - spiceOutputBuffer = new char[allowedOutputLength]; - timout_c(this->J2000Current, this->timeOutPicture.c_str(), (SpiceInt) allowedOutputLength, - spiceOutputBuffer); - std::string returnTimeString = spiceOutputBuffer; - delete[] spiceOutputBuffer; - return(returnTimeString); + char* spiceOutputBuffer; + int64_t allowedOutputLength; + + allowedOutputLength = (int64_t)this->timeOutPicture.size() - 5; + + if (allowedOutputLength < 0) { + bskLogger.bskLog(BSK_ERROR, + "The output format string is not long enough. It should be much larger than 5 characters. It " + "is currently: %s", + this->timeOutPicture.c_str()); + return (""); + } + + spiceOutputBuffer = new char[allowedOutputLength]; + timout_c(this->J2000Current, this->timeOutPicture.c_str(), (SpiceInt)allowedOutputLength, spiceOutputBuffer); + std::string returnTimeString = spiceOutputBuffer; + delete[] spiceOutputBuffer; + return (returnTimeString); } diff --git a/src/simulation/environment/spiceInterface/spiceInterface.h b/src/simulation/environment/spiceInterface/spiceInterface.h old mode 100755 new mode 100644 index e162ae1f6b..18fd8e2d98 --- a/src/simulation/environment/spiceInterface/spiceInterface.h +++ b/src/simulation/environment/spiceInterface/spiceInterface.h @@ -36,61 +36,63 @@ #include "architecture/messaging/messaging.h" /*! @brief spice interface class */ -class SpiceInterface: public SysModel { -public: +class SpiceInterface : public SysModel +{ + public: SpiceInterface(); ~SpiceInterface(); void UpdateState(uint64_t CurrentSimNanos); - int loadSpiceKernel(char *kernelName, const char *dataPath); - int unloadSpiceKernel(char *kernelName, const char *dataPath); - std::string getCurrentTimeString(); //!< class method + int loadSpiceKernel(char* kernelName, const char* dataPath); + int unloadSpiceKernel(char* kernelName, const char* dataPath); + std::string getCurrentTimeString(); //!< class method void Reset(uint64_t CurrentSimNanos); void initTimeData(); void computeGPSData(); - void pullSpiceData(std::vector *spiceData); + void pullSpiceData(std::vector* spiceData); void writeOutputMessages(uint64_t CurrentClock); - void clearKeeper(); //!< class method + void clearKeeper(); //!< class method void addPlanetNames(std::vector planetNames); void addSpacecraftNames(std::vector spacecraftNames); -public: - Message spiceTimeOutMsg; //!< spice time sampling output message - ReadFunctor epochInMsg; //!< (optional) input epoch message + public: + Message spiceTimeOutMsg; //!< spice time sampling output message + ReadFunctor epochInMsg; //!< (optional) input epoch message std::vector*> planetStateOutMsgs; //!< vector of planet state output messages std::vector*> scStateOutMsgs; //!< vector of spacecraft state output messages - std::vector*> attRefStateOutMsgs; //!< vector of spacecraft attitude reference state output messages - std::vector*> transRefStateOutMsgs; //!< vector of spacecraft translational reference state output messages - - std::string SPICEDataPath; //!< -- Path on file to SPICE data - std::string referenceBase; //!< -- Base reference frame to use - std::string zeroBase; //!< -- Base zero point to use for states - std::string timeOutPicture; //!< -- Optional parameter used to extract time strings - bool SPICELoaded; //!< -- Boolean indicating to reload spice + std::vector*> + attRefStateOutMsgs; //!< vector of spacecraft attitude reference state output messages + std::vector*> + transRefStateOutMsgs; //!< vector of spacecraft translational reference state output messages + + std::string SPICEDataPath; //!< -- Path on file to SPICE data + std::string referenceBase; //!< -- Base reference frame to use + std::string zeroBase; //!< -- Base zero point to use for states + std::string timeOutPicture; //!< -- Optional parameter used to extract time strings + bool SPICELoaded; //!< -- Boolean indicating to reload spice int charBufferSize; //!< -- avert your eyes we're getting SPICE - uint8_t *spiceBuffer; //!< -- General buffer to pass down to spice + uint8_t* spiceBuffer; //!< -- General buffer to pass down to spice std::string UTCCalInit; //!< -- UTC time string for init time - std::vectorplanetFrames; //!< -- Optional vector of planet frame names. Default values are IAU_ + planet name + std::vector + planetFrames; //!< -- Optional vector of planet frame names. Default values are IAU_ + planet name - bool timeDataInit; //!< -- Flag indicating whether time has been init - double J2000ETInit; //!< s Seconds elapsed since J2000 at init - double J2000Current; //!< s Current J2000 elapsed time - double julianDateCurrent; //!< s Current JulianDate - double GPSSeconds; //!< s Current GPS seconds - uint16_t GPSWeek; //!< -- Current GPS week value - uint64_t GPSRollovers; //!< -- Count on the number of GPS rollovers + bool timeDataInit; //!< -- Flag indicating whether time has been init + double J2000ETInit; //!< s Seconds elapsed since J2000 at init + double J2000Current; //!< s Current J2000 elapsed time + double julianDateCurrent; //!< s Current JulianDate + double GPSSeconds; //!< s Current GPS seconds + uint16_t GPSWeek; //!< -- Current GPS week value + uint64_t GPSRollovers; //!< -- Count on the number of GPS rollovers - BSKLogger bskLogger; //!< -- BSK Logging + BSKLogger bskLogger; //!< -- BSK Logging -private: - std::string GPSEpochTime; //!< -- String for the GPS epoch - double JDGPSEpoch; //!< s Epoch for GPS time. Saved for efficiency + private: + std::string GPSEpochTime; //!< -- String for the GPS epoch + double JDGPSEpoch; //!< s Epoch for GPS time. Saved for efficiency std::vector planetData; std::vector scData; - }; - #endif diff --git a/src/simulation/mujocoDynamics/MJSystemCoM/MJSystemCoM.cpp b/src/simulation/mujocoDynamics/MJSystemCoM/MJSystemCoM.cpp index c37e49fe64..26b9769349 100644 --- a/src/simulation/mujocoDynamics/MJSystemCoM/MJSystemCoM.cpp +++ b/src/simulation/mujocoDynamics/MJSystemCoM/MJSystemCoM.cpp @@ -17,7 +17,6 @@ */ - #include "simulation/mujocoDynamics/MJSystemCoM/MJSystemCoM.h" #include #include @@ -25,36 +24,36 @@ /*! Initialize C-wrapped output messages */ void -MJSystemCoM::SelfInit(){ +MJSystemCoM::SelfInit() +{ SCStatesMsg_C_init(&this->comStatesOutMsgC); } /*! This is the constructor for the module class. It sets default variable values and initializes the various parts of the model */ -MJSystemCoM::MJSystemCoM() -{ -} +MJSystemCoM::MJSystemCoM() {} /*! This method is used to reset the module and checks that required input messages are connect. -*/ -void MJSystemCoM::Reset(uint64_t CurrentSimNanos) + */ +void +MJSystemCoM::Reset(uint64_t CurrentSimNanos) { if (!scene) { bskLogger.bskLog(BSK_ERROR, "MJSystemCoM: scene pointer not set!"); } } - /*! This extracts the total spacecraft CoM position and velocity -*/ -void MJSystemCoM::UpdateState(uint64_t CurrentSimNanos) + */ +void +MJSystemCoM::UpdateState(uint64_t CurrentSimNanos) { const auto model = scene->getMujocoModel(); const auto data = scene->getMujocoData(); double massSC = 0.0; Eigen::Vector3d r_CN_N; Eigen::Vector3d v_CN_N; - SCStatesMsgPayload payload; //!< local copy of message buffer + SCStatesMsgPayload payload; //!< local copy of message buffer // always zero the output message buffers before assigning values payload = this->comStatesOutMsg.zeroMsgPayload; @@ -64,10 +63,11 @@ void MJSystemCoM::UpdateState(uint64_t CurrentSimNanos) // calculate CoM position for (int i = 1; i < model->nbody; i++) { const double mi = model->body_mass[i]; - if (mi <= 0.0) continue; + if (mi <= 0.0) + continue; // Body COM position in world: xipos[3*i : 3*i+3] - const Eigen::Vector3d ri = Eigen::Map(data->xipos + 3*i); + const Eigen::Vector3d ri = Eigen::Map(data->xipos + 3 * i); massSC += mi; r_CN_N += mi * ri; @@ -76,8 +76,8 @@ void MJSystemCoM::UpdateState(uint64_t CurrentSimNanos) // calculate CoM velocity if (model->nv > 0 && massSC > 0.0) { // jacp = linear 3×nv, jacr = angular 3×nv - std::vector jacp(3*model->nv, 0.0); - std::vector jacr(3*model->nv, 0.0); + std::vector jacp(3 * model->nv, 0.0); + std::vector jacr(3 * model->nv, 0.0); Eigen::Map qv(data->qvel, model->nv); @@ -85,7 +85,8 @@ void MJSystemCoM::UpdateState(uint64_t CurrentSimNanos) for (int i = 1; i < model->nbody; ++i) { const double mi = model->body_mass[i]; - if (mi <= 0.0) continue; + if (mi <= 0.0) + continue; // Fill jacobians at the BODY COM // (MuJoCo API: mj_jacBodyCom(m, d, jacp, jacr, body_id)) @@ -94,10 +95,9 @@ void MJSystemCoM::UpdateState(uint64_t CurrentSimNanos) mj_jacBodyCom(model, data, jacp.data(), jacr.data(), i); // vi = jacp * qvel (jacp is row-major 3×nv) - Eigen::Map> - Jp(jacp.data(), 3, model->nv); + Eigen::Map> Jp(jacp.data(), 3, model->nv); - const Eigen::Vector3d vi = Jp * qv; // linear COM velocity of body i + const Eigen::Vector3d vi = Jp * qv; // linear COM velocity of body i v_CN_N += mi * vi; } diff --git a/src/simulation/mujocoDynamics/MJSystemCoM/MJSystemCoM.h b/src/simulation/mujocoDynamics/MJSystemCoM/MJSystemCoM.h index b32334821c..f59abfc03f 100644 --- a/src/simulation/mujocoDynamics/MJSystemCoM/MJSystemCoM.h +++ b/src/simulation/mujocoDynamics/MJSystemCoM/MJSystemCoM.h @@ -17,7 +17,6 @@ */ - #ifndef MJSYSTEMCOM_H #define MJSYSTEMCOM_H @@ -29,11 +28,11 @@ #include "architecture/utilities/avsEigenSupport.h" #include - /*! @brief This is a C++ module to extract the system CoM position and velocity from Mujoco */ -class MJSystemCoM: public SysModel { -public: +class MJSystemCoM : public SysModel +{ + public: MJSystemCoM(); ~MJSystemCoM() = default; @@ -41,17 +40,13 @@ class MJSystemCoM: public SysModel { void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); -public: - - MJScene* scene{nullptr}; //!< pointer to the MuJoCo scene - - Message comStatesOutMsg; //!< spacecraft CoM states C++ output msg - SCStatesMsg_C comStatesOutMsgC = {}; //!< spacecraft CoM states C output msg - - BSKLogger bskLogger; //!< BSK Logging + public: + MJScene* scene{ nullptr }; //!< pointer to the MuJoCo scene + Message comStatesOutMsg; //!< spacecraft CoM states C++ output msg + SCStatesMsg_C comStatesOutMsgC = {}; //!< spacecraft CoM states C output msg + BSKLogger bskLogger; //!< BSK Logging }; - #endif 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/NBodyGravity.cpp b/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp index 46f7a9604d..fc9ca2ff21 100644 --- a/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp +++ b/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp @@ -25,39 +25,37 @@ void NBodyGravity::Reset(uint64_t CurrentSimNanos) { std::string errorPreffix = "In NBodyGravity '" + ModelTag + "': "; - for (auto&& [name, target] : targets) - { - if (!target.centerOfMassStateInMsg.isLinked()) - { - MJBasilisk::detail::logAndThrow(errorPreffix + "Target '" + name + "' centerOfMassStateInMsg is not linked!", &bskLogger); + for (auto&& [name, target] : targets) { + if (!target.centerOfMassStateInMsg.isLinked()) { + MJBasilisk::detail::logAndThrow( + errorPreffix + "Target '" + name + "' centerOfMassStateInMsg is not linked!", &bskLogger); } - if (!target.massPropertiesInMsg.isLinked()) - { - MJBasilisk::detail::logAndThrow(errorPreffix + "Target '" + name + "' massPropertiesInMsg is not linked!", &bskLogger); + if (!target.massPropertiesInMsg.isLinked()) { + MJBasilisk::detail::logAndThrow( + errorPreffix + "Target '" + name + "' massPropertiesInMsg is not linked!", &bskLogger); } } bool foundCentralSource = false; - for (auto&& [name, source] : sources) - { - if (sources.size() > 1 && !source.stateInMsg.isLinked()) - { - MJBasilisk::detail::logAndThrow(errorPreffix + "Source '" + name + "' stateInMsg is not linked but there are more than one sources!", &bskLogger); + for (auto&& [name, source] : sources) { + if (sources.size() > 1 && !source.stateInMsg.isLinked()) { + MJBasilisk::detail::logAndThrow( + errorPreffix + "Source '" + name + "' stateInMsg is not linked but there are more than one sources!", + &bskLogger); } auto error = source.model->initializeParameters(); - if (error) - { - MJBasilisk::detail::logAndThrow(errorPreffix + "While initializing source '" + name + "' gravity model, " + *error, &bskLogger); + if (error) { + MJBasilisk::detail::logAndThrow( + errorPreffix + "While initializing source '" + name + "' gravity model, " + *error, &bskLogger); } - if (source.isCentralBody) - { - if (foundCentralSource) - { - MJBasilisk::detail::logAndThrow(errorPreffix + "More than one central body!", &bskLogger); + if (source.isCentralBody) { + if (foundCentralSource) { + MJBasilisk::detail::logAndThrow(errorPreffix + "More than one central body!", + &bskLogger); } foundCentralSource = true; } @@ -67,10 +65,9 @@ NBodyGravity::Reset(uint64_t CurrentSimNanos) void NBodyGravity::UpdateState(uint64_t CurrentSimNanos) { - for (auto&& [_, target] : targets) - { + for (auto&& [_, target] : targets) { auto targetStatePayload = target.centerOfMassStateInMsg(); - Eigen::Matrix3d dcm_BN = std::invoke([&](){ + Eigen::Matrix3d dcm_BN = std::invoke([&]() { double dcm_BN[3][3]; MRP2C(targetStatePayload.sigma_BN, dcm_BN); return cArray2EigenMatrix3d(*dcm_BN); @@ -91,10 +88,9 @@ NBodyGravity::UpdateState(uint64_t CurrentSimNanos) GravitySource& NBodyGravity::addGravitySource(std::string name, std::shared_ptr gravityModel, bool isCentralBody) { - auto[source, actuallyEmplaced] = this->sources.emplace(name, GravitySource{gravityModel, {}, isCentralBody}); + auto [source, actuallyEmplaced] = this->sources.emplace(name, GravitySource{ gravityModel, {}, isCentralBody }); - if (!actuallyEmplaced) - { + if (!actuallyEmplaced) { MJBasilisk::detail::logAndThrow("Cannot use repeated source name " + name, &bskLogger); } @@ -108,14 +104,10 @@ NBodyGravity::addGravityTarget(std::string name) // GravityTarget. This avoids copying the `Message<...>` in GravityTarget, // which proved to cause weird bugs (multiple messages pointing to the same // address in memory for their payload). - auto[target, actuallyEmplaced] = this->targets.emplace( - std::piecewise_construct, - std::forward_as_tuple(name), - std::forward_as_tuple() - ); - - if (!actuallyEmplaced) - { + auto [target, actuallyEmplaced] = + this->targets.emplace(std::piecewise_construct, std::forward_as_tuple(name), std::forward_as_tuple()); + + if (!actuallyEmplaced) { MJBasilisk::detail::logAndThrow("Cannot use repeated target name " + name, &bskLogger); } @@ -128,13 +120,13 @@ NBodyGravity::addGravityTarget(std::string name, MJSite& site) auto& target = addGravityTarget(name); /* Tie the state outMsg of the site to the inMsg of the gravity target */ - target.centerOfMassStateInMsg.subscribeTo( &site.stateOutMsg ); + target.centerOfMassStateInMsg.subscribeTo(&site.stateOutMsg); /* Create a new actuator on the site, then tie the force outMsg of the gravity target to the actuator force inMsg */ std::string actuatorName = ModelTag + "_gravity_target_at_" + name; auto& actuator = site.getBody().getSpec().getScene().addForceActuator(actuatorName, site.getName()); - actuator.forceInMsg.subscribeTo( &target.massFixedForceOutMsg ); + actuator.forceInMsg.subscribeTo(&target.massFixedForceOutMsg); return target; } @@ -142,7 +134,7 @@ GravityTarget& NBodyGravity::addGravityTarget(std::string name, MJBody& body) { auto& target = addGravityTarget(std::move(name), body.getCenterOfMass()); - target.massPropertiesInMsg.subscribeTo( &body.massPropertiesOutMsg ); + target.massPropertiesInMsg.subscribeTo(&body.massPropertiesOutMsg); return target; } @@ -151,9 +143,8 @@ NBodyGravity::computeAccelerationFromSource(GravitySource& source, Eigen::Vector { // Orientation and positon of the gravity source in J2000 Eigen::Matrix3d dcm_sourceFixedJ2000 = Eigen::Matrix3d::Identity(); - Eigen::Vector3d r_sourceJ200= Eigen::Vector3d::Zero(); - if (source.stateInMsg.isLinked()) - { + Eigen::Vector3d r_sourceJ200 = Eigen::Vector3d::Zero(); + if (source.stateInMsg.isLinked()) { auto spicePayload = source.stateInMsg(); dcm_sourceFixedJ2000 = c2DArray2EigenMatrix3d(spicePayload.J20002Pfix); // .transpose() r_sourceJ200 = cArray2EigenVector3d(spicePayload.PositionVector); @@ -198,13 +189,14 @@ NBodyGravity::computeAccelerationOnTarget(GravityTarget& target) Eigen::Vector3d r_centralSourceSpiceRef_N = Eigen::Vector3d::Zero(); bool centralSourceExists = false; - for (auto&& [_, source] : sources) - { - if (!source.isCentralBody) continue; + for (auto&& [_, source] : sources) { + if (!source.isCentralBody) + continue; centralSourceExists = true; - if (!source.stateInMsg.isLinked()) break; + if (!source.stateInMsg.isLinked()) + break; auto spicePayload = source.stateInMsg(); r_centralSourceSpiceRef_N = cArray2EigenVector3d(spicePayload.PositionVector); @@ -221,15 +213,13 @@ NBodyGravity::computeAccelerationOnTarget(GravityTarget& target) // Total acceleration is the sum of accelerations produced by each source // But also taking into account the relative acceleration of the central body - for (auto&& [_, source] : sources) - { + for (auto&& [_, source] : sources) { grav_N += computeAccelerationFromSource(source, r_targetSpiceRef_N); // If there is a central body, and 'source' is not this one, // then we need to take into account the acceleration of the central // body due to the effect of this other source. - if (centralSourceExists && !source.isCentralBody) - { + if (centralSourceExists && !source.isCentralBody) { grav_N -= computeAccelerationFromSource(source, r_centralSourceSpiceRef_N); } } diff --git a/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.h b/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.h index da7741dc8f..ce53dc868b 100644 --- a/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.h +++ b/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.h @@ -41,9 +41,9 @@ */ struct GravitySource { - std::shared_ptr model; ///< The gravity model associated with the source. + std::shared_ptr model; ///< The gravity model associated with the source. ReadFunctor stateInMsg; ///< Input message providing the state of the source. - bool isCentralBody; ///< Flag indicating whether this source is the central body. + bool isCentralBody; ///< Flag indicating whether this source is the central body. }; /** @@ -55,7 +55,7 @@ struct GravityTarget { ReadFunctor massPropertiesInMsg; ///< Input message for the target's mass properties. ReadFunctor centerOfMassStateInMsg; ///< Input message for the target's state. - Message massFixedForceOutMsg; ///< Output message for the computed force. + Message massFixedForceOutMsg; ///< Output message for the computed force. }; /** @@ -67,7 +67,7 @@ struct GravityTarget */ class NBodyGravity : public SysModel { -public: + public: /** * @brief Resets the N-body gravity simulation. * @@ -179,10 +179,10 @@ class NBodyGravity : public SysModel */ Eigen::Vector3d computeAccelerationOnTarget(GravityTarget& target); -public: + public: BSKLogger bskLogger; ///< BSK Logging -protected: + protected: // NOTE: choosing map over unordered_map so that computations // are run in a consistent order 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/JointPIDController.h b/src/simulation/mujocoDynamics/PIDControllers/JointPIDController.h index dd18499a82..9844b965bf 100644 --- a/src/simulation/mujocoDynamics/PIDControllers/JointPIDController.h +++ b/src/simulation/mujocoDynamics/PIDControllers/JointPIDController.h @@ -29,38 +29,30 @@ * * Implements the required virtual methods for reading joint state and writing actuator input. */ -class JointPIDController : public PIDController +class JointPIDController + : public PIDController { -protected: + protected: /** * @brief Read the measured position from the joint state payload. * @param i Joint state message payload. * @return The joint position (radians or meters). */ - double readMeasuredPosition(const ScalarJointStateMsgPayload& i) const override - { - return i.state; - } + double readMeasuredPosition(const ScalarJointStateMsgPayload& i) const override { return i.state; } /** * @brief Read the measured velocity from the joint state payload. * @param i Joint state message payload. * @return The joint velocity (radians/sec or meters/sec). */ - double readMeasuredVelocity(const ScalarJointStateMsgPayload& i) const override - { - return i.state; - } + double readMeasuredVelocity(const ScalarJointStateMsgPayload& i) const override { return i.state; } /** * @brief Write the computed actuator input to the output payload. * @param o Actuator message payload. * @param val Value to write. */ - void writeOutput(SingleActuatorMsgPayload& o, double val) override - { - o.input = val; - } + void writeOutput(SingleActuatorMsgPayload& o, double val) override { o.input = val; } }; #endif 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"""