├── .clang-format ├── .dockerignore ├── .gitignore ├── CMakeLists.txt ├── Dockerfile ├── LICENSE ├── README.md ├── adcs └── __init__.py ├── cmake └── ProjectDeps.cmake ├── docker-compose.yml ├── examples ├── cpp │ ├── rigid_body.cpp │ └── vscmg.cpp └── python │ ├── rigid_body.py │ ├── rw4.py │ └── vscmg.py ├── include └── ADCS │ ├── Core │ ├── Controllers.h │ ├── IBaseSystem.h │ ├── NumPyArrayData.h │ ├── kinematics.h │ └── utils.h │ └── Systems │ ├── RW4.h │ ├── RigidBody.h │ └── VSCMG.h ├── setup.py └── src └── ADCS ├── Systems ├── RW4.cpp ├── RigidBody.cpp └── VSCMG.cpp └── py_controller.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: WebKit 4 | AccessModifierOffset: -4 5 | AlignAfterOpenBracket: DontAlign 6 | AlignConsecutiveMacros: false 7 | AlignConsecutiveAssignments: false 8 | AlignConsecutiveDeclarations: false 9 | AlignEscapedNewlines: Right 10 | AlignOperands: false 11 | AlignTrailingComments: false 12 | AllowAllArgumentsOnNextLine: true 13 | AllowAllConstructorInitializersOnNextLine: true 14 | AllowAllParametersOfDeclarationOnNextLine: true 15 | AllowShortBlocksOnASingleLine: Empty 16 | AllowShortCaseLabelsOnASingleLine: false 17 | AllowShortFunctionsOnASingleLine: All 18 | AllowShortLambdasOnASingleLine: All 19 | AllowShortIfStatementsOnASingleLine: Never 20 | AllowShortLoopsOnASingleLine: false 21 | AlwaysBreakAfterDefinitionReturnType: None 22 | AlwaysBreakAfterReturnType: None 23 | AlwaysBreakBeforeMultilineStrings: false 24 | AlwaysBreakTemplateDeclarations: MultiLine 25 | BinPackArguments: true 26 | BinPackParameters: true 27 | BraceWrapping: 28 | AfterCaseLabel: false 29 | AfterClass: false 30 | AfterControlStatement: false 31 | AfterEnum: false 32 | AfterFunction: true 33 | AfterNamespace: false 34 | AfterObjCDeclaration: false 35 | AfterStruct: false 36 | AfterUnion: false 37 | AfterExternBlock: false 38 | BeforeCatch: false 39 | BeforeElse: false 40 | IndentBraces: false 41 | SplitEmptyFunction: true 42 | SplitEmptyRecord: true 43 | SplitEmptyNamespace: true 44 | BreakBeforeBinaryOperators: All 45 | BreakBeforeBraces: WebKit 46 | BreakBeforeInheritanceComma: false 47 | BreakInheritanceList: BeforeColon 48 | BreakBeforeTernaryOperators: true 49 | BreakConstructorInitializersBeforeComma: false 50 | BreakConstructorInitializers: BeforeComma 51 | BreakAfterJavaFieldAnnotations: false 52 | BreakStringLiterals: true 53 | ColumnLimit: 0 54 | CommentPragmas: '^ IWYU pragma:' 55 | CompactNamespaces: false 56 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 57 | ConstructorInitializerIndentWidth: 4 58 | ContinuationIndentWidth: 4 59 | Cpp11BracedListStyle: false 60 | DeriveLineEnding: true 61 | DerivePointerAlignment: false 62 | DisableFormat: false 63 | ExperimentalAutoDetectBinPacking: false 64 | FixNamespaceComments: false 65 | ForEachMacros: 66 | - foreach 67 | - Q_FOREACH 68 | - BOOST_FOREACH 69 | IncludeBlocks: Preserve 70 | IncludeCategories: 71 | - Regex: '^"(llvm|llvm-c|clang|clang-c)/' 72 | Priority: 2 73 | SortPriority: 0 74 | - Regex: '^(<|"(gtest|gmock|isl|json)/)' 75 | Priority: 3 76 | SortPriority: 0 77 | - Regex: '.*' 78 | Priority: 1 79 | SortPriority: 0 80 | IncludeIsMainRegex: '(Test)?$' 81 | IncludeIsMainSourceRegex: '' 82 | IndentCaseLabels: false 83 | IndentGotoLabels: true 84 | IndentPPDirectives: None 85 | IndentWidth: 4 86 | IndentWrappedFunctionNames: false 87 | JavaScriptQuotes: Leave 88 | JavaScriptWrapImports: true 89 | KeepEmptyLinesAtTheStartOfBlocks: true 90 | MacroBlockBegin: '' 91 | MacroBlockEnd: '' 92 | MaxEmptyLinesToKeep: 1 93 | NamespaceIndentation: Inner 94 | ObjCBinPackProtocolList: Auto 95 | ObjCBlockIndentWidth: 4 96 | ObjCSpaceAfterProperty: true 97 | ObjCSpaceBeforeProtocolList: true 98 | PenaltyBreakAssignment: 2 99 | PenaltyBreakBeforeFirstCallParameter: 19 100 | PenaltyBreakComment: 300 101 | PenaltyBreakFirstLessLess: 120 102 | PenaltyBreakString: 1000 103 | PenaltyBreakTemplateDeclaration: 10 104 | PenaltyExcessCharacter: 1000000 105 | PenaltyReturnTypeOnItsOwnLine: 60 106 | PointerAlignment: Left 107 | ReflowComments: true 108 | SortIncludes: true 109 | SortUsingDeclarations: true 110 | SpaceAfterCStyleCast: false 111 | SpaceAfterLogicalNot: false 112 | SpaceAfterTemplateKeyword: true 113 | SpaceBeforeAssignmentOperators: true 114 | SpaceBeforeCpp11BracedList: true 115 | SpaceBeforeCtorInitializerColon: true 116 | SpaceBeforeInheritanceColon: true 117 | SpaceBeforeParens: ControlStatements 118 | SpaceBeforeRangeBasedForLoopColon: true 119 | SpaceInEmptyBlock: true 120 | SpaceInEmptyParentheses: false 121 | SpacesBeforeTrailingComments: 1 122 | SpacesInAngles: false 123 | SpacesInConditionalStatement: false 124 | SpacesInContainerLiterals: true 125 | SpacesInCStyleCastParentheses: false 126 | SpacesInParentheses: false 127 | SpacesInSquareBrackets: false 128 | SpaceBeforeSquareBrackets: false 129 | Standard: Latest 130 | StatementMacros: 131 | - Q_UNUSED 132 | - QT_REQUIRE_VERSION 133 | TabWidth: 8 134 | UseCRLF: false 135 | UseTab: Never 136 | ... 137 | 138 | -------------------------------------------------------------------------------- /.dockerignore: -------------------------------------------------------------------------------- 1 | **/.classpath 2 | **/.dockerignore 3 | **/.env 4 | **/.git 5 | **/.gitignore 6 | **/.project 7 | **/.settings 8 | **/.toolstarget 9 | **/.vs 10 | **/.vscode 11 | **/*.*proj.user 12 | **/*.dbmdl 13 | **/*.jfm 14 | **/bin 15 | **/charts 16 | **/docker-compose* 17 | **/compose* 18 | **/Dockerfile* 19 | **/node_modules 20 | **/npm-debug.log 21 | **/obj 22 | **/secrets.dev.yaml 23 | **/values.dev.yaml 24 | **/bin 25 | **/build 26 | README.md 27 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /build 2 | /bin 3 | /.vscode 4 | 5 | # Prerequisites 6 | *.d 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | *.obj 13 | 14 | # Precompiled Headers 15 | *.gch 16 | *.pch 17 | 18 | # Compiled Dynamic libraries 19 | *.so 20 | *.dylib 21 | *.dll 22 | 23 | # Fortran module files 24 | *.mod 25 | *.smod 26 | 27 | # Compiled Static libraries 28 | *.lai 29 | *.la 30 | *.a 31 | *.lib 32 | 33 | # Executables 34 | *.exe 35 | *.out 36 | *.app 37 | 38 | CMakeLists.txt.user 39 | CMakeCache.txt 40 | CMakeFiles 41 | CMakeScripts 42 | Testing 43 | Makefile 44 | cmake_install.cmake 45 | install_manifest.txt 46 | compile_commands.json 47 | CTestTestfile.cmake 48 | _deps 49 | 50 | 51 | 52 | # Byte-compiled / optimized / DLL files 53 | __pycache__/ 54 | *.py[cod] 55 | *$py.class 56 | 57 | # C extensions 58 | *.so 59 | 60 | # Distribution / packaging 61 | .Python 62 | build/ 63 | develop-eggs/ 64 | dist/ 65 | downloads/ 66 | eggs/ 67 | .eggs/ 68 | lib/ 69 | lib64/ 70 | parts/ 71 | sdist/ 72 | var/ 73 | wheels/ 74 | share/python-wheels/ 75 | *.egg-info/ 76 | .installed.cfg 77 | *.egg 78 | MANIFEST 79 | 80 | # PyInstaller 81 | # Usually these files are written by a python script from a template 82 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 83 | *.manifest 84 | *.spec 85 | 86 | # Installer logs 87 | pip-log.txt 88 | pip-delete-this-directory.txt 89 | 90 | # Unit test / coverage reports 91 | htmlcov/ 92 | .tox/ 93 | .nox/ 94 | .coverage 95 | .coverage.* 96 | .cache 97 | nosetests.xml 98 | coverage.xml 99 | *.cover 100 | *.py,cover 101 | .hypothesis/ 102 | .pytest_cache/ 103 | cover/ 104 | 105 | # Translations 106 | *.mo 107 | *.pot 108 | 109 | # Django stuff: 110 | *.log 111 | local_settings.py 112 | db.sqlite3 113 | db.sqlite3-journal 114 | 115 | # Flask stuff: 116 | instance/ 117 | .webassets-cache 118 | 119 | # Scrapy stuff: 120 | .scrapy 121 | 122 | # Sphinx documentation 123 | docs/_build/ 124 | 125 | # PyBuilder 126 | .pybuilder/ 127 | target/ 128 | 129 | # Jupyter Notebook 130 | .ipynb_checkpoints 131 | 132 | # IPython 133 | profile_default/ 134 | ipython_config.py 135 | 136 | # pyenv 137 | # For a library or package, you might want to ignore these files since the code is 138 | # intended to run in multiple environments; otherwise, check them in: 139 | # .python-version 140 | 141 | # pipenv 142 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 143 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 144 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 145 | # install all needed dependencies. 146 | #Pipfile.lock 147 | 148 | # poetry 149 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 150 | # This is especially recommended for binary packages to ensure reproducibility, and is more 151 | # commonly ignored for libraries. 152 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 153 | #poetry.lock 154 | 155 | # pdm 156 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 157 | #pdm.lock 158 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 159 | # in version control. 160 | # https://pdm.fming.dev/#use-with-ide 161 | .pdm.toml 162 | 163 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 164 | __pypackages__/ 165 | 166 | # Celery stuff 167 | celerybeat-schedule 168 | celerybeat.pid 169 | 170 | # SageMath parsed files 171 | *.sage.py 172 | 173 | # Environments 174 | .env 175 | .venv 176 | env/ 177 | venv/ 178 | ENV/ 179 | env.bak/ 180 | venv.bak/ 181 | 182 | # Spyder project settings 183 | .spyderproject 184 | .spyproject 185 | 186 | # Rope project settings 187 | .ropeproject 188 | 189 | # mkdocs documentation 190 | /site 191 | 192 | # mypy 193 | .mypy_cache/ 194 | .dmypy.json 195 | dmypy.json 196 | 197 | # Pyre type checker 198 | .pyre/ 199 | 200 | # pytype static type analyzer 201 | .pytype/ 202 | 203 | # Cython debug symbols 204 | cython_debug/ 205 | 206 | # PyCharm 207 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 208 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 209 | # and can be added to the global gitignore or merged into this file. For a more nuclear 210 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 211 | #.idea/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | project(VSCMG 3 | LANGUAGES CXX C 4 | VERSION 0.0.0) 5 | 6 | include(GNUInstallDirs) 7 | 8 | set(CMAKE_CXX_STANDARD 14) 9 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 10 | set(CMAKE_CXX_EXTENSIONS OFF) 11 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Type of build (Debug, Release etc." FORCE) 12 | if(NOT CMAKE_CONFIGURATION_TYPES) 13 | if(NOT CMAKE_BUILD_TYPE) 14 | message(STATUS "Setting build type to 'Release' as none was specified.") 15 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release") 16 | endif() 17 | endif() 18 | 19 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}") 20 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_SOURCE_DIR}/adcs") 21 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}") 22 | set(CMAKE_ROOT_PATH ${PROJECT_SOURCE_DIR}) 23 | 24 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) 25 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 26 | 27 | 28 | option(BUILD_EXAMPLES "Build Example " ON) 29 | option(BUILD_PYTHON_LIB "Build Python Wrapper " OFF) 30 | 31 | include_directories("include" "Systems/include") 32 | 33 | if(${BUILD_PYTHON_LIB}) 34 | add_compile_definitions(BUILD_PYTHON_LIB) 35 | find_package(Python3 COMPONENTS Interpreter Development REQUIRED) 36 | endif() 37 | 38 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake") 39 | include(ProjectDeps) 40 | 41 | ###################################################### 42 | # ADCS Core lib 43 | ###################################################### 44 | 45 | file(GLOB ADCS_SRC "include/ADCS/Core/*.h") 46 | add_library(ADCS SHARED ${ADCS_SRC}) 47 | target_link_libraries (ADCS Eigen3::Eigen ${Boost_LIBRARIES} ${PYTHON_LIBRARIES}) 48 | set_target_properties(ADCS PROPERTIES LINKER_LANGUAGE CXX) 49 | set_target_properties(ADCS PROPERTIES PUBLIC_HEADER 50 | "include/ADCS/Core/Controllers.h;include/ADCS/Core/IBaseSystem.h;include/ADCS/Core/NumPyArrayData.h;include/ADCS/Core/kinematics.h;include/ADCS/Core/utils.h" 51 | ) 52 | install(TARGETS ADCS 53 | PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/ADCS/Core/" 54 | ) 55 | 56 | ###################################################### 57 | # ADCS System modules 58 | ###################################################### 59 | 60 | #set(CMAKE_SHARED_LIBRARY_PREFIX "") 61 | file(GLOB SYSTEMS_MODULES "src/ADCS/Systems/*.cpp") 62 | foreach(module_path ${SYSTEMS_MODULES}) 63 | get_filename_component(module_name ${module_path} NAME_WE) 64 | 65 | add_library(${module_name} SHARED ${module_path} ) 66 | target_link_libraries (${module_name} ADCS Eigen3::Eigen ${Boost_LIBRARIES} ${PYTHON_LIBRARIES} ${BOOST_NUMPY_LIBRARY}) 67 | set_target_properties(${module_name} PROPERTIES LINKER_LANGUAGE CXX) 68 | set_target_properties(${module_name} PROPERTIES PUBLIC_HEADER "include/ADCS/Systems/${module_name}.h") 69 | 70 | install(TARGETS ${module_name} 71 | LIBRARY DESTINATION lib 72 | PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/ADCS/Systems/") 73 | 74 | endforeach() 75 | 76 | ###################################################### 77 | # Python wrapper library 78 | ###################################################### 79 | add_library(Controller MODULE "src/ADCS/py_controller.cpp") 80 | target_include_directories(Controller PRIVATE ${PYTHON_INCLUDE_DIRS}) 81 | target_link_libraries (Controller Eigen3::Eigen ${Boost_LIBRARIES} ${PYTHON_LIBRARIES} ${BOOST_NUMPY_LIBRARY}) 82 | IF (WIN32) 83 | set_target_properties(Controller PROPERTIES SUFFIX .pyd) 84 | ELSE() 85 | set_target_properties(Controller PROPERTIES SUFFIX ".so") 86 | ENDIF() 87 | 88 | install(TARGETS Controller 89 | LIBRARY DESTINATION DESTINATION lib 90 | ) 91 | 92 | #install(FILES __init__.py DESTINATION ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/adcs) 93 | #file(COPY __init__.py DESTINATION ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}) 94 | 95 | 96 | 97 | ###################################################### 98 | # Executable Samples 99 | ###################################################### 100 | 101 | if(${BUILD_EXAMPLES}) 102 | add_executable (TestVSCMG examples/cpp/vscmg.cpp ) 103 | target_link_libraries (TestVSCMG VSCMG) 104 | 105 | add_executable (TestRigidBody examples/cpp/rigid_body.cpp ) 106 | target_link_libraries (TestRigidBody RigidBody) 107 | endif() 108 | 109 | ###################################################### 110 | # copy python samples to binary output directory 111 | ###################################################### 112 | 113 | if(${BUILD_PYTHON_LIB}) 114 | file(GLOB python_samples "examples/python/*.py") 115 | foreach(sample ${python_samples}) 116 | file(COPY ${sample} DESTINATION ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}) 117 | endforeach() 118 | endif() 119 | 120 | 121 | 122 | # make uninstall 123 | add_custom_target("uninstall" COMMENT "Uninstall installed files") 124 | add_custom_command( 125 | TARGET "uninstall" 126 | POST_BUILD 127 | COMMENT "Uninstall files with install_manifest.txt" 128 | COMMAND xargs rm -vf < install_manifest.txt || echo Nothing in 129 | install_manifest.txt to be uninstalled! 130 | ) 131 | 132 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:focal 2 | LABEL Name=vscmgcpp Version=0.0.1 3 | 4 | # Non-interactive installation mode 5 | ENV DEBIAN_FRONTEND=noninteractive 6 | 7 | # These commands copy your files into the specified directory in the image 8 | # and set that as the working location 9 | COPY . /usr/src/vscmgcpp 10 | WORKDIR /usr/src/vscmgcpp 11 | 12 | 13 | # Update apt database and install reqired packages 14 | RUN apt-get update && apt-get install -y \ 15 | # g++ build essential 16 | build-essential \ 17 | # cmake and ccmake 18 | cmake cmake-curses-gui \ 19 | # Eigen3 for algebra 20 | libeigen3-dev \ 21 | # boost for ode integrator and python wrapper 22 | libboost-all-dev \ 23 | # python3 for python bindings 24 | python3 \ 25 | # pip 26 | python3-pip \ 27 | python-is-python3 \ 28 | && rm -rf /var/lib/apt/lists/* 29 | 30 | # install numpy 31 | RUN pip3 install numpy 32 | 33 | # Set the locale 34 | # RUN locale-gen en_US.UTF-8 35 | # build annd install adcs library 36 | RUN set -ex; \ 37 | mkdir -p /usr/src/vscmgcpp/build; \ 38 | cd /usr/src/vscmgcpp/build; \ 39 | cmake .. -DBUILD_PYTHON_LIB=True; \ 40 | make; \ 41 | make install 42 | 43 | WORKDIR /usr/src/vscmgcpp/ 44 | RUN touch README.md 45 | RUN python setup.py build 46 | RUN pip install . 47 | 48 | # Remove source code 49 | RUN rm -rf cmake/ docker include/ src/ Dockerfile adcs.egg-info/ 50 | 51 | WORKDIR /usr/src/vscmgcpp/examples/python 52 | 53 | CMD ["bash"] 54 | 55 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Siddharth Deore 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ADCS Toolbox 2 | 3 | C++ 14 CMake Linux 4 | Eigen (C++ library) 5 | BOOST (C++ library) 6 | 7 | High performance Spacecraft Attitude Dynamics and Control System toolbox with exposed python extention. Scalable toolbox uses Eigen3 for linear algebra and Boost for integration of system ODE . Currently there are three sample systems and more can be added provided that new systems are inherited from `IBaseSystem` base interface class and follow same structure. The `pyadcs` extention expose C++ classes and functions of `ADCS` library to python. 8 | 9 | 1. Compiled final report in pdf format [ 10 | tessi_DEORE_1823670.pdf](https://github.com/siddharthdeore/VSCMGThesisReport/releases/download/Defence/tessi_DEORE_1823670.pdf) 11 | 2. Thesis Report repository [VSCMGThesisReport](https://github.com/siddharthdeore/VSCMGThesisReport) 12 | 3. System URDF [vscmg_description](https://github.com/siddharthdeore/vscmg_description) 13 | 14 | # Systems 15 | - Rigid Body - Simple free floating rigid body. 16 | - VSCMG - Variable Speed Control Moment Gyroscope 17 | - RW4 - Satellite with tripod configration of 4 reaction wheels 18 | 19 | 20 | # Build 21 | 22 | Install Linux dependencies 23 | ``` 24 | sudo apt-get install -y libboost-all-dev 25 | sudo apt-get install -y libeigen3-dev 26 | sudo apt-get install -y python3 python3-pip 27 | ``` 28 | Make and install 29 | ``` 30 | mkdir build && cd build 31 | cmake .. 32 | make 33 | make install 34 | ``` 35 | 36 | # Uninstall adcs library 37 | ``` 38 | make uninstall 39 | ``` 40 | 41 | # Examples 42 | Executable targets are located in `bin` directory 43 | ``` 44 | # Rigid Body 45 | cd build/bin 46 | ./TestRigidBody 47 | 48 | # Variable Speed Control Moment Gyroscope 49 | cd build/bin 50 | ./TestVSCMG 51 | 52 | ``` 53 | # Python Interface 54 | Build with python interface wrapper library. Following comand executed in build directory shall build python interface library `pyadcs.so` in `bin` directory on linux or `pyadcs.dll` on windows. 55 | ```console 56 | cmake .. -DBUILD_PYTHON_LIB=True 57 | make 58 | ``` 59 | To install python interface library with pip, in projecto root directory 60 | ``` 61 | pip install . 62 | ``` 63 | To uninstall python interface library 64 | ``` 65 | pip uninstall adcs 66 | ``` 67 | 68 | # Run python samples 69 | To use library modules in python install library with `make install`, and navigate to `bin` directory and run sample scripts `rigid_body.py` or `vscmg.py`, eg. 70 | 71 | ``` 72 | python rigid_body.py 73 | ``` 74 | 75 | ## Source Tree 76 | ```console 77 | . 78 | ├── include 79 | │ └── ADCS # ADCS Core utils and interface 80 | │ ├── Core 81 | │ │ ├── Controllers.h 82 | │ │ ├── IBaseSystem.h 83 | : : : 84 | │ │ 85 | │ └── Systems # Public headers for Systems 86 | │ ├── RigidBody.h 87 | │ ├── RW4.h 88 | │ : 89 | └── src 90 | └── ADCS 91 | ├── py_controller.cpp # Python wrapper for controller 92 | └── Systems # targets in Systems are classes 93 | ├── RigidBody.cpp # inherited from IBaseSystem 94 | ├── RW4.cpp 95 | └── VSCMG.cpp 96 | 97 | ``` 98 | ## Requirements 99 | - C++ 14 100 | - [Boost](https://www.boost.org/) 101 | - [Eigen3](https://eigen.tuxfamily.org/index.php?title=Main_Page) 102 | - Python 2.7+ 103 | 104 | 105 | # Docker 106 | ``` 107 | # Pull latest image from dockerhub 108 | docker pull siddharthdeore/vscmgcpp:latest 109 | 110 | # run container in iteractive mode 111 | docker run --rm -it siddharthdeore/vscmgcpp:latest 112 | 113 | 114 | # Build image from source 115 | docker build --pull --rm -f "Dockerfile" -t vscmgcpp:latest "." 116 | # or 117 | # docker compose -f "docker-compose.yml" down 118 | 119 | 120 | # run python examples e.g. vscmg 121 | python3 vscmg.py 122 | ``` 123 | ⚠️ Note : This toolbox was written for MSc Thesis titled Neural Network based steering and Hardware in Loop Simulation of Variable Speed Control Moment Gyroscope 124 | 125 | ## Cite 126 | 127 | 1. Attitude control of a fast-retargeting agile nanosatellite using Neural Network based steering for Variable Speed Control Moment Gyroscopes} 128 | 129 | ```bibtex 130 | @inproceedings{inproceedings, 131 | author = {Deore, Siddharth and Santoni, Fabio and Piergentili, Fabrizio and Marzioli, Paolo}, 132 | year = {2021}, 133 | month = {06}, 134 | pages = {}, 135 | title = {Attitude control of a fast-retargeting agile nanosatellite using Neural Network based steering for Variable Speed Control Moment Gyroscopes} 136 | } 137 | ``` 138 | 139 | 2. Design and Manufacturing of Hardware in Loop simulation testbed equipped with Variable Speed Control Moment Gyroscopes 140 | 141 | ```bibtex 142 | @inproceedings{inproceedings, 143 | author = {Deore, Siddharth and Santoni, Fabio and Piergentili, Fabrizio and Marzioli, Paolo}, 144 | year = {2021}, 145 | month = {06}, 146 | pages = {}, 147 | title = {Design and Manufacturing of Hardware in Loop simulation testbed equipped with Variable Speed Control Moment Gyroscopes} 148 | } 149 | ``` 150 | 151 | ## Maintainers 152 | This repository is maintained by: 153 | || [siddharth deore](https://github.com/siddharthdeore)| 154 | |--|--| 155 | -------------------------------------------------------------------------------- /adcs/__init__.py: -------------------------------------------------------------------------------- 1 | from .libRigidBody import * 2 | from .libRW4 import * 3 | from .libVSCMG import * 4 | from .libController import * -------------------------------------------------------------------------------- /cmake/ProjectDeps.cmake: -------------------------------------------------------------------------------- 1 | 2 | find_package(Boost) 3 | include_directories(${Boost_INCLUDE_DIRS}) 4 | 5 | find_package (Eigen3 3.3 REQUIRED NO_MODULE) 6 | include_directories(${EIGEN3_INCLUDE_DIR}) 7 | 8 | 9 | if(${BUILD_PYTHON_LIB}) 10 | FIND_PACKAGE(PythonInterp) 11 | 12 | if (PYTHONINTERP_FOUND) 13 | if (UNIX AND NOT APPLE) 14 | if (PYTHON_VERSION_MAJOR EQUAL 3) 15 | FIND_PACKAGE(Boost COMPONENTS python${PYTHON_VERSION_SUFFIX} numpy${PYTHON_VERSION_SUFFIX}) 16 | FIND_PACKAGE(PythonInterp 3) 17 | FIND_PACKAGE(PythonLibs 3 REQUIRED) 18 | else() 19 | FIND_PACKAGE(Boost COMPONENTS python) 20 | FIND_PACKAGE(PythonInterp) 21 | FIND_PACKAGE(PythonLibs REQUIRED) 22 | endif() 23 | else() 24 | if (PYTHON_VERSION_MAJOR EQUAL 3) 25 | FIND_PACKAGE(Boost COMPONENTS python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR} numpy${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}) 26 | FIND_PACKAGE(PythonInterp 3) 27 | FIND_PACKAGE(PythonLibs 3 REQUIRED) 28 | else() 29 | FIND_PACKAGE(Boost COMPONENTS python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR} numpy${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}) 30 | FIND_PACKAGE(PythonInterp) 31 | FIND_PACKAGE(PythonLibs REQUIRED) 32 | endif() 33 | endif() 34 | else() 35 | message("Python not found") 36 | endif() 37 | 38 | message(STATUS "PYTHON_LIBRARIES = ${PYTHON_LIBRARIES}") 39 | message(STATUS "PYTHON_EXECUTABLE = ${PYTHON_EXECUTABLE}") 40 | message(STATUS "PYTHON_INCLUDE_DIRS = ${PYTHON_INCLUDE_DIRS}") 41 | message(STATUS "Boost_LIBRARIES = ${Boost_LIBRARIES}") 42 | 43 | include_directories(${PYTHON_INCLUDE_DIRS}) 44 | endif() -------------------------------------------------------------------------------- /docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: '3.4' 2 | 3 | services: 4 | vscmgcpp: 5 | image: vscmgcpp 6 | build: 7 | context: . 8 | dockerfile: ./Dockerfile 9 | -------------------------------------------------------------------------------- /examples/cpp/rigid_body.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | int main(int argc, char const* argv[]) 7 | { 8 | char* p; 9 | long max = 10000; 10 | 11 | if (argc > 1) { 12 | max = strtol(argv[1], &p, 10); 13 | } 14 | 15 | // satellite object of type RigidBody 16 | std::shared_ptr sat = std::make_shared(); 17 | sat->set_inertia(2.0, 1.0, 0.5); 18 | 19 | // initial state 20 | Eigen::Quaterniond q(cos(1.0), 0, sin(1.0), 0); 21 | Eigen::Quaterniond qd(1.0, 0, 0, 0); 22 | 23 | Eigen::Matrix w(10.0, -0.5, 0.2); 24 | 25 | RigidBody::state_type X0 = { q.w(), q.x(), q.y(), q.z(), w.x(), w.y(), w.z() }; 26 | 27 | sat->set_state(X0); 28 | 29 | // time 30 | double t = 0; 31 | 32 | auto t1 = std::chrono::high_resolution_clock::now(); 33 | for (size_t i = 0; i < max; i++) { 34 | // get current state of Rigid Body (observations) 35 | RigidBody::state_type X = sat->get_state(); 36 | 37 | q = Eigen::Quaterniond(X[0], X[1], X[2], X[3]); 38 | w = Eigen::Vector3d(X[4], X[5], X[6]); 39 | 40 | // calculate error in reference state and current state 41 | auto qe = get_quaternion_error(q, qd); 42 | 43 | // calcualte control torques with required to bring rigid body in steady state 44 | auto u = Controller::quaternion_feedback(qe, w, 1.0, 0.1); 45 | // discreate control action with actuator satuaration 46 | // auto u = Controller::quaternion_feedback_discreate(qe, w, 1.0, 0.1,0.1,0.001); 47 | 48 | // apply controlled torque action and take time step of forward dynamics 49 | sat->step(u, t, t + 0.001, 0.001); 50 | 51 | // print state 52 | if (i % 100 == 0) { 53 | std::cout << *sat << "\n"; 54 | } 55 | 56 | t += 0.001; 57 | } 58 | auto t2 = std::chrono::high_resolution_clock::now(); 59 | 60 | // benchmarking 61 | std::cout << " " 62 | << std::chrono::duration_cast(t2 - t1).count() 63 | << " ms \n"; 64 | 65 | return 0; 66 | } 67 | -------------------------------------------------------------------------------- /examples/cpp/vscmg.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | int main(int argc, char const* argv[]) 7 | { 8 | char* p; 9 | long max = 10000; 10 | 11 | if (argc > 1) { 12 | max = strtol(argv[1], &p, 10); 13 | } 14 | 15 | // satellite object of type VSCMG 16 | std::shared_ptr sat = std::make_shared(); 17 | 18 | // initial state 19 | Eigen::Quaterniond q(cos(0.5), 0, sin(0.5), 0); 20 | Eigen::Quaterniond qd(1.0, 0, 0, 0); 21 | Eigen::Matrix w(0.01, 0.01, -0.01); 22 | 23 | // Reaction wheel angular velocities 24 | Eigen::Matrix Omega(100, 100, 100, 100); 25 | 26 | // gimbal angles 27 | Eigen::Matrix delta(0, 0, 0, 0); 28 | 29 | VSCMG::state_type X0 = { q.w(), q.x(), q.y(), q.z(), w.x(), w.y(), w.z(), delta[0], delta[1], delta[2], delta[3], Omega[0], Omega[1], Omega[2], Omega[3] }; 30 | sat->set_state(X0); 31 | sat->set_gimbal_angle(delta); 32 | sat->set_wheel_velocity(Omega); 33 | VSCMG::action_type act; 34 | act.setZero(); 35 | 36 | // time 37 | double t = 0; 38 | 39 | auto t1 = std::chrono::high_resolution_clock::now(); 40 | for (size_t i = 0; i < max; i++) { 41 | 42 | // get current state of Rigid Body (observations) 43 | VSCMG::state_type X = sat->get_state(); 44 | 45 | q = Eigen::Quaterniond(X[0], X[1], X[2], X[3]); 46 | w = Eigen::Vector3d(X[4], X[5], X[6]); 47 | 48 | // calculate error in reference state and current state 49 | auto qe = get_quaternion_error(q, qd); 50 | 51 | // calcualte control torques which required to bring rigid body in steady state 52 | auto u = Controller::quaternion_feedback(qe, w, 10.0, 0.1); 53 | act = sat->calc_steering(u, t); 54 | // integrate a step 55 | sat->step(act, t, t + 0.001, 0.001); 56 | 57 | // verbose 58 | if (i % 100 == 0) 59 | std::cout << *sat << "\n"; 60 | 61 | t += 0.001; 62 | } 63 | auto t2 = std::chrono::high_resolution_clock::now(); 64 | 65 | // benchmarking 66 | std::cout << " " 67 | << std::chrono::duration_cast(t2 - t1).count() 68 | << " ms \n"; 69 | 70 | return 0; 71 | } 72 | -------------------------------------------------------------------------------- /examples/python/rigid_body.py: -------------------------------------------------------------------------------- 1 | from cmath import pi 2 | import sys 3 | import time 4 | import numpy as np 5 | 6 | # import rigid body and quaternion_control from pyadcs 7 | from adcs import RigidBody 8 | from adcs import quaternion_control 9 | 10 | # create RigidBody object instance 11 | body = RigidBody() 12 | 13 | # pretty print states 14 | np.set_printoptions(formatter={'float': '{: 0.4f}'.format}) 15 | 16 | 17 | def main(): 18 | 19 | # controller stiffness gain 20 | K = 10.0 21 | 22 | # controller damping gain 23 | C = 1.0 24 | 25 | # time 26 | t = 0 27 | delta_t = 0.01 28 | 29 | # set desired state vector in form of numpy array [qw qx qy qz wx wy wz] 30 | DESIRED_STATE = np.array( 31 | [np.cos(45*pi/180.), 0., 0., np.sin(45*pi/180.), 0., 0., 0.]) 32 | 33 | t_next = time.time() + 0.01 34 | body.set_inertia(1.0,2.0,3.0,0.0,0.0,0.0) 35 | 36 | while(True): 37 | 38 | try: 39 | # get current body state 40 | state = body.get_state() 41 | 42 | # compute torques with quaternion feedback control 43 | action = quaternion_control(state, DESIRED_STATE, K, C) 44 | 45 | # integrate step in time with control action 46 | state = body.step(action, t, t+delta_t, delta_t/10.) 47 | 48 | # pretty print states 49 | if(t_next < time.time() + 0.025): 50 | print("Quaternion :" + str(state[0:4])) 51 | print("Rate :" + str(state[4:7])+"\n") 52 | t_next = t_next + 0.025 53 | 54 | # increament time 55 | t = t + delta_t 56 | 57 | except KeyboardInterrupt: 58 | # quit 59 | sys.exit() 60 | 61 | 62 | if __name__ == "__main__": 63 | main() 64 | -------------------------------------------------------------------------------- /examples/python/rw4.py: -------------------------------------------------------------------------------- 1 | from cmath import pi 2 | import sys, time 3 | import numpy as np 4 | 5 | # import rigid body and quaternion_control from pyadcs 6 | from adcs import RW4 7 | from adcs import quaternion_control 8 | 9 | # create RW4 object instance 10 | body = RW4() 11 | 12 | # pretty print states 13 | np.set_printoptions(formatter={'float': '{: 0.4f}'.format}) 14 | 15 | def main(): 16 | 17 | # controller stiffness gain 18 | K = 10.0 19 | 20 | # controller damping gain 21 | C = 10.0 22 | 23 | # time 24 | t = 0.0 25 | delta_t = 0.01 26 | 27 | # set desired state vector in form of numpy array [qw qx qy qz wx wy wz] 28 | desired_state = np.array( 29 | [np.cos(45*pi/180.), 0., 0., np.sin(45*pi/180.), 0., 0.,0.]) 30 | 31 | t_next = time.time() + 0.01 32 | body.set_inertia(1.0,2.0,3.0,0.0,0.0,0.0) 33 | 34 | while(True): 35 | 36 | try: 37 | # get current body state 38 | state = body.get_state() 39 | 40 | # compute torques with quaternion feedback control 41 | torque = quaternion_control(state, desired_state, K, C) 42 | 43 | # compute required actuator signals from given torque 44 | action = body.calc_steering(torque, 0.0) 45 | 46 | # integrate step in time with control action 47 | state = body.step(action, t, t+delta_t, delta_t/10.) 48 | if(t_next< time.time() + 0.025): 49 | print("Quaternion :" + str(state[0:4])) 50 | print("Rate :" + str(state[4:7])) 51 | print("Omega :" + str(state[7:11]) + "\n") 52 | t_next = t_next + 0.025 53 | 54 | # increament time 55 | t = t + delta_t 56 | 57 | except KeyboardInterrupt: 58 | # quit 59 | sys.exit() 60 | 61 | 62 | if __name__ == "__main__": 63 | main() 64 | -------------------------------------------------------------------------------- /examples/python/vscmg.py: -------------------------------------------------------------------------------- 1 | from cmath import pi 2 | import sys, time 3 | import numpy as np 4 | 5 | # import rigid body and quaternion_control from pyadcs 6 | from adcs import VSCMG 7 | from adcs import quaternion_control 8 | 9 | # create VSCMG object instance 10 | body = VSCMG() 11 | 12 | # pretty print states 13 | np.set_printoptions(formatter={'float': '{: 0.4f}'.format}) 14 | 15 | def get_quaternion_from_euler(roll, pitch, yaw): 16 | """ 17 | Convert an Euler angle to a quaternion. 18 | 19 | Input 20 | :param roll: The roll (rotation around x-axis) angle in radians. 21 | :param pitch: The pitch (rotation around y-axis) angle in radians. 22 | :param yaw: The yaw (rotation around z-axis) angle in radians. 23 | 24 | Output 25 | :return qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format 26 | """ 27 | qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) 28 | qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) 29 | qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) 30 | qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) 31 | 32 | return [qw, qx, qy, qz] 33 | 34 | def main(): 35 | 36 | # controller stiffness gain 37 | K = 1.0 38 | 39 | # controller damping gain 40 | C = 10. 41 | 42 | # time 43 | t = 0.0 44 | delta_t = 0.01 45 | deg = pi/180. 46 | quat = get_quaternion_from_euler(0*deg,0*deg,180*deg) 47 | w = [0.,0.,0.] 48 | # set desired state vector in form of numpy array [qw qx qy qz wx wy wz] 49 | desired_state = np.array(quat+w) 50 | body.set_inertia(1.0,2.0,3.0,0.0,0.0,0.0) 51 | 52 | t_next = time.time() + 0.01 53 | 54 | while(True): 55 | 56 | try: 57 | # get current body state 58 | state = body.get_state() 59 | 60 | # compute torques with quaternion feedback control 61 | torque = quaternion_control(state, desired_state, K, C) 62 | 63 | # compute required actuator signals from given torque 64 | action = body.calc_steering(torque, t) 65 | 66 | # integrate step in time with control action 67 | state = body.step(action, t, t+delta_t, delta_t/10.) 68 | 69 | if(t_next< time.time() + 0.025): 70 | print("Quaternion :" + str(state[0:4])) 71 | print("Rate :" + str(state[4:7])) 72 | print("Delta :" + str(state[7:11])) 73 | print("Omega :" + str(state[11:15]) + "\n") 74 | t_next = t_next + 0.025 75 | 76 | # increament time 77 | t = t + delta_t 78 | 79 | except KeyboardInterrupt: 80 | # quit 81 | sys.exit() 82 | 83 | 84 | if __name__ == "__main__": 85 | main() 86 | -------------------------------------------------------------------------------- /include/ADCS/Core/Controllers.h: -------------------------------------------------------------------------------- 1 | #ifndef _ADCS_CONTROLLERS_H 2 | #define _ADCS_CONTROLLERS_H 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | namespace Controller { 10 | 11 | /** 12 | * @brief Generalized Quaternion Feedback Control law 13 | * 14 | * @param qe Error in desired and current orieantation 15 | * @param w Angular velocity of rigid body 16 | * @param K artificial stiffness coefficient 17 | * @param C damping 18 | * @return Eigen::Matrix 19 | */ 20 | static Eigen::Matrix quaternion_feedback( 21 | const Eigen::Quaternion& qe, 22 | const Eigen::Matrix& w, 23 | const double& K, 24 | const double& C); 25 | 26 | /** 27 | * @brief quaternion feedback with discreate step output 28 | * 29 | * @param qe 30 | * @param w 31 | * @param K 32 | * @param C 33 | * @param C 34 | * @param step posible step of torque in Nm, default step value is resolution of 12 bit actuator of max 1 Nm 35 | * @return Eigen::Matrix 36 | */ 37 | static Eigen::Matrix quaternion_feedback_discreate( 38 | const Eigen::Quaternion& qe, 39 | const Eigen::Matrix& w, 40 | const double& K, 41 | const double& C, 42 | const double max_torque = 1.0, 43 | const double step = 0.000244140625); 44 | 45 | static Eigen::Matrix quaternion_feedback( 46 | const Eigen::Quaternion& qe, 47 | const Eigen::Matrix& w, 48 | const double& K, 49 | const double& C) 50 | { 51 | return -K * qe.vec() * qe.w() - C * w; 52 | } 53 | static Eigen::Matrix quaternion_feedback_discreate( 54 | const Eigen::Quaternion& qe, 55 | const Eigen::Matrix& w, 56 | const double& K, 57 | const double& C, 58 | const double max_torque, 59 | const double step) 60 | { 61 | const Eigen::Matrix u = utils::clamp(-K * qe.vec() * qe.w() - C * w, -max_torque, max_torque) / step; 62 | return u.array().round() * step; 63 | } 64 | } 65 | 66 | #endif -------------------------------------------------------------------------------- /include/ADCS/Core/IBaseSystem.h: -------------------------------------------------------------------------------- 1 | #ifndef _I_BASE_SYSTEM_H 2 | #define _I_BASE_SYSTEM_H 3 | 4 | #include //std::shared_ptr 5 | 6 | #include 7 | #include // Quaternion 8 | 9 | #include 10 | #include 11 | #include 12 | #include // std:: bind, std::placeholders 13 | #ifdef BUILD_PYTHON_LIB 14 | #include "NumPyArrayData.h" 15 | #include 16 | #include 17 | namespace numpy = boost::python::numpy; 18 | 19 | #define EXPOSE_SYSTEM_TO_PYTHON(PYSYS_NAME) \ 20 | BOOST_PYTHON_MODULE(lib##PYSYS_NAME) \ 21 | { \ 22 | using namespace boost::python; \ 23 | numpy::initialize(); \ 24 | Py_Initialize(); \ 25 | class_(#PYSYS_NAME) \ 26 | .def("get_state", &PYSYS_NAME::py_get_state, "Returns numpy array of #PYSYS_NAME State vector") \ 27 | .def("set_state", &PYSYS_NAME::py_set_state, "Set #PYSYS_NAME state vector with numpy array") \ 28 | .def("step", &PYSYS_NAME::py_step, "Evaluation of system dynamics in time") \ 29 | .def("calc_steering", &PYSYS_NAME::py_calc_steering, "Transformation of torque vector to control action") \ 30 | .def("set_inertia", &PYSYS_NAME::py_set_inertia, "set body inertia") \ 31 | .def("get_sample_action", &PYSYS_NAME::py_get_sample_action, "Get random action type array "); \ 32 | } 33 | #endif 34 | 35 | namespace pl = std::placeholders; 36 | namespace odeint = boost::numeric::odeint; 37 | template 38 | class IBaseSystem { 39 | public: 40 | typedef std::shared_ptr Ptr; 41 | typedef boost::array state_type; 42 | typedef Eigen::Matrix action_type; 43 | 44 | IBaseSystem() { } 45 | ~IBaseSystem() { } 46 | 47 | inline void set_inertia(const Eigen::Matrix& Jp) 48 | { 49 | _Jp = Jp; 50 | } 51 | 52 | inline void set_inertia(const Eigen::Matrix& JpVec) 53 | { 54 | _Jp = Eigen::Vector3d(JpVec).asDiagonal(); 55 | } 56 | 57 | inline void set_inertia(const double& Ixx, const double& Iyy, const double& Izz) 58 | { 59 | _Jp = Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal(); 60 | } 61 | 62 | /** 63 | * @brief Set the state of system 64 | * 65 | * @param state 66 | */ 67 | inline void set_state(const state_type& state) 68 | { 69 | _state = std::move(state); 70 | } 71 | 72 | /** 73 | * @brief Get the state of system 74 | * 75 | * @param state 76 | */ 77 | inline state_type get_state() const 78 | { 79 | return _state; 80 | } 81 | 82 | /** 83 | * @brief Eqauation of motion of system with state X in form of 84 | * dX/dt = f(x,t); 85 | * 86 | * @param x_ system state 87 | * @param dxdt_ state derivative 88 | * @param t time 89 | */ 90 | virtual void operator()(const state_type& x_, state_type& dxdt_, double t) = 0; 91 | 92 | /** 93 | * @brief transformation of torque in body frame to controlable actuator signal vector 94 | * 95 | * @param torque 96 | * @param t 97 | * @return action_type 98 | */ 99 | virtual action_type calc_steering(const Eigen::Matrix& torque, const double& t = 0) = 0; 100 | 101 | /** 102 | * @brief Set control action variable, action is controlable variable in system 103 | * 104 | * @param action 105 | */ 106 | inline void set_action(const action_type& action) 107 | { 108 | _action = action; 109 | } 110 | 111 | /** 112 | * @brief Time evaluation step, numerical integrate in time with control action 113 | * 114 | * @param action 115 | * @param t_start 116 | * @param t_end 117 | * @param dt 118 | */ 119 | inline void step(const action_type& action = Eigen::Matrix::setZero(), const double& t_start = 0.0, const double& t_end = 0.01, const double& dt = 0.001) 120 | { 121 | // set action 122 | set_action(action); 123 | // integrate in time 124 | integrate_const(stepper, 125 | std::bind(&IBaseSystem::operator(), std::ref(*this), pl::_1, pl::_2, pl::_3), 126 | _state, t_start, t_end, dt); 127 | } 128 | 129 | inline action_type get_sample_action() 130 | { 131 | action_type x; 132 | x.setRandom(); 133 | return x; 134 | } 135 | 136 | #ifdef BUILD_PYTHON_LIB 137 | // Python wrappers 138 | void py_set_state(const numpy::ndarray& state) 139 | { 140 | state_type X; 141 | NumPyArrayData d(state); 142 | for (size_t i = 0; i < STATE_SIZE; i++) { 143 | X[i] = d(i); 144 | } 145 | set_state(X); 146 | } 147 | void py_set_inertia(double Ixx, double Iyy, double Izz, double Ixy, double Ixz, double Iyz) 148 | { 149 | _Jp << Ixx, Ixy, Ixz, 150 | Ixy, Iyy, Iyz, 151 | Ixz, Iyz, Izz; 152 | } 153 | 154 | numpy::ndarray py_get_state() 155 | { 156 | Py_intptr_t shape[1] = { _state.size() }; 157 | numpy::ndarray result = numpy::zeros(1, shape, numpy::dtype::get_builtin()); 158 | std::copy(_state.begin(), _state.end(), reinterpret_cast(result.get_data())); 159 | return result; 160 | } 161 | 162 | numpy::ndarray py_get_sample_action() 163 | { 164 | action_type act = get_sample_action(); 165 | 166 | // Eigen3::Matrix to numpy array 167 | Py_intptr_t shape[1] = { ACTION_SIZE }; 168 | numpy::ndarray result_vector = numpy::zeros(1, shape, numpy::dtype::get_builtin()); 169 | for (size_t i = 0; i < ACTION_SIZE; i++) { 170 | result_vector[i] = act[i]; 171 | } 172 | return result_vector; 173 | } 174 | 175 | numpy::ndarray py_step(const numpy::ndarray& action, const double& t_start = 0.0, const double& t_end = 0.01, const double& dt = 0.001) 176 | { 177 | action_type u; 178 | 179 | // numpy array to Eigen3::Matrix 180 | NumPyArrayData data(action); 181 | for (size_t i = 0; i < ACTION_SIZE; i++) { 182 | u[i] = data(i); 183 | } 184 | 185 | step(u); 186 | return py_get_state(); 187 | } 188 | 189 | numpy::ndarray py_calc_steering(const numpy::ndarray& torque, const double& t) 190 | { 191 | NumPyArrayData data(torque); 192 | Eigen::Vector3d u(data(0), data(1), data(2)); 193 | action_type act = calc_steering(u, t); 194 | 195 | // Eigen3::Matrix to numpy array 196 | Py_intptr_t shape[1] = { ACTION_SIZE }; 197 | numpy::ndarray result_vector = numpy::zeros(1, shape, numpy::dtype::get_builtin()); 198 | for (size_t i = 0; i < ACTION_SIZE; i++) { 199 | result_vector[i] = act[i]; 200 | } 201 | return result_vector; 202 | } 203 | #endif 204 | 205 | protected: 206 | // body inertia 207 | Eigen::Matrix _Jp; 208 | 209 | // control action vector 210 | action_type _action; 211 | 212 | // state vector 213 | state_type _state; 214 | 215 | // numeric integrator type 216 | odeint::runge_kutta4 stepper; 217 | }; 218 | #endif //#ifndef _I_BASE_SYSTEM_H -------------------------------------------------------------------------------- /include/ADCS/Core/NumPyArrayData.h: -------------------------------------------------------------------------------- 1 | #ifndef NUMPYARRAY_H 2 | #define NUMPYARRAY_H 3 | 4 | #pragma once 5 | // from http://www.skotheim.net/wiki/Boost_Python 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace bp = boost::python; 12 | namespace np = boost::python::numpy; 13 | 14 | // Helper class for fast access to array elements 15 | template 16 | class NumPyArrayData { 17 | char* m_data; 18 | const Py_intptr_t* m_strides; 19 | 20 | public: 21 | NumPyArrayData(const np::ndarray& arr) 22 | { 23 | np::dtype dtype = arr.get_dtype(); 24 | np::dtype dtype_expected = np::dtype::get_builtin(); 25 | 26 | if (dtype != dtype_expected) { 27 | std::stringstream ss; 28 | ss << "NumPyArrayData: Unexpected data type (" << bp::extract(dtype.attr("__str__")()) << ") received. "; 29 | ss << "Expected " << bp::extract(dtype_expected.attr("__str__")()); 30 | throw std::runtime_error(ss.str().c_str()); 31 | } 32 | 33 | m_data = arr.get_data(); 34 | m_strides = arr.get_strides(); 35 | } 36 | 37 | T* data() 38 | { 39 | return reinterpret_cast(m_data); 40 | } 41 | 42 | const Py_intptr_t* strides() 43 | { 44 | return m_strides; 45 | } 46 | 47 | // 1D array access 48 | inline T& operator()(int i) 49 | { 50 | return *reinterpret_cast(m_data + i * m_strides[0]); 51 | } 52 | 53 | // 2D array access 54 | inline T& operator()(int i, int j) 55 | { 56 | return *reinterpret_cast(m_data + i * m_strides[0] + j * m_strides[1]); 57 | } 58 | 59 | // 3D array access 60 | inline T& operator()(int i, int j, int k) 61 | { 62 | return *reinterpret_cast(m_data + i * m_strides[0] + j * m_strides[1] + k * m_strides[2]); 63 | } 64 | 65 | // 4D array access 66 | inline T& operator()(int i, int j, int k, int l) 67 | { 68 | return *reinterpret_cast(m_data + i * m_strides[0] + j * m_strides[1] + k * m_strides[2] + l * m_strides[3]); 69 | } 70 | }; 71 | 72 | #endif -------------------------------------------------------------------------------- /include/ADCS/Core/kinematics.h: -------------------------------------------------------------------------------- 1 | #ifndef KINEMATICS_H 2 | #define KINEMATICS_H 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | /** 10 | * @brief Special Orthogonal Group 11 | * 12 | */ 13 | namespace so3 { 14 | /** 15 | * @brief The hat operator takes a vector and transforms it into its equivalent matrix 16 | * to represent the cross product operation 17 | * 18 | * @param vec 19 | * @return Eigen::Matrix 20 | */ 21 | static Eigen::Matrix hat( 22 | const Eigen::Matrix& vec); 23 | 24 | static Eigen::Matrix hat( 25 | const Eigen::Matrix& vec) 26 | { 27 | Eigen::Matrix mat; 28 | mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0; 29 | return mat; 30 | } 31 | } 32 | /** 33 | * @brief The quaternion error vector, The quaternion associated with 34 | * the rotation that takes desired frame (Fd) onto body frame (Fb), 35 | * the amplitude of which thus provides the misalignment error of body 36 | * frame (Fb) with respect to desired frame (Fd). 37 | * @param q 38 | * @param w 39 | * @return Eigen::Quaternion 40 | */ 41 | static Eigen::Quaternion get_quaternion_error( 42 | const Eigen::Quaternion& q, 43 | const Eigen::Quaternion& qd); 44 | 45 | /** 46 | * @brief The time evolution of time varying quaternion q with angular velocity w 47 | * 48 | * @param q 49 | * @param w 50 | * @return Eigen::Quaternion 51 | */ 52 | static Eigen::Quaternion get_quaternion_kinematics( 53 | const Eigen::Quaternion& q, 54 | const Eigen::Matrix& w); 55 | 56 | static Eigen::Quaternion get_quaternion_kinematics( 57 | const Eigen::Quaternion& q, 58 | const Eigen::Matrix& w) 59 | { 60 | Eigen::Matrix Omega; 61 | Omega(0, 0) = 0.0; 62 | Omega.block<3, 3>(1, 1) = -so3::hat(w); 63 | Omega.block<3, 1>(1, 0) = w; 64 | Omega.block<1, 3>(0, 1) = -w.transpose(); 65 | Eigen::Matrix qv(q.w(), q.x(), q.y(), q.z()); 66 | auto qvout = 0.5 * Omega * qv; 67 | return Eigen::Quaternion(qvout(0), qvout(1), qvout(2), qvout(3)); 68 | } 69 | 70 | static Eigen::Quaternion get_quaternion_error( 71 | const Eigen::Quaternion& q, 72 | const Eigen::Quaternion& qd) 73 | { 74 | return qd.inverse() * q; 75 | } 76 | 77 | #endif -------------------------------------------------------------------------------- /include/ADCS/Core/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef _ADCS_CORE_UTILS_H 2 | #define _ADCS_CORE_UTILS_H 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace utils { 11 | using namespace Eigen; 12 | 13 | /** 14 | * @brief Returns vector containig max value among two input vectors 15 | * 16 | * @tparam Derived 17 | * @tparam T 18 | * @param a 19 | * @param b 20 | * @return Derived 21 | */ 22 | template 23 | inline Derived max(const MatrixBase& a, T b) 24 | { 25 | return a.array().max(b); 26 | } 27 | 28 | /** 29 | * @brief Returns max value given two variables 30 | * 31 | * @tparam T 32 | * @tparam std::enable_if_t::value> 33 | * @param a 34 | * @param b 35 | * @return T 36 | */ 37 | template ::value>> 38 | inline T max(T a, T b) 39 | { 40 | return std::max(a, b); 41 | } 42 | 43 | /** 44 | * @brief Returns min value given two variables 45 | * 46 | * @tparam T 47 | * @tparam std::enable_if_t::value> 48 | * @param a 49 | * @param b 50 | * @return T 51 | */ 52 | template ::value>> 53 | inline T min(T a, T b) 54 | { 55 | return std::min(a, b); 56 | } 57 | 58 | /** 59 | * @brief Clamps two input between min and max value. 60 | * 61 | * @tparam T 62 | * @tparam std::enable_if_t::value> 63 | * @param x 64 | * @param minVal 65 | * @param maxVal 66 | * @return T 67 | */ 68 | template ::value>> 69 | inline T clamp(T x, T minVal, T maxVal) 70 | { 71 | return min(max(x, minVal), maxVal); 72 | } 73 | 74 | /** 75 | * @brief Clamps two input Vector between min and max Vector. 76 | * 77 | * @param x 78 | * @param minVal 79 | * @param maxVal 80 | * @return Eigen::Vector3d 81 | */ 82 | inline Eigen::Vector3d clamp(Eigen::Vector3d x, Eigen::Vector3d minVal, Eigen::Vector3d maxVal) 83 | { 84 | return Eigen::Vector3d(clamp(x.x(), minVal.x(), maxVal.x()), clamp(x.y(), minVal.y(), maxVal.y()), clamp(x.z(), minVal.z(), maxVal.z())); 85 | } 86 | 87 | /** 88 | * @brief Clamps two input Vector between min and max Vector. 89 | * 90 | * @param x 91 | * @param minVal 92 | * @param maxVal 93 | * @return Eigen::Vector3d 94 | */ 95 | inline Eigen::Vector3d clamp(const Eigen::Vector3d& x, const double minVal, const double maxVal) 96 | { 97 | return Eigen::Vector3d(clamp(x.x(), minVal, maxVal), clamp(x.y(), minVal, maxVal), clamp(x.z(), minVal, maxVal)); 98 | } 99 | 100 | /** 101 | * @brief Returns sign of input parameter. 102 | * 103 | * @tparam T 104 | * @tparam std::enable_if_t::value> 105 | * @param val 106 | * @return constexpr T 107 | */ 108 | template ::value>> 109 | [[nodiscard]] constexpr inline T sign(T val) 110 | { 111 | return static_cast((0 < val) - (val < 0)); 112 | } 113 | } 114 | 115 | #endif -------------------------------------------------------------------------------- /include/ADCS/Systems/RW4.h: -------------------------------------------------------------------------------- 1 | #ifndef _ADCS_RW4_BODY_H 2 | #define _ADCS_RW4_BODY_H 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | class RW4 : public IBaseSystem<11, 4> { 12 | public: 13 | RW4(); 14 | 15 | RW4( 16 | const Eigen::Quaternion& q, 17 | const Eigen::Matrix& w, 18 | const Eigen::Matrix& Omega, 19 | const double& Jw); 20 | 21 | ~RW4(); 22 | 23 | void operator()(const state_type& x, state_type& dxdt, double t); 24 | 25 | friend std::ostream& operator<<(std::ostream& os, const RW4& obj) 26 | { 27 | os << std::setw(6) << std::setprecision(4) << std::fixed; 28 | os << "Quat : " << obj._state[0] << ", " << obj._state[1] << ", " << obj._state[2] << ", " << obj._state[3] << "\n"; 29 | os << "Rate : " << obj._state[4] << ", " << obj._state[5] << ", " << obj._state[6] << "\n"; 30 | return os; 31 | } 32 | action_type calc_steering(const Eigen::Matrix& torque, const double& t = 0); 33 | 34 | private: 35 | Eigen::Matrix _Omega; // reaction wheel velocities 36 | Eigen::Matrix Gs; 37 | Eigen::Matrix Gs_psudo_inv; 38 | double _Jw = 0.01; 39 | }; 40 | 41 | #endif -------------------------------------------------------------------------------- /include/ADCS/Systems/RigidBody.h: -------------------------------------------------------------------------------- 1 | #ifndef _ADCS_RIGID_BODY_H 2 | #define _ADCS_RIGID_BODY_H 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | class RigidBody : public IBaseSystem<7, 3> { 12 | public: 13 | RigidBody(); 14 | 15 | RigidBody( 16 | const Eigen::Quaternion& q, 17 | const Eigen::Matrix& w); 18 | 19 | ~RigidBody(); 20 | 21 | void operator()(const state_type& x, state_type& dxdt, double t); 22 | 23 | friend std::ostream& operator<<(std::ostream& os, const RigidBody& obj) 24 | { 25 | os << std::setw(6) << std::setprecision(4) << std::fixed; 26 | os << "Quat : " << obj._state[0] << ", " << obj._state[1] << ", " << obj._state[2] << ", " << obj._state[3] << "\n"; 27 | os << "Rate : " << obj._state[4] << ", " << obj._state[5] << ", " << obj._state[6] << "\n"; 28 | return os; 29 | } 30 | action_type calc_steering(const Eigen::Matrix& torque, const double& t = 0); 31 | }; 32 | 33 | #endif -------------------------------------------------------------------------------- /include/ADCS/Systems/VSCMG.h: -------------------------------------------------------------------------------- 1 | #ifndef VSCMG_H 2 | #define VSCMG_H 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | class VSCMG : public IBaseSystem<15, 8> { 13 | public: 14 | VSCMG(); 15 | 16 | VSCMG(const double& beta, 17 | const Eigen::Matrix& Jp, 18 | const double& Jw, 19 | const double& Jcmg, 20 | const double& Jt); 21 | 22 | ~VSCMG(); 23 | 24 | void operator()(const state_type& x_, state_type& dxdt_, double t); 25 | 26 | void updateCMGAxes(); 27 | 28 | void set_gimbal_angle(const Eigen::Matrix& delta); 29 | 30 | void set_wheel_velocity(const Eigen::Matrix& Omega); 31 | 32 | /** 33 | * @brief Matrix of each column represents orientation of gimbal axis 34 | * 35 | * @return Eigen::Matrix 36 | */ 37 | Eigen::Matrix get_gimbal_matrix() const; 38 | 39 | /** 40 | * @brief Matrix of each column represents orientation of Reaction wheel spin axis 41 | * 42 | * @return Eigen::Matrix 43 | */ 44 | Eigen::Matrix get_spin_matrix() const; 45 | 46 | /** 47 | * @brief Matrix of each column represents orientation of cross product of 48 | * Reaction wheel spin axis and gimbal axis. 49 | * 50 | * @return Eigen::Matrix 51 | */ 52 | Eigen::Matrix get_transverse_matrix() const; 53 | 54 | action_type calc_steering(const Eigen::Matrix& torque, const double& t); 55 | 56 | friend std::ostream& operator<<(std::ostream& os, const VSCMG& obj) 57 | { 58 | os << std::setw(6) << std::setprecision(4) << std::fixed; 59 | os << "Quat : " << obj._state[0] << ", " << obj._state[1] << ", " << obj._state[2] << ", " << obj._state[3] << "\n"; 60 | os << "Rate : " << obj._state[4] << ", " << obj._state[5] << ", " << obj._state[6] << "\n"; 61 | os << "delta : " << obj._state[7] << ", " << obj._state[8] << ", " << obj._state[9] << ", " << obj._state[10] << "\n"; 62 | os << "Omega : " << obj._state[11] << ", " << obj._state[12] << ", " << obj._state[13] << ", " << obj._state[14] << "\n"; 63 | return os; 64 | } 65 | 66 | private: 67 | double _beta; // skew angle of pyramid 68 | 69 | Eigen::Matrix _delta; // gimbal angles 70 | Eigen::Matrix _Omega; // reaction wheel velocities 71 | 72 | Eigen::Matrix Gg; // Gimbal Axis Matrix 73 | Eigen::Matrix Gs; // Spin Axis Matrix 74 | Eigen::Matrix Gt; // Transverse Axis Matrix 75 | Eigen::Matrix Q; // [Gt Gs] 76 | 77 | Eigen::Matrix _Jp; // platform inertia 78 | 79 | double _Jw; // wheel inertia about spin axis 80 | double _Jt; // cmg transvers 81 | double _Jcmg; // cmg inertia about gimbal axis 82 | }; 83 | 84 | #endif -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | from setuptools import setup 3 | 4 | def read(fname): 5 | return open(os.path.join(os.path.dirname(__file__), fname)).read() 6 | 7 | setup( 8 | name = "adcs", 9 | version = "0.0.1", 10 | author = "Siddharth Deore", 11 | author_email = "siddharthdeore@gmail.com", 12 | description = ("ADCS Toolbox with spacecraft systems such as" 13 | "VSCMG, ReactionWheels "), 14 | license = "BSD", 15 | keywords = "adcs vscmg", 16 | url = "http://deore.in/", 17 | packages=['adcs'], 18 | package_dir={'adcs': 'adcs'}, 19 | package_data={'adcs': ['*.so','*.pyd']}, 20 | include_package_data = True, 21 | long_description=read('README.md'), 22 | classifiers=[ 23 | "Development Status :: 3 - Alpha", 24 | "Topic :: Utilities", 25 | "License :: OSI Approved :: BSD License", 26 | ], 27 | ) 28 | -------------------------------------------------------------------------------- /src/ADCS/Systems/RW4.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | RW4::RW4() 3 | { 4 | 5 | _state = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 , 0.0, 0.0, 0.0, 0.0 }; 6 | _Jp = Eigen::Matrix::Identity(3, 3); 7 | _Jw = 0.001; 8 | 9 | 10 | // 3 reaction wheels aligned to body axis, forth placed equidistant to all axis 11 | Gs << 1.0, 0, 0, -1.0 / sqrt(3.0), 12 | 0, 1.0, 0, -1.0 / sqrt(3.0), 13 | 0, 0, 1.0, -1.0 / sqrt(3.0); 14 | 15 | Gs_psudo_inv = Gs.transpose() * (Gs * Gs.transpose()).inverse(); 16 | } 17 | 18 | RW4::RW4( 19 | const Eigen::Quaternion& q, 20 | const Eigen::Matrix& w, 21 | const Eigen::Matrix& Omega, 22 | const double& Jw) 23 | { 24 | _state = { q.w(), q.x(), q.y(), q.z(), w.x(), w.y(), w.z(), Omega[0], Omega[1], Omega[2], Omega[3] }; 25 | 26 | _Jw = Jw; 27 | 28 | // 3 reaction wheels aligned to body axis, forth placed equidistant to all axis 29 | Gs << 1.0, 0, 0, -1.0 / sqrt(3.0), 30 | 0, 1.0, 0, -1.0 / sqrt(3.0), 31 | 0, 0, 1.0, -1.0 / sqrt(3.0); 32 | 33 | Gs_psudo_inv = Gs.transpose() * (Gs * Gs.transpose()).inverse(); 34 | } 35 | 36 | RW4::~RW4() { } 37 | 38 | void RW4::operator()(const state_type& x, state_type& dxdt, double t) 39 | { 40 | const Eigen::Quaterniond q(x[0], x[1], x[2], x[3]); 41 | const Eigen::Vector3d w(x[4], x[5], x[6]); 42 | const Eigen::Matrix Omega(x[7], x[8], x[9], x[10]); 43 | 44 | // kinematics 45 | const Eigen::Quaterniond q_dot = get_quaternion_kinematics(q, w); 46 | action_type Omega_dot = -_action; 47 | 48 | // rigid body dymaics Jw_dot + w^Jw = u 49 | const Eigen::Vector3d rw_term = Gs * (_Jw * Omega_dot); 50 | const Eigen::Vector3d gyro_effect = w.cross(_Jp * w + Gs * _Jw * Omega); 51 | 52 | const Eigen::Vector3d w_dot = -_Jp.inverse() * (rw_term + gyro_effect); 53 | 54 | dxdt[0] = q_dot.w(); 55 | dxdt[1] = q_dot.x(); 56 | dxdt[2] = q_dot.y(); 57 | dxdt[3] = q_dot.z(); 58 | 59 | dxdt[4] = w_dot.x(); 60 | dxdt[5] = w_dot.y(); 61 | dxdt[6] = w_dot.z(); 62 | 63 | dxdt[7] = Omega_dot[0]; 64 | dxdt[8] = Omega_dot[1]; 65 | dxdt[9] = Omega_dot[2]; 66 | dxdt[10] = Omega_dot[3]; 67 | } 68 | RW4::action_type RW4::calc_steering(const Eigen::Matrix& torque, const double& t) 69 | { 70 | return Gs_psudo_inv * torque; 71 | } 72 | 73 | #ifdef BUILD_PYTHON_LIB 74 | EXPOSE_SYSTEM_TO_PYTHON(RW4); 75 | #endif -------------------------------------------------------------------------------- /src/ADCS/Systems/RigidBody.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | RigidBody::RigidBody() 3 | { 4 | _Jp = Eigen::Matrix::Identity(3, 3); 5 | _state = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; 6 | } 7 | RigidBody::RigidBody(const Eigen::Quaternion& q, const Eigen::Matrix& w) 8 | { 9 | _state = { q.w(), q.x(), q.y(), q.z(), w.x(), w.y(), w.z() }; 10 | _Jp = Eigen::Matrix::Identity(3, 3); 11 | } 12 | 13 | RigidBody::~RigidBody() { } 14 | 15 | void RigidBody::operator()(const state_type& x, state_type& dxdt, double t) 16 | { 17 | const Eigen::Quaterniond q(x[0], x[1], x[2], x[3]); 18 | const Eigen::Vector3d w(x[4], x[5], x[6]); 19 | 20 | // kinematics 21 | const Eigen::Quaterniond q_dot = get_quaternion_kinematics(q, w); 22 | 23 | // rigid body dymaics Jw_dot + w^Jw = u 24 | const Eigen::Vector3d w_dot = -_Jp.inverse() * (w.cross(_Jp * w) - _action); 25 | 26 | dxdt[0] = q_dot.w(); 27 | dxdt[1] = q_dot.x(); 28 | dxdt[2] = q_dot.y(); 29 | dxdt[3] = q_dot.z(); 30 | 31 | dxdt[4] = w_dot.x(); 32 | dxdt[5] = w_dot.y(); 33 | dxdt[6] = w_dot.z(); 34 | } 35 | RigidBody::action_type RigidBody::calc_steering(const Eigen::Matrix& torque, const double& t) 36 | { 37 | return torque; 38 | } 39 | 40 | #ifdef BUILD_PYTHON_LIB 41 | EXPOSE_SYSTEM_TO_PYTHON(RigidBody); 42 | #endif -------------------------------------------------------------------------------- /src/ADCS/Systems/VSCMG.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include /* sin, cos */ 4 | 5 | VSCMG::VSCMG() 6 | { 7 | _beta = 0.9553166181245093, 8 | _Jp = Eigen::Matrix::Identity(3, 3), 9 | _Jw = 0.01, 10 | _Jcmg = 0.01, 11 | _Jt = 0.01; 12 | 13 | _state = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; 14 | } 15 | VSCMG::VSCMG(const double& beta, 16 | const Eigen::Matrix& Jp, 17 | const double& Jw, 18 | const double& Jcmg, 19 | const double& Jt) 20 | : _beta(beta) 21 | , _Jp(Jp) 22 | , _Jcmg(Jcmg) 23 | , _Jt(Jt) 24 | { 25 | _state = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; 26 | } 27 | VSCMG::~VSCMG() 28 | { 29 | } 30 | 31 | void VSCMG::set_gimbal_angle(const Eigen::Matrix& delta) 32 | { 33 | _delta = delta; 34 | updateCMGAxes(); 35 | } 36 | void VSCMG::set_wheel_velocity(const Eigen::Matrix& Omega) 37 | { 38 | _Omega = Omega; 39 | } 40 | 41 | void VSCMG::updateCMGAxes() 42 | { 43 | const auto sb = std::sin(_beta); 44 | const auto cb = std::cos(_beta); 45 | const auto cd = _delta.array().cos(); 46 | const auto sd = _delta.array().sin(); 47 | 48 | Gg << -sb, 0, sb, 0, 49 | 0, sb, 0, -sb, 50 | cb, cb, cb, cb; 51 | 52 | Gs << cb * cd(0), -sd(1), -cb * cd(2), sd(3), 53 | -sd(0), -cb * cd(1), sd(2), cb * cd(3), 54 | sb * cd(0), sb * cd(1), sb * cd(2), sb * cd(3); 55 | 56 | Gt << cb * sd(0), cd(1), -cb * sd(2), -cd(3), 57 | cd(0), -cb * sd(1), -cd(2), cb * sd(3), 58 | sb * sd(0), sb * sd(1), sb * sd(2), sb * sd(3); 59 | } 60 | 61 | Eigen::Matrix VSCMG::get_gimbal_matrix() const 62 | { 63 | return Gg; 64 | } 65 | 66 | Eigen::Matrix VSCMG::get_spin_matrix() const 67 | { 68 | return Gs; 69 | } 70 | 71 | Eigen::Matrix VSCMG::get_transverse_matrix() const 72 | { 73 | return Gt; 74 | } 75 | 76 | Eigen::Matrix VSCMG::calc_steering(const Eigen::Matrix& torque, const double& t) 77 | { 78 | 79 | const auto D = _Jw * Gs; // RW 80 | const auto C = _Jw * (Gt * _Omega.asDiagonal()); // CMG 81 | Q << C, D; 82 | 83 | const auto cm = (Gt * Gt.transpose()).determinant(); // CMG Singularity coeficient 84 | const auto wm = (Gs * Gs.transpose()).determinant(); // RW Singularity coeficient 85 | const auto vm = (Q * Q.transpose()).determinant(); // RW Singularity coeficient 86 | 87 | // Singularity avoiding Steering Law 88 | const auto beta = std::min(10000.0 * std::exp(-0.0001 * cm / (wm + 1e-6)), 10000.0); 89 | 90 | // Weight matrix 91 | Eigen::Matrix vec; 92 | vec << 1.0, 1.0, 1.0, 1.0, beta, beta, beta, beta; 93 | const auto W = vec.asDiagonal(); 94 | 95 | // Avoid singularities using SVD 96 | Eigen::JacobiSVD> svd(Q, Eigen::ComputeFullV); 97 | const auto gamma = 0.0001 * std::exp(-10 * vm); 98 | Eigen::Matrix V = gamma * svd.matrixV().col(0); 99 | 100 | // Escape Singularities using off diagonal harmonics 101 | const auto alpha = 0.001 * std::exp(-10 * cm); 102 | 103 | const auto e1 = alpha * std::sin(0.01 * t); 104 | const auto e2 = alpha * std::sin(0.01 * t + M_PI_2); 105 | const auto e3 = alpha * std::sin(0.01 * t + M_PI); 106 | 107 | // Off diagonal harmonics 108 | Eigen::Matrix E; 109 | E << 1.0, e3, e2, 110 | e3, 1.0, e1, 111 | e2, e1, 1.0; 112 | 113 | const auto QWQT = Q * W * Q.transpose(); 114 | 115 | // Singularity Escaping Steering step 116 | const Eigen::Matrix delta_Omega_dot = W * Q.transpose() * (QWQT + E).inverse() * torque + V; 117 | return delta_Omega_dot; 118 | } 119 | 120 | void VSCMG::operator()(const state_type& x, state_type& dxdt, double t) 121 | { 122 | const Eigen::Quaterniond q(x[0], x[1], x[2], x[3]); 123 | const Eigen::Vector3d w(x[4], x[5], x[6]); 124 | const Eigen::Matrix delta(x[7], x[8], x[9], x[10]); 125 | const Eigen::Matrix Omega(x[11], x[12], x[13], x[14]); 126 | // CMG direction coisins 127 | set_gimbal_angle(delta); 128 | set_wheel_velocity(Omega); 129 | 130 | const Eigen::Matrix Gs = get_spin_matrix(); 131 | const Eigen::Matrix Gt = get_transverse_matrix(); 132 | const Eigen::Matrix Gg = get_gimbal_matrix(); 133 | // System Inertia Tensor 134 | const Eigen::Matrix J = _Jp + _Jw * Gs * Gs.transpose() + _Jt * Gg * Gg.transpose() + _Jcmg * Gt * Gt.transpose(); 135 | 136 | // kinematics 137 | const Eigen::Quaterniond q_dot = get_quaternion_kinematics(q, w); 138 | 139 | const Eigen::Matrix delta_Omega_dot = -_action; 140 | 141 | // gimbal velocity deviation 142 | const auto delta_dot = delta_Omega_dot.block<4, 1>(0, 0); 143 | const auto Omega_dot = delta_Omega_dot.block<4, 1>(4, 0); 144 | 145 | /* dynamics */ 146 | 147 | // Gimbal velocity contribution 148 | const Eigen::Matrix term1 = (Gt * (delta_dot.asDiagonal() * (_Jw - _Jt)) * Gs.transpose() + Gs * ((delta_dot.asDiagonal()) * (_Jw - _Jt)) * Gt.transpose()) * w; 149 | 150 | // Reaction wheel contribution 151 | const Eigen::Matrix term2 = Gt * (_Jw * Omega.asDiagonal() * delta_dot) + Gs * (_Jw * Omega_dot); 152 | 153 | // Gyroscopic effects due to variation of angular momentum of entire system 154 | const Eigen::Matrix term3 = w.cross(J * w + Gg * _Jcmg * delta_dot + Gs * _Jw * Omega); 155 | 156 | // eqation of motion of VSCMG dynamics 157 | const Eigen::Matrix w_dot = J.inverse() * (-term1 - term2 - term3); 158 | 159 | dxdt[0] = q_dot.w(); 160 | dxdt[1] = q_dot.x(); 161 | dxdt[2] = q_dot.y(); 162 | dxdt[3] = q_dot.z(); 163 | 164 | dxdt[4] = w_dot.x(); 165 | dxdt[5] = w_dot.y(); 166 | dxdt[6] = w_dot.z(); 167 | 168 | dxdt[7] = delta_dot[0]; 169 | dxdt[8] = delta_dot[1]; 170 | dxdt[9] = delta_dot[2]; 171 | dxdt[10] = delta_dot[3]; 172 | 173 | dxdt[11] = Omega_dot[0]; 174 | dxdt[12] = Omega_dot[1]; 175 | dxdt[13] = Omega_dot[2]; 176 | dxdt[14] = Omega_dot[3]; 177 | } 178 | 179 | #ifdef BUILD_PYTHON_LIB 180 | EXPOSE_SYSTEM_TO_PYTHON(VSCMG) 181 | #endif -------------------------------------------------------------------------------- /src/ADCS/py_controller.cpp: -------------------------------------------------------------------------------- 1 | #ifdef BUILD_PYTHON_LIB 2 | #include 3 | #include 4 | #include 5 | #include 6 | namespace numpy = boost::python::numpy; 7 | 8 | numpy::ndarray py_quaternion_control( 9 | const numpy::ndarray& state, 10 | const numpy::ndarray& des_state, 11 | const double& K = 10.0, const double C = 1.0) 12 | { 13 | NumPyArrayData data(state); 14 | NumPyArrayData des_data(des_state); 15 | 16 | const Eigen::Quaterniond q(data(0), data(1), data(2), data(3)); 17 | const Eigen::Quaterniond qd(des_data(0), des_data(1), des_data(2), des_data(3)); 18 | const Eigen::Vector3d w(data(4), data(5), data(6)); 19 | const Eigen::Quaterniond qe = get_quaternion_error(q, qd); 20 | const Eigen::Vector3d u = Controller::quaternion_feedback(qe, w, K, C); 21 | Py_intptr_t shape[1] = { u.size() }; 22 | numpy::ndarray result = numpy::zeros(1, shape, numpy::dtype::get_builtin()); 23 | result[0] = u[0]; 24 | result[1] = u[1]; 25 | result[2] = u[2]; 26 | return result; 27 | } 28 | 29 | BOOST_PYTHON_MODULE(libController) 30 | { 31 | using namespace boost::python; 32 | numpy::initialize(); 33 | Py_Initialize(); 34 | 35 | def("quaternion_control", &py_quaternion_control); 36 | } 37 | #endif --------------------------------------------------------------------------------