├── .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 |
4 |
5 |
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
--------------------------------------------------------------------------------